ekf2: mag control cleanup mag reset

[dagar]

current/old

• if yaw estimator available
  • reset heading to yaw estimator
  • use WMM to reset mag_I
  • use WMM (and new yaw estimator heading) to reset mag_B
• otherwise reset mag_B to zero and mag_I to mag field

PR

• always use WMM to reset mag_I if available
• if yaw estimator available use it to reset mag_B (so that existing mag will produce desired heading)
• perform resetMagHeading() (using mag bias if available)

This was broken out of #21212 (somewhat).

[Sidzeppelin95]

not sure about the first one will have to look through the log once more but for
./platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/cuav_x7pro_test/cuav_x7pro_test.elf || true
probably the stack has identical or similar objects (could be just names or the object itself). Could also be a deleted object. You can add an exception while tracing this track for similar objects or delete the identical one altogether if it is not related to anything important that's running.

[dagar]

@Sidzeppelin95 I don't think I'm following what you're saying, but can you please open a new dedicated issue and add more detail?

[bresch]

Squashed, rebased and f-puched

[dagar]

@bresch we need to take a closer look (large change indication diff).

[bresch]

I had a look at the diffs in the change indicator; they are related to the earth field states and with the prints I found what's happening:
With this PR, the mag field states are initialized using the WMM if available (which make sense). However, the GNSS coordinates of the change indicator flight lead to an inconsistent inclination

reset mag heading 0.374 -> 0.376 rad (bias:[0.000, 0.000, 0.000], declination:0.0)
reset to WMM. decl: 3.917351 deg, incl: 0.573786 deg
meas inc = 64.794960 deg
resetting mag I [0.203, 0.002, 0.435] -> [0.367, 0.004, 0.025]

In summary, the earth field state is different in the replay because the measured field doesn't match the WMM (or the GNSS coordinates are incorrect).

[bresch]

If you change this line by

_state.mag_I = _R_to_earth * mag;

the whole change indicator diff disappears. So I'm pretty sure it's all good.

[bresch]

Added unit test for mag earth field reset to WMM. Will add an additional check on the inclination in a next PR.