File content as of revision 0:a573c39fe4e8:

#include "mbed.h"
#include "BNO055.h"
#include "config.h"
#include "RF24.h"


 
I2C i2c(i2c_sda, i2c_scl);
Serial pc(TXD, RXD, 9600);
DigitalOut myled(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 1
#define PACKET_I 2

void ScanMUX();     //Scanning on all channels
void Read1MUX();    //Read sensors on 0x28 address (all channels)
void Read2MUX();    //Read sensors on 0x29 address (all channels)
void ChannelMUX(uint8_t channel);
void ReadIMU1(uint8_t channel);     //Single sensor reading on 0x28 address
void ReadIMU2(uint8_t channel);     //Single sensor reading on 0x29 address

uint8_t AcclerometerScale = 3; // 3 = 16G
bool AGCalFlag = false;
bool MCalFlag = false;
bool SendFlag = false;

// Sensor arrays - contain available sensors
bool sensarray1[] = {false, false, false, false, false, false, false};  // 0x28 array
bool sensarray2[] = {false, false, false, false, false, false, false};  // 0x29 array


struct imuData {
    uint8_t sensorID;
    uint8_t packageCntr;
    int16_t q[4];
    int32_t linAcc[3];

} imuData;

BNO055 imu1(i2c_sda, i2c_scl, unused_pin, 0x50, MODE_NDOF);        // Address: 0x28
BNO055 imu2(i2c_sda, i2c_scl, unused_pin, 0x52, MODE_NDOF);        // Address: 0x29

BNO055_ID_INF_TypeDef bno055_id_inf;
BNO055_QUATERNION_TypeDef BNO055_quaternion;
BNO055_EULER_TypeDef  euler_angles;
BNO055_LIN_ACC_TypeDef linear_acceleration;

struct SyncMsg {
    bool Sleep;             //false if sensor should not sleep
} SyncMsg;
 
//InterruptIn NRF_irq(PA_0);
 
void sendIRQ();
void SendMSG();
void SetupRadio();

 
int main()
{   
    
    myled=1;
    int i;
         
    i2c.frequency(400000);
    
    //radio setup
    SetupRadio();
    

    imuData.packageCntr = 0;
    
    // Sensor scanning
    ScanMUX();
    
    // Print out sensor arrays
    pc.printf("\r\n\t\tCH0\tCH1\tCH2\tCH3\tCH4\tCH5\tCH6");
    pc.printf("\r\n0x28 sensors:");
    for(i=0; i<7; i++){
        pc.printf("\t%d", sensarray1[i]);}
    
    pc.printf("\r\n0x29 sensors:");
    for(i=0; i<7; i++){
        pc.printf("\t%d", sensarray2[i]);}
    pc.printf("\r\n");
    
       
    while (1) {
                 
        Read1MUX();
        Read2MUX();
            
        
        myled = !myled;
        
        }
 
}


void ScanMUX(){

  for(uint8_t x=0; x<7; x++)                            
     {
       
        ChannelMUX(x);                             
            
        pc.printf("Scanning CH: %d\r\n", x);           
                      
            wait(0.001);
            imu1.reset();
            imu2.reset();
            wait(0.01);
            
        if(imu1.chip_ready() == 0){
            pc.printf("Bosch BNO055 is NOT available at address 0x28!!\r\n");
            
            imu1.reset();
            
            }
            
         if(imu2.chip_ready() == 0){
            pc.printf("Bosch BNO055 is NOT available at address 0x29!!\r\n");
            
            imu2.reset();
            
            }   
            
            
        if(imu1.chip_ready() == 1){    
            pc.printf("Bosch BNO055 is AVAILABLE at address 0x28!!\r\n");
            imu1.read_id_inf(&bno055_id_inf);
            pc.printf("CHIP ID:0x%02x, ACC ID:0x%02x, MAG ID:0x%02x, GYR ID:0x%02x, ",
                      bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, bno055_id_inf.gyr_id);
            pc.printf("SW REV:0x%04x, BL REV:0x%02x\r\n",
                       bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id); 
            
            imu1.set_mounting_position(MT_P0);
            imu1.configure_accelerometer_range(AcclerometerScale);
            
            sensarray1[x] = true;
         
            }
            
            
            
        if(imu2.chip_ready() == 1){
            pc.printf("Bosch BNO055 is AVAILABLE at address 0x29!!\r\n");          
            imu2.read_id_inf(&bno055_id_inf);
            pc.printf("CHIP ID:0x%02x, ACC ID:0x%02x, MAG ID:0x%02x, GYR ID:0x%02x, ",
                       bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, bno055_id_inf.gyr_id);
            pc.printf("SW REV:0x%04x, BL REV:0x%02x\r\n",
                       bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id); 
                        
            imu2.set_mounting_position(MT_P0);
            imu2.configure_accelerometer_range(AcclerometerScale);
            
            sensarray2[x] = true;
            
            }
        }
 
 
 }
 
 
 void Read1MUX(){
    
    uint8_t i=0;
        
    for( i=0; i<7; i++)
     {
       
       ChannelMUX(i);

            if(sensarray1[i]){
            ReadIMU1(i);
            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]);
        
            }
             
        }

     }
     

void Read2MUX(){
     
    uint8_t i=0;
        
    for( i=0; i<7; i++)
     {
       
       ChannelMUX(i);
   
            if(sensarray2[i]){
            ReadIMU2(i);
            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]);
        
            }
    
        }

 }
 
 
void ChannelMUX(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 ReadIMU1(uint8_t channel)
{
    imuData.sensorID = (channel * 10);
    imu1.get_quaternion(&BNO055_quaternion);

    imuData.q[0] = BNO055_quaternion.w;
    imuData.q[1] = BNO055_quaternion.x;
    imuData.q[2] = BNO055_quaternion.y;
    imuData.q[3] = BNO055_quaternion.z;


    imu1.get_abs_accel(&linear_acceleration);

    float lax = 1.0;
    float lay = 2.0;
    float laz = 3.0;

    lax = (float)linear_acceleration.x;
    lay = (float)linear_acceleration.y;
    laz = (float)linear_acceleration.z;

    memcpy(imuData.linAcc , &lax, 4);
    memcpy(&imuData.linAcc[1] , &lay, 4);
    memcpy(&imuData.linAcc[2] , &laz, 4);

}

 void ReadIMU2(uint8_t channel)
{
    imuData.sensorID = ((channel * 10) + 1);              // ID: CH number + 1
    imu2.get_quaternion(&BNO055_quaternion);

    imuData.q[0] = BNO055_quaternion.w;
    imuData.q[1] = BNO055_quaternion.x;
    imuData.q[2] = BNO055_quaternion.y;
    imuData.q[3] = BNO055_quaternion.z;


    imu2.get_abs_accel(&linear_acceleration);

    float lax = 1.0;
    float lay = 2.0;
    float laz = 3.0;

    lax = (float)linear_acceleration.x;
    lay = (float)linear_acceleration.y;
    laz = (float)linear_acceleration.z;

    memcpy(imuData.linAcc , &lax, 4);
    memcpy(&imuData.linAcc[1] , &lay, 4);
    memcpy(&imuData.linAcc[2] , &laz, 4);

}

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()
{
    radio.stopListening();
    radio.write(&imuData, sizeof(imuData), true);
    radio.startListening();
 
    if(imuData.packageCntr < 255)
        imuData.packageCntr++;
    else
        imuData.packageCntr = 0;
    myled = !myled;
}