Improved gyro initialisation (#456)

* Improved gyro initialisation
* Fixed acc filter initialisation. Improved detection routines
* Fixed biquad filter initialisation
* Changed gyroSoftLpfHz from int8_t to uint8_t

src/main/blackbox/blackbox.c

@@ -379,9 +379,6 @@ extern uint32_t currentTime;
 //From rx.c:
 extern uint16_t rssi;
 
-//From gyro.c
-extern uint32_t targetLooptime;
-
 static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED;
 
 static uint32_t blackboxLastArmingBeep = 0;
@@ -1303,7 +1300,7 @@ static bool blackboxWriteSysinfo()
             }
             );
 
-        BLACKBOX_PRINT_HEADER_LINE("looptime:%d",                           targetLooptime);
+        BLACKBOX_PRINT_HEADER_LINE("looptime:%d",                           gyro.targetLooptime);
         BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d",                             masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcExpo8);
         BLACKBOX_PRINT_HEADER_LINE("rcYawExpo:%d",                          masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcYawExpo8);
         BLACKBOX_PRINT_HEADER_LINE("thrMid:%d",                             masterConfig.controlRateProfiles[masterConfig.current_profile_index].thrMid8);

src/main/blackbox/blackbox_io.c

@@ -618,7 +618,7 @@ bool blackboxDeviceOpen(void)
                  *                              = floor((looptime_ns * 3) / 500.0)
                  *                              = (looptime_ns * 3) / 500
                  */
-                blackboxMaxHeaderBytesPerIteration = constrain((targetLooptime * 3) / 500, 1, BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION);
+                blackboxMaxHeaderBytesPerIteration = constrain((gyro.targetLooptime * 3) / 500, 1, BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION);
 
                 return blackboxPort != NULL;
             }

src/main/common/filter.c

@@ -29,23 +29,17 @@
 #define BIQUAD_Q    (1.0f / 1.41421356f)     /* quality factor - butterworth (1 / sqrt(2)) */
 #define M_PI_FLOAT  3.14159265358979323846f
 
-/* sets up a biquad Filter */
+void biquadFilterInit(biquadFilter_t *filter, uint32_t filterCutFreq, uint32_t refreshRate)
-void biquadFilterInit(biquadFilter_t *newState, uint8_t filterCutFreq, int16_t samplingRate)
 {
-    float omega, sn, cs, alpha;
+    // setup variables
+    const float sampleRate = 1.0f / ((float)refreshRate * 0.000001f);
+    const float omega = 2 * M_PIf * ((float)filterCutFreq) / sampleRate;
+    const float sn = sin_approx(omega);
+    const float cs = cos_approx(omega);
+    const float alpha = sn / (2 * BIQUAD_Q);
+
     float a0, a1, a2, b0, b1, b2;
 
-    /* If sampling rate == 0 - use main loop target rate */
-    if (!samplingRate) {
-        samplingRate = 1000000 / targetLooptime;
-    }
-
-    /* setup variables */
-    omega = 2 * M_PIf * (float)filterCutFreq / (float)samplingRate;
-    sn = sin_approx(omega);
-    cs = cos_approx(omega);
-    alpha = sn / (2 * BIQUAD_Q);
-
     b0 = (1 - cs) / 2;
     b1 = 1 - cs;
     b2 = (1 - cs) / 2;
@@ -53,26 +47,23 @@ void biquadFilterInit(biquadFilter_t *newState, uint8_t filterCutFreq, int16_t s
     a1 = -2 * cs;
     a2 = 1 - alpha;
 
-    /* precompute the coefficients */
+    // precompute the coefficients
-    newState->b0 = b0 / a0;
+    filter->b0 = b0 / a0;
-    newState->b1 = b1 / a0;
+    filter->b1 = b1 / a0;
-    newState->b2 = b2 / a0;
+    filter->b2 = b2 / a0;
-    newState->a1 = a1 / a0;
+    filter->a1 = a1 / a0;
-    newState->a2 = a2 / a0;
+    filter->a2 = a2 / a0;
 
-    /* zero initial samples */
+    // zero initial samples
-    newState->d1 = newState->d2 = 1;
+    filter->d1 = filter->d2 = 1;
 }
 
 /* Computes a biquad_t filter on a sample */
-float biquadFilterApply(biquadFilter_t *state, float sample)
+float biquadFilterApply(biquadFilter_t *filter, float input)
 {
-    float result;
+    const float result = filter->b0 * input + filter->d1;
-
+    filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2;
-    result = state->b0 * sample + state->d1;
+    filter->d2 = filter->b2 * input - filter->a2 * result;
-    state->d1 = state->b1 * sample - state->a1 * result + state->d2;
-    state->d2 = state->b2 * sample - state->a2 * result;
-
     return result;
 }
 

src/main/common/filter.h

@@ -47,7 +47,7 @@ void pt1FilterReset(pt1Filter_t *filter, float input);
 void rateLimitFilterInit(rateLimitFilter_t *filter);
 float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_limit, float dT);
 
