Diff: main.cpp

Revision:

1:a89ab6413920

Parent:

0:fb01b60e139b

diff -r fb01b60e139b -r a89ab6413920 main.cpp
--- a/main.cpp	Tue Aug 28 16:57:38 2018 +0000
+++ b/main.cpp	Mon Sep 10 20:11:12 2018 +0000
@@ -4,6 +4,9 @@
 #define CHANNELS_NUMBER 12 
 #define CMD_VALUE 65535
 #define CMD_GET_STATUS 1
+#define CMD_INITIALIZE 2
+
+#define RCV_TIMEOUT_MS 150
 
 nRF24L01P radio(       PB_5,    // MOSI
                        PB_4,    // MISO
@@ -25,6 +28,17 @@
 uint16_t channels[CHANNELS_NUMBER]; 
 uint16_t buffer  [CHANNELS_NUMBER]; 
 
+Timer t;
+int timeOffsetMs=0;
+
+int getSubmarineTimeMs(){
+    return t.read_ms()+timeOffsetMs;    
+}
+
+void updateTimeOffset(int subTime){
+    timeOffsetMs = subTime-t.read_ms();
+}
+
 void readChannels() {
     channels[0]=(uint16_t)(1024*CH1.read());
     channels[1]=(uint16_t)(1024*CH2.read());
@@ -32,10 +46,34 @@
     channels[3]=(uint16_t)(1024*CH4.read());
 }
 
+bool isInitializated(uint16_t state) {
+    if((state>>8) & 0x01) return true;
+    return false;
+}
+
 void zeroBuffer(){
     memset(buffer, 0, sizeof(buffer));     
 }
 
+bool receiveBuffer(){
+    radio.setReceiveMode();
+    
+    int startTime=t.read_ms();
+    
+    while( !radio.readable() && t.read_ms()-startTime < RCV_TIMEOUT_MS);
+        
+    if(radio.readable())
+    {
+        pc.printf("Availiable %d\r\n",radio.readable());
+        zeroBuffer();
+        int rxDataCnt = radio.read( NRF24L01P_PIPE_P0, (char*)buffer, sizeof( buffer ) );
+            
+            
+        return true;
+    }
+    return false;
+}
+
 void askFor(uint16_t query) {
     zeroBuffer();
     buffer[0] = CMD_VALUE;
@@ -53,7 +91,7 @@
 }
 
 int main() {
-    
+    myled=0;
     pc.baud(115200);
     pc.format();
     pc.printf("Hello world\r\n");
@@ -73,10 +111,12 @@
     radio.setTransferSize( CHANNELS_NUMBER*sizeof(uint16_t) );
     radio.enable();
     
-    //CH1.read()
-    //CH2.read()
-    //CH3.read()
-    //CH4.read()
+    for(int i=4;i>0;i--){
+        myled=!myled;
+        wait(0.5);    
+    }
+    
+    t.start();
     
     while(1) {
         readChannels();
@@ -87,22 +127,32 @@
         if (bytesWritten<2*CHANNELS_NUMBER){
             pc.printf("Transmit error");
         }
+        
         askFor(CMD_GET_STATUS);
-        //wait(0.1);
-        radio.setReceiveMode();
-        //wait(0.1);
-        while( !radio.readable() );
-        {
-            pc.printf("Availiable %d\r\n",radio.readable());
-            zeroBuffer();
-            int rxDataCnt = radio.read( NRF24L01P_PIPE_P0, (char*)buffer, sizeof( buffer ) );
-            pc.printf("Telem %s\r\n",(char*)buffer);
+        if (!receiveBuffer()) {
+            pc.printf("Timeout\r\n",(char*)buffer);   
+            myled=1; 
+            continue;
         }
-        //wait(0.5);
+        myled=0;
+        pc.printf("Received telemetry: %s\r\n",(char*)buffer);
+        if (!isInitializated(buffer[0])){
+            pc.printf("Need initialization\r\n");
+            updateTimeOffset(buffer[1]);
+            askFor(CMD_INITIALIZE);
+            if (!receiveBuffer()) {
+                pc.printf("Timeout\r\n",(char*)buffer);   
+                continue; 
+            }
+            
+        } else {
+          
+        }
         
-        /*myled = 1;
-        wait(0.01);
-        myled = 0;
-        wait(0.01);*/
+        /*
+        value & 0xff;
+        (value >> 8) & 0xff;
+        */
+        //pc.printf("Telem %s\r\n",(char*)buffer);
     }
 }