File content as of revision 10:6cefe0eae1b1:

#include "mbed.h"
#include "LSM9DS0.h"
#include "rtos.h"
#include "PID.h"
#include "IMUfilter.h"

//Gravity at Earth's surface in m/s/s
#define g0 9.812865328
//Number of samples to average.
#define SAMPLES 4
//Number of samples to be averaged for a null bias calculation
//during calibration.
#define CALIBRATION_SAMPLES 1024
//Convert from radians to degrees.
#define toDegrees(x) (x * 57.2957795)
//Convert from degrees to radians.
#define toRadians(x) (x * 0.01745329252)
//LSM9DS0 gyroscope sensitivity is 8.75 (millidegrees/sec)/digit when set to +-2G
#define GYROSCOPE_GAIN (0.007476806640625)
//Full scale resolution on the accelerometer is 0.0001220703125g/LSB.
#define ACCELEROMETER_GAIN (0.00006103515625 * g0)
//Sampling gyroscope at 100Hz.
#define GYRO_RATE   0.01
//Sampling accelerometer at 200Hz.
#define ACC_RATE    0.005
//Updating filter at 40Hz.
#define FILTER_RATE 0.1
// SDO_XM and SDO_G are both grounded, so our addresses are:
#define LSM9DS0_XM  0x1E // Would be 0x1E if SDO_XM is LOW
#define LSM9DS0_G   0x6A // Would be 0x6A if SDO_G is LOW
 
Serial pc(USBTX, USBRX);
//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);
LSM9DS0 imu(p28, p27, LSM9DS0_G, LSM9DS0_XM);
DigitalIn DReady(p23);
Ticker accelerometerTicker;
Ticker gyroscopeTicker;
Ticker filterTicker;
Mutex pcMutex;
DigitalOut led1(LED1);
DigitalOut led2(LED2);
//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.
double w_xBias;
double w_yBias;
double w_zBias;
 
//Offsets for the accelerometer.
//Same as with the gyroscope.
double a_xBias;
double a_yBias;
double a_zBias;
 
//Accumulators used for oversampling and then averaging.
double a_xAccumulator = 0;
double a_yAccumulator = 0;
double a_zAccumulator = 0;
double w_xAccumulator = 0;
double w_yAccumulator = 0;
double w_zAccumulator = 0;
 
//Accelerometer and gyroscope readings for x, y, z axes.
double a_x;
double a_y;
double a_z;
double w_x;
double w_y;
double w_z;

//Previous Acceleromerter and gyroscope readings for integration
double last_a_x;
double last_a_y;
double last_a_z;
 
//Buffer for accelerometer readings.
int readings[3];
//Number of accelerometer samples we're on.
int accelerometerSamples = 0;
//Number of gyroscope samples we're on.
int gyroscopeSamples = 0;

//current readings of linear velocity
double v_x = 0;
double v_y = 0;
double v_z = 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);
 

 
void sampleAccelerometer(void const *args) {
 while(1) {
    //Have we taken enough samples?
    if (accelerometerSamples == SAMPLES) {
 
        //Average the samples, remove the bias, and calculate the acceleration
        //in m/s/s.
        a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
        a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
        a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
 
        a_xAccumulator = 0;
        a_yAccumulator = 0;
        a_zAccumulator = 0;
        accelerometerSamples = 0;
        pcMutex.lock();
        pc.printf("a_x: %f  a_y: %f  a_z: %f \n",a_x,a_y,a_z);
        pcMutex.unlock();
        
        // integrate to get velocity. make sure to subtract gravity downwards!
        
        v_x = v_x + (a_x + last_a_x)/2 * ACC_RATE * SAMPLES;
        v_y = v_y + (a_y + last_a_y)/2 * ACC_RATE * SAMPLES;
        v_z = v_z + (a_z + last_a_z)/2 * ACC_RATE * SAMPLES;
        
        last_a_x = a_x;
        last_a_y = a_y;
        last_a_z = a_z;
        pcMutex.lock();
        pc.printf("v_x: %f  v_y: %f  v_z: %f \n",v_x,v_y,v_z);
        pcMutex.unlock();
 
    } else {
        //Take another sample.
        imu.readAccel();
        pcMutex.lock();
        pc.printf("A: ");
        pc.printf("%d", imu.ax);
        pc.printf(", ");
        pc.printf("%d",imu.ay);
        pc.printf(", ");
        pc.printf("%d\n",imu.az);
        pcMutex.unlock();
        
        a_xAccumulator += imu.ax;
        a_yAccumulator += imu.ay;
        a_zAccumulator += imu.az;
 
        accelerometerSamples++;
        Thread::wait(ACC_RATE * 1000);
    }


}
}
 
