diff --git a/files/styles/style-dark.css b/files/styles/style-dark.css index 270e01c427b1ea935c5f2d49c3fc69e9bd1945de..db24be25841fda72f1c7f2d8a8c3976606f8479f 100644 --- a/files/styles/style-dark.css +++ b/files/styles/style-dark.css @@ -248,6 +248,19 @@ QLabel#noUas { font-size: 30pt; } +QMessageBox QLabel { + font-size: 12pt; +} + +QLabel#calibrationExplanationLabel { + font-size: 20pt; +} + +QLabel#instructionLabel { + color: #FEC654; + font-size: 26pt; +} + QLineEdit { border: 1px solid #777; border-radius: 2px; diff --git a/src/ui/QGCPX4VehicleConfig.cc b/src/ui/QGCPX4VehicleConfig.cc index 5fc4d96c1300c67152b0e7fb1d5eaa8ff0645154..740b14714990e4412c3ffa7a4b876f34d370f14e 100644 --- a/src/ui/QGCPX4VehicleConfig.cc +++ b/src/ui/QGCPX4VehicleConfig.cc @@ -1027,9 +1027,9 @@ void QGCPX4VehicleConfig::setActiveUAS(UASInterface* active) foreach(QWidget* child, ui->advanceColumnContents->findChildren()) { child->deleteLater(); } - foreach(QWidget* child, ui->sensorLayout->findChildren()) { - child->deleteLater(); - } +// foreach(QWidget* child, ui->sensorLayout->findChildren()) { +// child->deleteLater(); +// } foreach(QWidget* child, ui->airframeLayout->findChildren()) { diff --git a/src/ui/px4_configuration/QGCPX4SensorCalibration.cc b/src/ui/px4_configuration/QGCPX4SensorCalibration.cc index a979a575a20ecdb2476e6f2a56727a1426c099b1..f0d328eb29314cd507d2cb0a4dfe205dd9dc095b 100644 --- a/src/ui/px4_configuration/QGCPX4SensorCalibration.cc +++ b/src/ui/px4_configuration/QGCPX4SensorCalibration.cc @@ -3,16 +3,35 @@ #include #include #include +#include QGCPX4SensorCalibration::QGCPX4SensorCalibration(QWidget *parent) : QWidget(parent), activeUAS(NULL), clearAction(new QAction(tr("Clear Text"), this)), + accelStarted(false), + gyroStarted(false), + magStarted(false), ui(new Ui::QGCPX4SensorCalibration) { + accelAxes << "x+"; + accelAxes << "x-"; + accelAxes << "y+"; + accelAxes << "y-"; + accelAxes << "z+"; + accelAxes << "z-"; + ui->setupUi(this); connect(clearAction, SIGNAL(triggered()), ui->textView, SLOT(clear())); + connect(ui->gyroButton, SIGNAL(clicked()), this, SLOT(gyroButtonClicked())); + connect(ui->magButton, SIGNAL(clicked()), this, SLOT(magButtonClicked())); + connect(ui->accelButton, SIGNAL(clicked()), this, SLOT(accelButtonClicked())); + + ui->gyroButton->setEnabled(false); + ui->magButton->setEnabled(false); + ui->accelButton->setEnabled(false); + setInstructionImage("./files/images/px4/calibration/accel_z-.png"); setObjectName("PX4_SENSOR_CALIBRATION"); @@ -21,6 +40,7 @@ QGCPX4SensorCalibration::QGCPX4SensorCalibration(QWidget *parent) : setActiveUAS(UASManager::instance()->getActiveUAS()); connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); + ui->progressBar->setValue(0); } QGCPX4SensorCalibration::~QGCPX4SensorCalibration() @@ -59,12 +79,90 @@ void QGCPX4SensorCalibration::setActiveUAS(UASInterface* uas) ui->textView->clear(); } + ui->gyroButton->setEnabled(true); + ui->magButton->setEnabled(true); + ui->accelButton->setEnabled(true); + connect(uas, SIGNAL(textMessageReceived(int,int,int,QString)), this, SLOT(handleTextMessage(int,int,int,QString))); activeUAS = uas; } void QGCPX4SensorCalibration::handleTextMessage(int uasid, int compId, int severity, QString text) { + if (text.startsWith("[cmd]") || + text.startsWith("[mavlink pm]")) + return; + + if (text.contains("progress")) { + qDebug() << "PROGRESS:" << text; + QString percent = text.split("<").last().split(">").first(); + qDebug() << "PERCENT CANDIDATE" << percent; + bool ok; + int p = percent.toInt(&ok); + if (ok) + ui->progressBar->setValue(p); + return; + } + + + if (text.contains("accel")) { + qDebug() << "ACCEL" << text; + + if (text.startsWith("accel meas started: ")) { + QString axis = text.split("meas started: ").last().right(2); + qDebug() << "AXIS" << axis << "STR" << text; + setInstructionImage(QString("./files/images/px4/calibration/accel_%1.png").arg(axis)); + + } + } + + if (text.startsWith("meas result for")) { + + QString axis = text.split("meas result for ").last().left(2); + + qDebug() << "ACCELDONE AXIS" << axis << "STR" << text; + + for (int i = 0; i < 6; i++) + { + if (accelAxes[i] == axis) + accelDone[i] = true; + + if (!accelDone[i]) { + setInstructionImage(QString("./files/images/px4/calibration/accel_%1.png").arg(accelAxes[i])); + ui->calibrationExplanationLabel->setText(tr("Axis %1 completed. Please rotate to a different axis, e.g. the one shown here.").arg(axis)); + } + } + } + + if (text.contains("please rotate in a figure 8")) { + ui->calibrationExplanationLabel->setText(tr("Rotate the system around all its axes, e.g. in a figure eight.")); + setInstructionImage("./files/images/px4/calibration/mag_calibration_figure8.png"); + } + + if (text.contains("accel calibration done")) { + accelStarted = false; + // XXX use a confirmation image or something + setInstructionImage("./files/images/px4/calibration/accel_z-.png"); + ui->calibrationExplanationLabel->setText(tr("Accelerometer calibration completed. Parameters permanently stored.")); + } + + if (text.contains("gyro calibration done")) { + gyroStarted = false; + // XXX use a confirmation image or something + setInstructionImage("./files/images/px4/calibration/accel_z-.png"); + ui->calibrationExplanationLabel->setText(tr("Gyroscope calibration completed. Parameters permanently stored.")); + } + + if (text.contains("mag calibration done")) { + magStarted = false; + // XXX use a confirmation image or something + setInstructionImage("./files/images/px4/calibration/accel_z-.png"); + ui->calibrationExplanationLabel->setText(tr("Magnetometer calibration completed. Parameters permanently stored.")); + } + + ui->instructionLabel->setText(QString("%1").arg(text)); + + // XXX color messages according to severity QPlainTextEdit *msgWidget = ui->textView; @@ -73,22 +171,43 @@ void QGCPX4SensorCalibration::handleTextMessage(int uasid, int compId, int sever msgWidget->setUpdatesEnabled(false); QScrollBar *scroller = msgWidget->verticalScrollBar(); - UASInterface *uas = UASManager::instance()->getUASForId(uasid); - QString uasName(uas->getUASName()); - QString colorName(uas->getColor().name()); - //change styling based on severity - if (160 == severity ) { //TODO where is the constant for "critical" severity? - //GAudioOutput::instance()->say(text.toLower()); - msgWidget->appendHtml(QString("

