diff --git a/src/AutoPilotPlugins/Common/RadioComponent.qml b/src/AutoPilotPlugins/Common/RadioComponent.qml index 44c150b..f4df9cc 100644 --- a/src/AutoPilotPlugins/Common/RadioComponent.qml +++ b/src/AutoPilotPlugins/Common/RadioComponent.qml @@ -63,7 +63,8 @@ QGCView { } onChannelCountChanged: updateChannelCount() - onFunctionMappingChangedAPMReboot: showMessage(qsTr("Reboot required"), qsTr("Your stick mappings have changed, you must reboot the vehicle for correct operation."), StandardButton.Ok) + onFunctionMappingChangedAPMReboot: showMessage(qsTr("Reboot required"), qsTr("Your stick mappings have changed, you must reboot the vehicle for correct operation."), StandardButton.Ok) + onThrottleReversedCalFailure: showMessage(qsTr("Throttle channel reversed"), qsTr("Calibration failed. The throttle channel on your transmitter is reversed. You must correct this on your transmitter in order to complete calibration."), StandardButton.Ok) } onCompleted: { diff --git a/src/AutoPilotPlugins/Common/RadioComponentController.cc b/src/AutoPilotPlugins/Common/RadioComponentController.cc index 9a222d1..6e70cd4 100644 --- a/src/AutoPilotPlugins/Common/RadioComponentController.cc +++ b/src/AutoPilotPlugins/Common/RadioComponentController.cc @@ -325,7 +325,7 @@ void RadioComponentController::_saveAllTrims(void) // trims reset to correct values. for (int i=0; i<_chanCount; i++) { - qCDebug(RadioComponentControllerLog) << "_saveAllTrims trim" << _rcRawValue[i]; + qCDebug(RadioComponentControllerLog) << "_saveAllTrims channel trim" << i<< _rcRawValue[i]; _rgChannelInfo[i].rcTrim = _rcRawValue[i]; } _advanceState(); @@ -751,77 +751,83 @@ void RadioComponentController::_writeCalibration(void) if (_px4Vehicle()) { _uas->stopCalibration(); } - - _validateCalibration(); - - QString minTpl("RC%1_MIN"); - QString maxTpl("RC%1_MAX"); - QString trimTpl("RC%1_TRIM"); - QString revTpl("RC%1_REV"); - - // 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++) { - 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 - 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); - } + if (!_px4Vehicle() && _rgChannelInfo[_rgFunctionChannelMapping[rcCalFunctionThrottle]].reversed) { + // A reversed throttle could lead to dangerous power up issues if the firmware doesn't handle it absolutely correctly in all places. + // So in this case fail the calibration for anything other than PX4 which is known to be able to handle this correctly. + emit throttleReversedCalFailure(); + } else { + _validateCalibration(); - // For multi-rotor we can determine reverse setting during radio cal. For anything other than multi-rotor, servo installation - // may affect channel reversing so we can't automatically determine it. - if (_vehicle->multiRotor()) { - // APM multi-rotor has a backwards interpretation of "reversed" on the Pitch control. So be careful. - float reversedParamValue; - if (_px4Vehicle() || info->function != rcCalFunctionPitch) { - reversedParamValue = info->reversed ? -1.0f : 1.0f; - } else { - reversedParamValue = info->reversed ? 1.0f : -1.0f; + QString minTpl("RC%1_MIN"); + QString maxTpl("RC%1_MAX"); + QString trimTpl("RC%1_TRIM"); + QString revTpl("RC%1_REV"); + + // 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++) { + 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 + continue; } - paramFact = getParameterFact(FactSystem::defaultComponentId, revTpl.arg(oneBasedChannel)); + + Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, trimTpl.arg(oneBasedChannel)); if (paramFact) { - paramFact->setRawValue(reversedParamValue); + 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); } - } - } - - // Write function mapping parameters - for (size_t i=0; irawValue().toInt() != paramChannel) { - paramFact = getParameterFact(FactSystem::defaultComponentId, _functionInfo()[i].parameterName); + // For multi-rotor we can determine reverse setting during radio cal. For anything other than multi-rotor, servo installation + // may affect channel reversing so we can't automatically determine it. + if (_vehicle->multiRotor()) { + // APM multi-rotor has a backwards interpretation of "reversed" on the Pitch control. So be careful. + float reversedParamValue; + if (_px4Vehicle() || info->function != rcCalFunctionPitch) { + reversedParamValue = info->reversed ? -1.0f : 1.0f; + } else { + reversedParamValue = info->reversed ? 1.0f : -1.0f; + } + paramFact = getParameterFact(FactSystem::defaultComponentId, revTpl.arg(oneBasedChannel)); if (paramFact) { - paramFact->setRawValue(paramChannel); + paramFact->setRawValue(reversedParamValue); + } + } + } + + // Write function mapping parameters + for (size_t i=0; irawValue().toInt() != paramChannel) { + paramFact = getParameterFact(FactSystem::defaultComponentId, _functionInfo()[i].parameterName); + if (paramFact) { + paramFact->setRawValue(paramChannel); + } } } } } - + if (_px4Vehicle()) { // If the RC_CHAN_COUNT parameter is available write the channel count if (parameterExists(FactSystem::defaultComponentId, "RC_CHAN_CNT")) { diff --git a/src/AutoPilotPlugins/Common/RadioComponentController.h b/src/AutoPilotPlugins/Common/RadioComponentController.h index 882e0a2..2aced17 100644 --- a/src/AutoPilotPlugins/Common/RadioComponentController.h +++ b/src/AutoPilotPlugins/Common/RadioComponentController.h @@ -124,12 +124,15 @@ signals: void imageHelpChanged(QString source); void transmitterModeChanged(int mode); - // @brief Signalled when in unit test mode and a message box should be displayed by the next button + /// Signalled when in unit test mode and a message box should be displayed by the next button void nextButtonMessageBoxDisplayed(void); - // Signaled to QML to indicator reboot is required + /// Signalled to QML to indicate reboot is required void functionMappingChangedAPMReboot(void); + /// Signalled to Qml to indicate cal failure due to reversed throttle + void throttleReversedCalFailure(void); + private slots: void _rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels]);