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

OP-1406 - Improve handling of external magnetometer, add additional settings

Browse Source
This commit is contained in:
Alessio Morale 2014-07-17 20:52:10 +02:00
parent 7abbf5fa5a
commit e511190ad3
10 changed files with 180 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
flight/libraries/inc/CoordinateConversions.h
View File

@ -112,5 +112,73 @@ inline void matrix_mult_3x3f(float a[3][3], float b[3][3], float result[3][3])
result[2][2] = a[0][2] * b[2][0] + a[1][2] * b[2][1] + a[2][2] * b[2][2];
}
inline void matrix_inline_scale_3f(float a[3][3], float scale)
{
a[0][0] *= scale;
a[0][1] *= scale;
a[0][2] *= scale;
a[1][0] *= scale;
a[1][1] *= scale;
a[1][2] *= scale;
a[2][0] *= scale;
a[2][1] *= scale;
a[2][2] *= scale;
}
inline void rot_about_axis_x(const float rotation, float R[3][3])
{
float s = sinf(rotation);
float c = cosf(rotation);
R[0][0] = 1;
R[0][1] = 0;
R[0][2] = 0;
R[1][0] = 0;
R[1][1] = c;
R[1][2] = -s;
R[2][0] = 0;
R[2][1] = s;
R[2][2] = c;
}
inline void rot_about_axis_y(const float rotation, float R[3][3])
{
float s = sinf(rotation);
float c = cosf(rotation);
R[0][0] = c;
R[0][1] = 0;
R[0][2] = s;
R[1][0] = 0;
R[1][1] = 1;
R[1][2] = 0;
R[2][0] = -s;
R[2][1] = 0;
R[2][2] = c;
}
inline void rot_about_axis_z(const float rotation, float R[3][3])
{
float s = sinf(rotation);
float c = cosf(rotation);
R[0][0] = c;
R[0][1] = -s;
R[0][2] = 0;
R[1][0] = s;
R[1][1] = c;
R[1][2] = 0;
R[2][0] = 0;
R[2][1] = 0;
R[2][2] = 1;
}
#endif // COORDINATECONVERSIONS_H_

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

