From 327f3a0803fc404191e007a5f95ea41664b52d43 Mon Sep 17 00:00:00 2001 From: Hwurzburg Date: Mon, 30 Aug 2021 13:57:10 -0500 Subject: [PATCH] ArduPlane: add option for man throttle center to be TRIM_THROTTLE --- ArduPlane/AP_Arming.cpp | 8 ++++++++ ArduPlane/Parameters.cpp | 2 +- ArduPlane/Plane.h | 1 + ArduPlane/defines.h | 1 + ArduPlane/reverse_thrust.cpp | 16 ++++++++++++++++ ArduPlane/servos.cpp | 3 ++- 6 files changed, 29 insertions(+), 2 deletions(-) diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index 0d6d5a15f58ea..5167e404c06e8 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -122,6 +122,14 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure) ret = false; } + if (plane.g2.flight_options & FlightOptions::CENTER_THROTTLE_TRIM){ + int16_t trim = plane.channel_throttle->get_radio_trim(); + if (trim < 1250 || trim > 1750) { + check_failed(display_failure, "Throttle trim not near center stick(%u)",trim ); + ret = false; + } + } + return ret; } diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index b5b4d7d69ad54..1a955de75040f 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1099,7 +1099,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Param: FLIGHT_OPTIONS // @DisplayName: Flight mode options // @Description: Flight mode specific options - // @Bitmask: 0:Rudder mixing in direct flight modes only (Manual / Stabilize / Acro),1:Use centered throttle in Cruise or FBWB to indicate trim airspeed, 2:Disable attitude check for takeoff arming, 3:Force target airspeed to trim airspeed in Cruise or FBWB, 4: Climb to ALT_HOLD_RTL before turning for RTL, 5: Enable yaw damper in acro mode, 6: Surpress speed scaling during auto takeoffs to be 1 or less to prevent oscillations without airpseed sensor., 7:EnableDefaultAirspeed for takeoff, 8: Remove the TRIM_PITCH_CD on the GCS horizon, 9: Remove the TRIM_PITCH_CD on the OSD horizon + // @Bitmask: 0:Rudder mixing in direct flight modes only (Manual / Stabilize / Acro),1:Use centered throttle in Cruise or FBWB to indicate trim airspeed, 2:Disable attitude check for takeoff arming, 3:Force target airspeed to trim airspeed in Cruise or FBWB, 4: Climb to ALT_HOLD_RTL before turning for RTL, 5: Enable yaw damper in acro mode, 6: Surpress speed scaling during auto takeoffs to be 1 or less to prevent oscillations without airpseed sensor., 7:EnableDefaultAirspeed for takeoff, 8: Remove the TRIM_PITCH_CD on the GCS horizon, 9: Remove the TRIM_PITCH_CD on the OSD horizon, 10: Adjust mid-throttle to be TRIM_THROTTLE in non-auto throttle modes except MANUAL // @User: Advanced AP_GROUPINFO("FLIGHT_OPTIONS", 13, ParametersG2, flight_options, 0), diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 06fde5c7faa2f..b54fb676275f2 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1093,6 +1093,7 @@ class Plane : public AP_Vehicle { bool allow_reverse_thrust(void) const; bool have_reverse_thrust(void) const; int16_t get_throttle_input(bool no_deadzone=false) const; + int16_t get_adjusted_throttle_input(bool no_deadzone=false) const; enum Failsafe_Action { Failsafe_Action_None = 0, diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index e5918779f9374..cc32ceafec6c8 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -163,6 +163,7 @@ enum FlightOptions { ENABLE_DEFAULT_AIRSPEED = (1<<7), GCS_REMOVE_TRIM_PITCH_CD = (1 << 8), OSD_REMOVE_TRIM_PITCH_CD = (1 << 9), + CENTER_THROTTLE_TRIM = (1<<10), }; enum CrowFlapOptions { diff --git a/ArduPlane/reverse_thrust.cpp b/ArduPlane/reverse_thrust.cpp index a652de78e5a0f..c984e123c5c16 100644 --- a/ArduPlane/reverse_thrust.cpp +++ b/ArduPlane/reverse_thrust.cpp @@ -137,3 +137,19 @@ int16_t Plane::get_throttle_input(bool no_deadzone) const } return ret; } + +/* + return control in from the radio throttle channel with curve giving mid-stick equal to TRIM_THROTTLE. + */ +int16_t Plane::get_adjusted_throttle_input(bool no_deadzone) const +{ + if ((plane.channel_throttle->get_type() != RC_Channel::RC_CHANNEL_TYPE_RANGE) || (g2.flight_options & FlightOptions::CENTER_THROTTLE_TRIM) == 0) { + return get_throttle_input(no_deadzone); + } + int16_t ret = channel_throttle->get_range() * throttle_curve(aparm.throttle_cruise * 0.01, 0, 0.5 + 0.5*channel_throttle->norm_input()); + if (reversed_throttle) { + // RC option for reverse throttle has been set + return -ret; + } + return ret; +} diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 018c7e7ff817f..1ccab9369e6f6 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -541,8 +541,9 @@ void Plane::set_servos_controlled(void) // STABILIZE mode with THR_PASS_STAB set SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true)); } else { + // get throttle, but adjust center to output TRIM_THROTTLE if flight option set SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, - constrain_int16(get_throttle_input(true), min_throttle, max_throttle)); + constrain_int16(get_adjusted_throttle_input(true), min_throttle, max_throttle)); } } else if (control_mode->is_guided_mode() && guided_throttle_passthru) {