From c66c6fa16adb9761d70d93b84a78156b34e487d8 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 7 Mar 2024 03:08:08 -0800 Subject: [PATCH] dang, we're dropping messages --- selfdrive/controls/lib/longitudinal_planner.py | 7 ++++--- selfdrive/controls/plannerd.py | 3 ++- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 39450448fa..eb67b999b4 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -90,12 +90,13 @@ class LongitudinalPlanner: j = np.zeros(len(T_IDXS_MPC)) return x, v, a, j - def update(self, sm): + def update(self, sm, carState_sock): # if self.param_read_counter % 50 == 0: # self.read_param() # decrement personality on button press (Toyota can only decrement) - if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in sm['carState'].buttonEvents): - self.personality = (self.personality - 1) % 3 + 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.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 eeeeda050e..aaba814abc 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -30,11 +30,12 @@ def plannerd_thread(): pm = messaging.PubMaster(['longitudinalPlan', 'uiPlan']) sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'], poll='modelV2', ignore_avg_freq=['radarState']) + carState_sock = messaging.sub_sock('carState') while True: sm.update() if sm.updated['modelV2']: - longitudinal_planner.update(sm) + longitudinal_planner.update(sm, carState_sock) longitudinal_planner.publish(sm, pm) publish_ui_plan(sm, pm, longitudinal_planner)