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 next into 'corvuscorax/OP-1296_barometer-initialization-fix'

Browse Source
This commit is contained in:
Corvus Corax 2014-04-08 18:42:33 +02:00
commit cb16cf04e1
5 changed files with 83 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/modules/Attitude/revolution/attitude.c
View File

@ -278,7 +278,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY:
ret_val = updateAttitudeComplementary(first_run);
break;
case REVOSETTINGS_FUSIONALGORITHM_INS13OUTDOOR:
case REVOSETTINGS_FUSIONALGORITHM_INS13GPSOUTDOOR:
ret_val = updateAttitudeINSGPS(first_run, true);
break;
case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR:

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

@ -36,7 +36,7 @@
// Private constants
#define STACK_REQUIRED 64
#define STACK_REQUIRED 128
#define INIT_CYCLES 100
// Private types
@ -44,30 +44,59 @@ struct data {
float baroOffset;
float baroGPSOffsetCorrectionAlpha;
float baroAlt;
float gpsAlt;
int16_t first_run;
bool useGPS;
};
// Private variables
// Private functions
static int32_t init(stateFilter *self);
static int32_t initwithgps(stateFilter *self);
static int32_t initwithoutgps(stateFilter *self);
static int32_t maininit(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
int32_t filterBaroInitialize(stateFilter *handle)
{
handle->init = &init;
handle->init = &initwithgps;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
}
static int32_t init(stateFilter *self)
int32_t filterBaroiInitialize(stateFilter *handle)
{
handle->init = &initwithoutgps;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
}
static int32_t initwithgps(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->useGPS = 1;
return maininit(self);
}
static int32_t initwithoutgps(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->useGPS = 0;
return maininit(self);
}
static int32_t maininit(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->baroOffset = 0.0f;
this->gpsAlt = 0.0f;
this->first_run = INIT_CYCLES;
RevoSettingsInitialize();
@ -81,17 +110,29 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
struct data *this = (struct data *)self->localdata;
if (this->first_run) {
// Make sure initial location is initialized properly before continuing
if (this->useGPS && IS_SET(state->updated, SENSORUPDATES_pos)) {
if (this->first_run == INIT_CYCLES) {
this->gpsAlt = state->pos[2];
this->first_run--;
}
}
// Initialize to current altitude reading at initial location
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
this->baroOffset = ((float)(INIT_CYCLES)-this->first_run) / (float)(INIT_CYCLES)*this->baroOffset + (this->first_run / (float)(INIT_CYCLES)) * state->baro[0];
this->baroAlt = 0;
this->first_run--;
if (this->first_run < INIT_CYCLES || !this->useGPS) {
this->baroOffset = (((float)(INIT_CYCLES)-this->first_run) / (float)(INIT_CYCLES)) * this->baroOffset + (this->first_run / (float)(INIT_CYCLES)) * (state->baro[0] + this->gpsAlt);
this->baroAlt = state->baro[0];
this->first_run--;
}
UNSET_MASK(state->updated, SENSORUPDATES_baro);
}
// make sure we raise an error until properly initialized - would not be good if people arm and
// use altitudehold without initialized barometer filter
return 2;
} else {
// Track barometric altitude offset with a low pass filter
// based on GPS altitude if available
if (IS_SET(state->updated, SENSORUPDATES_pos)) {
if (this->useGPS && IS_SET(state->updated, SENSORUPDATES_pos)) {
this->baroOffset = this->baroOffset * this->baroGPSOffsetCorrectionAlpha +
(1.0f - this->baroGPSOffsetCorrectionAlpha) * (this->baroAlt + state->pos[2]);
}
@ -100,9 +141,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
this->baroAlt = state->baro[0];
state->baro[0] -= this->baroOffset;
}
return 0;
}
return 0;
}

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

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

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

