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) |
@@ -486,4 +488,4 @@
 | yaw_lpf_hz | 30 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) |
 | yaw_rate | 20 | Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. |
 
-> Note: this table is autogenerated. Do not edit it manually.
+> Note: this table is autogenerated. Do not edit it manually.

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
@@ -1886,13 +1888,13 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
 // Control System Design, Lecture Notes for ME 155A by Karl Johan Åström (p.228)
 // http://www.cds.caltech.edu/~murray/courses/cds101/fa02/caltech/astrom-ch6.pdf
 float navPidApply3(
-    pidController_t *pid, 
+    pidController_t *pid,
-    const float setpoint, 
+    const float setpoint,
-    const float measurement, 
+    const float measurement,
-    const float dt, 
+    const float dt,
-    const float outMin, 
+    const float outMin,
-    const float outMax, 
+    const float outMax,
-    const pidControllerFlags_e pidFlags, 
+    const pidControllerFlags_e pidFlags,
     const float gainScaler,
     const float dTermScaler
 ) {
@@ -2418,7 +2420,7 @@ static navigationHomeFlags_t navigationActualStateHomeValidity(void)
 /*******************************************************
  * Is a safehome being used instead of the arming point?
  *******************************************************/
-bool isSafeHomeInUse(void) 
+bool isSafeHomeInUse(void)
 {
     return (safehome_used > -1 && safehome_used < MAX_SAFE_HOMES);
 }
@@ -2429,7 +2431,7 @@ bool isSafeHomeInUse(void)
  **********************************************************/
 bool foundNearbySafeHome(void)
 {
-    safehome_used = -1; 
+    safehome_used = -1;
     fpVector3_t safeHome;
     gpsLocation_t shLLH;
     shLLH.alt = 0;
@@ -2437,7 +2439,7 @@ bool foundNearbySafeHome(void)
         shLLH.lat = safeHomeConfig(i)->lat;
         shLLH.lon = safeHomeConfig(i)->lon;
         geoConvertGeodeticToLocal(&safeHome, &posControl.gpsOrigin, &shLLH, GEO_ALT_RELATIVE);
-        safehome_distance = calculateDistanceToDestination(&safeHome); 
+        safehome_distance = calculateDistanceToDestination(&safeHome);
         if (safehome_distance < 20000) { // 200 m
              safehome_used = i;
              setHomePosition(&safeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
@@ -2937,7 +2939,7 @@ bool saveNonVolatileWaypointList(void)
 
 #if defined(USE_SAFE_HOME)
 
-void resetSafeHomes(void) 
+void resetSafeHomes(void)
 {
     memset(safeHomeConfigMutable(0), 0, sizeof(navSafeHome_t) * MAX_SAFE_HOMES);
 }
@@ -3237,7 +3239,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
             return NAV_FSM_EVENT_SWITCH_TO_IDLE;
         }
 
-        // Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded 
+        // Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded
         if (IS_RC_MODE_ACTIVE(BOXNAVWP)) {
             if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivatePosHold && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME)))
                 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;

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

@@ -51,8 +51,8 @@
 
 #include "rx/rx.h"
 
-// Base frequencies for smoothing pitch and roll 
+// Base frequencies for smoothing pitch and roll
-#define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ     2.0f      
+#define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ     2.0f
 #define NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ     10.0f
 
 // If we are going slower than NAV_FW_MIN_VEL_SPEED_BOOST - boost throttle to fight against the wind
@@ -70,12 +70,21 @@ static bool isAutoThrottleManuallyIncreased = false;
 static int32_t navHeadingError;
 static int8_t loiterDirYaw = 1;
 
-// Calculates the cutoff frequency for smoothing out roll/pitch commands 
+// Calculates the cutoff frequency for smoothing out roll/pitch commands
 // control_smoothness valid range from 0 to 9
-// resulting cutoff_freq ranging from baseFreq downwards to ~0.11Hz 
+// resulting cutoff_freq ranging from baseFreq downwards to ~0.11Hz
 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;
 }
 
@@ -205,7 +214,7 @@ static fpVector3_t virtualDesiredPosition;
 static pt1Filter_t fwPosControllerCorrectionFilterState;
 
 /*
- * TODO Currently this function resets both FixedWing and Rover & Boat position controller 
+ * TODO Currently this function resets both FixedWing and Rover & Boat position controller
  */
 void resetFixedWingPositionController(void)
 {
@@ -300,9 +309,9 @@ float processHeadingYawController(timeDelta_t deltaMicros, int32_t navHeadingErr
     const pidControllerFlags_e yawPidFlags = errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0;
 
     const float yawAdjustment = navPidApply2(
-        &posControl.pids.fw_heading, 
+        &posControl.pids.fw_heading,
-        0, 
+        0,
-        applyDeadband(navHeadingError, navConfig()->fw.yawControlDeadband * 100), 
+        applyDeadband(navHeadingError, navConfig()->fw.yawControlDeadband * 100),
         US2S(deltaMicros),
         -limit,
         limit,
@@ -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) {