Commit 1eba9577 authored by Lorenz Meier's avatar Lorenz Meier

Merge pull request #1155 from thomasgubler/rcparamtune

Rc param tune
parents 6d95cdc1 29d0fa44
Subproject commit 7d0aa0205ef8bd13bf855b21d3ebe054dc9109ef
Subproject commit 42f1a37397335b3c4c96b0a41c73d85e80d54a60
......@@ -166,10 +166,10 @@ WindowsBuild {
}
#
# By default warnings as errors are turned off. Even so, in order for a pull request
# to be accepted you must compile cleanly with warnings as errors turned on the default
# set of OS builds. See http://www.qgroundcontrol.org/dev/contribute for more details.
# You can use the WarningsAsErrorsOn CONFIG switch to turn warnings as errors on for your
# By default warnings as errors are turned off. Even so, in order for a pull request
# to be accepted you must compile cleanly with warnings as errors turned on the default
# set of OS builds. See http://www.qgroundcontrol.org/dev/contribute for more details.
# You can use the WarningsAsErrorsOn CONFIG switch to turn warnings as errors on for your
# own builds.
#
......@@ -239,7 +239,7 @@ RESOURCES += qgroundcontrol.qrc
TRANSLATIONS += \
es-MX.ts \
en-US.ts
DEPENDPATH += \
. \
plugins
......@@ -343,6 +343,7 @@ FORMS += \
src/ui/px4_configuration/PX4FirmwareUpgrade.ui \
src/ui/QGCUASFileView.ui \
src/QGCQmlWidgetHolder.ui \
src/ui/QGCMapRCToParamDialog.ui
HEADERS += \
src/MG.h \
......@@ -495,6 +496,8 @@ HEADERS += \
src/QGCQuickWidget.h \
src/QGCPalette.h \
src/QGCQmlWidgetHolder.h \
src/ui/QGCParamTreeWidget.h \
src/ui/QGCMapRCToParamDialog.h
SOURCES += \
src/main.cc \
......@@ -636,13 +639,15 @@ SOURCES += \
src/QGCQuickWidget.cc \
src/QGCPalette.cc \
src/QGCQmlWidgetHolder.cpp \
src/ui/QGCParamTreeWidget.cpp \
src/ui/QGCMapRCToParamDialog.cpp
#
# Unit Test specific configuration goes here
#
# We have to special case Windows debug_and_release builds because you can't have files
# which are only in the debug variant [QTBUG-40351]. So in this case we include unit tests
# even in the release variant. If you want a Windows release build with no unit tests run
# which are only in the debug variant [QTBUG-40351]. So in this case we include unit tests
# even in the release variant. If you want a Windows release build with no unit tests run
# qmake with CONFIG-=debug_and_release CONFIG+=release.
#
......
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
#ifndef MOCKUAS_H
......@@ -39,43 +39,43 @@
class MockUAS : public UASInterface
{
Q_OBJECT
signals:
// The following UASInterface signals are supported
// void parameterChanged(int uas, int component, QString parameterName, QVariant value);
// void remoteControlChannelRawChanged(int channelId, float raw);
public:
// Implemented UASInterface overrides
virtual int getSystemType(void) { return _systemType; }
virtual int getUASID(void) const { return _systemId; }
virtual QGCUASParamManagerInterface* getParamManager() { return &_paramManager; };
// sendMessage is only supported if a mavlink plugin is installed.
virtual void sendMessage(mavlink_message_t message);
public:
// MockUAS methods
MockUAS(void);
// Use these methods to setup/control the mock UAS
void setMockSystemType(int systemType) { _systemType = systemType; }
void setMockSystemId(int systemId) { _systemId = systemId; }
/// @return returns mock QGCUASParamManager associated with the UAS. This mock implementation
/// allows you to simulate parameter input and validate parameter setting
MockQGCUASParamManager* getMockQGCUASParamManager(void) { return &_paramManager; }
/// @brief Sets the parameter map into the mock QGCUASParamManager and signals parameterChanged for
/// each param
void setMockParametersAndSignal(MockQGCUASParamManager::ParamMap_t& map);
void emitRemoteControlChannelRawChanged(int channel, float raw) { emit remoteControlChannelRawChanged(channel, raw); }
/// @brief Installs a mavlink plugin. Only a single mavlink plugin is supported at a time.
void setMockMavlinkPlugin(MockMavlinkInterface* mavlinkPlugin) { _mavlinkPlugin = mavlinkPlugin; };
public:
// Unimplemented UASInterface overrides
virtual QString getUASName() const { Q_ASSERT(false); return _bogusString; };
......@@ -168,16 +168,19 @@ public slots:
virtual void sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x,
float flow_comp_m_y, quint8 quality, float ground_distance)
{ Q_UNUSED(time_us); Q_UNUSED(flow_x); Q_UNUSED(flow_y); Q_UNUSED(flow_comp_m_x); Q_UNUSED(flow_comp_m_y); Q_UNUSED(quality); Q_UNUSED(ground_distance); Q_ASSERT(false);};
virtual void sendMapRCToParam(QString param_id, float scale, float value0, quint8 param_rc_channel_index, float valueMin, float valueMax)
{ Q_UNUSED(param_id); Q_UNUSED(scale); Q_UNUSED(value0); Q_UNUSED(param_rc_channel_index); Q_UNUSED(valueMin); Q_UNUSED(valueMax); }
virtual void unsetRCToParameterMap() {}
virtual bool isRotaryWing() { Q_ASSERT(false); return false; }
virtual bool isFixedWing() { Q_ASSERT(false); return false; }
private:
int _systemType;
int _systemId;
MockQGCUASParamManager _paramManager;
MockMavlinkInterface* _mavlinkPlugin; ///< Mock Mavlink plugin, NULL for none
// Bogus variables used for return types of NYI methods
......
......@@ -2357,7 +2357,7 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName,
// Insert with correct type
// TODO: This is a hack for MAV_AUTOPILOT_ARDUPILOTMEGA until the new version of MAVLink and a fix for their param handling.
switch (rawValue.param_type) {
case MAV_PARAM_TYPE_REAL32:
if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
......@@ -2366,7 +2366,7 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName,
paramValue = QVariant(paramUnion.param_float);
}
break;
case MAV_PARAM_TYPE_UINT8:
if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
paramValue = QVariant(QChar((unsigned char)paramUnion.param_float));
......@@ -2374,7 +2374,7 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName,
paramValue = QVariant(QChar((unsigned char)paramUnion.param_uint8));
}
break;
case MAV_PARAM_TYPE_INT8:
if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
paramValue = QVariant(QChar((char)paramUnion.param_float));
......@@ -2382,7 +2382,7 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName,
paramValue = QVariant(QChar((char)paramUnion.param_int8));
}
break;
case MAV_PARAM_TYPE_INT16:
if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
paramValue = QVariant((short)paramUnion.param_float);
......@@ -2390,7 +2390,7 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName,
paramValue = QVariant(paramUnion.param_int16);
}
break;
case MAV_PARAM_TYPE_UINT32:
if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
paramValue = QVariant((unsigned int)paramUnion.param_float);
......@@ -2405,13 +2405,13 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName,
paramValue = QVariant(paramUnion.param_int32);
}
break;
default:
qCritical() << "INVALID DATA TYPE USED AS PARAMETER VALUE: " << rawValue.param_type;
}
qCDebug(UASLog) << "Received PARAM_VALUE" << paramName << paramValue << rawValue.param_type;
parameters.value(compId)->insert(paramName, paramValue);
emit parameterChanged(uasId, compId, paramName, paramValue);
emit parameterChanged(uasId, compId, rawValue.param_count, rawValue.param_index, paramName, paramValue);
......@@ -3290,12 +3290,12 @@ void UAS::removeLink(QObject* object)
{
qCDebug(UASLog) << "removeLink:" << QString("%1").arg((ulong)object, 0, 16);
qCDebug(UASLog) << "link count:" << links.count();
// Do not dynamic cast or de-reference QObject, since object is either in destructor or may have already
// been destroyed.
LinkInterface* link = (LinkInterface*)object;
int index = links.indexOf(link);
Q_ASSERT(index != -1);
links.removeAt(index);
......@@ -3467,3 +3467,55 @@ void UAS::stopLowBattAlarm()
lowBattAlarm = false;
}
}
void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 param_rc_channel_index, float valueMin, float valueMax)
{
mavlink_message_t message;
char param_id_cstr[MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = {};
// Copy string into buffer, ensuring not to exceed the buffer size
for (unsigned int i = 0; i < sizeof(param_id_cstr); i++)
{
if ((int)i < param_id.length())
{
param_id_cstr[i] = param_id.toLatin1()[i];
}
}
mavlink_msg_param_map_rc_pack(mavlink->getSystemId(),
mavlink->getComponentId(),
&message,
this->uasId,
0,
param_id_cstr,
-1,
param_rc_channel_index,
value0,
scale,
valueMin,
valueMax);
sendMessage(message);
qDebug() << "Mavlink message sent";
}
void UAS::unsetRCToParameterMap()
{
char param_id_cstr[MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = {};
for (int i = 0; i < 3; i++) {
mavlink_message_t message;
mavlink_msg_param_map_rc_pack(mavlink->getSystemId(),
mavlink->getComponentId(),
&message,
this->uasId,
0,
param_id_cstr,
-2,
i,
0.0f,
0.0f,
0.0f,
0.0f);
sendMessage(message);
}
}
......@@ -883,6 +883,12 @@ public slots:
/** @brief Triggers the action associated with the given ID. */
void triggerAction(int action);
/** @brief Send command to map a RC channel to a parameter */
void sendMapRCToParam(QString param_id, float scale, float value0, quint8 param_rc_channel_index, float valueMin, float valueMax);
/** @brief Send command to disable all bindings/maps between RC and parameters */
void unsetRCToParameterMap();
signals:
/** @brief The main/battery voltage has changed/was updated */
//void voltageChanged(int uasId, double voltage); // Defined in UASInterface already
......
......@@ -199,32 +199,32 @@ public:
*/
static QColor getNextColor() {
/* Create color map */
static QList<QColor> colors = QList<QColor>()
<< QColor(231,72,28)
<< QColor(104,64,240)
<< QColor(203,254,121)
static QList<QColor> colors = QList<QColor>()
<< QColor(231,72,28)
<< QColor(104,64,240)
<< QColor(203,254,121)
<< QColor(161,252,116)
<< QColor(232,33,47)
<< QColor(116,251,110)
<< QColor(234,38,107)
<< QColor(232,33,47)
<< QColor(116,251,110)
<< QColor(234,38,107)
<< QColor(104,250,138)
<< QColor(235,43,165)
<< QColor(98,248,176)
<< QColor(236,48,221)
<< QColor(235,43,165)
<< QColor(98,248,176)
<< QColor(236,48,221)
<< QColor(92,247,217)
<< QColor(200,54,238)
<< QColor(87,231,246)
<< QColor(151,59,239)
<< QColor(200,54,238)
<< QColor(87,231,246)
<< QColor(151,59,239)
<< QColor(81,183,244)
<< QColor(75,133,243)
<< QColor(242,255,128)
<< QColor(75,133,243)
<< QColor(242,255,128)
<< QColor(230,126,23);
static int nextColor = -1;
if(nextColor == 18){//if at the end of the list
nextColor = -1;//go back to the beginning
}
nextColor++;
nextColor++;
return colors[nextColor];//return the next color
}
......@@ -391,6 +391,12 @@ public slots:
virtual void sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x,
float flow_comp_m_y, quint8 quality, float ground_distance) = 0;
/** @brief Send command to map a RC channel to a parameter */
virtual void sendMapRCToParam(QString param_id, float scale, float value0, quint8 param_rc_channel_index, float valueMin, float valueMax) = 0;
/** @brief Send command to disable all bindings/maps between RC and parameters */
virtual void unsetRCToParameterMap() = 0;
protected:
QColor color;
......
......@@ -95,6 +95,11 @@ void QGCBaseParamWidget::requestOnboardParamsUpdate()
paramMgr->requestParameterList();
}
void QGCBaseParamWidget::requestOnboardParamUpdate(QString parameterName)
{
paramMgr->requestParameterUpdate(paramMgr->getDefaultComponentId(), parameterName);
}
void QGCBaseParamWidget::saveParametersToFile()
{
......
......@@ -29,7 +29,7 @@ protected:
virtual void disconnectFromParamManager(); ///< Disconnect from any connected param manager signals
signals:
public slots:
virtual void handleOnboardParamUpdate(int component,const QString& parameterName, QVariant value) = 0;
virtual void handlePendingParamUpdate(int compId, const QString& paramName, QVariant value, bool isPending) = 0;
......@@ -43,6 +43,9 @@ public slots:
/** @brief Request list of parameters from MAV */
virtual void requestOnboardParamsUpdate();
/** @brief Request single parameter from MAV */
virtual void requestOnboardParamUpdate(QString parameterName);
/** @brief Store parameters to a file */
virtual void saveParametersToFile();
......
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/// @file
/// @author Thomas Gubler <thomasgubler@gmail.com>
#include "QGCMapRCToParamDialog.h"
#include "ui_QGCMapRCToParamDialog.h"
#include <QDebug>
#include <QTimer>
#include <QEventLoop>
#include <QShowEvent>
#include <QPushButton>
QGCMapRCToParamDialog::QGCMapRCToParamDialog(QString param_id,
UASInterface *mav, QWidget *parent) :
QDialog(parent),
param_id(param_id),
mav(mav),
ui(new Ui::QGCMapRCToParamDialog)
{
ui->setupUi(this);
// only enable ok button when param was refreshed
QPushButton *okButton = ui->buttonBox->button(QDialogButtonBox::Ok);
okButton->setEnabled(false);
ui->paramIdLabel->setText(param_id);
// Refresh the param
ParamLoader *paramLoader = new ParamLoader(param_id, mav);
paramLoader->moveToThread(&paramLoadThread);
connect(&paramLoadThread, &QThread::finished, paramLoader, &QObject::deleteLater);
connect(this, &QGCMapRCToParamDialog::refreshParam, paramLoader, &ParamLoader::load);
connect(paramLoader, &ParamLoader::paramLoaded, this, &QGCMapRCToParamDialog::paramLoaded);
paramLoadThread.start();
emit refreshParam();
}
QGCMapRCToParamDialog::~QGCMapRCToParamDialog()
{
delete ui;
}
void QGCMapRCToParamDialog::accept() {
emit mapRCToParamDialogResult(param_id,
(float)ui->scaleDoubleSpinBox->value(),
(float)ui->value0DoubleSpinBox->value(),
(quint8)ui->rcParamChannelComboBox->currentIndex(),
(float)ui->minValueDoubleSpinBox->value(),
(float)ui->maxValueDoubleSpinBox->value());
QDialog::accept();
}
void QGCMapRCToParamDialog::paramLoaded(bool success, float value, QString message)
{
paramLoadThread.quit();
if (success) {
ui->infoLabel->setText("Parameter value is up to date");
ui->value0DoubleSpinBox->setValue(value);
ui->value0DoubleSpinBox->setEnabled(true);
connect(this, &QGCMapRCToParamDialog::mapRCToParamDialogResult,
mav, &UASInterface::sendMapRCToParam);
QPushButton *okButton = ui->buttonBox->button(QDialogButtonBox::Ok);
okButton->setEnabled(true);
} else {
qDebug() << "Error while reading param" << param_id;
ui->infoLabel->setText("Error while refreshing param (" + message + ")");
}
}
void ParamLoader::load()
{
// refresh the parameter from onboard to make sure the current value is used
paramMgr->requestParameterUpdate(paramMgr->getDefaultComponentId(), param_id);
// wait until parameter update is received
QEventLoop loop;
QTimer timer;
connect(&timer, &QTimer::timeout, &loop, &QEventLoop::quit);
timer.start(10 * 1e3);
// overloaded signal:
connect(mav, static_cast<void (UASInterface::*)(
int, int, QString, QVariant)>(&UASInterface::parameterChanged),
this, &ParamLoader::handleParameterChanged);
connect(this, &ParamLoader::correctParameterChanged,
&loop, &QEventLoop::quit);
loop.exec();
if (!param_received == true) {
// timeout
emit paramLoaded(false, 0.0f, "Timeout");
return;
}
QVariant current_param_value;
bool got_param = paramMgr->getParameterValue(paramMgr->getDefaultComponentId(),
param_id, current_param_value);
QString message = got_param ? "" : "param manager Error";
emit paramLoaded(got_param, current_param_value.toFloat(), message);
}
void ParamLoader::handleParameterChanged(int uas, int component, QString parameterName, QVariant value)
{
Q_UNUSED(component);
Q_UNUSED(value);
if (uas == mav->getUASID() && parameterName == param_id) {
param_received = true;
emit correctParameterChanged();
}
}
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/// @file
/// @brief Dialog to configure RC to paramter mapping
/// @author Thomas Gubler <thomasgubler@gmail.com>
#ifndef QGCMAPRCTOPARAMDIALOG_H
#define QGCMAPRCTOPARAMDIALOG_H
#include <QDialog>
#include <QThread>
#include "UASInterface.h"
namespace Ui {
class QGCMapRCToParamDialog;
}
class ParamLoader : public QObject
{
Q_OBJECT
public:
ParamLoader(QString param_id, UASInterface *mav, QObject * parent = 0):
QObject(parent),
mav(mav),
paramMgr(mav->getParamManager()),
param_id(param_id),
param_received(false)
{}
public slots:
void load();
void handleParameterChanged(int uas, int component, QString parameterName, QVariant value);
signals:
void paramLoaded(bool success, float value, QString message = "");
void correctParameterChanged();
protected:
UASInterface *mav;
QGCUASParamManagerInterface* paramMgr;
QString param_id;
bool param_received;
};
class QGCMapRCToParamDialog : public QDialog
{
Q_OBJECT
QThread paramLoadThread;
public:
explicit QGCMapRCToParamDialog(QString param_id,
UASInterface *mav, QWidget *parent = 0);
~QGCMapRCToParamDialog();
signals:
void mapRCToParamDialogResult(QString param_id, float scale, float value0,
quint8 param_rc_channel_index, float valueMin, float valueMax);
void refreshParam();
public slots:
void accept();
void paramLoaded(bool success, float value, QString message);
protected:
// void showEvent(QShowEvent * event );
QString param_id;
UASInterface *mav;
private:
Ui::QGCMapRCToParamDialog *ui;
};
#endif // QGCMAPRCTOPARAMDIALOG_H
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>QGCMapRCToParamDialog</class>
<widget class="QDialog" name="QGCMapRCToParamDialog">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>400</width>
<height>315</height>
</rect>
</property>
<property name="windowTitle">
<string>Dialog</string>
</property>
<widget class="QDialogButtonBox" name="buttonBox">
<property name="geometry">
<rect>
<x>30</x>
<y>280</y>
<width>341</width>
<height>32</height>
</rect>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="standardButtons">
<set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
</property>
</widget>
<widget class="QWidget" name="formLayoutWidget">
<property name="geometry">
<rect>
<x>9</x>
<y>9</y>
<width>381</width>
<height>271</height>
</rect>
</property>
<layout class="QFormLayout" name="formLayout">
<property name="fieldGrowthPolicy">
<enum>QFormLayout::AllNonFixedFieldsGrow</enum>
</property>
<item row="0" column="0">
<widget class="QLabel" name="label1">
<property name="text">
<string>Bind</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="rcParamChannelInfoLabel">
<property name="text">
<string>RC-Parameter Channel (Knob No.)</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QComboBox" name="rcParamChannelComboBox">
<property name="editable">
<bool>false</bool>
</property>
<property name="currentText">
<string>1</string>
</property>
<item>
<property name="text">
<string>1</string>
</property>
</item>
<item>
<property name="text">
<string>2</string>
</property>
</item>
<item>
<property name="text">
<string>3</string>
</property>
</item>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label2">
<property name="text">
<string>to</string>
</property>
</widget>
</item>