From 66fa948e27c247ad1108049accf13e80a18fe7fd Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 17 May 2019 15:53:39 +0900
Subject: [PATCH] Rover: send position-target-global-int mavlink messages

---
 APMrover2/GCS_Mavlink.cpp | 25 +++++++++++++++++++++++++
 APMrover2/GCS_Mavlink.h   |  2 ++
 2 files changed, 27 insertions(+)

diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp
index 63d075f5a7..eb92f69432 100644
--- a/APMrover2/GCS_Mavlink.cpp
+++ b/APMrover2/GCS_Mavlink.cpp
@@ -73,6 +73,30 @@ MAV_STATE GCS_MAVLINK_Rover::system_status() const
     return MAV_STATE_ACTIVE;
 }
 
+void GCS_MAVLINK_Rover::send_position_target_global_int()
+{
+    Location target;
+    if (!rover.control_mode->get_desired_location(target)) {
+        return;
+    }
+    mavlink_msg_position_target_global_int_send(
+        chan,
+        AP_HAL::millis(), // time_boot_ms
+        MAV_FRAME_GLOBAL_INT, // targets are always global altitude
+        0xFFF8, // ignore everything except the x/y/z components
+        target.lat, // latitude as 1e7
+        target.lng, // longitude as 1e7
+        target.alt * 0.01f, // altitude is sent as a float
+        0.0f, // vx
+        0.0f, // vy
+        0.0f, // vz
+        0.0f, // afx
+        0.0f, // afy
+        0.0f, // afz
+        0.0f, // yaw
+        0.0f); // yaw_rate
+}
+
 void GCS_MAVLINK_Rover::send_nav_controller_output() const
 {
     if (!rover.control_mode->is_autopilot_mode()) {
@@ -438,6 +462,7 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
     MSG_GPS2_RTK,
     MSG_NAV_CONTROLLER_OUTPUT,
     MSG_FENCE_STATUS,
+    MSG_POSITION_TARGET_GLOBAL_INT,
 };
 static const ap_message STREAM_POSITION_msgs[] = {
     MSG_LOCATION,
diff --git a/APMrover2/GCS_Mavlink.h b/APMrover2/GCS_Mavlink.h
index 7e56f23378..81e0dca5c8 100644
--- a/APMrover2/GCS_Mavlink.h
+++ b/APMrover2/GCS_Mavlink.h
@@ -21,6 +21,8 @@ protected:
     MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet) override;
     MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override;
 
+    void send_position_target_global_int() override;
+
     virtual bool in_hil_mode() const override;
 
     bool persist_streamrates() const override { return true; }