Solo param usage fixes (#3663)

* Deal with missing parameters on Solo

* Fix trigger set

src/AutoPilotPlugins/APM/APMSensorsComponent.cc

@@ -62,6 +62,12 @@ QStringList APMSensorsComponent::setupCompleteChangedTriggerList(void) const
     // Accelerometer triggers
     triggers << "INS_ACCOFFS_X" << "INS_ACCOFFS_Y" << "INS_ACCOFFS_Z";
 
+    if (_autopilot->parameterExists(FactSystem::defaultComponentId, QStringLiteral("INS_USE"))) {
+        triggers << QStringLiteral("INS_USE") << QStringLiteral("INS_USE2") << QStringLiteral("INS_USE3");
+        triggers << QStringLiteral("INS_ACC2OFFS_X") << QStringLiteral("INS_ACC2OFFS_Y") << QStringLiteral("INS_ACC2OFFS_Z");
+        triggers << QStringLiteral("INS_ACC3OFFS_X") << QStringLiteral("INS_ACC3OFFS_Y") << QStringLiteral("INS_ACC3OFFS_Z");
+    }
+
     return triggers;
 }
 
@@ -117,20 +123,33 @@ bool APMSensorsComponent::compassSetupNeeded(void) const
 
 bool APMSensorsComponent::accelSetupNeeded(void) const
 {
-    const size_t    cAccel = 3;
-    const size_t    cOffset = 3;
-    QStringList     rgUse;
-    QStringList     rgOffsets[cAccel];
-
-    rgUse << QStringLiteral("INS_USE") << QStringLiteral("INS_USE2") << QStringLiteral("INS_USE3");
-    rgOffsets[0] << QStringLiteral("INS_ACCOFFS_X") << QStringLiteral("INS_ACCOFFS_Y") << QStringLiteral("INS_ACCOFFS_Z");
-    rgOffsets[1] << QStringLiteral("INS_ACC2OFFS_X") << QStringLiteral("INS_ACC2OFFS_Y") << QStringLiteral("INS_ACC2OFFS_Z");
-    rgOffsets[2] << QStringLiteral("INS_ACC3OFFS_X") << QStringLiteral("INS_ACC3OFFS_Y") << QStringLiteral("INS_ACC3OFFS_Z");
-
-    for (size_t i=0; i<cAccel; i++) {
-        if (_autopilot->getParameterFact(FactSystem::defaultComponentId, rgUse[i])->rawValue().toInt() != 0) {
-            for (size_t j=0; j<cOffset; j++) {
-                if (_autopilot->getParameterFact(FactSystem::defaultComponentId, rgOffsets[i][j])->rawValue().toFloat() == 0.0f) {
+    QStringList         rgUse;
+    QStringList         rgOffsets;
+    QList<QStringList>  rgAccels;
+
+    // We always at a minimum test the first accel
+    rgOffsets << QStringLiteral("INS_ACCOFFS_X") << QStringLiteral("INS_ACCOFFS_Y") << QStringLiteral("INS_ACCOFFS_Z");
+    rgAccels << rgOffsets;
+    rgOffsets.clear();
+
+    // This parameter is not available in all firmware version. Specifically missing from older Solo firmware.
+    if (_autopilot->parameterExists(FactSystem::defaultComponentId, QStringLiteral("INS_USE"))) {
+        rgUse << QStringLiteral("INS_USE") << QStringLiteral("INS_USE2") << QStringLiteral("INS_USE3");
+
+        // We have usage information for the remaining accels, so we can test them sa well
+        rgOffsets << QStringLiteral("INS_ACC2OFFS_X") << QStringLiteral("INS_ACC2OFFS_Y") << QStringLiteral("INS_ACC2OFFS_Z");
+        rgAccels << rgOffsets;
+        rgOffsets.clear();
+
+        rgOffsets << QStringLiteral("INS_ACC3OFFS_X") << QStringLiteral("INS_ACC3OFFS_Y") << QStringLiteral("INS_ACC3OFFS_Z");
+        rgAccels << rgOffsets;
+        rgOffsets.clear();
+    }
+
+    for (int i=0; i<rgAccels.count(); i++) {
+        if (rgUse.count() == 0 || _autopilot->getParameterFact(FactSystem::defaultComponentId, rgUse[i])->rawValue().toInt() != 0) {
+            for (int j=0; j<rgAccels[0].count(); j++) {
+                if (_autopilot->getParameterFact(FactSystem::defaultComponentId, rgAccels[i][j])->rawValue().toFloat() == 0.0f) {
                     return true;
                 }
             }

src/FactSystem/ParameterLoader.cc

@@ -1196,7 +1196,7 @@ QString ParameterLoader::_remapParamNameToVersion(const QString& paramName)
 
     const FirmwarePlugin::remapParamNameMinorVersionRemapMap_t& remapMinorVersion = majorVersionRemap[majorVersion];
 
-    // We must map from the highest known minor version to one above the vehicle's minor version
+    // We must map backwards 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)) {

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -373,6 +373,7 @@ bool APMFirmwarePlugin::_handleStatusText(Vehicle* vehicle, mavlink_message_t* m
 
     if (messageText.contains(APM_SOLO_REXP)) {
         qDebug() << "Found Solo";
+        vehicle->setSoloFirmware(true);
 
         // Fix up severity
         _setInfoSeverity(message);

src/Vehicle/Vehicle.cc

@@ -67,6 +67,8 @@ Vehicle::Vehicle(LinkInterface*             link,
     , _vehicleType(vehicleType)
     , _firmwarePlugin(NULL)
     , _autopilotPlugin(NULL)
+    , _mavlink(NULL)
+    , _soloFirmware(false)
     , _joystickMode(JoystickModeRC)
     , _joystickEnabled(false)
     , _uas(NULL)
@@ -1697,6 +1699,14 @@ int Vehicle::defaultComponentId(void)
     return _parameterLoader->defaultComponenentId();
 }
 
+void Vehicle::setSoloFirmware(bool soloFirmware)
+{
+    if (soloFirmware != _soloFirmware) {
+        _soloFirmware = soloFirmware;
+        emit soloFirmwareChanged(soloFirmware);
+    }
+}
+
 const char* VehicleGPSFactGroup::_hdopFactName =                "hdop";
 const char* VehicleGPSFactGroup::_vdopFactName =                "vdop";
 const char* VehicleGPSFactGroup::_courseOverGroundFactName =    "courseOverGround";