From 297adc4e1e35cc6da512c35e8e6a0f6cc539dd25 Mon Sep 17 00:00:00 2001 From: Tobias Antonsson Date: Wed, 12 Apr 2017 17:18:53 +0200 Subject: [PATCH] #211 Fixed altitude hold broken by prev commit. --- src/modules/src/position_estimator_altitude.c | 11 ++++++++--- src/modules/src/stabilizer.c | 4 +++- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/src/modules/src/position_estimator_altitude.c b/src/modules/src/position_estimator_altitude.c index e5916fc68e..b905a70ec5 100644 --- a/src/modules/src/position_estimator_altitude.c +++ b/src/modules/src/position_estimator_altitude.c @@ -82,9 +82,14 @@ static void positionEstimateInternal(state_t* estimate, const sensorData_t* sens state->estimatedZ = filteredZ + (state->velocityFactor * state->velocityZ * dt); } } else { - // IIR filter asl - filteredZ = (state->estAlphaAsl ) * state->estimatedZ + - (1.0f - state->estAlphaAsl) * sensorData->baro.asl; + // FIXME: A bit of an hack to init IIR filter + if (state->estimatedZ == 0.0f) { + filteredZ = sensorData->baro.asl; + } else { + // IIR filter asl + filteredZ = (state->estAlphaAsl ) * state->estimatedZ + + (1.0f - state->estAlphaAsl) * sensorData->baro.asl; + } // Use asl as base and add velocity changes. state->estimatedZ = filteredZ + (state->velocityFactor * state->velocityZ * dt); } diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index 89bd2c1e80..ccc83e6319 100644 --- a/src/modules/src/stabilizer.c +++ b/src/modules/src/stabilizer.c @@ -95,7 +95,7 @@ bool stabilizerTest(void) static void stabilizerTask(void* param) { - uint32_t tick = 0; + uint32_t tick; uint32_t lastWakeTime; vTaskSetApplicationTaskTag(0, (void*)TASK_STABILIZER_ID_NBR); @@ -107,6 +107,8 @@ static void stabilizerTask(void* param) while(!sensorsAreCalibrated()) { vTaskDelayUntil(&lastWakeTime, F2T(RATE_MAIN_LOOP)); } + // Initialize tick to something else then 0 + tick = 1; while(1) { vTaskDelayUntil(&lastWakeTime, F2T(RATE_MAIN_LOOP));