From 1a87d7f5fff6636073e20c4b67f4b1eba129111a Mon Sep 17 00:00:00 2001
From: Gus Grubba <gus@auterion.com>
Date: Mon, 3 Jun 2019 09:37:34 -0400
Subject: [PATCH] Add gimbal control command to Vehicle

---
 src/Vehicle/Vehicle.cc | 15 +++++++++++++++
 src/Vehicle/Vehicle.h  |  2 ++
 2 files changed, 17 insertions(+)

diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index 81ca6f1ba..cf12aee90 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -3977,6 +3977,21 @@ void Vehicle::flashBootloader(void)
 }
 #endif
 
+void Vehicle::gimbalControlValue(double pitch, double yaw)
+{
+    qDebug() << "Gimbal: " << pitch << yaw;
+    sendMavCommand(
+        _defaultComponentId,
+        MAV_CMD_DO_MOUNT_CONTROL,
+        false,                               // show errors
+        static_cast<float>(pitch),           // Pitch 0 - 90
+        0,                                   // Roll (not used)
+        static_cast<float>(yaw),             // Yaw -180 - 180
+        0,                                   // Altitude (not used)
+        0,                                   // Latitude (not used)
+        0,                                   // Longitude (not used)
+        MAV_MOUNT_MODE_MAVLINK_TARGETING);   // MAVLink Roll,Pitch,Yaw
+}
 
 //-----------------------------------------------------------------------------
 //-----------------------------------------------------------------------------
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index eb00e19fa..6c02c4ccf 100644
--- a/src/Vehicle/Vehicle.h
+++ b/src/Vehicle/Vehicle.h
@@ -761,6 +761,8 @@ public:
 
     Q_INVOKABLE void setPIDTuningTelemetryMode(bool pidTuning);
 
+    Q_INVOKABLE void gimbalControlValue(double pitch, double yaw);
+
 #if !defined(NO_ARDUPILOT_DIALECT)
     Q_INVOKABLE void flashBootloader(void);
 #endif
-- 
2.22.0