From 263f3b116be84528969f87a69907a9b6ad828681 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 20 Dec 2013 11:39:00 +1100 Subject: [PATCH] Plane: added FS_GCS_ENABL==2 option this allows GCS failsafe when the GCS isn't getting updates from the aircraft --- ArduPlane/ArduPlane.pde | 3 +++ ArduPlane/GCS_Mavlink.pde | 7 +++++++ ArduPlane/Parameters.pde | 4 ++-- ArduPlane/defines.h | 10 ++++++++++ ArduPlane/system.pde | 12 ++++++++---- 5 files changed, 30 insertions(+), 6 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index aa35dc88c9..d5ecb8cf54 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -410,6 +410,9 @@ static struct { uint32_t ch3_timer_ms; uint32_t last_valid_rc_ms; + + // last RADIO status packet + uint32_t last_radio_status_remrssi_ms; } failsafe; diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 07a413cbf2..b49cb6b3c0 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -2158,6 +2158,13 @@ mission_failed: { mavlink_radio_t packet; mavlink_msg_radio_decode(msg, &packet); + + // record if the GCS has been receiving radio messages from + // the aircraft + if (packet.remrssi != 0) { + failsafe.last_radio_status_remrssi_ms = hal.scheduler->millis(); + } + // use the state of the transmit buffer in the radio to // control the stream rate, giving us adaptive software // flow control diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 2097b4a9ee..eb38ea339e 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -440,8 +440,8 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: FS_GCS_ENABL // @DisplayName: GCS failsafe enable - // @Description: Enable ground control station telemetry failsafe. Failsafe will trigger after 20 seconds of no MAVLink heartbeat messages. WARNING: Enabling this option opens up the possibility of your plane going into failsafe mode and running the motor on the ground it it loses contact with your ground station. If this option is enabled on an electric plane then either use a separate motor arming switch or remove the propeller in any ground testing. - // @Values: 0:Disabled,1:Enabled + // @Description: Enable ground control station telemetry failsafe. Failsafe will trigger after FS_LONG_TIMEOUT seconds of no MAVLink heartbeat messages. There are two possible enabled settings. Seeing FS_GCS_ENABL to 1 means that GCS failsafe will be triggered when the aircraft has not received a MAVLink HEARTBEAT message. Setting FS_GCS_ENABL to 2 means that GCS failsafe will be triggerded on either a loss of HEARTBEAT messages, or a RADIO_STATUS message from a MAVLink enabled 3DR radio indicating that the ground station is not receiving status updates from the aircraft, which is indicated by the RADIO_STATUS.remrssi field being zero (this may happen if you have a one way link due to asymmetric noise on the ground station and aircraft radios). WARNING: Enabling this option opens up the possibility of your plane going into failsafe mode and running the motor on the ground it it loses contact with your ground station. If this option is enabled on an electric plane then you should enable ARMING_REQUIRED. + // @Values: 0:Disabled,1:Heartbeat,2:HeartbeatAndREMRSSI // @User: Standard GSCALAR(gcs_heartbeat_fs_enabled, "FS_GCS_ENABL", GCS_HEARTBEAT_FAILSAFE), diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 368ecb5bdd..e9dcb229d3 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -26,6 +26,16 @@ enum failsafe_state { }; +// GCS failsafe types for FS_GCS_ENABL parameter +enum gcs_failsafe { + GCS_FAILSAFE_OFF = 0, // no GCS failsafe + GCS_FAILSAFE_HEARTBEAT = 1, // failsafe if we stop receiving heartbeat + GCS_FAILSAFE_HB_RSSI = 2 // failsafe if we stop receiving + // heartbeat or if RADIO.remrssi + // drops to 0 +}; + + // active altitude sensor // ---------------------- #define SONAR 0 diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 13984d12a2..d994ff9bc0 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -377,11 +377,15 @@ static void check_long_failsafe() failsafe_long_on_event(FAILSAFE_LONG); } else if (!failsafe.rc_override_active && failsafe.state == FAILSAFE_SHORT && - (tnow - failsafe.ch3_timer_ms) > g.long_fs_timeout*1000) { + (tnow - failsafe.ch3_timer_ms) > g.long_fs_timeout*1000) { failsafe_long_on_event(FAILSAFE_LONG); - } else if (g.gcs_heartbeat_fs_enabled && - failsafe.last_heartbeat_ms != 0 && - (tnow - failsafe.last_heartbeat_ms) > g.long_fs_timeout*1000) { + } else if (g.gcs_heartbeat_fs_enabled != GCS_FAILSAFE_OFF && + failsafe.last_heartbeat_ms != 0 && + (tnow - failsafe.last_heartbeat_ms) > g.long_fs_timeout*1000) { + failsafe_long_on_event(FAILSAFE_GCS); + } else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_RSSI && + failsafe.last_radio_status_remrssi_ms != 0 && + (tnow - failsafe.last_radio_status_remrssi_ms) > g.long_fs_timeout*1000) { failsafe_long_on_event(FAILSAFE_GCS); } } else {