File content as of revision 3:6bba74b99bd3:

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

#define uniqueSerialAddr_stm32 (unsigned long *)0x1FFFF7E8 
#define STM32_UUID ((uint32_t *)0x40048058)

DigitalOut led(ledpin);
I2C i2c(i2c_sda, i2c_scl);

RF24 radio(spi_MOSI, spi_MISO, spi_SCK, nrf_CE, nrf_CSN );

const uint64_t DataAddress =  0xF0F0F0F0E1LL;
const uint64_t SyncAddress = 0xF0F0F0F0D2LL;



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

struct SyncMsg {
    bool Sleep;             //false if sensor should not sleep
} SyncMsg;

//InterruptIn NRF_irq(PA_0);

void sendIRQ();
void ReadIMU();
void SendMSG();
void SetupRadio();



BNO055 imu(i2c, PA_2); //RESET: not conected, just an unused pin

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

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



int main()
{

    if (imu.chip_ready() == 0) {
        do {
            led = !led;
            wait(0.1);
            led = !led;
            wait(0.1);
        } while(imu.reset());
    }

    imu.set_mounting_position(MT_P0);
    imu.configure_accelerometer_range(AcclerometerScale);

    imu.read_id_inf(&bno055_id_inf);  //read sensor IDs


    SetupRadio();
    
    uint32_t idPart1 = STM32_UUID[0];
    uint32_t idPart2 = STM32_UUID[1];
    uint32_t idPart3 = STM32_UUID[2];
    //radio setup


    //Attach Interrupt
    //NRF_irq.fall(&sendIRQ);



    imuData.sensorID = idPart2;
    imuData.packageCntr = 0;

    while(true) {
        //imu.get_Euler_Angles(&euler_angles);


        if(radio.available()) {
            if(radio.getDynamicPayloadSize() < 1)// Corrupt payload has been flushed
                continue;
            radio.read(&SyncMsg,sizeof(SyncMsg));

            wait_ms( (NodeID-1) * SendDelay); // wait before Send

            ReadIMU();
            SendMSG();
        }



    }//while


}

/*
void sendIRQ(){
     if(radio.getDynamicPayloadSize() < 1)// Corrupt payload has been flushed
        return;

    radio.read(&Sync,sizeof(Sync));
    if(Sync.ID == imuData.sensorID){
        if(Sync.ARange > 0)
            AcclerometerScale = Sync.ARange;

        AGCalFlag = Sync.AGCal;
        MCalFlag = Sync.MCal;
        SendFlag = true;
    }

}*/

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 ReadIMU()
{
    imu.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;


    imu.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 SendMSG()
{
    radio.stopListening();
    radio.write(&imuData, sizeof(imuData), true);
    radio.startListening();

    if(imuData.packageCntr < 255)
        imuData.packageCntr++;
    else
        imuData.packageCntr = 0;
    led = !led;
}