#211 Fixed altitude hold broken by prev commit.

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);
   }

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));