X-Git-Url: http://git.droids-corp.org/?p=protos%2Fimu.git;a=blobdiff_plain;f=MadgwickAHRS.c;fp=MadgwickAHRS.c;h=5f03d6f4b1dae4912e05aeff95cbfecdb401d4de;hp=240470fdecf189e678ab69a8134e279c6d207ffe;hb=2998e2d15b6098be4c272b187af9438d95b140cf;hpb=849b29b998a8cbc10cd1900d3e5dd1a8488b77f6 diff --git a/MadgwickAHRS.c b/MadgwickAHRS.c index 240470f..5f03d6f 100644 --- a/MadgwickAHRS.c +++ b/MadgwickAHRS.c @@ -1,6 +1,25 @@ -//===================================================================================================== +/* + * Copyright (c) 2014, Olivier MATZ + * Copyright (c) 2011-2012, SOH Madgwick + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +//============================================================================ // MadgwickAHRS.c -//===================================================================================================== +//============================================================================ // // Implementation of Madgwick's IMU and AHRS algorithms. // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms @@ -10,76 +29,73 @@ // 02/10/2011 SOH Madgwick Optimised for reduced CPU load // 19/02/2012 SOH Madgwick Magnetometer measurement is normalised // -//===================================================================================================== - -//--------------------------------------------------------------------------------------------------- -// Header files +//============================================================================ #include "MadgwickAHRS.h" #include - -//--------------------------------------------------------------------------------------------------- -// Definitions - //#define sampleFreq 512.0f // sample frequency in Hz //#define sampleFreq 46.0f // sample frequency in Hz #define sampleFreq 85.0f // sample frequency in Hz #define betaDef 0.1f // 2 * proportional gain -//--------------------------------------------------------------------------------------------------- -// Variable definitions - volatile float beta = betaDef; // 2 * proportional gain (Kp) volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame -//--------------------------------------------------------------------------------------------------- -// Function declarations - -float invSqrt(float x); - -//==================================================================================================== -// Functions - -//--------------------------------------------------------------------------------------------------- -// AHRS algorithm update +static float invSqrt(float x) +{ + return 1.0f / sqrtf(x); +} -void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { +/* AHRS algorithm update */ +void MadgwickAHRSupdate(const struct imu_info *imu, struct quaternion *quat) +{ float recipNorm; float s0, s1, s2, s3; float qDot1, qDot2, qDot3, qDot4; float hx, hy; - float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; - - // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) - if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { - MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az); + float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz; + float _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3; + float q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; + float mx, my, mz, ax, ay, az; + float q0 = quat->q0; + float q1 = quat->q1; + float q2 = quat->q2; + float q3 = quat->q3; + + /* Use IMU algorithm if magnetometer measurement invalid (avoids NaN in + * magnetometer normalisation) */ + if ((imu->mx == 0.0f) && (imu->my == 0.0f) && (imu->mz == 0.0f)) { + MadgwickAHRSupdateIMU(imu, quat); return; } - // Rate of change of quaternion from gyroscope - qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); - qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); - qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); - qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); - - // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { - - // Normalise accelerometer measurement - recipNorm = invSqrt(ax * ax + ay * ay + az * az); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; - - // Normalise magnetometer measurement - recipNorm = invSqrt(mx * mx + my * my + mz * mz); - mx *= recipNorm; - my *= recipNorm; - mz *= recipNorm; - - // Auxiliary variables to avoid repeated arithmetic + /* Rate of change of quaternion from gyroscope */ + qDot1 = 0.5f * (-q1 * imu->gx - q2 * imu->gy - q3 * imu->gz); + qDot2 = 0.5f * (q0 * imu->gx + q2 * imu->gz - q3 * imu->gy); + qDot3 = 0.5f * (q0 * imu->gy - q1 * imu->gz + q3 * imu->gx); + qDot4 = 0.5f * (q0 * imu->gz + q1 * imu->gy - q2 * imu->gx); + + /* Compute feedback only if accelerometer measurement valid (avoids NaN + * in accelerometer normalisation) */ + if (!((imu->ax == 0.0f) && (imu->ay == 0.0f) && (imu->az == 0.0f))) { + + /* Normalise accelerometer measurement */ + recipNorm = invSqrt(imu->ax * imu->ax + imu->ay * imu->ay + + imu->az * imu->az); + ax = imu->ax * recipNorm; + ay = imu->ay * recipNorm; + az = imu->az * recipNorm; + + /* Normalise magnetometer measurement */ + recipNorm = invSqrt(imu->mx * imu->mx + imu->my * imu->my + + imu->mz * imu->mz); + mx = imu->mx * recipNorm; + my = imu->my * recipNorm; + mz = imu->mz * recipNorm; + + /* Auxiliary variables to avoid repeated arithmetic */ _2q0mx = 2.0f * q0 * mx; _2q0my = 2.0f * q0 * my; _2q0mz = 2.0f * q0 * mz; @@ -101,7 +117,7 @@ void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float q2q3 = q2 * q3; q3q3 = q3 * q3; - // Reference direction of Earth's magnetic field + /* Reference direction of Earth's magnetic field */ hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3; hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3; _2bx = sqrt(hx * hx + hy * hy); @@ -109,64 +125,70 @@ void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float _4bx = 2.0f * _2bx; _4bz = 2.0f * _2bz; - // Gradient decent algorithm corrective step + /* Gradient decent algorithm corrective step */ s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude + + /* normalize step magnitude */ + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); s0 *= recipNorm; s1 *= recipNorm; s2 *= recipNorm; s3 *= recipNorm; - // Apply feedback step + /* Apply feedback step */ qDot1 -= beta * s0; qDot2 -= beta * s1; qDot3 -= beta * s2; qDot4 -= beta * s3; } - // Integrate rate of change of quaternion to yield quaternion + /* Integrate rate of change of quaternion to yield quaternion */ q0 += qDot1 * (1.0f / sampleFreq); q1 += qDot2 * (1.0f / sampleFreq); q2 += qDot3 * (1.0f / sampleFreq); q3 += qDot4 * (1.0f / sampleFreq); - // Normalise quaternion + /* Normalise quaternion */ recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; + quat->q0 = q0 * recipNorm; + quat->q1 = q1 * recipNorm; + quat->q2 = q2 * recipNorm; + quat->q3 = q3 * recipNorm; } -//--------------------------------------------------------------------------------------------------- -// IMU algorithm update - -void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) { +/* IMU algorithm update (does not take magneto in account) */ +void MadgwickAHRSupdateIMU(const struct imu_info *imu, struct quaternion *quat) +{ float recipNorm; float s0, s1, s2, s3; float qDot1, qDot2, qDot3, qDot4; float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; + float ax, ay, az; + float q0 = quat->q0; + float q1 = quat->q1; + float q2 = quat->q2; + float q3 = quat->q3; - // Rate of change of quaternion from gyroscope - qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); - qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); - qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); - qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); + /* Rate of change of quaternion from gyroscope */ + qDot1 = 0.5f * (-q1 * imu->gx - q2 * imu->gy - q3 * imu->gz); + qDot2 = 0.5f * (q0 * imu->gx + q2 * imu->gz - q3 * imu->gy); + qDot3 = 0.5f * (q0 * imu->gy - q1 * imu->gz + q3 * imu->gx); + qDot4 = 0.5f * (q0 * imu->gz + q1 * imu->gy - q2 * imu->gx); - // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + /* Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) */ + if(!((imu->ax == 0.0f) && (imu->ay == 0.0f) && (imu->az == 0.0f))) { - // Normalise accelerometer measurement - recipNorm = invSqrt(ax * ax + ay * ay + az * az); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; + /* Normalise accelerometer measurement */ + recipNorm = invSqrt(imu->ax * imu->ax + imu->ay * imu->ay + imu->az * imu->az); + ax = imu->ax * recipNorm; + ay = imu->ay * recipNorm; + az = imu->az * recipNorm; - // Auxiliary variables to avoid repeated arithmetic + /* Auxiliary variables to avoid repeated arithmetic */ _2q0 = 2.0f * q0; _2q1 = 2.0f * q1; _2q2 = 2.0f * q2; @@ -181,65 +203,37 @@ void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, flo q2q2 = q2 * q2; q3q3 = q3 * q3; - - // Gradient decent algorithm corrective step + /* Gradient decent algorithm corrective step */ s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; - recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude - - + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); /* normalise step magnitude */ s0 *= recipNorm; s1 *= recipNorm; s2 *= recipNorm; s3 *= recipNorm; - - // Apply feedback step + /* Apply feedback step */ qDot1 -= beta * s0; qDot2 -= beta * s1; qDot3 -= beta * s2; qDot4 -= beta * s3; } - // Integrate rate of change of quaternion to yield quaternion + /* Integrate rate of change of quaternion to yield quaternion */ q0 += qDot1 * (1.0f / sampleFreq); q1 += qDot2 * (1.0f / sampleFreq); q2 += qDot3 * (1.0f / sampleFreq); q3 += qDot4 * (1.0f / sampleFreq); - - // Normalise quaternion - recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; - - //printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", q0, q1, q2); - + /* Normalise quaternion */ + recipNorm = invSqrt(q0 * q0 + q1 * q1 + + q2 * q2 + q3 * q3); + quat->q0 = q0 * recipNorm; + quat->q1 = q1 * recipNorm; + quat->q2 = q2 * recipNorm; + quat->q3 = q3 * recipNorm; } - -//--------------------------------------------------------------------------------------------------- -// Fast inverse square-root -// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root -/* -float invSqrt(float x) { - float halfx = 0.5f * x; - float y = x; - long i = *(long*)&y; - i = 0x5f3759df - (i>>1); - y = *(float*)&i; - y = y * (1.5f - (halfx * y * y)); - return y; -} -*/ -float invSqrt(float x) { - return 1.0f / sqrtf(x); -} -//==================================================================================================== -// END OF CODE -//====================================================================================================