Diff: soes.cpp

Revision:

21:6150ca891301

Parent:

20:ad13d4f5fd98

Child:

22:3f3a9f55054c

--- a/soes.cpp	Fri Mar 06 08:27:52 2015 +0000
+++ b/soes.cpp	Fri Mar 06 09:16:18 2015 +0000
@@ -69,10 +69,10 @@
 SPI et1100_spi(ET1100_MOSI,ET1100_MISO,ET1100_SCK);
 MODSERIAL xbus_serial(PTC4,PTC3);
 xbus_t xbus_master;
-
+//MODSERIAL pc(USBTX,USBRX);
 
-DigitalOut ploep(PTA13);
-#define PLOEP do{ploep = !(ploep);}while(0);
+//DigitalOut ploep(PTA13);
+//#define PLOEP do{ploep = !(ploep);}while(0);
 
 /* Private function prototypes -----------------------------------------------*/
 /* Private functions ---------------------------------------------------------*/
@@ -172,8 +172,16 @@
     } 
 }
 
-
-
+//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)
+{
+	for( int i = 0 ; i < num_bytes ; i+=2 )
+	{
+		dest[i]   = source[i+1];
+		dest[i+1] = source[i];
+	}
+}
 
 int main(void)
 {
@@ -211,10 +219,11 @@
         	{
         		if(xbus_master.rx.buffer[2] == 0x32)
         		{
-        			memcpy(&Rb.first.acc[0],&xbus_master.rx.buffer[4],60);
+        			memcpy_byteswap((uint8_t *)&Rb.first.acc[0],&xbus_master.rx.buffer[6],60);
+        			//pc.printf("%u , %u\n", Rb.first.acc[0] , *((uint16_t *)(&xbus_master.rx.buffer[6])) );
         		}
         		xbus_master.rx.checksum_ok = 0;
-        		PLOEP;
+        		//PLOEP;
         	}
     	}
         ESC_read(ESCREG_LOCALTIME, &ESCvar.Time, sizeof(ESCvar.Time), &ESCvar.ALevent);