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

some fixes to filtercf and stateestimation and filterekf - Thanks Werner for help :-)

Browse Source
This commit is contained in:
Corvus Corax 2013-05-21 07:27:31 +02:00
parent 3dda543608
commit ecc4a529a3
4 changed files with 65 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
flight/modules/StateEstimation/filtercf.c
View File

@ -52,8 +52,9 @@ static AttitudeSettingsData attitudeSettings;
static FlightStatusData flightStatus; static FlightStatusData flightStatus;
static HomeLocationData homeLocation; static HomeLocationData homeLocation;
static bool first_run = 1; static bool initialized = 0;
static bool useMag = 0; static bool first_run = 1;
static bool useMag = 0;
static float currentAccel[3]; static float currentAccel[3];
static float currentMag[3]; static float currentMag[3];
static float gyroBias[3]; static float gyroBias[3];
@ -74,17 +75,28 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3],
static void flightStatusUpdatedCb(UAVObjEvent *ev); static void flightStatusUpdatedCb(UAVObjEvent *ev);
static void globalInit(void);
static void globalInit(void)
{
if (!initialized) {
initialized = 1;
FlightStatusConnectCallback(&flightStatusUpdatedCb);
flightStatusUpdatedCb(NULL);
}
}
void filterCFInitialize(stateFilter *handle) void filterCFInitialize(stateFilter *handle)
{ {
globalInit();
handle->init = &initwithoutmag; handle->init = &initwithoutmag;
handle->filter = &filter; handle->filter = &filter;
FlightStatusConnectCallback(&flightStatusUpdatedCb);
flightStatusUpdatedCb(NULL);
} }
void filterCFMInitialize(stateFilter *handle) void filterCFMInitialize(stateFilter *handle)
{ {
globalInit();
handle->init = &initwithmag; handle->init = &initwithmag;
handle->filter = &filter; handle->filter = &filter;
} }
@ -158,9 +170,6 @@ static int32_t filter(stateEstimation *state)
accelUpdated = 0; accelUpdated = 0;
magUpdated = 0; magUpdated = 0;
} }
state->gyr[0] -= gyroBias[0];
state->gyr[1] -= gyroBias[1];
state->gyr[2] -= gyroBias[2];
} }
return result; return result;
} }
@ -226,13 +235,11 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3],
attitudeState.Pitch = RAD2DEG(attitudeState.Pitch); attitudeState.Pitch = RAD2DEG(attitudeState.Pitch);
attitudeState.Yaw = RAD2DEG(attitudeState.Yaw); attitudeState.Yaw = RAD2DEG(attitudeState.Yaw);
RPY2Quaternion(&attitudeState.Roll, &attitudeState.q1); RPY2Quaternion(&attitudeState.Roll, q);
q[0] = attitudeState.q1;
q[1] = attitudeState.q1;
q[2] = attitudeState.q1;
q[3] = attitudeState.q1;
timeval = PIOS_DELAY_GetRaw(); first_run = 0;
timeval = PIOS_DELAY_GetRaw();
return 0; return 0;
} }
@ -338,14 +345,12 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3],
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
gyroBias[0] -= accel_err[0] * attitudeSettings.AccelKi; gyroBias[0] -= accel_err[0] * attitudeSettings.AccelKi;
gyroBias[1] -= accel_err[1] * attitudeSettings.AccelKi; gyroBias[1] -= accel_err[1] * attitudeSettings.AccelKi;
if (useMag) { gyroBias[2] -= mag_err[2] * magKi;
gyroBias[2] -= mag_err[2] * magKi;
}
// Correct rates based on integral coefficient // Correct rates based on integral coefficient
gyro[0] -= gyroBias[0]; gyro[0] -= gyroBias[0];
gyro[1] -= gyroBias[1]; gyro[1] -= gyroBias[1];
gyro[2] -= gyroBias[2]; gyro[2] -= gyroBias[2];
float gyrotmp[3] = { gyro[0], gyro[1], gyro[2] }; float gyrotmp[3] = { gyro[0], gyro[1], gyro[2] };
// Correct rates based on proportional coefficient // Correct rates based on proportional coefficient
@ -388,10 +393,7 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3],
// If quaternion has become inappropriately short or is nan reinit. // If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN // THIS SHOULD NEVER ACTUALLY HAPPEN
if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) { if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) {
q[0] = 1.0f; first_run = 1;
q[1] = 0.0f;
q[2] = 0.0f;
q[3] = 0.0f;
return 2; return 2;
} }

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

