Tidied initialisation, especially PID filters

src/main/drivers/accgyro.h

@@ -23,6 +23,15 @@
 #define MPU_I2C_INSTANCE I2C_DEVICE
 #endif
 
+#define GYRO_LPF_256HZ      0
+#define GYRO_LPF_188HZ      1
+#define GYRO_LPF_98HZ       2
+#define GYRO_LPF_42HZ       3
+#define GYRO_LPF_20HZ       4
+#define GYRO_LPF_10HZ       5
+#define GYRO_LPF_5HZ        6
+#define GYRO_LPF_NONE       7
+
 typedef struct gyro_s {
     sensorGyroInitFuncPtr init;                             // initialize function
     sensorReadFuncPtr read;                                 // read 3 axis data function

src/main/drivers/gyro_sync.c

@@ -23,15 +23,6 @@ bool gyroSyncCheckUpdate(const gyro_t *gyro)
     return gyro->intStatus();
 }
 
-#define GYRO_LPF_256HZ      0
-#define GYRO_LPF_188HZ      1
-#define GYRO_LPF_98HZ       2
-#define GYRO_LPF_42HZ       3
-#define GYRO_LPF_20HZ       4
-#define GYRO_LPF_10HZ       5
-#define GYRO_LPF_5HZ        6
-#define GYRO_LPF_NONE       7
-
 uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator)
 {
     int gyroSamplePeriod;

src/main/fc/config.c

@@ -564,7 +564,7 @@ void createDefaultConfig(master_t *config)
     config->current_profile_index = 0;    // default profile
     config->dcm_kp = 2500;                // 1.0 * 10000
     config->dcm_ki = 0;                   // 0.003 * 10000
-    config->gyro_lpf = 0;                 // 256HZ default
+    config->gyro_lpf = GYRO_LPF_256HZ;    // 256HZ default
 #ifdef STM32F10X
     config->gyro_sync_denom = 8;
     config->pid_process_denom = 1;
@@ -830,13 +830,6 @@ void activateConfig(void)
         &currentProfile->pidProfile
     );
 
-    // Prevent invalid notch cutoff
-    if (masterConfig.gyro_soft_notch_cutoff_1 >= masterConfig.gyro_soft_notch_hz_1)
-        masterConfig.gyro_soft_notch_hz_1 = 0;
-
-    if (masterConfig.gyro_soft_notch_cutoff_2 >= masterConfig.gyro_soft_notch_hz_2)
-        masterConfig.gyro_soft_notch_hz_2 = 0;
-
     gyroUseConfig(&masterConfig.gyroConfig,
         masterConfig.gyro_soft_lpf_hz,
         masterConfig.gyro_soft_notch_hz_1,
@@ -998,12 +991,30 @@ void validateAndFixConfig(void)
     if (!isSerialConfigValid(serialConfig)) {
         resetSerialConfig(serialConfig);
     }
-    
+
+    validateAndFixGyroConfig();
+
 #if defined(TARGET_VALIDATECONFIG)
     targetValidateConfiguration(&masterConfig);
 #endif
 }
 
+void validateAndFixGyroConfig(void)
+{
+    // Prevent invalid notch cutoff
+    if (masterConfig.gyro_soft_notch_cutoff_1 >= masterConfig.gyro_soft_notch_hz_1) {
+        masterConfig.gyro_soft_notch_hz_1 = 0;
+    }
+    if (masterConfig.gyro_soft_notch_cutoff_2 >= masterConfig.gyro_soft_notch_hz_2) {
+        masterConfig.gyro_soft_notch_hz_2 = 0;
+    }
+
+    if (masterConfig.gyro_lpf != GYRO_LPF_256HZ && masterConfig.gyro_lpf != GYRO_LPF_NONE) {
+        masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
+        masterConfig.gyro_sync_denom = 1;
+    }
+}
+
 void readEEPROMAndNotify(void)
 {
     // re-read written data

src/main/fc/config.h

@@ -75,6 +75,7 @@ void ensureEEPROMContainsValidData(void);
 
 void saveConfigAndNotify(void);
 void validateAndFixConfig(void);
+void validateAndFixGyroConfig(void);
 void activateConfig(void);
 
 uint8_t getCurrentProfile(void);

src/main/fc/fc_msp.c

@@ -1458,6 +1458,18 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
             masterConfig.gyro_soft_notch_hz_2 = sbufReadU16(src);
             masterConfig.gyro_soft_notch_cutoff_2 = sbufReadU16(src);
         }
+        // reinitialize the gyro filters with the new values
+        validateAndFixGyroConfig();
+        gyroUseConfig(&masterConfig.gyroConfig,
+            masterConfig.gyro_soft_lpf_hz,
+            masterConfig.gyro_soft_notch_hz_1,
+            masterConfig.gyro_soft_notch_cutoff_1,
+            masterConfig.gyro_soft_notch_hz_2,
+            masterConfig.gyro_soft_notch_cutoff_2,
+            masterConfig.gyro_soft_type);
+        gyroInit();
+        // reinitialize the PID filters with the new values
+        pidInitFilters(&currentProfile->pidProfile);
         break;
 
     case MSP_SET_PID_ADVANCED:

src/main/flight/pid.c

@@ -56,7 +56,7 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
 
 static float errorGyroIf[3];
 
-void setTargetPidLooptime(uint32_t pidLooptime)
+void pidSetTargetLooptime(uint32_t pidLooptime)
 {
     targetPidLooptime = pidLooptime;
 }
@@ -82,7 +82,7 @@ static void *dtermFilterLpf[2];
 static filterApplyFnPtr ptermYawFilterApplyFn;
 static void *ptermYawFilter;
 
