diff --git a/flight/modules/StateEstimation/filteraltitude.c b/flight/modules/StateEstimation/filteraltitude.c index dc1cbb314..e04561731 100644 --- a/flight/modules/StateEstimation/filteraltitude.c +++ b/flight/modules/StateEstimation/filteraltitude.c @@ -32,6 +32,7 @@ #include "inc/stateestimation.h" #include +#include #include @@ -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]; diff --git a/flight/modules/StateEstimation/filterbaro.c b/flight/modules/StateEstimation/filterbaro.c index 7c5793717..e31454c70 100644 --- a/flight/modules/StateEstimation/filterbaro.c +++ b/flight/modules/StateEstimation/filterbaro.c @@ -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 diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 188b15bab..3e653692c 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -89,7 +89,8 @@ UAVOBJSRCFILENAMES += hwsettings UAVOBJSRCFILENAMES += receiveractivity UAVOBJSRCFILENAMES += cameradesired UAVOBJSRCFILENAMES += camerastabsettings -UAVOBJSRCFILENAMES += altitudeholdsettings +UAVOBJSRCFILENAMES += altitudeholdsettings +UAVOBJSRCFILENAMES += altitudefiltersettings UAVOBJSRCFILENAMES += altitudeholddesired UAVOBJSRCFILENAMES += waypoint UAVOBJSRCFILENAMES += waypointactive diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index 996708520..ff47a51f2 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -94,7 +94,8 @@ UAVOBJSRCFILENAMES += hwsettings UAVOBJSRCFILENAMES += receiveractivity UAVOBJSRCFILENAMES += cameradesired UAVOBJSRCFILENAMES += camerastabsettings -UAVOBJSRCFILENAMES += altitudeholdsettings +UAVOBJSRCFILENAMES += altitudeholdsettings +UAVOBJSRCFILENAMES += altitudefiltersettings UAVOBJSRCFILENAMES += altitudeholddesired UAVOBJSRCFILENAMES += waypoint UAVOBJSRCFILENAMES += waypointactive diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 58ad986ca..d9dcd6116 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -95,6 +95,7 @@ UAVOBJSRCFILENAMES += receiveractivity UAVOBJSRCFILENAMES += cameradesired UAVOBJSRCFILENAMES += camerastabsettings UAVOBJSRCFILENAMES += altitudeholdsettings +UAVOBJSRCFILENAMES += altitudefiltersettings UAVOBJSRCFILENAMES += revosettings UAVOBJSRCFILENAMES += altitudeholddesired UAVOBJSRCFILENAMES += ekfconfiguration diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index a4c5c51d7..62f4fc937 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -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 \ diff --git a/shared/uavobjectdefinition/altitudefiltersettings.xml b/shared/uavobjectdefinition/altitudefiltersettings.xml new file mode 100644 index 000000000..9f5bd873f --- /dev/null +++ b/shared/uavobjectdefinition/altitudefiltersettings.xml @@ -0,0 +1,11 @@ + + + Settings for the @ref State Estimator module plugin altitudeFilter + + + + + + + +