Merge pull request #4224 from DonLakeFlyer/APMAccelCheck

Only depend on accel 1 for setup complete

src/AutoPilotPlugins/APM/APMSensorsComponent.cc

@@ -62,12 +62,6 @@ QStringList APMSensorsComponent::setupCompleteChangedTriggerList(void) const
     // Accelerometer triggers
     triggers << "INS_ACCOFFS_X" << "INS_ACCOFFS_Y" << "INS_ACCOFFS_Z";
 
-    if (_vehicle->parameterManager()->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;
 }
 
@@ -123,39 +117,18 @@ bool APMSensorsComponent::compassSetupNeeded(void) const
 
 bool APMSensorsComponent::accelSetupNeeded(void) const
 {
-    QStringList         rgUse;
-    QStringList         rgOffsets;
-    QList<QStringList>  rgAccels;
+    QStringList rgOffsets;
 
-    // We always at a minimum test the first accel
+    // The best we can do is test the first accel which will always be there. We don't have enough information to know
+    // whether any of the other accels are available.
     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 (_vehicle->parameterManager()->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 || _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, rgUse[i])->rawValue().toInt() != 0) {
-            for (int j=0; j<rgAccels[0].count(); j++) {
-                if (_vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, rgAccels[i][j])->rawValue().toFloat() == 0.0f) {
-                    return true;
-                }
-            }
+    int zeroCount = 0;
+    for (int i=0; i<rgOffsets.count(); i++) {
+        if (_vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, rgOffsets[i])->rawValue().toFloat() == 0.0f) {
+            zeroCount++;
         }
     }
 
-    return false;
+    return zeroCount == rgOffsets.count();
 }
-