rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00
Code Issues Releases Activity

changed default coefficients and exposed them to a settings uavobject as they should be

Browse Source
This commit is contained in:
Corvus Corax 2013-06-29 01:16:02 +02:00
parent 2621b087a2
commit 7745f93935
7 changed files with 45 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -32,6 +32,7 @@
#include "inc/stateestimation.h"
#include <attitudestate.h>
#include <altitudefiltersettings.h>
#include <CoordinateConversions.h>
@ -39,8 +40,6 @@
#define STACK_REQUIRED 128
#define BARO_SENSOR_KP 0.03f
#define ACCEL_DRIFT_KP 0.001f
#define DT_ALPHA 1e-3f
// Private types
@ -51,9 +50,11 @@ struct data {
float dTA;
float dTA2;
int32_t lastTime;
float accelLast;
float baroLast;
int32_t baroLastTime;
bool first_run;
AltitudeFilterSettingsData settings;
};
// Private variables
@ -70,6 +71,7 @@ int32_t filterAltitudeInitialize(stateFilter *handle)
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
AttitudeStateInitialize();
AltitudeFilterSettingsInitialize();
return STACK_REQUIRED;
}
@ -86,9 +88,12 @@ static int32_t init(stateFilter *self)
this->vel[0] = 0.0f;
this->vel[1] = 0.0f;
this->vel[2] = 0.0f;
this->dTA = -1;
this->dTA2 = -1;
this->dTA = -1.0f;
this->dTA2 = -1.0f;
this->baroLast = 0.0f;
this->accelLast = 0.0f;
this->first_run = 1;
AltitudeFilterSettingsGet(&this->settings);
return 0;
}
@ -104,8 +109,6 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
this->first_run = 0;
this->baroLastTime = PIOS_DELAY_GetRaw();
this->state[0] = state->baro[0];
this->baroLast = state->baro[0];
}
} else {
// save existing position and velocity updates so GPS will still work
@ -130,7 +133,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
float current = -(Rbe[0][2] * state->accel[0] + Rbe[1][2] * state->accel[1] + Rbe[2][2] * state->accel[2] + 9.81f);
// correct accel offset (low pass zeroing)
this->state[2] = (1.0f - ACCEL_DRIFT_KP) * this->state[2] + ACCEL_DRIFT_KP * current;
this->state[2] = (1.0f - this->settings.AccelDriftKi) * this->state[2] + this->settings.AccelDriftKi * current;
// correct velocity and position state (integration)
// low pass for average dT, compensate timing jitter from scheduler
@ -144,8 +147,13 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
} else {
this->dTA = this->dTA * (1.0f - DT_ALPHA) + dT * DT_ALPHA;
}
this->state[1] += (current - this->state[1]) * this->dTA;
this->state[0] += this->state[1] * this->dTA;
float speedLast = this->state[1];
this->state[1] += 0.5f * (this->accelLast + (current - this->state[2])) * this->dTA;
this->accelLast = current - this->state[2];
this->state[0] += 0.5f * (speedLast + this->state[1]) * this->dTA;
state->pos[0] = this->pos[0];
state->pos[1] = this->pos[1];
@ -159,7 +167,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
}
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
// correct the altitude state (simple low pass)
this->state[0] = (1.0f - BARO_SENSOR_KP) * this->state[0] + BARO_SENSOR_KP * state->baro[0];
this->state[0] = (1.0f - this->settings.BaroKp) * this->state[0] + this->settings.BaroKp * state->baro[0];
// correct the velocity state (low pass differentiation)
// low pass for average dT, compensate timing jitter from scheduler
@ -173,7 +181,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
} else {
this->dTA2 = this->dTA2 * (1.0f - DT_ALPHA) + dT * DT_ALPHA;
}
this->state[1] = (1.0f - (BARO_SENSOR_KP * BARO_SENSOR_KP)) * this->state[1] + (BARO_SENSOR_KP * BARO_SENSOR_KP) * (state->baro[0] - this->baroLast) / this->dTA2;
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;
this->baroLast = state->baro[0];
state->pos[0] = this->pos[0];

