From 066a443e5f201dff624f9964dee654679b19267b Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 8 Mar 2019 14:39:59 +0900
Subject: [PATCH] GCS_MAVLink: add send_set_position_target_global_int

this supports sending the position target to an offboard navigation controller
---
 libraries/GCS_MAVLink/GCS.h          |  1 +
 libraries/GCS_MAVLink/GCS_Common.cpp | 24 ++++++++++++++++++++++++
 2 files changed, 25 insertions(+)

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index a8fc9bfded..964fc4584c 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -232,6 +232,7 @@ public:
     void send_accelcal_vehicle_position(uint32_t position);
     void send_scaled_imu(uint8_t instance, void (*send_fn)(mavlink_channel_t chan, uint32_t time_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag));
     void send_sys_status();
+    void send_set_position_target_global_int(uint8_t target_system, uint8_t target_component, const Location& loc);
 
     // return a bitmap of active channels. Used by libraries to loop
     // over active channels to send to all active channels    
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index 14c40f2159..9c6d9fe1a1 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -3925,6 +3925,30 @@ void GCS_MAVLINK::send_mount_status() const
     mount->send_mount_status(chan);
 }
 
+void GCS_MAVLINK::send_set_position_target_global_int(uint8_t target_system, uint8_t target_component, const Location& loc)
+{
+
+    uint16_t type_mask = POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE | \
+                         POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE | \
+                         POSITION_TARGET_TYPEMASK_YAW_IGNORE | POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE;
+
+    uint8_t mav_frame = loc.relative_alt ? MAV_FRAME_GLOBAL_RELATIVE_ALT_INT : MAV_FRAME_GLOBAL_INT;
+
+    mavlink_msg_set_position_target_global_int_send(
+            chan,
+            AP_HAL::millis(),
+            target_system,
+            target_component,
+            mav_frame,
+            type_mask,
+            loc.lat,
+            loc.lng,
+            loc.alt,
+            0,0,0,  // vx, vy, vz
+            0,0,0,  // ax, ay, az
+            0,0);   // yaw, yaw_rate
+}
+
 bool GCS_MAVLINK::try_send_message(const enum ap_message id)
 {
     bool ret = true;