rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

LP-240 Change names to Auxiliary - Disable auxMag alarm if not available.

Browse Source
This commit is contained in:
Laurent Lalanne 2016-02-24 22:09:32 +01:00
parent 9c4b7498cd
commit e1dadbce02
2 changed files with 90 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
ground/gcs/src/plugins/config/configrevowidget.cpp
View File

@ -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);
// Normalize vector
float magLenght = 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;
if (auxMagData.Status > AuxMagSensor::STATUS_NONE) {
auxMag[0] = auxMagData.x;
auxMag[1] = auxMagData.y;
auxMag[2] = auxMagData.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 vectors
float magLenght = sqrt((onboardMagFiltered[0] * onboardMagFiltered[0]) +
(onboardMagFiltered[1] * onboardMagFiltered[1]) +
(onboardMagFiltered[2] * onboardMagFiltered[2]));
// Normalize vector
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;
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;
float yDiff = (normalizedMag[1] - normalizedAuxMag[1]) * 25.0f;
float zDiff = (normalizedMag[2] - normalizedAuxMag[2]) * 25.0f;
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,20 +595,21 @@ 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
// Auxiliary Mag
if (errorAuxMag > -1.0f) {
if (errorAuxMag < revoSettingsData.MagnetometerMaxDeviation[RevoSettings::MAGNETOMETERMAXDEVIATION_WARNING]) {
auxMagWarningCount = 0;
auxMagErrorCount = 0;
@ -606,18 +617,22 @@ void ConfigRevoWidget::updateMagAlarm(float errorMag, float errorAuxMag)
if (errorAuxMag < revoSettingsData.MagnetometerMaxDeviation[RevoSettings::MAGNETOMETERMAXDEVIATION_ERROR]) {
auxMagErrorCount = 0;
if (auxMagWarningCount > ALARM_THRESHOLD) {
if (auxMagWarningCount > MAG_ALARM_THRESHOLD) {
bgColorAuxMag = "orange";
} else {
auxMagWarningCount++;
}
}
if (auxMagErrorCount > ALARM_THRESHOLD) {
if (auxMagErrorCount > MAG_ALARM_THRESHOLD) {
bgColorAuxMag = "red";
} else {
auxMagErrorCount++;
}
} else {
// Disable aux mag alarm
bgColorAuxMag = "grey";
}
m_ui->onBoardMagStatus->setStyleSheet(
"QLabel { background-color: " + bgColorMag + ";"
@ -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("");
}
}

46
ground/gcs/src/plugins/config/revosensors.ui
View File

@ -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:'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>
&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>
</property>
<property name="textInteractionFlags">
<set>Qt::NoTextInteraction</set>
@ -955,7 +955,7 @@ A setting of 0.00 disables the filter.</string>
<bool>true</bool>
</property>
<attribute name="title">
<string>Mag Settings</string>
<string>Magnetometer</string>
</attribute>
<layout class="QVBoxLayout" name="verticalLayout_5">
<item>
@ -975,7 +975,7 @@ A setting of 0.00 disables the filter.</string>
<item row="0" column="1">
<widget class="QGroupBox" name="gridGroupBox">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<sizepolicy hsizetype="Expanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
@ -987,7 +987,7 @@ A setting of 0.00 disables the filter.</string>
</size>
</property>
<property name="title">
<string>External Mag Orientation Help</string>
<string>Auxiliary Magnetometer Orientation Help</string>
</property>
<layout class="QGridLayout" name="gridLayout_10">
<property name="leftMargin">
@ -1117,8 +1117,8 @@ font:bold;</string>
&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 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 and auxiliary 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 auxiliary 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 both magnetometers are calibrated and without alarm.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="textInteractionFlags">
@ -1228,7 +1228,7 @@ font:bold;</string>
</size>
</property>
<property name="text">
<string>Mag use:</string>
<string>Mag usage:</string>
</property>
</widget>
</item>
@ -1266,7 +1266,7 @@ font:bold;</string>
</size>
</property>
<property name="toolTip">
<string>Select how to use magnetometers.</string>
<string>Select how to use available magnetometers.</string>
</property>
</widget>
</item>
@ -1404,19 +1404,6 @@ font:bold;</string>
</property>
</widget>
</item>
<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">
@ -1465,6 +1452,19 @@ font:bold;</string>
</property>
</spacer>
</item>
<item row="1" column="1" colspan="2">
<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>
</layout>
</widget>
</item>
@ -1477,7 +1477,7 @@ font:bold;</string>
</sizepolicy>
</property>
<property name="title">
<string>Rotate Magnetometer Orientation</string>
<string>Auxiliary Magnetometer Orientation</string>
</property>
<layout class="QGridLayout" name="gridLayout_12" columnstretch="0,0,0">
<property name="sizeConstraint">