Send IMU data on nrf channel 120 by 100Hz
Dependencies: BNO055_fusion_irq RF24_fork mbed-dev-no-serial
main.cpp
- Committer:
- mrcrsch
- Date:
- 2017-02-13
- Revision:
- 0:36135689a3f3
File content as of revision 0:36135689a3f3:
#include "mbed.h" #include "BNO055.h" #include "RF24.h" #include "config.h" 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 rcvStruct{ uint8_t channel_num; }rcvStruct; InterruptIn NRF_irq(nrf_irq); InterruptIn IMU_irq(mpu_irq); void nrf_irq_handler(); void imu_irq_handler(); void ReadIMU(); void SendMSG(); void SetupRadio(); BNO055 imu(i2c, PA_2); //RESET: PA_2 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; 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 //imu.setpowermode(1); SetupRadio(); //radio setup //Attach Interrupt NRF_irq.fall(&nrf_irq_handler); /*IMU_irq.mode(PullUp); IMU_irq.rise(&imu_irq_handler);*/ //imu.initIntr(); imuData.sensorID = NodeID; imuData.packageCntr = 0; while(true) { ReadIMU(); SendMSG(); //led = !led; wait_ms(10); }//while } void imu_irq_handler(){ led = !led; //imu.resetIntr(); } void nrf_irq_handler(){ if(radio.getDynamicPayloadSize() < 1)// Corrupt payload has been flushed return; radio.read(&rcvStruct,sizeof(rcvStruct)); radio.setChannel(rcvStruct.channel_num); } 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; }