Commit f966a3f6 authored by Don Gagne's avatar Don Gagne

VehicleSetup assures active as and parameters ready

parent ddde7cce
...@@ -161,13 +161,14 @@ void PX4RCCalibrationTest::init(void) ...@@ -161,13 +161,14 @@ void PX4RCCalibrationTest::init(void)
_mockUAS = new MockUAS(); _mockUAS = new MockUAS();
Q_CHECK_PTR(_mockUAS); Q_CHECK_PTR(_mockUAS);
// This will instatiate the widget with no active UAS set _mockUASManager->setMockActiveUAS(_mockUAS);
// This will instatiate the widget with an active uas with ready parameters
_calWidget = new PX4RCCalibration(); _calWidget = new PX4RCCalibration();
Q_CHECK_PTR(_calWidget); Q_CHECK_PTR(_calWidget);
_calWidget->_setUnitTestMode(); _calWidget->_setUnitTestMode();
_calWidget->setVisible(true); _calWidget->setVisible(true);
_mockUASManager->setMockActiveUAS(_mockUAS);
// Get pointers to the push buttons // Get pointers to the push buttons
_cancelButton = _calWidget->findChild<QPushButton*>("rcCalCancel"); _cancelButton = _calWidget->findChild<QPushButton*>("rcCalCancel");
...@@ -215,17 +216,6 @@ void PX4RCCalibrationTest::cleanup(void) ...@@ -215,17 +216,6 @@ void PX4RCCalibrationTest::cleanup(void)
} }
/// @brief Tests for correct behavior when active UAS is set into widget.
void PX4RCCalibrationTest::_setUAS_test(void)
{
// Widget is initialized with UAS, so it should be enabled
QCOMPARE(_calWidget->isEnabled(), true);
// Take away the UAS and widget should disable
_mockUASManager->setMockActiveUAS(NULL);
QCOMPARE(_calWidget->isEnabled(), false);
}
/// @brief Test for correct behavior in determining minimum numbers of channels for flight. /// @brief Test for correct behavior in determining minimum numbers of channels for flight.
void PX4RCCalibrationTest::_minRCChannels_test(void) void PX4RCCalibrationTest::_minRCChannels_test(void)
{ {
......
...@@ -48,7 +48,6 @@ private slots: ...@@ -48,7 +48,6 @@ private slots:
void init(void); void init(void);
void cleanup(void); void cleanup(void);
void _setUAS_test(void);
void _minRCChannels_test(void); void _minRCChannels_test(void);
void _fullCalibration_test(void); void _fullCalibration_test(void);
......
...@@ -86,7 +86,6 @@ PX4RCCalibration::PX4RCCalibration(QWidget *parent) : ...@@ -86,7 +86,6 @@ PX4RCCalibration::PX4RCCalibration(QWidget *parent) :
_rcCalState(rcCalStateChannelWait), _rcCalState(rcCalStateChannelWait),
_mav(NULL), _mav(NULL),
_paramMgr(NULL), _paramMgr(NULL),
_parameterListUpToDateSignalled(false),
_ui(new Ui::PX4RCCalibration), _ui(new Ui::PX4RCCalibration),
_unitTestMode(false) _unitTestMode(false)
{ {
...@@ -120,15 +119,21 @@ PX4RCCalibration::PX4RCCalibration(QWidget *parent) : ...@@ -120,15 +119,21 @@ PX4RCCalibration::PX4RCCalibration(QWidget *parent) :
_rgAttitudeControl[4].function = rcCalFunctionFlaps; _rgAttitudeControl[4].function = rcCalFunctionFlaps;
_rgAttitudeControl[4].valueWidget = _ui->flapsValue; _rgAttitudeControl[4].valueWidget = _ui->flapsValue;
_setActiveUAS(UASManager::instance()->getActiveUAS()); // This code assume we already have an active UAS with ready parameters
_mav = UASManager::instance()->getActiveUAS();
Q_ASSERT(_mav);
// Connect new signals
// Connect signals
bool fSucceeded; bool fSucceeded;
Q_UNUSED(fSucceeded); Q_UNUSED(fSucceeded);
fSucceeded = connect(_mav, SIGNAL(remoteControlChannelRawChanged(int,float)), this, SLOT(_remoteControlChannelRawChanged(int,float)));
fSucceeded = connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(_setActiveUAS(UASInterface*)));
Q_ASSERT(fSucceeded); Q_ASSERT(fSucceeded);
_paramMgr = _mav->getParamManager();
Q_ASSERT(_paramMgr);
Q_ASSERT(_paramMgr->parametersReady());
connect(_ui->spektrumBind, &QPushButton::clicked, this, &PX4RCCalibration::_spektrumBind); connect(_ui->spektrumBind, &QPushButton::clicked, this, &PX4RCCalibration::_spektrumBind);
_updateTimer.setInterval(150); _updateTimer.setInterval(150);
...@@ -159,6 +164,9 @@ PX4RCCalibration::PX4RCCalibration(QWidget *parent) : ...@@ -159,6 +164,9 @@ PX4RCCalibration::PX4RCCalibration(QWidget *parent) :
connect(_ui->mode2, &QAbstractButton::toggled, this, &PX4RCCalibration::_mode2Toggled); connect(_ui->mode2, &QAbstractButton::toggled, this, &PX4RCCalibration::_mode2Toggled);
_stopCalibration(); _stopCalibration();
connect(&_updateTimer, &QTimer::timeout, this, &PX4RCCalibration::_updateView);
_setInternalCalibrationValuesFromParameters();
} }
PX4RCCalibration::~PX4RCCalibration() PX4RCCalibration::~PX4RCCalibration()
...@@ -699,84 +707,82 @@ void PX4RCCalibration::_setInternalCalibrationValuesFromParameters(void) ...@@ -699,84 +707,82 @@ void PX4RCCalibration::_setInternalCalibrationValuesFromParameters(void)
{ {
Q_ASSERT(_paramMgr); Q_ASSERT(_paramMgr);
if (_parameterListUpToDateSignalled) { // Initialize all function mappings to not set
// Initialize all function mappings to not set
for (size_t i=0; i<_chanMax; i++) {
struct ChannelInfo* info = &_rgChannelInfo[i];
info->function = rcCalFunctionMax;
}
for (size_t i=0; i<rcCalFunctionMax; i++) {
_rgFunctionChannelMapping[i] = _chanMax;
}
// FIXME: Hardwired component id
// Pull parameters and update
QString minTpl("RC%1_MIN");
QString maxTpl("RC%1_MAX");
QString trimTpl("RC%1_TRIM");
QString revTpl("RC%1_REV");
QVariant value;
bool paramFound;
bool convertOk;
int componentId = _paramMgr->getDefaultComponentId();
for (int i = 0; i < _chanMax; ++i) {
struct ChannelInfo* info = &_rgChannelInfo[i];
for (size_t i=0; i<_chanMax; i++) { paramFound = _paramMgr->getParameterValue(componentId, trimTpl.arg(i+1), value);
struct ChannelInfo* info = &_rgChannelInfo[i]; Q_ASSERT(paramFound);
info->function = rcCalFunctionMax; if (paramFound) {
info->rcTrim = value.toInt(&convertOk);
Q_ASSERT(convertOk);
} }
for (size_t i=0; i<rcCalFunctionMax; i++) { paramFound = _paramMgr->getParameterValue(componentId, minTpl.arg(i+1), value);
_rgFunctionChannelMapping[i] = _chanMax; Q_ASSERT(paramFound);
if (paramFound) {
info->rcMin = value.toInt(&convertOk);
Q_ASSERT(convertOk);
} }
// FIXME: Hardwired component id
// Pull parameters and update
QString minTpl("RC%1_MIN");
QString maxTpl("RC%1_MAX");
QString trimTpl("RC%1_TRIM");
QString revTpl("RC%1_REV");
QVariant value;
bool paramFound;
bool convertOk;
int componentId = _paramMgr->getDefaultComponentId();
for (int i = 0; i < _chanMax; ++i) {
struct ChannelInfo* info = &_rgChannelInfo[i];
paramFound = _paramMgr->getParameterValue(componentId, trimTpl.arg(i+1), value);
Q_ASSERT(paramFound);
if (paramFound) {
info->rcTrim = value.toInt(&convertOk);
Q_ASSERT(convertOk);
}
paramFound = _paramMgr->getParameterValue(componentId, minTpl.arg(i+1), value);
Q_ASSERT(paramFound);
if (paramFound) {
info->rcMin = value.toInt(&convertOk);
Q_ASSERT(convertOk);
}
paramFound = _paramMgr->getParameterValue(componentId, maxTpl.arg(i+1), value); paramFound = _paramMgr->getParameterValue(componentId, maxTpl.arg(i+1), value);
Q_ASSERT(paramFound); Q_ASSERT(paramFound);
if (paramFound) { if (paramFound) {
info->rcMax = value.toInt(&convertOk); info->rcMax = value.toInt(&convertOk);
Q_ASSERT(convertOk); Q_ASSERT(convertOk);
} }
paramFound = _paramMgr->getParameterValue(componentId, revTpl.arg(i+1), value); paramFound = _paramMgr->getParameterValue(componentId, revTpl.arg(i+1), value);
Q_ASSERT(paramFound); Q_ASSERT(paramFound);
if (paramFound) { if (paramFound) {
float floatReversed = value.toFloat(&convertOk); float floatReversed = value.toFloat(&convertOk);
Q_ASSERT(convertOk); Q_ASSERT(convertOk);
Q_ASSERT(floatReversed == 1.0f || floatReversed == -1.0f); Q_ASSERT(floatReversed == 1.0f || floatReversed == -1.0f);
info->reversed = floatReversed == -1.0f; info->reversed = floatReversed == -1.0f;
}
} }
}
for (int i=0; i<rcCalFunctionMax; i++) {
int32_t paramChannel;
for (int i=0; i<rcCalFunctionMax; i++) { paramFound = _paramMgr->getParameterValue(componentId, _rgFunctionInfo[i].parameterName, value);
int32_t paramChannel; Q_ASSERT(paramFound);
if (paramFound) {
paramChannel = value.toInt(&convertOk);
Q_ASSERT(convertOk);
paramFound = _paramMgr->getParameterValue(componentId, _rgFunctionInfo[i].parameterName, value); if (paramChannel != 0) {
Q_ASSERT(paramFound); _rgFunctionChannelMapping[i] = paramChannel - 1;
if (paramFound) { _rgChannelInfo[paramChannel - 1].function = (enum rcCalFunctions)i;
paramChannel = value.toInt(&convertOk);
Q_ASSERT(convertOk);
if (paramChannel != 0) {
_rgFunctionChannelMapping[i] = paramChannel - 1;
_rgChannelInfo[paramChannel - 1].function = (enum rcCalFunctions)i;
}
} }
} }
_showMinMaxOnRadioWidgets(true);
_showTrimOnRadioWidgets(true);
} }
_showMinMaxOnRadioWidgets(true);
_showTrimOnRadioWidgets(true);
} }
/// @brief Sets a connected Spektrum receiver into bind mode /// @brief Sets a connected Spektrum receiver into bind mode
...@@ -811,38 +817,6 @@ void PX4RCCalibration::_spektrumBind(void) ...@@ -811,38 +817,6 @@ void PX4RCCalibration::_spektrumBind(void)
} }
} }
void PX4RCCalibration::_setActiveUAS(UASInterface* active)
{
// Disconnect old signals
if (_mav) {
disconnect(_mav, SIGNAL(remoteControlChannelRawChanged(int,float)), this, SLOT(_remoteControlChannelRawChanged(int,float)));
disconnect(_paramMgr, SIGNAL(parameterListUpToDate()), this, SLOT(_parameterListUpToDate()));
_paramMgr = NULL;
}
_mav = active;
if (_mav) {
// Connect new signals
bool fSucceeded;
Q_UNUSED(fSucceeded);
fSucceeded = connect(_mav, SIGNAL(remoteControlChannelRawChanged(int,float)), this, SLOT(_remoteControlChannelRawChanged(int,float)));
Q_ASSERT(fSucceeded);
_paramMgr = _mav->getParamManager();
Q_ASSERT(_paramMgr);
fSucceeded = connect(_paramMgr, SIGNAL(parameterListUpToDate()), this, SLOT(_parameterListUpToDate()));
Q_ASSERT(fSucceeded);
if (_paramMgr->parametersReady()) {
_parameterListUpToDate();
}
}
setEnabled(_mav ? true : false);
}
/// @brief Validates the current settings against the calibration rules resetting values as necessary. /// @brief Validates the current settings against the calibration rules resetting values as necessary.
void PX4RCCalibration::_validateCalibration(void) void PX4RCCalibration::_validateCalibration(void)
{ {
...@@ -1073,17 +1047,6 @@ void PX4RCCalibration::_showTrimOnRadioWidgets(bool show) ...@@ -1073,17 +1047,6 @@ void PX4RCCalibration::_showTrimOnRadioWidgets(bool show)
} }
} }
void PX4RCCalibration::_parameterListUpToDate(void)
{
_parameterListUpToDateSignalled = true;
// Don't start updating the view until we have parameters
connect(&_updateTimer, &QTimer::timeout, this, &PX4RCCalibration::_updateView);
if (_currentStep == -1) {
_setInternalCalibrationValuesFromParameters();
}
}
void PX4RCCalibration::_loadSettings(void) void PX4RCCalibration::_loadSettings(void)
{ {
......
...@@ -71,9 +71,6 @@ private slots: ...@@ -71,9 +71,6 @@ private slots:
void _updateView(void); void _updateView(void);
void _remoteControlChannelRawChanged(int chan, float val); void _remoteControlChannelRawChanged(int chan, float val);
void _setActiveUAS(UASInterface* uas);
void _parameterListUpToDate(void);
private: private:
/// @brief These identify the various controls functions. They are also used as indices into the _rgFunctioInfo /// @brief These identify the various controls functions. They are also used as indices into the _rgFunctioInfo
...@@ -257,8 +254,6 @@ private: ...@@ -257,8 +254,6 @@ private:
UASInterface* _mav; ///< The current MAV UASInterface* _mav; ///< The current MAV
QGCUASParamManagerInterface* _paramMgr; QGCUASParamManagerInterface* _paramMgr;
bool _parameterListUpToDateSignalled; ///< true: we have received a parameterListUpToDate signal
Ui::PX4RCCalibration* _ui; Ui::PX4RCCalibration* _ui;
QTimer _updateTimer; ///< Timer used to update widgete ui QTimer _updateTimer; ///< Timer used to update widgete ui
......
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