![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Send on Sync message
Dependencies: BNO055_fusion RF24_fork mbed
Diff: main.cpp
- Revision:
- 0:5283bf22044b
- Child:
- 1:03c9c0e3fc70
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Jan 16 16:51:56 2017 +0000 @@ -0,0 +1,167 @@ +#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; +} \ No newline at end of file