@ -145,6 +145,7 @@ static filterPipeline *filterChain = NULL;
// different filters available to state estimation
static stateFilter magFilter;
static stateFilter baroFilter;
static stateFilter baroiFilter;
static stateFilter altitudeFilter;
static stateFilter airFilter;
static stateFilter stationaryFilter;
@ -160,15 +161,28 @@ static filterPipeline *cfQueue = &(filterPipeline) {
.next = &(filterPipeline) {
.filter = &airFilter,
.next = &(filterPipeline) {
.filter = &llaFilter,
.filter = &baroiFilter,
.next = &(filterPipeline) {
.filter = &baroFilter,
.filter = &altitudeFilter,
.next = &(filterPipeline) {
.filter = &altitudeFilter,
.next = &(filterPipeline) {
.filter = &cfFilter,
.next = NULL,
}
.filter = &cfFilter,
.next = NULL,
}
}
}
}
};
static const filterPipeline *cfmiQueue = &(filterPipeline) {
.filter = &magFilter,
.next = &(filterPipeline) {
.filter = &airFilter,
.next = &(filterPipeline) {
.filter = &baroiFilter,
.next = &(filterPipeline) {
.filter = &altitudeFilter,
.next = &(filterPipeline) {
.filter = &cfmFilter,
.next = NULL,
}
}
}
@ -198,15 +212,12 @@ static const filterPipeline *ekf13iQueue = &(filterPipeline) {
.next = &(filterPipeline) {
.filter = &airFilter,
.next = &(filterPipeline) {
.filter = &llaFilter,
.filter = &baroiFilter,
.next = &(filterPipeline) {
.filter = &baroFilter,
.filter = &stationaryFilter,
.next = &(filterPipeline) {
.filter = &stationaryFilter,
.next = &(filterPipeline) {
.filter = &ekf13iFilter,
.next = NULL,
}
.filter = &ekf13iFilter,
.next = NULL,
}
}
}
@ -283,6 +294,7 @@ int32_t StateEstimationInitialize(void)
uint32_t stack_required = STACK_SIZE_BYTES;
// Initialize Filters
stack_required = maxint32_t(stack_required, filterMagInitialize(&magFilter));
stack_required = maxint32_t(stack_required, filterBaroiInitialize(&baroiFilter));
stack_required = maxint32_t(stack_required, filterBaroInitialize(&baroFilter));
stack_required = maxint32_t(stack_required, filterAltitudeInitialize(&altitudeFilter));
stack_required = maxint32_t(stack_required, filterAirInitialize(&airFilter));
@ -361,12 +373,15 @@ static void StateEstimationCb(void)
newFilterChain = cfQueue;
break;
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAG:
newFilterChain = cfmiQueue;
break;
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR:
newFilterChain = cfmQueue;
break;
case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR:
newFilterChain = ekf13iQueue;
break;
case REVOSETTINGS_FUSIONALGORITHM_INS13OUTDOOR:
case REVOSETTINGS_FUSIONALGORITHM_INS13GPSOUTDOOR:
newFilterChain = ekf13Queue;
break;
default:

2
shared/uavobjectdefinition/revosettings.xml
View File

@ -1,7 +1,7 @@
<xml>
<object name="RevoSettings" singleinstance="true" settings="true" category="State">
<description>Settings for the revo to control the algorithm and what is updated</description>
<field name="FusionAlgorithm" units="" type="enum" elements="1" options="None,Complementary,Complementary+Mag,INS13Indoor,INS13Outdoor" defaultvalue="Complementary"/>
<field name="FusionAlgorithm" units="" type="enum" elements="1" options="None,Complementary,Complementary+Mag,Complementary+Mag+GPSOutdoor,INS13Indoor,INS13GPSOutdoor" defaultvalue="Complementary"/>
<!-- Low pass filter configuration to calculate offset of barometric altitude sensor.
Defaults: updates at 5 Hz, tau = 300s settle time, exp(-(1/f)/tau) ~= 0.9993335555062