Diff: MPUService.h

Revision:

2:44bc61abdf33

Parent:

0:f90c3452d779

--- a/MPUService.h	Wed Jun 10 10:00:59 2015 +0000
+++ b/MPUService.h	Wed Jun 10 12:56:48 2015 +0000
@@ -63,8 +63,8 @@
         ble(_ble),
         receiveBuffer(),
         yprValueBytes(initialYPRData),
-        numBytesReceived(0),
-        receiveBufferIndex(0),
+        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){
@@ -138,26 +138,29 @@
     /* 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 = sizeof(uint8_t) + 3*sizeof(float);
+        //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 */
-            bytes[OFFSET_OF_FLAGS] =  1;
             updateYawPitchRoll(yprData);
         }
 
 
         void updateYawPitchRoll(float *yprData) {
             
-            
-            uint32_t yaw_ieee11073 = quick_ieee11073_from_float(yprData[0]);
-            uint32_t pitch_ieee11073 = quick_ieee11073_from_float(yprData[1]);
-            uint32_t roll_ieee11073 = quick_ieee11073_from_float(yprData[2]);
-            memcpy(&bytes[OFFSET_OF_VALUE], &yaw_ieee11073, sizeof(float));
-            memcpy(&bytes[OFFSET_OF_VALUE+sizeof(float)], &pitch_ieee11073, sizeof(float));
-            memcpy(&bytes[OFFSET_OF_VALUE+2*sizeof(float)], &roll_ieee11073, sizeof(float));
+            /*
+            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) {
@@ -174,10 +177,10 @@
          * @param temperature The temperature as a float.
          * @return The temperature in 11073-20601 FLOAT-Type format.
          */
-        uint32_t quick_ieee11073_from_float(float yprData) {
-            uint8_t  exponent = 0xFE; //exponent is -2
-            uint32_t mantissa = (uint32_t)(yprData * 100);
-            return (((uint32_t)exponent) << 24) | mantissa;
+        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:
@@ -199,7 +202,7 @@
     uint8_t             YPRBuffer[BLE_MPU_SERVICE_MAX_DATA_LEN];
 
 
-    uint8_t             numBytesReceived;
+    uint8_t             numBytesReceived ;
     uint8_t             receiveBufferIndex;
 
     YawPitchRollValueBytes  yprValueBytes;