diff --git a/src/FactSystem/FactMetaData.cc b/src/FactSystem/FactMetaData.cc index dc79a8fb7dd74172080e8e320cc7b8977946361d..b1c3161cb7b52d58529a5c0007345ab6a363f8d0 100644 --- a/src/FactSystem/FactMetaData.cc +++ b/src/FactSystem/FactMetaData.cc @@ -434,3 +434,28 @@ FactMetaData::ValueType_t FactMetaData::stringToType(const QString& typeString, return valueTypeDouble; } + +size_t FactMetaData::typeToSize(ValueType_t type) +{ + switch (type) { + case valueTypeUint8: + case valueTypeInt8: + return 1; + + case valueTypeUint16: + case valueTypeInt16: + return 2; + + case valueTypeUint32: + case valueTypeInt32: + case valueTypeFloat: + return 4; + + case valueTypeDouble: + return 8; + + default: + qWarning() << "Unsupported fact value type" << type; + return 4; + } +} diff --git a/src/FactSystem/FactMetaData.h b/src/FactSystem/FactMetaData.h index 1a1c92a2372b512976147e34408c6693f697d8dd..9a3661f3fa8e13a3abf1b92cd20c517c7a753890 100644 --- a/src/FactSystem/FactMetaData.h +++ b/src/FactSystem/FactMetaData.h @@ -121,6 +121,7 @@ public: static const int defaultDecimalPlaces = 3; static ValueType_t stringToType(const QString& typeString, bool& unknownType); + static size_t typeToSize(ValueType_t type); private: QVariant _minForType(void) const; diff --git a/src/FactSystem/ParameterLoader.cc b/src/FactSystem/ParameterLoader.cc index 74152027a3bd455869e9566399a406e75136d923..c07f40e3e3d384dad866f964dc9d3c61e6ca0ade 100644 --- a/src/FactSystem/ParameterLoader.cc +++ b/src/FactSystem/ParameterLoader.cc @@ -72,14 +72,11 @@ ParameterLoader::ParameterLoader(AutoPilotPlugin* autopilot, Vehicle* vehicle, Q _waitingParamTimeoutTimer.setInterval(1000); connect(&_waitingParamTimeoutTimer, &QTimer::timeout, this, &ParameterLoader::_waitingParamTimeout); - _cacheTimeoutTimer.setSingleShot(true); - _cacheTimeoutTimer.setInterval(2500); - connect(&_cacheTimeoutTimer, &QTimer::timeout, this, &ParameterLoader::_timeoutRefreshAll); - connect(_vehicle->uas(), &UASInterface::parameterUpdate, this, &ParameterLoader::_parameterUpdate); - /* Initially attempt a local cache load, refresh over the link if it fails */ - _tryCacheLookup(); + // Ensure the cache directory exists + QFileInfo(QSettings().fileName()).dir().mkdir("ParamCache"); + refreshAllParameters(); } ParameterLoader::~ParameterLoader() @@ -119,8 +116,7 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param if (parameterName == "_HASH_CHECK") { /* we received a cache hash, potentially load from cache */ - _cacheTimeoutTimer.stop(); - _tryCacheHashLoad(uasId, value); + _tryCacheHashLoad(uasId, componentId, value); return; } _dataMutex.lock(); @@ -269,7 +265,7 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param if (waitingParamCount == 0) { // Now that we know vehicle is up to date persist _saveToEEPROM(); - _writeLocalParamCache(); + _writeLocalParamCache(uasId, componentId); } _checkInitialLoadComplete(); @@ -522,19 +518,6 @@ Out: } } -void ParameterLoader::_tryCacheLookup() -{ - /* Start waiting for 2.5 seconds to get a cache hit and avoid loading all params over the radio */ - _cacheTimeoutTimer.start(); - - MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); - Q_ASSERT(mavlink); - - mavlink_message_t msg; - mavlink_msg_param_request_read_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, _vehicle->id(), MAV_COMP_ID_ALL, "_HASH_CHECK", -1); - _vehicle->sendMessageOnLink(_dedicatedLink, msg); -} - void ParameterLoader::_readParameterRaw(int componentId, const QString& paramName, int paramIndex) { mavlink_message_t msg; @@ -604,40 +587,42 @@ void ParameterLoader::_writeParameterRaw(int componentId, const QString& paramNa _vehicle->sendMessageOnLink(_dedicatedLink, msg); } -void ParameterLoader::_writeLocalParamCache() +void ParameterLoader::_writeLocalParamCache(int uasId, int componentId) { - QMap cache_map; + MapID2NamedParam cache_map; - foreach(int component, _mapParameterId2Name.keys()) { - foreach(int id, _mapParameterId2Name[component].keys()) { - const QString name(_mapParameterId2Name[component][id]); - const Fact *fact = _mapParameterName2Variant[component][name].value(); - cache_map[component][id] = NamedParam(name, ParamTypeVal(fact->type(), fact->rawValue())); - } + foreach(int id, _mapParameterId2Name[componentId].keys()) { + const QString name(_mapParameterId2Name[componentId][id]); + const Fact *fact = _mapParameterName2Variant[componentId][name].value(); + cache_map[id] = NamedParam(name, ParamTypeVal(fact->type(), fact->rawValue())); } - QFile cache_file(QFileInfo(QSettings().fileName()).path() + QDir::separator() + "param_cache"); + QFile cache_file(parameterCacheFile(uasId, componentId)); cache_file.open(QIODevice::WriteOnly | QIODevice::Truncate); QDataStream ds(&cache_file); ds << cache_map; } -QString ParameterLoader::parameterCacheFile(void) +QDir ParameterLoader::parameterCacheDir() { - const QDir settingsDir(QFileInfo(QSettings().fileName()).dir()); - return settingsDir.filePath("param_cache"); + const QString spath(QFileInfo(QSettings().fileName()).dir().absolutePath()); + return spath + QDir::separator() + "ParamCache"; } -void ParameterLoader::_tryCacheHashLoad(int uasId, QVariant hash_value) +QString ParameterLoader::parameterCacheFile(int uasId, int componentId) +{ + return parameterCacheDir().filePath(QString("%1_%2").arg(uasId).arg(componentId)); +} + +void ParameterLoader::_tryCacheHashLoad(int uasId, int componentId, QVariant hash_value) { uint32_t crc32_value = 0; /* The datastructure of the cache table */ - QMap cache_map; - QFile cache_file(parameterCacheFile()); + MapID2NamedParam cache_map; + QFile cache_file(parameterCacheFile(uasId, componentId)); if (!cache_file.exists()) { - /* no local cache, immediately refresh all params */ - refreshAllParameters(); + /* no local cache, just wait for them to come in*/ return; } cache_file.open(QIODevice::ReadOnly); @@ -647,29 +632,38 @@ void ParameterLoader::_tryCacheHashLoad(int uasId, QVariant hash_value) ds >> cache_map; /* compute the crc of the local cache to check against the remote */ - foreach(int component, cache_map.keys()) { - foreach(int id, cache_map[component].keys()) { - const QString name(cache_map[component][id].first); - const void *vdat = cache_map[component][id].second.second.constData(); - crc32_value = QGC::crc32((const uint8_t *)qPrintable(name), name.length(), crc32_value); - crc32_value = QGC::crc32((const uint8_t *)vdat, sizeof(uint32_t), crc32_value); - } + + foreach(int id, cache_map.keys()) { + const QString name(cache_map[id].first); + const void *vdat = cache_map[id].second.second.constData(); + const FactMetaData::ValueType_t fact_type = static_cast(cache_map[id].second.first); + crc32_value = QGC::crc32((const uint8_t *)qPrintable(name), name.length(), crc32_value); + crc32_value = QGC::crc32((const uint8_t *)vdat, FactMetaData::typeToSize(fact_type), crc32_value); } if (crc32_value == hash_value.toUInt()) { + qCInfo(ParameterLoaderLog) << "Parameters loaded from cache" << qPrintable(QFileInfo(cache_file).absoluteFilePath()); /* if the two param set hashes match, just load from the disk */ - foreach(int component, cache_map.keys()) { - int count = cache_map[component].count(); - foreach(int id, cache_map[component].keys()) { - const QString &name = cache_map[component][id].first; - const QVariant &value = cache_map[component][id].second.second; - const int mavType = _factTypeToMavType(static_cast(cache_map[component][id].second.first)); - _parameterUpdate(uasId, component, name, count, id, mavType, value); - } + int count = cache_map.count(); + foreach(int id, cache_map.keys()) { + const QString &name = cache_map[id].first; + const QVariant &value = cache_map[id].second.second; + const FactMetaData::ValueType_t fact_type = static_cast(cache_map[id].second.first); + const int mavType = _factTypeToMavType(fact_type); + _parameterUpdate(uasId, componentId, name, count, id, mavType, value); } - } else { - /* cache and remote hashes differ. Immediately request all params */ - refreshAllParameters(); + // Return the hash value to notify we don't want any more updates + mavlink_param_set_t p; + mavlink_param_union_t union_value; + p.param_type = MAV_PARAM_TYPE_UINT32; + strncpy(p.param_id, "_HASH_CHECK", sizeof(p.param_id)); + union_value.param_uint32 = crc32_value; + p.param_value = union_value.param_float; + p.target_system = (uint8_t)_vehicle->id(); + p.target_component = (uint8_t)componentId; + mavlink_message_t msg; + mavlink_msg_param_set_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &p); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); } } @@ -901,9 +895,3 @@ void ParameterLoader::_initialRequestTimeout(void) refreshAllParameters(); _initialRequestTimeoutTimer.start(); } - -void ParameterLoader::_timeoutRefreshAll() -{ - refreshAllParameters(); -} - diff --git a/src/FactSystem/ParameterLoader.h b/src/FactSystem/ParameterLoader.h index 7c28709e1f3ca520e2ed046277314b9e95628e3b..48638d51a0dfec41dfe7efb9faa4e5b4a70c47f0 100644 --- a/src/FactSystem/ParameterLoader.h +++ b/src/FactSystem/ParameterLoader.h @@ -53,8 +53,11 @@ public: ~ParameterLoader(); + /// @return Directory of parameter caches + static QDir parameterCacheDir(); + /// @return Location of parameter cache file - static QString parameterCacheFile(void); + static QString parameterCacheFile(int uasId, int componentId); /// Returns true if the full set of facts are ready bool parametersAreReady(void) { return _parametersReady; } @@ -109,9 +112,6 @@ protected: void _waitingParamTimeout(void); void _tryCacheLookup(void); void _initialRequestTimeout(void); - -private slots: - void _timeoutRefreshAll(); private: static QVariant _stringToTypedVariant(const QString& string, FactMetaData::ValueType_t type, bool failOk = false); @@ -120,8 +120,8 @@ private: void _setupGroupMap(void); void _readParameterRaw(int componentId, const QString& paramName, int paramIndex); void _writeParameterRaw(int componentId, const QString& paramName, const QVariant& value); - void _writeLocalParamCache(); - void _tryCacheHashLoad(int uasId, QVariant hash_value); + void _writeLocalParamCache(int uasId, int componentId); + void _tryCacheHashLoad(int uasId, int componentId, QVariant hash_value); MAV_PARAM_TYPE _factTypeToMavType(FactMetaData::ValueType_t factType); FactMetaData::ValueType_t _mavTypeToFactType(MAV_PARAM_TYPE mavType); @@ -157,7 +157,6 @@ private: QTimer _initialRequestTimeoutTimer; QTimer _waitingParamTimeoutTimer; - QTimer _cacheTimeoutTimer; QMutex _dataMutex; diff --git a/src/MissionEditor/MissionItemStatus.qml b/src/MissionEditor/MissionItemStatus.qml index dd75fdfb49214a5642d0b1b5b21c9a24e7abd53b..c2cf3d787b6696714b9fee1d135477cf207009cd 100644 --- a/src/MissionEditor/MissionItemStatus.qml +++ b/src/MissionEditor/MissionItemStatus.qml @@ -34,7 +34,7 @@ Rectangle { property real expandedWidth ///< Width of control when expanded width: _expanded ? expandedWidth : _collapsedWidth - height: expandLabel.y + expandLabel.height + _margins + height: azimuthLabel.y + azimuthLabel.height + _margins radius: ScreenTools.defaultFontPixelWidth color: qgcPal.window opacity: 0.80 @@ -44,13 +44,16 @@ Rectangle { property real _collapsedWidth: distanceLabel.width + (margins * 2) property bool _expanded: true - property real _distance: _currentMissionItem ? _currentMissionItem.distance : -1 - property real _altDifference: _currentMissionItem ? _currentMissionItem.altDifference : -1 - property real _azimuth: _currentMissionItem ? _currentMissionItem.azimuth : -1 - property real _isHomePosition: _currentMissionItem ? _currentMissionItem.homePosition : false - property bool _statusValid: _distance != -1 - property string _distanceText: _statusValid ? Math.round(_distance) + " meters" : "" - property string _altText: _statusValid ? Math.round(_altDifference) + " meters" : "" + property real _distance: _statusValid ? _currentMissionItem.distance : 0 + property real _altDifference: _statusValid ? _currentMissionItem.altDifference : 0 + property real _gradient: _statusValid ? Math.atan(_currentMissionItem.altDifference / _currentMissionItem.distance) : 0 + property real _gradientPercent: isNaN(_gradient) ? 0 : _gradient * 100 + property real _azimuth: _statusValid ? _currentMissionItem.azimuth : -1 + property real _isHomePosition: _statusValid ? _currentMissionItem.homePosition : false + property bool _statusValid: currentMissionItem != undefined + property string _distanceText: _statusValid ? _distance.toFixed(2) + " m" : "" + property string _altText: _statusValid ? _altDifference.toFixed(2) + " m" : "" + property string _gradientText: _statusValid ? _gradientPercent.toFixed(0) + "%" : "" property string _azimuthText: _statusValid ? Math.round(_azimuth) : "" readonly property real _margins: ScreenTools.defaultFontPixelWidth @@ -76,17 +79,17 @@ Rectangle { } QGCLabel { - id: azimuthLabel - anchors.left: altLabel.left + id: gradientLabel + anchors.left: distanceLabel.left anchors.top: altLabel.bottom - text: "Azimuth: " + _azimuthText + text: "Gradient: " + _gradientText } QGCLabel { - id: expandLabel - anchors.left: azimuthLabel.left - anchors.top: azimuthLabel.bottom - text: _expanded ? "<<" : ">>" + id: azimuthLabel + anchors.left: distanceLabel.left + anchors.top: gradientLabel.bottom + text: "Azimuth: " + _azimuthText } QGCFlickable { @@ -128,12 +131,15 @@ Rectangle { visible: object.relativeAltitude ? true : (object.homePosition || graphAbsolute) } + /* + Taking these off for now since there really isn't room for the numbers QGCLabel { anchors.bottom: parent.bottom anchors.horizontalCenter: parent.horizontalCenter font.pixelSize: ScreenTools.smallFontPixelSize - text: (object.relativeAltitude ? "" : "=") + object.coordinate.altitude + text: (object.relativeAltitude ? "" : "=") + object.coordinate.altitude.toFixed(0) } + */ } } diff --git a/src/MissionManager/MavCmdInfoCommon.json b/src/MissionManager/MavCmdInfoCommon.json index 74f83dde495ccf32db0db661ddadecba877c1e8c..1fe6f851f46e21b0ddbc241f5c80618e14939ac6 100644 --- a/src/MissionManager/MavCmdInfoCommon.json +++ b/src/MissionManager/MavCmdInfoCommon.json @@ -542,6 +542,7 @@ "friendlyName": "Land start", "description": "Marker to indicate start of landing sequence.", "specifiesCoordinate": true, + "standaloneCoordinate": true, "friendlyEdit": true, "category": "Basic" }, diff --git a/src/MissionManager/MissionCommandList.cc b/src/MissionManager/MissionCommandList.cc index 2adfabee0280785022e7214e97ad7ffd74bba989..d6429ddabaad315f2d476093d623d0126f8989f2 100644 --- a/src/MissionManager/MissionCommandList.cc +++ b/src/MissionManager/MissionCommandList.cc @@ -82,7 +82,7 @@ void MissionCommandList::_loadMavCmdInfoJson(const QString& jsonFilename) QJsonParseError jsonParseError; QJsonDocument doc = QJsonDocument::fromJson(bytes, &jsonParseError); if (jsonParseError.error != QJsonParseError::NoError) { - qWarning() << "Unable to open json document" << jsonParseError.errorString(); + qWarning() << jsonFilename << "Unable to open json document" << jsonParseError.errorString(); return; } @@ -90,20 +90,20 @@ void MissionCommandList::_loadMavCmdInfoJson(const QString& jsonFilename) int version = json.value(_versionJsonKey).toInt(); if (version != 1) { - qWarning() << "Invalid version" << version; + qWarning() << jsonFilename << "Invalid version" << version; return; } QJsonValue jsonValue = json.value(_mavCmdInfoJsonKey); if (!jsonValue.isArray()) { - qWarning() << "mavCmdInfo not array"; + qWarning() << jsonFilename << "mavCmdInfo not array"; return; } QJsonArray jsonArray = jsonValue.toArray(); foreach(QJsonValue info, jsonArray) { if (!info.isObject()) { - qWarning() << "mavCmdArray should contain objects"; + qWarning() << jsonFilename << "mavCmdArray should contain objects"; return; } QJsonObject jsonObject = info.toObject(); @@ -113,7 +113,7 @@ void MissionCommandList::_loadMavCmdInfoJson(const QString& jsonFilename) QStringList requiredKeys; requiredKeys << _idJsonKey << _rawNameJsonKey; if (!JsonHelper::validateRequiredKeys(jsonObject, requiredKeys, errorString)) { - qWarning() << errorString; + qWarning() << jsonFilename << errorString; return; } @@ -126,7 +126,7 @@ void MissionCommandList::_loadMavCmdInfoJson(const QString& jsonFilename) types << QJsonValue::Double << QJsonValue::String << QJsonValue::String<< QJsonValue::String << QJsonValue::Bool << QJsonValue::Bool << QJsonValue::Bool << QJsonValue::Object << QJsonValue::Object << QJsonValue::Object << QJsonValue::Object << QJsonValue::String; if (!JsonHelper::validateKeyTypes(jsonObject, keys, types, errorString)) { - qWarning() << errorString; + qWarning() << jsonFilename << errorString; return; } @@ -152,7 +152,7 @@ void MissionCommandList::_loadMavCmdInfoJson(const QString& jsonFilename) << mavCmdInfo->_friendlyEdit; if (_mavCmdInfoMap.contains((MAV_CMD)mavCmdInfo->command())) { - qWarning() << "Duplicate command" << mavCmdInfo->command(); + qWarning() << jsonFilename << "Duplicate command" << mavCmdInfo->command(); return; } @@ -172,14 +172,14 @@ void MissionCommandList::_loadMavCmdInfoJson(const QString& jsonFilename) keys << _defaultJsonKey << _decimalPlacesJsonKey << _enumStringsJsonKey << _enumValuesJsonKey << _labelJsonKey << _unitsJsonKey; types << QJsonValue::Double << QJsonValue::Double << QJsonValue::String << QJsonValue::String << QJsonValue::String << QJsonValue::String; if (!JsonHelper::validateKeyTypes(jsonObject, keys, types, errorString)) { - qWarning() << errorString; + qWarning() << jsonFilename << errorString; return; } mavCmdInfo->_friendlyEdit = true; // Assume friendly edit if we have params if (!paramObject.contains(_labelJsonKey)) { - qWarning() << "param object missing label key" << mavCmdInfo->rawName() << paramKey; + qWarning() << jsonFilename << "param object missing label key" << mavCmdInfo->rawName() << paramKey; return; } @@ -198,14 +198,14 @@ void MissionCommandList::_loadMavCmdInfoJson(const QString& jsonFilename) double value = enumValue.toDouble(&convertOk); if (!convertOk) { - qWarning() << "Bad enumValue" << enumValue; + qWarning() << jsonFilename << "Bad enumValue" << enumValue; return; } paramInfo->_enumValues << QVariant(value); } if (paramInfo->_enumValues.count() != paramInfo->_enumStrings.count()) { - qWarning() << "enum strings/values count mismatch" << paramInfo->_enumStrings.count() << paramInfo->_enumValues.count(); + qWarning() << jsonFilename << "enum strings/values count mismatch" << paramInfo->_enumStrings.count() << paramInfo->_enumValues.count(); return; } @@ -224,11 +224,11 @@ void MissionCommandList::_loadMavCmdInfoJson(const QString& jsonFilename) if (mavCmdInfo->friendlyEdit()) { if (mavCmdInfo->description().isEmpty()) { - qWarning() << "Missing description" << mavCmdInfo->rawName(); + qWarning() << jsonFilename << "Missing description" << mavCmdInfo->rawName(); return; } if (mavCmdInfo->rawName() == mavCmdInfo->friendlyName()) { - qWarning() << "Missing friendly name" << mavCmdInfo->rawName() << mavCmdInfo->friendlyName(); + qWarning() << jsonFilename << "Missing friendly name" << mavCmdInfo->rawName() << mavCmdInfo->friendlyName(); return; } } diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index dddcfdd0e8c1f27e4554c082f406a4ffbb8cf57f..69e6b7322d671536a975fd2354bfc72e76a9c947 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -778,7 +778,8 @@ bool MissionController::_findLastAltitude(double* lastAltitude) bool found = false; double foundAltitude; - for (int i=0; i<_missionItems->count(); i++) { + // Don't use home position + for (int i=1; i<_missionItems->count(); i++) { MissionItem* item = qobject_cast(_missionItems->get(i)); if (item->specifiesCoordinate() && !item->standaloneCoordinate()) { diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index 72bf6b06c8b1c89951fd0d1ecf6249fc5a6cbdb7..ca6bdc36487176108f7a0986585e8b14cd87d949 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -374,7 +374,9 @@ QGCApplication::QGCApplication(int &argc, char* argv[], bool unitTesting) QFile::remove(PX4AirframeLoader::aiframeMetaDataFile()); QFile::remove(PX4ParameterMetaData::parameterMetaDataFile()); - QFile::remove(ParameterLoader::parameterCacheFile()); + QDir paramDir(ParameterLoader::parameterCacheDir()); + paramDir.removeRecursively(); + paramDir.mkpath(paramDir.absolutePath()); } _lastKnownHomePosition.setLatitude(settings.value(_lastKnownHomePositionLatKey, 37.803784).toDouble());