File content as of revision 7:34014895dda8:

/**
 ******************************************************************************
 * @file    CustomSoftwareService.h
 * @author  AST / EST
 * @version V0.0.1
 * @date    16-Dec-2015
 * @brief   Ble quaternion service of Bluemicrosystem1 application
 ******************************************************************************
 * @attention
 *
 * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *   1. Redistributions of source code must retain the above copyright notice,
 *      this list of conditions and the following disclaimer.
 *   2. Redistributions in binary form must reproduce the above copyright notice,
 *      this list of conditions and the following disclaimer in the documentation
 *      and/or other materials provided with the distribution.
 *   3. Neither the name of STMicroelectronics nor the names of its contributors
 *      may be used to endorse or promote products derived from this software
 *      without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 ******************************************************************************
 */

#ifndef __CUSTOM_BLE_SOFTWARE_SERVICE_H__
#define __CUSTOM_BLE_SOFTWARE_SERVICE_H__
#include "BLE.h"

#define SEND_N_QUATERNIONS  3   // The number of quaternions to be sent per BLE update (could be 1,2,3)

#if SEND_N_QUATERNIONS    == 1
#elif  SEND_N_QUATERNIONS == 2
#elif  SEND_N_QUATERNIONS == 3
#else
#error SEND_N_QUATERNIONS could be only 1,2,3
#endif

#define SIZEOF_QUAT         2+2*3*SEND_N_QUATERNIONS  // timestamp+sizeof(quat)*xyz*nquat
#define SIZEOF_FLOAT_QUAT   2+4*3

const LongUUIDBytes_t SOFTWARE_SERVICE_UUID_128                 = { 0x00,0x00,0x00,0x00,0x00,0x02,0x11,0xe1,0x9a,0xb4,0x00,0x02,0xa5,0xd5,0xc5,0x1b };
const LongUUIDBytes_t QUATERNIONS_CHAR_UUID_128                 = { 0x00,0x00,0x01,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b};   
const LongUUIDBytes_t QUATERNIONS_FLOAT_CHAR_UUID_128           = { 0x00,0x00,0x00,0x80,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b};


/* Custom Software Service */
class CustomSoftwareService {
public:
    CustomSoftwareService(BLE &_ble, CustomBleErrManagement * ErrMgr=NULL ) :
        ble(_ble), 
                swQuaternionsCharacteristic(QUATERNIONS_CHAR_UUID_128, swQuaternions, SIZEOF_QUAT, SIZEOF_QUAT, 
                                                                        GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY),
                swFloatQuaternionsCharacteristic(QUATERNIONS_FLOAT_CHAR_UUID_128, swFloatQuaternions, SIZEOF_FLOAT_QUAT, SIZEOF_FLOAT_QUAT, 
                                                                        GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY)
    {

            static bool serviceAdded = false; /* We should only ever need to add the heart rate service once. */
            if (serviceAdded) {
                return;
            }
    
        GattCharacteristic *charTable[] = {&swQuaternionsCharacteristic, &swFloatQuaternionsCharacteristic};

        GattService   quaternionsService(SOFTWARE_SERVICE_UUID_128, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));               
                
        ble.gattServer().addService(quaternionsService);             

        isEnabledQuatNotify                     = false;
        isEnabledFloatQuatnotify                = false;
        
        memset (swQuaternions, 0, SIZEOF_QUAT); 
        memset (swFloatQuaternions, 0, SIZEOF_FLOAT_QUAT);          
        
