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

Fix bad assumption: _position_setpoint is normally at _target

Browse Source
This commit is contained in:
Julian Kent
2020-01-06 15:39:07 +01:00
committed by Mathieu Bresciani
parent 603a993380
commit 33702be3e4
1 changed files with 10 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+10 -2
View File
@@ -197,7 +197,12 @@ float FlightTaskAutoLineSmoothVel::_getMaxXYSpeed() const
// constrain velocity to go to the position setpoint first if the position setpoint has been modified by an external source // constrain velocity to go to the position setpoint first if the position setpoint has been modified by an external source
// (eg. Obstacle Avoidance) // (eg. Obstacle Avoidance)
if ((pos_traj - _position_setpoint).longerThan(_target_acceptance_radius)) { bool xy_valid = PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1));
bool xy_modified = xy_valid && Vector2f((_target - _position_setpoint).xy()).longerThan(FLT_EPSILON);
bool z_valid = PX4_ISFINITE(_position_setpoint(2));
bool z_modified = z_valid && fabsf((_target - _position_setpoint)(2)) > FLT_EPSILON;
if (xy_modified || z_modified) {
waypoints[1] = _position_setpoint; waypoints[1] = _position_setpoint;
waypoints[2] = _target; waypoints[2] = _target;
@@ -220,7 +225,10 @@ float FlightTaskAutoLineSmoothVel::_getMaxZSpeed() const
// constrain velocity to go to the position setpoint first if the position setpoint has been modified by an external source // constrain velocity to go to the position setpoint first if the position setpoint has been modified by an external source
// (eg. Obstacle Avoidance) // (eg. Obstacle Avoidance)
if ((pos_traj - _position_setpoint).longerThan(_target_acceptance_radius)) { bool z_valid = PX4_ISFINITE(_position_setpoint(2));
bool z_modified = z_valid && fabsf((_target - _position_setpoint)(2)) > FLT_EPSILON;
if (z_modified) {
z_setpoint = _position_setpoint(2); z_setpoint = _position_setpoint(2);
} }