ArduPlane: add option for man throttle center to be TRIM_THROTTLE

Icatme · Sep 7, 2021 · 327f3a0 · 327f3a0
1 parent 95fbf39
commit 327f3a0
Show file tree

Hide file tree

Showing 6 changed files with 29 additions and 2 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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) {