Atanas Batinkov
4 years ago
committed by
GitHub
14 changed files with 458 additions and 46 deletions
@ -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 @@ |
|||||||
|
/****************************************************************************
|
||||||
|
* |
||||||
|
* (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 @@ |
|||||||
|
/****************************************************************************
|
||||||
|
* |
||||||
|
* (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