Send IMU data on nrf channel 120 by 100Hz
Dependencies: BNO055_fusion_irq RF24_fork mbed-dev-no-serial
main.cpp@0:36135689a3f3, 2017-02-13 (annotated)
- Committer:
- mrcrsch
- Date:
- Mon Feb 13 14:26:46 2017 +0000
- Revision:
- 0:36135689a3f3
init
Who changed what in which revision?
User | Revision | Line number | New 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 | } |