diff --git a/src/AutoPilotPlugins/Common/RadioComponentController.cc b/src/AutoPilotPlugins/Common/RadioComponentController.cc index 3598f99031a19af334a8380afe88ff9641e99730..eebd44ecd2c5ec353963d83816b864e60d05d385 100644 --- a/src/AutoPilotPlugins/Common/RadioComponentController.cc +++ b/src/AutoPilotPlugins/Common/RadioComponentController.cc @@ -98,10 +98,6 @@ 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(); } @@ -217,7 +213,7 @@ void RadioComponentController::_setupCurrentState(void) _statusText->setProperty("text", instructions); _setHelpImage(helpImage); - _stickDetectChannel = _chanMax(); + _stickDetectChannel = _chanMax; _stickDetectSettleStarted = false; _rcCalSaveCurrentValues(); @@ -229,9 +225,7 @@ void RadioComponentController::_setupCurrentState(void) /// Connected to Vehicle::rcChannelsChanged signal void RadioComponentController::_rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels]) { - int maxChannel = std::min(channelCount, _chanMax()); - - for (int channel=0; channel _rcCalMoveDelta) { @@ -439,7 +433,7 @@ void RadioComponentController::_inputStickMin(enum rcCalFunctions function, int return; } - if (_stickDetectChannel == _chanMax()) { + if (_stickDetectChannel == _chanMax) { // Setup up to detect stick being pegged to extreme position if (_rgChannelInfo[channel].reversed) { if (value > _rcCalPWMCenterPoint + _rcCalMoveDelta) { @@ -486,7 +480,7 @@ void RadioComponentController::_inputCenterWait(enum rcCalFunctions function, in return; } - if (_stickDetectChannel == _chanMax()) { + if (_stickDetectChannel == _chanMax) { // Sticks have not yet moved close enough to center if (abs(_rcCalPWMCenterPoint - value) < _rcCalRoughCenterDelta) { @@ -564,7 +558,7 @@ void RadioComponentController::_inputSwitchDetect(enum rcCalFunctions function, void RadioComponentController::_resetInternalCalibrationValues(void) { // Set all raw channels to not reversed and center point values - for (int i=0; i<_chanMax(); i++) { + for (int i=0; i<_chanMax; i++) { struct ChannelInfo* info = &_rgChannelInfo[i]; info->function = rcCalFunctionMax; info->reversed = false; @@ -575,7 +569,7 @@ void RadioComponentController::_resetInternalCalibrationValues(void) // Initialize attitude function mapping to function channel not set for (size_t i=0; ifunction = rcCalFunctionMax; } for (size_t i=0; ircTrim = 1500; - info->rcMin = 1100; - info->rcMax = 1900; - info->reversed = false; - continue; - } + if (!parameterExists(FactSystem::defaultComponentId, minTpl.arg(i+1))) { + info->rcTrim = 1500; + info->rcMin = 1100; + info->rcMax = 1900; + info->reversed = false; + continue; } Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, trimTpl.arg(i+1)); @@ -642,7 +633,7 @@ void RadioComponentController::_setInternalCalibrationValuesFromParameters(void) if (paramFact) { paramChannel = paramFact->rawValue().toInt(); - if (paramChannel > 0 && paramChannel <= _chanMax()) { + if (paramChannel > 0 && paramChannel <= _chanMax) { _rgFunctionChannelMapping[i] = paramChannel - 1; _rgChannelInfo[paramChannel - 1].function = static_cast(i); } @@ -661,7 +652,7 @@ void RadioComponentController::spektrumBindMode(int mode) /// @brief Validates the current settings against the calibration rules resetting values as necessary. void RadioComponentController::_validateCalibration(void) { - for (int chan = 0; chan<_chanMax(); chan++) { + for (int chan = 0; chan<_chanMax; chan++) { struct ChannelInfo* info = &_rgChannelInfo[chan]; if (chan < _chanCount) { @@ -725,12 +716,11 @@ void RadioComponentController::_writeCalibration(void) QString trimTpl("RC%1_TRIM"); // Note that the rc parameters are all float, so you must cast to float in order to get the right QVariant - for (int chan = 0; chan<_chanMax(); chan++) { + for (int chan = 0; chan<_chanMax; chan++) { struct ChannelInfo* info = &_rgChannelInfo[chan]; int oneBasedChannel = chan + 1; - 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 + if (!parameterExists(FactSystem::defaultComponentId, minTpl.arg(chan+1))) { continue; } @@ -764,7 +754,7 @@ void RadioComponentController::_writeCalibration(void) // Write function mapping parameters for (size_t i=0; i _apmPossibleMissingRCChannelParams; ///< List of possible missing RC*_* params for APM stack + struct ChannelInfo _rgChannelInfo[_chanMax]; ///< Information associated with each rc channel enum rcCalStates _rcCalState; ///< Current calibration state int _rcCalStateCurrentChannel; ///< Current channel being worked on in rcCalStateIdentify and rcCalStateDetectInversion @@ -302,9 +296,9 @@ private: QString _revParamFormat; bool _revParamIsBool; - int _rcValueSave[_chanMaxAny]; ///< Saved values prior to detecting channel movement + int _rcValueSave[_chanMax]; ///< Saved values prior to detecting channel movement - int _rcRawValue[_chanMaxAny]; ///< Current set of raw channel values + int _rcRawValue[_chanMax]; ///< Current set of raw channel values int _stickDetectChannel; int _stickDetectInitialValue; diff --git a/src/QmlControls/RCChannelMonitorController.cc b/src/QmlControls/RCChannelMonitorController.cc index 2ab945af36c5a9078f702275aee5053853b69913..fddd8fdcd374a4514103308bdb4a21c639e3c9ad 100644 --- a/src/QmlControls/RCChannelMonitorController.cc +++ b/src/QmlControls/RCChannelMonitorController.cc @@ -18,9 +18,7 @@ RCChannelMonitorController::RCChannelMonitorController(void) void RCChannelMonitorController::_rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels]) { - int maxChannel = std::min(channelCount, _chanMax()); - - for (int channel=0; channelfirmwareType() == MAV_AUTOPILOT_PX4 ? _chanMaxPX4 : _chanMaxAPM; -} diff --git a/src/QmlControls/RCChannelMonitorController.h b/src/QmlControls/RCChannelMonitorController.h index 1bd5971b5861e325a3b4fee57e5711c9e992be6c..68d158fd7d4706499fa1774e37c5a7b0d4519259 100644 --- a/src/QmlControls/RCChannelMonitorController.h +++ b/src/QmlControls/RCChannelMonitorController.h @@ -37,12 +37,7 @@ private slots: void _rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels]); private: - int _chanMax(void) const; - int _chanCount; - - static const int _chanMaxPX4 = 18; ///< Maximum number of supported rc channels, PX4 Firmware - static const int _chanMaxAPM = 14; ///< Maximum number of supported rc channels, APM firmware }; #endif // RCChannelMonitorController_H