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_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--;
UNSET_MASK(state->updated, SENSORUPDATES_baro);
}
} else {
// Track barometric altitude offset with a low pass filter
// based on GPS altitude if available
if (IS_SET(state->updated, SENSORUPDATES_pos)) {
this->baroOffset = this->baroOffset * this->baroGPSOffsetCorrectionAlpha +
(1.0f - this->baroGPSOffsetCorrectionAlpha) * (this->baroAlt + state->pos[2]);
}
// calculate bias corrected altitude
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
this->baroAlt = state->baro[0];
state->baro[0] -= this->baroOffset;
}
}
return 0;
}
static filterResult 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)) {
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
// Here, the filter is not initialized, return ERROR
return FILTERRESULT_CRITICAL;
} else {
// Track barometric altitude offset with a low pass filter
// based on GPS altitude if available
if (this->useGPS && IS_SET(state->updated, SENSORUPDATES_pos)) {
this->baroOffset = this->baroOffset * this->baroGPSOffsetCorrectionAlpha +
(1.0f - this->baroGPSOffsetCorrectionAlpha) * (this->baroAlt + state->pos[2]);
}
// calculate bias corrected altitude
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
this->baroAlt = state->baro[0];
state->baro[0] -= this->baroOffset;
}
return FILTERRESULT_OK;
}
}
