File content as of revision 0:0309bb86b703:

#include "IMU.h"
#include "mbed.h"

IMU::IMU(PinName XGyro, PinName YAccel, PinName ZAccel, PinName AZ):
        _XGyro(XGyro), _YAccel(YAccel), _ZAccel(ZAccel), _AZ(AZ) {
    _AZ = 0;
}

void IMU::initialise(void) {
    _AZ = 1;
    wait_us(1500);
    _AZ = 0;
    wait(0.01); //requires 7ms to calibrate

    samples = 100;
    for (int i=0; i<100; i++)    reading[i]=_XGyro.read();
    Median();

    Gyro_offset = median;
    t.start();
}

//****************************************************************************/
// Median Filter
//****************************************************************************/
void IMU::Median(void)  {
    //sort the value of the array
    for (int i=0; i< samples-1; i++) {
        for (int j=i+1; j< samples; j++) {
            if (reading[i] > reading[j]) {

                w = reading[i];
                reading[i] = reading[j];
                reading[j] = w;
            }
        }

    }

    //find the middle value of the sample
    middle = (samples+1)/2;
    median = reading[middle];
}

void IMU::update(void) {

    Xrate = -(_XGyro.read() -Gyro_offset)*3.3/0.002; //Voltage - Gyro_offset / Sensitivity
    GyroAngleX += Xrate*t.read();

    //Accelerometer Y
    samples = 60;
    for (int i=0; i<samples; i++)    reading[i]=_YAccel.read();
    Median(); //100 point Median Filter

    Y = (median*3.3 - ZERO_G)/0.33; //Voltage - ZERO G bias (3.3/2) / Sensitivity ( + 0.03711048?)

    //Accelerometer Z

    samples = 60;
    for (int i=0; i<samples; i++)    reading[i]=_ZAccel.read();
    Median(); //100 point Median Filter

    Z = (median*3.3 - ZERO_G)/0.33; //Voltage - ZERO G bias (3.3/2) / Sensitivity

    //Calculate Angle + map range
    AccAngleX = Rad2Deg* atan2(-Y,-Z) - 90; //arctan = O/A, radian to degree

    if (AccAngleX<-180)  AccAngleX=AccAngleX+360; //Maintain -180 to 180

    //Complementary Filter
    Roll = (0.98)*(Roll + Xrate*t.read()) + (0.02)*(AccAngleX);
    t.reset();
}


float IMU::getRoll(void) {

    return Roll;
}

float IMU::getGyrox(void) {

    return GyroAngleX;
}

float IMU::getAccelx(void) {

    return AccAngleX;
}