Motion: Expose acceleration FIFO via BLE

The motion BLE service now broadcasts the entire FIFO buffer,
in order to expose a higher frequency measurement.

A high enough MTU has to be negotiated to fit all max. 192 bytes.
The format is backwards-compatible.

Author: StarGate01

src/components/ble/MotionService.cpp

@@ -8,10 +8,8 @@ using namespace Pinetime::Controllers;
 namespace {
   // 0003yyxx-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 }
-    };
+    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}};
   }
 
   // 00030000-78fc-48fe-8e23-433b3a1942d0
@@ -45,11 +43,7 @@ MotionService::MotionService(Pinetime::System::SystemTask& system, Controllers::
                                .val_handle = &motionValuesHandle},
                               {0}},
     serviceDefinition {
-      {
-       .type = BLE_GATT_SVC_TYPE_PRIMARY,
-       .uuid = &motionServiceUuid.u,
-       .characteristics = characteristicDefinition
-      },
+      {.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)
@@ -72,8 +66,8 @@ int MotionService::OnStepCountRequested(uint16_t connectionHandle, uint16_t attr
 
     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() };
+  } 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;
@@ -82,7 +76,8 @@ int MotionService::OnStepCountRequested(uint16_t connectionHandle, uint16_t attr
 }
 
 void MotionService::OnNewStepCountValue(uint32_t stepCount) {
-  if(!stepCountNoficationEnabled) return;
+  if (!stepCountNoficationEnabled)
+    return;
 
   uint32_t buffer = stepCount;
   auto* om = ble_hs_mbuf_from_flat(&buffer, 4);
@@ -95,31 +90,33 @@ void MotionService::OnNewStepCountValue(uint32_t stepCount) {
 
   ble_gattc_notify_custom(connectionHandle, stepCountHandle, om);
 }
-void MotionService::OnNewMotionValues(int16_t x, int16_t y, int16_t z) {
-  if(!motionValuesNoficationEnabled) return;
+void MotionService::OnNewMotionValues(int16_t* samples, uint16_t samples_length) {
+  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));
+  if (samples_length > 0 && samples != nullptr) {
+    auto* om = ble_hs_mbuf_from_flat(samples, samples_length * 3 * sizeof(int16_t));
 
-  uint16_t connectionHandle = system.nimble().connHandle();
+    uint16_t connectionHandle = system.nimble().connHandle();
 
-  if (connectionHandle == 0 || connectionHandle == BLE_HS_CONN_HANDLE_NONE) {
-    return;
-  }
+    if (connectionHandle == 0 || connectionHandle == BLE_HS_CONN_HANDLE_NONE) {
+      return;
+    }
 
-  ble_gattc_notify_custom(connectionHandle, motionValuesHandle, om);
+    ble_gattc_notify_custom(connectionHandle, motionValuesHandle, om);
+  }
 }
 
 void MotionService::SubscribeNotification(uint16_t connectionHandle, uint16_t attributeHandle) {
-  if(attributeHandle == stepCountHandle)
+  if (attributeHandle == stepCountHandle)
     stepCountNoficationEnabled = true;
-  else if(attributeHandle == motionValuesHandle)
+  else if (attributeHandle == motionValuesHandle)
     motionValuesNoficationEnabled = true;
 }
 
 void MotionService::UnsubscribeNotification(uint16_t connectionHandle, uint16_t attributeHandle) {
-  if(attributeHandle == stepCountHandle)
+  if (attributeHandle == stepCountHandle)
     stepCountNoficationEnabled = false;
-  else if(attributeHandle == motionValuesHandle)
+  else if (attributeHandle == motionValuesHandle)
     motionValuesNoficationEnabled = false;
 }

src/components/ble/MotionService.h

@@ -18,7 +18,7 @@ namespace Pinetime {
       void Init();
       int OnStepCountRequested(uint16_t connectionHandle, uint16_t attributeHandle, ble_gatt_access_ctxt* context);
       void OnNewStepCountValue(uint32_t stepCount);
-      void OnNewMotionValues(int16_t x, int16_t y, int16_t z);
+      void OnNewMotionValues(int16_t* samples, uint16_t samples_length);
 
       void SubscribeNotification(uint16_t connectionHandle, uint16_t attributeHandle);
       void UnsubscribeNotification(uint16_t connectionHandle, uint16_t attributeHandle);

src/components/motion/MotionController.cpp

@@ -2,13 +2,13 @@
 #include "os/os_cputime.h"
 using namespace Pinetime::Controllers;
 
-void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps) {
+void MotionController::Update(uint32_t nbSteps, int16_t x, int16_t y, int16_t z, int16_t* samples, uint16_t samples_length) {
   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);
+    service->OnNewMotionValues(samples, samples_length);
   }
 
   this->x = x;

src/components/motion/MotionController.h

@@ -8,7 +8,7 @@ namespace Pinetime {
   namespace Controllers {
     class MotionController {
     public:
-      void Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps);
+      void Update(uint32_t nbSteps, int16_t x, int16_t y, int16_t z, int16_t* samples, uint16_t samples_length);
 
       int16_t X() const {
         return x;

src/systemtask/SystemTask.cpp

@@ -480,11 +480,6 @@ void SystemTask::UpdateMotion() {
     return;
   }
 
-  if (state == SystemTaskState::Sleeping && !(settingsController.isWakeUpModeOn(Pinetime::Controllers::Settings::WakeUpMode::RaiseWrist) ||
-                                              settingsController.isWakeUpModeOn(Pinetime::Controllers::Settings::WakeUpMode::Shake))) {
-    return;
-  }
-
   if (stepCounterMustBeReset) {
     motionSensor.ResetStepCounter();
     stepCounterMustBeReset = false;
@@ -493,7 +488,8 @@ void SystemTask::UpdateMotion() {
   auto motionValues = motionSensor.Process();
 
   motionController.IsSensorOk(motionSensor.IsInitialized());
-  motionController.Update(motionValues.x, motionValues.y, motionValues.z, motionValues.steps);
+  motionController.Update(
+    motionValues.steps, motionValues.x, motionValues.y, motionValues.z, motionValues.samples, motionValues.samples_length);
 
   if (settingsController.isWakeUpModeOn(Pinetime::Controllers::Settings::WakeUpMode::RaiseWrist) &&
       motionController.Should_RaiseWake(state == SystemTaskState::Sleeping)) {