Diff: MadgwickFilter.hpp

Revision:

6:eff5ebc4ea13

Parent:

5:1e6fecaea25e

Child:

7:c20656d96585

--- a/MadgwickFilter.hpp	Sat Dec 16 02:41:00 2017 +0000
+++ b/MadgwickFilter.hpp	Sun Feb 25 19:45:22 2018 +0000
@@ -18,7 +18,7 @@
         @param  B   double型, この値を大きくすると重力の影響を大きく取るようになります．
         @note   引数無しの場合，B = 0.1fが代入されます．
     */
-    MadgwickFilter(double B = BETA_DEF);
+    MadgwickFilter(float B = BETA_DEF);
     
 public:
     /**
@@ -29,7 +29,7 @@
         @note   角速度は[rad]にしてください．この関数は出来るだけ高速に呼び出し続けた方が良いと思います．
         @note   外部でローパスフィルタなどをかけることをお勧めします．
     */
-    void MadgwickAHRSupdate(double gx, double gy, double gz, double ax, double ay, double az, double mx, double my, double mz);
+    void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
     
     /**
         @bref   MadgwickFilterを角速度と加速度のみで動かし，姿勢計算を更新します．
@@ -37,7 +37,7 @@
         @param  ax,ay,az    加速度データ, 特に規格化は必要ありません
         @note   通常の関数でも，地磁気成分を0.0にすればこの関数が呼ばれます．
     */
-    void MadgwickAHRSupdateIMU(double gx, double gy, double gz, double ax, double ay, double az);
+    void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);
     
     /**
         @bref   姿勢を四元数で取得します．
@@ -54,22 +54,22 @@
         @param  _q3 虚部k, double型, アドレス
         @note   unityに入れる際は軸方向を修正してください．
     */
-    void getAttitude(double *_q0, double *_q1, double *_q2, double *_q3);
+    void getAttitude(float *_q0, float *_q1, float *_q2, float *_q3);
     
     /**
     @bref   オイラー角で姿勢を取得します．
     @param  val ロール，ピッチ，ヨーの順に配列に格納します．３つ以上の要素の配列を入れてください．
     @note   値は[rad]です．[degree]に変換が必要な場合は別途計算して下さい．
     */
-    void getEulerAngle(double *val);
+    void getEulerAngle(float *val);
 public:
     Timer madgwickTimer;
     Quaternion q;
-    double q0,q1,q2,q3;
-    double beta;  
+    float q0,q1,q2,q3;
+    float beta;  
 };
 
