AP_Follow: discard target state when FOLL_SYSID changes

[lthall]

Summary

FOLL_SYSID only filters incoming messages; it does not invalidate the target data already held, so after a change the library keeps reporting the previous vehicle's position, velocity and heading as valid. This makes the held target state belong to the system named by FOLL_SYSID.

Classification & Testing (check all that apply and add your own)

• Checked by a human programmer
• Tested manually, description below (e.g. SITL)

Found in a Copter flight log where a Lua script switched FOLL_SYSID between two beacons roughly 5 m apart. The vehicle continued to be positioned relative to the previous beacon, and the mode consuming the follow estimate advanced its state machine against the old beacon's geometry. SITL Copter ModeFollow_with_FOLLOW_TARGET and three ship-landing tests exercising GLOBAL_POSITION_INT follow pass with this change on a downstream branch. No existing autotest changes FOLL_SYSID in flight, so the retarget path itself is not covered by automated testing.

Description

_sysid is used only as an inbound filter in should_handle_message(). The cached target state carries no record of which system supplied it, and have_target() checks only the age of _last_location_update_ms. So for up to FOLL_TIMEOUT after FOLL_SYSID changes, get_target_sysid() reports the new vehicle while every getter returns the previous one. update_estimates() noticed the change but only cleared _estimate_valid, and the re-init branch immediately re-seeded the estimate from the previous target's last position and marked it valid again.

Two other pieces of state also survived a retarget. _using_follow_target latches when a FOLLOW_TARGET message arrives and causes every GLOBAL_POSITION_INT to be rejected, so retargeting from a FOLLOW_TARGET vehicle to one that only sends GLOBAL_POSITION_INT stalled until the 10 second sysid timeout. _automatic_sysid was set on adoption and never cleared, so a later explicit FOLL_SYSID write could be discarded by that same timeout and the parameter reset to 0.

The change gives _sysid_used a single meaning, the system that supplied the data currently held, updated wherever an update is accepted. have_target() rejects a mismatch, so the estimate and every getter report no target until the new system transmits. On a mismatch the FOLLOW_TARGET preference is dropped and _automatic_sysid is cleared, since a parameter write has superseded an adoption. The estimate is reset when an accepted update comes from a different system, so it snaps to the new target rather than shaping across the gap.

Sysid adoption for FOLL_SYSID = 0 was duplicated in both message decoders and is now done once in handle_msg(), along with the _using_follow_target gate on GLOBAL_POSITION_INT. No parameter changes, and no behaviour change while FOLL_SYSID is fixed.

[rmackay9]

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]

_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.

[Copilot]

Pull request overview

This PR updates the AP_Follow library to ensure cached target state is treated as belonging to the currently-selected FOLL_SYSID, and is discarded/invalidated when FOLL_SYSID changes so consumers don’t keep using stale target position/velocity/heading.

Changes:

• Moves sysid-change handling out of update_estimates() into message handling + have_target() gating, so stale cached data is rejected immediately after FOLL_SYSID changes.
• Resets FOLLOW_TARGET-vs-GLOBAL_POSITION_INT selection behavior on sysid mismatch and consolidates sysid adoption (FOLL_SYSID=0) into handle_msg().
• Ensures the estimate is reset when the source system changes (or estimate error is too large), preventing shaping across a retarget.

Reviewed changes

Copilot reviewed 2 out of 2 changed files in this pull request and generated 1 comment.

File	Description
libraries/AP_Follow/AP_Follow.h	Clarifies _sysid_used meaning as the sysid that supplied the currently-held target data.
libraries/AP_Follow/AP_Follow.cpp	Reworks sysid mismatch/retarget behavior to invalidate cached target state and reset FOLLOW_TARGET preference and estimate when appropriate.

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[timtuxworth]

I ran it by Claude. This is what I got:

