diff --git a/Makefile b/Makefile
index 621e0361..7ebbc0c1 100644
--- a/Makefile
+++ b/Makefile
@@ -55,6 +55,7 @@ endif
 
 ifeq ($(strip $(BLE)), 1)
 CFLAGS += -DBLE=1
+endif
 
 OBJS += src/ble/ble.o
 OBJS += src/ble/ble_crazyflies.o
@@ -80,7 +81,6 @@ CFLAGS += -I$(NRF_S110)/Include/
 CFLAGS += -I$(NRF51_SDK)/Include/app_common/
 CFLAGS += -I$(NRF51_SDK)/Include/sd_common/
 CFLAGS += -I$(NRF51_SDK)/Include/sdk/
-endif
 
 OBJS += src/main.o gcc_startup_nrf51.o system_nrf51.o src/uart.o \
         src/syslink.o src/pm.o src/systick.o src/button.o src/swd.o src/ow.o \
diff --git a/src/ble/ble.c b/src/ble/ble.c
index 374315f0..52057a2e 100644
--- a/src/ble/ble.c
+++ b/src/ble/ble.c
@@ -40,12 +40,9 @@
 
 #include "ble_crazyflies.h"
 
-
-#if BLE
 #if S110==0
 #error S110 shall be used when using BLE
 #endif
-#endif
 
 #define DEVICE_NAME                          "Crazyflie"                              /**< Name of device. Will be included in the advertising data. */
 #define MANUFACTURER_NAME                    "Bitcraze"                     /**< Manufacturer. Will be passed to Device Information Service. */
diff --git a/src/esb.c b/src/esb.c
index f862e846..9e65fc87 100644
--- a/src/esb.c
+++ b/src/esb.c
@@ -32,14 +32,14 @@
 
 #include <nrf.h>
 
-#ifdef BLE
 #include <ble_gap.h>
 #include <nrf_soc.h>
-#endif
 
 #define RXQ_LEN 8
 #define TXQ_LEN 8
 
+extern int bleEnabled;
+
 static bool isInit = true;
 
 static int channel = 80;
