rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00
Code Issues Releases Activity

Merge remote-tracking branch 'origin/corvuscorax/OP-1161_magnetometer-alarm' into next

Browse Source
This commit is contained in:
Alessio Morale 2014-05-09 11:51:02 +02:00
commit 00012d90a1
6 changed files with 161 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
flight/modules/StateEstimation/filterekf.c
View File

@ -36,6 +36,7 @@
#include <ekfconfiguration.h>
#include <ekfstatevariance.h>
#include <attitudestate.h>
#include <systemalarms.h>
#include <homelocation.h>
#include <insgps.h>
@ -348,7 +349,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
INSCovariancePrediction(dT);
if (IS_SET(this->work.updated, SENSORUPDATES_mag)) {
sensors |= MAG_SENSORS;
SystemAlarmsAlarmData alarms;
SystemAlarmsAlarmGet(&alarms);
if (alarms.Magnetometer == SYSTEMALARMS_ALARM_OK) {
sensors |= MAG_SENSORS;
}
}
if (IS_SET(this->work.updated, SENSORUPDATES_baro)) {

71
flight/modules/StateEstimation/filtermag.c
View File

@ -33,6 +33,8 @@
#include "inc/stateestimation.h"
#include <attitudestate.h>
#include <revocalibration.h>
#include <revosettings.h>
#include <systemalarms.h>
#include <homelocation.h>
#include <CoordinateConversions.h>
@ -43,9 +45,13 @@
// Private types
struct data {
HomeLocationData homeLocation;
RevoCalibrationData revoCalibration;
float magBias[3];
RevoSettingsData revoSettings;
uint8_t warningcount;
uint8_t errorcount;
float homeLocationBe[3];
float magBe2;
float magBias[3];
};
// Private variables
@ -54,6 +60,7 @@ struct data {
static int32_t init(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
static void checkMagValidity(struct data *this, float mag[3]);
static void magOffsetEstimation(struct data *this, float mag[3]);
@ -70,9 +77,13 @@ static int32_t init(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->magBias[0] = this->magBias[1] = this->magBias[2] = 0.0f;
HomeLocationGet(&this->homeLocation);
this->magBias[0] = this->magBias[1] = this->magBias[2] = 0.0f;
this->warningcount = this->errorcount = 0;
HomeLocationBeGet(this->homeLocationBe);
// magBe2 holds the squared magnetic vector length (extpected)
this->magBe2 = this->homeLocationBe[0] * this->homeLocationBe[0] + this->homeLocationBe[1] * this->homeLocationBe[1] + this->homeLocationBe[2] * this->homeLocationBe[2];
RevoCalibrationGet(&this->revoCalibration);
RevoSettingsGet(&this->revoSettings);
return 0;
}
@ -81,6 +92,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
struct data *this = (struct data *)self->localdata;
if (IS_SET(state->updated, SENSORUPDATES_mag)) {
checkMagValidity(this, state->mag);
if (this->revoCalibration.MagBiasNullingRate > 0) {
magOffsetEstimation(this, state->mag);
}
@ -89,6 +101,53 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
return 0;
}
/**
* check validity of magnetometers
*/
static void checkMagValidity(struct data *this, float mag[3])
{
#define ALARM_THRESHOLD 5
// mag2 holds the actual magnetic vector length (squared)
float mag2 = mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2];
// warning and error thresholds
// avoud sqrt() : minlimit<actual<maxlimit === minlimit²<actual²<maxlimit²
//
// actual = |mag|
// minlimit = |Be| - maxDeviation*|Be| = |Be| * (1 - maxDeviation)
// maxlimit = |Be| + maxDeviation*|Be| = |Be| * (1 + maxDeviation)
// minlimit² = |Be|² * ( 1 - 2*maxDeviation + maxDeviation²)
// maxlimit² = |Be|² * ( 1 + 2*maxDeviation + maxDeviation²)
//
float minWarning2 = this->magBe2 * (1.0f - 2.0f * this->revoSettings.MagnetometerMaxDeviation.Warning + this->revoSettings.MagnetometerMaxDeviation.Warning * this->revoSettings.MagnetometerMaxDeviation.Warning);
float maxWarning2 = this->magBe2 * (1.0f + 2.0f * this->revoSettings.MagnetometerMaxDeviation.Warning + this->revoSettings.MagnetometerMaxDeviation.Warning * this->revoSettings.MagnetometerMaxDeviation.Warning);
float minError2 = this->magBe2 * (1.0f - 2.0f * this->revoSettings.MagnetometerMaxDeviation.Error + this->revoSettings.MagnetometerMaxDeviation.Error * this->revoSettings.MagnetometerMaxDeviation.Error);
float maxError2 = this->magBe2 * (1.0f + 2.0f * this->revoSettings.MagnetometerMaxDeviation.Error + this->revoSettings.MagnetometerMaxDeviation.Error * this->revoSettings.MagnetometerMaxDeviation.Error);
// set errors
if (minWarning2 < mag2 && mag2 < maxWarning2) {
this->warningcount = 0;
this->errorcount = 0;
AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER);
} else if (minError2 < mag2 && mag2 < maxError2) {
this->errorcount = 0;
if (this->warningcount > ALARM_THRESHOLD) {
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING);
} else {
this->warningcount++;
}
} else {
if (this->errorcount > ALARM_THRESHOLD) {
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL);
} else {
this->errorcount++;
}
}
}
/**
* Perform an update of the @ref MagBias based on
* Magmeter Offset Cancellation: Theory and Implementation,
@ -137,8 +196,8 @@ static void magOffsetEstimation(struct data *this, float mag[3])
}
#else // if 0
const float Rxy = sqrtf(this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1]);
const float Rz = this->homeLocation.Be[2];
const float Rxy = sqrtf(this->homeLocationBe[0] * this->homeLocationBe[0] + this->homeLocationBe[1] * this->homeLocationBe[1]);
const float Rz = this->homeLocationBe[2];
const float rate = this->revoCalibration.MagBiasNullingRate;
float Rot[3][3];

13
flight/modules/StateEstimation/stateestimation.c
View File

@ -161,17 +161,14 @@ static float gyroDelta[3];
// preconfigured filter chains selectable via revoSettings.FusionAlgorithm
static const filterPipeline *cfQueue = &(filterPipeline) {
.filter = &magFilter,
.filter = &airFilter,
.next = &(filterPipeline) {
.filter = &airFilter,
.filter = &baroiFilter,
.next = &(filterPipeline) {
.filter = &baroiFilter,
.filter = &altitudeFilter,
.next = &(filterPipeline) {
.filter = &altitudeFilter,
.next = &(filterPipeline) {
.filter = &cfFilter,
.next = NULL,
}
.filter = &cfFilter,
.next = NULL,
}
}
}

92
ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg
View File

@ -27,10 +27,10 @@
borderopacity="1.0"
inkscape:pageopacity="0.0"
inkscape:pageshadow="2"
inkscape:zoom="7.4629556"
inkscape:cx="32.393349"
inkscape:cy="37.450437"
inkscape:current-layer="foreground"
inkscape:zoom="29.851822"
inkscape:cx="82.604436"
inkscape:cy="54.252252"
inkscape:current-layer="background"
id="namedview3608"
showgrid="true"
inkscape:window-width="1366"
@ -1002,12 +1002,13 @@
inkscape:label="#rect4550-8-1-4-21-5-13" />
<rect
style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
id="rect4550-8-1-4-21-5-8"
id="Magnetometer"
width="18.966711"
height="10.202584"
x="570.72327"
y="361.65591"
ry="1" />
ry="1"
inkscape:label="#rect4550-8-1-4-21-5-8" />
<rect
style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
id="Battery"
@ -1123,6 +1124,21 @@
ry="1"
inkscape:label="#rect4550-8-1-4-21-5-33" />
</g>
<g
inkscape:groupmode="layer"
id="layer42"
inkscape:label="Magnetometer-OK"
style="display:none">
<rect
style="fill:#008000;fill-opacity:1;stroke:none;display:inline"
id="Magnetometer-OK"
width="18.966711"
height="10.202584"
x="73.057671"
y="17.391815"
ry="1"
inkscape:label="#rect4550-8-1-4-21-5-33" />
</g>
<g
inkscape:groupmode="layer"
id="layer10"
@ -1399,7 +1415,7 @@
inkscape:groupmode="layer"
id="g5044"
inkscape:label="Airspeed-Warning"
style="display:inline">
style="display:none">
<rect
style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
id="Airspeed-Warning"
@ -1410,6 +1426,21 @@
ry="1"
inkscape:label="#rect4550-8-1-4-21-5-33" />
</g>
<g
inkscape:groupmode="layer"
id="layer41"
inkscape:label="Magnetometer-Warning"
style="display:none">
<rect
style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
id="Magnetometer-Warning"
width="18.966711"
height="10.202584"
x="73.091171"
y="17.425316"
ry="1"
inkscape:label="#rect4550-8-1-4-21-5-33" />
</g>
<g
inkscape:groupmode="layer"
id="g5047"
@ -1686,7 +1717,7 @@
inkscape:groupmode="layer"
id="g5420"
inkscape:label="Airspeed-Critical"
style="display:inline">
style="display:none">
<rect
style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
id="Airspeed-Critical"
@ -1697,6 +1728,21 @@
ry="1"
inkscape:label="#rect4550-8-1-4-21-5-33" />
</g>
<g
inkscape:groupmode="layer"
id="layer40"
inkscape:label="Magnetometer-Critical"
style="display:none">
<rect
style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
id="Magnetometer-Critical"
width="18.966711"
height="10.202584"
x="73.02417"
y="17.391815"
ry="1"
inkscape:label="#rect4550-8-1-4-21-5-33" />
</g>
<g
inkscape:groupmode="layer"
id="g5423"
@ -2055,6 +2101,28 @@
style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19167542;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
</g>
</g>
<g
inkscape:groupmode="layer"
id="layer4"
inkscape:label="Magnetometer-Error"
style="display:none">
<g
style="display:inline"
id="Magnetometer-Error"
inkscape:label="#g3398"
transform="matrix(1.0309004,0,0,1,20.190366,0.03602308)">
<path
inkscape:connector-curvature="0"
id="path6233-9-9-79-9"
d="m 51.59088,18.071902 18.268111,8.770366"
style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19476616;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
<path
inkscape:connector-curvature="0"
id="path6233-9-9-4-3-4"
d="M 51.580734,26.818576 69.902677,18.119165"
style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19167542;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
</g>
</g>
<g
inkscape:groupmode="layer"
id="layer2"
@ -2450,15 +2518,15 @@
<text
xml:space="preserve"
style="font-size:4px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
x="93.336571"
x="94.605125"
y="20.116049"
id="text5330-7-7-8"
sodipodi:linespacing="125%"
transform="scale(0.83127393,1.2029729)"><tspan
sodipodi:role="line"
id="tspan4295"
x="93.336571"
y="20.116049">BARO</tspan></text>
id="tspan3835"
x="94.605125"
y="20.116049">MAG</tspan></text>
<text
xml:space="preserve"
style="font-size:3px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 103 KiB

After

Width:  |  Height:  |  Size: 105 KiB

4
shared/uavobjectdefinition/revosettings.xml
View File

@ -7,6 +7,10 @@
Defaults: updates at 5 Hz, tau = 300s settle time, exp(-(1/f)/tau) ~= 0.9993335555062
Set BaroGPSOffsetCorrectionAlpha = 1.0 to completely disable baro offset updates. -->
<field name="BaroGPSOffsetCorrectionAlpha" units="" type="float" elements="1" defaultvalue="0.9993335555062"/>
<!-- Configuration for magnetometer vector validity check -->
<field name="MagnetometerMaxDeviation" units="%" type="float" elementnames="Warning,Error" defaultvalue="0.05,0.15" />
<!-- Cooefficients for the polynomial that models the barometer pressure bias as a function of temperature
bias = a + b * temp + c * temp^2 + d * temp^3 -->
<field name="BaroTempCorrectionPolynomial" units="" type="float" elements="4" elementnames="a,b,c,d" defaultvalue="0,0,0,0"/>

1
shared/uavobjectdefinition/systemalarms.xml
View File

@ -15,6 +15,7 @@
<elementname>Actuator</elementname>
<elementname>Attitude</elementname>
<elementname>Sensors</elementname>
<elementname>Magnetometer</elementname>
<elementname>Airspeed</elementname>
<elementname>Stabilization</elementname>
<elementname>Guidance</elementname>