![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
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
- Committer:
- gume
- Date:
- 2017-02-24
- Revision:
- 1:01b7a386487e
- Parent:
- 0:a573c39fe4e8
- Child:
- 2:65b259a97e52
File content as of revision 1:01b7a386487e:
#include "mbed.h" #include "BNO055.h" #include "config.h" #include "RF24.h" I2C i2c(i2c_sda, i2c_scl); DigitalOut led(ledpin); RF24 radio(spi_MOSI, spi_MISO, spi_SCK, nrf_CE, nrf_CSN ); const uint64_t DataAddress = 0xF0F0F0F0E1LL; const uint64_t SyncAddress = 0xF0F0F0F0D2LL; #define PACKET_Q 3 #define PACKET_I 4 int scanSensors(uint8_t *s0, uint8_t *s1, uint8_t* qids); void selectMUX(uint8_t channel); void readQIMU(uint8_t iid, void *qptr); uint8_t AcclerometerScale = 3; // 3 = 16G typedef struct { uint16_t sensorId; uint8_t packetType; uint8_t packetCntr; uint8_t payload[28]; // int16_t q[4], uint8_t c, uint8_t temp[2], int16_t la[3]; } ImuData; typedef struct { uint16_t schedule[16]; // Order of the sensors } SyncData; BNO055 imu0(i2c_sda, i2c_scl, unused_pin, 0x50, MODE_NDOF); // Address: 0x28 BNO055 imu1(i2c_sda, i2c_scl, unused_pin, 0x52, MODE_NDOF); // Address: 0x29 BNO055_QUATERNION_TypeDef BNO055_quaternion; //InterruptIn NRF_irq(PA_0); void sendIRQ(); void sendMSG(uint8_t *data, uint8_t len); void setupRadio(); int main() { SyncData syncData; ImuData imuData; #define ID_UNIQUE_ADDRESS 0x1FF80050 // STM32L0 // #define ID_UNIQUE_ADDRESS 0x1FFFF7AC // STM32F0, STM32F3 // #define ID_UNIQUE_ADDRESS 0x1FFF7A10 // STM32F2, STM32F4 // #define ID_UNIQUE_ADDRESS 0x1FF0F420 // STM32F7 uint16_t *uniqueSerialAddr_stm32 = (uint16_t *)ID_UNIQUE_ADDRESS; imuData.sensorId = uniqueSerialAddr_stm32[0] + uniqueSerialAddr_stm32[1] + uniqueSerialAddr_stm32[2] + uniqueSerialAddr_stm32[3] + uniqueSerialAddr_stm32[4] + uniqueSerialAddr_stm32[5]; imuData.packetCntr = 0; led = 1; i2c.frequency(400000); //radio setup setupRadio(); // Sensor scanning uint8_t s0 = 0; uint8_t s1 = 0; uint8_t qids[16]; memset(&qids, 0, 16); int qc = scanSensors(&s0, &s1, (uint8_t*) qids); int lastRead = qc - 1; while (1) { int rc = 0; uint8_t qbuff[8]; int currRead = (lastRead + 1) % qc; while (rc < 3) { imuData.payload[rc * 9] = qids[currRead]; readQIMU(qids[currRead], qbuff); memcpy(((uint8_t*) imuData.payload) + rc * 9 + 1, qbuff, 8); rc++; if (currRead == lastRead) break; currRead = (currRead + 1) % qc; } lastRead = currRead; if(radio.available()) { if(radio.getDynamicPayloadSize() < 1)// Corrupt payload has been flushed continue; radio.read(&syncData, sizeof(syncData)); int s = 0; // This is the schedule index bool schok = false; for (s = 0; s < 16; s++) { if (syncData.schedule[s] == imuData.sensorId) { schok = true; break; } if (syncData.schedule[s] == 0) break; } wait_ms(s * SendDelay); // wait before Send if (schok) { imuData.packetType = PACKET_Q; sendMSG((uint8_t*) &imuData, 4 + rc * 9); } else { // If not scheduled, then send a short message imuData.packetType = PACKET_I; memcpy(&imuData.payload, &qids, qc); sendMSG((uint8_t*) &imuData, 4 + qc); } imuData.packetCntr++; } } } int scanSensors(uint8_t *s0, uint8_t *s1, uint8_t *qids) { BNO055 *imu; uint8_t *s; int c = 0; for (uint8_t y = 0; y < 2; y++) { for (uint8_t x = 0; x < 7; x++) { selectMUX(x); if (y == 0) { imu = &imu0; s = s0; } else { imu = &imu1; s = s1; } wait(0.001); imu->reset(); wait(0.01); if (imu->chip_ready()) { // IMU available // imu0.set_mounting_position(MT_P0); imu->configure_accelerometer_range(AcclerometerScale); *s |= 1 << x; qids[c++] = x * 10 + y; } } } return c; } void selectMUX(uint8_t channel) { char get; char select; select = 1 << channel; i2c.write(0xE0, &select, 1); wait(0.001); /* i2c.read(0xE0, &get, 1); wait(0.001); i2c.write(0xE0, &select, 1); wait(0.001); i2c.read(0xE0, &get, 1); */ } void readQIMU(uint8_t iid, void* qptr) { BNO055_QUATERNION_TypeDef *quaternion = (BNO055_QUATERNION_TypeDef *) qptr; BNO055 *imu; if (iid % 10 == 0) imu = &imu0; else imu = &imu1; selectMUX(iid / 10); imu->get_quaternion(quaternion); } void setupRadio() { radio.begin(); radio.setPALevel(RF24_PA_MAX) ; radio.setDataRate(RF24_2MBPS); radio.setCRCLength(RF24_CRC_16); radio.setChannel(RadioChannel); radio.setRetries(0,2); radio.enableDynamicAck(); radio.enableDynamicPayloads(); radio.openWritingPipe(DataAddress); radio.openReadingPipe(1,SyncAddress); //radio.stopListening(); //radio.setAutoAck(1,false); radio.startListening(); } void sendMSG(uint8_t *data, uint8_t len) { radio.stopListening(); radio.write(data, len, true); radio.startListening(); led = !led; }