Atanas Batinkov
4 years ago
committed by
GitHub
14 changed files with 458 additions and 46 deletions
@ -0,0 +1,106 @@
@@ -0,0 +1,106 @@
|
||||
/**************************************************************************** |
||||
* |
||||
* (c) 2009-2021 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org> |
||||
* |
||||
* 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 |
||||
} |
||||
} |
||||
} |
||||
} |
||||
} |
||||
@ -0,0 +1,177 @@
@@ -0,0 +1,177 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (c) 2009-2021 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
* |
||||
* QGroundControl is licensed according to the terms in the file |
||||
* COPYING.md in the root of the source code directory. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#include <QtGlobal> |
||||
|
||||
#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<Autotune *>(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); |
||||
} |
||||
@ -0,0 +1,67 @@
@@ -0,0 +1,67 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (c) 2009-2021 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
* |
||||
* QGroundControl is licensed according to the terms in the file |
||||
* COPYING.md in the root of the source code directory. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <QTimer> |
||||
#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 (); |
||||
}; |
||||
Loading…
Reference in new issue