Skip to content

AP_Follow: discard target state when FOLL_SYSID changes #33891

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

Open
lthall wants to merge 3 commits into
base: master
Choose a base branch
from
+67 −39
Open

AP_Follow: discard target state when FOLL_SYSID changes #33891

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
455eba3
AP_Follow: consolidate sysid adoption into handle_msg
lthall Jul 31, 2026
6b68514
AP_Follow: discard target state when FOLL_SYSID changes
lthall Jul 31, 2026
43bbccf
AP_Follow: never adopt or follow sysid 0
lthall Jul 31, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
101 changes: 63 additions & 38 deletions libraries/AP_Follow/AP_Follow.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,7 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {

// @Param: _SYSID
// @DisplayName: Follow target's mavlink system id
// @Description: Follow target's mavlink system id
// @Description: MAVLink system id of the vehicle to follow. Zero means follow the first system that sends a suitable position message.
// @Range: 0 255
// @User: Standard
AP_GROUPINFO("_SYSID", 3, AP_Follow, _sysid, 0),
Expand Down Expand Up @@ -245,12 +245,6 @@ void AP_Follow::update_estimates()
return;
}

// if sysid changed, reset the estimation state
if (_sysid != _sysid_used) {
_sysid_used = _sysid;
_estimate_valid = false;
}

const uint32_t now = AP_HAL::millis();

