fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

flight task auto: fix offtrack mission landing bug (#25725)

Browse Source 
During a mission the last waypoint is often a LAND. If the previous waypoint is not directly above the land waypoint the offtrack calculation is incorrect. This regression was introduced when the offtrack calculation switched from 2D to 3D.
This commit is contained in:
Jacob Dahl 2025-10-13 19:56:23 -08:00 committed by GitHub
parent 87559f717b
commit 2c62caeb7d
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
View File

@ -612,6 +612,7 @@ State FlightTaskAuto::_getCurrentState()
const Vector3f u_prev_to_target = (_triplet_target - _triplet_prev_wp).unit_or_zero();
const Vector3f prev_to_pos = _position - _triplet_prev_wp;
const Vector3f pos_to_target = _triplet_target - _position;
// Calculate the closest point to the vehicle position on the line prev_wp - target
_closest_pt = _triplet_prev_wp + u_prev_to_target * (prev_to_pos * u_prev_to_target);
@ -629,10 +630,9 @@ State FlightTaskAuto::_getCurrentState()
// Previous is in front
return_state = State::previous_infront;
} else if ((_position - _closest_pt).longerThan(_target_acceptance_radius)) {
} else if (_type != WaypointType::land && (_position - _closest_pt).longerThan(_target_acceptance_radius)) {
// Vehicle too far from the track
return_state = State::offtrack;
}
return return_state;