mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
refactor(motion_planning): split XY/Z velocity components in smoother
PositionSmoothing::_generateVelocitySetpoint computed a single constrained 3D velocity vector and then ran clampToXYNorm / clampToZNorm in series. The XY clamp could shorten the Z component (and vice versa), so a Z speed limit ended up capping forward speed and a tight altitude error capped horizontal motion. Compute XY and Z velocity components independently using their own speed limits so they don't mutually clamp. Companion to the per-axis time-stretch fix (commit added later in the chain). Together they decouple the horizontal and vertical trajectory dynamics in the smoother.
This commit is contained in:
1 parent
43131d474c
commit
1ae8f2383f
1 file changed
+11
-3
@@ -208,9 +208,17 @@ const Vector3f PositionSmoothing::_generateVelocitySetpoint(const Vector3f &posi
|
||||
_max_speed_previous = xy_speed;
|
||||
}
|
||||
|
||||
Vector3f vel_sp_constrained = u_pos_traj_to_dest * sqrtf(xy_speed * xy_speed + z_speed * z_speed);
|
||||
math::trajectory::clampToXYNorm(vel_sp_constrained, xy_speed, 0.5f);
|
||||
math::trajectory::clampToZNorm(vel_sp_constrained, z_speed, 0.5f);
|
||||
// Compute XY and Z velocity components independently to avoid
|
||||
// Z clamping from reducing XY speed (and vice versa)
|
||||
const Vector3f pos_to_dest = crossing_point - pos_traj;
|
||||
const Vector2f u_pos_to_dest_xy = Vector2f(pos_to_dest).unit_or_zero();
|
||||
const float z_sign = matrix::sign(pos_to_dest(2));
|
||||
|
||||
Vector3f vel_sp_constrained{
|
||||
u_pos_to_dest_xy(0) *xy_speed,
|
||||
u_pos_to_dest_xy(1) *xy_speed,
|
||||
z_sign *z_speed
|
||||
};
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
// If available, use the existing velocity as a feedforward, otherwise replace it
|
||||
|
||||
Reference in new issue
Block a user