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

AutoSmoothVel: desynchronize XY from Z for small changes in speed

Browse Source 
This fixes the issue that makes the drone slow-down even in straight
lines due to the Z component being constrained to a really small value
This commit is contained in:
bresch
2020-03-26 08:31:09 +01:00
committed by Mathieu Bresciani
parent 2103b8f7de
commit dc38930bb2
2 changed files with 41 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+2 -2
View File
@@ -263,8 +263,8 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
const float z_speed = _getMaxZSpeed();
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);
math::trajectory::clampToZNorm(vel_sp_constrained, z_speed);
math::trajectory::clampToXYNorm(vel_sp_constrained, xy_speed, 0.5f);
math::trajectory::clampToZNorm(vel_sp_constrained, z_speed, 0.5f);
for (int i = 0; i < 3; i++) {
// If available, use the existing velocity as a feedforward, otherwise replace it