From e1dadbce020c47d655ef5cadab679aa95926c8d6 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Wed, 24 Feb 2016 22:09:32 +0100 Subject: [PATCH] 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