diff --git a/ground/gcs/src/plugins/config/configrevowidget.cpp b/ground/gcs/src/plugins/config/configrevowidget.cpp index 0c2fb05d5..8216145fa 100644 --- a/ground/gcs/src/plugins/config/configrevowidget.cpp +++ b/ground/gcs/src/plugins/config/configrevowidget.cpp @@ -60,8 +60,10 @@ // #define DEBUG +#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: @@ -193,6 +195,27 @@ 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("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(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())); + + 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())); @@ -384,6 +407,9 @@ 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); + + updateMagBeVector(); + onBoardAuxMagError(); } void ConfigRevoWidget::updateObjectsFromWidgets() @@ -451,6 +477,225 @@ void ConfigRevoWidget::enableAllCalibrations() m_ui->thermalBiasStart->setEnabled(true); } +void ConfigRevoWidget::onBoardAuxMagError() +{ + MagSensor *magSensor = MagSensor::GetInstance(getObjectManager()); + + Q_ASSERT(magSensor); + AuxMagSensor *auxMagSensor = AuxMagSensor::GetInstance(getObjectManager()); + Q_ASSERT(auxMagSensor); + + if (m_ui->tabWidget->currentIndex() != 2) { + // Apply default metadata + if (displayMagError) { + magSensor->setMetadata(magSensor->getDefaultMetadata()); + auxMagSensor->setMetadata(auxMagSensor->getDefaultMetadata()); + displayMagError = false; + } + return; + } + + if (!displayMagError) { + // Apply new rates + UAVObject::Metadata mdata = magSensor->getMetadata(); + UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); + mdata.flightTelemetryUpdatePeriod = 300; + magSensor->setMetadata(mdata); + + mdata = auxMagSensor->getMetadata(); + UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); + mdata.flightTelemetryUpdatePeriod = 300; + auxMagSensor->setMetadata(mdata); + + displayMagError = true; + return; + } + + float onboardMag[3]; + float auxMag[3]; + + onboardMag[0] = magSensor->x(); + onboardMag[1] = magSensor->y(); + onboardMag[2] = magSensor->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.7f; + 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); + + // Normalize vector + float magLength = sqrt((onboardMagFiltered[0] * onboardMagFiltered[0]) + + (onboardMagFiltered[1] * onboardMagFiltered[1]) + + (onboardMagFiltered[2] * onboardMagFiltered[2])); + + normalizedMag[0] = onboardMagFiltered[0] / magLength; + normalizedMag[1] = onboardMagFiltered[1] / magLength; + normalizedMag[2] = onboardMagFiltered[2] / magLength; + + 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 auxMagLength = sqrt((auxMagFiltered[0] * auxMagFiltered[0]) + + (auxMagFiltered[1] * auxMagFiltered[1]) + + (auxMagFiltered[2] * auxMagFiltered[2])); + + 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; + 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), (auxMagSensor->status() == (int)AuxMagSensor::STATUS_NONE) ? -1.0f : getMagError(auxMag)); +} + +void ConfigRevoWidget::updateMagAlarm(float errorMag, float errorAuxMag) +{ + RevoSettings *revoSettings = RevoSettings::GetInstance(getObjectManager()); + + Q_ASSERT(revoSettings); + RevoSettings::DataFields revoSettingsData = revoSettings->getData(); + + 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]) { + magWarningCount = 0; + magErrorCount = 0; + } + + if (errorMag < revoSettingsData.MagnetometerMaxDeviation[RevoSettings::MAGNETOMETERMAXDEVIATION_ERROR]) { + magErrorCount = 0; + if (magWarningCount > MAG_ALARM_THRESHOLD) { + bgColorMag = AlarmColor[MAG_WARNING]; + } else { + magWarningCount++; + } + } + + if (magErrorCount > MAG_ALARM_THRESHOLD) { + bgColorMag = AlarmColor[MAG_ERROR]; + } else { + magErrorCount++; + } + + // Auxiliary Mag + if (errorAuxMag > -1.0f) { + if (errorAuxMag < revoSettingsData.MagnetometerMaxDeviation[RevoSettings::MAGNETOMETERMAXDEVIATION_WARNING]) { + auxMagWarningCount = 0; + auxMagErrorCount = 0; + } + + if (errorAuxMag < revoSettingsData.MagnetometerMaxDeviation[RevoSettings::MAGNETOMETERMAXDEVIATION_ERROR]) { + auxMagErrorCount = 0; + if (auxMagWarningCount > MAG_ALARM_THRESHOLD) { + bgColorAuxMag = AlarmColor[MAG_WARNING]; + } else { + auxMagWarningCount++; + } + } + + if (auxMagErrorCount > MAG_ALARM_THRESHOLD) { + bgColorAuxMag = AlarmColor[MAG_ERROR]; + } 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 = 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->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::updateMagBeVector() +{ + 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() +{ + MagState *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")); + 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->setToolTip(""); + } else if (magStateData.Source == MagState::SOURCE_AUX) { + m_ui->magStatusSource->setText(tr("Auxiliary magnetometer")); + m_ui->magStatusSource->setToolTip(""); + } else { + m_ui->magStatusSource->setText(tr("Unknown")); + m_ui->magStatusSource->setToolTip(""); + } +} + 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..c4c810b06 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 @@ -67,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(); @@ -87,6 +103,13 @@ private slots: void disableAllCalibrations(); void enableAllCalibrations(); + void onBoardAuxMagError(); + void updateMagStatus(); + void updateMagBeVector(); + 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 1e7fbf587..ea89c996d 100644 --- a/ground/gcs/src/plugins/config/revosensors.ui +++ b/ground/gcs/src/plugins/config/revosensors.ui @@ -6,8 +6,8 @@ 0 0 - 1014 - 725 + 935 + 726 @@ -950,6 +950,705 @@ A setting of 0.00 disables the filter. + + + true + + + Magnetometer + + + + + + 6 + + + 6 + + + 6 + + + 6 + + + + + + 0 + 0 + + + + + 500 + 0 + + + + Auxiliary Magnetometer 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 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> + + + 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 + + + + + + 0 + 0 + + + + Magnetometer Settings + + + + QLayout::SetMinimumSize + + + + + + 80 + 0 + + + + Mag type: + + + + + + + + 80 + 0 + + + + Mag usage: + + + + + + + + 0 + 0 + + + + + 100 + 0 + + + + Select the magnetometer type. + + + + + + + + 0 + 0 + + + + + 100 + 0 + + + + Select how to use available magnetometers. + + + + + + + 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::Expanding + + + + 40 + 20 + + + + + + + + + + + + 0 + 0 + + + + Magnetometer Status + + + + QLayout::SetMinimumSize + + + + + + 0 + 0 + + + + + 100 + 35 + + + + background-color: green; +color: rgb(255, 255, 255); +border-radius: 5; +margin:1px; +font:bold; + + + Onboard + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + + 100 + 40 + + + + background-color: green; +color: rgb(255, 255, 255); +border-radius: 5; +margin:1px; +font:bold; + + + AuxMag + + + Qt::AlignCenter + + + + + + + + 110 + 20 + + + + Mag source: + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + Mag alarms: + + + + + + + Qt::Horizontal + + + QSizePolicy::Expanding + + + + 40 + 20 + + + + + + + + + 0 + 0 + + + + external + + + + + + + + + + + 0 + 0 + + + + Auxiliary Magnetometer Orientation + + + + QLayout::SetMinimumSize + + + + + + 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 + + + + + + + + + + + + + + Qt::Vertical + + + QSizePolicy::Expanding + + + + 20 + 40 + + + + + + Help