From 455eba31961f67fecfedb67a07a488cbdc3e5b7a Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 31 Jul 2026 22:25:48 +0930 Subject: [PATCH 1/3] AP_Follow: consolidate sysid adoption into handle_msg --- libraries/AP_Follow/AP_Follow.cpp | 35 +++++++++++++------------------ 1 file changed, 14 insertions(+), 21 deletions(-) diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index c541b999cb0de8..79ba64c6ec5c9a 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -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), @@ -520,6 +520,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; @@ -532,6 +536,14 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg) } if (updated) { + // if sysid not yet assigned, 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; + } + // Check if estimate needs reset based on position and velocity errors if (estimate_error_too_large()) { _estimate_valid = false; @@ -633,11 +645,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; @@ -695,14 +702,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; } @@ -793,13 +792,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; From 6b68514155513623104a3cf84a5750d6b2caddc8 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 31 Jul 2026 22:27:26 +0930 Subject: [PATCH 2/3] AP_Follow: discard target state when FOLL_SYSID changes --- libraries/AP_Follow/AP_Follow.cpp | 59 +++++++++++++++++++++---------- libraries/AP_Follow/AP_Follow.h | 5 ++- 2 files changed, 44 insertions(+), 20 deletions(-) diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index 79ba64c6ec5c9a..5966f09c66357c 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -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 @@ -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; } @@ -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; } @@ -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; } @@ -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; } @@ -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; } @@ -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)) { @@ -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; } @@ -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; } @@ -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) { + _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) || @@ -539,16 +545,17 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg) // if sysid not yet assigned, 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; } - // Check if estimate needs reset based on position and velocity errors - if (estimate_error_too_large()) { + // 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(); @@ -812,7 +819,7 @@ void AP_Follow::init_offsets_if_required() } _offsets_were_zero = true; - if (!_estimate_valid) { + if (!estimate_usable()) { return; } @@ -953,6 +960,11 @@ bool AP_Follow::have_target(void) const 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; @@ -960,6 +972,15 @@ bool AP_Follow::have_target(void) const 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 //============================================================================== diff --git a/libraries/AP_Follow/AP_Follow.h b/libraries/AP_Follow/AP_Follow.h index 24ee0be15c36fc..c8e78818367650 100644 --- a/libraries/AP_Follow/AP_Follow.h +++ b/libraries/AP_Follow/AP_Follow.h @@ -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; @@ -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 From 43bbccf7e31caf10a5187d6bd3bebda0062da208 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 31 Jul 2026 22:30:04 +0930 Subject: [PATCH 3/3] AP_Follow: never adopt or follow sysid 0 --- libraries/AP_Follow/AP_Follow.cpp | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index 5966f09c66357c..4f3bbc15870987 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -542,8 +542,9 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg) } if (updated) { - // if sysid not yet assigned, adopt sender's sysid and enable automatic sysid tracking - if (_sysid == 0) { + // 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; } @@ -576,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; @@ -960,6 +966,11 @@ 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;