Send IMU data on nrf channel 120 by 100Hz
Dependencies: BNO055_fusion_irq RF24_fork mbed-dev-no-serial
Diff: main.cpp
- Revision:
- 0:36135689a3f3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Feb 13 14:26:46 2017 +0000 @@ -0,0 +1,176 @@ +#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; + +} \ No newline at end of file