diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index 43d8247653e0676edaf7704fdb98780534644fe6..2126b1c48419671515c553cc3b991b3a18f69731 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 a71e6a552d8c04110ae33b25e595bd39e95c60a8..71190b5c6071ff340f05ac03648992bc385f7b41 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