rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

OP-1211 adapted existing code to use new pios functionality instead of separate error prone implementations

Browse Source
This commit is contained in:
Corvus Corax 2014-02-04 19:48:33 +01:00
parent a3bb523bf3
commit 28be9cc8ce
4 changed files with 43 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
flight/modules/Attitude/attitude.c
View File

@ -72,10 +72,16 @@
#define UPDATE_RATE 25.0f
#define GYRO_NEUTRAL 1665
#define UPDATE_EXPECTED (1.0f / 666.0f)
#define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f
// Private types
// Private variables
static xTaskHandle taskHandle;
static PiOSDeltatimeConfig dtconfig;
// Private functions
static void AttitudeTask(void *parameters);
@ -214,6 +220,8 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
// Force settings update to make sure rotation loaded
settingsUpdatedCb(AttitudeSettingsHandle());
PIOS_DELTATIME_Init(&dtconfig, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
// Main task loop
while (1) {
FlightStatusData flightStatus;
@ -473,12 +481,7 @@ static inline void apply_accel_filter(const float *raw, float *filtered)
static void updateAttitude(AccelStateData *accelStateData, GyroStateData *gyrosData)
{
float dT;
portTickType thisSysTime = xTaskGetTickCount();
static portTickType lastSysTime = 0;
dT = (thisSysTime == lastSysTime) ? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f;
lastSysTime = thisSysTime;
float dT = PIOS_DELTATIME_GetAverageSeconds(&dtconfig);
// Bad practice to assume structure order, but saves memory
float *gyros = &gyrosData->x;

1
flight/modules/Stabilization/stabilization.c
View File

@ -234,7 +234,6 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
}
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
fprintf(stderr, "dt is %f\n", dT);
FlightStatusGet(&flightStatus);
StabilizationDesiredGet(&stabDesired);
AttitudeStateGet(&attitudeState);

63
flight/modules/StateEstimation/filteraltitude.c
View File

@ -40,20 +40,22 @@
#define STACK_REQUIRED 128
#define DT_ALPHA 1e-3f
#define DT_ALPHA 1e-2f
#define DT_MIN 1e-6f
#define DT_MAX 1.0f
#define DT_AVERAGE 1e-3f
// Private types
struct data {
float state[4]; // state = altitude,velocity,accel_offset,accel
float pos[3]; // position updates from other filters
float vel[3]; // position updates from other filters
float dTA;
float dTA2;
int32_t lastTime;
float accelLast;
float baroLast;
int32_t baroLastTime;
bool first_run;
float state[4]; // state = altitude,velocity,accel_offset,accel
float pos[3]; // position updates from other filters
float vel[3]; // position updates from other filters
PiOSDeltatimeConfig dt1config;
PiOSDeltatimeConfig dt2config;
float accelLast;
float baroLast;
bool first_run;
AltitudeFilterSettingsData settings;
};
@ -89,8 +91,8 @@ static int32_t init(stateFilter *self)
this->vel[0] = 0.0f;
this->vel[1] = 0.0f;
this->vel[2] = 0.0f;
this->dTA = -1.0f;
this->dTA2 = -1.0f;
PIOS_DELTATIME_Init(&this->dt1config, DT_AVERAGE, DT_MIN, DT_MAX, DT_ALPHA);
PIOS_DELTATIME_Init(&this->dt2config, DT_AVERAGE, DT_MIN, DT_MAX, DT_ALPHA);
this->baroLast = 0.0f;
this->accelLast = 0.0f;
this->first_run = 1;
@ -104,12 +106,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
if (this->first_run) {
// Initialize to current altitude reading at initial location
if (IS_SET(state->updated, SENSORUPDATES_accel)) {
this->lastTime = PIOS_DELAY_GetRaw();
}
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
this->first_run = 0;
this->baroLastTime = PIOS_DELAY_GetRaw();
this->first_run = 0;
}
} else {
// save existing position and velocity updates so GPS will still work
@ -141,22 +139,14 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
// correct velocity and position state (integration)
// low pass for average dT, compensate timing jitter from scheduler
float dT = PIOS_DELAY_DiffuS(this->lastTime) / 1.0e6f;
this->lastTime = PIOS_DELAY_GetRaw();
if (dT < 0.001f) {
dT = 0.001f;
}
if (this->dTA < 0) {
this->dTA = dT;
} else {
this->dTA = this->dTA * (1.0f - DT_ALPHA) + dT * DT_ALPHA;
}
//
float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt1config);
float speedLast = this->state[1];
this->state[1] += 0.5f * (this->accelLast + (this->state[3] - this->state[2])) * this->dTA;
this->state[1] += 0.5f * (this->accelLast + (this->state[3] - this->state[2])) * dT;
this->accelLast = this->state[3] - this->state[2];
this->state[0] += 0.5f * (speedLast + this->state[1]) * this->dTA;
this->state[0] += 0.5f * (speedLast + this->state[1]) * dT;
state->pos[0] = this->pos[0];
@ -175,17 +165,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
// correct the velocity state (low pass differentiation)
// low pass for average dT, compensate timing jitter from scheduler
float dT = PIOS_DELAY_DiffuS(this->baroLastTime) / 1.0e6f;
this->baroLastTime = PIOS_DELAY_GetRaw();
if (dT < 0.001f) {
dT = 0.001f;
}
if (this->dTA2 < 0) {
this->dTA2 = dT;
} else {
this->dTA2 = this->dTA2 * (1.0f - DT_ALPHA) + dT * DT_ALPHA;
}
this->state[1] = (1.0f - (this->settings.BaroKp * this->settings.BaroKp)) * this->state[1] + (this->settings.BaroKp * this->settings.BaroKp) * (state->baro[0] - this->baroLast) / this->dTA2;
float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt2config);
this->state[1] = (1.0f - (this->settings.BaroKp * this->settings.BaroKp)) * this->state[1] + (this->settings.BaroKp * this->settings.BaroKp) * (state->baro[0] - this->baroLast) / dT;
this->baroLast = state->baro[0];
state->pos[0] = this->pos[0];

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