void calibrateAccelerometer(void) {

    led1 = 1;
    
    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 += (double) imu.ax;
        a_yAccumulator += (double) imu.ay;
        a_zAccumulator += (double) imu.az;

        wait(ACC_RATE);
 
    }
 
    a_xAccumulator /= CALIBRATION_SAMPLES;
    a_yAccumulator /= CALIBRATION_SAMPLES;
    a_zAccumulator /= CALIBRATION_SAMPLES;
 
    //At 4mg/LSB, 250 LSBs is 1g.
    a_xBias = a_xAccumulator;
    a_yBias = a_yAccumulator;
    //a_zBias = (a_zAccumulator - (1/0.00006103515625));
    a_zBias = a_zAccumulator; // calibrate so that gravity is already out of the equation
    
    pcMutex.lock();
    pc.printf("A_Bias: ");
    pc.printf("%f", a_xBias);
    pc.printf(", ");
    pc.printf("%f",a_yBias);
    pc.printf(", ");
    pc.printf("%f\n",a_zBias);
    pcMutex.unlock();
    
    a_xAccumulator = 0;
    a_yAccumulator = 0;
    a_zAccumulator = 0;
    pc.printf("done calibrating accel\n");
    led1 = 0;
}
 
 
void calibrateGyroscope(void) {
    led2 = 1;
    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 += (double) imu.gx;
        w_yAccumulator += (double) imu.gy;
        w_zAccumulator += (double) imu.gz;
        Thread::wait(GYRO_RATE * 1000);
 
    }
 
    //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;
     
    pcMutex.lock();
    pc.printf("G_Bias: ");
    pc.printf("%f", w_xBias);
    pc.printf(", ");
    pc.printf("%f",w_yBias);
    pc.printf(", ");
    pc.printf("%f\n",w_zBias);
    pcMutex.unlock();
    
    w_xAccumulator = 0;
    w_yAccumulator = 0;
    w_zAccumulator = 0;
    pc.printf("done calibrating gyro\n");
    led2 = 0;
}
 
void sampleGyroscope(void const *args) {
 while(1){
    //Have we taken enough samples?
    if (gyroscopeSamples == SAMPLES) {
 
        //Average the samples, remove the bias, and calculate the angular
        //velocity in deg/s.
        w_x = ((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN;
        w_y = ((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN;
        w_z = ((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN;
        pcMutex.lock();
        pc.printf("w_x: %f  w_y: %f  w_z: %f \n",w_x,w_y,w_z);
        pcMutex.unlock();
        w_xAccumulator = 0;
        w_yAccumulator = 0;
        w_zAccumulator = 0;
        gyroscopeSamples = 0;
 
    } else {
        imu.readGyro();
        pcMutex.lock();
        pc.printf("G: ");
        pc.printf("%d", imu.gx);
        pc.printf(", ");
        pc.printf("%d",imu.gy);
        pc.printf(", ");
        pc.printf("%d\n",imu.gz);
        pcMutex.unlock();
        
        w_xAccumulator += imu.gx;
        w_yAccumulator += imu.gy;
        w_zAccumulator += imu.gz;
        
        gyroscopeSamples++;
        Thread::wait(GYRO_RATE * 1000);
    }

Thread::wait(GYRO_RATE * 1000);
}
}
 
void filter(void const *args) {
 while(1){
    //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();
    Thread::wait(FILTER_RATE);
    }
}
 
 
 void setup()
{
    pc.baud(9600); // Start serial at 115200 bps
    // Use the begin() function to initialize the LSM9DS0 library.
    // You can either call it with no parameters (the easy way):
    //uint16_t status = dof.begin();
    
    //Or call it with declarations for sensor scales and data rates:
    //uint16_t status = imu.begin(dof.G_SCALE_2000DPS,
    //                            dof.A_SCALE_2G, dof.M_SCALE_2GS);
                                
    uint16_t status = imu.begin(imu.G_SCALE_245DPS, imu.A_SCALE_2G, imu.M_SCALE_2GS,
                                imu.G_ODR_760_BW_100, imu.A_ODR_400, imu.M_ODR_50);
    
    // begin() returns a 16-bit value which includes both the gyro
    // and accelerometers WHO_AM_I response. You can check this to
    // make sure communication was successful.
    pc.printf("LSM9DS0 WHO_AM_I's returned: 0x");
    pc.printf("%x\n",status);
    pc.printf("Should be 0x49D4\n");
    pc.printf("\n");
}


int main() {
 
    pc.printf("Starting IMU filter test...\n");
    setup();
    
    
    //Initialize inertial sensors.
    calibrateAccelerometer();
    calibrateGyroscope();

    Thread accelThread(sampleAccelerometer);
    Thread gyroThread(sampleGyroscope);
    //Thread filterThread(filter);
    /*
    pcMutex.lock();
   pc.printf("done attaching tickers\n\n");
   pcMutex.unlock();
*/
    while (1) {
        //pc.printf("in loop\n");
        Thread::wait(2000);
 /*
        pcMutex.lock();
        pc.printf("%f,%f,%f\n",toDegrees(imuFilter.getRoll()),
                  toDegrees(imuFilter.getPitch()),
                  toDegrees(imuFilter.getYaw()));
        pcMutex.unlock();
*/
    }

}