-void biquadFilterInit(biquadFilter_t *filter, uint8_t filterCutFreq, int16_t samplingRate);
+void biquadFilterInit(biquadFilter_t *filter, uint32_t filterCutFreq, uint32_t refreshRate);
 float biquadFilterApply(biquadFilter_t *filter, float sample);
 
 void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs);

src/main/config/config.c

@@ -801,7 +801,7 @@ void activateConfig(void)
         &currentProfile->pidProfile
     );
 
-    useGyroConfig(&masterConfig.gyroConfig, currentProfile->pidProfile.gyro_soft_lpf_hz);
+    gyroUseConfig(&masterConfig.gyroConfig, currentProfile->pidProfile.gyro_soft_lpf_hz);
 
 #ifdef TELEMETRY
     telemetryUseConfig(&masterConfig.telemetryConfig);

src/main/drivers/accgyro.h

@@ -27,6 +27,7 @@ typedef struct gyro_s {
     sensorReadFuncPtr temperature;                          // read temperature if available
     sensorInterruptFuncPtr intStatus;
     float scale;                                            // scalefactor
+    uint32_t targetLooptime;
 } gyro_t;
 
 typedef struct acc_s {

src/main/drivers/gyro_sync.c

@@ -35,7 +35,6 @@
 
 extern gyro_t gyro;
 
-uint32_t targetLooptime;
 static uint8_t mpuDividerDrops;
 
 bool getMpuDataStatus(gyro_t *gyro)
@@ -48,7 +47,7 @@ bool gyroSyncCheckUpdate(void)
     return getMpuDataStatus(&gyro);
 }
 
-void gyroSetSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator)
+uint32_t gyroSetSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator)
 {
     if (gyroSync) {
         int gyroSamplePeriod;
@@ -60,11 +59,11 @@ void gyroSetSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t
         }
 
         mpuDividerDrops  = gyroSyncDenominator - 1;
-        targetLooptime = gyroSyncDenominator * gyroSamplePeriod;
+        looptime = gyroSyncDenominator * gyroSamplePeriod;
     } else {
         mpuDividerDrops = 0;
-        targetLooptime = looptime;
     }
+    return looptime;
 }
 
 uint8_t gyroMPU6xxxCalculateDivider(void)

src/main/drivers/gyro_sync.h

@@ -17,8 +17,6 @@
 
 #define INTERRUPT_WAIT_TIME 10
 
-extern uint32_t targetLooptime;
-
 bool gyroSyncCheckUpdate(void);
 uint8_t gyroMPU6xxxCalculateDivider(void);
-void gyroSetSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator);
+uint32_t gyroSetSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator);

src/main/fc/mw.c

@@ -498,7 +498,7 @@ void filterRc(bool isRXDataNew)
 
     // Calculate average cycle time (1Hz LPF on cycle time)
     if (!filterInitialised) {
-        biquadFilterInit(&filteredCycleTimeState, 1, 0);
+        biquadFilterInit(&filteredCycleTimeState, 1, gyro.targetLooptime);
         filterInitialised = true;
     }
 
@@ -641,7 +641,7 @@ void taskMainPidLoopChecker(void) {
 
     if (masterConfig.gyroSync) {
         while (1) {
-            if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - currentTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
+            if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - currentTime)) >= (gyro.targetLooptime + GYRO_WATCHDOG_DELAY))) {
                 break;
             }
         }

src/main/flight/mixer.c

@@ -47,6 +47,7 @@
 
 #include "sensors/sensors.h"
 #include "sensors/acceleration.h"
+#include "sensors/gyro.h"
 
 #include "flight/mixer.h"
 #include "flight/failsafe.h"
