mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
LP-240 Change names to Auxiliary - Disable auxMag alarm if not available.
This commit is contained in:
Laurent Lalanne
parent
9c4b7498cd
commit
e1dadbce02
@ -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("");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -438,8 +438,8 @@
|
||||
<string><!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></string>
|
||||
</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></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>
|
||||
<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></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">
|
||||
|
||||
Loading…
Reference in New Issue
Block a user