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;
}
Example #2
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;
    }
}