diff --git a/custom-example/qgroundcontrol.qrc b/custom-example/qgroundcontrol.qrc
index a95301ea07b2..58486f4c2b55 100644
--- a/custom-example/qgroundcontrol.qrc
+++ b/custom-example/qgroundcontrol.qrc
@@ -125,6 +125,7 @@
../src/QmlControls/PreFlightCheckGroup.qml
../src/QmlControls/PreFlightCheckModel.qml
../src/QmlControls/QGCButton.qml
+ ../src/QmlControls/AutotuneUI.qml
../src/QmlControls/QGCCheckBox.qml
../src/QmlControls/QGCColoredImage.qml
../src/QmlControls/QGCControlDebug.qml
diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index 8f6cb98346f5..cd7832cf972a 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -498,6 +498,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin {
src/qgcunittest/MultiSignalSpy.h \
src/qgcunittest/MultiSignalSpyV2.h \
src/qgcunittest/UnitTest.h \
+ src/Vehicle/Autotune.h \
src/Vehicle/FTPManagerTest.h \
src/Vehicle/InitialConnectTest.h \
src/Vehicle/RequestMessageTest.h \
@@ -547,6 +548,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin {
src/qgcunittest/MultiSignalSpyV2.cc \
src/qgcunittest/UnitTest.cc \
src/qgcunittest/UnitTestList.cc \
+ src/Vehicle/Autotune.cpp \
src/Vehicle/FTPManagerTest.cc \
src/Vehicle/InitialConnectTest.cc \
src/Vehicle/RequestMessageTest.cc \
diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc
index 677bda3d11b9..36abbec4d480 100644
--- a/qgroundcontrol.qrc
+++ b/qgroundcontrol.qrc
@@ -126,6 +126,7 @@
src/QmlControls/PreFlightCheckGroup.qml
src/QmlControls/PreFlightCheckModel.qml
src/QmlControls/QGCButton.qml
+ src/QmlControls/AutotuneUI.qml
src/QmlControls/QGCCheckBox.qml
src/QmlControls/QGCColoredImage.qml
src/QmlControls/QGCControlDebug.qml
diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc
index 87a32815595c..b61992e8bffe 100644
--- a/src/FirmwarePlugin/FirmwarePlugin.cc
+++ b/src/FirmwarePlugin/FirmwarePlugin.cc
@@ -16,6 +16,7 @@
#include "QGCFileDownload.h"
#include "QGCCameraManager.h"
#include "RadioComponentController.h"
+#include "Autotune.h"
#include
#include
@@ -1109,3 +1110,8 @@ void FirmwarePlugin::sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionRe
vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), message);
}
}
+
+Autotune* FirmwarePlugin::createAutotune(Vehicle *vehicle)
+{
+ return new Autotune(vehicle);
+}
diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h
index d94c0b169388..7b9fec3009b0 100644
--- a/src/FirmwarePlugin/FirmwarePlugin.h
+++ b/src/FirmwarePlugin/FirmwarePlugin.h
@@ -28,6 +28,7 @@
class Vehicle;
class QGCCameraControl;
class QGCCameraManager;
+class Autotune;
/// This is the base class for Firmware specific plugins
///
@@ -333,6 +334,9 @@ class FirmwarePlugin : public QObject
// gets hobbs meter from autopilot. This should be reimplmeented for each firmware
virtual QString getHobbsMeter(Vehicle* vehicle) { Q_UNUSED(vehicle); return "Not Supported"; }
+ /// Creates Autotune object.
+ virtual Autotune* createAutotune(Vehicle *vehicle);
+
signals:
void toolIndicatorsChanged(void);
void modeIndicatorsChanged(void);
diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc
index cda50e14fce3..f2e93e59839a 100644
--- a/src/QGCApplication.cc
+++ b/src/QGCApplication.cc
@@ -101,6 +101,7 @@
#include "ToolStripActionList.h"
#include "QGCMAVLink.h"
#include "VehicleLinkManager.h"
+#include "Autotune.h"
#if defined(QGC_ENABLE_PAIRING)
#include "PairingManager.h"
@@ -523,6 +524,7 @@ void QGCApplication::_initCommon()
qmlRegisterUncreatableType (kQGCVehicle, 1, 0, "QGCVideoStreamInfo", kRefOnly);
qmlRegisterUncreatableType (kQGCVehicle, 1, 0, "LinkInterface", kRefOnly);
qmlRegisterUncreatableType (kQGCVehicle, 1, 0, "VehicleLinkManager", kRefOnly);
+ qmlRegisterUncreatableType (kQGCVehicle, 1, 0, "Autotune", kRefOnly);
qmlRegisterUncreatableType (kQGCControllers, 1, 0, "MissionController", kRefOnly);
qmlRegisterUncreatableType (kQGCControllers, 1, 0, "GeoFenceController", kRefOnly);
@@ -530,7 +532,7 @@ void QGCApplication::_initCommon()
qmlRegisterUncreatableType (kQGroundControl, 1, 0, "MissionItem", kRefOnly);
qmlRegisterUncreatableType (kQGroundControl, 1, 0, "VisualMissionItem", kRefOnly);
- qmlRegisterUncreatableType (kQGroundControl, 1, 0, "FlightPathSegment", kRefOnly);
+ qmlRegisterUncreatableType (kQGroundControl, 1, 0, "FlightPathSegment", kRefOnly);
qmlRegisterUncreatableType (kQGroundControl, 1, 0, "QmlObjectListModel", kRefOnly);
qmlRegisterUncreatableType (kQGroundControl, 1, 0, "MissionCommandTree", kRefOnly);
qmlRegisterUncreatableType (kQGroundControl, 1, 0, "CameraCalc", kRefOnly);
diff --git a/src/QmlControls/AutotuneUI.qml b/src/QmlControls/AutotuneUI.qml
new file mode 100644
index 000000000000..b15f58c9d6b2
--- /dev/null
+++ b/src/QmlControls/AutotuneUI.qml
@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ * (c) 2009-2021 QGROUNDCONTROL PROJECT
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+import QtQuick 2.3
+import QtQuick.Controls 1.2
+
+import QGroundControl.FactSystem 1.0
+import QGroundControl.FactControls 1.0
+import QGroundControl.Controllers 1.0
+import QGroundControl.Palette 1.0
+import QGroundControl.Controls 1.0
+import QGroundControl.ScreenTools 1.0
+
+Item {
+ id: _root
+
+ property var _autotune: globals.activeVehicle.autotune
+ property real _margins: ScreenTools.defaultFontPixelHeight
+
+ QGCPalette {
+ id: palette
+ colorGroupEnabled: enabled
+ }
+
+ Rectangle {
+ width: _root.width
+ height: statusColumn.height + (2 * _margins)
+ color: palette.windowShade
+ enabled: _autotune.autotuneEnabled
+
+ QGCButton {
+ id: autotuneButton
+ primary: true
+ text: "Autotune"
+ enabled: !_autotune.autotuneInProgress
+ anchors {
+ left: parent.left
+ leftMargin: _margins
+ verticalCenter: parent.verticalCenter
+ }
+
+ onClicked: {
+ _autotune.autotuneRequest()
+ }
+ }
+
+ Column {
+ id: statusColumn
+ spacing: _margins
+ anchors {
+ left: autotuneButton.right
+ right: parent.right
+ leftMargin: _margins
+ rightMargin: _margins
+ verticalCenter: parent.verticalCenter
+ }
+
+ QGCLabel {
+ text: _autotune.autotuneStatus
+
+ anchors {
+ left: parent.left
+ }
+ }
+
+ ProgressBar {
+ value: _autotune.autotuneProgress
+
+ anchors {
+ left: parent.left
+ right: parent.right
+ }
+ }
+ }
+ }
+}
diff --git a/src/QmlControls/CMakeLists.txt b/src/QmlControls/CMakeLists.txt
index db02bfca28b7..93f0f99b30e4 100644
--- a/src/QmlControls/CMakeLists.txt
+++ b/src/QmlControls/CMakeLists.txt
@@ -81,6 +81,7 @@ add_custom_target(QmlControlsQml
PreFlightCheckList.qml
PreFlightCheckModel.qml
QGCButton.qml
+ AutotuneUI.qml
QGCCheckBox.qml
QGCColoredImage.qml
QGCComboBox.qml
diff --git a/src/QmlControls/PIDTuning.qml b/src/QmlControls/PIDTuning.qml
index ea690a09138d..265039df1082 100644
--- a/src/QmlControls/PIDTuning.qml
+++ b/src/QmlControls/PIDTuning.qml
@@ -177,77 +177,111 @@ RowLayout {
Layout.minimumWidth: contentWidth
Layout.maximumWidth: contentWidth
Layout.alignment: Qt.AlignTop
+
Column {
spacing: _margins
Layout.alignment: Qt.AlignTop
+
width: parent.width
id: rightColumn
+ Row {
+ spacing: _margins
+
+ Switch {
+ id: autotuningEnabled
+ checked: true
+ }
+
+ QGCLabel {
+ color: qgcPal.text
+ text: autotuningEnabled.checked ? qsTr("Autotune enabled") : qsTr("Autotune disabled")
+ }
+ }
+
Column {
+ width: parent.width
+ visible: autotuningEnabled.checked
- RowLayout {
- spacing: _margins
- visible: axis.length > 1
+ AutotuneUI {
+ anchors {
+ top: parent.top
+ topMargin: _margins * 2
+ }
- QGCLabel { text: qsTr("Select Tuning:") }
+ width: parent.width
+ }
+ }
- Repeater {
- model: axis
- QGCRadioButton {
- text: modelData.name
- checked: index == _currentAxis
- onClicked: _currentAxis = index
+ Column {
+ width: parent.width
+ visible: !autotuningEnabled.checked
+
+ Column {
+ RowLayout {
+ spacing: _margins
+ visible: axis.length > 1
+
+ QGCLabel { text: qsTr("Select Tuning:") }
+
+ Repeater {
+ model: axis
+ QGCRadioButton {
+ text: modelData.name
+ checked: index == _currentAxis
+ onClicked: _currentAxis = index
+ }
}
}
}
- }
- // Instantiate all sliders (instead of switching the model), so that
- // values are not changed unexpectedly if they do not match with a tick
- // value
- Repeater {
- model: axis
- FactSliderPanel {
- width: parent.width
- visible: _currentAxis === index
- sliderModel: axis[index].params
+ // Instantiate all sliders (instead of switching the model), so that
+ // values are not changed unexpectedly if they do not match with a tick
+ // value
+ Repeater {
+ model: axis
+ FactSliderPanel {
+ width: parent.width
+ visible: _currentAxis === index
+ sliderModel: axis[index].params
+ }
}
- }
- Column {
- QGCLabel { text: qsTr("Clipboard Values:") }
+ Column {
+ QGCLabel { text: qsTr("Clipboard Values:") }
- GridLayout {
- rows: savedRepeater.model.length
- flow: GridLayout.TopToBottom
- rowSpacing: 0
- columnSpacing: _margins
+ GridLayout {
+ rows: savedRepeater.model.length
+ flow: GridLayout.TopToBottom
+ rowSpacing: 0
+ columnSpacing: _margins
- Repeater {
- model: axis[_currentAxis].params
+ Repeater {
+ model: axis[_currentAxis].params
- QGCLabel { text: param }
- }
+ QGCLabel { text: param }
+ }
- Repeater {
- id: savedRepeater
+ Repeater {
+ id: savedRepeater
- QGCLabel { text: modelData }
+ QGCLabel { text: modelData }
+ }
}
}
- }
- RowLayout {
- spacing: _margins
+ RowLayout {
+ spacing: _margins
- QGCButton {
- text: qsTr("Save To Clipboard")
- onClicked: saveTuningParamValues()
- }
+ QGCButton {
+ text: qsTr("Save To Clipboard")
+ onClicked: saveTuningParamValues()
+ }
- QGCButton {
- text: qsTr("Restore From Clipboard")
- onClicked: resetToSavedTuningParamValues()
+ QGCButton {
+ text: qsTr("Restore From Clipboard")
+ onClicked: resetToSavedTuningParamValues()
+ }
}
}
}
diff --git a/src/QmlControls/QGroundControl/Controls/qmldir b/src/QmlControls/QGroundControl/Controls/qmldir
index 8bd165467bbb..557be2405cb8 100644
--- a/src/QmlControls/QGroundControl/Controls/qmldir
+++ b/src/QmlControls/QGroundControl/Controls/qmldir
@@ -53,6 +53,7 @@ PreFlightCheckButton 1.0 PreFlightCheckButton.qml
PreFlightCheckGroup 1.0 PreFlightCheckGroup.qml
PreFlightCheckModel 1.0 PreFlightCheckModel.qml
QGCButton 1.0 QGCButton.qml
+AutotuneUI 1.0 AutotuneUI.qml
QGCCheckBox 1.0 QGCCheckBox.qml
QGCColoredImage 1.0 QGCColoredImage.qml
QGCComboBox 1.0 QGCComboBox.qml
diff --git a/src/Vehicle/Autotune.cpp b/src/Vehicle/Autotune.cpp
new file mode 100644
index 000000000000..52cdab8eb259
--- /dev/null
+++ b/src/Vehicle/Autotune.cpp
@@ -0,0 +1,177 @@
+/****************************************************************************
+ *
+ * (c) 2009-2021 QGROUNDCONTROL PROJECT
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#include
+
+#include "QGCApplication.h"
+#include "Autotune.h"
+
+
+//-----------------------------------------------------------------------------
+Autotune::Autotune(Vehicle *vehicle) :
+ QObject(vehicle)
+ , _vehicle(vehicle)
+{
+ connect(_vehicle, &Vehicle::flyingChanged, this, &Autotune::handleEnabled);
+ connect(_vehicle, &Vehicle::landingChanged, this, &Autotune::handleEnabled);
+
+ _pollTimer.setInterval(1000); // 1s for the polling interval
+ _pollTimer.setSingleShot(false);
+ connect(&_pollTimer, &QTimer::timeout, this, &Autotune::sendMavlinkRequest);
+}
+
+
+//-----------------------------------------------------------------------------
+void Autotune::autotuneRequest()
+{
+ sendMavlinkRequest();
+
+ startTimers();
+ _autotuneInProgress = true;
+ _autotuneStatus = tr("Autotune: In progress");
+
+ emit autotuneChanged();
+}
+
+
+//-----------------------------------------------------------------------------
+bool Autotune::autotuneEnabled()
+{
+ return _vehicle->flying() || _autotuneInProgress;
+}
+
+
+//-----------------------------------------------------------------------------
+void Autotune::ackHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, Vehicle::MavCmdResultFailureCode_t failureCode)
+{
+ Q_UNUSED(compId);
+ Q_UNUSED(failureCode);
+
+ auto * autotune = static_cast(resultHandlerData);
+
+ if (autotune->_autotuneInProgress) {
+ if ((commandResult == MAV_RESULT_IN_PROGRESS) || (commandResult == MAV_RESULT_ACCEPTED)) {
+ autotune->handleAckStatus(progress);
+ }
+ else if (commandResult == MAV_RESULT_FAILED) {
+ autotune->handleAckFailure();
+ }
+ else {
+ autotune->handleAckError(commandResult);
+ }
+
+ emit autotune->autotuneChanged();
+ }
+ else {
+ qWarning() << "Ack received for a command different from MAV_CMD_DO_AUTOTUNE_ENABLE ot wrong UI state.";
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+void Autotune::handleEnabled()
+{
+ emit autotuneChanged();
+}
+
+
+//-----------------------------------------------------------------------------
+void Autotune::handleAckStatus(uint8_t ackProgress)
+{
+ _autotuneProgress = ackProgress/100.f;
+
+ if (ackProgress < 20) {
+ _autotuneStatus = tr("Autotune: initializing");
+ }
+ else if (ackProgress < 40) {
+ _autotuneStatus = tr("Autotune: roll");
+ }
+ else if (ackProgress < 60) {
+ _autotuneStatus = tr("Autotune: pitch");
+ }
+ else if (ackProgress < 80) {
+ _autotuneStatus = tr("Autotune: yaw");
+ }
+ else if (ackProgress == 95) {
+ _autotuneStatus = tr("Wait for disarm");
+
+ if(!_disarmMessageDisplayed) {
+ qgcApp()->showAppMessage(tr("Land and disarm the vehicle in order to apply the parameters."));
+ _disarmMessageDisplayed = true;
+ }
+ }
+ else if (ackProgress < 100) {
+ _autotuneStatus = tr("Autotune: in progress");
+ }
+ else { // success or unknown error
+ stopTimers();
+ _autotuneInProgress = false;
+
+ if (ackProgress == 100) {
+ _autotuneStatus = tr("Autotune: Success");
+
+ qgcApp()->showAppMessage(tr("Autotune successful."));
+ }
+ else {
+ _autotuneStatus = tr("Autotune: Unknown error");
+ }
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+void Autotune::handleAckFailure()
+{
+ stopTimers();
+ _autotuneInProgress = false;
+ _disarmMessageDisplayed = false;
+ _autotuneStatus = tr("Autotune: Vehicle failed");
+}
+
+
+//-----------------------------------------------------------------------------
+void Autotune::handleAckError(uint8_t ackError)
+{
+ stopTimers();
+ _autotuneInProgress = false;
+ _disarmMessageDisplayed = false;
+ _autotuneStatus = tr("Autotune: Ack error %1").arg(ackError);
+}
+
+
+//-----------------------------------------------------------------------------
+void Autotune::startTimers()
+{
+ _pollTimer.start();
+}
+
+
+//-----------------------------------------------------------------------------
+void Autotune::stopTimers()
+{
+ _pollTimer.stop();
+}
+
+
+//-----------------------------------------------------------------------------
+void Autotune::sendMavlinkRequest()
+{
+ _vehicle->sendMavCommandWithHandler(
+ ackHandler, // Ack callback
+ this, // Ack callback data
+ MAV_COMP_ID_AUTOPILOT1, // the ID of the autopilot
+ MAV_CMD_DO_AUTOTUNE_ENABLE, // the mavlink command
+ 1, // request autotune
+ 0, // unused parameter
+ 0, // unused parameter
+ 0, // unused parameter
+ 0, // unused parameter
+ 0, // unused parameter
+ 0);
+}
diff --git a/src/Vehicle/Autotune.h b/src/Vehicle/Autotune.h
new file mode 100644
index 000000000000..791e6dfc8ab9
--- /dev/null
+++ b/src/Vehicle/Autotune.h
@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ * (c) 2009-2021 QGROUNDCONTROL PROJECT
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include
+#include "Vehicle.h"
+#include "MAVLinkProtocol.h"
+
+class Autotune : public QObject
+{
+ Q_OBJECT
+
+
+public:
+ explicit Autotune(Vehicle *vehicle);
+
+ Q_PROPERTY(bool autotuneEnabled READ autotuneEnabled NOTIFY autotuneChanged)
+ Q_PROPERTY(bool autotuneInProgress READ autotuneInProgress NOTIFY autotuneChanged)
+ Q_PROPERTY(float autotuneProgress READ autotuneProgress NOTIFY autotuneChanged)
+ Q_PROPERTY(QString autotuneStatus READ autotuneStatus NOTIFY autotuneChanged)
+
+ Q_INVOKABLE void autotuneRequest ();
+
+ bool autotuneEnabled ();
+ bool autotuneInProgress () { return _autotuneInProgress; }
+ float autotuneProgress () { return _autotuneProgress; }
+ QString autotuneStatus () { return _autotuneStatus; }
+
+ static void ackHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, Vehicle::MavCmdResultFailureCode_t failureCode);
+
+
+public slots:
+ void handleEnabled ();
+ void sendMavlinkRequest();
+
+
+private:
+ void handleAckStatus(uint8_t ackProgress);
+ void handleAckFailure();
+ void handleAckError(uint8_t ackError);
+ void startTimers();
+ void stopTimers();
+
+
+private slots:
+
+
+private:
+ Vehicle* _vehicle {nullptr};
+ bool _autotuneInProgress {false};
+ float _autotuneProgress {0.0};
+ QString _autotuneStatus {tr("Autotune: Not performed")};
+ bool _disarmMessageDisplayed {false};
+
+ QTimer _pollTimer; // the frequency at which the polling should be performed
+
+
+signals:
+ void autotuneChanged ();
+};
diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index 919d724b8341..b6a438c308a3 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -53,6 +53,7 @@
#ifdef QT_DEBUG
#include "MockLink.h"
#endif
+#include "Autotune.h"
#if defined(QGC_AIRMAP_ENABLED)
#include "AirspaceVehicleManager.h"
@@ -386,6 +387,8 @@ void Vehicle::_commonInit()
_objectAvoidance = new VehicleObjectAvoidance(this, this);
+ _autotune = _firmwarePlugin->createAutotune(this);
+
// GeoFenceManager needs to access ParameterManager so make sure to create after
_geoFenceManager = new GeoFenceManager(this);
connect(_geoFenceManager, &GeoFenceManager::error, this, &Vehicle::_geoFenceManagerError);
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index bd17e5ca8ef3..880bc4a70e33 100644
--- a/src/Vehicle/Vehicle.h
+++ b/src/Vehicle/Vehicle.h
@@ -69,6 +69,7 @@ class RequestMessageTest;
class LinkInterface;
class LinkManager;
class InitialConnectStateMachine;
+class Autotune;
#if defined(QGC_AIRMAP_ENABLED)
class AirspaceVehicleManager;
@@ -272,6 +273,7 @@ class Vehicle : public FactGroup
Q_PROPERTY(ParameterManager* parameterManager READ parameterManager CONSTANT)
Q_PROPERTY(VehicleLinkManager* vehicleLinkManager READ vehicleLinkManager CONSTANT)
Q_PROPERTY(VehicleObjectAvoidance* objectAvoidance READ objectAvoidance CONSTANT)
+ Q_PROPERTY(Autotune* autotune READ autotune CONSTANT)
// FactGroup object model properties
@@ -664,6 +666,7 @@ class Vehicle : public FactGroup
FTPManager* ftpManager () { return _ftpManager; }
ComponentInformationManager* compInfoManager () { return _componentInformationManager; }
VehicleObjectAvoidance* objectAvoidance () { return _objectAvoidance; }
+ Autotune* autotune () const { return _autotune; }
static const int cMaxRcChannels = 18;
@@ -1105,6 +1108,7 @@ private slots:
JoystickManager* _joystickManager = nullptr;
ComponentInformationManager* _componentInformationManager = nullptr;
VehicleObjectAvoidance* _objectAvoidance = nullptr;
+ Autotune* _autotune = nullptr;
#if defined(QGC_AIRMAP_ENABLED)
AirspaceVehicleManager* _airspaceVehicleManager = nullptr;
#endif