File content as of revision 1:d90cd9d60f7e:

#include "LSM9DS1.h"

DigitalOut myled(LED1);
Serial pc(USBTX, USBRX);
LSM9DS1 lol(p28, p27, 0xD6, 0x3C); // IMU pins.
Timer t; // Timer to collect time series.

PwmOut servo(p21); // Servo pin

float PI = 3.14159265358979323846f;
float angle; // Angle of the servo
float pulseWidth; // Pulse width and other pulse variables help communicate to the servo the correct angle through the PMW
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}; // These matrices for bias and scale were found from the previous calibration
    float scale[3][3] = {{0.9918,0.0913,0.0922},
        {0.0913,1.6211,0.1597},
        {0.0922,0.1597,0.6479}
    };
    // Full completing the calibration using the values of bias and scale.
    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; // Creating variables
    float accel[3], mag[3], gyro[3];

    lol.begin(); // Starting the connecting to the imu
    if(!lol.begin()) { // If it didn't connect, print the script below.
        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(); // reading the different values from the IMU
        lol.readGyro();
        lol.readAccel();
        accel[0] = lol.calcAccel(lol.ax); // Putting the acceleration values all into one matrix
        accel[1] = lol.calcAccel(lol.ay);
        accel[2] = - lol.calcAccel(lol.az);
        gyro[0] = lol.calcGyro(lol.gx); // Putting all the gyro values all into one matrix
        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); // Correcting mag.
        mag[2] = -mag[2]; // Making mag negative.
        
        // Calculating roll pitch and yaw from the values above.
        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) { // Reseting yaw roll and pitch to 360 if it ever gets below zero.
            yaw = yaw+360;
        }
        if(roll<=0) {
            roll = roll+360;
        }
        if(pitch<=0) {
            pitch = pitch+360;
        }

        angle = yaw/4; // Setting the angle of the servo to be proportional to the angle of the yaw
        if (angle < 0) { // Ensuring the angle cannot exceed 190 or go below 0.
            angle = 0;
        } else if (angle > 90) {
            angle = 90.0;
        }
        pulseWidth = pulseCoeff * angle + pulseOffset; // Calculting the pulse width from the angle value
        servo.pulsewidth(pulseWidth/1000000.000); // Setting the physical servo to the right angle.


        // Printing the values to coolterm
        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);
    }
}