Revert "odd drain sock proc replay behavior"

This reverts commit 29b70b3941.
pull/31792/head
Shane Smiskol 1 year ago
parent 787db54cd4
commit 8368cb69cb
  1. 9
      selfdrive/controls/lib/longitudinal_planner.py
  2. 8
      selfdrive/controls/plannerd.py
  3. 3
      selfdrive/test/longitudinal_maneuvers/plant.py

@ -93,14 +93,15 @@ class LongitudinalPlanner:
j = np.zeros(len(T_IDXS_MPC)) j = np.zeros(len(T_IDXS_MPC))
return x, v, a, j return x, v, a, j
def update(self, sm, buttonEvents): def update(self, sm, carState_sock):
if self.param_read_counter % 50 == 0: if self.param_read_counter % 50 == 0:
self.read_param() self.read_param()
# decrement personality on distance button press # decrement personality on distance button press
if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in buttonEvents): for m in messaging.drain_sock(carState_sock, wait_for_one=False):
self.personality = (self.personality - 1) % 3 if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in m.carState.buttonEvents):
self.write_param() self.personality = (self.personality - 1) % 3
self.write_param()
self.param_read_counter += 1 self.param_read_counter += 1
self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'

@ -30,18 +30,14 @@ def plannerd_thread():
pm = messaging.PubMaster(['longitudinalPlan', 'uiPlan']) pm = messaging.PubMaster(['longitudinalPlan', 'uiPlan'])
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'], sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'],
poll='modelV2', ignore_avg_freq=['radarState']) poll='modelV2', ignore_avg_freq=['radarState'])
carState_sock = messaging.sub_sock('carState')
buttonEvents = []
while True: while True:
sm.update() sm.update()
if sm.updated['carState']:
buttonEvents.extend(sm['carState'].buttonEvents)
if sm.updated['modelV2']: if sm.updated['modelV2']:
longitudinal_planner.update(sm, buttonEvents) longitudinal_planner.update(sm, carState_sock)
longitudinal_planner.publish(sm, pm) longitudinal_planner.publish(sm, pm)
publish_ui_plan(sm, pm, longitudinal_planner) publish_ui_plan(sm, pm, longitudinal_planner)
buttonEvents.clear()
def main(): def main():
plannerd_thread() plannerd_thread()

@ -50,6 +50,7 @@ class Plant:
from openpilot.selfdrive.car.honda.interface import CarInterface from openpilot.selfdrive.car.honda.interface import CarInterface
self.planner = LongitudinalPlanner(CarInterface.get_non_essential_params(CAR.CIVIC), init_v=self.speed) self.planner = LongitudinalPlanner(CarInterface.get_non_essential_params(CAR.CIVIC), init_v=self.speed)
self.carState_sock = messaging.sub_sock('carState')
@property @property
def current_time(self): def current_time(self):
@ -121,7 +122,7 @@ class Plant:
'carState': car_state.carState, 'carState': car_state.carState,
'controlsState': control.controlsState, 'controlsState': control.controlsState,
'modelV2': model.modelV2} 'modelV2': model.modelV2}
self.planner.update(sm, []) self.planner.update(sm, self.carState_sock)
self.speed = self.planner.v_desired_filter.x self.speed = self.planner.v_desired_filter.x
self.acceleration = self.planner.a_desired self.acceleration = self.planner.a_desired
self.speeds = self.planner.v_desired_trajectory.tolist() self.speeds = self.planner.v_desired_trajectory.tolist()

Loading…
Cancel
Save