From 2e23822b27a4570f64deeadfbeec34385d1344dc Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 5 Feb 2021 13:28:08 +1100 Subject: [PATCH] ArduSub: move handling of last-seen-SYSID_MYGCS up to GCS base class --- ArduSub/GCS_Mavlink.cpp | 21 +++------------------ ArduSub/GCS_Mavlink.h | 1 - ArduSub/Sub.h | 1 - ArduSub/failsafe.cpp | 10 ++++++++-- 4 files changed, 11 insertions(+), 22 deletions(-) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index c89b288383..a8bbf515d0 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -529,15 +529,6 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) { switch (msg.msgid) { - case MAVLINK_MSG_ID_HEARTBEAT: { // MAV ID: 0 - // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes - if (msg.sysid != sub.g.sysid_my_gcs) { - break; - } - sub.failsafe.last_heartbeat_ms = AP_HAL::millis(); - break; - } - case MAVLINK_MSG_ID_MANUAL_CONTROL: { // MAV ID: 69 if (msg.sysid != sub.g.sysid_my_gcs) { break; // Only accept control from our gcs @@ -552,8 +543,9 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) sub.transform_manual_control_to_rc_override(packet.x,packet.y,packet.z,packet.r,packet.buttons); sub.failsafe.last_pilot_input_ms = AP_HAL::millis(); - // a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes - sub.failsafe.last_heartbeat_ms = AP_HAL::millis(); + // a RC override message is considered to be a 'heartbeat' + // from the ground station for failsafe purposes + gcs().sysid_myggcs_seen(AP_HAL::millis()); break; } @@ -787,13 +779,6 @@ uint64_t GCS_MAVLINK_Sub::capabilities() const ); } -// a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes -void GCS_MAVLINK_Sub::handle_rc_channels_override(const mavlink_message_t &msg) -{ - sub.failsafe.last_heartbeat_ms = AP_HAL::millis(); - GCS_MAVLINK::handle_rc_channels_override(msg); -} - MAV_RESULT GCS_MAVLINK_Sub::handle_flight_termination(const mavlink_command_long_t &packet) { if (packet.param1 > 0.5f) { sub.arming.disarm(AP_Arming::Method::TERMINATION); diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h index b7c39663dc..2631b00a57 100644 --- a/ArduSub/GCS_Mavlink.h +++ b/ArduSub/GCS_Mavlink.h @@ -44,7 +44,6 @@ private: void handleMessage(const mavlink_message_t &msg) override; bool handle_guided_request(AP_Mission::Mission_Command &cmd) override; void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override; - void handle_rc_channels_override(const mavlink_message_t &msg) override; bool try_send_message(enum ap_message id) override; bool send_info(void); diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 660c321b7f..ea2075dc65 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -222,7 +222,6 @@ private: struct { uint32_t last_leak_warn_ms; // last time a leak warning was sent to gcs uint32_t last_gcs_warn_ms; - uint32_t last_heartbeat_ms; // the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe uint32_t last_pilot_input_ms; // last time we received pilot input in the form of MANUAL_CONTROL or RC_CHANNELS_OVERRIDE messages uint32_t terrain_first_failure_ms; // the first time terrain data access failed - used to calculate the duration of the failure uint32_t terrain_last_failure_ms; // the most recent time terrain data access failed diff --git a/ArduSub/failsafe.cpp b/ArduSub/failsafe.cpp index e27c8152ef..454373fcbe 100644 --- a/ArduSub/failsafe.cpp +++ b/ArduSub/failsafe.cpp @@ -308,14 +308,20 @@ void Sub::failsafe_gcs_check() { // return immediately if we have never had contact with a gcs, or if gcs failsafe action is disabled // 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.last_heartbeat_ms == 0 || (!g.failsafe_gcs && g.failsafe_gcs == FS_GCS_DISABLED)) { + if (!g.failsafe_gcs && g.failsafe_gcs == FS_GCS_DISABLED) { + return; + } + + const uint32_t gcs_last_seen_ms = gcs().sysid_myggcs_last_seen_time_ms(); + if (gcs_last_seen_ms == 0) { + // we've never seen a GCS, so we don't failsafe if we stop seeing it return; } uint32_t tnow = AP_HAL::millis(); // Check if we have gotten a GCS heartbeat recently (GCS sysid must match SYSID_MYGCS parameter) - if (tnow - failsafe.last_heartbeat_ms < FS_GCS_TIMEOUT_MS) { + if (tnow - gcs_last_seen_ms < FS_GCS_TIMEOUT_MS) { // Log event if we are recovering from previous gcs failsafe if (failsafe.gcs) { AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED);