Lorenz Meier
12 years ago
6 changed files with 758 additions and 313 deletions
@ -0,0 +1,306 @@
@@ -0,0 +1,306 @@
|
||||
/*=====================================================================
|
||||
|
||||
QGroundControl Open Source Ground Control Station |
||||
|
||||
(c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
|
||||
This file is part of the QGROUNDCONTROL project |
||||
|
||||
QGROUNDCONTROL is free software: you can redistribute it and/or modify |
||||
it under the terms of the GNU General Public License as published by |
||||
the Free Software Foundation, either version 3 of the License, or |
||||
(at your option) any later version. |
||||
|
||||
QGROUNDCONTROL is distributed in the hope that it will be useful, |
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||
GNU General Public License for more details. |
||||
|
||||
You should have received a copy of the GNU General Public License |
||||
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
======================================================================*/ |
||||
|
||||
#include "FlightModeConfigTest.h" |
||||
#include "FlightModeConfig.h" |
||||
#include "UASManager.h" |
||||
#include "MockQGCUASParamManager.h" |
||||
|
||||
/// @file
|
||||
/// @brief FlightModeConfig unit test
|
||||
///
|
||||
/// @author Don Gagne <don@thegagnes.com>
|
||||
|
||||
FlightModeConfigUnitTest::FlightModeConfigUnitTest(void) : |
||||
_mockUASManager(NULL) |
||||
{ |
||||
|
||||
} |
||||
|
||||
void FlightModeConfigUnitTest::init(void) |
||||
{ |
||||
_mockUASManager = new MockUASManager(); |
||||
Q_ASSERT(_mockUASManager); |
||||
|
||||
UASManager::setMockUASManager(_mockUASManager); |
||||
|
||||
_mockUAS = new MockUAS(); |
||||
Q_ASSERT(_mockUAS); |
||||
} |
||||
|
||||
void FlightModeConfigUnitTest::cleanup(void) |
||||
{ |
||||
Q_ASSERT(_mockUAS); |
||||
delete _mockUAS; |
||||
|
||||
UASManager::setMockUASManager(NULL); |
||||
|
||||
Q_ASSERT(_mockUASManager); |
||||
delete _mockUASManager; |
||||
} |
||||
|
||||
void FlightModeConfigUnitTest::_nullUAS_test(void) |
||||
{ |
||||
// When there is no UAS the widget should be disabled
|
||||
FlightModeConfig* fmc = new FlightModeConfig(); |
||||
QCOMPARE(fmc->isEnabled(), false); |
||||
} |
||||
|
||||
void FlightModeConfigUnitTest::_validUAS_test(void) |
||||
{ |
||||
// With an active UAS widget should be enabled
|
||||
_mockUASManager->setMockActiveUAS(_mockUAS); |
||||
FlightModeConfig* fmc = new FlightModeConfig(); |
||||
QCOMPARE(fmc->isEnabled(), true); |
||||
} |
||||
|
||||
void FlightModeConfigUnitTest::_nullToValidUAS_test(void) |
||||
{ |
||||
// start with no UAS
|
||||
FlightModeConfig* fmc = new FlightModeConfig(); |
||||
|
||||
// clear it out and validate widget gets disabled
|
||||
_mockUASManager->setMockActiveUAS(NULL); |
||||
QCOMPARE(fmc->isEnabled(), false); |
||||
} |
||||
|
||||
void FlightModeConfigUnitTest::_simpleModeFixedWing_test(void) |
||||
{ |
||||
// Fixed wing does not have simple mode, all checkboxes should be disabled
|
||||
_mockUAS->setMockSystemType(MAV_TYPE_FIXED_WING); |
||||
_mockUASManager->setMockActiveUAS(_mockUAS); |
||||
FlightModeConfig* fmc = new FlightModeConfig(); |
||||
_findControls(fmc); |
||||
|
||||
for (size_t i=0; i<_cCombo; i++) { |
||||
QCOMPARE(_rgSimpleModeCheckBox[i]->isEnabled(), false); |
||||
} |
||||
} |
||||
|
||||
void FlightModeConfigUnitTest::_simpleModeRover_test(void) |
||||
{ |
||||
// Rover does not have simple mode, all checkboxes should be disabled
|
||||
_mockUAS->setMockSystemType(MAV_TYPE_GROUND_ROVER); |
||||
_mockUASManager->setMockActiveUAS(_mockUAS); |
||||
FlightModeConfig* fmc = new FlightModeConfig(); |
||||
_findControls(fmc); |
||||
|
||||
for (size_t i=0; i<_cCombo; i++) { |
||||
QCOMPARE(_rgSimpleModeCheckBox[i]->isEnabled(), false); |
||||
} |
||||
} |
||||
|
||||
void FlightModeConfigUnitTest::_simpleModeRotor_test(void) |
||||
{ |
||||
// Rotor has simple mode, all checkboxes should be enabled
|
||||
_mockUAS->setMockSystemType(MAV_TYPE_QUADROTOR); |
||||
_mockUASManager->setMockActiveUAS(_mockUAS); |
||||
FlightModeConfig* fmc = new FlightModeConfig(); |
||||
_findControls(fmc); |
||||
|
||||
for (size_t i=0; i<_cCombo; i++) { |
||||
QCOMPARE(_rgSimpleModeCheckBox[i]->isEnabled(), true); |
||||
} |
||||
} |
||||
|
||||
void FlightModeConfigUnitTest::_modeSwitchParam_test(void) |
||||
{ |
||||
_mockUAS->setMockSystemType(MAV_TYPE_QUADROTOR); |
||||
_mockUASManager->setMockActiveUAS(_mockUAS); |
||||
FlightModeConfig* fmc = new FlightModeConfig(); |
||||
_findControls(fmc); |
||||
|
||||
const char* rgModeSwitchParam[_cCombo] = { "FLTMODE1", "FLTMODE2", "FLTMODE3", "FLTMODE4", "FLTMODE5", "FLTMODE6" }; |
||||
const int rgModeSwitchValue[_cCombo] = { 0, 2, 4, 6, 8, 10 }; |
||||
|
||||
MockQGCUASParamManager::ParamMap_t mapParams; |
||||
for (size_t i=0; i<_cCombo; i++) { |
||||
mapParams[rgModeSwitchParam[i]] = QVariant(QChar(rgModeSwitchValue[i])); |
||||
} |
||||
|
||||
int simpleModeBitMask = 21; |
||||
mapParams["SIMPLE"] = QVariant(QChar(simpleModeBitMask)); |
||||
_mockUAS->setMockParametersAndSignal(mapParams); |
||||
|
||||
// Check that the UI is showing the correct information
|
||||
for (size_t i=0; i<_cCombo; i++) { |
||||
QComboBox* combo = _rgCombo[i]; |
||||
|
||||
// Check for the correct selection in the combo boxes. Combo boxes store the mode
|
||||
// in the item data, so use that to compare
|
||||
QCOMPARE(combo->itemData(combo->currentIndex()), mapParams[rgModeSwitchParam[i]]); |
||||
|
||||
// Make sure the text for the current selection doesn't contain the text Unknown
|
||||
// which means that it received an unsupported mode
|
||||
QCOMPARE(combo->currentText().contains("unknown", Qt::CaseInsensitive), QBool(false)); |
||||
|
||||
// Check that the right simple mode check boxes are checked
|
||||
QCOMPARE(_rgSimpleModeCheckBox[i]->isChecked(), !!((1 << i) & simpleModeBitMask)); |
||||
} |
||||
|
||||
|
||||
// Click Save button and make sure we get the same values back through setParameter calls
|
||||
QTest::mouseClick(_saveButton, Qt::LeftButton); |
||||
MockQGCUASParamManager* paramMgr = _mockUAS->getMockQGCUASParamManager(); |
||||
MockQGCUASParamManager::ParamMap_t mapParamsSet = paramMgr->getMockSetParameters(); |
||||
QMapIterator<QString, QVariant> i(mapParams); |
||||
while (i.hasNext()) { |
||||
i.next(); |
||||
QCOMPARE(mapParamsSet.contains(i.key()), true); |
||||
int receivedValue = mapParamsSet[i.key()].toInt(); |
||||
int expectedValue = i.value().toInt(); |
||||
QCOMPARE(receivedValue, expectedValue); |
||||
} |
||||
} |
||||
|
||||
void FlightModeConfigUnitTest::_pwmTestWorker(int systemType, int modeSwitchChannel, const char* modeSwitchParam) |
||||
{ |
||||
_mockUAS->setMockSystemType(systemType); |
||||
_mockUASManager->setMockActiveUAS(_mockUAS); |
||||
FlightModeConfig* fmc = new FlightModeConfig(); |
||||
_findControls(fmc); |
||||
|
||||
MockQGCUASParamManager::ParamMap_t mapParams; |
||||
if (modeSwitchParam != NULL) { |
||||
// Param is 1-based, code in here is all 0-base
|
||||
mapParams[modeSwitchParam] = QVariant(QChar(modeSwitchChannel+1)); |
||||
_mockUAS->setMockParametersAndSignal(mapParams); |
||||
} |
||||
|
||||
const int pwmBoundary[] = { 1230, 1360, 1490, 1620, 1749, 1900 }; |
||||
|
||||
int lowerPWM = 0; |
||||
for (size_t i=0; i<_cCombo; i++) { |
||||
// emit a PWM value at the mid point of the switch position
|
||||
int pwmMidPoint = ((pwmBoundary[i] - lowerPWM) / 2) + lowerPWM; |
||||
_mockUAS->emitRemoteControlChannelRawChanged(modeSwitchChannel, pwmMidPoint); |
||||
|
||||
// Make sure that only the correct pwm label has a style set on it for highlight
|
||||
for (size_t j=0; j<_cCombo; j++) { |
||||
if (j == i) { |
||||
QVERIFY(_rgPWMLabel[j]->styleSheet().length() > 0); |
||||
} else { |
||||
QCOMPARE(_rgPWMLabel[j]->styleSheet().length(), 0); |
||||
} |
||||
} |
||||
|
||||
lowerPWM = pwmBoundary[i]; |
||||
} |
||||
} |
||||
|
||||
void FlightModeConfigUnitTest::_pwmFixedWing_test(void) |
||||
{ |
||||
// FixedWing has mode switch channel on FLTMODE_CH param
|
||||
_pwmTestWorker(MAV_TYPE_FIXED_WING, 7, "FLTMODE_CH"); |
||||
} |
||||
|
||||
void FlightModeConfigUnitTest::_pwmRotor_test(void) |
||||
{ |
||||
// Rotor is hardwired to 0-based rc channel 4 for mode wsitch
|
||||
_pwmTestWorker(MAV_TYPE_QUADROTOR, 4, NULL); |
||||
} |
||||
|
||||
void FlightModeConfigUnitTest::_pwmRover_test(void) |
||||
{ |
||||
// Rover has mode switch channel on MODE_CH param
|
||||
_pwmTestWorker(MAV_TYPE_GROUND_ROVER, 7, "MODE_CH"); |
||||
} |
||||
|
||||
void FlightModeConfigUnitTest::_pwmInvalidChannel_test(void) |
||||
{ |
||||
// Rover has mode switch channel on MODE_CH param
|
||||
_mockUAS->setMockSystemType(MAV_TYPE_GROUND_ROVER); |
||||
_mockUASManager->setMockActiveUAS(_mockUAS); |
||||
FlightModeConfig* fmc = new FlightModeConfig(); |
||||
_findControls(fmc); |
||||
|
||||
int modeSwitchChannel = 7; |
||||
MockQGCUASParamManager::ParamMap_t mapParams; |
||||
mapParams["MODE_CH"] = QVariant(QChar(modeSwitchChannel+1)); // 1-based
|
||||
_mockUAS->setMockParametersAndSignal(mapParams); |
||||
|
||||
const int pwmBoundary[] = { 1230, 1360, 1490, 1620, 1749, 1900 }; |
||||
|
||||
int lowerPWM = 0; |
||||
for (size_t i=0; i<_cCombo; i++) { |
||||
// emit a PWM value at the mid point of the switch position
|
||||
int pwmMidPoint = ((pwmBoundary[i] - lowerPWM) / 2) + lowerPWM; |
||||
// emit rc values on wrong channel
|
||||
_mockUAS->emitRemoteControlChannelRawChanged(modeSwitchChannel-1, pwmMidPoint); |
||||
|
||||
// Make sure no label have a style set on it for highlight
|
||||
for (size_t j=0; j<_cCombo; j++) { |
||||
QCOMPARE(_rgPWMLabel[j]->styleSheet().length(), 0); |
||||
} |
||||
|
||||
lowerPWM = pwmBoundary[i]; |
||||
} |
||||
} |
||||
|
||||
void FlightModeConfigUnitTest::_unknownSystemType_test(void) |
||||
{ |
||||
// Set the system type to something we can't handle, make sure we are disabled
|
||||
_mockUAS->setMockSystemType(MAV_TYPE_ENUM_END); |
||||
_mockUASManager->setMockActiveUAS(_mockUAS); |
||||
FlightModeConfig* fmc = new FlightModeConfig(); |
||||
QCOMPARE(fmc->isEnabled(), false); |
||||
} |
||||
|
||||
void FlightModeConfigUnitTest::_unknownMode_test(void) |
||||
{ |
||||
_mockUAS->setMockSystemType(MAV_TYPE_QUADROTOR); |
||||
_mockUASManager->setMockActiveUAS(_mockUAS); |
||||
FlightModeConfig* fmc = new FlightModeConfig(); |
||||
_findControls(fmc); |
||||
|
||||
// Set an unknown mode
|
||||
MockQGCUASParamManager::ParamMap_t mapParams; |
||||
mapParams["FLTMODE1"] = QVariant(QChar(100)); |
||||
_mockUAS->setMockParametersAndSignal(mapParams); |
||||
|
||||
// Check for the correct selection in the combo boxes. Combo boxes store the mode
|
||||
// in the item data, so use that to compare
|
||||
QCOMPARE(_rgCombo[0]->itemData(_rgCombo[0]->currentIndex()), mapParams["FLTMODE1"]); |
||||
|
||||
// Make sure the text for the current selection contains the text Unknown
|
||||
QCOMPARE(_rgCombo[0]->currentText().contains("unknown", Qt::CaseInsensitive), QBool(true)); |
||||
} |
||||
|
||||
void FlightModeConfigUnitTest::_findControls(QObject* fmc) |
||||
{ |
||||
// Find all the controls
|
||||
for (size_t i=0; i<_cCombo; i++) { |
||||
_rgLabel[i] = fmc->findChild<QLabel*>(QString("mode%1Label").arg(i)); |
||||
_rgCombo[i] = fmc->findChild<QComboBox*>(QString("mode%1ComboBox").arg(i)); |
||||
_rgSimpleModeCheckBox[i] = fmc->findChild<QCheckBox*>(QString("mode%1SimpleCheckBox").arg(i)); |
||||
_rgPWMLabel[i] = fmc->findChild<QLabel*>(QString("mode%1PWMLabel").arg(i)); |
||||
Q_ASSERT(_rgLabel[i]); |
||||
Q_ASSERT(_rgCombo[i]); |
||||
Q_ASSERT(_rgSimpleModeCheckBox[i]); |
||||
Q_ASSERT(_rgPWMLabel[i]); |
||||
} |
||||
|
||||
_saveButton = fmc->findChild<QPushButton*>("savePushButton"); |
||||
Q_ASSERT(_saveButton); |
||||
} |
||||
|
||||
@ -0,0 +1,87 @@
@@ -0,0 +1,87 @@
|
||||
/*=====================================================================
|
||||
|
||||
QGroundControl Open Source Ground Control Station |
||||
|
||||
(c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
|
||||
This file is part of the QGROUNDCONTROL project |
||||
|
||||
QGROUNDCONTROL is free software: you can redistribute it and/or modify |
||||
it under the terms of the GNU General Public License as published by |
||||
the Free Software Foundation, either version 3 of the License, or |
||||
(at your option) any later version. |
||||
|
||||
QGROUNDCONTROL is distributed in the hope that it will be useful, |
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||
GNU General Public License for more details. |
||||
|
||||
You should have received a copy of the GNU General Public License |
||||
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
======================================================================*/ |
||||
|
||||
#ifndef FLIGHTMODECONFIGTEST_H |
||||
#define FLIGHTMODECONFIGTEST_H |
||||
|
||||
#include "AutoTest.h" |
||||
#include "MockUASManager.h" |
||||
#include "MockUAS.h" |
||||
#include <QLabel> |
||||
#include <QComboBox> |
||||
#include <QCheckBox> |
||||
#include <QLabel> |
||||
#include <QPushButton> |
||||
|
||||
class FlightModeConfig; |
||||
|
||||
/// @file
|
||||
/// @brief FlightModeConfig unit test
|
||||
///
|
||||
/// @author Don Gagne <don@thegagnes.com>
|
||||
|
||||
class FlightModeConfigUnitTest : public QObject |
||||
{ |
||||
Q_OBJECT |
||||
|
||||
public: |
||||
FlightModeConfigUnitTest(void); |
||||
|
||||
private slots: |
||||
void init(void); |
||||
void cleanup(void); |
||||
|
||||
void _nullUAS_test(void); |
||||
void _validUAS_test(void); |
||||
void _nullToValidUAS_test(void); |
||||
void _simpleModeFixedWing_test(void); |
||||
void _simpleModeRover_test(void); |
||||
void _simpleModeRotor_test(void); |
||||
void _modeSwitchParam_test(void); |
||||
void _pwmFixedWing_test(void); |
||||
void _pwmRotor_test(void); |
||||
void _pwmRover_test(void); |
||||
void _pwmInvalidChannel_test(void); |
||||
void _unknownSystemType_test(void); |
||||
void _unknownMode_test(void); |
||||
|
||||
private: |
||||
void _findControls(QObject* fmc); |
||||
void _pwmTestWorker(int systemType, int modeSwitchChannel, const char* modeSwitchParam); |
||||
|
||||
private: |
||||
MockUASManager* _mockUASManager; |
||||
MockUAS* _mockUAS; |
||||
|
||||
// FlightModeConfig ui elements
|
||||
static const size_t _cCombo = 6; |
||||
QLabel* _rgLabel[_cCombo]; |
||||
QComboBox* _rgCombo[_cCombo]; |
||||
QCheckBox* _rgSimpleModeCheckBox[_cCombo]; |
||||
QLabel* _rgPWMLabel[_cCombo]; |
||||
QPushButton* _saveButton; |
||||
}; |
||||
|
||||
DECLARE_TEST(FlightModeConfigUnitTest) |
||||
|
||||
#endif |
||||
@ -1,307 +1,287 @@
@@ -1,307 +1,287 @@
|
||||
/*=====================================================================
|
||||
|
||||
QGroundControl Open Source Ground Control Station |
||||
|
||||
(c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
|
||||
This file is part of the QGROUNDCONTROL project |
||||
|
||||
QGROUNDCONTROL is free software: you can redistribute it and/or modify |
||||
it under the terms of the GNU General Public License as published by |
||||
the Free Software Foundation, either version 3 of the License, or |
||||
(at your option) any later version. |
||||
|
||||
QGROUNDCONTROL is distributed in the hope that it will be useful, |
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||
GNU General Public License for more details. |
||||
|
||||
You should have received a copy of the GNU General Public License |
||||
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
======================================================================*/ |
||||
|
||||
#include "FlightModeConfig.h" |
||||
|
||||
// We used the _rgModeInfo* arrays to populate the combo box choices. The numeric value
|
||||
// is the flight mode value that corresponds to the label. We store that value in the
|
||||
// combo box item data. There is a different set or each vehicle type.
|
||||
|
||||
FlightModeConfig::FlightModeConfig(QWidget *parent) : AP2ConfigWidget(parent) |
||||
{ |
||||
ui.setupUi(this); |
||||
connect(ui.savePushButton,SIGNAL(clicked()),this,SLOT(saveButtonClicked())); |
||||
initConnections(); |
||||
} |
||||
const FlightModeConfig::FlightModeInfo_t FlightModeConfig::_rgModeInfoFixedWing[] = { |
||||
{ "Manual", 0 }, |
||||
{ "Circle", 1 }, |
||||
{ "Stabilize", 2 }, |
||||
{ "Training", 3 }, |
||||
{ "FBW A", 5 }, |
||||
{ "FBW B", 6 }, |
||||
{ "Auto", 10 }, |
||||
{ "RTL", 11 }, |
||||
{ "Loiter", 12 }, |
||||
{ "Guided", 15 }, |
||||
}; |
||||
|
||||
FlightModeConfig::~FlightModeConfig() |
||||
{ |
||||
} |
||||
void FlightModeConfig::activeUASSet(UASInterface *uas) |
||||
{ |
||||
if (m_uas) |
||||
{ |
||||
disconnect(m_uas,SIGNAL(modeChanged(int,QString,QString)),this,SLOT(modeChanged(int,QString,QString))); |
||||
disconnect(m_uas,SIGNAL(remoteControlChannelRawChanged(int,float)),this,SLOT(remoteControlChannelRawChanged(int,float))); |
||||
} |
||||
AP2ConfigWidget::activeUASSet(uas); |
||||
if (!uas) return; |
||||
connect(m_uas,SIGNAL(modeChanged(int,QString,QString)),this,SLOT(modeChanged(int,QString,QString))); |
||||
connect(m_uas,SIGNAL(remoteControlChannelRawChanged(int,float)),this,SLOT(remoteControlChannelRawChanged(int,float))); |
||||
QStringList itemlist; |
||||
if (m_uas->getSystemType() == MAV_TYPE_FIXED_WING) |
||||
{ |
||||
itemlist << "Manual"; |
||||
itemlist << "Circle"; |
||||
itemlist << "Stabilize"; |
||||
itemlist << "Training"; |
||||
itemlist << "FBW A"; |
||||
itemlist << "FBW B"; |
||||
itemlist << "Auto"; |
||||
itemlist << "RTL"; |
||||
itemlist << "Loiter"; |
||||
itemlist << "Guided"; |
||||
const FlightModeConfig::FlightModeInfo_t FlightModeConfig::_rgModeInfoRotor[] = { |
||||
{ "Stabilize", 0 }, |
||||
{ "Acro", 1 }, |
||||
{ "Alt Hold", 2 }, |
||||
{ "Auto", 3 }, |
||||
{ "Guided", 4 }, |
||||
{ "Loiter", 5 }, |
||||
{ "RTL", 6 }, |
||||
{ "Circle", 7 }, |
||||
{ "Position", 8 }, |
||||
{ "Land", 9 }, |
||||
{ "OF_Loiter", 10 }, |
||||
{ "Toy A", 11 }, |
||||
{ "Toy M", 12 }, |
||||
}; |
||||
|
||||
planeModeIndexToUiIndex[0] = 0; |
||||
planeModeUiIndexToIndex[0] = 0; |
||||
const FlightModeConfig::FlightModeInfo_t FlightModeConfig::_rgModeInfoRover[] = { |
||||
{ "Manual", 0 }, |
||||
{ "Learning", 2 }, |
||||
{ "Steering", 3 }, |
||||
{ "Hold", 4 }, |
||||
{ "Auto", 10 }, |
||||
{ "RTL", 11 }, |
||||
{ "Guided", 15 }, |
||||
}; |
||||
|
||||
planeModeIndexToUiIndex[1] = 1; |
||||
planeModeUiIndexToIndex[1] = 1; |
||||
// We use the _rgModeParam* arrays to store the parameter names for each selectable
|
||||
// flight mode. The order of these corresponds to the combox box order as well. Array
|
||||
// element 0, is the parameter for mode0ComboBox, array element 1 = mode1ComboBox and
|
||||
// so on. The number of elements in the array also determines how many combo boxes we
|
||||
// are going to need. Different vehicle types have different numbers of selectable
|
||||
// flight modes.
|
||||
|
||||
planeModeIndexToUiIndex[2] = 2; |
||||
planeModeUiIndexToIndex[2] = 2; |
||||
const char* FlightModeConfig::_rgModeParamFixedWing[_cModes] = { "FLTMODE1", "FLTMODE2", "FLTMODE3", "FLTMODE4", "FLTMODE5", "FLTMODE6" }; |
||||
|
||||
planeModeIndexToUiIndex[3] = 3; |
||||
planeModeUiIndexToIndex[3] = 3; |
||||
const char* FlightModeConfig::_rgModeParamRotor[_cModes] = { "FLTMODE1", "FLTMODE2", "FLTMODE3", "FLTMODE4", "FLTMODE5", "FLTMODE6" }; |
||||
|
||||
planeModeIndexToUiIndex[5] = 4; |
||||
planeModeUiIndexToIndex[4] = 5; |
||||
const char* FlightModeConfig::_rgModeParamRover[_cModes] = { "MODE1", "MODE2", "MODE3", "MODE4", "MODE5", "MODE6" }; |
||||
|
||||
planeModeIndexToUiIndex[6] = 5; |
||||
planeModeUiIndexToIndex[5] = 6; |
||||
// Parameter which contains simple mode bitmask
|
||||
const char* FlightModeConfig::_simpleModeBitMaskParam = "SIMPLE"; |
||||
|
||||
planeModeIndexToUiIndex[10] = 6; |
||||
planeModeUiIndexToIndex[6] = 10; |
||||
// Parameter which controls which RC channel mode switching is on.
|
||||
// ArduCopter is hardcoded so no param
|
||||
const char* FlightModeConfig::_modeSwitchRCChannelParamFixedWing = "FLTMODE_CH"; |
||||
const char* FlightModeConfig::_modeSwitchRCChannelParamRover = "MODE_CH"; |
||||
|
||||
planeModeIndexToUiIndex[11] = 7; |
||||
planeModeUiIndexToIndex[7] = 11; |
||||
// PWM values which are the boundaries between each mode selection
|
||||
const int FlightModeConfig::_rgModePWMBoundary[_cModes] = { 1230, 1360, 1490, 1620, 1749, 20000 }; |
||||
|
||||
planeModeIndexToUiIndex[12] = 8; |
||||
planeModeUiIndexToIndex[8] = 12; |
||||
|
||||
planeModeIndexToUiIndex[15] = 9; |
||||
planeModeUiIndexToIndex[9] = 15; |
||||
|
||||
ui.mode6ComboBox->setEnabled(true); |
||||
FlightModeConfig::FlightModeConfig(QWidget *parent) : |
||||
AP2ConfigWidget(parent), |
||||
_rgModeInfo(NULL), |
||||
_cModeInfo(0), |
||||
_rgModeParam(NULL), |
||||
_simpleModeSupported(false), |
||||
_modeSwitchRCChannel(_modeSwitchRCChannelInvalid) |
||||
{ |
||||
_ui.setupUi(this); |
||||
|
||||
// Find all the widgets we are going to programmatically control
|
||||
for (size_t i=0; i<_cModes; i++) { |
||||
_rgLabel[i] = findChild<QLabel*>(QString("mode%1Label").arg(i)); |
||||
_rgCombo[i] = findChild<QComboBox*>(QString("mode%1ComboBox").arg(i)); |
||||
_rgSimpleModeCheckBox[i] = findChild<QCheckBox*>(QString("mode%1SimpleCheckBox").arg(i)); |
||||
_rgPWMLabel[i] = findChild<QLabel*>(QString("mode%1PWMLabel").arg(i)); |
||||
Q_ASSERT(_rgLabel[i]); |
||||
Q_ASSERT(_rgCombo[i]); |
||||
Q_ASSERT(_rgSimpleModeCheckBox[i]); |
||||
Q_ASSERT(_rgPWMLabel[i]); |
||||
} |
||||
else if (m_uas->getSystemType() == MAV_TYPE_GROUND_ROVER) |
||||
{ |
||||
itemlist << "Manual"; |
||||
itemlist << "Learning"; |
||||
itemlist << "Steering"; |
||||
itemlist << "Hold"; |
||||
itemlist << "Auto"; |
||||
itemlist << "RTL"; |
||||
itemlist << "Guided"; |
||||
itemlist << "Initialising"; |
||||
ui.mode6ComboBox->setEnabled(false); |
||||
roverModeIndexToUiIndex[0] = 0; |
||||
roverModeUiIndexToIndex[0] = 0; |
||||
|
||||
roverModeIndexToUiIndex[2] = 1; |
||||
roverModeUiIndexToIndex[1] = 2; |
||||
|
||||
roverModeIndexToUiIndex[3] = 2; |
||||
roverModeUiIndexToIndex[2] = 3; |
||||
|
||||
roverModeIndexToUiIndex[4] = 3; |
||||
roverModeUiIndexToIndex[3] = 4; |
||||
|
||||
roverModeIndexToUiIndex[10] = 5; |
||||
roverModeUiIndexToIndex[5] = 10; |
||||
|
||||
roverModeIndexToUiIndex[11] = 6; |
||||
roverModeUiIndexToIndex[6] = 11; |
||||
|
||||
roverModeIndexToUiIndex[15] = 7; |
||||
roverModeUiIndexToIndex[7] = 15; |
||||
|
||||
roverModeIndexToUiIndex[16] = 8; |
||||
roverModeUiIndexToIndex[8] = 16; |
||||
|
||||
// Start disabled until we get a UAS
|
||||
setEnabled(false); |
||||
|
||||
connect(_ui.savePushButton, SIGNAL(clicked()), this, SLOT(saveButtonClicked())); |
||||
initConnections(); |
||||
} |
||||
|
||||
void FlightModeConfig::activeUASSet(UASInterface *uas) |
||||
{ |
||||
// Clear the highlighting on PWM labels
|
||||
for (size_t i=0; i<_cModes; i++) { |
||||
_rgPWMLabel[i]->setStyleSheet(QString("")); |
||||
} |
||||
else if (m_uas->getSystemType() == MAV_TYPE_QUADROTOR) |
||||
|
||||
// Disconnect from old UAS
|
||||
if (m_uas) |
||||
{ |
||||
itemlist << "Stabilize"; |
||||
itemlist << "Acro"; |
||||
itemlist << "Alt Hold"; |
||||
itemlist << "Auto"; |
||||
itemlist << "Guided"; |
||||
itemlist << "Loiter"; |
||||
itemlist << "RTL"; |
||||
itemlist << "Circle"; |
||||
itemlist << "Pos Hold"; |
||||
itemlist << "Land"; |
||||
itemlist << "OF_LOITER"; |
||||
itemlist << "Toy"; |
||||
ui.mode6ComboBox->setEnabled(true); |
||||
disconnect(m_uas, SIGNAL(remoteControlChannelRawChanged(int, float)), this, SLOT(remoteControlChannelRawChanged(int, float))); |
||||
} |
||||
ui.mode1ComboBox->addItems(itemlist); |
||||
ui.mode2ComboBox->addItems(itemlist); |
||||
ui.mode3ComboBox->addItems(itemlist); |
||||
ui.mode4ComboBox->addItems(itemlist); |
||||
ui.mode5ComboBox->addItems(itemlist); |
||||
ui.mode6ComboBox->addItems(itemlist); |
||||
} |
||||
void FlightModeConfig::modeChanged(int sysId, QString status, QString description) |
||||
{ |
||||
//Unused?
|
||||
} |
||||
void FlightModeConfig::saveButtonClicked() |
||||
{ |
||||
if (m_uas->getSystemType() == MAV_TYPE_FIXED_WING) |
||||
{ |
||||
m_uas->getParamManager()->setParameter(1,"FLTMODE1",(char)planeModeUiIndexToIndex[ui.mode1ComboBox->currentIndex()]); |
||||
m_uas->getParamManager()->setParameter(1,"FLTMODE2",(char)planeModeUiIndexToIndex[ui.mode2ComboBox->currentIndex()]); |
||||
m_uas->getParamManager()->setParameter(1,"FLTMODE3",(char)planeModeUiIndexToIndex[ui.mode3ComboBox->currentIndex()]); |
||||
m_uas->getParamManager()->setParameter(1,"FLTMODE4",(char)planeModeUiIndexToIndex[ui.mode4ComboBox->currentIndex()]); |
||||
m_uas->getParamManager()->setParameter(1,"FLTMODE5",(char)planeModeUiIndexToIndex[ui.mode5ComboBox->currentIndex()]); |
||||
m_uas->getParamManager()->setParameter(1,"FLTMODE6",(char)planeModeUiIndexToIndex[ui.mode6ComboBox->currentIndex()]); |
||||
|
||||
// Connect to new UAS (if any)
|
||||
AP2ConfigWidget::activeUASSet(uas); |
||||
if (!uas) { |
||||
setEnabled(false); |
||||
return; |
||||
} |
||||
else if (m_uas->getSystemType() == MAV_TYPE_GROUND_ROVER) |
||||
{ |
||||
m_uas->getParamManager()->setParameter(1,"MODE1",(char)roverModeUiIndexToIndex[ui.mode1ComboBox->currentIndex()]); |
||||
m_uas->getParamManager()->setParameter(1,"MODE2",(char)roverModeUiIndexToIndex[ui.mode2ComboBox->currentIndex()]); |
||||
m_uas->getParamManager()->setParameter(1,"MODE3",(char)roverModeUiIndexToIndex[ui.mode3ComboBox->currentIndex()]); |
||||
m_uas->getParamManager()->setParameter(1,"MODE4",(char)roverModeUiIndexToIndex[ui.mode4ComboBox->currentIndex()]); |
||||
m_uas->getParamManager()->setParameter(1,"MODE5",(char)roverModeUiIndexToIndex[ui.mode5ComboBox->currentIndex()]); |
||||
connect(m_uas, SIGNAL(remoteControlChannelRawChanged(int, float)), this, SLOT(remoteControlChannelRawChanged(int, float))); |
||||
setEnabled(true); |
||||
|
||||
// Select the correct set of Flight Modes and Flight Mode Parameters. If the rc channel
|
||||
// associated with the mode switch is settable through a param, it is invalid until
|
||||
// we get that param through.
|
||||
switch (m_uas->getSystemType()) { |
||||
case MAV_TYPE_FIXED_WING: |
||||
_rgModeInfo = &_rgModeInfoFixedWing[0]; |
||||
_cModeInfo = sizeof(_rgModeInfoFixedWing)/sizeof(_rgModeInfoFixedWing[0]); |
||||
_rgModeParam = _rgModeParamFixedWing; |
||||
_simpleModeSupported = false; |
||||
_modeSwitchRCChannelParam = _modeSwitchRCChannelParamFixedWing; |
||||
_modeSwitchRCChannel = _modeSwitchRCChannelInvalid; |
||||
break; |
||||
case MAV_TYPE_GROUND_ROVER: |
||||
_rgModeInfo = &_rgModeInfoRover[0]; |
||||
_cModeInfo = sizeof(_rgModeInfoRover)/sizeof(_rgModeInfoRover[0]); |
||||
_rgModeParam = _rgModeParamRover; |
||||
_simpleModeSupported = false; |
||||
_modeSwitchRCChannelParam = _modeSwitchRCChannelParamRover; |
||||
_modeSwitchRCChannel = _modeSwitchRCChannelInvalid; |
||||
break; |
||||
case MAV_TYPE_QUADROTOR: |
||||
case MAV_TYPE_TRICOPTER: |
||||
case MAV_TYPE_HEXAROTOR: |
||||
case MAV_TYPE_OCTOROTOR: |
||||
case MAV_TYPE_HELICOPTER: |
||||
_rgModeInfo = &_rgModeInfoRotor[0]; |
||||
_cModeInfo = sizeof(_rgModeInfoRotor)/sizeof(_rgModeInfoRotor[0]); |
||||
_rgModeParam = _rgModeParamRotor; |
||||
_simpleModeSupported = true; |
||||
_modeSwitchRCChannelParam = NULL; |
||||
_modeSwitchRCChannel = _modeSwitchRCChannelRotor; // Rotor is harcoded
|
||||
break; |
||||
default: |
||||
// We've gotten a mav type we can't handle, just disable to whole thing
|
||||
qDebug() << QString("Unknown System Type %1").arg(m_uas->getSystemType()); |
||||
setEnabled(false); |
||||
return; |
||||
} |
||||
else if (m_uas->getSystemType() == MAV_TYPE_QUADROTOR) |
||||
{ |
||||
m_uas->getParamManager()->setParameter(1,"FLTMODE1",(char)ui.mode1ComboBox->currentIndex()); |
||||
m_uas->getParamManager()->setParameter(1,"FLTMODE2",(char)ui.mode2ComboBox->currentIndex()); |
||||
m_uas->getParamManager()->setParameter(1,"FLTMODE3",(char)ui.mode3ComboBox->currentIndex()); |
||||
m_uas->getParamManager()->setParameter(1,"FLTMODE4",(char)ui.mode4ComboBox->currentIndex()); |
||||
m_uas->getParamManager()->setParameter(1,"FLTMODE5",(char)ui.mode5ComboBox->currentIndex()); |
||||
m_uas->getParamManager()->setParameter(1,"FLTMODE6",(char)ui.mode6ComboBox->currentIndex()); |
||||
|
||||
// Set up the combo boxes
|
||||
for (size_t i=0; i<_cModes; i++) { |
||||
// Fill each combo box with the available flight modes
|
||||
for (size_t j=0; j<_cModeInfo; j++) { |
||||
_rgCombo[i]->addItem(_rgModeInfo[j].label, QVariant(QChar((char)_rgModeInfo[j].value))); |
||||
} |
||||
|
||||
// Not all vehicle types support simple mode, hide/show as appropriate
|
||||
_rgSimpleModeCheckBox[i]->setEnabled(_simpleModeSupported); |
||||
} |
||||
} |
||||
|
||||
void FlightModeConfig::remoteControlChannelRawChanged(int chan,float val) |
||||
void FlightModeConfig::saveButtonClicked(void) |
||||
{ |
||||
if (chan == 4) |
||||
{ |
||||
//Channel 5 (0 array) is the mode switch.
|
||||
///TODO: Make this configurable
|
||||
if (val <= 1230) |
||||
{ |
||||
ui.mode1Label->setStyleSheet("background-color: rgb(0, 255, 0);color: rgb(0, 0, 0);"); |
||||
ui.mode2Label->setStyleSheet(""); |
||||
ui.mode3Label->setStyleSheet(""); |
||||
ui.mode4Label->setStyleSheet(""); |
||||
ui.mode5Label->setStyleSheet(""); |
||||
ui.mode6Label->setStyleSheet(""); |
||||
} |
||||
else if (val <= 1360) |
||||
{ |
||||
ui.mode1Label->setStyleSheet(""); |
||||
ui.mode2Label->setStyleSheet("background-color: rgb(0, 255, 0);color: rgb(0, 0, 0);"); |
||||
ui.mode3Label->setStyleSheet(""); |
||||
ui.mode4Label->setStyleSheet(""); |
||||
ui.mode5Label->setStyleSheet(""); |
||||
ui.mode6Label->setStyleSheet(""); |
||||
} |
||||
else if (val <= 1490) |
||||
{ |
||||
ui.mode1Label->setStyleSheet(""); |
||||
ui.mode2Label->setStyleSheet(""); |
||||
ui.mode3Label->setStyleSheet("background-color: rgb(0, 255, 0);color: rgb(0, 0, 0);"); |
||||
ui.mode4Label->setStyleSheet(""); |
||||
ui.mode5Label->setStyleSheet(""); |
||||
ui.mode6Label->setStyleSheet(""); |
||||
} |
||||
else if (val <=1620) |
||||
{ |
||||
ui.mode1Label->setStyleSheet(""); |
||||
ui.mode2Label->setStyleSheet(""); |
||||
ui.mode3Label->setStyleSheet(""); |
||||
ui.mode4Label->setStyleSheet("background-color: rgb(0, 255, 0);color: rgb(0, 0, 0);"); |
||||
ui.mode5Label->setStyleSheet(""); |
||||
ui.mode6Label->setStyleSheet(""); |
||||
} |
||||
else if (val <=1749) |
||||
{ |
||||
ui.mode1Label->setStyleSheet(""); |
||||
ui.mode2Label->setStyleSheet(""); |
||||
ui.mode3Label->setStyleSheet(""); |
||||
ui.mode4Label->setStyleSheet(""); |
||||
ui.mode5Label->setStyleSheet("background-color: rgb(0, 255, 0);color: rgb(0, 0, 0);"); |
||||
ui.mode6Label->setStyleSheet(""); |
||||
} |
||||
else |
||||
{ |
||||
ui.mode1Label->setStyleSheet(""); |
||||
ui.mode2Label->setStyleSheet(""); |
||||
ui.mode3Label->setStyleSheet(""); |
||||
ui.mode4Label->setStyleSheet(""); |
||||
ui.mode5Label->setStyleSheet(""); |
||||
ui.mode6Label->setStyleSheet("background-color: rgb(0, 255, 0);color: rgb(0, 0, 0);"); |
||||
// Save the current setting for each flight mode slot
|
||||
for (size_t i=0; i<_cModes; i++) { |
||||
m_uas->getParamManager()->setParameter(1, _rgModeParam[i], _rgCombo[i]->itemData(_rgCombo[i]->currentIndex())); |
||||
QVariant var =_rgCombo[i]->itemData(_rgCombo[i]->currentIndex()); |
||||
} |
||||
|
||||
// Save Simple Mode bit mask if supported
|
||||
if (_simpleModeSupported) { |
||||
int bitMask = 0; |
||||
for (size_t i=0; i<_cModes; i++) { |
||||
if (_rgSimpleModeCheckBox[i]->isChecked()) { |
||||
bitMask |= 1 << i; |
||||
} |
||||
} |
||||
m_uas->getParamManager()->setParameter(1, _simpleModeBitMaskParam, QVariant(QChar(bitMask))); |
||||
} |
||||
} |
||||
|
||||
void FlightModeConfig::parameterChanged(int uas, int component, QString parameterName, QVariant value) |
||||
// Higlights the PWM label for currently selected mode according the mode switch
|
||||
// rc channel value.
|
||||
void FlightModeConfig::remoteControlChannelRawChanged(int chan, float val) |
||||
{ |
||||
if (m_uas->getSystemType() == MAV_TYPE_FIXED_WING) |
||||
{ |
||||
if (parameterName == "FLTMODE1") |
||||
{ |
||||
ui.mode1ComboBox->setCurrentIndex(planeModeIndexToUiIndex[value.toInt()]); |
||||
} |
||||
else if (parameterName == "FLTMODE2") |
||||
{ |
||||
ui.mode2ComboBox->setCurrentIndex(planeModeIndexToUiIndex[value.toInt()]); |
||||
} |
||||
else if (parameterName == "FLTMODE3") |
||||
{ |
||||
ui.mode3ComboBox->setCurrentIndex(planeModeIndexToUiIndex[value.toInt()]); |
||||
} |
||||
else if (parameterName == "FLTMODE4") |
||||
{ |
||||
ui.mode4ComboBox->setCurrentIndex(planeModeIndexToUiIndex[value.toInt()]); |
||||
} |
||||
else if (parameterName == "FLTMODE5") |
||||
{ |
||||
ui.mode5ComboBox->setCurrentIndex(planeModeIndexToUiIndex[value.toInt()]); |
||||
} |
||||
else if (parameterName == "FLTMODE6") |
||||
{ |
||||
ui.mode6ComboBox->setCurrentIndex(planeModeIndexToUiIndex[value.toInt()]); |
||||
} |
||||
} |
||||
else if (m_uas->getSystemType() == MAV_TYPE_GROUND_ROVER) |
||||
qDebug() << chan << val << _modeSwitchRCChannel; |
||||
// Until we get the _modeSwitchRCChannel value from a parameter it will be set
|
||||
// to -1, which is an invalid channel thus the labels won't update
|
||||
if (chan == _modeSwitchRCChannel) |
||||
{ |
||||
if (parameterName == "MODE1") |
||||
{ |
||||
ui.mode1ComboBox->setCurrentIndex(roverModeIndexToUiIndex[value.toInt()]); |
||||
qDebug() << chan << val; |
||||
size_t highlightIndex; |
||||
|
||||
for (size_t i=0; i<_cModes; i++) { |
||||
if (val < _rgModePWMBoundary[i]) { |
||||
highlightIndex = i; |
||||
break; |
||||
} |
||||
} |
||||
else if (parameterName == "MODE2") |
||||
{ |
||||
ui.mode2ComboBox->setCurrentIndex(roverModeIndexToUiIndex[value.toInt()]); |
||||
} |
||||
else if (parameterName == "MODE3") |
||||
{ |
||||
ui.mode3ComboBox->setCurrentIndex(roverModeIndexToUiIndex[value.toInt()]); |
||||
} |
||||
else if (parameterName == "MODE4") |
||||
{ |
||||
ui.mode4ComboBox->setCurrentIndex(roverModeIndexToUiIndex[value.toInt()]); |
||||
} |
||||
else if (parameterName == "MODE5") |
||||
{ |
||||
ui.mode5ComboBox->setCurrentIndex(roverModeIndexToUiIndex[value.toInt()]); |
||||
|
||||
for (size_t i=0; i<_cModes; i++) { |
||||
QString styleSheet; |
||||
if (i == highlightIndex) { |
||||
styleSheet = "background-color: rgb(0, 255, 0);color: rgb(0, 0, 0);"; |
||||
} else { |
||||
styleSheet = ""; |
||||
} |
||||
_rgPWMLabel[i]->setStyleSheet(styleSheet); |
||||
} |
||||
} |
||||
else if (m_uas->getSystemType() == MAV_TYPE_QUADROTOR) |
||||
{ |
||||
if (parameterName == "FLTMODE1") |
||||
{ |
||||
ui.mode1ComboBox->setCurrentIndex(value.toInt()); |
||||
} |
||||
else if (parameterName == "FLTMODE2") |
||||
{ |
||||
ui.mode2ComboBox->setCurrentIndex(value.toInt()); |
||||
} |
||||
else if (parameterName == "FLTMODE3") |
||||
{ |
||||
ui.mode3ComboBox->setCurrentIndex(value.toInt()); |
||||
} |
||||
else if (parameterName == "FLTMODE4") |
||||
{ |
||||
ui.mode4ComboBox->setCurrentIndex(value.toInt()); |
||||
} |
||||
else if (parameterName == "FLTMODE5") |
||||
{ |
||||
ui.mode5ComboBox->setCurrentIndex(value.toInt()); |
||||
} |
||||
|
||||
void FlightModeConfig::parameterChanged(int uas, int component, QString parameterName, QVariant value) |
||||
{ |
||||
Q_UNUSED(uas); |
||||
Q_UNUSED(component); |
||||
|
||||
int iValue = value.toInt(); |
||||
|
||||
if (parameterName == _modeSwitchRCChannelParam) { |
||||
_modeSwitchRCChannel = iValue - 1; // 1-based in params
|
||||
} else if (parameterName == _simpleModeBitMaskParam) { |
||||
if (_simpleModeSupported) { |
||||
for (size_t i=0; i<_cModes; i++) { |
||||
_rgSimpleModeCheckBox[i]->setCheckState((iValue & (1 << i)) ? Qt::Checked : Qt::Unchecked); |
||||
} |
||||
} else { |
||||
qDebug() << "Received simple mode parameter on non simple mode vehicle type"; |
||||
} |
||||
else if (parameterName == "FLTMODE6") |
||||
{ |
||||
ui.mode6ComboBox->setCurrentIndex(value.toInt()); |
||||
} else { |
||||
// Loop over the flight mode params looking for a match
|
||||
for (size_t i=0; i<_cModes; i++) { |
||||
if (parameterName == _rgModeParam[i]) { |
||||
// We found a match, i is now the index of the combo box which displays that mode slot
|
||||
// Loop over the mode info till we find the matching value, this tells us which row in the
|
||||
// combo box to select.
|
||||
QComboBox* combo = _rgCombo[i]; |
||||
for (size_t j=0; j<_cModeInfo; j++) { |
||||
if (_rgModeInfo[j].value == iValue) { |
||||
combo->setCurrentIndex(j); |
||||
return; |
||||
} |
||||
} |
||||
|
||||
// We should have been able to find the flight mode value. If we didn't, we have been passed a
|
||||
// flight mode that we don't understand. Possibly a newer firmware version we are not yet up
|
||||
// to date with. In this case, add a new item to the combo box to represent it.
|
||||
qDebug() << QString("Unknown flight mode value %1").arg(iValue); |
||||
combo->addItem(QString("%1 - Unknown").arg(iValue), QVariant(iValue)); |
||||
combo->setCurrentIndex(combo->count() - 1); |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
||||
Loading…
Reference in new issue