File content as of revision 2:44bc61abdf33:

/* mbed Microcontroller Library
 * Copyright (c) 2006-2013 ARM Limited
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef __BLE_MPU_SERVICE_H__
#define __BLE_MPU_SERVICE_H__
#include "mbed.h"
#include "Stream.h"
#include "UUID.h"
#include "BLEDevice.h"

extern const uint16_t MPUServiceShortUUID;
extern const uint16_t MPUServiceYPRCharacteristicShortUUID;

extern const uint16_t MPUServiceAccelCharacteristicShortUUID;
extern const uint16_t MPUServiceQuatCharacteristicShortUUID;
extern const uint16_t MPUServiceTimeCharacteristicShortUUID;
extern const uint16_t MPUServiceSettingCharacteristicShortUUID;

extern const uint8_t  MPUServiceBaseUUID[LENGTH_OF_LONG_UUID];
extern const uint8_t  MPUServiceUUID[LENGTH_OF_LONG_UUID];
extern const uint8_t  MPUServiceUUID_reversed[LENGTH_OF_LONG_UUID];

extern const uint8_t  MPUServiceYPRCharacteristicUUID[LENGTH_OF_LONG_UUID];

extern const uint8_t  MPUServiceAccelCharacteristicUUID[LENGTH_OF_LONG_UUID];
extern const uint8_t  MPUServiceQuatCharacteristicUUID[LENGTH_OF_LONG_UUID];
extern const uint8_t  MPUServiceTimeCharacteristicUUID[LENGTH_OF_LONG_UUID];
extern const uint8_t  MPUServiceSettingCharacteristicUUID[LENGTH_OF_LONG_UUID];
/**
* @class MPUService
* @brief BLE Service to enable MPU6050 over BLE
*/
class MPUService
{

public:
    /**< Maximum length of data (in bytes) that can be transmitted by the UART service module to the peer. */
    static const unsigned GATT_MTU_SIZE_DEFAULT          = 23;
    static const unsigned GATT_MTU_SIZE_POSITION         = 10;
    static const unsigned BLE_MPU_SERVICE_MAX_DATA_LEN = (GATT_MTU_SIZE_DEFAULT - 3);
    static const unsigned BLE_MPU_SERVICE_MAX_POSITION_DATA_LEN = (GATT_MTU_SIZE_POSITION - 3);

public:

    /**
    * @param[ref] ble
    *                 BLEDevice object for the underlying controller.
    */
    MPUService(BLEDevice &_ble, float *initialYPRData) :
        ble(_ble),
        receiveBuffer(),
        yprValueBytes(initialYPRData),
        numBytesReceived(),
        receiveBufferIndex(),
        
        YPRCharacteristic(MPUServiceYPRCharacteristicShortUUID, (YawPitchRollValueBytes *)yprValueBytes.getPointer(),GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY),
        settingCharacteristic(MPUServiceSettingCharacteristicShortUUID, receiveBuffer, 1, BLE_MPU_SERVICE_MAX_DATA_LEN,GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE){
        
        static bool serviceAdded = false; /* We should only ever need to add the heart rate service once. */
        if (serviceAdded) {
            return;
        }
        
        GattCharacteristic *charTable[] = {&settingCharacteristic, &YPRCharacteristic};
        GattService         mpuService(MPUServiceShortUUID, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));
        ble.addService(mpuService);
        serviceAdded = true;
        ble.onDataWritten(this, &MPUService::onDataWritten);
    }


   


    /**
     * Note: TX and RX characteristics are to be interpreted from the viewpoint of the GATT client using this service.
     */
    uint16_t getSettingCharacteristic() {
        return settingCharacteristic.getValueAttribute().getHandle();
    }




    /**
    * @brief Update the temperature being broadcast
    *
    * @param[in] temperature
    *                   Floating point value of the temperature
    *
    */
    void updateYawPitchRoll(float *yprData) {
        if (ble.getGapState().connected) {
            yprValueBytes.updateYawPitchRoll(yprData);
            ble.updateCharacteristicValue(YPRCharacteristic.getValueAttribute().getHandle(), yprValueBytes.getPointer(), sizeof(YawPitchRollValueBytes));
        }
    }