@@ -922,7 +923,7 @@ void filterServos(void)
         // Initialize servo lowpass filter (servos are calculated at looptime rate)
         if (!servoFilterIsSet) {
             for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
-                biquadFilterInit(&servoFitlerState[servoIdx], mixerConfig->servo_lowpass_freq, 0);
+                biquadFilterInit(&servoFitlerState[servoIdx], mixerConfig->servo_lowpass_freq, gyro.targetLooptime);
             }
 
             servoFilterIsSet = true;

src/main/main.c

@@ -422,15 +422,15 @@ void init(void)
     }
 #endif
 
-    // Set gyro sampling rate divider before initialization
-    gyroSetSampleRate(masterConfig.looptime, masterConfig.gyro_lpf, masterConfig.gyroSync, masterConfig.gyroSyncDenominator);
-
     if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
-            masterConfig.gyro_lpf,
             masterConfig.acc_hardware,
             masterConfig.mag_hardware,
             masterConfig.baro_hardware,
-            currentProfile->mag_declination)) {
+            currentProfile->mag_declination,
+            masterConfig.looptime,
+            masterConfig.gyro_lpf,
+            masterConfig.gyroSync,
+            masterConfig.gyroSyncDenominator)) {
 
         // if gyro was not detected due to whatever reason, we give up now.
         failureMode(FAILURE_MISSING_ACC);
@@ -585,7 +585,7 @@ int main(void)
     /* Setup scheduler */
     schedulerInit();
 
-    rescheduleTask(TASK_GYROPID, targetLooptime);
+    rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
     setTaskEnabled(TASK_GYROPID, true);
 
     setTaskEnabled(TASK_SERIAL, true);

src/main/sensors/acceleration.c

@@ -42,6 +42,7 @@
 acc_t acc;                       // acc access functions
 int32_t accADC[XYZ_AXIS_COUNT];
 sensor_align_e accAlign = 0;
+uint32_t accTargetLooptime;
 
 static uint16_t calibratingA = 0;      // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
 static int16_t accADCRaw[XYZ_AXIS_COUNT];
@@ -49,9 +50,18 @@ static int16_t accADCRaw[XYZ_AXIS_COUNT];
 static flightDynamicsTrims_t * accZero;
 static flightDynamicsTrims_t * accGain;
 
-static int8_t accLpfCutHz = 0;
+static uint8_t accLpfCutHz = 0;
-static biquadFilter_t accFilterState[XYZ_AXIS_COUNT];
+static biquadFilter_t accFilter[XYZ_AXIS_COUNT];
-static bool accFilterInitialised = false;
+
+void accInit(uint32_t targetLooptime)
+{
+    accTargetLooptime = targetLooptime;
+    if (accLpfCutHz) {
+        for (int axis = 0; axis < 3; axis++) {
+            biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime);
+        }
+    }
+}
 
 void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
 {
@@ -166,7 +176,7 @@ void performAcclerationCalibration(void)
     calibratingA--;
 }
 
-void applyAccelerationZero(flightDynamicsTrims_t * accZero, flightDynamicsTrims_t * accGain)
+static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const flightDynamicsTrims_t * accGain)
 {
     accADC[X] = (accADC[X] - accZero->raw[X]) * accGain->raw[X] / 4096;
     accADC[Y] = (accADC[Y] - accZero->raw[Y]) * accGain->raw[Y] / 4096;
@@ -179,23 +189,13 @@ void updateAccelerationReadings(void)
         return;
     }
 
-    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) accADC[axis] = accADCRaw[axis];
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        accADC[axis] = accADCRaw[axis];
+    }
 
     if (accLpfCutHz) {
-        if (!accFilterInitialised) {
+        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            if (targetLooptime) {  /* Initialisation needs to happen once sample rate is known */
+            accADC[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float) accADC[axis]));
-                for (int axis = 0; axis < 3; axis++) {
-                    biquadFilterInit(&accFilterState[axis], accLpfCutHz, 0);
-                }
-
-                accFilterInitialised = true;
-            }
-        }
-
-        if (accFilterInitialised) {
-            for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-                accADC[axis] = lrintf(biquadFilterApply(&accFilterState[axis], (float) accADC[axis]));
-            }
         }
     }
 
@@ -218,7 +218,12 @@ void setAccelerationGain(flightDynamicsTrims_t * accGainToUse)
     accGain = accGainToUse;
 }
 
