rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-22 14:19:42 +01:00
Code Issues Releases Activity

OP-1406 - Allows filtermag to switch between onboard and aux mag

Browse Source
This commit is contained in:
Alessio Morale 2014-07-17 20:41:14 +02:00
parent 52ad63be33
commit 9d7a493c26
4 changed files with 77 additions and 20 deletions
flight/modules/StateEstimation
filtermag.c
inc
stateestimation.h
stateestimation.c
shared/uavobjectdefinition
magstate.xml

39
flight/modules/StateEstimation/filtermag.c

@ -60,7 +60,7 @@ struct data {
static int32_t init(stateFilter *self);
static filterResult filter(stateFilter *self, stateEstimation *state);
static void checkMagValidity(struct data *this, float mag[3]);
static bool checkMagValidity(struct data *this, float mag[3], bool setAlarms);
static void magOffsetEstimation(struct data *this, float mag[3]);
@ -91,8 +91,25 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
{
struct data *this = (struct data *)self->localdata;
// 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)) {
checkMagValidity(this, state->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);
}
@ -104,10 +121,10 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
/**
* check validity of magnetometers
*/
static void checkMagValidity(struct data *this, float mag[3])
static bool checkMagValidity(struct data *this, float mag[3], bool setAlarms)
{
#define ALARM_THRESHOLD 5
#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];
@ -130,21 +147,29 @@ static void checkMagValidity(struct data *this, float mag[3])
if (minWarning2 < mag2 && mag2 < maxWarning2) {
this->warningcount = 0;
this->errorcount = 0;
valid = true;
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);
valid = false;
if (setAlarms) {
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING);
}
} else {
this->warningcount++;
}
} else {
if (this->errorcount > ALARM_THRESHOLD) {
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL);
valid = false;
if (setAlarms) {
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL);
}
} else {
this->errorcount++;
}
}
return valid;
}

24
flight/modules/StateEstimation/inc/stateestimation.h

@ -47,6 +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_attitude = 1 << 3,
SENSORUPDATES_pos = 1 << 4,
SENSORUPDATES_vel = 1 << 5,
@ -55,15 +57,21 @@ typedef enum {
SENSORUPDATES_lla = 1 << 8,
} sensorUpdates;
#define MAGSTATUS_ONBOARD 1
#define MAGSTATUS_AUX 2
#define MAGSTATUS_INVALID 0
typedef struct {
float gyro[3];
float accel[3];
float mag[3];
float attitude[4];
float pos[3];
float vel[3];
float airspeed[2];
float baro[1];
float gyro[3];
float accel[3];
float mag[3];
float attitude[4];
float pos[3];
float vel[3];
float airspeed[2];
float baro[1];
float auxMag[3];
uint8_t magStatus;
float boardMag[3];
sensorUpdates updated;
} stateEstimation;

27
flight/modules/StateEstimation/stateestimation.c

@ -41,6 +41,7 @@
#include <gpspositionsensor.h>
#include <gpsvelocitysensor.h>
#include <homelocation.h>
#include <auxmagsensor.h>
#include <gyrostate.h>
#include <accelstate.h>
@ -267,6 +268,7 @@ int32_t StateEstimationInitialize(void)
GyroSensorInitialize();
MagSensorInitialize();
AuxMagSensorInitialize();
BaroSensorInitialize();
AirspeedSensorInitialize();
GPSVelocitySensorInitialize();
@ -423,7 +425,8 @@ static void StateEstimationCb(void)
gyroRaw[2] = states.gyro[2];
}
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AccelSensor, accel, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, mag, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, boardMag, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AuxMagSensor, auxMag, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, TrueAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
@ -467,7 +470,27 @@ static void StateEstimationCb(void)
gyroDelta[2] = states.gyro[2] - gyroRaw[2];
}
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(AccelState, accel, x, y, z);
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(MagState, mag, x, y, z);
{
MagStateData s;
MagStateGet(&s);
s.x = states.mag[0];
s.y = states.mag[1];
s.z = states.mag[2];
switch (states.magStatus) {
case MAGSTATUS_ONBOARD:
s.Source = MAGSTATE_SOURCE_ONBOARD;
break;
case MAGSTATUS_AUX:
s.Source = MAGSTATE_SOURCE_AUX;
break;
default:
s.Source = MAGSTATE_SOURCE_INVALID;
}
MagStateSet(&s);
}
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(PositionState, pos, North, East, Down);
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(VelocityState, vel, North, East, Down);
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_2_DIMENSIONS(AirspeedState, airspeed, CalibratedAirspeed, TrueAirspeed);

7
shared/uavobjectdefinition/magstate.xml

@ -1,9 +1,10 @@
<xml>
<object name="MagState" singleinstance="true" settings="false" category="State">
<description>The filtered magnet vector.</description>
<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="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"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>