File content as of revision 0:042921e9375d:

#include "LSM9DS1.h"

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

float PI=3.1415926535897;

void mag_correction(float mx, float my, float mz, float mag_c[3])
{
    float bias[3] = {-0.1102,0.3965,-0.0683};
    float scale[3][3] = {{1.0558, 0.0640, -0.0220},
        {0.0640, 1.0657, 0.0645},
        {-0.0220, 0.0645, 1.0650}
    };

    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(1);
    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); //reverse z
        gyro[0]= lol.calcGyro(lol.gx);
        gyro[1]= lol.calcGyro(lol.gy);
        gyro[2]= - lol.calcGyro(lol.gz); //reverse z
        mag_correction(lol.calcMag(lol.mx), lol.calcMag(lol.my), lol.calcMag(lol.mz), mag);
        mag[2] = - mag[2]; //reverse z
        //reg geometry conversion
        roll = atan2 (accel[1], accel[2]/abs(accel[2])*(sqrt ((accel[0]*accel[0]) +(accel[2] * accel[2]))));
        //negative is added after testing pose in real situation
        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);
    }
}