Start decoupling imu from config.

src/main/config/config.c

@@ -48,6 +48,7 @@
 #include "io/rc_curves.h"
 #include "io/gps.h"
 #include "flight/failsafe.h"
+#include "flight/imu.h"
 
 #include "config/runtime_config.h"
 #include "config/config.h"
@@ -362,6 +363,8 @@ static bool isEEPROMContentValid(void)
 
 void activateConfig(void)
 {
+    static imuRuntimeConfig_t imuRuntimeConfig;
+
     generatePitchCurve(&currentProfile.controlRateConfig);
     generateThrottleCurve(&currentProfile.controlRateConfig, &masterConfig.escAndServoConfig);
 
@@ -382,6 +385,12 @@ void activateConfig(void)
             &currentProfile.gimbalConfig
             );
 
+    imuRuntimeConfig.gyro_cmpf_factor = masterConfig.gyro_cmpf_factor;
+    imuRuntimeConfig.gyro_cmpfm_factor = masterConfig.gyro_cmpfm_factor;
+    imuRuntimeConfig.acc_lpf_factor = currentProfile.acc_lpf_factor;
+
+    configureImu(&imuRuntimeConfig);
+
     calculateThrottleAngleScale(currentProfile.throttle_correction_angle);
 
 #ifdef BARO

src/main/flight/imu.c

@@ -57,6 +57,7 @@
 #include "config/config_profile.h"
 #include "config/config_master.h"
 
+#include "flight/imu.h"
 
 int16_t gyroADC[XYZ_AXIS_COUNT], accADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AXIS_COUNT];
 int32_t accSum[XYZ_AXIS_COUNT];
@@ -103,14 +104,21 @@ void calculateThrottleAngleScale(uint16_t throttle_correction_angle)
     throttleAngleScale = (1800.0f / M_PI) * (900.0f / throttle_correction_angle);
 }
 
-void computeIMU(void)
+imuRuntimeConfig_t *imuRuntimeConfig;
+
+void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig)
+{
+    imuRuntimeConfig = initialImuRuntimeConfig;
+}
+
+void computeIMU(rollAndPitchTrims_t *accelerometerTrims)
 {
     uint32_t axis;
     static int16_t gyroYawSmooth = 0;
 
     gyroGetADC();
     if (sensors(SENSOR_ACC)) {
-        updateAccelerationReadings(&currentProfile.accelerometerTrims);
+        updateAccelerationReadings(accelerometerTrims);
         getEstimatedAttitude();
     } else {
         accADC[X] = 0;
@@ -299,8 +307,8 @@ static void getEstimatedAttitude(void)
     // Initialization
     for (axis = 0; axis < 3; axis++) {
         deltaGyroAngle.raw[axis] = gyroADC[axis] * scale;
-        if (currentProfile.acc_lpf_factor > 0) {
+        if (imuRuntimeConfig->acc_lpf_factor > 0) {
-            accLPF[axis] = accLPF[axis] * (1.0f - (1.0f / currentProfile.acc_lpf_factor)) + accADC[axis] * (1.0f / currentProfile.acc_lpf_factor);
+            accLPF[axis] = accLPF[axis] * (1.0f - (1.0f / imuRuntimeConfig->acc_lpf_factor)) + accADC[axis] * (1.0f / imuRuntimeConfig->acc_lpf_factor);
             accSmooth[axis] = accLPF[axis];
         } else {
             accSmooth[axis] = accADC[axis];
@@ -321,19 +329,19 @@ static void getEstimatedAttitude(void)
     // If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
     // To do that, we just skip filter, as EstV already rotated by Gyro
 
-    float invGyroComplimentaryFilterFactor = (1.0f / ((float)masterConfig.gyro_cmpf_factor + 1.0f));
+    float invGyroComplimentaryFilterFactor = (1.0f / (imuRuntimeConfig->gyro_cmpf_factor + 1.0f));
 
     if (72 < (uint16_t)accMag && (uint16_t)accMag < 133) {
         for (axis = 0; axis < 3; axis++)
-            EstG.A[axis] = (EstG.A[axis] * (float)masterConfig.gyro_cmpf_factor + accSmooth[axis]) * invGyroComplimentaryFilterFactor;
+            EstG.A[axis] = (EstG.A[axis] * imuRuntimeConfig->gyro_cmpf_factor + accSmooth[axis]) * invGyroComplimentaryFilterFactor;
     }
 
     // FIXME what does the _M_ mean?
-    float invGyroComplimentaryFilter_M_Factor = (1.0f / ((float)masterConfig.gyro_cmpfm_factor + 1.0f));
+    float invGyroComplimentaryFilter_M_Factor = (1.0f / (imuRuntimeConfig->gyro_cmpfm_factor + 1.0f));
 
     if (sensors(SENSOR_MAG)) {
         for (axis = 0; axis < 3; axis++)
-            EstM.A[axis] = (EstM.A[axis] * (float)masterConfig.gyro_cmpfm_factor + magADC[axis]) * invGyroComplimentaryFilter_M_Factor;
+            EstM.A[axis] = (EstM.A[axis] * imuRuntimeConfig->gyro_cmpfm_factor + magADC[axis]) * invGyroComplimentaryFilter_M_Factor;
     }
 
     f.SMALL_ANGLE = (EstG.A[Z] > smallAngle);

src/main/flight/imu.h

@@ -21,7 +21,15 @@ extern int32_t errorAltitudeI;
 extern int32_t BaroPID;
 extern int16_t throttleAngleCorrection;
 
+typedef struct imuRuntimeConfig_s {
+    uint8_t acc_lpf_factor;
+    float gyro_cmpf_factor;
+    float gyro_cmpfm_factor;
+} imuRuntimeConfig_t;
+
+void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig);
+
 int getEstimatedAltitude(void);
-void computeIMU(void);
+void computeIMU(rollAndPitchTrims_t *accelerometerTrims);
 void calculateThrottleAngleScale(uint16_t throttle_correction_angle);
 int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);

src/main/mw.c

@@ -36,9 +36,9 @@
 #include "sensors/battery.h"
 #include "io/buzzer.h"
 #include "io/escservo.h"
+#include "flight/flight.h"
 #include "flight/failsafe.h"
 #include "flight/imu.h"
-#include "flight/flight.h"
 #include "flight/autotune.h"
 #include "flight/mixer.h"
 #include "io/gimbal.h"
@@ -535,7 +535,7 @@ void loop(void)
     if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
         loopTime = currentTime + masterConfig.looptime;
 
-        computeIMU();
+        computeIMU(&currentProfile.accelerometerTrims);
         annexCode();
         // Measure loop rate just afer reading the sensors
         currentTime = micros();