AC_PrecLand: Use sensor timestamp to match inertial frame corrections #18548

amilcarlucas · 2021-09-02T14:42:51Z

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        116 (+0.0071%)  0 (0.0000%)  4 (+0.0015%)   116 (+0.0071%)                                   335640
blimp            0 (0.0000%)     0 (0.0000%)  0 (0.0000%)    0 (0.0000%)                                      620052
arducopter       116 (+0.0065%)  0 (0.0000%)  -4 (-0.0015%)  116 (+0.0065%)                                   190732
arduplane        116 (+0.0066%)  0 (0.0000%)  -4 (-0.0015%)  116 (+0.0065%)                                   191404
ardusub          0 (0.0000%)     0 (0.0000%)  0 (0.0000%)    0 (0.0000%)                                      397076
antennatracker   0 (0.0000%)     0 (0.0000%)  0 (0.0000%)    0 (0.0000%)                                      642736
arducopter-heli  116 (+0.0065%)  0 (0.0000%)  -4 (-0.0015%)  116 (+0.0065%)                                   184300

rishabsingh3003 · 2021-09-02T14:52:39Z

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 · 2021-09-02T15:13:12Z

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

fnoop · 2021-09-04T17:55:45Z

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 · 2024-02-13T15:30:03Z

Rebased and solved conflicts again. Any feedback?

tridge · 2024-02-14T08:42:24Z

@rishabsingh3003 need your input on this one

tridge · 2024-02-14T08:42:57Z

libraries/AC_PrecLand/AC_PrecLand.cpp

