From 4253c7f74d6aa0f63b0eacb8c2286e7a6b8fed62 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Wed, 25 Apr 2018 19:06:19 -0700 Subject: [PATCH] RC_Channel: Move overrides out of the HAL --- libraries/RC_Channel/RC_Channel.cpp | 22 +++++++++++++++- libraries/RC_Channel/RC_Channel.h | 15 ++++++++++- libraries/RC_Channel/RC_Channels.cpp | 38 +++++++++++++++++++++++++--- 3 files changed, 69 insertions(+), 6 deletions(-) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 7aedba4481..17855593e6 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -112,7 +112,11 @@ RC_Channel::get_reverse(void) const void RC_Channel::set_pwm(int16_t pwm) { - radio_in = pwm; + if (has_override()) { + radio_in = override_value; + } else { + radio_in = pwm; + } if (type_in == RC_CHANNEL_TYPE_RANGE) { control_in = pwm_to_range(); @@ -335,6 +339,22 @@ bool RC_Channel::in_trim_dz() return is_bounded_int32(radio_in, radio_trim - dead_zone, radio_trim + dead_zone); } +void RC_Channel::set_override(const uint16_t v, const uint32_t timestamp_us) +{ + last_override_time = timestamp_us != 0 ? timestamp_us : AP_HAL::millis(); + override_value = v; +} + +void RC_Channel::clear_override() +{ + last_override_time = 0; + override_value = 0; +} + +bool RC_Channel::has_override() const +{ + return (override_value > 0) && ((AP_HAL::millis() - last_override_time) < *RC_Channels::override_timeout * 1000); +} bool RC_Channel::min_max_configured() const { diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 8e8becc6bd..3837e861ac 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -82,6 +82,10 @@ public: int16_t get_control_in() const { return control_in;} void set_control_in(int16_t val) { control_in = val;} + void clear_override(); + void set_override(const uint16_t v, const uint32_t timestamp_us=0); + bool has_override() const; + // get control input with zero deadzone int16_t get_control_in_zero_dz(void); @@ -123,6 +127,10 @@ private: // the input channel this corresponds to uint8_t ch_in; + // overrides + uint16_t override_value; + uint32_t last_override_time; + // bits set when channel has been identified as configured static uint32_t configured_mask; @@ -137,6 +145,7 @@ private: class RC_Channels { public: friend class SRV_Channels; + friend class RC_Channel; // constructor RC_Channels(void); @@ -155,10 +164,14 @@ public: static bool read_input(void); // returns true if new input has been read in static void clear_overrides(void); // clears any active overrides static bool receiver_bind(const int dsmMode); // puts the reciever in bind mode if present, returns true if success - static bool set_override(const uint8_t chan, const int16_t value); // set a channels override value + static void set_override(const uint8_t chan, const int16_t value, const uint32_t timestamp_ms = 0); // set a channels override value + static bool has_active_overrides(void); // returns true if there are overrides applied that are valid private: // this static arrangement is to avoid static pointers in AP_Param tables static RC_Channel *channels; + static bool has_new_overrides; + static AP_Float *override_timeout; RC_Channel obj_channels[NUM_RC_CHANNELS]; + AP_Float _override_timeout; }; diff --git a/libraries/RC_Channel/RC_Channels.cpp b/libraries/RC_Channel/RC_Channels.cpp index ac79392ac2..e86b46680e 100644 --- a/libraries/RC_Channel/RC_Channels.cpp +++ b/libraries/RC_Channel/RC_Channels.cpp @@ -29,6 +29,8 @@ extern const AP_HAL::HAL& hal; #include "RC_Channel.h" RC_Channel *RC_Channels::channels; +bool RC_Channels::has_new_overrides; +AP_Float *RC_Channels::override_timeout; const AP_Param::GroupInfo RC_Channels::var_info[] = { // @Group: 1_ @@ -94,6 +96,14 @@ const AP_Param::GroupInfo RC_Channels::var_info[] = { // @Group: 16_ // @Path: RC_Channel.cpp AP_SUBGROUPINFO(obj_channels[15], "16_", 16, RC_Channels, RC_Channel), + + // @Param: _OVERRIDE_TIME + // @DisplayName: RC override timeout + // @Description: Timeout after which RC overrides will no longer be used, and RC input will resume, 0 will disable RC overrides + // @User: Advanced + // @Range: 0.0 120.0 + // @Units: s + AP_GROUPINFO("_OVERRIDE_TIME", 32, RC_Channels, _override_timeout, 3.0), AP_GROUPEND }; @@ -105,6 +115,8 @@ const AP_Param::GroupInfo RC_Channels::var_info[] = { RC_Channels::RC_Channels(void) { channels = obj_channels; + + override_timeout = &_override_timeout; // set defaults from the parameter table AP_Param::setup_object_defaults(this, var_info); @@ -144,10 +156,12 @@ uint8_t RC_Channels::get_radio_in(uint16_t *chans, const uint8_t num_channels) bool RC_Channels::read_input(void) { - if (!hal.rcin->new_input()) { + if (!hal.rcin->new_input() && !has_new_overrides) { return false; } + has_new_overrides = false; + for (uint8_t i=0; iclear_overrides(); + for (uint8_t i = 0; i < NUM_RC_CHANNELS; i++) { + channels[i].clear_override(); + } + // we really should set has_new_overrides to true, and rerun read_input from + // the vehicle code however doing so currently breaks the failsafe system on + // copter and plane, RC_Channels needs to control failsafes to resolve this } -bool RC_Channels::set_override(const uint8_t chan, const int16_t value) +void RC_Channels::set_override(const uint8_t chan, const int16_t value, const uint32_t timestamp_ms) { if (chan < NUM_RC_CHANNELS) { - return hal.rcin->set_override(chan, value); + channels[chan].set_override(value, timestamp_ms); + has_new_overrides = true; } +} + +bool RC_Channels::has_active_overrides() +{ + for (uint8_t i = 0; i < NUM_RC_CHANNELS; i++) { + if (channels[i].has_override()) { + return true; + } + } + return false; }