File content as of revision 2:405f8e1a01d3:

#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.812865328f
//Convert from degrees to radians. radians = (degrees*pi)/180
#define toRadians(x) (x * 0.01745329252f)
//Convert from radians to degrees.
#define toDegrees(x) (x * 57.2957795)

const int   LSM6DS3_ADDRESS = 0xD4;
float       DEGREES[3];
float       ACCELERATIONS[3];
float       ANGLES[3];
//const int   LSM6DS3_ADDRESS = 0x69;

Serial      pc(SERIAL_TX, SERIAL_RX); 
AnalogIn    PRESSURE_SENSOR(PA_0);
LSM6DS3     imu(PB_9, PB_8, LSM6DS3_ADDRESS);
IMUfilter   imuFilter(0.1, 10);
Ticker      ScreenTicker; 

//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");
}

void screenUpdate(){
    pc.printf("Accel (m/s^2):\r\n");
    pc.printf("AX: %2f\r\n", ACCELERATIONS[0]);
    pc.printf("AY: %2f\r\n", ACCELERATIONS[1]);
    pc.printf("AZ: %2f\r\n", ACCELERATIONS[2]);

    pc.printf("Gyro (degrees/s):\r\n");
    pc.printf("GX: %f\r\n", DEGREES[0]);
    pc.printf("GY: %f\r\n", DEGREES[1]);
    pc.printf("GZ: %2f\r\n", DEGREES[2]);
    
    pc.printf("Angle (degrees):\r\n");
    pc.printf("X: %f\r\n", ANGLES[0]);
    pc.printf("Y: %f\r\n", ANGLES[1]);
    pc.printf("Z: %f\r\n\r\n", ANGLES[2]); 
}
 
int main()
{
    bool started = false;
    while (true){
        
        if(PRESSURE_SENSOR > 0.9f){
            
            if (not started){
                setup();  //Setup sensor and Serial
                pc.printf("\r\n------ LSM6DS3 Demo -----------\r\n");
                started=true;
                ScreenTicker.attach(&screenUpdate, 1);
            }
            
            wait_ms(REFRESH_TIME_MS);     
            imu.readAccel();
            imu.readGyro();
            
            
            DEGREES[0] = imu.gx;
            DEGREES[1] = imu.gy;
            DEGREES[2] = imu.gz;
            
            // m/(s^2) = g0*g
            ACCELERATIONS[0] = g0*imu.ax;
            ACCELERATIONS[1] = g0*imu.ay;
            ACCELERATIONS[2] = g0*imu.az;
                        
            imuFilter.updateFilter(toRadians(imu.gx), toRadians(imu.gy), toRadians(imu.gz), 
                                   g0*imu.ax, g0*imu.ay, g0*imu.az);
            imuFilter.computeEuler();
            
            ANGLES[0] = toDegrees(imuFilter.getRoll());
            ANGLES[1] = toDegrees(imuFilter.getPitch());
            ANGLES[2] = toDegrees(imuFilter.getYaw());            
        }
        else {
            started=false;
            imuFilter.reset();
            ScreenTicker.detach();
        }
    }
}