Commit 4dab0e1e authored by Don Gagne's avatar Don Gagne

Handle ArudPilot parameter changes in newer firmwares

parent 1a2ce057
......@@ -55,21 +55,23 @@ SetupPage {
property Fact _mountAngMinPan: controller.getParameterFact(-1, "MNT_ANGMIN_PAN")
property Fact _mountAngMaxPan: controller.getParameterFact(-1, "MNT_ANGMAX_PAN")
property Fact _rc5Function: controller.getParameterFact(-1, "RC5_FUNCTION")
property Fact _rc6Function: controller.getParameterFact(-1, "RC6_FUNCTION")
property Fact _rc7Function: controller.getParameterFact(-1, "RC7_FUNCTION")
property Fact _rc8Function: controller.getParameterFact(-1, "RC8_FUNCTION")
property Fact _rc9Function: controller.getParameterFact(-1, "RC9_FUNCTION")
property Fact _rc10Function: controller.getParameterFact(-1, "RC10_FUNCTION")
property Fact _rc11Function: controller.getParameterFact(-1, "RC11_FUNCTION")
property Fact _rc12Function: controller.getParameterFact(-1, "RC12_FUNCTION")
property Fact _rc13Function: controller.getParameterFact(-1, "RC13_FUNCTION")
property Fact _rc14Function: controller.getParameterFact(-1, "RC14_FUNCTION")
property Fact _rc5Function: controller.getParameterFact(-1, "r.SERVO5_FUNCTION")
property Fact _rc6Function: controller.getParameterFact(-1, "r.SERVO6_FUNCTION")
property Fact _rc7Function: controller.getParameterFact(-1, "r.SERVO7_FUNCTION")
property Fact _rc8Function: controller.getParameterFact(-1, "r.SERVO8_FUNCTION")
property Fact _rc9Function: controller.getParameterFact(-1, "r.SERVO9_FUNCTION")
property Fact _rc10Function: controller.getParameterFact(-1, "r.SERVO10_FUNCTION")
property Fact _rc11Function: controller.getParameterFact(-1, "r.SERVO11_FUNCTION")
property Fact _rc12Function: controller.getParameterFact(-1, "r.SERVO12_FUNCTION")
property Fact _rc13Function: controller.getParameterFact(-1, "r.SERVO13_FUNCTION")
property Fact _rc14Function: controller.getParameterFact(-1, "r.SERVO14_FUNCTION")
property bool _tiltEnabled: false
property bool _panEnabled: false
property bool _rollEnabled: false
property bool _servoReverseIsBool: controller.parameterExists(-1, "RC5_REVERSED")
// Gimbal Settings not available on older firmware
property bool _showGimbaLSettings: controller.parameterExists(-1, "MNT_DEFLT_MODE")
......@@ -95,10 +97,17 @@ SetupPage {
loader.gimbalOutIndex = channel - 4
loader.servoPWMMinFact = controller.getParameterFact(-1, rcPrefix + "MIN")
loader.servoPWMMaxFact = controller.getParameterFact(-1, rcPrefix + "MAX")
if (controller.parameterExists(-1, "RC5_REVERSED")) {
// Newer firmware parameter
loader.servoReverseFact = controller.getParameterFact(-1, rcPrefix + "REVERSED")
} else {
// Older firmware parameter
loader.servoReverseFact = controller.getParameterFact(-1, rcPrefix + "REV")
}
/// Gimbal output channels are stored in RC#_FUNCTION parameters. We need to loop through those
}
/// Gimbal output channels are stored in SERVO#_FUNCTION parameters. We need to loop through those
/// to find them and setup the ui accordindly.
function calcGimbalOutValues() {
gimbalDirectionTiltLoader.gimbalOutIndex = 0
......@@ -108,7 +117,7 @@ SetupPage {
_panEnabled = false
_rollEnabled = false
for (var channel=_firstGimbalOutChannel; channel<=_lastGimbalOutChannel; channel++) {
var functionFact = controller.getParameterFact(-1, "RC" + channel + "_FUNCTION")
var functionFact = controller.getParameterFact(-1, "r.SERVO" + channel + "_FUNCTION")
if (functionFact.value == _rcFunctionMountTilt) {
_tiltEnabled = true
setGimbalSettingsServoInfo(gimbalDirectionTiltLoader, channel)
......@@ -125,7 +134,7 @@ SetupPage {
function setRCFunction(channel, rcFunction) {
// First clear any previous settings for this function
for (var index=_firstGimbalOutChannel; index<=_lastGimbalOutChannel; index++) {
var functionFact = controller.getParameterFact(-1, "RC" + index + "_FUNCTION")
var functionFact = controller.getParameterFact(-1, "r.SERVO" + index + "_FUNCTION")
if (functionFact.value != _rcFunctionDisabled && functionFact.value == rcFunction) {
functionFact.value = _rcFunctionDisabled
}
......@@ -133,12 +142,12 @@ SetupPage {
// Now set the function into the new channel
if (channel != 0) {
var functionFact = controller.getParameterFact(-1, "RC" + channel + "_FUNCTION")
var functionFact = controller.getParameterFact(-1, "r.SERVO" + channel + "_FUNCTION")
functionFact.value = rcFunction
}
}
// Whenever any RC#_FUNCTION parameters chagnes we need to go looking for gimbal output channels again
// Whenever any SERVO#_FUNCTION parameters changes we need to go looking for gimbal output channels again
Connections { target: _rc5Function; onValueChanged: calcGimbalOutValues() }
Connections { target: _rc6Function; onValueChanged: calcGimbalOutValues() }
Connections { target: _rc7Function; onValueChanged: calcGimbalOutValues() }
......@@ -195,6 +204,7 @@ SetupPage {
// property Fact servoPWMMinFact
// property Fact servoPWMMaxFact
// property Fact servoReverseFact
// property bool servoReverseIsBool
// property int rcFunction
Item {
......@@ -234,10 +244,12 @@ SetupPage {
anchors.top: mountStabCheckBox.bottom
anchors.right: parent.right
text: qsTr("Servo reverse")
checkedValue: 1
uncheckedValue: 0
checkedValue: _servoReverseIsBool ? 1 : -1
uncheckedValue: _servoReverseIsBool ? 0 : 1
fact: servoReverseFact
enabled: directionEnabled
property bool _servoReverseIsBool: servoReverseIsBool
}
QGCLabel {
......@@ -462,6 +474,7 @@ SetupPage {
property Fact servoPWMMinFact: Fact { }
property Fact servoPWMMaxFact: Fact { }
property Fact servoReverseFact: Fact { }
property bool servoReverseIsBool: _servoReverseIsBool
property int rcFunction: _rcFunctionMountTilt
}
......@@ -479,6 +492,7 @@ SetupPage {
property Fact servoPWMMinFact: Fact { }
property Fact servoPWMMaxFact: Fact { }
property Fact servoReverseFact: Fact { }
property bool servoReverseIsBool: _servoReverseIsBool
property int rcFunction: _rcFunctionMountRoll
}
......@@ -496,6 +510,7 @@ SetupPage {
property Fact servoPWMMinFact: Fact { }
property Fact servoPWMMaxFact: Fact { }
property Fact servoReverseFact: Fact { }
property bool servoReverseIsBool: _servoReverseIsBool
property int rcFunction: _rcFunctionMountPan
}
......
......@@ -32,16 +32,16 @@ SetupPage {
QGCPalette { id: palette; colorGroupEnabled: true }
property Fact _rc5Function: controller.getParameterFact(-1, "RC5_FUNCTION")
property Fact _rc6Function: controller.getParameterFact(-1, "RC6_FUNCTION")
property Fact _rc7Function: controller.getParameterFact(-1, "RC7_FUNCTION")
property Fact _rc8Function: controller.getParameterFact(-1, "RC8_FUNCTION")
property Fact _rc9Function: controller.getParameterFact(-1, "RC9_FUNCTION")
property Fact _rc10Function: controller.getParameterFact(-1, "RC10_FUNCTION")
property Fact _rc11Function: controller.getParameterFact(-1, "RC11_FUNCTION")
property Fact _rc12Function: controller.getParameterFact(-1, "RC12_FUNCTION")
property Fact _rc13Function: controller.getParameterFact(-1, "RC13_FUNCTION")
property Fact _rc14Function: controller.getParameterFact(-1, "RC14_FUNCTION")
property Fact _rc5Function: controller.getParameterFact(-1, "r.SERVO5_FUNCTION")
property Fact _rc6Function: controller.getParameterFact(-1, "r.SERVO6_FUNCTION")
property Fact _rc7Function: controller.getParameterFact(-1, "r.SERVO7_FUNCTION")
property Fact _rc8Function: controller.getParameterFact(-1, "r.SERVO8_FUNCTION")
property Fact _rc9Function: controller.getParameterFact(-1, "r.SERVO9_FUNCTION")
property Fact _rc10Function: controller.getParameterFact(-1, "r.SERVO10_FUNCTION")
property Fact _rc11Function: controller.getParameterFact(-1, "r.SERVO11_FUNCTION")
property Fact _rc12Function: controller.getParameterFact(-1, "r.SERVO12_FUNCTION")
property Fact _rc13Function: controller.getParameterFact(-1, "r.SERVO13_FUNCTION")
property Fact _rc14Function: controller.getParameterFact(-1, "r.SERVO14_FUNCTION")
readonly property real _margins: ScreenTools.defaultFontPixelHeight
readonly property int _rcFunctionDisabled: 0
......@@ -54,13 +54,13 @@ SetupPage {
calcLightOutValues()
}
/// Light output channels are stored in RC#_FUNCTION parameters. We need to loop through those
/// Light output channels are stored in SERVO#_FUNCTION parameters. We need to loop through those
/// to find them and setup the ui accordindly.
function calcLightOutValues() {
lightsLoader.lights1OutIndex = 0
lightsLoader.lights2OutIndex = 0
for (var channel=_firstLightsOutChannel; channel<=_lastLightsOutChannel; channel++) {
var functionFact = controller.getParameterFact(-1, "RC" + channel + "_FUNCTION")
var functionFact = controller.getParameterFact(-1, "r.SERVO" + channel + "_FUNCTION")
if (functionFact.value == _rcFunctionRCIN9) {
lightsLoader.lights1OutIndex = channel - 4
} else if (functionFact.value == _rcFunctionRCIN10) {
......@@ -72,7 +72,7 @@ SetupPage {
function setRCFunction(channel, rcFunction) {
// First clear any previous settings for this function
for (var index=_firstLightsOutChannel; index<=_lastLightsOutChannel; index++) {
var functionFact = controller.getParameterFact(-1, "RC" + index + "_FUNCTION")
var functionFact = controller.getParameterFact(-1, "r.SERVO" + index + "_FUNCTION")
if (functionFact.value != _rcFunctionDisabled && functionFact.value == rcFunction) {
functionFact.value = _rcFunctionDisabled
}
......@@ -80,12 +80,12 @@ SetupPage {
// Now set the function into the new channel
if (channel != 0) {
var functionFact = controller.getParameterFact(-1, "RC" + channel + "_FUNCTION")
var functionFact = controller.getParameterFact(-1, "r.SERVO" + channel + "_FUNCTION")
functionFact.value = rcFunction
}
}
// Whenever any RC#_FUNCTION parameters chagnes we need to go looking for light output channels again
// Whenever any SERVO#_FUNCTION parameters chagnes we need to go looking for light output channels again
Connections { target: _rc5Function; onValueChanged: calcLightOutValues() }
Connections { target: _rc6Function; onValueChanged: calcLightOutValues() }
Connections { target: _rc7Function; onValueChanged: calcLightOutValues() }
......
......@@ -14,16 +14,16 @@ FactPanel {
QGCPalette { id: qgcPal; colorGroupEnabled: enabled }
FactPanelController { id: controller; factPanel: panel }
property Fact _rc5Function: controller.getParameterFact(-1, "RC5_FUNCTION")
property Fact _rc6Function: controller.getParameterFact(-1, "RC6_FUNCTION")
property Fact _rc7Function: controller.getParameterFact(-1, "RC7_FUNCTION")
property Fact _rc8Function: controller.getParameterFact(-1, "RC8_FUNCTION")
property Fact _rc9Function: controller.getParameterFact(-1, "RC9_FUNCTION")
property Fact _rc10Function: controller.getParameterFact(-1, "RC10_FUNCTION")
property Fact _rc11Function: controller.getParameterFact(-1, "RC11_FUNCTION")
property Fact _rc12Function: controller.getParameterFact(-1, "RC12_FUNCTION")
property Fact _rc13Function: controller.getParameterFact(-1, "RC13_FUNCTION")
property Fact _rc14Function: controller.getParameterFact(-1, "RC14_FUNCTION")
property Fact _rc5Function: controller.getParameterFact(-1, "r.SERVO5_FUNCTION")
property Fact _rc6Function: controller.getParameterFact(-1, "r.SERVO6_FUNCTION")
property Fact _rc7Function: controller.getParameterFact(-1, "r.SERVO7_FUNCTION")
property Fact _rc8Function: controller.getParameterFact(-1, "r.SERVO8_FUNCTION")
property Fact _rc9Function: controller.getParameterFact(-1, "r.SERVO9_FUNCTION")
property Fact _rc10Function: controller.getParameterFact(-1, "r.SERVO10_FUNCTION")
property Fact _rc11Function: controller.getParameterFact(-1, "r.SERVO11_FUNCTION")
property Fact _rc12Function: controller.getParameterFact(-1, "r.SERVO12_FUNCTION")
property Fact _rc13Function: controller.getParameterFact(-1, "r.SERVO13_FUNCTION")
property Fact _rc14Function: controller.getParameterFact(-1, "r.SERVO14_FUNCTION")
readonly property int _rcFunctionRCIN9: 59
readonly property int _rcFunctionRCIN10: 60
......@@ -34,13 +34,13 @@ FactPanel {
calcLightOutValues()
}
/// Light output channels are stored in RC#_FUNCTION parameters. We need to loop through those
/// Light output channels are stored in SERVO#_FUNCTION parameters. We need to loop through those
/// to find them and setup the ui accordindly.
function calcLightOutValues() {
lightsLoader.lights1OutIndex = 0
lightsLoader.lights2OutIndex = 0
for (var channel=_firstLightsOutChannel; channel<=_lastLightsOutChannel; channel++) {
var functionFact = controller.getParameterFact(-1, "RC" + channel + "_FUNCTION")
var functionFact = controller.getParameterFact(-1, "r.SERVO" + channel + "_FUNCTION")
if (functionFact.value == _rcFunctionRCIN9) {
lightsLoader.lights1OutIndex = channel - 4
} else if (functionFact.value == _rcFunctionRCIN10) {
......@@ -49,7 +49,7 @@ FactPanel {
}
}
// Whenever any RC#_FUNCTION parameters chagnes we need to go looking for light output channels again
// Whenever any SERVO#_FUNCTION parameters chagnes we need to go looking for light output channels again
Connections { target: _rc5Function; onValueChanged: calcLightOutValues() }
Connections { target: _rc6Function; onValueChanged: calcLightOutValues() }
Connections { target: _rc7Function; onValueChanged: calcLightOutValues() }
......
......@@ -84,7 +84,7 @@ SetupPage {
calcAutoTuneChannel()
}
/// The AutoTune switch is stored in one of the RC#_FUNCTION parameters. We need to loop through those
/// The AutoTune switch is stored in one of the CH#_OPT parameters. We need to loop through those
/// to find them and setup the ui accordindly.
function calcAutoTuneChannel() {
_autoTuneSwitchChannelIndex = 0
......
......@@ -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;
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
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
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
}
} 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)
_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)
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)
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)
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)
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)
// 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)
/// @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)
} 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
{
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);
}
}
}
......@@ -24,6 +24,7 @@
#include "AutoPilotPlugin.h"
Q_DECLARE_LOGGING_CATEGORY(RadioComponentControllerLog)
Q_DECLARE_LOGGING_CATEGORY(RadioComponentControllerVerboseLog)
class RadioConfigest;
......@@ -233,6 +234,9 @@ private:
int _chanMax(void) const;
bool _channelReversedParamValue(int channel);
void _setChannelReversedParamValue(int channel, bool reversed);
// @brief Called by unit test code to set the mode to unit testing
void _setUnitTestMode(void){ _unitTestMode = true; }
......@@ -293,6 +297,11 @@ private:
static const int _rcCalSettleDelta;
static const int _rcCalMinDelta;
static const char* _px4RevParamFormat;
static const char* _apmNewRevParamFormat;
QString _revParamFormat;
bool _revParamIsBool;
int _rcValueSave[_chanMaxAny]; ///< Saved values prior to detecting channel movement
int _rcRawValue[_chanMaxAny]; ///< Current set of raw channel values
......
......@@ -11,12 +11,9 @@ QGCCheckBox {
property variant checkedValue: 1
property variant uncheckedValue: 0
partiallyCheckedEnabled: fact ? fact.value !== checkedValue && fact.value !== uncheckedValue : false
checkedState: fact ? fact.value === checkedValue ? Qt.Checked : (fact.value === uncheckedValue ? Qt.Unchecked : Qt.PartiallyChecked) : false
checkedState: fact ? (fact.value === checkedValue ? Qt.Checked : Qt.Unchecked) : Qt.Unchecked
text: qsTr("Label")
onClicked: {
fact.value = checked ? checkedValue : uncheckedValue
}
onClicked: fact.value = checked ? checkedValue : uncheckedValue
}
......@@ -127,8 +127,9 @@ Fact* FactPanelController::getParameterFact(int componentId, const QString& name
QQmlEngine::setObjectOwnership(fact, QQmlEngine::CppOwnership);
return fact;
} else {
if(reportMissing)
if (reportMissing) {
_reportMissingParameter(componentId, name);
}
return NULL;
}
}
......
......@@ -735,6 +735,9 @@ void APMFirmwarePlugin::_artooSocketError(QAbstractSocket::SocketError socketErr
QString APMFirmwarePlugin::internalParameterMetaDataFile(Vehicle* vehicle)
{
int majorVersion = vehicle->firmwareMajorVersion();
int minorVersion = vehicle->firmwareMinorVersion();
switch (vehicle->vehicleType()) {
case MAV_TYPE_QUADROTOR:
case MAV_TYPE_HEXAROTOR:
......@@ -742,22 +745,57 @@ QString APMFirmwarePlugin::internalParameterMetaDataFile(Vehicle* vehicle)
case MAV_TYPE_TRICOPTER:
case MAV_TYPE_COAXIAL:
case MAV_TYPE_HELICOPTER:
if (vehicle->firmwareMajorVersion() < 3 || (vehicle->firmwareMajorVersion() == 3 && vehicle->firmwareMinorVersion() <= 3)) {
if (majorVersion < 3) {
return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.3.xml");
} else if (majorVersion == 3) {
switch (minorVersion) {
case 3:
return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.3.xml");
} else if (vehicle->firmwareMajorVersion() == 3 && vehicle->firmwareMinorVersion() == 4) {
case 4:
return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.4.xml");
} else {
case 5:
return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.5.xml");
default:
if (minorVersion < 3) {
return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.3.xml");
}
}
}
return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.5.xml");
case MAV_TYPE_FIXED_WING:
if (vehicle->firmwareMajorVersion() < 3 || (vehicle->firmwareMajorVersion() == 3 && vehicle->firmwareMinorVersion() <= 3)) {
if (majorVersion < 3) {
return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.3.xml");
} else {
} else if (majorVersion == 3) {
switch (minorVersion) {
case 3:
case 4:
return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.3.xml");
case 5:
case 6:
return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.5.xml");
case 7:
return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.7.xml");
default:
if (minorVersion < 3) {
return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.3.xml");
}
}
}
return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.8.xml");
case MAV_TYPE_GROUND_ROVER:
case MAV_TYPE_SURFACE_BOAT:
if (majorVersion < 3) {
return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Rover.3.0.xml");
} else if (majorVersion == 3) {
switch (minorVersion) {
case 0:
case 1:
return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Rover.3.0.xml");
default:
return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Rover.3.2.xml");
}
}
return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Rover.3.2.xml");
case MAV_TYPE_SUBMARINE:
return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Sub.3.4.xml");
default:
......
This source diff could not be displayed because it is too large. You can view the blob instead.
This source diff could not be displayed because it is too large. You can view the blob instead.
This source diff could not be displayed because it is too large. You can view the blob instead.
......@@ -43,10 +43,12 @@
<file alias="APMParameterFactMetaData.Plane.3.3.xml">APMParameterFactMetaData.Plane.3.3.xml</file>
<file alias="APMParameterFactMetaData.Plane.3.5.xml">APMParameterFactMetaData.Plane.3.5.xml</file>
<file alias="APMParameterFactMetaData.Plane.3.7.xml">APMParameterFactMetaData.Plane.3.7.xml</file>
<file alias="APMParameterFactMetaData.Plane.3.8.xml">APMParameterFactMetaData.Plane.3.8.xml</file>
<file alias="APMParameterFactMetaData.Copter.3.3.xml">APMParameterFactMetaData.Copter.3.3.xml</file>
<file alias="APMParameterFactMetaData.Copter.3.4.xml">APMParameterFactMetaData.Copter.3.4.xml</file>
<file alias="APMParameterFactMetaData.Copter.3.5.xml">APMParameterFactMetaData.Copter.3.5.xml</file>
<file alias="APMParameterFactMetaData.Rover.3.0.xml">APMParameterFactMetaData.Rover.3.0.xml</file>
<file alias="APMParameterFactMetaData.Rover.3.2.xml">APMParameterFactMetaData.Rover.3.2.xml</file>
<file alias="APMParameterFactMetaData.Sub.3.4.xml">APMParameterFactMetaData.Sub.3.4.xml</file>
<file alias="CopterGeoFenceEditor.qml">CopterGeoFenceEditor.qml</file>
<file alias="PlaneGeoFenceEditor.qml">PlaneGeoFenceEditor.qml</file>
......
......@@ -67,36 +67,51 @@ ArduCopterFirmwarePlugin::ArduCopterFirmwarePlugin(void)
setSupportedModes(supportedFlightModes);
if (!_remapParamNameIntialized) {
FirmwarePlugin::remapParamNameMap_t& remap = _remapParamName[3][4];
remap["ATC_ANG_RLL_P"] = QStringLiteral("STB_RLL_P");
remap["ATC_ANG_PIT_P"] = QStringLiteral("STB_PIT_P");
remap["ATC_ANG_YAW_P"] = QStringLiteral("STB_YAW_P");
remap["ATC_RAT_RLL_P"] = QStringLiteral("RATE_RLL_P");
remap["ATC_RAT_RLL_I"] = QStringLiteral("RATE_RLL_I");
remap["ATC_RAT_RLL_IMAX"] = QStringLiteral("RATE_RLL_IMAX");
remap["ATC_RAT_RLL_D"] = QStringLiteral("RATE_RLL_D");
remap["ATC_RAT_RLL_FILT"] = QStringLiteral("RATE_RLL_FILT_HZ");
remap["ATC_RAT_PIT_P"] = QStringLiteral("RATE_PIT_P");
remap["ATC_RAT_PIT_I"] = QStringLiteral("RATE_PIT_I");
remap["ATC_RAT_PIT_IMAX"] = QStringLiteral("RATE_PIT_IMAX");
remap["ATC_RAT_PIT_D"] = QStringLiteral("RATE_PIT_D");
remap["ATC_RAT_PIT_FILT"] = QStringLiteral("RATE_PIT_FILT_HZ");
remap["ATC_RAT_YAW_P"] = QStringLiteral("RATE_YAW_P");
remap["ATC_RAT_YAW_I"] = QStringLiteral("RATE_YAW_I");
remap["ATC_RAT_YAW_IMAX"] = QStringLiteral("RATE_YAW_IMAX");
remap["ATC_RAT_YAW_D"] = QStringLiteral("RATE_YAW_D");
remap["ATC_RAT_YAW_FILT"] = QStringLiteral("RATE_YAW_FILT_HZ");
FirmwarePlugin::remapParamNameMap_t& remapV3_4 = _remapParamName[3][4];
remapV3_4["ATC_ANG_RLL_P"] = QStringLiteral("STB_RLL_P");
remapV3_4["ATC_ANG_PIT_P"] = QStringLiteral("STB_PIT_P");
remapV3_4["ATC_ANG_YAW_P"] = QStringLiteral("STB_YAW_P");
remapV3_4["ATC_RAT_RLL_P"] = QStringLiteral("RATE_RLL_P");
remapV3_4["ATC_RAT_RLL_I"] = QStringLiteral("RATE_RLL_I");
remapV3_4["ATC_RAT_RLL_IMAX"] = QStringLiteral("RATE_RLL_IMAX");
remapV3_4["ATC_RAT_RLL_D"] = QStringLiteral("RATE_RLL_D");
remapV3_4["ATC_RAT_RLL_FILT"] = QStringLiteral("RATE_RLL_FILT_HZ");
remapV3_4["ATC_RAT_PIT_P"] = QStringLiteral("RATE_PIT_P");
remapV3_4["ATC_RAT_PIT_I"] = QStringLiteral("RATE_PIT_I");
remapV3_4["ATC_RAT_PIT_IMAX"] = QStringLiteral("RATE_PIT_IMAX");
remapV3_4["ATC_RAT_PIT_D"] = QStringLiteral("RATE_PIT_D");
remapV3_4["ATC_RAT_PIT_FILT"] = QStringLiteral("RATE_PIT_FILT_HZ");
remapV3_4["ATC_RAT_YAW_P"] = QStringLiteral("RATE_YAW_P");
remapV3_4["ATC_RAT_YAW_I"] = QStringLiteral("RATE_YAW_I");
remapV3_4["ATC_RAT_YAW_IMAX"] = QStringLiteral("RATE_YAW_IMAX");
remapV3_4["ATC_RAT_YAW_D"] = QStringLiteral("RATE_YAW_D");
remapV3_4["ATC_RAT_YAW_FILT"] = QStringLiteral("RATE_YAW_FILT_HZ");
FirmwarePlugin::remapParamNameMap_t& remapV3_5 = _remapParamName[3][5];
remapV3_5["SERVO5_FUNCTION"] = QStringLiteral("RC5_FUNCTION");
remapV3_5["SERVO6_FUNCTION"] = QStringLiteral("RC6_FUNCTION");
remapV3_5["SERVO7_FUNCTION"] = QStringLiteral("RC7_FUNCTION");
remapV3_5["SERVO8_FUNCTION"] = QStringLiteral("RC8_FUNCTION");
remapV3_5["SERVO9_FUNCTION"] = QStringLiteral("RC9_FUNCTION");
remapV3_5["SERVO10_FUNCTION"] = QStringLiteral("RC10_FUNCTION");
remapV3_5["SERVO11_FUNCTION"] = QStringLiteral("RC11_FUNCTION");
remapV3_5["SERVO12_FUNCTION"] = QStringLiteral("RC12_FUNCTION");
remapV3_5["SERVO13_FUNCTION"] = QStringLiteral("RC13_FUNCTION");
remapV3_5["SERVO14_FUNCTION"] = QStringLiteral("RC14_FUNCTION");
_remapParamNameIntialized = true;
}
}
int ArduCopterFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const
{
// Remapping supports up to 3.4
return majorVersionNumber == 3 ? 4: Vehicle::versionNotSetValue;
// Remapping supports up to 3.5
return majorVersionNumber == 3 ? 5 : Vehicle::versionNotSetValue;
}
bool ArduCopterFirmwarePlugin::isCapable(const Vehicle* vehicle, FirmwareCapabilities capabilities)
......
......@@ -7,12 +7,11 @@
*
****************************************************************************/
/// @file
/// @author Pritam Ghanghas <pritam.ghanghas@gmail.com>
#include "ArduPlaneFirmwarePlugin.h"
bool ArduPlaneFirmwarePlugin::_remapParamNameIntialized = false;
FirmwarePlugin::remapParamNameMajorVersionMap_t ArduPlaneFirmwarePlugin::_remapParamName;
APMPlaneMode::APMPlaneMode(uint32_t mode, bool settable)
: APMCustomMode(mode, settable)
{
......@@ -63,6 +62,29 @@ ArduPlaneFirmwarePlugin::ArduPlaneFirmwarePlugin(void)
supportedFlightModes << APMPlaneMode(APMPlaneMode::QLAND ,true);
supportedFlightModes << APMPlaneMode(APMPlaneMode::QRTL ,true);
setSupportedModes(supportedFlightModes);
if (!_remapParamNameIntialized) {
FirmwarePlugin::remapParamNameMap_t& remapV3_8 = _remapParamName[3][8];
remapV3_8["SERVO5_FUNCTION"] = QStringLiteral("RC5_FUNCTION");
remapV3_8["SERVO6_FUNCTION"] = QStringLiteral("RC6_FUNCTION");
remapV3_8["SERVO7_FUNCTION"] = QStringLiteral("RC7_FUNCTION");
remapV3_8["SERVO8_FUNCTION"] = QStringLiteral("RC8_FUNCTION");
remapV3_8["SERVO9_FUNCTION"] = QStringLiteral("RC9_FUNCTION");
remapV3_8["SERVO10_FUNCTION"] = QStringLiteral("RC10_FUNCTION");
remapV3_8["SERVO11_FUNCTION"] = QStringLiteral("RC11_FUNCTION");
remapV3_8["SERVO12_FUNCTION"] = QStringLiteral("RC12_FUNCTION");
remapV3_8["SERVO13_FUNCTION"] = QStringLiteral("RC13_FUNCTION");
remapV3_8["SERVO14_FUNCTION"] = QStringLiteral("RC14_FUNCTION");
_remapParamNameIntialized = true;
}
}
int ArduPlaneFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const
{
// Remapping supports up to 3.8
return majorVersionNumber == 3 ? 8 : Vehicle::versionNotSetValue;
}
QString ArduPlaneFirmwarePlugin::takeControlFlightMode(void)
......
......@@ -58,6 +58,12 @@ public:
// Overrides from FirmwarePlugin
QString offlineEditingParamFile(Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Plane.OfflineEditing.params"); }
QString takeControlFlightMode(void) final;
const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; }
int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final;
private:
static bool _remapParamNameIntialized;
static FirmwarePlugin::remapParamNameMajorVersionMap_t _remapParamName;
};
#endif
......@@ -7,12 +7,11 @@
*
****************************************************************************/
/// @file
/// @author Pritam Ghanghas <pritam.ghanghas@gmail.com>
#include "ArduRoverFirmwarePlugin.h"
bool ArduRoverFirmwarePlugin::_remapParamNameIntialized = false;
FirmwarePlugin::remapParamNameMajorVersionMap_t ArduRoverFirmwarePlugin::_remapParamName;
APMRoverMode::APMRoverMode(uint32_t mode, bool settable)
: APMCustomMode(mode, settable)
{
......@@ -41,4 +40,28 @@ ArduRoverFirmwarePlugin::ArduRoverFirmwarePlugin(void)
supportedFlightModes << APMRoverMode(APMRoverMode::GUIDED ,true);
supportedFlightModes << APMRoverMode(APMRoverMode::INITIALIZING ,false);
setSupportedModes(supportedFlightModes);
if (!_remapParamNameIntialized) {
FirmwarePlugin::remapParamNameMap_t& remapV3_2 = _remapParamName[3][2];
remapV3_2["SERVO5_FUNCTION"] = QStringLiteral("RC5_FUNCTION");
remapV3_2["SERVO6_FUNCTION"] = QStringLiteral("RC6_FUNCTION");
remapV3_2["SERVO7_FUNCTION"] = QStringLiteral("RC7_FUNCTION");
remapV3_2["SERVO8_FUNCTION"] = QStringLiteral("RC8_FUNCTION");
remapV3_2["SERVO9_FUNCTION"] = QStringLiteral("RC9_FUNCTION");
remapV3_2["SERVO10_FUNCTION"] = QStringLiteral("RC10_FUNCTION");
remapV3_2["SERVO11_FUNCTION"] = QStringLiteral("RC11_FUNCTION");
remapV3_2["SERVO12_FUNCTION"] = QStringLiteral("RC12_FUNCTION");
remapV3_2["SERVO13_FUNCTION"] = QStringLiteral("RC13_FUNCTION");
remapV3_2["SERVO14_FUNCTION"] = QStringLiteral("RC14_FUNCTION");
_remapParamNameIntialized = true;
}
}
int ArduRoverFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const
{
// Remapping supports up to 3.2
return majorVersionNumber == 3 ? 2 : Vehicle::versionNotSetValue;
}
......@@ -49,6 +49,14 @@ class ArduRoverFirmwarePlugin : public APMFirmwarePlugin
public:
ArduRoverFirmwarePlugin(void);
// Overrides from FirmwarePlugin
const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; }
int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final;
private:
static bool _remapParamNameIntialized;
static FirmwarePlugin::remapParamNameMajorVersionMap_t _remapParamName;
};
#endif
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment