Diff: MPU6050Service.h

Revision:

9:6a28d9c0e486

Parent:

7:af3e2b9c137a

Child:

10:b305e261e7d1

--- a/MPU6050Service.h	Mon Jul 13 15:33:09 2015 +0000
+++ b/MPU6050Service.h	Wed Jul 22 16:03:03 2015 +0000
@@ -3,67 +3,73 @@
 #define __MPU6050_SERVICE_H__
 
 #include "BLE.h"
-
-typedef struct
-{
-    uint16_t    a1, a2, a3;
-    uint16_t    temp;
-    uint16_t    g1, g2, g3;
-} mpu6050SensorReading_t;
-
-typedef struct
-{
-    mpu6050SensorReading_t  data;
-    uint32_t                clock_us; // ~ 1h 11min 34s 967ms 296us
-                                      // (so we have to resynchronize our clock approx. every hour)
-} mpu6050SensorData_t;
-
-typedef enum
-{
-    ACCEL_RANGE_2G  = 0x00, // 0b__00
-    ACCEL_RANGE_4G  = 0x01, // 0b__01
-    ACCEL_RANGE_8G  = 0x02, // 0b__10
-    ACCEL_RANGE_16G = 0x03, // 0b__11
-    
-    GYRO_RANGE_250  = 0x00, // 0b00__
-    GYRO_RANGE_500  = 0x04, // 0b01__
-    GYRO_RANGE_1000 = 0x08, // 0b10__
-    GYRO_RANGE_2000 = 0x0C  // 0b11__
-} mpu6050MeasuringRange_t;
-
-inline mpu6050MeasuringRange_t operator|(mpu6050MeasuringRange_t a, mpu6050MeasuringRange_t b) { return static_cast<mpu6050MeasuringRange_t>(static_cast<int>(a) | static_cast<int>(b)); }
+#include "MPU6050.h"
 
 class MPU6050Service
 {
     public:        
         enum
         {
-            UUID_MP6050_SERVICE                 = 0xA123,
+            UUID_MPU6050_SERVICE                = 0xA123,
             
             UUID_MPU6050_SENSOR_DATA_CHAR       = 0xA124,
             UUID_MPU6050_MEASURING_RANGE_CHAR   = 0xA125,
             UUID_MPU6050_MASTER_CLOCK_CHAR      = 0xA126
         };
         
+        enum MeasuringRange
+        {
+            ACCEL_RANGE_2G                      = 0x00, // 0b__00
+            ACCEL_RANGE_4G                      = 0x01, // 0b__01
+            ACCEL_RANGE_8G                      = 0x02, // 0b__10
+            ACCEL_RANGE_16G                     = 0x03, // 0b__11
+            
+            GYRO_RANGE_250                      = 0x00, // 0b00__
+            GYRO_RANGE_500                      = 0x04, // 0b01__
+            GYRO_RANGE_1000                     = 0x08, // 0b10__
+            GYRO_RANGE_2000                     = 0x0C  // 0b11__
+        };
+        friend inline MeasuringRange operator|(MeasuringRange a, MeasuringRange b) { return static_cast<MeasuringRange>(static_cast<int>(a) | static_cast<int>(b)); }
+        
+        typedef struct
+        {
+            int16_t     accelX, accelY, accelZ;
+            
+            int16_t     temperature;
+            
+            int16_t     gyroX, gyroY, gyroZ;
+            
+            uint32_t    clock_us;
+        } SensorData;
+        
         MPU6050Service
         (
-            BLE                            &ble, 
-            const mpu6050SensorData_t      *initialData         = NULL, 
-            const mpu6050MeasuringRange_t   measuringRange      = ACCEL_RANGE_2G | GYRO_RANGE_250, 
-            const uint32_t                  masterClock_us      = 0
+            BLE                    &ble,
+            MPU6050                &mpu6050,
+            const SensorData       *initialData         = NULL, 
+            const MeasuringRange    measuringRange      = ACCEL_RANGE_2G | GYRO_RANGE_250, 
+            const uint32_t          masterClock_us      = 0
         ) :
             gattServer(ble.gattServer()),
+            mpu(mpu6050),
+            timeAccumulationTicker(),
+            
+            _isRunning(false),
+            updateSensorDataCharacteristicTrigger(false),
+            
+            time(masterClock_us),
+            
             sensorDataCharacteristic
             (
                 UUID_MPU6050_SENSOR_DATA_CHAR,
-                (mpu6050SensorData_t*)initialData,
+                (SensorData*)initialData,
                 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY // improving latency by repurposing notifications and "writing" the client
                                                                     // (i.e. indicating the client but not wait for a response)
             ),
             measuringRangeCharacteristic
             (
                 UUID_MPU6050_MEASURING_RANGE_CHAR,
-                (mpu6050MeasuringRange_t*)&measuringRange,
+                (MeasuringRange*)&measuringRange,
                 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE // also improving latency 
             ),
             masterClockCharacteristic
@@ -76,6 +82,8 @@
             static bool serviceAdded = false;
             if (serviceAdded)
                 return;
+             
+            timeAccumulationTicker.attach(&MPU6050::timeAccumulationPeriodicCallback, 1.0f / (float)(1000 * 1000 * timerResolution_us));
                 
             GattCharacteristic *charTable[] =
             {
@@ -84,7 +92,7 @@
                 &masterClockCharacteristic
             };
             
-            GattService mpu6050Service(UUID_MP6050_SERVICE, charTable, sizeof(charTable) / sizeof(GattCharacteristic*));
+            GattService mpu6050Service(UUID_MPU6050_SERVICE, charTable, sizeof(charTable) / sizeof(GattCharacteristic*));
             
             gattServer.addService(mpu6050Service);
             gattServer.onDataSent(this, &MPU6050Service::dataSentCallback);
@@ -93,43 +101,71 @@
             serviceAdded = true;
         }
         
