Send on Sync message
Dependencies: BNO055_fusion RF24_fork mbed
Diff: main.cpp
- Revision:
- 1:03c9c0e3fc70
- Parent:
- 0:5283bf22044b
diff -r 5283bf22044b -r 03c9c0e3fc70 main.cpp --- 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