@@ -312,12 +312,12 @@ void esbInit()
 {
   NRF_RADIO->POWER = 1;
   // Enable Radio interrupts
-#ifndef BLE
-  NVIC_SetPriority(RADIO_IRQn, 3);
-  NVIC_EnableIRQ(RADIO_IRQn);
-#else
-  NVIC_EnableIRQ(RADIO_IRQn);
-#endif
+  if (!bleEnabled) {
+    NVIC_SetPriority(RADIO_IRQn, 3);
+    NVIC_EnableIRQ(RADIO_IRQn);
+  } else {
+    NVIC_EnableIRQ(RADIO_IRQn);
+  }
 
 
   NRF_RADIO->TXPOWER = (txpower << RADIO_TXPOWER_TXPOWER_Pos);
@@ -392,24 +392,27 @@ void esbInit()
 void esbReset()
 {
   if (!isInit) return;
-#ifndef BLE
-  __disable_irq();
-#endif
+
+  if (!bleEnabled) {
+    __disable_irq();
+  }
+
   NRF_RADIO->TASKS_DISABLE = 1;
   NRF_RADIO->POWER = 0;
 
   NVIC_GetPendingIRQ(RADIO_IRQn);
   __enable_irq();
-#ifndef BLE
-  esbInit();
-#endif
+
+  if (!bleEnabled) {
+    esbInit();
+  }
 }
 
 void esbDeinit()
 {
-#ifndef BLE
-  NVIC_DisableIRQ(RADIO_IRQn);
-#endif
+  if (!bleEnabled) {
+    NVIC_DisableIRQ(RADIO_IRQn);
+  }
 
   NRF_RADIO->INTENCLR = RADIO_INTENSET_END_Msk;
   NRF_RADIO->SHORTS = 0;
@@ -496,13 +499,13 @@ void esbSetContwave(bool enable)
 {
   contwave = enable;
 
-#ifdef BLE
-  if (enable)
-    ble_advertising_stop();
-  else
-    advertising_start();
-#endif
-
+  if (bleEnabled) {
+    if (enable) {
+      ble_advertising_stop();
+    } else {
+      advertising_start();
+    }
+  }
 
   esbReset();
 }
diff --git a/src/main.c b/src/main.c
index f0d266df..53f6b516 100644
--- a/src/main.c
+++ b/src/main.c
@@ -23,9 +23,7 @@
  */
 #include <nrf.h>
 
-#ifdef BLE
-#include <nrf_soc.h>
-#endif
+
 
 #include <stdio.h>
 #include <string.h>
@@ -41,13 +39,14 @@
 #include "pm.h"
 #include "pinout.h"
 #include "systick.h"
+#include "nrf_sdm.h"
+#include "nrf_soc.h"
 
 #include "memory.h"
 #include "ownet.h"
 
-#ifdef BLE
 #include "ble_crazyflies.h"
-#endif
+
 
 extern void  initialise_monitor_handles(void);
 extern int ble_init(void);
@@ -69,10 +68,17 @@ static void mainloop(void);
 #undef BLE
 #endif
 
+#ifdef BLE
+int bleEnabled = 1;
+#else
+int bleEnabled = 0;
+#endif
+
 static bool boottedFromBootloader;
 
 static void handleRadioCmd(struct esbPacket_s * packet);
 static void handleBootloaderCmd(struct esbPacket_s *packet);
+static void disableBle();
 
 static int stmStartTime = 0;
 int main()
@@ -86,12 +92,12 @@ int main()
   systickInit();
   memoryInit();
 
-#ifdef BLE
-  ble_init();
-#else
-  NRF_CLOCK->TASKS_HFCLKSTART = 1UL;
-  while(!NRF_CLOCK->EVENTS_HFCLKSTARTED);
-#endif
+  if (bleEnabled) {
+    ble_init();
+  } else {
+    NRF_CLOCK->TASKS_HFCLKSTART = 1UL;
+    while(!NRF_CLOCK->EVENTS_HFCLKSTARTED);
+  }
 
 #ifdef SEMIHOSTING
   initialise_monitor_handles();
@@ -125,12 +131,12 @@ int main()
 
   NRF_GPIO->PIN_CNF[RADIO_PAEN_PIN] |= GPIO_PIN_CNF_DIR_Output | (GPIO_PIN_CNF_DRIVE_S0H1<<GPIO_PIN_CNF_DRIVE_Pos);
 
-#ifndef BLE
-  esbInit();
+  if (!bleEnabled) {
+    esbInit();
 
-  esbSetDatarate(DEFAULT_RADIO_RATE);
-  esbSetChannel(DEFAULT_RADIO_CHANNEL);
-#endif
+    esbSetDatarate(DEFAULT_RADIO_RATE);
+    esbSetChannel(DEFAULT_RADIO_CHANNEL);
+  }
 
   DEBUG_PRINT("Started\n");
 
@@ -159,16 +165,16 @@ void mainloop()
   while(1)
   {
 
-#ifdef BLE
-    if ((esbReceived == false) && bleCrazyfliesIsPacketReceived()) {
-      EsbPacket* packet = bleCrazyfliesGetRxPacket();
-      memcpy(esbRxPacket.data, packet->data, packet->size);
-      esbRxPacket.size = packet->size;
-      esbReceived = true;
-      bleCrazyfliesReleaseRxPacket(packet);
+    if (bleEnabled) {
+      if ((esbReceived == false) && bleCrazyfliesIsPacketReceived()) {
+        EsbPacket* packet = bleCrazyfliesGetRxPacket();
+        memcpy(esbRxPacket.data, packet->data, packet->size);
+        esbRxPacket.size = packet->size;
+        esbReceived = true;
+        bleCrazyfliesReleaseRxPacket(packet);
+      }
     }
 
-#endif
 #ifndef CONT_WAVE_TEST
 
     if ((esbReceived == false) && esbIsRxPacket())
@@ -190,6 +196,8 @@ void mainloop()
       esbRxPacket.size = packet->size;
       esbReceived = true;
       esbReleaseRxPacket(packet);
+
+      disableBle();
     }
 
     if (esbReceived)
@@ -249,14 +257,15 @@ void mainloop()
             }
             bzero(slRxPacket.data, SYSLINK_MTU);
           }
-#ifdef BLE
-          if (slRxPacket.length < SYSLINK_MTU) {
-            static EsbPacket pk;
-            memcpy(pk.data,  slRxPacket.data, slRxPacket.length);
-            pk.size = slRxPacket.length;
-            bleCrazyfliesSendPacket(&pk);
+
+          if (bleEnabled) {
+            if (slRxPacket.length < SYSLINK_MTU) {
+              static EsbPacket pk;
+              memcpy(pk.data,  slRxPacket.data, slRxPacket.length);
+              pk.size = slRxPacket.length;
+              bleCrazyfliesSendPacket(&pk);
+            }
           }
-#endif
 
           break;
         case SYSLINK_RADIO_CHANNEL:
@@ -464,9 +473,10 @@ static void handleBootloaderCmd(struct esbPacket_s *packet)
       memcpy(&(txpk.data[3]), (uint32_t*)NRF_FICR->DEVICEADDR, 6);
 
       txpk.size = 9;
-#if BLE
-      bleCrazyfliesSendPacket(&txpk);
-#endif
+      if (bleEnabled) {
+        bleCrazyfliesSendPacket(&txpk);
+      }
+
       if (esbCanTxPacket()) {
         struct esbPacket_s *pk = esbGetTxPacket();
         memcpy(pk, &txpk, sizeof(struct esbPacket_s));
@@ -483,11 +493,11 @@ static void handleBootloaderCmd(struct esbPacket_s *packet)
           //Set bit 0x20 forces boot to firmware
           NRF_POWER->GPREGRET |= 0x20U;
         }
-#ifdef BLE
-        sd_nvic_SystemReset();
-#else
-        NVIC_SystemReset();
-#endif
+        if (bleEnabled) {
+          sd_nvic_SystemReset();
+        } else {
+          NVIC_SystemReset();
+        }
       }
       break;
     case BOOTLOADER_CMD_ALLOFF:
@@ -528,3 +538,11 @@ static void handleBootloaderCmd(struct esbPacket_s *packet)
       break;
   }
 }
