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

PositionSmoothing: constrain L1 point to line segment length

Browse Source 
This additional logic handles edge cases where we are either behind the previous_setpoint, or ahead of the current_setpoint.
This commit is contained in:
mahima-yoga
2026-02-19 20:21:45 +01:00
committed by Matthias Grob
parent d289a502f7
commit a7335cd637
1 changed files with 10 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/motion_planning/PositionSmoothing.cpp
+10 -10
View File
@@ -141,13 +141,8 @@ float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const
const Vector3f PositionSmoothing::_getCrossingPoint(const Vector3f &position, const Vector3f(&waypoints)[3]) const
{
const auto &target = waypoints[1];
if (!_isTurning(target)) {
return target;
}
// Get the crossing point using L1-style guidance
// Always use L1 guidance to handle edge cases (vehicle behind previous waypoint, etc.)
// The L1 logic will clamp to the segment bounds and handle straight-line cases appropriately
return _getL1Point(position, waypoints);
}
@@ -157,8 +152,9 @@ const Vector3f PositionSmoothing::_getL1Point(const Vector3f &position, const Ve
_trajectory[2].getCurrentPosition());
const Vector3f u_prev_to_target = (waypoints[1] - waypoints[0]).unit_or_zero();
const Vector3f prev_to_pos(pos_traj - waypoints[0]);
const Vector3f prev_to_closest(u_prev_to_target * (prev_to_pos * u_prev_to_target));
const Vector3f closest_pt = waypoints[0] + prev_to_closest;
const float projection = prev_to_pos * u_prev_to_target;
const Vector3f closest_pt = waypoints[0] + u_prev_to_target * projection;
// Compute along-track error using L1 distance and cross-track error
const float crosstrack_error = (closest_pt - pos_traj).length();
@@ -171,8 +167,12 @@ const Vector3f PositionSmoothing::_getL1Point(const Vector3f &position, const Ve
alongtrack_error = sqrtf(l1 * l1 - crosstrack_error * crosstrack_error);
}
// Clamp the final point to the line segment bounds
const float segment_length = (waypoints[1] - waypoints[0]).length();
const float constrained_projection = math::constrain(projection + alongtrack_error, 0.0f, segment_length);
// Position of the point on the line where L1 intersect the line between the two waypoints
return closest_pt + alongtrack_error * u_prev_to_target;
return waypoints[0] + u_prev_to_target * constrained_projection;
}
const Vector3f PositionSmoothing::_generateVelocitySetpoint(const Vector3f &position, const Vector3f(&waypoints)[3],