diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index e17f04d3a2..e535984bff 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -58,7 +58,7 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure) } if (plane.channel_throttle->get_reverse() && - plane.g.throttle_fs_enabled && + Plane::ThrFailsafe(plane.g.throttle_fs_enabled.get()) != Plane::ThrFailsafe::Disabled && plane.g.throttle_fs_value < plane.channel_throttle->get_radio_max()) { check_failed(display_failure, "Invalid THR_FS_VALUE for rev throttle"); diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 01da8a881f..87e67dbb59 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -460,10 +460,10 @@ const AP_Param::Info Plane::var_info[] = { // @Param: THR_FAILSAFE // @DisplayName: Throttle and RC Failsafe Enable - // @Description: This enables failsafe on loss of RC input. How this is detected depends on the type of RC receiver being used. For older radios an input below the THR_FS_VALUE is used to trigger failsafe. For newer radios the failsafe trigger is part of the protocol between the autopilot and receiver. - // @Values: 0:Disabled,1:Enabled + // @Description: This enables failsafe on loss of RC input. How this is detected depends on the type of RC receiver being used. For older radios an input below the THR_FS_VALUE is used to trigger failsafe. For newer radios the failsafe trigger is part of the protocol between the autopilot and receiver. A value of 2 means that the RC input won't be used when throttle goes below the THR_FS_VALUE, but it won't trigger a failsafe + // @Values: 0:Disabled,1:Enabled,2:EnabledNoFailsafe // @User: Standard - GSCALAR(throttle_fs_enabled, "THR_FAILSAFE", 1), + GSCALAR(throttle_fs_enabled, "THR_FAILSAFE", int(ThrFailsafe::Enabled)), // @Param: THR_FS_VALUE diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 5ebe6332c5..abc6d36e06 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1056,6 +1056,12 @@ private: static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1, "_failsafe_priorities is missing the sentinel"); + enum class ThrFailsafe { + Disabled = 0, + Enabled = 1, + EnabledNoFS = 2 + }; + public: void mavlink_delay_cb(); void failsafe_check(void); diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index ac1563c0fd..5d4d9a1f09 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -282,7 +282,7 @@ void Plane::control_failsafe() } } - if(g.throttle_fs_enabled == 0) { + if (ThrFailsafe(g.throttle_fs_enabled.get()) != ThrFailsafe::Enabled) { return; } @@ -375,7 +375,7 @@ bool Plane::trim_radio() */ bool Plane::rc_throttle_value_ok(void) const { - if (!g.throttle_fs_enabled) { + if (ThrFailsafe(g.throttle_fs_enabled.get()) == ThrFailsafe::Disabled) { return true; } if (channel_throttle->get_reverse()) {