Rover: move handling of last-seen-SYSID_MYGCS up to GCS base class

Rover/GCS_Mavlink.cpp

@@ -732,22 +732,12 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_do_reposition(const mavlink_com
     return MAV_RESULT_ACCEPTED;
 }
 
-// a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes
-void GCS_MAVLINK_Rover::handle_rc_channels_override(const mavlink_message_t &msg)
-{
-    rover.failsafe.last_heartbeat_ms = AP_HAL::millis();
-    GCS_MAVLINK::handle_rc_channels_override(msg);
-}
-
 void GCS_MAVLINK_Rover::handleMessage(const mavlink_message_t &msg)
 {
     switch (msg.msgid) {
     case MAVLINK_MSG_ID_MANUAL_CONTROL:
         handle_manual_control(msg);
         break;
-    case MAVLINK_MSG_ID_HEARTBEAT:
-        handle_heartbeat(msg);
-        break;
 
     case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
         handle_set_attitude_target(msg);
@@ -796,17 +786,9 @@ void GCS_MAVLINK_Rover::handle_manual_control(const mavlink_message_t &msg)
     manual_override(rover.channel_steer, packet.y, 1000, 2000, tnow);
     manual_override(rover.channel_throttle, packet.z, 1000, 2000, tnow);
 
-    // a manual control message is considered to be a 'heartbeat' from the ground station for failsafe purposes
+    // a manual control message is considered to be a 'heartbeat' from
-    rover.failsafe.last_heartbeat_ms = tnow;
+    // the ground station for failsafe purposes
-}
+    gcs().sysid_myggcs_seen(tnow);
-
-void GCS_MAVLINK_Rover::handle_heartbeat(const mavlink_message_t &msg)
-{
-    // we keep track of the last time we received a heartbeat from our GCS for failsafe purposes
-    if (msg.sysid != rover.g.sysid_my_gcs) {
-        return;
-    }
-    rover.failsafe.last_heartbeat_ms = AP_HAL::millis();
 }
 
 void GCS_MAVLINK_Rover::handle_set_attitude_target(const mavlink_message_t &msg)

Rover/GCS_Mavlink.h

@@ -38,11 +38,9 @@ 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;
 
     void handle_manual_control(const mavlink_message_t &msg);
-    void handle_heartbeat(const mavlink_message_t &msg);
     void handle_set_attitude_target(const mavlink_message_t &msg);
     void handle_set_position_target_local_ned(const mavlink_message_t &msg);
     void handle_set_position_target_global_int(const mavlink_message_t &msg);

Rover/Rover.cpp

@@ -280,7 +280,17 @@ void Rover::gcs_failsafe_check(void)
     }
 
     // check for updates from GCS within 2 seconds
-    failsafe_trigger(FAILSAFE_EVENT_GCS, "GCS", failsafe.last_heartbeat_ms != 0 && (millis() - failsafe.last_heartbeat_ms) > 2000);
+    const uint32_t gcs_last_seen_ms = gcs().sysid_myggcs_last_seen_time_ms();
+    bool do_failsafe = true;
+    if (gcs_last_seen_ms == 0) {
+        // we've never seen the GCS, so we never failsafe for not seeing it
+        do_failsafe = false;
+    } else if (millis() - gcs_last_seen_ms <= 2000) {
+        // we've never seen the GCS in the last couple of seconds, so all good
+        do_failsafe = false;
+    }
+
+    failsafe_trigger(FAILSAFE_EVENT_GCS, "GCS", do_failsafe);
 }
 
 /*

Rover/Rover.h

@@ -210,7 +210,6 @@ private:
         uint32_t start_time;        // start time of the earliest failsafe
         uint8_t triggered;          // bit flags of failsafes that have triggered an action
         uint32_t last_valid_rc_ms;  // system time of most recent RC input from pilot
-        uint32_t last_heartbeat_ms; // system time of most recent heartbeat from ground station
         bool ekf;
     } failsafe;
 

Rover/failsafe.cpp

@@ -149,6 +149,6 @@ void Rover::handle_battery_failsafe(const char* type_str, const int8_t action)
 void Rover::afs_fs_check(void)
 {
     // perform AFS failsafe checks
-    g2.afs.check(failsafe.last_heartbeat_ms, g2.fence.get_breaches() != 0, failsafe.last_valid_rc_ms);
+    g2.afs.check(g2.fence.get_breaches() != 0, failsafe.last_valid_rc_ms);
 }
 #endif