Diff: soes.cpp

Revision:

39:8c5329c37de1

Parent:

37:d00717883d98

Child:

40:0528d79e63f2

--- a/soes.cpp	Fri Sep 11 11:59:04 2015 +0000
+++ b/soes.cpp	Fri Sep 11 14:20:31 2015 +0000
@@ -270,11 +270,15 @@
         	{
         		if(xbus_master.rx.buffer[2] == 0x32)
         		{
-        			memcpy_byteswap((uint8_t *)&local_Rb.timestamp   ,&xbus_master.rx.buffer[4], 2);
-        			memcpy_floatswap((uint8_t *)&local_Rb.first.q0,&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;
+        			for(int sensor = 0; sensor < 3 ; sensor++)
+        			{
+        				const int num_bytes = 50;//4 floats * 4 bytes
+	        			//memcpy_byteswap((uint8_t *)&local_Rb.first.timestamp   ,&xbus_master.rx.buffer[4], 2);
+	        			memcpy_floatswap((uint8_t *)&local_Rb.xsens_imu[sensor].q0,&xbus_master.rx.buffer[6+(sensor*num_bytes)], num_bytes);
+	        			memcpy_byteswap((uint8_t *)&local_Rb.xsens_imu[sensor].samplecounter,&xbus_master.rx.buffer[6+num_bytes+(sensor*num_bytes)],2);
+	        			//pc.printf("%x %x\n",Rb.timestamp, *((uint16_t *)(&xbus_master.rx.buffer[4])));
+	        			PLOEP;
+        			}
         		}
         		xbus_master.rx.checksum_ok = 0;
         	}