Tidy of IMU header

src/main/blackbox/blackbox.c

@@ -32,6 +32,7 @@
 
 #include "common/axis.h"
 #include "common/encoding.h"
+#include "common/maths.h"
 #include "common/utils.h"
 
 #include "config/config_profile.h"

src/main/blackbox/blackbox_io.c

@@ -15,6 +15,7 @@
 #include "build/build_config.h"
 
 #include "common/encoding.h"
+#include "common/maths.h"
 #include "common/printf.h"
 
 #include "config/config_profile.h"

src/main/config/config_master.h

@@ -37,6 +37,7 @@
 
 #include "fc/rc_adjustments.h"
 #include "fc/rc_controls.h"
+#include "fc/fc_core.h"
 
 #include "flight/failsafe.h"
 #include "flight/mixer.h"

src/main/fc/fc_core.h

@@ -18,11 +18,19 @@
 #pragma once
 
 #include "common/time.h"
+#include "config/parameter_group.h"
 
 extern int16_t magHold;
 extern bool isRXDataNew;
 extern int16_t headFreeModeHold;
 
+typedef struct throttleCorrectionConfig_s {
+    uint16_t throttle_correction_angle;     // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg
+    uint8_t throttle_correction_value;      // the correction that will be applied at throttle_correction_angle.
+} throttleCorrectionConfig_t;
+
+PG_DECLARE(throttleCorrectionConfig_t, throttleCorrectionConfig);
+
 union rollAndPitchTrims_u;
 void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta);
 void handleInflightCalibrationStickPosition();

src/main/flight/imu.c

@@ -21,40 +21,40 @@
 #include <stdint.h>
 #include <math.h>
 
-#include "common/maths.h"
-
 #include "platform.h"
 
 #include "build/build_config.h"
 #include "build/debug.h"
 
 #include "common/axis.h"
+#include "common/maths.h"
 
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"
 
 #include "drivers/system.h"
 
-#include "sensors/sensors.h"
+#include "fc/runtime_config.h"
-#include "sensors/gyro.h"
-#include "sensors/compass.h"
-#include "sensors/acceleration.h"
-#include "sensors/barometer.h"
-#include "sensors/sonar.h"
 
+#include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
-#include "flight/imu.h"
 
 #include "io/gps.h"
 
-#include "fc/runtime_config.h"
+#include "sensors/acceleration.h"
+#include "sensors/barometer.h"
+#include "sensors/compass.h"
+#include "sensors/gyro.h"
+#include "sensors/sensors.h"
+#include "sensors/sonar.h"
 
 
 // the limit (in degrees/second) beyond which we stop integrating
 // omega_I. At larger spin rates the DCM PI controller can get 'dizzy'
 // which results in false gyro drift. See
 // http://gentlenav.googlecode.com/files/fastRotations.pdf
+
 #define SPIN_RATE_LIMIT 20
 
 int32_t accSum[XYZ_AXIS_COUNT];
@@ -63,11 +63,11 @@ uint32_t accTimeSum = 0;        // keep track for integration of acc
 int accSumCount = 0;
 float accVelScale;
 
-float throttleAngleScale;
+static float throttleAngleScale;
-float fc_acc;
+static float fc_acc;
-float smallAngleCosZ = 0;
+static float smallAngleCosZ = 0;
 
-float magneticDeclination = 0.0f;       // calculated at startup from config
+static float magneticDeclination = 0.0f;       // calculated at startup from config
 
 static imuRuntimeConfig_t imuRuntimeConfig;
 static pidProfile_t *pidProfile;
@@ -103,6 +103,19 @@ STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
     rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
 }
 
+/*
+* Calculate RC time constant used in the accZ lpf.
+*/
+static float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
+{
+    return 0.5f / (M_PIf * accz_lpf_cutoff);
+}
+
+static float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
+{
+    return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
+}
+
 void imuConfigure(
     imuConfig_t *imuConfig,
     pidProfile_t *initialPidProfile,
@@ -127,19 +140,6 @@ void imuInit(void)
     imuComputeRotationMatrix();
 }
 
-float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
-{
-    return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
-}
-
-/*
-* Calculate RC time constant used in the accZ lpf.
-*/
-float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
-{
-    return 0.5f / (M_PIf * accz_lpf_cutoff);
-}
-
 void imuResetAccelerationSum(void)
 {
     accSum[0] = 0;
@@ -149,14 +149,12 @@ void imuResetAccelerationSum(void)
     accTimeSum = 0;
 }
 
-void imuTransformVectorBodyToEarth(t_fp_vector * v)
+static void imuTransformVectorBodyToEarth(t_fp_vector * v)
 {
-    float x,y,z;
-
     /* From body frame to earth frame */
-    x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z;
+    const float x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z;
-    y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z;
+    const float y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z;
-    z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z;
+    const float z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z;
 
     v->V.X = x;
     v->V.Y = -y;
@@ -164,16 +162,15 @@ void imuTransformVectorBodyToEarth(t_fp_vector * v)
 }
 
 // rotate acc into Earth frame and calculate acceleration in it