-void setAccelerationFilter(int8_t initialAccLpfCutHz)
+void setAccelerationFilter(uint8_t initialAccLpfCutHz)
 {
     accLpfCutHz = initialAccLpfCutHz;
+    if (accTargetLooptime) {
+        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime);
+        }
+    }
 }

src/main/sensors/acceleration.h

@@ -37,9 +37,11 @@ extern acc_t acc;
 
 extern int32_t accADC[XYZ_AXIS_COUNT];
 
+void accInit(uint32_t accTargetLooptime);
 bool isAccelerationCalibrationComplete(void);
 void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
 void updateAccelerationReadings(void);
-void setAccelerationZero(flightDynamicsTrims_t * accZeroToUse);
+union flightDynamicsTrims_u;
-void setAccelerationGain(flightDynamicsTrims_t * accGainToUse);
+void setAccelerationZero(union flightDynamicsTrims_u * accZeroToUse);
-void setAccelerationFilter(int8_t initialAccLpfCutHz);
+void setAccelerationGain(union flightDynamicsTrims_u * accGainToUse);
+void setAccelerationFilter(uint8_t initialAccLpfCutHz);

src/main/sensors/boardalignment.c

@@ -30,12 +30,12 @@
 static bool standardBoardAlignment = true;     // board orientation correction
 static float boardRotation[3][3];              // matrix
 
-static bool isBoardAlignmentStandard(boardAlignment_t *boardAlignment)
+static bool isBoardAlignmentStandard(const boardAlignment_t *boardAlignment)
 {
     return !boardAlignment->rollDeciDegrees && !boardAlignment->pitchDeciDegrees && !boardAlignment->yawDeciDegrees;
 }
 
-void initBoardAlignment(boardAlignment_t *boardAlignment)
+void initBoardAlignment(const boardAlignment_t *boardAlignment)
 {
     if (isBoardAlignmentStandard(boardAlignment)) {
         standardBoardAlignment = true;
@@ -75,7 +75,7 @@ static void alignBoard(int32_t *vec)
     vec[Z] = lrintf(boardRotation[0][Z] * x + boardRotation[1][Z] * y + boardRotation[2][Z] * z);
 }
 
-void alignSensors(int32_t *src, int32_t *dest, uint8_t rotation)
+void alignSensors(const int32_t *src, int32_t *dest, uint8_t rotation)
 {
     static uint32_t swap[3];
     memcpy(swap, src, sizeof(swap));

src/main/sensors/boardalignment.h

@@ -23,6 +23,6 @@ typedef struct boardAlignment_s {
     int16_t yawDeciDegrees;
 } boardAlignment_t;
 
-void alignSensors(int32_t *src, int32_t *dest, uint8_t rotation);
+void alignSensors(const int32_t *src, int32_t *dest, uint8_t rotation);
-void initBoardAlignment(boardAlignment_t *boardAlignment);
+void initBoardAlignment(const boardAlignment_t *boardAlignment);
 void updateBoardAlignment(boardAlignment_t *boardAlignment, int16_t roll, int16_t pitch);

src/main/sensors/gyro.c

@@ -41,20 +41,27 @@ sensor_align_e gyroAlign = 0;
 
 int32_t gyroADC[XYZ_AXIS_COUNT];
 
-static gyroConfig_t *gyroConfig;
+static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
+static const gyroConfig_t *gyroConfig;
 
 static uint16_t calibratingG = 0;
-static int16_t gyroADCRaw[XYZ_AXIS_COUNT];
-static int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
 
-static int8_t gyroLpfCutHz = 0;
+static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
-static biquadFilter_t gyroFilterState[XYZ_AXIS_COUNT];
+static uint8_t gyroSoftLpfHz = 0;
-static bool gyroFilterInitialised = false;
 
-void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t initialGyroLpfCutHz)
+void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz)
 {
     gyroConfig = gyroConfigToUse;
-    gyroLpfCutHz = initialGyroLpfCutHz;
+    gyroSoftLpfHz = gyro_soft_lpf_hz;
+}
+
+void gyroInit(void)
+{
+    if (gyroSoftLpfHz && gyro.targetLooptime) {  // Initialisation needs to happen once samplingrate is known
+        for (int axis = 0; axis < 3; axis++) {
+            biquadFilterInit(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
+        }
+    }
 }
 
 void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
@@ -125,29 +132,21 @@ static void applyGyroZero(void)
 
 void gyroUpdate(void)
 {
+    int16_t gyroADCRaw[XYZ_AXIS_COUNT];
+
     // range: +/- 8192; +/- 2000 deg/sec
     if (!gyro.read(gyroADCRaw)) {
         return;
     }
 
     // Prepare a copy of int32_t gyroADC for mangling to prevent overflow
-    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = gyroADCRaw[axis];
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        gyroADC[axis] = gyroADCRaw[axis];
+    }
 
-    if (gyroLpfCutHz) {
+    if (gyroSoftLpfHz) {
-        if (!gyroFilterInitialised) {
+        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            if (targetLooptime) {  /* Initialisation needs to happen once sample rate is known */
+            gyroADC[axis] = lrintf(biquadFilterApply(&gyroFilterLPF[axis], (float) gyroADC[axis]));
-                for (int axis = 0; axis < 3; axis++) {
-                    biquadFilterInit(&gyroFilterState[axis], gyroLpfCutHz, 0);
-                }
-
-                gyroFilterInitialised = true;
-            }
-        }
-
-        if (gyroFilterInitialised) {
-            for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-                gyroADC[axis] = lrintf(biquadFilterApply(&gyroFilterState[axis], (float) gyroADC[axis]));
-            }
         }
     }
 

src/main/sensors/gyro.h

@@ -32,7 +32,6 @@ typedef enum {
 } gyroSensor_e;
 
 extern gyro_t gyro;
-extern sensor_align_e gyroAlign;
 
 extern int32_t gyroADC[XYZ_AXIS_COUNT];
 
@@ -40,7 +39,9 @@ typedef struct gyroConfig_s {
     uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
 } gyroConfig_t;
 
-void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t initialGyroLpfCutHz);
+
+void gyroInit(void);
+void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t initialGyroLpfCutHz);
 void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
 void gyroUpdate(void);
 bool isGyroCalibrationComplete(void);

