HC Voort
SOES implementation i.c.w. ET1100 (Beckhoff ASIC)
Dependencies: KL25Z_ClockControl MODSERIAL mbed
Fork of
EtherCAT-XbusMaster
by 
First Last
Diff: xbus.cpp
- Revision:
- 16:bfc7ea6bd1af
- Parent:
- 13:5e4dcbd44786
- Child:
- 17:c5946a0fde83
--- a/xbus.cpp Tue Mar 03 14:51:37 2015 +0000
+++ b/xbus.cpp Tue Mar 03 15:01:29 2015 +0000
@@ -2,128 +2,135 @@
#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 XbusImuMsgGoToMeasurement[] = { 0xFA,
+ 0xFF,
+ 0x10,
+ 0x00,
+ 0xF1
+ };
uint8_t XbusImuMsgGoToConfig[] = { 0xFA,
- 0xFF,
- 0x30,
- 0x00,
- 0xD1};
+ 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 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 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_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_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 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_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_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_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};
+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);
+ 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));
+ 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)
- {
+ switch(xbus->rx.state) {
+ case XBUS_IDLE: {
+ if(rxdata == 0xFA) {
xbus->rx.counter = 0;
xbus->rx.buffer[0] = 0xFA;
xbus->rx.message_complete = 0;
@@ -131,28 +138,24 @@
}
break;
}
- case XBUS_BID:
- {
+ 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:
- {
+ 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:
- {
+ 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
- {
+ else {
xbus->rx.counter = 3;
xbus->rx.buffer[xbus->rx.counter] = rxdata;
xbus->rx.checksum = xbus->rx.checksum + rxdata;
@@ -163,8 +166,7 @@
}
break;
}
- case XBUS_DATA:
- {
+ case XBUS_DATA: {
xbus->rx.checksum += rxdata;
xbus->rx.counter++;
xbus->rx.buffer[xbus->rx.counter] = rxdata;
@@ -172,24 +174,22 @@
xbus->rx.state = XBUS_CS; // go to checksum
break;
}
- case XBUS_CS:
- {
- volatile uint16_t cs_calc;
+ 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;
+ xbus->rx.state = XBUS_IDLE;
cs_calc = xbus->rx.checksum + rxdata;
- if( (cs_calc & 0x00FF) == 0)
+ if( (cs_calc & 0x00FF) == 0)
xbus->rx.checksum_ok = 1;
- else
- asm("nop");
+ else
+ asm("nop");
break;
}
- default:
- {
+ default: {
xbus->rx.state = XBUS_IDLE;
break;
}
@@ -199,7 +199,7 @@
void XbusSetupReceiver(struct xbus_struct * xbus)
{
- xbus_pc.uart = &XBUS_UART_PC;
+ //xbus_pc.uart = &XBUS_UART_PC;
xbus->rx.message_complete = 0;
xbus->rx.state = XBUS_IDLE;
xbus->rx.counter = 0;
@@ -209,14 +209,13 @@
/*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;
+ 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;
}