-void imuCalculateAcceleration(uint32_t deltaT)
+static void imuCalculateAcceleration(uint32_t deltaT)
 {
     static int32_t accZoffset = 0;
     static float accz_smooth = 0;
-    float dT;
-    t_fp_vector accel_ned;
 
     // deltaT is measured in us ticks
-    dT = (float)deltaT * 1e-6f;
+    const float dT = (float)deltaT * 1e-6f;
 
+    t_fp_vector accel_ned;
     accel_ned.V.X = acc.accSmooth[X];
     accel_ned.V.Y = acc.accSmooth[Y];
     accel_ned.V.Z = acc.accSmooth[Z];
@@ -186,8 +183,9 @@ void imuCalculateAcceleration(uint32_t deltaT)
             accZoffset += accel_ned.V.Z;
         }
         accel_ned.V.Z -= accZoffset / 64;  // compensate for gravitation on z-axis
-    } else
+    } else {
         accel_ned.V.Z -= acc.dev.acc_1G;
+    }
 
     accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
 
@@ -227,15 +225,12 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
                                 bool useYaw, float yawError)
 {
     static float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;    // integral error terms scaled by Ki
-    float recipNorm;
-    float hx, hy, bx;
-    float ex = 0, ey = 0, ez = 0;
-    float qa, qb, qc;
 
     // Calculate general spin rate (rad/s)
-    float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));
+    const float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));
 
     // Use raw heading error (from GPS or whatever else)
+    float ez = 0;
     if (useYaw) {
         while (yawError >  M_PIf) yawError -= (2.0f * M_PIf);
         while (yawError < -M_PIf) yawError += (2.0f * M_PIf);
@@ -244,7 +239,8 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
     }
 
     // Use measured magnetic field vector
