FlightTask: Fix ekf2 reset race condition during task switch (#14692)

* FlightTask: Fix ekf2 reset race condition during task switch

During a loss of GPS data when using GPS as primary height source,
the height is reset to baro and the local position gets invalid at the
same time. This triggers a switch to altitude flight task and a setpoint
reset.
This combination of events had the effect to ignore the height reset,
the large sudden height error could create an abrupt change of altitude
or even a crash.
The ekf2 reset is now done at the beginning of each update call.

src/lib/flight_tasks/FlightTasks.cpp

@@ -36,6 +36,16 @@ const vehicle_local_position_setpoint_s FlightTasks::getPositionSetpoint()
 	}
 }
 
+const ekf_reset_counters_s FlightTasks::getResetCounters()
+{
+	if (isAnyTaskActive()) {
+		return _current_task.task->getResetCounters();
+
+	} else {
+		return FlightTask::zero_reset_counters;
+	}
+}
+
 const vehicle_constraints_s FlightTasks::getConstraints()
 {
 	if (isAnyTaskActive()) {
@@ -64,7 +74,8 @@ FlightTaskError FlightTasks::switchTask(FlightTaskIndex new_task_index)
 	}
 
 	// Save current setpoints for the next FlightTask
-	vehicle_local_position_setpoint_s last_setpoint = getPositionSetpoint();
+	const vehicle_local_position_setpoint_s last_setpoint = getPositionSetpoint();
+	const ekf_reset_counters_s last_reset_counters = getResetCounters();
 
 	if (_initTask(new_task_index)) {
 		// invalid task
@@ -84,6 +95,8 @@ FlightTaskError FlightTasks::switchTask(FlightTaskIndex new_task_index)
 		return FlightTaskError::ActivationFailed;
 	}
 
+	_current_task.task->setResetCounters(last_reset_counters);
+
 	return FlightTaskError::NoError;
 }