From: Olivier Matz Date: Thu, 26 Jun 2014 18:15:54 +0000 (+0200) Subject: madgwick: remove unused code (fixed point lib) X-Git-Url: http://git.droids-corp.org/?a=commitdiff_plain;h=786cb15c1461ea7fac8419b520f0a0281cdbaf7f;p=protos%2Fimu.git madgwick: remove unused code (fixed point lib) --- diff --git a/MadgwickAHRS.c b/MadgwickAHRS.c index fc547d2..240470f 100644 --- a/MadgwickAHRS.c +++ b/MadgwickAHRS.c @@ -18,8 +18,6 @@ #include "MadgwickAHRS.h" #include -#include - //--------------------------------------------------------------------------------------------------- // Definitions @@ -225,200 +223,6 @@ void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, flo } -//--------------------------------------------------------------------------------------------------- -// 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 diff --git a/MadgwickAHRS.h b/MadgwickAHRS.h index 7e42291..18e19fa 100644 --- a/MadgwickAHRS.h +++ b/MadgwickAHRS.h @@ -24,7 +24,7 @@ extern volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az); -void Mad_f32_init(); +void Mad_f32_init(void); void MadgwickAHRSupdateIMU_f32(float gx, float gy, float gz, float ax, float ay, float az); #endif