BME SmartLab / Mbed 2 deprecated BabaTalp-NRF

NRF version of the "BabaTalp" code. (The original radioless code is in python)

Dependencies:   BNO055_fusion RF24 mbed

Fork of L053R8_mux_BNO by Daniel Mako

main.cpp@0:a573c39fe4e8, 2017-02-22 (annotated)

Committer:
Makodan
Date:
Wed Feb 22 15:52:57 2017 +0000
Revision:
0:a573c39fe4e8
Child:
1:01b7a386487e
1;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Makodan 0:a573c39fe4e8 1 #include "mbed.h"
Makodan 0:a573c39fe4e8 2 #include "BNO055.h"
Makodan 0:a573c39fe4e8 3 #include "config.h"
Makodan 0:a573c39fe4e8 4 #include "RF24.h"
Makodan 0:a573c39fe4e8 5
Makodan 0:a573c39fe4e8 6
Makodan 0:a573c39fe4e8 7
Makodan 0:a573c39fe4e8 8 I2C i2c(i2c_sda, i2c_scl);
Makodan 0:a573c39fe4e8 9 Serial pc(TXD, RXD, 9600);
Makodan 0:a573c39fe4e8 10 DigitalOut myled(ledpin);
Makodan 0:a573c39fe4e8 11
Makodan 0:a573c39fe4e8 12 RF24 radio(spi_MOSI, spi_MISO, spi_SCK, nrf_CE, nrf_CSN );
Makodan 0:a573c39fe4e8 13
Makodan 0:a573c39fe4e8 14 const uint64_t DataAddress = 0xF0F0F0F0E1LL;
Makodan 0:a573c39fe4e8 15 const uint64_t SyncAddress = 0xF0F0F0F0D2LL;
Makodan 0:a573c39fe4e8 16
Makodan 0:a573c39fe4e8 17 #define PACKET_Q 1
Makodan 0:a573c39fe4e8 18 #define PACKET_I 2
Makodan 0:a573c39fe4e8 19
Makodan 0:a573c39fe4e8 20 void ScanMUX(); //Scanning on all channels
Makodan 0:a573c39fe4e8 21 void Read1MUX(); //Read sensors on 0x28 address (all channels)
Makodan 0:a573c39fe4e8 22 void Read2MUX(); //Read sensors on 0x29 address (all channels)
Makodan 0:a573c39fe4e8 23 void ChannelMUX(uint8_t channel);
Makodan 0:a573c39fe4e8 24 void ReadIMU1(uint8_t channel); //Single sensor reading on 0x28 address
Makodan 0:a573c39fe4e8 25 void ReadIMU2(uint8_t channel); //Single sensor reading on 0x29 address
Makodan 0:a573c39fe4e8 26
Makodan 0:a573c39fe4e8 27 uint8_t AcclerometerScale = 3; // 3 = 16G
Makodan 0:a573c39fe4e8 28 bool AGCalFlag = false;
Makodan 0:a573c39fe4e8 29 bool MCalFlag = false;
Makodan 0:a573c39fe4e8 30 bool SendFlag = false;
Makodan 0:a573c39fe4e8 31
Makodan 0:a573c39fe4e8 32 // Sensor arrays - contain available sensors
Makodan 0:a573c39fe4e8 33 bool sensarray1[] = {false, false, false, false, false, false, false}; // 0x28 array
Makodan 0:a573c39fe4e8 34 bool sensarray2[] = {false, false, false, false, false, false, false}; // 0x29 array
Makodan 0:a573c39fe4e8 35
Makodan 0:a573c39fe4e8 36
Makodan 0:a573c39fe4e8 37 struct imuData {
Makodan 0:a573c39fe4e8 38 uint8_t sensorID;
Makodan 0:a573c39fe4e8 39 uint8_t packageCntr;
Makodan 0:a573c39fe4e8 40 int16_t q[4];
Makodan 0:a573c39fe4e8 41 int32_t linAcc[3];
Makodan 0:a573c39fe4e8 42
Makodan 0:a573c39fe4e8 43 } imuData;
Makodan 0:a573c39fe4e8 44
Makodan 0:a573c39fe4e8 45 BNO055 imu1(i2c_sda, i2c_scl, unused_pin, 0x50, MODE_NDOF); // Address: 0x28
Makodan 0:a573c39fe4e8 46 BNO055 imu2(i2c_sda, i2c_scl, unused_pin, 0x52, MODE_NDOF); // Address: 0x29
Makodan 0:a573c39fe4e8 47
Makodan 0:a573c39fe4e8 48 BNO055_ID_INF_TypeDef bno055_id_inf;
Makodan 0:a573c39fe4e8 49 BNO055_QUATERNION_TypeDef BNO055_quaternion;
Makodan 0:a573c39fe4e8 50 BNO055_EULER_TypeDef euler_angles;
Makodan 0:a573c39fe4e8 51 BNO055_LIN_ACC_TypeDef linear_acceleration;
Makodan 0:a573c39fe4e8 52
Makodan 0:a573c39fe4e8 53 struct SyncMsg {
Makodan 0:a573c39fe4e8 54 bool Sleep; //false if sensor should not sleep
Makodan 0:a573c39fe4e8 55 } SyncMsg;
Makodan 0:a573c39fe4e8 56
Makodan 0:a573c39fe4e8 57 //InterruptIn NRF_irq(PA_0);
Makodan 0:a573c39fe4e8 58
Makodan 0:a573c39fe4e8 59 void sendIRQ();
Makodan 0:a573c39fe4e8 60 void SendMSG();
Makodan 0:a573c39fe4e8 61 void SetupRadio();
Makodan 0:a573c39fe4e8 62
Makodan 0:a573c39fe4e8 63
Makodan 0:a573c39fe4e8 64 int main()
Makodan 0:a573c39fe4e8 65 {
Makodan 0:a573c39fe4e8 66
Makodan 0:a573c39fe4e8 67 myled=1;
Makodan 0:a573c39fe4e8 68 int i;
Makodan 0:a573c39fe4e8 69
Makodan 0:a573c39fe4e8 70 i2c.frequency(400000);
Makodan 0:a573c39fe4e8 71
Makodan 0:a573c39fe4e8 72 //radio setup
Makodan 0:a573c39fe4e8 73 SetupRadio();
Makodan 0:a573c39fe4e8 74
Makodan 0:a573c39fe4e8 75
Makodan 0:a573c39fe4e8 76 imuData.packageCntr = 0;
Makodan 0:a573c39fe4e8 77
Makodan 0:a573c39fe4e8 78 // Sensor scanning
Makodan 0:a573c39fe4e8 79 ScanMUX();
Makodan 0:a573c39fe4e8 80
Makodan 0:a573c39fe4e8 81 // Print out sensor arrays
Makodan 0:a573c39fe4e8 82 pc.printf("\r\n\t\tCH0\tCH1\tCH2\tCH3\tCH4\tCH5\tCH6");
Makodan 0:a573c39fe4e8 83 pc.printf("\r\n0x28 sensors:");
Makodan 0:a573c39fe4e8 84 for(i=0; i<7; i++){
Makodan 0:a573c39fe4e8 85 pc.printf("\t%d", sensarray1[i]);}
Makodan 0:a573c39fe4e8 86
Makodan 0:a573c39fe4e8 87 pc.printf("\r\n0x29 sensors:");
Makodan 0:a573c39fe4e8 88 for(i=0; i<7; i++){
Makodan 0:a573c39fe4e8 89 pc.printf("\t%d", sensarray2[i]);}
Makodan 0:a573c39fe4e8 90 pc.printf("\r\n");
Makodan 0:a573c39fe4e8 91
Makodan 0:a573c39fe4e8 92
Makodan 0:a573c39fe4e8 93 while (1) {
Makodan 0:a573c39fe4e8 94
Makodan 0:a573c39fe4e8 95 Read1MUX();
Makodan 0:a573c39fe4e8 96 Read2MUX();
Makodan 0:a573c39fe4e8 97
Makodan 0:a573c39fe4e8 98
Makodan 0:a573c39fe4e8 99 myled = !myled;
Makodan 0:a573c39fe4e8 100
Makodan 0:a573c39fe4e8 101 }
Makodan 0:a573c39fe4e8 102
Makodan 0:a573c39fe4e8 103 }
Makodan 0:a573c39fe4e8 104
Makodan 0:a573c39fe4e8 105
Makodan 0:a573c39fe4e8 106 void ScanMUX(){
Makodan 0:a573c39fe4e8 107
Makodan 0:a573c39fe4e8 108 for(uint8_t x=0; x<7; x++)
Makodan 0:a573c39fe4e8 109 {
Makodan 0:a573c39fe4e8 110
Makodan 0:a573c39fe4e8 111 ChannelMUX(x);
Makodan 0:a573c39fe4e8 112
Makodan 0:a573c39fe4e8 113 pc.printf("Scanning CH: %d\r\n", x);
Makodan 0:a573c39fe4e8 114
Makodan 0:a573c39fe4e8 115 wait(0.001);
Makodan 0:a573c39fe4e8 116 imu1.reset();
Makodan 0:a573c39fe4e8 117 imu2.reset();
Makodan 0:a573c39fe4e8 118 wait(0.01);
Makodan 0:a573c39fe4e8 119
Makodan 0:a573c39fe4e8 120 if(imu1.chip_ready() == 0){
Makodan 0:a573c39fe4e8 121 pc.printf("Bosch BNO055 is NOT available at address 0x28!!\r\n");
Makodan 0:a573c39fe4e8 122
Makodan 0:a573c39fe4e8 123 imu1.reset();
Makodan 0:a573c39fe4e8 124
Makodan 0:a573c39fe4e8 125 }
Makodan 0:a573c39fe4e8 126
Makodan 0:a573c39fe4e8 127 if(imu2.chip_ready() == 0){
Makodan 0:a573c39fe4e8 128 pc.printf("Bosch BNO055 is NOT available at address 0x29!!\r\n");
Makodan 0:a573c39fe4e8 129
Makodan 0:a573c39fe4e8 130 imu2.reset();
Makodan 0:a573c39fe4e8 131
Makodan 0:a573c39fe4e8 132 }
Makodan 0:a573c39fe4e8 133
Makodan 0:a573c39fe4e8 134
Makodan 0:a573c39fe4e8 135 if(imu1.chip_ready() == 1){
Makodan 0:a573c39fe4e8 136 pc.printf("Bosch BNO055 is AVAILABLE at address 0x28!!\r\n");
Makodan 0:a573c39fe4e8 137 imu1.read_id_inf(&bno055_id_inf);
Makodan 0:a573c39fe4e8 138 pc.printf("CHIP ID:0x%02x, ACC ID:0x%02x, MAG ID:0x%02x, GYR ID:0x%02x, ",
Makodan 0:a573c39fe4e8 139 bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, bno055_id_inf.gyr_id);
Makodan 0:a573c39fe4e8 140 pc.printf("SW REV:0x%04x, BL REV:0x%02x\r\n",
Makodan 0:a573c39fe4e8 141 bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id);
Makodan 0:a573c39fe4e8 142
Makodan 0:a573c39fe4e8 143 imu1.set_mounting_position(MT_P0);
Makodan 0:a573c39fe4e8 144 imu1.configure_accelerometer_range(AcclerometerScale);
Makodan 0:a573c39fe4e8 145
Makodan 0:a573c39fe4e8 146 sensarray1[x] = true;
Makodan 0:a573c39fe4e8 147
Makodan 0:a573c39fe4e8 148 }
Makodan 0:a573c39fe4e8 149
Makodan 0:a573c39fe4e8 150
Makodan 0:a573c39fe4e8 151
Makodan 0:a573c39fe4e8 152 if(imu2.chip_ready() == 1){
Makodan 0:a573c39fe4e8 153 pc.printf("Bosch BNO055 is AVAILABLE at address 0x29!!\r\n");
Makodan 0:a573c39fe4e8 154 imu2.read_id_inf(&bno055_id_inf);
Makodan 0:a573c39fe4e8 155 pc.printf("CHIP ID:0x%02x, ACC ID:0x%02x, MAG ID:0x%02x, GYR ID:0x%02x, ",
Makodan 0:a573c39fe4e8 156 bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, bno055_id_inf.gyr_id);
Makodan 0:a573c39fe4e8 157 pc.printf("SW REV:0x%04x, BL REV:0x%02x\r\n",
Makodan 0:a573c39fe4e8 158 bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id);
Makodan 0:a573c39fe4e8 159
Makodan 0:a573c39fe4e8 160 imu2.set_mounting_position(MT_P0);
Makodan 0:a573c39fe4e8 161 imu2.configure_accelerometer_range(AcclerometerScale);
Makodan 0:a573c39fe4e8 162
Makodan 0:a573c39fe4e8 163 sensarray2[x] = true;
Makodan 0:a573c39fe4e8 164
Makodan 0:a573c39fe4e8 165 }
Makodan 0:a573c39fe4e8 166 }
Makodan 0:a573c39fe4e8 167
Makodan 0:a573c39fe4e8 168
Makodan 0:a573c39fe4e8 169 }
Makodan 0:a573c39fe4e8 170
Makodan 0:a573c39fe4e8 171
Makodan 0:a573c39fe4e8 172 void Read1MUX(){
Makodan 0:a573c39fe4e8 173
Makodan 0:a573c39fe4e8 174 uint8_t i=0;
Makodan 0:a573c39fe4e8 175
Makodan 0:a573c39fe4e8 176 for( i=0; i<7; i++)
Makodan 0:a573c39fe4e8 177 {
Makodan 0:a573c39fe4e8 178
Makodan 0:a573c39fe4e8 179 ChannelMUX(i);
Makodan 0:a573c39fe4e8 180
Makodan 0:a573c39fe4e8 181 if(sensarray1[i]){
Makodan 0:a573c39fe4e8 182 ReadIMU1(i);
Makodan 0:a573c39fe4e8 183 pc.printf("ID:%d\t%d\t%d\t%d\t%d\r\n",imuData.sensorID,imuData.q[0],imuData.q[1],imuData.q[2],imuData.q[3]);
Makodan 0:a573c39fe4e8 184
Makodan 0:a573c39fe4e8 185 }
Makodan 0:a573c39fe4e8 186
Makodan 0:a573c39fe4e8 187 }
Makodan 0:a573c39fe4e8 188
Makodan 0:a573c39fe4e8 189 }
Makodan 0:a573c39fe4e8 190
Makodan 0:a573c39fe4e8 191
Makodan 0:a573c39fe4e8 192 void Read2MUX(){
Makodan 0:a573c39fe4e8 193
Makodan 0:a573c39fe4e8 194 uint8_t i=0;
Makodan 0:a573c39fe4e8 195
Makodan 0:a573c39fe4e8 196 for( i=0; i<7; i++)
Makodan 0:a573c39fe4e8 197 {
Makodan 0:a573c39fe4e8 198
Makodan 0:a573c39fe4e8 199 ChannelMUX(i);
Makodan 0:a573c39fe4e8 200
Makodan 0:a573c39fe4e8 201 if(sensarray2[i]){
Makodan 0:a573c39fe4e8 202 ReadIMU2(i);
Makodan 0:a573c39fe4e8 203 pc.printf("ID:%d\t%d\t%d\t%d\t%d\r\n",imuData.sensorID,imuData.q[0],imuData.q[1],imuData.q[2],imuData.q[3]);
Makodan 0:a573c39fe4e8 204
Makodan 0:a573c39fe4e8 205 }
Makodan 0:a573c39fe4e8 206
Makodan 0:a573c39fe4e8 207 }
Makodan 0:a573c39fe4e8 208
Makodan 0:a573c39fe4e8 209 }
Makodan 0:a573c39fe4e8 210
Makodan 0:a573c39fe4e8 211
Makodan 0:a573c39fe4e8 212 void ChannelMUX(uint8_t channel)
Makodan 0:a573c39fe4e8 213 {
Makodan 0:a573c39fe4e8 214 char get;
Makodan 0:a573c39fe4e8 215 char select;
Makodan 0:a573c39fe4e8 216
Makodan 0:a573c39fe4e8 217 select = 1 << channel;
Makodan 0:a573c39fe4e8 218
Makodan 0:a573c39fe4e8 219 i2c.write(0xE0, &select ,1);
Makodan 0:a573c39fe4e8 220 wait(0.001);
Makodan 0:a573c39fe4e8 221 i2c.read(0xE0, &get, 1);
Makodan 0:a573c39fe4e8 222 wait(0.001);
Makodan 0:a573c39fe4e8 223 i2c.write(0xE0, &select ,1);
Makodan 0:a573c39fe4e8 224 wait(0.001);
Makodan 0:a573c39fe4e8 225 i2c.read(0xE0, &get, 1);
Makodan 0:a573c39fe4e8 226
Makodan 0:a573c39fe4e8 227 }
Makodan 0:a573c39fe4e8 228
Makodan 0:a573c39fe4e8 229
Makodan 0:a573c39fe4e8 230 void ReadIMU1(uint8_t channel)
Makodan 0:a573c39fe4e8 231 {
Makodan 0:a573c39fe4e8 232 imuData.sensorID = (channel * 10);
Makodan 0:a573c39fe4e8 233 imu1.get_quaternion(&BNO055_quaternion);
Makodan 0:a573c39fe4e8 234
Makodan 0:a573c39fe4e8 235 imuData.q[0] = BNO055_quaternion.w;
Makodan 0:a573c39fe4e8 236 imuData.q[1] = BNO055_quaternion.x;
Makodan 0:a573c39fe4e8 237 imuData.q[2] = BNO055_quaternion.y;
Makodan 0:a573c39fe4e8 238 imuData.q[3] = BNO055_quaternion.z;
Makodan 0:a573c39fe4e8 239
Makodan 0:a573c39fe4e8 240
Makodan 0:a573c39fe4e8 241 imu1.get_abs_accel(&linear_acceleration);
Makodan 0:a573c39fe4e8 242
Makodan 0:a573c39fe4e8 243 float lax = 1.0;
Makodan 0:a573c39fe4e8 244 float lay = 2.0;
Makodan 0:a573c39fe4e8 245 float laz = 3.0;
Makodan 0:a573c39fe4e8 246
Makodan 0:a573c39fe4e8 247 lax = (float)linear_acceleration.x;
Makodan 0:a573c39fe4e8 248 lay = (float)linear_acceleration.y;
Makodan 0:a573c39fe4e8 249 laz = (float)linear_acceleration.z;
Makodan 0:a573c39fe4e8 250
Makodan 0:a573c39fe4e8 251 memcpy(imuData.linAcc , &lax, 4);
Makodan 0:a573c39fe4e8 252 memcpy(&imuData.linAcc[1] , &lay, 4);
Makodan 0:a573c39fe4e8 253 memcpy(&imuData.linAcc[2] , &laz, 4);
Makodan 0:a573c39fe4e8 254
Makodan 0:a573c39fe4e8 255 }
Makodan 0:a573c39fe4e8 256
Makodan 0:a573c39fe4e8 257 void ReadIMU2(uint8_t channel)
Makodan 0:a573c39fe4e8 258 {
Makodan 0:a573c39fe4e8 259 imuData.sensorID = ((channel * 10) + 1); // ID: CH number + 1
Makodan 0:a573c39fe4e8 260 imu2.get_quaternion(&BNO055_quaternion);
Makodan 0:a573c39fe4e8 261
Makodan 0:a573c39fe4e8 262 imuData.q[0] = BNO055_quaternion.w;
Makodan 0:a573c39fe4e8 263 imuData.q[1] = BNO055_quaternion.x;
Makodan 0:a573c39fe4e8 264 imuData.q[2] = BNO055_quaternion.y;
Makodan 0:a573c39fe4e8 265 imuData.q[3] = BNO055_quaternion.z;
Makodan 0:a573c39fe4e8 266
Makodan 0:a573c39fe4e8 267
Makodan 0:a573c39fe4e8 268 imu2.get_abs_accel(&linear_acceleration);
Makodan 0:a573c39fe4e8 269
Makodan 0:a573c39fe4e8 270 float lax = 1.0;
Makodan 0:a573c39fe4e8 271 float lay = 2.0;
Makodan 0:a573c39fe4e8 272 float laz = 3.0;
Makodan 0:a573c39fe4e8 273
Makodan 0:a573c39fe4e8 274 lax = (float)linear_acceleration.x;
Makodan 0:a573c39fe4e8 275 lay = (float)linear_acceleration.y;
Makodan 0:a573c39fe4e8 276 laz = (float)linear_acceleration.z;
Makodan 0:a573c39fe4e8 277
Makodan 0:a573c39fe4e8 278 memcpy(imuData.linAcc , &lax, 4);
Makodan 0:a573c39fe4e8 279 memcpy(&imuData.linAcc[1] , &lay, 4);
Makodan 0:a573c39fe4e8 280 memcpy(&imuData.linAcc[2] , &laz, 4);
Makodan 0:a573c39fe4e8 281
Makodan 0:a573c39fe4e8 282 }
Makodan 0:a573c39fe4e8 283
Makodan 0:a573c39fe4e8 284 void SetupRadio()
Makodan 0:a573c39fe4e8 285 {
Makodan 0:a573c39fe4e8 286 radio.begin();
Makodan 0:a573c39fe4e8 287 radio.setPALevel(RF24_PA_MAX) ;
Makodan 0:a573c39fe4e8 288 radio.setDataRate(RF24_2MBPS);
Makodan 0:a573c39fe4e8 289 radio.setCRCLength(RF24_CRC_16);
Makodan 0:a573c39fe4e8 290 radio.setChannel(RadioChannel);
Makodan 0:a573c39fe4e8 291
Makodan 0:a573c39fe4e8 292 radio.setRetries(0,2);
Makodan 0:a573c39fe4e8 293
Makodan 0:a573c39fe4e8 294 radio.enableDynamicAck();
Makodan 0:a573c39fe4e8 295 radio.enableDynamicPayloads();
Makodan 0:a573c39fe4e8 296
Makodan 0:a573c39fe4e8 297 radio.openWritingPipe(DataAddress);
Makodan 0:a573c39fe4e8 298 radio.openReadingPipe(1,SyncAddress);
Makodan 0:a573c39fe4e8 299 //radio.stopListening();
Makodan 0:a573c39fe4e8 300 //radio.setAutoAck(1,false);
Makodan 0:a573c39fe4e8 301 radio.startListening();
Makodan 0:a573c39fe4e8 302
Makodan 0:a573c39fe4e8 303 }
Makodan 0:a573c39fe4e8 304
Makodan 0:a573c39fe4e8 305 void SendMSG()
Makodan 0:a573c39fe4e8 306 {
Makodan 0:a573c39fe4e8 307 radio.stopListening();
Makodan 0:a573c39fe4e8 308 radio.write(&imuData, sizeof(imuData), true);
Makodan 0:a573c39fe4e8 309 radio.startListening();
Makodan 0:a573c39fe4e8 310
Makodan 0:a573c39fe4e8 311 if(imuData.packageCntr < 255)
Makodan 0:a573c39fe4e8 312 imuData.packageCntr++;
Makodan 0:a573c39fe4e8 313 else
Makodan 0:a573c39fe4e8 314 imuData.packageCntr = 0;
Makodan 0:a573c39fe4e8 315 myled = !myled;
Makodan 0:a573c39fe4e8 316 }