mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-24 15:32:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

longplanner: expose dt (#30941)

Browse Source
This commit is contained in:
YassineYousfi
2024-01-08 19:46:13 -08:00
committed by GitHub
parent 88e445ef15
commit d6cece756c
1 changed files with 5 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_planner.py
+5 -4
View File
@@ -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')