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@42:ef78d11f2bd7, 2015-09-11 (annotated)

Committer:
vsluiter
Date:
Fri Sep 11 15:33:31 2015 +0000
Revision:
42:ef78d11f2bd7
Parent:
36:8e6ac6082e97 
Also implemented samplecounter

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 18:6629e8c5d59e 4
vsluiter 18:6629e8c5d59e 5 extern MODSERIAL xbus_serial;
vsluiter 18:6629e8c5d59e 6
vsluiter 16:bfc7ea6bd1af 7 uint8_t XbusImuMsgGoToMeasurement[] = { 0xFA,
vsluiter 16:bfc7ea6bd1af 8 0xFF,
vsluiter 16:bfc7ea6bd1af 9 0x10,
vsluiter 16:bfc7ea6bd1af 10 0x00,
vsluiter 16:bfc7ea6bd1af 11 0xF1
vsluiter 16:bfc7ea6bd1af 12 };
vsluiter 13:5e4dcbd44786 13
vsluiter 13:5e4dcbd44786 14 uint8_t XbusImuMsgGoToConfig[] = { 0xFA,
vsluiter 16:bfc7ea6bd1af 15 0xFF,
vsluiter 16:bfc7ea6bd1af 16 0x30,
vsluiter 16:bfc7ea6bd1af 17 0x00,
vsluiter 16:bfc7ea6bd1af 18 0xD1
vsluiter 16:bfc7ea6bd1af 19 };
vsluiter 13:5e4dcbd44786 20
vsluiter 16:bfc7ea6bd1af 21 uint8_t XbusImuMsgSetPeriod[] = { 0xFA,
vsluiter 16:bfc7ea6bd1af 22 0xFF,
vsluiter 16:bfc7ea6bd1af 23 0x04, /* MID: SetPeriod */
vsluiter 16:bfc7ea6bd1af 24 0x02,
vsluiter 16:bfc7ea6bd1af 25 0x04, /* MSB time */
vsluiter 16:bfc7ea6bd1af 26 0x80, /* LSB time */ /*time in 8.68us*/ /*0480 eq. 10ms*/
vsluiter 16:bfc7ea6bd1af 27 0x77
vsluiter 16:bfc7ea6bd1af 28 };
vsluiter 13:5e4dcbd44786 29
vsluiter 16:bfc7ea6bd1af 30 uint8_t XbusImuMsgSetOutputMode[] = { 0xFA,
vsluiter 16:bfc7ea6bd1af 31 0xFF,
vsluiter 16:bfc7ea6bd1af 32 0xD0, /* MID: SetOutpuMode */
vsluiter 16:bfc7ea6bd1af 33 0x02,
vsluiter 16:bfc7ea6bd1af 34 0x40, /* MSB */
vsluiter 16:bfc7ea6bd1af 35 0x00, /* LSB */ /*4000 eq. raw data mode*/
vsluiter 16:bfc7ea6bd1af 36 0xEF
vsluiter 16:bfc7ea6bd1af 37 };
vsluiter 13:5e4dcbd44786 38
vsluiter 16:bfc7ea6bd1af 39 uint8_t XbusImuMsgSetSyncIn_SetSync[] = {0xFA,
vsluiter 16:bfc7ea6bd1af 40 0xFF,
vsluiter 16:bfc7ea6bd1af 41 0xD6, /* MID: SetSyncIn */
vsluiter 16:bfc7ea6bd1af 42 0x03,
vsluiter 16:bfc7ea6bd1af 43 0x00, /* SyncInMode*/
vsluiter 16:bfc7ea6bd1af 44 0x00, /* Reserved */
vsluiter 16:bfc7ea6bd1af 45 0x01, /* Sample ADC, RisingEdge*/
vsluiter 16:bfc7ea6bd1af 46 0x27
vsluiter 16:bfc7ea6bd1af 47 };
vsluiter 13:5e4dcbd44786 48
vsluiter 16:bfc7ea6bd1af 49 uint8_t XbusImuMsgSetSyncIn_SetSkip[] = {0xFA,
vsluiter 16:bfc7ea6bd1af 50 0xFF,
vsluiter 16:bfc7ea6bd1af 51 0xD6, /* MID: SetSyncIn */
vsluiter 16:bfc7ea6bd1af 52 0x03,
vsluiter 16:bfc7ea6bd1af 53 0x01, /* SkipFactor*/
vsluiter 16:bfc7ea6bd1af 54 0x00, /* MSB SkipFactor */
vsluiter 16:bfc7ea6bd1af 55 0x00, /* LSB SkipFactor */
vsluiter 16:bfc7ea6bd1af 56 0x27
vsluiter 16:bfc7ea6bd1af 57 };
vsluiter 13:5e4dcbd44786 58
vsluiter 16:bfc7ea6bd1af 59 uint8_t XbusImuMsgSetSyncIn_SetOffs[] = {0xFA,
vsluiter 16:bfc7ea6bd1af 60 0xFF,
vsluiter 16:bfc7ea6bd1af 61 0xD6, /* MID: SetSyncIn */
vsluiter 16:bfc7ea6bd1af 62 0x05,
vsluiter 16:bfc7ea6bd1af 63 0x02, /* Offset*/
vsluiter 16:bfc7ea6bd1af 64 0x00, /* Bit 31..24 */
vsluiter 16:bfc7ea6bd1af 65 0x01, /* Bit 23..16 */
vsluiter 16:bfc7ea6bd1af 66 0x06, /* Bit 15..8 */
vsluiter 16:bfc7ea6bd1af 67 0xB9, /* Bit 7..0 */ /*minimum value: 264*33.9ns*/ /*E675 eq. 2ms*/
vsluiter 16:bfc7ea6bd1af 68 0x64
vsluiter 16:bfc7ea6bd1af 69 };
vsluiter 13:5e4dcbd44786 70
vsluiter 16:bfc7ea6bd1af 71 uint8_t XbusImuMsgSetSyncOut_SetSync[] = {0xFA,
vsluiter 16:bfc7ea6bd1af 72 0xFF,
vsluiter 16:bfc7ea6bd1af 73 0xD8, /* MID: SetSyncOut */
vsluiter 16:bfc7ea6bd1af 74 0x03,
vsluiter 16:bfc7ea6bd1af 75 0x00, /* SyncOutMode*/
vsluiter 16:bfc7ea6bd1af 76 0x00, /* Reserved */
vsluiter 16:bfc7ea6bd1af 77 0x12, /* Postitive Pulse, Pulse Mode*/
vsluiter 16:bfc7ea6bd1af 78 0x14
vsluiter 16:bfc7ea6bd1af 79 };
vsluiter 13:5e4dcbd44786 80
vsluiter 16:bfc7ea6bd1af 81 uint8_t XbusImuMsgSetSyncOut_SetSkip[] = {0xFA,
vsluiter 16:bfc7ea6bd1af 82 0xFF,
vsluiter 16:bfc7ea6bd1af 83 0xD8, /* MID: SetSyncOut */
vsluiter 16:bfc7ea6bd1af 84 0x03,
vsluiter 16:bfc7ea6bd1af 85 0x01, /* SkipFactor*/
vsluiter 16:bfc7ea6bd1af 86 0x00, /* MSB SkipFactor */
vsluiter 16:bfc7ea6bd1af 87 0x00, /* LSB SkipFactor */
vsluiter 16:bfc7ea6bd1af 88 0x25
vsluiter 16:bfc7ea6bd1af 89 };
vsluiter 13:5e4dcbd44786 90
vsluiter 16:bfc7ea6bd1af 91 uint8_t XbusImuMsgSetSyncOut_SetOffs[] = {0xFA,
vsluiter 16:bfc7ea6bd1af 92 0xFF,
vsluiter 16:bfc7ea6bd1af 93 0xD8, /* MID: SetSyncOut */
vsluiter 16:bfc7ea6bd1af 94 0x05,
vsluiter 16:bfc7ea6bd1af 95 0x02, /* Offset*/
vsluiter 16:bfc7ea6bd1af 96 0x00, /* Bit 31..24 */
vsluiter 16:bfc7ea6bd1af 97 0x00, /* Bit 23..16 */
vsluiter 16:bfc7ea6bd1af 98 0xE6, /* Bit 15..8 */
vsluiter 16:bfc7ea6bd1af 99 0x75, /* Bit 7..0 */ /*minimum value: 513*33.9ns*/ /*E675 eq. 2ms*/
vsluiter 16:bfc7ea6bd1af 100 0xC7
vsluiter 16:bfc7ea6bd1af 101 };
vsluiter 13:5e4dcbd44786 102
vsluiter 16:bfc7ea6bd1af 103 uint8_t XbusImuMsgSetSyncOut_SetWidth[] = {0xFA,
vsluiter 16:bfc7ea6bd1af 104 0xFF,
vsluiter 16:bfc7ea6bd1af 105 0xD8, /* MID: SetSyncOut */
vsluiter 16:bfc7ea6bd1af 106 0x05,
vsluiter 16:bfc7ea6bd1af 107 0x03, /* Width*/
vsluiter 16:bfc7ea6bd1af 108 0x00, /* Bit 31..24 */
vsluiter 16:bfc7ea6bd1af 109 0x00, /* Bit 23..16 */
vsluiter 16:bfc7ea6bd1af 110 0xE6, /* Bit 15..8 */
vsluiter 16:bfc7ea6bd1af 111 0x75, /* Bit 7..0 */ /*minimum value: 1700*33.9ns*/ /*E675 eq. 2ms*/
vsluiter 16:bfc7ea6bd1af 112 0xC6
vsluiter 16:bfc7ea6bd1af 113 };
vsluiter 13:5e4dcbd44786 114
vsluiter 17:c5946a0fde83 115 static void XbusSendArray(uint8_t *array)
vsluiter 13:5e4dcbd44786 116 {
vsluiter 17:c5946a0fde83 117 for(int i = 0; i < array[3]+5 ; i++ ) { //make use of len
vsluiter 18:6629e8c5d59e 118 xbus_serial.putc(array[i]);
vsluiter 16:bfc7ea6bd1af 119 }
vsluiter 16:bfc7ea6bd1af 120 wait_ms(10);
vsluiter 13:5e4dcbd44786 121 }
vsluiter 13:5e4dcbd44786 122
vsluiter 26:c7959f1fd09a 123
vsluiter 26:c7959f1fd09a 124 //uses baudrate values in Xbus Master documentation
vsluiter 26:c7959f1fd09a 125 void XbusSetBaudRate(uint8_t baud)
vsluiter 26:c7959f1fd09a 126 {
vsluiter 26:c7959f1fd09a 127 uint8_t msg[10];
vsluiter 26:c7959f1fd09a 128 XbusCreateMessage(0xFF,0x18,1,&baud,msg);
vsluiter 26:c7959f1fd09a 129 XbusSendArray(msg);
vsluiter 26:c7959f1fd09a 130 }
vsluiter 26:c7959f1fd09a 131
vsluiter 26:c7959f1fd09a 132 void XbusReset()
vsluiter 26:c7959f1fd09a 133 {
vsluiter 26:c7959f1fd09a 134 uint8_t msg[10];
vsluiter 26:c7959f1fd09a 135 XbusCreateMessage(0xFF,0x40,0,msg,msg);//Reset
vsluiter 26:c7959f1fd09a 136 XbusSendArray(msg);
vsluiter 26:c7959f1fd09a 137 wait_ms(1000);
vsluiter 26:c7959f1fd09a 138 }
vsluiter 26:c7959f1fd09a 139
vsluiter 27:93c0e4ae943e 140 void XbusGoToConfig(void)
vsluiter 27:93c0e4ae943e 141 {
vsluiter 27:93c0e4ae943e 142 uint8_t msg[30];
vsluiter 27:93c0e4ae943e 143 XbusCreateMessage(0xFF,0x30,0,msg,msg);//GoToConfig
vsluiter 27:93c0e4ae943e 144 XbusSendArray(msg);
vsluiter 27:93c0e4ae943e 145 }
vsluiter 27:93c0e4ae943e 146
vsluiter 13:5e4dcbd44786 147 void XbusInitializeXbusMaster(void)
vsluiter 13:5e4dcbd44786 148 {
vsluiter 17:c5946a0fde83 149 uint8_t msg[30];
vsluiter 17:c5946a0fde83 150 uint8_t data[10];
vsluiter 17:c5946a0fde83 151
vsluiter 17:c5946a0fde83 152 //XbusCreateMessage(0xFF,0x40,0,msg,msg);//Reset
vsluiter 17:c5946a0fde83 153 //XbusSendArray(msg);
vsluiter 17:c5946a0fde83 154 //wait_ms(1000);
vsluiter 17:c5946a0fde83 155
vsluiter 17:c5946a0fde83 156 XbusCreateMessage(0xFF,0x30,0,msg,msg);//GoToConfig
vsluiter 17:c5946a0fde83 157 XbusSendArray(msg);
vsluiter 17:c5946a0fde83 158
vsluiter 17:c5946a0fde83 159
vsluiter 17:c5946a0fde83 160 data[0] = 0x04;
vsluiter 17:c5946a0fde83 161 data[1] = 0x80;
vsluiter 17:c5946a0fde83 162 XbusCreateMessage(0xFF,0x04,2,data,msg);//SetPeriod
vsluiter 17:c5946a0fde83 163 XbusSendArray(msg);
vsluiter 17:c5946a0fde83 164
vsluiter 36:8e6ac6082e97 165 /*
vsluiter 36:8e6ac6082e97 166 data[0] = 0;//0x00;
vsluiter 36:8e6ac6082e97 167 data[1] = 2;//0x02;//0002: callibrated data
vsluiter 36:8e6ac6082e97 168 */
vsluiter 36:8e6ac6082e97 169 data[0] = 0;
vsluiter 36:8e6ac6082e97 170 data[1] = 0x04;//0004: orientation data
vsluiter 17:c5946a0fde83 171 for (int i = 0 ; i<3 ; i++)
vsluiter 17:c5946a0fde83 172 {
vsluiter 36:8e6ac6082e97 173 XbusCreateMessage(i+1,0xD0,2,data,msg);//Set Output Mode
vsluiter 36:8e6ac6082e97 174 XbusSendArray(msg);
vsluiter 36:8e6ac6082e97 175 }
vsluiter 36:8e6ac6082e97 176
vsluiter 36:8e6ac6082e97 177 data[0] = 0;//Reserved
vsluiter 36:8e6ac6082e97 178 data[1] = 0;// LLAWGS84
vsluiter 36:8e6ac6082e97 179 data[2] = 0;//Float output, Enable aux outputs
vsluiter 36:8e6ac6082e97 180 data[3] = 0x01;//SampleCounter, Quaternion, Enable all sensor outputs, Reserved
vsluiter 36:8e6ac6082e97 181
vsluiter 36:8e6ac6082e97 182 for (int i = 0 ; i<3 ; i++)
vsluiter 36:8e6ac6082e97 183 {
vsluiter 36:8e6ac6082e97 184 XbusCreateMessage(i+1,0xD2,4,data,msg);//Set Output Settings
vsluiter 17:c5946a0fde83 185 XbusSendArray(msg);
vsluiter 17:c5946a0fde83 186 }
vsluiter 17:c5946a0fde83 187 wait_ms(10);
vsluiter 36:8e6ac6082e97 188
vsluiter 17:c5946a0fde83 189 XbusCreateMessage(0xFF,0x10,0,msg,msg);//GoToMeasurement
vsluiter 17:c5946a0fde83 190 XbusSendArray(msg);
vsluiter 13:5e4dcbd44786 191 }
vsluiter 13:5e4dcbd44786 192
vsluiter 18:6629e8c5d59e 193 void XbusReceiveState(xbus_t * xbus, uint8_t rxdata)
vsluiter 13:5e4dcbd44786 194 {
vsluiter 16:bfc7ea6bd1af 195 switch(xbus->rx.state) {
vsluiter 16:bfc7ea6bd1af 196 case XBUS_IDLE: {
vsluiter 16:bfc7ea6bd1af 197 if(rxdata == 0xFA) {
vsluiter 13:5e4dcbd44786 198 xbus->rx.counter = 0;
vsluiter 13:5e4dcbd44786 199 xbus->rx.buffer[0] = 0xFA;
vsluiter 13:5e4dcbd44786 200 xbus->rx.message_complete = 0;
vsluiter 13:5e4dcbd44786 201 xbus->rx.state = XBUS_BID;
vsluiter 13:5e4dcbd44786 202 }
vsluiter 13:5e4dcbd44786 203 break;
vsluiter 13:5e4dcbd44786 204 }
vsluiter 16:bfc7ea6bd1af 205 case XBUS_BID: {
vsluiter 13:5e4dcbd44786 206 xbus->rx.counter = 1;
vsluiter 13:5e4dcbd44786 207 xbus->rx.buffer[xbus->rx.counter] = rxdata;
vsluiter 13:5e4dcbd44786 208 xbus->rx.checksum = rxdata;
vsluiter 13:5e4dcbd44786 209 xbus->rx.state = XBUS_MID;
vsluiter 13:5e4dcbd44786 210 break;
vsluiter 13:5e4dcbd44786 211 }
vsluiter 16:bfc7ea6bd1af 212 case XBUS_MID: {
vsluiter 13:5e4dcbd44786 213 xbus->rx.counter = 2;
vsluiter 13:5e4dcbd44786 214 xbus->rx.buffer[xbus->rx.counter] = rxdata;
vsluiter 13:5e4dcbd44786 215 xbus->rx.checksum = xbus->rx.checksum + rxdata;
vsluiter 13:5e4dcbd44786 216 xbus->rx.state = XBUS_LEN;
vsluiter 13:5e4dcbd44786 217 break;
vsluiter 13:5e4dcbd44786 218 }
vsluiter 16:bfc7ea6bd1af 219 case XBUS_LEN: {
vsluiter 13:5e4dcbd44786 220 if(rxdata > XBUS_BUFFER_LENGTH - 4) // if message longer than buffer can contain
vsluiter 13:5e4dcbd44786 221 xbus->rx.state = XBUS_IDLE; // EXLEN not supported!
vsluiter 16:bfc7ea6bd1af 222 else {
vsluiter 13:5e4dcbd44786 223 xbus->rx.counter = 3;
vsluiter 13:5e4dcbd44786 224 xbus->rx.buffer[xbus->rx.counter] = rxdata;
vsluiter 13:5e4dcbd44786 225 xbus->rx.checksum = xbus->rx.checksum + rxdata;
vsluiter 13:5e4dcbd44786 226 if(rxdata == 0) // no data sent
vsluiter 13:5e4dcbd44786 227 xbus->rx.state = XBUS_CS; // go to checksum
vsluiter 13:5e4dcbd44786 228 else
vsluiter 13:5e4dcbd44786 229 xbus->rx.state = XBUS_DATA;
vsluiter 13:5e4dcbd44786 230 }
vsluiter 13:5e4dcbd44786 231 break;
vsluiter 13:5e4dcbd44786 232 }
vsluiter 16:bfc7ea6bd1af 233 case XBUS_DATA: {
vsluiter 13:5e4dcbd44786 234 xbus->rx.checksum += rxdata;
vsluiter 13:5e4dcbd44786 235 xbus->rx.counter++;
vsluiter 13:5e4dcbd44786 236 xbus->rx.buffer[xbus->rx.counter] = rxdata;
vsluiter 13:5e4dcbd44786 237 if(xbus->rx.counter == (xbus->rx.buffer[3] + 3) ) // if all data received (calculated from LEN
vsluiter 13:5e4dcbd44786 238 xbus->rx.state = XBUS_CS; // go to checksum
vsluiter 13:5e4dcbd44786 239 break;
vsluiter 13:5e4dcbd44786 240 }
vsluiter 16:bfc7ea6bd1af 241 case XBUS_CS: {
vsluiter 16:bfc7ea6bd1af 242 volatile uint16_t cs_calc;
vsluiter 13:5e4dcbd44786 243 xbus->rx.checksum_ok = 0;
vsluiter 13:5e4dcbd44786 244 xbus->rx.message_complete = 1;
vsluiter 13:5e4dcbd44786 245 xbus->rx.counter++;
vsluiter 13:5e4dcbd44786 246 xbus->rx.buffer[xbus->rx.counter] = rxdata;
vsluiter 13:5e4dcbd44786 247 //xbus->rx.checksum = 255 - xbus->rx.checksum;
vsluiter 16:bfc7ea6bd1af 248 xbus->rx.state = XBUS_IDLE;
vsluiter 13:5e4dcbd44786 249 cs_calc = xbus->rx.checksum + rxdata;
vsluiter 16:bfc7ea6bd1af 250 if( (cs_calc & 0x00FF) == 0)
vsluiter 13:5e4dcbd44786 251 xbus->rx.checksum_ok = 1;
vsluiter 16:bfc7ea6bd1af 252 else
vsluiter 16:bfc7ea6bd1af 253 asm("nop");
vsluiter 13:5e4dcbd44786 254 break;
vsluiter 13:5e4dcbd44786 255 }
vsluiter 16:bfc7ea6bd1af 256 default: {
vsluiter 13:5e4dcbd44786 257 xbus->rx.state = XBUS_IDLE;
vsluiter 13:5e4dcbd44786 258 break;
vsluiter 13:5e4dcbd44786 259 }
vsluiter 13:5e4dcbd44786 260 }
vsluiter 13:5e4dcbd44786 261 }
vsluiter 13:5e4dcbd44786 262
vsluiter 13:5e4dcbd44786 263
vsluiter 18:6629e8c5d59e 264 void XbusSetupReceiver(xbus_t * xbus)
vsluiter 13:5e4dcbd44786 265 {
vsluiter 16:bfc7ea6bd1af 266 //xbus_pc.uart = &XBUS_UART_PC;
vsluiter 13:5e4dcbd44786 267 xbus->rx.message_complete = 0;
vsluiter 13:5e4dcbd44786 268 xbus->rx.state = XBUS_IDLE;
vsluiter 13:5e4dcbd44786 269 xbus->rx.counter = 0;
vsluiter 13:5e4dcbd44786 270 }
vsluiter 13:5e4dcbd44786 271
vsluiter 17:c5946a0fde83 272 void XbusCreateMessage(uint8_t bid, uint8_t mid, uint8_t message_size, uint8_t *message, uint8_t *buffer )
vsluiter 17:c5946a0fde83 273 {
vsluiter 17:c5946a0fde83 274 buffer[0] = 0xFA;
vsluiter 17:c5946a0fde83 275 buffer[1] = bid;
vsluiter 17:c5946a0fde83 276 buffer[2] = mid;
vsluiter 17:c5946a0fde83 277 buffer[3] = message_size;
vsluiter 17:c5946a0fde83 278 if(message_size > 0)
vsluiter 17:c5946a0fde83 279 {
vsluiter 17:c5946a0fde83 280 for( int i = 0; i< message_size ; i++)
vsluiter 17:c5946a0fde83 281 {
vsluiter 17:c5946a0fde83 282 buffer[i+4] = message[i];
vsluiter 17:c5946a0fde83 283 }
vsluiter 17:c5946a0fde83 284 }
vsluiter 17:c5946a0fde83 285 buffer[message_size + 4] = XbusCreateChecksum(buffer, message_size + 5);
vsluiter 17:c5946a0fde83 286
vsluiter 17:c5946a0fde83 287 }
vsluiter 17:c5946a0fde83 288
vsluiter 13:5e4dcbd44786 289 /*Calculate xbus checksum from received message*/
vsluiter 13:5e4dcbd44786 290 uint8_t XbusCreateChecksum(uint8_t * array, uint8_t arraysize)
vsluiter 13:5e4dcbd44786 291 {
vsluiter 16:bfc7ea6bd1af 292 uint8_t counter;
vsluiter 16:bfc7ea6bd1af 293 uint16_t temp =0;
vsluiter 16:bfc7ea6bd1af 294 uint8_t checksum;
vsluiter 16:bfc7ea6bd1af 295 for(counter = 1; counter < (arraysize-1) ; counter++) { //start at BID, end before checksum
vsluiter 16:bfc7ea6bd1af 296 temp += array[counter];
vsluiter 16:bfc7ea6bd1af 297 }
vsluiter 16:bfc7ea6bd1af 298 checksum = (uint8_t)(0x100 - temp);
vsluiter 16:bfc7ea6bd1af 299 //checksum++;
vsluiter 16:bfc7ea6bd1af 300 return checksum;
vsluiter 13:5e4dcbd44786 301 }