adding unified sensor support for sensor service

Author: hathach

libraries/BLEAdafruitService/src/services/BLEAdafruitAccel.cpp

@@ -60,15 +60,15 @@ BLEAdafruitAccel::BLEAdafruitAccel(void)
 
 }
 
-err_t BLEAdafruitAccel::begin (void)
+err_t BLEAdafruitAccel::begin(Adafruit_Sensor* sensor, int32_t ms)
 {
   // Setup Measurement Characteristic
   _measurement.setProperties(CHR_PROPS_READ | CHR_PROPS_NOTIFY);
   _measurement.setPermission(SECMODE_OPEN, SECMODE_NO_ACCESS);
   _measurement.setFixedLen(4*3);
 
   // Invoke base class begin(), this will add Service, Measurement and Period characteristics
-  VERIFY_STATUS( BLEAdafruitSensor::begin(DEFAULT_PERIOD) );
+  VERIFY_STATUS( BLEAdafruitSensor::begin(sensor, ms) );
 
   return ERROR_NONE;
 }

libraries/BLEAdafruitService/src/services/BLEAdafruitAccel.h

@@ -33,7 +33,7 @@ class BLEAdafruitAccel : public BLEAdafruitSensor
     static const int32_t DEFAULT_PERIOD = 1000;
 
     BLEAdafruitAccel(void);
-    virtual err_t begin(void);
+    virtual err_t begin(Adafruit_Sensor* sensor, int32_t ms = DEFAULT_PERIOD);
 };
 
 #endif /* BLEADAFRUIT_ACCEL_H_ */

libraries/BLEAdafruitService/src/services/BLEAdafruitAddressablePixel.h

@@ -25,7 +25,7 @@
 #ifndef BLEADAFRUIT_ADDRESSABLEPIXEL_H_
 #define BLEADAFRUIT_ADDRESSABLEPIXEL_H_
 
-#include "Adafruit_NeoPixel.h"
+#include <Adafruit_NeoPixel.h>
 
 class BLEAdafruitAddressablePixel : public BLEService
 {

libraries/BLEAdafruitService/src/services/BLEAdafruitSensor.cpp

@@ -62,24 +62,18 @@ err_t BLEAdafruitSensor::begin(int32_t ms)
   return ERROR_NONE;
 }
 
-void BLEAdafruitSensor::setPeriod(int32_t period_ms)
+err_t BLEAdafruitSensor::begin(Adafruit_Sensor* sensor, int32_t ms)
 {
-  _period.write32(period_ms);
-  _update_timer(period_ms);
+  _sensor = sensor;
+  return begin(ms);
 }
 
-
-void BLEAdafruitSensor::startMeasuring(void)
-{
-  _timer.start();
-}
-
-void BLEAdafruitSensor::stopMeasuring(void)
+void BLEAdafruitSensor::setPeriod(int32_t period_ms)
 {
-  _timer.stop();
+  _period.write32(period_ms);
+  _update_timer(period_ms);
 }
 
