From 82ffa82cc0423ed83ab94def6d3295f424a7c03c Mon Sep 17 00:00:00 2001 From: Vegard Storheil Eriksen Date: Mon, 11 Apr 2011 05:46:10 +0200 Subject: Test IMU filter. --- IMU.cpp | 92 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ IMU.h | 26 ++++++++++++++++++ SConstruct | 4 +-- main.cpp | 41 ++++++++++++++++++++++++++-- 4 files changed, 159 insertions(+), 4 deletions(-) create mode 100755 IMU.cpp create mode 100755 IMU.h diff --git a/IMU.cpp b/IMU.cpp new file mode 100755 index 0000000..8a4926c --- /dev/null +++ b/IMU.cpp @@ -0,0 +1,92 @@ +//===================================================================================================== +// IMU.c +// S.O.H. Madgwick +// 25th September 2010 +//===================================================================================================== +// Description: +// +// Quaternion implementation of the 'DCM filter' [Mayhony et al]. +// +// User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'. +// +// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated +// orientation. See my report for an overview of the use of quaternions in this application. +// +// User must call 'IMUupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz') +// and accelerometer ('ax', 'ay', 'ay') data. Gyroscope units are radians/second, accelerometer +// units are irrelevant as the vector is normalised. +// +//===================================================================================================== + +//---------------------------------------------------------------------------------------------------- +// Header files + +#include "IMU.h" +#include +#include + +//---------------------------------------------------------------------------------------------------- +// Definitions + +#define Kp 2.0f // proportional gain governs rate of convergence to accelerometer/magnetometer +#define Ki 0.005f // integral gain governs rate of convergence of gyroscope biases +#define halfT (0.5f / 100) // half the sample period + +//--------------------------------------------------------------------------------------------------- +// Variable definitions + +float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation +float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error + +//==================================================================================================== +// Function +//==================================================================================================== + +void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) { + float norm; + float vx, vy, vz; + float ex, ey, ez; + + // normalise the measurements + norm = sqrt(ax*ax + ay*ay + az*az); + ax = ax / norm; + ay = ay / norm; + az = az / norm; + + // estimated direction of gravity + vx = 2*(q1*q3 - q0*q2); + vy = 2*(q0*q1 + q2*q3); + vz = q0*q0 - q1*q1 - q2*q2 + q3*q3; + + // error is sum of cross product between reference direction of field and direction measured by sensor + ex = (ay*vz - az*vy); + ey = (az*vx - ax*vz); + ez = (ax*vy - ay*vx); + + // integral error scaled integral gain + exInt = exInt + ex*Ki; + eyInt = eyInt + ey*Ki; + ezInt = ezInt + ez*Ki; + + // adjusted gyroscope measurements + gx = gx + Kp*ex + exInt; + gy = gy + Kp*ey + eyInt; + gz = gz + Kp*ez + ezInt; + + // integrate quaternion rate and normalise + q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT; + q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT; + q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT; + q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT; + + // normalise quaternion + norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); + q0 = q0 / norm; + q1 = q1 / norm; + q2 = q2 / norm; + q3 = q3 / norm; +} + +//==================================================================================================== +// END OF CODE +//==================================================================================================== diff --git a/IMU.h b/IMU.h new file mode 100755 index 0000000..f25f38c --- /dev/null +++ b/IMU.h @@ -0,0 +1,26 @@ +//===================================================================================================== +// IMU.h +// S.O.H. Madgwick +// 25th September 2010 +//===================================================================================================== +// +// See IMU.c file for description. +// +//===================================================================================================== +#ifndef IMU_h +#define IMU_h + +//---------------------------------------------------------------------------------------------------- +// Variable declaration + +extern float q0, q1, q2, q3; // quaternion elements representing the estimated orientation + +//--------------------------------------------------------------------------------------------------- +// Function declaration + +void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az); + +#endif +//===================================================================================================== +// End of file +//===================================================================================================== \ No newline at end of file diff --git a/SConstruct b/SConstruct index db94e74..73683fd 100644 --- a/SConstruct +++ b/SConstruct @@ -13,7 +13,7 @@ env = Environment( #CPPDEFINES = [], LINK = 'arm-none-eabi-gcc', - LINKFLAGS = '-Wall -mcpu=cortex-m3 -mthumb -Wl,--gc-sections -Wl,-Tch.ld', + LINKFLAGS = '-Wall -mcpu=cortex-m3 -mthumb -Wl,-Tch.ld', # -Wl,--gc-sections AR = 'arm-none-eabi-ar', RANLIB = 'arm-none-eabi-ranlib', @@ -24,7 +24,7 @@ env = Environment( ], #LIBPATH = [], - #LIBS = [] + LIBS = ['m'], ) sources = \ diff --git a/main.cpp b/main.cpp index 911e760..14f8a8a 100644 --- a/main.cpp +++ b/main.cpp @@ -7,6 +7,8 @@ #include #include +#include "IMU.h" + class LEDThread : public BaseThread { public: noreturn_t thread_main() { @@ -61,6 +63,11 @@ USBThread usb_thread; USBSerial usbs; #include "foo.h" +#include + +uint8_t syncword[] = {0xff, 0x00, 0xaa, 0x55}; +uint8_t buf[64]; +int16_t* sensordata = (int16_t*)buf; class I2CThread : public BaseThread { public: @@ -77,7 +84,9 @@ class I2CThread : public BaseThread { acc.init(); magn.init(); + systime_t nexttime = chTimeNow(); while (1) { + nexttime += MS2ST(100); gyro.update(); acc.update(); magn.update(); @@ -85,11 +94,39 @@ class I2CThread : public BaseThread { y = gyro.y; z = gyro.z; + IMUupdate(gyro.x * 0.0012141420883438813, gyro.y * 0.0012141420883438813, gyro.z * 0.0012141420883438813, acc.x, acc.y, acc.z); + + //float pitch = asinf(2*(q0*q2 - q3*q1)); + int16_t pitch = atan2f(2*(q2*q3 + q0*q1), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767; + int16_t roll = atan2f(2*(-q1*q3 + q0*q2), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767; + int16_t yaw = atan2f(2*(q2*q1 + q0*q3), 1 - 2 * (q3*q3 + q2*q2)) / M_PI * 32767; + + sensordata[0] = gyro.x; + sensordata[1] = gyro.y; + sensordata[2] = gyro.z; + sensordata[3] = acc.x; + sensordata[4] = acc.y; + sensordata[5] = acc.z; + sensordata[6] = magn.x; + sensordata[7] = magn.y; + sensordata[8] = magn.z; + sensordata[9] = pitch; + sensordata[10] = roll; + sensordata[11] = yaw; + + + usbs.write(syncword, sizeof(syncword)); + usbs.write(buf, sizeof(buf)); - usbprintf(usbs, "%6d, %6d, %6d\r\n", x, y, z); + /*usbprintf(usbs, "%6d, %6d, %6d | %6d, %6d, %6d | %6d, %6d, %6d | %6d, %6d, %6d, %6d | %6d, %6d, %6d\r\n", + gyro.x, gyro.y, gyro.z, + acc.x, acc.y, acc.z, + magn.x, magn.y, magn.z, + int(q0 * 10000), int(q1 * 10000), int(q2 * 10000), int(q3 * 10000), + int(pitch * 10000), int(roll * 10000), int(yaw * 10000));*/ - chThdSleepMilliseconds(100); + //chThdSleepUntil(nexttime); } } }; -- cgit v1.2.3