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

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;