mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
OP-1406 - Allows filtermag to switch between onboard and aux mag
This commit is contained in:
parent
52ad63be33
commit
9d7a493c26
@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
@ -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;
|
||||
|
||||
|
@ -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);
|
||||
|
@ -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"/>
|
||||
|
Loading…
x
Reference in New Issue
Block a user