Revision 1:ffef6386027b, committed 2010-08-26

Comitter:

aberk

Date:

Thu Aug 26 14:41:08 2010 +0000

Parent:

0:8d15dc761944

Commit message:

Added additional comments and documentation.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU.cpp	Thu Aug 26 14:41:08 2010 +0000
@@ -0,0 +1,336 @@
+/**
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * IMU consisting of ADXL345 accelerometer and ITG-3200 gyroscope using
+ * orientation filter developed by Sebastian Madgwick.
+ *
+ * Find more details about his paper here:
+ *
+ * http://code.google.com/p/imumargalgorithm30042010sohm/
+ */
+
+
+/**
+ * Includes
+ */
+#include "IMU.h"
+
+IMU::IMU(float imuRate,
+         double gyroscopeMeasurementError,
+         float accelerometerRate,
+         float gyroscopeRate) : accelerometer(p5, p6, p7, p8),
+        gyroscope(p9, p10), imuFilter(imuRate, gyroscopeMeasurementError) {
+
+    imuRate_ = imuRate;
+    accelerometerRate_ = accelerometerRate;
+    gyroscopeRate_ = gyroscopeRate;
+
+    //Initialize sampling variables.
+    a_xAccumulator = 0;
+    a_yAccumulator = 0;
+    a_zAccumulator = 0;
+    w_xAccumulator = 0;
+    w_yAccumulator = 0;
+    w_zAccumulator = 0;
+
+    accelerometerSamples = 0;
+    gyroscopeSamples = 0;
+
+    //Initialize and calibrate sensors.
+    initializeAccelerometer();
+    calibrateAccelerometer();
+
+    initializeGyroscope();
+    calibrateGyroscope();
+
+    //To reduce the number of interrupts we'll remove the separate tickers for
+    //the accelerometer, gyro and filter update and combine them all into one.
+
+    //accelerometerTicker.attach(this, &IMU::sampleAccelerometer, accelerometerRate_);
+    //gyroscopeTicker.attach(this, &IMU::sampleGyroscope, gyroscopeRate_);
+    sampleTicker.attach(this, &IMU::sample, accelerometerRate_);
+    //filterTicker.attach(this, &IMU::filter, imuRate_);
+
+}
+
+double IMU::getRoll(void) {
+
+    return toDegrees(imuFilter.getRoll());
+
+}
+
+double IMU::getPitch(void) {
+
+    return toDegrees(imuFilter.getPitch());
+
+}
+
+double IMU::getYaw(void) {
+
+    return toDegrees(imuFilter.getYaw());
+
+}
+
+void IMU::initializeAccelerometer(void) {
+
+    //Go into standby mode to configure the device.
+    accelerometer.setPowerControl(0x00);
+    //Full resolution, +/-16g, 4mg/LSB.
+    accelerometer.setDataFormatControl(0x0B);
+    //200Hz data rate.
+    accelerometer.setDataRate(ADXL345_200HZ);
+    //Measurement mode.
+    accelerometer.setPowerControl(0x08);
+    //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
+    wait_ms(22);
+
+}
+
+void IMU::sampleAccelerometer(void) {
+
+    //If we've taken a certain number of samples,
+    //average them, remove the bias and convert the units.
+    if (accelerometerSamples == SAMPLES) {
+
+        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;
+
+    }
+    //Otherwise, accumulate another reading. 
+    else {
+
+        accelerometer.getOutput(readings);
+
+        a_xAccumulator += (int16_t) readings[0];
+        a_yAccumulator += (int16_t) readings[1];
+        a_zAccumulator += (int16_t) readings[2];
+
+        accelerometerSamples++;
+
+    }
+
+}
+
+void IMU::calibrateAccelerometer(void) {
+
+    a_xAccumulator = 0;
+    a_yAccumulator = 0;
+    a_zAccumulator = 0;
+
+    //Accumulate a certain number of samples.
+    for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
+
+        accelerometer.getOutput(readings);
+
+        a_xAccumulator += (int16_t) readings[0];
+        a_yAccumulator += (int16_t) readings[1];
+        a_zAccumulator += (int16_t) readings[2];
+
+        wait(accelerometerRate_);
+
+    }
+
+    //Average the samples.
+    a_xAccumulator /= CALIBRATION_SAMPLES;
+    a_yAccumulator /= CALIBRATION_SAMPLES;
+    a_zAccumulator /= CALIBRATION_SAMPLES;
+
+    //These are our zero g offsets.
+    //250 = 9.81m/s/s @ 4mg/LSB.
+    a_xBias = a_xAccumulator;
+    a_yBias = a_yAccumulator;
+    a_zBias = (a_zAccumulator - 250);
+
+    //Reset accumulators.
+    a_xAccumulator = 0;
+    a_yAccumulator = 0;
+    a_zAccumulator = 0;
+
+}
+
+void IMU::initializeGyroscope(void) {
+
+    //Low pass filter bandwidth of 42Hz.
+    gyroscope.setLpBandwidth(LPFBW_42HZ);
+    //Internal sample rate of 200Hz.
+    gyroscope.setSampleRateDivider(4);
+
+}
+
+void IMU::calibrateGyroscope(void) {
+
+    w_xAccumulator = 0;
+    w_yAccumulator = 0;
+    w_zAccumulator = 0;
+
+    //Accumulate a certain number of samples.
+    for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
+
+        w_xAccumulator += gyroscope.getGyroX();
+        w_yAccumulator += gyroscope.getGyroY();
+        w_zAccumulator += gyroscope.getGyroZ();
+        wait(gyroscopeRate_);
+
+    }
+
+    //Average the samples.
+    w_xAccumulator /= CALIBRATION_SAMPLES;
+    w_yAccumulator /= CALIBRATION_SAMPLES;
+    w_zAccumulator /= CALIBRATION_SAMPLES;
+
+    //Set the null bias.
+    w_xBias = w_xAccumulator;
+    w_yBias = w_yAccumulator;
+    w_zBias = w_zAccumulator;
+
+    //Reset the accumulators.
+    w_xAccumulator = 0;
+    w_yAccumulator = 0;
+    w_zAccumulator = 0;
+
+}
+
+void IMU::sampleGyroscope(void) {
+
+    //If we've taken the required number of samples then,
+    //average the samples, removed the null bias and convert the units
+    //to rad/s.
+    if (gyroscopeSamples == SAMPLES) {
+
+        w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
+        w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
+        w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
+
+        w_xAccumulator = 0;
+        w_yAccumulator = 0;
+        w_zAccumulator = 0;
+        gyroscopeSamples = 0;
+
+    }
+    //Accumulate another sample. 
+    else {
+
+        w_xAccumulator += gyroscope.getGyroX();
+        w_yAccumulator += gyroscope.getGyroY();
+        w_zAccumulator += gyroscope.getGyroZ();
+
+        gyroscopeSamples++;
+
+    }
+
+}
+
+void IMU::sample(void) {
+
+    //If we've taken enough samples then,
+    //average the samples, remove the offsets and convert to appropriate units.
+    //Feed this information into the filter to calculate the new Euler angles.
+    if (accelerometerSamples == SAMPLES) {
+
+        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;
+
+        w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
+        w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
+        w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
+
+        w_xAccumulator = 0;
+        w_yAccumulator = 0;
+        w_zAccumulator = 0;
+        gyroscopeSamples = 0;
+
+        //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();
+
+    }
+    //Accumulate another sample. 
+    else {
+
+        accelerometer.getOutput(readings);
+
+        a_xAccumulator += (int16_t) readings[0];
+        a_yAccumulator += (int16_t) readings[1];
+        a_zAccumulator += (int16_t) readings[2];
+
+        w_xAccumulator += gyroscope.getGyroX();
+        w_yAccumulator += gyroscope.getGyroY();
+        w_zAccumulator += gyroscope.getGyroZ();
+
+        accelerometerSamples++;
+
+    }
+
+}
+
+void IMU::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();
+
+}
+
+void IMU::reset(void) {
+
+    //Disable interrupts.
+    sampleTicker.detach();
+    
+    //Recalibrate sensors.
+    calibrateAccelerometer();
+    calibrateGyroscope();
+    
+    //Reset the IMU filter.
+    imuFilter.reset();
+    
+    //Reset the working variables.
+    a_xAccumulator = 0;
+    a_yAccumulator = 0;
+    a_zAccumulator = 0;
+    w_xAccumulator = 0;
+    w_yAccumulator = 0;
+    w_zAccumulator = 0;
+    accelerometerSamples = 0;
+    gyroscopeSamples = 0;
+    
+    //Enable interrupts.
+    sampleTicker.attach(this, &IMU::sample, accelerometerRate_);
+
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU.h	Thu Aug 26 14:41:08 2010 +0000
@@ -0,0 +1,208 @@
+/**
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * IMU consisting of ADXL345 accelerometer and ITG-3200 gyroscope using
+ * orientation filter developed by Sebastian Madgwick.
+ *
+ * Find more details about his paper here:
+ *
+ * http://code.google.com/p/imumargalgorithm30042010sohm/
+ */
+
+#ifndef MBED_IMU_H
+#define MBED_IMU_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+#include "ADXL345.h"
+#include "ITG3200.h"
+#include "IMUfilter.h"
+
+/**
+ * Defines
+ */
+#define IMU_RATE           0.025
+#define ACCELEROMETER_RATE 0.005
+#define GYROSCOPE_RATE     0.005
+#define GYRO_MEAS_ERROR    0.3 //IMUfilter tuning parameter.
+
+//Gravity at Earth's surface in m/s/s
+#define g0 9.812865328
+//Number of samples to average
+#define SAMPLES 4
+#define CALIBRATION_SAMPLES 128
+//Multiply radians to get degrees.
+#define toDegrees(x) (x * 57.2957795)
+//Multiply degrees to get radians.
+#define toRadians(x) (x * 0.01745329252)
+//Full scale resolution on the ADXL345 is 4mg/LSB.
+//Multiply ADC count readings from ADXL345 to get acceleration in m/s/s.
+#define toAcceleration(x) (x * (4 * g0 * 0.001))
+//14.375 LSB/(degrees/sec)
+#define GYROSCOPE_GAIN (1 / 14.375)
+#define ACCELEROMETER_GAIN (0.004 * g0)
+
+/**
+ * IMU consisting of ADXL345 accelerometer and ITG-3200 gyroscope to calculate
+ * roll, pitch and yaw angles.
+ */
+class IMU {
+
+public:
+
+    /**
+     * Constructor.
+     *
+     * @param imuRate Rate which IMUfilter update and Euler angle calculation
+     *                occurs.
+     * @param gyroscopeMeasurementError IMUfilter tuning parameter.
+     * @param accelerometerRate Rate at which accelerometer data is sampled.
+     * @param gyroscopeRate Rate at which gyroscope data is sampled.
+     */
+    IMU(float imuRate,
+        double gyroscopeMeasurementError,
+        float accelerometerRate,
+        float gyroscopeRate);
+
+    /**
+     * Get the current roll angle.
+     *
+     * @return The current roll angle in degrees.
+     */
+    double getRoll(void);
+
+    /**
+     * Get the current pitch angle.
+     *
+     * @return The current pitch angle in degrees.
+     */
+    double getPitch(void);
+
+    /**
+     * Get the current yaw angle.
+     *
+     * @return The current yaw angle in degrees.
+     */
+    double getYaw(void);
+
+    /**
+     * Sample the sensors, and if enough samples have been taken,
+     * take an average, and compute the new Euler angles.
+     */
+    void sample(void);
+    
+    /**
+     * Recalibrate the sensors and reset the IMU filter.
+     */
+    void reset(void);
+
+private:
+
+    /**
+     * Set up the ADXL345 appropriately.
+     */
+    void initializeAccelerometer(void);
+
+    /**
+     * Calculate the zero g offset.
+     */
+    void calibrateAccelerometer(void);
+
+    /**
+     * Take a set of samples and average them.
+     */
+    void sampleAccelerometer(void);
+
+    /**
+     * Set up the ITG-3200 appropriately.
+     */
+    void initializeGyroscope(void);
+
+    /**
+     * Calculate the bias offset.
+     */
+    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);
+
+    ADXL345   accelerometer;
+    ITG3200   gyroscope;
+    IMUfilter imuFilter;
+
+    Ticker accelerometerTicker;
+    Ticker gyroscopeTicker;
+    Ticker sampleTicker;
+    Ticker filterTicker;
+
+    float accelerometerRate_;
+    float gyroscopeRate_;
+    float imuRate_;
+
+    //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;
+
+    double a_xBias;
+    double a_yBias;
+    double a_zBias;
+
+    volatile double a_xAccumulator;
+    volatile double a_yAccumulator;
+    volatile double a_zAccumulator;
+    volatile double w_xAccumulator;
+    volatile double w_yAccumulator;
+    volatile double w_zAccumulator;
+
+    //Accelerometer and gyroscope readings for x, y, z axes.
+    volatile double a_x;
+    volatile double a_y;
+    volatile double a_z;
+    volatile double w_x;
+    volatile double w_y;
+    volatile double w_z;
+
+    //Buffer for accelerometer readings.
+    int readings[3];
+    int accelerometerSamples;
+    int gyroscopeSamples;
+
+};
+
+#endif /* MBED_IMU_H */