-MadgwickFilter::MadgwickFilter(double B){
+MadgwickFilter::MadgwickFilter(float B){
     q.w = 1.0f;
     q.x = 0.0f;
     q.y = 0.0f;
@@ -89,7 +89,7 @@
 
 
 
-void MadgwickFilter::getAttitude(double *_q0, double *_q1, double *_q2, double *_q3){
+void MadgwickFilter::getAttitude(float *_q0, float *_q1, float *_q2, float *_q3){
     *_q0 = q0;
     *_q1 = q1;
     *_q2 = q2;
@@ -98,8 +98,8 @@
 }
 
 
-void MadgwickFilter::getEulerAngle(double *val){
-    double q0q0 = q0 * q0, q1q1q2q2 = q1 * q1 - q2 * q2, q3q3 = q3 * q3;
+void MadgwickFilter::getEulerAngle(float *val){
+    float q0q0 = q0 * q0, q1q1q2q2 = q1 * q1 - q2 * q2, q3q3 = q3 * q3;
     val[0] = (atan2(2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1q2q2 + q3q3));
     val[1] = (-asin(2.0f * (q1 * q3 - q0 * q2)));
     val[2] = (atan2(2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1q2q2 - q3q3));
@@ -107,16 +107,16 @@
 //---------------------------------------------------------------------------------------------------
 // AHRS algorithm update
 
-inline void MadgwickFilter::MadgwickAHRSupdate(double gx, double gy, double gz, double ax, double ay, double az, double mx, double my, double mz) {
+inline void MadgwickFilter::MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
     
-    double acc_norm;
-    static double deltaT = 0;
+    static float acc_norm;
+    static float deltaT = 0.0f;
     static unsigned int newTime = 0, oldTime = 0;
-    double recipNorm;
-    double s0, s1, s2, s3;
-    double qDot1, qDot2, qDot3, qDot4;
-    double hx, hy;
-    double _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
+    static float recipNorm;
+    static float s0, s1, s2, s3;
+    static float qDot1, qDot2, qDot3, qDot4;
+    static float hx, hy;
+    static 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)) {
@@ -135,13 +135,13 @@
 
         // Normalise accelerometer measurement
         acc_norm = sqrt(ax * ax + ay * ay + az * az);
-        recipNorm = 1.0 / acc_norm;
+        recipNorm = 1.0f / acc_norm;
         ax *= recipNorm;
         ay *= recipNorm;
         az *= recipNorm;   
 
         // Normalise magnetometer measurement
-        recipNorm = 1.0 / sqrt(mx * mx + my * my + mz * mz);
+        recipNorm = 1.0f / sqrt(mx * mx + my * my + mz * mz);
         mx *= recipNorm;
         my *= recipNorm;
         mz *= recipNorm;
@@ -181,18 +181,18 @@
         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 = 1.0 / sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
+        recipNorm = 1.0f / sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
         s0 *= recipNorm;
         s1 *= recipNorm;
         s2 *= recipNorm;
         s3 *= recipNorm;
 
-        double deltaA = fabs(acc_norm - 1.00);
+        float deltaA = fabs(acc_norm - 1.00f);
         //beta = 0.1*exp(-1.0*deltaA*deltaA);
         //beta = 0.3*exp(-20.0*deltaA*deltaA);
-        beta = beta*exp(-30.0*deltaA*deltaA);
+        beta = beta*exp(-30.0f*deltaA*deltaA);
         //printf("%f\r\n", beta);
-        beta = 1.0;
+        //beta = 1.0;
         //if(deltaA > 0.3)    beta = 0.0;
         // Apply feedback step
         qDot1 -= beta * s0;
@@ -203,7 +203,7 @@
 
     // Integrate rate of change of quaternion to yield quaternion
     newTime = (unsigned int)madgwickTimer.read_us();
-    deltaT = (newTime - oldTime) / 1000000.0;
+    deltaT = (newTime - oldTime) / 1000000.0f;
     deltaT = fabs(deltaT);
     oldTime = newTime;
     
@@ -213,7 +213,7 @@
     q3 += qDot4 * deltaT;//(1.0f / sampleFreq);
 
     // Normalise quaternion
-    recipNorm = 1.0 / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+    recipNorm = 1.0f / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
     q0 *= recipNorm;
     q1 *= recipNorm;
     q2 *= recipNorm;
@@ -228,14 +228,14 @@
 //---------------------------------------------------------------------------------------------------
 // IMU algorithm update
 
-inline void MadgwickFilter::MadgwickAHRSupdateIMU(double gx, double gy, double gz, double ax, double ay, double az) {
-    static double deltaT = 0;
+inline void MadgwickFilter::MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
+    static float deltaT = 0;
     static unsigned int newTime = 0, oldTime = 0;
-    double recipNorm;
-    double s0, s1, s2, s3;
-    double qDot1, qDot2, qDot3, qDot4;
-    double _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
-    double acc_norm;
+    static float recipNorm;
+    static float s0, s1, s2, s3;
+    static float qDot1, qDot2, qDot3, qDot4;
+    static float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
+    static float acc_norm;
 
     // Rate of change of quaternion from gyroscope
     qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
@@ -248,7 +248,7 @@
 
         // Normalise accelerometer measurement
         acc_norm = sqrt(ax * ax + ay * ay + az * az);
-        recipNorm = 1.0 / acc_norm;
+        recipNorm = 1.0f / acc_norm;
         ax *= recipNorm;
         ay *= recipNorm;
         az *= recipNorm;   
@@ -273,17 +273,18 @@
         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 = 1.0 / sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
+        recipNorm = 1.0f / sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
         s0 *= recipNorm;
         s1 *= recipNorm;
         s2 *= recipNorm;
         s3 *= recipNorm;
 
         // Apply feedback step
-        double deltaA = fabs(acc_norm - 1.00);
+        static float deltaA;
+        deltaA = fabs(acc_norm - 1.00f);
         //beta = 0.5*exp(-20.0*deltaA*deltaA);
-        if(deltaA > 0.3) beta = 0.0;
-        else    beta = 0.1;
+        if(deltaA > 0.3f) beta = 0.0f;
+        else    beta = 0.1f;
         qDot1 -= beta * s0;
         qDot2 -= beta * s1;
         qDot3 -= beta * s2;
@@ -292,7 +293,7 @@
 
     // Integrate rate of change of quaternion to yield quaternion
     newTime = (unsigned int)madgwickTimer.read_us();
-    deltaT = (newTime - oldTime) / 1000000.0;
+    deltaT = (newTime - oldTime) / 1000000.0f;
     deltaT = fabs(deltaT);
     oldTime = newTime;
     q0 += qDot1 * deltaT;;
@@ -301,7 +302,7 @@
     q3 += qDot4 * deltaT;;
 
     // Normalise quaternion
-    recipNorm = 1.0 / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+    recipNorm = 1.0f / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
     q0 *= recipNorm;
     q1 *= recipNorm;
     q2 *= recipNorm;