File content as of revision 0:eb5dec59e660:

#include "LSM9DS1.h"

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

PwmOut servo(p21);

float PI = 3.14159265358979323846f;
float angle;
float pulseWidth;
float pulseCoeff = 10.0;
float pulseOffset = 400;

void mag_correction(float mx, float my, float mz, float mag_c[3])
{
    float bias[3] = {0.2230,0.8809,0.1835};
    float scale[3][3] = {{0.9918,0.0913,0.0922},
        {0.0913,1.6211,0.1597},
        {0.0922,0.1597,0.6479}
    };
    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);
        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;
        }

        angle = yaw/4;
        if (angle < 0) { // Ensuring the angle cannot exceed 180 or go below 0.
            angle = 0;
        } else if (angle > 90) {
            angle = 90.0;
        }
        pulseWidth = pulseCoeff * angle + pulseOffset;
        servo.pulsewidth(pulseWidth/1000000.000);



        pc.printf("$IMU,3,11,%f,%3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f;\r\n", t.read(), lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz), lol.calcAccel(lol.ax), lol.calcAccel(lol.ay), lol.calcAccel(lol.az), roll, pitch, yaw, pulseWidth);
    }
}