Merge pull request #3235 from DonLakeFlyer/VersionParameterRemapping

Version based parameter name remapping

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")
 

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<Fact*>();
+    return _mapParameterName2Variant[componentId][mappedParamName].value<Fact*>();
 }
 
 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;
+}

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<int, QMap<QString, int> >  _waitingReadParamNameMap;   ///< Key: Component id, Value: Map { Key: parameter name still waiting for, Value: retry count }
     QMap<int, QMap<QString, int> >  _waitingWriteParamNameMap;  ///< Key: Component id, Value: Map { Key: parameter name still waiting for, Value: retry count }
     QMap<int, QList<int> >          _failedReadParamIndexMap;   ///< Key: Component id, Value: failed parameter index
-    
+
     int _totalParamCount;   ///< Number of parameters across all components
     
     QTimer _initialRequestTimeoutTimer;

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -313,7 +313,7 @@ bool APMFirmwarePlugin::_handleStatusText(Vehicle* vehicle, mavlink_message_t* m
     mavlink_statustext_t statusText;
     mavlink_msg_statustext_decode(message, &statusText);
 
-    if (vehicle->firmwareMajorVersion() == -1 || statusText.severity < MAV_SEVERITY_NOTICE) {
+    if (vehicle->firmwareMajorVersion() == Vehicle::versionNotSetValue || statusText.severity < MAV_SEVERITY_NOTICE) {
         messageText = _getMessageText(message);
         qCDebug(APMFirmwarePluginLog) << messageText;
 

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)

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

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -209,3 +209,16 @@ 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
+{
+    static const remapParamNameMajorVersionMap_t remap;
+
+    return remap;
+}
+
+int FirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const
+{
+    Q_UNUSED(majorVersionNumber);
+    return 0;
+}

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<QString, QString> remapParamNameMap_t;
+
+    /// Maps from firmware minor version to remapParamNameMap_t entry
+    ///     key:    firmware minor version
+    ///     value:  remapParamNameMap_t entry
+    typedef QMap<int, remapParamNameMap_t> remapParamNameMinorVersionRemapMap_t;
+
+    /// Maps from firmware major version number to remapParamNameMinorVersionRemapMap_t entry
+    ///     key:    firmware major version
+    ///     value:  remapParamNameMinorVersionRemapMap_t entry
+    typedef QMap<int, remapParamNameMinorVersionRemapMap_t> 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

src/Vehicle/Vehicle.cc

@@ -121,9 +121,9 @@ Vehicle::Vehicle(LinkInterface*             link,
     , _messageSeq(0)
     , _compID(0)
     , _heardFrom(false)
-    , _firmwareMajorVersion(-1)
-    , _firmwareMinorVersion(-1)
-    , _firmwarePatchVersion(-1)
+    , _firmwareMajorVersion(versionNotSetValue)
+    , _firmwareMinorVersion(versionNotSetValue)
+    , _firmwarePatchVersion(versionNotSetValue)
     , _rollFact             (0, _rollFactName,              FactMetaData::valueTypeDouble)
     , _pitchFact            (0, _pitchFactName,             FactMetaData::valueTypeDouble)
     , _headingFact          (0, _headingFactName,           FactMetaData::valueTypeDouble)
@@ -291,9 +291,9 @@ Vehicle::Vehicle(QObject* parent)
     , _messageSeq(0)
     , _compID(0)
     , _heardFrom(false)
-    , _firmwareMajorVersion(-1)
-    , _firmwareMinorVersion(-1)
-    , _firmwarePatchVersion(-1)
+    , _firmwareMajorVersion(versionNotSetValue)
+    , _firmwareMinorVersion(versionNotSetValue)
+    , _firmwarePatchVersion(versionNotSetValue)
     , _rollFact             (0, _rollFactName,              FactMetaData::valueTypeDouble)
     , _pitchFact            (0, _pitchFactName,             FactMetaData::valueTypeDouble)
     , _headingFact          (0, _headingFactName,           FactMetaData::valueTypeDouble)

src/Vehicle/Vehicle.h

@@ -524,6 +524,7 @@ public:
     int firmwareMinorVersion(void) const { return _firmwareMinorVersion; }
     int firmwarePatchVersion(void) const { return _firmwarePatchVersion; }
     void setFirmwareVersion(int majorVersion, int minorVersion, int patchVersion);
+    static const int versionNotSetValue = -1;
 
 public slots:
     void setLatitude(double latitude);