diff --git a/custom-example/qgroundcontrol.qrc b/custom-example/qgroundcontrol.qrc index a95301e..58486f4 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 8f6cb98..3d3293c 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -572,6 +572,7 @@ HEADERS += \ src/AnalyzeView/ULogParser.h \ src/AnalyzeView/MavlinkConsoleController.h \ src/Audio/AudioOutput.h \ + src/Vehicle/Autotune.h \ src/Camera/QGCCameraControl.h \ src/Camera/QGCCameraIO.h \ src/Camera/QGCCameraManager.h \ @@ -815,6 +816,7 @@ SOURCES += \ src/AnalyzeView/ULogParser.cc \ src/AnalyzeView/MavlinkConsoleController.cc \ src/Audio/AudioOutput.cc \ + src/Vehicle/Autotune.cpp \ src/Camera/QGCCameraControl.cc \ src/Camera/QGCCameraIO.cc \ src/Camera/QGCCameraManager.cc \ diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index 677bda3..36abbec 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 2f7abfb..8b1ddbf 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 @@ -1125,3 +1126,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 d94c0b1..7b9fec3 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 @@ public: // 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 cda50e1..f2e93e5 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 0000000..80b6712 --- /dev/null +++ b/src/QmlControls/AutotuneUI.qml @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * (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 QtQuick.Dialogs 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 + + readonly property string dialogTitle: qsTr("Autotune") + + QGCPalette { + id: palette + colorGroupEnabled: enabled + } + + Component { + id: autotuneConfirmationDialogComponent + QGCViewMessage { + message: qsTr("WARNING!\ +\n\nThe auto-tuning procedure should be executed with caution and requires the vehicle to fly stable enough before \ +attempting the procedure!\n\nBefore starting the auto-tuning process, make sure that: \ +\n1. You have read the auto-tuning guide and have followed the preliminary steps \ +\n2. The current control gains are good enough to stabilize the drone in presence of medium disturbances \ +\n3. You are ready to abort the auto-tuning sequence by moving the RC sticks, if anything unexpected happens. \ +\n\nClick Ok to start the auto-tuning process.\n") + + function accept() { + hideDialog() + _autotune.autotuneRequest() + } + } + } + + Rectangle { + width: _root.width + height: statusColumn.height + (2 * _margins) + color: palette.windowShade + enabled: _autotune.autotuneEnabled + + QGCButton { + id: autotuneButton + primary: true + text: dialogTitle + enabled: !_autotune.autotuneInProgress + anchors { + left: parent.left + leftMargin: _margins + verticalCenter: parent.verticalCenter + } + + onClicked: { + mainWindow.showComponentDialog(autotuneConfirmationDialogComponent, + dialogTitle, + mainWindow.showDialogDefaultWidth, + StandardButton.Ok | StandardButton.Cancel) + } + } + + 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 db02bfc..cd88582 100644 --- a/src/QmlControls/CMakeLists.txt +++ b/src/QmlControls/CMakeLists.txt @@ -50,6 +50,7 @@ add_custom_target(QmlControlsQml EditPositionDialog.qml ExclusiveGroupItem.qml FactSliderPanel.qml + AutotuneUI.qml FileButton.qml FlightModeDropdown.qml FlightModeMenu.qml @@ -81,6 +82,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 ea690a0..fca2765 100644 --- a/src/QmlControls/PIDTuning.qml +++ b/src/QmlControls/PIDTuning.qml @@ -177,77 +177,113 @@ RowLayout { Layout.minimumWidth: contentWidth Layout.maximumWidth: contentWidth Layout.alignment: Qt.AlignTop + Column { spacing: _margins Layout.alignment: Qt.AlignTop + width: parent.width id: rightColumn + Row { + id: _autotuneSelectRow + spacing: _margins + visible: tuningMode === Vehicle.ModeRateAndAttitude + + Switch { + id: autotuningEnabled + checked: true + } + + QGCLabel { + color: qgcPal.text + text: autotuningEnabled.checked ? qsTr("Autotune enabled") : qsTr("Autotune disabled") + } + } + Column { + width: parent.width + visible: _autotuneSelectRow.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: !_autotuneSelectRow.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 8bd1654..557be24 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 0000000..8b6e18b --- /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(); +} + + +//----------------------------------------------------------------------------- +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."; + } +} + + +//----------------------------------------------------------------------------- +bool Autotune::autotuneEnabled() +{ + return _vehicle->flying() || _autotuneInProgress; +} + + +//----------------------------------------------------------------------------- +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: 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 0000000..6da1883 --- /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 (); + + static void ackHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, Vehicle::MavCmdResultFailureCode_t failureCode); + + bool autotuneEnabled (); + bool autotuneInProgress () { return _autotuneInProgress; } + float autotuneProgress () { return _autotuneProgress; } + QString autotuneStatus () { return _autotuneStatus; } + + +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 19390e1..058d09c 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 bd17e5c..880bc4a 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 @@ public: 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 @@ public: 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: JoystickManager* _joystickManager = nullptr; ComponentInformationManager* _componentInformationManager = nullptr; VehicleObjectAvoidance* _objectAvoidance = nullptr; + Autotune* _autotune = nullptr; #if defined(QGC_AIRMAP_ENABLED) AirspaceVehicleManager* _airspaceVehicleManager = nullptr; #endif