From bd00459eb6dc3287bccf21c67e9bba6c8df0e9d2 Mon Sep 17 00:00:00 2001 From: ClockeNessMnstr Date: Tue, 7 Jun 2022 12:41:03 -0400 Subject: [PATCH] update DH names + notes for MPC output curvatures (#24701) * update names + notes for MPC outputs "current_curvature" is not the correct description of what the MPC is outputting in it's curvature_ego state. The MPC is integrating it's free variable, curvature_rate, such that curvature[0] is actually the desired_curvature before any delay. inversely: the curvature_rate_desired is the desired rate of change to the setpoint and not the actual curvature rate. If we were to set the initial curvature = measured curvature in the MPC initiation these names would be correct. This was possibly how it was initially set up but the nomenclature here is now confusing. * more notes * match * Clarify #1 --- selfdrive/controls/lib/drive_helpers.py | 23 +++++++++++---------- selfdrive/controls/lib/latcontrol_indi.py | 1 + selfdrive/controls/lib/latcontrol_torque.py | 2 ++ selfdrive/controls/lib/lateral_planner.py | 3 +++ 4 files changed, 18 insertions(+), 11 deletions(-) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index ca5c1d3329d6908..b48a6380143c3a0 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -90,26 +90,27 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates): psis = [0.0]*CONTROL_N curvatures = [0.0]*CONTROL_N curvature_rates = [0.0]*CONTROL_N - + v_ego = max(v_ego, 0.1) + # TODO this needs more thought, use .2s extra for now to estimate other delays delay = CP.steerActuatorDelay + .2 - current_curvature = curvatures[0] - psi = interp(delay, T_IDXS[:CONTROL_N], psis) - desired_curvature_rate = curvature_rates[0] - + # MPC can plan to turn the wheel and turn back before t_delay. This means # in high delay cases some corrections never even get commanded. So just use # psi to calculate a simple linearization of desired curvature - curvature_diff_from_psi = psi / (max(v_ego, 1e-1) * delay) - current_curvature - desired_curvature = current_curvature + 2 * curvature_diff_from_psi - - v_ego = max(v_ego, 0.1) + current_curvature_desired = curvatures[0] + psi = interp(delay, T_IDXS[:CONTROL_N], psis) + average_curvature_desired = psi / (v_ego * delay) + desired_curvature = 2 * average_curvature_desired - current_curvature_desired + + # This is the "desired rate of the setpoint" not an actual desired rate + desired_curvature_rate = curvature_rates[0] max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) safe_desired_curvature_rate = clip(desired_curvature_rate, -max_curvature_rate, max_curvature_rate) safe_desired_curvature = clip(desired_curvature, - current_curvature - max_curvature_rate * DT_MDL, - current_curvature + max_curvature_rate * DT_MDL) + current_curvature_desired - max_curvature_rate * DT_MDL, + current_curvature_desired + max_curvature_rate * DT_MDL) return safe_desired_curvature, safe_desired_curvature_rate diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index b5041eb172fc2cd..79c881d11be37e4 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -78,6 +78,7 @@ def update(self, active, CS, VM, params, last_actuators, desired_curvature, desi steers_des += math.radians(params.angleOffsetDeg) indi_log.steeringAngleDesiredDeg = math.degrees(steers_des) + # desired rate is the desired rate of change in the setpoint, not the absolute desired curvature rate_des = VM.get_steer_from_curvature(-desired_curvature_rate, CS.vEgo, 0) indi_log.steeringRateDesiredDeg = math.degrees(rate_des) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 820862af9e26e94..45157ee3279ca2d 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -59,6 +59,8 @@ def update(self, active, CS, VM, params, last_actuators, desired_curvature, desi actual_curvature_llk = llk.angularVelocityCalibrated.value[2] / CS.vEgo actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk]) desired_lateral_accel = desired_curvature * CS.vEgo ** 2 + + # desired rate is the desired rate of change in the setpoint, not the absolute desired curvature desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2 actual_lateral_accel = actual_curvature * CS.vEgo ** 2 diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index b14124238185a3b..554a4eb8fb1df93 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -79,6 +79,9 @@ def update(self, sm): y_pts, heading_pts) # init state for next + # mpc.u_sol is the desired curvature rate given x0 curv state. + # with x0[3] = measured_curvature, this would be the actual desired rate. + # instead, interpolate x_sol so that x0[3] is the desired curvature for lat_control. self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3]) # Check for infeasible MPC solution