Merge pull request #754 from InfiniTimeOrg/add-motion-service

Add motion service

12 files changed, 252 insertions, 13 deletions

diff --git a/doc/MotionService.md b/doc/MotionService.md
new file mode 100644
index 00000000..0d0a5514
--- /dev/null
+++ b/doc/MotionService.md
@@ -0,0 +1,17 @@
+# Motion Service
+## Introduction
+The motion service exposes step count and raw X/Y/Z motion value as READ and NOTIFY characteristics.
+
+## Service
+The service UUID is **00020000-78fc-48fe-8e23-433b3a1942d0**
+
+## Characteristics
+### Step count (UUID 00020001-78fc-48fe-8e23-433b3a1942d0)
+The current number of steps represented as a single `uint32_t` (4 bytes) value.
+
+### Raw motion values (UUID 00020002-78fc-48fe-8e23-433b3a1942d0)
+The current raw motion values. This is a 3 `int16_t` array:
+
+ - [0] : X
+ - [1] : Y
+ - [2] : Z
diff --git a/doc/ble.md b/doc/ble.md
index 2f8ba41c..8573166f 100644
--- a/doc/ble.md
+++ b/doc/ble.md
@@ -65,14 +65,19 @@ When the service does not exist in the BLE specification, InfiniTime implements
 The following custom services are implemented in InfiniTime:
 
  - Since InfiniTime 0.8:
-   ```
-    * Music Service :             00000000-78fc-48fe-8e23-433b3a1942d0
-   ```
-   
+    * Music Service : 00000000-78fc-48fe-8e23-433b3a1942d0
+ 
+
  - Since InfiniTime 0.11:
