File content as of revision 8:005b0a85be70:

/**
 * Uses the Fusion library for AHRS algorithm:
 *      https://github.com/xioTechnologies/Fusion
 */


#include <cmath>
#include "mbed.h"
#include "uLCD_4DGL.h"
#include "LSM9DS1.h"
#define PI 3.14159
#include "Fusion/Fusion.h"
#define SAMPLE_RATE (100) // replace this with actual sample rate

uLCD_4DGL uLCD(p9,p10,p30);
LSM9DS1 imu(p28, p27, 0xD6, 0x3C);
Serial pc(USBTX, USBRX);

void getAttitude(LSM9DS1& imu, float& roll, float& pitch, float& yaw)
{
    float ax = imu.calcAccel(imu.ax),
          ay = imu.calcAccel(imu.ay),
          az = imu.calcAccel(imu.az),
          mx = -imu.calcMag(imu.mx),
          my = imu.calcMag(imu.my),
          mz = imu.calcMag(imu.mz);

    pitch = atan2(-ax, sqrt(ay * ay + az * az));

    if (az == 0.0)  roll = (ay < 0.0) ? 180 : 0.0;
    else            roll = atan2(ay, az);

    if (my == 0.0)  yaw = (mx < 0.0) ? 180.0 : 0.0;
    else            yaw = atan2(mx, my);

    if (yaw > 180.0)        yaw -=PI;
    else if (yaw < -180.0)  yaw += PI;

}

int main()
{
    ////////////////////
    // INITIALIZATION //
    ////////////////////

    imu.begin();
    if (!imu.begin()) {
        pc.printf("Failed to communicate with LSM9DS1.\n");
    }
    imu.setGyroScale(2000);
    imu.setAccelScale(16);
    imu.calibrate(1);

    // Initialise algorithms
    FusionAhrs ahrs;
    FusionAhrsInitialise(&ahrs);

    // Set AHRS algorithm settings
    const FusionAhrsSettings settings = {
        .gain = 0.5f,
        .accelerationRejection = 10.0f,
        .magneticRejection = 20.0f,
        .rejectionTimeout = 5 * SAMPLE_RATE, /* 5 seconds */
    };
    FusionAhrsSetSettings(&ahrs, &settings);

    Timer timer;
    timer.start();



    //////////////
    // IMU LOOP //
    //////////////

    while (true) {
        // Read IMU
        while(!imu.accelAvailable());
        imu.readAccel();

        while (!imu.gyroAvailable());
        imu.readGyro();

        while (!imu.magAvailable());
        imu.readMag();

        // Elementary Algorithm:
        //float roll, pitch, yaw;
        //getAttitude(imu, roll, pitch, yaw);
        //pc.printf("%7f %7f %7f\r\n", roll, pitch, yaw);

        // Construct algorithm inputs
        FusionVector gyroscope = {          // Gyro data in degrees / s
            imu.calcGyro(imu.gx),
            imu.calcGyro(imu.gy),
            imu.calcGyro(imu.gz)
        };
        FusionVector accelerometer = {      // Accelerometer data in g
            imu.calcAccel(imu.ax),
            imu.calcAccel(imu.ay),
            imu.calcAccel(imu.az)
        };
        FusionVector magnetometer = {       // Accelerometer data in g
            imu.calcMag(imu.mx),
            imu.calcMag(imu.my),
            imu.calcMag(imu.mz)
        };

        // Update AHRS algorithm
        float dt = timer.read();
        timer.reset();
        FusionAhrsUpdate(&ahrs, gyroscope, accelerometer, magnetometer, dt);

        // Print algorithm outputs
        const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));
        const FusionVector earth = FusionAhrsGetEarthAcceleration(&ahrs);

        printf("R %5.1f   P %5.1f   Y %5.1f\n\r",
               euler.angle.roll, euler.angle.pitch, euler.angle.yaw);

        float roll = euler.angle.roll   / 360 * 2 * PI,
              pitch = euler.angle.pitch / 360 * 2 * PI,
              yaw = euler.angle.yaw     / 360 * 2 * PI;

        uLCD.cls();

        float x = 64 + roll * 60;
        float y = 64 + pitch * 60;
        float dx = 12 * cos(yaw);
        float dy = 12 * sin(yaw);

        uLCD.circle(x, y, 2, GREEN);
        uLCD.line(x-dx, y-dy, x+dx, y+dy, GREEN);
        wait(.05);
    }
}