File content as of revision 3:4b320a163565:

#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;
}