Diff: soes.cpp

Revision:

33:b4844666684f

Parent:

32:1f6a705cd794

Child:

37:d00717883d98

--- a/soes.cpp	Tue Mar 10 11:45:32 2015 +0000
+++ b/soes.cpp	Tue Mar 10 12:09:58 2015 +0000
@@ -192,6 +192,21 @@
     } 
 }
 
+float ReverseFloat( const float inFloat )
+{
+   float retVal;
+   char *floatToConvert = ( char* ) & inFloat;
+   char *returnFloat = ( char* ) & retVal;
+
+   // swap the bytes into a temporary buffer
+   returnFloat[0] = floatToConvert[3];
+   returnFloat[1] = floatToConvert[2];
+   returnFloat[2] = floatToConvert[1];
+   returnFloat[3] = floatToConvert[0];
+
+   return retVal;
+}
+
 //Watch out, this is an uggly fix; for odd numbers of num_bytes, still the trailing byte will be used too.
 //No memory protection what so ever.
 void memcpy_byteswap(uint8_t * dest, uint8_t * source, uint16_t num_bytes)
@@ -203,6 +218,20 @@
 	}
 }
 
+//Watch out, this is an uggly fix; for odd numbers of num_bytes, still the trailing byte will be used too.
+//No memory protection what so ever.
+void memcpy_floatswap(uint8_t * dest, uint8_t * source, uint16_t num_bytes)
+{
+	for( int i = 0 ; i < num_bytes ; i+=4 )
+	{
+		dest[i]   = source[i+3];
+		dest[i+1] = source[i+2];
+		dest[i+2]   = source[i+1];
+		dest[i+3] = source[i];
+	}
+}
+
+
 int main(void)
 {
 
@@ -242,7 +271,8 @@
         		if(xbus_master.rx.buffer[2] == 0x32)
         		{
         			memcpy_byteswap((uint8_t *)&local_Rb.timestamp   ,&xbus_master.rx.buffer[4], 2);
-        			memcpy_byteswap((uint8_t *)&local_Rb.first.acc[0],&xbus_master.rx.buffer[6],xbus_master.rx.buffer[3] - 2);
+        			memcpy_floatswap((uint8_t *)&local_Rb.first.acc[0],&xbus_master.rx.buffer[6],xbus_master.rx.buffer[3] - 2);
+        			//memcpy_byteswap((uint8_t *)&local_Rb.first.acc[0],&xbus_master.rx.buffer[6],xbus_master.rx.buffer[3] - 2);
         			//pc.printf("%x %x\n",Rb.timestamp, *((uint16_t *)(&xbus_master.rx.buffer[4])));
         			PLOEP;
         		}