diff --git a/src/AutoPilotPlugins/PX4/RadioComponent.cc b/src/AutoPilotPlugins/PX4/RadioComponent.cc index 338518c7280ef519f68921a3069649d7b4786282..1a3536d8827dff5f7421945bd2a8ae79aa878ed0 100644 --- a/src/AutoPilotPlugins/PX4/RadioComponent.cc +++ b/src/AutoPilotPlugins/PX4/RadioComponent.cc @@ -28,9 +28,6 @@ #include "PX4RCCalibration.h" #include "PX4AutoPilotPlugin.h" -/// @brief Parameters which signal a change in setupComplete state -static const char* triggerParams[] = { "RC_MAP_ROLL", "RC_MAP_PITCH", "RC_MAP_YAW", "RC_MAP_THROTTLE", NULL }; - RadioComponent::RadioComponent(UASInterface* uas, AutoPilotPlugin* autopilot, QObject* parent) : PX4Component(uas, autopilot, parent), _name(tr("Radio")) @@ -61,9 +58,12 @@ bool RadioComponent::requiresSetup(void) const bool RadioComponent::setupComplete(void) const { - for (size_t i=0; triggerParams[i] != NULL; i++) { + // Check for mapped attitude controls + QStringList attitudeMappings; + attitudeMappings << "RC_MAP_ROLL" << "RC_MAP_PITCH" << "RC_MAP_YAW" << "RC_MAP_THROTTLE"; + foreach(QString mapParam, attitudeMappings) { QVariant value; - if (_paramMgr->getParameterValue(_paramMgr->getDefaultComponentId(), triggerParams[0], value)) { + if (_paramMgr->getParameterValue(_paramMgr->getDefaultComponentId(), mapParam, value)) { if (value.toInt() == 0) { return false; } @@ -73,6 +73,47 @@ bool RadioComponent::setupComplete(void) const } } + // Check for min/max/trim defaults for channel 1-4 + + static const int rcMinDefault = 1000; + static const int rcMaxDefault = 2000; + static const int rcTrimDefault = 1500; + + for (int i=1; i<5; i++) { + QVariant value; + int rcMin, rcMax, rcTrim; + + QString param; + + param = QString("RC%1_MIN").arg(i); + if (_paramMgr->getParameterValue(_paramMgr->getDefaultComponentId(), param, value)) { + rcMin = value.toInt(); + } else { + Q_ASSERT(false); + return false; + } + + param = QString("RC%1_MAX").arg(i); + if (_paramMgr->getParameterValue(_paramMgr->getDefaultComponentId(), param, value)) { + rcMax = value.toInt(); + } else { + Q_ASSERT(false); + return false; + } + + param = QString("RC%1_TRIM").arg(i); + if (_paramMgr->getParameterValue(_paramMgr->getDefaultComponentId(), param, value)) { + rcTrim = value.toInt(); + } else { + Q_ASSERT(false); + return false; + } + + if (rcMin == rcMinDefault && rcMax == rcMaxDefault && rcTrim == rcTrimDefault) { + return false; + } + } + return true; } @@ -88,9 +129,25 @@ QString RadioComponent::setupStateDescription(void) const return QString(stateDescription); } -const char** RadioComponent::setupCompleteChangedTriggerList(void) const +QStringList RadioComponent::setupCompleteChangedTriggerList(void) const { - return triggerParams; + QStringList triggers; + + // The best we can do to detect the need for a radio calibration is look for trim/min/max still being + // at defaults. We also look for attitude controls to be mapped. But since they default to channels + // they are not a very reliable source. + + // Attitude control mapping is always a trigger + triggers << "RC_MAP_ROLL" << "RC_MAP_PITCH" << "RC_MAP_YAW" << "RC_MAP_THROTTLE"; + + // We also trigger on min/max/trim for channels 1-4 which would normally be the attitude + // control channels. This may not always be the case, but it's the best we can + triggers << "RC1_MIN" << "RC1_MAX" << "RC1_TRIM"; + triggers << "RC2_MIN" << "RC2_MAX" << "RC2_TRIM"; + triggers << "RC3_MIN" << "RC3_MAX" << "RC3_TRIM"; + triggers << "RC4_MIN" << "RC4_MAX" << "RC4_TRIM"; + + return triggers; } QStringList RadioComponent::paramFilterList(void) const