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

Merge branch 'corvuscorax/complementary_altitude_filter' into next

Browse Source 
Conflicts:
	flight/targets/boards/revolution/firmware/UAVObjects.inc
This commit is contained in:
Corvus Corax 2013-07-10 14:27:25 +02:00
commit 551780c74b
9 changed files with 250 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

210
flight/modules/StateEstimation/filteraltitude.c Normal file
View File

@ -0,0 +1,210 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup State Estimation
* @brief Acquires sensor data and computes state estimate
* @{
*
* @file filteraltitude.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
* @brief Barometric altitude filter, calculates vertical speed and true
* altitude based on Barometric altitude and accelerometers
*
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "inc/stateestimation.h"
#include <attitudestate.h>
#include <altitudefiltersettings.h>
#include <CoordinateConversions.h>
// Private constants
#define STACK_REQUIRED 128
#define DT_ALPHA 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;
AltitudeFilterSettingsData settings;
};
// Private variables
// Private functions
static int32_t init(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
int32_t filterAltitudeInitialize(stateFilter *handle)
{
handle->init = &init;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
AttitudeStateInitialize();
AltitudeFilterSettingsInitialize();
return STACK_REQUIRED;
}
static int32_t init(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->state[0] = 0.0f;
this->state[1] = 0.0f;
this->state[2] = 0.0f;
this->state[3] = 0.0f;
this->pos[0] = 0.0f;
this->pos[1] = 0.0f;
this->pos[2] = 0.0f;
this->vel[0] = 0.0f;
this->vel[1] = 0.0f;
this->vel[2] = 0.0f;
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;
}
static int32_t filter(stateFilter *self, stateEstimation *state)
{
struct data *this = (struct data *)self->localdata;
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();
}
} else {
// save existing position and velocity updates so GPS will still work
if (IS_SET(state->updated, SENSORUPDATES_pos)) {
this->pos[0] = state->pos[0];
this->pos[1] = state->pos[1];
this->pos[2] = state->pos[2];
state->pos[2] = -this->state[0];
}
if (IS_SET(state->updated, SENSORUPDATES_vel)) {
this->vel[0] = state->vel[0];
this->vel[1] = state->vel[1];
this->vel[2] = state->vel[2];
state->vel[2] = -this->state[1];
}
if (IS_SET(state->updated, SENSORUPDATES_accel)) {
// rotate accels into global coordinate frame
AttitudeStateData att;
AttitudeStateGet(&att);
float Rbe[3][3];
Quaternion2R(&att.q1, Rbe);
float current = -(Rbe[0][2] * state->accel[0] + Rbe[1][2] * state->accel[1] + Rbe[2][2] * state->accel[2] + 9.81f);
// low pass filter accelerometers
this->state[3] = (1.0f - this->settings.AccelLowPassKp) * this->state[3] + this->settings.AccelLowPassKp * current;
// correct accel offset (low pass zeroing)
this->state[2] = (1.0f - this->settings.AccelDriftKi) * this->state[2] + this->settings.AccelDriftKi * this->state[3];
// 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 speedLast = this->state[1];
this->state[1] += 0.5f * (this->accelLast + (this->state[3] - this->state[2])) * this->dTA;
this->accelLast = this->state[3] - 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];
state->pos[2] = -this->state[0];
state->updated |= SENSORUPDATES_pos;
state->vel[0] = this->vel[0];
state->vel[1] = this->vel[1];
state->vel[2] = -this->state[1];
state->updated |= SENSORUPDATES_vel;
}
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
// correct the altitude state (simple low pass)
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
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;
this->baroLast = state->baro[0];
state->pos[0] = this->pos[0];
state->pos[1] = this->pos[1];
state->pos[2] = -this->state[0];
state->updated |= SENSORUPDATES_pos;
state->vel[0] = this->vel[0];
state->vel[1] = this->vel[1];
state->vel[2] = -this->state[1];
state->updated |= SENSORUPDATES_vel;
}
}
return 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

1
flight/modules/StateEstimation/inc/stateestimation.h
View File

@ -64,6 +64,7 @@ typedef struct stateFilterStruct {
int32_t filterMagInitialize(stateFilter *handle);
int32_t filterBaroInitialize(stateFilter *handle);
int32_t filterAltitudeInitialize(stateFilter *handle);
int32_t filterAirInitialize(stateFilter *handle);
int32_t filterStationaryInitialize(stateFilter *handle);
int32_t filterCFInitialize(stateFilter *handle);

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

@ -123,6 +123,7 @@ static filterPipeline *filterChain = NULL;
// different filters available to state estimation
static stateFilter magFilter;
static stateFilter baroFilter;
static stateFilter altitudeFilter;
static stateFilter airFilter;
static stateFilter stationaryFilter;
static stateFilter cfFilter;
@ -138,9 +139,12 @@ static filterPipeline *cfQueue = &(filterPipeline) {
.next = &(filterPipeline) {
.filter = &baroFilter,
.next = &(filterPipeline) {
.filter = &cfFilter,
.next = NULL,
},
.filter = &altitudeFilter,
.next = &(filterPipeline) {
.filter = &cfFilter,
.next = NULL,
}
}
}
}
};
@ -151,8 +155,11 @@ static const filterPipeline *cfmQueue = &(filterPipeline) {
.next = &(filterPipeline) {
.filter = &baroFilter,
.next = &(filterPipeline) {
.filter = &cfmFilter,
.next = NULL,
.filter = &altitudeFilter,
.next = &(filterPipeline) {
.filter = &cfmFilter,
.next = NULL,
}
}
}
}
@ -237,6 +244,7 @@ int32_t StateEstimationInitialize(void)
// Initialize Filters
stack_required = maxint32_t(stack_required, filterMagInitialize(&magFilter));
stack_required = maxint32_t(stack_required, filterBaroInitialize(&baroFilter));
stack_required = maxint32_t(stack_required, filterAltitudeInitialize(&altitudeFilter));
stack_required = maxint32_t(stack_required, filterAirInitialize(&airFilter));
stack_required = maxint32_t(stack_required, filterStationaryInitialize(&stationaryFilter));
stack_required = maxint32_t(stack_required, filterCFInitialize(&cfFilter));

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

@ -93,7 +93,7 @@ UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += oplinksettings
UAVOBJSRCFILENAMES += oplinkstatus
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 \
@ -119,6 +120,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 \

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

@ -0,0 +1,12 @@
<xml>
<object name="AltitudeFilterSettings" singleinstance="true" settings="true" category="State">
<description>Settings for the @ref State Estimator module plugin altitudeFilter</description>
<field name="AccelLowPassKp" units="m/s^2" type="float" elements="1" defaultvalue="0.07"/>
<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>