Revision 0:d536780893d9, committed 2012-06-08

Comitter:

ohararp

Date:

Fri Jun 08 13:42:48 2012 +0000

Commit message:

Example Code modified to run with Sparkfun 9DOF stick

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ADXL345.lib	Fri Jun 08 13:42:48 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/ohararp/code/ADXL345/#16aa3688583f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC5843.lib	Fri Jun 08 13:42:48 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/ohararp/code/HMC5843/#11dff0a5ada6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMUfilter.lib	Fri Jun 08 13:42:48 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/ohararp/code/IMUfilter/#f082b9b3ec3e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ITG3200.lib	Fri Jun 08 13:42:48 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/ohararp/code/ITG3200/#68f51849a94d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jun 08 13:42:48 2012 +0000
@@ -0,0 +1,259 @@
+#include "mbed.h"
+#include "HMC5843.h"
+#include "ADXL345.h"
+#include "ITG3200.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 128
+//Convert from radians to degrees.
+#define toDegrees(x) (x * 57.2957795)
+//Convert from degrees to radians.
+#define toRadians(x) (x * 0.01745329252)
+//ITG-3200 sensitivity is 14.375 LSB/(degrees/sec).
+#define GYROSCOPE_GAIN (1 / 14.375)
+//Full scale resolution on the ADXL345 is 4mg/LSB.
+#define ACCELEROMETER_GAIN (0.004 * g0)
+//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
+
+DigitalOut myled(LED1);
+HMC5843 cmp(p28, p27);      // sda, scl
+ADXL345 acc(p28, p27);      // sda, scl
+ITG3200 gyr(p28, p27);      // sda, scl
+Serial pc(USBTX, USBRX);    // tx, rx
+
+// Init Tickers for IMU Filter Sampling
+Ticker accelerometerTicker;
+Ticker gyroscopeTicker;
+Ticker filterTicker;
+
+//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);
+
+//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 acc.
+//Same as with the gyroscope.
+double a_xBias;
+double a_yBias;
+double a_zBias;
+
+//Accumulators used for oversampling and then averaging.
+volatile double a_xAccumulator = 0;
+volatile double a_yAccumulator = 0;
+volatile double a_zAccumulator = 0;
+volatile double w_xAccumulator = 0;
+volatile double w_yAccumulator = 0;
+volatile double w_zAccumulator = 0;
+
+//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];
+//Number of accelerometer samples we're on.
+int accelerometerSamples = 0;
+//Number of gyroscope samples we're on.
+int gyroscopeSamples = 0;
+
+void initializeAccelerometer(void) {
+    // Get Acc Id
+    pc.printf("Acc Id=%x \n", acc.getDeviceID());
+    //Go into standby mode to configure the device.
+    acc.setPowerControl(0x00);
+    //Full resolution, +/-16g, 4mg/LSB.
+    acc.setDataFormatControl(0x0B);
+    //200Hz data rate.
+    acc.setDataRate(ADXL345_200HZ);
+    //Measurement mode.
+    acc.setPowerControl(0x08);
+    //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
+    wait_ms(22); 
+}
+
+void sampleAccelerometer(void) {
+    //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;
+
+    } else {
+        //Take another sample.
+        acc.getOutput(readings);
+
+        a_xAccumulator += (int16_t) readings[0];
+        a_yAccumulator += (int16_t) readings[1];
+        a_zAccumulator += (int16_t) readings[2];
+
+        accelerometerSamples++;
+    }
+}
+
+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++) {
+
+        acc.getOutput(readings);
+
+        a_xAccumulator += (int16_t) readings[0];
+        a_yAccumulator += (int16_t) readings[1];
+        a_zAccumulator += (int16_t) readings[2];
+
+        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 - 250);
+
+    a_xAccumulator = 0;
+    a_yAccumulator = 0;
+    a_zAccumulator = 0;
+}
+
+void initializeGyroscope(void) {
+    //Get Gyro ID
+    pc.printf("Gyro Id=%x \n", gyr.getWhoAmI());
+    //Low pass filter bandwidth of 42Hz.
+    gyr.setLpBandwidth(LPFBW_42HZ);
+    //Internal sample rate of 200Hz. (1kHz / 5).
+    gyr.setSampleRateDivider(4);
+}
+
+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++) {
+
+        w_xAccumulator += gyr.getGyroX();
+        w_yAccumulator += gyr.getGyroY();
+        w_zAccumulator += gyr.getGyroZ();
+        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) * 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;
+    } else {
+        //Take another sample.
+        w_xAccumulator += gyr.getGyroX();
+        w_yAccumulator += gyr.getGyroY();
+        w_zAccumulator += gyr.getGyroZ();
+        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() {
+    pc.baud(115200);            // Baudrate
+    pc.printf("Starting IMU filter test...\n");
+
+    //Initialize inertial sensors.
+    initializeAccelerometer();
+    calibrateAccelerometer();
+    initializeGyroscope();
+    calibrateGyroscope();
+
+
+    //Set up timers.
+    //Accelerometer data rate is 200Hz, so we'll sample at this speed.
+    accelerometerTicker.attach(&sampleAccelerometer, 0.005);
+    //Gyroscope data rate is 200Hz, so we'll sample at this speed.
+    gyroscopeTicker.attach(&sampleGyroscope, 0.005);
+    //Update the filter variables at the correct rate.
+    filterTicker.attach(&filter, FILTER_RATE);
+
+    while (1) {
+
+        wait(FILTER_RATE);
+       
+        pc.printf("%f,%f,%f\n",toDegrees(imuFilter.getRoll()),
+                  toDegrees(imuFilter.getPitch()),
+                  toDegrees(imuFilter.getYaw()));
+                
+        if (myled) {
+            myled=0;
+        } else {
+            myled=1;
+        }        
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Jun 08 13:42:48 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/737756e0b479