@ -36,7 +36,7 @@
#include <revosettings.h>
#include <systemalarms.h>
#include <homelocation.h>
#include <auxmagsettings.h>
#include <CoordinateConversions.h>
// Private constants
@ -47,10 +47,12 @@
struct data {
RevoCalibrationData revoCalibration;
RevoSettingsData revoSettings;
AuxMagSettingsUsageOptions auxMagUsage;
uint8_t warningcount;
uint8_t errorcount;
float homeLocationBe[3];
float magBe2;
float magBe;
float invMagBe;
float magBias[3];
};
@ -60,9 +62,9 @@ struct data {
static int32_t init(stateFilter *self);
static filterResult filter(stateFilter *self, stateEstimation *state);
static bool checkMagValidity(struct data *this, float mag[3], bool setAlarms);
static void magOffsetEstimation(struct data *this, float mag[3]);
static bool checkMagValidity(struct data *this, float error, bool setAlarms);
// static void magOffsetEstimation(struct data *this, float mag[3]);
static float getMagError(struct data *this, float mag[3]);
int32_t filterMagInitialize(stateFilter *handle)
{
@ -80,105 +82,131 @@ static int32_t init(stateFilter *self)
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];
// magBe holds the squared magnetic vector length (extpected)
this->magBe = sqrtf(this->homeLocationBe[0] * this->homeLocationBe[0] + this->homeLocationBe[1] * this->homeLocationBe[1] + this->homeLocationBe[2] * this->homeLocationBe[2]);
this->invMagBe = 1.0f / this->magBe;
RevoCalibrationGet(&this->revoCalibration);
RevoSettingsGet(&this->revoSettings);
AuxMagSettingsUsageGet(&this->auxMagUsage);
return 0;
}
static filterResult filter(stateFilter *self, stateEstimation *state)
{
struct data *this = (struct data *)self->localdata;
float auxMagError;
float boardMagError;
float temp_mag[3] = { 0 };
uint8_t temp_status = MAGSTATUS_INVALID;
uint8_t magSamples = 0;
// Uses the external mag when available
if (IS_SET(state->updated, SENSORUPDATES_auxMag) && checkMagValidity(this, state->auxMag, false)) {
state->mag[0] = state->auxMag[0];
state->mag[1] = state->auxMag[1];
state->mag[2] = state->auxMag[2];
state->magStatus = MAGSTATUS_AUX;
return FILTERRESULT_OK;
}
// Defaults to board magnetometer
state->mag[0] = state->boardMag[0];
state->mag[1] = state->boardMag[1];
state->mag[2] = state->boardMag[2];
state->magStatus = MAGSTATUS_ONBOARD;
if (IS_SET(state->updated, SENSORUPDATES_mag)) {
// set alarm as aux sensor is invalid too
if (!checkMagValidity(this, state->mag, true)) {
state->magStatus = MAGSTATUS_INVALID;
}
if (this->revoCalibration.MagBiasNullingRate > 0) {
magOffsetEstimation(this, state->mag);
if ((this->auxMagUsage != AUXMAGSETTINGS_USAGE_ONBOARDONLY) &&
IS_SET(state->updated, SENSORUPDATES_auxMag)) {
auxMagError = getMagError(this, state->auxMag);
// Handles alarms only if it will rely on aux mag only
if (checkMagValidity(this, auxMagError,
(this->auxMagUsage == AUXMAGSETTINGS_USAGE_AUXONLY))) {
temp_mag[0] = state->auxMag[0];
temp_mag[1] = state->auxMag[1];
temp_mag[2] = state->auxMag[2];
temp_status = MAGSTATUS_AUX;
magSamples++;
}
}
if ((this->auxMagUsage != AUXMAGSETTINGS_USAGE_AUXONLY) &&
IS_SET(state->updated, SENSORUPDATES_boardMag)) {
// TODO!
/*if (this->revoCalibration.MagBiasNullingRate > 0) {
magOffsetEstimation(this, state->boardMag);
}*/
boardMagError = getMagError(this, state->boardMag);
// sets warning only if no mag data are available (aux is invalid or missing)
if (checkMagValidity(this, boardMagError, temp_status == MAGSTATUS_INVALID)) {
temp_mag[0] += state->boardMag[0];
temp_mag[1] += state->boardMag[1];
temp_mag[2] += state->boardMag[2];
temp_status = MAGSTATUS_OK;
magSamples++;
}
}
if (magSamples > 1) {
temp_mag[0] *= 0.5f;
temp_mag[1] *= 0.5f;
temp_mag[2] *= 0.5f;
}
if (temp_status != MAGSTATUS_INVALID) {
temp_mag[0] *= this->invMagBe;
temp_mag[1] *= this->invMagBe;
temp_mag[2] *= this->invMagBe;
state->mag[0] = temp_mag[0];
state->mag[1] = temp_mag[1];
state->mag[2] = temp_mag[2];
state->updated |= SENSORUPDATES_mag;
}
state->magStatus = temp_status;
return FILTERRESULT_OK;
}
/**
* check validity of magnetometers
*/
static bool checkMagValidity(struct data *this, float mag[3], bool setAlarms)
static bool checkMagValidity(struct data *this, float error, bool setAlarms)
{
#define ALARM_THRESHOLD 5
bool valid = true;
// 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) {
if (error < this->revoSettings.MagnetometerMaxDeviation.Warning) {
this->warningcount = 0;
this->errorcount = 0;
valid = true;
AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER);
} else if (minError2 < mag2 && mag2 < maxError2) {
return true;
}
if (error < this->revoSettings.MagnetometerMaxDeviation.Error) {
this->errorcount = 0;
if (this->warningcount > ALARM_THRESHOLD) {
valid = false;
if (setAlarms) {
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING);
}
return false;
} else {
this->warningcount++;
return true;
}
} else {
}
if (this->errorcount > ALARM_THRESHOLD) {
valid = false;
if (setAlarms) {
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL);
}
return false;
} else {
this->errorcount++;
}
}
return valid;
// still in "grace period"
return true;
}
static float getMagError(struct data *this, float mag[3])
{
// vector norm
float magnitude = sqrtf(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
// absolute value of relative error against Be
float error = fabsf(magnitude - this->magBe) * this->invMagBe;
return error;
}
#if 0
/**
* Perform an update of the @ref MagBias based on
* Magmeter Offset Cancellation: Theory and Implementation,
* revisited William Premerlani, October 14, 2011
*/
static void magOffsetEstimation(struct data *this, float mag[3])
void magOffsetEstimation(struct data *this, float mag[3])
{
#if 0
// Constants, to possibly go into a UAVO
@ -269,7 +297,7 @@ static void magOffsetEstimation(struct data *this, float mag[3])
#endif // if 0
}
#endif /* if 0 */
/**
* @}
* @}

6
flight/modules/StateEstimation/inc/stateestimation.h
View File

@ -47,8 +47,8 @@ typedef enum {
SENSORUPDATES_gyro = 1 << 0,
SENSORUPDATES_accel = 1 << 1,
SENSORUPDATES_mag = 1 << 2,
SENSORUPDATES_auxMag = 1 << 2,
SENSORUPDATES_boardMag = 1 << 2,
SENSORUPDATES_boardMag = 1 << 10,
SENSORUPDATES_auxMag = 1 << 9,
SENSORUPDATES_attitude = 1 << 3,
SENSORUPDATES_pos = 1 << 4,
SENSORUPDATES_vel = 1 << 5,
@ -57,7 +57,7 @@ typedef enum {
SENSORUPDATES_lla = 1 << 8,
} sensorUpdates;
#define MAGSTATUS_ONBOARD 1
#define MAGSTATUS_OK 1
#define MAGSTATUS_AUX 2
#define MAGSTATUS_INVALID 0
typedef struct {

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

@ -292,6 +292,7 @@ int32_t StateEstimationInitialize(void)
MagSensorConnectCallback(&sensorUpdatedCb);
BaroSensorConnectCallback(&sensorUpdatedCb);
AirspeedSensorConnectCallback(&sensorUpdatedCb);
AuxMagSensorConnectCallback(&sensorUpdatedCb);
GPSVelocitySensorConnectCallback(&sensorUpdatedCb);
GPSPositionSensorConnectCallback(&sensorUpdatedCb);
@ -470,8 +471,7 @@ static void StateEstimationCb(void)
gyroDelta[2] = states.gyro[2] - gyroRaw[2];
}
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(AccelState, accel, x, y, z);
{
if (IS_SET(states.updated, SENSORUPDATES_mag)) {
MagStateData s;
MagStateGet(&s);
@ -479,8 +479,8 @@ static void StateEstimationCb(void)
s.y = states.mag[1];
s.z = states.mag[2];
switch (states.magStatus) {
case MAGSTATUS_ONBOARD:
s.Source = MAGSTATE_SOURCE_ONBOARD;
case MAGSTATUS_OK:
s.Source = MAGSTATE_SOURCE_OK;
break;
case MAGSTATUS_AUX:
s.Source = MAGSTATE_SOURCE_AUX;
@ -590,7 +590,11 @@ static void sensorUpdatedCb(UAVObjEvent *ev)
}
if (ev->obj == MagSensorHandle()) {
updatedSensors |= SENSORUPDATES_mag;
updatedSensors |= SENSORUPDATES_boardMag;
}
if (ev->obj == AuxMagSensorHandle()) {
updatedSensors |= SENSORUPDATES_auxMag;
}
if (ev->obj == GPSPositionSensorHandle()) {

2
flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc
View File

@ -32,7 +32,7 @@ UAVOBJSRCFILENAMES += accelstate
UAVOBJSRCFILENAMES += accelsensor
UAVOBJSRCFILENAMES += magsensor
UAVOBJSRCFILENAMES += auxmagsensor
UAVOBJSRCFILENAMES += auxmagcalibration
UAVOBJSRCFILENAMES += auxmagsettings
UAVOBJSRCFILENAMES += magstate
UAVOBJSRCFILENAMES += barosensor
UAVOBJSRCFILENAMES += airspeedsensor

2
flight/targets/boards/revolution/firmware/UAVObjects.inc
View File

@ -32,7 +32,7 @@ UAVOBJSRCFILENAMES += accelstate
UAVOBJSRCFILENAMES += accelsensor
UAVOBJSRCFILENAMES += magsensor
UAVOBJSRCFILENAMES += auxmagsensor
UAVOBJSRCFILENAMES += auxmagcalibration
UAVOBJSRCFILENAMES += auxmagsettings
UAVOBJSRCFILENAMES += magstate
UAVOBJSRCFILENAMES += barosensor
UAVOBJSRCFILENAMES += airspeedsensor

2
flight/targets/boards/revoproto/firmware/UAVObjects.inc
View File

@ -32,7 +32,7 @@ UAVOBJSRCFILENAMES += accelstate
UAVOBJSRCFILENAMES += accelsensor
UAVOBJSRCFILENAMES += magsensor
UAVOBJSRCFILENAMES += auxmagsensor
UAVOBJSRCFILENAMES += auxmagcalibration
UAVOBJSRCFILENAMES += auxmagsettings
UAVOBJSRCFILENAMES += magstate
UAVOBJSRCFILENAMES += barosensor
UAVOBJSRCFILENAMES += airspeedsensor

2
flight/targets/boards/simposix/firmware/UAVObjects.inc
View File

@ -37,7 +37,7 @@ UAVOBJSRCFILENAMES += accelstate
UAVOBJSRCFILENAMES += accelsensor
UAVOBJSRCFILENAMES += magsensor
UAVOBJSRCFILENAMES += auxmagsensor
UAVOBJSRCFILENAMES += auxmagcalibration
UAVOBJSRCFILENAMES += auxmagsettings
UAVOBJSRCFILENAMES += magstate
UAVOBJSRCFILENAMES += barosensor
UAVOBJSRCFILENAMES += airspeedsensor

5
shared/uavobjectdefinition/auxmagcalibration.xml → shared/uavobjectdefinition/auxmagsettings.xml
View File

@ -1,10 +1,13 @@
<xml>
<object name="AuxMagCalibration" singleinstance="true" settings="true" category="Sensors">
<object name="AuxMagSettings" singleinstance="true" settings="true" category="Sensors">
<description>Settings for auxiliary magnetometer calibration</description>
<field name="mag_bias" units="mGau" type="float" elementnames="X,Y,Z" defaultvalue="0,0,0"/>
<field name="mag_transform" units="gain" type="float" elementnames="r0c0,r0c1,r0c2,r1c0,r1c1,r1c2,r2c0,r2c1,r2c2"
defaultvalue="1,0,0,0,1,0,0,0,1"/>
<field name="MagBiasNullingRate" units="" type="float" elements="1" defaultvalue="0"/>
<field name="Orientation" units="degrees" type="float" elements="1" defaultvalue="0"/>
<field name="Type" units="" type="enum" elements="1" options="GPSV9,Ext" defaultvalue="GPSV9"/>
<field name="Usage" units="" type="enum" elements="1" options="Both,OnboardOnly,AuxOnly" defaultvalue="Both"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>

2
shared/uavobjectdefinition/magstate.xml
View File

@ -4,7 +4,7 @@
<field name="x" units="mGa" type="float" elements="1"/>
<field name="y" units="mGa" type="float" elements="1"/>
<field name="z" units="mGa" type="float" elements="1"/>
<field name="Source" units="" type="enum" elements="1" options="Invalid,OnBoard,Aux" defaultvalue="Invalid"/>
<field name="Source" units="" type="enum" elements="1" options="Invalid,Ok,Aux" defaultvalue="Invalid"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>