![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Send on Sync message
Dependencies: BNO055_fusion RF24_fork mbed
Fork of F042_BNO055_toNRF24 by
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
- Child:
- 3:6bba74b99bd3
actual;
Who changed what in which revision?
User | Revision | Line number | New 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 | } |