EtherCAT slave that reads 3 Xsens IMU's connected to a Xbus Master
Dependencies: MODSERIAL mbed KL25Z_ClockControl
Fork of EtherCAT by
xbus.cpp
- Committer:
- vsluiter
- Date:
- 2015-03-09
- Revision:
- 27:93c0e4ae943e
- Parent:
- 26:c7959f1fd09a
- Child:
- 28:8505285f65ed
File content as of revision 27:93c0e4ae943e:
#include "xbus.h" #define ARRAY(x) x, sizeof(x) extern MODSERIAL xbus_serial; uint8_t XbusImuMsgGoToMeasurement[] = { 0xFA, 0xFF, 0x10, 0x00, 0xF1 }; uint8_t XbusImuMsgGoToConfig[] = { 0xFA, 0xFF, 0x30, 0x00, 0xD1 }; uint8_t XbusImuMsgSetPeriod[] = { 0xFA, 0xFF, 0x04, /* MID: SetPeriod */ 0x02, 0x04, /* MSB time */ 0x80, /* LSB time */ /*time in 8.68us*/ /*0480 eq. 10ms*/ 0x77 }; uint8_t XbusImuMsgSetOutputMode[] = { 0xFA, 0xFF, 0xD0, /* MID: SetOutpuMode */ 0x02, 0x40, /* MSB */ 0x00, /* LSB */ /*4000 eq. raw data mode*/ 0xEF }; uint8_t XbusImuMsgSetSyncIn_SetSync[] = {0xFA, 0xFF, 0xD6, /* MID: SetSyncIn */ 0x03, 0x00, /* SyncInMode*/ 0x00, /* Reserved */ 0x01, /* Sample ADC, RisingEdge*/ 0x27 }; uint8_t XbusImuMsgSetSyncIn_SetSkip[] = {0xFA, 0xFF, 0xD6, /* MID: SetSyncIn */ 0x03, 0x01, /* SkipFactor*/ 0x00, /* MSB SkipFactor */ 0x00, /* LSB SkipFactor */ 0x27 }; uint8_t XbusImuMsgSetSyncIn_SetOffs[] = {0xFA, 0xFF, 0xD6, /* MID: SetSyncIn */ 0x05, 0x02, /* Offset*/ 0x00, /* Bit 31..24 */ 0x01, /* Bit 23..16 */ 0x06, /* Bit 15..8 */ 0xB9, /* Bit 7..0 */ /*minimum value: 264*33.9ns*/ /*E675 eq. 2ms*/ 0x64 }; uint8_t XbusImuMsgSetSyncOut_SetSync[] = {0xFA, 0xFF, 0xD8, /* MID: SetSyncOut */ 0x03, 0x00, /* SyncOutMode*/ 0x00, /* Reserved */ 0x12, /* Postitive Pulse, Pulse Mode*/ 0x14 }; uint8_t XbusImuMsgSetSyncOut_SetSkip[] = {0xFA, 0xFF, 0xD8, /* MID: SetSyncOut */ 0x03, 0x01, /* SkipFactor*/ 0x00, /* MSB SkipFactor */ 0x00, /* LSB SkipFactor */ 0x25 }; uint8_t XbusImuMsgSetSyncOut_SetOffs[] = {0xFA, 0xFF, 0xD8, /* MID: SetSyncOut */ 0x05, 0x02, /* Offset*/ 0x00, /* Bit 31..24 */ 0x00, /* Bit 23..16 */ 0xE6, /* Bit 15..8 */ 0x75, /* Bit 7..0 */ /*minimum value: 513*33.9ns*/ /*E675 eq. 2ms*/ 0xC7 }; uint8_t XbusImuMsgSetSyncOut_SetWidth[] = {0xFA, 0xFF, 0xD8, /* MID: SetSyncOut */ 0x05, 0x03, /* Width*/ 0x00, /* Bit 31..24 */ 0x00, /* Bit 23..16 */ 0xE6, /* Bit 15..8 */ 0x75, /* Bit 7..0 */ /*minimum value: 1700*33.9ns*/ /*E675 eq. 2ms*/ 0xC6 }; static void XbusSendArray(uint8_t *array) { for(int i = 0; i < array[3]+5 ; i++ ) { //make use of len xbus_serial.putc(array[i]); } wait_ms(10); } //uses baudrate values in Xbus Master documentation void XbusSetBaudRate(uint8_t baud) { uint8_t msg[10]; XbusCreateMessage(0xFF,0x18,1,&baud,msg); XbusSendArray(msg); } void XbusReset() { uint8_t msg[10]; XbusCreateMessage(0xFF,0x40,0,msg,msg);//Reset XbusSendArray(msg); wait_ms(1000); } void XbusGoToConfig(void) { uint8_t msg[30]; XbusCreateMessage(0xFF,0x30,0,msg,msg);//GoToConfig XbusSendArray(msg); } void XbusInitializeXbusMaster(void) { uint8_t msg[30]; uint8_t data[10]; //XbusCreateMessage(0xFF,0x40,0,msg,msg);//Reset //XbusSendArray(msg); //wait_ms(1000); XbusCreateMessage(0xFF,0x30,0,msg,msg);//GoToConfig XbusSendArray(msg); data[0] = 0x04; data[1] = 0x80; XbusCreateMessage(0xFF,0x04,2,data,msg);//SetPeriod XbusSendArray(msg); data[0] = 0x40; data[1] = 0x00; for (int i = 0 ; i<3 ; i++) { XbusCreateMessage(i+1,0xD0,2,data,msg);//Set Output Data Format XbusSendArray(msg); } wait_ms(10); XbusCreateMessage(0xFF,0x10,0,msg,msg);//GoToMeasurement XbusSendArray(msg); } void XbusReceiveState(xbus_t * xbus, uint8_t rxdata) { switch(xbus->rx.state) { case XBUS_IDLE: { if(rxdata == 0xFA) { xbus->rx.counter = 0; xbus->rx.buffer[0] = 0xFA; xbus->rx.message_complete = 0; xbus->rx.state = XBUS_BID; } break; } case XBUS_BID: { xbus->rx.counter = 1; xbus->rx.buffer[xbus->rx.counter] = rxdata; xbus->rx.checksum = rxdata; xbus->rx.state = XBUS_MID; break; } case XBUS_MID: { xbus->rx.counter = 2; xbus->rx.buffer[xbus->rx.counter] = rxdata; xbus->rx.checksum = xbus->rx.checksum + rxdata; xbus->rx.state = XBUS_LEN; break; } case XBUS_LEN: { if(rxdata > XBUS_BUFFER_LENGTH - 4) // if message longer than buffer can contain xbus->rx.state = XBUS_IDLE; // EXLEN not supported! else { xbus->rx.counter = 3; xbus->rx.buffer[xbus->rx.counter] = rxdata; xbus->rx.checksum = xbus->rx.checksum + rxdata; if(rxdata == 0) // no data sent xbus->rx.state = XBUS_CS; // go to checksum else xbus->rx.state = XBUS_DATA; } break; } case XBUS_DATA: { xbus->rx.checksum += rxdata; xbus->rx.counter++; xbus->rx.buffer[xbus->rx.counter] = rxdata; if(xbus->rx.counter == (xbus->rx.buffer[3] + 3) ) // if all data received (calculated from LEN xbus->rx.state = XBUS_CS; // go to checksum break; } case XBUS_CS: { volatile uint16_t cs_calc; xbus->rx.checksum_ok = 0; xbus->rx.message_complete = 1; xbus->rx.counter++; xbus->rx.buffer[xbus->rx.counter] = rxdata; //xbus->rx.checksum = 255 - xbus->rx.checksum; xbus->rx.state = XBUS_IDLE; cs_calc = xbus->rx.checksum + rxdata; if( (cs_calc & 0x00FF) == 0) xbus->rx.checksum_ok = 1; else asm("nop"); break; } default: { xbus->rx.state = XBUS_IDLE; break; } } } void XbusSetupReceiver(xbus_t * xbus) { //xbus_pc.uart = &XBUS_UART_PC; xbus->rx.message_complete = 0; xbus->rx.state = XBUS_IDLE; xbus->rx.counter = 0; } void XbusCreateMessage(uint8_t bid, uint8_t mid, uint8_t message_size, uint8_t *message, uint8_t *buffer ) { buffer[0] = 0xFA; buffer[1] = bid; buffer[2] = mid; buffer[3] = message_size; if(message_size > 0) { for( int i = 0; i< message_size ; i++) { buffer[i+4] = message[i]; } } buffer[message_size + 4] = XbusCreateChecksum(buffer, message_size + 5); } /*Calculate xbus checksum from received message*/ uint8_t XbusCreateChecksum(uint8_t * array, uint8_t arraysize) { uint8_t counter; uint16_t temp =0; uint8_t checksum; for(counter = 1; counter < (arraysize-1) ; counter++) { //start at BID, end before checksum temp += array[counter]; } checksum = (uint8_t)(0x100 - temp); //checksum++; return checksum; }