[%1:%2] %3

").arg(uasName).arg(compId).arg(text)); - } - else { - msgWidget->appendHtml(QString("

[%2:%3] %4

").arg(colorName).arg(uasName).arg(compId).arg(text)); - } + msgWidget->appendHtml(QString("%4").arg(text)); // Ensure text area scrolls correctly scroller->setValue(scroller->maximum()); msgWidget->setUpdatesEnabled(true); +} +void QGCPX4SensorCalibration::gyroButtonClicked() +{ + setInstructionImage("./files/images/px4/calibration/accel_z-.png"); + activeUAS->executeCommand(MAV_CMD_PREFLIGHT_CALIBRATION, 1, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0); + ui->progressBar->setValue(0); + ui->calibrationExplanationLabel->setText(tr("Please do not move the system at all and wait for calibration to complete.")); +} + +void QGCPX4SensorCalibration::magButtonClicked() +{ + setInstructionImage("./files/images/px4/calibration/accel_z-.png"); + activeUAS->executeCommand(MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0); + ui->progressBar->setValue(0); + ui->calibrationExplanationLabel->setText(tr("Please put the system in a rest position and wait for instructions.")); +} + +void QGCPX4SensorCalibration::accelButtonClicked() +{ + setInstructionImage("./files/images/px4/calibration/accel_z-.png"); + activeUAS->executeCommand(MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0); + ui->progressBar->setValue(0); + accelStarted = true; + accelDone[0] = false; + accelDone[1] = false; + accelDone[2] = false; + accelDone[3] = false; + accelDone[4] = false; + accelDone[5] = false; + + ui->calibrationExplanationLabel->setText(tr("Please hold the system very still in the shown orientations. Start with the one shown.")); } void QGCPX4SensorCalibration::contextMenuEvent(QContextMenuEvent* event) diff --git a/src/ui/px4_configuration/QGCPX4SensorCalibration.h b/src/ui/px4_configuration/QGCPX4SensorCalibration.h index 48d0093f5bcc1fc7b0d3d97b3f79be21b7a4ea11..b6cfd1bd22cc0b525e29b5b84dc18da5dc2d6ca3 100644 --- a/src/ui/px4_configuration/QGCPX4SensorCalibration.h +++ b/src/ui/px4_configuration/QGCPX4SensorCalibration.h @@ -32,6 +32,10 @@ public slots: */ void handleTextMessage(int uasid, int componentid, int severity, QString text); + void gyroButtonClicked(); + void magButtonClicked(); + void accelButtonClicked(); + /** * @brief Hand context menu event * @param event @@ -44,6 +48,11 @@ protected: UASInterface* activeUAS; QAction* clearAction; QPixmap instructionIcon; + bool accelStarted; + bool accelDone[6]; + bool gyroStarted; + bool magStarted; + QStringList accelAxes; virtual void resizeEvent(QResizeEvent* event); diff --git a/src/ui/px4_configuration/QGCPX4SensorCalibration.ui b/src/ui/px4_configuration/QGCPX4SensorCalibration.ui index eaca16b7c02fb6c1fe6ee4e5184b1f89d645dfb8..95ca42e8cfea4214221486ce3e798c62456aa1a7 100644 --- a/src/ui/px4_configuration/QGCPX4SensorCalibration.ui +++ b/src/ui/px4_configuration/QGCPX4SensorCalibration.ui @@ -36,35 +36,28 @@ QPushButton#gyroButton, QPushButton#accelButton { border: 2px solid #465158; } - - - - - Magnetometer Calibration - - - - - - - - - - + + + + + Qt::Horizontal - - true + + + 40 + 20 + - + - - + + - To calibrate the system, execute the three calibration steps below. + Magnetometer Calibration - + Qt::Vertical @@ -77,14 +70,24 @@ QPushButton#gyroButton, QPushButton#accelButton { - + + + + + + + + 24 - + + + + @@ -121,18 +124,22 @@ QPushButton#gyroButton, QPushButton#accelButton { - - - - Qt::Horizontal + + + + - - - 40 - 20 - + + true - + + + + + + To calibrate the system, execute the three calibration steps below. + +