From 18c494b25f4131d9a56552722ee1e5cd8cd99573 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 31 Jan 2018 13:59:06 +1100
Subject: [PATCH] Sub: move sending of vfr_hud up

---
 ArduSub/GCS_Mavlink.cpp | 16 ++--------------
 ArduSub/GCS_Mavlink.h   |  4 ++++
 ArduSub/Sub.h           |  1 -
 3 files changed, 6 insertions(+), 15 deletions(-)

diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp
index cdd03e55f8..22ab352c06 100644
--- a/ArduSub/GCS_Mavlink.cpp
+++ b/ArduSub/GCS_Mavlink.cpp
@@ -251,16 +251,9 @@ void NOINLINE Sub::send_nav_controller_output(mavlink_channel_t chan)
         0);
 }
 
-void NOINLINE Sub::send_vfr_hud(mavlink_channel_t chan)
+int16_t GCS_MAVLINK_Sub::vfr_hud_throttle() const
 {
-    mavlink_msg_vfr_hud_send(
-        chan,
-        gps.ground_speed(),
-        gps.ground_speed(),
-        (ahrs.yaw_sensor / 100) % 360,
-        (int16_t)(motors.get_throttle() * 100),
-        current_loc.alt / 100.0f,
-        climb_rate / 100.0f);
+    return (int16_t)(sub.motors.get_throttle() * 100);
 }
 
 /*
@@ -427,11 +420,6 @@ bool GCS_MAVLINK_Sub::try_send_message(enum ap_message id)
         sub.send_nav_controller_output(chan);
         break;
 
-    case MSG_VFR_HUD:
-        CHECK_PAYLOAD_SIZE(VFR_HUD);
-        sub.send_vfr_hud(chan);
-        break;
-
     case MSG_RPM:
 #if RPM_ENABLED == ENABLED
         CHECK_PAYLOAD_SIZE(RPM);
diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h
index 9b1a2934c1..8c7d7e5c40 100644
--- a/ArduSub/GCS_Mavlink.h
+++ b/ArduSub/GCS_Mavlink.h
@@ -46,4 +46,8 @@ private:
     MAV_MODE base_mode() const override;
     uint32_t custom_mode() const override;
     MAV_STATE system_status() const override;
+
+    int16_t vfr_hud_throttle() const override;
+    bool vfr_hud_make_alt_relative() const override { return true; }
+
 };
diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index a721db8cbe..0761e89415 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -476,7 +476,6 @@ private:
     void send_heartbeat(mavlink_channel_t chan);
     void send_extended_status1(mavlink_channel_t chan);
     void send_nav_controller_output(mavlink_channel_t chan);
-    void send_vfr_hud(mavlink_channel_t chan);
 #if RPM_ENABLED == ENABLED
     void send_rpm(mavlink_channel_t chan);
     void rpm_update();