Dterm scaling correction // Code cleanup

src/main/config/config.c

@@ -128,7 +128,7 @@ static uint32_t activeFeaturesLatch = 0;
 static uint8_t currentControlRateProfileIndex = 0;
 controlRateConfig_t *currentControlRateProfile;
 
-static const uint8_t EEPROM_CONF_VERSION = 107;
+static const uint8_t EEPROM_CONF_VERSION = 108;
 
 static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
 {
@@ -289,12 +289,12 @@ void resetSerialConfig(serialConfig_t *serialConfig)
 }
 
 static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
-    controlRateConfig->rcRate8 = 90;
+    controlRateConfig->rcRate8 = 100;
-    controlRateConfig->rcExpo8 = 65;
+    controlRateConfig->rcExpo8 = 70;
     controlRateConfig->thrMid8 = 50;
     controlRateConfig->thrExpo8 = 0;
     controlRateConfig->dynThrPID = 0;
-    controlRateConfig->rcYawExpo8 = 0;
+    controlRateConfig->rcYawExpo8 = 20;
     controlRateConfig->tpa_breakpoint = 1500;
 
     for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) {
@@ -380,7 +380,6 @@ static void resetConf(void)
     masterConfig.current_profile_index = 0;     // default profile
     masterConfig.gyro_cmpf_factor = 600;        // default MWC
     masterConfig.gyro_cmpfm_factor = 250;       // default MWC
-    masterConfig.gyro_lpf = 188;                // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead
 
     resetAccelerometerTrims(&masterConfig.accZero);
 
@@ -390,7 +389,6 @@ static void resetConf(void)
     masterConfig.boardAlignment.pitchDegrees = 0;
     masterConfig.boardAlignment.yawDegrees = 0;
     masterConfig.acc_hardware = ACC_DEFAULT;     // default/autodetect
-    masterConfig.acc_for_fast_looptime = 0;      // FIXME - Needs to be removed through the code
     masterConfig.max_angle_inclination = 500;    // 50 degrees
     masterConfig.yaw_control_direction = 1;
     masterConfig.gyroConfig.gyroMovementCalibrationThreshold = 32;
@@ -455,7 +453,6 @@ static void resetConf(void)
     resetSerialConfig(&masterConfig.serialConfig);
 
     masterConfig.emf_avoidance = 0;
-    masterConfig.rcSmoothing = 1;
 
     resetPidProfile(&currentProfile->pidProfile);
 
@@ -546,7 +543,6 @@ static void resetConf(void)
     masterConfig.escAndServoConfig.minthrottle = 1000;
     masterConfig.escAndServoConfig.maxthrottle = 2000;
     masterConfig.motor_pwm_rate = 32000;
-    masterConfig.looptime = 0;
     masterConfig.rcSmoothing = 1;
     currentProfile->pidProfile.pidController = 3;
     currentProfile->pidProfile.P8[ROLL] = 36;

src/main/config/config_master.h

@@ -26,7 +26,6 @@ typedef struct master_t {
     uint8_t mixerMode;
     uint32_t enabledFeatures;
     uint8_t emf_avoidance;                   // change pll settings to avoid noise in the uhf band
-    uint8_t rcSmoothing;                    // Enable Interpolation of RC command
 
     motorMixer_t customMotorMixer[MAX_SUPPORTED_MOTORS];
 #ifdef USE_SERVOS

src/main/drivers/gyro_sync.c

@@ -43,58 +43,14 @@ bool gyroSyncCheckUpdate(void) {
     return getMpuDataStatus(&gyro);
 }
 
-void gyroUpdateSampleRate(uint8_t lpf) {
+void gyroUpdateSampleRate(void) {
     int gyroSamplePeriod;
     int minLooptime;
 
-#if defined(SPRACINGF3) || defined(ALIENWIIF3) || defined(NAZE32PRO) || defined(STM32F3DISCOVERY) || defined(CHEBUZZF3) || defined(PORT103R) || defined(MOTOLAB)  || defined(SPARKY)
+    gyroSamplePeriod = 1000; // gyro sampling rate 1khz
-        if (lpf == INV_FILTER_256HZ_NOLPF2) {
+    minLooptime = 1000;      // Full 1khz sampling
-            gyroSamplePeriod = 125;
 
-            if(!sensors(SENSOR_ACC)) {
+    // calculate gyro divider and targetLooptime (expected cycleTime)
-                minLooptime = 500;   // Max refresh 2khz
-            } else {
-                minLooptime = 625;   // Max refresh 1,6khz
-            }
-        } else {
-            gyroSamplePeriod = 1000;
-            minLooptime = 1000;      // Full sampling
-        }
-#elif defined(CC3D)
-        if (lpf == INV_FILTER_256HZ_NOLPF2) {
-            gyroSamplePeriod = 125;
-
-            if(!sensors(SENSOR_ACC)) {
-                minLooptime = 890;   // Max refresh 1,12khz
-            } else {
-                minLooptime = 1000;  // Max refresh 1khz
-            }
-        } else {
-            gyroSamplePeriod = 1000;
-            minLooptime = 1000;      // Full sampling
-        }
-#elif defined(COLIBRI_RACE) || defined(EUSTM32F103RC)
-        if (lpf == INV_FILTER_256HZ_NOLPF2) {
-            gyroSamplePeriod = 125;
-
-            if(!sensors(SENSOR_ACC)) { // TODO - increase to 8khz when oneshot125 can be limited
-                minLooptime = 250;   // Max refresh 4khz
-            } else {
-                minLooptime = 250;   // Max refresh 4khz
-            }
-        } else {
-            gyroSamplePeriod = 1000;
-            minLooptime = 1000;      // Full sampling
-        }
-#else
-        if (lpf == INV_FILTER_256HZ_NOLPF2) {
-            gyroSamplePeriod = 125;
-            minLooptime = 625;      // Max refresh 1,6khz
-        } else {
-            gyroSamplePeriod = 1000;
-            minLooptime = 1000;     // Full sampling without ACC
-        }
-#endif
     mpuDividerDrops  = (minLooptime + gyroSamplePeriod -1 ) / gyroSamplePeriod - 1;
     targetLooptime = (mpuDividerDrops + 1) * gyroSamplePeriod;
 }

src/main/drivers/gyro_sync.h

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

src/main/flight/imu.c

@@ -73,7 +73,7 @@ static pidProfile_t *pidProfile;
 static accDeadband_t *accDeadband;
 static accProcessor_t accProc;
 
-static void qAccProcessingStateMachine(rollAndPitchTrims_t *accelerometerTrims, uint8_t acc_for_fast_looptime);
+static void qAccProcessingStateMachine(rollAndPitchTrims_t *accelerometerTrims);
 
 
 void imuConfigure(
@@ -182,12 +182,12 @@ int16_t imuCalculateHeading(t_fp_vector *vec)
     return head;
 }
 
-void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t acc_for_fast_looptime)
+void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
 {
     gyroUpdate();
 
     if (sensors(SENSOR_ACC)) {
-        qAccProcessingStateMachine(accelerometerTrims, acc_for_fast_looptime);
+        qAccProcessingStateMachine(accelerometerTrims);
     } else {
         accADC[X] = 0;
         accADC[Y] = 0;
@@ -578,7 +578,7 @@ void qimuInit()
 
 //////////////////////////////////////////////////////////////////////
 
-static void qAccProcessingStateMachine(rollAndPitchTrims_t *accelerometerTrims, uint8_t acc_for_fast_looptime)
+static void qAccProcessingStateMachine(rollAndPitchTrims_t *accelerometerTrims)
 {
     int axis;
     const float gyro_drift_factor = 0.00f;
@@ -595,7 +595,7 @@ static void qAccProcessingStateMachine(rollAndPitchTrims_t *accelerometerTrims,
     // get time step.. TODO: this should really be fixed to division of MPU sample rate
     static float dT;
 
-    bool keepProcessing = !acc_for_fast_looptime;               // (keepProcessing == true): causes all states to execute (for slow cycle times)
+    bool keepProcessing = true;               // (keepProcessing == true): causes all states to execute (for slow cycle times)
 
     do {
         switch (accProc.state) {

src/main/flight/imu.h

@@ -84,4 +84,4 @@ float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);
 int16_t imuCalculateHeading(t_fp_vector *vec);
 
 void imuResetAccelerationSum(void);
-void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t acc_for_fast_looptime);
+void imuUpdate(rollAndPitchTrims_t *accelerometerTrims);

src/main/flight/pid.c

@@ -210,7 +210,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
             delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
         }
 
-        DTerm = constrainf((delta / 3.0f) * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
+        DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
 
         // -----calculate total PID output
         axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
@@ -301,7 +301,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
             delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
         }
 
-        DTerm = (delta * dynD8[axis]) / 32;
+        DTerm = (delta * 3  * dynD8[axis]) / 32;
         axisPID[axis] = PTerm + ITerm - DTerm;
 
 #ifdef BLACKBOX
@@ -386,7 +386,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
             delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
         }
 
-        DTerm = ((int32_t)delta * dynD8[axis]) >> 5;   // 32 bits is needed for calculation
+        DTerm = ((int32_t)delta * 3 * dynD8[axis]) >> 5;   // 32 bits is needed for calculation
 
         axisPID[axis] = PTerm + ITerm - DTerm;
 
@@ -504,7 +504,7 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
             delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
         }
 
-        DTerm = (delta * dynD8[axis]) / 32;
+        DTerm = (delta * 3 * dynD8[axis]) / 32;
         axisPID[axis] = PTerm + ITerm - DTerm;
 
 #ifdef BLACKBOX
@@ -775,7 +775,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
             delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
         }
 
-        DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
+        DTerm = (delta * 3 * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; // Multiplied by 3 to match old scaling
 
         // -----calculate total PID output
         axisPID[axis] = PTerm + ITerm + DTerm;

src/main/io/serial_cli.c

@@ -325,7 +325,6 @@ typedef struct {
 
 const clivalue_t valueTable[] = {
     { "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 },
@@ -411,7 +410,6 @@ const clivalue_t valueTable[] = {
 
     { "max_angle_inclination",      VAR_UINT16 | MASTER_VALUE,  &masterConfig.max_angle_inclination, 100, 900 },
 
-    { "gyro_lpf",                   VAR_UINT16 | MASTER_VALUE,  &masterConfig.gyro_lpf, 0, 256 },
     { "moron_threshold",            VAR_UINT8  | MASTER_VALUE,  &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, 0, 128 },
     { "gyro_cmpf_factor",           VAR_UINT16 | MASTER_VALUE,  &masterConfig.gyro_cmpf_factor, 100, 1000 },
     { "gyro_cmpfm_factor",          VAR_UINT16 | MASTER_VALUE,  &masterConfig.gyro_cmpfm_factor, 100, 1000 },

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, masterConfig.baro_hardware, currentProfile->mag_declination)) {
+    if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination)) {
         // if gyro was not detected due to whatever reason, we give up now.
         failureMode(FAILURE_MISSING_ACC);
     }

src/main/mw.c

@@ -773,7 +773,7 @@ void loop(void)
     if (gyroSyncCheckUpdate() || (int32_t)(currentTime - (loopTime + GYRO_WATCHDOG_DELAY)) >= 0) {
 
         loopTime = currentTime + targetLooptime;
-        imuUpdate(&currentProfile->accelerometerTrims, masterConfig.acc_for_fast_looptime);
+        imuUpdate(&currentProfile->accelerometerTrims);
 
 
         // Measure loop rate just after reading the sensors
@@ -781,13 +781,11 @@ void loop(void)
         cycleTime = (int32_t)(currentTime - previousTime);
         previousTime = currentTime;
 
-        dT = (float)cycleTime * 0.000001f;
+        dT = (float)targetLooptime * 0.000001f;
 
         filterApply7TapFIR(gyroADC);
 
-        if (masterConfig.rcSmoothing) {
         filterRc();
-        }
 
         annexCode();
 #if defined(BARO) || defined(SONAR)

src/main/sensors/gyro.h

@@ -29,6 +29,8 @@ typedef enum {
     GYRO_FAKE
 } gyroSensor_e;
 
+#define GYRO_LPF 188
+
 extern gyro_t gyro;
 extern sensor_align_e gyroAlign;
 

src/main/sensors/initialisation.c

@@ -195,8 +195,9 @@ bool fakeAccDetect(acc_t *acc)
 }
 #endif
 
-bool detectGyro(uint16_t gyroLpf)
+bool detectGyro(void)
 {
+	uint16_t gyroLpf = GYRO_LPF;
     gyroSensor_e gyroHardware = GYRO_DEFAULT;
 
     gyroAlign = ALIGN_DEFAULT;
@@ -633,14 +634,14 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
     }
 }
 
-bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig)
+bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig)
 {
     int16_t deg, min;
 
     memset(&acc, 0, sizeof(acc));
     memset(&gyro, 0, sizeof(gyro));
 
-    if (!detectGyro(gyroLpf)) {
+    if (!detectGyro()) {
         return false;
     }
     detectAcc(accHardwareToUse);
@@ -651,7 +652,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(gyroLpf);  // Set gyro refresh rate before initialisation
+    gyroUpdateSampleRate();  // 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, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig);
+bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig);