First Last / Mbed 2 deprecated EtherCAT-XbusMaster

EtherCAT slave that reads 3 Xsens IMU's connected to a Xbus Master

Dependencies:   MODSERIAL mbed KL25Z_ClockControl

Fork of EtherCAT by First Last

xbus.cpp@16:bfc7ea6bd1af, 2015-03-03 (annotated)

Committer:
vsluiter
Date:
Tue Mar 03 15:01:29 2015 +0000
Revision:
16:bfc7ea6bd1af
Parent:
13:5e4dcbd44786
Child:
17:c5946a0fde83
Fixed compilation errors, now compiling correctly;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 13:5e4dcbd44786 1 #include "xbus.h"
vsluiter 13:5e4dcbd44786 2
vsluiter 13:5e4dcbd44786 3 #define ARRAY(x) x, sizeof(x)
vsluiter 13:5e4dcbd44786 4 static inline void XbusReceiveState(struct xbus_struct *, uint8_t );
vsluiter 16:bfc7ea6bd1af 5 uint8_t XbusImuMsgGoToMeasurement[] = { 0xFA,
vsluiter 16:bfc7ea6bd1af 6 0xFF,
vsluiter 16:bfc7ea6bd1af 7 0x10,
vsluiter 16:bfc7ea6bd1af 8 0x00,
vsluiter 16:bfc7ea6bd1af 9 0xF1
vsluiter 16:bfc7ea6bd1af 10 };
vsluiter 13:5e4dcbd44786 11
vsluiter 13:5e4dcbd44786 12 uint8_t XbusImuMsgGoToConfig[] = { 0xFA,
vsluiter 16:bfc7ea6bd1af 13 0xFF,
vsluiter 16:bfc7ea6bd1af 14 0x30,
vsluiter 16:bfc7ea6bd1af 15 0x00,
vsluiter 16:bfc7ea6bd1af 16 0xD1
vsluiter 16:bfc7ea6bd1af 17 };
vsluiter 13:5e4dcbd44786 18
vsluiter 16:bfc7ea6bd1af 19 uint8_t XbusImuMsgSetPeriod[] = { 0xFA,
vsluiter 16:bfc7ea6bd1af 20 0xFF,
vsluiter 16:bfc7ea6bd1af 21 0x04, /* MID: SetPeriod */
vsluiter 16:bfc7ea6bd1af 22 0x02,
vsluiter 16:bfc7ea6bd1af 23 0x04, /* MSB time */
vsluiter 16:bfc7ea6bd1af 24 0x80, /* LSB time */ /*time in 8.68us*/ /*0480 eq. 10ms*/
vsluiter 16:bfc7ea6bd1af 25 0x77
vsluiter 16:bfc7ea6bd1af 26 };
vsluiter 13:5e4dcbd44786 27
vsluiter 16:bfc7ea6bd1af 28 uint8_t XbusImuMsgSetOutputMode[] = { 0xFA,
vsluiter 16:bfc7ea6bd1af 29 0xFF,
vsluiter 16:bfc7ea6bd1af 30 0xD0, /* MID: SetOutpuMode */
vsluiter 16:bfc7ea6bd1af 31 0x02,
vsluiter 16:bfc7ea6bd1af 32 0x40, /* MSB */
vsluiter 16:bfc7ea6bd1af 33 0x00, /* LSB */ /*4000 eq. raw data mode*/
vsluiter 16:bfc7ea6bd1af 34 0xEF
vsluiter 16:bfc7ea6bd1af 35 };
vsluiter 13:5e4dcbd44786 36
vsluiter 16:bfc7ea6bd1af 37 uint8_t XbusImuMsgSetSyncIn_SetSync[] = {0xFA,
vsluiter 16:bfc7ea6bd1af 38 0xFF,
vsluiter 16:bfc7ea6bd1af 39 0xD6, /* MID: SetSyncIn */
vsluiter 16:bfc7ea6bd1af 40 0x03,
vsluiter 16:bfc7ea6bd1af 41 0x00, /* SyncInMode*/
vsluiter 16:bfc7ea6bd1af 42 0x00, /* Reserved */
vsluiter 16:bfc7ea6bd1af 43 0x01, /* Sample ADC, RisingEdge*/
vsluiter 16:bfc7ea6bd1af 44 0x27
vsluiter 16:bfc7ea6bd1af 45 };
vsluiter 13:5e4dcbd44786 46
vsluiter 16:bfc7ea6bd1af 47 uint8_t XbusImuMsgSetSyncIn_SetSkip[] = {0xFA,
vsluiter 16:bfc7ea6bd1af 48 0xFF,
vsluiter 16:bfc7ea6bd1af 49 0xD6, /* MID: SetSyncIn */
vsluiter 16:bfc7ea6bd1af 50 0x03,
vsluiter 16:bfc7ea6bd1af 51 0x01, /* SkipFactor*/
vsluiter 16:bfc7ea6bd1af 52 0x00, /* MSB SkipFactor */
vsluiter 16:bfc7ea6bd1af 53 0x00, /* LSB SkipFactor */
vsluiter 16:bfc7ea6bd1af 54 0x27
vsluiter 16:bfc7ea6bd1af 55 };
vsluiter 13:5e4dcbd44786 56
vsluiter 16:bfc7ea6bd1af 57 uint8_t XbusImuMsgSetSyncIn_SetOffs[] = {0xFA,
vsluiter 16:bfc7ea6bd1af 58 0xFF,
vsluiter 16:bfc7ea6bd1af 59 0xD6, /* MID: SetSyncIn */
vsluiter 16:bfc7ea6bd1af 60 0x05,
vsluiter 16:bfc7ea6bd1af 61 0x02, /* Offset*/
vsluiter 16:bfc7ea6bd1af 62 0x00, /* Bit 31..24 */
vsluiter 16:bfc7ea6bd1af 63 0x01, /* Bit 23..16 */
vsluiter 16:bfc7ea6bd1af 64 0x06, /* Bit 15..8 */
vsluiter 16:bfc7ea6bd1af 65 0xB9, /* Bit 7..0 */ /*minimum value: 264*33.9ns*/ /*E675 eq. 2ms*/
vsluiter 16:bfc7ea6bd1af 66 0x64
vsluiter 16:bfc7ea6bd1af 67 };
vsluiter 13:5e4dcbd44786 68
vsluiter 16:bfc7ea6bd1af 69 uint8_t XbusImuMsgSetSyncOut_SetSync[] = {0xFA,
vsluiter 16:bfc7ea6bd1af 70 0xFF,
vsluiter 16:bfc7ea6bd1af 71 0xD8, /* MID: SetSyncOut */
vsluiter 16:bfc7ea6bd1af 72 0x03,
vsluiter 16:bfc7ea6bd1af 73 0x00, /* SyncOutMode*/
vsluiter 16:bfc7ea6bd1af 74 0x00, /* Reserved */
vsluiter 16:bfc7ea6bd1af 75 0x12, /* Postitive Pulse, Pulse Mode*/
vsluiter 16:bfc7ea6bd1af 76 0x14
vsluiter 16:bfc7ea6bd1af 77 };
vsluiter 13:5e4dcbd44786 78
vsluiter 16:bfc7ea6bd1af 79 uint8_t XbusImuMsgSetSyncOut_SetSkip[] = {0xFA,
vsluiter 16:bfc7ea6bd1af 80 0xFF,
vsluiter 16:bfc7ea6bd1af 81 0xD8, /* MID: SetSyncOut */
vsluiter 16:bfc7ea6bd1af 82 0x03,
vsluiter 16:bfc7ea6bd1af 83 0x01, /* SkipFactor*/
vsluiter 16:bfc7ea6bd1af 84 0x00, /* MSB SkipFactor */
vsluiter 16:bfc7ea6bd1af 85 0x00, /* LSB SkipFactor */
vsluiter 16:bfc7ea6bd1af 86 0x25
vsluiter 16:bfc7ea6bd1af 87 };
vsluiter 13:5e4dcbd44786 88
vsluiter 16:bfc7ea6bd1af 89 uint8_t XbusImuMsgSetSyncOut_SetOffs[] = {0xFA,
vsluiter 16:bfc7ea6bd1af 90 0xFF,
vsluiter 16:bfc7ea6bd1af 91 0xD8, /* MID: SetSyncOut */
vsluiter 16:bfc7ea6bd1af 92 0x05,
vsluiter 16:bfc7ea6bd1af 93 0x02, /* Offset*/
vsluiter 16:bfc7ea6bd1af 94 0x00, /* Bit 31..24 */
vsluiter 16:bfc7ea6bd1af 95 0x00, /* Bit 23..16 */
vsluiter 16:bfc7ea6bd1af 96 0xE6, /* Bit 15..8 */
vsluiter 16:bfc7ea6bd1af 97 0x75, /* Bit 7..0 */ /*minimum value: 513*33.9ns*/ /*E675 eq. 2ms*/
vsluiter 16:bfc7ea6bd1af 98 0xC7
vsluiter 16:bfc7ea6bd1af 99 };
vsluiter 13:5e4dcbd44786 100
vsluiter 16:bfc7ea6bd1af 101 uint8_t XbusImuMsgSetSyncOut_SetWidth[] = {0xFA,
vsluiter 16:bfc7ea6bd1af 102 0xFF,
vsluiter 16:bfc7ea6bd1af 103 0xD8, /* MID: SetSyncOut */
vsluiter 16:bfc7ea6bd1af 104 0x05,
vsluiter 16:bfc7ea6bd1af 105 0x03, /* Width*/
vsluiter 16:bfc7ea6bd1af 106 0x00, /* Bit 31..24 */
vsluiter 16:bfc7ea6bd1af 107 0x00, /* Bit 23..16 */
vsluiter 16:bfc7ea6bd1af 108 0xE6, /* Bit 15..8 */
vsluiter 16:bfc7ea6bd1af 109 0x75, /* Bit 7..0 */ /*minimum value: 1700*33.9ns*/ /*E675 eq. 2ms*/
vsluiter 16:bfc7ea6bd1af 110 0xC6
vsluiter 16:bfc7ea6bd1af 111 };
vsluiter 13:5e4dcbd44786 112
vsluiter 13:5e4dcbd44786 113 static void XbusSendArray(uint8_t *array, uint8_t size)
vsluiter 13:5e4dcbd44786 114 {
vsluiter 16:bfc7ea6bd1af 115 for(int i = 0; i < size ; i++ ) {
vsluiter 16:bfc7ea6bd1af 116 xbus.putc(array[i]);
vsluiter 16:bfc7ea6bd1af 117 }
vsluiter 16:bfc7ea6bd1af 118 wait_ms(10);
vsluiter 13:5e4dcbd44786 119 }
vsluiter 13:5e4dcbd44786 120
vsluiter 13:5e4dcbd44786 121 void XbusInitializeXbusMaster(void)
vsluiter 13:5e4dcbd44786 122 {
vsluiter 16:bfc7ea6bd1af 123 XbusSendArray(ARRAY(XbusImuMsgGoToConfig));
vsluiter 16:bfc7ea6bd1af 124 XbusSendArray(ARRAY(XbusImuMsgSetPeriod));
vsluiter 16:bfc7ea6bd1af 125 XbusSendArray(ARRAY(XbusImuMsgSetOutputMode));
vsluiter 16:bfc7ea6bd1af 126 XbusSendArray(ARRAY(XbusImuMsgGoToMeasurement));
vsluiter 13:5e4dcbd44786 127 }
vsluiter 13:5e4dcbd44786 128
vsluiter 13:5e4dcbd44786 129 void XbusReceiveState(struct xbus_struct * xbus, uint8_t rxdata)
vsluiter 13:5e4dcbd44786 130 {
vsluiter 16:bfc7ea6bd1af 131 switch(xbus->rx.state) {
vsluiter 16:bfc7ea6bd1af 132 case XBUS_IDLE: {
vsluiter 16:bfc7ea6bd1af 133 if(rxdata == 0xFA) {
vsluiter 13:5e4dcbd44786 134 xbus->rx.counter = 0;
vsluiter 13:5e4dcbd44786 135 xbus->rx.buffer[0] = 0xFA;
vsluiter 13:5e4dcbd44786 136 xbus->rx.message_complete = 0;
vsluiter 13:5e4dcbd44786 137 xbus->rx.state = XBUS_BID;
vsluiter 13:5e4dcbd44786 138 }
vsluiter 13:5e4dcbd44786 139 break;
vsluiter 13:5e4dcbd44786 140 }
vsluiter 16:bfc7ea6bd1af 141 case XBUS_BID: {
vsluiter 13:5e4dcbd44786 142 xbus->rx.counter = 1;
vsluiter 13:5e4dcbd44786 143 xbus->rx.buffer[xbus->rx.counter] = rxdata;
vsluiter 13:5e4dcbd44786 144 xbus->rx.checksum = rxdata;
vsluiter 13:5e4dcbd44786 145 xbus->rx.state = XBUS_MID;
vsluiter 13:5e4dcbd44786 146 break;
vsluiter 13:5e4dcbd44786 147 }
vsluiter 16:bfc7ea6bd1af 148 case XBUS_MID: {
vsluiter 13:5e4dcbd44786 149 xbus->rx.counter = 2;
vsluiter 13:5e4dcbd44786 150 xbus->rx.buffer[xbus->rx.counter] = rxdata;
vsluiter 13:5e4dcbd44786 151 xbus->rx.checksum = xbus->rx.checksum + rxdata;
vsluiter 13:5e4dcbd44786 152 xbus->rx.state = XBUS_LEN;
vsluiter 13:5e4dcbd44786 153 break;
vsluiter 13:5e4dcbd44786 154 }
vsluiter 16:bfc7ea6bd1af 155 case XBUS_LEN: {
vsluiter 13:5e4dcbd44786 156 if(rxdata > XBUS_BUFFER_LENGTH - 4) // if message longer than buffer can contain
vsluiter 13:5e4dcbd44786 157 xbus->rx.state = XBUS_IDLE; // EXLEN not supported!
vsluiter 16:bfc7ea6bd1af 158 else {
vsluiter 13:5e4dcbd44786 159 xbus->rx.counter = 3;
vsluiter 13:5e4dcbd44786 160 xbus->rx.buffer[xbus->rx.counter] = rxdata;
vsluiter 13:5e4dcbd44786 161 xbus->rx.checksum = xbus->rx.checksum + rxdata;
vsluiter 13:5e4dcbd44786 162 if(rxdata == 0) // no data sent
vsluiter 13:5e4dcbd44786 163 xbus->rx.state = XBUS_CS; // go to checksum
vsluiter 13:5e4dcbd44786 164 else
vsluiter 13:5e4dcbd44786 165 xbus->rx.state = XBUS_DATA;
vsluiter 13:5e4dcbd44786 166 }
vsluiter 13:5e4dcbd44786 167 break;
vsluiter 13:5e4dcbd44786 168 }
vsluiter 16:bfc7ea6bd1af 169 case XBUS_DATA: {
vsluiter 13:5e4dcbd44786 170 xbus->rx.checksum += rxdata;
vsluiter 13:5e4dcbd44786 171 xbus->rx.counter++;
vsluiter 13:5e4dcbd44786 172 xbus->rx.buffer[xbus->rx.counter] = rxdata;
vsluiter 13:5e4dcbd44786 173 if(xbus->rx.counter == (xbus->rx.buffer[3] + 3) ) // if all data received (calculated from LEN
vsluiter 13:5e4dcbd44786 174 xbus->rx.state = XBUS_CS; // go to checksum
vsluiter 13:5e4dcbd44786 175 break;
vsluiter 13:5e4dcbd44786 176 }
vsluiter 16:bfc7ea6bd1af 177 case XBUS_CS: {
vsluiter 16:bfc7ea6bd1af 178 volatile uint16_t cs_calc;
vsluiter 13:5e4dcbd44786 179 xbus->rx.checksum_ok = 0;
vsluiter 13:5e4dcbd44786 180 xbus->rx.message_complete = 1;
vsluiter 13:5e4dcbd44786 181 xbus->rx.counter++;
vsluiter 13:5e4dcbd44786 182 xbus->rx.buffer[xbus->rx.counter] = rxdata;
vsluiter 13:5e4dcbd44786 183 //xbus->rx.checksum = 255 - xbus->rx.checksum;
vsluiter 16:bfc7ea6bd1af 184 xbus->rx.state = XBUS_IDLE;
vsluiter 13:5e4dcbd44786 185 cs_calc = xbus->rx.checksum + rxdata;
vsluiter 16:bfc7ea6bd1af 186 if( (cs_calc & 0x00FF) == 0)
vsluiter 13:5e4dcbd44786 187 xbus->rx.checksum_ok = 1;
vsluiter 16:bfc7ea6bd1af 188 else
vsluiter 16:bfc7ea6bd1af 189 asm("nop");
vsluiter 13:5e4dcbd44786 190 break;
vsluiter 13:5e4dcbd44786 191 }
vsluiter 16:bfc7ea6bd1af 192 default: {
vsluiter 13:5e4dcbd44786 193 xbus->rx.state = XBUS_IDLE;
vsluiter 13:5e4dcbd44786 194 break;
vsluiter 13:5e4dcbd44786 195 }
vsluiter 13:5e4dcbd44786 196 }
vsluiter 13:5e4dcbd44786 197 }
vsluiter 13:5e4dcbd44786 198
vsluiter 13:5e4dcbd44786 199
vsluiter 13:5e4dcbd44786 200 void XbusSetupReceiver(struct xbus_struct * xbus)
vsluiter 13:5e4dcbd44786 201 {
vsluiter 16:bfc7ea6bd1af 202 //xbus_pc.uart = &XBUS_UART_PC;
vsluiter 13:5e4dcbd44786 203 xbus->rx.message_complete = 0;
vsluiter 13:5e4dcbd44786 204 xbus->rx.state = XBUS_IDLE;
vsluiter 13:5e4dcbd44786 205 xbus->rx.counter = 0;
vsluiter 13:5e4dcbd44786 206 }
vsluiter 13:5e4dcbd44786 207
vsluiter 13:5e4dcbd44786 208
vsluiter 13:5e4dcbd44786 209 /*Calculate xbus checksum from received message*/
vsluiter 13:5e4dcbd44786 210 uint8_t XbusCreateChecksum(uint8_t * array, uint8_t arraysize)
vsluiter 13:5e4dcbd44786 211 {
vsluiter 16:bfc7ea6bd1af 212 uint8_t counter;
vsluiter 16:bfc7ea6bd1af 213 uint16_t temp =0;
vsluiter 16:bfc7ea6bd1af 214 uint8_t checksum;
vsluiter 16:bfc7ea6bd1af 215 for(counter = 1; counter < (arraysize-1) ; counter++) { //start at BID, end before checksum
vsluiter 16:bfc7ea6bd1af 216 temp += array[counter];
vsluiter 16:bfc7ea6bd1af 217 }
vsluiter 16:bfc7ea6bd1af 218 checksum = (uint8_t)(0x100 - temp);
vsluiter 16:bfc7ea6bd1af 219 //checksum++;
vsluiter 16:bfc7ea6bd1af 220 return checksum;
vsluiter 13:5e4dcbd44786 221 }