--- a/IMU.lib	Mon Aug 16 09:46:28 2010 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/aberk/code/IMU/#51d01aa47c71

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMUfilter.cpp	Thu Aug 26 14:41:08 2010 +0000
@@ -0,0 +1,226 @@
+/**
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * IMU orientation filter developed by Sebastian Madgwick.
+ *
+ * Find more details about his paper here:
+ *
+ * http://code.google.com/p/imumargalgorithm30042010sohm/
+ */
+
+/**
+ * Includes
+ */
+#include "IMUfilter.h"
+
+IMUfilter::IMUfilter(double rate, double gyroscopeMeasurementError) {
+
+    firstUpdate = 0;
+
+    //Quaternion orientation of earth frame relative to auxiliary frame.
+    AEq_1 = 1;
+    AEq_2 = 0;
+    AEq_3 = 0;
+    AEq_4 = 0;
+
+    //Estimated orientation quaternion elements with initial conditions.
+    SEq_1 = 1;
+    SEq_2 = 0;
+    SEq_3 = 0;
+    SEq_4 = 0;
+
+    //Sampling period (typical value is ~0.1s).
+    deltat = rate;
+
+    //Gyroscope measurement error (in degrees per second).
+    gyroMeasError = gyroscopeMeasurementError;
+
+    //Compute beta.
+    beta = sqrt(3.0 / 4.0) * (PI * (gyroMeasError / 180.0));
+
+}
+
+void IMUfilter::updateFilter(double w_x, double w_y, double w_z, double a_x, double a_y, double a_z) {
+
+    //Local system variables.
+
+    //Vector norm.
+    double norm;
+    //Quaternion rate from gyroscope elements.
+    double SEqDot_omega_1;
+    double SEqDot_omega_2;
+    double SEqDot_omega_3;
+    double SEqDot_omega_4;
+    //Objective function elements.
+    double f_1;
+    double f_2;
+    double f_3;
+    //Objective function Jacobian elements.
+    double J_11or24;
+    double J_12or23;
+    double J_13or22;
+    double J_14or21;
+    double J_32;
+    double J_33;
+    //Objective function gradient elements.
+    double nablaf_1;
+    double nablaf_2;
+    double nablaf_3;
+    double nablaf_4;
+
+    //Auxiliary variables to avoid reapeated calcualtions.
+    double halfSEq_1 = 0.5 * SEq_1;
+    double halfSEq_2 = 0.5 * SEq_2;
+    double halfSEq_3 = 0.5 * SEq_3;
+    double halfSEq_4 = 0.5 * SEq_4;
+    double twoSEq_1 = 2.0 * SEq_1;
+    double twoSEq_2 = 2.0 * SEq_2;
+    double twoSEq_3 = 2.0 * SEq_3;
+
+    //Compute the quaternion rate measured by gyroscopes.
+    SEqDot_omega_1 = -halfSEq_2 * w_x - halfSEq_3 * w_y - halfSEq_4 * w_z;
+    SEqDot_omega_2 = halfSEq_1 * w_x + halfSEq_3 * w_z - halfSEq_4 * w_y;
+    SEqDot_omega_3 = halfSEq_1 * w_y - halfSEq_2 * w_z + halfSEq_4 * w_x;
+    SEqDot_omega_4 = halfSEq_1 * w_z + halfSEq_2 * w_y - halfSEq_3 * w_x;
+
+    //Normalise the accelerometer measurement.
+    norm = sqrt(a_x * a_x + a_y * a_y + a_z * a_z);
+    a_x /= norm;
+    a_y /= norm;
+    a_z /= norm;
+
+    //Compute the objective function and Jacobian.
+    f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - a_x;
+    f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - a_y;
+    f_3 = 1.0 - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - a_z;
+    //J_11 negated in matrix multiplication.
+    J_11or24 = twoSEq_3;
+    J_12or23 = 2 * SEq_4;
+    //J_12 negated in matrix multiplication
+    J_13or22 = twoSEq_1;
+    J_14or21 = twoSEq_2;
+    //Negated in matrix multiplication.
+    J_32 = 2 * J_14or21;
+    //Negated in matrix multiplication.
+    J_33 = 2 * J_11or24;
+
+    //Compute the gradient (matrix multiplication).
+    nablaf_1 = J_14or21 * f_2 - J_11or24 * f_1;
+    nablaf_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3;
+    nablaf_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1;
+    nablaf_4 = J_14or21 * f_1 + J_11or24 * f_2;
+
+    //Normalise the gradient.
+    norm = sqrt(nablaf_1 * nablaf_1 + nablaf_2 * nablaf_2 + nablaf_3 * nablaf_3 + nablaf_4 * nablaf_4);
+    nablaf_1 /= norm;
+    nablaf_2 /= norm;
+    nablaf_3 /= norm;
+    nablaf_4 /= norm;
+
+    //Compute then integrate the estimated quaternion rate.
+    SEq_1 += (SEqDot_omega_1 - (beta * nablaf_1)) * deltat;
+    SEq_2 += (SEqDot_omega_2 - (beta * nablaf_2)) * deltat;
+    SEq_3 += (SEqDot_omega_3 - (beta * nablaf_3)) * deltat;
+    SEq_4 += (SEqDot_omega_4 - (beta * nablaf_4)) * deltat;
+
+    //Normalise quaternion
+    norm = sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
+    SEq_1 /= norm;
+    SEq_2 /= norm;
+    SEq_3 /= norm;
+    SEq_4 /= norm;
+
+    if (firstUpdate == 0) {
+        //Store orientation of auxiliary frame.
+        AEq_1 = SEq_1;
+        AEq_2 = SEq_2;
+        AEq_3 = SEq_3;
+        AEq_4 = SEq_4;
+        firstUpdate = 1;
+    }
+
+}
+
+void IMUfilter::computeEuler(void) {
+
+    //Quaternion describing orientation of sensor relative to earth.
+    double ESq_1, ESq_2, ESq_3, ESq_4;
+    //Quaternion describing orientation of sensor relative to auxiliary frame.
+    double ASq_1, ASq_2, ASq_3, ASq_4;
+
+    //Compute the quaternion conjugate.
+    ESq_1 = SEq_1;
+    ESq_2 = -SEq_2;
+    ESq_3 = -SEq_3;
+    ESq_4 = -SEq_4;
+
+    //Compute the quaternion product.
+    ASq_1 = ESq_1 * AEq_1 - ESq_2 * AEq_2 - ESq_3 * AEq_3 - ESq_4 * AEq_4;
+    ASq_2 = ESq_1 * AEq_2 + ESq_2 * AEq_1 + ESq_3 * AEq_4 - ESq_4 * AEq_3;
+    ASq_3 = ESq_1 * AEq_3 - ESq_2 * AEq_4 + ESq_3 * AEq_1 + ESq_4 * AEq_2;
+    ASq_4 = ESq_1 * AEq_4 + ESq_2 * AEq_3 - ESq_3 * AEq_2 + ESq_4 * AEq_1;
+
+    //Compute the Euler angles from the quaternion.
+    phi = atan2(2 * ASq_3 * ASq_4 - 2 * ASq_1 * ASq_2, 2 * ASq_1 * ASq_1 + 2 * ASq_4 * ASq_4 - 1);
+    theta = asin(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_3);
+    psi = atan2(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_4, 2 * ASq_1 * ASq_1 + 2 * ASq_2 * ASq_2 - 1);
+
+}
+
+double IMUfilter::getRoll(void) {
+
+    return phi;
+
+}
+
+double IMUfilter::getPitch(void) {
+
+    return theta;
+
+}
+
+double IMUfilter::getYaw(void) {
+
+    return psi;
+
+}
+
+void IMUfilter::reset(void) {
+
+    firstUpdate = 0;
+
+    //Quaternion orientation of earth frame relative to auxiliary frame.
+    AEq_1 = 1;
+    AEq_2 = 0;
+    AEq_3 = 0;
+    AEq_4 = 0;
+
+    //Estimated orientation quaternion elements with initial conditions.
+    SEq_1 = 1;
+    SEq_2 = 0;
+    SEq_3 = 0;
+    SEq_4 = 0;
+
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMUfilter.h	Thu Aug 26 14:41:08 2010 +0000
@@ -0,0 +1,141 @@
+/**
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * IMU orientation filter developed by Sebastian Madgwick.
+ *
+ * Find more details about his paper here:
+ *
+ * http://code.google.com/p/imumargalgorithm30042010sohm/
+ */
+
+#ifndef IMU_FILTER_H
+#define IMU_FILTER_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+
+/**
+ * Defines
+ */
+#define PI 3.1415926536
+
+/**
+ * IMU orientation filter.
+ */
+class IMUfilter {
+
+public:
+
+    /**
+     * Constructor.
+     *
+     * Initializes filter variables.
+     *
+     * @param rate The rate at which the filter should be updated.
+     * @param gyroscopeMeasurementError The error of the gyroscope in degrees
+     *  per second. This used to calculate a tuning constant for the filter.
+     *  Try changing this value if there are jittery readings, or they change
+     *  too much or too fast when rotating the IMU.
+     */
+    IMUfilter(double rate, double gyroscopeMeasurementError);
+
+    /**
+     * Update the filter variables.
+     *
+     * @param w_x X-axis gyroscope reading in rad/s.
+     * @param w_y Y-axis gyroscope reading in rad/s.
+     * @param w_z Z-axis gyroscope reading in rad/s.
+     * @param a_x X-axis accelerometer reading in m/s/s.
+     * @param a_y Y-axis accelerometer reading in m/s/s.
+     * @param a_z Z-axis accelerometer reading in m/s/s.
+     */
+    void updateFilter(double w_x, double w_y, double w_z,
+                      double a_x, double a_y, double a_z);
+
+    /**
+     * Compute the Euler angles based on the current filter data.
+     */
+    void computeEuler(void);
+
+    /**
+     * Get the current roll.
+     *
+     * @return The current roll angle in radians.
+     */
+    double getRoll(void);
+
+    /**
+     * Get the current pitch.
+     *
+     * @return The current pitch angle in radians.
+     */
+    double getPitch(void);
+
+    /**
+     * Get the current yaw.
+     *
+     * @return The current yaw angle in radians.
+     */
+    double getYaw(void);
+    
+    /**
+     * Reset the filter.
+     */
+    void reset(void);
+
+private:
+
+    int firstUpdate;
+
+    //Quaternion orientation of earth frame relative to auxiliary frame.
+    double AEq_1;
+    double AEq_2;
+    double AEq_3;
+    double AEq_4;
+
+    //Estimated orientation quaternion elements with initial conditions.
+    double SEq_1;
+    double SEq_2;
+    double SEq_3;
+    double SEq_4;
+
+    //Sampling period
+    double deltat;
+
+    //Gyroscope measurement error (in degrees per second).
+    double gyroMeasError;
+
+    //Compute beta (filter tuning constant..
+    double beta;
+
+    double phi;
+    double theta;
+    double psi;
+
+};
+
+#endif /* IMU_FILTER_H */

--- a/IMUfilter_lib.lib	Mon Aug 16 09:46:28 2010 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/aberk/programs/IMUfilter_lib/latest
\ No newline at end of file

--- a/Rover.cpp	Mon Aug 16 09:46:28 2010 +0000
+++ b/Rover.cpp	Thu Aug 26 14:41:08 2010 +0000
@@ -61,8 +61,6 @@
  */
 #include "Rover.h"
 
-Serial pc2(USBTX, USBRX);
-
 Rover::Rover(PinName leftMotorPwm,
              PinName leftMotorBrake,
              PinName leftMotorDirection,
@@ -91,7 +89,7 @@
         rightController(Kc, Ti, Td, PID_RATE),
         stateTicker(),
         logTicker(),
-        imu(IMU_RATE,
+        imu(IMU_RATE_,
             GYRO_MEAS_ERROR,
             ACCELEROMETER_RATE,
             GYROSCOPE_RATE) {
@@ -150,6 +148,7 @@
     degreesTurned_    = 0.0;
     leftStopFlag_     = 0;
     rightStopFlag_    = 0;
+    logIndex          = 0;
 
     //--------
     // BEGIN!
@@ -160,7 +159,14 @@
 
 }
 
-void Rover::move(int distance) {
+void Rover::move(float distance) {
+
+    //Convert from metres into millimetres.
+    distance *= 1000;
+    //Work out how many pulses are required to go that many millimetres.
+    distance *= PULSES_PER_MM;
+    //Make sure we scale the number of pulses according to our encoding method.
+    distance /= ENCODING;
 
     positionSetPoint_ = distance;
 
@@ -183,7 +189,15 @@
 
 void Rover::turn(int degrees) {
 
-    headingSetPoint_ = abs(degrees);
+    //Correct the amount to turn based on deviation during last segment.
+    headingSetPoint_ = abs(degrees) + (endHeading_ - startHeading_);
+    
+    //In case the rover tries to [pointlessly] turn >360 degrees.
+    if (headingSetPoint_ > 359.8){
+    
+        headingSetPoint_ -= 359.8;
+    
+    }
 
     //Rotating clockwise.
     if (degrees > 0) {
@@ -210,22 +224,28 @@
 
 void Rover::startLogging(void) {
 
-    logFile = fopen("/sd/roverlog.csv", "w");
-    fprintf(logFile, "leftPulses, rightPulses, leftVelocity, rightVelocity, heading, degreesTurned\n");
-    logTicker.attach(this, &Rover::log, LOG_RATE);
+    logFile = fopen("/local/roverlog.csv", "w");
+    fprintf(logFile, "leftPulses, rightPulses, leftVelocity, rightVelocity, heading\n");
+    //logTicker.attach(this, &Rover::log, LOG_RATE);
 
 }
 
 void Rover::stopLogging(void) {
 
+    //logFile = fopen("/local/roverlog.csv", "w");
+    //fprintf(logFile, "leftPulses, rightPulses, leftVelocity, rightVelocity, heading, degreesTurned\n");
+    //fprintf(logFile, "leftVelocity, rightVelocity\n");
+    //for(int i = 0; i < 1024; i++){
+    //    fprintf(logFile, "%f, %f\n", leftVelocityBuffer[i], rightVelocityBuffer[i]);
+    //}
     fclose(logFile);
 
 }
 
 void Rover::log(void) {
 
-    fprintf(logFile, "%i,%i,%f,%f,%f,%f\n",
-            leftPulses_, rightPulses_, leftVelocity_, rightVelocity_, imu.getYaw(), degreesTurned_);
+    //fprintf(logFile, "%i,%i,%f,%f,%f,%f\n",
+    //        leftPulses_, rightPulses_, leftVelocity_, rightVelocity_, imu.getYaw(), degreesTurned_);
 
 }
 
@@ -235,6 +255,7 @@
 
             //We're not moving so don't do anything!
         case (STATE_STATIONARY):
+
             break;
 
         case (STATE_MOVING_FORWARD):
@@ -247,7 +268,7 @@
                 leftVelocity_ = (leftPulses_ - leftPrevPulses_) / PID_RATE;
                 leftPrevPulses_ = leftPulses_;
                 leftController.setProcessValue(leftVelocity_);
-                leftPwmDuty_ = leftController.getRealOutput();
+                leftPwmDuty_ = leftController.compute();
 
             } else {
                 leftStopFlag_ = 1;
@@ -261,8 +282,8 @@
                 rightVelocity_ = (rightPulses_ - rightPrevPulses_) / PID_RATE;
                 rightPrevPulses_ = rightPulses_;
                 rightController.setProcessValue(rightVelocity_);
-                rightPwmDuty_ = rightController.getRealOutput();
-                
+                rightPwmDuty_ = rightController.compute();
+
             } else {
                 rightStopFlag_ = 1;
             }
@@ -275,6 +296,7 @@
                 rightPwmDuty_ = 1.0;
                 leftMotors.setPwmDuty(leftPwmDuty_);
                 rightMotors.setPwmDuty(rightPwmDuty_);
+                endHeading_ = imu.getYaw();
                 enterState(STATE_STATIONARY);
             }
 
@@ -289,7 +311,7 @@
                 leftVelocity_ = (leftPulses_ - leftPrevPulses_) / PID_RATE;
                 leftPrevPulses_ = leftPulses_;
                 leftController.setProcessValue(fabs(leftVelocity_));
-                leftPwmDuty_ = leftController.getRealOutput();
+                leftPwmDuty_ = leftController.compute();
             } else {
                 leftStopFlag_ = 1;
             }
@@ -301,7 +323,7 @@
                 rightVelocity_ = (rightPulses_ - rightPrevPulses_) / PID_RATE;
                 rightPrevPulses_ = rightPulses_;
                 rightController.setProcessValue(fabs(rightVelocity_));
-                rightPwmDuty_ = rightController.getRealOutput();
+                rightPwmDuty_ = rightController.compute();
 
             } else {
                 rightStopFlag_ = 1;
@@ -334,13 +356,13 @@
                 leftVelocity_ = (leftPulses_ - leftPrevPulses_) / PID_RATE;
                 leftPrevPulses_ = leftPulses_;
                 leftController.setProcessValue(leftVelocity_);
-                leftPwmDuty_ = leftController.getRealOutput();
+                leftPwmDuty_ = leftController.compute();
 
                 rightPulses_ = rightQei.getPulses();
                 rightVelocity_ = (rightPulses_ - rightPrevPulses_) / PID_RATE;
                 rightPrevPulses_ = rightPulses_;
                 rightController.setProcessValue(fabs(rightVelocity_));
-                rightPwmDuty_ = rightController.getRealOutput();
+                rightPwmDuty_ = rightController.compute();
 
                 leftMotors.setPwmDuty(leftPwmDuty_);
                 rightMotors.setPwmDuty(rightPwmDuty_);
@@ -371,13 +393,13 @@
                 leftVelocity_ = (leftPulses_ - leftPrevPulses_) / PID_RATE;
                 leftPrevPulses_ = leftPulses_;
                 leftController.setProcessValue(fabs(leftVelocity_));
-                leftPwmDuty_ = leftController.getRealOutput();
+                leftPwmDuty_ = leftController.compute();
 
                 rightPulses_ = rightQei.getPulses();
                 rightVelocity_ = (rightPulses_ - rightPrevPulses_) / PID_RATE;
                 rightPrevPulses_ = rightPulses_;
                 rightController.setProcessValue(rightVelocity_);
-                rightPwmDuty_ = rightController.getRealOutput();
+                rightPwmDuty_ = rightController.compute();
 
                 leftMotors.setPwmDuty(leftPwmDuty_);
                 rightMotors.setPwmDuty(rightPwmDuty_);
@@ -405,6 +427,15 @@
 
     }
 
+    if (logIndex < 1024) {
+        headingBuffer[logIndex] = imu.getYaw();
+        leftVelocityBuffer[logIndex] = leftVelocity_;
+        rightVelocityBuffer[logIndex] = rightVelocity_;
+        leftPositionBuffer[logIndex] = leftPulses_;
+        rightPositionBuffer[logIndex] = rightPulses_;
+        logIndex++;
+    }
+
 }
 
 void Rover::enterState(State state) {
@@ -449,6 +480,18 @@
             leftStopFlag_     = 0;
             rightStopFlag_    = 0;
 
+            for (int i = 0; i < logIndex; i++) {
+                fprintf(logFile, "%i, %i, %f, %f, %f\n", leftPositionBuffer[i],
+                        rightPositionBuffer[i],
+                        leftVelocityBuffer[i],
+                        rightVelocityBuffer[i],
+                        headingBuffer[i]);
+            }
+
+            logIndex = 0;
+            
+            imu.reset();
+
             state_ = STATE_STATIONARY;
 
             break;
@@ -466,6 +509,10 @@
             leftController.setSetPoint(1000);
             rightController.setSetPoint(1000);
 
+            logIndex = 0;
+            
+            startHeading_ = imu.getYaw();
+
             state_ = STATE_MOVING_FORWARD;
 
             break;
@@ -483,6 +530,8 @@
             leftController.setSetPoint(1000);
             rightController.setSetPoint(1000);
 
+            logIndex = 0;
+
             state_ = STATE_MOVING_BACKWARD;
 
             break;
@@ -504,6 +553,8 @@
             heading_ = fabs(imu.getYaw());
             prevHeading_ = heading_;
 
+            logIndex = 0;
+
             state_ = STATE_ROTATING_CLOCKWISE;
 
             break;
@@ -525,12 +576,16 @@
             heading_ = fabs(imu.getYaw());
             prevHeading_ = heading_;
 
+            logIndex = 0;
+
             state_ = STATE_ROTATING_COUNTER_CLOCKWISE;
 
             break;
 
         default:
 
+            state_ = STATE_STATIONARY;
+
             break;
 
     }

--- a/Rover.h	Mon Aug 16 09:46:28 2010 +0000
+++ b/Rover.h	Thu Aug 26 14:41:08 2010 +0000
@@ -39,7 +39,7 @@
  * ---------------
  *
  * The set up assumes the H-bridge being used has pins for:
- * 
+ *
  * - PWM input
  * - Brake
  * - Direction
@@ -68,6 +68,7 @@
 #include "PID.h"
 #include "IMU.h"
 #include "SDFileSystem.h"
+#include "HMC6352.h"
 
 /**
  * Defines
@@ -81,6 +82,9 @@
 #define REVS_PER_ROTATION     (ROTATION_DISTANCE / WHEEL_DIAMETER)
 #define PULSES_PER_ROTATION   (REVS_PER_ROTATION * PULSES_PER_REV)
 #define PULSES_PER_MM         (PULSES_PER_REV / WHEEL_DIAMETER)
+#define DISTANCE_PER_PULSE    (WHEEL_DIAMETER / PULSES_PER_REV)
+#define ENCODING              2 //Use X2 encoding
+#define WHEEL_DISTANCE        (ROTATION_DISTANCE / DISTANCE_PER_PULSE)
 //-----
 // PID
 //-----
@@ -96,13 +100,13 @@
 //---------
 // Logging
 //---------
-#define LOG_RATE     0.01
+#define LOG_RATE     0.025
 //-----
 // IMU
 //-----
 #define ACCELEROMETER_RATE 0.005
 #define GYROSCOPE_RATE     0.005
-#define IMU_RATE           0.025
+#define IMU_RATE_          0.025
 #define GYRO_MEAS_ERROR    0.3
 
 class Rover {
@@ -175,12 +179,17 @@
      * +ve distance -> forward
      * -ve distance -> backward
      *
-     * @param distance The distance to move.
+     * @param distance The distance to move in metres.
      */
-    void move(int distance);
+    void move(float distance);
 
     /**
      * Turn the rover left or right a certain number of degrees.
+     *
+     * +ve degrees -> clockwise
+     * -ve degrees -> counter-clockwise
+     *
+     * @param degrees The number of degrees to rotate.
      */
     void turn(int degrees);
 
@@ -246,12 +255,12 @@
     IMU           imu;
 
     FILE*          logFile;
-    
+
     volatile int   leftStopFlag_;
     volatile int   rightStopFlag_;
 
     volatile int   positionSetPoint_;
-    volatile int   headingSetPoint_;
+    volatile float headingSetPoint_;
     volatile float degreesTurned_;
 
     volatile int   leftPulses_;
@@ -269,6 +278,16 @@
 
     volatile State state_;
 
+    volatile float headingBuffer[1024];
+    volatile int   leftPositionBuffer[1024];
+    volatile int   rightPositionBuffer[1024];
+    volatile float leftVelocityBuffer[1024];
+    volatile float rightVelocityBuffer[1024];
+    volatile int   logIndex;
+    
+    volatile float startHeading_;
+    volatile float endHeading_;
+    
 };
 
 #endif /* ROVER_H */

--- a/main.cpp	Mon Aug 16 09:46:28 2010 +0000
+++ b/main.cpp	Thu Aug 26 14:41:08 2010 +0000
@@ -37,6 +37,7 @@
 
 //Debugging.
 Serial pc(USBTX, USBRX);
+DigitalOut led1(LED1);
 
 //Globals.
 LocalFileSystem local("local");
@@ -55,34 +56,44 @@
     //-----------------------
     // Simple command parser
     //-----------------------
-    char cmd0[16]; //{"move", "turn"}
-    char cmd1[16]; //{{"forward", "backward"}, {"left", "right"}}
-    int  cmd2 = 0; //{distance, angle}
+    char  cmd0[16]; //{"move", "turn"}
+    char  cmd1[16]; //{{"forward", "backward"}, {"left", "right"}}
+    float cmd2 = 0; //{distance in METRES, angle in DEGREES}
 
     pc.printf("Starting...\n");
 
+    //Wait a bit before we start moving.
     wait(3);
 
+    //Open the command script.
     FILE *fp = fopen("/local/commands.txt", "r");
 
+    //Check if we were successful in opening the command script.
     if (fp == NULL) {
         pc.printf("Opening file failed...\n");
     } else {
         pc.printf("Opening file succeeded!\n");
     }
 
-    while (fscanf(fp, "%s%s%d", cmd0, cmd1, &cmd2) >= 0) {
+    //While there's another line to read from the command script.
+    while (fscanf(fp, "%s%s%f", cmd0, cmd1, &cmd2) >= 0) {
+    
+        led1 = 1;
+    
+        pc.printf("Start of new command\n");
 
         wait(1);
 
-        pc.printf("%s %s %d\n", cmd0, cmd1, cmd2);
+        pc.printf("%s %s %f\n", cmd0, cmd1, cmd2);
 
         //move command.
         if (strcmp(cmd0, "move") == 0) {
             if (strcmp(cmd1, "forward") == 0) {
                 myRover.move(cmd2);
                 while (myRover.getState() != Rover::STATE_STATIONARY) {
+                
                 }
+                pc.printf("Finished!\n");
             } else if (strcmp(cmd1, "backward") == 0) {
                 myRover.move(-cmd2);
                 while (myRover.getState() != Rover::STATE_STATIONARY) {
@@ -101,6 +112,9 @@
                 }
             }
         }
+        
+        pc.printf("End of command\n");
+        led1 = 0;
 
     }