RC_Channel: Move overrides out of the HAL

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
 {

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;
 };

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_
@@ -95,6 +97,14 @@ const AP_Param::GroupInfo RC_Channels::var_info[] = {
     // @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
 };
 
@@ -106,6 +116,8 @@ 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; i<NUM_RC_CHANNELS; i++) {
         channels[i].set_pwm(channels[i].read());
     }
@@ -167,14 +181,30 @@ int16_t RC_Channels::get_receiver_rssi(void)
 
 void RC_Channels::clear_overrides(void)
 {
-    hal.rcin->clear_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;
 }