madgwick: remove unused code (fixed point lib)

diff --git a/MadgwickAHRS.c b/MadgwickAHRS.c
index fc547d2..240470f 100644 (file)
--- a/MadgwickAHRS.c
+++ b/MadgwickAHRS.c
@@ -18,8 +18,6 @@
 #include "MadgwickAHRS.h"\r
 #include <math.h>\r
 \r
-#include <f32.h>\r
-\r
 \r
 //---------------------------------------------------------------------------------------------------\r
 // Definitions\r
@@ -225,200 +223,6 @@ void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, flo
 }\r
 \r
 \r
-//---------------------------------------------------------------------------------------------------\r
-// IMU algorithm update\r
-\r
-f32 f_q0;\r
-f32 f_q1;\r
-f32 f_q2;\r
-f32 f_q3;\r
-\r
-\r
-void Mad_f32_init()\r
-{\r
-       f_q0 = f32_from_double((double)1.0);\r
-       f_q1 = f32_from_double((double)0.0);\r
-       f_q2 = f32_from_double((double)0.0);\r
-       f_q3 = f32_from_double((double)0.0);\r
-\r
-}\r
-void MadgwickAHRSupdateIMU_f32(float gx, float gy, float gz, float ax, float ay, float az) {\r
-       f32 f_recipNorm;\r
-       f32 f_s0, f_s1, f_s2, f_s3;\r
-       f32 f_qDot1, f_qDot2, f_qDot3, f_qDot4;\r
-       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;\r
-\r
-       f32 f_gx, f_gy, f_gz;\r
-       f32 f_ax, f_ay, f_az;\r
-\r
-       f32 f_beta;\r
-       f32 f_sampleFreq;\r
-\r
-\r
-       f_gx = f32_from_double((double)gx);\r
-       f_gy = f32_from_double((double)gy);\r
-       f_gz = f32_from_double((double)gz);\r
-\r
-       f_ax = f32_from_double((double)ax);\r
-       f_ay = f32_from_double((double)ay);\r
-       f_az = f32_from_double((double)az);\r
-\r
-       f_beta = f32_from_double((double)beta);\r
-       f_sampleFreq = f32_from_double((double)sampleFreq);\r
-\r
-\r
-       // Rate of change of quaternion from gyroscope\r
-       /*\r
-       qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);\r
-       qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);\r
-       qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);\r
-       qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);\r
-       */\r
-\r
-       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));\r
-       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));\r
-       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));\r
-       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));\r
-\r
-\r
-\r
-\r
-       // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)\r
-       if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {\r
-\r
-               // Normalise accelerometer measurement\r
-               //recipNorm = invSqrt(ax * ax + ay * ay + az * az);\r
-               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))))));\r
-               /*\r
-               ax *= recipNorm;\r
-               ay *= recipNorm;\r
-               az *= recipNorm;\r
-               */\r
-\r
-               f_ax = f32_mul(f_ax, f_recipNorm);\r
-               f_ay = f32_mul(f_ay, f_recipNorm);\r
-               f_az = f32_mul(f_az, f_recipNorm);\r
-\r
-               // Auxiliary variables to avoid repeated arithmetic\r
-\r
-               /*\r
-               _2q0 = 2.0f * q0;\r
-               _2q1 = 2.0f * q1;\r
-               _2q2 = 2.0f * q2;\r
-               _2q3 = 2.0f * q3;\r
-               _4q0 = 4.0f * q0;\r
-               _4q1 = 4.0f * q1;\r
-               _4q2 = 4.0f * q2;\r
-               _8q1 = 8.0f * q1;\r
-               _8q2 = 8.0f * q2;\r
-               q0q0 = q0 * q0;\r
-               q1q1 = q1 * q1;\r
-               q2q2 = q2 * q2;\r
-               q3q3 = q3 * q3;\r
-               */\r
-\r
-               f__2q0 = f32_mul(f32_from_double(2.0f), f_q0);\r
-               f__2q1 = f32_mul(f32_from_double(2.0f), f_q1);\r
-               f__2q2 = f32_mul(f32_from_double(2.0f), f_q2);\r
-               f__2q3 = f32_mul(f32_from_double(2.0f), f_q3);\r
-               f__4q0 = f32_mul(f32_from_double(4.0f), f_q0);\r
-               f__4q1 = f32_mul(f32_from_double(4.0f), f_q1);\r
-               f__4q2 = f32_mul(f32_from_double(4.0f), f_q2);\r
-               f__8q1 = f32_mul(f32_from_double(8.0f), f_q1);\r
-               f__8q2 = f32_mul(f32_from_double(8.0f), f_q2);\r
-               f_q0q0 = f32_mul(f_q0, f_q0);\r
-               f_q1q1 = f32_mul(f_q1, f_q1);\r
-               f_q2q2 = f32_mul(f_q2, f_q2);\r
-               f_q3q3 = f32_mul(f_q3, f_q3);\r
-\r
-\r
-               // Gradient decent algorithm corrective step\r
-\r
-               /*\r
-               s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;\r
-               s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;\r
-               s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;\r
-               s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;\r
-               recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude\r
-               */\r
-\r
-               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));\r
-               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))));\r
-               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))));\r
-               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));\r
-               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))))));\r
-\r
-               /*\r
-               s0 *= recipNorm;\r
-               s1 *= recipNorm;\r
-               s2 *= recipNorm;\r
-               s3 *= recipNorm;\r
-               */\r
-\r
-               f_s0 = f32_mul(f_s0, f_recipNorm);\r
-               f_s1 = f32_mul(f_s1, f_recipNorm);\r
-               f_s2 = f32_mul(f_s2, f_recipNorm);\r
-               f_s3 = f32_mul(f_s3, f_recipNorm);\r
-\r
-\r
-               // Apply feedback step\r
-\r
-               /*\r
-               qDot1 -= beta * s0;\r
-               qDot2 -= beta * s1;\r
-               qDot3 -= beta * s2;\r
-               qDot4 -= beta * s3;\r
-               */\r
-\r
-               f_qDot1 = f32_sub(f_qDot1, f32_mul(f_beta, f_s0));\r
-               f_qDot2 = f32_sub(f_qDot2, f32_mul(f_beta, f_s1));\r
-               f_qDot3 = f32_sub(f_qDot3, f32_mul(f_beta, f_s2));\r
-               f_qDot4 = f32_sub(f_qDot4, f32_mul(f_beta, f_s3));\r
-\r
-       }\r
-\r
-       // Integrate rate of change of quaternion to yield quaternion\r
-\r
-       /*\r
-       q0 += qDot1 * (1.0f / sampleFreq);\r
-       q1 += qDot2 * (1.0f / sampleFreq);\r
-       q2 += qDot3 * (1.0f / sampleFreq);\r
-       q3 += qDot4 * (1.0f / sampleFreq);\r
-       */\r
-\r
-       f_q0 = f32_add(f_q0, f32_mul(f_qDot1, f32_inv(f_sampleFreq)));\r
-       f_q1 = f32_add(f_q1, f32_mul(f_qDot2, f32_inv(f_sampleFreq)));\r
-       f_q2 = f32_add(f_q2, f32_mul(f_qDot3, f32_inv(f_sampleFreq)));\r
-       f_q3 = f32_add(f_q3, f32_mul(f_qDot4, f32_inv(f_sampleFreq)));\r
-\r
-       // Normalise quaternion\r
-       //recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);\r
-\r
-       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))))));\r
-\r
-       /*\r
-       q0 *= recipNorm;\r
-       q1 *= recipNorm;\r
-       q2 *= recipNorm;\r
-       q3 *= recipNorm;\r
-       */\r
-\r
-       f_q0 = f32_mul(f_q0, f_recipNorm);\r
-       f_q1 = f32_mul(f_q1, f_recipNorm);\r
-       f_q2 = f32_mul(f_q2, f_recipNorm);\r
-       f_q3 = f32_mul(f_q3, f_recipNorm);\r
-\r
-\r
-       q0 = f32_to_double(f_q0);\r
-       q1 = f32_to_double(f_q1);\r
-       q2 = f32_to_double(f_q2);\r
-       q3 = f32_to_double(f_q3);\r
-\r
-       //printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", q0, q1, q2);\r
-\r
-}\r
-\r
-\r
 //---------------------------------------------------------------------------------------------------\r
 // Fast inverse square-root\r
 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root\r

diff --git a/MadgwickAHRS.h b/MadgwickAHRS.h
index 7e42291..18e19fa 100644 (file)
--- a/MadgwickAHRS.h
+++ b/MadgwickAHRS.h
@@ -24,7 +24,7 @@ extern volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to
 \r
 void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);\r
 void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);\r
-void Mad_f32_init();\r
+void Mad_f32_init(void);\r
 void MadgwickAHRSupdateIMU_f32(float gx, float gy, float gz, float ax, float ay, float az);\r
 \r
 #endif\r