Plane: FBWA four stages throttle to pitch compensation (ArduPilot#166)

ArduPlane/Attitude.cpp

@@ -191,10 +191,10 @@ float Plane::stabilize_pitch_get_pitch_out(float speed_scaler)
     const float throttle_value = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
 
     if (is_positive(throttle_value)) {
-        throttle_pitch_mix_ratio = sqrtf(linear_interpolate(0, 1, throttle_value, aparm.throttle_cruise, 100));
+        throttle_pitch_mix_ratio = square_curve_interpolate(0, 1, 100, throttle_value, aparm.throttle_cruise.get(), 100);
     }
 
-    int32_t demanded_pitch = nav_pitch_cd + (g.pitch_trim + throttle_pitch_mix_ratio * g.kff_throttle_above_trim_to_pitch) * 100;
+    int32_t demanded_pitch = nav_pitch_cd + (g.pitch_trim + throttle_pitch_mix_ratio * g.kff_throttle_above_trim_to_pitch.get()) * 100;
     bool disable_integrator = false;
     if (control_mode == &mode_stabilize && !is_zero(channel_pitch->get_control_in())) {
         disable_integrator = true;
@@ -737,11 +737,11 @@ void Plane::set_auto_thr_gliding(bool gliding_requested)
 void Plane::calc_nav_pitch()
 {
     if (auto_thr_gliding_state != ATGS_DISABLED && !(ahrs.airspeed_sensor_enabled() || TECS_controller.is_using_synthetic_airspeed())) {
-        if (auto_thr_gliding_state == ATGS_WAITING_FOR_CRUISE_THR && (TECS_controller.get_throttle_demand() >= aparm.throttle_cruise || channel_pitch->get_control_in() < 0)) {
+        if (auto_thr_gliding_state == ATGS_WAITING_FOR_CRUISE_THR && (TECS_controller.get_throttle_demand() >= aparm.throttle_cruise.get() || channel_pitch->get_control_in() < 0)) {
             auto_thr_gliding_state = ATGS_DISABLED;
         } else {
             nav_pitch_cd = 0;
-            adjust_nav_pitch_throttle();
+            fbwa_throttle_to_pitch_compensation(false);
             return;
         }
     }
@@ -809,27 +809,53 @@ void Plane::calc_nav_roll()
     update_load_factor();
 }
 
-/*
-  adjust nav_pitch_cd for STAB_PITCH_DOWN_CD. This is used to make
-  keeping up good airspeed in FBWA mode easier, as the plane will
-  automatically pitch down a little when at low throttle. It makes
-  FBWA landings without stalling much easier.
- */
-void Plane::adjust_nav_pitch_throttle(void)
+void Plane::fbwa_throttle_to_pitch_compensation(bool do_pitch_up)
 {
     if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_VTOL) {
-        float throttle_pitch_mix = 100; // %
+        // necessary because we call this not only for FBWA but also in auto throttle modes when gliding is requested
         const float throttle_value = control_mode->does_auto_throttle() ? TECS_controller.get_throttle_demand() : get_throttle_input(true);
 
-        if (is_positive(throttle_value)) {
-            throttle_pitch_mix = square_expo_curve(linear_interpolate(100, 0, throttle_value, 0, aparm.throttle_cruise), g.stab_pitch_down_curve);
-        }
+        if (throttle_value < aparm.throttle_cruise) {
+
+            const float max_pitch_up = do_pitch_up && !is_zero(g.fbwa_max_pitch_up_thr.get()) ? g.fbwa_max_pitch_up.get() : 0;
+            const float min_pitch_up_thr = is_zero(g.fbwa_min_pitch_up_thr) ? aparm.throttle_cruise.get() : MIN(g.fbwa_min_pitch_up_thr.get(), aparm.throttle_cruise.get());
+            const float max_pitch_up_thr = is_zero(max_pitch_up) ? aparm.throttle_cruise.get() : MIN(g.fbwa_max_pitch_up_thr.get(), aparm.throttle_cruise.get());
+            const float max_pitch_down_thr = MIN(g.fbwa_max_pitch_down_thr.get(), max_pitch_up_thr);
+
+            float ia, ib, oa, ob;
+
+            ia = min_pitch_up_thr;
+            ib = max_pitch_up_thr;
+            oa = 0;
+            ob = max_pitch_up;
+
+            if (g.fbwa_pitch_up_curve_reversal) {
+                swap_float(ia, ib);
+                swap_float(oa, ob);
+            }
 
-        nav_pitch_cd -= throttle_pitch_mix * g.stab_pitch_down;
+            // pitch up compensation
+            // const float pitch_up_compensation = cube_curve_interpolate(max_pitch_up, 0, g.fbwa_pitch_up_curve.get(), throttle_value, max_pitch_up_thr, aparm.throttle_cruise.get());
+            const float pitch_up_compensation = cube_curve_interpolate(oa, ob, g.fbwa_pitch_up_curve.get(), throttle_value, ia, ib);
+
+            ia = max_pitch_up_thr;
+            ib = max_pitch_down_thr;
+            oa = 0;
+            ob = max_pitch_up + g.fbwa_max_pitch_down.get();
+
+            if (g.fbwa_pitch_down_curve_reversal) {
+                swap_float(ia, ib);
+                swap_float(oa, ob);
+            }
+
+            // pitch down compensation
+            const float pitch_down_compensation = cube_curve_interpolate(oa, ob, g.fbwa_pitch_down_curve.get(), throttle_value, ia, ib);
+
+            nav_pitch_cd += (pitch_up_compensation - pitch_down_compensation) * 100;
+        }
     }
 }
 
-
 /*
   calculate a new aerodynamic_load_factor and limit nav_roll_cd to
   ensure that the load factor does not take us below the sustainable

ArduPlane/Parameters.cpp

@@ -92,23 +92,82 @@ const AP_Param::Info Plane::var_info[] = {
     // @User: Advanced
     GSCALAR(kff_flap_to_elevator,  "KFF_FLAP2ELEV",   0),
 
-    // @Param: STAB_PITCH_DOWN
-    // @DisplayName: Low throttle pitch down trim
-    // @Description: Degrees of down pitch added when throttle is below TRIM_THROTTLE in FBWA and AUTOTUNE modes. Scales linearly so full value is added when THR_MIN is reached. Helps to keep airspeed higher in glides or landing approaches and prevents accidental stalls. 2 degrees recommended for most planes.
+    // @Param: FBWA_PITCH_DOWN
+    // @DisplayName: Max pitch down in FBWA/AUTOTUNE applied when throttle is bellow FBWA_MPTCHDN_THR
+    // @Description: Part of the FBWA throttle to pitch compensation system. See the documentation for this system for more information. Max pitch down in FBWA/AUTOTUNE applied when throttle is bellow FBWA_MPTCHDN_THR
     // @Range: 0 15
     // @Increment: 0.1
     // @Units: deg
     // @User: Advanced
-    GSCALAR(stab_pitch_down, "STAB_PITCH_DOWN",   2.0f),
+    GSCALAR(fbwa_max_pitch_down, "FBWA_PITCH_DOWN",   2.0f),
 
-    // @Param: STAB_PITCH_DCRV
-    // @DisplayName: Low throttle pitch down curve
-    // @Description: -100=square pitch down curve, 0=linear pitch down curve, 100=square root pitch down curve
+    // @Param: FBWA_MXPTCHD_THR
+    // @DisplayName: Throttle bellow which the plane will pitch down at FBWA_PITCH_DOWN
+    // @Description: Part of the FBWA throttle to pitch compensation system. See the documentation for this system for more information. Throttle bellow which the plane will pitch down at FBWA_PITCH_DOWN
+    // @Range: 0 100
+    // @Increment: 1
+    // @Units: %
+    // @User: Advanced
+    GSCALAR(fbwa_max_pitch_down_thr, "FBWA_MXPTCHD_THR",   0),
+
+    // @Param: FBWA_PTCHDN_CRV
+    // @DisplayName: Determines the shape of the pitch down curve applied by the FBWA throttle to pitch compensation system
+    // @Description: -100=square pitch down curve, 0=linear pitch curve, 100=square root pitch curve
     // @Range: -100 100
     // @Increment: 1
     // @Units: %
     // @User: Advanced
-    GSCALAR(stab_pitch_down_curve, "STAB_PITCH_DCRV",   100),
+    GSCALAR(fbwa_pitch_down_curve, "FBWA_PTCHDN_CRV",   0),
+
+    // @Param: FBWA_PTCHDN_CRVR
+    // @DisplayName: Pitch down curve reversal
+    // @Description: Pitch down curve reversal
+    // @Range: 0 1
+    // @User: Advanced
+    GSCALAR(fbwa_pitch_down_curve_reversal, "FBWA_PTCHDN_CRVR",   0),
+
+    // @Param: FBWA_PITCH_UP
+    // @DisplayName: Max pitch up in FBWA/AUTOTUNE applied when throttle is at FBWA_MPTCHUP_THR
+    // @Description: Part of the FBWA throttle to pitch compensation system. See the documentation for this system for more information. Max pitch up in FBWA/AUTOTUNE applied when throttle is at FBWA_MPTCHUP_THR
+    // @Range: 0 15
+    // @Increment: 0.1
+    // @Units: deg
+    // @User: Advanced
+    GSCALAR(fbwa_max_pitch_up, "FBWA_PITCH_UP",   2.0f),
+
+    // @Param: FBWA_MXPTCHU_THR
+    // @DisplayName: Throttle at which the maximum pitch up FBWA_PITCH_UP will be applied
+    // @Description: Part of the FBWA throttle to pitch compensation system. See the documentation for this system for more information. 
+    // @Range: 0 100
+    // @Increment: 1
+    // @Units: %
+    // @User: Advanced
+    GSCALAR(fbwa_max_pitch_up_thr, "FBWA_MXPTCHU_THR",   0),
+
+    // @Param: FBWA_MNPTCHU_THR
+    // @DisplayName: Throttle at bellow which to start pitching up
+    // @Description: Part of the FBWA throttle to pitch compensation system. See the documentation for this system for more information. 
+    // @Range: 0 100
+    // @Increment: 1
+    // @Units: %
+    // @User: Advanced
+    GSCALAR(fbwa_min_pitch_up_thr, "FBWA_MNPTCHU_THR",   0),
+
+    // @Param: FBWA_PTCHUP_CRV
+    // @DisplayName: Determines the shape of the pitch up curve applied by the FBWA throttle to pitch compensation system
+    // @Description: -100=square pitch curve, 0=linear pitch curve, 100=square root pitch curve
+    // @Range: -100 100
+    // @Increment: 1
+    // @Units: %
+    // @User: Advanced
+    GSCALAR(fbwa_pitch_up_curve, "FBWA_PTCHUP_CRV",   0),
+
+    // @Param: FBWA_PTCHUP_CRVR
+    // @DisplayName: Pitch up curve reversal
+    // @Description: Pitch up curve reversal
+    // @Range: 0 1
+    // @User: Advanced
+    GSCALAR(fbwa_pitch_up_curve_reversal, "FBWA_PTCHUP_CRVR",   0),
 
     // @Param: GLIDE_SLOPE_MIN
     // @DisplayName: Glide slope minimum
@@ -1477,10 +1536,7 @@ static const AP_Param::ConversionInfo conversion_table[] = {
     { Parameters::k_param_land_pre_flare_alt, 0,      AP_PARAM_FLOAT, "LAND_PF_ALT" },
     { Parameters::k_param_land_pre_flare_airspeed, 0, AP_PARAM_FLOAT, "LAND_PF_ARSPD" },
     { Parameters::k_param_land_throttle_slewrate, 0,  AP_PARAM_INT8,  "LAND_THR_SLEW" },
-    { Parameters::k_param_land_disarm_delay,  0,      AP_PARAM_INT8,  "LAND_DISARMDELAY" },
     { Parameters::k_param_land_then_servos_neutral,0, AP_PARAM_INT8,  "LAND_THEN_NEUTRAL" },
-    { Parameters::k_param_land_abort_throttle_enable,0,AP_PARAM_INT8, "LAND_ABORT_THR" },
-    { Parameters::k_param_land_flap_percent,  0,      AP_PARAM_INT8,  "LAND_FLAP_PERCENT" },
 
     // battery failsafes
     { Parameters::k_param_fs_batt_mah,        0,      AP_PARAM_FLOAT, "BATT_LOW_MAH" },
@@ -1508,7 +1564,6 @@ static const RCConversionInfo rc_option_conversion[] = {
     { Parameters::k_param_fence_channel, 0, RC_Channel::AUX_FUNC::FENCE},
     { Parameters::k_param_reset_mission_chan, 0, RC_Channel::AUX_FUNC::MISSION_RESET},
     { Parameters::k_param_parachute_channel, 0, RC_Channel::AUX_FUNC::PARACHUTE_RELEASE},
-    { Parameters::k_param_fbwa_tdrag_chan, 0, RC_Channel::AUX_FUNC::FBWA_TAILDRAGGER},
     { Parameters::k_param_reset_switch_chan, 0, RC_Channel::AUX_FUNC::MODE_SWITCH_RESET},
 };
 

ArduPlane/Parameters.h

@@ -116,29 +116,29 @@ class Parameters {
         k_param_rangefinder,
         k_param_terrain,
         k_param_terrain_follow,
-        k_param_stab_pitch_down_curve,
+        k_param_fbwa_pitch_down_curve,
         k_param_glide_slope_min,
-        k_param_stab_pitch_down,
+        k_param_fbwa_max_pitch_down,
         k_param_terrain_lookahead,
-        k_param_fbwa_tdrag_chan, // unused - moved to RC option
+        k_param_fbwa_max_pitch_down_thr,
         k_param_rangefinder_landing,
-        k_param_land_flap_percent,  // unused - moved to AP_Landing
+        k_param_fbwa_max_pitch_up_thr,
         k_param_takeoff_flap_percent,
         k_param_flap_slewrate,
         k_param_rtl_autoland,
         k_param_override_channel,
         k_param_stall_prevention,
         k_param_optflow,
-        k_param_cli_enabled_old, // unused - CLI removed
-        k_param_trim_rc_at_start, // unused
-        k_param_hil_mode_unused,  // unused
-        k_param_land_disarm_delay,  // unused - moved to AP_Landing
+        k_param_fbwa_max_pitch_up,
+        k_param_fbwa_pitch_up_curve,
+        k_param_fbwa_min_pitch_up_thr,
+        k_param_fbwa_pitch_up_curve_reversal,
         k_param_glide_slope_threshold,
         k_param_rudder_only,
         k_param_gcs3,            // 93
         k_param_gcs_pid_mask,
         k_param_crash_detection_enable,
-        k_param_land_abort_throttle_enable, // unused - moved to AP_Landing
+        k_param_fbwa_pitch_down_curve_reversal,
         k_param_rssi = 97,
         k_param_rpm_sensor,
         k_param_parachute,
@@ -376,8 +376,15 @@ class Parameters {
     AP_Float kff_flap_to_elevator;
     AP_Float ground_steer_alt;
     AP_Int16 ground_steer_dps;
-    AP_Float stab_pitch_down;
-    AP_Float stab_pitch_down_curve;
+    AP_Float fbwa_max_pitch_down;
+    AP_Float fbwa_max_pitch_down_thr;
+    AP_Float fbwa_pitch_down_curve;
+    AP_Int8 fbwa_pitch_down_curve_reversal;
+    AP_Float fbwa_max_pitch_up;
+    AP_Float fbwa_max_pitch_up_thr;
+    AP_Float fbwa_min_pitch_up_thr;
+    AP_Float fbwa_pitch_up_curve;
+    AP_Int8 fbwa_pitch_up_curve_reversal;
 
     // speed used for speed scaling
     AP_Float scaling_speed;

ArduPlane/Plane.h

@@ -882,7 +882,7 @@ class Plane : public AP_Vehicle {
     bool rc_failsafe(void) const override { return failsafe.rc_failsafe; }
 
     // Attitude.cpp
-    void adjust_nav_pitch_throttle(void);
+    void fbwa_throttle_to_pitch_compensation(bool do_pitch_up);
     void update_load_factor(void);
     void adjust_altitude_target();
     void setup_glide_slope(void);

ArduPlane/mode_fbwa.cpp

@@ -12,7 +12,7 @@ void ModeFBWA::update()
     } else {
         plane.nav_pitch_cd = -(pitch_input * plane.pitch_limit_min_cd);
     }
-    plane.adjust_nav_pitch_throttle();
+    plane.fbwa_throttle_to_pitch_compensation(true);
     plane.nav_pitch_cd = constrain_int32(plane.nav_pitch_cd, plane.pitch_limit_min_cd, plane.aparm.pitch_limit_max_cd.get());
     if (plane.fly_inverted()) {
         plane.nav_pitch_cd = -plane.nav_pitch_cd;

ArduPlane/radio.cpp

@@ -192,8 +192,8 @@ void Plane::read_radio()
         if (ahrs.airspeed_sensor_enabled()) {
             airspeed_nudge_cm = (aparm.airspeed_max * 100 - aparm.airspeed_cruise_cm) * nudge;
         } else {
-            const float throttle_cruise = square_expo_curve(aparm.throttle_cruise, g2.throttle_expo_auto);
-            const float throttle_max = square_expo_curve(aparm.throttle_max, g2.throttle_expo_auto);
+            const float throttle_cruise = square_expo_curve_100(aparm.throttle_cruise, g2.throttle_expo_auto);
+            const float throttle_max = square_expo_curve_100(aparm.throttle_max, g2.throttle_expo_auto);
             throttle_nudge = (throttle_max - throttle_cruise) * nudge;
         }
     }

ArduPlane/servos.cpp

@@ -533,7 +533,7 @@ void Plane::apply_throttle_expo(void)
 {
     const float expo_param = control_mode->does_auto_throttle() ? g2.throttle_expo_auto : g2.throttle_expo_manual;
     const float input_throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
-    const float output_throttle = square_expo_curve(input_throttle, -expo_param);
+    const float output_throttle = square_expo_curve_100(input_throttle, -expo_param);
     SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, output_throttle);
 }
 
@@ -587,8 +587,8 @@ void Plane::set_servos_controlled(void)
     }
 
     // convert 0 to 100% (or -100 to +100) into PWM
-    int8_t min_throttle = square_expo_curve(aparm.throttle_min.get(), g2.throttle_expo_auto);
-    int8_t max_throttle = square_expo_curve(aparm.throttle_max.get(), g2.throttle_expo_auto);
+    int8_t min_throttle = square_expo_curve_100(aparm.throttle_min.get(), g2.throttle_expo_auto);
+    int8_t max_throttle = square_expo_curve_100(aparm.throttle_max.get(), g2.throttle_expo_auto);
 
 #if AP_ICENGINE_ENABLED
     // apply idle governor