Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 04:06:33 +08:00
Code Issues Actions 10 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-25 14:17:56 +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
src/lib/flight_tasks/tasks/AutoLineSmoothVel/TrajectoryConstraints.hpp
+39 -15
View File
@@ -126,30 +126,54 @@ float computeXYSpeedFromWaypoints(const Vector3f waypoints[N], const VehicleDyna
return max_speed;
}
inline bool clampToXYNorm(Vector3f &target, float max_xy_norm)
/*
* Constrain the 3D vector given a maximum XY norm
* If the XY norm of the 3D vector is larger than the maximum norm, the whole vector
* is scaled down to respect the constraint.
* If the maximum norm is small (defined by the "accuracy" parameter),
* only the XY components are scaled down to avoid affecting
* Z in case of numerical issues
*/
inline void clampToXYNorm(Vector3f &target, float max_xy_norm, float accuracy = FLT_EPSILON)
{
const float xynorm = target.xy().norm();
const float scale_factor = max_xy_norm / xynorm;
const float scale_factor = (xynorm > FLT_EPSILON)
? max_xy_norm / xynorm
: 1.f;
if (scale_factor < 1 && PX4_ISFINITE(scale_factor) && xynorm > FLT_EPSILON) {
target *= scale_factor;
return true;
if (scale_factor < 1.f) {
if (max_xy_norm < accuracy && xynorm < accuracy) {
target.xy() = Vector2f(target) * scale_factor;
} else {
target *= scale_factor;
}
}
return false;
}
inline bool clampToZNorm(Vector3f &target, float max_z_norm)
/*
* Constrain the 3D vector given a maximum Z norm
* If the Z component of the 3D vector is larger than the maximum norm, the whole vector
* is scaled down to respect the constraint.
* If the maximum norm is small (defined by the "accuracy parameter),
* only the Z component is scaled down to avoid affecting
* XY in case of numerical issues
*/
inline void clampToZNorm(Vector3f &target, float max_z_norm, float accuracy = FLT_EPSILON)
{
float znorm = fabs(target(2));
const float scale_factor = max_z_norm / znorm;
const float znorm = fabs(target(2));
const float scale_factor = (znorm > FLT_EPSILON)
? max_z_norm / znorm
: 1.f;
if (scale_factor < 1 && PX4_ISFINITE(scale_factor) && znorm > FLT_EPSILON) {
target *= scale_factor;
return true;
if (scale_factor < 1.f) {
if (max_z_norm < accuracy && znorm < accuracy) {
target(2) *= scale_factor;
} else {
target *= scale_factor;
}
}
return false;
}
}