From 1bf69c0a707f68018ee3db4280a182b609cfe089 Mon Sep 17 00:00:00 2001 From: tstellanova Date: Tue, 6 Aug 2013 19:52:30 -0700 Subject: [PATCH] start moving comms mgmt code to a separate class --- qgroundcontrol.pro | 6 +- src/uas/QGCUASParamManager.cc | 63 ++++---- src/uas/UASParameterCommsMgr.cc | 278 ++++++++++++++++++++++++++++++++ src/uas/UASParameterCommsMgr.h | 72 +++++++++ 4 files changed, 386 insertions(+), 33 deletions(-) create mode 100644 src/uas/UASParameterCommsMgr.cc create mode 100644 src/uas/UASParameterCommsMgr.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index fd4df5e5b..6f0f51ca4 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -469,7 +469,8 @@ HEADERS += src/MG.h \ src/ui/configuration/terminalconsole.h \ src/ui/configuration/ApmHighlighter.h \ src/ui/configuration/ApmFirmwareConfig.h \ - src/uas/UASParameterDataModel.h + src/uas/UASParameterDataModel.h \ + src/uas/UASParameterCommsMgr.h # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010|win32-msvc2012::HEADERS += src/ui/map3D/QGCGoogleEarthView.h @@ -685,7 +686,8 @@ SOURCES += src/main.cc \ src/ui/configuration/SerialSettingsDialog.cc \ src/ui/configuration/ApmHighlighter.cc \ src/ui/configuration/ApmFirmwareConfig.cc \ - src/uas/UASParameterDataModel.cc + src/uas/UASParameterDataModel.cc \ + src/uas/UASParameterCommsMgr.cc # Enable Google Earth only on Mac OS and Windows with Visual Studio compiler macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010|win32-msvc2012::SOURCES += src/ui/map3D/QGCGoogleEarthView.cc diff --git a/src/uas/QGCUASParamManager.cc b/src/uas/QGCUASParamManager.cc index 9103a2b43..de1d19afd 100644 --- a/src/uas/QGCUASParamManager.cc +++ b/src/uas/QGCUASParamManager.cc @@ -66,37 +66,6 @@ void QGCUASParamManager::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) -{ -// qDebug() << "setRetransmissionGuardEnabled: " << enabled; - - if (enabled) { - retransmissionTimer.start(retransmissionTimeout); - } else { - retransmissionTimer.stop(); - } -} - -void QGCUASParamManager::setParameterStatusMsg(const QString& msg) -{ - qDebug() << "parameterStatusMsg: " << msg; - parameterStatusMsg = msg; -} - -void QGCUASParamManager::setParamDescriptions(const QMap& paramInfo) { - paramDataModel->setParamDescriptions(paramInfo); -} - - - void QGCUASParamManager::retransmissionGuardTick() { if (transmissionActive) { @@ -199,3 +168,35 @@ void QGCUASParamManager::retransmissionGuardTick() 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(); + } +} + +void QGCUASParamManager::setParameterStatusMsg(const QString& msg) +{ + qDebug() << "parameterStatusMsg: " << msg; + parameterStatusMsg = msg; +} + +void QGCUASParamManager::setParamDescriptions(const QMap& paramInfo) { + paramDataModel->setParamDescriptions(paramInfo); +} + + diff --git a/src/uas/UASParameterCommsMgr.cc b/src/uas/UASParameterCommsMgr.cc new file mode 100644 index 000000000..9f0d60e05 --- /dev/null +++ b/src/uas/UASParameterCommsMgr.cc @@ -0,0 +1,278 @@ +#include "UASParameterCommsMgr.h" + +#include "QGCUASParamManager.h" +#include "UASInterface.h" + +UASParameterCommsMgr::UASParameterCommsMgr(QObject *parent, UASInterface *uas) : + QObject(parent) +{ + mav = uas; + + // Sending params to the UAS + connect(this, SIGNAL(parameterChanged(int,QString,QVariant)), mav, SLOT(setParameter(int,QString,QVariant))); + + // New parameters from UAS + connect(mav, SIGNAL(parameterChanged(int,int,int,int,QString,QVariant)), this, SLOT(receivedParameterUpdate(int,int,int,int,QString,QVariant))); + +} + + +/** + * Send a request to deliver the list of onboard parameters + * from the MAV. + */ +void UASParameterCommsMgr::requestParameterList() +{ + if (!mav) { + return; + } + + //TODO check: no need to cause datamodel to forget params here? +// paramDataModel->forgetAllOnboardParameters(); + + // Clear transmission state + receivedParamsList.clear(); + transmissionListSizeKnown.clear(); + + transmissionListMode = true; + foreach (int key, transmissionMissingPackets.keys()) { + transmissionMissingPackets.value(key)->clear(); + } + transmissionActive = true; + + setParameterStatusMsg(tr("Requested param list.. waiting")); + mav->requestParameters(); + +} + + +void UASParameterCommsMgr::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 UASParameterCommsMgr::setRetransmissionGuardEnabled(bool enabled) +{ +// qDebug() << "setRetransmissionGuardEnabled: " << enabled; + + if (enabled) { + retransmissionTimer.start(retransmissionTimeout); + } else { + retransmissionTimer.stop(); + } +} + +void UASParameterCommsMgr::requestParameterUpdate(int component, const QString& parameter) +{ + if (mav) { + mav->requestParameter(component, parameter); + } +} + +/** + * @param component the subsystem which has the parameter + * @param parameterName name of the parameter, as delivered by the system + * @param value value of the parameter + */ +void UASParameterCommsMgr::setParameter(int component, QString parameterName, QVariant value) +{ + if (parameterName.isEmpty()) { + return; + } + + double dblValue = value.toDouble(); + + if (paramDataModel->isValueLessThanParamMin(parameterName,dblValue)) { + setParameterStatusMsg(tr("REJ. %1, %2 < min").arg(parameterName).arg(dblValue)); + return; + } + if (paramDataModel->isValueGreaterThanParamMax(parameterName,dblValue)) { + setParameterStatusMsg(tr("REJ. %1, %2 > max").arg(parameterName).arg(dblValue)); + return; + } + QVariant onboardVal; + paramDataModel->getOnboardParameterValue(component,parameterName,onboardVal); + if (onboardVal == value) { + setParameterStatusMsg(tr("REJ. %1 already %2").arg(parameterName).arg(dblValue)); + return; + } + + //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; + } + + // Wait for parameter to be written back + // mark it therefore as missing + if (!transmissionMissingWriteAckPackets.contains(component)) + { + transmissionMissingWriteAckPackets.insert(component, new QMap()); + } + + // Insert it in missing write ACK list + transmissionMissingWriteAckPackets.value(component)->insert(parameterName, value); + + // Set timeouts + if (transmissionActive) + { + transmissionTimeout += rewriteTimeout; + } + else + { + quint64 newTransmissionTimeout = QGC::groundTimeMilliseconds() + rewriteTimeout; + if (newTransmissionTimeout > transmissionTimeout) + { + transmissionTimeout = newTransmissionTimeout; + } + transmissionActive = true; + } + + // Enable guard / reset timeouts + setRetransmissionGuardEnabled(true); +} + +void UASParameterCommsMgr::setParameterStatusMsg(const QString& msg) +{ + qDebug() << "parameterStatusMsg: " << msg; + parameterStatusMsg = msg; +} diff --git a/src/uas/UASParameterCommsMgr.h b/src/uas/UASParameterCommsMgr.h new file mode 100644 index 000000000..72da05d8f --- /dev/null +++ b/src/uas/UASParameterCommsMgr.h @@ -0,0 +1,72 @@ +#ifndef UASPARAMETERCOMMSMGR_H +#define UASPARAMETERCOMMSMGR_H + +#include +#include +#include +#include +#include + +class UASInterface; +class UASParameterDataModel; + +class UASParameterCommsMgr : public QObject +{ + Q_OBJECT +public: + explicit UASParameterCommsMgr(QObject *parent = 0, UASInterface* uas = NULL); + + +protected: + /** @brief Activate / deactivate parameter retransmission */ + virtual void setRetransmissionGuardEnabled(bool enabled); + + virtual void setParameterStatusMsg(const QString& msg); + +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 parameterListUpToDate(int component); + void parameterUpdateRequested(int component, const QString& parameter); + void parameterUpdateRequestedById(int componentId, int paramId); + + +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 */ + virtual void retransmissionGuardTick(); + + /** @brief Request a single parameter update by name */ + virtual void requestParameterUpdate(int component, const QString& parameter); + +protected: + + UASInterface* mav; ///< The MAV we're talking to + + UASParameterDataModel* paramDataModel; + + // 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 + + // Status + QString parameterStatusMsg; + +}; + + +#endif // UASPARAMETERCOMMSMGR_H -- 2.22.0