Diff: ButtonService.h

Revision:

3:3157e61f2bfd

Parent:

2:02175845b24c

--- a/ButtonService.h	Sun Oct 15 18:24:03 2017 +0000
+++ b/ButtonService.h	Tue Oct 17 20:55:04 2017 +0000
@@ -25,15 +25,44 @@
 
     ButtonService(BLE &_ble, bool buttonPressedInitial) :
         ble(_ble), buttonState(BUTTON_STATE_CHARACTERISTIC_UUID,
-                               (uint8_t []) {0,0},
+                               (uint8_t []) {
+        0,0
+    },
     GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY) {
         GattCharacteristic *charTable[] = {&buttonState};
         GattService         buttonService(ButtonService::BUTTON_SERVICE_UUID, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));
         ble.gattServer().addService(buttonService);
     }
 
-    void updateButtonState(uint8_t v[12]) {        
-        ble.gattServer().write(buttonState.getValueHandle(), (uint8_t *)v, sizeof(v));
+    void updateButtonState(float roll, float pitch, float yaw, float gx, float gy, float gz) {
+        uint8_t sensorValues[12] = {0};
+
+        uint16_t rollValue = (uint16_t)roll;
+        uint16_t pitchValue = (uint16_t)pitch;
+        uint16_t yawValue = (uint16_t)yaw;
+
+        uint16_t gxValue = (uint16_t)gx;
+        uint16_t gyValue = (uint16_t)gy;
+        uint16_t gzValue = (uint16_t)gz;
+        sensorValues[0]= rollValue & 0xff;
+        sensorValues[1]= (rollValue >> 8);
+
+        sensorValues[2]= pitchValue & 0xff;
+        sensorValues[3]= (pitchValue >> 8);
+
+        sensorValues[4]= yawValue & 0xff;
+        sensorValues[5]= (yawValue >> 8);
+
+        sensorValues[6]= gxValue & 0xff;
+        sensorValues[7]= (gxValue >> 8);
+
+        sensorValues[8]= gyValue & 0xff;
+        sensorValues[9]= (gyValue >> 8);
+
+        sensorValues[10]= gzValue & 0xff;
+        sensorValues[11]= (gzValue >> 8);
+
+        ble.gattServer().write(buttonState.getValueHandle(), (uint8_t *)sensorValues, sizeof(sensorValues));
         //ble.updateCharacteristicValue(buttonState.getValueHandle(), (uint8_t *)v ,sizeof(v));
     }