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)
 {

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