Change defaults // Add optional filter config

src/main/config/config.c

@@ -240,13 +240,13 @@ static void resetPidProfile(pidProfile_t *pidProfile)
 
     // Betaflight PID controller parameters
     pidProfile->ptermSetpointWeight = 75;
-    pidProfile->dtermSetpointWeight = 0;
+    pidProfile->dtermSetpointWeight = 200;
     pidProfile->pidMaxVelocity = 1000;
     pidProfile->pidMaxVelocityYaw = 50;
-    pidProfile->toleranceBand = 15;
+    pidProfile->toleranceBand = 20;
     pidProfile->toleranceBandReduction = 40;
     pidProfile->zeroCrossAllowanceCount = 2;
-    pidProfile->itermThrottleGain = 10;
+    pidProfile->itermThrottleGain = 0;
 
 #ifdef GTUNE
     pidProfile->gtune_lolimP[ROLL] = 10;          // [0..200] Lower limit of ROLL P during G tune.
@@ -475,11 +475,12 @@ static void resetConf(void)
     masterConfig.gyro_sync_denom = 4;
     masterConfig.pid_process_denom = 2;
 #endif
-    masterConfig.gyro_soft_lpf_hz = 100;
+    masterConfig.gyro_soft_type = GYRO_FILTER_PT1;
+    masterConfig.gyro_soft_lpf_hz = 80;
     masterConfig.gyro_soft_notch_hz = 0;
     masterConfig.gyro_soft_notch_q = 5;
 
-    masterConfig.debug_mode = 0;
+    masterConfig.debug_mode = DEBUG_NONE;
 
     resetAccelerometerTrims(&masterConfig.accZero);
 
@@ -527,7 +528,7 @@ static void resetConf(void)
     masterConfig.rxConfig.rssi_channel = 0;
     masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT;
     masterConfig.rxConfig.rssi_ppm_invert = 0;
-    masterConfig.rxConfig.rcSmoothing = RC_SMOOTHING_OFF;
+    masterConfig.rxConfig.rcSmoothing = RC_SMOOTHING_AUTO;
     masterConfig.rxConfig.rcSmoothInterval = 9;
     masterConfig.rxConfig.fpvCamAngleDegrees = 0;
     masterConfig.rxConfig.max_aux_channel = MAX_AUX_CHANNELS;
@@ -742,7 +743,7 @@ void activateConfig(void)
         &currentProfile->pidProfile
     );
 
-    gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz, masterConfig.gyro_soft_notch_hz, masterConfig.gyro_soft_notch_q);
+    gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz, masterConfig.gyro_soft_notch_hz, masterConfig.gyro_soft_notch_q, masterConfig.gyro_soft_type);
 
 #ifdef TELEMETRY
     telemetryUseConfig(&masterConfig.telemetryConfig);

src/main/config/config_master.h

