From 4e1acddda88b875b346f095c3bb2411d7d49958f Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Fri, 15 Apr 2016 13:03:30 -0700 Subject: [PATCH] Add support for version based parameter remapping --- .../APM/APMTuningComponentCopter.qml | 8 +-- src/FactSystem/ParameterLoader.cc | 64 +++++++++++++++++-- src/FactSystem/ParameterLoader.h | 3 +- .../APM/ArduCopterFirmwarePlugin.cc | 35 ++++++++++ .../APM/ArduCopterFirmwarePlugin.h | 6 ++ src/FirmwarePlugin/FirmwarePlugin.cc | 11 ++++ src/FirmwarePlugin/FirmwarePlugin.h | 20 ++++++ 7 files changed, 136 insertions(+), 11 deletions(-) diff --git a/src/AutoPilotPlugins/APM/APMTuningComponentCopter.qml b/src/AutoPilotPlugins/APM/APMTuningComponentCopter.qml index e31e594b9..e3b6268e0 100644 --- a/src/AutoPilotPlugins/APM/APMTuningComponentCopter.qml +++ b/src/AutoPilotPlugins/APM/APMTuningComponentCopter.qml @@ -41,10 +41,10 @@ QGCView { property Fact _throttleMid: controller.getParameterFact(-1, "THR_MID") property Fact _rcFeel: controller.getParameterFact(-1, "RC_FEEL_RP") - property Fact _rateRollP: controller.getParameterFact(-1, "RATE_RLL_P") - property Fact _rateRollI: controller.getParameterFact(-1, "RATE_RLL_I") - property Fact _ratePitchP: controller.getParameterFact(-1, "RATE_PIT_P") - property Fact _ratePitchI: controller.getParameterFact(-1, "RATE_PIT_I") + property Fact _rateRollP: controller.getParameterFact(-1, "r.ATC_RAT_RLL_P") + property Fact _rateRollI: controller.getParameterFact(-1, "r.ATC_RAT_RLL_I") + property Fact _ratePitchP: controller.getParameterFact(-1, "r.ATC_RAT_PIT_P") + property Fact _ratePitchI: controller.getParameterFact(-1, "r.ATC_RAT_PIT_I") property Fact _rateClimbP: controller.getParameterFact(-1, "ACCEL_Z_P") property Fact _rateClimbI: controller.getParameterFact(-1, "ACCEL_Z_I") diff --git a/src/FactSystem/ParameterLoader.cc b/src/FactSystem/ParameterLoader.cc index 894dcbc19..d0435103d 100644 --- a/src/FactSystem/ParameterLoader.cc +++ b/src/FactSystem/ParameterLoader.cc @@ -416,8 +416,10 @@ void ParameterLoader::refreshParameter(int componentId, const QString& name) Q_ASSERT(_waitingReadParamNameMap.contains(componentId)); if (_waitingReadParamNameMap.contains(componentId)) { - _waitingReadParamNameMap[componentId].remove(name); // Remove old wait entry if there - _waitingReadParamNameMap[componentId][name] = 0; // Add new wait entry and update retry count + QString mappedParamName = _remapParamNameToVersion(name); + + _waitingReadParamNameMap[componentId].remove(mappedParamName); // Remove old wait entry if there + _waitingReadParamNameMap[componentId][mappedParamName] = 0; // Add new wait entry and update retry count emit restartWaitingParamTimer(); } @@ -444,7 +446,7 @@ bool ParameterLoader::parameterExists(int componentId, const QString& name) componentId = _actualComponentId(componentId); if (_mapParameterName2Variant.contains(componentId)) { - ret = _mapParameterName2Variant[componentId].contains(name); + ret = _mapParameterName2Variant[componentId].contains(_remapParamNameToVersion(name)); } return ret; @@ -454,12 +456,13 @@ Fact* ParameterLoader::getFact(int componentId, const QString& name) { componentId = _actualComponentId(componentId); - if (!_mapParameterName2Variant.contains(componentId) || !_mapParameterName2Variant[componentId].contains(name)) { - qgcApp()->reportMissingParameter(componentId, name); + QString mappedParamName = _remapParamNameToVersion(name); + if (!_mapParameterName2Variant.contains(componentId) || !_mapParameterName2Variant[componentId].contains(mappedParamName)) { + qgcApp()->reportMissingParameter(componentId, mappedParamName); return &_defaultFact; } - return _mapParameterName2Variant[componentId][name].value(); + return _mapParameterName2Variant[componentId][mappedParamName].value(); } QStringList ParameterLoader::parameterNames(int componentId) @@ -1163,3 +1166,52 @@ void ParameterLoader::cacheMetaDataFile(const QString& metaDataFile, MAV_AUTOPIL newFile.copy(cacheFile.fileName()); } } + +/// Remap a parameter from one firmware version to another +QString ParameterLoader::_remapParamNameToVersion(const QString& paramName) +{ + QString mappedParamName; + + if (!paramName.startsWith(QStringLiteral("r."))) { + // No version mapping wanted + return paramName; + } + + int majorVersion = _vehicle->firmwareMajorVersion(); + int minorVersion = _vehicle->firmwareMinorVersion(); + + qCDebug(ParameterLoaderLog) << "_remapParamNameToVersion" << paramName << majorVersion << minorVersion; + + mappedParamName = paramName.right(paramName.count() - 2); + + if (majorVersion == Vehicle::versionNotSetValue) { + // Vehicle version unknown + return mappedParamName; + } + + const FirmwarePlugin::remapParamNameMajorVersionMap_t& majorVersionRemap = _vehicle->firmwarePlugin()->paramNameRemapMajorVersionMap(); + + if (!majorVersionRemap.contains(majorVersion)) { + // No mapping for this major version + qCDebug(ParameterLoaderLog) << "_remapParamNameToVersion: no major version mapping"; + return mappedParamName; + } + + const FirmwarePlugin::remapParamNameMinorVersionRemapMap_t& remapMinorVersion = majorVersionRemap[majorVersion]; + + // We must map from the highest known minor version to one above the vehicle's minor version + + for (int currentMinorVersion=_vehicle->firmwarePlugin()->remapParamNameHigestMinorVersionNumber(majorVersion); currentMinorVersion>minorVersion; currentMinorVersion--) { + if (remapMinorVersion.contains(currentMinorVersion)) { + const FirmwarePlugin::remapParamNameMap_t& remap = remapMinorVersion[currentMinorVersion]; + + if (remap.contains(mappedParamName)) { + QString toParamName = remap[mappedParamName]; + qCDebug(ParameterLoaderLog) << "_remapParamNameToVersion: remapped currentMinor:from:to"<< currentMinorVersion << mappedParamName << toParamName; + mappedParamName = toParamName; + } + } + } + + return mappedParamName; +} diff --git a/src/FactSystem/ParameterLoader.h b/src/FactSystem/ParameterLoader.h index 43714083a..3b1e622c5 100644 --- a/src/FactSystem/ParameterLoader.h +++ b/src/FactSystem/ParameterLoader.h @@ -135,6 +135,7 @@ private: void _writeLocalParamCache(int uasId, int componentId); void _tryCacheHashLoad(int uasId, int componentId, QVariant hash_value); void _addMetaDataToDefaultComponent(void); + QString _remapParamNameToVersion(const QString& paramName); MAV_PARAM_TYPE _factTypeToMavType(FactMetaData::ValueType_t factType); FactMetaData::ValueType_t _mavTypeToFactType(MAV_PARAM_TYPE mavType); @@ -171,7 +172,7 @@ private: QMap > _waitingReadParamNameMap; ///< Key: Component id, Value: Map { Key: parameter name still waiting for, Value: retry count } QMap > _waitingWriteParamNameMap; ///< Key: Component id, Value: Map { Key: parameter name still waiting for, Value: retry count } QMap > _failedReadParamIndexMap; ///< Key: Component id, Value: failed parameter index - + int _totalParamCount; ///< Number of parameters across all components QTimer _initialRequestTimeoutTimer; diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index 48a469b31..b4906e97b 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -28,6 +28,9 @@ #include "QGCApplication.h" #include "MissionManager.h" +bool ArduCopterFirmwarePlugin::_remapParamNameIntialized = false; +FirmwarePlugin::remapParamNameMajorVersionMap_t ArduCopterFirmwarePlugin::_remapParamName; + APMCopterMode::APMCopterMode(uint32_t mode, bool settable) : APMCustomMode(mode, settable) { @@ -74,6 +77,38 @@ ArduCopterFirmwarePlugin::ArduCopterFirmwarePlugin(void) supportedFlightModes << APMCopterMode(APMCopterMode::POS_HOLD ,true); supportedFlightModes << APMCopterMode(APMCopterMode::BRAKE ,true); setSupportedModes(supportedFlightModes); + + if (!_remapParamNameIntialized) { + FirmwarePlugin::remapParamNameMap_t& remap = _remapParamName[3][4]; + + remap["ATC_ANG_RLL_P"] = QStringLiteral("STB_RLL_P"); + remap["ATC_ANG_PIT_P"] = QStringLiteral("STB_PIT_P"); + remap["ATC_ANG_YAW_P"] = QStringLiteral("STB_YAW_P"); + + remap["ATC_RAT_RLL_P"] = QStringLiteral("RATE_RLL_P"); + remap["ATC_RAT_RLL_I"] = QStringLiteral("RATE_RLL_I"); + remap["ATC_RAT_RLL_IMAX"] = QStringLiteral("RATE_RLL_IMAX"); + remap["ATC_RAT_RLL_D"] = QStringLiteral("RATE_RLL_D"); + remap["ATC_RAT_RLL_FILT"] = QStringLiteral("RATE_RLL_FILT_HZ"); + + remap["ATC_RAT_PIT_P"] = QStringLiteral("RATE_PIT_P"); + remap["ATC_RAT_PIT_I"] = QStringLiteral("RATE_PIT_I"); + remap["ATC_RAT_PIT_IMAX"] = QStringLiteral("RATE_PIT_IMAX"); + remap["ATC_RAT_PIT_D"] = QStringLiteral("RATE_PIT_D"); + remap["ATC_RAT_PIT_FILT"] = QStringLiteral("RATE_PIT_FILT_HZ"); + + remap["ATC_RAT_YAW_P"] = QStringLiteral("RATE_YAW_P"); + remap["ATC_RAT_YAW_I"] = QStringLiteral("RATE_YAW_I"); + remap["ATC_RAT_YAW_IMAX"] = QStringLiteral("RATE_YAW_IMAX"); + remap["ATC_RAT_YAW_D"] = QStringLiteral("RATE_YAW_D"); + remap["ATC_RAT_YAW_FILT"] = QStringLiteral("RATE_YAW_FILT_HZ"); + } +} + +int ArduCopterFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const +{ + // Remapping supports up to 3.4 + return majorVersionNumber == 3 ? 4: Vehicle::versionNotSetValue; } bool ArduCopterFirmwarePlugin::isCapable(FirmwareCapabilities capabilities) diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h index c4555d0cf..b806c19cb 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h @@ -75,6 +75,12 @@ public: void guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) final; void guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final; void guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel) final; + const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; } + virtual int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final; + +private: + static bool _remapParamNameIntialized; + static FirmwarePlugin::remapParamNameMajorVersionMap_t _remapParamName; }; #endif diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 825a8a717..18c5ea97a 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -209,3 +209,14 @@ void FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeR Q_UNUSED(altitudeRel); qgcApp()->showMessage(QStringLiteral("Guided mode not supported by Vehicle.")); } + +const FirmwarePlugin::remapParamNameMajorVersionMap_t& FirmwarePlugin::paramNameRemapMajorVersionMap(void) const +{ + return remapParamNameMajorVersionMap_t(); +} + +int FirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const +{ + Q_UNUSED(majorVersionNumber); + return 0; +} diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 6b6787bb2..b2afe94de 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -55,6 +55,21 @@ public: PauseVehicleCapability = 1 << 2, ///< Vehicle supports pausing at current location GuidedModeCapability = 1 << 3, ///< Vehicle Support guided mode commands } FirmwareCapabilities; + + /// Maps from on parameter name to another + /// key: parameter name to translate from + /// value: mapped parameter name + typedef QMap remapParamNameMap_t; + + /// Maps from firmware minor version to remapParamNameMap_t entry + /// key: firmware minor version + /// value: remapParamNameMap_t entry + typedef QMap remapParamNameMinorVersionRemapMap_t; + + /// Maps from firmware major version number to remapParamNameMinorVersionRemapMap_t entry + /// key: firmware major version + /// value: remapParamNameMinorVersionRemapMap_t entry + typedef QMap remapParamNameMajorVersionMap_t; /// Called when Vehicle is first created to send any necessary mavlink messages to the firmware. virtual void initializeVehicle(Vehicle* vehicle); @@ -171,6 +186,11 @@ public: /// @param[out] multiRotorJsonFilename Filename for multi rotor overrides virtual void missionCommandOverrides(QString& commonJsonFilename, QString& fixedWingJsonFilename, QString& multiRotorJsonFilename) const; + /// Returns the mapping structure which is used to map from one parameter name to another based on firmware version. + virtual const remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const; + + /// Returns the highest major version number that is known to the remap for this specified major version. + virtual int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const; }; #endif -- 2.22.0