diff --git a/qtlogging.ini b/files/QLoggingCategory/qtlogging.ini similarity index 100% rename from qtlogging.ini rename to files/QLoggingCategory/qtlogging.ini diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index dd04adb544144e158b5eb6fa25f6f10616104d01..d1ba57b8d68b8c65c0afbba88bb195482779dac4 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -231,4 +231,7 @@ src/qgcunittest/MockLink.param + + files/QLoggingCategory/qtlogging.ini + diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index 469bcee53e95885b8a3fccfdba1a23897acedc8d..57bb92bab156b42bba3149a8189d22ce73e944e6 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -92,6 +92,36 @@ QGCApplication::QGCApplication(int &argc, char* argv[], bool unitTesting) : Q_ASSERT(_app == NULL); _app = this; + +#ifdef QT_DEBUG + // First thing we want to do is set up the qtlogging.ini file. If it doesn't already exist we copy + // it to the correct location. This way default debug builds will have logging turned off. + + bool loggingDirectoryOk = false; + QDir iniFileLocation(QStandardPaths::writableLocation(QStandardPaths::GenericConfigLocation)); + if (!iniFileLocation.cd("QtProjects")) { + if (!iniFileLocation.mkdir("QtProjects")) { + qDebug() << "Unable to create qtlogging.ini directory" << iniFileLocation.filePath("QtProjects"); + } else { + if (!iniFileLocation.cd("QtProjects")) { + qDebug() << "Unable to access qtlogging.ini directory" << iniFileLocation.filePath("QtProjects");; + } + loggingDirectoryOk = true; + } + } else { + loggingDirectoryOk = true; + } + + if (loggingDirectoryOk) { + qDebug () << iniFileLocation; + if (!iniFileLocation.exists("qtlogging.ini")) { + if (!QFile::copy(":QLoggingCategory/qtlogging.ini", iniFileLocation.filePath("qtlogging.ini"))) { + qDebug() << "Unable to copy qtlogging.ini to" << iniFileLocation; + } + } + } +#endif + // Set application information if (_runningUnitTests) { // We don't want unit tests to use the same QSettings space as the normal app. So we tweak the app diff --git a/src/qgcunittest/MockUAS.h b/src/qgcunittest/MockUAS.h index b6bda697570b42202fa73eca1619be0abe6cbc3f..e9a48d9dea2ed416099e72c8f88f91e52de4cecc 100644 --- a/src/qgcunittest/MockUAS.h +++ b/src/qgcunittest/MockUAS.h @@ -133,7 +133,6 @@ public slots: virtual void setTargetPosition(float x, float y, float z, float yaw) { Q_UNUSED(x); Q_UNUSED(y); Q_UNUSED(z); Q_UNUSED(yaw); Q_ASSERT(false); }; virtual void setLocalOriginAtCurrentGPSPosition() { Q_ASSERT(false); }; virtual void setHomePosition(double lat, double lon, double alt) { Q_UNUSED(lat); Q_UNUSED(lon); Q_UNUSED(alt); Q_ASSERT(false); }; - virtual void requestParameters() { Q_ASSERT(false); }; virtual void requestParameter(int component, const QString& parameter) { Q_UNUSED(component); Q_UNUSED(parameter); Q_ASSERT(false); }; virtual void writeParametersToStorage() { Q_ASSERT(false); }; virtual void readParametersFromStorage() { Q_ASSERT(false); }; diff --git a/src/uas/QGCMAVLinkUASFactory.cc b/src/uas/QGCMAVLinkUASFactory.cc index 39df612c3ec3cfc0c3cb0a74a75577439c7bb550..07bfcf2053d97b6a0fe7aa1fe1f4abc72ded8738 100644 --- a/src/uas/QGCMAVLinkUASFactory.cc +++ b/src/uas/QGCMAVLinkUASFactory.cc @@ -79,7 +79,7 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte uas->addLink(link); // First thing we do with a new UAS is get the parameters - uas->requestParameters(); + uas->getParamManager()->requestParameterList(); // Now add UAS to "official" list, which makes the whole application aware of it UASManager::instance()->addUAS(uas); diff --git a/src/uas/QGCUASParamManager.cc b/src/uas/QGCUASParamManager.cc index 7212d9caa56184a0f0aac9496eed2f462967a633..f1de5ab4f104d8470379943afb3bb1d9a79a54b5 100644 --- a/src/uas/QGCUASParamManager.cc +++ b/src/uas/QGCUASParamManager.cc @@ -217,6 +217,8 @@ void QGCUASParamManager::requestRcCalibrationParamsUpdate() { void QGCUASParamManager::_parameterListUpToDate(void) { + qDebug() << "Emitting parameters ready, count:" << paramDataModel.countOnboardParams(); + _parametersReady = true; emit parameterListUpToDate(); } diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 1e7f16d0e99e04f6dbc15d6fc05ee42706bbfb17..29a618e0e2d941971c201b3f4b3d7e32958473a6 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -1968,16 +1968,6 @@ int UAS::getCommunicationStatus() const return commStatus; } -void UAS::requestParameters() -{ - mavlink_message_t msg; - mavlink_msg_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_ALL); - sendMessage(msg); - - QDateTime time = QDateTime::currentDateTime(); - qDebug() << __FILE__ << ":" << __LINE__ << time.toString() << "LOADING PARAM LIST"; -} - void UAS::writeParametersToStorage() { mavlink_message_t msg; diff --git a/src/uas/UAS.h b/src/uas/UAS.h index a359de316da857a3509cfaba8300aff1a3bd8938..ed203a81af6813a7187f595f1235f1715c11f789 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -830,9 +830,6 @@ public slots: /** @brief Set current mode of operation, e.g. auto or manual, does not check the arming status, for anything else than arming/disarming operations use setMode instead */ void setModeArm(uint8_t newBaseMode, uint32_t newCustomMode); - /** @brief Request all parameters */ - void requestParameters(); - /** @brief Request a single parameter by name */ void requestParameter(int component, const QString& parameter); /** @brief Request a single parameter by index */ diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 2d7beb74a0616f20e401a351b526a04d5d5a8a09..6a6b56660a6ff379c246f7b136dc95ceefcdfc67 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -315,8 +315,6 @@ public slots: virtual void setLocalOriginAtCurrentGPSPosition() = 0; /** @brief Set world frame origin / home position at this GPS position */ virtual void setHomePosition(double lat, double lon, double alt) = 0; - /** @brief Request all onboard parameters of all components */ - virtual void requestParameters() = 0; /** @brief Request one specific onboard parameter */ virtual void requestParameter(int component, const QString& parameter) = 0; /** @brief Write parameter to permanent storage */ diff --git a/src/uas/UASParameterCommsMgr.cc b/src/uas/UASParameterCommsMgr.cc index 137c2385600f9ef94b995f11c60462baf302b8ae..ea5aff996f05a360693418360a6ac728dd7bfd7c 100644 --- a/src/uas/UASParameterCommsMgr.cc +++ b/src/uas/UASParameterCommsMgr.cc @@ -5,11 +5,13 @@ #include "QGCUASParamManagerInterface.h" #include "UASInterface.h" - - +#include "MAVLinkProtocol.h" +#include "MainWindow.h" #define RC_CAL_CHAN_MAX 8 +Q_LOGGING_CATEGORY(UASParameterCommsMgrLog, "UASParameterCommsMgrLog") + UASParameterCommsMgr::UASParameterCommsMgr(QObject *parent) : QObject(parent), lastReceiveTime(0), @@ -67,7 +69,15 @@ void UASParameterCommsMgr::loadParamCommsSettings() settings.endGroup(); } - +void UASParameterCommsMgr::_sendParamRequestListMsg(void) +{ + MAVLinkProtocol* mavlink = MainWindow::instance()->getMAVLink(); + Q_ASSERT(mavlink); + + mavlink_message_t msg; + mavlink_msg_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, mav->getUASID(), MAV_COMP_ID_ALL); + mav->sendMessage(msg); +} /** * Send a request to deliver the list of onboard parameters @@ -78,16 +88,20 @@ void UASParameterCommsMgr::requestParameterList() if (!mav) { return; } + if (!transmissionListMode) { + qCDebug(UASParameterCommsMgrLog) << "Requesting full parameter list"; transmissionListMode = true;//TODO eliminate? //we use (compId 0, paramId 0) as indicating all params for the system markReadParamWaiting(0,0); - mav->requestParameters(); + + _sendParamRequestListMsg(); + updateSilenceTimer(); } else { - qDebug() << __FILE__ << __LINE__ << "Ignoring requestParameterList because we're receiving params list"; + qCDebug(UASParameterCommsMgrLog) << "Ignoring requestParameterList because we're receiving params list"; } } @@ -119,7 +133,7 @@ void UASParameterCommsMgr::markWriteParamWaiting(int compId, QString paramName, */ void UASParameterCommsMgr::clearRetransmissionLists(int& missingReadCount, int& missingWriteCount ) { - qDebug() << __FILE__ << __LINE__ << "clearRetransmissionLists"; + qCDebug(UASParameterCommsMgrLog) << "Clearing re-transmission lists"; missingReadCount = 0; QList compIds = readsWaiting.keys(); @@ -192,7 +206,7 @@ void UASParameterCommsMgr::resendReadWriteRequests() qDebug() << "compId " << compId << "readsWaiting:" << missingReadParams->count(); foreach (int paramId, *missingReadParams) { if (0 == paramId && 0 == compId) { - mav->requestParameters(); + _sendParamRequestListMsg(); //don't request any other params individually for this component break; } @@ -203,7 +217,7 @@ void UASParameterCommsMgr::resendReadWriteRequests() requestedReadCount++; } else { - qDebug() << "Throttling read retransmit requests at" << requestedReadCount; + qCDebug(UASParameterCommsMgrLog) << "Throttling read retransmit requests at" << requestedReadCount; break; } } @@ -223,7 +237,7 @@ void UASParameterCommsMgr::resendReadWriteRequests() requestedWriteCount++; } else { - qDebug() << "Throttling write retransmit requests at" << requestedWriteCount; + qCDebug(UASParameterCommsMgrLog) << "Throttling write retransmit requests at" << requestedWriteCount; break; } } @@ -243,7 +257,7 @@ void UASParameterCommsMgr::silenceTimerExpired() { quint64 curTime = QGC::groundTimeMilliseconds(); int elapsed = (int)(curTime - lastSilenceTimerReset); - qDebug() << "silenceTimerExpired elapsed:" << elapsed; + qCDebug(UASParameterCommsMgrLog) << "silenceTimerExpired elapsed:" << elapsed; if (elapsed < silenceTimeout) { //reset the guard timer: it fired prematurely @@ -253,7 +267,7 @@ void UASParameterCommsMgr::silenceTimerExpired() int totalElapsed = (int)(curTime - lastReceiveTime); if (totalElapsed > maxSilenceTimeout) { - qDebug() << "maxSilenceTimeout exceeded: " << totalElapsed; + qCDebug(UASParameterCommsMgrLog) << "maxSilenceTimeout exceeded: " << totalElapsed; int missingReads, missingWrites; clearRetransmissionLists(missingReads,missingWrites); emit _stopSilenceTimer(); // Stop timer on our thread; @@ -299,7 +313,7 @@ void UASParameterCommsMgr::requestRcCalibrationParamsUpdate() } } else { - qDebug() << __FILE__ << __LINE__ << "Ignoring requestRcCalibrationParamsUpdate because we're receiving params list"; + qCDebug(UASParameterCommsMgrLog) << "Ignoring requestRcCalibrationParamsUpdate because we're receiving params list"; } } @@ -377,6 +391,7 @@ void UASParameterCommsMgr::updateSilenceTimer() } else { //all parameters have been received, broadcast to UI + qCDebug(UASParameterCommsMgrLog) << "emitting parameterListUpToDate"; emit parameterListUpToDate(); resetAfterListReceive(); emit _stopSilenceTimer(); // Stop timer on our thread; @@ -396,6 +411,8 @@ void UASParameterCommsMgr::setParameterStatusMsg(const QString& msg, ParamCommsS void UASParameterCommsMgr::receivedParameterUpdate(int uas, int compId, int paramCount, int paramId, QString paramName, QVariant value) { + qCDebug(UASParameterCommsMgrLog) << QString("Received parameter update for: name(%1) count(%2) index(%3)").arg(paramName).arg(paramCount).arg(paramId); + Q_UNUSED(uas); //this object is assigned to one UAS only lastReceiveTime = QGC::groundTimeMilliseconds(); // qDebug() << "compId" << compId << "receivedParameterUpdate:" << paramName; @@ -423,7 +440,7 @@ void UASParameterCommsMgr::receivedParameterUpdate(int uas, int compId, int para //remove our placeholder read request for all params readsWaiting.value(0)->remove(0); - qDebug() << "Mark all parameters as missing: " << paramCount; + qCDebug(UASParameterCommsMgrLog) << "receivedParameterUpdate: Mark all parameters as missing: " << paramCount; for (int i = 1; i < paramCount; ++i) { //param Id 0 is "all parameters" and not valid compMissingReads->insert(i); } @@ -533,7 +550,7 @@ void UASParameterCommsMgr::sendPendingParameters(bool copyToPersistent, bool for } else { setParameterStatusMsg(tr("Transmitting %1 parameters.").arg(parametersSent)); - qDebug() << "Pending parameters now:" << paramDataModel->countPendingParams(); + qCDebug(UASParameterCommsMgrLog) << "Pending parameters now:" << paramDataModel->countPendingParams(); } @@ -546,7 +563,7 @@ UASParameterCommsMgr::~UASParameterCommsMgr() QString ptrStr; ptrStr.sprintf("%8p", this); - qDebug() << "UASParameterCommsMgr destructor: " << ptrStr ; + qCDebug(UASParameterCommsMgrLog) << "UASParameterCommsMgr destructor: " << ptrStr ; } diff --git a/src/uas/UASParameterCommsMgr.h b/src/uas/UASParameterCommsMgr.h index 20a24bb12ba8733cee67cb174192fad66e52e599..bb68ebebd4bf6e049d5d260657967c873585a344 100644 --- a/src/uas/UASParameterCommsMgr.h +++ b/src/uas/UASParameterCommsMgr.h @@ -6,11 +6,12 @@ #include #include #include +#include class UASInterface; class UASParameterDataModel; - +Q_DECLARE_LOGGING_CATEGORY(UASParameterCommsMgrLog) class UASParameterCommsMgr : public QObject { @@ -122,6 +123,9 @@ private slots: /// @brief We signal this to ourselves in order to get timer started/stopped on our own thread. void _startSilenceTimerOnThisThread(void); void _stopSilenceTimerOnThisThread(void); + +private: + void _sendParamRequestListMsg(void); };