-static void pidInitFilters(const pidProfile_t *pidProfile)
+void pidInitFilters(const pidProfile_t *pidProfile)
 {
     static bool initialized = false; // !!TODO - remove this temporary measure once filter lazy initialization is removed
     static biquadFilter_t biquadFilterNotch[2];
@@ -166,8 +166,6 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
         dT = (float)targetPidLooptime * 0.000001f;
     }
 
-    pidInitFilters(pidProfile);
-
     float horizonLevelStrength = 1;
     if (FLIGHT_MODE(HORIZON_MODE)) {
         // Figure out the raw stick positions

src/main/flight/pid.h

@@ -104,5 +104,6 @@ extern uint8_t PIDweight[3];
 
 void pidResetErrorGyroState(void);
 void pidStabilisationState(pidStabilisationState_e pidControllerState);
-void setTargetPidLooptime(uint32_t pidLooptime);
+void pidSetTargetLooptime(uint32_t pidLooptime);
+void pidInitFilters(const pidProfile_t *pidProfile);
 

src/main/main.c

@@ -421,7 +421,8 @@ void init(void)
             &masterConfig.sensorSelectionConfig,
             masterConfig.compassConfig.mag_declination,
             masterConfig.gyro_lpf,
-            masterConfig.gyro_sync_denom)) {
+            masterConfig.gyro_sync_denom,
+            &masterConfig.sonarConfig)) {
         // if gyro was not detected due to whatever reason, we give up now.
         failureMode(FAILURE_MISSING_ACC);
     }
@@ -443,10 +444,9 @@ void init(void)
     LED0_OFF;
     LED1_OFF;
 
-#ifdef MAG
+    // gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidSetTargetLooptime()
-    if (sensors(SENSOR_MAG))
+    pidSetTargetLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime
-        compassInit();
+    pidInitFilters(&currentProfile->pidProfile);
-#endif
 
     imuInit();
 
@@ -478,12 +478,6 @@ void init(void)
     }
 #endif
 
-#ifdef SONAR
-    if (feature(FEATURE_SONAR)) {
-        sonarInit(&masterConfig.sonarConfig);
-    }
-#endif
-
 #ifdef LED_STRIP
     ledStripInit(&masterConfig.ledStripConfig);
 
@@ -537,13 +531,6 @@ void init(void)
     }
 #endif
 
-    if (masterConfig.gyro_lpf > 0 && masterConfig.gyro_lpf < 7) {
-        masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
-        masterConfig.gyro_sync_denom = 1;
-    }
-
-    setTargetPidLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime
-
 #ifdef BLACKBOX
     initBlackbox();
 #endif

src/main/sensors/initialisation.c

@@ -24,6 +24,8 @@
 
 #include "common/axis.h"
 
+#include "config/feature.h"
+
 #include "drivers/io.h"
 #include "drivers/system.h"
 #include "drivers/exti.h"
@@ -61,6 +63,7 @@
 
 #include "drivers/sonar_hcsr04.h"
 
+#include "fc/config.h"
 #include "fc/runtime_config.h"
 
 #include "sensors/sensors.h"
@@ -609,6 +612,19 @@ retry:
 }
 #endif
 
+#ifdef SONAR
+static bool detectSonar(void)
+{
+    if (feature(FEATURE_SONAR)) {
+        // the user has set the sonar feature, so assume they have an HC-SR04 plugged in,
+        // since there is no way to detect it
+        sensorsSet(SENSOR_SONAR);
+        return true;
+    }
+    return false;
+}
+#endif
+
 void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
 {
     if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) {
@@ -626,7 +642,8 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
         sensorSelectionConfig_t *sensorSelectionConfig,
         int16_t magDeclinationFromConfig,
         uint8_t gyroLpf,
-        uint8_t gyroSyncDenominator)
+        uint8_t gyroSyncDenominator,
+        const sonarConfig_t *sonarConfig)
 {
     memset(&acc, 0, sizeof(acc));
     memset(&gyro, 0, sizeof(gyro));
@@ -664,6 +681,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
         const int16_t deg = magDeclinationFromConfig / 100;
         const int16_t min = magDeclinationFromConfig % 100;
         magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
+        compassInit();
     }
 #else
     UNUSED(magDeclinationFromConfig);
@@ -673,6 +691,14 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
     detectBaro(sensorSelectionConfig->baro_hardware);
 #endif
 
+#ifdef SONAR
+    if (detectSonar()) {
+        sonarInit(sonarConfig);
+    }
+#else
+    UNUSED(sonarConfig);
+#endif
+
     reconfigureAlignment(sensorAlignmentConfig);
 
     return true;

src/main/sensors/initialisation.h

@@ -16,5 +16,7 @@
  */
 
 #pragma once
+
 struct sensorSelectionConfig_s;
-bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, struct sensorSelectionConfig_s *sensorSelectionConfig, int16_t magDeclinationFromConfig, uint8_t gyroLpf, uint8_t gyroSyncDenominator);
+struct sonarConfig_s;
+bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig, const struct sensorSelectionConfig_s *sensorSelectionConfig, int16_t magDeclinationFromConfig, uint8_t gyroLpf, uint8_t gyroSyncDenominator, const struct sonarConfig_s *sonarConfig);

src/main/target/NAZE/config.c

@@ -43,7 +43,7 @@ void targetConfiguration(master_t *config)
 
     config->motorConfig.minthrottle = 1049;
 
-    config->gyro_lpf = 1;
+    config->gyro_lpf = GYRO_LPF_188HZ;
     config->gyro_soft_lpf_hz = 100;
     config->gyro_soft_notch_hz_1 = 0;
     config->gyro_soft_notch_hz_2 = 0;