Send on Sync message
Dependencies: BNO055_fusion RF24_fork mbed
main.cpp
- Committer:
- mrcrsch
- Date:
- 2017-01-20
- Revision:
- 1:03c9c0e3fc70
- Parent:
- 0:5283bf22044b
File content as of revision 1:03c9c0e3fc70:
#include "mbed.h" #include "BNO055.h" #include "RF24.h" #include "config.h" #ifdef DEVICE_SERIAL #undef DEVICE_SERIAL #endif DigitalOut led(ledpin); I2C i2c(i2c_sda, i2c_scl); RF24 radio(spi_MOSI, spi_MISO, spi_SCK, nrf_CE, nrf_CSN ); const uint64_t DataAddress = 0xF0F0F0F0E1LL; const uint64_t SyncAddress = 0xF0F0F0F0D2LL; struct imuData { uint8_t sensorID; uint8_t packageCntr; int16_t q[4]; int32_t linAcc[3]; } imuData; struct SyncMsg { bool Sleep; //false if sensor should not sleep } SyncMsg; //InterruptIn NRF_irq(PA_0); void sendIRQ(); void ReadIMU(); void SendMSG(); void SetupRadio(); BNO055 imu(i2c, PA_2); //RESET: not conected, just an unused pin BNO055_ID_INF_TypeDef bno055_id_inf; BNO055_QUATERNION_TypeDef BNO055_quaternion; BNO055_EULER_TypeDef euler_angles; BNO055_LIN_ACC_TypeDef linear_acceleration; uint8_t AcclerometerScale = 3; // 3 = 16G bool AGCalFlag = false; bool MCalFlag = false; bool SendFlag = false; Timer SendTimer; int main() { if (imu.chip_ready() == 0) { do { led = !led; wait(0.1); led = !led; wait(0.1); } while(imu.reset()); } imu.set_mounting_position(MT_P0); imu.configure_accelerometer_range(AcclerometerScale); imu.read_id_inf(&bno055_id_inf); //read sensor IDs SetupRadio(); //radio setup //Attach Interrupt //NRF_irq.fall(&sendIRQ); imuData.sensorID = NodeID; imuData.packageCntr = 0; while(true) { //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 } /* void sendIRQ(){ if(radio.getDynamicPayloadSize() < 1)// Corrupt payload has been flushed 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 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; imuData.q[1] = BNO055_quaternion.x; imuData.q[2] = BNO055_quaternion.y; imuData.q[3] = BNO055_quaternion.z; imu.get_abs_accel(&linear_acceleration); float lax = 1.0; float lay = 2.0; float laz = 3.0; lax = (float)linear_acceleration.x; lay = (float)linear_acceleration.y; laz = (float)linear_acceleration.z; memcpy(imuData.linAcc , &lax, 4); memcpy(&imuData.linAcc[1] , &lay, 4); memcpy(&imuData.linAcc[2] , &laz, 4); } void SendMSG() { radio.stopListening(); radio.write(&imuData, sizeof(imuData), true); radio.startListening(); if(imuData.packageCntr < 255) imuData.packageCntr++; else imuData.packageCntr = 0; led = !led; }