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 HomeLocationData homeLocation;
static bool first_run = 1;
static bool useMag = 0;
static bool initialized = 0;
static bool first_run = 1;
static bool useMag = 0;
static float currentAccel[3];
static float currentMag[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 globalInit(void);
static void globalInit(void)
{
if (!initialized) {
initialized = 1;
FlightStatusConnectCallback(&flightStatusUpdatedCb);
flightStatusUpdatedCb(NULL);
}
}
void filterCFInitialize(stateFilter *handle)
{
globalInit();
handle->init = &initwithoutmag;
handle->filter = &filter;
FlightStatusConnectCallback(&flightStatusUpdatedCb);
flightStatusUpdatedCb(NULL);
}
void filterCFMInitialize(stateFilter *handle)
{
globalInit();
handle->init = &initwithmag;
handle->filter = &filter;
}
@ -158,9 +170,6 @@ static int32_t filter(stateEstimation *state)
accelUpdated = 0;
magUpdated = 0;
}
state->gyr[0] -= gyroBias[0];
state->gyr[1] -= gyroBias[1];
state->gyr[2] -= gyroBias[2];
}
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.Yaw = RAD2DEG(attitudeState.Yaw);
RPY2Quaternion(&attitudeState.Roll, &attitudeState.q1);
q[0] = attitudeState.q1;
q[1] = attitudeState.q1;
q[2] = attitudeState.q1;
q[3] = attitudeState.q1;
RPY2Quaternion(&attitudeState.Roll, q);
timeval = PIOS_DELAY_GetRaw();
first_run = 0;
timeval = PIOS_DELAY_GetRaw();
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
gyroBias[0] -= accel_err[0] * 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
gyro[0] -= gyroBias[0];
gyro[1] -= gyroBias[1];
gyro[2] -= gyroBias[2];
gyro[0] -= gyroBias[0];
gyro[1] -= gyroBias[1];
gyro[2] -= gyroBias[2];
float gyrotmp[3] = { gyro[0], gyro[1], gyro[2] };
// 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.
// THIS SHOULD NEVER ACTUALLY HAPPEN
if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) {
q[0] = 1.0f;
q[1] = 0.0f;
q[2] = 0.0f;
q[3] = 0.0f;
first_run = 1;
return 2;
}

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

@ -51,8 +51,9 @@
static EKFConfigurationData ekfConfiguration;
static HomeLocationData homeLocation;
static bool first_run = 1;
static bool usePos = 0;
static bool initialized = 0;
static bool first_run = 1;
static bool usePos = 0;
// Private functions
@ -64,14 +65,27 @@ static int32_t filter(stateEstimation *state);
static inline bool invalid(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)
{
globalInit();
handle->init = &init13i;
handle->filter = &filter;
}
void filterEKF13Initialize(stateFilter *handle)
{
globalInit();
handle->init = &init13;
handle->filter = &filter;
}
@ -80,11 +94,13 @@ void filterEKF13Initialize(stateFilter *handle)
// XXX
void filterEKF16iInitialize(stateFilter *handle)
{
globalInit();
handle->init = &init13i;
handle->filter = &filter;
}
void filterEKF16Initialize(stateFilter *handle)
{
globalInit();
handle->init = &init13;
handle->filter = &filter;
}

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

@ -260,7 +260,9 @@ MODULE_INITCALL(StateEstimationInitialize, StateEstimationStart)
static void StateEstimationCb(void)
{
// 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
if (updatedSensors == 0) {
@ -422,13 +424,27 @@ static void StateEstimationCb(void)
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
if (alarm == 1) {
if (lastAlarm == 1) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
} else if (alarm == 2) {
} else if (lastAlarm == 2) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
} else if (alarm >= 3) {
} else if (lastAlarm >= 3) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
} else {
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
MODULES += Sensors
MODULES += Attitude/revolution
#MODULES += StateEstimation # use instead of Attitude
#MODULES += Attitude/revolution
MODULES += StateEstimation # use instead of Attitude
MODULES += Altitude/revolution
MODULES += Airspeed/revolution
MODULES += AltitudeHold