File content as of revision 1:be4184d4674a:

#include "mbed.h"
#include "LSM9DS1.h"

DigitalOut myled(LED1);
Serial pc(USBTX, USBRX);
LSM9DS1 lol(p28, p27, 0xD6, 0x3C);
Timer t;

float PI = 3.14159265358979323846f;

void mag_correction(float mx, float my, float mz, float mag_c[3])
{
    float bias[3]= {-0.0259,0.2015,0.1879};
    float scale[3][3]= {{1.0269,0.0512,0.0242}, {0.0512,1.0074,0.0203}, {0.0242,0.0203,1.0419}};
    mag_c[0]=(mx-bias[0])*scale[0][0]+(my-bias[1])*scale[1][0]+(mz-bias[2])*scale[2][0];
    mag_c[1]=(mx-bias[0])*scale[0][1]+(my-bias[1])*scale[1][1]+(mz-bias[2])*scale[2][1];
    mag_c[2]=(mx-bias[0])*scale[0][2]+(my-bias[1])*scale[1][2]+(mz-bias[2])*scale[2][2];
}

int main()
{
    float roll, pitch, yaw;
    float accel[3], mag[3], gyro[3];

    lol.begin();
    if (!lol.begin()) {
        pc.printf("Failed to communicate with LSM9DS1. \n");
    }

    lol.calibrate(true);
    pc.printf("Gyro bias = %f,%f,%f\r\n", lol.gBias[0], lol.gBias[1], lol.gBias[2]);
    pc.printf("Accel bias = %f,%f,%f\r\n", lol.aBias[0], lol.aBias[1], lol.aBias[2]);

    wait(5);
    t.start();

    while(1) {
        lol.readMag();
        lol.readGyro();
        lol.readAccel();

        accel[0]=lol.calcAccel(lol.ax);
        accel[1]=lol.calcAccel(lol.ay);
        accel[2]=-lol.calcAccel(lol.az);
        gyro[0]=lol.calcGyro(lol.gx);
        gyro[1]=lol.calcGyro(lol.gy);
        gyro[2]=-lol.calcGyro(lol.gz);

        mag_correction(lol.calcMag(lol.mx), lol.calcMag(lol.my), lol.calcMag(lol.mz), mag);
        mag[2]=-mag[2];

        roll=atan2(accel[1], accel[2]/abs(accel[2])*(sqrt((accel[0]*accel[0])+(accel[2]*accel[2]))));
        pitch=-atan2(-accel[0],(sqrt((accel[1]*accel[1])+(accel[2]*accel[2]))));

        float Yh=(mag[1]*cos(roll))-(mag[2]*sin(roll));
        float Xh=(mag[0]*cos(pitch))+(mag[1]*sin(roll)*sin(pitch))+(mag[2]*cos(roll)*sin(pitch));

        yaw=atan2(Yh, Xh);
        pitch *=180.0f/PI;
        yaw *=180.0f/PI;
        roll *=180.0f/PI;

        if(yaw<=0) {
            yaw=yaw+360;
        }
        if(roll<=0) {
            roll=roll+360;
        }
        if(pitch<=0) {
            pitch=pitch+360;
        }

        pc.printf("$IMU,4,4,%f,%3.3f,%3.3f,%3.3f;\r\n",t.read(),roll,pitch,yaw);
    }
}