Added nav_fw_pitch2thr_smoothing to cli settings and osd menu

docs/Settings.md

@@ -271,7 +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_threshold | 0 | Threshold from zero pitch before nav_fw_pitch2thr is applied [centidegrees] |
+| nav_fw_pitch2thr_smoothing | 1 | Number of fixedWingPitchToThrottleCorrection cycles used to calculate a moving average pitch |
+| nav_fw_pitch2thr_threshold | 0 | Threshold from average pitch before nav_fw_pitch2thr is applied [centidegrees] |
 | 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,7 @@ static const OSD_Entry cmsx_menuFixedWingEntries[] =
     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

@@ -2260,6 +2260,12 @@ groups:
         field: fw.pitch_to_throttle
         min: 0
         max: 100
+      - name: nav_fw_pitch2thr_smoothing
+        description: "Threshold from zero pitch before pitch_to_throttle is applied [centidegrees]"
+        default_value: "1"
+        field: fw.pitch_to_throttle_smooth
+        min: 1
+        max: 4096
       - name: nav_fw_pitch2thr_threshold
         description: "Threshold from zero pitch before pitch_to_throttle is applied [centidegrees]"
         default_value: "0"

src/main/navigation/navigation.c

@@ -152,7 +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_thresh = 0,          // Threshold from zero pitch before pitch_to_throttle is applied [centidegrees]
+        .pitch_to_throttle_smooth = 1,          // Number of fixedWingPitchToThrottleCorrection cycles used to calculate a moving average pitch
+        .pitch_to_throttle_thresh = 0,          // Threshold from average pitch before pitch_to_throttle is applied [centidegrees]
         .loiter_radius = 5000,                  // 50m
 
         //Fixed wing landing
@@ -1882,13 +1883,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, 
-    const float setpoint, 
-    const float measurement, 
-    const float dt, 
-    const float outMin, 
-    const float outMax, 
-    const pidControllerFlags_e pidFlags, 
+    pidController_t *pid,
+    const float setpoint,
+    const float measurement,
+    const float dt,
+    const float outMin,
+    const float outMax,
+    const pidControllerFlags_e pidFlags,
     const float gainScaler,
     const float dTermScaler
 ) {
@@ -2414,7 +2415,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);
 }
@@ -2425,7 +2426,7 @@ bool isSafeHomeInUse(void)
  **********************************************************/
 bool foundNearbySafeHome(void)
 {
-    safehome_used = -1; 
+    safehome_used = -1;
     fpVector3_t safeHome;
     gpsLocation_t shLLH;
     shLLH.alt = 0;
@@ -2433,7 +2434,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());
@@ -2919,7 +2920,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);
 }
@@ -3219,7 +3220,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

@@ -222,6 +222,7 @@ 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
         uint8_t  pitch_to_throttle_thresh;   // Threshold from zero pitch before pitch_to_throttle is applied [centidegrees]
         uint16_t loiter_radius;              // Loiter radius when executing PH on a fixed wing
         int8_t land_dive_angle;

src/main/navigation/navigation_fixedwing.c

@@ -464,7 +464,7 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
 int16_t fixedWingPitchToThrottleCorrection(int16_t pitch)
 {
     // Calculate base throttle correction from pitch moving average
-    const int16_t movingAverageCycles = 128; //Number of main loop cycles for average calculation
+    const int16_t movingAverageCycles = navConfig()->fw.pitch_to_throttle_smooth;
     static int16_t averagePitch = 0;
 
     averagePitch = (averagePitch * movingAverageCycles + pitch - averagePitch) / movingAverageCycles;