MPU6050Service.h

00001 #pragma once
00002 #ifndef __MPU6050_SERVICE_H__
00003 #define __MPU6050_SERVICE_H__
00004 
00005 #include "BLE.h"
00006 #include "MPU6050.h"
00007 
00008 class MPU6050Service
00009 {
00010     public:        
00011         enum
00012         {
00013             UUID_MPU6050_SERVICE                = 0xA123,
00014             
00015             UUID_MPU6050_SENSOR_DATA_CHAR       = 0xA124,
00016             UUID_MPU6050_MOTION_INDICATOR_CHAR  = 0xA125,
00017             UUID_MPU6050_MEASURING_RANGE_CHAR   = 0xA126,
00018             UUID_MPU6050_MASTER_CLOCK_CHAR      = 0xA127
00019         };
00020         
00021         enum MeasuringRange
00022         {
00023             ACCEL_RANGE_2G                      = 0x00, // 0b__00
00024             ACCEL_RANGE_4G                      = 0x01, // 0b__01
00025             ACCEL_RANGE_8G                      = 0x02, // 0b__10
00026             ACCEL_RANGE_16G                     = 0x03, // 0b__11
00027             
00028             ACCEL_RANGE_MASK                    = 0x03, // 0b__11
00029             
00030             GYRO_RANGE_250                      = 0x00, // 0b00__
00031             GYRO_RANGE_500                      = 0x04, // 0b01__
00032             GYRO_RANGE_1000                     = 0x08, // 0b10__
00033             GYRO_RANGE_2000                     = 0x0C, // 0b11__
00034             
00035             GYRO_RANGE_MASK                     = 0x0C  // 0b11__
00036         };
00037         friend inline MeasuringRange operator|(MeasuringRange a, MeasuringRange b) { return static_cast<MeasuringRange>(static_cast<int>(a) | static_cast<int>(b)); }
00038         friend inline MeasuringRange operator&(MeasuringRange a, MeasuringRange b) { return static_cast<MeasuringRange>(static_cast<int>(a) & static_cast<int>(b)); }
00039         
00040         enum MotionIndicator
00041         {
00042             MOTION_IND_NEGATIVE_X               = MPU6050::MOT_NEGATIVE_X,
00043             MOTION_IND_POSITIVE_X               = MPU6050::MOT_POSITIVE_X,
00044             
00045             MOTION_IND_NEGATIVE_Y               = MPU6050::MOT_NEGATIVE_Y,
00046             MOTION_IND_POSITIVE_Y               = MPU6050::MOT_POSITIVE_Y,
00047             
00048             MOTION_IND_NEGATIVE_Z               = MPU6050::MOT_NEGATIVE_Z,
00049             MOTION_IND_POSITIVE_Z               = MPU6050::MOT_POSITIVE_Z,
00050             
00051             MOTION_IND_FREEFALL                 = 0x02, // 0b00000010
00052             MOTION_IND_ZERO_MOTION              = MPU6050::MOT_ZERO
00053         };
00054         friend inline MotionIndicator operator|(MotionIndicator a, MotionIndicator b) { return static_cast<MotionIndicator>(static_cast<int>(a) | static_cast<int>(b)); }
00055         friend inline MotionIndicator operator&(MotionIndicator a, MotionIndicator b) { return static_cast<MotionIndicator>(static_cast<int>(a) & static_cast<int>(b)); }
00056         
00057         typedef __packed struct
00058         {
00059             int16_t         accelX, accelY, accelZ;
00060             
00061             int16_t         temperature;
00062             
00063             int16_t         gyroX, gyroY, gyroZ;
00064             
00065             uint32_t        clock_us;
00066         } SensorData;
00067         
00068         typedef __packed struct
00069         {
00070             MotionIndicator indicator;
00071             
00072             uint32_t        clock_us;
00073             
00074         } MotionIndication;
00075         
00076         MPU6050Service
00077         (
00078             BLE                    &ble,
00079             MPU6050                &mpu6050,
00080             InterruptIn            *interruptIn         = NULL,
00081             const SensorData       *initialData         = NULL, 
00082             const MeasuringRange    measuringRange      = ACCEL_RANGE_2G | GYRO_RANGE_250, 
00083             const uint32_t          masterClock_us      = 0
00084         ) :
00085             gattServer(ble.gattServer()),
00086             mpu(mpu6050),
00087             intIn(interruptIn),
00088             timer(),
00089             
00090             _isRunning(false),
00091             updateSensorDataCharacteristicTrigger(false),
00092             updateMotionIndicatorCharacteristicTrigger(false),
00093             updateMeasuringRangeTrigger(false),
00094             
00095             timerOffset_us(masterClock_us),
00096             
00097             sensorDataCharacteristic
00098             (
00099                 UUID_MPU6050_SENSOR_DATA_CHAR,
00100                 (SensorData*)initialData,
00101                 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY // improving latency by repurposing notifications and "writing" the client
00102                                                                     // (i.e. indicating the client but not wait for a response)
00103             ),
00104             motionIndicatorCharaceristic
00105             (
00106                 UUID_MPU6050_MOTION_INDICATOR_CHAR,
00107                 NULL,
00108                 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY
00109             ),
00110             measuringRangeCharacteristic
00111             (
00112                 UUID_MPU6050_MEASURING_RANGE_CHAR,
00113                 (MeasuringRange*)&measuringRange,
00114                 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE // also improving latency 
00115             ),
00116             masterClockCharacteristic
00117             (
00118                 UUID_MPU6050_MASTER_CLOCK_CHAR,
00119                 (uint32_t*)&masterClock_us,
00120                 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE // look above again
00121             )
00122         {
00123             static bool serviceAdded = false;
00124             if (serviceAdded)
00125                 return;
00126                 
00127             GattCharacteristic *charTable[] =
00128             {
00129                 &sensorDataCharacteristic,
00130                 &motionIndicatorCharaceristic,
00131                 &measuringRangeCharacteristic,
00132                 &masterClockCharacteristic
00133             };
00134             
00135             GattService mpu6050Service(UUID_MPU6050_SERVICE, charTable, sizeof(charTable) / sizeof(GattCharacteristic*));
00136             
00137             gattServer.addService(mpu6050Service);
00138             gattServer.onDataSent(this, &MPU6050Service::dataSentCallback);
00139             gattServer.onDataWritten(this, &MPU6050Service::dataWrittenCallback);
00140             
00141             if (intIn != NULL)
00142                 intIn->rise(this, &MPU6050Service::interruptRiseCallback);
00143             
00144             serviceAdded = true;
00145         }
00146         
00147         bool isRunning()
00148         {
00149             return _isRunning;
00150         }
00151         
00152         bool start()
00153         {            
00154             if (!deviceReset())
00155             {
00156                 _isRunning = false;
00157                 return false;
00158             }
00159             
00160             timer.stop();
00161             timer.reset();
00162             timer.start();
00163                 
00164             _isRunning = true;
00165                 
00166             // sending data once will invoke the dataSent(...) callback later on
00167             // 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)
00168             updateSensorDataCharacteristicTrigger = true;
00169             
00170             return true;
00171         }
00172         
00173         void stop()
00174         {            
00175             _isRunning = false;
00176         }
00177         
00178         void handleService()
00179         {
00180             if (_isRunning)
00181             {
00182                 if (updateMeasuringRangeTrigger)
00183                     updateMeasuringRange();
00184                 if (updateSensorDataCharacteristicTrigger)
00185                     updateSensorDataCharacteristic();
00186                 if (updateMotionIndicatorCharacteristicTrigger)
00187                     updateMotionIndicatorCharacteristic();
00188             }
00189         }
00190         
00191     private:  
00192         static const int                                        MAX_SENSOR_POLLS_PER_UPDATE = 10;
00193     
00194         GattServer                                             &gattServer; 
00195         MPU6050                                                &mpu;
00196         InterruptIn                                            *intIn;
00197         Timer                                                   timer;
00198         
00199         volatile bool                                           _isRunning;
00200         bool                                                    updateSensorDataCharacteristicTrigger;
00201         bool                                                    updateMotionIndicatorCharacteristicTrigger;
00202         bool                                                    updateMeasuringRangeTrigger;
00203         
00204         uint32_t                                                timerOffset_us;
00205         
00206         ReadOnlyGattCharacteristic<SensorData>                  sensorDataCharacteristic;        
00207         ReadOnlyGattCharacteristic<MotionIndicator>             motionIndicatorCharaceristic;
00208         WriteOnlyGattCharacteristic<MeasuringRange>             measuringRangeCharacteristic;
00209         WriteOnlyGattCharacteristic<uint32_t>                   masterClockCharacteristic;  
00210         
00211         void dataSentCallback(unsigned count)
00212         {
00213             // we came here, if we just sent data, 
00214             // so lets keep the BLE stack busy by sending data again
00215             updateSensorDataCharacteristicTrigger = true;
00216         }
00217         
00218         void dataWrittenCallback(const GattWriteCallbackParams *context)
00219         {
00220             // update master clock immediately
00221             if (context->handle == masterClockCharacteristic.getValueAttribute().getHandle())
00222             {
00223                 timer.stop();
00224                 timer.reset();
00225                 
00226                 timerOffset_us = *((uint32_t*)context->data);
00227                 
00228                 timer.start();
00229             }
00230             // delay measuring range update
00231             else if (context->handle == measuringRangeCharacteristic.getValueAttribute().getHandle())
00232             {
00233                 updateMeasuringRangeTrigger = true;
00234             }
00235         }
00236         
00237         void interruptRiseCallback()
00238         {
00239             updateMotionIndicatorCharacteristicTrigger = true;
00240         }
00241         
00242         void updateSensorDataCharacteristic()
00243         {
00244             updateSensorDataCharacteristicTrigger = false;
00245               
00246             int         n       = 0;
00247             SensorData  data;
00248               
00249             for (; n < MAX_SENSOR_POLLS_PER_UPDATE; n++)
00250                 if (mpu.getMotionAndTemperature
00251                 (
00252                     (int16_t*)&(data.accelX), (int16_t*)&(data.accelY), (int16_t*)&(data.accelZ),
00253                     (int16_t*)&(data.temperature),
00254                     (int16_t*)&(data.gyroX), (int16_t*)&(data.gyroY), (int16_t*)&(data.gyroZ), 
00255                     0.0
00256                 ))
00257                 {   
00258                     data.clock_us = timer.read_us() + timerOffset_us;
00259                     
00260                     if (gattServer.write(sensorDataCharacteristic.getValueHandle(), (uint8_t*)&data, sizeof(SensorData)) != BLE_ERROR_NONE)
00261                         break;
00262                 }
00263                 
00264             if (n == 0)
00265                 updateSensorDataCharacteristicTrigger = true;
00266         }
00267         
00268         void updateMotionIndicatorCharacteristic()
00269         {
00270             updateMotionIndicatorCharacteristicTrigger = false;
00271             
00272             MPU6050::Interrupt  intSet  = static_cast<MPU6050::Interrupt>(0);
00273             MPU6050::Motion     motSet  = static_cast<MPU6050::Motion>(0);
00274             MotionIndicator mot;
00275                        
00276             mpu.getInterruptStatuses(&intSet);
00277             mpu.getMotionStatus(&motSet); 
00278             
00279             mot = *((MotionIndicator*)&motSet);
00280             
00281             if ((intSet & MPU6050::INT_FREEFALL) != 0)
00282                 mot = mot | MOTION_IND_FREEFALL;
00283             else
00284                 mot = mot & static_cast<MotionIndicator>(~0x02);
00285                 
00286             gattServer.write(motionIndicatorCharaceristic.getValueHandle(), (uint8_t*)&mot, sizeof(MotionIndicator));
00287         }
00288         
00289         void updateMeasuringRange()
00290         {   
00291             static const uint16_t sizeOfMeasuringRange = sizeof(MeasuringRange);
00292          
00293             updateMeasuringRangeTrigger = false;
00294             
00295             union
00296             {
00297                 MeasuringRange  range;
00298                 uint8_t         bytes[sizeOfMeasuringRange];
00299             } conv;
00300             
00301             if (gattServer.read(measuringRangeCharacteristic.getValueHandle(), conv.bytes, (uint16_t*)&sizeOfMeasuringRange) == BLE_ERROR_NONE)
00302             {            
00303                 switch (conv.range & ACCEL_RANGE_MASK)
00304                 {
00305                     case ACCEL_RANGE_2G:
00306                         mpu.setAccelRange(MPU6050::ACCEL_RANGE_2G);
00307                         break;
00308                     case ACCEL_RANGE_4G:
00309                         mpu.setAccelRange(MPU6050::ACCEL_RANGE_4G);
00310                         break;
00311                     case ACCEL_RANGE_8G:
00312                         mpu.setAccelRange(MPU6050::ACCEL_RANGE_8G);
00313                         break;
00314                     case ACCEL_RANGE_16G:
00315                         mpu.setAccelRange(MPU6050::ACCEL_RANGE_16G);
00316                         break;
00317                 }
00318                 
00319                 switch (conv.range & GYRO_RANGE_MASK)
00320                 {
00321                     case GYRO_RANGE_250:
00322                         mpu.setGyroRange(MPU6050::GYRO_RANGE_250);
00323                         break;
00324                     case GYRO_RANGE_500:
00325                         mpu.setGyroRange(MPU6050::GYRO_RANGE_500);
00326                         break;
00327                     case GYRO_RANGE_1000:
00328                         mpu.setGyroRange(MPU6050::GYRO_RANGE_1000);
00329                         break;
00330                     case GYRO_RANGE_2000:
00331                         mpu.setGyroRange(MPU6050::GYRO_RANGE_2000);
00332                         break;
00333                 }            
00334             }
00335         }
00336         
00337         inline bool deviceReset()
00338         {
00339             uint8_t id;
00340             
00341             return 
00342                 mpu.getDeviceId(&id) &&
00343                 (id == MPU6050::MPU6050_ID) &&
00344                 mpu.reset() &&
00345                 mpu.setClockSource(MPU6050::CLOCK_SRC_INTERNAL_8MHZ) &&                
00346                 mpu.setInterruptsEnabled(MPU6050::INT_FREEFALL | MPU6050::INT_MOTION | MPU6050::INT_ZERO_MOTION) &&
00347                 mpu.setSleepEnabled(false);
00348         }
00349 };
00350 
00351 #endif