Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

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

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
+57 −2
Open

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
55 changes: 53 additions & 2 deletions libraries/AC_PrecLand/AC_PrecLand.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -491,7 +491,10 @@ void AC_PrecLand::handle_msg(const mavlink_landing_target_t &packet, uint32_t ti

void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_valid)
{
// Set inertial_data_delayed to the oldest sensor data in the buffer, by default.
_inertial_data_delayed = (*_inertial_history)[0];
// construct_pos_meas_using_rangefinder() is then called which attempts to match timestamped
// sensor frame and overwrites _target_pos_rel_meas_NED calculated from matched inertial frame, if available

switch ((EstimatorType)_estimator_type.get()) {
case EstimatorType::RAW_SENSOR: {
Expand Down Expand Up @@ -646,7 +649,8 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m,
{
Vector3f target_vec_unit_body;
if (retrieve_los_meas(target_vec_unit_body)) {
_inertial_data_delayed = (*_inertial_history)[0];
// Match sensor and inertial frames or push front of buffer, into _inertial_data_delayed
sync_frames(_last_backend_los_meas_ms);

const bool target_vec_valid = target_vec_unit_body.projected(_approach_vector_body).dot(_approach_vector_body) > 0.0f;
const Vector3f target_vec_unit_ned = _inertial_data_delayed->Tbn * target_vec_unit_body;
Expand Down Expand Up @@ -692,13 +696,60 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m,
return false;
}

// Set the active inertial frame, based on either sensor timestamp (higher priority) or PLND_LAG parameter
// Return true if sensor does not send timestamp and front of buffer is used, or if sensor timestamp is successfully synced
// Return false otherwise, so invalid sync is not propagated
bool AC_PrecLand::sync_frames(const uint32_t los_meas_time_ms)
{
uint64_t minimum_delta_us = UINT32_MAX * 1000UL;
uint16_t nearest_index = 0; // If timestamp isn't set, use the oldest inertial buffer frame
// Retrieve timestamp from backend measurement. If not zero, attempt to correlate with inertial history.
const uint64_t los_meas_time_us = los_meas_time_ms * 1000UL; // overflows after approximately 49.7 days
if (los_meas_time_us != 0UL) {
// 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_us;
uint64_t ih_point_us = (*_inertial_history)[i]->time_usec; // overflows after approximately 584 554.531 years
// line-of-sight measurement time (los_meas_time_ms) was in ms and overflows at this point
// so we remove it from the IMU so that the two timestamps can be compared
while (ih_point_us >= (UINT32_MAX + 1UL) * 1000UL) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

I added the comment that you asked.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

ih_point_us -= (UINT32_MAX + 1UL) * 1000UL; // subtract units of 49,7 days
}
// doing the comparison makes sure time wraps can be handled bellow
// warning the wrap point is NOT UINT64_MAX but (UINT32_MAX + 1) * 1000UL
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;
}
// deal with time wraps
if (delta_us > UINT32_MAX * 1000UL / 2UL) {
delta_us = (UINT32_MAX + 1UL) * 1000UL - delta_us;
}
if (delta_us < minimum_delta_us) {
minimum_delta_us = delta_us;
nearest_index = i;
}
}
// If the nearest timestamp delta is longer than a frame, it's after the last frame or before the first, so reject
if (minimum_delta_us > AP::scheduler().get_loop_period_us()) {
_inertial_frame_index = 0;
_inertial_data_delayed = (*_inertial_history)[_inertial_frame_index];
return false;
}
}
_inertial_frame_index = nearest_index;
_inertial_data_delayed = (*_inertial_history)[_inertial_frame_index];
return true;
}

void AC_PrecLand::run_output_prediction()
{
_target_pos_rel_out_NE = _target_pos_rel_est_NE;
_target_vel_rel_out_NE = _target_vel_rel_est_NE;

// Predict forward from delayed time horizon
for (uint8_t i=1; i<_inertial_history->available(); i++) {
for (uint8_t i=1+_inertial_frame_index; i<_inertial_history->available(); i++) {
const struct inertial_data_frame_s *inertial_data = (*_inertial_history)[i];
_target_vel_rel_out_NE.x -= inertial_data->correctedVehicleDeltaVelocityNED.x;
_target_vel_rel_out_NE.y -= inertial_data->correctedVehicleDeltaVelocityNED.y;
Expand Down
4 changes: 4 additions & 0 deletions libraries/AC_PrecLand/AC_PrecLand.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -187,6 +187,9 @@ class AC_PrecLand
// results are stored in_target_pos_rel_out_NE, _target_vel_rel_out_NE
void run_output_prediction();

// Attempt to match sensor frame with inertial frame, otherwise use the front of the inertial buffer (PLND_LAG parameter)
bool sync_frames(const uint32_t los_meas_time_ms);

// parameters
AP_Int8 _enabled; // enabled/disabled
AP_Enum<Type> _type; // precision landing sensor type
Expand Down Expand Up @@ -243,6 +246,7 @@ class AC_PrecLand
};
ObjectArray<inertial_data_frame_s> *_inertial_history;
struct inertial_data_frame_s *_inertial_data_delayed;
uint16_t _inertial_frame_index;

// backend state
struct precland_state {
Expand Down