Merge pull request #6104 from Airwide/aw-fw-nav-smooth-throttle

Smooth throttle in navigation modes on FW

docs/Settings.md

@@ -272,6 +272,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 | 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 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/cms/cms_menu_navigation.c

@@ -109,6 +109,8 @@ static const OSD_Entry cmsx_menuFWCruiseEntries[] =
     OSD_SETTING_ENTRY("PITCH TO THR RATIO", SETTING_NAV_FW_PITCH2THR),
     OSD_SETTING_ENTRY("LOITER RADIUS", SETTING_NAV_FW_LOITER_RADIUS),
     OSD_SETTING_ENTRY("CONTROL SMOOTHNESS", SETTING_NAV_FW_CONTROL_SMOOTHNESS),
+    OSD_SETTING_ENTRY("PITCH TO THR SMOOTHING", SETTING_NAV_FW_PITCH2THR_SMOOTHING),
+    OSD_SETTING_ENTRY("PITCH TO THR THRESHOLD", SETTING_NAV_FW_PITCH2THR_THRESHOLD),
 
     OSD_BACK_AND_END_ENTRY,
 };

src/main/fc/settings.yaml

@@ -2284,6 +2284,18 @@ groups:
         field: fw.pitch_to_throttle
         min: 0
         max: 100
+      - name: nav_fw_pitch2thr_smoothing
+        description:  "How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh."
+        default_value: "0"
+        field: fw.pitch_to_throttle_smooth
+        min: 0
+        max: 9
+      - name: nav_fw_pitch2thr_threshold
+        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
+        max: 900
       - name: nav_fw_loiter_radius
         description: "PosHold radius. 3000 to 7500 is a good value (30-75m) [cm]"
         default_value: "5000"

src/main/flight/rth_estimator.c

@@ -77,7 +77,7 @@ static int8_t RTHInitialAltitudeChangePitchAngle(float altitudeChange) {
 // pitch in degrees
 // output in Watt
 static float estimatePitchPower(float pitch) {
-    int16_t altitudeChangeThrottle = fixedWingPitchToThrottleCorrection(DEGREES_TO_DECIDEGREES(pitch));
+    int16_t altitudeChangeThrottle = (int16_t)pitch * navConfig()->fw.pitch_to_throttle;
     altitudeChangeThrottle = constrain(altitudeChangeThrottle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
     const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - getThrottleIdleValue()) / (navConfig()->fw.cruise_throttle - getThrottleIdleValue());
     return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100;

src/main/navigation/navigation.c

@@ -156,6 +156,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 = 0,
+        .pitch_to_throttle_thresh = 0,
         .loiter_radius = 5000,                  // 50m
 
         //Fixed wing landing

src/main/navigation/navigation.h

@@ -228,6 +228,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;    // 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 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
@@ -468,7 +470,7 @@ bool loadNonVolatileWaypointList(void);
 bool saveNonVolatileWaypointList(void);
 
 float getFinalRTHAltitude(void);
-int16_t fixedWingPitchToThrottleCorrection(int16_t pitch);
+int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs);
 
 /* Geodetic functions */
 typedef enum {

src/main/navigation/navigation_fixedwing.c

@@ -76,6 +76,15 @@ static int8_t loiterDirYaw = 1;
 static float getSmoothnessCutoffFreq(float baseFreq)
 {
     uint16_t smoothness = 10 - navConfig()->fw.control_smoothness;
+    return 0.001f * baseFreq * (float)(smoothness*smoothness*smoothness) + 0.01f;
+}
+
+// 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;
 }
 
@@ -461,9 +470,25 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
     return throttleSpeedAdjustment;
 }
 
-int16_t fixedWingPitchToThrottleCorrection(int16_t pitch)
+int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs)
 {
-    return DECIDEGREES_TO_DEGREES(pitch) * navConfig()->fw.pitch_to_throttle;
+    static timeUs_t previousTimePitchToThrCorr = 0;
+    const timeDelta_t deltaMicrosPitchToThrCorr = currentTimeUs -  previousTimePitchToThrCorr;
+    previousTimePitchToThrCorr = currentTimeUs;
+
+    static pt1Filter_t pitchToThrFilterState;
+
+    // Apply low-pass filter to pitch angle to smooth throttle correction
+    int16_t filteredPitch = (int16_t)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(filteredPitch) * navConfig()->fw.pitch_to_throttle;
+    }
 }
 
 void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
@@ -485,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) {