File content as of revision 7:10653d0475f5:

//************************************
// test fusion
// ***********************************
//
#include "Fusion.h"
#include "LSM6DS33_GR1.h"
//
float samplePeriod = 0.01f; // replace this value with actual sample period in seconds/
// tracteur
FusionBias TBias;
FusionAhrs TAhrs;
FusionVector3 gTSensitivity = {
    .axis.x = 1.0f,
    .axis.y = 1.0f,
    .axis.z = 1.0f,
}; // replace these values with actual sensitivity in degrees per second per lsb as specified in gyroscope datasheet
FusionVector3 aTSensitivity = {
    .axis.x = 1.0f,
    .axis.y = 1.0f,
    .axis.z = 1.0f,
}; // replace these values with actual sensitivity in g per lsb as specified in accelerometer datasheet
//
// outil
FusionBias OBias;
FusionAhrs OAhrs;
FusionVector3 gOSensitivity = {
    .axis.x = 1.0f,
    .axis.y = 1.0f,
    .axis.z = 1.0f,
}; // replace these values with actual sensitivity in degrees per second per lsb as specified in gyroscope datasheet
FusionVector3 aOSensitivity = {
    .axis.x = 1.0f,
    .axis.y = 1.0f,
    .axis.z = 1.0f,
}; // replace these values with actual sensitivity in g per lsb as specified in accelerometer datasheet



int flag_fusion=0;
Serial pc(SERIAL_TX, SERIAL_RX);  // I/O terminal PC
DigitalOut led(LED1);  // led
LSM6DS33 AGT(PB_9, PB_8);  // accelero gyro tracteur



float gg[3];
float aRes,gRes;
Ticker affich;
//

//
signed long round(double value)
{
    return((long)floor(value+0.5));
}

