Changed to PT1 filter in fixedWingPitchToThrottleCorrection

docs/Settings.md

@@ -271,8 +271,8 @@
 | nav_fw_max_thr | 1700 | Maximum throttle for flying wing in GPS assisted modes |
 | nav_fw_min_thr | 1200 | Minimum throttle for flying wing in GPS assisted modes |
 | nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) |
-| nav_fw_pitch2thr_smoothing | 1 | Number of fixedWingPitchToThrottleCorrection cycles used to calculate a moving average pitch |
+| nav_fw_pitch2thr_smoothing | 0 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by nav_fw_pitch2thr_threshold |
-| nav_fw_pitch2thr_threshold | 0 | Threshold from average pitch before nav_fw_pitch2thr is applied [decidegrees] |
+| nav_fw_pitch2thr_threshold | 0 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] |
 | nav_fw_pos_hdg_d | 0 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) |
 | nav_fw_pos_hdg_i | 0 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) |
 | nav_fw_pos_hdg_p | 60 | P gain of heading PID controller. (Fixedwing, rovers, boats) |

src/main/fc/settings.yaml

@@ -2261,13 +2261,13 @@ groups:
         min: 0
         max: 100
       - name: nav_fw_pitch2thr_smoothing
-        description: "Number of fixedWingPitchToThrottleCorrection cycles used to calculate a moving average pitch"
+        description:  "How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh."
-        default_value: "1"
+        default_value: "0"
         field: fw.pitch_to_throttle_smooth
-        min: 1
+        min: 0
-        max: 4096
+        max: 9
       - name: nav_fw_pitch2thr_threshold
-        description: "Threshold from zero pitch before pitch_to_throttle is applied [decidegrees]"
+        description: "Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees]"
         default_value: "0"
         field: fw.pitch_to_throttle_thresh
         min: 0

src/main/navigation/navigation.c

@@ -152,8 +152,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .max_throttle = 1700,
         .min_throttle = 1200,
         .pitch_to_throttle = 10,                // pwm units per degree of pitch (10pwm units ~ 1% throttle)
-        .pitch_to_throttle_smooth = 1,          // Number of fixedWingPitchToThrottleCorrection cycles used to calculate a moving average pitch
+        .pitch_to_throttle_smooth = 0,
-        .pitch_to_throttle_thresh = 0,          // Threshold from average pitch before pitch_to_throttle is applied [decidegrees]
+        .pitch_to_throttle_thresh = 0,
         .loiter_radius = 5000,                  // 50m
 
         //Fixed wing landing

src/main/navigation/navigation.h

@@ -222,8 +222,8 @@ typedef struct navConfig_s {
         uint16_t min_throttle;               // Minimum allowed throttle in auto mode
         uint16_t max_throttle;               // Maximum allowed throttle in auto mode
         uint8_t  pitch_to_throttle;          // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10)
-        uint16_t pitch_to_throttle_smooth;    // Number of fixedWingPitchToThrottleCorrection cycles used to calculate a moving average pitch
+        uint16_t pitch_to_throttle_smooth;    // How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh.
-        uint8_t  pitch_to_throttle_thresh;   // Threshold from zero pitch before pitch_to_throttle is applied [decidegrees]
+        uint8_t  pitch_to_throttle_thresh;   // Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees]
         uint16_t loiter_radius;              // Loiter radius when executing PH on a fixed wing
         int8_t land_dive_angle;
         uint16_t launch_velocity_thresh;     // Velocity threshold for swing launch detection

src/main/navigation/navigation_fixedwing.c

@@ -79,6 +79,15 @@ static float getSmoothnessCutoffFreq(float baseFreq)
     return 0.001f * baseFreq * (float)(smoothness*smoothness*smoothness) + 0.1f;
 }
 
+// Calculates the cutoff frequency for smoothing out pitchToThrottleCorrection
+// pitch_to_throttle_smooth valid range from 0 to 9
+// resulting cutoff_freq ranging from baseFreq downwards to ~0.11Hz
+static float getPitchToThrottleSmoothnessCutoffFreq(float baseFreq)
+{
+    uint16_t smoothness = 10 - navConfig()->fw.pitch_to_throttle_smooth;
+    return 0.001f * baseFreq * (float)(smoothness*smoothness*smoothness) + 0.1f;
+}
+
 /*-----------------------------------------------------------
  * Altitude controller
  *-----------------------------------------------------------*/
@@ -461,20 +470,24 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
     return throttleSpeedAdjustment;
 }
 
-int16_t fixedWingPitchToThrottleCorrection(int16_t pitch)
+int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs)
 {
-    const int16_t movingAverageCycles = navConfig()->fw.pitch_to_throttle_smooth;
+    static timeUs_t previousTimePitchToThrCorr = 0;
-    static int16_t averagePitch = 0;
+    const timeDelta_t deltaMicrosPitchToThrCorr = currentTimeUs -  previousTimePitchToThrCorr;
+    previousTimePitchToThrCorr = currentTimeUs;
 
-    averagePitch = (averagePitch * movingAverageCycles + pitch - averagePitch) / movingAverageCycles;
+    static pt1Filter_t pitchToThrFilterState;
 
-    if (pitch > (averagePitch + navConfig()->fw.pitch_to_throttle_thresh) || pitch < (averagePitch - navConfig()->fw.pitch_to_throttle_thresh)) {
+    // Apply low-pass filter to pitch angle to smooth throttle correction
+    filteredPitch = pt1FilterApply4(&pitchToThrFilterState, pitch, getPitchToThrottleSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicrosPitchToThrCorr));
+
+    if (abs(pitch - filteredPitch) > navConfig()->fw.pitch_to_throttle_thresh) {
         // Unfiltered throttle correction outside of pitch deadband
         return DECIDEGREES_TO_DEGREES(pitch) * navConfig()->fw.pitch_to_throttle;
     }
     else {
         // Filtered throttle correction inside of pitch deadband
-        return DECIDEGREES_TO_DEGREES(averagePitch) * navConfig()->fw.pitch_to_throttle;
+        return DECIDEGREES_TO_DEGREES(filteredPitch) * navConfig()->fw.pitch_to_throttle;
     }
 }
 
@@ -497,7 +510,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
         // PITCH >0 dive, <0 climb
         int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));
         rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]);
-        int16_t throttleCorrection = fixedWingPitchToThrottleCorrection(pitchCorrection);
+        int16_t throttleCorrection = fixedWingPitchToThrottleCorrection(pitchCorrection, currentTimeUs);
 
 #ifdef NAV_FIXED_WING_LANDING
         if (navStateFlags & NAV_CTL_LAND) {