From 74f1acf5aada45f2a910eb8f0fa70f4bc49da771 Mon Sep 17 00:00:00 2001 From: tstellanova Date: Wed, 7 Aug 2013 17:40:32 -0700 Subject: [PATCH] wip-- more mvc refactoring --- src/uas/QGCUASParamManager.cc | 337 ++++++++++++--------- src/uas/QGCUASParamManager.h | 38 ++- src/uas/UAS.cc | 4 + src/uas/UAS.h | 6 + src/uas/UASInterface.h | 3 + src/uas/UASParameterCommsMgr.cc | 217 ++++++++++++- src/uas/UASParameterCommsMgr.h | 31 +- src/uas/UASParameterDataModel.cc | 10 +- src/uas/UASParameterDataModel.h | 11 +- src/ui/QGCParamWidget.cc | 505 ++++++++++++------------------- src/ui/QGCParamWidget.h | 33 +- 11 files changed, 686 insertions(+), 509 deletions(-) diff --git a/src/uas/QGCUASParamManager.cc b/src/uas/QGCUASParamManager.cc index de1d19afd..79b03d205 100644 --- a/src/uas/QGCUASParamManager.cc +++ b/src/uas/QGCUASParamManager.cc @@ -1,22 +1,43 @@ #include "QGCUASParamManager.h" #include "UASInterface.h" +#include "UASParameterCommsMgr.h" + QGCUASParamManager::QGCUASParamManager(UASInterface* uas, QWidget *parent) : QWidget(parent), - mav(uas), - transmissionListMode(false), - transmissionActive(false), - transmissionTimeout(0), - retransmissionTimeout(350), - rewriteTimeout(500), - retransmissionBurstRequestSize(5) + mav(uas) { paramDataModel = uas->getParamDataModel(); - uas->setParamManager(this); + paramCommsMgr = uas->getParamCommsMgr(); + mav->setParamManager(this); + + // Load default values and tooltips + loadParamMetaInfoCSV(); + // Connect retransmission guard - connect(this, SIGNAL(parameterUpdateRequested(int,QString)), this, SLOT(requestParameterUpdate(int,QString))); - connect(this, SIGNAL(parameterUpdateRequestedById(int,int)), mav, SLOT(requestParameter(int,int))); + connect(this, SIGNAL(parameterUpdateRequested(int,QString)), + this, SLOT(requestParameterUpdate(int,QString))); + +// //TODO connect in paramCommsMgr instead +// connect(this, SIGNAL(parameterUpdateRequestedById(int,int)), +// mav, SLOT(requestParameter(int,int))); + + // New parameters from UAS + + 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)); + +// 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)))); + } @@ -31,14 +52,12 @@ bool QGCUASParamManager::getParameterValue(int component, const QString& paramet void QGCUASParamManager::requestParameterUpdate(int component, const QString& parameter) { if (mav) { - mav->requestParameter(component, parameter); + paramCommsMgr->requestParameterUpdate(component,parameter); } } - - /** * Send a request to deliver the list of onboard parameters * to the MAV. @@ -48,146 +67,133 @@ void QGCUASParamManager::requestParameterList() if (!mav) { return; } - - paramDataModel->forgetAllOnboardParameters(); - - // Clear transmission state - receivedParamsList.clear(); - transmissionListSizeKnown.clear(); - - transmissionListMode = true; - foreach (int key, transmissionMissingPackets.keys()) { - transmissionMissingPackets.value(key)->clear(); - } - transmissionActive = true; - + //paramDataModel->forgetAllOnboardParameters(); //TODO really?? setParameterStatusMsg(tr("Requested param list.. waiting")); - mav->requestParameters(); - -} - -void QGCUASParamManager::retransmissionGuardTick() -{ - if (transmissionActive) { - //qDebug() << __FILE__ << __LINE__ << "RETRANSMISSION GUARD ACTIVE, CHECKING FOR DROPS.."; - - // Check for timeout - // stop retransmission attempts on timeout - if (QGC::groundTimeMilliseconds() > transmissionTimeout) { - setRetransmissionGuardEnabled(false); - transmissionActive = false; - - // Empty read retransmission list - // Empty write retransmission list - int missingReadCount = 0; - QList readKeys = transmissionMissingPackets.keys(); - foreach (int component, readKeys) { - missingReadCount += transmissionMissingPackets.value(component)->count(); - transmissionMissingPackets.value(component)->clear(); - } - - // Empty write retransmission list - int missingWriteCount = 0; - QList writeKeys = transmissionMissingWriteAckPackets.keys(); - foreach (int component, writeKeys) { - missingWriteCount += transmissionMissingWriteAckPackets.value(component)->count(); - transmissionMissingWriteAckPackets.value(component)->clear(); - } - setParameterStatusMsg(tr("TIMEOUT! MISSING: %1 read, %2 write.").arg(missingReadCount).arg(missingWriteCount)); - } - - // Re-request at maximum retransmissionBurstRequestSize parameters at once - // to prevent link flooding - QMap*>::iterator i; - QMap*> onboardParams = paramDataModel->getOnboardParameters(); - for (i = onboardParams.begin(); i != onboardParams.end(); ++i) { - // Iterate through the parameters of the component - int component = i.key(); - // Request n parameters from this component (at maximum) - QList * paramList = transmissionMissingPackets.value(component, NULL); - if (paramList) { - int count = 0; - foreach (int id, *paramList) { - if (count < retransmissionBurstRequestSize) { - //qDebug() << __FILE__ << __LINE__ << "RETRANSMISSION GUARD REQUESTS RETRANSMISSION OF PARAM #" << id << "FROM COMPONENT #" << component; - //TODO mavlink msg type for "request parameter set" ? - emit parameterUpdateRequestedById(component, id); - setParameterStatusMsg(tr("Requested retransmission of #%1").arg(id+1)); - count++; - } else { - break; - } - } - } - } - - // Re-request at maximum retransmissionBurstRequestSize parameters at once - // to prevent write-request link flooding - // Empty write retransmission list - QList writeKeys = transmissionMissingWriteAckPackets.keys(); - foreach (int component, writeKeys) { - int count = 0; - QMap * missingParams = transmissionMissingWriteAckPackets.value(component); - foreach (QString key, missingParams->keys()) { - if (count < retransmissionBurstRequestSize) { - // Re-request write operation - QVariant value = missingParams->value(key); - switch ((int)onboardParams.value(component)->value(key).type()) - { - case QVariant::Int: - { - QVariant fixedValue(value.toInt()); - emit parameterChanged(component, key, fixedValue); - } - break; - case QVariant::UInt: - { - QVariant fixedValue(value.toUInt()); - emit parameterChanged(component, key, fixedValue); - } - break; - case QMetaType::Float: - { - QVariant fixedValue(value.toFloat()); - emit parameterChanged(component, key, fixedValue); - } - break; - default: - //qCritical() << "ABORTED PARAM RETRANSMISSION, NO VALID QVARIANT TYPE"; - return; - } - setParameterStatusMsg(tr("Requested rewrite of: %1: %2").arg(key).arg(missingParams->value(key).toDouble())); - count++; - } else { - break; - } - } - } - } else { - //qDebug() << __FILE__ << __LINE__ << "STOPPING RETRANSMISSION GUARD GRACEFULLY"; - setRetransmissionGuardEnabled(false); - } + paramCommsMgr->requestParameterList(); } - - -/** - * Enabling the retransmission guard enables the parameter widget to track - * dropped parameters and to re-request them. This works for both individual - * parameter reads as well for whole list requests. - * - * @param enabled True if retransmission checking should be enabled, false else - */ -void QGCUASParamManager::setRetransmissionGuardEnabled(bool enabled) -{ +//void QGCUASParamManager::retransmissionGuardTick() +//{ +// if (transmissionActive) { +// //qDebug() << __FILE__ << __LINE__ << "RETRANSMISSION GUARD ACTIVE, CHECKING FOR DROPS.."; + +// // Check for timeout +// // stop retransmission attempts on timeout +// if (QGC::groundTimeMilliseconds() > transmissionTimeout) { +// setRetransmissionGuardEnabled(false); +// transmissionActive = false; + +// // Empty read retransmission list +// // Empty write retransmission list +// int missingReadCount = 0; +// QList readKeys = transmissionMissingPackets.keys(); +// foreach (int component, readKeys) { +// missingReadCount += transmissionMissingPackets.value(component)->count(); +// transmissionMissingPackets.value(component)->clear(); +// } + +// // Empty write retransmission list +// int missingWriteCount = 0; +// QList writeKeys = transmissionMissingWriteAckPackets.keys(); +// foreach (int component, writeKeys) { +// missingWriteCount += transmissionMissingWriteAckPackets.value(component)->count(); +// transmissionMissingWriteAckPackets.value(component)->clear(); +// } +// setParameterStatusMsg(tr("TIMEOUT! MISSING: %1 read, %2 write.").arg(missingReadCount).arg(missingWriteCount)); +// } + +// // Re-request at maximum retransmissionBurstRequestSize parameters at once +// // to prevent link flooding +// QMap*>::iterator i; +// QMap*> onboardParams = paramDataModel->getOnboardParameters(); +// for (i = onboardParams.begin(); i != onboardParams.end(); ++i) { +// // Iterate through the parameters of the component +// int component = i.key(); +// // Request n parameters from this component (at maximum) +// QList * paramList = transmissionMissingPackets.value(component, NULL); +// if (paramList) { +// int count = 0; +// foreach (int id, *paramList) { +// if (count < retransmissionBurstRequestSize) { +// //qDebug() << __FILE__ << __LINE__ << "RETRANSMISSION GUARD REQUESTS RETRANSMISSION OF PARAM #" << id << "FROM COMPONENT #" << component; +// //TODO mavlink msg type for "request parameter set" ? +// emit parameterUpdateRequestedById(component, id); +// setParameterStatusMsg(tr("Requested retransmission of #%1").arg(id+1)); +// count++; +// } else { +// break; +// } +// } +// } +// } + +// // Re-request at maximum retransmissionBurstRequestSize parameters at once +// // to prevent write-request link flooding +// // Empty write retransmission list +// QList writeKeys = transmissionMissingWriteAckPackets.keys(); +// foreach (int component, writeKeys) { +// int count = 0; +// QMap * missingParams = transmissionMissingWriteAckPackets.value(component); +// foreach (QString key, missingParams->keys()) { +// if (count < retransmissionBurstRequestSize) { +// // Re-request write operation +// QVariant value = missingParams->value(key); +// switch ((int)onboardParams.value(component)->value(key).type()) +// { +// case QVariant::Int: +// { +// QVariant fixedValue(value.toInt()); +// emit parameterChanged(component, key, fixedValue); +// } +// break; +// case QVariant::UInt: +// { +// QVariant fixedValue(value.toUInt()); +// emit parameterChanged(component, key, fixedValue); +// } +// break; +// case QMetaType::Float: +// { +// QVariant fixedValue(value.toFloat()); +// emit parameterChanged(component, key, fixedValue); +// } +// break; +// default: +// //qCritical() << "ABORTED PARAM RETRANSMISSION, NO VALID QVARIANT TYPE"; +// return; +// } +// setParameterStatusMsg(tr("Requested rewrite of: %1: %2").arg(key).arg(missingParams->value(key).toDouble())); +// count++; +// } else { +// break; +// } +// } +// } +// } else { +// //qDebug() << __FILE__ << __LINE__ << "STOPPING RETRANSMISSION GUARD GRACEFULLY"; +// setRetransmissionGuardEnabled(false); +// } +//} + + + +//* +// * Enabling the retransmission guard enables the parameter widget to track +// * dropped parameters and to re-request them. This works for both individual +// * parameter reads as well for whole list requests. +// * +// * @param enabled True if retransmission checking should be enabled, false else + +//void QGCUASParamManager::setRetransmissionGuardEnabled(bool enabled) +//{ // qDebug() << "setRetransmissionGuardEnabled: " << enabled; - - if (enabled) { - retransmissionTimer.start(retransmissionTimeout); - } else { - retransmissionTimer.stop(); - } -} +// +// if (enabled) { +// retransmissionTimer.start(retransmissionTimeout); +// } else { +// retransmissionTimer.stop(); +// } +//} void QGCUASParamManager::setParameterStatusMsg(const QString& msg) { @@ -200,3 +206,36 @@ void QGCUASParamManager::setParamDescriptions(const QMap& param } + + +void QGCUASParamManager::loadParamMetaInfoCSV() +{ + // Load default values and tooltips + QString autopilot(mav->getAutopilotTypeName()); + + QDir appDir = QApplication::applicationDirPath(); + appDir.cd("files"); + QString fileName = QString("%1/%2/parameter_tooltips/tooltips.txt").arg(appDir.canonicalPath()).arg(autopilot.toLower()); + QFile paramMetaFile(fileName); + + qDebug() << "loadParamMetaInfoCSV for autopilot: " << autopilot << " from file: " << fileName; + + if (!paramMetaFile.open(QIODevice::ReadOnly | QIODevice::Text)) { + qWarning() << "loadParamMetaInfoCSV couldn't open:" << fileName; + return; + } + + QTextStream in(¶mMetaFile); + paramDataModel->loadParamMetaInfoFromStream(in); + paramMetaFile.close(); + +} + +/** + * @return The MAV of this mgr. Unless the MAV object has been destroyed, this + * pointer is never zero. + */ +UASInterface* QGCUASParamManager::getUAS() +{ + return mav; +} diff --git a/src/uas/QGCUASParamManager.h b/src/uas/QGCUASParamManager.h index 6a87ba763..313c0927a 100644 --- a/src/uas/QGCUASParamManager.h +++ b/src/uas/QGCUASParamManager.h @@ -6,7 +6,9 @@ #include #include +//forward declarations class UASInterface; +class UASParameterCommsMgr; class UASParameterDataModel; class QGCUASParamManager : public QWidget @@ -15,25 +17,29 @@ class QGCUASParamManager : public QWidget public: QGCUASParamManager(UASInterface* uas, QWidget *parent = 0); + /** @brief Get the known, confirmed value of a parameter */ virtual bool getParameterValue(int component, const QString& parameter, QVariant& value) const; /** @brief Provide tooltips / user-visible descriptions for parameters */ virtual void setParamDescriptions(const QMap& paramDescs); -protected: - /** @brief Activate / deactivate parameter retransmission */ - virtual void setRetransmissionGuardEnabled(bool enabled); + /** @brief Get the UAS of this widget + * @return The MAV of this mgr. Unless the MAV object has been destroyed, this is never null. + */ + UASInterface* getUAS(); +protected: //TODO decouple this UI message display further virtual void setParameterStatusMsg(const QString& msg); + /** @brief Load parameter meta information from appropriate CSV file */ + virtual void loadParamMetaInfoCSV(); signals: void parameterChanged(int component, QString parameter, QVariant value); void parameterChanged(int component, int parameterIndex, QVariant value); - void parameterListUpToDate(int component); - void parameterUpdateRequested(int component, const QString& parameter); - void parameterUpdateRequestedById(int componentId, int paramId); +// void parameterUpdateRequested(int component, const QString& parameter); +// void parameterUpdateRequestedById(int componentId, int paramId); public slots: @@ -41,30 +47,20 @@ public slots: virtual void setParameter(int component, QString parameterName, QVariant value) = 0; /** @brief Request list of parameters from MAV */ virtual void requestParameterList(); - /** @brief Check for missing parameters */ - virtual void retransmissionGuardTick(); /** @brief Request a single parameter by name */ 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; + + protected: // Parameter data model UASInterface* mav; ///< The MAV this widget is controlling UASParameterDataModel* paramDataModel;///< Shared data model of parameters - - // Communications management - QVector receivedParamsList; ///< Successfully received parameters - QMap* > transmissionMissingPackets; ///< Missing packets - QMap* > transmissionMissingWriteAckPackets; ///< Missing write ACK packets - bool transmissionListMode; ///< Currently requesting list - QMap transmissionListSizeKnown; ///< List size initialized? - bool transmissionActive; ///< Missing packets, working on list? - quint64 transmissionTimeout; ///< Timeout - QTimer retransmissionTimer; ///< Timer handling parameter retransmission - int retransmissionTimeout; ///< Retransmission request timeout, in milliseconds - int rewriteTimeout; ///< Write request timeout, in milliseconds - int retransmissionBurstRequestSize; ///< Number of packets requested for retransmission per burst + UASParameterCommsMgr* paramCommsMgr; ///< Shared comms mgr for parameters // Status QString parameterStatusMsg; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 3e34dd48a..772341413 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -26,6 +26,7 @@ #include "QGCMAVLink.h" #include "LinkManager.h" #include "SerialLink.h" +#include "UASParameterCommsMgr.h" #include #ifdef QGC_PROTOBUF_ENABLED @@ -132,6 +133,7 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), paramsOnceRequested(false), paramManager(NULL), paramDataModel(NULL), + paramCommsMgr(NULL), simulation(0), @@ -155,6 +157,8 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), paramDataModel = new UASParameterDataModel(this); paramDataModel->setUASID(this->getUASID()); + paramCommsMgr = new UASParameterCommsMgr(this,this); + // Store a list of available actions for this UAS. // Basically everything exposted as a SLOT with no return value or arguments. diff --git a/src/uas/UAS.h b/src/uas/UAS.h index c8b9a604c..5acd2c9ef 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -41,6 +41,7 @@ This file is part of the QGROUNDCONTROL project #include "QGCJSBSimLink.h" #include "QGCXPlaneLink.h" + /** * @brief A generic MAVLINK-connected MAV/UAV * @@ -493,6 +494,7 @@ protected: //COMMENTS FOR TEST UNIT bool paramsOnceRequested; ///< If the parameter list has been read at least once QGCUASParamManager* paramManager; ///< Parameter manager class UASParameterDataModel* paramDataModel; ///< The parameter data model for this UAS + UASParameterCommsMgr* paramCommsMgr; /// SIMULATION QGCHilLink* simulation; ///< Hardware in the loop simulation link @@ -526,6 +528,10 @@ public: return paramDataModel; } + UASParameterCommsMgr* getParamCommsMgr() { + return paramCommsMgr; + } + /** @brief Get the HIL simulation */ QGCHilLink* getHILSimulation() const { diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index ed2aed28c..6e6fe2fb3 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -157,6 +157,9 @@ public: /** @brief Access the parameter data model for this UAS (sans widget). This is the same parameter data model used by the parameter manager. **/ virtual UASParameterDataModel* getParamDataModel() = 0; + + virtual UASParameterCommsMgr* getParamCommsMgr() = 0; + /** @brief Get reference to the param manager **/ virtual QGCUASParamManager* getParamManager() const = 0; diff --git a/src/uas/UASParameterCommsMgr.cc b/src/uas/UASParameterCommsMgr.cc index 9f0d60e05..22940e5fd 100644 --- a/src/uas/UASParameterCommsMgr.cc +++ b/src/uas/UASParameterCommsMgr.cc @@ -4,9 +4,22 @@ #include "UASInterface.h" UASParameterCommsMgr::UASParameterCommsMgr(QObject *parent, UASInterface *uas) : - QObject(parent) + QObject(parent), + mav(uas), + transmissionListMode(false), + transmissionActive(false), + transmissionTimeout(0), + retransmissionTimeout(350), + rewriteTimeout(500), + retransmissionBurstRequestSize(5) { mav = uas; + loadParamCommsSettings(); + + + //Requesting parameters one-by-one from mav + connect(this, SIGNAL(parameterUpdateRequestedById(int,int)), + mav, SLOT(requestParameter(int,int))); // Sending params to the UAS connect(this, SIGNAL(parameterChanged(int,QString,QVariant)), mav, SLOT(setParameter(int,QString,QVariant))); @@ -14,9 +27,29 @@ UASParameterCommsMgr::UASParameterCommsMgr(QObject *parent, UASInterface *uas) : // New parameters from UAS connect(mav, SIGNAL(parameterChanged(int,int,int,int,QString,QVariant)), this, SLOT(receivedParameterUpdate(int,int,int,int,QString,QVariant))); + //connecto retransmissionTimer + connect(&retransmissionTimer, SIGNAL(timeout()), this, SLOT(retransmissionGuardTick())); + } + +void UASParameterCommsMgr::loadParamCommsSettings() +{ + QSettings settings; + settings.beginGroup("QGC_MAVLINK_PROTOCOL"); + bool valid; + int val = settings.value("PARAMETER_RETRANSMISSION_TIMEOUT", retransmissionTimeout).toInt(&ok); + if (valid) { + retransmissionTimeout = temp; + } + val = settings.value("PARAMETER_REWRITE_TIMEOUT", rewriteTimeout).toInt(&ok); + if (valid) { + rewriteTimeout = temp; + } + settings.endGroup(); +} + /** * Send a request to deliver the list of onboard parameters * from the MAV. @@ -271,8 +304,188 @@ void UASParameterCommsMgr::setParameter(int component, QString parameterName, QV setRetransmissionGuardEnabled(true); } -void UASParameterCommsMgr::setParameterStatusMsg(const QString& msg) +void UASParameterCommsMgr::setParameterStatusMsg(const QString& msg, ParamCommsStatusLevel_t level) { qDebug() << "parameterStatusMsg: " << msg; parameterStatusMsg = msg; + + //TODO indicate OK status somehow (eg color) +// QPalette pal = statusLabel->palette(); +// pal.setColor(backgroundRole(), QGC::colorGreen); +// statusLabel->setPalette(pal); + +// pal.setColor(backgroundRole(), QGC::colorRed); + // pal.setColor(backgroundRole(), QGC::colorOrange); + + } + + +/** + * @param uas System which has the component + * @param component id of the component + * @param parameterName human friendly name of the parameter + */ +void UASParameterCommsMgr::receivedParameterUpdate(int uas, int compId, int paramCount, int paramId, QString paramName, QVariant value) +{ + + // Missing packets list has to be instantiated for all components + if (!transmissionMissingPackets.contains(compId)) { + transmissionMissingPackets.insert(compId, new QList()); + } + + // List mode is different from single parameter transfers + if (transmissionListMode) { + // Only accept the list size once on the first packet from + // each component + if (!transmissionListSizeKnown.contains(compId)) { + // Mark list size as known + transmissionListSizeKnown.insert(compId, true); + + // Mark all parameters as missing + for (int i = 0; i < paramCount; ++i) { + if (!transmissionMissingPackets.value(compId)->contains(i)) { + transmissionMissingPackets.value(compId)->append(i); + } + } + + // There is only one transmission timeout for all components + // since components do not manage their transmission, + // the longest timeout is safe for all components. + quint64 thisTransmissionTimeout = QGC::groundTimeMilliseconds() + ((paramCount)*retransmissionTimeout); + if (thisTransmissionTimeout > transmissionTimeout) { + transmissionTimeout = thisTransmissionTimeout; + } + } + + // Start retransmission guard + // or reset timer + paramCommsMgr->setRetransmissionGuardEnabled(true); //TODO + } + + // Mark this parameter as received in read list + int index = transmissionMissingPackets.value(compId)->indexOf(paramId); + // If the MAV sent the parameter without request, it wont be in missing list + if (index != -1) { + transmissionMissingPackets.value(compId)->removeAt(index); + } + + bool justWritten = false; + bool writeMismatch = false; + //bool lastWritten = false; + // Mark this parameter as received in write ACK list + QMap* map = transmissionMissingWriteAckPackets.value(compId); + if (map && map->contains(paramName)) { + justWritten = true; + QVariant newval = map->value(paramName); + if (map->value(paramName) != value) { + writeMismatch = true; + } + map->remove(paramName); + } + + int missCount = 0; + foreach (int key, transmissionMissingPackets.keys()) { + missCount += transmissionMissingPackets.value(key)->count(); + } + + int missWriteCount = 0; + foreach (int key, transmissionMissingWriteAckPackets.keys()) { + missWriteCount += transmissionMissingWriteAckPackets.value(key)->count(); + } + + //TODO simplify this if-else tree + if (justWritten && !writeMismatch && missWriteCount == 0) { + // Just wrote one and count went to 0 - this was the last missing write parameter + setParameterStatusMsg(tr("SUCCESS: WROTE ALL PARAMETERS")); + } + else if (justWritten && !writeMismatch) { + setParameterStatusMsg(tr("SUCCESS: Wrote %2 (#%1/%4): %3").arg(paramId+1).arg(paramName).arg(value.toDouble()).arg(paramCount)); + } + else if (justWritten && writeMismatch) { + // Mismatch, tell user + setParameterStatusMsg(tr("FAILURE: Wrote %1: sent %2 != onboard %3").arg(paramName).arg(map->value(paramName).toDouble()).arg(value.toDouble()), + ParamCommsStatusLevel_Warning); + } + else { + QString val = QString("%1").arg(value.toFloat(), 5, 'f', 1, QChar(' ')); + if (missCount == 0) { + // Transmission done + QTime time = QTime::currentTime(); + QString timeString = time.toString(); + setParameterStatusMsg(tr("All received. (updated at %1)").arg(timeString)); + } + else { + // Transmission in progress + setParameterStatusMsg(tr("OK: %1 %2 (%3/%4)").arg(paramName).arg(val).arg(paramCount-missCount).arg(paramCount), + ParamCommsStatusLevel_Warning); + } + } + + // Check if last parameter was received + if (missCount == 0 && missWriteCount == 0) { + this->transmissionActive = false; + this->transmissionListMode = false; + transmissionListSizeKnown.clear(); + foreach (int key, transmissionMissingPackets.keys()) { + transmissionMissingPackets.value(key)->clear(); + } + + //all parameters have been received, broadcast to UI + emit parameterListUpToDate(); + //TODO in UI + // Expand visual tree + //tree->expandItem(tree->topLevelItem(0)); + } +} + + +void UASParameterCommsMgr::writeParamsToPersistentStorage() +{ + if (mav) { + mav->writeParametersToStorage(); //TODO track timeout, retransmit etc? + } +} + + +void UASParameterCommsMgr::sendPendingParameters() +{ + // Iterate through all components, through all pending parameters and send them to UAS + int parametersSent = 0; + QMap*> changedValues = paramDataModel->getPendingParameters(); + QMap*>::iterator i; + for (i = changedValues.begin(); i != changedValues.end(); ++i) { + // Iterate through the parameters of the component + int compid = i.key(); + QMap* comp = i.value(); + { + QMap::iterator j; + for (j = comp->begin(); j != comp->end(); ++j) { + //TODO mavlink command for "set parameter list" ? + setParameter(compid, j.key(), j.value()); + parametersSent++; + } + } + } + + // Change transmission status if necessary + if (parametersSent == 0) { + setParameterStatusMsg(tr("No transmission: No changed values."),ParamCommsStatusLevel_Warning); + } else { + setParameterStatusMsg(tr("Transmitting %1 parameters.").arg(parametersSent)); + // Set timeouts + if (transmissionActive) { + transmissionTimeout += parametersSent*rewriteTimeout; + } + else { + transmissionActive = true; + quint64 newTransmissionTimeout = QGC::groundTimeMilliseconds() + parametersSent*rewriteTimeout; + if (newTransmissionTimeout > transmissionTimeout) { + transmissionTimeout = newTransmissionTimeout; + } + } + // Enable guard + setRetransmissionGuardEnabled(true); + } +} + diff --git a/src/uas/UASParameterCommsMgr.h b/src/uas/UASParameterCommsMgr.h index 72da05d8f..ff366c38e 100644 --- a/src/uas/UASParameterCommsMgr.h +++ b/src/uas/UASParameterCommsMgr.h @@ -10,9 +10,19 @@ class UASInterface; class UASParameterDataModel; + + 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); @@ -21,19 +31,30 @@ protected: /** @brief Activate / deactivate parameter retransmission */ virtual void setRetransmissionGuardEnabled(bool enabled); - virtual void setParameterStatusMsg(const QString& msg); + virtual void setParameterStatusMsg(const QString& msg, ParamCommsStatusLevel_t level=ParamCommsStatusLevel_OK); + + /** @brief Load settings that control eg retransmission timeouts */ + void loadParamCommsSettings(); signals: void parameterChanged(int component, QString parameter, QVariant value); void parameterChanged(int component, int parameterIndex, QVariant value); - void parameterValueConfirmed(int component, QString parameter, QVariant value); + void parameterValueConfirmed(int uas, int component,int paramCount, int paramId, QString parameter, QVariant value); void parameterListUpToDate(int component); void parameterUpdateRequested(int component, const QString& parameter); void parameterUpdateRequestedById(int componentId, int paramId); + /** @brief We updated the parameter status message */ + void parameterStatusMsgUpdated(QString msg, ParamCommsStatusLevel_t level); public slots: + /** @brief Iterate through all components, through all pending parameters and send them to UAS */ + virtual void sendPendingParameters(); + + /** @brief Write the current onboard parameters from transient RAM into persistent storage, e.g. EEPROM or harddisk */ + virtual void writeParamsToPersistentStorage(); + /** @brief Write one parameter to the MAV */ virtual void setParameter(int component, QString parameterName, QVariant value); /** @brief Request list of parameters from MAV */ @@ -44,6 +65,12 @@ public slots: /** @brief Request a single parameter update by name */ virtual void requestParameterUpdate(int component, const QString& parameter); + 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: UASInterface* mav; ///< The MAV we're talking to diff --git a/src/uas/UASParameterDataModel.cc b/src/uas/UASParameterDataModel.cc index d678385dc..0a5f9d972 100644 --- a/src/uas/UASParameterDataModel.cc +++ b/src/uas/UASParameterDataModel.cc @@ -103,13 +103,13 @@ void UASParameterDataModel::setOnboardParameterWithType(int componentId, QString } } -void UASParameterDataModel::addComponent(int componentId) +void UASParameterDataModel::addComponent(int compId) { - if (!onboardParameters.contains(componentId)) { - onboardParameters.insert(componentId, new QMap()); + if (!onboardParameters.contains(compId)) { + onboardParameters.insert(compId, new QMap()); } - if (!pendingParameters.contains(componentId)) { - pendingParameters.insert(componentId, new QMap()); + if (!pendingParameters.contains(compId)) { + pendingParameters.insert(compId, new QMap()); } } diff --git a/src/uas/UASParameterDataModel.h b/src/uas/UASParameterDataModel.h index 0f89ed436..240d37507 100644 --- a/src/uas/UASParameterDataModel.h +++ b/src/uas/UASParameterDataModel.h @@ -14,6 +14,7 @@ public: explicit UASParameterDataModel(QObject *parent = 0); + int getTotalOnboardParams() { return totalOnboardParameters; } //Parameter meta info bool isParamMinKnown(const QString& param) { return paramMin.contains(param); } virtual bool isValueLessThanParamMin(const QString& param, double dblVal); @@ -28,8 +29,11 @@ public: virtual QString getParamDescription(const QString& param) { return paramDescriptions.value(param, ""); } virtual void setParamDescriptions(const QMap& paramInfo); - - virtual void addComponent(int componentId); + //TODO make this method protected? + /** @brief Ensure that the data model is aware of this component + * @param compId Id of the component + */ + virtual void addComponent(int compId); /** @brief Write a new pending parameter value that may be eventually sent to the UAS */ virtual void setPendingParameter(int componentId, QString& key, const QVariant& value); @@ -83,6 +87,8 @@ public: signals: + void parameterUpdated(int compId, int paramId, QString paramName, QVariant value); + public slots: @@ -90,6 +96,7 @@ protected: int uasId; ///< The UAS / MAV to which this data model pertains QMap* > pendingParameters; ///< Changed values that have not yet been transmitted to the UAS, by component ID QMap* > onboardParameters; ///< All parameters confirmed to be stored onboard the UAS, by component ID + int totalOnboardParameters;///< The known count of onboard parameters, may not match onboardParameters until all params are received // Tooltip data structures QMap paramDescriptions; ///< Tooltip values diff --git a/src/ui/QGCParamWidget.cc b/src/ui/QGCParamWidget.cc index 4c973cd2a..97e0d1eb3 100644 --- a/src/ui/QGCParamWidget.cc +++ b/src/ui/QGCParamWidget.cc @@ -36,12 +36,13 @@ This file is part of the QGROUNDCONTROL project #include #include #include +#include -#include "QGCParamWidget.h" -#include "UASInterface.h" #include "MainWindow.h" -#include #include "QGC.h" +#include "QGCParamWidget.h" +#include "UASInterface.h" +#include "UASParameterCommsMgr.h" /** * @param uas MAV to set the parameters on @@ -51,13 +52,6 @@ QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) : QGCUASParamManager(uas, parent), componentItems(new QMap()) { - // Load settings - loadSettings(); - - // Load default values and tooltips - QString autoPilotName(uas->getAutopilotTypeName()); - QString sysTypeName(uas->getSystemTypeName()); - loadParamMetaInfoCSV(autoPilotName, sysTypeName); // Create tree widget tree = new QTreeWidget(this); @@ -145,14 +139,10 @@ QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) : connect(uas, SIGNAL(parameterChanged(int,int,int,int,QString,QVariant)), this, SLOT(receivedParameterUpdate(int,int,int,int,QString,QVariant))); - connect(&retransmissionTimer, SIGNAL(timeout()), this, SLOT(retransmissionGuardTick())); - - +// connect(&retransmissionTimer, SIGNAL(timeout()), this, SLOT(retransmissionGuardTick())); // connect(this, SIGNAL(requestParameter(int,QString)), uas, SLOT(requestParameter(int,QString))); // connect(this, SIGNAL(requestParameter(int,int)), uas, SLOT(requestParameter(int,int))); - - // Get parameters if (uas) { requestAllParamsUpdate(); @@ -160,245 +150,203 @@ QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) : } -void QGCParamWidget::loadSettings() -{ - QSettings settings; - settings.beginGroup("QGC_MAVLINK_PROTOCOL"); - bool ok; - int temp = settings.value("PARAMETER_RETRANSMISSION_TIMEOUT", retransmissionTimeout).toInt(&ok); - if (ok) retransmissionTimeout = temp; - temp = settings.value("PARAMETER_REWRITE_TIMEOUT", rewriteTimeout).toInt(&ok); - if (ok) rewriteTimeout = temp; - settings.endGroup(); -} - - -void QGCParamWidget::loadParamMetaInfoCSV(const QString& autopilot, const QString& airframe) -{ - Q_UNUSED(airframe); - qDebug() << "ATTEMPTING TO LOAD CSV"; - QDir appDir = QApplication::applicationDirPath(); - appDir.cd("files"); - QString fileName = QString("%1/%2/parameter_tooltips/tooltips.txt").arg(appDir.canonicalPath()).arg(autopilot.toLower()); - QFile paramMetaFile(fileName); - qDebug() << "AUTOPILOT:" << autopilot; - qDebug() << "FILENAME: " << fileName; - - // Load CSV data - if (!paramMetaFile.open(QIODevice::ReadOnly | QIODevice::Text)) - { - //qDebug() << "COULD NOT OPEN PARAM META INFO FILE:" << fileName; - return; - } - - - QTextStream in(¶mMetaFile); - paramDataModel->loadParamMetaInfoFromStream(in); - paramMetaFile.close(); - -} - -/** - * @return The MAV of this widget. Unless the MAV object has been destroyed, this - * pointer is never zero. - */ -UASInterface* QGCParamWidget::getUAS() -{ - return mav; -} - -/** - * - * @param uas System which has the component - * @param component id of the component - * @param componentName human friendly name of the component - */ -void QGCParamWidget::addComponentItem(int uas, int component, QString componentName) +void QGCParamWidget::addComponentItem( int compId, QString compName) { - Q_UNUSED(uas); - if (componentItems->contains(component)) { + if (componentItems->contains(compId)) { // Update existing - componentItems->value(component)->setData(0, Qt::DisplayRole, QString("%1 (#%2)").arg(componentName).arg(component)); + componentItems->value(compId)->setData(0, Qt::DisplayRole, QString("%1 (#%2)").arg(compName).arg(compId)); //components->value(component)->setData(1, Qt::DisplayRole, QString::number(component)); - componentItems->value(component)->setFirstColumnSpanned(true); + componentItems->value(compId)->setFirstColumnSpanned(true); } else { // Add new - QStringList list(QString("%1 (#%2)").arg(componentName).arg(component)); + QStringList list(QString("%1 (#%2)").arg(compName).arg(compId)); QTreeWidgetItem* comp = new QTreeWidgetItem(list); comp->setFirstColumnSpanned(true); - componentItems->insert(component, comp); + componentItems->insert(compId, comp); // Create grouping and update maps - paramGroups.insert(component, new QMap()); + paramGroups.insert(compId, new QMap()); tree->addTopLevelItem(comp); tree->update(); } - paramDataModel->addComponent(component); + //TODO it seems unlikely that the UI would know about a component before the data model... + paramDataModel->addComponent(compId); } + + /** * @param uas System which has the component * @param component id of the component * @param parameterName human friendly name of the parameter */ -void QGCParamWidget::receivedParameterUpdate(int uas, int component, int paramCount, int paramId, QString parameterName, QVariant value) +//void QGCParamWidget::receivedParameterUpdate(int uas, int component, int paramCount, int paramId, QString parameterName, QVariant value) +//{ + +// updateParameterDisplay(component, parameterName, value); + +// // Missing packets list has to be instantiated for all components +// if (!transmissionMissingPackets.contains(component)) { +// transmissionMissingPackets.insert(component, new QList()); +// } + +// // List mode is different from single parameter transfers +// if (transmissionListMode) { +// // Only accept the list size once on the first packet from +// // each component +// if (!transmissionListSizeKnown.contains(component)) +// { +// // Mark list size as known +// transmissionListSizeKnown.insert(component, true); + +// // Mark all parameters as missing +// for (int i = 0; i < paramCount; ++i) +// { +// if (!transmissionMissingPackets.value(component)->contains(i)) +// { +// transmissionMissingPackets.value(component)->append(i); +// } +// } + +// // There is only one transmission timeout for all components +// // since components do not manage their transmission, +// // the longest timeout is safe for all components. +// quint64 thisTransmissionTimeout = QGC::groundTimeMilliseconds() + ((paramCount)*retransmissionTimeout); +// if (thisTransmissionTimeout > transmissionTimeout) +// { +// transmissionTimeout = thisTransmissionTimeout; +// } +// } + +// // Start retransmission guard +// // or reset timer +// paramCommsMgr->setRetransmissionGuardEnabled(true); //TODO +// } + +// // Mark this parameter as received in read list +// int index = transmissionMissingPackets.value(component)->indexOf(paramId); +// // If the MAV sent the parameter without request, it wont be in missing list +// if (index != -1) transmissionMissingPackets.value(component)->removeAt(index); + +// bool justWritten = false; +// bool writeMismatch = false; +// //bool lastWritten = false; +// // Mark this parameter as received in write ACK list +// QMap* map = transmissionMissingWriteAckPackets.value(component); +// if (map && map->contains(parameterName)) +// { +// justWritten = true; +// QVariant newval = map->value(parameterName); +// if (map->value(parameterName) != value) +// { +// writeMismatch = true; +// } +// map->remove(parameterName); +// } + +// int missCount = 0; +// foreach (int key, transmissionMissingPackets.keys()) +// { +// missCount += transmissionMissingPackets.value(key)->count(); +// } + +// int missWriteCount = 0; +// foreach (int key, transmissionMissingWriteAckPackets.keys()) +// { +// missWriteCount += transmissionMissingWriteAckPackets.value(key)->count(); +// } + +// if (justWritten && !writeMismatch && missWriteCount == 0) +// { +// // Just wrote one and count went to 0 - this was the last missing write parameter +// statusLabel->setText(tr("SUCCESS: WROTE ALL PARAMETERS")); +// QPalette pal = statusLabel->palette(); +// pal.setColor(backgroundRole(), QGC::colorGreen); +// statusLabel->setPalette(pal); +// } else if (justWritten && !writeMismatch) +// { +// statusLabel->setText(tr("SUCCESS: Wrote %2 (#%1/%4): %3").arg(paramId+1).arg(parameterName).arg(value.toDouble()).arg(paramCount)); +// QPalette pal = statusLabel->palette(); +// pal.setColor(backgroundRole(), QGC::colorGreen); +// statusLabel->setPalette(pal); +// } else if (justWritten && writeMismatch) +// { +// // Mismatch, tell user +// QPalette pal = statusLabel->palette(); +// pal.setColor(backgroundRole(), QGC::colorRed); +// statusLabel->setPalette(pal); +// statusLabel->setText(tr("FAILURE: Wrote %1: sent %2 != onboard %3").arg(parameterName).arg(map->value(parameterName).toDouble()).arg(value.toDouble())); +// } +// else +// { +// if (missCount > 0) +// { +// QPalette pal = statusLabel->palette(); +// pal.setColor(backgroundRole(), QGC::colorOrange); +// statusLabel->setPalette(pal); +// } +// else +// { +// QPalette pal = statusLabel->palette(); +// pal.setColor(backgroundRole(), QGC::colorGreen); +// statusLabel->setPalette(pal); +// } +// QString val = QString("%1").arg(value.toFloat(), 5, 'f', 1, QChar(' ')); +// //statusLabel->setText(tr("OK: %1 %2 #%3/%4, %5 miss").arg(parameterName).arg(val).arg(paramId+1).arg(paramCount).arg(missCount)); +// if (missCount == 0) +// { +// // Transmission done +// QTime time = QTime::currentTime(); +// QString timeString = time.toString(); +// statusLabel->setText(tr("All received. (updated at %1)").arg(timeString)); +// } +// else +// { +// // Transmission in progress +// statusLabel->setText(tr("OK: %1 %2 (%3/%4)").arg(parameterName).arg(val).arg(paramCount-missCount).arg(paramCount)); +// } +// } + +// // Check if last parameter was received +// if (missCount == 0 && missWriteCount == 0) +// { +// this->transmissionActive = false; +// this->transmissionListMode = false; +// transmissionListSizeKnown.clear(); +// foreach (int key, transmissionMissingPackets.keys()) +// { +// transmissionMissingPackets.value(key)->clear(); +// } + +// // Expand visual tree +// tree->expandItem(tree->topLevelItem(0)); +// } +//} + + +void QGCParamWidget::handleParameterUpdate(int componentId, int paramCount, int paramId, const QString& paramName, QVariant value) { + Q_UNUSED(paramCount); + Q_UNUSED(paramId); + updateParameterDisplay(componentId, paramName, value); +} - updateParameterDisplay(uas, component, parameterName, value); - - // Missing packets list has to be instantiated for all components - if (!transmissionMissingPackets.contains(component)) { - transmissionMissingPackets.insert(component, new QList()); - } - - // List mode is different from single parameter transfers - if (transmissionListMode) { - // Only accept the list size once on the first packet from - // each component - if (!transmissionListSizeKnown.contains(component)) - { - // Mark list size as known - transmissionListSizeKnown.insert(component, true); - - // Mark all parameters as missing - for (int i = 0; i < paramCount; ++i) - { - if (!transmissionMissingPackets.value(component)->contains(i)) - { - transmissionMissingPackets.value(component)->append(i); - } - } - - // There is only one transmission timeout for all components - // since components do not manage their transmission, - // the longest timeout is safe for all components. - quint64 thisTransmissionTimeout = QGC::groundTimeMilliseconds() + ((paramCount)*retransmissionTimeout); - if (thisTransmissionTimeout > transmissionTimeout) - { - transmissionTimeout = thisTransmissionTimeout; - } - } - - // Start retransmission guard - // or reset timer - setRetransmissionGuardEnabled(true); - } - - // Mark this parameter as received in read list - int index = transmissionMissingPackets.value(component)->indexOf(paramId); - // If the MAV sent the parameter without request, it wont be in missing list - if (index != -1) transmissionMissingPackets.value(component)->removeAt(index); - - bool justWritten = false; - bool writeMismatch = false; - //bool lastWritten = false; - // Mark this parameter as received in write ACK list - QMap* map = transmissionMissingWriteAckPackets.value(component); - if (map && map->contains(parameterName)) - { - justWritten = true; - QVariant newval = map->value(parameterName); - if (map->value(parameterName) != value) - { - writeMismatch = true; - } - map->remove(parameterName); - } - - int missCount = 0; - foreach (int key, transmissionMissingPackets.keys()) - { - missCount += transmissionMissingPackets.value(key)->count(); - } - - int missWriteCount = 0; - foreach (int key, transmissionMissingWriteAckPackets.keys()) - { - missWriteCount += transmissionMissingWriteAckPackets.value(key)->count(); - } - - if (justWritten && !writeMismatch && missWriteCount == 0) - { - // Just wrote one and count went to 0 - this was the last missing write parameter - statusLabel->setText(tr("SUCCESS: WROTE ALL PARAMETERS")); - QPalette pal = statusLabel->palette(); - pal.setColor(backgroundRole(), QGC::colorGreen); - statusLabel->setPalette(pal); - } else if (justWritten && !writeMismatch) - { - statusLabel->setText(tr("SUCCESS: Wrote %2 (#%1/%4): %3").arg(paramId+1).arg(parameterName).arg(value.toDouble()).arg(paramCount)); - QPalette pal = statusLabel->palette(); - pal.setColor(backgroundRole(), QGC::colorGreen); - statusLabel->setPalette(pal); - } else if (justWritten && writeMismatch) - { - // Mismatch, tell user - QPalette pal = statusLabel->palette(); - pal.setColor(backgroundRole(), QGC::colorRed); - statusLabel->setPalette(pal); - statusLabel->setText(tr("FAILURE: Wrote %1: sent %2 != onboard %3").arg(parameterName).arg(map->value(parameterName).toDouble()).arg(value.toDouble())); - } - else - { - if (missCount > 0) - { - QPalette pal = statusLabel->palette(); - pal.setColor(backgroundRole(), QGC::colorOrange); - statusLabel->setPalette(pal); - } - else - { - QPalette pal = statusLabel->palette(); - pal.setColor(backgroundRole(), QGC::colorGreen); - statusLabel->setPalette(pal); - } - QString val = QString("%1").arg(value.toFloat(), 5, 'f', 1, QChar(' ')); - //statusLabel->setText(tr("OK: %1 %2 #%3/%4, %5 miss").arg(parameterName).arg(val).arg(paramId+1).arg(paramCount).arg(missCount)); - if (missCount == 0) - { - // Transmission done - QTime time = QTime::currentTime(); - QString timeString = time.toString(); - statusLabel->setText(tr("All received. (updated at %1)").arg(timeString)); - } - else - { - // Transmission in progress - statusLabel->setText(tr("OK: %1 %2 (%3/%4)").arg(parameterName).arg(val).arg(paramCount-missCount).arg(paramCount)); - } - } - - // Check if last parameter was received - if (missCount == 0 && missWriteCount == 0) - { - this->transmissionActive = false; - this->transmissionListMode = false; - transmissionListSizeKnown.clear(); - foreach (int key, transmissionMissingPackets.keys()) - { - transmissionMissingPackets.value(key)->clear(); - } - // Expand visual tree - tree->expandItem(tree->topLevelItem(0)); - } +void QGCParamWidget::handleParameterListUpToDate(int component) +{ + // Expand visual tree + tree->expandItem(tree->topLevelItem(0)); } -/** - * @param uas System which has the component - * @param component id of the component - * @param parameterName human friendly name of the parameter - */ -void QGCParamWidget::updateParameterDisplay(int uas, int componentId, QString parameterName, QVariant value) -{ - Q_UNUSED(uas); +void QGCParamWidget::updateParameterDisplay(int componentId, QString parameterName, QVariant value) +{ // QString ptrStr; // ptrStr.sprintf("%8p", this); @@ -410,11 +358,12 @@ void QGCParamWidget::updateParameterDisplay(int uas, int componentId, QString pa // Get component if (!componentItems->contains(componentId)) { QString componentName = tr("Component #%1").arg(componentId); - addComponentItem(uas, componentId, componentName); + addComponentItem(componentId, componentName); } - // Replace value in data model - paramDataModel->handleParameterUpdate(componentId,parameterName,value); + //TODO this should be bubbling up from the model, not vice-versa, right? +// // Replace value in data model +// paramDataModel->handleParameterUpdate(componentId,parameterName,value); QString splitToken = "_"; @@ -439,36 +388,30 @@ void QGCParamWidget::updateParameterDisplay(int uas, int componentId, QString pa for (int i = 0; i < parentItem->childCount(); i++) { QTreeWidgetItem* child = parentItem->child(i); QString key = child->data(0, Qt::DisplayRole).toString(); - if (key == parameterName) - { + if (key == parameterName) { //qDebug() << "UPDATED CHILD"; parameterItem = child; - if (value.type() == QVariant::Char) - { + if (value.type() == QVariant::Char) { parameterItem->setData(1, Qt::DisplayRole, value.toUInt()); } - else - { + else { parameterItem->setData(1, Qt::DisplayRole, value); } found = true; } } - if (!found) - { + if (!found) { // Insert parameter into map QStringList plist; plist.append(parameterName); // CREATE PARAMETER ITEM parameterItem = new QTreeWidgetItem(plist); // CONFIGURE PARAMETER ITEM - if (value.type() == QVariant::Char) - { + if (value.type() == QVariant::Char) { parameterItem->setData(1, Qt::DisplayRole, value.toUInt()); } - else - { + else { parameterItem->setData(1, Qt::DisplayRole, value); } @@ -476,16 +419,13 @@ void QGCParamWidget::updateParameterDisplay(int uas, int componentId, QString pa parameterItem->setFlags(parameterItem->flags() | Qt::ItemIsEditable); } } - else - { + else { bool found = false; QTreeWidgetItem* parent = componentItems->value(componentId); - for (int i = 0; i < parent->childCount(); i++) - { + for (int i = 0; i < parent->childCount(); i++) { QTreeWidgetItem* child = parent->child(i); QString key = child->data(0, Qt::DisplayRole).toString(); - if (key == parameterName) - { + if (key == parameterName) { //qDebug() << "UPDATED CHILD"; parameterItem = child; parameterItem->setData(1, Qt::DisplayRole, value); @@ -493,8 +433,7 @@ void QGCParamWidget::updateParameterDisplay(int uas, int componentId, QString pa } } - if (!found) - { + if (!found) { // Insert parameter into map QStringList plist; plist.append(parameterName); @@ -525,7 +464,7 @@ void QGCParamWidget::updateParameterDisplay(int uas, int componentId, QString pa parameterItem->setToolTip(0, tooltipFormat); parameterItem->setToolTip(1, tooltipFormat); - paramDataModel->handleParameterUpdate(componentId,parameterName,value); + //paramDataModel->handleParameterUpdate(componentId,parameterName,value); } @@ -600,21 +539,12 @@ void QGCParamWidget::setParameterStatusMsg(const QString& msg) statusLabel->setText(msg); } - - void QGCParamWidget::requestAllParamsUpdate() { if (!mav) { return; } - // FIXME This call does not belong here - // Once the comm handling is moved to a new - // Param manager class the settings can be directly - // loaded from MAVLink protocol - loadSettings(); - // End of FIXME - // Clear view and request param list clear(); @@ -628,45 +558,7 @@ void QGCParamWidget::requestAllParamsUpdate() */ void QGCParamWidget::setParameters() { - // Iterate through all components, through all parameters and emit them - int parametersSent = 0; - QMap*> changedValues = paramDataModel->getPendingParameters(); - QMap*>::iterator i; - for (i = changedValues.begin(); i != changedValues.end(); ++i) { - // Iterate through the parameters of the component - int compid = i.key(); - QMap* comp = i.value(); - { - QMap::iterator j; - for (j = comp->begin(); j != comp->end(); ++j) { - //TODO mavlink command for "set parameter list" ? - setParameter(compid, j.key(), j.value()); - parametersSent++; - } - } - } - - // Change transmission status if necessary - if (parametersSent == 0) { - statusLabel->setText(tr("No transmission: No changed values.")); - } else { - statusLabel->setText(tr("Transmitting %1 parameters.").arg(parametersSent)); - // Set timeouts - if (transmissionActive) - { - transmissionTimeout += parametersSent*rewriteTimeout; - } - else - { - transmissionActive = true; - quint64 newTransmissionTimeout = QGC::groundTimeMilliseconds() + parametersSent*rewriteTimeout; - if (newTransmissionTimeout > transmissionTimeout) { - transmissionTimeout = newTransmissionTimeout; - } - } - // Enable guard - setRetransmissionGuardEnabled(true); - } + paramCommsMgr->sendPendingParameters(); } /** @@ -680,28 +572,19 @@ void QGCParamWidget::writeParameters() QMap*>::iterator i; QMap*> changedValues = paramDataModel->getPendingParameters(); - for (i = changedValues.begin(); i != changedValues.end() , (0 == changedParamCount); ++i) - { + for (i = changedValues.begin(); i != changedValues.end() , (0 == changedParamCount); ++i) { // Iterate through the parameters of the component - QMap* comp = i.value(); - QMap::iterator j; - for (j = comp->begin(); j != comp->end(); ++j) - { - changedParamCount++; - break;//it only takes one changed param to warrant warning the user - } + QMap* compPending = i.value(); + changedParamCount += compPending->count(); } - if (changedParamCount > 0) - { + if (changedParamCount > 0) { QMessageBox msgBox; msgBox.setText(tr("There are locally changed parameters. Please transmit them first () or update them with the onboard values () before storing onboard from RAM to ROM.")); msgBox.exec(); } - else - { - if (!mav) return; - mav->writeParametersToStorage(); + else { + paramCommsMgr->writeParamsToPersistentStorage(); } } @@ -796,7 +679,7 @@ void QGCParamWidget::setParameter(int component, QString parameterName, QVariant } // Enable guard / reset timeouts - setRetransmissionGuardEnabled(true); + paramCommsMgr->setRetransmissionGuardEnabled(true); //TODO } void QGCParamWidget::readParameters() diff --git a/src/ui/QGCParamWidget.h b/src/ui/QGCParamWidget.h index af58eabf3..0ada126d7 100644 --- a/src/ui/QGCParamWidget.h +++ b/src/ui/QGCParamWidget.h @@ -48,9 +48,6 @@ class QGCParamWidget : public QGCUASParamManager Q_OBJECT public: QGCParamWidget(UASInterface* uas, QWidget *parent = 0); - /** @brief Get the UAS of this widget */ - UASInterface* getUAS(); - protected: virtual void setParameterStatusMsg(const QString& msg); @@ -61,12 +58,20 @@ signals: public slots: - /** @brief Add a component to the list */ - void addComponentItem(int uas, int component, QString componentName); + /** @brief Add a component to the list + * @param compId Component id of the component + * @param compName Human friendly name of the component + */ + void addComponentItem(int compId, QString compName); + /** @brief Add a parameter to the list with retransmission / safety checks */ - void receivedParameterUpdate(int uas, int component, int paramCount, int paramId, QString parameterName, QVariant value); - /** @brief Add a parameter to the list */ - void updateParameterDisplay(int uas, int component, QString parameterName, QVariant value); +// 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); + + /** @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 */ @@ -91,17 +96,11 @@ public slots: protected: QTreeWidget* tree; ///< The parameter tree - QLabel* statusLabel; ///< Parameter transmission label - QMap* componentItems; ///< The list of component items, stored by component ID - QMap* > paramGroups; ///< Parameter groups - - + QLabel* statusLabel; ///< User-facing parameter status label + QMap* componentItems; ///< The tree of component items, stored by component ID + QMap* > paramGroups; ///< Parameter groups to organize component items - /** @brief Load settings */ - void loadSettings(); - /** @brief Load meta information from CSV */ - void loadParamMetaInfoCSV(const QString& autopilot, const QString& airframe); }; #endif // QGCPARAMWIDGET_H -- 2.22.0