EtherCAT slave that reads 3 Xsens IMU's connected to a Xbus Master
Dependencies: MODSERIAL mbed KL25Z_ClockControl
Fork of EtherCAT by
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; }