File content as of revision 1:0e63617e1965:

#include "mbed.h"
#include "esc.h" 
#include "LSM6DS3.h"
#include "IMUfilter.h"

// refresh time. set to 500 for part 2 and 50 for part 4
#define REFRESH_TIME_MS 100
//Gravity at Earth's surface in m/s/s
#define g0 9.812865328
//Convert from degrees to radians.
#define toRadians(x) (x * 0.01745329252)

const int   LSM6DS3_ADDRESS = 0xD4;
int PERIOD = 20;

//float       MOTOR_PRINT; 
Serial      pc(SERIAL_TX, SERIAL_RX); 
ESC         MOTOR(PA_8, PERIOD);
AnalogIn    PRESSURE_SENSOR(PA_0);
//Ticker      UpdateScreen;
LSM6DS3     imu(PF_0, PF_1, LSM6DS3_ADDRESS);
IMUfilter imuFilter(REFRESH_TIME_MS/1000, 10);

//Init Serial port and LSM6DS3 chip
void setup()
{
    // Use the begin() function to initialize the LSM9DS0 library.
    // You can either call it with no parameters (the easy way):
    // SLEEP
//    uint16_t status = imu.begin(imu.G_SCALE_245DPS, imu.A_SCALE_2G,
//                                imu.G_POWER_DOWN, imu.A_POWER_DOWN);
    // LOWEST
//    uint16_t status = imu.begin(imu.G_SCALE_245DPS, imu.A_SCALE_2G,
//                                imu.G_ODR_13_BW_0, imu.A_ODR_13);
    // HIGHEST
//    uint16_t status = imu.begin(imu.G_SCALE_2000DPS, imu.A_SCALE_8G,
//                                imu.G_ODR_1660, imu.A_ODR_6660);
                                
    uint16_t status = imu.begin(imu.G_SCALE_245DPS, imu.A_SCALE_2G,
                                imu.G_ODR_1660, imu.A_ODR_6660);
 
    //Make sure communication is working
    pc.printf("LSM6DS3 WHO_AM_I's returned: 0x%X\r\n", status);
    pc.printf("Should be 0x69\r\n");
}
 
int main()
{
    //MOTOR.setThrottle(0.0); 
    bool started = false;
    
    while (true){
        
        if(PRESSURE_SENSOR > 0.9){
            
            if (not started){
                setup();  //Setup sensor and Serial
                pc.printf("\r\n------ LSM6DS3 Demo -----------\r\n");
                started=true;
            }
 
//            imu.readTemp();
//            pc.printf("Temp:\r\n");
//            pc.printf("TC: %2f\r\n", imu.temperature_c);
//            pc.printf("TF: %2f\r\n", imu.temperature_f);
//            
            imu.readAccel();
//            pc.printf("Accel:\r\n");
//            pc.printf("AX: %2f\r\n", imu.ax);
//            pc.printf("AY: %2f\r\n", imu.ay);
//            pc.printf("AZ: %2f\r\n", imu.az);
//     
            imu.readGyro();
//            pc.printf("Gyro:\r\n");
//            pc.printf("GX: %2f\r\n", imu.gx);
//            pc.printf("GY: %2f\r\n", imu.gy);
//            pc.printf("GZ: %2f\r\n\r\n", imu.gz);
            
            // radians = (degrees*pi)/180
            double rx = (imu.gx*PI)/180; 
            double ry = (imu.gy*PI)/180;
            double rz = (imu.gz*PI)/180;
            
            // m/(s^2) = g0*g
            double mx = g0*imu.ax;
            double my = g0*imu.ay;
            double mz = g0*imu.az;
            
            imuFilter.updateFilter(rx, ry, rz, mx, my, mz);
            imuFilter.computeEuler();
            pc.printf("%f, %f, %f\r\n\r\n", imuFilter.getRoll(), imuFilter.getPitch(), imuFilter.getYaw());
           
            wait_ms(REFRESH_TIME_MS);
        }
        else {
            started=false;
            imuFilter.reset();
        }
    }
}