-    recipNorm = sq(mx) + sq(my) + sq(mz);
+    float ex = 0, ey = 0;
+    float recipNorm = sq(mx) + sq(my) + sq(mz);
     if (useMag && recipNorm > 0.01f) {
         // Normalise magnetometer measurement
         recipNorm = invSqrt(recipNorm);
@@ -257,12 +253,12 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
 
         // (hx; hy; 0) - measured mag field vector in EF (assuming Z-component is zero)
         // (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
-        hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
+        const float hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
-        hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
+        const float hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
-        bx = sqrtf(hx * hx + hy * hy);
+        const float bx = sqrtf(hx * hx + hy * hy);
 
         // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
-        float ez_ef = -(hy * bx);
+        const float ez_ef = -(hy * bx);
 
         // Rotate mag error vector back to BF and accumulate
         ex += rMat[2][0] * ez_ef;
@@ -289,7 +285,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
     if(imuRuntimeConfig.dcm_ki > 0.0f) {
         // Stop integrating if spinning beyond the certain limit
         if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) {
-            float dcmKiGain = imuRuntimeConfig.dcm_ki;
+            const float dcmKiGain = imuRuntimeConfig.dcm_ki;
             integralFBx += dcmKiGain * ex * dt;    // integral error scaled by Ki
             integralFBy += dcmKiGain * ey * dt;
             integralFBz += dcmKiGain * ez * dt;
@@ -302,7 +298,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
     }
 
     // Calculate kP gain. If we are acquiring initial attitude (not armed and within 20 sec from powerup) scale the kP to converge faster
-    float dcmKpGain = imuRuntimeConfig.dcm_kp * imuGetPGainScaleFactor();
+    const float dcmKpGain = imuRuntimeConfig.dcm_kp * imuGetPGainScaleFactor();
 
     // Apply proportional and integral feedback
     gx += dcmKpGain * ex + integralFBx;
@@ -314,9 +310,9 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
     gy *= (0.5f * dt);
     gz *= (0.5f * dt);
 
-    qa = q0;
+    const float qa = q0;
-    qb = q1;
+    const float qb = q1;
-    qc = q2;
+    const float qc = q2;
     q0 += (-qb * gx - qc * gy - q3 * gz);
     q1 += (qa * gx + qc * gz - q3 * gy);
     q2 += (qa * gy - qb * gz + q3 * gx);
@@ -353,10 +349,8 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
 
 static bool imuIsAccelerometerHealthy(void)
 {
-    int32_t axis;
     int32_t accMagnitude = 0;
-
+    for (int axis = 0; axis < 3; axis++) {
-    for (axis = 0; axis < 3; axis++) {
         accMagnitude += (int32_t)acc.accSmooth[axis] * acc.accSmooth[axis];
     }
 

src/main/flight/imu.h

@@ -18,13 +18,10 @@
 #pragma once
 
 #include "common/axis.h"
-#include "common/maths.h"
 #include "common/time.h"
 
 #include "config/parameter_group.h"
 
-#include "sensors/acceleration.h"
-
 // Exported symbols
 extern uint32_t accTimeSum;
 extern int accSumCount;
@@ -49,13 +46,6 @@ typedef struct accDeadband_s {
     uint8_t z;                  // set the acc deadband for z-Axis, this ignores small accelerations
 } accDeadband_t;
 
-typedef struct throttleCorrectionConfig_s {
-    uint16_t throttle_correction_angle;     // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg
-    uint8_t throttle_correction_value;      // the correction that will be applied at throttle_correction_angle.
-} throttleCorrectionConfig_t;
-
-PG_DECLARE(throttleCorrectionConfig_t, throttleCorrectionConfig);
-
 typedef struct imuConfig_s {
     uint16_t dcm_kp;                        // DCM filter proportional gain ( x 10000)
     uint16_t dcm_ki;                        // DCM filter integral gain ( x 10000)
@@ -74,38 +64,13 @@ typedef struct imuRuntimeConfig_s {
     accDeadband_t accDeadband;
 } imuRuntimeConfig_t;
 
-typedef enum {
-    ACCPROC_READ = 0,
-    ACCPROC_CHUNK_1,
-    ACCPROC_CHUNK_2,
-    ACCPROC_CHUNK_3,
-    ACCPROC_CHUNK_4,
-    ACCPROC_CHUNK_5,
-    ACCPROC_CHUNK_6,
-    ACCPROC_CHUNK_7,
-    ACCPROC_COPY
-} accProcessorState_e;
-
-typedef struct accProcessor_s {
-    accProcessorState_e state;
-} accProcessor_t;
-
 struct pidProfile_s;
-void imuConfigure(
+void imuConfigure(imuConfig_t *imuConfig, struct pidProfile_s *initialPidProfile, uint16_t throttle_correction_angle);
-    imuConfig_t *imuConfig,
-    struct pidProfile_s *initialPidProfile,
-    uint16_t throttle_correction_angle
-);
 
 float getCosTiltAngle(void);
 void calculateEstimatedAltitude(timeUs_t currentTimeUs);
 void imuUpdateAttitude(timeUs_t currentTimeUs);
-float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
 int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
-float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_hz);
-
-union u_fp_vector;
-int16_t imuCalculateHeading(union u_fp_vector *vec);
 
 void imuResetAccelerationSum(void);
 void imuInit(void);

src/main/flight/servos.c

@@ -27,6 +27,7 @@
 #include "build/build_config.h"
 
 #include "common/filter.h"
+#include "common/maths.h"
 
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"

src/main/io/osd.c

@@ -38,6 +38,7 @@
 #include "build/version.h"
 
 #include "common/printf.h"
+#include "common/maths.h"
 #include "common/utils.h"
 
 #include "config/config_profile.h"

src/main/io/vtx.c

@@ -26,6 +26,8 @@
 #include "io/osd.h"
 
 //External dependencies
+#include "common/maths.h"
+
 #include "config/config_eeprom.h"
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"

src/main/sensors/battery.c

@@ -23,6 +23,7 @@
 #include "build/debug.h"
 
 #include "common/filter.h"
+#include "common/maths.h"
 #include "common/utils.h"
 
 #include "config/feature.h"

src/main/sensors/esc_sensor.c

@@ -21,12 +21,15 @@
 
 #include <platform.h>
 
+#ifdef USE_DSHOT
+
 #include "build/debug.h"
 
 #include "config/feature.h"
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"
 
+#include "common/maths.h"
 #include "common/utils.h"
 
 #include "drivers/pwm_output.h"
@@ -71,7 +74,6 @@ set debug_mode = DEBUG_ESC_TELEMETRY in cli
 
 */
 
-#ifdef USE_DSHOT
 enum {
     DEBUG_ESC_MOTOR_INDEX = 0,
     DEBUG_ESC_NUM_TIMEOUTS = 1,

src/main/target/COLIBRI_RACE/config.c

@@ -15,19 +15,21 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include <stdbool.h>
 #include <stdint.h>
 #include <string.h>
 
 #include <platform.h>
 
+#include "common/maths.h"
+
+#include "config/config_master.h"
+#include "config/feature.h"
+
+#include "io/ledstrip.h"
 #include "io/motors.h"
 
 #include "sensors/battery.h"
 
-#include "config/config_master.h"
-#include "config/feature.h"
-#include "io/ledstrip.h"
 
 void targetApplyDefaultLedStripConfig(ledConfig_t *ledConfigs)
 {

src/main/target/MULTIFLITEPICO/config.c

@@ -21,6 +21,7 @@
 #include <platform.h>
 
 #include "common/axis.h"
+#include "common/maths.h"
 
 #include "drivers/sensor.h"
 #include "drivers/compass.h"

src/main/telemetry/crsf.c

@@ -35,6 +35,7 @@
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"
 
+#include "common/maths.h"
 #include "common/streambuf.h"
 #include "common/utils.h"