File content as of revision 42:ef78d11f2bd7:

#include "xbus.h"

#define ARRAY(x) x, sizeof(x)

extern MODSERIAL xbus_serial;

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)
{
    for(int i = 0; i  < array[3]+5 ; i++ ) { //make use of len 
        xbus_serial.putc(array[i]);
    }
    wait_ms(10);
}


//uses baudrate values in Xbus Master documentation
void XbusSetBaudRate(uint8_t baud)
{
    uint8_t msg[10];
    XbusCreateMessage(0xFF,0x18,1,&baud,msg);
    XbusSendArray(msg);
}

void XbusReset()
{    
    uint8_t msg[10];
    XbusCreateMessage(0xFF,0x40,0,msg,msg);//Reset
    XbusSendArray(msg);
    wait_ms(1000);
}

void XbusGoToConfig(void)
{
    uint8_t msg[30];
    XbusCreateMessage(0xFF,0x30,0,msg,msg);//GoToConfig
    XbusSendArray(msg);
}

void XbusInitializeXbusMaster(void)
{
    uint8_t msg[30];
    uint8_t data[10];
    
    //XbusCreateMessage(0xFF,0x40,0,msg,msg);//Reset
    //XbusSendArray(msg);
    //wait_ms(1000);
    
    XbusCreateMessage(0xFF,0x30,0,msg,msg);//GoToConfig
    XbusSendArray(msg);
    
    
    data[0] = 0x04;
    data[1] = 0x80;
    XbusCreateMessage(0xFF,0x04,2,data,msg);//SetPeriod
    XbusSendArray(msg);
    
    /*
    data[0] = 0;//0x00;
    data[1] = 2;//0x02;//0002: callibrated data
    */
    data[0] = 0;
    data[1] = 0x04;//0004: orientation data
    for (int i = 0 ; i<3 ; i++)
    {
        XbusCreateMessage(i+1,0xD0,2,data,msg);//Set Output Mode
        XbusSendArray(msg);
    }
    
    data[0] = 0;//Reserved
    data[1] = 0;// LLAWGS84
    data[2] = 0;//Float output, Enable aux outputs
    data[3] = 0x01;//SampleCounter, Quaternion, Enable all sensor outputs, Reserved 
    
    for (int i = 0 ; i<3 ; i++)
    {
        XbusCreateMessage(i+1,0xD2,4,data,msg);//Set Output Settings
        XbusSendArray(msg);
    }
    wait_ms(10);
    
    XbusCreateMessage(0xFF,0x10,0,msg,msg);//GoToMeasurement
    XbusSendArray(msg);
}

void XbusReceiveState(xbus_t * 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(xbus_t * xbus)
{
    //xbus_pc.uart = &XBUS_UART_PC;
    xbus->rx.message_complete = 0;
    xbus->rx.state = XBUS_IDLE;
    xbus->rx.counter = 0;
}

void XbusCreateMessage(uint8_t bid, uint8_t mid, uint8_t message_size, uint8_t *message, uint8_t *buffer )
{
    buffer[0] = 0xFA;
    buffer[1] = bid;
    buffer[2] = mid;
    buffer[3] = message_size;
    if(message_size > 0)
    {
        for( int i = 0; i< message_size ; i++)
        {
            buffer[i+4] = message[i];
        }
    }
    buffer[message_size + 4] = XbusCreateChecksum(buffer, message_size + 5);
            
}
 
/*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;
}