PCA9548 + BNO055
Dependencies: BNO055_fusion RF24_fork mbed
main.cpp@0:a573c39fe4e8, 2017-02-22 (annotated)
- Committer:
- Makodan
- Date:
- Wed Feb 22 15:52:57 2017 +0000
- Revision:
- 0:a573c39fe4e8
1;
Who changed what in which revision?
User | Revision | Line number | New 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 | } |