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

@@ -95,19 +95,20 @@ 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) {
+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) {

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,14 @@
 #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;
@@ -69,18 +70,8 @@ int32_t MotionController::currentShakeSpeed() {
 void MotionController::IsSensorOk(bool isOk) {
   isSensorOk = isOk;
 }
-void MotionController::Init(Pinetime::Drivers::Bma421::DeviceTypes types) {
-  switch (types) {
-    case Drivers::Bma421::DeviceTypes::BMA421:
-      this->deviceType = DeviceTypes::BMA421;
-      break;
-    case Drivers::Bma421::DeviceTypes::BMA425:
-      this->deviceType = DeviceTypes::BMA425;
-      break;
-    default:
-      this->deviceType = DeviceTypes::Unknown;
-      break;
-  }
+void MotionController::Init(Pinetime::Drivers::AccelerationDeviceTypes types) {
+  this->deviceType = types;
 }
 void MotionController::SetService(Pinetime::Controllers::MotionService* service) {
   this->service = service;

src/components/motion/MotionController.h

@@ -9,7 +9,8 @@ namespace Pinetime {
     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

@@ -481,11 +481,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;
@@ -494,6 +489,8 @@ void SystemTask::UpdateMotion() {
   auto motionValues = motionSensor.Process();
 
   motionController.IsSensorOk(motionSensor.IsInitialized());
+  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)) {