Test IMU filter.

4 files changed, 159 insertions, 4 deletions

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 <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

+//====================================================================================================

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 <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);

 			}

 		}

 };