-        void start()
+        bool isRunning()
         {
+            return _isRunning;
+        }
+        
+        bool start()
+        {            
+            if (!deviceReset())
+            {
+                _isRunning = false;
+                return false;
+            }
+                
+            _isRunning = true;
+                
             // sending data once will invoke the dataSent(...) callback later on
-            // which will be looped by the event driven behavior of the BLE API (because it will send data again on it's own)
-            updateSensorDataCharacteristic();
+            // which will be looped by the event driven behavior of the BLE API (because it will send data again on it's own and so on)
+            updateSensorDataCharacteristicTrigger = true;
+            
+            return true;
+        }
+        
+        void stop()
+        {
+            _isRunning = false;
+        }
+        
+        void handleService()
+        {
+            if (_isRunning && updateSensorDataCharacteristicTrigger)
+                updateSensorDataCharacteristic();
         }
         
     private:
+        static const uint32_t                                   timerResolution_us                      = 100;
+    
         GattServer                                             &gattServer; 
+        MPU6050                                                &mpu;
+        Ticker                                                  timeAccumulationTicker;
         
-        ReadOnlyGattCharacteristic<mpu6050SensorData_t>         sensorDataCharacteristic;
-        WriteOnlyGattCharacteristic<mpu6050MeasuringRange_t>    measuringRangeCharacteristic;
+        volatile bool                                           _isRunning;
+        bool                                                    updateSensorDataCharacteristicTrigger;
+        
+        uint32_t                                                time;
+        
+        ReadOnlyGattCharacteristic<SensorData>                  sensorDataCharacteristic;
+        ReadWriteGattCharacteristic<MeasuringRange>             measuringRangeCharacteristic;
         WriteOnlyGattCharacteristic<uint32_t>                   masterClockCharacteristic;
         
         union
         {
             struct
             {
-                uint8_t accel_x_h,  accel_x_l;
-                uint8_t accel_y_h,  accel_y_l;
-                uint8_t accel_z_h,  accel_z_l;
-                
-                uint8_t temp_h,     temp_l;
-                
-                uint8_t gyro_x_h,   gyro_x_l;
-                uint8_t gyro_y_h,   gyro_y_l;
-                uint8_t gyro_z_h,   gyro_z_l;
-            } registers;
+                MPU6050::SensorReading  reading;
+                uint32_t                clock_us;
+            }           input;
             
-            mpu6050SensorReading_t data;
+            SensorData  data;
         } converter;
         
         void dataSentCallback(unsigned count)
         {
             // we came here, if we just sent data, 
             // so lets keep the BLE stack busy by sending data again
-            updateSensorDataCharacteristic();
+            updateSensorDataCharacteristicTrigger = true;
         }
         
         void dataWrittenCallback(const GattWriteCallbackParams *context)
@@ -137,25 +173,41 @@
             // some writable characteristics were updated by the client
         }
         
+        void timeAccumulationPeriodicCallback()
+        {
+            // we do this here to achieve a better resolution
+            time += timerResolution_us;
+        }
+        
         void updateSensorDataCharacteristic()
-        {         
-            mpu6050SensorData_t data;                       
+        {                              
             // flood the transmission buffer of the BLE stack until it's full
             do
-            {          
-                // TODO: poll and accumulate data
-                
-                // ...
-                
-                data.data = converter.data;
-                data.clock_us = 0;  
+            {   
+                if (mpu.getMotionAndTemperature(&(converter.input.reading)))
+                {
+                    if (!gattServer.write(sensorDataCharacteristic.getValueHandle(), reinterpret_cast<uint8_t*>(&(converter.data)), sizeof(SensorData)) == BLE_ERROR_NONE)
+                        break;
+                }                 
                 // write data in characteristic and repeat
-            } while (gattServer.write(sensorDataCharacteristic.getValueHandle(), reinterpret_cast<uint8_t*>(&data), sizeof(mpu6050SensorData_t)) == BLE_ERROR_NONE);
+            } while (true);
             
             // error case or buffer is full (or break out of loop);
             // this also means the last data update will be lost, if not sent again 
             // (we are ignoring this fact and just keep on going, because we're NOT interested in the QUALITY of data, but in the QUANTITY
         }
+        
+        bool deviceReset()
+        {
+            uint8_t id;
+            
+            return 
+                mpu.getDeviceId(&id) &&
+                (id == MPU6050::MPU6050_ID) &&
+                mpu.reset() &&
+                mpu.setSleepEnabled(false) &&
+                mpu.setFIFOEnabled(false);                
+        }
 };
 
 #endif
\ No newline at end of file