X-Git-Url: http://git.droids-corp.org/?p=protos%2Fimu.git;a=blobdiff_plain;f=MadgwickAHRS.c;h=5f03d6f4b1dae4912e05aeff95cbfecdb401d4de;hp=fc547d2ddb0b9e1b4cef0bc0edc6755f5141668a;hb=950c56ac3c1e5651f54965700f385076ab63c8ea;hpb=96d834bdfb8df4e3369ca1b3c7bc7bc8534fda31 diff --git a/MadgwickAHRS.c b/MadgwickAHRS.c index fc547d2..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,78 +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 -#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; @@ -103,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); @@ -111,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; @@ -183,259 +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; } - -//--------------------------------------------------------------------------------------------------- -// IMU algorithm update - -f32 f_q0; -f32 f_q1; -f32 f_q2; -f32 f_q3; - - -void Mad_f32_init() -{ - f_q0 = f32_from_double((double)1.0); - f_q1 = f32_from_double((double)0.0); - f_q2 = f32_from_double((double)0.0); - f_q3 = f32_from_double((double)0.0); - -} -void MadgwickAHRSupdateIMU_f32(float gx, float gy, float gz, float ax, float ay, float az) { - f32 f_recipNorm; - f32 f_s0, f_s1, f_s2, f_s3; - f32 f_qDot1, f_qDot2, f_qDot3, f_qDot4; - f32 f__2q0, f__2q1, f__2q2, f__2q3, f__4q0, f__4q1, f__4q2 ,f__8q1, f__8q2, f_q0q0, f_q1q1, f_q2q2, f_q3q3; - - f32 f_gx, f_gy, f_gz; - f32 f_ax, f_ay, f_az; - - f32 f_beta; - f32 f_sampleFreq; - - - f_gx = f32_from_double((double)gx); - f_gy = f32_from_double((double)gy); - f_gz = f32_from_double((double)gz); - - f_ax = f32_from_double((double)ax); - f_ay = f32_from_double((double)ay); - f_az = f32_from_double((double)az); - - f_beta = f32_from_double((double)beta); - f_sampleFreq = f32_from_double((double)sampleFreq); - - - // 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); - */ - - f_qDot1 = f32_mul(f32_sub(f32_sub(f32_neg(f32_mul(f_gx, f_q1)), f32_mul(f_gy, f_q2)), f32_mul(f_gz, f_q3)), f32_from_double(0.5)); - f_qDot2 = f32_mul(f32_sub(f32_add(f32_mul(f_gx, f_q0), f32_mul(f_gz, f_q2)), f32_mul(f_gy, f_q3)), f32_from_double(0.5)); - f_qDot3 = f32_mul(f32_add(f32_mul(f_gx, f_q3), f32_sub(f32_mul(f_gy, f_q0), f32_mul(f_gz, f_q1))), f32_from_double(0.5)); - f_qDot4 = f32_mul(f32_sub(f32_add(f32_mul(f_gy, f_q1), f32_mul(f_gz, f_q0)), f32_mul(f_gx, f_q2)), f32_from_double(0.5)); - - - - - // 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); - f_recipNorm = f32_inv(f32_sqrt((f32_add(f32_mul(f_ax, f_ax), f32_add(f32_mul(f_ay, f_ay), f32_mul(f_az, f_az)))))); - /* - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; - */ - - f_ax = f32_mul(f_ax, f_recipNorm); - f_ay = f32_mul(f_ay, f_recipNorm); - f_az = f32_mul(f_az, f_recipNorm); - - // Auxiliary variables to avoid repeated arithmetic - - /* - _2q0 = 2.0f * q0; - _2q1 = 2.0f * q1; - _2q2 = 2.0f * q2; - _2q3 = 2.0f * q3; - _4q0 = 4.0f * q0; - _4q1 = 4.0f * q1; - _4q2 = 4.0f * q2; - _8q1 = 8.0f * q1; - _8q2 = 8.0f * q2; - q0q0 = q0 * q0; - q1q1 = q1 * q1; - q2q2 = q2 * q2; - q3q3 = q3 * q3; - */ - - f__2q0 = f32_mul(f32_from_double(2.0f), f_q0); - f__2q1 = f32_mul(f32_from_double(2.0f), f_q1); - f__2q2 = f32_mul(f32_from_double(2.0f), f_q2); - f__2q3 = f32_mul(f32_from_double(2.0f), f_q3); - f__4q0 = f32_mul(f32_from_double(4.0f), f_q0); - f__4q1 = f32_mul(f32_from_double(4.0f), f_q1); - f__4q2 = f32_mul(f32_from_double(4.0f), f_q2); - f__8q1 = f32_mul(f32_from_double(8.0f), f_q1); - f__8q2 = f32_mul(f32_from_double(8.0f), f_q2); - f_q0q0 = f32_mul(f_q0, f_q0); - f_q1q1 = f32_mul(f_q1, f_q1); - f_q2q2 = f32_mul(f_q2, f_q2); - f_q3q3 = f32_mul(f_q3, f_q3); - - - // 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 - */ - - f_s0 = f32_sub(f32_add(f32_mul(f__2q2, f_ax), f32_add(f32_mul(f__4q0, f_q1q1), f32_mul(f__4q0, f_q2q2))), f32_mul(f__2q1, f_ay)); - f_s1 = f32_add(f32_mul(f__4q1, f_az), f32_add(f32_mul(f__8q1, f_q1q1), f32_add(f32_mul(f__8q1, f_q2q2), f32_sub(f32_sub(f32_add(f32_mul(f_q0q0, f32_mul(f_q1, f32_from_double(4.0f))), f32_sub(f32_mul(f__4q1, f_q3q3), f32_mul(f__2q3, f_ax))), f32_mul(f__2q0, f_ay)), f__4q1)))); - f_s2 = f32_add(f32_mul(f__4q2, f_az), f32_add(f32_mul(f__8q2, f_q1q1), f32_add(f32_mul(f__8q2, f_q2q2), f32_sub(f32_sub(f32_add(f32_mul(f__2q0, f_ax), f32_add(f32_mul(f__4q2, f_q3q3), f32_mul(f_q0q0, f32_mul(f_q2, f32_from_double(4.0))))), f32_mul(f__2q3, f_ay)), f__4q2)))); - f_s3 = f32_sub(f32_add(f32_mul(f_q2q2, f32_mul(f_q3, f32_from_double(4.0))), f32_sub(f32_mul(f_q1q1, f32_mul(f_q3, f32_from_double(4.0))), f32_mul(f__2q1, f_ax))), f32_mul(f__2q2, f_ay)); - f_recipNorm = f32_inv(f32_sqrt(f32_add(f32_mul(f_s0, f_s0), f32_add(f32_mul(f_s1, f_s1), f32_add(f32_mul(f_s2, f_s2), f32_mul(f_s3, f_s3)))))); - - /* - s0 *= recipNorm; - s1 *= recipNorm; - s2 *= recipNorm; - s3 *= recipNorm; - */ - - f_s0 = f32_mul(f_s0, f_recipNorm); - f_s1 = f32_mul(f_s1, f_recipNorm); - f_s2 = f32_mul(f_s2, f_recipNorm); - f_s3 = f32_mul(f_s3, f_recipNorm); - - - // Apply feedback step - - /* - qDot1 -= beta * s0; - qDot2 -= beta * s1; - qDot3 -= beta * s2; - qDot4 -= beta * s3; - */ - - f_qDot1 = f32_sub(f_qDot1, f32_mul(f_beta, f_s0)); - f_qDot2 = f32_sub(f_qDot2, f32_mul(f_beta, f_s1)); - f_qDot3 = f32_sub(f_qDot3, f32_mul(f_beta, f_s2)); - f_qDot4 = f32_sub(f_qDot4, f32_mul(f_beta, f_s3)); - - } - - // 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); - */ - - f_q0 = f32_add(f_q0, f32_mul(f_qDot1, f32_inv(f_sampleFreq))); - f_q1 = f32_add(f_q1, f32_mul(f_qDot2, f32_inv(f_sampleFreq))); - f_q2 = f32_add(f_q2, f32_mul(f_qDot3, f32_inv(f_sampleFreq))); - f_q3 = f32_add(f_q3, f32_mul(f_qDot4, f32_inv(f_sampleFreq))); - - // Normalise quaternion - //recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - - f_recipNorm = f32_inv(f32_sqrt(f32_add(f32_mul(f_q0, f_q0), f32_add(f32_mul(f_q1, f_q1), f32_add(f32_mul(f_q2, f_q2), f32_mul(f_q3, f_q3)))))); - - /* - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; - */ - - f_q0 = f32_mul(f_q0, f_recipNorm); - f_q1 = f32_mul(f_q1, f_recipNorm); - f_q2 = f32_mul(f_q2, f_recipNorm); - f_q3 = f32_mul(f_q3, f_recipNorm); - - - q0 = f32_to_double(f_q0); - q1 = f32_to_double(f_q1); - q2 = f32_to_double(f_q2); - q3 = f32_to_double(f_q3); - - //printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", q0, q1, q2); - -} - - -//--------------------------------------------------------------------------------------------------- -// 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 -//====================================================================================================