fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-05 19:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Avoidance interface should be a stop point

Browse Source
This commit is contained in:
Julian Kent
2019-12-13 17:32:35 +01:00
committed by Mathieu Bresciani
parent 33702be3e4
commit af961f2391
1 changed files with 5 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+5 -10
View File
@@ -192,9 +192,6 @@ float FlightTaskAutoLineSmoothVel::_getMaxXYSpeed() const
config.max_speed_xy = _mc_cruise_speed;
config.max_acc_xy_radius_scale = _param_mpc_xy_traj_p.get();
Vector3f waypoints[3];
waypoints[0] = pos_traj;
// constrain velocity to go to the position setpoint first if the position setpoint has been modified by an external source
// (eg. Obstacle Avoidance)
bool xy_valid = PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1));
@@ -202,18 +199,16 @@ float FlightTaskAutoLineSmoothVel::_getMaxXYSpeed() const
bool z_valid = PX4_ISFINITE(_position_setpoint(2));
bool z_modified = z_valid && fabsf((_target - _position_setpoint)(2)) > FLT_EPSILON;
float max_xy_speed;
if (xy_modified || z_modified) {
waypoints[1] = _position_setpoint;
waypoints[2] = _target;
max_xy_speed = math::trajectory::computeXYSpeedFromWaypoints<2>({pos_traj, _position_setpoint}, config);
} else {
waypoints[1] = _target;
waypoints[2] = _next_wp;
max_xy_speed = math::trajectory::computeXYSpeedFromWaypoints<3>({pos_traj, _target, _next_wp}, config);
}
float max_speed = math::trajectory::computeXYSpeedFromWaypoints<3>(waypoints, config);
return max_speed;
return max_xy_speed;
}
float FlightTaskAutoLineSmoothVel::_getMaxZSpeed() const