Browse Source

setAutoConfig when setting autostart

- minor refactor of airframeconfig.cc
- add placeholder for new scratch build multirotor
QGC4.4
tstellanova 12 years ago
parent
commit
a6fcb12098
  1. 10
      src/ui/QGCPX4VehicleConfig.cc
  2. 28
      src/ui/px4_configuration/QGCPX4AirframeConfig.cc
  3. 1
      src/ui/px4_configuration/QGCPX4AirframeConfig.h

10
src/ui/QGCPX4VehicleConfig.cc

@ -176,6 +176,11 @@ QGCPX4VehicleConfig::QGCPX4VehicleConfig(QWidget *parent) :
updateTimer.start(); updateTimer.start();
} }
QGCPX4VehicleConfig::~QGCPX4VehicleConfig()
{
delete ui;
}
void QGCPX4VehicleConfig::rcMenuButtonClicked() void QGCPX4VehicleConfig::rcMenuButtonClicked()
{ {
//TODO eg ui->stackedWidget->findChild("rcConfig"); //TODO eg ui->stackedWidget->findChild("rcConfig");
@ -240,10 +245,7 @@ void QGCPX4VehicleConfig::identifyChannelMapping(int aert_index)
} }
QGCPX4VehicleConfig::~QGCPX4VehicleConfig()
{
delete ui;
}
void QGCPX4VehicleConfig::setRCModeIndex(int newRcMode) void QGCPX4VehicleConfig::setRCModeIndex(int newRcMode)
{ {

28
src/ui/px4_configuration/QGCPX4AirframeConfig.cc

@ -32,6 +32,7 @@ QGCPX4AirframeConfig::QGCPX4AirframeConfig(QWidget *parent) :
ui->quadXComboBox->addItem(tr("Standard 10\" Quad"), 1); ui->quadXComboBox->addItem(tr("Standard 10\" Quad"), 1);
ui->quadXComboBox->addItem(tr("DJI F330 8\" Quad"), 10); ui->quadXComboBox->addItem(tr("DJI F330 8\" Quad"), 10);
ui->quadXComboBox->addItem(tr("Turnigy Talon v2 X550 Quad"), 666);
connect(ui->quadXPushButton, SIGNAL(clicked()), this, SLOT(quadXSelected())); connect(ui->quadXPushButton, SIGNAL(clicked()), this, SLOT(quadXSelected()));
connect(ui->quadXComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(quadXSelected(int))); connect(ui->quadXComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(quadXSelected(int)));
@ -88,6 +89,7 @@ void QGCPX4AirframeConfig::setActiveUAS(UASInterface* uas)
return; return;
mav = uas; mav = uas;
paramMgr = mav->getParamManager();
connect(mav, SIGNAL(parameterChanged(int,int,QString,QVariant)), this, SLOT(parameterChanged(int,int,QString,QVariant))); connect(mav, SIGNAL(parameterChanged(int,int,QString,QVariant)), this, SLOT(parameterChanged(int,int,QString,QVariant)));
} }
@ -108,7 +110,7 @@ void QGCPX4AirframeConfig::uncheckAll()
void QGCPX4AirframeConfig::setAirframeID(int id) void QGCPX4AirframeConfig::setAirframeID(int id)
{ {
selectedId = id; selectedId = id;
qDebug() << "ID" << id; qDebug() << "setAirframeID" << selectedId;
ui->statusLabel->setText(tr("Ground start script ID: #%1").arg(selectedId)); ui->statusLabel->setText(tr("Ground start script ID: #%1").arg(selectedId));
// XXX too much boilerplate code here - this widget is really just // XXX too much boilerplate code here - this widget is really just
@ -117,6 +119,7 @@ void QGCPX4AirframeConfig::setAirframeID(int id)
if (id > 0 && id < 15) { if (id > 0 && id < 15) {
ui->quadXPushButton->setChecked(true); ui->quadXPushButton->setChecked(true);
ui->statusLabel->setText(tr("Selected quad X (ID: #%1)").arg(selectedId));
} }
else if (id >= 15 && id < 20) else if (id >= 15 && id < 20)
{ {
@ -152,15 +155,15 @@ void QGCPX4AirframeConfig::applyAndReboot()
if (!mav) if (!mav)
return; return;
if (mav->getParamManager()->countOnboardParams() == 0 && if (paramMgr->countOnboardParams() == 0 &&
mav->getParamManager()->countPendingParams() == 0) paramMgr->countPendingParams() == 0)
{ {
mav->getParamManager()->requestParameterListIfEmpty(); paramMgr->requestParameterListIfEmpty();
QGC::SLEEP::msleep(100); QGC::SLEEP::msleep(100);
} }
// Guard against the case of an edit where we didn't receive all params yet // Guard against the case of an edit where we didn't receive all params yet
if (mav->getParamManager()->countPendingParams() > 0) if (paramMgr->countPendingParams() > 0)
{ {
QMessageBox msgBox; QMessageBox msgBox;
msgBox.setText(tr("Parameter sync with UAS not yet complete")); msgBox.setText(tr("Parameter sync with UAS not yet complete"));
@ -172,7 +175,7 @@ void QGCPX4AirframeConfig::applyAndReboot()
return; return;
} }
QList<int> components = mav->getParamManager()->getComponentForParam("SYS_AUTOSTART"); QList<int> components = paramMgr->getComponentForParam("SYS_AUTOSTART");
// Guard against multiple components responding - this will never show in practice // Guard against multiple components responding - this will never show in practice
if (components.count() != 1) { if (components.count() != 1) {
@ -188,14 +191,17 @@ void QGCPX4AirframeConfig::applyAndReboot()
qDebug() << "Setting comp" << components.first() << "SYS_AUTOSTART" << (qint32)selectedId; qDebug() << "Setting comp" << components.first() << "SYS_AUTOSTART" << (qint32)selectedId;
mav->setParameter(components.first(), "SYS_AUTOSTART", (qint32)selectedId); paramMgr->setPendingParam(components.first(),"SYS_AUTOSTART", (qint32)selectedId);
//mav->getParamManager()->setParameter(components.first(), "SYS_AUTOSTART", (qint32)selectedId); //need to set autoconfig in order for PX4 to pick up the selected airframe params
setAutoConfig(true);
// Send pending params // Send pending params and then write them to persistent storage when done
mav->getParamManager()->sendPendingParameters(true); paramMgr->sendPendingParameters(true);
// Reboot // Reboot
//TODO right now this relies upon the above send & persist finishing before the reboot command is received...
QGC::SLEEP::msleep(3000);
mav->executeCommand(MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 1, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0); mav->executeCommand(MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 1, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0);
} }
@ -203,7 +209,7 @@ void QGCPX4AirframeConfig::setAutoConfig(bool enabled)
{ {
if (!mav) if (!mav)
return; return;
mav->getParamManager()->setParameter(0, "SYS_AUTOCONFIG", (qint32) ((enabled) ? 1 : 0)); paramMgr->setPendingParam(0, "SYS_AUTOCONFIG", (qint32) ((enabled) ? 1 : 0));
} }
void QGCPX4AirframeConfig::flyingWingSelected() void QGCPX4AirframeConfig::flyingWingSelected()

1
src/ui/px4_configuration/QGCPX4AirframeConfig.h

@ -85,6 +85,7 @@ protected:
private: private:
UASInterface* mav; UASInterface* mav;
QGCUASParamManager *paramMgr;
int selectedId; int selectedId;
Ui::QGCPX4AirframeConfig *ui; Ui::QGCPX4AirframeConfig *ui;
}; };

Loading…
Cancel
Save