-
 void BLEAdafruitSensor::_update_timer(int32_t ms)
 {
   // TODO handle period = 0 which notify on changes ASAP
@@ -97,24 +91,39 @@ void BLEAdafruitSensor::_update_timer(int32_t ms)
 
 void BLEAdafruitSensor::_timer_callback(void)
 {
-  if (_measure_cb)
+  uint16_t len = _measurement.getMaxLen();
+  uint8_t buf[len];
+
+  // Use unified sensor API if available, only with fixed length sensor
+  if (_sensor && _measurement.isFixedLen())
   {
-    uint8_t buf[_measurement.getMaxLen()];
-    uint16_t len = _measure_cb(buf, sizeof(buf));
-    len = min(len, sizeof(buf));
+    sensors_event_t event;
+    _sensor->getEvent(&event);
 
-    // Period = 0, compare with old data, only update on changes
-    if ( 0 == _period.read32() )
-    {
-      uint8_t prev_buf[_measurement.getMaxLen()];
-      _measurement.read(prev_buf, sizeof(prev_buf));
+    memcpy(buf, event.data, len);
+  }
+  // Else use callback
+  else if (_measure_cb)
+  {
+    len = _measure_cb(buf, sizeof(buf));
+    len = min(len, sizeof(buf));
+  }else
+  {
+    return; // nothing to measure
+  }
 
-      // skip notify if there is no changes
-      if ( 0 == memcmp(prev_buf, buf, len) ) return;
-    }
+  // Period = 0, compare with old data, only update on changes
+  if ( 0 == _period.read32() )
+  {
+    uint8_t prev_buf[_measurement.getMaxLen()];
+    _measurement.read(prev_buf, sizeof(prev_buf));
 
-    _measurement.notify(buf, len);
+    // skip notify if there is no changes
+    if ( 0 == memcmp(prev_buf, buf, len) ) return;
   }
+
+  // TODO multiple connections
+  _measurement.notify(buf, len);
 }
 
 //--------------------------------------------------------------------+

libraries/BLEAdafruitService/src/services/BLEAdafruitSensor.h

@@ -25,6 +25,8 @@
 #ifndef BLEADAFRUIT_SENSOR_H_
 #define BLEADAFRUIT_SENSOR_H_
 
+#include <Adafruit_Sensor.h>
+
 class BLEAdafruitSensor : public BLEService
 {
   public:
@@ -33,16 +35,17 @@ class BLEAdafruitSensor : public BLEService
     BLEAdafruitSensor(BLEUuid service_uuid, BLEUuid data_uuid);
     virtual err_t begin(int32_t ms);
 
+    virtual err_t begin(Adafruit_Sensor* sensor, int32_t ms);
+
     void setMeasureCallback(measure_callback_t fp);
     void setPeriod(int32_t period_ms);
 
-    void startMeasuring(void);
-    void stopMeasuring(void);
-
   protected:
     BLECharacteristic _period;
     BLECharacteristic _measurement;
 
+    Adafruit_Sensor*  _sensor;
+
     measure_callback_t _measure_cb;
     SoftwareTimer _timer;
 

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

@@ -54,17 +54,6 @@ uint16_t measure_temperature(uint8_t* buf, uint16_t bufsize)
   return 4;
 }
 
-uint16_t measure_accel(uint8_t* buf, uint16_t bufsize)
-{  
-  float* accel_buf = (float*) buf;
-
-  accel_buf[0] = CircuitPlayground.motionX();
-  accel_buf[1] = CircuitPlayground.motionY();
-  accel_buf[2] = CircuitPlayground.motionZ();
-
-  return 3*sizeof(float); // 12
-}
-
 uint16_t measure_light(uint8_t* buf, uint16_t bufsize)
 {
   float lux;
@@ -104,6 +93,8 @@ uint16_t measure_button(uint8_t* buf, uint16_t bufsize)
 #include <Adafruit_BMP280.h>
 #include <Adafruit_SHT31.h>
 
+#include <Adafruit_AHRS.h>
+
 #if defined(ARDUINO_NRF52840_CLUE)
   #define DEVICE_NAME     "CLUE"
 #else
@@ -113,40 +104,34 @@ uint16_t measure_button(uint8_t* buf, uint16_t bufsize)
 #define NEOPIXEL_COUNT    1
 
 
-BLEAdafruitGyro     bleGyro;
-BLEAdafruitMagnetic bleMagnetic;
-BLEAdafruitHumid    bleHumid;
-BLEAdafruitBaro     bleBaro;
+//BLEAdafruitGyro     bleGyro;
+//BLEAdafruitMagnetic bleMagnetic;
+
+BLEAdafruitHumid      bleHumid;
+BLEAdafruitBaro       bleBaro;
+
+BLEAdafruitQuaternion bleQuater;
 
-Adafruit_APDS9960 apds9960; // Proximity, Light, Gesture, Color
 Adafruit_LSM6DS33 lsm6ds33; // Gyro and Accel
 Adafruit_LIS3MDL  lis3mdl;  // Magnetometer
+
+Adafruit_APDS9960 apds9960; // Proximity, Light, Gesture, Color
 Adafruit_BMP280   bmp280;   // Temperature, Barometric
 Adafruit_SHT31    sht30;    // Humid
 
+
+// pick your filter! slower == better quality output
+//Adafruit_NXPSensorFusion filter; // slowest
+//Adafruit_Madgwick filter;  // faster than NXP
+Adafruit_Mahony filter;  // fastest/smalleset
+
 uint16_t measure_temperature(uint8_t* buf, uint16_t bufsize)
 {
   float temp = bmp280.readTemperature();
   memcpy(buf, &temp, 4);
   return 4;
 }
 
-uint16_t measure_accel(uint8_t* buf, uint16_t bufsize)
-{
-  float* float_buf = (float*) buf;
-
-  sensors_event_t accel, gyro, temp;
-  (void) gyro; (void) temp;
-
-  lsm6ds33.getEvent(&accel, &gyro, &temp);
-
-  float_buf[0] = accel.acceleration.x;
-  float_buf[1] = accel.acceleration.y;
-  float_buf[2] = accel.acceleration.z;
-
-  return 12;
-}
-
 uint16_t measure_light(uint8_t* buf, uint16_t bufsize)
 {
   float lux;
@@ -226,9 +211,12 @@ Adafruit_NeoPixel strip = Adafruit_NeoPixel(NEOPIXEL_COUNT, PIN_NEOPIXEL, NEO_GR
 //------------- Setup -------------//
 void setup()
 {
+  Adafruit_Sensor* accel_sensor;
+
 #if defined ARDUINO_NRF52840_CIRCUITPLAY
   CircuitPlayground.begin();
 
+  accel_sensor = &CircuitPlayground.lis;
 #else
 
   // Button
@@ -241,10 +229,27 @@ void setup()
   apds9960.begin();
   apds9960.enableColor(true);
 
-  lsm6ds33.begin_I2C();
   bmp280.begin();
-  lis3mdl.begin_I2C();
   sht30.begin(0x44);
+
+  lsm6ds33.begin_I2C();
+  lis3mdl.begin_I2C();
+
+  // set lowest range
+  lsm6ds33.setAccelRange(LSM6DS_ACCEL_RANGE_2_G);
+  lsm6ds33.setGyroRange(LSM6DS_GYRO_RANGE_250_DPS);
+  lis3mdl.setRange(LIS3MDL_RANGE_4_GAUSS);
+
+  // set slightly above refresh rate
+  lsm6ds33.setAccelDataRate(LSM6DS_RATE_104_HZ);
+  lsm6ds33.setGyroDataRate(LSM6DS_RATE_104_HZ);
+  lis3mdl.setDataRate(LIS3MDL_DATARATE_1000_HZ);
+  lis3mdl.setPerformanceMode(LIS3MDL_MEDIUMMODE);
+  lis3mdl.setOperationMode(LIS3MDL_CONTINUOUSMODE);
+
+  filter.begin(100); // sample rate in hz
+
+  accel_sensor = lsm6ds33.getAccelerometerSensor();
 #endif
 
   Serial.begin(115200);
@@ -288,8 +293,7 @@ void setup()
   bleTemp.begin();
   bleTemp.setMeasureCallback(measure_temperature);
 
-  bleAccel.begin();
-  bleAccel.setMeasureCallback(measure_accel);
+  bleAccel.begin(accel_sensor);
 
   bleLight.begin();
   bleLight.setMeasureCallback(measure_light);
@@ -305,17 +309,21 @@ void setup()
 
   // CPB doesn't support these on-board sensor
 #ifndef ARDUINO_NRF52840_CIRCUITPLAY
-  bleGyro.begin();
-  bleGyro.setMeasureCallback(measure_gyro);
-
-  bleMagnetic.begin();
-  bleMagnetic.setMeasureCallback(measure_magnetic);
+//  bleGyro.begin();
+//  bleGyro.setMeasureCallback(measure_gyro);
+//
+//  bleMagnetic.begin();
+//  bleMagnetic.setMeasureCallback(measure_magnetic);
 
   bleHumid.begin();
   bleHumid.setMeasureCallback(measure_humid);
 
   bleBaro.begin();
   bleBaro.setMeasureCallback(measure_baro);
+
+  // Quaternion
+  bleQuater.begin(&filter, accel_sensor, lsm6ds33.getGyroSensor(), &lis3mdl);
+
 #endif
 
   // Set up and start advertising