Marcell Rausch / Mbed 2 deprecated F042_BNO055_toNRF24

Send on Sync message

Dependencies:   BNO055_fusion RF24_fork mbed

Fork of F042_BNO055_toNRF24 by BME SmartLab

main.cpp@0:5283bf22044b, 2017-01-16 (annotated)

Committer:
mrcrsch
Date:
Mon Jan 16 16:51:56 2017 +0000
Revision:
0:5283bf22044b
Child:
1:03c9c0e3fc70
init

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mrcrsch 0:5283bf22044b 1 #include "mbed.h"
mrcrsch 0:5283bf22044b 2 #include "BNO055.h"
mrcrsch 0:5283bf22044b 3 #include "RF24.h"
mrcrsch 0:5283bf22044b 4 #include "config.h"
mrcrsch 0:5283bf22044b 5
mrcrsch 0:5283bf22044b 6 #ifdef DEVICE_SERIAL
mrcrsch 0:5283bf22044b 7 #undef DEVICE_SERIAL
mrcrsch 0:5283bf22044b 8 #endif
mrcrsch 0:5283bf22044b 9
mrcrsch 0:5283bf22044b 10 DigitalOut led(ledpin);
mrcrsch 0:5283bf22044b 11 I2C i2c(i2c_sda,i2c_scl);
mrcrsch 0:5283bf22044b 12
mrcrsch 0:5283bf22044b 13 RF24 radio(spi_MOSI, spi_MISO, spi_SCK, nrf_CE, nrf_CSN );
mrcrsch 0:5283bf22044b 14
mrcrsch 0:5283bf22044b 15 const uint64_t DataAddress = 0xF0F0F0F0E1LL;
mrcrsch 0:5283bf22044b 16 const uint64_t SyncAddress = 0xF0F0F0F0D2LL;
mrcrsch 0:5283bf22044b 17
mrcrsch 0:5283bf22044b 18
mrcrsch 0:5283bf22044b 19
mrcrsch 0:5283bf22044b 20 struct imuData {
mrcrsch 0:5283bf22044b 21 uint8_t sensorID;
mrcrsch 0:5283bf22044b 22 uint8_t packageCntr;
mrcrsch 0:5283bf22044b 23 int16_t q[4];
mrcrsch 0:5283bf22044b 24 int32_t linAcc[3];
mrcrsch 0:5283bf22044b 25
mrcrsch 0:5283bf22044b 26 } imuData;
mrcrsch 0:5283bf22044b 27
mrcrsch 0:5283bf22044b 28 struct Sync {
mrcrsch 0:5283bf22044b 29 uint8_t ID;
mrcrsch 0:5283bf22044b 30 uint8_t ARange;
mrcrsch 0:5283bf22044b 31 bool AGCal;
mrcrsch 0:5283bf22044b 32 bool MCal;
mrcrsch 0:5283bf22044b 33 } Sync;
mrcrsch 0:5283bf22044b 34
mrcrsch 0:5283bf22044b 35 //InterruptIn NRF_irq(PA_0);
mrcrsch 0:5283bf22044b 36
mrcrsch 0:5283bf22044b 37 void sendIRQ();
mrcrsch 0:5283bf22044b 38 void ReadIMU();
mrcrsch 0:5283bf22044b 39 void SendMSG();
mrcrsch 0:5283bf22044b 40
mrcrsch 0:5283bf22044b 41
mrcrsch 0:5283bf22044b 42
mrcrsch 0:5283bf22044b 43 BNO055 imu(i2c, PA_2); //RESET: not conected, just an unused pin
mrcrsch 0:5283bf22044b 44
mrcrsch 0:5283bf22044b 45 BNO055_ID_INF_TypeDef bno055_id_inf;
mrcrsch 0:5283bf22044b 46 BNO055_QUATERNION_TypeDef BNO055_quaternion;
mrcrsch 0:5283bf22044b 47 BNO055_EULER_TypeDef euler_angles;
mrcrsch 0:5283bf22044b 48 BNO055_LIN_ACC_TypeDef linear_acceleration;
mrcrsch 0:5283bf22044b 49
mrcrsch 0:5283bf22044b 50 uint8_t AcclerometerScale = 3; // 3 = 16G
mrcrsch 0:5283bf22044b 51 bool AGCalFlag = false;
mrcrsch 0:5283bf22044b 52 bool MCalFlag = false;
mrcrsch 0:5283bf22044b 53 bool SendFlag = false;
mrcrsch 0:5283bf22044b 54
mrcrsch 0:5283bf22044b 55
mrcrsch 0:5283bf22044b 56 //Timer SendTimer;
mrcrsch 0:5283bf22044b 57
mrcrsch 0:5283bf22044b 58
mrcrsch 0:5283bf22044b 59 int main()
mrcrsch 0:5283bf22044b 60 {
mrcrsch 0:5283bf22044b 61
mrcrsch 0:5283bf22044b 62 if (imu.chip_ready() == 0) {
mrcrsch 0:5283bf22044b 63 do {
mrcrsch 0:5283bf22044b 64 led = !led;
mrcrsch 0:5283bf22044b 65 wait(0.1);
mrcrsch 0:5283bf22044b 66 led = !led;
mrcrsch 0:5283bf22044b 67 wait(0.1);
mrcrsch 0:5283bf22044b 68 } while(imu.reset());
mrcrsch 0:5283bf22044b 69 }
mrcrsch 0:5283bf22044b 70
mrcrsch 0:5283bf22044b 71 imu.set_mounting_position(MT_P0);
mrcrsch 0:5283bf22044b 72 imu.configure_accelerometer_range(AcclerometerScale);
mrcrsch 0:5283bf22044b 73
mrcrsch 0:5283bf22044b 74 imu.read_id_inf(&bno055_id_inf); //read sensor IDs
mrcrsch 0:5283bf22044b 75
mrcrsch 0:5283bf22044b 76
mrcrsch 0:5283bf22044b 77
mrcrsch 0:5283bf22044b 78 //radio setup
mrcrsch 0:5283bf22044b 79 radio.begin();
mrcrsch 0:5283bf22044b 80 radio.setPALevel(RF24_PA_MAX) ;
mrcrsch 0:5283bf22044b 81 radio.setDataRate(RF24_2MBPS);
mrcrsch 0:5283bf22044b 82 radio.setCRCLength(RF24_CRC_16);
mrcrsch 0:5283bf22044b 83 radio.setChannel(120);
mrcrsch 0:5283bf22044b 84 /*
mrcrsch 0:5283bf22044b 85 radio.enableDynamicAck();
mrcrsch 0:5283bf22044b 86 radio.enableDynamicPayloads();
mrcrsch 0:5283bf22044b 87 */
mrcrsch 0:5283bf22044b 88 radio.openWritingPipe(DataAddress);
mrcrsch 0:5283bf22044b 89 radio.openReadingPipe(1,SyncAddress);
mrcrsch 0:5283bf22044b 90 radio.stopListening();
mrcrsch 0:5283bf22044b 91 /* radio.setAutoAck(1,false);
mrcrsch 0:5283bf22044b 92 radio.startListening();*/
mrcrsch 0:5283bf22044b 93
mrcrsch 0:5283bf22044b 94 //Attach Interrupt
mrcrsch 0:5283bf22044b 95 //NRF_irq.fall(&sendIRQ);
mrcrsch 0:5283bf22044b 96
mrcrsch 0:5283bf22044b 97
mrcrsch 0:5283bf22044b 98
mrcrsch 0:5283bf22044b 99 imuData.sensorID = NodeID;
mrcrsch 0:5283bf22044b 100 imuData.packageCntr = 0;
mrcrsch 0:5283bf22044b 101
mrcrsch 0:5283bf22044b 102 while(true) {
mrcrsch 0:5283bf22044b 103 //imu.get_Euler_Angles(&euler_angles);
mrcrsch 0:5283bf22044b 104
mrcrsch 0:5283bf22044b 105 wait_ms(33);
mrcrsch 0:5283bf22044b 106 ReadIMU();
mrcrsch 0:5283bf22044b 107 SendMSG();
mrcrsch 0:5283bf22044b 108
mrcrsch 0:5283bf22044b 109
mrcrsch 0:5283bf22044b 110
mrcrsch 0:5283bf22044b 111
mrcrsch 0:5283bf22044b 112 }//while
mrcrsch 0:5283bf22044b 113
mrcrsch 0:5283bf22044b 114
mrcrsch 0:5283bf22044b 115 }
mrcrsch 0:5283bf22044b 116
mrcrsch 0:5283bf22044b 117 /*
mrcrsch 0:5283bf22044b 118 void sendIRQ(){
mrcrsch 0:5283bf22044b 119 if(radio.getDynamicPayloadSize() < 1)// Corrupt payload has been flushed
mrcrsch 0:5283bf22044b 120 return;
mrcrsch 0:5283bf22044b 121
mrcrsch 0:5283bf22044b 122 radio.read(&Sync,sizeof(Sync));
mrcrsch 0:5283bf22044b 123 if(Sync.ID == imuData.sensorID){
mrcrsch 0:5283bf22044b 124 if(Sync.ARange > 0)
mrcrsch 0:5283bf22044b 125 AcclerometerScale = Sync.ARange;
mrcrsch 0:5283bf22044b 126
mrcrsch 0:5283bf22044b 127 AGCalFlag = Sync.AGCal;
mrcrsch 0:5283bf22044b 128 MCalFlag = Sync.MCal;
mrcrsch 0:5283bf22044b 129 SendFlag = true;
mrcrsch 0:5283bf22044b 130 }
mrcrsch 0:5283bf22044b 131
mrcrsch 0:5283bf22044b 132 }*/
mrcrsch 0:5283bf22044b 133
mrcrsch 0:5283bf22044b 134 void ReadIMU(){
mrcrsch 0:5283bf22044b 135 imu.get_quaternion(&BNO055_quaternion);
mrcrsch 0:5283bf22044b 136
mrcrsch 0:5283bf22044b 137 imuData.q[0] = BNO055_quaternion.w;
mrcrsch 0:5283bf22044b 138 imuData.q[1] = BNO055_quaternion.x;
mrcrsch 0:5283bf22044b 139 imuData.q[2] = BNO055_quaternion.y;
mrcrsch 0:5283bf22044b 140 imuData.q[3] = BNO055_quaternion.z;
mrcrsch 0:5283bf22044b 141
mrcrsch 0:5283bf22044b 142
mrcrsch 0:5283bf22044b 143 imu.get_abs_accel(&linear_acceleration);
mrcrsch 0:5283bf22044b 144
mrcrsch 0:5283bf22044b 145 float lax = 1.0;
mrcrsch 0:5283bf22044b 146 float lay = 2.0;
mrcrsch 0:5283bf22044b 147 float laz = 3.0;
mrcrsch 0:5283bf22044b 148
mrcrsch 0:5283bf22044b 149 lax = (float)linear_acceleration.x;
mrcrsch 0:5283bf22044b 150 lay = (float)linear_acceleration.y;
mrcrsch 0:5283bf22044b 151 laz = (float)linear_acceleration.z;
mrcrsch 0:5283bf22044b 152
mrcrsch 0:5283bf22044b 153 memcpy(imuData.linAcc , &lax, 4);
mrcrsch 0:5283bf22044b 154 memcpy(&imuData.linAcc[1] , &lay, 4);
mrcrsch 0:5283bf22044b 155 memcpy(&imuData.linAcc[2] , &laz, 4);
mrcrsch 0:5283bf22044b 156
mrcrsch 0:5283bf22044b 157 }
mrcrsch 0:5283bf22044b 158
mrcrsch 0:5283bf22044b 159 void SendMSG(){
mrcrsch 0:5283bf22044b 160 radio.write(&imuData, sizeof(imuData));
mrcrsch 0:5283bf22044b 161
mrcrsch 0:5283bf22044b 162 if(imuData.packageCntr < 255)
mrcrsch 0:5283bf22044b 163 imuData.packageCntr++;
mrcrsch 0:5283bf22044b 164 else
mrcrsch 0:5283bf22044b 165 imuData.packageCntr = 0;
mrcrsch 0:5283bf22044b 166 led = !led;
mrcrsch 0:5283bf22044b 167 }