1 files changed, 16 insertions, 8 deletions
diff --git a/src/components/ble/NimbleController.cpp b/src/components/ble/NimbleController.cpp
index 74c8f926..5059007a 100644
--- a/src/components/ble/NimbleController.cpp
+++ b/src/components/ble/NimbleController.cpp
@@ -42,13 +42,12 @@ NimbleController::NimbleController(Pinetime::System::SystemTask& systemTask,
     anService {systemTask, notificationManager},
     alertNotificationClient {systemTask, notificationManager},
     currentTimeService {dateTimeController},
-    musicService {systemTask},
-    weatherService {systemTask, dateTimeController},
-    navService {systemTask},
+    musicService {*this},
+    weatherService {dateTimeController},
     batteryInformationService {batteryController},
     immediateAlertService {systemTask, notificationManager},
-    heartRateService {systemTask, heartRateController},
-    motionService {systemTask, motionController},
+    heartRateService {*this, heartRateController},
+    motionService {*this, motionController},
     fsService {systemTask, fs},
     serviceDiscovery({&currentTimeClient, &alertNotificationClient}) {
 }
@@ -159,7 +158,10 @@ void NimbleController::StartAdvertising() {
   }
 
   fields.flags = BLE_HS_ADV_F_DISC_GEN | BLE_HS_ADV_F_BREDR_UNSUP;
-  fields.uuids128 = &dfuServiceUuid;
+  fields.uuids16 = &HeartRateService::heartRateServiceUuid;
+  fields.num_uuids16 = 1;
+  fields.uuids16_is_complete = 1;
+  fields.uuids128 = &DfuService::serviceUuid;
   fields.num_uuids128 = 1;
   fields.uuids128_is_complete = 1;
   fields.tx_pwr_lvl = BLE_HS_ADV_TX_PWR_LVL_AUTO;
@@ -452,9 +454,15 @@ void NimbleController::PersistBond(struct ble_gap_conn_desc& desc) {
     /* Wakeup Spi and SpiNorFlash before accessing the file system
      * This should be fixed in the FS driver
      */
-    systemTask.PushMessage(Pinetime::System::Messages::GoToRunning);
     systemTask.PushMessage(Pinetime::System::Messages::DisableSleeping);
-    vTaskDelay(10);
+
+    // This isn't quite correct
+    // SystemTask could receive EnableSleeping right after passing this check
+    // We need some guarantee that the SystemTask has processed the above message
+    // before we can continue
+    while (!systemTask.IsSleepDisabled()) {
+      vTaskDelay(pdMS_TO_TICKS(5));
+    }
 
     lfs_file_t file_p;