rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +01:00
Code Issues Releases Activity

OP-1161 moved mag validity check function from sensors to state estimation

Browse Source
This commit is contained in:
Corvus Corax 2014-01-14 16:38:16 +01:00
parent c180846ca9
commit 75b1a7ff5f
2 changed files with 65 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
flight/modules/Sensors/sensors.c
View File

@ -55,8 +55,6 @@
#include <attitudestate.h>
#include <attitudesettings.h>
#include <revocalibration.h>
#include <revosettings.h>
#include <homelocation.h>
#include <flightstatus.h>
#include <taskinfo.h>
@ -75,7 +73,6 @@
// Private functions
static void SensorsTask(void *parameters);
static void settingsUpdatedCb(UAVObjEvent *objEv);
static void checkMagValidity(MagSensorData *mag);
// static void magOffsetEstimation(MagSensorData *mag);
// Private variables
@ -95,8 +92,6 @@ static float R[3][3] = {
{ 0 }
};
static int8_t rotate = 0;
static RevoSettingsMagnetometerMaxDeviationData magMaxDev;
static float Be[3] = { 0 };
/**
* API for sensor fusion algorithms:
@ -117,17 +112,12 @@ int32_t SensorsInitialize(void)
AccelSensorInitialize();
MagSensorInitialize();
RevoCalibrationInitialize();
RevoSettingsInitialize();
AttitudeSettingsInitialize();
AttitudeStateInitialize();
HomeLocationInitialize();
rotate = 0;
RevoCalibrationConnectCallback(&settingsUpdatedCb);
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
RevoSettingsConnectCallback(&settingsUpdatedCb);
HomeLocationConnectCallback(&settingsUpdatedCb);
return 0;
}
@ -414,7 +404,6 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
MagSensorSet(&mag);
mag_update_time = PIOS_DELAY_GetRaw();
checkMagValidity(&mag);
}
#endif /* if defined(PIOS_INCLUDE_HMC5883) */
@ -426,57 +415,6 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
}
}
/**
* check validity of magnetometers
*/
static void checkMagValidity(MagSensorData *mag)
{
#define lowPassAlpha 0.01f
#define idleCount 100
static float average[3] = { 0 };
static uint8_t noteverytime = idleCount;
// low pass filter sensor to not give warnings due to noise
average[0] = (1.0f - lowPassAlpha) * average[0] + lowPassAlpha * mag->x;
average[1] = (1.0f - lowPassAlpha) * average[1] + lowPassAlpha * mag->y;
average[2] = (1.0f - lowPassAlpha) * average[2] + lowPassAlpha * mag->z;
// throttle this check, thanks to low pass filter it is not necessary every iteration
if (!noteverytime--) {
noteverytime = idleCount;
// calculate expected Be vector
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
float Rot[3][3];
float expected[3];
Quaternion2R(&attitudeState.q1, Rot);
rot_mult(Rot, Be, expected);
// calculate maximum allowed deviation
float warning = expected[0] * expected[0] + expected[1] * expected[1] + expected[2] * expected[2];
float error = magMaxDev.Error * magMaxDev.Error * warning;
warning = magMaxDev.Warning * magMaxDev.Warning * warning;
// calculate difference
expected[0] = expected[0] - average[0];
expected[1] = expected[1] - average[1];
expected[2] = expected[2] - average[2];
float deviation = expected[0] * expected[0] + expected[1] * expected[1] + expected[2] * expected[2];
// set errors
if (deviation < warning) {
AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER);
} else if (deviation < error) {
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_ERROR);
}
}
}
/**
* Locally cache some variables from the AtttitudeSettings object
*/
@ -520,9 +458,6 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
Quaternion2R(rotationQuat, R);
rotate = 1;
}
RevoSettingsMagnetometerMaxDeviationGet(&magMaxDev);
HomeLocationBeGet(Be);
}
/**
* @}

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

@ -33,6 +33,7 @@
#include "inc/stateestimation.h"
#include <attitudestate.h>
#include <revocalibration.h>
#include <revosettings.h>
#include <systemalarms.h>
#include <homelocation.h>
@ -46,7 +47,11 @@
struct data {
HomeLocationData homeLocation;
RevoCalibrationData revoCalibration;
float magBias[3];
RevoSettingsData revoSettings;
uint16_t counter;
bool validity;
float magAverage[3];
float magBias[3];
};
// Private variables
@ -55,6 +60,7 @@ struct data {
static int32_t init(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
static bool checkMagValidity(struct data *this, float mag[3]);
static void magOffsetEstimation(struct data *this, float mag[3]);
@ -71,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;
this->magBias[0] = this->magBias[1] = this->magBias[2] = 0.0f;
this->magAverage[0] = this->magAverage[1] = this->magAverage[2] = 0.0f;
this->counter = 0;
this->validity = true;
HomeLocationGet(&this->homeLocation);
RevoCalibrationGet(&this->revoCalibration);
RevoSettingsGet(&this->revoSettings);
return 0;
}
@ -82,9 +92,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
struct data *this = (struct data *)self->localdata;
if (IS_SET(state->updated, SENSORUPDATES_mag)) {
SystemAlarmsAlarmData alarms;
SystemAlarmsAlarGet(&alarms);
if (alarms.Magnetometer != SYSTEMALARMS_ALARM_OK) {
if (!checkMagValidity(this, state->mag)) {
UNSET_MASK(state->updated, SENSORUPDATES_mag);
} else {
if (this->revoCalibration.MagBiasNullingRate > 0) {
@ -96,6 +104,58 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
return 0;
}
/**
* check validity of magnetometers
*/
static bool checkMagValidity(struct data *this, float mag[3])
{
#define MAG_LOW_PASS_ALPHA 0.05f
#define IDLE_COUNT 20
// low pass filter sensor to not give warnings due to noise
this->magAverage[0] = (1.0f - MAG_LOW_PASS_ALPHA) * this->magAverage[0] + MAG_LOW_PASS_ALPHA * mag[0];
this->magAverage[1] = (1.0f - MAG_LOW_PASS_ALPHA) * this->magAverage[1] + MAG_LOW_PASS_ALPHA * mag[0];
this->magAverage[2] = (1.0f - MAG_LOW_PASS_ALPHA) * this->magAverage[2] + MAG_LOW_PASS_ALPHA * mag[0];
// throttle this check, thanks to low pass filter it is not necessary every iteration
if (!this->counter--) {
this->counter = IDLE_COUNT;
// calculate expected Be vector
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
float Rot[3][3];
float expected[3];
Quaternion2R(&attitudeState.q1, Rot);
rot_mult(Rot, this->homeLocation.Be, expected);
// calculate maximum allowed deviation
float warning2 = expected[0] * expected[0] + expected[1] * expected[1] + expected[2] * expected[2];
float error2 = this->revoSettings.MagnetometerMaxDeviation.Error * this->revoSettings.MagnetometerMaxDeviation.Error * warning2;
warning2 = this->revoSettings.MagnetometerMaxDeviation.Warning * this->revoSettings.MagnetometerMaxDeviation.Warning * warning2;
// calculate difference
expected[0] = expected[0] - this->magAverage[0];
expected[1] = expected[1] - this->magAverage[1];
expected[2] = expected[2] - this->magAverage[2];
float deviation2 = expected[0] * expected[0] + expected[1] * expected[1] + expected[2] * expected[2];
// set errors
if (deviation2 < warning2) {
AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER);
this->validity = true;
} else if (deviation2 < error2) {
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING);
this->validity = false;
} else {
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_ERROR);
this->validity = false;
}
}
return this->validity;
}
/**
* Perform an update of the @ref MagBias based on
* Magmeter Offset Cancellation: Theory and Implementation,