From fad8604434f05e17dadac1f25dc2f41bbad11d6f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 2 Aug 2018 20:48:04 +1000 Subject: [PATCH 1/8] RC_Channel: move handling of disable-gcs-rc-overrides-channel-option --- libraries/RC_Channel/RC_Channel.cpp | 25 +++++++++++++++++++++++++ libraries/RC_Channel/RC_Channel.h | 9 +++++++++ 2 files changed, 34 insertions(+) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index c0304725879ca..dc4f431436783 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -412,6 +412,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 +548,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 +576,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; diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 37d142b494023..c4f6a84bfccc1 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -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,11 @@ 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; + } + private: static RC_Channels *_singleton; // this static arrangement is to avoid static pointers in AP_Param tables @@ -334,6 +341,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(); From 834a23fcb642f202774c5b81bd3d3d3bcdc20aae Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 2 Aug 2018 20:48:21 +1000 Subject: [PATCH 2/8] Copter: move handling of disable-gcs-rc-overrides-channel-option up --- ArduCopter/Copter.h | 2 +- ArduCopter/GCS_Mavlink.cpp | 2 +- ArduCopter/RC_Channel.cpp | 18 ------------------ ArduCopter/radio.cpp | 3 --- ArduCopter/system.cpp | 3 --- 5 files changed, 2 insertions(+), 26 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 1d4e39e9855ea..08333da78997e 100644 --- a/ArduCopter/Copter.h +++ b/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; diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 66313774d3314..4d0156a947a78 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1052,7 +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 (!rc().gcs_overrides_enabled()) { if (copter.failsafe.rc_override_active) { // if overrides were active previously, disable them copter.failsafe.rc_override_active = false; RC_Channels::clear_overrides(); diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index cdcbfe97287b5..5deb3587ce7c2 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/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); diff --git a/ArduCopter/radio.cpp b/ArduCopter/radio.cpp index f1cbe6069e68a..89db9527f1bc0 100644 --- a/ArduCopter/radio.cpp +++ b/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 diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 2b12dc167b49c..2939bb791ea05 100644 --- a/ArduCopter/system.cpp +++ b/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 From 2a500ed1d603c121d89dc7dd680eeed8abb01944 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 3 Aug 2018 12:23:53 +1000 Subject: [PATCH 3/8] RC_Channel: honour gcs_overrides_enabled as part of set_override This will stop situations like we currently have where the MANUAL_CONTROL GCS mavlink packets can still affect the vehicle Also resolves an issue where has_new_overrides may not be set appropriately --- libraries/RC_Channel/RC_Channel.cpp | 4 ++++ libraries/RC_Channel/RC_Channel.h | 5 ++++- libraries/RC_Channel/RC_Channels.cpp | 1 - 3 files changed, 8 insertions(+), 2 deletions(-) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index dc4f431436783..f8f4587b677e4 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -324,8 +324,12 @@ 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; + } 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() diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index c4f6a84bfccc1..76d89fae687e4 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/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 @@ -324,6 +324,9 @@ class RC_Channels { 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: diff --git a/libraries/RC_Channel/RC_Channels.cpp b/libraries/RC_Channel/RC_Channels.cpp index 8906c7727aca1..631ccd4c872c6 100644 --- a/libraries/RC_Channel/RC_Channels.cpp +++ b/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; } } From 645de368d41111373f8bfd88325f7d4f25d75f89 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 3 Aug 2018 16:20:25 +1000 Subject: [PATCH 4/8] Tools: expand test to cover use of disable-gcs-rc-overrides switch --- Tools/autotest/apmrover2.py | 49 +++++++++++++++++++++++++++++++++---- 1 file changed, 44 insertions(+), 5 deletions(-) diff --git a/Tools/autotest/apmrover2.py b/Tools/autotest/apmrover2.py index 3cfd185c4542c..84c31f3e81f66 100644 --- a/Tools/autotest/apmrover2.py +++ b/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 From e32a330da5aafe3c2891b1be72049dbec3a222b3 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 3 Aug 2018 16:20:49 +1000 Subject: [PATCH 5/8] RC_Channel: note that Rover now has disable-gcs-overrides channel option --- libraries/RC_Channel/RC_Channel.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index f8f4587b677e4..b6e418b0a9df5 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/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), From fb72b158b0e33b1e0a8647879fe563316b783a4e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 3 Aug 2018 16:22:05 +1000 Subject: [PATCH 6/8] RC_Channel: treat UINT16_MAX as a value of 0 in set_override This shouldn't really in in RC_Channels - when we move the mavlink packet handling up to the GCS_MAVLink base class we should move this into there. --- libraries/RC_Channel/RC_Channel.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index b6e418b0a9df5..4f8432fc106a5 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -327,6 +327,12 @@ 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; From 9bd1cd941da12524c7ab1e5a1c035f2d5c629385 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 3 Aug 2018 17:23:34 +1000 Subject: [PATCH 7/8] Copter: eliminate failsafe.rc_override_active Use RC_Channels::has_active_overrides() instead --- ArduCopter/Copter.h | 1 - ArduCopter/GCS_Mavlink.cpp | 14 +------------- ArduCopter/events.cpp | 3 +-- ArduCopter/radio.cpp | 12 +++++++----- 4 files changed, 9 insertions(+), 21 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 08333da78997e..e1179094cdc1e 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -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 diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 4d0156a947a78..ebd59755d974c 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/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 (!rc().gcs_overrides_enabled()) { - 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; diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 64a2ad76d6449..ff02e062efaa5 100644 --- a/ArduCopter/events.cpp +++ b/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(); diff --git a/ArduCopter/radio.cpp b/ArduCopter/radio.cpp index 89db9527f1bc0..00e22bbcaa372 100644 --- a/ArduCopter/radio.cpp +++ b/ArduCopter/radio.cpp @@ -90,7 +90,7 @@ 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; @@ -98,9 +98,10 @@ void Copter::read_radio() 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) @@ -112,7 +113,8 @@ void Copter::read_radio() }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))) && + const bool has_active_overrides = RC_Channels::has_active_overrides(); + if (((!has_active_overrides && (elapsed >= FS_RADIO_TIMEOUT_MS)) || (has_active_overrides && (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); From 2a0f7bd60a0f576a1947cebb134734cb5bf139ac Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 4 Aug 2018 21:32:31 +1000 Subject: [PATCH 8/8] Copter: rewrite read_radio for clarity --- ArduCopter/radio.cpp | 38 ++++++++++++++++++++++++++++---------- 1 file changed, 28 insertions(+), 10 deletions(-) diff --git a/ArduCopter/radio.cpp b/ArduCopter/radio.cpp index 00e22bbcaa372..01d6f837e83b0 100644 --- a/ArduCopter/radio.cpp +++ b/ArduCopter/radio.cpp @@ -107,19 +107,37 @@ void Copter::read_radio() // 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 - const bool has_active_overrides = RC_Channels::has_active_overrides(); - if (((!has_active_overrides && (elapsed >= FS_RADIO_TIMEOUT_MS)) || (has_active_overrides && (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