From 220b329cd093990a5400fc4e06ca3f8bdf27e2a3 Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Thu, 2 May 2019 14:40:05 -0700
Subject: [PATCH] PID tuning rate adjustment

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

diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index 43d824765..2126b1c48 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -3859,6 +3859,32 @@ int  Vehicle::versionCompare(int major, int minor, int patch)
     return _firmwarePlugin->versionCompare(this, major, minor, patch);
 }
 
+void Vehicle::setPIDTuningTelemetryMode(bool pidTuning)
+{
+    qDebug() << "setPIDTuningTelemetryMode" << pidTuning;
+    QList<int> rgTelemetry;
+
+    if (pidTuning) {
+        rgTelemetry << MAVLINK_MSG_ID_ATTITUDE << MAVLINK_MSG_ID_ATTITUDE_TARGET;
+        for (int telemetry: rgTelemetry) {
+            sendMavCommand(defaultComponentId(),
+                           MAV_CMD_SET_MESSAGE_INTERVAL,
+                           true,                        // show error
+                           telemetry,
+                           pidTuning ? (1000000.0 / 30.0) /* Hz */ : -1 /* default Hz */);
+        }
+    }
+
+    setLiveUpdates(pidTuning);
+    _setpointFactGroup.setLiveUpdates(pidTuning);
+
+    sendMavCommand(defaultComponentId(),
+                   MAV_CMD_GET_MESSAGE_INTERVAL,
+                   true,                        // show error
+                   MAVLINK_MSG_ID_ATTITUDE);
+}
+
+
 #if !defined(NO_ARDUPILOT_DIALECT)
 void Vehicle::flashBootloader(void)
 {
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index a71e6a552..71190b5c6 100644
--- a/src/Vehicle/Vehicle.h
+++ b/src/Vehicle/Vehicle.h
@@ -758,6 +758,8 @@ public:
     ///     @param percent 0-no power, 100-full power
     Q_INVOKABLE void motorTest(int motor, int percent);
 
+    Q_INVOKABLE void setPIDTuningTelemetryMode(bool pidTuning);
+
 #if !defined(NO_ARDUPILOT_DIALECT)
     Q_INVOKABLE void flashBootloader(void);
 #endif
-- 
2.22.0