jetfishteam / Mbed 2 deprecated sparkfun6dof

for interfacing the sparkfun boards

Dependencies:   ADXL345_I2C HMC5883L IMUfilter ITG3200_HelloWorld mbed

Files at this revision

API Documentation at this revision

Revision 0:441caaf895d8, committed 2014-04-29

Comitter:
sandwich
Date:
Tue Apr 29 00:04:48 2014 +0000
Commit message:
imufilter works. 6dof

Changed in this revision

ADXL345HL.cpp Show annotated file Show diff for this revision Revisions of this file
ADXL345HL.h Show annotated file Show diff for this revision Revisions of this file
ADXL345_I2C.lib Show annotated file Show diff for this revision Revisions of this file
HMC5883L.lib Show annotated file Show diff for this revision Revisions of this file
IMUfilter.lib Show annotated file Show diff for this revision Revisions of this file
ITG3200HL.cpp Show annotated file Show diff for this revision Revisions of this file
ITG3200HL.h Show annotated file Show diff for this revision Revisions of this file
ITG3200_HelloWorld.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file 
diff -r 000000000000 -r 441caaf895d8 ADXL345HL.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ADXL345HL.cpp	Tue Apr 29 00:04:48 2014 +0000
@@ -0,0 +1,90 @@
+#include "ADXL345HL.h"
+
+void ADXL345HL::init(int calibsamples, int readsampls, float samplrate)
+{
+    accelerometer=new ADXL345_I2C(p28,p27);
+    calibrationsamples=calibsamples;
+    readsamples=readsampls;
+    samplerate=samplrate;
+    output=new double[3];
+    //Go into standby mode to configure the device.
+    accelerometer->setPowerControl(0x00);
+    wait(0.001);
+    //Full resolution, +/-16g, 4mg/LSB.
+    accelerometer->setDataFormatControl(0x0B);
+    wait(0.001);
+    //200Hz data rate.
+    accelerometer->setDataRate(ADXL345_200HZ);
+    wait(0.001);
+    //Measurement mode.
+    accelerometer->setPowerControl(0x08);
+    //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
+    wait_ms(22);
+}
+
+void ADXL345HL::calibrateAccelerometer(void)
+{
+
+    double a_xAccumulator = 0;
+    double a_yAccumulator = 0;
+    double a_zAccumulator = 0;
+
+    //Take a number of readings and average them
+    //to calculate the zero g offset.
+    for (int i = 0; i < calibrationsamples; i++) {
+
+        accelerometer->getOutput(readings);
+
+        a_xAccumulator += (int16_t) readings[0];
+        a_yAccumulator += (int16_t) readings[1];
+        a_zAccumulator += (int16_t) readings[2];
+
+        wait(samplerate);
+
+    }
+
+    a_xAccumulator /= calibrationsamples;
+    a_yAccumulator /= calibrationsamples;
+    a_zAccumulator /= calibrationsamples;
+
+    //At 4mg/LSB, 250 LSBs is 1g.
+    xBias = a_xAccumulator;
+    yBias = a_yAccumulator;
+    zBias=a_zAccumulator;
+    //zBias = (a_zAccumulator - 250);
+
+    a_xAccumulator = 0;
+    a_yAccumulator = 0;
+    a_zAccumulator = 0;
+}
+
+
+double* ADXL345HL::sampleAccelerometer(void)
+{
+    double a_xAccumulator = 0;
+    double a_yAccumulator = 0;
+    double a_zAccumulator = 0;
+    for (int i=0; i<readsamples; ++i) {
+        accelerometer->getOutput(readings);
+        a_xAccumulator += (int16_t) readings[0];
+        a_yAccumulator += (int16_t) readings[1];
+        a_zAccumulator += (int16_t) readings[2];
+    }
+    //Average the samples, remove the bias, and calculate the acceleration
+    //in m/s/s.
+    output[0] = ((a_xAccumulator / readsamples) - xBias) * ACCELEROMETER_GAIN;
+    output[1] = ((a_yAccumulator / readsamples) - yBias) * ACCELEROMETER_GAIN;
+    output[2] = ((a_zAccumulator / readsamples) - zBias) * ACCELEROMETER_GAIN;
+    return output;
+}
+
+
+ADXL345HL::ADXL345HL()
+{
+}
+
+ADXL345HL::~ADXL345HL()
+{
+    delete[] output;
+    delete accelerometer;
+}
\ No newline at end of file

