MC: Fix yaw jump when switching from stabilized to another mode

[bresch]

Solved Problem

When switching from a flight mode that is not a flight task (e.g.: stabilized). In this case, the reset counters were initialized to 0 and deltas were applied to the first setpoints if the EKF had any of its reset counters different from 0.

In the plot below, we can see a “delta_heading” of 89 degrees. Adding 89 to the current heading of 70 gives the setpoint of 159 degrees.

Solution

Initialize the reset counters when starting the task instead of passing the ones of the previous task.

Test coverage

SITL tests

[dagar]

The change looks fine, I just wish the dependency on updated vehicle_local_position was a little more clear.

[dagar]

I think this would be a lot clearer if all combined with the vehicle_local_position update here.

PX4-Autopilot/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp

Line 44 in 86465c3

_sub_vehicle_local_position.update();

We could perform both initEkfResetCounters() (if uninitialized) and _checkEkfResetCounters()

[MaEtUgR]

Aaah 🤦‍♂️ If you didn't have a previous flight task the reset counters would get initialized with zero and then the "next" flight task would assume there was a change compared to the non-existing previous one. Did we really overlook that pretty basic case in review here:
#14692

You're basically reverting ca96289

Are we sure the originally solved problem doesn't come back with this change?
Why did we not find this issue earlier? It should be obvious on any switch from e.g. Manual to Position mode 🤔
I'm just trying to avoid any other side effects. Thanks for looking into the problem!