fully implement quaternion by override virtual func

hathach · hathach · commit 5e81644b7bc5 · 2020-02-22T00:03:28.000+07:00
diff --git a/libraries/BLEAdafruitService/src/services/BLEAdafruitQuaternion.cpp b/libraries/BLEAdafruitService/src/services/BLEAdafruitQuaternion.cpp
@@ -77,33 +77,89 @@ err_t BLEAdafruitQuaternion::begin(Adafruit_AHRS_FusionInterface* filter, Adafru
   // Invoke base class begin(), this will add Service, Measurement and Period characteristics
   VERIFY_STATUS( BLEAdafruitSensor::_begin(DEFAULT_PERIOD) );
 
+  // filter timer run faster than measurement timer, but not too fast
+  int32_t const filter_ms = minof(5, DEFAULT_PERIOD / FILTER_MEASURE_RATIO);
+
+  _filter_timer.begin(filter_ms, quaternion_filter_timer_cb, this, true);
+
   return ERROR_NONE;
 }
 
+void BLEAdafruitQuaternion::_notify_cb(uint16_t conn_hdl, uint16_t value)
+{
+  // Start/Stop filter timer
+  if (value & BLE_GATT_HVX_NOTIFICATION)
+  {
+    _filter_timer.start();
+  }else
+  {
+    _filter_timer.stop();
+  }
+
+  // Call SuperClass function
+  BLEAdafruitSensor::_notify_cb(conn_hdl, value);
+}
+
+void BLEAdafruitQuaternion::_update_timer(int32_t ms)
+{
+  int32_t const filter_ms = minof(5, ms / FILTER_MEASURE_RATIO);
+
+  if ( filter_ms < 0 )
+  {
+    _filter_timer.stop();
+  }else if ( filter_ms > 0)
+  {
+    _filter_timer.setPeriod(filter_ms);
+  }else
+  {
+    // Period = 0: keeping the current interval, but report on changes only
+    // Not applicable for this service
+  }
+
+  // Call SuperClass function
+  BLEAdafruitSensor::_update_timer(ms);
+}
+
 // Invoked by period timer in Base class
 // Note invoked in RTOS Timer thread
 void BLEAdafruitQuaternion::_measure_handler(void)
 {
+  float quater[4]; // w, x, y, z
+
+  _filter->getQuaternion(&quater[0], &quater[1], &quater[2], &quater[3]);
+
   // TODO multiple connections
-  _measurement.notify("test", 4);
-
-//  // get sensor
-//  sensors_event_t accel_evt, gyro_evt, mag_evt;
-//
-//  _accel->getEvent(&accel_evt);
-//  _gyro->getEvent(&gyro_evt);
-//  _mag->getEvent(&mag_evt);
-//
-//  // calibrate
-//
-//
-//  // Convert gyro from Rad/s to Degree/s
-//  gyro_evt.gyro.x *= SENSORS_RADS_TO_DPS;
-//  gyro_evt.gyro.y *= SENSORS_RADS_TO_DPS;
-//  gyro_evt.gyro.z *= SENSORS_RADS_TO_DPS;
-//
-//  // apply filter, update 10 times before notify
-//  filter->update(gyro_evt.gyro.x , gyro_evt.gyro.y, gyro_evt.gyro.z,
-//                 accel_evt.acceleration.x, accel_evt.acceleration.y, accel_evt.acceleration.z,
-//                 mag_evt.magnetic.x, mag_evt.magnetic.y, mag_evt.magnetic.z);
+  _measurement.notify(quater, sizeof(quater));
+}
+
+void BLEAdafruitQuaternion::_fitler_update(void)
+{
+  // get sensor
+  sensors_event_t accel_evt, gyro_evt, mag_evt;
+
+  _accel->getEvent(&accel_evt);
+  _gyro->getEvent(&gyro_evt);
+  _mag->getEvent(&mag_evt);
+
+  // calibrate
+
+
+  // Convert gyro from Rad/s to Degree/s
+  gyro_evt.gyro.x *= SENSORS_RADS_TO_DPS;
+  gyro_evt.gyro.y *= SENSORS_RADS_TO_DPS;
+  gyro_evt.gyro.z *= SENSORS_RADS_TO_DPS;
+
+  // apply filter, update 10 times before notify
+  _filter->update(gyro_evt.gyro.x , gyro_evt.gyro.y, gyro_evt.gyro.z,
+                  accel_evt.acceleration.x, accel_evt.acceleration.y, accel_evt.acceleration.z,
+                  mag_evt.magnetic.x, mag_evt.magnetic.y, mag_evt.magnetic.z);
+}
+
+//--------------------------------------------------------------------+
+// Static Methods
+//--------------------------------------------------------------------+
+void BLEAdafruitQuaternion::quaternion_filter_timer_cb(TimerHandle_t xTimer)
+{
+  BLEAdafruitQuaternion* svc = (BLEAdafruitQuaternion*) pvTimerGetTimerID(xTimer);
+  svc->_fitler_update();
 }
diff --git a/libraries/BLEAdafruitService/src/services/BLEAdafruitQuaternion.h b/libraries/BLEAdafruitService/src/services/BLEAdafruitQuaternion.h
@@ -34,12 +34,17 @@ class BLEAdafruitQuaternion : public BLEAdafruitSensor
   public:
     static const uint8_t UUID128_SERVICE[16];
     static const uint8_t UUID128_CHR_DATA[16];
