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