finish quaternion service

use adacalll instead of updating filter inside timer callback since it
take 6 ms to complete (while the callback is 10 ms), which may cause
lots of lag on other services.

Author: hathach

libraries/BLEAdafruitService/src/services/BLEAdafruitQuaternion.cpp

@@ -24,6 +24,7 @@
 
 #include "BLEAdafruitService.h"
 #include <Adafruit_AHRS.h>
+#include <Adafruit_Sensor_Calibration.h>
 
 //--------------------------------------------------------------------+
 // MACRO TYPEDEF CONSTANT ENUM DECLARATION
@@ -38,7 +39,7 @@
  *    ms between measurements, -1: stop reading, 0: update when changes
  *
  * Quaternion service  0D00
- *  - Quater              0D01 | float[4] | Read + Notify | qw, qx, qy, qz
+ *  - Quater            #include <Adafruit>  0D01 | float[4] | Read + Notify | qw, qx, qy, qz
  *  - Measurement Period  0001
  */
 
@@ -60,6 +61,7 @@ BLEAdafruitQuaternion::BLEAdafruitQuaternion(void)
 {
   _accel = _gyro = _mag = NULL;
   _filter = NULL;
+  _calib = NULL;
 
   // Setup Measurement Characteristic
   _measurement.setProperties(CHR_PROPS_READ | CHR_PROPS_NOTIFY);
@@ -78,13 +80,18 @@ err_t BLEAdafruitQuaternion::begin(Adafruit_AHRS_FusionInterface* filter, Adafru
   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);
+  int32_t const filter_ms = maxof(10, DEFAULT_PERIOD / FILTER_MEASURE_RATIO);
 
   _filter_timer.begin(filter_ms, quaternion_filter_timer_cb, this, true);
 
   return ERROR_NONE;
 }
 
