File content as of revision 3:78cf56b8eb21:

#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)
//Number of samples to average.
#define SAMPLES 4
//Number of samples to be averaged for a null bias calculation
//during calibration.
#define CALIBRATION_SAMPLES 128
//Sampling gyroscope at 200Hz.
#define GYRO_RATE   0.005
//Sampling accelerometer at 200Hz.
#define ACC_RATE    0.005
//Updating filter at 40Hz.
#define FILTER_RATE 0.1
const int   LSM6DS3_ADDRESS = 0xD4;
//const int   LSM6DS3_ADDRESS = 0x69;

Serial      pc(SERIAL_TX, SERIAL_RX); 
AnalogIn    PRESSURE_SENSOR(PA_0);
LSM6DS3     imu(PB_9, PB_8, LSM6DS3_ADDRESS);
//At rest the gyroscope is centred around 0 and goes between about
//-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly
//5/15 = 0.3 degrees/sec.
IMUfilter   imuFilter(FILTER_RATE, 0.3);
Ticker      filterTicker;
Ticker      ScreenTicker; 

//Offsets for the gyroscope.
//The readings we take when the gyroscope is stationary won't be 0, so we'll
//average a set of readings we do get when the gyroscope is stationary and
//take those away from subsequent readings to ensure the gyroscope is offset
//or "biased" to 0.
float w_xBias;
float w_yBias;
float w_zBias;
 
//Offsets for the accelerometer.
//Same as with the gyroscope.
float a_xBias;
float a_yBias;
float a_zBias;
 
//Accumulators used for oversampling and then averaging.
volatile float a_xAccumulator = 0;
volatile float a_yAccumulator = 0;
volatile float a_zAccumulator = 0;
volatile float w_xAccumulator = 0;
volatile float w_yAccumulator = 0;
volatile float w_zAccumulator = 0;
 
//Accelerometer and gyroscope readings for x, y, z axes.
volatile float a_x;
volatile float a_y;
volatile float a_z;
volatile float w_x;
volatile float w_y;
volatile float w_z;
 
//Buffer for accelerometer readings.
float readings[3];
//Number of accelerometer samples we're on.
int accelerometerSamples = 0;
//Number of gyroscope samples we're on.
int gyroscopeSamples = 0;
 
/**
 * Prototypes
 */
//Calculate the null bias.
void calibrateAccelerometer(void);
//Take a set of samples and average them.
void sampleAccelerometer(void);
//Calculate the null bias.
void calibrateGyroscope(void);
//Take a set of samples and average them.
void sampleGyroscope(void);
//Update the filter and calculate the Euler angles.
void filter(void);

//Init Serial port and LSM6DS3 chip
void setup_LSM6DS3()
{
    // 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("%f,%f,%f\r\n",toDegrees(imuFilter.getRoll()),
          toDegrees(imuFilter.getPitch()),
          toDegrees(imuFilter.getYaw()));
}

void calibrateAccelerometer(void) {
 
    a_xAccumulator = 0;
    a_yAccumulator = 0;
    a_zAccumulator = 0;
    
    //Take a number of readings and average them
    //to calculate the zero g offset.
    for (int i = 0; i < CALIBRATION_SAMPLES; i++) {

        imu.readAccel();
        a_xAccumulator += imu.ax;
        a_yAccumulator += imu.ay;
        a_zAccumulator += imu.az;  
 
        wait(ACC_RATE);
    }
 
    a_xAccumulator /= CALIBRATION_SAMPLES;
    a_yAccumulator /= CALIBRATION_SAMPLES;
    a_zAccumulator /= CALIBRATION_SAMPLES;
 
    a_xBias = a_xAccumulator;
    a_yBias = a_yAccumulator;
    // When laying on the table, default a_z is 1g
    a_zBias = (a_zAccumulator - 1.0f);
 
    a_xAccumulator = 0;
    a_yAccumulator = 0;
    a_zAccumulator = 0;
}

void sampleAccelerometer(void) {
 
    //Have we taken enough samples?
    if (accelerometerSamples == SAMPLES) {
 
        //Average the samples, remove the bias and convert to m/s/s.
        a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * g0;
        a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * g0;
        a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * g0;
 
        a_xAccumulator = 0;
        a_yAccumulator = 0;
        a_zAccumulator = 0;
        accelerometerSamples = 0;
 
    } else {
        //Take another sample.
        imu.readAccel();
        a_xAccumulator += imu.ax;
        a_yAccumulator += imu.ay;
        a_zAccumulator += imu.az;        
 
        accelerometerSamples++;
    }
}

void calibrateGyroscope(void) {
 
    w_xAccumulator = 0;
    w_yAccumulator = 0;
    w_zAccumulator = 0;
 
    //Take a number of readings and average them
    //to calculate the gyroscope bias offset.
    for (int i = 0; i < CALIBRATION_SAMPLES; i++) {

        imu.readGyro();
        w_xAccumulator += imu.gx;
        w_yAccumulator += imu.gy;
        w_zAccumulator += imu.gz;
        
        wait(GYRO_RATE);
    }
 
    //Average the samples.
    w_xAccumulator /= CALIBRATION_SAMPLES;
    w_yAccumulator /= CALIBRATION_SAMPLES;
    w_zAccumulator /= CALIBRATION_SAMPLES;
 
    w_xBias = w_xAccumulator;
    w_yBias = w_yAccumulator;
    w_zBias = w_zAccumulator;
 
    w_xAccumulator = 0;
    w_yAccumulator = 0;
    w_zAccumulator = 0;
}
 
void sampleGyroscope(void) {
 
    //Have we taken enough samples?
    if (gyroscopeSamples == SAMPLES) {
 
        //Average the samples, remove the bias, and calculate the angular
        //velocity in rad/s.
        w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias));
        w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias));
        w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias));
 
        w_xAccumulator = 0;
        w_yAccumulator = 0;
        w_zAccumulator = 0;
        gyroscopeSamples = 0;
 
    } else {
        //Take another sample.
        imu.readGyro();
        w_xAccumulator += imu.gx;
        w_yAccumulator += imu.gy;
        w_zAccumulator += imu.gz;
 
        gyroscopeSamples++;
    }
}
 
void filter(void) {
 
    //Update the filter variables.
    imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
    //Calculate the new Euler angles.
    imuFilter.computeEuler();
 
}
 
int main()
{
    bool started = false;
    while (true){
        
        if(PRESSURE_SENSOR > 0.9f){
            
            if (not started){
                setup_LSM6DS3();  //Setup sensor and Serial
                pc.printf("\r\n------ LSM6DS3 Demo -----------\r\n");
                started=true;
                
                calibrateAccelerometer();
                calibrateGyroscope();
                pc.printf("\r\n------ Calibrated -----------\r\n");
                
                //Set up timers.
                filterTicker.attach(&filter, FILTER_RATE);
                ScreenTicker.attach(&screenUpdate, FILTER_RATE);
            }
            
            wait(0.005);
            sampleAccelerometer();
            sampleGyroscope();
               
        }
        else {
            started=false;
            imuFilter.reset();
            filterTicker.detach();
            ScreenTicker.detach();
        }
    }
}