Diff: main.cpp

Revision:

1:03c9c0e3fc70

Parent:

0:5283bf22044b

Child:

3:6bba74b99bd3

--- a/main.cpp	Mon Jan 16 16:51:56 2017 +0000
+++ b/main.cpp	Fri Jan 20 13:43:21 2017 +0000
@@ -8,7 +8,7 @@
 #endif
 
 DigitalOut led(ledpin);
-I2C i2c(i2c_sda,i2c_scl);
+I2C i2c(i2c_sda, i2c_scl);
 
 RF24 radio(spi_MOSI, spi_MISO, spi_SCK, nrf_CE, nrf_CSN );
 
@@ -25,18 +25,16 @@
 
 } imuData;
 
-struct Sync {
-    uint8_t ID;
-    uint8_t ARange;
-    bool AGCal;
-    bool MCal;
-} Sync;
+struct SyncMsg {
+    bool Sleep;             //false if sensor should not sleep
+} SyncMsg;
 
 //InterruptIn NRF_irq(PA_0);
 
 void sendIRQ();
 void ReadIMU();
 void SendMSG();
+void SetupRadio();
 
 
 
@@ -53,7 +51,7 @@
 bool SendFlag = false;
 
 
-//Timer SendTimer;
+Timer SendTimer;
 
 
 int main()
@@ -74,41 +72,35 @@
     imu.read_id_inf(&bno055_id_inf);  //read sensor IDs
 
 
-
+    SetupRadio();
     //radio setup
-    radio.begin();
-    radio.setPALevel(RF24_PA_MAX) ;
-    radio.setDataRate(RF24_2MBPS);
-    radio.setCRCLength(RF24_CRC_16);
-    radio.setChannel(120);
-    /*
-    radio.enableDynamicAck();
-    radio.enableDynamicPayloads();
-    */
-    radio.openWritingPipe(DataAddress);    
-    radio.openReadingPipe(1,SyncAddress);
-    radio.stopListening();
-   /* radio.setAutoAck(1,false);
-    radio.startListening();*/
-    
+
+
     //Attach Interrupt
     //NRF_irq.fall(&sendIRQ);
-    
-    
+
+
 
     imuData.sensorID = NodeID;
     imuData.packageCntr = 0;
 
     while(true) {
-       //imu.get_Euler_Angles(&euler_angles);
-    
-        wait_ms(33);
-        ReadIMU();
-        SendMSG();
-        
+        //imu.get_Euler_Angles(&euler_angles);
 
 
-        
+        if(radio.available()) {
+            if(radio.getDynamicPayloadSize() < 1)// Corrupt payload has been flushed
+                continue;
+            radio.read(&SyncMsg,sizeof(SyncMsg));
+
+            wait_ms( (NodeID-1) * SendDelay); // wait before Send
+
+            ReadIMU();
+            SendMSG();
+        }
+
+
+
     }//while
 
 
@@ -117,21 +109,44 @@
 /*
 void sendIRQ(){
      if(radio.getDynamicPayloadSize() < 1)// Corrupt payload has been flushed
-        return; 
-        
+        return;
+
     radio.read(&Sync,sizeof(Sync));
     if(Sync.ID == imuData.sensorID){
         if(Sync.ARange > 0)
             AcclerometerScale = Sync.ARange;
-            
+
         AGCalFlag = Sync.AGCal;
         MCalFlag = Sync.MCal;
         SendFlag = true;
     }
-    
+
 }*/
 
-void ReadIMU(){
+void SetupRadio()
+{
+    radio.begin();
+    radio.setPALevel(RF24_PA_MAX) ;
+    radio.setDataRate(RF24_2MBPS);
+    radio.setCRCLength(RF24_CRC_16);
+    radio.setChannel(RadioChannel);
+
+    radio.setRetries(0,2);
+
+    radio.enableDynamicAck();
+    radio.enableDynamicPayloads();
+
+    radio.openWritingPipe(DataAddress);
+    radio.openReadingPipe(1,SyncAddress);
+    //radio.stopListening();
+    //radio.setAutoAck(1,false);
+    radio.startListening();
+
+}
+
+
+void ReadIMU()
+{
     imu.get_quaternion(&BNO055_quaternion);
 
     imuData.q[0] = BNO055_quaternion.w;
@@ -153,15 +168,18 @@
     memcpy(imuData.linAcc , &lax, 4);
     memcpy(&imuData.linAcc[1] , &lay, 4);
     memcpy(&imuData.linAcc[2] , &laz, 4);
-    
+
 }
 
-void SendMSG(){
-    radio.write(&imuData, sizeof(imuData));
+void SendMSG()
+{
+    radio.stopListening();
+    radio.write(&imuData, sizeof(imuData), true);
+    radio.startListening();
 
-        if(imuData.packageCntr < 255)
-            imuData.packageCntr++;
-        else
-            imuData.packageCntr = 0;
-        led = !led;
+    if(imuData.packageCntr < 255)
+        imuData.packageCntr++;
+    else
+        imuData.packageCntr = 0;
+    led = !led;
 }
\ No newline at end of file