NRF version of the "BabaTalp" code. (The original radioless code is in python)
Dependencies: BNO055_fusion RF24 mbed
Fork of L053R8_mux_BNO by
main.cpp@2:65b259a97e52, 2017-02-24 (annotated)
- Committer:
- gume
- Date:
- Fri Feb 24 15:46:05 2017 +0000
- Revision:
- 2:65b259a97e52
- Parent:
- 1:01b7a386487e
Change channel to 101
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 | I2C i2c(i2c_sda, i2c_scl); |
gume | 1:01b7a386487e | 7 | DigitalOut led(ledpin); |
Makodan | 0:a573c39fe4e8 | 8 | |
Makodan | 0:a573c39fe4e8 | 9 | RF24 radio(spi_MOSI, spi_MISO, spi_SCK, nrf_CE, nrf_CSN ); |
Makodan | 0:a573c39fe4e8 | 10 | |
Makodan | 0:a573c39fe4e8 | 11 | const uint64_t DataAddress = 0xF0F0F0F0E1LL; |
Makodan | 0:a573c39fe4e8 | 12 | const uint64_t SyncAddress = 0xF0F0F0F0D2LL; |
gume | 1:01b7a386487e | 13 | |
gume | 1:01b7a386487e | 14 | #define PACKET_Q 3 |
gume | 1:01b7a386487e | 15 | #define PACKET_I 4 |
Makodan | 0:a573c39fe4e8 | 16 | |
gume | 1:01b7a386487e | 17 | int scanSensors(uint8_t *s0, uint8_t *s1, uint8_t* qids); |
gume | 1:01b7a386487e | 18 | void selectMUX(uint8_t channel); |
gume | 1:01b7a386487e | 19 | void readQIMU(uint8_t iid, void *qptr); |
Makodan | 0:a573c39fe4e8 | 20 | |
Makodan | 0:a573c39fe4e8 | 21 | uint8_t AcclerometerScale = 3; // 3 = 16G |
Makodan | 0:a573c39fe4e8 | 22 | |
Makodan | 0:a573c39fe4e8 | 23 | |
gume | 1:01b7a386487e | 24 | typedef struct { |
gume | 1:01b7a386487e | 25 | uint16_t sensorId; |
gume | 1:01b7a386487e | 26 | uint8_t packetType; |
gume | 1:01b7a386487e | 27 | uint8_t packetCntr; |
gume | 1:01b7a386487e | 28 | uint8_t payload[28]; // int16_t q[4], uint8_t c, uint8_t temp[2], int16_t la[3]; |
gume | 1:01b7a386487e | 29 | } ImuData; |
Makodan | 0:a573c39fe4e8 | 30 | |
gume | 1:01b7a386487e | 31 | typedef struct { |
gume | 1:01b7a386487e | 32 | uint16_t schedule[16]; // Order of the sensors |
gume | 1:01b7a386487e | 33 | } SyncData; |
Makodan | 0:a573c39fe4e8 | 34 | |
gume | 1:01b7a386487e | 35 | BNO055 imu0(i2c_sda, i2c_scl, unused_pin, 0x50, MODE_NDOF); // Address: 0x28 |
gume | 1:01b7a386487e | 36 | BNO055 imu1(i2c_sda, i2c_scl, unused_pin, 0x52, MODE_NDOF); // Address: 0x29 |
gume | 1:01b7a386487e | 37 | |
Makodan | 0:a573c39fe4e8 | 38 | BNO055_QUATERNION_TypeDef BNO055_quaternion; |
Makodan | 0:a573c39fe4e8 | 39 | |
Makodan | 0:a573c39fe4e8 | 40 | //InterruptIn NRF_irq(PA_0); |
Makodan | 0:a573c39fe4e8 | 41 | |
Makodan | 0:a573c39fe4e8 | 42 | void sendIRQ(); |
gume | 1:01b7a386487e | 43 | void sendMSG(uint8_t *data, uint8_t len); |
gume | 1:01b7a386487e | 44 | void setupRadio(); |
Makodan | 0:a573c39fe4e8 | 45 | |
Makodan | 0:a573c39fe4e8 | 46 | |
gume | 1:01b7a386487e | 47 | int main() { |
gume | 1:01b7a386487e | 48 | |
gume | 1:01b7a386487e | 49 | SyncData syncData; |
gume | 1:01b7a386487e | 50 | ImuData imuData; |
gume | 1:01b7a386487e | 51 | |
gume | 1:01b7a386487e | 52 | #define ID_UNIQUE_ADDRESS 0x1FF80050 // STM32L0 |
gume | 1:01b7a386487e | 53 | // #define ID_UNIQUE_ADDRESS 0x1FFFF7AC // STM32F0, STM32F3 |
gume | 1:01b7a386487e | 54 | // #define ID_UNIQUE_ADDRESS 0x1FFF7A10 // STM32F2, STM32F4 |
gume | 1:01b7a386487e | 55 | // #define ID_UNIQUE_ADDRESS 0x1FF0F420 // STM32F7 |
Makodan | 0:a573c39fe4e8 | 56 | |
gume | 1:01b7a386487e | 57 | uint16_t *uniqueSerialAddr_stm32 = (uint16_t *)ID_UNIQUE_ADDRESS; |
gume | 1:01b7a386487e | 58 | imuData.sensorId = uniqueSerialAddr_stm32[0] + uniqueSerialAddr_stm32[1] + uniqueSerialAddr_stm32[2] + |
gume | 1:01b7a386487e | 59 | uniqueSerialAddr_stm32[3] + uniqueSerialAddr_stm32[4] + uniqueSerialAddr_stm32[5]; |
gume | 1:01b7a386487e | 60 | |
gume | 1:01b7a386487e | 61 | imuData.packetCntr = 0; |
gume | 1:01b7a386487e | 62 | |
gume | 1:01b7a386487e | 63 | led = 1; |
gume | 1:01b7a386487e | 64 | |
Makodan | 0:a573c39fe4e8 | 65 | i2c.frequency(400000); |
Makodan | 0:a573c39fe4e8 | 66 | |
Makodan | 0:a573c39fe4e8 | 67 | //radio setup |
gume | 1:01b7a386487e | 68 | setupRadio(); |
gume | 1:01b7a386487e | 69 | |
Makodan | 0:a573c39fe4e8 | 70 | // Sensor scanning |
gume | 1:01b7a386487e | 71 | uint8_t s0 = 0; |
gume | 1:01b7a386487e | 72 | uint8_t s1 = 0; |
gume | 1:01b7a386487e | 73 | uint8_t qids[16]; |
gume | 1:01b7a386487e | 74 | memset(&qids, 0, 16); |
gume | 1:01b7a386487e | 75 | int qc = scanSensors(&s0, &s1, (uint8_t*) qids); |
gume | 1:01b7a386487e | 76 | int lastRead = qc - 1; |
Makodan | 0:a573c39fe4e8 | 77 | |
Makodan | 0:a573c39fe4e8 | 78 | while (1) { |
Makodan | 0:a573c39fe4e8 | 79 | |
gume | 1:01b7a386487e | 80 | int rc = 0; |
gume | 1:01b7a386487e | 81 | uint8_t qbuff[8]; |
Makodan | 0:a573c39fe4e8 | 82 | |
gume | 1:01b7a386487e | 83 | int currRead = (lastRead + 1) % qc; |
gume | 1:01b7a386487e | 84 | while (rc < 3) { |
gume | 1:01b7a386487e | 85 | imuData.payload[rc * 9] = qids[currRead]; |
gume | 1:01b7a386487e | 86 | readQIMU(qids[currRead], qbuff); |
gume | 1:01b7a386487e | 87 | memcpy(((uint8_t*) imuData.payload) + rc * 9 + 1, qbuff, 8); |
gume | 1:01b7a386487e | 88 | rc++; |
gume | 1:01b7a386487e | 89 | if (currRead == lastRead) break; |
gume | 1:01b7a386487e | 90 | currRead = (currRead + 1) % qc; |
Makodan | 0:a573c39fe4e8 | 91 | } |
gume | 1:01b7a386487e | 92 | lastRead = currRead; |
gume | 1:01b7a386487e | 93 | |
gume | 1:01b7a386487e | 94 | if(radio.available()) { |
gume | 1:01b7a386487e | 95 | if(radio.getDynamicPayloadSize() < 1)// Corrupt payload has been flushed |
gume | 1:01b7a386487e | 96 | continue; |
gume | 1:01b7a386487e | 97 | radio.read(&syncData, sizeof(syncData)); |
gume | 1:01b7a386487e | 98 | |
gume | 1:01b7a386487e | 99 | int s = 0; // This is the schedule index |
gume | 1:01b7a386487e | 100 | bool schok = false; |
gume | 1:01b7a386487e | 101 | |
gume | 1:01b7a386487e | 102 | for (s = 0; s < 16; s++) { |
gume | 1:01b7a386487e | 103 | if (syncData.schedule[s] == imuData.sensorId) { |
gume | 1:01b7a386487e | 104 | schok = true; |
gume | 1:01b7a386487e | 105 | break; |
gume | 1:01b7a386487e | 106 | } |
gume | 1:01b7a386487e | 107 | if (syncData.schedule[s] == 0) break; |
gume | 1:01b7a386487e | 108 | } |
gume | 1:01b7a386487e | 109 | wait_ms(s * SendDelay); // wait before Send |
gume | 1:01b7a386487e | 110 | |
gume | 1:01b7a386487e | 111 | if (schok) { |
gume | 1:01b7a386487e | 112 | imuData.packetType = PACKET_Q; |
gume | 1:01b7a386487e | 113 | sendMSG((uint8_t*) &imuData, 4 + rc * 9); |
gume | 1:01b7a386487e | 114 | } else { |
gume | 1:01b7a386487e | 115 | // If not scheduled, then send a short message |
gume | 1:01b7a386487e | 116 | imuData.packetType = PACKET_I; |
gume | 1:01b7a386487e | 117 | memcpy(&imuData.payload, &qids, qc); |
gume | 1:01b7a386487e | 118 | sendMSG((uint8_t*) &imuData, 4 + qc); |
gume | 1:01b7a386487e | 119 | } |
gume | 1:01b7a386487e | 120 | imuData.packetCntr++; |
gume | 1:01b7a386487e | 121 | } |
gume | 1:01b7a386487e | 122 | } |
Makodan | 0:a573c39fe4e8 | 123 | } |
Makodan | 0:a573c39fe4e8 | 124 | |
Makodan | 0:a573c39fe4e8 | 125 | |
gume | 1:01b7a386487e | 126 | int scanSensors(uint8_t *s0, uint8_t *s1, uint8_t *qids) { |
gume | 1:01b7a386487e | 127 | |
gume | 1:01b7a386487e | 128 | BNO055 *imu; |
gume | 1:01b7a386487e | 129 | uint8_t *s; |
gume | 1:01b7a386487e | 130 | int c = 0; |
gume | 1:01b7a386487e | 131 | |
gume | 1:01b7a386487e | 132 | for (uint8_t y = 0; y < 2; y++) { |
gume | 1:01b7a386487e | 133 | for (uint8_t x = 0; x < 7; x++) { |
Makodan | 0:a573c39fe4e8 | 134 | |
gume | 1:01b7a386487e | 135 | selectMUX(x); |
gume | 1:01b7a386487e | 136 | if (y == 0) { imu = &imu0; s = s0; } |
gume | 1:01b7a386487e | 137 | else { imu = &imu1; s = s1; } |
Makodan | 0:a573c39fe4e8 | 138 | |
gume | 1:01b7a386487e | 139 | wait(0.001); imu->reset(); wait(0.01); |
Makodan | 0:a573c39fe4e8 | 140 | |
gume | 1:01b7a386487e | 141 | if (imu->chip_ready()) { |
gume | 1:01b7a386487e | 142 | // IMU available |
gume | 1:01b7a386487e | 143 | // imu0.set_mounting_position(MT_P0); |
gume | 1:01b7a386487e | 144 | imu->configure_accelerometer_range(AcclerometerScale); |
Makodan | 0:a573c39fe4e8 | 145 | |
gume | 1:01b7a386487e | 146 | *s |= 1 << x; |
gume | 1:01b7a386487e | 147 | qids[c++] = x * 10 + y; |
Makodan | 0:a573c39fe4e8 | 148 | } |
gume | 1:01b7a386487e | 149 | } |
gume | 1:01b7a386487e | 150 | } |
gume | 1:01b7a386487e | 151 | return c; |
gume | 1:01b7a386487e | 152 | } |
gume | 1:01b7a386487e | 153 | |
gume | 1:01b7a386487e | 154 | void selectMUX(uint8_t channel) |
Makodan | 0:a573c39fe4e8 | 155 | { |
gume | 2:65b259a97e52 | 156 | // char get; |
Makodan | 0:a573c39fe4e8 | 157 | char select; |
Makodan | 0:a573c39fe4e8 | 158 | |
Makodan | 0:a573c39fe4e8 | 159 | select = 1 << channel; |
Makodan | 0:a573c39fe4e8 | 160 | |
gume | 1:01b7a386487e | 161 | i2c.write(0xE0, &select, 1); |
Makodan | 0:a573c39fe4e8 | 162 | wait(0.001); |
gume | 1:01b7a386487e | 163 | /* |
Makodan | 0:a573c39fe4e8 | 164 | i2c.read(0xE0, &get, 1); |
Makodan | 0:a573c39fe4e8 | 165 | wait(0.001); |
gume | 1:01b7a386487e | 166 | i2c.write(0xE0, &select, 1); |
Makodan | 0:a573c39fe4e8 | 167 | wait(0.001); |
gume | 1:01b7a386487e | 168 | i2c.read(0xE0, &get, 1); |
gume | 1:01b7a386487e | 169 | */ |
Makodan | 0:a573c39fe4e8 | 170 | } |
Makodan | 0:a573c39fe4e8 | 171 | |
gume | 1:01b7a386487e | 172 | void readQIMU(uint8_t iid, void* qptr) { |
gume | 1:01b7a386487e | 173 | |
gume | 1:01b7a386487e | 174 | BNO055_QUATERNION_TypeDef *quaternion = (BNO055_QUATERNION_TypeDef *) qptr; |
gume | 1:01b7a386487e | 175 | BNO055 *imu; |
gume | 1:01b7a386487e | 176 | |
gume | 1:01b7a386487e | 177 | if (iid % 10 == 0) imu = &imu0; |
gume | 1:01b7a386487e | 178 | else imu = &imu1; |
gume | 1:01b7a386487e | 179 | |
gume | 1:01b7a386487e | 180 | selectMUX(iid / 10); |
gume | 1:01b7a386487e | 181 | imu->get_quaternion(quaternion); |
Makodan | 0:a573c39fe4e8 | 182 | } |
Makodan | 0:a573c39fe4e8 | 183 | |
gume | 1:01b7a386487e | 184 | void setupRadio() |
Makodan | 0:a573c39fe4e8 | 185 | { |
Makodan | 0:a573c39fe4e8 | 186 | radio.begin(); |
Makodan | 0:a573c39fe4e8 | 187 | radio.setPALevel(RF24_PA_MAX) ; |
Makodan | 0:a573c39fe4e8 | 188 | radio.setDataRate(RF24_2MBPS); |
Makodan | 0:a573c39fe4e8 | 189 | radio.setCRCLength(RF24_CRC_16); |
Makodan | 0:a573c39fe4e8 | 190 | radio.setChannel(RadioChannel); |
Makodan | 0:a573c39fe4e8 | 191 | |
Makodan | 0:a573c39fe4e8 | 192 | radio.setRetries(0,2); |
Makodan | 0:a573c39fe4e8 | 193 | |
Makodan | 0:a573c39fe4e8 | 194 | radio.enableDynamicAck(); |
Makodan | 0:a573c39fe4e8 | 195 | radio.enableDynamicPayloads(); |
Makodan | 0:a573c39fe4e8 | 196 | |
Makodan | 0:a573c39fe4e8 | 197 | radio.openWritingPipe(DataAddress); |
Makodan | 0:a573c39fe4e8 | 198 | radio.openReadingPipe(1,SyncAddress); |
Makodan | 0:a573c39fe4e8 | 199 | //radio.stopListening(); |
Makodan | 0:a573c39fe4e8 | 200 | //radio.setAutoAck(1,false); |
Makodan | 0:a573c39fe4e8 | 201 | radio.startListening(); |
Makodan | 0:a573c39fe4e8 | 202 | } |
Makodan | 0:a573c39fe4e8 | 203 | |
gume | 1:01b7a386487e | 204 | void sendMSG(uint8_t *data, uint8_t len) { |
gume | 1:01b7a386487e | 205 | |
Makodan | 0:a573c39fe4e8 | 206 | radio.stopListening(); |
gume | 1:01b7a386487e | 207 | radio.write(data, len, true); |
Makodan | 0:a573c39fe4e8 | 208 | radio.startListening(); |
gume | 1:01b7a386487e | 209 | |
gume | 1:01b7a386487e | 210 | led = !led; |
Makodan | 0:a573c39fe4e8 | 211 | } |