private:
    /**
     * This callback allows the UART service to receive updates to the
     * txCharacteristic. The application should forward the call to this
     * function from the global onDataWritten() callback handler; or if that's
     * not used, this method can be used as a callback directly.
     */
    void onDataWritten(const GattCharacteristicWriteCBParams *params) {
        if (params->charHandle == getSettingCharacteristic()) {
            uint16_t bytesRead = params->len;
            if (bytesRead <= BLE_MPU_SERVICE_MAX_DATA_LEN) {
                numBytesReceived   = bytesRead;
                receiveBufferIndex = 0;
                memcpy(receiveBuffer, params->data, numBytesReceived);
            }
        }
    }




private:
    /* Private internal representation for the bytes used to work with the vaulue of the heart-rate characteristic. */
    struct YawPitchRollValueBytes {
        static const unsigned OFFSET_OF_FLAGS    = 0;
        //static const unsigned OFFSET_OF_VALUE    = OFFSET_OF_FLAGS + sizeof(uint8_t);
        static const unsigned SIZEOF_VALUE_BYTES = 3*sizeof(uint16_t);


        YawPitchRollValueBytes(float *yprData) : bytes() {
            /* assumption: temperature values are expressed in Celsius */
            updateYawPitchRoll(yprData);
        }


        void updateYawPitchRoll(float *yprData) {
            
            /*
            int32_t yaw_ieee11073 = quick_ieee11073_from_float(yprData[0]);
            int32_t pitch_ieee11073 = quick_ieee11073_from_float(yprData[1]);
            int32_t roll_ieee11073 = quick_ieee11073_from_float(yprData[2]);
            */
            uint16_t yaw_ieee11073   = (uint16_t)(100*(yprData[0]+180));
            uint16_t pitch_ieee11073 = (uint16_t)(100*(yprData[1]+180));
            uint16_t roll_ieee11073  = (uint16_t)(100*(yprData[2]+180));
            memcpy(&bytes[0], &yaw_ieee11073, sizeof(uint16_t));
            memcpy(&bytes[OFFSET_OF_FLAGS+sizeof(uint16_t)], &pitch_ieee11073, sizeof(uint16_t));
            memcpy(&bytes[OFFSET_OF_FLAGS+2*sizeof(uint16_t)], &roll_ieee11073, sizeof(uint16_t));
        }

        uint8_t       *getPointer(void) {
            return bytes;
        }

        const uint8_t *getPointer(void) const {
            return bytes;
        }

    private:
        /**
         * @brief A very quick conversion between a float temperature and 11073-20601 FLOAT-Type.
         * @param temperature The temperature as a float.
         * @return The temperature in 11073-20601 FLOAT-Type format.
         */
        int32_t quick_ieee11073_from_float(float yprData) {
            int8_t  exponent = 0xFE; //exponent is -2
            int32_t mantissa = (int32_t)(yprData * 100);
            return (((int32_t)exponent) << 24) | mantissa;
        }

    private:
        /* First byte = 8-bit flags, Second field is a float holding the temperature value. */
        /* See --> https://developer.bluetooth.org/gatt/characteristics/Pages/CharacteristicViewer.aspx?u=org.bluetooth.characteristic.temperature_measurement.xml */
        uint8_t bytes[SIZEOF_VALUE_BYTES];
    };





private:
    BLEDevice          &ble;

    uint8_t             receiveBuffer[BLE_MPU_SERVICE_MAX_DATA_LEN]; /**< The local buffer into which we receive
                                                                       *   inbound data before forwarding it to the
                                                                       *   application. */
    uint8_t             YPRBuffer[BLE_MPU_SERVICE_MAX_DATA_LEN];


    uint8_t             numBytesReceived ;
    uint8_t             receiveBufferIndex;

    YawPitchRollValueBytes  yprValueBytes;


    ReadOnlyGattCharacteristic<YawPitchRollValueBytes>  YPRCharacteristic;

    GattCharacteristic  settingCharacteristic;

};

#endif /* #ifndef __BLE_MPU_SERVICE_H__*/