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

Auto - Update velocity setpoint generator to avoid overshoot at high cruise speed. 

The linear mapping from position error to cruise velocity is changed by
a combination of that linear mapping and a nonlinear function containing
the maximum acceleration and jerk to avoid overshoots at waypoints due to
overoptimistic breaking distance.
This commit is contained in:
bresch 2019-07-04 18:03:12 +02:00 committed by Mathieu Bresciani
parent 59c555aec3
commit 6df1600357
1 changed files with 8 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -160,7 +160,14 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
Vector2f closest_pt = Vector2f(_prev_wp) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest);
Vector2f u_pos_traj_to_dest_xy(Vector2f(pos_traj_to_dest).unit_or_zero());
float speed_sp_track = Vector2f(pos_traj_to_dest).length() * _param_mpc_xy_traj_p.get();
// Compute the maximum possible velocity on the track given the remaining distance, the maximum acceleration and the maximum jerk.
// We assume a constant acceleration profile with a delay of 2*accel/jerk (time to reach the desired acceleration from opposite max acceleration)
// Equation to solve: 0 = vel^2 - 2*acc*(x - vel*2*acc/jerk)
// To avoid high gain at low distance due to the sqrt, we take the minimum of this velocity and a slope of "traj_p" m/s per meter
float b = 4.f * _param_mpc_acc_hor.get() * _param_mpc_acc_hor.get() / _param_mpc_jerk_auto.get();
float c = - 2.f * _param_mpc_acc_hor.get() * pos_traj_to_dest.length();
float max_speed = 0.5f * (-b + sqrtf(b * b - 4.f * c));
float speed_sp_track = math::min(max_speed, pos_traj_to_dest.length() * _param_mpc_xy_traj_p.get());
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;