|
|
|
|
@ -18,6 +18,7 @@
@@ -18,6 +18,7 @@
|
|
|
|
|
#include <QSettings> |
|
|
|
|
|
|
|
|
|
QGC_LOGGING_CATEGORY(RadioComponentControllerLog, "RadioComponentControllerLog") |
|
|
|
|
QGC_LOGGING_CATEGORY(RadioComponentControllerVerboseLog, "RadioComponentControllerVerboseLog") |
|
|
|
|
|
|
|
|
|
#ifdef UNITTEST_BUILD |
|
|
|
|
// Nasty hack to expose controller to unit test code
|
|
|
|
|
@ -26,7 +27,6 @@ RadioComponentController* RadioComponentController::_unitTestController = NULL;
@@ -26,7 +27,6 @@ RadioComponentController* RadioComponentController::_unitTestController = NULL;
|
|
|
|
|
|
|
|
|
|
const int RadioComponentController::_updateInterval = 150; ///< Interval for timer which updates radio channel widgets
|
|
|
|
|
const int RadioComponentController::_rcCalPWMCenterPoint = ((RadioComponentController::_rcCalPWMValidMaxValue - RadioComponentController::_rcCalPWMValidMinValue) / 2.0f) + RadioComponentController::_rcCalPWMValidMinValue; |
|
|
|
|
// FIXME: Double check these mins againt 150% throws
|
|
|
|
|
const int RadioComponentController::_rcCalPWMValidMinValue = 1300; ///< Largest valid minimum PWM Min range value
|
|
|
|
|
const int RadioComponentController::_rcCalPWMValidMaxValue = 1700; ///< Smallest valid maximum PWM Max range value
|
|
|
|
|
const int RadioComponentController::_rcCalPWMDefaultMinValue = 1000; ///< Default value for Min if not set
|
|
|
|
|
@ -56,36 +56,49 @@ const char* RadioComponentController::_imageSwitchMinMax = "radioSwitchMinMax.p
@@ -56,36 +56,49 @@ const char* RadioComponentController::_imageSwitchMinMax = "radioSwitchMinMax.p
|
|
|
|
|
const char* RadioComponentController::_settingsGroup = "RadioCalibration"; |
|
|
|
|
const char* RadioComponentController::_settingsKeyTransmitterMode = "TransmitterMode"; |
|
|
|
|
|
|
|
|
|
const char* RadioComponentController::_px4RevParamFormat = "RC%1_REV"; |
|
|
|
|
const char* RadioComponentController::_apmNewRevParamFormat = "RC%1_REVERSED"; |
|
|
|
|
|
|
|
|
|
const struct RadioComponentController::FunctionInfo RadioComponentController::_rgFunctionInfoPX4[RadioComponentController::rcCalFunctionMax] = { |
|
|
|
|
{ "RC_MAP_ROLL" }, |
|
|
|
|
{ "RC_MAP_PITCH" }, |
|
|
|
|
{ "RC_MAP_YAW" }, |
|
|
|
|
{ "RC_MAP_THROTTLE" } |
|
|
|
|
{ "RC_MAP_ROLL" }, |
|
|
|
|
{ "RC_MAP_PITCH" }, |
|
|
|
|
{ "RC_MAP_YAW" }, |
|
|
|
|
{ "RC_MAP_THROTTLE" } |
|
|
|
|
}; |
|
|
|
|
|
|
|
|
|
const struct RadioComponentController::FunctionInfo RadioComponentController::_rgFunctionInfoAPM[RadioComponentController::rcCalFunctionMax] = { |
|
|
|
|
{ "RCMAP_ROLL" }, |
|
|
|
|
{ "RCMAP_PITCH" }, |
|
|
|
|
{ "RCMAP_YAW" }, |
|
|
|
|
{ "RCMAP_THROTTLE" } |
|
|
|
|
{ "RCMAP_ROLL" }, |
|
|
|
|
{ "RCMAP_PITCH" }, |
|
|
|
|
{ "RCMAP_YAW" }, |
|
|
|
|
{ "RCMAP_THROTTLE" } |
|
|
|
|
}; |
|
|
|
|
|
|
|
|
|
RadioComponentController::RadioComponentController(void) : |
|
|
|
|
_currentStep(-1), |
|
|
|
|
_transmitterMode(2), |
|
|
|
|
_chanCount(0), |
|
|
|
|
_rcCalState(rcCalStateChannelWait), |
|
|
|
|
_unitTestMode(false), |
|
|
|
|
_statusText(NULL), |
|
|
|
|
_cancelButton(NULL), |
|
|
|
|
_nextButton(NULL), |
|
|
|
|
_skipButton(NULL) |
|
|
|
|
RadioComponentController::RadioComponentController(void) |
|
|
|
|
: _currentStep(-1) |
|
|
|
|
, _transmitterMode(2) |
|
|
|
|
, _chanCount(0) |
|
|
|
|
, _rcCalState(rcCalStateChannelWait) |
|
|
|
|
, _unitTestMode(false) |
|
|
|
|
, _statusText(NULL) |
|
|
|
|
, _cancelButton(NULL) |
|
|
|
|
, _nextButton(NULL) |
|
|
|
|
, _skipButton(NULL) |
|
|
|
|
{ |
|
|
|
|
#ifdef UNITTEST_BUILD |
|
|
|
|
// Nasty hack to expose controller to unit test code
|
|
|
|
|
_unitTestController = this; |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
if (parameterExists(FactSystem::defaultComponentId, QStringLiteral("RC1_REVERSED"))) { |
|
|
|
|
// Newer ardupilot firmwares have a different reverse param naming scheme and value scheme
|
|
|
|
|
_revParamFormat = _apmNewRevParamFormat; |
|
|
|
|
_revParamIsBool = true; // param value is boolean 0/1 for reversed or not
|
|
|
|
|
} else { |
|
|
|
|
// Older ardupilot firmwares share the same naming convention as PX4
|
|
|
|
|
_revParamFormat = _px4RevParamFormat; |
|
|
|
|
_revParamIsBool = false; // paeram value if -1 indicates reversed
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
connect(_vehicle, &Vehicle::rcChannelsChanged, this, &RadioComponentController::_rcChannelsChanged); |
|
|
|
|
_loadSettings(); |
|
|
|
|
|
|
|
|
|
@ -226,7 +239,7 @@ void RadioComponentController::_rcChannelsChanged(int channelCount, int pwmValue
@@ -226,7 +239,7 @@ void RadioComponentController::_rcChannelsChanged(int channelCount, int pwmValue
|
|
|
|
|
int channelValue = pwmValues[channel]; |
|
|
|
|
|
|
|
|
|
if (channelValue != -1) { |
|
|
|
|
qCDebug(RadioComponentControllerLog) << "Raw value" << channel << channelValue; |
|
|
|
|
qCDebug(RadioComponentControllerVerboseLog) << "Raw value" << channel << channelValue; |
|
|
|
|
|
|
|
|
|
_rcRawValue[channel] = channelValue; |
|
|
|
|
emit channelRCValueChanged(channel, channelValue); |
|
|
|
|
@ -259,10 +272,13 @@ void RadioComponentController::_rcChannelsChanged(int channelCount, int pwmValue
@@ -259,10 +272,13 @@ void RadioComponentController::_rcChannelsChanged(int channelCount, int pwmValue
|
|
|
|
|
} |
|
|
|
|
} else { |
|
|
|
|
const stateMachineEntry* state = _getStateMachineEntry(_currentStep); |
|
|
|
|
Q_ASSERT(state); |
|
|
|
|
if (state) { |
|
|
|
|
if (state->rcInputFn) { |
|
|
|
|
(this->*state->rcInputFn)(state->function, channel, channelValue); |
|
|
|
|
} |
|
|
|
|
} else { |
|
|
|
|
qWarning() << "Internal error: NULL _getStateMachineEntry return"; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
@ -283,20 +299,27 @@ void RadioComponentController::nextButtonClicked(void)
@@ -283,20 +299,27 @@ void RadioComponentController::nextButtonClicked(void)
|
|
|
|
|
_startCalibration(); |
|
|
|
|
} else { |
|
|
|
|
const stateMachineEntry* state = _getStateMachineEntry(_currentStep); |
|
|
|
|
Q_ASSERT(state); |
|
|
|
|
Q_ASSERT(state->nextFn); |
|
|
|
|
if (state && state->nextFn) { |
|
|
|
|
(this->*state->nextFn)(); |
|
|
|
|
} else { |
|
|
|
|
qWarning() << "Internal error: NULL _getStateMachineEntry return"; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void RadioComponentController::skipButtonClicked(void) |
|
|
|
|
{ |
|
|
|
|
Q_ASSERT(_currentStep != -1); |
|
|
|
|
if (_currentStep == -1) { |
|
|
|
|
qWarning() << "Internal error: _currentStep == -1"; |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
const stateMachineEntry* state = _getStateMachineEntry(_currentStep); |
|
|
|
|
Q_ASSERT(state); |
|
|
|
|
Q_ASSERT(state->skipFn); |
|
|
|
|
if (state && state->skipFn) { |
|
|
|
|
(this->*state->skipFn)(); |
|
|
|
|
} else { |
|
|
|
|
qWarning() << "Internal error: NULL _getStateMachineEntry return"; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void RadioComponentController::cancelButtonClicked(void) |
|
|
|
|
@ -581,9 +604,6 @@ void RadioComponentController::_setInternalCalibrationValuesFromParameters(void)
@@ -581,9 +604,6 @@ void RadioComponentController::_setInternalCalibrationValuesFromParameters(void)
|
|
|
|
|
QString minTpl("RC%1_MIN"); |
|
|
|
|
QString maxTpl("RC%1_MAX"); |
|
|
|
|
QString trimTpl("RC%1_TRIM"); |
|
|
|
|
QString revTpl("RC%1_REV"); |
|
|
|
|
|
|
|
|
|
bool convertOk; |
|
|
|
|
|
|
|
|
|
for (int i = 0; i < _chanMax(); ++i) { |
|
|
|
|
struct ChannelInfo* info = &_rgChannelInfo[i]; |
|
|
|
|
@ -601,29 +621,20 @@ void RadioComponentController::_setInternalCalibrationValuesFromParameters(void)
@@ -601,29 +621,20 @@ void RadioComponentController::_setInternalCalibrationValuesFromParameters(void)
|
|
|
|
|
|
|
|
|
|
Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, trimTpl.arg(i+1)); |
|
|
|
|
if (paramFact) { |
|
|
|
|
info->rcTrim = paramFact->rawValue().toInt(&convertOk); |
|
|
|
|
Q_ASSERT(convertOk); |
|
|
|
|
info->rcTrim = paramFact->rawValue().toInt(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
paramFact = getParameterFact(FactSystem::defaultComponentId, minTpl.arg(i+1)); |
|
|
|
|
if (paramFact) { |
|
|
|
|
info->rcMin = paramFact->rawValue().toInt(&convertOk); |
|
|
|
|
Q_ASSERT(convertOk); |
|
|
|
|
info->rcMin = paramFact->rawValue().toInt(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
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); |
|
|
|
|
info->rcMax = getParameterFact(FactSystem::defaultComponentId, maxTpl.arg(i+1))->rawValue().toInt(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
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; |
|
|
|
|
} |
|
|
|
|
info->reversed = _channelReversedParamValue(i); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
for (int i=0; i<rcCalFunctionMax; i++) { |
|
|
|
|
@ -633,8 +644,7 @@ void RadioComponentController::_setInternalCalibrationValuesFromParameters(void)
@@ -633,8 +644,7 @@ void RadioComponentController::_setInternalCalibrationValuesFromParameters(void)
|
|
|
|
|
if (paramName) { |
|
|
|
|
Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, paramName); |
|
|
|
|
if (paramFact) { |
|
|
|
|
paramChannel = paramFact->rawValue().toInt(&convertOk); |
|
|
|
|
Q_ASSERT(convertOk); |
|
|
|
|
paramChannel = paramFact->rawValue().toInt(); |
|
|
|
|
|
|
|
|
|
if (paramChannel != 0) { |
|
|
|
|
_rgFunctionChannelMapping[i] = paramChannel - 1; |
|
|
|
|
@ -717,7 +727,6 @@ void RadioComponentController::_writeCalibration(void)
@@ -717,7 +727,6 @@ void RadioComponentController::_writeCalibration(void)
|
|
|
|
|
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++) { |
|
|
|
|
@ -746,16 +755,13 @@ void RadioComponentController::_writeCalibration(void)
@@ -746,16 +755,13 @@ void RadioComponentController::_writeCalibration(void)
|
|
|
|
|
// may affect channel reversing so we can't automatically determine it. This is ok for PX4 given how it uses mixers, but not for ArduPilot.
|
|
|
|
|
if (_vehicle->px4Firmware() || _vehicle->multiRotor()) { |
|
|
|
|
// APM multi-rotor has a backwards interpretation of "reversed" on the Pitch control. So be careful.
|
|
|
|
|
float reversedParamValue; |
|
|
|
|
bool reversed; |
|
|
|
|
if (_px4Vehicle() || info->function != rcCalFunctionPitch) { |
|
|
|
|
reversedParamValue = info->reversed ? -1.0f : 1.0f; |
|
|
|
|
reversed = info->reversed; |
|
|
|
|
} else { |
|
|
|
|
reversedParamValue = info->reversed ? 1.0f : -1.0f; |
|
|
|
|
} |
|
|
|
|
paramFact = getParameterFact(FactSystem::defaultComponentId, revTpl.arg(oneBasedChannel)); |
|
|
|
|
if (paramFact) { |
|
|
|
|
paramFact->setRawValue(reversedParamValue); |
|
|
|
|
reversed = !info->reversed; |
|
|
|
|
} |
|
|
|
|
_setChannelReversedParamValue(chan, reversed); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
@ -797,7 +803,10 @@ void RadioComponentController::_writeCalibration(void)
@@ -797,7 +803,10 @@ void RadioComponentController::_writeCalibration(void)
|
|
|
|
|
/// @brief Starts the calibration process
|
|
|
|
|
void RadioComponentController::_startCalibration(void) |
|
|
|
|
{ |
|
|
|
|
Q_ASSERT(_chanCount >= _chanMinimum); |
|
|
|
|
if (_chanCount < _chanMinimum) { |
|
|
|
|
qWarning() << "Call to RadioComponentController::_startCalibration with _chanCount < _chanMinimum"; |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
_resetInternalCalibrationValues(); |
|
|
|
|
|
|
|
|
|
@ -895,7 +904,8 @@ void RadioComponentController::_setHelpImage(const char* imageFile)
@@ -895,7 +904,8 @@ void RadioComponentController::_setHelpImage(const char* imageFile)
|
|
|
|
|
} else if (_transmitterMode == 2) { |
|
|
|
|
file += _imageFileMode2Dir; |
|
|
|
|
} else { |
|
|
|
|
Q_ASSERT(false); |
|
|
|
|
qWarning() << "Internal error: Bad _transmitterMode value"; |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
file += imageFile; |
|
|
|
|
|
|
|
|
|
@ -1045,3 +1055,34 @@ int RadioComponentController::_chanMax(void) const
@@ -1045,3 +1055,34 @@ int RadioComponentController::_chanMax(void) const
|
|
|
|
|
{ |
|
|
|
|
return _px4Vehicle() ? _chanMaxPX4 : _chanMaxAPM; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
bool RadioComponentController::_channelReversedParamValue(int channel) |
|
|
|
|
{ |
|
|
|
|
Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, _revParamFormat.arg(channel+1)); |
|
|
|
|
if (paramFact) { |
|
|
|
|
if (_revParamIsBool) { |
|
|
|
|
return paramFact->rawValue().toBool(); |
|
|
|
|
} else { |
|
|
|
|
bool convertOk; |
|
|
|
|
float floatReversed = paramFact->rawValue().toFloat(&convertOk); |
|
|
|
|
if (!convertOk) { |
|
|
|
|
floatReversed = 1.0f; |
|
|
|
|
} |
|
|
|
|
return floatReversed == -1.0f; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void RadioComponentController::_setChannelReversedParamValue(int channel, bool reversed) |
|
|
|
|
{ |
|
|
|
|
Fact* paramFact = getParameterFact(FactSystem::defaultComponentId, _revParamFormat.arg(channel+1)); |
|
|
|
|
if (paramFact) { |
|
|
|
|
if (_revParamIsBool) { |
|
|
|
|
paramFact->setRawValue(reversed); |
|
|
|
|
} else { |
|
|
|
|
paramFact->setRawValue(reversed ? -1.0f : 1.0f); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
Don Gagne
8 years ago
committed by
GitHub