1. The _estimate_valid clearing on retarget is deferred, not immediate. L495–502 clears _using_follow_target and _automatic_sysid but leaves _estimate_valid alone, relying on the new have_target() gate to invalidate it via update_estimates(). That means every getter still returns the old target's position for one scheduler tick after the write. It's ~2.5 ms on Copter and probably irrelevant, but it's a strictly weaker guarantee than the PR description claims ("the estimate and every getter report no target until the new system transmits"), and clearing _estimate_valid directly in that block costs nothing.
2. The Lua-driven case from the PR's own motivation may now fail closed. With have_target() gating on _sysid_used != _sysid, U only catches S when an accepted update arrives. If a script rewrites FOLL_SYSID faster than the beacons transmit, U never converges and follow reports no target at all rather than a stale one. Arguably correct, but it's a real behaviour change for exactly the flight log that motivated the PR, and the description doesn't mention it. Worth a line about beacon rate vs. switch rate.
3. Decode-failure paths silently block convergence. updated is false when lat/lon are zero, est_capabilities bit 0 is unset, or AHRS origin isn't available — so _sysid_used never syncs and have_target() stays false indefinitely, with no GCS message explaining why. Under master this was masked because update_estimates() re-synced _sysid_used at loop rate regardless. Removing that sync (the L248–252 hunk) is the right call, but it removes the only unconditional re-sync in the library.

I asked it for a suggested improvement:

The root cause of #3 is that _sysid_used is doing two jobs: "which system supplied the data I hold" (its documented meaning) and "have I converged onto the configured target" (its new job in have_target()). Convergence is only reachable through a successful decode, so any decode failure wedges the gate.

Fix: stop using the mismatch as a gate, and make it a positive discard instead — rmackay9's instinct, just via a different mechanism than his _estimate_valid_ms.

I checked where update_estimates() actually runs, because the placement matters:

• ArduCopter/inertia.cpp:9 — inside read_inertia(), a FAST_TASK (400 Hz), mode-independent
• Rover/Rover.cpp:321 — loop rate, mode-independent
• ArduPlane/Plane.cpp:209 — loop rate, #if AP_SCRIPTING_ENABLED && AP_FOLLOW_ENABLED

So it's loop-rate and message-independent on all three. That's the right home for the discard.

Suggested shape