//
void aff(void)
{
    flag_fusion=1;
    // tracteur
    // Calibrate gyroscope

}
//
int main()
{
    wait(1);
    pc.baud(115200);
    pc.printf("Hello Fusion \r\n");
    //aRes = 2.0 / 32768.0;
    //gRes = 250.0 / 32768.0;
    //pi=4*atan(1.0);
    //init IMU
    AGT.begin(LSM6DS33::G_SCALE_500DPS,LSM6DS33::A_SCALE_8G,LSM6DS33::G_ODR_104,LSM6DS33::A_ODR_104);
    //wait(0.5);
    AGT.calibration(1000);
    //
    char rr= AGT.readReg(0x15);
    pc.printf(" reg 0x15 : %x \r\n",rr);
    rr= AGT.readReg(0x10);
    pc.printf(" reg 0x10 : %x \r\n",rr);
    AGT.writeReg(0x10,rr|0x03);  // 50HZ
    rr= AGT.readReg(0x10);
    pc.printf(" reg 0x10 : %x \r\n",rr);
    rr= AGT.readReg(0x15);
    pc.printf(" reg 0x15 : %x \r\n",rr);
    rr= AGT.readReg(0x11);
    pc.printf(" reg 0x11 : %x \r\n",rr);
    
AGT.writeReg(0x13,0x80);  // BW SCAL
rr= AGT.readReg(0x13);
    pc.printf(" reg 0x13 : %x \r\n",rr);
    //
    // Initialise gyroscope bias correction algorithm
    FusionBiasInitialise(&TBias, 20.0f, samplePeriod); // stationary threshold = 0.5 degrees per second
    FusionBiasInitialise(&OBias, 20.0f, samplePeriod); // stationary threshold = 0.5 degrees per second

    // Initialise AHRS algorithm
    FusionAhrsInitialise(&TAhrs, 0.5f); // gain = 0.5
    FusionAhrsInitialise(&OAhrs, 0.5f); // gain = 0.5

    // The contents of this do while loop should be called for each time new sensor measurements are available
    //
    int cpt=0;
    affich.attach(&aff,0.01);
    while(1) {
        // lecture accelero / gyro

        if(flag_fusion==1) {
            cpt++;
            AGT.readAccel();
            AGT.readGyro();
            AGT.readTemp();
            FusionVector3 uncalTGyroscope = {
                .axis.x = AGT.gx-AGT.gx_off, /* replace this value with actual gyroscope x axis measurement in lsb */
                .axis.y = AGT.gy-AGT.gy_off, /* replace this value with actual gyroscope y axis measurement in lsb */
                .axis.z = AGT.gz-AGT.gz_off, /* replace this value with actual gyroscope z axis measurement in lsb */
                //.axis.z = 0, /* replace this value with actual gyroscope z axis measurement in lsb */
            };
            FusionVector3 calTGyroscope = FusionCalibrationInertial(uncalTGyroscope, FUSION_ROTATION_MATRIX_IDENTITY, gTSensitivity, FUSION_VECTOR3_ZERO);
            // Calibrate accelerometer
            FusionVector3 uncalTAccelerometer = {
                .axis.x = AGT.ax, /* replace this value with actual accelerometer x axis measurement in lsb */
                .axis.y = AGT.ay, /* replace this value with actual accelerometer y axis measurement in lsb */
                .axis.z = AGT.az, /* replace this value with actual accelerometer z axis measurement in lsb */
            };
            FusionVector3 calTAccelerometer = FusionCalibrationInertial(uncalTAccelerometer, FUSION_ROTATION_MATRIX_IDENTITY, aTSensitivity, FUSION_VECTOR3_ZERO);
            // Update gyroscope bias correction algorithm
            calTGyroscope = FusionBiasUpdate(&TBias, calTGyroscope);
            led= FusionBiasIsActive(&TBias);
            // Update AHRS algorithm
            FusionAhrsUpdateWithoutMagnetometer(&TAhrs, calTGyroscope, calTAccelerometer, samplePeriod);
//
            // Outil
            // Calibrate gyroscope
            FusionVector3 uncalOGyroscope = {
                .axis.x = -(AGT.gz-AGT.gz_off), /* replace this value with actual gyroscope x axis measurement in lsb */
                .axis.y = -(AGT.gy-AGT.gy_off), /* replace this value with actual gyroscope y axis measurement in lsb */
                .axis.z = -(AGT.gx-AGT.gx_off), /* replace this value with actual gyroscope z axis measurement in lsb */
            };
            FusionVector3 calOGyroscope = FusionCalibrationInertial(uncalOGyroscope, FUSION_ROTATION_MATRIX_IDENTITY, gOSensitivity, FUSION_VECTOR3_ZERO);
            // Calibrate accelerometer
            FusionVector3 uncalOAccelerometer = {
                .axis.x = -AGT.az, /* replace this value with actual accelerometer x axis measurement in lsb */
                .axis.y = -AGT.ay, /* replace this value with actual accelerometer y axis measurement in lsb */
                .axis.z = -AGT.ax, /* replace this value with actual accelerometer z axis measurement in lsb */
            };
            FusionVector3 calOAccelerometer = FusionCalibrationInertial(uncalOAccelerometer, FUSION_ROTATION_MATRIX_IDENTITY, aOSensitivity, FUSION_VECTOR3_ZERO);
            // Update gyroscope bias correction algorithm
            calOGyroscope = FusionBiasUpdate(&OBias, calOGyroscope);
            // Update AHRS algorithm
            FusionAhrsUpdateWithoutMagnetometer(&OAhrs, calOGyroscope, calOAccelerometer, samplePeriod);
FusionEulerAngles eulerTAngles = FusionQuaternionToEulerAngles(FusionAhrsGetQuaternion(&TAhrs));

        FusionEulerAngles eulerOAngles = FusionQuaternionToEulerAngles(FusionAhrsGetQuaternion(&OAhrs));
            flag_fusion=0;
            if(cpt%1==0){
                pc.printf("$%f %f;", eulerTAngles.angle.roll, eulerTAngles.angle.pitch);
                 //pc.printf("$%f %f %f %f;", uncalTGyroscope.axis.x,calTGyroscope.axis.x,uncalTGyroscope.axis.y,calTGyroscope.axis.y);
                
                }
        }
        // Print Euler angles
        
        //printf("Roll = %0.1f, Pitch = %0.1f, Yaw = %0.1f\r\n", eulerAngles.angle.roll, eulerAngles.angle.pitch, eulerAngles.angle.yaw);
        // pc.printf("$%f %f %f;", eulerAngles.angle.roll, eulerAngles.angle.pitch, eulerAngles.angle.yaw);
        // pc.printf("$%f %f %f %f;", eulerAngles.angle.roll, eulerAngles.angle.pitch,angle_trac,cc);
        //pc.printf("$%f %f %f %f ;", eulerTAngles.angle.roll, eulerTAngles.angle.pitch,eulerOAngles.angle.roll, eulerOAngles.angle.pitch);
        //////////// pc.printf("$%f %f %f;", eulerOAngles.angle.roll, eulerOAngles.angle.pitch,AGT.temperature_c);
        ////////////pc.printf("$%f %f;", eulerTAngles.angle.roll, eulerTAngles.angle.pitch);
        //pc.printf("$%f %f %f;", gyroT_out[0],accT_out[0],angle_trac);
        //pc.printf("$%f %f %f;", AGT.gx-AGT.gx_off,AGT.gy-AGT.gy_off,AGT.gz-AGT.gz_off);
        //pc.printf("$%f %f %f;", gg[0],gg[1],gg[2]);
        //
        //wait(0.1);
        // pc.printf(">>> %f %f %f  \r\n",GXOT,GYOT,GZOT);
        ////pc.printf(">>> %f %f %f  \r\n",AGT.gx_off,AGT.gy_off,AGT.gz_off);
        // pc.printf("> %f %f %f %f %f %f \r\n",AXT,AYT,AZT,GXT,GYT,GZT);
        ////pc.printf("< %f %f %f %f %f %f \r\n\r\n",AGT.ax,AGT.ay,AGT.az,AGT.gx-AGT.gx_off,AGT.gy,AGT.gz);
        // pc.printf("> %f %f %f %f %f %f \r\n",AXM,AYM,AZM,GXM,GYM,GZM);
        // pc.printf(">>> %f %f %f  \r\n",GXOM,GYOM,GZOM);
        // allichage des differents ecrans

    }
}