diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 96c993eec7..eb895c3e86 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -93,14 +93,15 @@ class LongitudinalPlanner: j = np.zeros(len(T_IDXS_MPC)) return x, v, a, j - def update(self, sm, buttonEvents): + def update(self, sm, carState_sock): if self.param_read_counter % 50 == 0: self.read_param() # decrement personality on distance button press - if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in buttonEvents): - self.personality = (self.personality - 1) % 3 - self.write_param() + for m in messaging.drain_sock(carState_sock, wait_for_one=False): + if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in m.carState.buttonEvents): + self.personality = (self.personality - 1) % 3 + self.write_param() self.param_read_counter += 1 self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 32660dcb02..aaba814abc 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -30,18 +30,14 @@ def plannerd_thread(): pm = messaging.PubMaster(['longitudinalPlan', 'uiPlan']) sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'], poll='modelV2', ignore_avg_freq=['radarState']) - - buttonEvents = [] + carState_sock = messaging.sub_sock('carState') while True: sm.update() - if sm.updated['carState']: - buttonEvents.extend(sm['carState'].buttonEvents) if sm.updated['modelV2']: - longitudinal_planner.update(sm, buttonEvents) + longitudinal_planner.update(sm, carState_sock) longitudinal_planner.publish(sm, pm) publish_ui_plan(sm, pm, longitudinal_planner) - buttonEvents.clear() def main(): plannerd_thread() diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 1be6e71e8b..7e9576fdec 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -50,6 +50,7 @@ class Plant: from openpilot.selfdrive.car.honda.interface import CarInterface self.planner = LongitudinalPlanner(CarInterface.get_non_essential_params(CAR.CIVIC), init_v=self.speed) + self.carState_sock = messaging.sub_sock('carState') @property def current_time(self): @@ -121,7 +122,7 @@ class Plant: 'carState': car_state.carState, 'controlsState': control.controlsState, 'modelV2': model.modelV2} - self.planner.update(sm, []) + self.planner.update(sm, self.carState_sock) self.speed = self.planner.v_desired_filter.x self.acceleration = self.planner.a_desired self.speeds = self.planner.v_desired_trajectory.tolist()