|
|
|
@ -69,6 +69,7 @@ class LongitudinalPlanner: |
|
|
|
|
self.mpc = LongitudinalMpc(dt=dt) |
|
|
|
|
self.fcw = False |
|
|
|
|
self.dt = dt |
|
|
|
|
self.enable_frames = 0 |
|
|
|
|
|
|
|
|
|
self.a_desired = init_a |
|
|
|
|
self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt) |
|
|
|
@ -105,8 +106,13 @@ class LongitudinalPlanner: |
|
|
|
|
long_control_off = sm['controlsState'].longControlState == LongCtrlState.off |
|
|
|
|
force_slow_decel = sm['controlsState'].forceDecel |
|
|
|
|
|
|
|
|
|
if not long_control_off: |
|
|
|
|
self.enable_frames += 1 |
|
|
|
|
else: |
|
|
|
|
self.enable_frames = 0 |
|
|
|
|
|
|
|
|
|
# Reset current state when not engaged, or user is controlling the speed |
|
|
|
|
reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['selfdriveState'].enabled |
|
|
|
|
reset_state = (long_control_off or self.enable_frames < 10) if self.CP.openpilotLongitudinalControl else not sm['selfdriveState'].enabled |
|
|
|
|
|
|
|
|
|
# No change cost when user is controlling the speed, or when standstill |
|
|
|
|
prev_accel_constraint = not (reset_state or sm['carState'].standstill) |
|
|
|
|