New Clli params RC Smoothing and sync_gyro_to_loop

src/main/config/config.c

@@ -449,8 +449,10 @@ static void resetConf(void)
 
     resetSerialConfig(&masterConfig.serialConfig);
 
-    masterConfig.looptime = 3500;
+    masterConfig.looptime = 2000;
     masterConfig.emf_avoidance = 0;
+    masterConfig.syncGyroToLoop = 1;
+    masterConfig.rcSmoothing = 1;
 
     resetPidProfile(&currentProfile->pidProfile);
 
@@ -540,6 +542,7 @@ static void resetConf(void)
     masterConfig.escAndServoConfig.maxthrottle = 2000;
     masterConfig.motor_pwm_rate = 32000;
     masterConfig.looptime = 2000;
+    masterConfig.rcSmoothing = 1;
     currentProfile->pidProfile.pidController = 3;
     currentProfile->pidProfile.P8[ROLL] = 36;
     currentProfile->pidProfile.P8[PITCH] = 36;

src/main/config/config_master.h

@@ -27,6 +27,8 @@ typedef struct master_t {
     uint32_t enabledFeatures;
     uint16_t looptime;                      // imu loop time in us
     uint8_t emf_avoidance;                   // change pll settings to avoid noise in the uhf band
+    uint8_t syncGyroToLoop;                 // Enable interrupt based loop
+    uint8_t rcSmoothing;                    // Enable Interpolation of RC command
 
     motorMixer_t customMotorMixer[MAX_SUPPORTED_MOTORS];
 #ifdef USE_SERVOS

src/main/drivers/gyro_sync.c

@@ -43,49 +43,55 @@ bool gyroSyncCheckUpdate(void) {
     return getMpuInterrupt(&gyro);
 }
 
-void gyroUpdateSampleRate(uint32_t looptime, uint8_t lpf) {
+void gyroUpdateSampleRate(uint32_t looptime, uint8_t lpf, uint8_t syncGyroToLoop) {
     int gyroSamplePeriod;
     int minLooptime;
 
+    if (syncGyroToLoop) {
 #ifdef STM32F303xC
-    if (lpf == INV_FILTER_256HZ_NOLPF2) {
+        if (lpf == INV_FILTER_256HZ_NOLPF2) {
-        gyroSamplePeriod = 125;
+            gyroSamplePeriod = 125;
 
-        if(!sensors(SENSOR_ACC)) {
+            if(!sensors(SENSOR_ACC)) {
-            minLooptime = 500;   // Max refresh 2khz
+                minLooptime = 500;   // Max refresh 2khz
+            }
+            else {
+                minLooptime = 625;   // Max refresh 1,6khz
+            }
         }
         else {
-            minLooptime = 625;   // Max refresh 1,6khz
+            gyroSamplePeriod = 1000;
+            minLooptime = 1000;      // Full sampling
         }
-    }
-    else {
-        gyroSamplePeriod = 1000;
-        minLooptime = 1000;      // Full sampling
-    }
 #elif STM32F10X
-    if (lpf == INV_FILTER_256HZ_NOLPF2) {
+        if (lpf == INV_FILTER_256HZ_NOLPF2) {
-        gyroSamplePeriod = 125;
+            gyroSamplePeriod = 125;
 
-        if(!sensors(SENSOR_ACC)) {
+            if(!sensors(SENSOR_ACC)) {
-            minLooptime = 625;   // Max refresh 1,33khz
+                minLooptime = 625;   // Max refresh 1,33khz
+            }
+            else {
+                minLooptime = 1625;  // Max refresh 615hz
+            }
         }
         else {
-            minLooptime = 1625;  // Max refresh 615hz
+            gyroSamplePeriod = 1000;
+            if(!sensors(SENSOR_ACC)) {
+                minLooptime = 1000;  // Full sampling without ACC
+            }
+            else {
+                minLooptime = 2000;
+            }
         }
+#endif
+        looptime = constrain(looptime, minLooptime, 4000);
+        mpuDivider  = (looptime + gyroSamplePeriod -1 ) / gyroSamplePeriod - 1;
+        targetLooptime = (mpuDivider + 1) * gyroSamplePeriod;
     }
     else {
-        gyroSamplePeriod = 1000;
+    	mpuDivider = 0;
-        if(!sensors(SENSOR_ACC)) {
+    	targetLooptime = looptime;
-            minLooptime = 1000;  // Full sampling without ACC
-        }
-        else {
-            minLooptime = 2000;
-        }
     }
-#endif
-    looptime = constrain(looptime, minLooptime, 4000);
-    mpuDivider  = (looptime + gyroSamplePeriod -1 ) / gyroSamplePeriod - 1;
-    targetLooptime = (mpuDivider + 1) * gyroSamplePeriod;
 }
 
 uint8_t gyroMPU6xxxGetDivider(void) {

src/main/drivers/gyro_sync.h

@@ -9,4 +9,4 @@ extern uint32_t targetLooptime;
 
 bool gyroSyncCheckUpdate(void);
 uint8_t gyroMPU6xxxGetDivider(void);
-void gyroUpdateSampleRate(uint32_t looptime, uint8_t lpf);
+void gyroUpdateSampleRate(uint32_t looptime, uint8_t lpf, uint8_t syncGyroToLoop);

src/main/io/serial_cli.c

@@ -315,8 +315,10 @@ typedef struct {
 } clivalue_t;
 
 const clivalue_t valueTable[] = {
-    { "looptime",                   VAR_UINT16 | MASTER_VALUE,  &masterConfig.looptime, 0, 9000 },
+    { "looptime",                   VAR_UINT16 | MASTER_VALUE,  &masterConfig.looptime, 0, 4000 },
+	{ "sync_gyro_to_loop",          VAR_UINT8  | MASTER_VALUE,  &masterConfig.syncGyroToLoop, 0, 1 },
     { "emf_avoidance",              VAR_UINT8  | MASTER_VALUE,  &masterConfig.emf_avoidance, 0, 1 },
+	{ "rc_smoothing",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.rcSmoothing, 0, 1 },
 
     { "mid_rc",                     VAR_UINT16 | MASTER_VALUE,  &masterConfig.rxConfig.midrc, 1200, 1700 },
     { "min_check",                  VAR_UINT16 | MASTER_VALUE,  &masterConfig.rxConfig.mincheck, PWM_RANGE_ZERO, PWM_RANGE_MAX },
@@ -2146,7 +2148,7 @@ static void cliVersion(char *cmdline)
 {
     UNUSED(cmdline);
 
-    printf("# Betaflight/%s %s %s / %s (%s)",
+    printf("# BetaFlight/%s %s %s / %s (%s)",
         targetName,
         FC_VERSION_STRING,
         buildDate,

src/main/main.c

@@ -371,7 +371,7 @@ void init(void)
     }
 #endif
 
-    if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, currentProfile->mag_declination, masterConfig.looptime)) {
+    if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, currentProfile->mag_declination, masterConfig.looptime, masterConfig.syncGyroToLoop)) {
         // if gyro was not detected due to whatever reason, we give up now.
         failureMode(3);
     }

src/main/mw.c

@@ -93,7 +93,6 @@ enum {
 /* IBat monitoring interval (in microseconds) - 6 default looptimes */
 #define IBATINTERVAL (6 * 3500)
 #define GYRO_WATCHDOG_DELAY 500  // Watchdog for boards without interrupt for gyro
-
 #define LOOP_DEADBAND 400 // Dead band for loop to modify to rcInterpolationFactor in RC Filtering for unstable looptimes
 
 uint32_t currentTime = 0;
@@ -693,18 +692,7 @@ void filterGyro(void) {
     static filterStatePt1_t gyroADCState[XYZ_AXIS_COUNT];
 
     for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-    	if (masterConfig.looptime > 0) {
+        gyroADC[axis] = filterApplyPt1(gyroADC[axis], &gyroADCState[axis], currentProfile->pidProfile.gyro_cut_hz, dT);
-    		// Static dT calculation based on configured looptime
-            if (!gyroADCState[axis].constdT) {
-                gyroADCState[axis].constdT = (float)masterConfig.looptime * 0.000001f;
-            }
-
-            gyroADC[axis] = filterApplyPt1(gyroADC[axis], &gyroADCState[axis], currentProfile->pidProfile.gyro_cut_hz, gyroADCState[axis].constdT);
-    	}
-
-        else {
-	        gyroADC[axis] = filterApplyPt1(gyroADC[axis], &gyroADCState[axis], currentProfile->pidProfile.gyro_cut_hz, dT);
-        }
     }
 }
 void getArmingChannel(modeActivationCondition_t *modeActivationConditions, uint8_t *armingChannel) {
@@ -813,6 +801,29 @@ void filterRc(void){
     }
 }
 
+// Function for loop trigger
+bool runLoop(uint32_t loopTime) {
+	bool loopTrigger = false;
+
+    if (masterConfig.syncGyroToLoop) {
+        if (ARMING_FLAG(ARMED)) {
+            if (gyroSyncCheckUpdate() || (int32_t)(currentTime - loopTime + GYRO_WATCHDOG_DELAY) >= 0) {
+            	loopTrigger = true;
+            }
+        }
+        // Blheli arming workaround (stable looptime prior to arming)
+        else if (!ARMING_FLAG(ARMED) && ((int32_t)(currentTime - loopTime) >= 0)) {
+        	loopTrigger = true;
+        }
+    }
+
+    else if ((int32_t)(currentTime - loopTime) >= 0){
+    	loopTrigger = true;
+    }
+
+    return loopTrigger;
+}
+
 void loop(void)
 {
     static uint32_t loopTime;
@@ -859,8 +870,9 @@ void loop(void)
     }
 
     currentTime = micros();
-    if (gyroSyncCheckUpdate() || (int32_t)(currentTime - loopTime) >= 0) {
+    if (runLoop(loopTime)) {
-        loopTime = currentTime + targetLooptime + GYRO_WATCHDOG_DELAY;
+
+        loopTime = currentTime + targetLooptime;
 
         imuUpdate(&currentProfile->accelerometerTrims);
 
@@ -875,7 +887,9 @@ void loop(void)
             filterGyro();
         }
 
-        filterRc();
+        if (masterConfig.rcSmoothing) {
+            filterRc();
+        }
 
         annexCode();
 #if defined(BARO) || defined(SONAR)

src/main/sensors/initialisation.c

@@ -597,7 +597,7 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
     }
 }
 
-bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, int16_t magDeclinationFromConfig, uint32_t looptime)
+bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, int16_t magDeclinationFromConfig, uint32_t looptime, uint8_t syncGyroToLoop)
 {
     int16_t deg, min;
 
@@ -615,7 +615,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
     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(looptime, gyroLpf);  // Set gyro refresh rate before initialisation
+    gyroUpdateSampleRate(looptime, gyroLpf, syncGyroToLoop);  // Set gyro refresh rate before initialisation
     gyro.init();
 
     detectMag(magHardwareToUse);

src/main/sensors/initialisation.h

@@ -17,4 +17,4 @@
 
 #pragma once
 
-bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, int16_t magDeclinationFromConfig, uint32_t looptime);
+bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, int16_t magDeclinationFromConfig, uint32_t looptime, uint8_t syncGyroToLoop);