From 17fc6302a6af77f11310cdfcd4babd1c2ae8e263 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 Aug 2013 16:30:57 +0200 Subject: [PATCH] Improved calibration screen a lot, gives now much better feedback what to do --- files/styles/style-dark.css | 30 ++- .../QGCPX4SensorCalibration.cc | 232 +++++++++++++++++- .../QGCPX4SensorCalibration.h | 20 ++ .../QGCPX4SensorCalibration.ui | 115 ++++++--- 4 files changed, 359 insertions(+), 38 deletions(-) diff --git a/files/styles/style-dark.css b/files/styles/style-dark.css index 26b68253b..136c7bc32 100644 --- a/files/styles/style-dark.css +++ b/files/styles/style-dark.css @@ -208,6 +208,27 @@ QGCToolBar .QWidget { background-color: transparent; } +QGCPX4SensorCalibration QLabel#magLabel { + font-size: 15pt; + font-weight: bold; + border-radius: 4px; + min-height: 25px; +} + +QGCPX4SensorCalibration QLabel#gyroLabel { + font-size: 15pt; + font-weight: bold; + border-radius: 4px; + min-height: 25px; +} + +QGCPX4SensorCalibration QLabel#accelLabel { + font-size: 15pt; + font-weight: bold; + border-radius: 4px; + min-height: 25px; +} + QGCToolWidgetItem { border: 1px solid #666; border-radius: 3px; @@ -409,12 +430,9 @@ QPushButton#viewModeGeneric, QPushButton#viewModePX4, QPushButton#viewModeAPM, Q } QPushButton#magButton, QPushButton#gyroButton, QPushButton#accelButton { - background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #73D95D, stop: 1 #18A154); - border-radius: 8px; - min-height: 30px; - max-height: 50px; - min-width: 60px; - border: 3px solid #465158; + background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #757575, stop: 1 #333); + border-radius: 5px; + border: 1px solid #000000; } QWidget#containerWidget { diff --git a/src/ui/px4_configuration/QGCPX4SensorCalibration.cc b/src/ui/px4_configuration/QGCPX4SensorCalibration.cc index aeaf072d5..e9c1fb72c 100644 --- a/src/ui/px4_configuration/QGCPX4SensorCalibration.cc +++ b/src/ui/px4_configuration/QGCPX4SensorCalibration.cc @@ -33,7 +33,67 @@ QGCPX4SensorCalibration::QGCPX4SensorCalibration(QWidget *parent) : ui->magButton->setEnabled(false); ui->accelButton->setEnabled(false); + ui->autopilotComboBox->setEnabled(false); + ui->magComboBox->setEnabled(false); + setInstructionImage(":/files/images/px4/calibration/accel_z-.png"); + setAutopilotImage(":/files/images/px4/calibration/accel_z-.png"); + setGpsImage(":/files/images/px4/calibration/accel_z-.png"); + + // Fill combo boxes + ui->autopilotComboBox->addItem(tr("Default Orientation"), 0); + ui->autopilotComboBox->addItem(tr("ROTATION_YAW_45"), 1); + ui->autopilotComboBox->addItem(tr("ROTATION_YAW_90"), 2); + ui->autopilotComboBox->addItem(tr("ROTATION_YAW_135"), 3); + ui->autopilotComboBox->addItem(tr("ROTATION_YAW_180"), 4); + ui->autopilotComboBox->addItem(tr("ROTATION_YAW_225"), 5); + ui->autopilotComboBox->addItem(tr("ROTATION_YAW_270"), 6); + ui->autopilotComboBox->addItem(tr("ROTATION_YAW_315"), 7); + ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_180"), 8); + ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_180_YAW_45"), 9); + ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_180_YAW_90"), 10); + ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_180_YAW_135"), 11); + ui->autopilotComboBox->addItem(tr("ROTATION_PITCH_180"), 12); + ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_180_YAW_225"), 13); + ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_180_YAW_270"), 14); + ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_180_YAW_315"), 15); + ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_90"), 16); + ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_90_YAW_45"), 17); + ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_90_YAW_90"), 18); + ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_90_YAW_135"), 19); + ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_270"), 20); + ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_270_YAW_45"), 21); + ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_270_YAW_90"), 22); + ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_270_YAW_135"), 23); + ui->autopilotComboBox->addItem(tr("ROTATION_PITCH_90"), 24); + ui->autopilotComboBox->addItem(tr("ROTATION_PITCH_270"), 25); + + ui->magComboBox->addItem(tr("Default Orientation"), 0); + ui->magComboBox->addItem(tr("ROTATION_YAW_45"), 1); + ui->magComboBox->addItem(tr("ROTATION_YAW_90"), 2); + ui->magComboBox->addItem(tr("ROTATION_YAW_135"), 3); + ui->magComboBox->addItem(tr("ROTATION_YAW_180"), 4); + ui->magComboBox->addItem(tr("ROTATION_YAW_225"), 5); + ui->magComboBox->addItem(tr("ROTATION_YAW_270"), 6); + ui->magComboBox->addItem(tr("ROTATION_YAW_315"), 7); + ui->magComboBox->addItem(tr("ROTATION_ROLL_180"), 8); + ui->magComboBox->addItem(tr("ROTATION_ROLL_180_YAW_45"), 9); + ui->magComboBox->addItem(tr("ROTATION_ROLL_180_YAW_90"), 10); + ui->magComboBox->addItem(tr("ROTATION_ROLL_180_YAW_135"), 11); + ui->magComboBox->addItem(tr("ROTATION_PITCH_180"), 12); + ui->magComboBox->addItem(tr("ROTATION_ROLL_180_YAW_225"), 13); + ui->magComboBox->addItem(tr("ROTATION_ROLL_180_YAW_270"), 14); + ui->magComboBox->addItem(tr("ROTATION_ROLL_180_YAW_315"), 15); + ui->magComboBox->addItem(tr("ROTATION_ROLL_90"), 16); + ui->magComboBox->addItem(tr("ROTATION_ROLL_90_YAW_45"), 17); + ui->magComboBox->addItem(tr("ROTATION_ROLL_90_YAW_90"), 18); + ui->magComboBox->addItem(tr("ROTATION_ROLL_90_YAW_135"), 19); + ui->magComboBox->addItem(tr("ROTATION_ROLL_270"), 20); + ui->magComboBox->addItem(tr("ROTATION_ROLL_270_YAW_45"), 21); + ui->magComboBox->addItem(tr("ROTATION_ROLL_270_YAW_90"), 22); + ui->magComboBox->addItem(tr("ROTATION_ROLL_270_YAW_135"), 23); + ui->magComboBox->addItem(tr("ROTATION_PITCH_90"), 24); + ui->magComboBox->addItem(tr("ROTATION_PITCH_270"), 25); setObjectName("PX4_SENSOR_CALIBRATION"); @@ -42,6 +102,11 @@ QGCPX4SensorCalibration::QGCPX4SensorCalibration(QWidget *parent) : setActiveUAS(UASManager::instance()->getActiveUAS()); connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); ui->progressBar->setValue(0); + + connect(ui->autopilotComboBox, SIGNAL(activated(int)), this, SLOT(setAutopilotOrientation(int))); + connect(ui->magComboBox, SIGNAL(activated(int)), this, SLOT(setGpsOrientation(int))); + + // XXX ask for params (if not loaded) } QGCPX4SensorCalibration::~QGCPX4SensorCalibration() @@ -49,6 +114,106 @@ QGCPX4SensorCalibration::~QGCPX4SensorCalibration() delete ui; } +void QGCPX4SensorCalibration::parameterChanged(int uas, int component, QString parameterName, QVariant value) +{ + Q_UNUSED(uas); + Q_UNUSED(component); + + int index = (int)value.toFloat(); + + if (parameterName.contains("SENS_BOARD_ROT")) + { + ui->autopilotComboBox->setCurrentIndex(index); + setAutopilotImage(index); + ui->autopilotComboBox->setEnabled(true); + } + + if (parameterName.contains("SENS_EXT_MAG_ROT")) + { + ui->magComboBox->setCurrentIndex(index); + setGpsImage(index); + ui->magComboBox->setEnabled(true); + } + + // Check mag calibration naively + if (parameterName.contains("SENS_MAG_XOFF")) { + float offset = value.toFloat(); + if (offset < 0.000001f && offset > -0.000001f) { + // Must be zero, not good + setMagCalibrated(false); + } else { + setMagCalibrated(true); + } + } + + // Check gyro calibration naively + if (parameterName.contains("SENS_GYRO_XOFF")) { + float offset = value.toFloat(); + if (offset < 0.000001f && offset > -0.000001f) { + // Must be zero, not good + setGyroCalibrated(false); + } else { + setGyroCalibrated(true); + } + } + + // Check accel calibration naively + if (parameterName.contains("SENS_ACC_XOFF")) { + float offset = value.toFloat(); + if (offset < 0.000001f && offset > -0.000001f) { + // Must be zero, not good + setAccelCalibrated(false); + } else { + setAccelCalibrated(true); + } + } +} + +void QGCPX4SensorCalibration::setMagCalibrated(bool calibrated) +{ + if (calibrated) { + ui->magLabel->setText(tr("MAG CALIBRATED")); + ui->magLabel->setStyleSheet("QLabel { color: #FFFFFF;" + "background-color: #20AA20;" + "}"); + } else { + ui->magLabel->setText(tr("MAG UNCALIBRATED")); + ui->magLabel->setStyleSheet("QLabel { color: #FFFFFF;" + "background-color: #FF0037;" + "}"); + } +} + +void QGCPX4SensorCalibration::setGyroCalibrated(bool calibrated) +{ + if (calibrated) { + ui->gyroLabel->setText(tr("GYRO CALIBRATED")); + ui->gyroLabel->setStyleSheet("QLabel { color: #FFFFFF;" + "background-color: #20AA20;" + "}"); + } else { + ui->gyroLabel->setText(tr("GYRO UNCALIBRATED")); + ui->gyroLabel->setStyleSheet("QLabel { color: #FFFFFF;" + "background-color: #FF0037;" + "}"); + } +} + +void QGCPX4SensorCalibration::setAccelCalibrated(bool calibrated) +{ + if (calibrated) { + ui->accelLabel->setText(tr("ACCEL CALIBRATED")); + ui->accelLabel->setStyleSheet("QLabel { color: #FFFFFF;" + "background-color: #20AA20;" + "}"); + } else { + ui->accelLabel->setText(tr("ACCEL UNCALIBRATED")); + ui->accelLabel->setStyleSheet("QLabel { color: #FFFFFF;" + "background-color: #FF0037;" + "}"); + } +} + void QGCPX4SensorCalibration::setInstructionImage(const QString &path) { instructionIcon.load(path); @@ -56,7 +221,53 @@ void QGCPX4SensorCalibration::setInstructionImage(const QString &path) int w = ui->iconLabel->width(); int h = ui->iconLabel->height(); - ui->iconLabel->setPixmap(instructionIcon.scaled(w, h, Qt::KeepAspectRatio)); + ui->iconLabel->setPixmap(instructionIcon.scaled(w, h, Qt::KeepAspectRatio, Qt::SmoothTransformation)); +} + +void QGCPX4SensorCalibration::setAutopilotImage(int index) +{ + setAutopilotImage(QString(":/files/images/px4/calibration/pixhawk_%1.png").arg(index)); +} + +void QGCPX4SensorCalibration::setGpsImage(int index) +{ + setGpsImage(QString(":/files/images/px4/calibration/gps_%1.png").arg(index)); +} + +void QGCPX4SensorCalibration::setAutopilotOrientation(int index) +{ + if (activeUAS) { + activeUAS->getParamManager()->setPendingParam(0, "SENS_BOARD_ROT", (int)index); + activeUAS->getParamManager()->sendPendingParameters(true); + } +} + +void QGCPX4SensorCalibration::setGpsOrientation(int index) +{ + if (activeUAS) { + activeUAS->getParamManager()->setPendingParam(0, "SENS_EXT_MAG_ROT", (int)index); + activeUAS->getParamManager()->sendPendingParameters(true); + } +} + +void QGCPX4SensorCalibration::setAutopilotImage(const QString &path) +{ + autopilotIcon.load(path); + + int w = ui->autopilotLabel->width(); + int h = ui->autopilotLabel->height(); + + ui->autopilotLabel->setPixmap(autopilotIcon.scaled(w, h, Qt::KeepAspectRatio, Qt::SmoothTransformation)); +} + +void QGCPX4SensorCalibration::setGpsImage(const QString &path) +{ + gpsIcon.load(path); + + int w = ui->gpsLabel->width(); + int h = ui->gpsLabel->height(); + + ui->gpsLabel->setPixmap(gpsIcon.scaled(w, h, Qt::KeepAspectRatio, Qt::SmoothTransformation)); } void QGCPX4SensorCalibration::resizeEvent(QResizeEvent* event) @@ -64,8 +275,15 @@ void QGCPX4SensorCalibration::resizeEvent(QResizeEvent* event) int w = ui->iconLabel->width(); int h = ui->iconLabel->height(); + ui->iconLabel->setPixmap(instructionIcon.scaled(w, h, Qt::KeepAspectRatio, Qt::SmoothTransformation)); + + int wa = ui->autopilotLabel->width(); + int ha = ui->autopilotLabel->height(); + ui->autopilotLabel->setPixmap(autopilotIcon.scaled(wa, ha, Qt::KeepAspectRatio, Qt::SmoothTransformation)); - ui->iconLabel->setPixmap(instructionIcon.scaled(w, h, Qt::KeepAspectRatio)); + int wg = ui->gpsLabel->width(); + int hg = ui->gpsLabel->height(); + ui->gpsLabel->setPixmap(gpsIcon.scaled(wg, hg, Qt::KeepAspectRatio, Qt::SmoothTransformation)); QWidget::resizeEvent(event); } @@ -76,7 +294,8 @@ void QGCPX4SensorCalibration::setActiveUAS(UASInterface* uas) return; if (activeUAS) { - disconnect(uas, SIGNAL(textMessageReceived(int,int,int,QString)), this, SLOT(handleTextMessage(int,int,int,QString))); + disconnect(activeUAS, SIGNAL(textMessageReceived(int,int,int,QString)), this, SLOT(handleTextMessage(int,int,int,QString))); + disconnect(activeUAS, SIGNAL(parameterChanged(int,int,QString,QVariant)), this, SLOT(parameterChanged(int,int,QString,QVariant))); ui->textView->clear(); } @@ -85,6 +304,7 @@ void QGCPX4SensorCalibration::setActiveUAS(UASInterface* uas) ui->accelButton->setEnabled(true); connect(uas, SIGNAL(textMessageReceived(int,int,int,QString)), this, SLOT(handleTextMessage(int,int,int,QString))); + connect(uas, SIGNAL(parameterChanged(int,int,QString,QVariant)), this, SLOT(parameterChanged(int,int,QString,QVariant))); activeUAS = uas; } @@ -161,12 +381,16 @@ void QGCPX4SensorCalibration::handleTextMessage(int uasid, int compId, int sever accelStarted = false; // XXX use a confirmation image or something setInstructionImage(":/files/images/px4/calibration/accel_z-.png"); + if (activeUAS) + activeUAS->requestParameter(0, "SENS_ACC_XOFF"); } if (text.contains("gyro calibration done")) { gyroStarted = false; // XXX use a confirmation image or something setInstructionImage(":/files/images/px4/calibration/accel_z-.png"); + if (activeUAS) + activeUAS->requestParameter(0, "SENS_GYRO_XOFF"); } if (text.contains("mag calibration done") @@ -174,6 +398,8 @@ void QGCPX4SensorCalibration::handleTextMessage(int uasid, int compId, int sever magStarted = false; // XXX use a confirmation image or something setInstructionImage(":/files/images/px4/calibration/accel_z-.png"); + if (activeUAS) + activeUAS->requestParameter(0, "SENS_MAG_XOFF"); } if (text.contains("accel calibration started")) { diff --git a/src/ui/px4_configuration/QGCPX4SensorCalibration.h b/src/ui/px4_configuration/QGCPX4SensorCalibration.h index b6cfd1bd2..50500a83c 100644 --- a/src/ui/px4_configuration/QGCPX4SensorCalibration.h +++ b/src/ui/px4_configuration/QGCPX4SensorCalibration.h @@ -42,12 +42,28 @@ public slots: */ virtual void contextMenuEvent(QContextMenuEvent* event); + void setAutopilotOrientation(int index); + void setGpsOrientation(int index); + void parameterChanged(int uas, int component, QString parameterName, QVariant value); + +protected slots: + void setInstructionImage(const QString &path); + void setAutopilotImage(const QString &path); + + void setGpsImage(const int index); + + void setAutopilotImage(const int index); + + void setGpsImage(const QString &path); + protected: UASInterface* activeUAS; QAction* clearAction; QPixmap instructionIcon; + QPixmap autopilotIcon; + QPixmap gpsIcon; bool accelStarted; bool accelDone[6]; bool gyroStarted; @@ -55,6 +71,10 @@ protected: QStringList accelAxes; virtual void resizeEvent(QResizeEvent* event); + + void setMagCalibrated(bool calibrated); + void setGyroCalibrated(bool calibrated); + void setAccelCalibrated(bool calibrated); private: Ui::QGCPX4SensorCalibration *ui; diff --git a/src/ui/px4_configuration/QGCPX4SensorCalibration.ui b/src/ui/px4_configuration/QGCPX4SensorCalibration.ui index 7f6b0b821..0533f386b 100644 --- a/src/ui/px4_configuration/QGCPX4SensorCalibration.ui +++ b/src/ui/px4_configuration/QGCPX4SensorCalibration.ui @@ -6,8 +6,8 @@ 0 0 - 531 - 448 + 657 + 598 @@ -36,8 +36,29 @@ QPushButton#gyroButton, QPushButton#accelButton { border: 2px solid #465158; } - - + + + + + Autopilot and GPS / Compass Orientation + + + + + + + + + + + + + + 24 + + + + Qt::Vertical @@ -45,12 +66,12 @@ QPushButton#gyroButton, QPushButton#accelButton { 498 - 21 + 5 - + Qt::Horizontal @@ -63,19 +84,51 @@ QPushButton#gyroButton, QPushButton#accelButton { - - + + + + + + + + + + + + + true + - - - - + + + + - Magnetometer + + + + Qt::AlignCenter + + + + + + + + + + Qt::AlignCenter + + + + + + + Gyroscope @@ -83,10 +136,10 @@ QPushButton#gyroButton, QPushButton#accelButton { - - + + - Gyroscope + Magnetometer @@ -94,7 +147,7 @@ QPushButton#gyroButton, QPushButton#accelButton { - + Accelerometer @@ -105,28 +158,32 @@ QPushButton#gyroButton, QPushButton#accelButton { + + + + + + + Qt::AlignCenter + + + - - + + - - true - - - - - 24 + + + + - - - -- 2.22.0