Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-27 02:06:27 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

FlightTaskAutoLine/SmoothVel: stop at waypoint if altitude is has not been reached yet

Browse Source
This commit is contained in:
Dennis Mannhart
2019-07-18 13:53:39 +02:00
committed by Mathieu Bresciani
parent 255c911155
commit 9e9b2ab9e8
2 changed files with 14 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp
+1 -1
View File
@@ -93,7 +93,7 @@ void FlightTaskAutoLine::_generateXYsetpoints()
{ {
_setSpeedAtTarget(); _setSpeedAtTarget();
Vector2f pos_sp_to_dest(_target - _position_setpoint); Vector2f pos_sp_to_dest(_target - _position_setpoint);
const bool has_reached_altitude = fabsf(_target(2) - _position(2)) < _target_acceptance_radius; const bool has_reached_altitude = fabsf(_target(2) - _position(2)) < _param_nav_mc_alt_rad.get();
if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < _target_acceptance_radius) || if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < _target_acceptance_radius) ||
(!has_reached_altitude && pos_sp_to_dest.length() < _target_acceptance_radius)) { (!has_reached_altitude && pos_sp_to_dest.length() < _target_acceptance_radius)) {
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+13 -7
View File
@@ -243,17 +243,23 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
// Use position setpoints to generate velocity setpoints // Use position setpoints to generate velocity setpoints
// Get various path specific vectors. */ // Get various path specific vectors. */
Vector2f pos_traj; Vector3f pos_traj;
pos_traj(0) = _trajectory[0].getCurrentPosition(); pos_traj(0) = _trajectory[0].getCurrentPosition();
pos_traj(1) = _trajectory[1].getCurrentPosition(); pos_traj(1) = _trajectory[1].getCurrentPosition();
Vector2f pos_sp_xy(_position_setpoint); pos_traj(2) = _trajectory[2].getCurrentPosition();
Vector2f pos_traj_to_dest(pos_sp_xy - pos_traj); Vector2f pos_traj_to_dest_xy(_position_setpoint - pos_traj);
Vector2f prev_to_pos(pos_traj - Vector2f(_prev_wp)); Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero());
Vector2f u_pos_traj_to_dest_xy(Vector2f(pos_traj_to_dest).unit_or_zero()); const bool has_reached_altitude = fabsf(_position_setpoint(2) - pos_traj(2)) < _param_nav_mc_alt_rad.get();
float speed_sp_track = _getMaxSpeedFromDistance(pos_traj_to_dest.length()); float speed_sp_track = _getMaxSpeedFromDistance(pos_traj_to_dest_xy.length());
speed_sp_track = math::constrain(speed_sp_track, _getSpeedAtTarget(), _mc_cruise_speed); if (has_reached_altitude) {
speed_sp_track = math::constrain(speed_sp_track, _getSpeedAtTarget(), _mc_cruise_speed);
} else {
speed_sp_track = math::constrain(speed_sp_track, 0.0f, _mc_cruise_speed);
}
Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * speed_sp_track; Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * speed_sp_track;