// calculate time since last location update in seconds
Expand Down Expand Up @@ -366,7 +360,7 @@ void AP_Follow::update_estimates()
// Retrieves the estimated target position, velocity, and acceleration in the NED frame (relative to origin).
bool AP_Follow::get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss) const
{
if (!_estimate_valid) {
if (!estimate_usable()) {
return false;
}

Expand All @@ -380,7 +374,7 @@ bool AP_Follow::get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &ve
// Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured offsets.
bool AP_Follow::get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss) const
{
if (!_estimate_valid) {
if (!estimate_usable()) {
return false;
}

Expand All @@ -396,7 +390,7 @@ bool AP_Follow::get_target_dist_and_vel_NED_m(Vector3f &dist_ned, Vector3f &dist
{
WITH_SEMAPHORE(_follow_sem);

if (!_estimate_valid) {
if (!estimate_usable()) {
return false;
}

Expand All @@ -417,7 +411,7 @@ bool AP_Follow::get_target_dist_and_vel_NED_m(Vector3f &dist_ned, Vector3f &dist
// Retrieves the estimated target heading and heading rate in radians.
bool AP_Follow::get_heading_heading_rate_rad(float &heading_rad, float &heading_rate_rads) const
{
if (!_estimate_valid) {
if (!estimate_usable()) {
return false;
}

Expand All @@ -432,7 +426,7 @@ bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ne
{
WITH_SEMAPHORE(_follow_sem);

if (!_estimate_valid) {
if (!estimate_usable()) {
return false;
}

Expand All @@ -451,7 +445,7 @@ bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &ve
{
WITH_SEMAPHORE(_follow_sem);

if (!_estimate_valid) {
if (!estimate_usable()) {
return false;
}
if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _ofs_estimate_pos_ned_m)) {
Expand All @@ -467,7 +461,7 @@ bool AP_Follow::get_target_heading_deg(float &heading_deg)
{
WITH_SEMAPHORE(_follow_sem);

if (!_estimate_valid) {
if (!estimate_usable()) {
return false;
}

Expand All @@ -481,7 +475,7 @@ bool AP_Follow::get_target_heading_rate_degs(float &heading_rate_degs)
{
WITH_SEMAPHORE(_follow_sem);

if (!_estimate_valid) {
if (!estimate_usable()) {
return false;
}

Expand All @@ -498,6 +492,18 @@ bool AP_Follow::get_target_heading_rate_degs(float &heading_rate_degs)
// Handles incoming MAVLink messages to update the target's position, velocity, and heading.
void AP_Follow::handle_msg(const mavlink_message_t &msg)
{
// FOLL_SYSID no longer matches the system that supplied the data we hold, so discard
// that data. This must run before the timeout check below: clearing _automatic_sysid
// here is what stops a retarget with stale data from taking the timeout path and
// zeroing the FOLL_SYSID the user has just written.
if (_sysid != _sysid_used) {

@rmackay9 rmackay9 Jul 30, 2026 •
edited
Loading

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

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

Doesn't this mean that if there are two different leaders both publishing their follow-state we would end up constantly invalidating the valid leader's state?

An alternative solution might be to replace _estimate_valid with a uint32_t _estimate_valid_ms and then somehow force a timeout if the target sysid changes.

@lthall lthall Jul 30, 2026

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

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

_sysid is the current sysid that is set.
_sysid_used is the last one that was consumed.
msg.msgid is the sysid of the message that has just arrived.

So the sysid of the message has no impact on this.

_using_follow_target = false; // re-learn the new target's message type
if (_automatic_sysid) {
// a parameter write has superseded a sysid we adopted ourselves
_automatic_sysid = false;
}
}

// Invalidate the estimate if no position update has been received within the timeout period.
// If using automatic sysid tracking, clear the sysid and reset tracking state.
if ((_last_location_update_ms == 0) ||
Expand All @@ -520,6 +526,10 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg)

switch (msg.msgid) {
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
// if we are using follow_target, ignore global_position_int messages
if (_using_follow_target) {
return;
}
// handle standard global position messages
updated = handle_global_position_int_message(msg);
break;
Expand All @@ -532,11 +542,21 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg)
}

if (updated) {
// Check if estimate needs reset based on position and velocity errors
if (estimate_error_too_large()) {
// if sysid not yet assigned, adopt sender's sysid and enable automatic sysid tracking.
// sysid 0 is the broadcast system and is never a valid target.
if (_sysid == 0 && msg.sysid != 0) {
_sysid.set(msg.sysid);
_automatic_sysid = true;
}

// reset the estimate on a large error or a change of source system
if (estimate_error_too_large() || (_sysid_used != _sysid)) {
_estimate_valid = false;
}

// record the system that supplied this update
_sysid_used = _sysid;

#if HAL_LOGGING_ENABLED
// log current follow diagnostic data
Log_Write_FOLL();
Expand All @@ -557,6 +577,11 @@ bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
return false;
}

// skip sysid of zero
if (msg.sysid == 0) {
return false;
}

// skip message if not from our target
if (_sysid != 0 && msg.sysid != _sysid) {
return false;
Expand Down Expand Up @@ -633,11 +658,6 @@ bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
return false;
}

if (_using_follow_target) {
// if we are using follow_target, ignore global_position_int messages
return false;
}

Location target_location;
target_location.lat = packet.lat;
target_location.lng = packet.lon;
Expand Down Expand Up @@ -695,14 +715,6 @@ bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
// apply jitter-corrected timestamp to this update
_last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());

// if sysid not yet set, adopt sender’s sysid and enable automatic sysid tracking
if (_sysid == 0) {
_sysid.set(msg.sysid);
_sysid_used = 0;
_estimate_valid = false;
_automatic_sysid = true;
}

return true;
}

Expand Down Expand Up @@ -793,13 +805,7 @@ bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)
// apply jitter-corrected timestamp to this update
_last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.timestamp, AP_HAL::millis());

// if sysid not yet assigned, adopt sender's sysid and enable automatic sysid tracking
if (_sysid == 0) {
_sysid.set(msg.sysid);
_automatic_sysid = true;
}

// we are using follow_target: set sysid to sender's sysid
// prefer FOLLOW_TARGET over GLOBAL_POSITION_INT from now on
_using_follow_target = true;

return true;
Expand All @@ -819,7 +825,7 @@ void AP_Follow::init_offsets_if_required()
}
_offsets_were_zero = true;

if (!_estimate_valid) {
if (!estimate_usable()) {
return;
}

Expand Down Expand Up @@ -960,13 +966,32 @@ bool AP_Follow::have_target(void) const
return false;
}

// no target system configured or adopted yet
if (_sysid == 0) {
return false;
}

// we have not yet accepted an update from the configured system
if (_sysid_used != _sysid) {
return false;
}

// check for timeout
if ((_last_location_update_ms == 0) || ((AP_HAL::millis() - _last_location_update_ms) > (uint32_t)(_timeout * 1000.0f))) {
return false;
}
return true;
}

// Returns true if the estimate is populated and was supplied by the currently configured target.
// The getters that serve the estimate must use this rather than testing _estimate_valid alone,
// otherwise a FOLL_SYSID change is not seen until the next update_estimates(). update_estimates()
// tests _estimate_valid directly because it has already passed the have_target() gate.
bool AP_Follow::estimate_usable() const
{
return _estimate_valid && have_target();
}

//==============================================================================
// AP_Follow Accessor
//==============================================================================
Expand Down
5 changes: 4 additions & 1 deletion libraries/AP_Follow/AP_Follow.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -169,6 +169,9 @@ class AP_Follow
// returns true if we should extract information from msg
bool should_handle_message(const mavlink_message_t &msg) const;

// Returns true if the estimate is populated and was supplied by the currently configured target.
bool estimate_usable() const;

// Checks whether the current estimate should be reset based on position and velocity errors.
bool estimate_error_too_large() const;

Expand Down Expand Up @@ -237,7 +240,7 @@ class AP_Follow
Vector3f _ofs_estimate_accel_ned_mss; // Estimated acceleration with offsets applied (NED frame)

bool _automatic_sysid; // True if target sysid was automatically selected
int16_t _sysid_used; // Currently active sysid used for updates
int16_t _sysid_used; // sysid that supplied the target data we currently hold
float _dist_to_target_m; // Horizontal distance to target, for reporting (meters)
float _bearing_to_target_deg; // Bearing to target from vehicle (degrees, 0 = North)
bool _offsets_were_zero; // True if initial offset was zero before being initialized
Expand Down
Loading