diff -r 000000000000 -r 441caaf895d8 ADXL345HL.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ADXL345HL.h	Tue Apr 29 00:04:48 2014 +0000
@@ -0,0 +1,37 @@
+#pragma once
+#include "ADXL345_I2C.h"
+
+
+//Gravity at Earth's surface in m/s/s
+#define g0 9.812865328
+//Number of samples to average.
+#define SAMPLES 4
+//Convert from radians to degrees.
+#define toDegrees(x) (x * 57.2957795)
+//Convert from degrees to radians.
+#define toRadians(x) (x * 0.01745329252)
+//Full scale resolution on the ADXL345 is 4mg/LSB.
+#define ACCELEROMETER_GAIN (0.004 * g0)
+//Sampling accelerometer at 200Hz.
+#define ACC_RATE    0.005
+
+class ADXL345HL
+{
+private:
+    ADXL345_I2C* accelerometer;
+    int readings[3];
+    double xBias;
+    double yBias;
+    double zBias;
+    double* output; //x,y,z
+    char address;   //i^2c device address
+    int calibrationsamples;
+    int readsamples;
+    float samplerate;
+public:
+    void init(int calibsamples, int readsampls, float samplrate);
+    void calibrateAccelerometer(void);
+    double* sampleAccelerometer(void);
+    ADXL345HL();
+    ~ADXL345HL();
+};
\ No newline at end of file

diff -r 000000000000 -r 441caaf895d8 ADXL345_I2C.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ADXL345_I2C.lib	Tue Apr 29 00:04:48 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/jetfishteam/code/ADXL345_I2C/#d1f975baa680

diff -r 000000000000 -r 441caaf895d8 HMC5883L.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC5883L.lib	Tue Apr 29 00:04:48 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/tylerjw/code/HMC5883L/#bc4e1201e092

diff -r 000000000000 -r 441caaf895d8 IMUfilter.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMUfilter.lib	Tue Apr 29 00:04:48 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/IMUfilter/#8a920397b510

diff -r 000000000000 -r 441caaf895d8 ITG3200HL.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ITG3200HL.cpp	Tue Apr 29 00:04:48 2014 +0000
@@ -0,0 +1,73 @@
+#include "ITG3200HL.h"
+
+ITG3200HL::~ITG3200HL()
+{
+    delete[] output;
+    delete gyro;
+}
+
+void ITG3200HL::init(int calibsampls, int readsampls, float samplrate)
+{
+    gyro=new ITG3200(p28,p27);
+    gyro->setLpBandwidth(LPFBW_42HZ);
+    output=new double[3];
+    calibrationsamples=calibsampls;
+    samplerate=samplrate;
+    readsamples=readsampls;
+    output[0]=0;
+    output[1]=0;
+    output[2]=0;
+}
+
+void ITG3200HL::calibrate()
+{
+    double a_xAccumulator = 0;
+    double a_yAccumulator = 0;
+    double a_zAccumulator = 0;
+
+    //Take a number of readings and average them
+    //to calculate the zero g offset.
+    for (int i = 0; i < calibrationsamples; i++) {
+        int16_t readings[3]= {gyro->getGyroX(), gyro->getGyroY(), gyro->getGyroZ()};
+
+        a_xAccumulator += (int16_t) readings[0];
+        a_yAccumulator += (int16_t) readings[1];
+        a_zAccumulator += (int16_t) readings[2];
+
+        wait(samplerate);
+
+    }
+
+    a_xAccumulator /= calibrationsamples;
+    a_yAccumulator /= calibrationsamples;
+    a_zAccumulator /= calibrationsamples;
+
+    //At 4mg/LSB, 250 LSBs is 1g.
+    xBias = a_xAccumulator;
+    yBias = a_yAccumulator;
+    zBias=a_zAccumulator;
+    //zBias = (a_zAccumulator - 250);
+
+    a_xAccumulator = 0;
+    a_yAccumulator = 0;
+    a_zAccumulator = 0;
+}
+
+double* ITG3200HL::sample()
+{
+    double a_xAccumulator = 0;
+    double a_yAccumulator = 0;
+    double a_zAccumulator = 0;
+    for (int i=0; i<readsamples; ++i) {
+        int16_t readings[3]= {gyro->getGyroX(), gyro->getGyroY(), gyro->getGyroZ()};
+        a_xAccumulator += (int16_t) readings[0];
+        a_yAccumulator += (int16_t) readings[1];
+        a_zAccumulator += (int16_t) readings[2];
+    }
+    //Average the samples, remove the bias, and calculate the rotation
+    //in rad/s
+    output[0] = ((a_xAccumulator / readsamples) - xBias) * GYRO_GAIN;
+    output[1] = ((a_yAccumulator / readsamples) - yBias) * GYRO_GAIN;
+    output[2] = ((a_zAccumulator / readsamples) - zBias) * GYRO_GAIN;
+    return output;
+}
\ No newline at end of file

