From 45c42537b110819672e2288e1c20934e1a032cac Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Mon, 22 Feb 2016 00:02:37 +0100 Subject: [PATCH 1/6] LP-240 Add GUI tab for external Magnetometer --- .../src/plugins/config/configrevowidget.cpp | 108 ++++ .../gcs/src/plugins/config/configrevowidget.h | 28 + ground/gcs/src/plugins/config/revosensors.ui | 528 +++++++++++++++++- 3 files changed, 661 insertions(+), 3 deletions(-) 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 From 9c4b7498cd5df25921129d872d2459483f87b4c9 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Wed, 24 Feb 2016 15:53:18 +0100 Subject: [PATCH 2/6] LP-240 Add aux/onboard Mag alarms, max deviation level settings. Renamed Tab to "Mag settings" --- .../src/plugins/config/configrevowidget.cpp | 137 +++- .../gcs/src/plugins/config/configrevowidget.h | 13 +- ground/gcs/src/plugins/config/revosensors.ui | 613 +++++++++++------- 3 files changed, 529 insertions(+), 234 deletions(-) diff --git a/ground/gcs/src/plugins/config/configrevowidget.cpp b/ground/gcs/src/plugins/config/configrevowidget.cpp index 1823efa37..112341c79 100644 --- a/ground/gcs/src/plugins/config/configrevowidget.cpp +++ b/ground/gcs/src/plugins/config/configrevowidget.cpp @@ -196,12 +196,16 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : addWidgetBinding("AuxMagSettings", "Usage", m_ui->auxMagUsage, 0, 1, true); addWidgetBinding("AuxMagSettings", "Type", m_ui->auxMagType, 0, 1, true); + addWidgetBinding("RevoSettings", "MagnetometerMaxDeviation", m_ui->maxDeviationWarning, RevoSettings::MAGNETOMETERMAXDEVIATION_WARNING); + addWidgetBinding("RevoSettings", "MagnetometerMaxDeviation", m_ui->maxDeviationError, RevoSettings::MAGNETOMETERMAXDEVIATION_ERROR); + 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())); + connect(HomeLocation::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateMagBeVector())); addWidget(m_ui->internalAuxErrorX); addWidget(m_ui->internalAuxErrorY); @@ -400,6 +404,8 @@ void ConfigRevoWidget::refreshWidgetsValues(UAVObject *object) QString beStr = QString("%1:%2:%3").arg(QString::number(homeLocationData.Be[0]), QString::number(homeLocationData.Be[1]), QString::number(homeLocationData.Be[2])); m_ui->beBox->setText(beStr); + + getMagBeVector(); } void ConfigRevoWidget::updateObjectsFromWidgets() @@ -507,28 +513,45 @@ void ConfigRevoWidget::onBoardAuxMagError() MagSensor::DataFields magData = magSensor->getData(); AuxMagSensor::DataFields auxMagData = auxMagSensor->getData(); + onboardMag[0] = magData.x; + onboardMag[1] = magData.y; + onboardMag[2] = magData.z; + + auxMag[0] = auxMagData.x; + auxMag[1] = auxMagData.y; + auxMag[2] = auxMagData.z; + + float normalizedMag[3]; + float normalizedAuxMag[3]; + // 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); + onboardMagFiltered[0] = (onboardMagFiltered[0] * alpha) + (onboardMag[0] * inv_alpha); + onboardMagFiltered[1] = (onboardMagFiltered[1] * alpha) + (onboardMag[1] * inv_alpha); + onboardMagFiltered[2] = (onboardMagFiltered[2] * alpha) + (onboardMag[2] * inv_alpha); + + auxMagFiltered[0] = (auxMagFiltered[0] * alpha) + (auxMag[0] * inv_alpha); + auxMagFiltered[1] = (auxMagFiltered[1] * alpha) + (auxMag[1] * inv_alpha); + auxMagFiltered[2] = (auxMagFiltered[2] * alpha) + (auxMag[2] * 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])); + float magLenght = sqrt((onboardMagFiltered[0] * onboardMagFiltered[0]) + + (onboardMagFiltered[1] * onboardMagFiltered[1]) + + (onboardMagFiltered[2] * onboardMagFiltered[2])); + float auxMagLenght = sqrt((auxMagFiltered[0] * auxMagFiltered[0]) + + (auxMagFiltered[1] * auxMagFiltered[1]) + + (auxMagFiltered[2] * auxMagFiltered[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; + normalizedMag[0] = onboardMagFiltered[0] / magLenght; + normalizedMag[1] = onboardMagFiltered[1] / magLenght; + normalizedMag[2] = onboardMagFiltered[2] / magLenght; + + normalizedAuxMag[0] = auxMagFiltered[0] / auxMagLenght; + normalizedAuxMag[1] = auxMagFiltered[1] / auxMagLenght; + normalizedAuxMag[2] = auxMagFiltered[2] / auxMagLenght; // Calc diff and scale float xDiff = (normalizedMag[0] - normalizedAuxMag[0]) * 25.0f; @@ -539,6 +562,92 @@ void ConfigRevoWidget::onBoardAuxMagError() 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); + + updateMagAlarm(getMagError(onboardMag), getMagError(auxMag)); +} + +void ConfigRevoWidget::updateMagAlarm(float errorMag, float errorAuxMag) +{ + #define ALARM_THRESHOLD 20 + + RevoSettings *revoSettings = RevoSettings::GetInstance(getObjectManager()); + Q_ASSERT(revoSettings); + RevoSettings::DataFields revoSettingsData = revoSettings->getData(); + + QString bgColorMag = "green"; + QString bgColorAuxMag = "green"; + + // Onboard Mag + if (errorMag < revoSettingsData.MagnetometerMaxDeviation[RevoSettings::MAGNETOMETERMAXDEVIATION_WARNING]) { + magWarningCount = 0; + magErrorCount = 0; + } + + if (errorMag < revoSettingsData.MagnetometerMaxDeviation[RevoSettings::MAGNETOMETERMAXDEVIATION_ERROR]) { + magErrorCount = 0; + if (magWarningCount > ALARM_THRESHOLD) { + bgColorMag = "orange"; + } else { + magWarningCount++; + } + } + + if (magErrorCount > ALARM_THRESHOLD) { + bgColorMag = "red"; + } else { + magErrorCount++; + } + + // External Mag + if (errorAuxMag < revoSettingsData.MagnetometerMaxDeviation[RevoSettings::MAGNETOMETERMAXDEVIATION_WARNING]) { + auxMagWarningCount = 0; + auxMagErrorCount = 0; + } + + if (errorAuxMag < revoSettingsData.MagnetometerMaxDeviation[RevoSettings::MAGNETOMETERMAXDEVIATION_ERROR]) { + auxMagErrorCount = 0; + if (auxMagWarningCount > ALARM_THRESHOLD) { + bgColorAuxMag = "orange"; + } else { + auxMagWarningCount++; + } + } + + if (auxMagErrorCount > ALARM_THRESHOLD) { + bgColorAuxMag = "red"; + } else { + auxMagErrorCount++; + } + + m_ui->onBoardMagStatus->setStyleSheet( + "QLabel { background-color: " + bgColorMag + ";" + "color: rgb(255, 255, 255); border-radius: 5; margin:1px; font:bold; }"); + m_ui->auxMagStatus->setStyleSheet( + "QLabel { background-color: " + bgColorAuxMag + ";" + "color: rgb(255, 255, 255); border-radius: 5; margin:1px; font:bold; }"); +} + +float ConfigRevoWidget::getMagError(float mag[3]) +{ + float magnitude = sqrt((mag[0] * mag[0]) + (mag[1] * mag[1]) + (mag[2] * mag[2])); + float magnitudeBe = sqrt((magBe[0] * magBe[0]) + (magBe[1] * magBe[1]) + (magBe[2] * magBe[2])); + float invMagnitudeBe = 1.0f / magnitudeBe; + // Absolute value of relative error against Be + float error = fabsf(magnitude - magnitudeBe) * invMagnitudeBe; + + return error; +} + +void ConfigRevoWidget::getMagBeVector() +{ + HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager()); + + Q_ASSERT(homeLocation); + HomeLocation::DataFields homeLocationData = homeLocation->getData(); + + magBe[0] = homeLocationData.Be[0]; + magBe[1] = homeLocationData.Be[1]; + magBe[2] = homeLocationData.Be[2]; } void ConfigRevoWidget::updateMagStatus() diff --git a/ground/gcs/src/plugins/config/configrevowidget.h b/ground/gcs/src/plugins/config/configrevowidget.h index 01b40c1cf..dd0deb22b 100644 --- a/ground/gcs/src/plugins/config/configrevowidget.h +++ b/ground/gcs/src/plugins/config/configrevowidget.h @@ -74,9 +74,14 @@ public: float onboardMag[3]; float auxMag[3]; + float onboardMagFiltered[3]; + float auxMagFiltered[3]; + float magBe[3]; - float normalizedMag[3]; - float normalizedAuxMag[3]; + int magWarningCount; + int magErrorCount; + int auxMagWarningCount; + int auxMagErrorCount; private: OpenPilot::SixPointCalibrationModel *m_accelCalibrationModel; @@ -114,6 +119,10 @@ private slots: void onBoardAuxMagError(); void updateMagStatus(); + void getMagBeVector(); + void updateMagAlarm(float errorMag, float errorAuxMag); + + float getMagError(float mag[3]); void updateVisualHelp(); void openHelp(); diff --git a/ground/gcs/src/plugins/config/revosensors.ui b/ground/gcs/src/plugins/config/revosensors.ui index 7de5942cf..79d1cbc73 100644 --- a/ground/gcs/src/plugins/config/revosensors.ui +++ b/ground/gcs/src/plugins/config/revosensors.ui @@ -6,8 +6,8 @@ 0 0 - 890 - 725 + 935 + 726 @@ -955,13 +955,239 @@ A setting of 0.00 disables the filter. true - External Mag Settings + Mag Settings - - - + + + 6 + + + 6 + + + 6 + + + 6 + + + + + + 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; + + + Y axis + + + Qt::AlignCenter + + + + + + + + 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 Z axis + + + -50 + + + 50 + + + 0 + + + Qt::AlignCenter + + + %v + + + + + + + Difference on Y axis + + + -50 + + + 50 + + + 0 + + + Qt::AlignCenter + + + %v + + + + + + + + 0 + 0 + + + + <!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 both magnetometers are calibrated and without alarm.</p></body></html> + + + Qt::LinksAccessibleByKeyboard|Qt::LinksAccessibleByMouse|Qt::TextBrowserInteraction|Qt::TextSelectableByKeyboard|Qt::TextSelectableByMouse + + + + + + + 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 + + + + + + + + + + QLayout::SetDefaultConstraint + 0 @@ -976,7 +1202,10 @@ A setting of 0.00 disables the filter. Magnetometer Settings - + + + QLayout::SetMinimumSize + @@ -1003,8 +1232,14 @@ A setting of 0.00 disables the filter. - + + + + 0 + 0 + + 100 @@ -1016,10 +1251,10 @@ A setting of 0.00 disables the filter. - + - + 0 0 @@ -1035,13 +1270,55 @@ A setting of 0.00 disables the filter. - - + + + + Warning level in percent (default 5%) + + + 0.010000000000000 + + + 0.150000000000000 + + + 0.010000000000000 + + + 0.050000000000000 + + + + + + + Error level in percent (default 15%) + + + 0.150000000000000 + + + 0.300000000000000 + + + 0.010000000000000 + + + + + + + Warning / Error levels: + + + + + Qt::Horizontal - QSizePolicy::MinimumExpanding + QSizePolicy::Expanding @@ -1057,7 +1334,7 @@ A setting of 0.00 disables the filter. - + 0 0 @@ -1065,7 +1342,68 @@ A setting of 0.00 disables the filter. Magnetometer Status - + + + QLayout::SetMinimumSize + + + + + + 0 + 0 + + + + + 80 + 35 + + + + background-color: green; +color: rgb(255, 255, 255); +border-radius: 5; +margin:1px; +font:bold; + + + Onboard + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + + 80 + 35 + + + + background-color: green; +color: rgb(255, 255, 255); +border-radius: 5; +margin:1px; +font:bold; + + + AuxMag + + + Qt::AlignCenter + + + @@ -1092,13 +1430,48 @@ A setting of 0.00 disables the filter. + + + + + 0 + 0 + + + + + 0 + 0 + + + + Mag alarms: + + + + + + + Qt::Horizontal + + + QSizePolicy::Expanding + + + + 40 + 20 + + + + - + 0 0 @@ -1107,6 +1480,9 @@ A setting of 0.00 disables the filter. Rotate Magnetometer Orientation + + QLayout::SetMinimumSize + @@ -1253,208 +1629,6 @@ font:bold; - - - - - 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> - - - - - - @@ -1462,6 +1636,9 @@ p, li { white-space: pre-wrap; } Qt::Vertical + + QSizePolicy::Expanding + 20 From e1dadbce020c47d655ef5cadab679aa95926c8d6 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Wed, 24 Feb 2016 22:09:32 +0100 Subject: [PATCH 3/6] LP-240 Change names to Auxiliary - Disable auxMag alarm if not available. --- .../src/plugins/config/configrevowidget.cpp | 115 ++++++++++-------- ground/gcs/src/plugins/config/revosensors.ui | 46 +++---- 2 files changed, 90 insertions(+), 71 deletions(-) diff --git a/ground/gcs/src/plugins/config/configrevowidget.cpp b/ground/gcs/src/plugins/config/configrevowidget.cpp index 112341c79..c33bf96df 100644 --- a/ground/gcs/src/plugins/config/configrevowidget.cpp +++ b/ground/gcs/src/plugins/config/configrevowidget.cpp @@ -60,6 +60,8 @@ // #define DEBUG +#define MAG_ALARM_THRESHOLD 20 + // Uncomment this to enable 6 point calibration on the accels #define NOISE_SAMPLES 50 @@ -406,6 +408,7 @@ void ConfigRevoWidget::refreshWidgetsValues(UAVObject *object) m_ui->beBox->setText(beStr); getMagBeVector(); + onBoardAuxMagError(); } void ConfigRevoWidget::updateObjectsFromWidgets() @@ -517,59 +520,66 @@ void ConfigRevoWidget::onBoardAuxMagError() onboardMag[1] = magData.y; onboardMag[2] = magData.z; - auxMag[0] = auxMagData.x; - auxMag[1] = auxMagData.y; - auxMag[2] = auxMagData.z; - float normalizedMag[3]; float normalizedAuxMag[3]; + float xDiff = 0.0f; + float yDiff = 0.0f; + float zDiff = 0.0f; // Smooth Mag readings float alpha = 0.8f; float inv_alpha = (1.0f - alpha); - + // OnBoard mag onboardMagFiltered[0] = (onboardMagFiltered[0] * alpha) + (onboardMag[0] * inv_alpha); onboardMagFiltered[1] = (onboardMagFiltered[1] * alpha) + (onboardMag[1] * inv_alpha); onboardMagFiltered[2] = (onboardMagFiltered[2] * alpha) + (onboardMag[2] * inv_alpha); - auxMagFiltered[0] = (auxMagFiltered[0] * alpha) + (auxMag[0] * inv_alpha); - auxMagFiltered[1] = (auxMagFiltered[1] * alpha) + (auxMag[1] * inv_alpha); - auxMagFiltered[2] = (auxMagFiltered[2] * alpha) + (auxMag[2] * inv_alpha); + // Normalize vector + float magLenght = sqrt((onboardMagFiltered[0] * onboardMagFiltered[0]) + + (onboardMagFiltered[1] * onboardMagFiltered[1]) + + (onboardMagFiltered[2] * onboardMagFiltered[2])); - // Normalize vectors - float magLenght = sqrt((onboardMagFiltered[0] * onboardMagFiltered[0]) + - (onboardMagFiltered[1] * onboardMagFiltered[1]) + - (onboardMagFiltered[2] * onboardMagFiltered[2])); - float auxMagLenght = sqrt((auxMagFiltered[0] * auxMagFiltered[0]) + - (auxMagFiltered[1] * auxMagFiltered[1]) + - (auxMagFiltered[2] * auxMagFiltered[2])); + normalizedMag[0] = onboardMagFiltered[0] / magLenght; + normalizedMag[1] = onboardMagFiltered[1] / magLenght; + normalizedMag[2] = onboardMagFiltered[2] / magLenght; + if (auxMagData.Status > AuxMagSensor::STATUS_NONE) { + auxMag[0] = auxMagData.x; + auxMag[1] = auxMagData.y; + auxMag[2] = auxMagData.z; - normalizedMag[0] = onboardMagFiltered[0] / magLenght; - normalizedMag[1] = onboardMagFiltered[1] / magLenght; - normalizedMag[2] = onboardMagFiltered[2] / magLenght; + auxMagFiltered[0] = (auxMagFiltered[0] * alpha) + (auxMag[0] * inv_alpha); + auxMagFiltered[1] = (auxMagFiltered[1] * alpha) + (auxMag[1] * inv_alpha); + auxMagFiltered[2] = (auxMagFiltered[2] * alpha) + (auxMag[2] * inv_alpha); - normalizedAuxMag[0] = auxMagFiltered[0] / auxMagLenght; - normalizedAuxMag[1] = auxMagFiltered[1] / auxMagLenght; - normalizedAuxMag[2] = auxMagFiltered[2] / auxMagLenght; + // Normalize vector + float auxMagLenght = sqrt((auxMagFiltered[0] * auxMagFiltered[0]) + + (auxMagFiltered[1] * auxMagFiltered[1]) + + (auxMagFiltered[2] * auxMagFiltered[2])); - // 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; + normalizedAuxMag[0] = auxMagFiltered[0] / auxMagLenght; + normalizedAuxMag[1] = auxMagFiltered[1] / auxMagLenght; + normalizedAuxMag[2] = auxMagFiltered[2] / auxMagLenght; + + // Calc diff and scale + xDiff = (normalizedMag[0] - normalizedAuxMag[0]) * 25.0f; + yDiff = (normalizedMag[1] - normalizedAuxMag[1]) * 25.0f; + zDiff = (normalizedMag[2] - normalizedAuxMag[2]) * 25.0f; + } else { + auxMag[0] = auxMag[1] = auxMag[2] = 0.0f; + auxMagFiltered[0] = auxMagFiltered[1] = auxMagFiltered[2] = 0.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); - updateMagAlarm(getMagError(onboardMag), getMagError(auxMag)); + updateMagAlarm(getMagError(onboardMag), (auxMagData.Status == AuxMagSensor::STATUS_NONE) ? -1.0f : getMagError(auxMag)); } void ConfigRevoWidget::updateMagAlarm(float errorMag, float errorAuxMag) { - #define ALARM_THRESHOLD 20 - RevoSettings *revoSettings = RevoSettings::GetInstance(getObjectManager()); Q_ASSERT(revoSettings); RevoSettings::DataFields revoSettingsData = revoSettings->getData(); @@ -585,38 +595,43 @@ void ConfigRevoWidget::updateMagAlarm(float errorMag, float errorAuxMag) if (errorMag < revoSettingsData.MagnetometerMaxDeviation[RevoSettings::MAGNETOMETERMAXDEVIATION_ERROR]) { magErrorCount = 0; - if (magWarningCount > ALARM_THRESHOLD) { + if (magWarningCount > MAG_ALARM_THRESHOLD) { bgColorMag = "orange"; } else { magWarningCount++; } } - if (magErrorCount > ALARM_THRESHOLD) { + if (magErrorCount > MAG_ALARM_THRESHOLD) { bgColorMag = "red"; } else { magErrorCount++; } - // External Mag - if (errorAuxMag < revoSettingsData.MagnetometerMaxDeviation[RevoSettings::MAGNETOMETERMAXDEVIATION_WARNING]) { - auxMagWarningCount = 0; - auxMagErrorCount = 0; - } - - if (errorAuxMag < revoSettingsData.MagnetometerMaxDeviation[RevoSettings::MAGNETOMETERMAXDEVIATION_ERROR]) { - auxMagErrorCount = 0; - if (auxMagWarningCount > ALARM_THRESHOLD) { - bgColorAuxMag = "orange"; - } else { - auxMagWarningCount++; + // Auxiliary Mag + if (errorAuxMag > -1.0f) { + if (errorAuxMag < revoSettingsData.MagnetometerMaxDeviation[RevoSettings::MAGNETOMETERMAXDEVIATION_WARNING]) { + auxMagWarningCount = 0; + auxMagErrorCount = 0; } - } - if (auxMagErrorCount > ALARM_THRESHOLD) { - bgColorAuxMag = "red"; + if (errorAuxMag < revoSettingsData.MagnetometerMaxDeviation[RevoSettings::MAGNETOMETERMAXDEVIATION_ERROR]) { + auxMagErrorCount = 0; + if (auxMagWarningCount > MAG_ALARM_THRESHOLD) { + bgColorAuxMag = "orange"; + } else { + auxMagWarningCount++; + } + } + + if (auxMagErrorCount > MAG_ALARM_THRESHOLD) { + bgColorAuxMag = "red"; + } else { + auxMagErrorCount++; + } } else { - auxMagErrorCount++; + // Disable aux mag alarm + bgColorAuxMag = "grey"; } m_ui->onBoardMagStatus->setStyleSheet( @@ -659,12 +674,16 @@ void ConfigRevoWidget::updateMagStatus() if (magStateData.Source == MagState::SOURCE_INVALID) { m_ui->magStatusSource->setText(tr("Source invalid")); + m_ui->magStatusSource->setToolTip(tr("Currently no attitude estimation algorithm uses magnetometer or there is something wrong")); } else if (magStateData.Source == MagState::SOURCE_ONBOARD) { - m_ui->magStatusSource->setText(tr("OnBoard mag")); + m_ui->magStatusSource->setText(tr("OnBoard magnetometer")); + m_ui->magStatusSource->setToolTip(""); } else if (magStateData.Source == MagState::SOURCE_AUX) { - m_ui->magStatusSource->setText(tr("External mag")); + m_ui->magStatusSource->setText(tr("Auxiliary magnetometer")); + m_ui->magStatusSource->setToolTip(""); } else { m_ui->magStatusSource->setText(tr("Unknown")); + m_ui->magStatusSource->setToolTip(""); } } diff --git a/ground/gcs/src/plugins/config/revosensors.ui b/ground/gcs/src/plugins/config/revosensors.ui index 79d1cbc73..3ba04e856 100644 --- a/ground/gcs/src/plugins/config/revosensors.ui +++ b/ground/gcs/src/plugins/config/revosensors.ui @@ -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:'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> +</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> Qt::NoTextInteraction @@ -955,7 +955,7 @@ A setting of 0.00 disables the filter. true - Mag Settings + Magnetometer @@ -975,7 +975,7 @@ A setting of 0.00 disables the filter. - + 0 0 @@ -987,7 +987,7 @@ A setting of 0.00 disables the filter. - External Mag Orientation Help + Auxiliary Magnetometer Orientation Help @@ -1117,8 +1117,8 @@ font:bold; <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 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 and auxiliary 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 auxiliary 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 both magnetometers are calibrated and without alarm.</p></body></html> @@ -1228,7 +1228,7 @@ font:bold; - Mag use: + Mag usage: @@ -1266,7 +1266,7 @@ font:bold; - Select how to use magnetometers. + Select how to use available magnetometers. @@ -1404,19 +1404,6 @@ font:bold; - - - - - 0 - 0 - - - - external - - - @@ -1465,6 +1452,19 @@ font:bold; + + + + + 0 + 0 + + + + external + + + @@ -1477,7 +1477,7 @@ font:bold; - Rotate Magnetometer Orientation + Auxiliary Magnetometer Orientation From 1aaf9b9f0e578634ac30dc2717c38f418a9d74da Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Fri, 26 Feb 2016 00:49:23 +0100 Subject: [PATCH 4/6] LP-240 Remove delay (diff sliders working) on enter tab - Apply default metadata --- ground/gcs/src/plugins/config/configrevowidget.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/ground/gcs/src/plugins/config/configrevowidget.cpp b/ground/gcs/src/plugins/config/configrevowidget.cpp index c33bf96df..281421466 100644 --- a/ground/gcs/src/plugins/config/configrevowidget.cpp +++ b/ground/gcs/src/plugins/config/configrevowidget.cpp @@ -205,9 +205,10 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : addWidgetBinding("AuxMagSettings", "BoardRotation", m_ui->auxMagPitchRotation, AuxMagSettings::BOARDROTATION_PITCH); addWidgetBinding("AuxMagSettings", "BoardRotation", m_ui->auxMagYawRotation, AuxMagSettings::BOARDROTATION_YAW); + connect(m_ui->tabWidget, SIGNAL(currentChanged(int)), this, SLOT(onBoardAuxMagError())); connect(MagSensor::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(onBoardAuxMagError())); connect(MagState::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateMagStatus())); - connect(HomeLocation::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateMagBeVector())); + connect(HomeLocation::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getMagBeVector())); addWidget(m_ui->internalAuxErrorX); addWidget(m_ui->internalAuxErrorY); @@ -485,20 +486,16 @@ void ConfigRevoWidget::onBoardAuxMagError() Q_ASSERT(auxMagSensor); if (m_ui->tabWidget->currentIndex() != 2) { - // Restore metadata + // Apply default metadata if (displayMagError) { - magSensor->setMetadata(metamag.magSensorMetadata); - auxMagSensor->setMetadata(metamag.auxMagSensorMetadata); + magSensor->setMetadata(magSensor->getDefaultMetadata()); + auxMagSensor->setMetadata(auxMagSensor->getDefaultMetadata()); 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); @@ -511,6 +508,7 @@ void ConfigRevoWidget::onBoardAuxMagError() auxMagSensor->setMetadata(mdata); displayMagError = true; + return; } MagSensor::DataFields magData = magSensor->getData(); From 8211efe8044b05ddcb6c377155842d813ac840e7 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Fri, 26 Feb 2016 10:12:10 +0100 Subject: [PATCH 5/6] LP-240 Add Mag error display in percent - Lower rates for telemetry --- .../gcs/src/plugins/config/configrevowidget.cpp | 16 +++++++++++----- ground/gcs/src/plugins/config/revosensors.ui | 8 ++++---- 2 files changed, 15 insertions(+), 9 deletions(-) diff --git a/ground/gcs/src/plugins/config/configrevowidget.cpp b/ground/gcs/src/plugins/config/configrevowidget.cpp index 281421466..ea0002a88 100644 --- a/ground/gcs/src/plugins/config/configrevowidget.cpp +++ b/ground/gcs/src/plugins/config/configrevowidget.cpp @@ -60,10 +60,10 @@ // #define DEBUG -#define MAG_ALARM_THRESHOLD 20 +#define MAG_ALARM_THRESHOLD 5 // Uncomment this to enable 6 point calibration on the accels -#define NOISE_SAMPLES 50 +#define NOISE_SAMPLES 50 class Thread : public QThread { public: @@ -499,12 +499,12 @@ void ConfigRevoWidget::onBoardAuxMagError() // Apply new rates UAVObject::Metadata mdata = magSensor->getMetadata(); UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); - mdata.flightTelemetryUpdatePeriod = 100; + mdata.flightTelemetryUpdatePeriod = 300; magSensor->setMetadata(mdata); mdata = auxMagSensor->getMetadata(); UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); - mdata.flightTelemetryUpdatePeriod = 100; + mdata.flightTelemetryUpdatePeriod = 300; auxMagSensor->setMetadata(mdata); displayMagError = true; @@ -525,7 +525,7 @@ void ConfigRevoWidget::onBoardAuxMagError() float zDiff = 0.0f; // Smooth Mag readings - float alpha = 0.8f; + float alpha = 0.7f; float inv_alpha = (1.0f - alpha); // OnBoard mag onboardMagFiltered[0] = (onboardMagFiltered[0] * alpha) + (onboardMag[0] * inv_alpha); @@ -579,6 +579,7 @@ void ConfigRevoWidget::onBoardAuxMagError() void ConfigRevoWidget::updateMagAlarm(float errorMag, float errorAuxMag) { RevoSettings *revoSettings = RevoSettings::GetInstance(getObjectManager()); + Q_ASSERT(revoSettings); RevoSettings::DataFields revoSettingsData = revoSettings->getData(); @@ -627,11 +628,16 @@ void ConfigRevoWidget::updateMagAlarm(float errorMag, float errorAuxMag) } else { auxMagErrorCount++; } + errorAuxMag = ((errorAuxMag * 100.0f) <= 100.0f) ? errorAuxMag * 100.0f : 100.0f; + m_ui->auxMagStatus->setText("AuxMag\n" + QString::number(errorAuxMag, 'f', 1) + "%"); } else { // Disable aux mag alarm bgColorAuxMag = "grey"; + m_ui->auxMagStatus->setText("AuxMag\nnot found"); } + errorMag = ((errorMag * 100.0f) <= 100.0f) ? errorMag * 100.0f : 100.0f; + m_ui->onBoardMagStatus->setText("OnBoard\n" + QString::number(errorMag, 'f', 1) + "%"); m_ui->onBoardMagStatus->setStyleSheet( "QLabel { background-color: " + bgColorMag + ";" "color: rgb(255, 255, 255); border-radius: 5; margin:1px; font:bold; }"); diff --git a/ground/gcs/src/plugins/config/revosensors.ui b/ground/gcs/src/plugins/config/revosensors.ui index 3ba04e856..ea89c996d 100644 --- a/ground/gcs/src/plugins/config/revosensors.ui +++ b/ground/gcs/src/plugins/config/revosensors.ui @@ -1356,7 +1356,7 @@ font:bold; - 80 + 100 35 @@ -1385,8 +1385,8 @@ font:bold; - 80 - 35 + 100 + 40 @@ -1409,7 +1409,7 @@ font:bold; 110 - 0 + 20 From 91e5863939347d5eb636ba942adc10a76dd4f85f Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Fri, 26 Feb 2016 14:44:43 +0100 Subject: [PATCH 6/6] LP-240 Changes from review --- .../src/plugins/config/configrevowidget.cpp | 75 ++++++++++--------- .../gcs/src/plugins/config/configrevowidget.h | 38 +++------- 2 files changed, 52 insertions(+), 61 deletions(-) diff --git a/ground/gcs/src/plugins/config/configrevowidget.cpp b/ground/gcs/src/plugins/config/configrevowidget.cpp index ea0002a88..8216145fa 100644 --- a/ground/gcs/src/plugins/config/configrevowidget.cpp +++ b/ground/gcs/src/plugins/config/configrevowidget.cpp @@ -208,7 +208,7 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : connect(m_ui->tabWidget, SIGNAL(currentChanged(int)), this, SLOT(onBoardAuxMagError())); connect(MagSensor::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(onBoardAuxMagError())); connect(MagState::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateMagStatus())); - connect(HomeLocation::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getMagBeVector())); + connect(HomeLocation::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateMagBeVector())); addWidget(m_ui->internalAuxErrorX); addWidget(m_ui->internalAuxErrorY); @@ -408,7 +408,7 @@ void ConfigRevoWidget::refreshWidgetsValues(UAVObject *object) QString beStr = QString("%1:%2:%3").arg(QString::number(homeLocationData.Be[0]), QString::number(homeLocationData.Be[1]), QString::number(homeLocationData.Be[2])); m_ui->beBox->setText(beStr); - getMagBeVector(); + updateMagBeVector(); onBoardAuxMagError(); } @@ -479,10 +479,10 @@ void ConfigRevoWidget::enableAllCalibrations() void ConfigRevoWidget::onBoardAuxMagError() { - magSensor = MagSensor::GetInstance(getObjectManager()); - Q_ASSERT(magSensor); + MagSensor *magSensor = MagSensor::GetInstance(getObjectManager()); - auxMagSensor = AuxMagSensor::GetInstance(getObjectManager()); + Q_ASSERT(magSensor); + AuxMagSensor *auxMagSensor = AuxMagSensor::GetInstance(getObjectManager()); Q_ASSERT(auxMagSensor); if (m_ui->tabWidget->currentIndex() != 2) { @@ -511,12 +511,12 @@ void ConfigRevoWidget::onBoardAuxMagError() return; } - MagSensor::DataFields magData = magSensor->getData(); - AuxMagSensor::DataFields auxMagData = auxMagSensor->getData(); + float onboardMag[3]; + float auxMag[3]; - onboardMag[0] = magData.x; - onboardMag[1] = magData.y; - onboardMag[2] = magData.z; + onboardMag[0] = magSensor->x(); + onboardMag[1] = magSensor->y(); + onboardMag[2] = magSensor->z(); float normalizedMag[3]; float normalizedAuxMag[3]; @@ -527,37 +527,37 @@ void ConfigRevoWidget::onBoardAuxMagError() // Smooth Mag readings float alpha = 0.7f; float inv_alpha = (1.0f - alpha); - // OnBoard mag + // Onboard mag onboardMagFiltered[0] = (onboardMagFiltered[0] * alpha) + (onboardMag[0] * inv_alpha); onboardMagFiltered[1] = (onboardMagFiltered[1] * alpha) + (onboardMag[1] * inv_alpha); onboardMagFiltered[2] = (onboardMagFiltered[2] * alpha) + (onboardMag[2] * inv_alpha); // Normalize vector - float magLenght = sqrt((onboardMagFiltered[0] * onboardMagFiltered[0]) + + float magLength = sqrt((onboardMagFiltered[0] * onboardMagFiltered[0]) + (onboardMagFiltered[1] * onboardMagFiltered[1]) + (onboardMagFiltered[2] * onboardMagFiltered[2])); - normalizedMag[0] = onboardMagFiltered[0] / magLenght; - normalizedMag[1] = onboardMagFiltered[1] / magLenght; - normalizedMag[2] = onboardMagFiltered[2] / magLenght; + normalizedMag[0] = onboardMagFiltered[0] / magLength; + normalizedMag[1] = onboardMagFiltered[1] / magLength; + normalizedMag[2] = onboardMagFiltered[2] / magLength; - if (auxMagData.Status > AuxMagSensor::STATUS_NONE) { - auxMag[0] = auxMagData.x; - auxMag[1] = auxMagData.y; - auxMag[2] = auxMagData.z; + if (auxMagSensor->status() > (int)AuxMagSensor::STATUS_NONE) { + auxMag[0] = auxMagSensor->x(); + auxMag[1] = auxMagSensor->y(); + auxMag[2] = auxMagSensor->z(); auxMagFiltered[0] = (auxMagFiltered[0] * alpha) + (auxMag[0] * inv_alpha); auxMagFiltered[1] = (auxMagFiltered[1] * alpha) + (auxMag[1] * inv_alpha); auxMagFiltered[2] = (auxMagFiltered[2] * alpha) + (auxMag[2] * inv_alpha); // Normalize vector - float auxMagLenght = sqrt((auxMagFiltered[0] * auxMagFiltered[0]) + + float auxMagLength = sqrt((auxMagFiltered[0] * auxMagFiltered[0]) + (auxMagFiltered[1] * auxMagFiltered[1]) + (auxMagFiltered[2] * auxMagFiltered[2])); - normalizedAuxMag[0] = auxMagFiltered[0] / auxMagLenght; - normalizedAuxMag[1] = auxMagFiltered[1] / auxMagLenght; - normalizedAuxMag[2] = auxMagFiltered[2] / auxMagLenght; + normalizedAuxMag[0] = auxMagFiltered[0] / auxMagLength; + normalizedAuxMag[1] = auxMagFiltered[1] / auxMagLength; + normalizedAuxMag[2] = auxMagFiltered[2] / auxMagLength; // Calc diff and scale xDiff = (normalizedMag[0] - normalizedAuxMag[0]) * 25.0f; @@ -573,7 +573,7 @@ void ConfigRevoWidget::onBoardAuxMagError() 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); - updateMagAlarm(getMagError(onboardMag), (auxMagData.Status == AuxMagSensor::STATUS_NONE) ? -1.0f : getMagError(auxMag)); + updateMagAlarm(getMagError(onboardMag), (auxMagSensor->status() == (int)AuxMagSensor::STATUS_NONE) ? -1.0f : getMagError(auxMag)); } void ConfigRevoWidget::updateMagAlarm(float errorMag, float errorAuxMag) @@ -583,8 +583,12 @@ void ConfigRevoWidget::updateMagAlarm(float errorMag, float errorAuxMag) Q_ASSERT(revoSettings); RevoSettings::DataFields revoSettingsData = revoSettings->getData(); - QString bgColorMag = "green"; - QString bgColorAuxMag = "green"; + QStringList AlarmColor; + AlarmColor << "grey" << "green" << "orange" << "red"; + enum magAlarmState { MAG_NOT_FOUND = 0, MAG_OK = 1, MAG_WARNING = 2, MAG_ERROR = 3 }; + + QString bgColorMag = AlarmColor[MAG_OK]; + QString bgColorAuxMag = AlarmColor[MAG_OK]; // Onboard Mag if (errorMag < revoSettingsData.MagnetometerMaxDeviation[RevoSettings::MAGNETOMETERMAXDEVIATION_WARNING]) { @@ -595,14 +599,14 @@ void ConfigRevoWidget::updateMagAlarm(float errorMag, float errorAuxMag) if (errorMag < revoSettingsData.MagnetometerMaxDeviation[RevoSettings::MAGNETOMETERMAXDEVIATION_ERROR]) { magErrorCount = 0; if (magWarningCount > MAG_ALARM_THRESHOLD) { - bgColorMag = "orange"; + bgColorMag = AlarmColor[MAG_WARNING]; } else { magWarningCount++; } } if (magErrorCount > MAG_ALARM_THRESHOLD) { - bgColorMag = "red"; + bgColorMag = AlarmColor[MAG_ERROR]; } else { magErrorCount++; } @@ -617,14 +621,14 @@ void ConfigRevoWidget::updateMagAlarm(float errorMag, float errorAuxMag) if (errorAuxMag < revoSettingsData.MagnetometerMaxDeviation[RevoSettings::MAGNETOMETERMAXDEVIATION_ERROR]) { auxMagErrorCount = 0; if (auxMagWarningCount > MAG_ALARM_THRESHOLD) { - bgColorAuxMag = "orange"; + bgColorAuxMag = AlarmColor[MAG_WARNING]; } else { auxMagWarningCount++; } } if (auxMagErrorCount > MAG_ALARM_THRESHOLD) { - bgColorAuxMag = "red"; + bgColorAuxMag = AlarmColor[MAG_ERROR]; } else { auxMagErrorCount++; } @@ -632,12 +636,12 @@ void ConfigRevoWidget::updateMagAlarm(float errorMag, float errorAuxMag) m_ui->auxMagStatus->setText("AuxMag\n" + QString::number(errorAuxMag, 'f', 1) + "%"); } else { // Disable aux mag alarm - bgColorAuxMag = "grey"; + bgColorAuxMag = AlarmColor[MAG_NOT_FOUND]; m_ui->auxMagStatus->setText("AuxMag\nnot found"); } errorMag = ((errorMag * 100.0f) <= 100.0f) ? errorMag * 100.0f : 100.0f; - m_ui->onBoardMagStatus->setText("OnBoard\n" + QString::number(errorMag, 'f', 1) + "%"); + m_ui->onBoardMagStatus->setText("Onboard\n" + QString::number(errorMag, 'f', 1) + "%"); m_ui->onBoardMagStatus->setStyleSheet( "QLabel { background-color: " + bgColorMag + ";" "color: rgb(255, 255, 255); border-radius: 5; margin:1px; font:bold; }"); @@ -657,7 +661,7 @@ float ConfigRevoWidget::getMagError(float mag[3]) return error; } -void ConfigRevoWidget::getMagBeVector() +void ConfigRevoWidget::updateMagBeVector() { HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager()); @@ -671,7 +675,8 @@ void ConfigRevoWidget::getMagBeVector() void ConfigRevoWidget::updateMagStatus() { - magState = MagState::GetInstance(getObjectManager()); + MagState *magState = MagState::GetInstance(getObjectManager()); + Q_ASSERT(magState); MagState::DataFields magStateData = magState->getData(); @@ -680,7 +685,7 @@ void ConfigRevoWidget::updateMagStatus() m_ui->magStatusSource->setText(tr("Source invalid")); m_ui->magStatusSource->setToolTip(tr("Currently no attitude estimation algorithm uses magnetometer or there is something wrong")); } else if (magStateData.Source == MagState::SOURCE_ONBOARD) { - m_ui->magStatusSource->setText(tr("OnBoard magnetometer")); + m_ui->magStatusSource->setText(tr("Onboard magnetometer")); m_ui->magStatusSource->setToolTip(""); } else if (magStateData.Source == MagState::SOURCE_AUX) { m_ui->magStatusSource->setText(tr("Auxiliary magnetometer")); diff --git a/ground/gcs/src/plugins/config/configrevowidget.h b/ground/gcs/src/plugins/config/configrevowidget.h index dd0deb22b..c4c810b06 100644 --- a/ground/gcs/src/plugins/config/configrevowidget.h +++ b/ground/gcs/src/plugins/config/configrevowidget.h @@ -58,31 +58,6 @@ 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 onboardMagFiltered[3]; - float auxMagFiltered[3]; - float magBe[3]; - - int magWarningCount; - int magErrorCount; - int auxMagWarningCount; - int auxMagErrorCount; - private: OpenPilot::SixPointCalibrationModel *m_accelCalibrationModel; OpenPilot::SixPointCalibrationModel *m_magCalibrationModel; @@ -97,6 +72,17 @@ private: qint16 auxMagStoredBoardRotation[3]; bool isBoardRotationStored; + bool displayMagError; + + float onboardMagFiltered[3]; + float auxMagFiltered[3]; + float magBe[3]; + + int magWarningCount; + int magErrorCount; + int auxMagWarningCount; + int auxMagErrorCount; + private slots: void storeAndClearBoardRotation(); void recallBoardRotation(); @@ -119,7 +105,7 @@ private slots: void onBoardAuxMagError(); void updateMagStatus(); - void getMagBeVector(); + void updateMagBeVector(); void updateMagAlarm(float errorMag, float errorAuxMag); float getMagError(float mag[3]);