rename _timer_callback to _measure_handler

add _notify_cb

Author: hathach

libraries/BLEAdafruitService/src/services/BLEAdafruitQuaternion.cpp

@@ -60,6 +60,11 @@ BLEAdafruitQuaternion::BLEAdafruitQuaternion(void)
 {
   _accel = _gyro = _mag = NULL;
   _filter = NULL;
+
+  // Setup Measurement Characteristic
+  _measurement.setProperties(CHR_PROPS_READ | CHR_PROPS_NOTIFY);
+  _measurement.setPermission(SECMODE_OPEN, SECMODE_NO_ACCESS);
+  _measurement.setFixedLen(4*4);
 }
 
 err_t BLEAdafruitQuaternion::begin(Adafruit_AHRS_FusionInterface* filter, Adafruit_Sensor* accel, Adafruit_Sensor* gyro, Adafruit_Sensor* mag)
@@ -69,13 +74,36 @@ err_t BLEAdafruitQuaternion::begin(Adafruit_AHRS_FusionInterface* filter, Adafru
   _gyro  = gyro;
   _mag   = mag;
 
-  // Setup Measurement Characteristic
-  _measurement.setProperties(CHR_PROPS_READ | CHR_PROPS_NOTIFY);
-  _measurement.setPermission(SECMODE_OPEN, SECMODE_NO_ACCESS);
-  _measurement.setFixedLen(4*4);
-
   // Invoke base class begin(), this will add Service, Measurement and Period characteristics
-  VERIFY_STATUS( BLEAdafruitSensor::begin(DEFAULT_PERIOD) );
+  VERIFY_STATUS( BLEAdafruitSensor::_begin(DEFAULT_PERIOD) );
 
   return ERROR_NONE;
 }
+
+// Invoked by period timer in Base class
+// Note invoked in RTOS Timer thread
+void BLEAdafruitQuaternion::_measure_handler(void)
+{
+  // 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);
+}

libraries/BLEAdafruitService/src/services/BLEAdafruitQuaternion.h

@@ -38,12 +38,16 @@ class BLEAdafruitQuaternion : public BLEAdafruitSensor
     BLEAdafruitQuaternion(void);
     err_t begin(Adafruit_AHRS_FusionInterface* filter, Adafruit_Sensor* accel, Adafruit_Sensor* gyro, Adafruit_Sensor* mag);
 
+  protected:
+    virtual void _measure_handler(void);
+
   private:
     Adafruit_Sensor* _accel;
     Adafruit_Sensor* _gyro;
     Adafruit_Sensor* _mag;
 
     Adafruit_AHRS_FusionInterface* _filter;
+    SoftwareTimer _filter_timer;
 };
 
 #endif /* BLEADAFRUIT_QUATERNION_H_ */

libraries/BLEAdafruitService/src/services/BLEAdafruitSensor.cpp

@@ -76,6 +76,27 @@ void BLEAdafruitSensor::setPeriod(int32_t period_ms)
   _update_timer(period_ms);
 }
 
+//--------------------------------------------------------------------+
+// Internal API
+//--------------------------------------------------------------------+
+void BLEAdafruitSensor::_notify_cb(uint16_t conn_hdl, uint16_t value)
+{
+  // notify enabled
+  if (value & BLE_GATT_HVX_NOTIFICATION)
+  {
+    _timer.start();
+  }else
+  {
+    _timer.stop();
+  }
+
+  // send initial notification if period = 0
+  //  if ( 0 == svc._period.read32() )
+  //  {
+  //    svc._measurement.notify();
+  //  }
+}
+
 void BLEAdafruitSensor::_update_timer(int32_t ms)
 {
   // TODO handle period = 0 which notify on changes ASAP
@@ -91,7 +112,7 @@ void BLEAdafruitSensor::_update_timer(int32_t ms)
   }
 }
 
-void BLEAdafruitSensor::_timer_callback(void)
+void BLEAdafruitSensor::_measure_handler(void)
 {
   uint16_t len = _measurement.getMaxLen();
   uint8_t buf[len];
@@ -110,11 +131,9 @@ void BLEAdafruitSensor::_timer_callback(void)
     len = _measure_cb(buf, sizeof(buf));
     len = min(len, sizeof(buf));
   }
-  // Invoke internal measure callback
   else
   {
-    len = _internal_measure_cb(buf, sizeof(buf));
-    if (len == 0) return; // nothing to measure
+    return; // nothing to measure
   }
 
   // Period = 0, compare with old data, only update on changes
@@ -138,7 +157,7 @@ void BLEAdafruitSensor::_timer_callback(void)
 void BLEAdafruitSensor::sensor_timer_cb(TimerHandle_t xTimer)
 {
   BLEAdafruitSensor* svc = (BLEAdafruitSensor*) pvTimerGetTimerID(xTimer);
-  svc->_timer_callback();
+  svc->_measure_handler();
 }
 
 // Client update period, adjust timer accordingly
@@ -156,19 +175,6 @@ void BLEAdafruitSensor::sensor_data_cccd_cb(uint16_t conn_hdl, BLECharacteristic
 {
   BLEAdafruitSensor& svc = (BLEAdafruitSensor&) chr->parentService();
 
-  // notify enabled
-  if (value & BLE_GATT_HVX_NOTIFICATION)
-  {
-    svc._timer.start();
-  }else
-  {
-    svc._timer.stop();
-  }
-
-  // send initial notification if period = 0
-//  if ( 0 == svc._period.read32() )
-//  {
-//    svc._measurement.notify();
-//  }
+  svc._notify_cb(conn_hdl, value);
 }
 

libraries/BLEAdafruitService/src/services/BLEAdafruitSensor.h

@@ -51,9 +51,9 @@ class BLEAdafruitSensor : public BLEService
 
     err_t _begin(int32_t ms);
     void _update_timer(int32_t ms);
-    void _timer_callback(void);
 
-    virtual uint16_t _internal_measure_cb(uint8_t* buf, uint16_t bufsize) { return 0; }
+    virtual void _measure_handler(void);
+    virtual void _notify_cb(uint16_t conn_hdl, uint16_t value);
 
     static void sensor_timer_cb(TimerHandle_t xTimer);
     static void sensor_period_write_cb(uint16_t conn_hdl, BLECharacteristic* chr, uint8_t* data, uint16_t len);