From 9471d8069c13b46922b1f778417b5a22392b1d2e Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Fri, 5 Feb 2021 13:28:08 +1100
Subject: [PATCH] GCS_MAVLink: move handling of last-seen-SYSID_MYGCS up to GCS
 base class

---
 libraries/GCS_MAVLink/GCS.h          | 17 ++++++++++++++++-
 libraries/GCS_MAVLink/GCS_Common.cpp | 18 ++++++++++++++++++
 2 files changed, 34 insertions(+), 1 deletion(-)

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index b54756106f..d644153a4e 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -370,6 +370,8 @@ protected:
     // saveable rate of each stream
     AP_Int16        *streamRates;
 
+    void handle_heartbeat(const mavlink_message_t &msg) const;
+
     virtual bool persist_streamrates() const { return false; }
     void handle_request_data_stream(const mavlink_message_t &msg);
 
@@ -412,7 +414,7 @@ protected:
     void handle_param_request_list(const mavlink_message_t &msg);
     void handle_param_request_read(const mavlink_message_t &msg);
     virtual bool params_ready() const { return true; }
-    virtual void handle_rc_channels_override(const mavlink_message_t &msg);
+    void handle_rc_channels_override(const mavlink_message_t &msg);
     void handle_system_time_message(const mavlink_message_t &msg);
     void handle_common_rally_message(const mavlink_message_t &msg);
     void handle_rally_fetch_point(const mavlink_message_t &msg);
@@ -910,6 +912,16 @@ public:
         return _statustext_queue;
     }
 
+    // last time traffic was seen from my designated GCS.  traffic
+    // includes heartbeats and some manual control messages.
+    uint32_t sysid_myggcs_last_seen_time_ms() const {
+        return _sysid_mygcs_last_seen_time_ms;
+    }
+    // called when valid traffic has been seen from our GCS
+    void sysid_myggcs_seen(uint32_t seen_time_ms) {
+        _sysid_mygcs_last_seen_time_ms = seen_time_ms;
+    }
+
     void send_to_active_channels(uint32_t msgid, const char *pkt);
 
     void send_text(MAV_SEVERITY severity, const char *fmt, ...) FMT_PRINTF(3, 4);
@@ -1015,6 +1027,9 @@ private:
 
     void update_sensor_status_flags();
 
+    // time we last saw traffic from our GCS
+    uint32_t _sysid_mygcs_last_seen_time_ms;
+
     void service_statustext(void);
 #if HAL_MEM_CLASS <= HAL_MEM_CLASS_192 || CONFIG_HAL_BOARD == HAL_BOARD_SITL
     static const uint8_t _status_capacity = 5;
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index 5a02e3f180..bd6cc876d3 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -3278,6 +3278,9 @@ void GCS_MAVLINK::handle_rc_channels_override(const mavlink_message_t &msg)
             RC_Channels::set_override(i, value, tnow);
         }
     }
+
+    gcs().sysid_myggcs_seen(tnow);
+
 }
 
 // allow override of RC channel values for HIL or for complete GCS
@@ -3349,12 +3352,27 @@ void GCS_MAVLINK::handle_osd_param_config(const mavlink_message_t &msg) const
 #endif
 }
 
+void GCS_MAVLINK::handle_heartbeat(const mavlink_message_t &msg) const
+{
+    // if the heartbeat is from our GCS then we don't failsafe for
+    // now...
+    if (msg.sysid == sysid_my_gcs()) {
+        gcs().sysid_myggcs_seen(AP_HAL::millis());
+    }
+}
+
 /*
   handle messages which don't require vehicle specific data
  */
 void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg)
 {
     switch (msg.msgid) {
+
+    case MAVLINK_MSG_ID_HEARTBEAT: {
+        handle_heartbeat(msg);
+        break;
+    }
+
     case MAVLINK_MSG_ID_COMMAND_ACK: {
         handle_command_ack(msg);
         break;