@ -51,8 +51,9 @@
static EKFConfigurationData ekfConfiguration; static EKFConfigurationData ekfConfiguration;
static HomeLocationData homeLocation; static HomeLocationData homeLocation;
static bool first_run = 1; static bool initialized = 0;
static bool usePos = 0; static bool first_run = 1;
static bool usePos = 0;
// Private functions // Private functions
@ -64,14 +65,27 @@ static int32_t filter(stateEstimation *state);
static inline bool invalid(float data); static inline bool invalid(float data);
static inline bool invalid_var(float data); static inline bool invalid_var(float data);
static void globalInit(void);
static void globalInit(void)
{
if (!initialized) {
initialized = 1;
EKFConfigurationInitialize();
EKFStateVarianceInitialize();
}
}
void filterEKF13iInitialize(stateFilter *handle) void filterEKF13iInitialize(stateFilter *handle)
{ {
globalInit();
handle->init = &init13i; handle->init = &init13i;
handle->filter = &filter; handle->filter = &filter;
} }
void filterEKF13Initialize(stateFilter *handle) void filterEKF13Initialize(stateFilter *handle)
{ {
globalInit();
handle->init = &init13; handle->init = &init13;
handle->filter = &filter; handle->filter = &filter;
} }
@ -80,11 +94,13 @@ void filterEKF13Initialize(stateFilter *handle)
// XXX // XXX
void filterEKF16iInitialize(stateFilter *handle) void filterEKF16iInitialize(stateFilter *handle)
{ {
globalInit();
handle->init = &init13i; handle->init = &init13i;
handle->filter = &filter; handle->filter = &filter;
} }
void filterEKF16Initialize(stateFilter *handle) void filterEKF16Initialize(stateFilter *handle)
{ {
globalInit();
handle->init = &init13; handle->init = &init13;
handle->filter = &filter; handle->filter = &filter;
} }

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

@ -260,7 +260,9 @@ MODULE_INITCALL(StateEstimationInitialize, StateEstimationStart)
static void StateEstimationCb(void) static void StateEstimationCb(void)
{ {
// alarms flag // alarms flag
uint8_t alarm = 0; int8_t alarm = 0;
static int8_t lastAlarm = -1;
static uint16_t alarmcounter = 0;
// set alarm to warning if called through timeout // set alarm to warning if called through timeout
if (updatedSensors == 0) { if (updatedSensors == 0) {
@ -422,13 +424,27 @@ static void StateEstimationCb(void)
AttitudeStateSet(&s); AttitudeStateSet(&s);
} }
// throttle alarms, raise alarm flags immediately
// but require system to run for a while before decreasing
// to prevent alarm flapping
if (alarm >= lastAlarm) {
lastAlarm = alarm;
alarmcounter = 0;
} else {
if (alarmcounter < 100) {
alarmcounter++;
} else {
lastAlarm = alarm;
alarmcounter = 0;
}
}
// clear alarms if everything is alright, then schedule callback execution after timeout // clear alarms if everything is alright, then schedule callback execution after timeout
if (alarm == 1) { if (lastAlarm == 1) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
} else if (alarm == 2) { } else if (lastAlarm == 2) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
} else if (alarm >= 3) { } else if (lastAlarm >= 3) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
} else { } else {
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);

4
flight/targets/boards/revolution/firmware/Makefile
View File

@ -29,8 +29,8 @@ USE_DSP_LIB ?= NO
# List of mandatory modules to include # List of mandatory modules to include
MODULES += Sensors MODULES += Sensors
MODULES += Attitude/revolution #MODULES += Attitude/revolution
#MODULES += StateEstimation # use instead of Attitude MODULES += StateEstimation # use instead of Attitude
MODULES += Altitude/revolution MODULES += Altitude/revolution
MODULES += Airspeed/revolution MODULES += Airspeed/revolution
MODULES += AltitudeHold MODULES += AltitudeHold