        serviceAdded = true;
    }

    void updateQuaternions(AxesRaw_TypeDef *data, uint16_t TimeStamp) {        
        if (isEnabledQuatNotify) {
            if (bleErrMgr != NULL) {
                if (!bleErrMgr->isBleTxChannelOk()){
                    bleErrMgr->signalBleTxOk();  // skip tx because of previous error
                    return;
                }
            }                           
            STORE_LE_16(swQuaternions  ,TimeStamp);
#if SEND_N_QUATERNIONS == 1
            STORE_LE_16(swQuaternions+2,data[0].AXIS_X);
            STORE_LE_16(swQuaternions+4,data[0].AXIS_Y);
            STORE_LE_16(swQuaternions+6,data[0].AXIS_Z);
#elif SEND_N_QUATERNIONS == 2
            STORE_LE_16(swQuaternions+2,data[0].AXIS_X);
            STORE_LE_16(swQuaternions+4,data[0].AXIS_Y);
            STORE_LE_16(swQuaternions+6,data[0].AXIS_Z);

            STORE_LE_16(swQuaternions+8 ,data[1].AXIS_X);
            STORE_LE_16(swQuaternions+10,data[1].AXIS_Y);
            STORE_LE_16(swQuaternions+12,data[1].AXIS_Z);
#elif SEND_N_QUATERNIONS == 3
            STORE_LE_16(swQuaternions+2,data[0].AXIS_X);
            STORE_LE_16(swQuaternions+4,data[0].AXIS_Y);
            STORE_LE_16(swQuaternions+6,data[0].AXIS_Z);

            STORE_LE_16(swQuaternions+8 ,data[1].AXIS_X);
            STORE_LE_16(swQuaternions+10,data[1].AXIS_Y);
            STORE_LE_16(swQuaternions+12,data[1].AXIS_Z);

            STORE_LE_16(swQuaternions+14,data[2].AXIS_X);
            STORE_LE_16(swQuaternions+16,data[2].AXIS_Y);
            STORE_LE_16(swQuaternions+18,data[2].AXIS_Z);
#else
#error SEND_N_QUATERNIONS could be only 1,2,3
#endif          
             uint32_t err = ble.gattServer().write(swQuaternionsCharacteristic.getValueAttribute().getHandle(), swQuaternions, SIZEOF_QUAT,0);
             if (!err) {
                 bleErrMgr->signalBleTxOk();
             }else{
                 bleErrMgr->signalBleTxErr(err);
             }             
        }
  }

    void updateFloatQuaternions(float *QuatFloat, uint16_t TimeStamp) {
        if (isEnabledFloatQuatnotify) {
            if (bleErrMgr != NULL) {
                if (!bleErrMgr->isBleTxChannelOk()){
                    bleErrMgr->signalBleTxOk();  // skip tx because of previous error
                    return;
                }
            }                           
            STORE_LE_16(swFloatQuaternions  ,TimeStamp);
            STORE_LE_32(swFloatQuaternions+2 ,((uint32_t*)(QuatFloat))[0]);
            STORE_LE_32(swFloatQuaternions+6 ,((uint32_t*)(QuatFloat))[1]);
            STORE_LE_32(swFloatQuaternions+10,((uint32_t*)(QuatFloat))[2]);

            uint32_t err = ble.gattServer().write(swFloatQuaternionsCharacteristic.getValueAttribute().getHandle(), swFloatQuaternions, SIZEOF_FLOAT_QUAT,0);           
            if (!err) {
                 bleErrMgr->signalBleTxOk();
            }else{
                bleErrMgr->signalBleTxErr(err);
            }            
        }
    }
        
    void enNotify (Gap::Handle_t handle) {
            if (isQuatHandle(handle)) { isEnabledQuatNotify = true; return; } 
            if (isFloatQuatHandle(handle)) { isEnabledFloatQuatnotify = true; return; }               
    }
            
    void disNotify (Gap::Handle_t handle) {
            if (isQuatHandle(handle)) { isEnabledQuatNotify = false; return; }    
            if (isFloatQuatHandle(handle)) { isEnabledFloatQuatnotify = false; return; }              
    }       
        
    bool isQuatHandle (Gap::Handle_t handle) {
            if (handle == swQuaternionsCharacteristic.getValueAttribute().getHandle()) return true;
            return false;
    }       

    bool isFloatQuatHandle (Gap::Handle_t handle) {
            if (handle == swFloatQuaternionsCharacteristic.getValueAttribute().getHandle()) return true;
            return false;
    }               
            
    void updateConnectionStatus(ConnectionStatus_t status) {            
            isEnabledQuatNotify                         = false;
            isEnabledFloatQuatnotify                = false;        
            memset (swQuaternions, 0, SIZEOF_QUAT); 
            memset (swFloatQuaternions, 0, SIZEOF_FLOAT_QUAT);                      
    }
        
private:
        BLE           &ble;
        CustomBleErrManagement *bleErrMgr;        
        GattCharacteristic  swQuaternionsCharacteristic;
        GattCharacteristic  swFloatQuaternionsCharacteristic;
        uint8_t                         swQuaternions[SIZEOF_QUAT];     
        uint8_t                         swFloatQuaternions[SIZEOF_FLOAT_QUAT];              
        bool                                isEnabledQuatNotify;
        bool                                isEnabledFloatQuatnotify;
};

#endif /* #ifndef __CUSTOM_BLE_CONFIG_SERVICE_H__*/