diff options
| -rwxr-xr-x | IMU.cpp | 92 | ||||
| -rwxr-xr-x | IMU.h | 26 | ||||
| -rw-r--r-- | SConstruct | 4 | ||||
| -rw-r--r-- | main.cpp | 41 |
4 files changed, 159 insertions, 4 deletions
@@ -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 <math.h>
+#include <ch.h>
+
+//----------------------------------------------------------------------------------------------------
+// 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
+//====================================================================================================
@@ -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 @@ -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 = \
@@ -7,6 +7,8 @@ #include <ch.h>
#include <hal.h>
+#include "IMU.h"
+
class LEDThread : public BaseThread<LEDThread, 128> {
public:
noreturn_t thread_main() {
@@ -61,6 +63,11 @@ USBThread usb_thread; USBSerial usbs;
#include "foo.h"
+#include <cmath>
+
+uint8_t syncword[] = {0xff, 0x00, 0xaa, 0x55};
+uint8_t buf[64];
+int16_t* sensordata = (int16_t*)buf;
class I2CThread : public BaseThread<I2CThread, 256> {
public:
@@ -77,7 +84,9 @@ class I2CThread : public BaseThread<I2CThread, 256> { 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<I2CThread, 256> { 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);
}
}
};
|