diff --git a/src/uas/QGCUASParamManager.cc b/src/uas/QGCUASParamManager.cc index 79b03d20598a899b0099a1d0b6689d54d233f61f..186bfac74b0f33b63a71a1510e4e19a8eb6f12d6 100644 --- a/src/uas/QGCUASParamManager.cc +++ b/src/uas/QGCUASParamManager.cc @@ -1,6 +1,9 @@ #include "QGCUASParamManager.h" -#include "UASInterface.h" +#include > +#include + +#include "UASInterface.h" #include "UASParameterCommsMgr.h" QGCUASParamManager::QGCUASParamManager(UASInterface* uas, QWidget *parent) : @@ -28,15 +31,15 @@ QGCUASParamManager::QGCUASParamManager(UASInterface* uas, QWidget *parent) : void parameterUpdated(int compId, int paramId, QString paramName, QVariant value); connect(paramDataModel, SIGNAL(parameterUpdated(int, int, QString , QVariant )), - this, SLOT(handleParameterUpdate(int,int,int,QString,QVariant)); + this, SLOT(handleParameterUpdate(int,int,int,QString,QVariant))); // connect(uas, SIGNAL(parameterChanged(int,int,int,int,QString,QVariant)), // this, SLOT(receivedParameterUpdate(int,int,int,int,QString,QVariant))); // Listen for param list reload finished - connect(paramCommsMgr, SIGNAL(parameterListUpToDate(int)), - this, SLOT(handleParameterListUpToDate(int)))); + connect(paramCommsMgr, SIGNAL(parameterListUpToDate()), + this, SLOT(handleParameterListUpToDate())); @@ -206,6 +209,10 @@ void QGCUASParamManager::setParamDescriptions(const QMap& param } +void QGCUASParamManager::setParameter(int component, QString parameterName, QVariant value) +{ + paramCommsMgr->setParameter(component,parameterName,value); +} void QGCUASParamManager::loadParamMetaInfoCSV() diff --git a/src/uas/QGCUASParamManager.h b/src/uas/QGCUASParamManager.h index 313c0927adc1ada1f9e44b618ae45d61917f1115..3c1fb7e78cd868dfcfffa65a7fa2cef90f484fe0 100644 --- a/src/uas/QGCUASParamManager.h +++ b/src/uas/QGCUASParamManager.h @@ -43,16 +43,17 @@ signals: public slots: - /** @brief Write one parameter to the MAV */ - virtual void setParameter(int component, QString parameterName, QVariant value) = 0; + /** @brief Send one parameter to the MAV: changes value in transient memory of MAV */ + virtual void setParameter(int component, QString parameterName, QVariant value); + /** @brief Request list of parameters from MAV */ virtual void requestParameterList(); - /** @brief Request a single parameter by name */ + /** @brief Request a single parameter by name from the MAV */ virtual void requestParameterUpdate(int component, const QString& parameter); virtual void handleParameterUpdate(int component, int paramCount, int paramId, const QString& parameterName, QVariant value) = 0; - virtual void handleParameterListUpToDate(int component) = 0; + virtual void handleParameterListUpToDate() = 0; protected: diff --git a/src/uas/UASParameterCommsMgr.cc b/src/uas/UASParameterCommsMgr.cc index 22940e5fd3ec270755e5f494f26d4c08a7df60b6..594e2447ec69ba805b5eadca58dbed0c95c01429 100644 --- a/src/uas/UASParameterCommsMgr.cc +++ b/src/uas/UASParameterCommsMgr.cc @@ -1,5 +1,7 @@ #include "UASParameterCommsMgr.h" +#include + #include "QGCUASParamManager.h" #include "UASInterface.h" @@ -38,14 +40,14 @@ void UASParameterCommsMgr::loadParamCommsSettings() { QSettings settings; settings.beginGroup("QGC_MAVLINK_PROTOCOL"); - bool valid; + bool ok; int val = settings.value("PARAMETER_RETRANSMISSION_TIMEOUT", retransmissionTimeout).toInt(&ok); - if (valid) { - retransmissionTimeout = temp; + if (ok) { + retransmissionTimeout = val; } val = settings.value("PARAMETER_REWRITE_TIMEOUT", rewriteTimeout).toInt(&ok); - if (valid) { - rewriteTimeout = temp; + if (ok) { + rewriteTimeout = val; } settings.endGroup(); } @@ -224,17 +226,23 @@ void UASParameterCommsMgr::setParameter(int component, QString parameterName, QV double dblValue = value.toDouble(); if (paramDataModel->isValueLessThanParamMin(parameterName,dblValue)) { - setParameterStatusMsg(tr("REJ. %1, %2 < min").arg(parameterName).arg(dblValue)); + setParameterStatusMsg(tr("REJ. %1, %2 < min").arg(parameterName).arg(dblValue), + ParamCommsStatusLevel_Error + ); return; } if (paramDataModel->isValueGreaterThanParamMax(parameterName,dblValue)) { - setParameterStatusMsg(tr("REJ. %1, %2 > max").arg(parameterName).arg(dblValue)); + setParameterStatusMsg(tr("REJ. %1, %2 > max").arg(parameterName).arg(dblValue), + ParamCommsStatusLevel_Error + ); return; } QVariant onboardVal; paramDataModel->getOnboardParameterValue(component,parameterName,onboardVal); if (onboardVal == value) { - setParameterStatusMsg(tr("REJ. %1 already %2").arg(parameterName).arg(dblValue)); + setParameterStatusMsg(tr("REJ. %1 already %2").arg(parameterName).arg(dblValue), + ParamCommsStatusLevel_Warning + ); return; } @@ -246,28 +254,24 @@ void UASParameterCommsMgr::setParameter(int component, QString parameterName, QV { QVariant fixedValue(QChar((unsigned char)value.toInt())); emit parameterChanged(component, parameterName, fixedValue); - //qDebug() << "PARAM WIDGET SENT:" << fixedValue; } break; case QVariant::Int: { QVariant fixedValue(value.toInt()); emit parameterChanged(component, parameterName, fixedValue); - //qDebug() << "PARAM WIDGET SENT:" << fixedValue; } break; case QVariant::UInt: { QVariant fixedValue(value.toUInt()); emit parameterChanged(component, parameterName, fixedValue); - //qDebug() << "PARAM WIDGET SENT:" << fixedValue; } break; case QMetaType::Float: { QVariant fixedValue(value.toFloat()); emit parameterChanged(component, parameterName, fixedValue); - //qDebug() << "PARAM WIDGET SENT:" << fixedValue; } break; default: @@ -309,6 +313,8 @@ void UASParameterCommsMgr::setParameterStatusMsg(const QString& msg, ParamCommsS qDebug() << "parameterStatusMsg: " << msg; parameterStatusMsg = msg; + emit parameterStatusMsgUpdated(msg,level); + //TODO indicate OK status somehow (eg color) // QPalette pal = statusLabel->palette(); // pal.setColor(backgroundRole(), QGC::colorGreen); @@ -360,7 +366,7 @@ void UASParameterCommsMgr::receivedParameterUpdate(int uas, int compId, int para // Start retransmission guard // or reset timer - paramCommsMgr->setRetransmissionGuardEnabled(true); //TODO + setRetransmissionGuardEnabled(true); //TODO } // Mark this parameter as received in read list diff --git a/src/uas/UASParameterCommsMgr.h b/src/uas/UASParameterCommsMgr.h index ff366c38e1c8bfabfced09d4f70fa29c7b826107..68c908bade48ce361cee10055871064e2c4163c9 100644 --- a/src/uas/UASParameterCommsMgr.h +++ b/src/uas/UASParameterCommsMgr.h @@ -16,16 +16,16 @@ class UASParameterCommsMgr : public QObject { Q_OBJECT -typedef enum ParamCommsStatusLevel { - ParamCommsStatusLevel_OK = 0, - ParamCommsStatusLevel_Warning = 2, - ParamCommsStatusLevel_Error = 4, - ParamCommsStatusLevel_Count -} ParamCommsStatusLevel_t; public: explicit UASParameterCommsMgr(QObject *parent = 0, UASInterface* uas = NULL); - + typedef enum ParamCommsStatusLevel { + ParamCommsStatusLevel_OK = 0, + ParamCommsStatusLevel_Warning = 2, + ParamCommsStatusLevel_Error = 4, + ParamCommsStatusLevel_Count + } ParamCommsStatusLevel_t; + protected: /** @brief Activate / deactivate parameter retransmission */ @@ -41,7 +41,9 @@ signals: void parameterChanged(int component, int parameterIndex, QVariant value); void parameterValueConfirmed(int uas, int component,int paramCount, int paramId, QString parameter, QVariant value); - void parameterListUpToDate(int component); + /** @brief We have received a complete list of all parameters onboard the MAV */ + void parameterListUpToDate(); + void parameterUpdateRequested(int component, const QString& parameter); void parameterUpdateRequestedById(int componentId, int paramId); @@ -57,6 +59,7 @@ public slots: /** @brief Write one parameter to the MAV */ virtual void setParameter(int component, QString parameterName, QVariant value); + /** @brief Request list of parameters from MAV */ virtual void requestParameterList(); /** @brief Check for missing parameters */ @@ -67,9 +70,9 @@ public slots: virtual void receivedParameterUpdate(int uas, int compId, int paramCount, int paramId, QString paramName, QVariant value); -protected slots: - void receivedParameterChange(int uas, int component, QString parameterName, QVariant value); - void receivedParameterListChange(int uas, int component, int parameterCount, int parameterId, QString parameterName, QVariant value); +//protected slots: +// void receivedParameterChange(int uas, int component, QString parameterName, QVariant value); +// void receivedParameterListChange(int uas, int component, int parameterCount, int parameterId, QString parameterName, QVariant value); protected: diff --git a/src/ui/QGCParamWidget.cc b/src/ui/QGCParamWidget.cc index 97e0d1eb31a05e40bd9cfb954de9a75e9122ecd8..09ee9c970a7741685de6f9bcee1ac13e59054550 100644 --- a/src/ui/QGCParamWidget.cc +++ b/src/ui/QGCParamWidget.cc @@ -27,16 +27,17 @@ This file is part of the QGROUNDCONTROL project */ #include #include -#include -#include -#include +#include +#include #include +#include +#include + #include -#include -#include #include -#include -#include +#include +#include +#include #include "MainWindow.h" #include "QGC.h" @@ -338,7 +339,7 @@ void QGCParamWidget::handleParameterUpdate(int componentId, int paramCount, int } -void QGCParamWidget::handleParameterListUpToDate(int component) +void QGCParamWidget::handleParameterListUpToDate() { // Expand visual tree tree->expandItem(tree->topLevelItem(0)); @@ -593,94 +594,95 @@ void QGCParamWidget::writeParameters() * @param parameterName name of the parameter, as delivered by the system * @param value value of the parameter */ -void QGCParamWidget::setParameter(int component, QString parameterName, QVariant value) -{ - if (parameterName.isEmpty()) { - return; - } +//void QGCParamWidget::setParameter(int component, QString parameterName, QVariant value) +//{ - double dblValue = value.toDouble(); +// if (parameterName.isEmpty()) { +// return; +// } - if (paramDataModel->isValueLessThanParamMin(parameterName,dblValue)) { - statusLabel->setText(tr("REJ. %1, %2 < min").arg(parameterName).arg(dblValue)); - return; - } - if (paramDataModel->isValueGreaterThanParamMax(parameterName,dblValue)) { - statusLabel->setText(tr("REJ. %1, %2 > max").arg(parameterName).arg(dblValue)); - return; - } - QVariant onboardVal; - paramDataModel->getOnboardParameterValue(component,parameterName,onboardVal); - if (onboardVal == value) { - statusLabel->setText(tr("REJ. %1 already %2").arg(parameterName).arg(dblValue)); - return; - } +// double dblValue = value.toDouble(); - //int paramType = (int)onboardParameters.value(component)->value(parameterName).type(); - int paramType = (int)value.type(); - switch (paramType) - { - case QVariant::Char: - { - QVariant fixedValue(QChar((unsigned char)value.toInt())); - emit parameterChanged(component, parameterName, fixedValue); - //qDebug() << "PARAM WIDGET SENT:" << fixedValue; - } - break; - case QVariant::Int: - { - QVariant fixedValue(value.toInt()); - emit parameterChanged(component, parameterName, fixedValue); - //qDebug() << "PARAM WIDGET SENT:" << fixedValue; - } - break; - case QVariant::UInt: - { - QVariant fixedValue(value.toUInt()); - emit parameterChanged(component, parameterName, fixedValue); - //qDebug() << "PARAM WIDGET SENT:" << fixedValue; - } - break; - case QMetaType::Float: - { - QVariant fixedValue(value.toFloat()); - emit parameterChanged(component, parameterName, fixedValue); - //qDebug() << "PARAM WIDGET SENT:" << fixedValue; - } - break; - default: - qCritical() << "ABORTED PARAM SEND, NO VALID QVARIANT TYPE"; - return; - } +// if (paramDataModel->isValueLessThanParamMin(parameterName,dblValue)) { +// statusLabel->setText(tr("REJ. %1, %2 < min").arg(parameterName).arg(dblValue)); +// return; +// } +// if (paramDataModel->isValueGreaterThanParamMax(parameterName,dblValue)) { +// statusLabel->setText(tr("REJ. %1, %2 > max").arg(parameterName).arg(dblValue)); +// return; +// } +// QVariant onboardVal; +// paramDataModel->getOnboardParameterValue(component,parameterName,onboardVal); +// if (onboardVal == value) { +// statusLabel->setText(tr("REJ. %1 already %2").arg(parameterName).arg(dblValue)); +// return; +// } - // Wait for parameter to be written back - // mark it therefore as missing - if (!transmissionMissingWriteAckPackets.contains(component)) - { - transmissionMissingWriteAckPackets.insert(component, new QMap()); - } +// //int paramType = (int)onboardParameters.value(component)->value(parameterName).type(); +// int paramType = (int)value.type(); +// switch (paramType) +// { +// case QVariant::Char: +// { +// QVariant fixedValue(QChar((unsigned char)value.toInt())); +// emit parameterChanged(component, parameterName, fixedValue); +// //qDebug() << "PARAM WIDGET SENT:" << fixedValue; +// } +// break; +// case QVariant::Int: +// { +// QVariant fixedValue(value.toInt()); +// emit parameterChanged(component, parameterName, fixedValue); +// //qDebug() << "PARAM WIDGET SENT:" << fixedValue; +// } +// break; +// case QVariant::UInt: +// { +// QVariant fixedValue(value.toUInt()); +// emit parameterChanged(component, parameterName, fixedValue); +// //qDebug() << "PARAM WIDGET SENT:" << fixedValue; +// } +// break; +// case QMetaType::Float: +// { +// QVariant fixedValue(value.toFloat()); +// emit parameterChanged(component, parameterName, fixedValue); +// //qDebug() << "PARAM WIDGET SENT:" << fixedValue; +// } +// break; +// default: +// qCritical() << "ABORTED PARAM SEND, NO VALID QVARIANT TYPE"; +// return; +// } - // Insert it in missing write ACK list - transmissionMissingWriteAckPackets.value(component)->insert(parameterName, value); +// // Wait for parameter to be written back +// // mark it therefore as missing +// if (!transmissionMissingWriteAckPackets.contains(component)) +// { +// transmissionMissingWriteAckPackets.insert(component, new QMap()); +// } - // Set timeouts - if (transmissionActive) - { - transmissionTimeout += rewriteTimeout; - } - else - { - quint64 newTransmissionTimeout = QGC::groundTimeMilliseconds() + rewriteTimeout; - if (newTransmissionTimeout > transmissionTimeout) - { - transmissionTimeout = newTransmissionTimeout; - } - transmissionActive = true; - } +// // Insert it in missing write ACK list +// transmissionMissingWriteAckPackets.value(component)->insert(parameterName, value); - // Enable guard / reset timeouts - paramCommsMgr->setRetransmissionGuardEnabled(true); //TODO -} +// // Set timeouts +// if (transmissionActive) +// { +// transmissionTimeout += rewriteTimeout; +// } +// else +// { +// quint64 newTransmissionTimeout = QGC::groundTimeMilliseconds() + rewriteTimeout; +// if (newTransmissionTimeout > transmissionTimeout) +// { +// transmissionTimeout = newTransmissionTimeout; +// } +// transmissionActive = true; +// } + +// // Enable guard / reset timeouts +// paramCommsMgr->setRetransmissionGuardEnabled(true); //TODO +//} void QGCParamWidget::readParameters() { @@ -696,3 +698,20 @@ void QGCParamWidget::clear() tree->clear(); componentItems->clear(); } + +void QGCParamWidget::handleParamStatusMsgUpdate(QString msg, int level) +{ + QColor bgColor = QGC::colorGreen; + if ((int)UASParameterCommsMgr::ParamCommsStatusLevel_Warning == level) { + bgColor = QGC::colorOrange; + } + else if ((int)UASParameterCommsMgr::ParamCommsStatusLevel_Error == level) { + bgColor = QGC::colorRed; + } + + QPalette pal = statusLabel->palette(); + pal.setColor(backgroundRole(), bgColor); + statusLabel->setPalette(pal); + statusLabel->setText(msg); + +} diff --git a/src/ui/QGCParamWidget.h b/src/ui/QGCParamWidget.h index 0ada126d7e15f94ce299e0c6b8761a5d183387f3..3d2f7779eb1579bfa69f183f9c1443106b0cb385 100644 --- a/src/ui/QGCParamWidget.h +++ b/src/ui/QGCParamWidget.h @@ -68,14 +68,16 @@ public slots: // void receivedParameterUpdate(int uas, int component, int paramCount, int paramId, QString parameterName, QVariant value); virtual void handleParameterUpdate(int component, int paramCount, int paramId, const QString& parameterName, QVariant value); - virtual void handleParameterListUpToDate(int component); + virtual void handleParameterListUpToDate(); + + virtual void handleParamStatusMsgUpdate(QString msg, int level); /** @brief Ensure that view of parameter matches data in the model */ void updateParameterDisplay(int component, QString parameterName, QVariant value); /** @brief Request list of parameters from MAV */ void requestAllParamsUpdate(); /** @brief Set one parameter, changes value in RAM of MAV */ - virtual void setParameter(int component, QString parameterName, QVariant value); +// virtual void setParameter(int component, QString parameterName, QVariant value); /** @brief Set all parameters, changes the value in RAM of MAV */ void setParameters(); /** @brief Write the current parameters to permanent storage (EEPROM/HDD) */