diff --git a/ground/gcs/src/plugins/config/configrevowidget.cpp b/ground/gcs/src/plugins/config/configrevowidget.cpp index 0c2fb05d5..1823efa37 100644 --- a/ground/gcs/src/plugins/config/configrevowidget.cpp +++ b/ground/gcs/src/plugins/config/configrevowidget.cpp @@ -193,6 +193,22 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : addWidgetBinding("AttitudeSettings", "BoardRotation", m_ui->yawRotation, AttitudeSettings::BOARDROTATION_YAW); addWidgetBinding("AttitudeSettings", "AccelTau", m_ui->accelTau); + addWidgetBinding("AuxMagSettings", "Usage", m_ui->auxMagUsage, 0, 1, true); + addWidgetBinding("AuxMagSettings", "Type", m_ui->auxMagType, 0, 1, true); + + addWidgetBinding("AuxMagSettings", "BoardRotation", m_ui->auxMagRollRotation, AuxMagSettings::BOARDROTATION_ROLL); + addWidgetBinding("AuxMagSettings", "BoardRotation", m_ui->auxMagPitchRotation, AuxMagSettings::BOARDROTATION_PITCH); + addWidgetBinding("AuxMagSettings", "BoardRotation", m_ui->auxMagYawRotation, AuxMagSettings::BOARDROTATION_YAW); + + connect(MagSensor::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(onBoardAuxMagError())); + connect(MagState::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateMagStatus())); + + addWidget(m_ui->internalAuxErrorX); + addWidget(m_ui->internalAuxErrorY); + addWidget(m_ui->internalAuxErrorZ); + + displayMagError = false; + // Connect the help button connect(m_ui->attitudeHelp, SIGNAL(clicked()), this, SLOT(openHelp())); @@ -451,6 +467,98 @@ void ConfigRevoWidget::enableAllCalibrations() m_ui->thermalBiasStart->setEnabled(true); } +void ConfigRevoWidget::onBoardAuxMagError() +{ + magSensor = MagSensor::GetInstance(getObjectManager()); + Q_ASSERT(magSensor); + + auxMagSensor = AuxMagSensor::GetInstance(getObjectManager()); + Q_ASSERT(auxMagSensor); + + if (m_ui->tabWidget->currentIndex() != 2) { + // Restore metadata + if (displayMagError) { + magSensor->setMetadata(metamag.magSensorMetadata); + auxMagSensor->setMetadata(metamag.auxMagSensorMetadata); + displayMagError = false; + } + return; + } + + if (!displayMagError) { + // Store current metadata settings + metamag.magSensorMetadata = magSensor->getMetadata(); + metamag.auxMagSensorMetadata = auxMagSensor->getMetadata(); + + // Apply new rates + UAVObject::Metadata mdata = magSensor->getMetadata(); + UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); + mdata.flightTelemetryUpdatePeriod = 100; + magSensor->setMetadata(mdata); + + mdata = auxMagSensor->getMetadata(); + UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); + mdata.flightTelemetryUpdatePeriod = 100; + auxMagSensor->setMetadata(mdata); + + displayMagError = true; + } + + MagSensor::DataFields magData = magSensor->getData(); + AuxMagSensor::DataFields auxMagData = auxMagSensor->getData(); + + // Smooth Mag readings + float alpha = 0.8f; + float inv_alpha = (1.0f - alpha); + onboardMag[0] = (onboardMag[0] * alpha) + (magData.x * inv_alpha); + onboardMag[1] = (onboardMag[1] * alpha) + (magData.y * inv_alpha); + onboardMag[2] = (onboardMag[2] * alpha) + (magData.z * inv_alpha); + + auxMag[0] = (auxMag[0] * alpha) + (auxMagData.x * inv_alpha); + auxMag[1] = (auxMag[1] * alpha) + (auxMagData.y * inv_alpha); + auxMag[2] = (auxMag[2] * alpha) + (auxMagData.z * inv_alpha); + + // Normalize vectors + float magLenght = sqrt((onboardMag[0] * onboardMag[0]) + (onboardMag[1] * onboardMag[1]) + (onboardMag[2] * onboardMag[2])); + float auxMagLenght = sqrt((auxMag[0] * auxMag[0]) + (auxMag[1] * auxMag[1]) + (auxMag[2] * auxMag[2])); + + normalizedMag[0] = onboardMag[0] / magLenght; + normalizedMag[1] = onboardMag[1] / magLenght; + normalizedMag[2] = onboardMag[2] / magLenght; + + normalizedAuxMag[0] = auxMag[0] / auxMagLenght; + normalizedAuxMag[1] = auxMag[1] / auxMagLenght; + normalizedAuxMag[2] = auxMag[2] / auxMagLenght; + + // Calc diff and scale + float xDiff = (normalizedMag[0] - normalizedAuxMag[0]) * 25.0f; + float yDiff = (normalizedMag[1] - normalizedAuxMag[1]) * 25.0f; + float zDiff = (normalizedMag[2] - normalizedAuxMag[2]) * 25.0f; + + // Display Mag/AuxMag diff for every axis + m_ui->internalAuxErrorX->setValue(xDiff > 50.0f ? 50.0f : xDiff < -50.0f ? -50.0f : xDiff); + m_ui->internalAuxErrorY->setValue(yDiff > 50.0f ? 50.0f : yDiff < -50.0f ? -50.0f : yDiff); + m_ui->internalAuxErrorZ->setValue(zDiff > 50.0f ? 50.0f : zDiff < -50.0f ? -50.0f : zDiff); +} + +void ConfigRevoWidget::updateMagStatus() +{ + magState = MagState::GetInstance(getObjectManager()); + Q_ASSERT(magState); + + MagState::DataFields magStateData = magState->getData(); + + if (magStateData.Source == MagState::SOURCE_INVALID) { + m_ui->magStatusSource->setText(tr("Source invalid")); + } else if (magStateData.Source == MagState::SOURCE_ONBOARD) { + m_ui->magStatusSource->setText(tr("OnBoard mag")); + } else if (magStateData.Source == MagState::SOURCE_AUX) { + m_ui->magStatusSource->setText(tr("External mag")); + } else { + m_ui->magStatusSource->setText(tr("Unknown")); + } +} + void ConfigRevoWidget::openHelp() { QDesktopServices::openUrl(QUrl(QString(WIKI_URL_ROOT) + QString("Revo+Attitude+Configuration"), diff --git a/ground/gcs/src/plugins/config/configrevowidget.h b/ground/gcs/src/plugins/config/configrevowidget.h index 999b5ef58..01b40c1cf 100644 --- a/ground/gcs/src/plugins/config/configrevowidget.h +++ b/ground/gcs/src/plugins/config/configrevowidget.h @@ -37,6 +37,11 @@ #include "calibration/levelcalibrationmodel.h" #include "calibration/gyrobiascalibrationmodel.h" +#include +#include +#include +#include + #include #include #include @@ -53,6 +58,26 @@ public: ConfigRevoWidget(QWidget *parent = 0); ~ConfigRevoWidget(); + typedef struct { + UAVObject::Metadata magSensorMetadata; + UAVObject::Metadata auxMagSensorMetadata; + } MetaMag; + + MetaMag metamag; + + bool displayMagError; + + AuxMagSettings *auxMagSettings; + MagSensor *magSensor; + AuxMagSensor *auxMagSensor; + MagState *magState; + + float onboardMag[3]; + float auxMag[3]; + + float normalizedMag[3]; + float normalizedAuxMag[3]; + private: OpenPilot::SixPointCalibrationModel *m_accelCalibrationModel; OpenPilot::SixPointCalibrationModel *m_magCalibrationModel; @@ -87,6 +112,9 @@ private slots: void disableAllCalibrations(); void enableAllCalibrations(); + void onBoardAuxMagError(); + void updateMagStatus(); + void updateVisualHelp(); void openHelp(); diff --git a/ground/gcs/src/plugins/config/revosensors.ui b/ground/gcs/src/plugins/config/revosensors.ui index 1e7fbf587..7de5942cf 100644 --- a/ground/gcs/src/plugins/config/revosensors.ui +++ b/ground/gcs/src/plugins/config/revosensors.ui @@ -6,7 +6,7 @@ 0 0 - 1014 + 890 725 @@ -438,8 +438,8 @@ <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'MS Shell Dlg 2'; font-size:10pt; font-weight:400; font-style:normal;"> -<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-weight:600;"><br /></p></body></html> +</style></head><body style=" font-family:'Cantarell'; font-size:10pt; font-weight:400; font-style:normal;"> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'MS Shell Dlg 2'; font-weight:600;"><br /></p></body></html> Qt::NoTextInteraction @@ -950,6 +950,528 @@ A setting of 0.00 disables the filter. + + + true + + + External Mag Settings + + + + + + + + 0 + + + + + + 0 + 0 + + + + Magnetometer Settings + + + + + + + 80 + 0 + + + + Mag type: + + + + + + + + 80 + 0 + + + + Mag use: + + + + + + + + 100 + 0 + + + + Select the magnetometer type. + + + + + + + + 0 + 0 + + + + + 100 + 0 + + + + Select how to use magnetometers. + + + + + + + Qt::Horizontal + + + QSizePolicy::MinimumExpanding + + + + 40 + 20 + + + + + + + + + + + + 0 + 0 + + + + Magnetometer Status + + + + + + + 0 + 0 + + + + external + + + + + + + + 110 + 0 + + + + Mag source: + + + + + + + + + + + 0 + 0 + + + + Rotate Magnetometer Orientation + + + + + + + 70 + 30 + + + + + 0 + 30 + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; +margin:1px; +font:bold; + + + Roll + + + Qt::AlignCenter + + + + + + + 0 + + + -180.000000000000000 + + + 180.000000000000000 + + + + + + + 0 + + + -180.000000000000000 + + + 180.000000000000000 + + + + + + + + 0 + 0 + + + + + 70 + 30 + + + + + 0 + 30 + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; +margin:1px; +font:bold; + + + Pitch + + + Qt::AlignCenter + + + + + + + + + + 0 + + + -180.000000000000000 + + + 180.000000000000000 + + + + + + + + 0 + 0 + + + + + 70 + 30 + + + + + 0 + 30 + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; +margin:1px; +font:bold; + + + Yaw + + + Qt::AlignCenter + + + + + + + + + + + + + 0 + 0 + + + + + 500 + 0 + + + + External Mag Orientation Help + + + + 6 + + + 6 + + + 6 + + + 6 + + + + + + 80 + 30 + + + + + 16777215 + 30 + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; +margin:1px; +font:bold; + + + Z axis + + + Qt::AlignCenter + + + + + + + Difference on Y axis + + + -50 + + + 50 + + + 0 + + + Qt::AlignCenter + + + %v + + + + + + + Difference on Z axis + + + -50 + + + 50 + + + 0 + + + Qt::AlignCenter + + + %v + + + + + + + + 80 + 30 + + + + + 16777215 + 30 + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; +margin:1px; +font:bold; + + + Y axis + + + Qt::AlignCenter + + + + + + + Difference on X axis + + + -50 + + + 50 + + + 0 + + + Qt::AlignCenter + + + true + + + %v + + + + + + + + 80 + 30 + + + + + 16777215 + 30 + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; +margin:1px; +font:bold; + + + X axis + + + Qt::AlignCenter + + + + + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Cantarell'; font-size:11pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">The bargraphs show the difference between the onboard magnetometer and external magnetometer measurements. </p> +<p align="justify" style=" margin-top:12px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">When the external magnetometer rotation is set correctlly, all bargraphs should show all zero (bargraph centered) <a name="result_box"></a>whatever the vehicle's orientation.</p> +<p align="justify" style=" margin-top:12px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">This assumes the onboard magnetometer is also calibrated (Mag use=Both).</p></body></html> + + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + Help