From 0f390b743634ef4cdda03da8cb3f175524d59bd0 Mon Sep 17 00:00:00 2001 From: Vegard Storheil Eriksen Date: Sun, 3 Jul 2011 15:34:29 +0200 Subject: Removed old stuff. --- IMU.cpp | 92 ----------------------------------------------------------------- 1 file changed, 92 deletions(-) delete mode 100755 IMU.cpp (limited to 'IMU.cpp') diff --git a/IMU.cpp b/IMU.cpp deleted file mode 100755 index bcff537..0000000 --- a/IMU.cpp +++ /dev/null @@ -1,92 +0,0 @@ -//===================================================================================================== -// 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 -//==================================================================================================== -- cgit v1.2.3