finish quaternion service

Author: hathach

cores/nRF5/common_func.h

@@ -145,6 +145,7 @@ const char* dbg_err_str(int32_t err_id); // TODO move to other place
 #define PRTNT_HEAP()          if (CFG_DEBUG == 3) PRINTF("\n%s: %d: Heap free: %d\n", __FUNCTION__, __LINE__, util_heap_get_free_size())
 #define PRINT_STR(x)          PRINTF("%s: %d: " #x " = %s\n"   , __FUNCTION__, __LINE__, (char*)(x) )
 #define PRINT_INT(x)          PRINTF("%s: %d: " #x " = %ld\n"  , __FUNCTION__, __LINE__, (uint32_t) (x) )
+#define PRINT_FLOAT(x)        PRINTF("%s: %d: " #x " = %f\n"  , __FUNCTION__, __LINE__, (float) (x) )
 
 #define PRINT_HEX(x) \
   do {\
@@ -184,6 +185,7 @@ const char* dbg_err_str(int32_t err_id); // TODO move to other place
 #define PRINT_STR(x)
 #define PRINT_INT(x)
 #define PRINT_HEX(x)
+#define PRINT_FLOAT(x)
 #define PRINT_BUFFER(buf, n)
 #define ADALOG(...)
 

libraries/BLEAdafruitService/src/services/BLEAdafruitQuaternion.cpp

@@ -76,13 +76,16 @@ err_t BLEAdafruitQuaternion::begin(Adafruit_AHRS_FusionInterface* filter, Adafru
   _gyro  = gyro;
   _mag   = mag;
 
+  int32_t const period_ms = 100;
+
   // Invoke base class begin(), this will add Service, Measurement and Period characteristics
-  VERIFY_STATUS( BLEAdafruitSensor::_begin(DEFAULT_PERIOD) );
+  VERIFY_STATUS( BLEAdafruitSensor::_begin(period_ms) );
 
   // filter timer run faster than measurement timer, but not too fast
-  int32_t const filter_ms = maxof(10, DEFAULT_PERIOD / FILTER_MEASURE_RATIO);
+  int32_t const filter_ms = maxof(10, period_ms / FUSION_MEASURE_RATIO);
 
   _filter_timer.begin(filter_ms, quaternion_filter_timer_cb, this, true);
+  _filter->begin(1000.0f/filter_ms);
 
   return ERROR_NONE;
 }
@@ -109,14 +112,15 @@ void BLEAdafruitQuaternion::_notify_cb(uint16_t conn_hdl, uint16_t value)
 
 void BLEAdafruitQuaternion::_update_timer(int32_t ms)
 {
-  int32_t const filter_ms = maxof(10, ms / FILTER_MEASURE_RATIO);
+  int32_t const filter_ms = maxof(10, ms / FUSION_MEASURE_RATIO);
 
   if ( filter_ms < 0 )
   {
     _filter_timer.stop();
   }else if ( filter_ms > 0)
   {
     _filter_timer.setPeriod(filter_ms);
+    _filter->begin(1000.0f/filter_ms); // need to also change the sample frequency
   }else
   {
     // Period = 0: keeping the current interval, but report on changes only
@@ -127,6 +131,11 @@ void BLEAdafruitQuaternion::_update_timer(int32_t ms)
   BLEAdafruitSensor::_update_timer(ms);
 }
 
+static void print_quaternion(float quater[4])
+{
+  Serial.printf("Quaternion: %.04f, %.04f, %.04f, %.04f\n", quater[0], quater[1], quater[2], quater[3]);
+}
+
 // Invoked by period timer in Base class
 // Note invoked in RTOS Timer thread
 void BLEAdafruitQuaternion::_measure_handler(void)
@@ -138,29 +147,28 @@ void BLEAdafruitQuaternion::_measure_handler(void)
   // TODO multiple connections
   _measurement.notify(quater, sizeof(quater));
 
-//  Serial.printf("Orientation: %.02f, %.02f, %.02f\n", _filter->getYaw(), _filter->getPitch(), _filter->getRoll());
-//  Serial.printf("Quaternion: %.04f, %.04f, %.04f, %.04f\n", quater[0], quater[1], quater[2], quater[3]);
+  ada_callback(quater, sizeof(quater), print_quaternion, quater);
 }
 
 // Fusion Filter update
-// This function take ~ 6ms to get all sensor data (computing time is not much)
+// This function take ~ 1-2 ms to get all sensor data
 void BLEAdafruitQuaternion::_fitler_update(void)
 {
   // 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 sensor if available
   if (_calib)
   {
-    _calib->calibrate(mag_evt);
     _calib->calibrate(accel_evt);
     _calib->calibrate(gyro_evt);
+    _calib->calibrate(mag_evt);
   }
 
   // Convert gyro from Rad/s to Degree/s
@@ -184,10 +192,6 @@ void BLEAdafruitQuaternion::_fitler_update(void)
 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

@@ -34,7 +34,7 @@ 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
+    static const uint8_t FUSION_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);
@@ -58,9 +58,6 @@ class BLEAdafruitQuaternion : public BLEAdafruitSensor
 
     // filter timer callback, 10x faster than period timer
     static void quaternion_filter_timer_cb(TimerHandle_t xTimer);
-
-    // filter update deferred to ada callback since it takes lots of time to get 3 sensor data
-    static void quaternion_filter_update_dfr(BLEAdafruitQuaternion* svc);
 };
 
 #endif /* BLEADAFRUIT_QUATERNION_H_ */

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

@@ -187,7 +187,6 @@ void setup()
 
   bmp280.begin();
   sht30.begin(0x44);
-
   lsm6ds33.begin_I2C();
   lis3mdl.begin_I2C();
 
@@ -203,7 +202,8 @@ void setup()
   lis3mdl.setPerformanceMode(LIS3MDL_MEDIUMMODE);
   lis3mdl.setOperationMode(LIS3MDL_CONTINUOUSMODE);
 
-  filter.begin(100); // sample rate in hz
+  // Increase I2C speed to 400 Khz
+  Wire.setClock(400000);
 
   accel_sensor = lsm6ds33.getAccelerometerSensor();
 
@@ -212,6 +212,13 @@ void setup()
   fatfs.begin(&flash);
   cal.begin(FILE_SENSOR_CALIB, &fatfs);
   cal.loadCalibration();
+
+  PRINT_FLOAT(cal.mag_hardiron[0]); PRINT_FLOAT(cal.mag_hardiron[1]); PRINT_FLOAT(cal.mag_hardiron[2]);
+  PRINT_FLOAT(cal.mag_field);
+
+  PRINT_FLOAT(cal.mag_softiron[0]);PRINT_FLOAT(cal.mag_softiron[1]); PRINT_FLOAT(cal.mag_softiron[2]);
+  PRINT_FLOAT(cal.mag_softiron[3]); PRINT_FLOAT(cal.mag_softiron[4]); PRINT_FLOAT(cal.mag_softiron[5]);
+  PRINT_FLOAT(cal.mag_softiron[6]); PRINT_FLOAT(cal.mag_softiron[7]); PRINT_FLOAT(cal.mag_softiron[8]);
 #endif
 
   Serial.println("Bluefruit Playground Example");