+    static const uint8_t FILTER_MEASURE_RATIO = 10; // number of filter update for each measure report to client
 
     BLEAdafruitQuaternion(void);
     err_t begin(Adafruit_AHRS_FusionInterface* filter, Adafruit_Sensor* accel, Adafruit_Sensor* gyro, Adafruit_Sensor* mag);
 
   protected:
+    virtual void _update_timer(int32_t ms);
     virtual void _measure_handler(void);
+    virtual void _notify_cb(uint16_t conn_hdl, uint16_t value);
+
+    void _fitler_update(void);
 
   private:
     Adafruit_Sensor* _accel;
@@ -48,6 +53,8 @@ class BLEAdafruitQuaternion : public BLEAdafruitSensor
 
     Adafruit_AHRS_FusionInterface* _filter;
     SoftwareTimer _filter_timer;
+
+    static void quaternion_filter_timer_cb(TimerHandle_t xTimer);
 };
 
 #endif /* BLEADAFRUIT_QUATERNION_H_ */
diff --git a/libraries/BLEAdafruitService/src/services/BLEAdafruitSensor.cpp b/libraries/BLEAdafruitService/src/services/BLEAdafruitSensor.cpp
@@ -99,7 +99,6 @@ void BLEAdafruitSensor::_notify_cb(uint16_t conn_hdl, uint16_t value)
 
 void BLEAdafruitSensor::_update_timer(int32_t ms)
 {
-  // TODO handle period = 0 which notify on changes ASAP
   if ( ms < 0 )
   {
     _timer.stop();
diff --git a/libraries/BLEAdafruitService/src/services/BLEAdafruitSensor.h b/libraries/BLEAdafruitService/src/services/BLEAdafruitSensor.h
@@ -50,8 +50,8 @@ class BLEAdafruitSensor : public BLEService
     SoftwareTimer _timer;
 
     err_t _begin(int32_t ms);
-    void _update_timer(int32_t ms);
 
+    virtual void _update_timer(int32_t ms);
     virtual void _measure_handler(void);
     virtual void _notify_cb(uint16_t conn_hdl, uint16_t value);
 
diff --git a/libraries/Bluefruit52Lib/examples/Peripheral/bluefruit_playground/bluefruit_playground.ino b/libraries/Bluefruit52Lib/examples/Peripheral/bluefruit_playground/bluefruit_playground.ino
@@ -123,36 +123,6 @@ uint16_t measure_light(uint8_t* buf, uint16_t bufsize)
   return 4;
 }
 
-//uint16_t measure_gyro(uint8_t* buf, uint16_t bufsize)
-//{
-//  float* float_buf = (float*) buf;
-//
-//  sensors_event_t accel, gyro, temp;
-//  (void) accel; (void) temp;
-//
-//  lsm6ds33.getEvent(&accel, &gyro, &temp);
-//
-//  float_buf[0] = gyro.gyro.x;
-//  float_buf[1] = gyro.gyro.y;
-//  float_buf[2] = gyro.gyro.z;
-//
-//  return 12;
-//}
-//
-//uint16_t measure_magnetic(uint8_t* buf, uint16_t bufsize)
-//{
-//  float* float_buf = (float*) buf;
-//
-//  sensors_event_t mag;
-//  lis3mdl.getEvent(&mag);
-//
-//  float_buf[0] = mag.magnetic.x;
-//  float_buf[1] = mag.magnetic.y;
-//  float_buf[2] = mag.magnetic.z;
-//
-//  return 12;
-//}
-
 uint16_t measure_button(uint8_t* buf, uint16_t bufsize)
 {
   // Button is active LOW on most board except CPlay
@@ -261,12 +231,6 @@ void setup()
   blebas.write(100);
 
   //------------- Adafruit Service -------------//
-#ifdef ARDUINO_NRF52840_CIRCUITPLAY
-  bleTemp.begin(measure_temperature);
-#else
-  bleTemp.begin(bmp280.getTemperatureSensor());
-#endif
-
   bleAccel.begin(accel_sensor);
   bleLight.begin(measure_light);
 
@@ -279,7 +243,11 @@ void setup()
   blePixel.begin(&strip);
 
   // CPB doesn't support these on-board sensor
-#ifndef ARDUINO_NRF52840_CIRCUITPLAY
+#ifdef ARDUINO_NRF52840_CIRCUITPLAY
+  bleTemp.begin(measure_temperature);
+
+#else
+  bleTemp.begin(bmp280.getTemperatureSensor());
   bleHumid.begin(measure_humid);
   bleBaro.begin(bmp280.getPressureSensor());
 

Original file line number	Diff line number	Diff line change
`@@ -99,7 +99,6 @@ void BLEAdafruitSensor::_notify_cb(uint16_t conn_hdl, uint16_t value)`
`99`	`99`
`100`	`100`	`void BLEAdafruitSensor::_update_timer(int32_t ms)`
`101`	`101`	`{`
`102`		`- // TODO handle period = 0 which notify on changes ASAP`
`103`	`102`	`if ( ms < 0 )`
`104`	`103`	`{`
`105`	`104`	`_timer.stop();`