From b5be7fb7de1363c339809621dfece4850a66495c Mon Sep 17 00:00:00 2001 From: Tatsuya Yamaguchi Date: Mon, 29 Jul 2024 17:18:13 +0900 Subject: [PATCH] RC_Channel: add RC_OPTIONS bit to clear RC override by RC input change --- libraries/RC_Channel/RC_Channel.cpp | 16 ++++++++++++++- libraries/RC_Channel/RC_Channel.h | 8 ++++++++ libraries/RC_Channel/RC_Channels.cpp | 29 ++++++++++++++++++++++++++++ 3 files changed, 52 insertions(+), 1 deletion(-) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 4b4f30d79e2883..4545888df98a43 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -290,10 +290,12 @@ bool RC_Channel::get_reverse(void) const // read input from hal.rcin or overrides bool RC_Channel::update(void) { + raw_radio_in = hal.rcin->read(ch_in); + if (has_override() && !rc().option_is_enabled(RC_Channels::Option::IGNORE_OVERRIDES)) { radio_in = override_value; } else if (rc().has_had_rc_receiver() && !rc().option_is_enabled(RC_Channels::Option::IGNORE_RECEIVER)) { - radio_in = hal.rcin->read(ch_in); + radio_in = raw_radio_in; } else { return false; } @@ -479,6 +481,13 @@ bool RC_Channel::in_trim_dz() const return is_bounded_int32(radio_in, radio_trim - dead_zone, radio_trim + dead_zone); } +/* + return true if raw input is within deadzone of trim +*/ +bool RC_Channel::in_raw_trim_dz() const +{ + return is_bounded_int32(raw_radio_in, radio_trim - dead_zone, radio_trim + dead_zone); +} /* return trues if input is within deadzone of min @@ -494,6 +503,11 @@ void RC_Channel::set_override(const uint16_t v, const uint32_t timestamp_ms) return; } + // set the throttle value when the override is first activated + if (override_value == 0 && ch_in == rc().get_throttle_channel().ch_in) { + rc().override_start_throttle = raw_radio_in; + } + last_override_time = timestamp_ms != 0 ? timestamp_ms : AP_HAL::millis(); override_value = v; rc().new_override_received(); diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index b999d007cabae1..58258d9762c6d5 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -65,6 +65,8 @@ class RC_Channel { // return true if input is within deadzone of trim bool in_trim_dz() const; + // return true if raw input is within deadzone of trim + bool in_raw_trim_dz() const; int16_t get_radio_in() const { return radio_in;} void set_radio_in(int16_t val) {radio_in = val;} @@ -378,6 +380,7 @@ class RC_Channel { // pwm is stored here int16_t radio_in; + int16_t raw_radio_in; // value generated from PWM normalised to configured scale int16_t control_in; @@ -496,6 +499,9 @@ class RC_Channels { // is RC channel 1. Beware this is not a cheap call. uint16_t get_override_mask() const; + // determine if the current RC overrides should be cleared due to pilot input + bool should_clear_overrides(void); + class RC_Channel *find_channel_for_option(const RC_Channel::AUX_FUNC option); bool duplicate_options_exist(); RC_Channel::AuxSwitchPos get_channel_pos(const uint8_t rcmapchan) const; @@ -536,6 +542,7 @@ class RC_Channels { USE_CRSF_LQ_AS_RSSI = (1U << 11), // returns CRSF link quality as RSSI value, instead of RSSI CRSF_FM_DISARM_STAR = (1U << 12), // when disarmed, add a star at the end of the flight mode in CRSF telemetry ELRS_420KBAUD = (1U << 13), // use 420kbaud for ELRS protocol + CLEAR_OVERRIDES_BY_RC = (1U << 14), // clear MAVLink overrides if the pilot inputs any of roll/pitch/throttle/yaw }; bool option_is_enabled(Option option) const { @@ -634,6 +641,7 @@ class RC_Channels { bool has_new_overrides; bool _has_had_rc_receiver; // true if we have had a direct detach RC receiver, does not include overrides bool _has_had_override; // true if we have had an override on any channel + int16_t override_start_throttle; // throttle value when override was activated AP_Float _override_timeout; AP_Int32 _options; diff --git a/libraries/RC_Channel/RC_Channels.cpp b/libraries/RC_Channel/RC_Channels.cpp index cd32cf938ba3bd..f8167c6c38f094 100644 --- a/libraries/RC_Channel/RC_Channels.cpp +++ b/libraries/RC_Channel/RC_Channels.cpp @@ -31,6 +31,7 @@ extern const AP_HAL::HAL& hal; #include #include #include +#include #include "RC_Channel.h" @@ -90,9 +91,37 @@ bool RC_Channels::read_input(void) success |= channel(i)->update(); } + // check if clear overrides by RC is allowed by RC_OPTIONS and any change in RC input during RC overrides + if (should_clear_overrides()) { + // clear RC overrides + set_gcs_overrides_enabled(false); + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "RC overrides cleared by pilot input"); + } + return success; } +bool RC_Channels::should_clear_overrides(void) +{ + if (!rc().option_is_enabled(Option::CLEAR_OVERRIDES_BY_RC) || !has_active_overrides()) { + return false; + } + + // check for pilot input exceeding the deadzone on roll, pitch, or yaw + if (!get_roll_channel().in_raw_trim_dz() || + !get_pitch_channel().in_raw_trim_dz() || + !get_yaw_channel().in_raw_trim_dz()) { + return true; + } + + // check if the throttle input has changed beyond the deadzone since the override started + if (abs(get_throttle_channel().raw_radio_in - override_start_throttle) > get_throttle_channel().get_dead_zone()) { + return true; + } + + return false; +} + uint8_t RC_Channels::get_valid_channel_count(void) { return MIN(NUM_RC_CHANNELS, hal.rcin->num_channels());