void AP_Follow::update_estimates()
{
    WITH_SEMAPHORE(_follow_sem);

    // FOLL_SYSID has changed: everything we hold was supplied by the previous
    // system, so discard it. Nothing is reported until the newly named system
    // transmits and is accepted.
    if (_sysid != _sysid_used) {
        _sysid_used = _sysid;
        _last_location_update_ms = 0;   // we hold no data from this system
        _estimate_valid = false;
        _using_follow_target = false;   // re-learn the new target's message type
        _automatic_sysid = false;       // a write has superseded any adoption
    }

    if (!have_target()) {
        clear_dist_and_bearing_to_target();
        _estimate_valid = false;
        return;
    }
    ...

and then:

• have_target() reverts to master's form — drop the _sysid_used != _sysid term
• handle_msg() drops the new L495–502 block entirely
• the || (_sysid_used != _sysid) term at L549 becomes dead — _estimate_valid is already false, so the re-init branch still snaps rather than shapes
• handle_msg() keeps _sysid_used = _sysid; in the if (updated) block, needed so auto-adoption (_sysid 0 → msg.sysid) doesn't trip the discard on the next tick

The ordering is the part to get right: the detection must sit before the have_target() early-out. Master had it after, which is part of why master's version never actually worked — on a retarget have_target() could return early and the mismatch was never consumed.

Why this is better than the gate

#3 dissolves. _sysid_used converges on the tick after the write, regardless of whether anything decodes. _last_location_update_ms = 0 becomes the single "I hold nothing" sentinel — and it's already special-cased in both have_target() and the 10 s block, so you're reusing an existing state rather than inventing one. A target sending garbage leaves the library in a clean, honest, self-healing "no target yet" state instead of a permanently unsynced one.

#1 dissolves too. The discard runs at loop rate under _follow_sem and isn't gated on message arrival, so there's no window where a getter can still return the old target's position.

It fixes a threading hole. handle_msg() doesn't take _follow_sem (pre-existing, but real) — it runs on the MAVLink thread while update_estimates() and all six getters hold the semaphore. The PR currently puts the retarget state mutation in the unlocked path. Moving it into update_estimates() puts it under the lock.

Copilot's point goes away as a side effect. _sysid_used ends up written in exactly two places with one meaning, so = _sysid vs = msg.sysid stops being load-bearing.

Caveats worth stating in the PR

_automatic_sysid = false in the discard is unconditional here, unlike the PR's if (_automatic_sysid) wrapper — that wrapper was a no-op anyway, but be explicit that a write of FOLL_SYSID = 0 still re-enables adoption, because handle_msg sets A = true on the next adopt. Worth a comment so nobody "optimises" it back.

And on Plane the whole thing lives behind AP_SCRIPTING_ENABLED, so a scripting-disabled Plane build gets no discard. That's fine given Plane's follow is script-driven, but it's the kind of thing that bites later — probably worth a line in the PR description rather than a code change.

[lthall]

I ran it by Claude. This is what I got:

1. The _estimate_valid clearing on retarget is deferred, not immediate. L495–502 clears _using_follow_target and _automatic_sysid but leaves _estimate_valid alone, relying on the new have_target() gate to invalidate it via update_estimates(). That means every getter still returns the old target's position for one scheduler tick after the write. It's ~2.5 ms on Copter and probably irrelevant, but it's a strictly weaker guarantee than the PR description claims ("the estimate and every getter report no target until the new system transmits"), and clearing _estimate_valid directly in that block costs nothing.
2. The Lua-driven case from the PR's own motivation may now fail closed. With have_target() gating on _sysid_used != _sysid, U only catches S when an accepted update arrives. If a script rewrites FOLL_SYSID faster than the beacons transmit, U never converges and follow reports no target at all rather than a stale one. Arguably correct, but it's a real behaviour change for exactly the flight log that motivated the PR, and the description doesn't mention it. Worth a line about beacon rate vs. switch rate.
3. Decode-failure paths silently block convergence. updated is false when lat/lon are zero, est_capabilities bit 0 is unset, or AHRS origin isn't available — so _sysid_used never syncs and have_target() stays false indefinitely, with no GCS message explaining why. Under master this was masked because update_estimates() re-synced _sysid_used at loop rate regardless. Removing that sync (the L248–252 hunk) is the right call, but it removes the only unconditional re-sync in the library.

I asked it for a suggested improvement:

The root cause of #3 is that _sysid_used is doing two jobs: "which system supplied the data I hold" (its documented meaning) and "have I converged onto the configured target" (its new job in have_target()). Convergence is only reachable through a successful decode, so any decode failure wedges the gate.

Fix: stop using the mismatch as a gate, and make it a positive discard instead — rmackay9's instinct, just via a different mechanism than his _estimate_valid_ms.

I checked where update_estimates() actually runs, because the placement matters:

• ArduCopter/inertia.cpp:9 — inside read_inertia(), a FAST_TASK (400 Hz), mode-independent
• Rover/Rover.cpp:321 — loop rate, mode-independent
• ArduPlane/Plane.cpp:209 — loop rate, #if AP_SCRIPTING_ENABLED && AP_FOLLOW_ENABLED

So it's loop-rate and message-independent on all three. That's the right home for the discard.

Suggested shape

void AP_Follow::update_estimates()
{
    WITH_SEMAPHORE(_follow_sem);

    // FOLL_SYSID has changed: everything we hold was supplied by the previous
    // system, so discard it. Nothing is reported until the newly named system
    // transmits and is accepted.
    if (_sysid != _sysid_used) {
        _sysid_used = _sysid;
        _last_location_update_ms = 0;   // we hold no data from this system
        _estimate_valid = false;
        _using_follow_target = false;   // re-learn the new target's message type
        _automatic_sysid = false;       // a write has superseded any adoption
    }

    if (!have_target()) {
        clear_dist_and_bearing_to_target();
        _estimate_valid = false;
        return;
    }
    ...

and then:

• have_target() reverts to master's form — drop the _sysid_used != _sysid term
• handle_msg() drops the new L495–502 block entirely
• the || (_sysid_used != _sysid) term at L549 becomes dead — _estimate_valid is already false, so the re-init branch still snaps rather than shapes
• handle_msg() keeps _sysid_used = _sysid; in the if (updated) block, needed so auto-adoption (_sysid 0 → msg.sysid) doesn't trip the discard on the next tick

The ordering is the part to get right: the detection must sit before the have_target() early-out. Master had it after, which is part of why master's version never actually worked — on a retarget have_target() could return early and the mismatch was never consumed.

Why this is better than the gate

#3 dissolves. _sysid_used converges on the tick after the write, regardless of whether anything decodes. _last_location_update_ms = 0 becomes the single "I hold nothing" sentinel — and it's already special-cased in both have_target() and the 10 s block, so you're reusing an existing state rather than inventing one. A target sending garbage leaves the library in a clean, honest, self-healing "no target yet" state instead of a permanently unsynced one.

#1 dissolves too. The discard runs at loop rate under _follow_sem and isn't gated on message arrival, so there's no window where a getter can still return the old target's position.

It fixes a threading hole. handle_msg() doesn't take _follow_sem (pre-existing, but real) — it runs on the MAVLink thread while update_estimates() and all six getters hold the semaphore. The PR currently puts the retarget state mutation in the unlocked path. Moving it into update_estimates() puts it under the lock.

Copilot's point goes away as a side effect. _sysid_used ends up written in exactly two places with one meaning, so = _sysid vs = msg.sysid stops being load-bearing.

Caveats worth stating in the PR

_automatic_sysid = false in the discard is unconditional here, unlike the PR's if (_automatic_sysid) wrapper — that wrapper was a no-op anyway, but be explicit that a write of FOLL_SYSID = 0 still re-enables adoption, because handle_msg sets A = true on the next adopt. Worth a comment so nobody "optimises" it back.

And on Plane the whole thing lives behind AP_SCRIPTING_ENABLED, so a scripting-disabled Plane build gets no discard. That's fine given Plane's follow is script-driven, but it's the kind of thing that bites later — probably worth a line in the PR description rather than a code change.

Thanks, that was a useful read. Answers in order.

Fixed. Every getter now goes through estimate_usable(), which is _estimate_valid && have_target(), so a FOLL_SYSID write is seen on the next call rather than the next tick. get_distance_to_target_m() and get_bearing_to_target_deg() are reporting only and still clear on the next update_estimates(), which is one loop.

Agreed, and it is deliberate. It is now in the commit message. If a script rewrites FOLL_SYSID faster than the target transmits, follow reports no target rather than the previous vehicle's position.

I think this is the intended behaviour rather than a wedge. If the newly named system never decodes then we genuinely hold no data for it, so reporting no target is correct. It is also not a regression. On master a decode failure after a retarget left have_target() true while serving the previous vehicle's position, which is the bug this PR fixes. The part that is missing is a GCS message explaining why, which I would rather do separately.

On moving the discard into update_estimates(), I looked at it and stayed with handle_msg(). It reintroduces the race the current ordering exists to prevent. If _automatic_sysid is only cleared at loop rate, a message arriving between the FOLL_SYSID write and the next update_estimates() still takes the 10 second timeout path and zeroes the sysid the user just wrote. Clearing it in handle_msg() before the timeout check closes that window by construction. There is also the Plane point you raised, which cuts the other way. update_estimates() is inside #if AP_SCRIPTING_ENABLED at ArduPlane/Plane.cpp:209, so a scripting-disabled build would never run the discard, whereas handle_msg() and have_target() are always compiled.

On the semaphore, that is fair, but it is not new. handle_msg() already wrote _target_pos_ned_m, _last_location_update_ms and _estimate_valid from the MAVLink thread without the lock on master, so the semaphore has never protected against that writer. Worth fixing, but not in this PR.

Reviewing it again did find a real hole in the sysid 0 handling. Refusing to adopt sysid 0 was not enough, because with FOLL_SYSID zero the message still passed the filter and was decoded, so a FOLLOW_TARGET from sysid 0 latched _using_follow_target and permanently blocked auto-adoption via GLOBAL_POSITION_INT. should_handle_message() now rejects sysid 0 outright, matching MAVLink_routing::learn_route().

[lthall]

I have done a lot more work on this and I think I have finally filled the holes.

[Copilot]

Pull request overview

Copilot reviewed 2 out of 2 changed files in this pull request and generated no new comments.