@ -45,6 +45,8 @@
#define STACK_REQUIRED 2048
#define DT_ALPHA 1e-3f
#define DT_MIN 1e-6f
#define DT_MAX 1.0f
#define DT_INIT (1.0f / 666.0f) // initialize with 666 Hz (default sensor update rate on revo)
#define IMPORT_SENSOR_IF_UPDATED(shortname, num) \
@ -66,10 +68,9 @@ struct data {
stateEstimation work;
uint32_t ins_last_time;
bool inited;
bool inited;
float dTa;
PiOSDeltatimeConfig dtconfig;
};
// Private variables
@ -154,11 +155,10 @@ static int32_t maininit(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->inited = false;
this->init_stage = 0;
this->work.updated = 0;
this->ins_last_time = PIOS_DELAY_GetRaw();
this->dTa = DT_INIT;
this->inited = false;
this->init_stage = 0;
this->work.updated = 0;
PIOS_DELTATIME_Init(&this->dtconfig, DT_INIT, DT_MIN, DT_MAX, DT_ALPHA);
EKFConfigurationGet(&this->ekfConfiguration);
int t;
@ -224,17 +224,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
return 0;
}
dT = PIOS_DELAY_DiffuS(this->ins_last_time) / 1.0e6f;
this->ins_last_time = PIOS_DELAY_GetRaw();
// This should only happen at start up or at mode switches
if (dT > 0.01f) {
dT = 0.01f;
} else if (dT <= 0.001f) {
dT = 0.001f;
}
this->dTa = this->dTa * (1.0f - DT_ALPHA) + dT * DT_ALPHA; // low pass for average dT, compensate timing jitter from scheduler
dT = PIOS_DELTATIME_GetAverageSeconds(&this->dtconfig);
if (!this->inited && IS_SET(this->work.updated, SENSORUPDATES_mag) && IS_SET(this->work.updated, SENSORUPDATES_baro) && IS_SET(this->work.updated, SENSORUPDATES_pos)) {
// Don't initialize until all sensors are read
@ -300,7 +290,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
// Run prediction a bit before any corrections
float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) };
INSStatePrediction(gyros, this->work.accel, this->dTa);
INSStatePrediction(gyros, this->work.accel, dT);
// Copy the attitude into the state
// NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true
@ -335,7 +325,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) };
// Advance the state estimate
INSStatePrediction(gyros, this->work.accel, this->dTa);
INSStatePrediction(gyros, this->work.accel, dT);
// Copy the attitude into the state
// NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true
@ -355,7 +345,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
state->updated |= SENSORUPDATES_attitude | SENSORUPDATES_pos | SENSORUPDATES_vel;
// Advance the covariance estimate
INSCovariancePrediction(this->dTa);
INSCovariancePrediction(dT);
if (IS_SET(this->work.updated, SENSORUPDATES_mag)) {
sensors |= MAG_SENSORS;