First Last
EtherCAT slave that reads 3 Xsens IMU's connected to a Xbus Master
Dependencies: MODSERIAL mbed KL25Z_ClockControl
Fork of
EtherCAT
by 
First Last
Diff: xbus.cpp
- Revision:
- 13:5e4dcbd44786
- Child:
- 16:bfc7ea6bd1af
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/xbus.cpp Tue Mar 03 12:14:22 2015 +0000
@@ -0,0 +1,222 @@
+#include "xbus.h"
+
+#define ARRAY(x) x, sizeof(x)
+static inline void XbusReceiveState(struct xbus_struct *, uint8_t );
+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, uint8_t size)
+{
+ for(int i = 0; i < size ; i++ )
+ {
+ xbus.putc(array[i]);
+ }
+ wait_ms(10);
+}
+
+void XbusInitializeXbusMaster(void)
+{
+ XbusSendArray(ARRAY(XbusImuMsgGoToConfig));
+ XbusSendArray(ARRAY(XbusImuMsgSetPeriod));
+ XbusSendArray(ARRAY(XbusImuMsgSetOutputMode));
+ XbusSendArray(ARRAY(XbusImuMsgGoToMeasurement));
+}
+
+void XbusReceiveState(struct xbus_struct * 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(struct xbus_struct * xbus)
+{
+ xbus_pc.uart = &XBUS_UART_PC;
+ xbus->rx.message_complete = 0;
+ xbus->rx.state = XBUS_IDLE;
+ xbus->rx.counter = 0;
+}
+
+
+/*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;
+}