src/main/sensors/initialisation.c

@@ -82,6 +82,7 @@
 
 extern baro_t baro;
 extern mag_t mag;
+extern sensor_align_e gyroAlign;
 
 uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE };
 
@@ -702,12 +703,15 @@ static void reconfigureAlignment(const sensorAlignmentConfig_t *sensorAlignmentC
 }
 
 bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
-        uint8_t gyroLpf,
         uint8_t accHardwareToUse,
         uint8_t magHardwareToUse,
         uint8_t baroHardwareToUse,
-        int16_t magDeclinationFromConfig) {
+        int16_t magDeclinationFromConfig,
-
+        uint32_t looptime,
+        uint8_t gyroLpf,
+        uint8_t gyroSync,
+        uint8_t gyroSyncDenominator)
+{
     memset(&acc, 0, sizeof(acc));
     memset(&gyro, 0, sizeof(gyro));
 
@@ -719,11 +723,15 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
     if (!detectGyro()) {
         return false;
     }
-    gyro.init(gyroLpf);
+    // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
+    gyro.targetLooptime = gyroSetSampleRate(looptime, gyroLpf, gyroSync, gyroSyncDenominator);    // Set gyro sample rate before initialisation
+    gyro.init(gyroLpf); // driver initialisation
+    gyroInit(); // sensor initialisation
 
     if (detectAcc(accHardwareToUse)) {
         acc.acc_1G = 256; // set default
         acc.init(&acc);
+        accInit(gyro.targetLooptime); // acc and gyro updated at same frequency in taskMainPidLoop in mw.c
     }
 
 #ifdef BARO
@@ -746,12 +754,11 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
     UNUSED(magDeclinationFromConfig);
 #endif
 
-    reconfigureAlignment(sensorAlignmentConfig);
-
 #ifdef SONAR
     const rangefinderType_e rangefinderType = detectRangefinder();
     rangefinderInit(rangefinderType);
 #endif
+    reconfigureAlignment(sensorAlignmentConfig);
 
     return true;
 }

src/main/sensors/initialisation.h

@@ -17,6 +17,7 @@
 
 #pragma once
 
-bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t gyroLpf,
+bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
         uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse,
-        int16_t magDeclinationFromConfig);
+        int16_t magDeclinationFromConfig,
+        uint32_t looptime, uint8_t gyroLpf, uint8_t gyroSync, uint8_t gyroSyncDenominator);