-   ```
-     * Navigation Service :        00010000-78fc-48fe-8e23-433b3a1942d0
-   ```
+     * [Navigation Service](NavigationService.md) : 00010000-78fc-48fe-8e23-433b3a1942d0
+
+
+ - Since InfiniTime 0.13
+     * Call characteristic (extension to the Alert Notification Service): 00020001-78fc-48fe-8e23-433b3a1942d0
+ 
+  
+ - Since InfiniTime 1.7:
+   * [Motion Service](MotionService.md) :        00030000-78fc-48fe-8e23-433b3a1942d0
 
 ---
 
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 07eabe11..a839e080 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -477,6 +477,7 @@ list(APPEND SOURCE_FILES
         components/ble/ImmediateAlertService.cpp
         components/ble/ServiceDiscovery.cpp
         components/ble/HeartRateService.cpp
+        components/ble/MotionService.cpp
         components/firmwarevalidator/FirmwareValidator.cpp
         components/motor/MotorController.cpp
         components/settings/Settings.cpp
@@ -545,6 +546,7 @@ list(APPEND RECOVERY_SOURCE_FILES
         components/ble/ServiceDiscovery.cpp
         components/ble/NavigationService.cpp
         components/ble/HeartRateService.cpp
+        components/ble/MotionService.cpp
         components/firmwarevalidator/FirmwareValidator.cpp
         components/settings/Settings.cpp
         components/timer/TimerController.cpp
@@ -652,6 +654,7 @@ set(INCLUDE_FILES
         components/ble/ServiceDiscovery.h
         components/ble/BleClient.h
         components/ble/HeartRateService.h
+        components/ble/MotionService.h
         components/settings/Settings.h
         components/timer/TimerController.h
         components/alarm/AlarmController.h
diff --git a/src/components/ble/HeartRateService.cpp b/src/components/ble/HeartRateService.cpp
index 5b00f492..75a038a2 100644
--- a/src/components/ble/HeartRateService.cpp
+++ b/src/components/ble/HeartRateService.cpp
@@ -8,7 +8,7 @@ constexpr ble_uuid16_t HeartRateService::heartRateServiceUuid;
 constexpr ble_uuid16_t HeartRateService::heartRateMeasurementUuid;
 
 namespace {
-  int HeartRateServiceServiceCallback(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt* ctxt, void* arg) {
+  int HeartRateServiceCallback(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt* ctxt, void* arg) {
     auto* heartRateService = static_cast<HeartRateService*>(arg);
     return heartRateService->OnHeartRateRequested(conn_handle, attr_handle, ctxt);
   }
@@ -19,7 +19,7 @@ HeartRateService::HeartRateService(Pinetime::System::SystemTask& system, Control
   : system {system},
     heartRateController {heartRateController},
     characteristicDefinition {{.uuid = &heartRateMeasurementUuid.u,
-                               .access_cb = HeartRateServiceServiceCallback,
+                               .access_cb = HeartRateServiceCallback,
                                .arg = this,
                                .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY,
                                .val_handle = &heartRateMeasurementHandle},
@@ -56,6 +56,8 @@ int HeartRateService::OnHeartRateRequested(uint16_t connectionHandle, uint16_t a
 }
 
 void HeartRateService::OnNewHeartRateValue(uint8_t heartRateValue) {
+  if(!heartRateMeasurementNotificationEnable) return;
+
   uint8_t buffer[2] = {0, heartRateController.HeartRate()}; // [0] = flags, [1] = hr value
   auto* om = ble_hs_mbuf_from_flat(buffer, 2);
 
@@ -67,3 +69,13 @@ void HeartRateService::OnNewHeartRateValue(uint8_t heartRateValue) {
 
   ble_gattc_notify_custom(connectionHandle, heartRateMeasurementHandle, om);
 }
+
+void HeartRateService::SubscribeNotification(uint16_t connectionHandle, uint16_t attributeHandle) {
+  if(attributeHandle == heartRateMeasurementHandle)
+    heartRateMeasurementNotificationEnable = true;
+}
+
+void HeartRateService::UnsubscribeNotification(uint16_t connectionHandle, uint16_t attributeHandle) {
+  if(attributeHandle == heartRateMeasurementHandle)
+    heartRateMeasurementNotificationEnable = false;
+}
\ No newline at end of file
diff --git a/src/components/ble/HeartRateService.h b/src/components/ble/HeartRateService.h
index 0b16703f..4e4a5a42 100644
--- a/src/components/ble/HeartRateService.h
+++ b/src/components/ble/HeartRateService.h
@@ -2,6 +2,7 @@
 #define min // workaround: nimble's min/max macros conflict with libstdc++
 #define max
 #include <host/ble_gap.h>
+#include <atomic>
 #undef max
 #undef min
 
@@ -18,6 +19,9 @@ namespace Pinetime {
       int OnHeartRateRequested(uint16_t connectionHandle, uint16_t attributeHandle, ble_gatt_access_ctxt* context);
       void OnNewHeartRateValue(uint8_t hearRateValue);
 
+      void SubscribeNotification(uint16_t connectionHandle, uint16_t attributeHandle);
+      void UnsubscribeNotification(uint16_t connectionHandle, uint16_t attributeHandle);
+
     private:
       Pinetime::System::SystemTask& system;
       Controllers::HeartRateController& heartRateController;
@@ -28,10 +32,11 @@ namespace Pinetime {
 
       static constexpr ble_uuid16_t heartRateMeasurementUuid {.u {.type = BLE_UUID_TYPE_16}, .value = heartRateMeasurementId};
 
-      struct ble_gatt_chr_def characteristicDefinition[3];
+      struct ble_gatt_chr_def characteristicDefinition[2];
       struct ble_gatt_svc_def serviceDefinition[2];
 
       uint16_t heartRateMeasurementHandle;
+      std::atomic_bool heartRateMeasurementNotificationEnable {false};
     };
   }
 }
diff --git a/src/components/ble/MotionService.cpp b/src/components/ble/MotionService.cpp
new file mode 100644
index 00000000..e305021a
--- /dev/null
+++ b/src/components/ble/MotionService.cpp
@@ -0,0 +1,124 @@
+#include "MotionService.h"
+#include "components/motion//MotionController.h"
+#include "systemtask/SystemTask.h"
+
+using namespace Pinetime::Controllers;
+
+namespace {
+  // 0002yyxx-78fc-48fe-8e23-433b3a1942d0
+  constexpr ble_uuid128_t CharUuid(uint8_t x, uint8_t y) {
+    return ble_uuid128_t{
+      .u = {.type = BLE_UUID_TYPE_128},
+      .value =  { 0xd0, 0x42, 0x19, 0x3a, 0x3b, 0x43, 0x23, 0x8e, 0xfe, 0x48, 0xfc, 0x78, x, y, 0x03, 0x00 }
+    };
+  }
+
+  // 00020000-78fc-48fe-8e23-433b3a1942d0
+  constexpr ble_uuid128_t BaseUuid() {
+    return CharUuid(0x00, 0x00);
+  }
+
+  constexpr ble_uuid128_t motionServiceUuid {BaseUuid()};
+  constexpr ble_uuid128_t stepCountCharUuid {CharUuid(0x01, 0x00)};
+  constexpr ble_uuid128_t motionValuesCharUuid {CharUuid(0x02, 0x00)};
+
+  int MotionServiceCallback(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt* ctxt, void* arg) {
+    auto* motionService = static_cast<MotionService*>(arg);
+    return motionService->OnStepCountRequested(conn_handle, attr_handle, ctxt);
+  }
+}
+
+// TODO Refactoring - remove dependency to SystemTask
+MotionService::MotionService(Pinetime::System::SystemTask& system, Controllers::MotionController& motionController)
+  : system {system},
+    motionController {motionController},
+    characteristicDefinition {{.uuid = &stepCountCharUuid.u,
+                               .access_cb = MotionServiceCallback,
+                               .arg = this,
+                               .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY,
+                               .val_handle = &stepCountHandle},
+                              {.uuid = &motionValuesCharUuid.u,
+                               .access_cb = MotionServiceCallback,
+                               .arg = this,
+                               .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY,
+                               .val_handle = &motionValuesHandle},
+                              {0}},
+    serviceDefinition {
+      {
+       .type = BLE_GATT_SVC_TYPE_PRIMARY,
+       .uuid = &motionServiceUuid.u,
+       .characteristics = characteristicDefinition
+      },
+      {0},
+    } {
+  // TODO refactor to prevent this loop dependency (service depends on controller and controller depends on service)
+  motionController.SetService(this);
+}
+
+void MotionService::Init() {
+  int res = 0;
+  res = ble_gatts_count_cfg(serviceDefinition);
+  ASSERT(res == 0);
+
+  res = ble_gatts_add_svcs(serviceDefinition);
+  ASSERT(res == 0);
+}
+
+int MotionService::OnStepCountRequested(uint16_t connectionHandle, uint16_t attributeHandle, ble_gatt_access_ctxt* context) {
+  if (attributeHandle == stepCountHandle) {
+    NRF_LOG_INFO("Motion-stepcount : handle = %d", stepCountHandle);
+    uint32_t buffer = motionController.NbSteps();
+
+    int res = os_mbuf_append(context->om, &buffer, 4);
+    return (res == 0) ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+  } else if(attributeHandle == motionValuesHandle) {
+    int16_t buffer[3] = { motionController.X(), motionController.Y(), motionController.Z() };
+
+    int res = os_mbuf_append(context->om, buffer, 3 * sizeof(int16_t));
+    return (res == 0) ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
+  }
+  return 0;
+}
+
+void MotionService::OnNewStepCountValue(uint8_t stepCount) {
+  if(!stepCountNoficationEnabled) return;
+
+  uint32_t buffer = stepCount;
+  auto* om = ble_hs_mbuf_from_flat(&buffer, 4);
+
+  uint16_t connectionHandle = system.nimble().connHandle();
+
+  if (connectionHandle == 0 || connectionHandle == BLE_HS_CONN_HANDLE_NONE) {
+    return;
+  }
+
+  ble_gattc_notify_custom(connectionHandle, stepCountHandle, om);
+}
+void MotionService::OnNewMotionValues(int16_t x, int16_t y, int16_t z) {
+  if(!motionValuesNoficationEnabled) return;
+
+  int16_t buffer[3] = { motionController.X(), motionController.Y(), motionController.Z() };
+  auto* om = ble_hs_mbuf_from_flat(buffer, 3 * sizeof(int16_t));
+
+  uint16_t connectionHandle = system.nimble().connHandle();
+
+  if (connectionHandle == 0 || connectionHandle == BLE_HS_CONN_HANDLE_NONE) {
+    return;
+  }
+
+  ble_gattc_notify_custom(connectionHandle, motionValuesHandle, om);
+}
+
+void MotionService::SubscribeNotification(uint16_t connectionHandle, uint16_t attributeHandle) {
+  if(attributeHandle == stepCountHandle)
+    stepCountNoficationEnabled = true;
+  else if(attributeHandle == motionValuesHandle)
+    motionValuesNoficationEnabled = true;
+}
+
+void MotionService::UnsubscribeNotification(uint16_t connectionHandle, uint16_t attributeHandle) {
+  if(attributeHandle == stepCountHandle)
+    stepCountNoficationEnabled = false;
+  else if(attributeHandle == motionValuesHandle)
+    motionValuesNoficationEnabled = false;
+}
diff --git a/src/components/ble/MotionService.h b/src/components/ble/MotionService.h
new file mode 100644
index 00000000..75ad5182
--- /dev/null
+++ b/src/components/ble/MotionService.h
@@ -0,0 +1,39 @@
+#pragma once
+#define min // workaround: nimble's min/max macros conflict with libstdc++
+#define max
+#include <host/ble_gap.h>
+#include <atomic>
+#undef max
+#undef min
+
+namespace Pinetime {
+  namespace System {
+    class SystemTask;
+  }
+  namespace Controllers {
+    class MotionController;
+    class MotionService {
+    public:
+      MotionService(Pinetime::System::SystemTask& system, Controllers::MotionController& motionController);
+      void Init();
+      int OnStepCountRequested(uint16_t connectionHandle, uint16_t attributeHandle, ble_gatt_access_ctxt* context);
+      void OnNewStepCountValue(uint8_t stepCount);
+      void OnNewMotionValues(int16_t x, int16_t y, int16_t z);
+
+      void SubscribeNotification(uint16_t connectionHandle, uint16_t attributeHandle);
+      void UnsubscribeNotification(uint16_t connectionHandle, uint16_t attributeHandle);
+
+    private:
+      Pinetime::System::SystemTask& system;
+      Controllers::MotionController& motionController;
+
+      struct ble_gatt_chr_def characteristicDefinition[3];
+      struct ble_gatt_svc_def serviceDefinition[2];
+
+      uint16_t stepCountHandle;
+      uint16_t motionValuesHandle;
+      std::atomic_bool stepCountNoficationEnabled {false};
+      std::atomic_bool motionValuesNoficationEnabled {false};
+    };
+  }
+}
diff --git a/src/components/ble/NimbleController.cpp b/src/components/ble/NimbleController.cpp
index 879421e7..1bcae1bc 100644
--- a/src/components/ble/NimbleController.cpp
+++ b/src/components/ble/NimbleController.cpp
@@ -23,7 +23,8 @@ NimbleController::NimbleController(Pinetime::System::SystemTask& systemTask,
                                    Pinetime::Controllers::NotificationManager& notificationManager,
                                    Controllers::Battery& batteryController,
                                    Pinetime::Drivers::SpiNorFlash& spiNorFlash,
-                                   Controllers::HeartRateController& heartRateController)
+                                   Controllers::HeartRateController& heartRateController,
+                                   Controllers::MotionController& motionController)
   : systemTask {systemTask},
     bleController {bleController},
     dateTimeController {dateTimeController},
@@ -39,6 +40,7 @@ NimbleController::NimbleController(Pinetime::System::SystemTask& systemTask,
     batteryInformationService {batteryController},
     immediateAlertService {systemTask, notificationManager},
     heartRateService {systemTask, heartRateController},
+    motionService{systemTask, motionController},
     serviceDiscovery({&currentTimeClient, &alertNotificationClient}) {
 }
 
@@ -81,6 +83,7 @@ void NimbleController::Init() {
   batteryInformationService.Init();
   immediateAlertService.Init();
   heartRateService.Init();
+  motionService.Init();
 
   int rc;
   rc = ble_hs_util_ensure_addr(0);
@@ -215,6 +218,19 @@ int NimbleController::OnGAPEvent(ble_gap_event* event) {
                    event->subscribe.prev_notify,
                    event->subscribe.cur_notify,
                    event->subscribe.prev_indicate);
+
+      if(event->subscribe.reason == BLE_GAP_SUBSCRIBE_REASON_TERM) {
+        heartRateService.UnsubscribeNotification(event->subscribe.conn_handle, event->subscribe.attr_handle);
+        motionService.UnsubscribeNotification(event->subscribe.conn_handle, event->subscribe.attr_handle);
+      }
+      else if(event->subscribe.prev_notify == 0 && event->subscribe.cur_notify == 1) {
+        heartRateService.SubscribeNotification(event->subscribe.conn_handle, event->subscribe.attr_handle);
+        motionService.SubscribeNotification(event->subscribe.conn_handle, event->subscribe.attr_handle);
+      }
+      else if(event->subscribe.prev_notify == 1 && event->subscribe.cur_notify == 0) {
+        heartRateService.UnsubscribeNotification(event->subscribe.conn_handle, event->subscribe.attr_handle);
+        motionService.UnsubscribeNotification(event->subscribe.conn_handle, event->subscribe.attr_handle);
+      }
       break;
 
     case BLE_GAP_EVENT_MTU:
diff --git a/src/components/ble/NimbleController.h b/src/components/ble/NimbleController.h
index 473bb1af..76f89ba8 100644
--- a/src/components/ble/NimbleController.h
+++ b/src/components/ble/NimbleController.h
@@ -19,6 +19,7 @@
 #include "NavigationService.h"
 #include "ServiceDiscovery.h"
 #include "HeartRateService.h"
+#include "MotionService.h"
 
 namespace Pinetime {
   namespace Drivers {
@@ -43,7 +44,8 @@ namespace Pinetime {
                        Pinetime::Controllers::NotificationManager& notificationManager,
                        Controllers::Battery& batteryController,
                        Pinetime::Drivers::SpiNorFlash& spiNorFlash,
-                       Controllers::HeartRateController& heartRateController);
+                       Controllers::HeartRateController& heartRateController,
+                       Controllers::MotionController& motionController);
       void Init();
       void StartAdvertising();
       int OnGAPEvent(ble_gap_event* event);
@@ -95,6 +97,7 @@ namespace Pinetime {
       BatteryInformationService batteryInformationService;
       ImmediateAlertService immediateAlertService;
       HeartRateService heartRateService;
+      MotionService motionService;
 
       uint8_t addrType; // 1 = Random, 0 = PUBLIC
       uint16_t connectionHandle = BLE_HS_CONN_HANDLE_NONE;
diff --git a/src/components/motion/MotionController.cpp b/src/components/motion/MotionController.cpp
index b0dbada4..a2384d79 100644
--- a/src/components/motion/MotionController.cpp
+++ b/src/components/motion/MotionController.cpp
@@ -3,6 +3,14 @@
 using namespace Pinetime::Controllers;
 
 void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps) {
+  if (this->nbSteps != nbSteps && service != nullptr) {
+    service->OnNewStepCountValue(nbSteps);
+  }
+
+  if(service != nullptr && (this->x != x || this->y != y || this->z != z)) {
+    service->OnNewMotionValues(x, y, z);
+  }
+
   this->x = x;
   this->y = y;
   this->z = z;
@@ -41,3 +49,6 @@ void MotionController::Init(Pinetime::Drivers::Bma421::DeviceTypes types) {
     default: this->deviceType = DeviceTypes::Unknown; break;
   }
 }
+void MotionController::SetService(Pinetime::Controllers::MotionService* service) {
+  this->service = service;
+}
diff --git a/src/components/motion/MotionController.h b/src/components/motion/MotionController.h
index ff715093..c72d8a4a 100644
--- a/src/components/motion/MotionController.h
+++ b/src/components/motion/MotionController.h
@@ -2,6 +2,7 @@
 
 #include <cstdint>
 #include <drivers/Bma421.h>
+#include <components/ble/MotionService.h>
 
 namespace Pinetime {
   namespace Controllers {
@@ -39,6 +40,7 @@ namespace Pinetime {
       }
 
       void Init(Pinetime::Drivers::Bma421::DeviceTypes types);
+      void SetService(Pinetime::Controllers::MotionService* service);
 
     private:
       uint32_t nbSteps;
@@ -48,6 +50,7 @@ namespace Pinetime {
       int16_t lastYForWakeUp = 0;
       bool isSensorOk = false;
       DeviceTypes deviceType = DeviceTypes::Unknown;
+      Pinetime::Controllers::MotionService* service = nullptr;
     };
   }
 }
\ No newline at end of file
diff --git a/src/systemtask/SystemTask.cpp b/src/systemtask/SystemTask.cpp
index f1c5165a..e0a5907a 100644
--- a/src/systemtask/SystemTask.cpp
+++ b/src/systemtask/SystemTask.cpp
@@ -101,7 +101,8 @@ SystemTask::SystemTask(Drivers::SpiMaster& spi,
     heartRateApp(heartRateApp),
     fs {fs},
     touchHandler {touchHandler},
-    nimbleController(*this, bleController, dateTimeController, notificationManager, batteryController, spiNorFlash, heartRateController) {
+    nimbleController(*this, bleController, dateTimeController, notificationManager,
+                     batteryController, spiNorFlash, heartRateController, motionController) {
 }
 
 void SystemTask::Start() {