+    if (los_meas_time_us != 0) {
+        // Search through inertial history for the nearest frame to the measurement timestamp
+        for (uint8_t i=0; i<_inertial_history->available(); i++) {
+            uint64_t delta_usec = labs((*_inertial_history)[i]->time_usec - los_meas_time_us);


is there a time wrap issue here?

los_meas_time_ms will wrap after 71 days, and time_usec does not wrap, so there is a wrap issue

I solved that now and unit tested it with the script below.

tridge · 2024-02-14T08:45:32Z

libraries/AC_PrecLand/AC_PrecLand.cpp

+    if (los_meas_time_us != 0) {
+        // Search through inertial history for the nearest frame to the measurement timestamp
+        for (uint8_t i=0; i<_inertial_history->available(); i++) {
+            uint64_t delta_usec = labs((*_inertial_history)[i]->time_usec - los_meas_time_us);


los_meas_time_ms will wrap after 71 days, and time_usec does not wrap, so there is a wrap issue

rishabsingh3003

I assume you will remove all the commits that will go with #26238. That will make this PR much simpler.
Specifically the "sync_frame" changes look correct and will help the companion computer backend most likely. However, some amount of testing, atleast in SITL, is warranted before we add more complexity. Also the time-wrap bit looks mostly okay, but I would want to double check if that is being handled correctly.
We can potentially modify the SITL prec land backend to trivially test this PR.

rishabsingh3003 · 2024-02-19T07:10:31Z

libraries/AC_PrecLand/AC_PrecLand.cpp

+        for (uint8_t i=0; i<_inertial_history->available(); i++) {
+            uint64_t delta_us;
+            uint64_t ih_point_us = (*_inertial_history)[i]->time_usec; // overflows after approximately 584 554.531 years
+            while (ih_point_us >= (UINT32_MAX + 1UL) * 1000UL) {


A comment explaining that this is needed because of the comparison with "ms" time stamp given by the backend would be helpful

Why does the timestamp of the companion Computer Precision landing Vision messages has a half a second delay when compared with the IMU timestamps? It causes problems when comparing the timestamps in the precision landing companion autotest.
I would like to fix that, but I need help. Does this have to do with TIME_SYNC messages? Is the delay in there purposeful?

Yes, this is a followup of #26238. I moved the NFCish things into that one.

I added the comment that you asked.

The wrap code has been tested with this:

#!/bin/env python UINT32_MAX = 2**32 - 1 def compare_timestamps(los_meas_time_ms, ih_point_us): """ Compare two timestamps and calculate the difference in microseconds, accounting for 32-bit wrap-around. Args: los_meas_time_ms (int): The first timestamp in milliseconds. ih_point_us (int): The second timestamp in microseconds. Returns: int: The difference between the two timestamps in microseconds. Note: This function assumes that the timestamps are close to each other and handles the case where the difference exceeds the maximum value of a 32-bit unsigned integer. """ # simulate 32bit wrap in python while los_meas_time_ms >= (UINT32_MAX + 1): los_meas_time_ms -= (UINT32_MAX + 1) los_meas_time_us = los_meas_time_ms * 1000 while ih_point_us >= (UINT32_MAX + 1) * 1000: ih_point_us -= (UINT32_MAX + 1) * 1000 if ih_point_us > los_meas_time_us: delta_us = ih_point_us - los_meas_time_us else: delta_us = los_meas_time_us - ih_point_us if delta_us > UINT32_MAX * 1000 / 2: delta_us = (UINT32_MAX + 1) * 1000 - delta_us print(delta_us) return delta_us assert compare_timestamps(0, 0) == 0 assert compare_timestamps(1, 0) == 1000 assert compare_timestamps(0, 1) == 1 assert compare_timestamps(2, 0) == 2000 assert compare_timestamps(0, 2) == 2 assert compare_timestamps(2, 0) == 2000 assert compare_timestamps(UINT32_MAX, UINT32_MAX*1000) == 0 assert compare_timestamps(UINT32_MAX, UINT32_MAX*1000+1) == 1 assert compare_timestamps((UINT32_MAX+1)+0, (UINT32_MAX+1)*1000+0) == 0 assert compare_timestamps((UINT32_MAX+1)+0, (UINT32_MAX+1)*1000-1) == 1 assert compare_timestamps((UINT32_MAX+1)+0, (UINT32_MAX+1)*1000-2) == 2 assert compare_timestamps((UINT32_MAX+1)+0, (UINT32_MAX+1)*1000+1) == 1 assert compare_timestamps((UINT32_MAX+1)+0, (UINT32_MAX+1)*1000+2) == 2 assert compare_timestamps((UINT32_MAX+1)+0, 2*(UINT32_MAX+1)*1000+2) == 2 assert compare_timestamps((UINT32_MAX+1)+0, 3*(UINT32_MAX+1)*1000+4) == 4 assert compare_timestamps((UINT32_MAX+0)+0, 3*(UINT32_MAX+1)*1000+4) == 1004 assert compare_timestamps((UINT32_MAX+1)+7, 3*(UINT32_MAX+1)*1000+4) == 6996

amilcarlucas · 2024-02-19T12:25:19Z

I have updated the size comparison on top of this PR

amilcarlucas · 2024-03-06T09:03:52Z

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.

Co-authored-by: Dr.-Ing. Amilcar do Carmo Lucas <amilcar.lucas@iav.de> 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 time-stamping PLND_LAG should be set to something large like 0.250 (s). - Separate out sync_frames logic, make _inertial_data_delayed class struct - Run forward prediction from synced frame

amilcarlucas · 2024-03-20T08:39:59Z

@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?

amilcarlucas force-pushed the precland_adaptive branch 3 times, most recently from 93dc96d to 4c88919 Compare September 2, 2021 16:08

amilcarlucas requested review from rishabsingh3003 and khancyr September 2, 2021 16:08

amilcarlucas force-pushed the precland_adaptive branch from 66148e1 to b3248ed Compare February 13, 2024 15:29

amilcarlucas added Enhancement Copter Library DevCallEU labels Feb 13, 2024

tridge removed the DevCallEU label Feb 14, 2024

tridge reviewed Feb 14, 2024

View reviewed changes

tridge requested changes Feb 14, 2024

View reviewed changes

amilcarlucas force-pushed the precland_adaptive branch 4 times, most recently from bd23044 to 7f1c877 Compare February 15, 2024 15:58

rishabsingh3003 reviewed Feb 19, 2024

View reviewed changes

amilcarlucas force-pushed the precland_adaptive branch 2 times, most recently from 53fc196 to 8eace96 Compare February 19, 2024 13:33

amilcarlucas marked this pull request as draft February 22, 2024 14:49

amilcarlucas force-pushed the precland_adaptive branch 2 times, most recently from 6a5a5f9 to 8f0f4e3 Compare March 6, 2024 08:58

amilcarlucas added the Driver label Mar 6, 2024

amilcarlucas force-pushed the precland_adaptive branch from 8f0f4e3 to 5c965c2 Compare March 13, 2024 18:10

amilcarlucas requested review from rishabsingh3003 and tridge March 13, 2024 23:21

amilcarlucas marked this pull request as ready for review March 13, 2024 23:21

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

AC_PrecLand: Use sensor timestamp to match inertial frame corrections #18548

AC_PrecLand: Use sensor timestamp to match inertial frame corrections #18548

amilcarlucas commented Sep 2, 2021 •

edited

rishabsingh3003 commented Sep 2, 2021

amilcarlucas commented Sep 2, 2021

fnoop commented Sep 4, 2021

amilcarlucas commented Feb 13, 2024 •

edited

tridge commented Feb 14, 2024

tridge Feb 14, 2024

tridge Feb 14, 2024

amilcarlucas Feb 21, 2024

tridge Feb 14, 2024

rishabsingh3003 left a comment

rishabsingh3003 Feb 19, 2024

amilcarlucas Feb 19, 2024

amilcarlucas Feb 19, 2024

amilcarlucas Feb 19, 2024

amilcarlucas commented Feb 19, 2024

amilcarlucas commented Mar 6, 2024

amilcarlucas commented Mar 20, 2024

AC_PrecLand: Use sensor timestamp to match inertial frame corrections #18548

Are you sure you want to change the base?

AC_PrecLand: Use sensor timestamp to match inertial frame corrections #18548

Conversation

amilcarlucas commented Sep 2, 2021 • edited

rishabsingh3003 commented Sep 2, 2021

amilcarlucas commented Sep 2, 2021

fnoop commented Sep 4, 2021

amilcarlucas commented Feb 13, 2024 • edited

tridge commented Feb 14, 2024

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rishabsingh3003 left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

amilcarlucas commented Feb 19, 2024

amilcarlucas commented Mar 6, 2024

amilcarlucas commented Mar 20, 2024

amilcarlucas commented Sep 2, 2021 •

edited

amilcarlucas commented Feb 13, 2024 •

edited