xbus.cpp

00001 #include "xbus.h"
00002 
00003 #define ARRAY(x) x, sizeof(x)
00004 
00005 extern MODSERIAL xbus_serial;
00006 
00007 uint8_t XbusImuMsgGoToMeasurement[] = {   0xFA,
00008                                       0xFF,
00009                                       0x10,
00010                                       0x00,
00011                                       0xF1
00012                                       };
00013 
00014 uint8_t XbusImuMsgGoToConfig[] = { 0xFA,
00015                                    0xFF,
00016                                    0x30,
00017                                    0x00,
00018                                    0xD1
00019                                  };
00020 
00021 uint8_t XbusImuMsgSetPeriod[] = { 0xFA,
00022                                   0xFF,
00023                                   0x04, /* MID: SetPeriod */
00024                                   0x02,
00025                                   0x04, /* MSB time */
00026                                   0x80, /* LSB time */ /*time in 8.68us*/ /*0480 eq. 10ms*/
00027                                   0x77
00028                                 };
00029 
00030 uint8_t XbusImuMsgSetOutputMode[] = { 0xFA,
00031                                       0xFF,
00032                                       0xD0, /* MID: SetOutpuMode */
00033                                       0x02,
00034                                       0x40, /* MSB */
00035                                       0x00, /* LSB */ /*4000 eq. raw data mode*/
00036                                       0xEF
00037                                     };
00038 
00039 uint8_t XbusImuMsgSetSyncIn_SetSync[] = {0xFA,
00040                                         0xFF,
00041                                         0xD6, /* MID: SetSyncIn */
00042                                         0x03,
00043                                         0x00, /* SyncInMode*/
00044                                         0x00, /* Reserved */
00045                                         0x01, /* Sample ADC, RisingEdge*/
00046                                         0x27
00047                                         };
00048 
00049 uint8_t XbusImuMsgSetSyncIn_SetSkip[] = {0xFA,
00050                                         0xFF,
00051                                         0xD6, /* MID: SetSyncIn */
00052                                         0x03,
00053                                         0x01, /* SkipFactor*/
00054                                         0x00, /* MSB SkipFactor */
00055                                         0x00, /* LSB SkipFactor */
00056                                         0x27
00057                                         };
00058 
00059 uint8_t XbusImuMsgSetSyncIn_SetOffs[] = {0xFA,
00060                                         0xFF,
00061                                         0xD6, /* MID: SetSyncIn */
00062                                         0x05,
00063                                         0x02, /* Offset*/
00064                                         0x00, /* Bit 31..24 */
00065                                         0x01, /* Bit 23..16 */
00066                                         0x06, /* Bit 15..8  */
00067                                         0xB9, /* Bit  7..0  */ /*minimum value: 264*33.9ns*/ /*E675 eq. 2ms*/
00068                                         0x64
00069                                         };
00070 
00071 uint8_t XbusImuMsgSetSyncOut_SetSync[] = {0xFA,
00072         0xFF,
00073         0xD8, /* MID: SetSyncOut */
00074         0x03,
00075         0x00, /* SyncOutMode*/
00076         0x00, /* Reserved */
00077         0x12, /* Postitive Pulse, Pulse Mode*/
00078         0x14
00079                                          };
00080 
00081 uint8_t XbusImuMsgSetSyncOut_SetSkip[] = {0xFA,
00082         0xFF,
00083         0xD8, /* MID: SetSyncOut */
00084         0x03,
00085         0x01, /* SkipFactor*/
00086         0x00, /* MSB SkipFactor */
00087         0x00, /* LSB SkipFactor */
00088         0x25
00089                                          };
00090 
00091 uint8_t XbusImuMsgSetSyncOut_SetOffs[] = {0xFA,
00092         0xFF,
00093         0xD8, /* MID: SetSyncOut */
00094         0x05,
00095         0x02, /* Offset*/
00096         0x00, /* Bit 31..24 */
00097         0x00, /* Bit 23..16 */
00098         0xE6, /* Bit 15..8  */
00099         0x75, /* Bit  7..0  */ /*minimum value: 513*33.9ns*/ /*E675 eq. 2ms*/
00100         0xC7
00101                                          };
00102 
00103 uint8_t XbusImuMsgSetSyncOut_SetWidth[] = {0xFA,
00104         0xFF,
00105         0xD8, /* MID: SetSyncOut */
00106         0x05,
00107         0x03, /* Width*/
00108         0x00, /* Bit 31..24 */
00109         0x00, /* Bit 23..16 */
00110         0xE6, /* Bit 15..8  */
00111         0x75, /* Bit  7..0  */ /*minimum value: 1700*33.9ns*/ /*E675 eq. 2ms*/
00112         0xC6
00113                                           };
00114 
00115 static void XbusSendArray(uint8_t *array)
00116 {
00117     for(int i = 0; i  < array[3]+5 ; i++ ) { //make use of len 
00118         xbus_serial.putc(array[i]);
00119     }
00120     wait_ms(10);
00121 }
00122 
00123 
00124 //uses baudrate values in Xbus Master documentation
00125 void XbusSetBaudRate(uint8_t baud)
00126 {
00127     uint8_t msg[10];
00128     XbusCreateMessage(0xFF,0x18,1,&baud,msg);
00129     XbusSendArray(msg);
00130 }
00131 
00132 void XbusReset()
00133 {    
00134     uint8_t msg[10];
00135     XbusCreateMessage(0xFF,0x40,0,msg,msg);//Reset
00136     XbusSendArray(msg);
00137     wait_ms(1000);
00138 }
00139 
00140 void XbusGoToConfig(void)
00141 {
00142     uint8_t msg[30];
00143     XbusCreateMessage(0xFF,0x30,0,msg,msg);//GoToConfig
00144     XbusSendArray(msg);
00145 }
00146 
00147 void XbusInitializeXbusMaster(void)
00148 {
00149     uint8_t msg[30];
00150     uint8_t data[10];
00151     
00152     //XbusCreateMessage(0xFF,0x40,0,msg,msg);//Reset
00153     //XbusSendArray(msg);
00154     //wait_ms(1000);
00155     
00156     XbusCreateMessage(0xFF,0x30,0,msg,msg);//GoToConfig
00157     XbusSendArray(msg);
00158     
00159     
00160     data[0] = 0x04;
00161     data[1] = 0x80;
00162     XbusCreateMessage(0xFF,0x04,2,data,msg);//SetPeriod
00163     XbusSendArray(msg);
00164     
00165     /*
00166     data[0] = 0;//0x00;
00167     data[1] = 2;//0x02;//0002: callibrated data
00168     */
00169     data[0] = 0;
00170     data[1] = 0x04;//0004: orientation data
00171     for (int i = 0 ; i<3 ; i++)
00172     {
00173         XbusCreateMessage(i+1,0xD0,2,data,msg);//Set Output Mode
00174         XbusSendArray(msg);
00175     }
00176     
00177     data[0] = 0;//Reserved
00178     data[1] = 0;// LLAWGS84
00179     data[2] = 0;//Float output, Enable aux outputs
00180     data[3] = 0x01;//SampleCounter, Quaternion, Enable all sensor outputs, Reserved 
00181     
00182     for (int i = 0 ; i<3 ; i++)
00183     {
00184         XbusCreateMessage(i+1,0xD2,4,data,msg);//Set Output Settings
00185         XbusSendArray(msg);
00186     }
00187     wait_ms(10);
00188     
00189     XbusCreateMessage(0xFF,0x10,0,msg,msg);//GoToMeasurement
00190     XbusSendArray(msg);
00191 }
00192 
00193 void XbusReceiveState(xbus_t * xbus, uint8_t rxdata)
00194 {
00195     switch(xbus->rx.state) {
00196         case XBUS_IDLE: {
00197             if(rxdata == 0xFA) {
00198                 xbus->rx.counter = 0;
00199                 xbus->rx.buffer[0] = 0xFA;
00200                 xbus->rx.message_complete = 0;
00201                 xbus->rx.state = XBUS_BID;
00202             }
00203             break;
00204         }
00205         case XBUS_BID: {
00206             xbus->rx.counter = 1;
00207             xbus->rx.buffer[xbus->rx.counter] = rxdata;
00208             xbus->rx.checksum  = rxdata;
00209             xbus->rx.state = XBUS_MID;
00210             break;
00211         }
00212         case XBUS_MID: {
00213             xbus->rx.counter = 2;
00214             xbus->rx.buffer[xbus->rx.counter] = rxdata;
00215             xbus->rx.checksum  = xbus->rx.checksum + rxdata;
00216             xbus->rx.state = XBUS_LEN;
00217             break;
00218         }
00219         case XBUS_LEN: {
00220             if(rxdata > XBUS_BUFFER_LENGTH - 4)                                   // if message longer than buffer can contain
00221                 xbus->rx.state  = XBUS_IDLE;                                           // EXLEN not supported!
00222             else {
00223                 xbus->rx.counter = 3;
00224                 xbus->rx.buffer[xbus->rx.counter] = rxdata;
00225                 xbus->rx.checksum = xbus->rx.checksum + rxdata;
00226                 if(rxdata == 0)                  // no data sent
00227                     xbus->rx.state = XBUS_CS;    // go to checksum
00228                 else
00229                     xbus->rx.state = XBUS_DATA;
00230             }
00231             break;
00232         }
00233         case XBUS_DATA: {
00234             xbus->rx.checksum += rxdata;
00235             xbus->rx.counter++;
00236             xbus->rx.buffer[xbus->rx.counter] = rxdata;
00237             if(xbus->rx.counter == (xbus->rx.buffer[3] + 3) )     // if all data received (calculated from LEN
00238                 xbus->rx.state = XBUS_CS;                         // go to checksum
00239             break;
00240         }
00241         case XBUS_CS: {
00242             volatile uint16_t cs_calc;
00243             xbus->rx.checksum_ok = 0;
00244             xbus->rx.message_complete = 1;
00245             xbus->rx.counter++;
00246             xbus->rx.buffer[xbus->rx.counter] = rxdata;
00247             //xbus->rx.checksum = 255 - xbus->rx.checksum;
00248             xbus->rx.state = XBUS_IDLE;
00249             cs_calc = xbus->rx.checksum + rxdata;
00250             if( (cs_calc & 0x00FF) == 0)
00251                 xbus->rx.checksum_ok = 1;
00252             else
00253                 asm("nop");
00254             break;
00255         }
00256         default: {
00257             xbus->rx.state = XBUS_IDLE;
00258             break;
00259         }
00260     }
00261 }
00262 
00263 
00264 void XbusSetupReceiver(xbus_t * xbus)
00265 {
00266     //xbus_pc.uart = &XBUS_UART_PC;
00267     xbus->rx.message_complete = 0;
00268     xbus->rx.state = XBUS_IDLE;
00269     xbus->rx.counter = 0;
00270 }
00271 
00272 void XbusCreateMessage(uint8_t bid, uint8_t mid, uint8_t message_size, uint8_t *message, uint8_t *buffer )
00273 {
00274     buffer[0] = 0xFA;
00275     buffer[1] = bid;
00276     buffer[2] = mid;
00277     buffer[3] = message_size;
00278     if(message_size > 0)
00279     {
00280         for( int i = 0; i< message_size ; i++)
00281         {
00282             buffer[i+4] = message[i];
00283         }
00284     }
00285     buffer[message_size + 4] = XbusCreateChecksum(buffer, message_size + 5);
00286             
00287 }
00288  
00289 /*Calculate xbus checksum from received message*/
00290 uint8_t XbusCreateChecksum(uint8_t * array, uint8_t arraysize)
00291 {
00292     uint8_t counter;
00293     uint16_t temp =0;
00294     uint8_t checksum;
00295     for(counter = 1; counter < (arraysize-1) ; counter++) { //start at BID, end before checksum
00296         temp += array[counter];
00297     }
00298     checksum = (uint8_t)(0x100 - temp);
00299     //checksum++;
00300     return checksum;
00301 }