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:
- 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);