BME SmartLab / Mbed 2 deprecated BabaTalp-NRF

NRF version of the "BabaTalp" code. (The original radioless code is in python)

Dependencies:   BNO055_fusion RF24 mbed

Fork of L053R8_mux_BNO by Daniel Mako

main.cpp@3:4b320a163565, 2017-04-22 (annotated)

Committer:
gume
Date:
Sat Apr 22 10:52:21 2017 +0000
Revision:
3:4b320a163565
Parent:
2:65b259a97e52 
Change libraries

Who changed what in which revision?

UserRevisionLine numberNew 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 }