+void BLEAdafruitQuaternion::setCalibration(Adafruit_Sensor_Calibration* calib)
+{
+  _calib = calib;
+}
+
 void BLEAdafruitQuaternion::_notify_cb(uint16_t conn_hdl, uint16_t value)
 {
   // Start/Stop filter timer
@@ -102,7 +109,7 @@ void BLEAdafruitQuaternion::_notify_cb(uint16_t conn_hdl, uint16_t value)
 
 void BLEAdafruitQuaternion::_update_timer(int32_t ms)
 {
-  int32_t const filter_ms = minof(5, ms / FILTER_MEASURE_RATIO);
+  int32_t const filter_ms = maxof(10, ms / FILTER_MEASURE_RATIO);
 
   if ( filter_ms < 0 )
   {
@@ -132,34 +139,53 @@ void BLEAdafruitQuaternion::_measure_handler(void)
   _measurement.notify(quater, sizeof(quater));
 }
 
+// Fusion Filter update
+// This function take ~ 6ms to get all sensor data and computing
 void BLEAdafruitQuaternion::_fitler_update(void)
 {
-  // get sensor
+  // get sensor events
   sensors_event_t accel_evt, gyro_evt, mag_evt;
 
+//  int start_ms = millis();
+
+  _mag->getEvent(&mag_evt);
   _accel->getEvent(&accel_evt);
   _gyro->getEvent(&gyro_evt);
-  _mag->getEvent(&mag_evt);
 
-  // calibrate
+  start_ms = millis() - start_ms;
 
+  // calibrate sensor if available
+  if (_calib)
+  {
+    _calib->calibrate(mag_evt);
+    _calib->calibrate(accel_evt);
+    _calib->calibrate(gyro_evt);
+  }
 
   // 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
+  // apply filter
   _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);
+
+//  PRINT_INT(start_ms);
 }
 
 //--------------------------------------------------------------------+
 // Static Methods
 //--------------------------------------------------------------------+
+
 void BLEAdafruitQuaternion::quaternion_filter_timer_cb(TimerHandle_t xTimer)
 {
   BLEAdafruitQuaternion* svc = (BLEAdafruitQuaternion*) pvTimerGetTimerID(xTimer);
+  ada_callback(NULL, 0, quaternion_filter_update_dfr, svc);
+}
+
+void BLEAdafruitQuaternion::quaternion_filter_update_dfr(BLEAdafruitQuaternion* svc)
+{
   svc->_fitler_update();
 }

libraries/BLEAdafruitService/src/services/BLEAdafruitQuaternion.h

@@ -25,9 +25,9 @@
 #ifndef BLEADAFRUIT_QUATERNION_H_
 #define BLEADAFRUIT_QUATERNION_H_
 
-#include <Adafruit_Sensor.h>
-
+// forward declaration
 class Adafruit_AHRS_FusionInterface;
+class Adafruit_Sensor_Calibration;
 
 class BLEAdafruitQuaternion : public BLEAdafruitSensor
 {
@@ -38,6 +38,7 @@ class BLEAdafruitQuaternion : public BLEAdafruitSensor
 
     BLEAdafruitQuaternion(void);
     err_t begin(Adafruit_AHRS_FusionInterface* filter, Adafruit_Sensor* accel, Adafruit_Sensor* gyro, Adafruit_Sensor* mag);
+    void setCalibration(Adafruit_Sensor_Calibration* calib);
 
   protected:
     virtual void _update_timer(int32_t ms);
@@ -52,9 +53,14 @@ class BLEAdafruitQuaternion : public BLEAdafruitSensor
     Adafruit_Sensor* _mag;
 
     Adafruit_AHRS_FusionInterface* _filter;
+    Adafruit_Sensor_Calibration* _calib;
     SoftwareTimer _filter_timer;
 
+    // filter timer callback, 10x faster than period timer
     static void quaternion_filter_timer_cb(TimerHandle_t xTimer);
+
+    // filter update deferred to adacallback since it takes lots of computing time (6 ms)
+    static void quaternion_filter_update_dfr(BLEAdafruitQuaternion* svc);
 };
 
 #endif /* BLEADAFRUIT_QUATERNION_H_ */

libraries/BLEAdafruitService/src/services/BLEAdafruitSensor.cpp

@@ -162,18 +162,19 @@ void BLEAdafruitSensor::sensor_timer_cb(TimerHandle_t xTimer)
 // Client update period, adjust timer accordingly
 void BLEAdafruitSensor::sensor_period_write_cb(uint16_t conn_hdl, BLECharacteristic* chr, uint8_t* data, uint16_t len)
 {
-  BLEAdafruitSensor& svc = (BLEAdafruitSensor&) chr->parentService();
+  (void) conn_hdl;
+  BLEAdafruitSensor* svc = (BLEAdafruitSensor*) &chr->parentService();
 
   int32_t ms = 0;
   memcpy(&ms, data, len);
 
-  svc._update_timer(ms);
+  svc->_update_timer(ms);
 }
 
 void BLEAdafruitSensor::sensor_data_cccd_cb(uint16_t conn_hdl, BLECharacteristic* chr, uint16_t value)
 {
-  BLEAdafruitSensor& svc = (BLEAdafruitSensor&) chr->parentService();
+  BLEAdafruitSensor* svc = (BLEAdafruitSensor*) &chr->parentService();
 
-  svc._notify_cb(conn_hdl, value);
+  svc->_notify_cb(conn_hdl, value);
 }
 

libraries/Bluefruit52Lib/examples/Peripheral/bluefruit_playground/bluefruit_playground.ino

@@ -19,6 +19,8 @@
  *  - Feather Sense : https://www.adafruit.com/product/4516
  */
 
+#include <SPI.h>
+#include <SdFat.h>
 #include <Adafruit_LittleFS.h>
 #include <InternalFileSystem.h>
 #include <bluefruit.h>
@@ -83,7 +85,9 @@ uint16_t measure_button(uint8_t* buf, uint16_t bufsize)
 #include <Adafruit_BMP280.h>
 #include <Adafruit_SHT31.h>
 
+#include <Adafruit_SPIFlash.h>
 #include <Adafruit_AHRS.h>
+#include <Adafruit_Sensor_Calibration.h>
 
 #if defined(ARDUINO_NRF52840_CLUE)
   #define DEVICE_NAME     "CLUE"
@@ -110,6 +114,14 @@ Adafruit_SHT31    sht30;    // Humid
 //Adafruit_Madgwick filter;  // faster than NXP
 Adafruit_Mahony filter;  // fastest/smalleset
 
+// Sensor calibration
+#define FILE_SENSOR_CALIB       "sensor_calib.json"
+Adafruit_Sensor_Calibration_SDFat cal;
+
+Adafruit_FlashTransport_QSPI flashTransport;
+Adafruit_SPIFlash flash(&flashTransport);
+FatFileSystem fatfs;
+
 uint16_t measure_light(uint8_t* buf, uint16_t bufsize)
 {
   float lux;
@@ -154,6 +166,9 @@ void setup()
 {
   Adafruit_Sensor* accel_sensor;
 
+  Serial.begin(115200);
+//  while(!Serial) delay(10); // wait for native USB
+
 #if defined ARDUINO_NRF52840_CIRCUITPLAY
   CircuitPlayground.begin();
 
@@ -191,10 +206,13 @@ void setup()
   filter.begin(100); // sample rate in hz
 
   accel_sensor = lsm6ds33.getAccelerometerSensor();
-#endif
 
-  Serial.begin(115200);
-//  while(!Serial) delay(10); // wait for native USB
+  // Init flash, filesystem and calibration & load calib json
+  flash.begin();
+  fatfs.begin(&flash);
+  cal.begin(FILE_SENSOR_CALIB, &fatfs);
+  cal.loadCalibration();
+#endif
 
   Serial.println("Bluefruit Playground Example");
   Serial.println("---------------------------\n");
@@ -251,8 +269,10 @@ void setup()
   bleHumid.begin(measure_humid);
   bleBaro.begin(bmp280.getPressureSensor());
 
-  // Quaternion
+  // Quaternion with sensor calibration
+//  PRINT_HEX(lsm6ds33.getGyroSensor());
   bleQuater.begin(&filter, accel_sensor, lsm6ds33.getGyroSensor(), &lis3mdl);
+  bleQuater.setCalibration(&cal);
 #endif
 
   // Set up and start advertising