diff -r 000000000000 -r 441caaf895d8 ITG3200HL.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ITG3200HL.h	Tue Apr 29 00:04:48 2014 +0000
@@ -0,0 +1,23 @@
+#pragma once
+#include "ITG3200.h"
+
+#define GYRO_GAIN 0.00124 //(rad/sec) / LSB
+
+class ITG3200HL
+{
+    private:
+    ITG3200* gyro;
+    int calibrationsamples;
+    int readsamples;
+    float samplerate;
+    double xBias;
+    double yBias;
+    double zBias;
+    double* output;
+    
+    public:
+    void init(int calibsampls, int readsampls, float samplrate);
+    void calibrate();
+    double* sample();
+    ~ITG3200HL();
+};
\ No newline at end of file

diff -r 000000000000 -r 441caaf895d8 ITG3200_HelloWorld.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ITG3200_HelloWorld.lib	Tue Apr 29 00:04:48 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/jetfishteam/code/ITG3200_HelloWorld/#ce79874b9157

diff -r 000000000000 -r 441caaf895d8 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 29 00:04:48 2014 +0000
@@ -0,0 +1,33 @@
+#include "ADXL345_I2C.h"
+#include "ADXL345HL.h"
+#include "ITG3200HL.h"
+
+#include "IMUfilter.h"
+
+
+//ADXL345_I2C accelerometer(p28, p27);
+ADXL345HL accel;
+ITG3200HL gyro;
+//ITG3200 gyro(p28, p27);
+Serial pc(USBTX, USBRX);
+IMUfilter imuFilter(0.1, 10);
+
+int main()
+{
+    accel.init(20,4,0.004);
+    accel.calibrateAccelerometer();
+    gyro.init(20,4,0.004);
+    gyro.calibrate();
+    while (1) {
+
+        wait(0.1);
+        double* accelreadings=accel.sampleAccelerometer();
+        double* gyroreadings=gyro.sample();
+        imuFilter.updateFilter(gyroreadings[0],gyroreadings[1],gyroreadings[2],accelreadings[0],accelreadings[1],accelreadings[2]);
+        imuFilter.computeEuler();
+        pc.printf("roll: %f, pitch: %f, yaw: %f\n", imuFilter.getRoll(), imuFilter.getPitch(), imuFilter.getYaw());
+        //pc.printf("aceelerometer: %f, %f, %f\n", readings[0], readings[1], readings[2]);
+        //pc.printf("gyro: %i, %i, %i\n", gyros[0],gyros[1],gyros[2]);
+    }
+
+}
\ No newline at end of file

diff -r 000000000000 -r 441caaf895d8 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Apr 29 00:04:48 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/9114680c05da