diff options
Diffstat (limited to 'IMU.cpp')
| -rwxr-xr-x | IMU.cpp | 184 |
1 files changed, 92 insertions, 92 deletions
@@ -1,92 +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
-//====================================================================================================
+//===================================================================================================== +// 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 +//==================================================================================================== |