+
+static void disableBle() {
+  if (bleEnabled) {
+      sd_softdevice_disable();
+      bleEnabled = 0;
+      esbInit();
+  }
+}
diff --git a/src/memory.c b/src/memory.c
index a9806576..a6673cc6 100644
--- a/src/memory.c
+++ b/src/memory.c
@@ -39,7 +39,8 @@
 static bool isInit;
 static int nMemory;
 
-#ifdef BLE
+extern int bleEnabled;
+
 #define OW_MAX_CACHED 4
 static struct {unsigned char address[8]; unsigned char data[122];} owCache[OW_MAX_CACHED];
 
@@ -50,7 +51,7 @@ static void cacheMemory(int nMem)
     ds28e05ReadMemory(0, 0, owCache[nMem].data, 112);
   }
 }
-#else
+
 static bool selectMemory(int n)
 {
   if (owFirst(0, 1, 0))
@@ -69,7 +70,6 @@ static bool selectMemory(int n)
 
   return false;
 }
-#endif
 
 int owScan()
 {
@@ -80,17 +80,14 @@ int owScan()
 
   if (owFirst(0, 1, 0))
   {
-#ifdef BLE
     cacheMemory(0);
-#endif
     nMem++;
     while(owNext(0, 1, 0))
     {
-#ifdef BLE
-      cacheMemory(nMem);
-      if (nMem>=OW_MAX_CACHED)
-        break;
-#endif
+      if (nMem < OW_MAX_CACHED) {
+        cacheMemory(nMem);
+      }
+
       nMem++;
     }
   }
@@ -119,26 +116,23 @@ bool memorySyslink(struct syslinkPacket *pk) {
 
   switch (pk->type) {
     case SYSLINK_OW_SCAN:
-#ifndef BLE
-      nMemory = owScan();
-#endif
+      if (!bleEnabled) {
+        nMemory = owScan();
+      }
+
       pk->data[0] = nMemory;
       pk->length = 1;
       tx = true;
       break;
     case SYSLINK_OW_GETINFO:
-#ifdef BLE
-      if (command->nmem<nMemory) {
+      if (bleEnabled && command->nmem < nMemory) {
         memcpy(command->info.memId, owCache[command->nmem].address, 8);
         pk->length = 1+8;
         tx = true;
-#else
-      if (selectMemory(command->nmem))
-      {
+      } else if (!bleEnabled && selectMemory(command->nmem)) {
         owSerialNum(0, command->info.memId, 1);
         pk->length = 1+8;
         tx = true;
-#endif
       } else {
         //Cannot select the memory
         pk->data[0] = -1;
@@ -148,18 +142,15 @@ bool memorySyslink(struct syslinkPacket *pk) {
       break;
 
     case SYSLINK_OW_READ:
-#ifdef BLE
-      if (command->nmem<nMemory) {
+      if (bleEnabled && command->nmem<nMemory) {
         memcpy(command->read.data, &owCache[command->nmem].data[command->read.address], 29);
         pk->length = 32;
         tx=true;
-#else
-      if (selectMemory(command->nmem) &&
-          ds28e05ReadMemory(0, command->read.address, command->read.data, 29))
-      {
+
+      } else if (!bleEnabled && selectMemory(command->nmem) &&
+          ds28e05ReadMemory(0, command->read.address, command->read.data, 29)) {
         pk->length = 32;
         tx=true;
-#endif
       } else {
         //Cannot select the memory
         pk->data[0] = -1;
@@ -169,23 +160,22 @@ bool memorySyslink(struct syslinkPacket *pk) {
 
       break;
     case SYSLINK_OW_WRITE:
-#ifdef BLE
-      //Cannot currently write the memory when compiled with BLE
-      pk->data[0] = -2;
-      pk->length = 1;
-      tx=true;
-#else
-      if (selectMemory(command->nmem) &&
-          ds28e05WriteMemory(0, command->write.address, command->write.data, command->write.length))
-      {
-        tx=true;
-      } else {
-        //Cannot select the memory
-        pk->data[0] = -1;
+      if (bleEnabled) {
+        //Cannot currently write the memory when compiled with BLE
+        pk->data[0] = -2;
         pk->length = 1;
         tx=true;
+      } else {
+        if (selectMemory(command->nmem) &&
+            ds28e05WriteMemory(0, command->write.address, command->write.data, command->write.length)) {
+          tx=true;
+        } else {
+          //Cannot select the memory
+          pk->data[0] = -1;
+          pk->length = 1;
+          tx=true;
+        }
       }
-#endif
       break;
   }
 
diff --git a/src/pm.c b/src/pm.c
index c9b3e73d..f3d7e57d 100644
--- a/src/pm.c
+++ b/src/pm.c
@@ -25,9 +25,7 @@
 
 #include <nrf.h>
 #include <nrf_gpio.h>
-#ifdef BLE
 #include <nrf_soc.h>
-#endif
 
 #include "pm.h"
 #include "button.h"
@@ -41,6 +39,8 @@
 //#define ENABLE_FAST_CHARGE_1A
 //#define RFX2411N_BYPASS_MODE
 
+extern int bleEnabled;
+
 static PmConfig const *pmConfig;
 static PmState state;
 static PmState targetState;
@@ -129,11 +129,13 @@ static void pmNrfPower(bool enable)
 
     nrf_gpio_cfg_input(PM_VBAT_SINK_PIN, NRF_GPIO_PIN_NOPULL);
     NRF_POWER->GPREGRET |= 0x01; // Workaround for not being able to determine reset reason...
-#ifdef BLE
-    sd_power_system_off();
-#else
-    NRF_POWER->SYSTEMOFF = 1UL;
-#endif
+
+    if (bleEnabled) {
+      sd_power_system_off();
+    } else {
+      NRF_POWER->SYSTEMOFF = 1UL;
+    }
+
     while(1);
   }
 }
@@ -355,4 +357,3 @@ void pmProcess() {
     }
   }
 }
-