From 6df16003570645779f151ab38f368d3da18e0710 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 4 Jul 2019 18:03:12 +0200 Subject: [PATCH] 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. --- .../AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index 4f1eb70c8195..2e49d4187618 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -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;