@@ -58,6 +58,7 @@ typedef struct master_t {
     uint8_t acc_for_fast_looptime;          // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes.
     uint16_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
+    uint8_t gyro_soft_type;                 // Gyro Filter Type
     uint8_t gyro_soft_lpf_hz;               // Biquad gyro lpf hz
     uint16_t gyro_soft_notch_hz;            // Biquad gyro notch hz
     uint8_t gyro_soft_notch_q;              // Biquad gyro notch quality

src/main/io/serial_cli.c

@@ -497,6 +497,10 @@ static const char * const lookupTableRcSmoothing[] = {
     "OFF", "DEFAULT", "AUTO", "MANUAL"
 };
 
+static const char * const lookupTableGyroSoftLowpassType[] = {
+    "NORMAL", "HIGH"
+};
+
 typedef struct lookupTableEntry_s {
     const char * const *values;
     const uint8_t valueCount;
@@ -534,6 +538,7 @@ typedef enum {
     TABLE_MOTOR_PWM_PROTOCOL,
     TABLE_DELTA_METHOD,
     TABLE_RC_SMOOTHING,
+    TABLE_GYRO_LOWPASS_TYPE,
 #ifdef OSD
     TABLE_OSD,
 #endif
@@ -571,6 +576,7 @@ static const lookupTableEntry_t lookupTables[] = {
     { lookupTablePwmProtocol, sizeof(lookupTablePwmProtocol) / sizeof(char *) },
     { lookupTableDeltaMethod, sizeof(lookupTableDeltaMethod) / sizeof(char *) },
     { lookupTableRcSmoothing, sizeof(lookupTableRcSmoothing) / sizeof(char *) },
+    { lookupTableGyroSoftLowpassType, sizeof(lookupTableGyroSoftLowpassType) / sizeof(char *) },
 #ifdef OSD
     { lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) },
 #endif
@@ -742,6 +748,7 @@ const clivalue_t valueTable[] = {
     { "pid_delta_method",           VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } },
     { "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_lowpass_type",          VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.gyro_soft_type, .config.lookup = { TABLE_GYRO_LOWPASS_TYPE } },
     { "gyro_lowpass",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0,  255 } },
     { "gyro_notch_hz",              VAR_UINT16 | MASTER_VALUE,  &masterConfig.gyro_soft_notch_hz, .config.minmax = { 0,  500 } },
     { "gyro_notch_q",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.gyro_soft_notch_q, .config.minmax = { 1,  100 } },

src/main/sensors/gyro.c

@@ -47,17 +47,21 @@ static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
 static const gyroConfig_t *gyroConfig;
 static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
 static biquadFilter_t gyroFilterNotch[XYZ_AXIS_COUNT];
+static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT];
+static uint8_t gyroSoftLpfType;
 static uint16_t gyroSoftNotchHz;
 static uint8_t gyroSoftNotchQ;
 static uint8_t gyroSoftLpfHz;
 static uint16_t calibratingG = 0;
+static float gyroDt;
 
-void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q)
+void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q, uint8_t gyro_soft_lpf_type)
 {
     gyroConfig = gyroConfigToUse;
     gyroSoftLpfHz = gyro_soft_lpf_hz;
     gyroSoftNotchHz = gyro_soft_notch_hz;
     gyroSoftNotchQ = gyro_soft_notch_q;
+    gyroSoftLpfType = gyro_soft_lpf_type;
 }
 
 void gyroInit(void)
@@ -65,7 +69,10 @@ void gyroInit(void)
     if (gyroSoftLpfHz && gyro.targetLooptime) {  // Initialisation needs to happen once samplingrate is known
         for (int axis = 0; axis < 3; axis++) {
             biquadFilterInit(&gyroFilterNotch[axis], gyroSoftNotchHz, gyro.targetLooptime, ((float) gyroSoftNotchQ) / 10, FILTER_NOTCH);
+            if (gyroSoftLpfType == GYRO_FILTER_BIQUAD)
                 biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
+            else
+                gyroDt = gyro.targetLooptime / 1000.0f;
         }
     }
 }
@@ -175,7 +182,11 @@ void gyroUpdate(void)
                 debug[axis*2 + 1] = lrintf(sample);
             }
 
-            gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], sample);;
+            if (gyroSoftLpfType == GYRO_FILTER_BIQUAD) {
+                gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], sample);
+            } else {
+                gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], sample, gyroSoftLpfHz, gyroDt);
+            }
             gyroADC[axis] = lrintf(gyroADCf[axis]);
         }
     } else {

src/main/sensors/gyro.h

@@ -31,6 +31,11 @@ typedef enum {
     GYRO_MAX = GYRO_FAKE
 } gyroSensor_e;
 
+typedef enum {
+    GYRO_FILTER_PT1 = 0,
+    GYRO_FILTER_BIQUAD,
+} gyroFilter_e;
+
 extern gyro_t gyro;
 
 extern int32_t gyroADC[XYZ_AXIS_COUNT];
@@ -40,7 +45,7 @@ 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 gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q);
+void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q, uint8_t gyro_soft_lpf_type);
 void gyroSetCalibrationCycles(void);
 void gyroInit(void);
 void gyroUpdate(void);