diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponent.cc b/src/AutoPilotPlugins/APM/APMSensorsComponent.cc index e5933b554142c6040528b8616ef5f3b8df8aea1a..0efdf4e6ac5ad2616693ae39f61b913c93b52502 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponent.cc +++ b/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 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; iparameterManager()->getParameter(FactSystem::defaultComponentId, rgUse[i])->rawValue().toInt() != 0) { - for (int j=0; jparameterManager()->getParameter(FactSystem::defaultComponentId, rgAccels[i][j])->rawValue().toFloat() == 0.0f) { - return true; - } - } + int zeroCount = 0; + for (int i=0; iparameterManager()->getParameter(FactSystem::defaultComponentId, rgOffsets[i])->rawValue().toFloat() == 0.0f) { + zeroCount++; } } - return false; + return zeroCount == rgOffsets.count(); } -