From a45e3cba15de8d16c6d7ea883d34be7bd894fa80 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 22 Mar 2018 20:21:07 +1100
Subject: [PATCH] Rover: factor vehicle's mavlink send_heartbeat

---
 APMrover2/GCS_Mavlink.cpp | 56 ++++++++++++++++++++++++---------------
 APMrover2/GCS_Mavlink.h   |  5 ++++
 APMrover2/Rover.h         |  1 -
 3 files changed, 40 insertions(+), 22 deletions(-)

diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp
index aa4d0a27c3..d97ad1a6dd 100644
--- a/APMrover2/GCS_Mavlink.cpp
+++ b/APMrover2/GCS_Mavlink.cpp
@@ -4,14 +4,17 @@
 
 #include <AP_RangeFinder/RangeFinder_Backend.h>
 
-void Rover::send_heartbeat(mavlink_channel_t chan)
+MAV_TYPE GCS_MAVLINK_Rover::frame_type() const
+{
+    if (rover.is_boat()) {
+        return MAV_TYPE_SURFACE_BOAT;
+    }
+    return MAV_TYPE_GROUND_ROVER;
+}
+
+MAV_MODE GCS_MAVLINK_Rover::base_mode() const
 {
     uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
-    uint8_t system_status = MAV_STATE_ACTIVE;
-
-    if (failsafe.triggered != 0) {
-        system_status = MAV_STATE_CRITICAL;
-    }
 
     // work out the base_mode. This value is not very useful
     // for APM, but we calculate it as best we can so a generic
@@ -21,11 +24,11 @@ void Rover::send_heartbeat(mavlink_channel_t chan)
     // only get useful information from the custom_mode, which maps to
     // the APM flight mode and has a well defined meaning in the
     // ArduPlane documentation
-    if (control_mode->has_manual_input()) {
+    if (rover.control_mode->has_manual_input()) {
         base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
     }
 
-    if (control_mode->is_autopilot_mode()) {
+    if (rover.control_mode->is_autopilot_mode()) {
         base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
     }
 
@@ -40,25 +43,36 @@ void Rover::send_heartbeat(mavlink_channel_t chan)
 #if HIL_MODE != HIL_MODE_DISABLED
     base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
 #endif
-    if (control_mode == &mode_initializing) {
-        system_status = MAV_STATE_CALIBRATING;
-    }
-    if (control_mode == &mode_hold) {
-        system_status = MAV_STATE_STANDBY;
-    }
+
     // we are armed if we are not initialising
-    if (control_mode != &mode_initializing && arming.is_armed()) {
+    if (rover.control_mode != &rover.mode_initializing && rover.arming.is_armed()) {
         base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
     }
 
     // indicate we have set a custom mode
     base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
 
-    gcs().chan(chan-MAVLINK_COMM_0).send_heartbeat(
-                                            is_boat() ? MAV_TYPE_SURFACE_BOAT : MAV_TYPE_GROUND_ROVER,
-                                            base_mode,
-                                            control_mode->mode_number(),
-                                            system_status);
+return (MAV_MODE)base_mode;
+}
+
+uint32_t GCS_MAVLINK_Rover::custom_mode() const
+{
+    return rover.control_mode->mode_number();
+}
+
+MAV_STATE GCS_MAVLINK_Rover::system_status() const
+{
+    if (rover.failsafe.triggered != 0) {
+        return MAV_STATE_CRITICAL;
+    }
+    if (rover.control_mode == &rover.mode_initializing) {
+        return MAV_STATE_CALIBRATING;
+    }
+    if (rover.control_mode == &rover.mode_hold) {
+        return MAV_STATE_STANDBY;
+    }
+
+    return MAV_STATE_ACTIVE;
 }
 
 void Rover::send_attitude(mavlink_channel_t chan)
@@ -288,7 +302,7 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
     case MSG_HEARTBEAT:
         CHECK_PAYLOAD_SIZE(HEARTBEAT);
         last_heartbeat_time = AP_HAL::millis();
-        rover.send_heartbeat(chan);
+        send_heartbeat();
         return true;
 
     case MSG_EXTENDED_STATUS1:
diff --git a/APMrover2/GCS_Mavlink.h b/APMrover2/GCS_Mavlink.h
index 158dd1a038..edb1a0467f 100644
--- a/APMrover2/GCS_Mavlink.h
+++ b/APMrover2/GCS_Mavlink.h
@@ -38,4 +38,9 @@ private:
     bool handle_guided_request(AP_Mission::Mission_Command &cmd) override;
     void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override;
     bool try_send_message(enum ap_message id) override;
+
+    MAV_TYPE frame_type() const override;
+    MAV_MODE base_mode() const override;
+    uint32_t custom_mode() const override;
+    MAV_STATE system_status() const override;
 };
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index 3d4042e669..e85cf2ddb7 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -469,7 +469,6 @@ private:
     void fence_send_mavlink_status(mavlink_channel_t chan);
 
     // GCS_Mavlink.cpp
-    void send_heartbeat(mavlink_channel_t chan);
     void send_attitude(mavlink_channel_t chan);
     void send_extended_status1(mavlink_channel_t chan);
     void send_location(mavlink_channel_t chan);