diff --git a/src/AutoPilotPlugins/Common/RadioComponentController.cc b/src/AutoPilotPlugins/Common/RadioComponentController.cc index abf2e726b5e03fb8f4319e32f560ee24f5181acb..3ebc63be2dd09c7df1ded54d77cbd0f2fe1583ea 100644 --- a/src/AutoPilotPlugins/Common/RadioComponentController.cc +++ b/src/AutoPilotPlugins/Common/RadioComponentController.cc @@ -120,6 +120,10 @@ RadioComponentController::RadioComponentController(void) : connect(_vehicle, &Vehicle::rcChannelsChanged, this, &RadioComponentController::_rcChannelsChanged); _loadSettings(); + + // APM Stack has a bug where some RC params are missing. We need to know what these are so we can skip them if missing + // instead of popping missing param warnings. + _apmPossibleMissingRCChannelParams << 9 << 11 << 12 << 13 << 14; _resetInternalCalibrationValues(); } @@ -704,17 +708,20 @@ void RadioComponentController::_resetInternalCalibrationValues(void) QVariant value; enum rcCalFunctions curFunction = rgFlightModeFunctions[i]; - bool ok; - int switchChannel = getParameterFact(FactSystem::defaultComponentId, _functionInfo()[curFunction].parameterName)->rawValue().toInt(&ok); - Q_ASSERT(ok); + Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, _functionInfo()[curFunction].parameterName); + if (paramFact) { + bool ok; + int switchChannel = paramFact->rawValue().toInt(&ok); + Q_ASSERT(ok); - // Parameter: 1-based channel, 0=not mapped - // _rgFunctionChannelMapping: 0-based channel, _chanMax=not mapped + // Parameter: 1-based channel, 0=not mapped + // _rgFunctionChannelMapping: 0-based channel, _chanMax=not mapped - if (switchChannel != 0) { - qCDebug(RadioComponentControllerLog) << "Reserving 0-based switch channel" << switchChannel - 1; - _rgFunctionChannelMapping[curFunction] = switchChannel - 1; - _rgChannelInfo[switchChannel - 1].function = curFunction; + if (switchChannel != 0) { + qCDebug(RadioComponentControllerLog) << "Reserving 0-based switch channel" << switchChannel - 1; + _rgFunctionChannelMapping[curFunction] = switchChannel - 1; + _rgChannelInfo[switchChannel - 1].function = curFunction; + } } } } @@ -747,20 +754,43 @@ void RadioComponentController::_setInternalCalibrationValuesFromParameters(void) for (int i = 0; i < _chanMax(); ++i) { struct ChannelInfo* info = &_rgChannelInfo[i]; + + if (_px4Vehicle() && _apmPossibleMissingRCChannelParams.contains(i+1)) { + if (!parameterExists(FactSystem::defaultComponentId, minTpl.arg(i+1))) { + // Parameter is missing from this version of APM + info->rcTrim = 1500; + info->rcMin = 1100; + info->rcMax = 1900; + info->reversed = false; + continue; + } + } - info->rcTrim = getParameterFact(FactSystem::defaultComponentId, trimTpl.arg(i+1))->rawValue().toInt(&convertOk); - Q_ASSERT(convertOk); + Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, trimTpl.arg(i+1)); + if (paramFact) { + info->rcTrim = paramFact->rawValue().toInt(&convertOk); + Q_ASSERT(convertOk); + } - info->rcMin = getParameterFact(FactSystem::defaultComponentId, minTpl.arg(i+1))->rawValue().toInt(&convertOk); - Q_ASSERT(convertOk); + paramFact = getParameterFact(FactSystem::defaultComponentId, minTpl.arg(i+1)); + if (paramFact) { + info->rcMin = paramFact->rawValue().toInt(&convertOk); + Q_ASSERT(convertOk); + } - info->rcMax = getParameterFact(FactSystem::defaultComponentId, maxTpl.arg(i+1))->rawValue().toInt(&convertOk); - Q_ASSERT(convertOk); + paramFact = getParameterFact(FactSystem::defaultComponentId, maxTpl.arg(i+1)); + if (paramFact) { + info->rcMax = getParameterFact(FactSystem::defaultComponentId, maxTpl.arg(i+1))->rawValue().toInt(&convertOk); + Q_ASSERT(convertOk); + } - float floatReversed = getParameterFact(FactSystem::defaultComponentId, revTpl.arg(i+1))->rawValue().toFloat(&convertOk); - Q_ASSERT(convertOk); - Q_ASSERT(floatReversed == 1.0f || floatReversed == -1.0f); - info->reversed = floatReversed == -1.0f; + paramFact = getParameterFact(FactSystem::defaultComponentId, revTpl.arg(i+1)); + if (paramFact) { + float floatReversed = paramFact->rawValue().toFloat(&convertOk); + Q_ASSERT(convertOk); + Q_ASSERT(floatReversed == 1.0f || floatReversed == -1.0f); + info->reversed = floatReversed == -1.0f; + } } for (int i=0; irawValue().toInt(&convertOk); - Q_ASSERT(convertOk); - - if (paramChannel != 0) { - _rgFunctionChannelMapping[i] = paramChannel - 1; - _rgChannelInfo[paramChannel - 1].function = (enum rcCalFunctions)i; + Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, paramName); + if (paramFact) { + paramChannel = paramFact->rawValue().toInt(&convertOk); + Q_ASSERT(convertOk); + + if (paramChannel != 0) { + _rgFunctionChannelMapping[i] = paramChannel - 1; + _rgChannelInfo[paramChannel - 1].function = (enum rcCalFunctions)i; + } } } } @@ -851,9 +884,23 @@ void RadioComponentController::_writeCalibration(void) struct ChannelInfo* info = &_rgChannelInfo[chan]; int oneBasedChannel = chan + 1; - getParameterFact(FactSystem::defaultComponentId, trimTpl.arg(oneBasedChannel))->setRawValue((float)info->rcTrim); - getParameterFact(FactSystem::defaultComponentId, minTpl.arg(oneBasedChannel))->setRawValue((float)info->rcMin); - getParameterFact(FactSystem::defaultComponentId, maxTpl.arg(oneBasedChannel))->setRawValue((float)info->rcMax); + if (_px4Vehicle() && _apmPossibleMissingRCChannelParams.contains(chan+1) && !parameterExists(FactSystem::defaultComponentId, minTpl.arg(chan+1))) { + // RC parameters for this channel are missing from this version of APM + continue; + } + + Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, trimTpl.arg(oneBasedChannel)); + if (paramFact) { + paramFact->setRawValue((float)info->rcTrim); + } + paramFact = getParameterFact(FactSystem::defaultComponentId, minTpl.arg(oneBasedChannel)); + if (paramFact) { + paramFact->setRawValue((float)info->rcMin); + } + paramFact = getParameterFact(FactSystem::defaultComponentId, maxTpl.arg(oneBasedChannel)); + if (paramFact) { + paramFact->setRawValue((float)info->rcMax); + } // APM has a backwards interpretation of "reversed" on the Pitch control. So be careful. float reversedParamValue; @@ -862,7 +909,10 @@ void RadioComponentController::_writeCalibration(void) } else { reversedParamValue = info->reversed ? 1.0f : -1.0f; } - getParameterFact(FactSystem::defaultComponentId, revTpl.arg(oneBasedChannel))->setRawValue(reversedParamValue); + paramFact = getParameterFact(FactSystem::defaultComponentId, revTpl.arg(oneBasedChannel)); + if (paramFact) { + paramFact->setRawValue(reversedParamValue); + } } // Write function mapping parameters @@ -880,9 +930,12 @@ void RadioComponentController::_writeCalibration(void) if (paramName) { Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, _functionInfo()[i].parameterName); - if (paramFact->rawValue().toInt() != paramChannel) { + if (paramFact && paramFact->rawValue().toInt() != paramChannel) { functionMappingChanged = true; - getParameterFact(FactSystem::defaultComponentId, _functionInfo()[i].parameterName)->setRawValue(paramChannel); + paramFact = getParameterFact(FactSystem::defaultComponentId, _functionInfo()[i].parameterName); + if (paramFact) { + paramFact->setRawValue(paramChannel); + } } } } diff --git a/src/AutoPilotPlugins/Common/RadioComponentController.h b/src/AutoPilotPlugins/Common/RadioComponentController.h index 20a1f3ef6fe271a6c891f24906930fcd9b9519f5..21fdd0e7eaa3a92412f9dbc2a6e4dd15e3be1e9e 100644 --- a/src/AutoPilotPlugins/Common/RadioComponentController.h +++ b/src/AutoPilotPlugins/Common/RadioComponentController.h @@ -300,6 +300,8 @@ private: static const int _chanMinimum = 5; ///< Minimum numner of channels required to run struct ChannelInfo _rgChannelInfo[_chanMaxAny]; ///< Information associated with each rc channel + + QList _apmPossibleMissingRCChannelParams; ///< List of possible missing RC*_* params for APM stack enum rcCalStates _rcCalState; ///< Current calibration state int _rcCalStateCurrentChannel; ///< Current channel being worked on in rcCalStateIdentify and rcCalStateDetectInversion