AC_PrecLand: Use sensor timestamp to match inertial frame corrections

[amilcarlucas]

This is a rebase and cleanup of @fnoop #9020

If the sensor sets the timestamp with the sensor frame then it attempts to match it against inertial frame timestamps. Sensor frames can have widely varying latencies so this is essential to safely correlate the attitude compensation against any given sensor reading. This effectively auto-detects the sensor latency and makes the PLND_LAG manual setting unnecessary. The only caveat is that PLND_LAG must be set large enough to hold the biggest sensor latency. So if used with a sensor that supports timestamping PLND_LAG should be set to something large like 0.250 (s).

• Move inertial_data_delayed to _inertial_data_delayed
• Separate out sync_frames logic, make _inertial_data_delayed class struct
• Run forward prediction from synced frame
• Remove _ prefixes in non-member (local) variable's names

Binary Name      Text [B]        Data [B]     BSS (B)       Total Flash Change [B] (%)      Flash Free After PR (B)
---------------  --------------  -----------  ------------  ----------------------------  -------------------------
ardurover        124 (+0.0076%)  0 (0.0000%)  4 (+0.0015%)  124 (+0.0076%)                                   337212
blimp            0 (0.0000%)     0 (0.0000%)  0 (0.0000%)   0 (0.0000%)                                      623072
arducopter       120 (+0.0068%)  0 (0.0000%)  0 (0.0000%)   120 (+0.0068%)                                   192868
arduplane        0 (0.0000%)     0 (0.0000%)  0 (0.0000%)   0 (0.0000%)                                      201296
ardusub          0 (0.0000%)     0 (0.0000%)  0 (0.0000%)   0 (0.0000%)                                      400608
antennatracker   0 (0.0000%)     0 (0.0000%)  0 (0.0000%)   0 (0.0000%)                                      645948
arducopter-heli  128 (+0.0072%)  0 (0.0000%)  0 (0.0000%)   128 (+0.0072%)                                   186692

[rishabsingh3003]

I like the idea of this PR! Has this had any testing where this PR improved the performance? Like a before vs after comparison

[amilcarlucas]

No testing has been done yet. I would like to get feedback though.

[fnoop]

Thanks @amilcarlucas for rebasing this and pushing it forward, I had given up hope :) I tested it at least in SITL, if not real life - so long ago I can't remember. I tested all of my PRs extensively but this was the last bit of work and can't remember how well it was tested unfortunately. Certainly the frames were matching correctly on unstable frequency inputs which was a huge instability factor in PrecLand. The fixed lag parameter was a good step forward but this PR is really needed for proper stability, particularly on slower/non-realtime platforms or those with lots of other stuff running.

[amilcarlucas]

Rebased and solved conflicts again. Any feedback?

[tridge]

@rishabsingh3003 need your input on this one

[amilcarlucas]

I have updated the size comparison on top of this PR

[amilcarlucas]

This PR was started by @khancyr , later picked up by @fnoop and later on by me. It has been around by many years, but it is now a lot shorter and easier to review.

Unfortunately right now I do not have a vehicle that I can test this with, so it would be nice if someone with a companion computer and visual fiducial markers would test this.

[amilcarlucas]

@rishabsingh3003 the other 2 precland PRs from me got merged, only this one remains and it got a lot smaller now. Can you review this?