File content as of revision 13:496c0f5e8a61:

#pragma once
#ifndef __MPU6050_SERVICE_H__
#define __MPU6050_SERVICE_H__

#include "BLE.h"
#include "MPU6050.h"

class MPU6050Service
{
    public:        
        enum
        {
            UUID_MPU6050_SERVICE                = 0xA123,
            
            UUID_MPU6050_SENSOR_DATA_CHAR       = 0xA124,
            UUID_MPU6050_MOTION_INDICATOR_CHAR  = 0xA125,
            UUID_MPU6050_MEASURING_RANGE_CHAR   = 0xA126,
            UUID_MPU6050_MASTER_CLOCK_CHAR      = 0xA127
        };
        
        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
            
            ACCEL_RANGE_MASK                    = 0x03, // 0b__11
            
            GYRO_RANGE_250                      = 0x00, // 0b00__
            GYRO_RANGE_500                      = 0x04, // 0b01__
            GYRO_RANGE_1000                     = 0x08, // 0b10__
            GYRO_RANGE_2000                     = 0x0C, // 0b11__
            
            GYRO_RANGE_MASK                     = 0x0C  // 0b11__
        };
        friend inline MeasuringRange operator|(MeasuringRange a, MeasuringRange b) { return static_cast<MeasuringRange>(static_cast<int>(a) | static_cast<int>(b)); }
        friend inline MeasuringRange operator&(MeasuringRange a, MeasuringRange b) { return static_cast<MeasuringRange>(static_cast<int>(a) & static_cast<int>(b)); }
        
        enum MotionIndicator
        {
            MOTION_IND_NEGATIVE_X               = MPU6050::MOT_NEGATIVE_X,
            MOTION_IND_POSITIVE_X               = MPU6050::MOT_POSITIVE_X,
            
            MOTION_IND_NEGATIVE_Y               = MPU6050::MOT_NEGATIVE_Y,
            MOTION_IND_POSITIVE_Y               = MPU6050::MOT_POSITIVE_Y,
            
            MOTION_IND_NEGATIVE_Z               = MPU6050::MOT_NEGATIVE_Z,
            MOTION_IND_POSITIVE_Z               = MPU6050::MOT_POSITIVE_Z,
            
            MOTION_IND_FREEFALL                 = 0x02, // 0b00000010
            MOTION_IND_ZERO_MOTION              = MPU6050::MOT_ZERO
        };
        friend inline MotionIndicator operator|(MotionIndicator a, MotionIndicator b) { return static_cast<MotionIndicator>(static_cast<int>(a) | static_cast<int>(b)); }
        friend inline MotionIndicator operator&(MotionIndicator a, MotionIndicator b) { return static_cast<MotionIndicator>(static_cast<int>(a) & static_cast<int>(b)); }
        
        typedef __packed struct
        {
            int16_t         accelX, accelY, accelZ;
            
            int16_t         temperature;
            
            int16_t         gyroX, gyroY, gyroZ;
            
            uint32_t        clock_us;
        } SensorData;
        
        typedef __packed struct
        {
            MotionIndicator indicator;
            
            uint32_t        clock_us;
            
        } MotionIndication;
        
        MPU6050Service
        (
            BLE                    &ble,
            MPU6050                &mpu6050,
            InterruptIn            *interruptIn         = NULL,
            const SensorData       *initialData         = NULL, 
            const MeasuringRange    measuringRange      = ACCEL_RANGE_2G | GYRO_RANGE_250, 
            const uint32_t          masterClock_us      = 0
        ) :
            gattServer(ble.gattServer()),
            mpu(mpu6050),
            intIn(interruptIn),
            timer(),
            
            _isRunning(false),
            updateSensorDataCharacteristicTrigger(false),
            updateMotionIndicatorCharacteristicTrigger(false),
            updateMeasuringRangeTrigger(false),
            
            timerOffset_us(masterClock_us),
            
            sensorDataCharacteristic
            (
                UUID_MPU6050_SENSOR_DATA_CHAR,
                (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)
            ),
            motionIndicatorCharaceristic
            (
                UUID_MPU6050_MOTION_INDICATOR_CHAR,
                NULL,
                GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY
            ),
            measuringRangeCharacteristic
            (
                UUID_MPU6050_MEASURING_RANGE_CHAR,
                (MeasuringRange*)&measuringRange,
                GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE // also improving latency 
            ),
            masterClockCharacteristic
            (
                UUID_MPU6050_MASTER_CLOCK_CHAR,
                (uint32_t*)&masterClock_us,
                GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE // look above again
            )
        {
            static bool serviceAdded = false;
            if (serviceAdded)
                return;
                
            GattCharacteristic *charTable[] =
            {
                &sensorDataCharacteristic,
                &motionIndicatorCharaceristic,
                &measuringRangeCharacteristic,
                &masterClockCharacteristic
            };
            
            GattService mpu6050Service(UUID_MPU6050_SERVICE, charTable, sizeof(charTable) / sizeof(GattCharacteristic*));
            
            gattServer.addService(mpu6050Service);
            gattServer.onDataSent(this, &MPU6050Service::dataSentCallback);
            gattServer.onDataWritten(this, &MPU6050Service::dataWrittenCallback);
            
            if (intIn != NULL)
                intIn->rise(this, &MPU6050Service::interruptRiseCallback);
            
            serviceAdded = true;
        }
        
        bool isRunning()
        {
            return _isRunning;
        }
        
        bool start()
        {            
            if (!deviceReset())
            {
                _isRunning = false;
                return false;
            }
            
            timer.stop();
            timer.reset();
            timer.start();
                
            _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 and so on)
            updateSensorDataCharacteristicTrigger = true;
            
            return true;
        }
        
        void stop()
        {            
            _isRunning = false;
        }
        
        void handleService()
        {
            if (_isRunning)
            {
                if (updateMeasuringRangeTrigger)
                    updateMeasuringRange();
                if (updateSensorDataCharacteristicTrigger)
                    updateSensorDataCharacteristic();
                if (updateMotionIndicatorCharacteristicTrigger)
                    updateMotionIndicatorCharacteristic();
            }
        }
        
    private:  
        static const int                                        MAX_SENSOR_POLLS_PER_UPDATE = 10;
    
        GattServer                                             &gattServer; 
        MPU6050                                                &mpu;
        InterruptIn                                            *intIn;
        Timer                                                   timer;
        
        volatile bool                                           _isRunning;
        bool                                                    updateSensorDataCharacteristicTrigger;
        bool                                                    updateMotionIndicatorCharacteristicTrigger;
        bool                                                    updateMeasuringRangeTrigger;
        
        uint32_t                                                timerOffset_us;
        
        ReadOnlyGattCharacteristic<SensorData>                  sensorDataCharacteristic;        
        ReadOnlyGattCharacteristic<MotionIndicator>             motionIndicatorCharaceristic;
        WriteOnlyGattCharacteristic<MeasuringRange>             measuringRangeCharacteristic;
        WriteOnlyGattCharacteristic<uint32_t>                   masterClockCharacteristic;  
        
        void dataSentCallback(unsigned count)
        {
            // we came here, if we just sent data, 
            // so lets keep the BLE stack busy by sending data again
            updateSensorDataCharacteristicTrigger = true;
        }
        
        void dataWrittenCallback(const GattWriteCallbackParams *context)
        {
            // update master clock immediately
            if (context->handle == masterClockCharacteristic.getValueAttribute().getHandle())
            {
                timer.stop();
                timer.reset();
                
                timerOffset_us = *((uint32_t*)context->data);
                
                timer.start();
            }
            // delay measuring range update
            else if (context->handle == measuringRangeCharacteristic.getValueAttribute().getHandle())
            {
                updateMeasuringRangeTrigger = true;
            }
        }
        
        void interruptRiseCallback()
        {
            updateMotionIndicatorCharacteristicTrigger = true;
        }
        
        void updateSensorDataCharacteristic()
        {
            updateSensorDataCharacteristicTrigger = false;
              
            int         n       = 0;
            SensorData  data;
              
            for (; n < MAX_SENSOR_POLLS_PER_UPDATE; n++)
                if (mpu.getMotionAndTemperature
                (
                    (int16_t*)&(data.accelX), (int16_t*)&(data.accelY), (int16_t*)&(data.accelZ),
                    (int16_t*)&(data.temperature),
                    (int16_t*)&(data.gyroX), (int16_t*)&(data.gyroY), (int16_t*)&(data.gyroZ), 
                    0.0
                ))
                {   
                    data.clock_us = timer.read_us() + timerOffset_us;
                    
                    if (gattServer.write(sensorDataCharacteristic.getValueHandle(), (uint8_t*)&data, sizeof(SensorData)) != BLE_ERROR_NONE)
                        break;
                }
                
            if (n == 0)
                updateSensorDataCharacteristicTrigger = true;
        }
        
        void updateMotionIndicatorCharacteristic()
        {
            updateMotionIndicatorCharacteristicTrigger = false;
            
            MPU6050::Interrupt  intSet  = static_cast<MPU6050::Interrupt>(0);
            MPU6050::Motion     motSet  = static_cast<MPU6050::Motion>(0);
            MotionIndicator mot;
                       
            mpu.getInterruptStatuses(&intSet);
            mpu.getMotionStatus(&motSet); 
            
            mot = *((MotionIndicator*)&motSet);
            
            if ((intSet & MPU6050::INT_FREEFALL) != 0)
                mot = mot | MOTION_IND_FREEFALL;
            else
                mot = mot & static_cast<MotionIndicator>(~0x02);
                
            gattServer.write(motionIndicatorCharaceristic.getValueHandle(), (uint8_t*)&mot, sizeof(MotionIndicator));
        }
        
        void updateMeasuringRange()
        {   
            static const uint16_t sizeOfMeasuringRange = sizeof(MeasuringRange);
         
            updateMeasuringRangeTrigger = false;
            
            union
            {
                MeasuringRange  range;
                uint8_t         bytes[sizeOfMeasuringRange];
            } conv;
            
            if (gattServer.read(measuringRangeCharacteristic.getValueHandle(), conv.bytes, (uint16_t*)&sizeOfMeasuringRange) == BLE_ERROR_NONE)
            {            
                switch (conv.range & ACCEL_RANGE_MASK)
                {
                    case ACCEL_RANGE_2G:
                        mpu.setAccelRange(MPU6050::ACCEL_RANGE_2G);
                        break;
                    case ACCEL_RANGE_4G:
                        mpu.setAccelRange(MPU6050::ACCEL_RANGE_4G);
                        break;
                    case ACCEL_RANGE_8G:
                        mpu.setAccelRange(MPU6050::ACCEL_RANGE_8G);
                        break;
                    case ACCEL_RANGE_16G:
                        mpu.setAccelRange(MPU6050::ACCEL_RANGE_16G);
                        break;
                }
                
                switch (conv.range & GYRO_RANGE_MASK)
                {
                    case GYRO_RANGE_250:
                        mpu.setGyroRange(MPU6050::GYRO_RANGE_250);
                        break;
                    case GYRO_RANGE_500:
                        mpu.setGyroRange(MPU6050::GYRO_RANGE_500);
                        break;
                    case GYRO_RANGE_1000:
                        mpu.setGyroRange(MPU6050::GYRO_RANGE_1000);
                        break;
                    case GYRO_RANGE_2000:
                        mpu.setGyroRange(MPU6050::GYRO_RANGE_2000);
                        break;
                }            
            }
        }
        
        inline bool deviceReset()
        {
            uint8_t id;
            
            return 
                mpu.getDeviceId(&id) &&
                (id == MPU6050::MPU6050_ID) &&
                mpu.reset() &&
                mpu.setClockSource(MPU6050::CLOCK_SRC_INTERNAL_8MHZ) &&                
                mpu.setInterruptsEnabled(MPU6050::INT_FREEFALL | MPU6050::INT_MOTION | MPU6050::INT_ZERO_MOTION) &&
                mpu.setSleepEnabled(false);
        }
};

#endif