Newer
Older
// On Windows (for VS2010) stdint.h contains the limits normally contained in limits.h
// It also needs the __STDC_LIMIT_MACROS macro defined in order to include them (done
// in qgroundcontrol.pri).
#ifdef WIN32
#include <stdint.h>
#else
#include <limits.h>
#include <QTimer>
#include <QDir>
#include <QXmlStreamReader>
#include <QMessageBox>
Lorenz Meier
committed
#include "QGCVehicleConfig.h"
#include "QGC.h"
Lorenz Meier
committed
#include "QGCToolWidget.h"
#include "UASManager.h"
#include "UASParameterCommsMgr.h"
Lorenz Meier
committed
#include "ui_QGCVehicleConfig.h"
QGCVehicleConfig::QGCVehicleConfig(QWidget *parent) :
QWidget(parent),
chanCount(0),
Lorenz Meier
committed
rcRoll(0.0f),
rcPitch(0.0f),
rcYaw(0.0f),
rcThrottle(0.0f),
rcMode(0.0f),
rcAux1(0.0f),
rcAux2(0.0f),
rcAux3(0.0f),
Michael Carpenter
committed
rc_mode(RC_MODE_NONE),
calibrationEnabled(false),
Lorenz Meier
committed
ui(new Ui::QGCVehicleConfig)
{
Michael Carpenter
committed
doneLoadingConfig = false;
setObjectName("QGC_VEHICLECONFIG");
Lorenz Meier
committed
ui->setupUi(this);
Michael Carpenter
committed
ui->rollWidget->setOrientation(Qt::Horizontal);
ui->rollWidget->setName("Roll");
ui->yawWidget->setOrientation(Qt::Horizontal);
ui->yawWidget->setName("Yaw");
ui->pitchWidget->setName("Pitch");
ui->throttleWidget->setName("Throttle");
Michael Carpenter
committed
ui->radio5Widget->setOrientation(Qt::Horizontal);
ui->radio5Widget->setName("Radio 5");
ui->radio6Widget->setOrientation(Qt::Horizontal);
ui->radio6Widget->setName("Radio 6");
ui->radio7Widget->setOrientation(Qt::Horizontal);
ui->radio7Widget->setName("Radio 7");
ui->radio8Widget->setOrientation(Qt::Horizontal);
ui->radio8Widget->setName("Radio 8");
Michael Carpenter
committed
connect(ui->rcMenuButton,SIGNAL(clicked()),this,SLOT(rcMenuButtonClicked()));
connect(ui->sensorMenuButton,SIGNAL(clicked()),this,SLOT(sensorMenuButtonClicked()));
connect(ui->generalMenuButton,SIGNAL(clicked()),this,SLOT(generalMenuButtonClicked()));
connect(ui->advancedMenuButton,SIGNAL(clicked()),this,SLOT(advancedMenuButtonClicked()));
Lorenz Meier
committed
ui->rcModeComboBox->setCurrentIndex((int)rc_mode - 1);
ui->rcCalibrationButton->setCheckable(true);
connect(ui->rcCalibrationButton, SIGNAL(clicked(bool)), this, SLOT(toggleCalibrationRC(bool)));
connect(ui->setButton, SIGNAL(clicked()), this, SLOT(writeParameters()));
Lorenz Meier
committed
connect(ui->rcModeComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(setRCModeIndex(int)));
Michael Carpenter
committed
//connect(ui->setTrimButton, SIGNAL(clicked()), this, SLOT(setTrimPositions()));
/* Connect RC mapping assignments */
connect(ui->rollSpinBox, SIGNAL(valueChanged(int)), this, SLOT(setRollChan(int)));
connect(ui->pitchSpinBox, SIGNAL(valueChanged(int)), this, SLOT(setPitchChan(int)));
connect(ui->yawSpinBox, SIGNAL(valueChanged(int)), this, SLOT(setYawChan(int)));
connect(ui->throttleSpinBox, SIGNAL(valueChanged(int)), this, SLOT(setThrottleChan(int)));
connect(ui->modeSpinBox, SIGNAL(valueChanged(int)), this, SLOT(setModeChan(int)));
connect(ui->aux1SpinBox, SIGNAL(valueChanged(int)), this, SLOT(setAux1Chan(int)));
connect(ui->aux2SpinBox, SIGNAL(valueChanged(int)), this, SLOT(setAux2Chan(int)));
connect(ui->aux3SpinBox, SIGNAL(valueChanged(int)), this, SLOT(setAux3Chan(int)));
// Connect RC reverse assignments
connect(ui->invertCheckBox, SIGNAL(clicked(bool)), this, SLOT(setRollInverted(bool)));
connect(ui->invertCheckBox_2, SIGNAL(clicked(bool)), this, SLOT(setPitchInverted(bool)));
connect(ui->invertCheckBox_3, SIGNAL(clicked(bool)), this, SLOT(setYawInverted(bool)));
connect(ui->invertCheckBox_4, SIGNAL(clicked(bool)), this, SLOT(setThrottleInverted(bool)));
connect(ui->invertCheckBox_5, SIGNAL(clicked(bool)), this, SLOT(setModeInverted(bool)));
connect(ui->invertCheckBox_6, SIGNAL(clicked(bool)), this, SLOT(setAux1Inverted(bool)));
connect(ui->invertCheckBox_7, SIGNAL(clicked(bool)), this, SLOT(setAux2Inverted(bool)));
connect(ui->invertCheckBox_8, SIGNAL(clicked(bool)), this, SLOT(setAux3Inverted(bool)));
connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
setActiveUAS(UASManager::instance()->getActiveUAS());
for (unsigned int i = 0; i < chanMax; i++)
rcValue[i] = UINT16_MAX;
}
updateTimer.setInterval(150);
connect(&updateTimer, SIGNAL(timeout()), this, SLOT(updateView()));
updateTimer.start();
Michael Carpenter
committed
ui->advancedGroupBox->hide();
connect(ui->advancedCheckBox,SIGNAL(toggled(bool)),ui->advancedGroupBox,SLOT(setShown(bool)));
Lorenz Meier
committed
}
Michael Carpenter
committed
void QGCVehicleConfig::rcMenuButtonClicked()
{
ui->stackedWidget->setCurrentIndex(0);
}
void QGCVehicleConfig::sensorMenuButtonClicked()
{
ui->stackedWidget->setCurrentIndex(1);
}
void QGCVehicleConfig::generalMenuButtonClicked()
{
ui->stackedWidget->setCurrentIndex(ui->stackedWidget->count()-2);
}
Michael Carpenter
committed
void QGCVehicleConfig::advancedMenuButtonClicked()
{
ui->stackedWidget->setCurrentIndex(ui->stackedWidget->count()-1);
}
Lorenz Meier
committed
QGCVehicleConfig::~QGCVehicleConfig()
{
delete ui;
}
Lorenz Meier
committed
void QGCVehicleConfig::setRCModeIndex(int newRcMode)
{
if (newRcMode > 0 && newRcMode < 6)
Lorenz Meier
committed
{
//rc_mode = (enum RC_MODE) (newRcMode+1);
Lorenz Meier
committed
changed = true;
}
}
void QGCVehicleConfig::toggleCalibrationRC(bool enabled)
{
if (enabled)
{
startCalibrationRC();
}
else
{
stopCalibrationRC();
}
}
void QGCVehicleConfig::setTrimPositions()
{
// Set trim to min if stick is close to min
if (abs(rcValue[rcMapping[3]] - rcMin[rcMapping[3]]) < 100)
{
rcTrim[rcMapping[3]] = rcMin[rcMapping[3]]; // throttle
}
// Set trim to max if stick is close to max
else if (abs(rcValue[rcMapping[3]] - rcMax[rcMapping[3]]) < 100)
{
rcTrim[rcMapping[3]] = rcMax[rcMapping[3]]; // throttle
}
else
{
// Reject
QMessageBox msgBox;
msgBox.setText(tr("Throttle Stick Trim Position Invalid"));
msgBox.setInformativeText(tr("The throttle stick is not in the min position. Please set it to the minimum value"));
msgBox.setStandardButtons(QMessageBox::Ok);
msgBox.setDefaultButton(QMessageBox::Ok);
(void)msgBox.exec();
}
// Set trim for roll, pitch, yaw, throttle
rcTrim[rcMapping[0]] = rcValue[rcMapping[0]]; // roll
rcTrim[rcMapping[1]] = rcValue[rcMapping[1]]; // pitch
rcTrim[rcMapping[2]] = rcValue[rcMapping[2]]; // yaw
rcTrim[rcMapping[4]] = ((rcMax[rcMapping[4]] - rcMin[rcMapping[4]]) / 2.0f) + rcMin[rcMapping[4]]; // mode sw
rcTrim[rcMapping[5]] = ((rcMax[rcMapping[5]] - rcMin[rcMapping[5]]) / 2.0f) + rcMin[rcMapping[5]]; // aux 1
rcTrim[rcMapping[6]] = ((rcMax[rcMapping[6]] - rcMin[rcMapping[6]]) / 2.0f) + rcMin[rcMapping[6]]; // aux 2
rcTrim[rcMapping[7]] = ((rcMax[rcMapping[7]] - rcMin[rcMapping[7]]) / 2.0f) + rcMin[rcMapping[7]]; // aux 3
}
void QGCVehicleConfig::detectChannelInversion()
{
}
void QGCVehicleConfig::startCalibrationRC()
{
Michael Carpenter
committed
QMessageBox::information(0,"Warning!","You are about to start radio calibration.\nPlease ensure all motor power is disconnected AND all props are removed from the vehicle.\nAlso ensure transmitter and reciever are powered and connected\n\nClick OK to confirm");
QMessageBox::information(0,"Information","Click OK, then move all sticks to their extreme positions, watching the min/max values to ensure you get the most range from your controller. This includes all switches");
ui->rcTypeComboBox->setEnabled(false);
Lorenz Meier
committed
ui->rcCalibrationButton->setText(tr("Stop RC Calibration"));
resetCalibrationRC();
calibrationEnabled = true;
Michael Carpenter
committed
ui->rollWidget->showMinMax();
ui->pitchWidget->showMinMax();
ui->yawWidget->showMinMax();
ui->throttleWidget->showMinMax();
ui->radio5Widget->showMinMax();
ui->radio6Widget->showMinMax();
ui->radio7Widget->showMinMax();
ui->radio8Widget->showMinMax();
}
void QGCVehicleConfig::stopCalibrationRC()
{
Michael Carpenter
committed
QMessageBox::information(0,"Trims","Ensure all sticks are centeres and throttle is in the downmost position, click OK to continue");
calibrationEnabled = false;
ui->rcTypeComboBox->setEnabled(true);
Lorenz Meier
committed
ui->rcCalibrationButton->setText(tr("Start RC Calibration"));
Michael Carpenter
committed
ui->rollWidget->hideMinMax();
ui->pitchWidget->hideMinMax();
ui->yawWidget->hideMinMax();
ui->throttleWidget->hideMinMax();
ui->radio5Widget->hideMinMax();
ui->radio6Widget->hideMinMax();
ui->radio7Widget->hideMinMax();
ui->radio8Widget->hideMinMax();
QString statusstr;
statusstr = "Below you will find the detected radio calibration information that will be sent to the autopilot\n";
statusstr += "Normal values are around 1100 to 1900, with disconnected channels reading very close to 1500\n\n";
statusstr += "Channel\tMin\tCenter\tMax\n";
statusstr += "--------------------\n";
for (int i=0;i<8;i++)
{
statusstr += QString::number(i) + "\t" + QString::number(rcMin[i]) + "\t" + QString::number(rcValue[i]) + "\t" + QString::number(rcMax[i]) + "\n";
}
QMessageBox::information(0,"Status",statusstr);
Michael Carpenter
committed
void QGCVehicleConfig::loadQgcConfig(bool primary)
Q_UNUSED(primary);
QDir autopilotdir(qApp->applicationDirPath() + "/files/" + mav->getAutopilotTypeName().toLower());
QDir generaldir = QDir(autopilotdir.absolutePath() + "/general/widgets");
QDir vehicledir = QDir(autopilotdir.absolutePath() + "/" + mav->getSystemTypeName().toLower() + "/widgets");
if (!autopilotdir.exists("general"))
{
//TODO: Throw some kind of error here. There is no general configuration directory
qWarning() << "Invalid general dir. no general configuration will be loaded.";
}
if (!autopilotdir.exists(mav->getAutopilotTypeName().toLower()))
{
//TODO: Throw an error here too, no autopilot specific configuration
qWarning() << "Invalid vehicle dir, no vehicle specific configuration will be loaded.";
}
// Generate widgets for the General tab.
Michael Carpenter
committed
QGCToolWidget *tool;
bool left = true;
foreach (QString file,generaldir.entryList(QDir::Files | QDir::NoDotAndDotDot))
{
if (file.toLower().endsWith(".qgw")) {
QWidget* parent = left?ui->generalLeftContents:ui->generalRightContents;
John Tapsell
committed
tool = new QGCToolWidget("", "", parent);
if (tool->loadSettings(generaldir.absoluteFilePath(file), false))
{
toolWidgets.append(tool);
QGroupBox *box = new QGroupBox(parent);
box->setTitle(tool->objectName());
box->setLayout(new QVBoxLayout(box));
box->layout()->addWidget(tool);
Michael Carpenter
committed
if (left)
Michael Carpenter
committed
{
Michael Carpenter
committed
left = false;
ui->generalLeftLayout->addWidget(box);
Michael Carpenter
committed
}
else
{
left = true;
ui->generalRightLayout->addWidget(box);
Michael Carpenter
committed
}
} else {
delete tool;
}
}
}
// Generate widgets for the Advanced tab.
Michael Carpenter
committed
left = true;
foreach (QString file,vehicledir.entryList(QDir::Files | QDir::NoDotAndDotDot))
{
if (file.toLower().endsWith(".qgw")) {
QWidget* parent = left?ui->advancedLeftContents:ui->advancedRightContents;
John Tapsell
committed
tool = new QGCToolWidget("", "", parent);
if (tool->loadSettings(vehicledir.absoluteFilePath(file), false))
{
toolWidgets.append(tool);
QGroupBox *box = new QGroupBox(parent);
box->setTitle(tool->objectName());
box->setLayout(new QVBoxLayout(box));
box->layout()->addWidget(tool);
Michael Carpenter
committed
if (left)
Michael Carpenter
committed
{
Michael Carpenter
committed
left = false;
ui->advancedLeftLayout->addWidget(box);
Michael Carpenter
committed
}
else
{
left = true;
ui->advancedRightLayout->addWidget(box);
Michael Carpenter
committed
}
} else {
delete tool;
}
}
}
// Load tabs for general configuration
Michael Carpenter
committed
foreach (QString dir,generaldir.entryList(QDir::Dirs | QDir::NoDotAndDotDot))
{
Michael Carpenter
committed
QPushButton *button = new QPushButton(ui->scrollAreaWidgetContents_3);
connect(button,SIGNAL(clicked()),this,SLOT(menuButtonClicked()));
ui->navBarLayout->insertWidget(2,button);
Michael Carpenter
committed
button->setMinimumHeight(75);
Michael Carpenter
committed
button->setMinimumWidth(100);
button->show();
button->setText(dir);
QWidget *tab = new QWidget(ui->stackedWidget);
ui->stackedWidget->insertWidget(2,tab);
buttonToWidgetMap[button] = tab;
Michael Carpenter
committed
tab->setLayout(new QVBoxLayout());
Michael Carpenter
committed
tab->show();
QScrollArea *area = new QScrollArea(tab);
Michael Carpenter
committed
tab->layout()->addWidget(area);
QWidget *scrollArea = new QWidget(tab);
scrollArea->setLayout(new QVBoxLayout(tab));
Michael Carpenter
committed
area->setWidget(scrollArea);
area->setWidgetResizable(true);
area->show();
scrollArea->show();
Michael Carpenter
committed
QDir newdir = QDir(generaldir.absoluteFilePath(dir));
foreach (QString file,newdir.entryList(QDir::Files| QDir::NoDotAndDotDot))
{
if (file.toLower().endsWith(".qgw")) {
John Tapsell
committed
tool = new QGCToolWidget("", "", tab);
Michael Carpenter
committed
if (tool->loadSettings(newdir.absoluteFilePath(file), false))
{
toolWidgets.append(tool);
//ui->sensorLayout->addWidget(tool);
QGroupBox *box = new QGroupBox(tab);
Michael Carpenter
committed
box->setTitle(tool->objectName());
box->setLayout(new QVBoxLayout(tab));
Michael Carpenter
committed
box->layout()->addWidget(tool);
Michael Carpenter
committed
scrollArea->layout()->addWidget(box);
Michael Carpenter
committed
} else {
delete tool;
}
}
}
}
// Load additional tabs for vehicle specific configuration
Michael Carpenter
committed
foreach (QString dir,vehicledir.entryList(QDir::Dirs | QDir::NoDotAndDotDot))
{
Michael Carpenter
committed
QPushButton *button = new QPushButton(ui->scrollAreaWidgetContents_3);
connect(button,SIGNAL(clicked()),this,SLOT(menuButtonClicked()));
ui->navBarLayout->insertWidget(2,button);
QWidget *tab = new QWidget(ui->stackedWidget);
ui->stackedWidget->insertWidget(2,tab);
buttonToWidgetMap[button] = tab;
Michael Carpenter
committed
button->setMinimumHeight(75);
Michael Carpenter
committed
button->setMinimumWidth(100);
button->show();
button->setText(dir);
Michael Carpenter
committed
tab->setLayout(new QVBoxLayout());
Michael Carpenter
committed
tab->show();
QScrollArea *area = new QScrollArea(tab);
Michael Carpenter
committed
tab->layout()->addWidget(area);
QWidget *scrollArea = new QWidget(tab);
scrollArea->setLayout(new QVBoxLayout(tab));
Michael Carpenter
committed
area->setWidget(scrollArea);
area->setWidgetResizable(true);
area->show();
scrollArea->show();
Michael Carpenter
committed
QDir newdir = QDir(vehicledir.absoluteFilePath(dir));
foreach (QString file,newdir.entryList(QDir::Files| QDir::NoDotAndDotDot))
{
if (file.toLower().endsWith(".qgw")) {
John Tapsell
committed
tool = new QGCToolWidget("","", tab);
Michael Carpenter
committed
tool->addUAS(mav);
if (tool->loadSettings(newdir.absoluteFilePath(file), false))
{
toolWidgets.append(tool);
//ui->sensorLayout->addWidget(tool);
QGroupBox *box = new QGroupBox(tab);
Michael Carpenter
committed
box->setTitle(tool->objectName());
box->setLayout(new QVBoxLayout(box));
Michael Carpenter
committed
box->layout()->addWidget(tool);
Michael Carpenter
committed
scrollArea->layout()->addWidget(box);
box->show();
//gbox->layout()->addWidget(box);
Michael Carpenter
committed
} else {
delete tool;
}
}
}
}
// Load general calibration for autopilot
//TODO: Handle this more gracefully, maybe have it scan the directory for multiple calibration entries?
John Tapsell
committed
tool = new QGCToolWidget("", "", ui->sensorContents);
Michael Carpenter
committed
tool->addUAS(mav);
if (tool->loadSettings(autopilotdir.absolutePath() + "/general/calibration/calibration.qgw", false))
Lorenz Meier
committed
{
toolWidgets.append(tool);
QGroupBox *box = new QGroupBox(ui->sensorContents);
Michael Carpenter
committed
box->setTitle(tool->objectName());
box->setLayout(new QVBoxLayout(box));
Michael Carpenter
committed
box->layout()->addWidget(tool);
ui->sensorLayout->addWidget(box);
} else {
delete tool;
}
// Load vehicle-specific autopilot configuration
John Tapsell
committed
tool = new QGCToolWidget("", "", ui->sensorContents);
Michael Carpenter
committed
tool->addUAS(mav);
if (tool->loadSettings(autopilotdir.absolutePath() + "/" + mav->getSystemTypeName().toLower() + "/calibration/calibration.qgw", false))
{
toolWidgets.append(tool);
QGroupBox *box = new QGroupBox(ui->sensorContents);
Michael Carpenter
committed
box->setTitle(tool->objectName());
box->setLayout(new QVBoxLayout(box));
Michael Carpenter
committed
box->layout()->addWidget(tool);
ui->sensorLayout->addWidget(box);
} else {
delete tool;
Lorenz Meier
committed
}
Michael Carpenter
committed
//description.txt
QFile sensortipsfile(autopilotdir.absolutePath() + "/general/calibration/description.txt");
sensortipsfile.open(QIODevice::ReadOnly);
ui->sensorTips->setHtml(sensortipsfile.readAll());
sensortipsfile.close();
Michael Carpenter
committed
}
Michael Carpenter
committed
void QGCVehicleConfig::menuButtonClicked()
{
QPushButton *button = qobject_cast<QPushButton*>(sender());
if (!button)
{
return;
}
if (buttonToWidgetMap.contains(button))
{
ui->stackedWidget->setCurrentWidget(buttonToWidgetMap[button]);
}
}
Lorenz Meier
committed
Michael Carpenter
committed
void QGCVehicleConfig::loadConfig()
{
QGCToolWidget* tool;
QDir autopilotdir(qApp->applicationDirPath() + "/files/" + mav->getAutopilotTypeName().toLower());
QDir generaldir = QDir(autopilotdir.absolutePath() + "/general/widgets");
QDir vehicledir = QDir(autopilotdir.absolutePath() + "/" + mav->getSystemTypeName().toLower() + "/widgets");
if (!autopilotdir.exists("general"))
{
//TODO: Throw some kind of error here. There is no general configuration directory
qWarning() << "Invalid general dir. no general configuration will be loaded.";
Michael Carpenter
committed
}
if (!autopilotdir.exists(mav->getAutopilotTypeName().toLower()))
{
//TODO: Throw an error here too, no autopilot specific configuration
qWarning() << "Invalid vehicle dir, no vehicle specific configuration will be loaded.";
Michael Carpenter
committed
}
qDebug() << autopilotdir.absolutePath();
qDebug() << generaldir.absolutePath();
qDebug() << vehicledir.absolutePath();
QFile xmlfile(autopilotdir.absolutePath() + "/arduplane.pdef.xml");
if (xmlfile.exists() && !xmlfile.open(QIODevice::ReadOnly))
Michael Carpenter
committed
{
Michael Carpenter
committed
loadQgcConfig(false);
Michael Carpenter
committed
doneLoadingConfig = true;
Michael Carpenter
committed
return;
}
Michael Carpenter
committed
loadQgcConfig(true);
Michael Carpenter
committed
QXmlStreamReader xml(xmlfile.readAll());
xmlfile.close();
//TODO: Testing to ensure that incorrectly formatted XML won't break this.
Michael Carpenter
committed
while (!xml.atEnd())
{
if (xml.isStartElement() && xml.name() == "paramfile")
{
xml.readNext();
while ((xml.name() != "paramfile") && !xml.atEnd())
{
QString valuetype = "";
if (xml.isStartElement() && (xml.name() == "vehicles" || xml.name() == "libraries")) //Enter into the vehicles loop
{
valuetype = xml.name().toString();
xml.readNext();
while ((xml.name() != valuetype) && !xml.atEnd())
{
if (xml.isStartElement() && xml.name() == "parameters") //This is a parameter block
{
QString parametersname = "";
if (xml.attributes().hasAttribute("name"))
{
parametersname = xml.attributes().value("name").toString();
}
Michael Carpenter
committed
QVariantMap genset;
QVariantMap advset;
Michael Carpenter
committed
QString setname = parametersname;
xml.readNext();
Michael Carpenter
committed
int genarraycount = 0;
int advarraycount = 0;
Michael Carpenter
committed
while ((xml.name() != "parameters") && !xml.atEnd())
{
if (xml.isStartElement() && xml.name() == "param")
{
QString humanname = xml.attributes().value("humanName").toString();
QString name = xml.attributes().value("name").toString();
Michael Carpenter
committed
QString tab= xml.attributes().value("user").toString();
if (tab == "Advanced")
{
advset["title"] = parametersname;
}
else
{
genset["title"] = parametersname;
}
Michael Carpenter
committed
if (name.contains(":"))
{
name = name.split(":")[1];
}
QString docs = xml.attributes().value("documentation").toString();
paramTooltips[name] = name + " - " + docs;
int type = -1; //Type of item
QMap<QString,QString> fieldmap;
xml.readNext();
while ((xml.name() != "param") && !xml.atEnd())
{
if (xml.isStartElement() && xml.name() == "values")
{
type = 1; //1 is a combobox
Michael Carpenter
committed
if (tab == "Advanced")
{
advset[setname + "\\" + QString::number(advarraycount) + "\\" + "TYPE"] = "COMBO";
advset[setname + "\\" + QString::number(advarraycount) + "\\" + "QGC_PARAM_COMBOBOX_DESCRIPTION"] = humanname;
advset[setname + "\\" + QString::number(advarraycount) + "\\" + "QGC_PARAM_COMBOBOX_PARAMID"] = name;
advset[setname + "\\" + QString::number(advarraycount) + "\\" + "QGC_PARAM_COMBOBOX_COMPONENTID"] = 1;
}
else
{
genset[setname + "\\" + QString::number(genarraycount) + "\\" + "TYPE"] = "COMBO";
genset[setname + "\\" + QString::number(genarraycount) + "\\" + "QGC_PARAM_COMBOBOX_DESCRIPTION"] = humanname;
genset[setname + "\\" + QString::number(genarraycount) + "\\" + "QGC_PARAM_COMBOBOX_PARAMID"] = name;
genset[setname + "\\" + QString::number(genarraycount) + "\\" + "QGC_PARAM_COMBOBOX_COMPONENTID"] = 1;
}
Michael Carpenter
committed
int paramcount = 0;
xml.readNext();
while ((xml.name() != "values") && !xml.atEnd())
{
if (xml.isStartElement() && xml.name() == "value")
{
QString code = xml.attributes().value("code").toString();
QString arg = xml.readElementText();
Michael Carpenter
committed
if (tab == "Advanced")
{
advset[setname + "\\" + QString::number(advarraycount) + "\\" + "QGC_PARAM_COMBOBOX_ITEM_" + QString::number(paramcount) + "_TEXT"] = arg;
advset[setname + "\\" + QString::number(advarraycount) + "\\" + "QGC_PARAM_COMBOBOX_ITEM_" + QString::number(paramcount) + "_VAL"] = code.toInt();
}
else
{
genset[setname + "\\" + QString::number(genarraycount) + "\\" + "QGC_PARAM_COMBOBOX_ITEM_" + QString::number(paramcount) + "_TEXT"] = arg;
genset[setname + "\\" + QString::number(genarraycount) + "\\" + "QGC_PARAM_COMBOBOX_ITEM_" + QString::number(paramcount) + "_VAL"] = code.toInt();
}
Michael Carpenter
committed
paramcount++;
}
xml.readNext();
}
Michael Carpenter
committed
if (tab == "Advanced")
{
advset[setname + "\\" + QString::number(advarraycount) + "\\" + "QGC_PARAM_COMBOBOX_COUNT"] = paramcount;
}
else
{
genset[setname + "\\" + QString::number(genarraycount) + "\\" + "QGC_PARAM_COMBOBOX_COUNT"] = paramcount;
}
Michael Carpenter
committed
}
if (xml.isStartElement() && xml.name() == "field")
{
type = 2; //2 is a slider
QString fieldtype = xml.attributes().value("name").toString();
QString text = xml.readElementText();
fieldmap[fieldtype] = text;
}
xml.readNext();
}
if (type == -1)
{
//Nothing inside! Assume it's a value, give it a default range.
Michael Carpenter
committed
type = 2;
QString fieldtype = "Range";
QString text = "0 100"; //TODO: Determine a better way of figuring out default ranges.
Michael Carpenter
committed
fieldmap[fieldtype] = text;
}
if (type == 2)
{
Michael Carpenter
committed
if (tab == "Advanced")
{
advset[setname + "\\" + QString::number(advarraycount) + "\\" + "TYPE"] = "SLIDER";
advset[setname + "\\" + QString::number(advarraycount) + "\\" + "QGC_PARAM_SLIDER_DESCRIPTION"] = humanname;
advset[setname + "\\" + QString::number(advarraycount) + "\\" + "QGC_PARAM_SLIDER_PARAMID"] = name;
advset[setname + "\\" + QString::number(advarraycount) + "\\" + "QGC_PARAM_SLIDER_COMPONENTID"] = 1;
}
else
{
genset[setname + "\\" + QString::number(genarraycount) + "\\" + "TYPE"] = "SLIDER";
genset[setname + "\\" + QString::number(genarraycount) + "\\" + "QGC_PARAM_SLIDER_DESCRIPTION"] = humanname;
genset[setname + "\\" + QString::number(genarraycount) + "\\" + "QGC_PARAM_SLIDER_PARAMID"] = name;
genset[setname + "\\" + QString::number(genarraycount) + "\\" + "QGC_PARAM_SLIDER_COMPONENTID"] = 1;
}
Michael Carpenter
committed
if (fieldmap.contains("Range"))
{
float min = 0;
float max = 0;
//Some range fields list "0-10" and some list "0 10". Handle both.
Michael Carpenter
committed
if (fieldmap["Range"].split(" ").size() > 1)
{
min = fieldmap["Range"].split(" ")[0].trimmed().toFloat();
max = fieldmap["Range"].split(" ")[1].trimmed().toFloat();
}
else if (fieldmap["Range"].split("-").size() > 1)
{
min = fieldmap["Range"].split("-")[0].trimmed().toFloat();
max = fieldmap["Range"].split("-")[1].trimmed().toFloat();
}
Michael Carpenter
committed
if (tab == "Advanced")
{
advset[setname + "\\" + QString::number(advarraycount) + "\\" + "QGC_PARAM_SLIDER_MIN"] = min;
advset[setname + "\\" + QString::number(advarraycount) + "\\" + "QGC_PARAM_SLIDER_MAX"] = max;
}
else
{
genset[setname + "\\" + QString::number(genarraycount) + "\\" + "QGC_PARAM_SLIDER_MIN"] = min;
genset[setname + "\\" + QString::number(genarraycount) + "\\" + "QGC_PARAM_SLIDER_MAX"] = max;
}
Michael Carpenter
committed
}
}
Michael Carpenter
committed
if (tab == "Advanced")
{
advarraycount++;
advset["count"] = advarraycount;
}
else
{
genarraycount++;
genset["count"] = genarraycount;
}
Michael Carpenter
committed
}
xml.readNext();
}
Michael Carpenter
committed
if (genarraycount > 0)
Michael Carpenter
committed
{
QWidget* parent = this;
if (valuetype == "vehicles")
{
parent = ui->generalLeftContents;
}
else if (valuetype == "libraries")
{
parent = ui->generalRightContents;
}
John Tapsell
committed
tool = new QGCToolWidget(parametersname, parametersname, parent);
Michael Carpenter
committed
tool->addUAS(mav);
tool->setSettings(genset);
QList<QString> paramlist = tool->getParamList();
for (int i=0;i<paramlist.size();i++)
{
Michael Carpenter
committed
//Based on the airframe, we add the parameter to different categories.
if (parametersname == "ArduPlane") //MAV_TYPE_FIXED_WING FIXED_WING
{
systemTypeToParamMap["FIXED_WING"].insert(paramlist[i],tool);
Michael Carpenter
committed
}
else if (parametersname == "ArduCopter") //MAV_TYPE_QUADROTOR "QUADROTOR
{
systemTypeToParamMap["QUADROTOR"].insert(paramlist[i],tool);
Michael Carpenter
committed
}
else if (parametersname == "APMrover2") //MAV_TYPE_GROUND_ROVER GROUND_ROVER
{
systemTypeToParamMap["GROUND_ROVER"].insert(paramlist[i],tool);
Michael Carpenter
committed
}
else
{
libParamToWidgetMap.insert(paramlist[i],tool);
Michael Carpenter
committed
}
}
Michael Carpenter
committed
toolWidgets.append(tool);
QGroupBox *box = new QGroupBox(parent);
Michael Carpenter
committed
box->setTitle(tool->objectName());
box->setLayout(new QVBoxLayout(box));
Michael Carpenter
committed
box->layout()->addWidget(tool);
if (valuetype == "vehicles")
{
ui->generalLeftLayout->addWidget(box);
}
Michael Carpenter
committed
else if (valuetype == "libraries")
{
ui->generalRightLayout->addWidget(box);
}
Michael Carpenter
committed
box->hide();
toolToBoxMap[tool] = box;
}
if (advarraycount > 0)
{
QWidget* parent = this;
if (valuetype == "vehicles")
{
parent = ui->generalLeftContents;
}
else if (valuetype == "libraries")
{
parent = ui->generalRightContents;
}
John Tapsell
committed
tool = new QGCToolWidget(parametersname, parametersname, parent);
Michael Carpenter
committed
tool->addUAS(mav);
tool->setSettings(advset);
QList<QString> paramlist = tool->getParamList();
for (int i=0;i<paramlist.size();i++)
{
Michael Carpenter
committed
//Based on the airframe, we add the parameter to different categories.
if (parametersname == "ArduPlane") //MAV_TYPE_FIXED_WING FIXED_WING
{
systemTypeToParamMap["FIXED_WING"].insert(paramlist[i],tool);
Michael Carpenter
committed
}
else if (parametersname == "ArduCopter") //MAV_TYPE_QUADROTOR "QUADROTOR
{
systemTypeToParamMap["QUADROTOR"].insert(paramlist[i],tool);
Michael Carpenter
committed
}
else if (parametersname == "APMrover2") //MAV_TYPE_GROUND_ROVER GROUND_ROVER
{
systemTypeToParamMap["GROUND_ROVER"].insert(paramlist[i],tool);
Michael Carpenter
committed
}
else
{
libParamToWidgetMap.insert(paramlist[i],tool);
Michael Carpenter
committed
}
}
Michael Carpenter
committed
Michael Carpenter
committed
toolWidgets.append(tool);
QGroupBox *box = new QGroupBox(parent);
Michael Carpenter
committed
box->setTitle(tool->objectName());
box->setLayout(new QVBoxLayout(box));
Michael Carpenter
committed
box->layout()->addWidget(tool);
if (valuetype == "vehicles")
{
ui->generalLeftLayout->addWidget(box);
Michael Carpenter
committed
}
else if (valuetype == "libraries")
{
ui->generalRightLayout->addWidget(box);
Michael Carpenter
committed
}
box->hide();
toolToBoxMap[tool] = box;
Michael Carpenter
committed
}
}
xml.readNext();
}
}
xml.readNext();
}
}
xml.readNext();
}
if (!paramTooltips.isEmpty()) {
paramMgr->setParamDescriptions(paramTooltips);
}
Michael Carpenter
committed
doneLoadingConfig = true;
//Config is finished, lets do a parameter request to ensure none are missed if someone else started requesting before we were finished.
paramMgr->requestParameterListIfEmpty();
}
void QGCVehicleConfig::setActiveUAS(UASInterface* active)
{
// Hide items if NULL and abort
if (!active) {
ui->setButton->setEnabled(false);
ui->refreshButton->setEnabled(false);
ui->readButton->show();
ui->readButton->setEnabled(false);
ui->writeButton->show();
ui->writeButton->setEnabled(false);
ui->loadFileButton->setEnabled(false);
ui->saveFileButton->setEnabled(false);
return;
}
// Do nothing if system is the same
if (mav == active) return;
if (mav)
{
// Disconnect old system
disconnect(mav, SIGNAL(remoteControlChannelRawChanged(int,float)), this,
SLOT(remoteControlChannelRawChanged(int,float)));
disconnect(mav, SIGNAL(parameterChanged(int,int,QString,QVariant)), this,
SLOT(parameterChanged(int,int,QString,QVariant)));
disconnect(ui->refreshButton,SIGNAL(clicked()),mav,SLOT(requestParameters()));
Lorenz Meier
committed
// Delete all children from all fixed tabs.
foreach(QWidget* child, ui->generalLeftContents->findChildren<QWidget*>())
{
child->deleteLater();
}
foreach(QWidget* child, ui->generalRightContents->findChildren<QWidget*>())
{
child->deleteLater();
}
foreach(QWidget* child, ui->advancedLeftContents->findChildren<QWidget*>())
{
child->deleteLater();
}
foreach(QWidget* child, ui->advancedRightContents->findChildren<QWidget*>())
{
child->deleteLater();
}
foreach(QWidget* child, ui->sensorContents->findChildren<QWidget*>())
{
child->deleteLater();
}
// And then delete any custom tabs
foreach(QWidget* child, additionalTabs)
Lorenz Meier
committed
{
child->deleteLater();
Lorenz Meier
committed
}
additionalTabs.clear();
paramToWidgetMap.clear();
libParamToWidgetMap.clear();
systemTypeToParamMap.clear();
toolToBoxMap.clear();
paramTooltips.clear();
paramMgr = mav->getParamManager();
Lorenz Meier
committed
// Reset current state
resetCalibrationRC();
requestCalibrationRC();
mav->requestParameter(0, "RC_TYPE");
chanCount = 0;
// Connect new system
connect(active, SIGNAL(remoteControlChannelRawChanged(int,float)), this,
SLOT(remoteControlChannelRawChanged(int,float)));
connect(active, SIGNAL(parameterChanged(int,int,QString,QVariant)), this,
SLOT(parameterChanged(int,int,QString,QVariant)));
connect(ui->refreshButton, SIGNAL(clicked()), active, SLOT(requestParameters()));
Lorenz Meier
committed
if (systemTypeToParamMap.contains(mav->getSystemTypeName()))
Lorenz Meier
committed
{
paramToWidgetMap = systemTypeToParamMap[mav->getSystemTypeName()];
Lorenz Meier
committed
}
else
Lorenz Meier
committed
{
//Indication that we have no meta data for this system type.
qDebug() << "No parameters defined for system type:" << mav->getSystemTypeName();
paramToWidgetMap = systemTypeToParamMap[mav->getSystemTypeName()];
Lorenz Meier
committed
}
Michael Carpenter
committed
if (!paramTooltips.isEmpty())
Lorenz Meier
committed
{
mav->getParamManager()->setParamDescriptions(paramTooltips);
Lorenz Meier
committed
}
Michael Carpenter
committed
qDebug() << "CALIBRATION!! System Type Name:" << mav->getSystemTypeName();
Lorenz Meier
committed
Michael Carpenter
committed
//Load configuration after 1ms. This allows it to go into the event loop, and prevents application hangups due to the
//amount of time it actually takes to load the configuration windows.
QTimer::singleShot(1,this,SLOT(loadConfig()));
Lorenz Meier
committed
updateStatus(QString("Reading from system %1").arg(mav->getUASName()));
// Since a system is now connected, enable the VehicleConfig UI.
ui->setButton->setEnabled(true);
ui->refreshButton->setEnabled(true);
ui->readButton->setEnabled(true);
ui->writeButton->setEnabled(true);
ui->loadFileButton->setEnabled(true);
ui->saveFileButton->setEnabled(true);
if (mav->getAutopilotTypeName() == "ARDUPILOTMEGA")
{
ui->readButton->hide();
ui->writeButton->hide();
}
void QGCVehicleConfig::resetCalibrationRC()
{
for (unsigned int i = 0; i < chanMax; ++i)
{
Michael Carpenter
committed
rcMin[i] = 1500;
rcMax[i] = 1500;
}
}
/**
* Sends the RC calibration to the vehicle and stores it in EEPROM
*/
void QGCVehicleConfig::writeCalibrationRC()
{
if (!mav) return;
setTrimPositions();
QString minTpl("RC%1_MIN");
QString maxTpl("RC%1_MAX");
QString trimTpl("RC%1_TRIM");
QString revTpl("RC%1_REV");
// Do not write the RC type, as these values depend on this
// active onboard parameter
//TODO consolidate RC param sending in the UAS comms mgr
for (unsigned int i = 0; i < chanCount; ++i)
//qDebug() << "SENDING" << minTpl.arg(i+1) << rcMin[i];
mav->setParameter(0, minTpl.arg(i+1), rcMin[i]);
QGC::SLEEP::usleep(50000);
mav->setParameter(0, trimTpl.arg(i+1), rcTrim[i]);
QGC::SLEEP::usleep(50000);
mav->setParameter(0, maxTpl.arg(i+1), rcMax[i]);
QGC::SLEEP::usleep(50000);
mav->setParameter(0, revTpl.arg(i+1), (rcRev[i]) ? -1.0f : 1.0f);
QGC::SLEEP::usleep(50000);
}
// Write mappings
mav->setParameter(0, "RC_MAP_ROLL", (int32_t)(rcMapping[0]+1));
QGC::SLEEP::usleep(50000);
mav->setParameter(0, "RC_MAP_PITCH", (int32_t)(rcMapping[1]+1));
QGC::SLEEP::usleep(50000);
mav->setParameter(0, "RC_MAP_YAW", (int32_t)(rcMapping[2]+1));
QGC::SLEEP::usleep(50000);
mav->setParameter(0, "RC_MAP_THROTTLE", (int32_t)(rcMapping[3]+1));
QGC::SLEEP::usleep(50000);
mav->setParameter(0, "RC_MAP_MODE_SW", (int32_t)(rcMapping[4]+1));
QGC::SLEEP::usleep(50000);
mav->setParameter(0, "RC_MAP_AUX1", (int32_t)(rcMapping[5]+1));
QGC::SLEEP::usleep(50000);
mav->setParameter(0, "RC_MAP_AUX2", (int32_t)(rcMapping[6]+1));
QGC::SLEEP::usleep(50000);
mav->setParameter(0, "RC_MAP_AUX3", (int32_t)(rcMapping[7]+1));
QGC::SLEEP::usleep(50000);
}
void QGCVehicleConfig::requestCalibrationRC()
{
paramMgr->requestRcCalibrationParamsUpdate();
}
void QGCVehicleConfig::writeParameters()
{
updateStatus(tr("Writing all onboard parameters."));
writeCalibrationRC();
mav->writeParametersToStorage();
}
void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val)
{
// Check if index and values are sane
if (chan < 0 || static_cast<unsigned int>(chan) >= chanMax || val < 500 || val > 2500)
chanCount = chan+1;
}
// Update calibration data