Diff: xbus.cpp

Revision:

17:c5946a0fde83

Parent:

16:bfc7ea6bd1af

Child:

18:6629e8c5d59e

--- a/xbus.cpp	Tue Mar 03 15:01:29 2015 +0000
+++ b/xbus.cpp	Thu Mar 05 22:22:47 2015 +0000
@@ -110,9 +110,9 @@
         0xC6
                                           };
 
-static void XbusSendArray(uint8_t *array, uint8_t size)
+static void XbusSendArray(uint8_t *array)
 {
-    for(int i = 0; i  < size ; i++ ) {
+    for(int i = 0; i  < array[3]+5 ; i++ ) { //make use of len 
         xbus.putc(array[i]);
     }
     wait_ms(10);
@@ -120,10 +120,32 @@
 
 void XbusInitializeXbusMaster(void)
 {
-    XbusSendArray(ARRAY(XbusImuMsgGoToConfig));
-    XbusSendArray(ARRAY(XbusImuMsgSetPeriod));
-    XbusSendArray(ARRAY(XbusImuMsgSetOutputMode));
-    XbusSendArray(ARRAY(XbusImuMsgGoToMeasurement));
+    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] = 0x40;
+    data[1] = 0x00;
+    for (int i = 0 ; i<3 ; i++)
+    {
+        XbusCreateMessage(i+1,0xD0,2,data,msg);//Set Output Data Format
+        XbusSendArray(msg);
+    }
+    wait_ms(10);
+    XbusCreateMessage(0xFF,0x10,0,msg,msg);//GoToMeasurement
+    XbusSendArray(msg);
 }
 
 void XbusReceiveState(struct xbus_struct * xbus, uint8_t rxdata)
@@ -205,7 +227,23 @@
     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)
 {