Configurable gyro Denominator when gyro_lpf set to OFF

src/main/config/config.c

@@ -400,6 +400,7 @@ static void resetConf(void)
     masterConfig.dcm_kp = 2500;                // 1.0 * 10000
     masterConfig.dcm_ki = 0;                    // 0.003 * 10000
     masterConfig.gyro_lpf = 1;                 // 188HZ
+    masterConfig.gyro_sync_denom = 1;
     masterConfig.gyro_soft_lpf_hz = 60;
 
     resetAccelerometerTrims(&masterConfig.accZero);

src/main/config/config_master.h

@@ -57,7 +57,8 @@ typedef struct master_t {
     int8_t yaw_control_direction;           // change control direction of yaw (inverted, normal)
     uint8_t acc_hardware;                   // Which acc hardware to use on boards with more than one device
     uint8_t acc_for_fast_looptime;          // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes.
-    uint8_t gyro_lpf;                      // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
+    uint8_t gyro_lpf;                       // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
+    uint8_t gyro_sync_denom;                // Gyro sample divider
     float gyro_soft_lpf_hz;                 // Biqyad gyro lpf hz
     uint16_t dcm_kp;                        // DCM filter proportional gain ( x 10000)
     uint16_t dcm_ki;                        // DCM filter integral gain ( x 10000)

src/main/drivers/gyro_sync.c

@@ -41,27 +41,14 @@ bool gyroSyncCheckUpdate(void) {
     return getMpuDataStatus(&gyro);
 }
 
-void gyroUpdateSampleRate(uint8_t lpf) {
+void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator) {
-    int gyroSamplePeriod, gyroSyncDenominator;
+    int gyroSamplePeriod;
 
     if (!lpf) {
         gyroSamplePeriod = 125;
-#ifdef STM32F303xC
-#ifdef COLIBRI_RACE // Leave out LUX target for now.  Need to test 2.6Khz
-        gyroSyncDenominator = 3; // Sample every 3d gyro measurement 2,6khz
-#else
-        gyroSyncDenominator = 4; // Sample every 4th gyro measurement 2khz
-#endif
-#else
-        if (!sensors(SENSOR_ACC) && !sensors(SENSOR_BARO) && !sensors(SENSOR_MAG)) {
-            gyroSyncDenominator = 4; // Sample every 4th gyro measurement 2khz
-        } else {
-            gyroSyncDenominator = 8; // Sample every 8th gyro measurement 1khz
-        }
-#endif
     } else {
         gyroSamplePeriod = 1000;
-        gyroSyncDenominator = 1; // Full Sampling 1khz
+        gyroSyncDenominator = 1; // Always full Sampling 1khz
     }
 
     // calculate gyro divider and targetLooptime (expected cycleTime)

src/main/drivers/gyro_sync.h

@@ -11,4 +11,4 @@ extern uint32_t targetLooptime;
 
 bool gyroSyncCheckUpdate(void);
 uint8_t gyroMPU6xxxGetDividerDrops(void);
-void gyroUpdateSampleRate(uint8_t lpf);
+void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator);

src/main/io/serial_cli.c

@@ -597,6 +597,7 @@ const clivalue_t valueTable[] = {
     { "max_angle_inclination",      VAR_UINT16 | MASTER_VALUE,  &masterConfig.max_angle_inclination, .config.minmax = { 100,  900 } },
 
     { "gyro_lpf",                   VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
+    { "gyro_sync_denom",            VAR_UINT8  | MASTER_VALUE,  &masterConfig.gyro_sync_denom, .config.minmax = { 1,  8 } },
     { "gyro_soft_lpf",              VAR_FLOAT  | MASTER_VALUE,  &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0,  500 } },
     { "moron_threshold",            VAR_UINT8  | MASTER_VALUE,  &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0,  128 } },
     { "imu_dcm_kp",                 VAR_UINT16 | MASTER_VALUE,  &masterConfig.dcm_kp, .config.minmax = { 0,  50000 } },

src/main/io/serial_msp.c

@@ -101,6 +101,39 @@ extern uint16_t cycleTime; // FIXME dependency on mw.c
 extern uint16_t rssi; // FIXME dependency on mw.c
 extern void resetPidProfile(pidProfile_t *pidProfile);
 
+
+void setGyroSamplingSpeed(uint16_t looptime) {
+    uint16_t gyroSampleRate = 1000;
+    uint8_t maxDivider = 1;
+
+    if (looptime != targetLooptime) {
+#ifdef STM32F303xC
+        if (looptime < 1000) {
+            masterConfig.gyro_lpf = 1;
+            gyroSampleRate = 125;
+            maxDivider = 8;
+        } else {
+            masterConfig.gyro_lpf = 1;
+        }
+#else
+        if (looptime < 1000) {
+            masterConfig.gyro_lpf = 0;
+            masterConfig.acc_hardware = 1;
+            masterConfig.baro_hardware = 1;
+            masterConfig.mag_hardware = 1;
+            gyroSampleRate = 125;
+            maxDivider = 8;
+        } else {
+            masterConfig.gyro_lpf = 1;
+            masterConfig.acc_hardware = 0;
+            masterConfig.baro_hardware = 0;
+            masterConfig.mag_hardware = 0;
+        }
+#endif
+    }
+    masterConfig.gyro_sync_denom = constrain(looptime / gyroSampleRate, 1, maxDivider);
+}
+
 void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse);
 
 const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
