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
xbus.cpp
- Committer:
- vsluiter
- Date:
- 2015-03-03
- Revision:
- 13:5e4dcbd44786
- Child:
- 16:bfc7ea6bd1af
File content as of revision 13:5e4dcbd44786:
#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;
}