dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp
View File

@ -93,7 +93,7 @@ void FlightTaskAutoLine::_generateXYsetpoints()
{
_setSpeedAtTarget();
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) ||
(!has_reached_altitude && pos_sp_to_dest.length() < _target_acceptance_radius)) {

20
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
View File

@ -243,17 +243,23 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
// Use position setpoints to generate velocity setpoints
// Get various path specific vectors. */
Vector2f pos_traj;
Vector3f pos_traj;
pos_traj(0) = _trajectory[0].getCurrentPosition();
pos_traj(1) = _trajectory[1].getCurrentPosition();
Vector2f pos_sp_xy(_position_setpoint);
Vector2f pos_traj_to_dest(pos_sp_xy - pos_traj);
Vector2f prev_to_pos(pos_traj - Vector2f(_prev_wp));
Vector2f u_pos_traj_to_dest_xy(Vector2f(pos_traj_to_dest).unit_or_zero());
pos_traj(2) = _trajectory[2].getCurrentPosition();
Vector2f pos_traj_to_dest_xy(_position_setpoint - pos_traj);
Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.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;