BME SmartLab / SmartBall

Send IMU data on nrf channel 120 by 100Hz

Dependencies:   BNO055_fusion_irq RF24_fork mbed-dev-no-serial

main.cpp@1:22d552d9d146, 2017-02-13 (annotated)

Committer:
mrcrsch
Date:
Mon Feb 13 14:32:30 2017 +0000
Revision:
1:22d552d9d146
Parent:
0:36135689a3f3 
init

Who changed what in which revision?

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