From 0c8110ecb2a2134c353fbd6d7df9bc6676c47156 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 22 Mar 2018 19:50:24 +1100
Subject: [PATCH] Copter: factor vehicle's mavlink send_heartbeat

---
 ArduCopter/Copter.h        |  7 ++++--
 ArduCopter/GCS_Mavlink.cpp | 48 ++++++++++++++++++++++++--------------
 ArduCopter/GCS_Mavlink.h   |  5 ++++
 ArduCopter/system.cpp      |  2 +-
 4 files changed, 42 insertions(+), 20 deletions(-)

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index d0d0c61c8c..819e6df209 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -391,6 +391,10 @@ private:
         uint8_t adsb                : 1; // true if an adsb related failsafe has occurred
     } failsafe;
 
+    bool any_failsafe_triggered() const {
+        return failsafe.radio || battery.has_failsafed() || failsafe.gcs || failsafe.ekf || failsafe.terrain || failsafe.adsb;
+    }
+
     // sensor health for logging
     struct {
         uint8_t baro        : 1;    // true if baro is healthy
@@ -776,7 +780,6 @@ private:
     // GCS_Mavlink.cpp
     void gcs_send_heartbeat(void);
     void gcs_send_deferred(void);
-    void send_heartbeat(mavlink_channel_t chan);
     void send_attitude(mavlink_channel_t chan);
     void send_fence_status(mavlink_channel_t chan);
     void send_extended_status1(mavlink_channel_t chan);
@@ -949,7 +952,7 @@ private:
     void check_usb_mux(void);
     bool should_log(uint32_t mask);
     void set_default_frame_class();
-    uint8_t get_frame_mav_type();
+    MAV_TYPE get_frame_mav_type();
     const char* get_frame_string();
     void allocate_motors(void);
 
diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp
index 4c57dd5410..2eb0b1c8f7 100644
--- a/ArduCopter/GCS_Mavlink.cpp
+++ b/ArduCopter/GCS_Mavlink.cpp
@@ -22,17 +22,14 @@ void Copter::gcs_send_deferred(void)
  *  pattern below when adding any new messages
  */
 
-NOINLINE void Copter::send_heartbeat(mavlink_channel_t chan)
+MAV_TYPE GCS_MAVLINK_Copter::frame_type() const
 {
-    uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
-    uint8_t system_status = ap.land_complete ? MAV_STATE_STANDBY : MAV_STATE_ACTIVE;
-    uint32_t custom_mode = control_mode;
-
-    // set system as critical if any failsafe have triggered
-    if (failsafe.radio || battery.has_failsafed() || failsafe.gcs || failsafe.ekf || failsafe.terrain || failsafe.adsb)  {
-        system_status = MAV_STATE_CRITICAL;
-    }
+    return copter.get_frame_mav_type();
+}
 
+MAV_MODE GCS_MAVLINK_Copter::base_mode() const
+{
+    uint8_t base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
     // work out the base_mode. This value is not very useful
     // for APM, but we calculate it as best we can so a generic
     // MAVLink enabled ground station can work out something about
@@ -41,8 +38,7 @@ NOINLINE void Copter::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
-    base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
-    switch (control_mode) {
+    switch (copter.control_mode) {
     case AUTO:
     case RTL:
     case LOITER:
@@ -71,19 +67,37 @@ NOINLINE void Copter::send_heartbeat(mavlink_channel_t chan)
 #endif
 
     // we are armed if we are not initialising
-    if (motors != nullptr && motors->armed()) {
+    if (copter.motors != nullptr && copter.motors->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(get_frame_mav_type(),
-                                            base_mode,
-                                            custom_mode,
-                                            system_status);
+    return (MAV_MODE)base_mode;
 }
 
+uint32_t GCS_MAVLINK_Copter::custom_mode() const
+{
+    return copter.control_mode;
+}
+
+
+MAV_STATE GCS_MAVLINK_Copter::system_status() const
+{
+    // set system as critical if any failsafe have triggered
+    if (copter.any_failsafe_triggered())  {
+        return MAV_STATE_CRITICAL;
+    }
+
+    if (copter.ap.land_complete) {
+        return MAV_STATE_STANDBY;
+    }
+
+    return MAV_STATE_ACTIVE;
+}
+
+
 NOINLINE void Copter::send_attitude(mavlink_channel_t chan)
 {
     const Vector3f &gyro = ins.get_gyro();
@@ -296,7 +310,7 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
     case MSG_HEARTBEAT:
         CHECK_PAYLOAD_SIZE(HEARTBEAT);
         last_heartbeat_time = AP_HAL::millis();
-        copter.send_heartbeat(chan);
+        send_heartbeat();
         break;
 
     case MSG_EXTENDED_STATUS1:
diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h
index dbf387a3b4..4fd6a04274 100644
--- a/ArduCopter/GCS_Mavlink.h
+++ b/ArduCopter/GCS_Mavlink.h
@@ -48,4 +48,9 @@ private:
 
     void packetReceived(const mavlink_status_t &status,
                         mavlink_message_t &msg) 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/ArduCopter/system.cpp b/ArduCopter/system.cpp
index 672db545af..7648710be1 100644
--- a/ArduCopter/system.cpp
+++ b/ArduCopter/system.cpp
@@ -446,7 +446,7 @@ void Copter::set_default_frame_class()
 }
 
 // return MAV_TYPE corresponding to frame class
-uint8_t Copter::get_frame_mav_type()
+MAV_TYPE Copter::get_frame_mav_type()
 {
     switch ((AP_Motors::motor_frame_class)g2.frame_class.get()) {
         case AP_Motors::MOTOR_FRAME_QUAD: