fix sa

Author: finger563

components/filters/include/madgwick_filter.hpp

@@ -37,20 +37,16 @@ class MadgwickFilter {
   ///       fallback to using only accelerometer and gyroscope values
   void update(float dt, float ax, float ay, float az, float gx, float gy, float gz, float mx,
               float my, float mz) {
-    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)) {
       update(dt, ax, ay, az, gx, gy, gz);
       return;
     }
 
+    float recipNorm;
+    float qDot1, qDot2, qDot3, qDot4;
+
     // 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);
@@ -60,6 +56,10 @@ class MadgwickFilter {
     // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer
     // normalisation)
     if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+      float s0, s1, s2, s3;
+      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;
 
       // Normalise accelerometer measurement
       recipNorm = fast_inv_sqrt(ax * ax + ay * ay + az * az);
@@ -164,9 +164,7 @@ class MadgwickFilter {
   /// @note Gyroscope values should be in rad/s
   void update(float dt, float ax, float ay, float az, float gx, float gy, float gz) {
     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;
 
     // Rate of change of quaternion from gyroscope
     qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
@@ -177,6 +175,8 @@ class MadgwickFilter {
     // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer
     // normalisation)
     if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+      float s0, s1, s2, s3;
+      float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
 
       // Normalise accelerometer measurement
       recipNorm = fast_inv_sqrt(ax * ax + ay * ay + az * az);