From d6cece756cc1fd9c8e6ca6290bdc7b0fd6d69d5b Mon Sep 17 00:00:00 2001 From: YassineYousfi Date: Mon, 8 Jan 2024 19:46:13 -0800 Subject: [PATCH] longplanner: expose dt (#30941) --- selfdrive/controls/lib/longitudinal_planner.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 13127b4634..018768f248 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -47,13 +47,14 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): class LongitudinalPlanner: - def __init__(self, CP, init_v=0.0, init_a=0.0): + def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.CP = CP self.mpc = LongitudinalMpc() self.fcw = False + self.dt = dt self.a_desired = init_a - self.v_desired_filter = FirstOrderFilter(init_v, 2.0, DT_MDL) + self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt) self.v_model_error = 0.0 self.v_desired_trajectory = np.zeros(CONTROL_N) @@ -148,8 +149,8 @@ class LongitudinalPlanner: # Interpolate 0.05 seconds and save as starting point for next iteration a_prev = self.a_desired - self.a_desired = float(interp(DT_MDL, ModelConstants.T_IDXS[:CONTROL_N], self.a_desired_trajectory)) - self.v_desired_filter.x = self.v_desired_filter.x + DT_MDL * (self.a_desired + a_prev) / 2.0 + self.a_desired = float(interp(self.dt, ModelConstants.T_IDXS[:CONTROL_N], self.a_desired_trajectory)) + self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0 def publish(self, sm, pm): plan_send = messaging.new_message('longitudinalPlan')