15
flight/modules/StateEstimation/filterbaro.c
View File

@ -44,9 +44,9 @@
// Private types
struct data {
float baroOffset;
float baroAlt;
bool first_run;
float baroOffset;
float baroAlt;
int16_t first_run;
};
// Private variables
@ -70,7 +70,7 @@ static int32_t init(stateFilter *self)
struct data *this = (struct data *)self->localdata;
this->baroOffset = 0.0f;
this->first_run = 1;
this->first_run = 100;
return 0;
}
@ -81,9 +81,10 @@ 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_baro)) {
this->first_run = 0;
this->baroOffset = state->baro[0];
this->baroAlt = state->baro[0];
this->baroOffset = (100.f - this->first_run) / 100.f * this->baroOffset + (this->first_run / 100.f) * state->baro[0];
this->baroAlt = this->baroOffset;
this->first_run--;
UNSET_MASK(state->updated, SENSORUPDATES_baro);
}
} else {
// Track barometric altitude offset with a low pass filter

3
flight/targets/boards/revolution/firmware/UAVObjects.inc
View File

@ -89,7 +89,8 @@ UAVOBJSRCFILENAMES += hwsettings
UAVOBJSRCFILENAMES += receiveractivity
UAVOBJSRCFILENAMES += cameradesired
UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += altitudefiltersettings
UAVOBJSRCFILENAMES += altitudeholddesired
UAVOBJSRCFILENAMES += waypoint
UAVOBJSRCFILENAMES += waypointactive

3
flight/targets/boards/revoproto/firmware/UAVObjects.inc
View File

@ -94,7 +94,8 @@ UAVOBJSRCFILENAMES += hwsettings
UAVOBJSRCFILENAMES += receiveractivity
UAVOBJSRCFILENAMES += cameradesired
UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += altitudefiltersettings
UAVOBJSRCFILENAMES += altitudeholddesired
UAVOBJSRCFILENAMES += waypoint
UAVOBJSRCFILENAMES += waypointactive

1
flight/targets/boards/simposix/firmware/UAVObjects.inc
View File

@ -95,6 +95,7 @@ UAVOBJSRCFILENAMES += receiveractivity
UAVOBJSRCFILENAMES += cameradesired
UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += altitudefiltersettings
UAVOBJSRCFILENAMES += revosettings
UAVOBJSRCFILENAMES += altitudeholddesired
UAVOBJSRCFILENAMES += ekfconfiguration

2
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -33,6 +33,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/altholdsmoothed.h \
$$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \
$$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \
$$UAVOBJECT_SYNTHETICS/altitudefiltersettings.h \
$$UAVOBJECT_SYNTHETICS/ekfconfiguration.h \
$$UAVOBJECT_SYNTHETICS/ekfstatevariance.h \
$$UAVOBJECT_SYNTHETICS/revocalibration.h \
@ -118,6 +119,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/altholdsmoothed.cpp \
$$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \
$$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \
$$UAVOBJECT_SYNTHETICS/altitudefiltersettings.cpp \
$$UAVOBJECT_SYNTHETICS/ekfconfiguration.cpp \
$$UAVOBJECT_SYNTHETICS/ekfstatevariance.cpp \
$$UAVOBJECT_SYNTHETICS/revocalibration.cpp \

11
shared/uavobjectdefinition/altitudefiltersettings.xml Normal file
View File

@ -0,0 +1,11 @@
<xml>
<object name="AltitudeFilterSettings" singleinstance="true" settings="true" category="State">
<description>Settings for the @ref State Estimator module plugin altitudeFilter</description>
<field name="AccelDriftKi" units="m/s^2" type="float" elements="1" defaultvalue="0.0005"/>
<field name="BaroKp" units="m" type="float" elements="1" defaultvalue="0.02"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>