From f797567ef8c574027eec8da72a2c76cfb2fdafe1 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 5 Jul 2022 22:19:20 -0700 Subject: [PATCH] long planner: run when using stock longitudinal (#25017) * Squashed commit of the following: commit e27a5b4e2bfeab4e6a47440b1d4eb180ee4acf49 Author: Shane Smiskol Date: Fri Jul 1 14:10:06 2022 -0700 remove this test remove this test commit c3c10af82222ea4641d94c53a3a07b486cca8452 Author: Shane Smiskol Date: Fri Jul 1 14:08:15 2022 -0700 only planner changes commit 50e0f1d8704c15acfce8987faf3515c99e8af4f4 Merge: e088fde67 fcc5b3d70 Author: Shane Smiskol Date: Fri Jul 1 14:05:36 2022 -0700 Merge remote-tracking branch 'upstream/master' into enable-planner2 commit e088fde67edcc32ccfeea23b4ae9e44845240429 Author: Shane Smiskol Date: Thu Jun 30 13:58:38 2022 -0700 no walrus commit b7b425e530e949b9cc427833562473cc241d1367 Merge: f8634266b c49f997be Author: Shane Smiskol Date: Thu Jun 30 13:54:30 2022 -0700 Merge remote-tracking branch 'upstream/master' into enable-planner commit f8634266b49c3f692b255e6cfac66cccc438ae20 Author: Shane Smiskol Date: Wed Jun 29 16:07:35 2022 -0700 stricter test, speeds[-1] is 0.14 when starting here commit c9e446ad2183feba9d03ee39f9801091ab791c08 Merge: e6c4106ea 879a7c320 Author: Shane Smiskol Date: Wed Jun 29 16:01:32 2022 -0700 Merge remote-tracking branch 'upstream/master' into enable-planner commit e6c4106ea185c68a6c7b3d59d5bde664df8bdc9c Author: Shane Smiskol Date: Sat Jun 25 03:28:41 2022 -0700 fix test commit 0520c7f21613b57b804e08a8e8d10950ac059074 Author: Shane Smiskol Date: Sat Jun 25 03:26:16 2022 -0700 add test for resuming commit 04db5f80bff4a002f5241765a625d7cf57b74364 Merge: e23b37d3f d8bfe2f00 Author: Shane Smiskol Date: Wed Jun 22 20:15:50 2022 -0700 Merge remote-tracking branch 'upstream/master' into enable-planner commit e23b37d3fe8dd3dd07b46a32a4f0564fabade1aa Author: Shane Smiskol Date: Tue Jun 21 12:46:04 2022 -0700 0.1 should be pretty safe commit e7dc3960da3d713753f28732f50dbd25811fad28 Author: Shane Smiskol Date: Tue Jun 21 12:39:30 2022 -0700 try 0.2 commit ff0597ec92a0d2c52915316961ec123b0183c5cf Author: Shane Smiskol Date: Tue Jun 21 11:34:00 2022 -0700 Always run planner if not opLong commit 13997c55271f79fd3ca62d6db45ec3790b09aa60 Merge: d2f51ee55 95d8517a8 Author: Shane Smiskol Date: Tue Jun 21 11:29:22 2022 -0700 Merge remote-tracking branch 'upstream/master' into enable-planner commit d2f51ee55fd3bde38275371e76714d7741bc6f6b Author: Shane Smiskol Date: Tue Jun 21 11:27:45 2022 -0700 same for non-HDA2 commit 6a63bd60f09a0abd9185049cd173100d3ef6fefa Author: Shane Smiskol Date: Mon Jun 20 23:37:07 2022 -0700 mazda: ensure no resume if cancelling commit 5771cdecab7999765d9f5203c75a67f1555cf975 Author: Shane Smiskol Date: Mon Jun 20 23:27:58 2022 -0700 maintain original button msg rate commit 6c1fe0606fd0a0819ffeaac92526e43b3110f2f4 Author: Shane Smiskol Date: Wed Jun 15 23:45:26 2022 -0700 rename to resume commit 00b1df652f1679137c769f9db61eed7dd14e1542 Author: Shane Smiskol Date: Wed Jun 15 21:57:54 2022 -0700 remove comments commit 325ea9bbd5e0dd946961ede0cdcc446ad5e5bbdb Author: Shane Smiskol Date: Wed Jun 15 21:56:20 2022 -0700 vw commit 2c9061042b36fe1d6b029a4216655be69a980849 Author: Shane Smiskol Date: Wed Jun 15 21:54:37 2022 -0700 do rest but vw commit 3dc51f663dfdd4ea1fd72d239bcd5db8c7da4b47 Author: Shane Smiskol Date: Wed Jun 15 16:34:48 2022 -0700 only spam resume when future is > vEgoStarting commit 5f32cd1fcb402bee425d866a9dc76b6feea3d241 Author: Shane Smiskol Date: Wed Jun 15 16:09:43 2022 -0700 always log leads, we hide them in ui * reset when not CS.enabled remove comment * update refs --- selfdrive/controls/lib/longitudinal_planner.py | 4 ++-- selfdrive/controls/radard.py | 16 ++++++---------- selfdrive/test/process_replay/ref_commit | 2 +- 3 files changed, 9 insertions(+), 13 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index d4a6aaef8f..cf51136770 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -66,11 +66,11 @@ class Planner: v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX) v_cruise = v_cruise_kph * CV.KPH_TO_MS - long_control_state = sm['controlsState'].longControlState + long_control_off = sm['controlsState'].longControlState == LongCtrlState.off force_slow_decel = sm['controlsState'].forceDecel # Reset current state when not engaged, or user is controlling the speed - reset_state = long_control_state == LongCtrlState.off + reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['controlsState'].enabled # No change cost when user is controlling the speed, or when standstill prev_accel_constraint = not (reset_state or sm['carState'].standstill) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index b2c9914457..3d958139d6 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -102,7 +102,7 @@ class RadarD(): self.ready = False - def update(self, sm, rr, enable_lead): + def update(self, sm, rr): self.current_time = 1e-9*max(sm.logMonoTime.values()) if sm.updated['carState']: @@ -169,11 +169,10 @@ class RadarD(): radarState.radarErrors = list(rr.errors) radarState.carStateMonoTime = sm.logMonoTime['carState'] - if enable_lead: - leads_v3 = sm['modelV2'].leadsV3 - if len(leads_v3) > 1: - radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], low_speed_override=True) - radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], low_speed_override=False) + leads_v3 = sm['modelV2'].leadsV3 + if len(leads_v3) > 1: + radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], low_speed_override=True) + radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], low_speed_override=False) return dat @@ -203,9 +202,6 @@ def radard_thread(sm=None, pm=None, can_sock=None): rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None) RD = RadarD(CP.radarTimeStep, RI.delay) - # TODO: always log leads once we can hide them conditionally - enable_lead = CP.openpilotLongitudinalControl or not CP.radarOffCan - while 1: can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) rr = RI.update(can_strings) @@ -215,7 +211,7 @@ def radard_thread(sm=None, pm=None, can_sock=None): sm.update(0) - dat = RD.update(sm, rr, enable_lead) + dat = RD.update(sm, rr) dat.radarState.cumLagMs = -rk.remaining*1000. pm.send('radarState', dat) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 90444b1fa7..999081b4df 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -806984d4206056fb132625c5dad6c0ca1835a2d6 \ No newline at end of file +a57bbbffbee434e59e08b98b667dc13b6b505f08 \ No newline at end of file