1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-21 13:28:58 +01:00

LP-240 Add GUI tab for external Magnetometer

This commit is contained in:
Laurent Lalanne 2016-02-22 00:02:37 +01:00
parent 96ed1e40ba
commit 45c42537b1
3 changed files with 661 additions and 3 deletions

View File

@ -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"),

View File

@ -37,6 +37,11 @@
#include "calibration/levelcalibrationmodel.h"
#include "calibration/gyrobiascalibrationmodel.h"
#include <auxmagsettings.h>
#include <magsensor.h>
#include <auxmagsensor.h>
#include <magstate.h>
#include <QWidget>
#include <QtSvg/QSvgRenderer>
#include <QtSvg/QGraphicsSvgItem>
@ -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();

View File

@ -6,7 +6,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>1014</width>
<width>890</width>
<height>725</height>
</rect>
</property>
@ -438,8 +438,8 @@
<string>&lt;!DOCTYPE HTML PUBLIC &quot;-//W3C//DTD HTML 4.0//EN&quot; &quot;http://www.w3.org/TR/REC-html40/strict.dtd&quot;&gt;
&lt;html&gt;&lt;head&gt;&lt;meta name=&quot;qrichtext&quot; content=&quot;1&quot; /&gt;&lt;style type=&quot;text/css&quot;&gt;
p, li { white-space: pre-wrap; }
&lt;/style&gt;&lt;/head&gt;&lt;body style=&quot; font-family:'MS Shell Dlg 2'; font-size:10pt; font-weight:400; font-style:normal;&quot;&gt;
&lt;p style=&quot;-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;&quot;&gt;&lt;br /&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
&lt;/style&gt;&lt;/head&gt;&lt;body style=&quot; font-family:'Cantarell'; font-size:10pt; font-weight:400; font-style:normal;&quot;&gt;
&lt;p style=&quot;-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;&quot;&gt;&lt;br /&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="textInteractionFlags">
<set>Qt::NoTextInteraction</set>
@ -950,6 +950,528 @@ A setting of 0.00 disables the filter.</string>
</item>
</layout>
</widget>
<widget class="QWidget" name="externalMagTab">
<property name="enabled">
<bool>true</bool>
</property>
<attribute name="title">
<string>External Mag Settings</string>
</attribute>
<layout class="QVBoxLayout" name="verticalLayout_5">
<item>
<layout class="QHBoxLayout" name="horizontalLayout_6">
<item>
<layout class="QVBoxLayout" name="verticalLayout_7">
<property name="leftMargin">
<number>0</number>
</property>
<item>
<widget class="QGroupBox" name="groupBox_8">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="title">
<string>Magnetometer Settings</string>
</property>
<layout class="QGridLayout" name="gridLayout_13" columnstretch="0,0,0">
<item row="1" column="0">
<widget class="QLabel" name="label_31">
<property name="minimumSize">
<size>
<width>80</width>
<height>0</height>
</size>
</property>
<property name="text">
<string>Mag type:</string>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QLabel" name="label_32">
<property name="minimumSize">
<size>
<width>80</width>
<height>0</height>
</size>
</property>
<property name="text">
<string>Mag use:</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QComboBox" name="auxMagType">
<property name="minimumSize">
<size>
<width>100</width>
<height>0</height>
</size>
</property>
<property name="toolTip">
<string>Select the magnetometer type.</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QComboBox" name="auxMagUsage">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>100</width>
<height>0</height>
</size>
</property>
<property name="toolTip">
<string>Select how to use magnetometers.</string>
</property>
</widget>
</item>
<item row="0" column="2">
<spacer name="horizontalSpacer_5">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::MinimumExpanding</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_9">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="title">
<string>Magnetometer Status</string>
</property>
<layout class="QGridLayout" name="gridLayout_14" columnstretch="0,0">
<item row="1" column="1">
<widget class="QLabel" name="magStatusSource">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string>external</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label">
<property name="minimumSize">
<size>
<width>110</width>
<height>0</height>
</size>
</property>
<property name="text">
<string>Mag source:</string>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_7">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="title">
<string>Rotate Magnetometer Orientation</string>
</property>
<layout class="QGridLayout" name="gridLayout_12" columnstretch="0,0,0">
<item row="2" column="0">
<widget class="QLabel" name="label_29">
<property name="minimumSize">
<size>
<width>70</width>
<height>30</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>0</width>
<height>30</height>
</size>
</property>
<property name="styleSheet">
<string notr="true">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;</string>
</property>
<property name="text">
<string>Roll</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QDoubleSpinBox" name="auxMagPitchRotation">
<property name="decimals">
<number>0</number>
</property>
<property name="minimum">
<double>-180.000000000000000</double>
</property>
<property name="maximum">
<double>180.000000000000000</double>
</property>
</widget>
</item>
<item row="4" column="2">
<widget class="QDoubleSpinBox" name="auxMagYawRotation">
<property name="decimals">
<number>0</number>
</property>
<property name="minimum">
<double>-180.000000000000000</double>
</property>
<property name="maximum">
<double>180.000000000000000</double>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_28">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>70</width>
<height>30</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>0</width>
<height>30</height>
</size>
</property>
<property name="styleSheet">
<string notr="true">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;</string>
</property>
<property name="text">
<string>Pitch</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="auxMagRollRotation">
<property name="toolTip">
<string/>
</property>
<property name="decimals">
<number>0</number>
</property>
<property name="minimum">
<double>-180.000000000000000</double>
</property>
<property name="maximum">
<double>180.000000000000000</double>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QLabel" name="label_30">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>70</width>
<height>30</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>0</width>
<height>30</height>
</size>
</property>
<property name="styleSheet">
<string notr="true">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;</string>
</property>
<property name="text">
<string>Yaw</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
</layout>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QGroupBox" name="gridGroupBox">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>500</width>
<height>0</height>
</size>
</property>
<property name="title">
<string>External Mag Orientation Help</string>
</property>
<layout class="QGridLayout" name="gridLayout_10">
<property name="leftMargin">
<number>6</number>
</property>
<property name="topMargin">
<number>6</number>
</property>
<property name="rightMargin">
<number>6</number>
</property>
<property name="bottomMargin">
<number>6</number>
</property>
<item row="3" column="0">
<widget class="QLabel" name="label_22">
<property name="minimumSize">
<size>
<width>80</width>
<height>30</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>30</height>
</size>
</property>
<property name="styleSheet">
<string notr="true">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;</string>
</property>
<property name="text">
<string>Z axis</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QProgressBar" name="internalAuxErrorY">
<property name="toolTip">
<string>Difference on Y axis</string>
</property>
<property name="minimum">
<number>-50</number>
</property>
<property name="maximum">
<number>50</number>
</property>
<property name="value">
<number>0</number>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
<property name="format">
<string>%v</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QProgressBar" name="internalAuxErrorZ">
<property name="toolTip">
<string>Difference on Z axis</string>
</property>
<property name="minimum">
<number>-50</number>
</property>
<property name="maximum">
<number>50</number>
</property>
<property name="value">
<number>0</number>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
<property name="format">
<string>%v</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_23">
<property name="minimumSize">
<size>
<width>80</width>
<height>30</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>30</height>
</size>
</property>
<property name="styleSheet">
<string notr="true">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;</string>
</property>
<property name="text">
<string>Y axis</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QProgressBar" name="internalAuxErrorX">
<property name="toolTip">
<string>Difference on X axis</string>
</property>
<property name="minimum">
<number>-50</number>
</property>
<property name="maximum">
<number>50</number>
</property>
<property name="value">
<number>0</number>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
<property name="textVisible">
<bool>true</bool>
</property>
<property name="format">
<string>%v</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_24">
<property name="minimumSize">
<size>
<width>80</width>
<height>30</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>30</height>
</size>
</property>
<property name="styleSheet">
<string notr="true">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;</string>
</property>
<property name="text">
<string>X axis</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="0" column="0" colspan="2">
<widget class="QTextEdit" name="textEdit">
<property name="html">
<string>&lt;!DOCTYPE HTML PUBLIC &quot;-//W3C//DTD HTML 4.0//EN&quot; &quot;http://www.w3.org/TR/REC-html40/strict.dtd&quot;&gt;
&lt;html&gt;&lt;head&gt;&lt;meta name=&quot;qrichtext&quot; content=&quot;1&quot; /&gt;&lt;style type=&quot;text/css&quot;&gt;
p, li { white-space: pre-wrap; }
&lt;/style&gt;&lt;/head&gt;&lt;body style=&quot; font-family:'Cantarell'; font-size:11pt; font-weight:400; font-style:normal;&quot;&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;The bargraphs show the difference between the onboard magnetometer and external magnetometer measurements. &lt;/p&gt;
&lt;p align=&quot;justify&quot; style=&quot; margin-top:12px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;When the external magnetometer rotation is set correctlly, all bargraphs should show all zero (bargraph centered) &lt;a name=&quot;result_box&quot;&gt;&lt;/a&gt;whatever the vehicle's orientation.&lt;/p&gt;
&lt;p align=&quot;justify&quot; style=&quot; margin-top:12px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;This assumes the onboard magnetometer is also calibrated (Mag use=Both).&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
</widget>
</item>
</layout>
</widget>
</item>
</layout>
</item>
<item>
<spacer name="verticalSpacer_5">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
<widget class="QWidget" name="helpTab">
<attribute name="title">
<string>Help</string>