CustomSoftwareService.h

00001 /**
00002   ******************************************************************************
00003   * @file    CustomSoftawareService.h
00004   * @author  Central Labs / AST
00005   * @version V0.9.0
00006   * @date    23-Dec-2015
00007   * @brief   BLE 9axis sensors fusion/quaternions service
00008   ******************************************************************************
00009   * @attention
00010   *
00011   * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
00012   *
00013   * Redistribution and use in source and binary forms, with or without modification,
00014   * are permitted provided that the following conditions are met:
00015   *   1. Redistributions of source code must retain the above copyright notice,
00016   *      this list of conditions and the following disclaimer.
00017   *   2. Redistributions in binary form must reproduce the above copyright notice,
00018   *      this list of conditions and the following disclaimer in the documentation
00019   *      and/or other materials provided with the distribution.
00020   *   3. Neither the name of STMicroelectronics nor the names of its contributors
00021   *      may be used to endorse or promote products derived from this software
00022   *      without specific prior written permission.
00023   *
00024   * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00025   * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00026   * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00027   * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00028   * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00029   * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00030   * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031   * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00032   * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00033   * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00034   *
00035   ******************************************************************************
00036   */
00037 
00038 #ifndef __CUSTOM_BLE_SOFTWARE_SERVICE_H__
00039 #define __CUSTOM_BLE_SOFTWARE_SERVICE_H__
00040 #include "BLE.h"
00041 
00042 #define SEND_N_QUATERNIONS  3   // The number of quaternions to be sent per BLE update (could be 1,2,3)
00043 
00044 #if SEND_N_QUATERNIONS    == 1
00045 #elif  SEND_N_QUATERNIONS == 2
00046 #elif  SEND_N_QUATERNIONS == 3
00047 #else
00048 #error SEND_N_QUATERNIONS could be only 1,2,3
00049 #endif
00050 
00051 #define SIZEOF_QUAT         2+2*3*SEND_N_QUATERNIONS  // timestamp+sizeof(quat)*xyz*nquat
00052 #define SIZEOF_FLOAT_QUAT   2+4*3
00053 
00054 const LongUUIDBytes_t SOFTWARE_SERVICE_UUID_128                 = { 0x00,0x00,0x00,0x00,0x00,0x02,0x11,0xe1,0x9a,0xb4,0x00,0x02,0xa5,0xd5,0xc5,0x1b };
00055 const LongUUIDBytes_t QUATERNIONS_CHAR_UUID_128                 = { 0x00,0x00,0x01,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b};   
00056 const LongUUIDBytes_t QUATERNIONS_FLOAT_CHAR_UUID_128           = { 0x00,0x00,0x00,0x80,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b};
00057 
00058 /* Custom Software Service */
00059 class CustomSoftwareService {
00060 public:
00061     CustomSoftwareService(BLE &_ble) :
00062         ble(_ble), 
00063                 swQuaternionsCharacteristic(QUATERNIONS_CHAR_UUID_128, swQuaternions, SIZEOF_QUAT, SIZEOF_QUAT, 
00064                                                                         GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY),
00065                 swFloatQuaternionsCharacteristic(QUATERNIONS_FLOAT_CHAR_UUID_128, swFloatQuaternions, SIZEOF_FLOAT_QUAT, SIZEOF_FLOAT_QUAT, 
00066                                                                         GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY)
00067     {
00068 
00069             static bool serviceAdded = false; /* We should only ever need to add the heart rate service once. */
00070             if (serviceAdded) {
00071                 return;
00072             }
00073     
00074         GattCharacteristic *charTable[] = {&swQuaternionsCharacteristic, &swFloatQuaternionsCharacteristic};
00075 
00076         GattService   quaternionsService(SOFTWARE_SERVICE_UUID_128, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));               
00077                 
00078         ble.gattServer().addService(quaternionsService);             
00079 
00080         isEnabledQuatNotify                     = false;
00081         isEnabledFloatQuatnotify                = false;
00082         
00083         memset (swQuaternions, 0, SIZEOF_QUAT); 
00084         memset (swFloatQuaternions, 0, SIZEOF_FLOAT_QUAT);          
00085         
00086         serviceAdded = true;
00087     }
00088 
00089     uint32_t updateQuaternions(AxesRaw_TypeDef *data, uint16_t TimeStamp) {
00090             if (isEnabledQuatNotify) {
00091                 STORE_LE_16(swQuaternions  ,TimeStamp);
00092 #if SEND_N_QUATERNIONS == 1
00093                 STORE_LE_16(swQuaternions+2,data[0].AXIS_X);
00094                 STORE_LE_16(swQuaternions+4,data[0].AXIS_Y);
00095                 STORE_LE_16(swQuaternions+6,data[0].AXIS_Z);
00096 #elif SEND_N_QUATERNIONS == 2
00097                 STORE_LE_16(swQuaternions+2,data[0].AXIS_X);
00098                 STORE_LE_16(swQuaternions+4,data[0].AXIS_Y);
00099                 STORE_LE_16(swQuaternions+6,data[0].AXIS_Z);
00100 
00101                 STORE_LE_16(swQuaternions+8 ,data[1].AXIS_X);
00102                 STORE_LE_16(swQuaternions+10,data[1].AXIS_Y);
00103                 STORE_LE_16(swQuaternions+12,data[1].AXIS_Z);
00104 #elif SEND_N_QUATERNIONS == 3
00105                 STORE_LE_16(swQuaternions+2,data[0].AXIS_X);
00106                 STORE_LE_16(swQuaternions+4,data[0].AXIS_Y);
00107                 STORE_LE_16(swQuaternions+6,data[0].AXIS_Z);
00108 
00109                 STORE_LE_16(swQuaternions+8 ,data[1].AXIS_X);
00110                 STORE_LE_16(swQuaternions+10,data[1].AXIS_Y);
00111                 STORE_LE_16(swQuaternions+12,data[1].AXIS_Z);
00112 
00113                 STORE_LE_16(swQuaternions+14,data[2].AXIS_X);
00114                 STORE_LE_16(swQuaternions+16,data[2].AXIS_Y);
00115                 STORE_LE_16(swQuaternions+18,data[2].AXIS_Z);
00116 #else
00117 #error SEND_N_QUATERNIONS could be only 1,2,3
00118 #endif          
00119                  return ble.gattServer().write(swQuaternionsCharacteristic.getValueAttribute().getHandle(), swQuaternions, SIZEOF_QUAT,0);
00120             }
00121         return 0;
00122   }
00123 
00124     uint32_t updateFloatQuaternions(float *QuatFloat, uint16_t TimeStamp) {
00125             if (isEnabledFloatQuatnotify){
00126                 STORE_LE_16(swFloatQuaternions  ,TimeStamp);
00127                 STORE_LE_32(swFloatQuaternions+2 ,((uint32_t*)(QuatFloat))[0]);
00128                 STORE_LE_32(swFloatQuaternions+6 ,((uint32_t*)(QuatFloat))[1]);
00129                 STORE_LE_32(swFloatQuaternions+10,((uint32_t*)(QuatFloat))[2]);
00130 
00131                 return ble.gattServer().write(swFloatQuaternionsCharacteristic.getValueAttribute().getHandle(), swFloatQuaternions, SIZEOF_FLOAT_QUAT,0);           
00132             }
00133             return 0;
00134     }
00135         
00136     void enNotify (Gap::Handle_t handle) {
00137             if (isQuatHandle(handle - BLE_HANDLE_EN_DIS_OFFSET)) { isEnabledQuatNotify = true; return; } 
00138             if (isFloatQuatHandle(handle - BLE_HANDLE_EN_DIS_OFFSET)) { isEnabledFloatQuatnotify = true; return; }               
00139     }
00140             
00141     void disNotify (Gap::Handle_t handle) {
00142             if (isQuatHandle(handle - BLE_HANDLE_EN_DIS_OFFSET)) { isEnabledQuatNotify = false; return; }    
00143             if (isFloatQuatHandle(handle - BLE_HANDLE_EN_DIS_OFFSET)) { isEnabledFloatQuatnotify = false; return; }              
00144     }       
00145         
00146     bool isQuatHandle (Gap::Handle_t handle) {
00147             if (handle == swQuaternionsCharacteristic.getValueAttribute().getHandle() - BLE_HANDLE_VALUE_OFFSET) return true;
00148             return false;
00149     }       
00150 
00151     bool isFloatQuatHandle (Gap::Handle_t handle) {
00152             if (handle == swFloatQuaternionsCharacteristic.getValueAttribute().getHandle() - BLE_HANDLE_VALUE_OFFSET) return true;
00153             return false;
00154     }               
00155             
00156     void updateConnectionStatus(ConnectionStatus_t status) {            
00157             isEnabledQuatNotify                         = false;
00158             isEnabledFloatQuatnotify                = false;        
00159             memset (swQuaternions, 0, SIZEOF_QUAT); 
00160             memset (swFloatQuaternions, 0, SIZEOF_FLOAT_QUAT);                      
00161     }
00162         
00163 private:
00164         BLE                 &ble;
00165         GattCharacteristic  swQuaternionsCharacteristic;
00166         GattCharacteristic  swFloatQuaternionsCharacteristic;
00167 
00168         uint8_t             swQuaternions[SIZEOF_QUAT];     
00169         uint8_t             swFloatQuaternions[SIZEOF_FLOAT_QUAT];              
00170 
00171         bool                isEnabledQuatNotify;
00172         bool                isEnabledFloatQuatnotify;
00173 
00174         ConnectionStatus_t  isBTLEConnected;
00175 };
00176 
00177 #endif /* #ifndef __CUSTOM_BLE_CONFIG_SERVICE_H__*/