@@ -1203,7 +1236,6 @@ static bool processInCommand(void)
     uint32_t i;
     uint16_t tmp;
     uint8_t rate;
-    uint16_t gyroRefreshRate;
 #ifdef GPS
     uint8_t wp_no;
     int32_t lat = 0, lon = 0, alt = 0;
@@ -1248,34 +1280,7 @@ static bool processInCommand(void)
         masterConfig.disarm_kill_switch = read8();
         break;
     case MSP_SET_LOOP_TIME:
-        gyroRefreshRate = read16();
+        setGyroSamplingSpeed(read16());
-        if (gyroRefreshRate != targetLooptime) {
-            switch (gyroRefreshRate) {
-#ifdef STM32F303xC
-                default:
-                case(1000):
-                    masterConfig.gyro_lpf = 1;
-                    break;
-                case(500):
-                    masterConfig.gyro_lpf = 0;
-                    break;
-#else
-                default:
-                case(1000):
-                    masterConfig.gyro_lpf = 1;
-                    masterConfig.acc_hardware = 0;
-                    masterConfig.baro_hardware = 0;
-                    masterConfig.mag_hardware = 0;
-                    break;
-                case(500):
-                    masterConfig.gyro_lpf = 0;
-                    masterConfig.acc_hardware = 1;
-                    masterConfig.baro_hardware = 1;
-                    masterConfig.mag_hardware = 1;
-                    break;
-#endif
-            }
-        }
         break;
     case MSP_SET_PID_CONTROLLER:
         currentProfile->pidProfile.pidController = constrain(read8(), 1, 2);

src/main/main.c

@@ -455,7 +455,7 @@ void init(void)
     }
 #endif
 
-    if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, masterConfig.mag_declination, masterConfig.gyro_lpf)) {
+    if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, masterConfig.mag_declination, masterConfig.gyro_lpf, masterConfig.gyro_sync_denom)) {
         // if gyro was not detected due to whatever reason, we give up now.
         failureMode(FAILURE_MISSING_ACC);
     }

src/main/sensors/initialisation.c

@@ -689,7 +689,7 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
     }
 }
 
-bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig, uint8_t gyroLpf)
+bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig, uint8_t gyroLpf, uint8_t gyroSyncDenominator)
 {
     int16_t deg, min;
 
@@ -715,7 +715,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a
     if (sensors(SENSOR_ACC))
         acc.init();
     // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
-    gyroUpdateSampleRate(gyroLpf);    // Set gyro refresh rate before initialisation
+    gyroUpdateSampleRate(gyroLpf, gyroSyncDenominator);    // Set gyro refresh rate before initialisation
     gyro.init(gyroLpf);
 
     detectMag(magHardwareToUse);

src/main/sensors/initialisation.h

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