Move disabling of rc overrides up to RC_Channel base class

ArduCopter/Copter.h

@@ -342,7 +342,7 @@ class Copter : public AP_HAL::HAL::Callbacks {
             uint8_t in_arming_delay         : 1; // 24      // true while we are armed but waiting to spin motors
             uint8_t initialised_params      : 1; // 25      // true when the all parameters have been initialised. we cannot send parameters to the GCS until this is done
             uint8_t compass_init_location   : 1; // 26      // true when the compass's initial location has been set
-            uint8_t rc_override_enable      : 1; // 27      // aux switch rc_override is allowed
+            uint8_t unused2                 : 1; // 27      // aux switch rc_override is allowed
             uint8_t armed_with_switch       : 1; // 28      // we armed using a arming switch
         };
         uint32_t value;
@@ -381,7 +381,6 @@ class Copter : public AP_HAL::HAL::Callbacks {
 
         int8_t radio_counter;            // number of iterations with throttle below throttle_fs_value
 
-        uint8_t rc_override_active  : 1; // true if rc control are overwritten by ground station
         uint8_t radio               : 1; // A status flag for the radio failsafe
         uint8_t gcs                 : 1; // A status flag for the ground station failsafe
         uint8_t ekf                 : 1; // true if ekf failsafe has occurred

ArduCopter/GCS_Mavlink.cpp

@@ -1052,13 +1052,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
         if(msg->sysid != copter.g.sysid_my_gcs) {
             break; // Only accept control from our gcs
         }
-        if (!copter.ap.rc_override_enable) {
-            if (copter.failsafe.rc_override_active) {  // if overrides were active previously, disable them
-                copter.failsafe.rc_override_active = false;
-                RC_Channels::clear_overrides();
-            }
-            break;
-        }
+
         uint32_t tnow = AP_HAL::millis();
 
         mavlink_rc_channels_override_t packet;
@@ -1073,9 +1067,6 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
         RC_Channels::set_override(6, packet.chan7_raw, tnow);
         RC_Channels::set_override(7, packet.chan8_raw, tnow);
 
-        // record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation
-        copter.failsafe.rc_override_active = RC_Channels::has_active_overrides();
-
         // a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes
         copter.failsafe.last_heartbeat_ms = tnow;
         break;
@@ -1110,9 +1101,6 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
         RC_Channels::set_override(uint8_t(copter.rcmap.throttle() - 1), throttle, tnow);
         RC_Channels::set_override(uint8_t(copter.rcmap.yaw() - 1), yaw, tnow);
 
-        // record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation
-        copter.failsafe.rc_override_active = RC_Channels::has_active_overrides();
-
         // a manual control message is considered to be a 'heartbeat' from the ground station for failsafe purposes
         copter.failsafe.last_heartbeat_ms = tnow;
         break;

ArduCopter/RC_Channel.cpp

@@ -81,7 +81,6 @@ void RC_Channel_Copter::init_aux_function(const aux_func_t ch_option, const aux_
     case AVOID_PROXIMITY:
     case INVERTED:
     case WINCH_ENABLE:
-    case RC_OVERRIDE_ENABLE:
         do_aux_function(ch_option, ch_flag);
         break;
     // the following functions do not need to be initialised:
@@ -512,23 +511,6 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw
 #endif
             break;
 
-        case RC_OVERRIDE_ENABLE:
-            // Allow or disallow RC_Override
-            switch (ch_flag) {
-                case HIGH: {
-                    copter.ap.rc_override_enable = true;
-                    break;
-                }
-                case MIDDLE:
-                    // nothing
-                    break;
-                case LOW: {
-                    copter.ap.rc_override_enable = false;
-                    break;
-                }
-            }
-            break;
-
 #ifdef USERHOOK_AUXSWITCH
         case USER_FUNC1:
             userhook_auxSwitch1(ch_flag);

ArduCopter/events.cpp

@@ -92,7 +92,7 @@ void Copter::failsafe_gcs_check()
 
     // return immediately if gcs failsafe is disabled, gcs has never been connected or we are not overriding rc controls from the gcs and we are not in guided mode
     // this also checks to see if we have a GCS failsafe active, if we do, then must continue to process the logic for recovery from this state.
-    if ((!failsafe.gcs)&&(g.failsafe_gcs == FS_GCS_DISABLED || failsafe.last_heartbeat_ms == 0 || (!failsafe.rc_override_active && control_mode != GUIDED))) {
+    if ((!failsafe.gcs)&&(g.failsafe_gcs == FS_GCS_DISABLED || failsafe.last_heartbeat_ms == 0 || (!RC_Channels::has_active_overrides() && control_mode != GUIDED))) {
         return;
     }
 
@@ -122,7 +122,6 @@ void Copter::failsafe_gcs_check()
 
     // clear overrides so that RC control can be regained with radio.
     RC_Channels::clear_overrides();
-    failsafe.rc_override_active = false;
 
     if (should_disarm_on_failsafe()) {
         init_disarm_motors();

ArduCopter/radio.cpp

@@ -43,9 +43,6 @@ void Copter::init_rc_in()
 
     // initialise throttle_zero flag
     ap.throttle_zero = true;
-
-    // Allow override by default at start
-    ap.rc_override_enable = true;
 }
 
  // init_rc_out -- initialise motors and check if pilot wants to perform ESC calibration
@@ -93,34 +90,54 @@ void Copter::enable_motor_output()
 
 void Copter::read_radio()
 {
-    uint32_t tnow_ms = millis();
+    const uint32_t tnow_ms = millis();
 
     if (rc().read_input()) {
         ap.new_radio_frame = true;
 
         set_throttle_and_failsafe(channel_throttle->get_radio_in());
         set_throttle_zero_flag(channel_throttle->get_control_in());
 
-        // flag we must have an rc receiver attached
-        if (!failsafe.rc_override_active) {
-            ap.rc_receiver_present = true;
+        if (!ap.rc_receiver_present) {
+            // RC receiver must be attached if we've just go input and
+            // there are no overrides
+            ap.rc_receiver_present = !RC_Channels::has_active_overrides();
         }
 
         // pass pilot input through to motors (used to allow wiggling servos while disarmed on heli, single, coax copters)
         radio_passthrough_to_motors();
 
-        float dt = (tnow_ms - last_radio_update_ms)*1.0e-3f;
+        const float dt = (tnow_ms - last_radio_update_ms)*1.0e-3f;
         rc_throttle_control_in_filter.apply(channel_throttle->get_control_in(), dt);
         last_radio_update_ms = tnow_ms;
-    }else{
-        uint32_t elapsed = tnow_ms - last_radio_update_ms;
-        // turn on throttle failsafe if no update from the RC Radio for 500ms or 2000ms if we are using RC_OVERRIDE
-        if (((!failsafe.rc_override_active && (elapsed >= FS_RADIO_TIMEOUT_MS)) || (failsafe.rc_override_active && (elapsed >= FS_RADIO_RC_OVERRIDE_TIMEOUT_MS))) &&
-            (g.failsafe_throttle && (ap.rc_receiver_present||motors->armed()) && !failsafe.radio)) {
-            Log_Write_Error(ERROR_SUBSYSTEM_RADIO, ERROR_CODE_RADIO_LATE_FRAME);
-            set_failsafe_radio(true);
-        }
+        return;
+    }
+
+    // No radio input this time
+    if (failsafe.radio) {
+        // already in failsafe!
+        return;
     }
+
+    const uint32_t elapsed = tnow_ms - last_radio_update_ms;
+    // turn on throttle failsafe if no update from the RC Radio for 500ms or 2000ms if we are using RC_OVERRIDE
+    const uint32_t timeout = RC_Channels::has_active_overrides() ? FS_RADIO_RC_OVERRIDE_TIMEOUT_MS : FS_RADIO_TIMEOUT_MS;
+    if (elapsed < timeout) {
+        // not timed out yet
+        return;
+    }
+    if (!g.failsafe_throttle) {
+        // throttle failsafe not enabled
+        return;
+    }
+    if (!ap.rc_receiver_present && !motors->armed()) {
+        // we only failsafe if we are armed OR we have ever seen an RC receiver
+        return;
+    }
+
+    // Nobody ever talks to us.  Log an error and enter failsafe.
+    Log_Write_Error(ERROR_SUBSYSTEM_RADIO, ERROR_CODE_RADIO_LATE_FRAME);
+    set_failsafe_radio(true);
 }
 
 #define FS_COUNTER 3        // radio failsafe kicks in after 3 consecutive throttle values below failsafe_throttle_value

ArduCopter/system.cpp

@@ -275,9 +275,6 @@ void Copter::init_ardupilot()
     // disable safety if requested
     BoardConfig.init_safety();
 
-    // default enable RC override
-    copter.ap.rc_override_enable = true;
-
     hal.console->printf("\nReady to FLY ");
 
     // flag that initialisation has completed

Tools/autotest/apmrover2.py

@@ -600,6 +600,9 @@ def test_rc_overrides(self):
         self.context_push();
         ex = None
         try:
+            self.set_parameter("RC12_OPTION", 46)
+            self.reboot_sitl()
+
             self.mavproxy.send('switch 6\n')  # Manual mode
             self.wait_mode('MANUAL')
             self.wait_ready_to_arm()
@@ -609,8 +612,12 @@ def test_rc_overrides(self):
             normal_rc_throttle = 1700
             self.mavproxy.send('rc 3 %u\n' % normal_rc_throttle)
             self.wait_groundspeed(5, 100)
-            # now override to go backwards:
-            throttle_override = 1400
+
+            # allow overrides:
+            self.set_rc(12, 2000)
+
+            # now override to stop:
+            throttle_override = 1500
             while True:
                 print("Sending throttle of %u" % (throttle_override,))
                 self.mav.mav.rc_channels_override_send(
@@ -625,11 +632,42 @@ def test_rc_overrides(self):
                     65535, # chan7_raw
                     65535) # chan8_raw
 
+                m = self.mav.recv_match(type='VFR_HUD', blocking=True)
+                want_speed = 2.0
+                print("Speed=%f want=<%f" % (m.groundspeed, want_speed))
+                if m.groundspeed < want_speed:
+                    break
 
-                m = self.mav.recv_match(type='RC_CHANNELS_RAW', blocking=True)
-                print("%s" % m)
-                if m.chan3_raw == throttle_override:
+            # now override to stop - but set the switch on the RC
+            # transmitter to deny overrides; this should send the
+            # speed back up to 5 metres/second:
+            self.set_rc(12, 1000)
+
+            throttle_override = 1500
+            while True:
+                print("Sending throttle of %u" % (throttle_override,))
+                self.mav.mav.rc_channels_override_send(
+                    1, # target system
+                    1, # targe component
+                    65535, # chan1_raw
+                    65535, # chan2_raw
+                    throttle_override, # chan3_raw
+                    65535, # chan4_raw
+                    65535, # chan5_raw
+                    65535, # chan6_raw
+                    65535, # chan7_raw
+                    65535) # chan8_raw
+
+                m = self.mav.recv_match(type='VFR_HUD', blocking=True)
+                want_speed = 5.0
+                print("Speed=%f want=>%f" % (m.groundspeed, want_speed))
+
+                if m.groundspeed > want_speed:
                     break
+
+            # re-enable RC overrides
+            self.set_rc(12, 2000)
+
             # check we revert to normal RC inputs when gcs overrides cease:
             self.progress("Waiting for RC to revert to normal RC input")
             while True:
@@ -643,6 +681,7 @@ def test_rc_overrides(self):
             ex = e
 
         self.context_pop();
+        self.reboot_sitl()
 
         if ex is not None:
             raise ex

libraries/RC_Channel/RC_Channel.cpp

@@ -79,7 +79,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = {
     // @DisplayName: RC input option
     // @Description: Function assigned to this RC channel
     // @Values{Copter}: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl, 46:RC Override Enable, 47:User Function 1, 48:User Function 2, 49:User Function 3, 58:Clear Waypoints
-    // @Values{Rover}: 0:Do Nothing, 4:RTL, 7:Save WP, 9:Camera Trigger, 16:Auto, 28:Relay On/Off, 30:Lost Rover Sound, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 41:ArmDisarm, 42:SmartRTL, 50:LearnCruise, 51:Manual, 52:Acro, 53:Steering, 54:Hold, 55:Guided, 56:Loiter, 57:Follow, 58:Clear Waypoints
+    // @Values{Rover}: 0:Do Nothing, 4:RTL, 7:Save WP, 9:Camera Trigger, 16:Auto, 28:Relay On/Off, 30:Lost Rover Sound, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 41:ArmDisarm, 42:SmartRTL, 46:RC Override Enable, 50:LearnCruise, 51:Manual, 52:Acro, 53:Steering, 54:Hold, 55:Guided, 56:Loiter, 57:Follow, 58:Clear Waypoints
     // @User: Standard
     AP_GROUPINFO_FRAME("OPTION",  6, RC_Channel, option, 0, AP_PARAM_FRAME_COPTER|AP_PARAM_FRAME_ROVER),
 
@@ -324,8 +324,18 @@ bool RC_Channel::in_trim_dz()
 
 void RC_Channel::set_override(const uint16_t v, const uint32_t timestamp_us)
 {
+    if (!rc().gcs_overrides_enabled()) {
+        return;
+    }
+    // this UINT16_MAX stuff should really, really be in the
+    // mavlink packet handling code.  It can be moved once that
+    // code is in the GCS_MAVLink class!
+    if (v == UINT16_MAX) {
+        return;
+    }
     last_override_time = timestamp_us != 0 ? timestamp_us : AP_HAL::millis();
     override_value = v;
+    RC_Channels::has_new_overrides = true;
 }
 
 void RC_Channel::clear_override()
@@ -412,6 +422,9 @@ void RC_Channel::init_aux_function(const aux_func_t ch_option, const aux_switch_
 {
     // init channel options
     switch(ch_option) {
+    case RC_OVERRIDE_ENABLE:
+        do_aux_function(ch_option, ch_flag);
+        break;
     // the following functions to not need to be initialised:
     case RELAY:
     case RELAY2:
@@ -545,6 +558,23 @@ void RC_Channel::do_aux_function_lost_vehicle_sound(const aux_switch_pos_t ch_fl
     }
 }
 
+void RC_Channel::do_aux_function_rc_override_enable(const aux_switch_pos_t ch_flag)
+{
+    switch (ch_flag) {
+    case HIGH: {
+        rc().set_gcs_overrides_enabled(true);
+        break;
+    }
+    case MIDDLE:
+        // nothing
+        break;
+    case LOW: {
+        rc().set_gcs_overrides_enabled(false);
+        break;
+    }
+    }
+}
+
 void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_pos_t ch_flag)
 {
     switch(ch_option) {
@@ -556,6 +586,11 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po
         do_aux_function_gripper(ch_flag);
         break;
 
+    case RC_OVERRIDE_ENABLE:
+        // Allow or disallow RC_Override
+        do_aux_function_rc_override_enable(ch_flag);
+        break;
+
     case RELAY:
         do_aux_function_relay(0, ch_flag == HIGH);
         break;

libraries/RC_Channel/RC_Channel.h

@@ -77,7 +77,7 @@ class RC_Channel {
     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);
+    void       set_override(const uint16_t v, const uint32_t timestamp_us);
     bool       has_override() const;
 
     // get control input with zero deadzone
@@ -184,6 +184,7 @@ class RC_Channel {
     void do_aux_function_clear_wp(const aux_switch_pos_t ch_flag);
     void do_aux_function_gripper(const aux_switch_pos_t ch_flag);
     void do_aux_function_lost_vehicle_sound(const aux_switch_pos_t ch_flag);
+    void do_aux_function_rc_override_enable(const aux_switch_pos_t ch_flag);
     void do_aux_function_relay(uint8_t relay, bool val);
     void do_aux_function_sprayer(const aux_switch_pos_t ch_flag);
 
@@ -258,6 +259,7 @@ class RC_Channel {
 
     void reset_mode_switch();
     void read_mode_switch();
+
 };
 
 
@@ -319,6 +321,14 @@ class RC_Channels {
     // has_valid_input should be pure-virtual when Plane is converted
     virtual bool has_valid_input() const { return false; };
 
+    bool gcs_overrides_enabled() const { return _gcs_overrides_enabled; }
+    void set_gcs_overrides_enabled(bool enable) {
+        _gcs_overrides_enabled = enable;
+        if (!_gcs_overrides_enabled) {
+            clear_overrides();
+        }
+    }
+
 private:
     static RC_Channels *_singleton;
     // this static arrangement is to avoid static pointers in AP_Param tables
@@ -334,6 +344,8 @@ class RC_Channels {
     virtual int8_t flight_mode_channel_number() const = 0;
     RC_Channel *flight_mode_channel();
 
+    // Allow override by default at start
+    bool _gcs_overrides_enabled = true;
 };
 
 RC_Channels &rc();

libraries/RC_Channel/RC_Channels.cpp

@@ -114,7 +114,6 @@ void RC_Channels::set_override(const uint8_t chan, const int16_t value, const ui
     RC_Channels &_rc = rc();
     if (chan < NUM_RC_CHANNELS) {
         _rc.channel(chan)->set_override(value, timestamp_ms);
-        has_new_overrides = true;
     }
 }