![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Send on Sync message
Dependencies: BNO055_fusion RF24_fork mbed
Fork of F042_BNO055_toNRF24 by
main.cpp
- Committer:
- mrcrsch
- Date:
- 2017-01-16
- Revision:
- 0:5283bf22044b
- Child:
- 1:03c9c0e3fc70
File content as of revision 0:5283bf22044b:
#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 Sync { uint8_t ID; uint8_t ARange; bool AGCal; bool MCal; } Sync; //InterruptIn NRF_irq(PA_0); void sendIRQ(); void ReadIMU(); void SendMSG(); 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 //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(); }//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 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.write(&imuData, sizeof(imuData)); if(imuData.packageCntr < 255) imuData.packageCntr++; else imuData.packageCntr = 0; led = !led; }