BME SmartLab / Mbed 2 deprecated F042_BNO055_toNRF24

Send on Sync message

Dependencies:   BNO055_fusion RF24_fork mbed

main.cpp@1:03c9c0e3fc70, 2017-01-20 (annotated)

Committer:
mrcrsch
Date:
Fri Jan 20 13:43:21 2017 +0000
Revision:
1:03c9c0e3fc70
Parent:
0:5283bf22044b 
actual;

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 1:03c9c0e3fc70 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 1:03c9c0e3fc70 28 struct SyncMsg {
mrcrsch 1:03c9c0e3fc70 29 bool Sleep; //false if sensor should not sleep
mrcrsch 1:03c9c0e3fc70 30 } SyncMsg;
mrcrsch 0:5283bf22044b 31
mrcrsch 0:5283bf22044b 32 //InterruptIn NRF_irq(PA_0);
mrcrsch 0:5283bf22044b 33
mrcrsch 0:5283bf22044b 34 void sendIRQ();
mrcrsch 0:5283bf22044b 35 void ReadIMU();
mrcrsch 0:5283bf22044b 36 void SendMSG();
mrcrsch 1:03c9c0e3fc70 37 void SetupRadio();
mrcrsch 0:5283bf22044b 38
mrcrsch 0:5283bf22044b 39
mrcrsch 0:5283bf22044b 40
mrcrsch 0:5283bf22044b 41 BNO055 imu(i2c, PA_2); //RESET: not conected, just an unused pin
mrcrsch 0:5283bf22044b 42
mrcrsch 0:5283bf22044b 43 BNO055_ID_INF_TypeDef bno055_id_inf;
mrcrsch 0:5283bf22044b 44 BNO055_QUATERNION_TypeDef BNO055_quaternion;
mrcrsch 0:5283bf22044b 45 BNO055_EULER_TypeDef euler_angles;
mrcrsch 0:5283bf22044b 46 BNO055_LIN_ACC_TypeDef linear_acceleration;
mrcrsch 0:5283bf22044b 47
mrcrsch 0:5283bf22044b 48 uint8_t AcclerometerScale = 3; // 3 = 16G
mrcrsch 0:5283bf22044b 49 bool AGCalFlag = false;
mrcrsch 0:5283bf22044b 50 bool MCalFlag = false;
mrcrsch 0:5283bf22044b 51 bool SendFlag = false;
mrcrsch 0:5283bf22044b 52
mrcrsch 0:5283bf22044b 53
mrcrsch 1:03c9c0e3fc70 54 Timer SendTimer;
mrcrsch 0:5283bf22044b 55
mrcrsch 0:5283bf22044b 56
mrcrsch 0:5283bf22044b 57 int main()
mrcrsch 0:5283bf22044b 58 {
mrcrsch 0:5283bf22044b 59
mrcrsch 0:5283bf22044b 60 if (imu.chip_ready() == 0) {
mrcrsch 0:5283bf22044b 61 do {
mrcrsch 0:5283bf22044b 62 led = !led;
mrcrsch 0:5283bf22044b 63 wait(0.1);
mrcrsch 0:5283bf22044b 64 led = !led;
mrcrsch 0:5283bf22044b 65 wait(0.1);
mrcrsch 0:5283bf22044b 66 } while(imu.reset());
mrcrsch 0:5283bf22044b 67 }
mrcrsch 0:5283bf22044b 68
mrcrsch 0:5283bf22044b 69 imu.set_mounting_position(MT_P0);
mrcrsch 0:5283bf22044b 70 imu.configure_accelerometer_range(AcclerometerScale);
mrcrsch 0:5283bf22044b 71
mrcrsch 0:5283bf22044b 72 imu.read_id_inf(&bno055_id_inf); //read sensor IDs
mrcrsch 0:5283bf22044b 73
mrcrsch 0:5283bf22044b 74
mrcrsch 1:03c9c0e3fc70 75 SetupRadio();
mrcrsch 0:5283bf22044b 76 //radio setup
mrcrsch 1:03c9c0e3fc70 77
mrcrsch 1:03c9c0e3fc70 78
mrcrsch 0:5283bf22044b 79 //Attach Interrupt
mrcrsch 0:5283bf22044b 80 //NRF_irq.fall(&sendIRQ);
mrcrsch 1:03c9c0e3fc70 81
mrcrsch 1:03c9c0e3fc70 82
mrcrsch 0:5283bf22044b 83
mrcrsch 0:5283bf22044b 84 imuData.sensorID = NodeID;
mrcrsch 0:5283bf22044b 85 imuData.packageCntr = 0;
mrcrsch 0:5283bf22044b 86
mrcrsch 0:5283bf22044b 87 while(true) {
mrcrsch 1:03c9c0e3fc70 88 //imu.get_Euler_Angles(&euler_angles);
mrcrsch 0:5283bf22044b 89
mrcrsch 0:5283bf22044b 90
mrcrsch 1:03c9c0e3fc70 91 if(radio.available()) {
mrcrsch 1:03c9c0e3fc70 92 if(radio.getDynamicPayloadSize() < 1)// Corrupt payload has been flushed
mrcrsch 1:03c9c0e3fc70 93 continue;
mrcrsch 1:03c9c0e3fc70 94 radio.read(&SyncMsg,sizeof(SyncMsg));
mrcrsch 1:03c9c0e3fc70 95
mrcrsch 1:03c9c0e3fc70 96 wait_ms( (NodeID-1) * SendDelay); // wait before Send
mrcrsch 1:03c9c0e3fc70 97
mrcrsch 1:03c9c0e3fc70 98 ReadIMU();
mrcrsch 1:03c9c0e3fc70 99 SendMSG();
mrcrsch 1:03c9c0e3fc70 100 }
mrcrsch 1:03c9c0e3fc70 101
mrcrsch 1:03c9c0e3fc70 102
mrcrsch 1:03c9c0e3fc70 103
mrcrsch 0:5283bf22044b 104 }//while
mrcrsch 0:5283bf22044b 105
mrcrsch 0:5283bf22044b 106
mrcrsch 0:5283bf22044b 107 }
mrcrsch 0:5283bf22044b 108
mrcrsch 0:5283bf22044b 109 /*
mrcrsch 0:5283bf22044b 110 void sendIRQ(){
mrcrsch 0:5283bf22044b 111 if(radio.getDynamicPayloadSize() < 1)// Corrupt payload has been flushed
mrcrsch 1:03c9c0e3fc70 112 return;
mrcrsch 1:03c9c0e3fc70 113
mrcrsch 0:5283bf22044b 114 radio.read(&Sync,sizeof(Sync));
mrcrsch 0:5283bf22044b 115 if(Sync.ID == imuData.sensorID){
mrcrsch 0:5283bf22044b 116 if(Sync.ARange > 0)
mrcrsch 0:5283bf22044b 117 AcclerometerScale = Sync.ARange;
mrcrsch 1:03c9c0e3fc70 118
mrcrsch 0:5283bf22044b 119 AGCalFlag = Sync.AGCal;
mrcrsch 0:5283bf22044b 120 MCalFlag = Sync.MCal;
mrcrsch 0:5283bf22044b 121 SendFlag = true;
mrcrsch 0:5283bf22044b 122 }
mrcrsch 1:03c9c0e3fc70 123
mrcrsch 0:5283bf22044b 124 }*/
mrcrsch 0:5283bf22044b 125
mrcrsch 1:03c9c0e3fc70 126 void SetupRadio()
mrcrsch 1:03c9c0e3fc70 127 {
mrcrsch 1:03c9c0e3fc70 128 radio.begin();
mrcrsch 1:03c9c0e3fc70 129 radio.setPALevel(RF24_PA_MAX) ;
mrcrsch 1:03c9c0e3fc70 130 radio.setDataRate(RF24_2MBPS);
mrcrsch 1:03c9c0e3fc70 131 radio.setCRCLength(RF24_CRC_16);
mrcrsch 1:03c9c0e3fc70 132 radio.setChannel(RadioChannel);
mrcrsch 1:03c9c0e3fc70 133
mrcrsch 1:03c9c0e3fc70 134 radio.setRetries(0,2);
mrcrsch 1:03c9c0e3fc70 135
mrcrsch 1:03c9c0e3fc70 136 radio.enableDynamicAck();
mrcrsch 1:03c9c0e3fc70 137 radio.enableDynamicPayloads();
mrcrsch 1:03c9c0e3fc70 138
mrcrsch 1:03c9c0e3fc70 139 radio.openWritingPipe(DataAddress);
mrcrsch 1:03c9c0e3fc70 140 radio.openReadingPipe(1,SyncAddress);
mrcrsch 1:03c9c0e3fc70 141 //radio.stopListening();
mrcrsch 1:03c9c0e3fc70 142 //radio.setAutoAck(1,false);
mrcrsch 1:03c9c0e3fc70 143 radio.startListening();
mrcrsch 1:03c9c0e3fc70 144
mrcrsch 1:03c9c0e3fc70 145 }
mrcrsch 1:03c9c0e3fc70 146
mrcrsch 1:03c9c0e3fc70 147
mrcrsch 1:03c9c0e3fc70 148 void ReadIMU()
mrcrsch 1:03c9c0e3fc70 149 {
mrcrsch 0:5283bf22044b 150 imu.get_quaternion(&BNO055_quaternion);
mrcrsch 0:5283bf22044b 151
mrcrsch 0:5283bf22044b 152 imuData.q[0] = BNO055_quaternion.w;
mrcrsch 0:5283bf22044b 153 imuData.q[1] = BNO055_quaternion.x;
mrcrsch 0:5283bf22044b 154 imuData.q[2] = BNO055_quaternion.y;
mrcrsch 0:5283bf22044b 155 imuData.q[3] = BNO055_quaternion.z;
mrcrsch 0:5283bf22044b 156
mrcrsch 0:5283bf22044b 157
mrcrsch 0:5283bf22044b 158 imu.get_abs_accel(&linear_acceleration);
mrcrsch 0:5283bf22044b 159
mrcrsch 0:5283bf22044b 160 float lax = 1.0;
mrcrsch 0:5283bf22044b 161 float lay = 2.0;
mrcrsch 0:5283bf22044b 162 float laz = 3.0;
mrcrsch 0:5283bf22044b 163
mrcrsch 0:5283bf22044b 164 lax = (float)linear_acceleration.x;
mrcrsch 0:5283bf22044b 165 lay = (float)linear_acceleration.y;
mrcrsch 0:5283bf22044b 166 laz = (float)linear_acceleration.z;
mrcrsch 0:5283bf22044b 167
mrcrsch 0:5283bf22044b 168 memcpy(imuData.linAcc , &lax, 4);
mrcrsch 0:5283bf22044b 169 memcpy(&imuData.linAcc[1] , &lay, 4);
mrcrsch 0:5283bf22044b 170 memcpy(&imuData.linAcc[2] , &laz, 4);
mrcrsch 1:03c9c0e3fc70 171
mrcrsch 0:5283bf22044b 172 }
mrcrsch 0:5283bf22044b 173
mrcrsch 1:03c9c0e3fc70 174 void SendMSG()
mrcrsch 1:03c9c0e3fc70 175 {
mrcrsch 1:03c9c0e3fc70 176 radio.stopListening();
mrcrsch 1:03c9c0e3fc70 177 radio.write(&imuData, sizeof(imuData), true);
mrcrsch 1:03c9c0e3fc70 178 radio.startListening();
mrcrsch 0:5283bf22044b 179
mrcrsch 1:03c9c0e3fc70 180 if(imuData.packageCntr < 255)
mrcrsch 1:03c9c0e3fc70 181 imuData.packageCntr++;
mrcrsch 1:03c9c0e3fc70 182 else
mrcrsch 1:03c9c0e3fc70 183 imuData.packageCntr = 0;
mrcrsch 1:03c9c0e3fc70 184 led = !led;
mrcrsch 0:5283bf22044b 185 }