From 35f642312f3aef401f605e34d363c75e6b4cc956 Mon Sep 17 00:00:00 2001 From: John Christman Date: Mon, 5 Jan 2026 18:07:12 -0500 Subject: [PATCH 01/46] curve torque test --- selfdrive/ui/mici/onroad/hud_renderer.py | 4 ++-- selfdrive/ui/mici/onroad/torque_bar.py | 28 +++++++++++++----------- 2 files changed, 17 insertions(+), 15 deletions(-) diff --git a/selfdrive/ui/mici/onroad/hud_renderer.py b/selfdrive/ui/mici/onroad/hud_renderer.py index 7f489ccf98..2b8569934c 100644 --- a/selfdrive/ui/mici/onroad/hud_renderer.py +++ b/selfdrive/ui/mici/onroad/hud_renderer.py @@ -172,8 +172,8 @@ def _update_state(self) -> None: def _render(self, rect: rl.Rectangle) -> None: """Render HUD elements to the screen.""" - if ui_state.sm['controlsState'].lateralControlState.which() != 'angleState': - self._torque_bar.render(rect) + #if ui_state.sm['controlsState'].lateralControlState.which() != 'angleState': + self._torque_bar.render(rect) if self.is_cruise_set: self._draw_set_speed(rect) diff --git a/selfdrive/ui/mici/onroad/torque_bar.py b/selfdrive/ui/mici/onroad/torque_bar.py index c8485a3101..bb7829ac4b 100644 --- a/selfdrive/ui/mici/onroad/torque_bar.py +++ b/selfdrive/ui/mici/onroad/torque_bar.py @@ -162,19 +162,21 @@ def _update_state(self): # torque line if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState': - controls_state = ui_state.sm['controlsState'] - car_state = ui_state.sm['carState'] - live_parameters = ui_state.sm['liveParameters'] - lateral_acceleration = controls_state.curvature * car_state.vEgo ** 2 - live_parameters.roll * ACCELERATION_DUE_TO_GRAVITY - # TODO: pull from carparams - max_lateral_acceleration = 3 - - # from selfdrived - actual_lateral_accel = controls_state.curvature * car_state.vEgo ** 2 - desired_lateral_accel = controls_state.desiredCurvature * car_state.vEgo ** 2 - accel_diff = (desired_lateral_accel - actual_lateral_accel) - - self._torque_filter.update(min(max(lateral_acceleration / max_lateral_acceleration + accel_diff, -1), 1)) + self._torque_filter.update(-ui_state.sm['carOutput'].curvature / 0.02) + + # controls_state = ui_state.sm['controlsState'] + # car_state = ui_state.sm['carState'] + # live_parameters = ui_state.sm['liveParameters'] + # lateral_acceleration = controls_state.curvature * car_state.vEgo ** 2 - live_parameters.roll * ACCELERATION_DUE_TO_GRAVITY + # # TODO: pull from carparams + # max_lateral_acceleration = 3 + + # # from selfdrived + # actual_lateral_accel = controls_state.curvature * car_state.vEgo ** 2 + # desired_lateral_accel = controls_state.desiredCurvature * car_state.vEgo ** 2 + # accel_diff = (desired_lateral_accel - actual_lateral_accel) + + # self._torque_filter.update(min(max(lateral_acceleration / max_lateral_acceleration + accel_diff, -1), 1)) else: self._torque_filter.update(-ui_state.sm['carOutput'].actuatorsOutput.torque) From 3a9c6ba67d6ba803c53437ddac35883410511e02 Mon Sep 17 00:00:00 2001 From: John Christman Date: Mon, 5 Jan 2026 18:22:29 -0500 Subject: [PATCH 02/46] fix --- selfdrive/ui/mici/onroad/torque_bar.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/ui/mici/onroad/torque_bar.py b/selfdrive/ui/mici/onroad/torque_bar.py index bb7829ac4b..f76567e331 100644 --- a/selfdrive/ui/mici/onroad/torque_bar.py +++ b/selfdrive/ui/mici/onroad/torque_bar.py @@ -162,7 +162,7 @@ def _update_state(self): # torque line if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState': - self._torque_filter.update(-ui_state.sm['carOutput'].curvature / 0.02) + self._torque_filter.update(-ui_state.sm['carOutput'].actuatorsOutput.curvature / 0.02) # controls_state = ui_state.sm['controlsState'] # car_state = ui_state.sm['carState'] From ae2eb62eed79e99098040e919820c056a37a4841 Mon Sep 17 00:00:00 2001 From: John Christman Date: Mon, 5 Jan 2026 19:06:51 -0500 Subject: [PATCH 03/46] undo --- selfdrive/ui/mici/onroad/torque_bar.py | 28 ++++++++++++-------------- 1 file changed, 13 insertions(+), 15 deletions(-) diff --git a/selfdrive/ui/mici/onroad/torque_bar.py b/selfdrive/ui/mici/onroad/torque_bar.py index f76567e331..c8485a3101 100644 --- a/selfdrive/ui/mici/onroad/torque_bar.py +++ b/selfdrive/ui/mici/onroad/torque_bar.py @@ -162,21 +162,19 @@ def _update_state(self): # torque line if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState': - self._torque_filter.update(-ui_state.sm['carOutput'].actuatorsOutput.curvature / 0.02) - - # controls_state = ui_state.sm['controlsState'] - # car_state = ui_state.sm['carState'] - # live_parameters = ui_state.sm['liveParameters'] - # lateral_acceleration = controls_state.curvature * car_state.vEgo ** 2 - live_parameters.roll * ACCELERATION_DUE_TO_GRAVITY - # # TODO: pull from carparams - # max_lateral_acceleration = 3 - - # # from selfdrived - # actual_lateral_accel = controls_state.curvature * car_state.vEgo ** 2 - # desired_lateral_accel = controls_state.desiredCurvature * car_state.vEgo ** 2 - # accel_diff = (desired_lateral_accel - actual_lateral_accel) - - # self._torque_filter.update(min(max(lateral_acceleration / max_lateral_acceleration + accel_diff, -1), 1)) + controls_state = ui_state.sm['controlsState'] + car_state = ui_state.sm['carState'] + live_parameters = ui_state.sm['liveParameters'] + lateral_acceleration = controls_state.curvature * car_state.vEgo ** 2 - live_parameters.roll * ACCELERATION_DUE_TO_GRAVITY + # TODO: pull from carparams + max_lateral_acceleration = 3 + + # from selfdrived + actual_lateral_accel = controls_state.curvature * car_state.vEgo ** 2 + desired_lateral_accel = controls_state.desiredCurvature * car_state.vEgo ** 2 + accel_diff = (desired_lateral_accel - actual_lateral_accel) + + self._torque_filter.update(min(max(lateral_acceleration / max_lateral_acceleration + accel_diff, -1), 1)) else: self._torque_filter.update(-ui_state.sm['carOutput'].actuatorsOutput.torque) From f4b1ef0015ea680be48664d73481c4a255ba23b2 Mon Sep 17 00:00:00 2001 From: John Christman Date: Mon, 5 Jan 2026 19:33:24 -0500 Subject: [PATCH 04/46] try 2 --- selfdrive/ui/mici/onroad/torque_bar.py | 28 ++++++++++++++------------ 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/selfdrive/ui/mici/onroad/torque_bar.py b/selfdrive/ui/mici/onroad/torque_bar.py index c8485a3101..4a157bc0c1 100644 --- a/selfdrive/ui/mici/onroad/torque_bar.py +++ b/selfdrive/ui/mici/onroad/torque_bar.py @@ -162,19 +162,21 @@ def _update_state(self): # torque line if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState': - controls_state = ui_state.sm['controlsState'] - car_state = ui_state.sm['carState'] - live_parameters = ui_state.sm['liveParameters'] - lateral_acceleration = controls_state.curvature * car_state.vEgo ** 2 - live_parameters.roll * ACCELERATION_DUE_TO_GRAVITY - # TODO: pull from carparams - max_lateral_acceleration = 3 - - # from selfdrived - actual_lateral_accel = controls_state.curvature * car_state.vEgo ** 2 - desired_lateral_accel = controls_state.desiredCurvature * car_state.vEgo ** 2 - accel_diff = (desired_lateral_accel - actual_lateral_accel) - - self._torque_filter.update(min(max(lateral_acceleration / max_lateral_acceleration + accel_diff, -1), 1)) + self._torque_filter.update(-ui_state.sm['carControl'].actuators.curvature / 0.02) + + # controls_state = ui_state.sm['controlsState'] + # car_state = ui_state.sm['carState'] + # live_parameters = ui_state.sm['liveParameters'] + # lateral_acceleration = controls_state.curvature * car_state.vEgo ** 2 - live_parameters.roll * ACCELERATION_DUE_TO_GRAVITY + # # TODO: pull from carparams + # max_lateral_acceleration = 3 + + # # from selfdrived + # actual_lateral_accel = controls_state.curvature * car_state.vEgo ** 2 + # desired_lateral_accel = controls_state.desiredCurvature * car_state.vEgo ** 2 + # accel_diff = (desired_lateral_accel - actual_lateral_accel) + + # self._torque_filter.update(min(max(lateral_acceleration / max_lateral_acceleration + accel_diff, -1), 1)) else: self._torque_filter.update(-ui_state.sm['carOutput'].actuatorsOutput.torque) From 9e86e8eb98644633526a685a3acab81adcef4d02 Mon Sep 17 00:00:00 2001 From: John Christman Date: Tue, 6 Jan 2026 12:48:36 -0500 Subject: [PATCH 05/46] invert --- selfdrive/ui/mici/onroad/torque_bar.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/ui/mici/onroad/torque_bar.py b/selfdrive/ui/mici/onroad/torque_bar.py index 4a157bc0c1..d5e52f2357 100644 --- a/selfdrive/ui/mici/onroad/torque_bar.py +++ b/selfdrive/ui/mici/onroad/torque_bar.py @@ -162,7 +162,7 @@ def _update_state(self): # torque line if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState': - self._torque_filter.update(-ui_state.sm['carControl'].actuators.curvature / 0.02) + self._torque_filter.update(ui_state.sm['carControl'].actuators.curvature / 0.02) # controls_state = ui_state.sm['controlsState'] # car_state = ui_state.sm['carState'] From 54b3a5701dae9c735b47552bc9f8ad473080e2d6 Mon Sep 17 00:00:00 2001 From: John Christman Date: Tue, 6 Jan 2026 13:40:54 -0500 Subject: [PATCH 06/46] bound -1 to 1 --- selfdrive/ui/mici/onroad/torque_bar.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/ui/mici/onroad/torque_bar.py b/selfdrive/ui/mici/onroad/torque_bar.py index d5e52f2357..7d04db46a6 100644 --- a/selfdrive/ui/mici/onroad/torque_bar.py +++ b/selfdrive/ui/mici/onroad/torque_bar.py @@ -162,7 +162,7 @@ def _update_state(self): # torque line if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState': - self._torque_filter.update(ui_state.sm['carControl'].actuators.curvature / 0.02) + self._torque_filter.update(min(max(ui_state.sm['carControl'].actuators.curvature / 0.02, -1), 1)) # controls_state = ui_state.sm['controlsState'] # car_state = ui_state.sm['carState'] From fe9de0ca19d8a172fa4631c12e5729f16bfb4d7e Mon Sep 17 00:00:00 2001 From: John Christman Date: Tue, 6 Jan 2026 15:36:10 -0500 Subject: [PATCH 07/46] added per-car curvature_limit --- common/params_keys.h | 1 + .../opendbc/car/ford/carcontroller.py | 1 + selfdrive/ui/mici/onroad/torque_bar.py | 34 ++++++++++++------- 3 files changed, 24 insertions(+), 12 deletions(-) diff --git a/common/params_keys.h b/common/params_keys.h index 534b7b0c34..e178dbad5f 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -282,4 +282,5 @@ inline static std::unordered_map keys = { {"LC_PID_gain_UI", {PERSISTENT | BACKUP, FLOAT, "3.0"}}, {"disable_BP_lat_UI", {PERSISTENT | BACKUP, BOOL, "0"}}, {"vbatt_pause_charging", {PERSISTENT | BACKUP, FLOAT, "11.8"}}, + {"curvature_limit", {CLEAR_ON_MANAGER_START, FLOAT}}, // for torque bar display }; \ No newline at end of file diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index 8ab76d3390..c015395fec 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -83,6 +83,7 @@ def __init__(self, dbc_names, CP, CP_SP): #IntelligentCruiseButtonManagementInterface.__init__(self, CP, CP_SP) self.params = Params() + self.params.put("curvature_limit", float(0.02)) # Ford system max curvature self.packer = CANPacker(dbc_names[Bus.pt]) self.CAN = fordcan.CanBus(CP) diff --git a/selfdrive/ui/mici/onroad/torque_bar.py b/selfdrive/ui/mici/onroad/torque_bar.py index 7d04db46a6..7653f00510 100644 --- a/selfdrive/ui/mici/onroad/torque_bar.py +++ b/selfdrive/ui/mici/onroad/torque_bar.py @@ -2,6 +2,7 @@ import time from functools import wraps from collections import OrderedDict +from openpilot.common.params import Params import numpy as np import pyray as rl @@ -152,6 +153,9 @@ def __init__(self, demo: bool = False): self._torque_filter = FirstOrderFilter(0, 0.1, 1 / gui_app.target_fps) self._torque_line_alpha_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps) + self.params = Params() + self.curvature_limit = float(self.params.get("curvature_limit") or 0.0) + def update_filter(self, value: float): """Update the torque filter value (for demo mode).""" self._torque_filter.update(value) @@ -162,25 +166,31 @@ def _update_state(self): # torque line if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState': - self._torque_filter.update(min(max(ui_state.sm['carControl'].actuators.curvature / 0.02, -1), 1)) + if self.curvature_limit > 0: + self._torque_filter.update(min(max(ui_state.sm['carControl'].actuators.curvature / self.curvature_limit, -1), 1)) + else: + #incomplete implementation? + controls_state = ui_state.sm['controlsState'] + car_state = ui_state.sm['carState'] + live_parameters = ui_state.sm['liveParameters'] + lateral_acceleration = controls_state.curvature * car_state.vEgo ** 2 - live_parameters.roll * ACCELERATION_DUE_TO_GRAVITY + # TODO: pull from carparams + max_lateral_acceleration = 3 - # controls_state = ui_state.sm['controlsState'] - # car_state = ui_state.sm['carState'] - # live_parameters = ui_state.sm['liveParameters'] - # lateral_acceleration = controls_state.curvature * car_state.vEgo ** 2 - live_parameters.roll * ACCELERATION_DUE_TO_GRAVITY - # # TODO: pull from carparams - # max_lateral_acceleration = 3 + # from selfdrived + actual_lateral_accel = controls_state.curvature * car_state.vEgo ** 2 + desired_lateral_accel = controls_state.desiredCurvature * car_state.vEgo ** 2 + accel_diff = (desired_lateral_accel - actual_lateral_accel) - # # from selfdrived - # actual_lateral_accel = controls_state.curvature * car_state.vEgo ** 2 - # desired_lateral_accel = controls_state.desiredCurvature * car_state.vEgo ** 2 - # accel_diff = (desired_lateral_accel - actual_lateral_accel) + self._torque_filter.update(min(max(lateral_acceleration / max_lateral_acceleration + accel_diff, -1), 1)) - # self._torque_filter.update(min(max(lateral_acceleration / max_lateral_acceleration + accel_diff, -1), 1)) else: self._torque_filter.update(-ui_state.sm['carOutput'].actuatorsOutput.torque) def _render(self, rect: rl.Rectangle) -> None: + if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState' and self.curvature_limit == 0.0: + return + # adjust y pos with torque torque_line_offset = np.interp(abs(self._torque_filter.x), [0.5, 1], [22, 26]) torque_line_height = np.interp(abs(self._torque_filter.x), [0.5, 1], [14, 56]) From 4c3b5062fb4fb12405ade11aff8bf8cbd28ad4ec Mon Sep 17 00:00:00 2001 From: John Christman Date: Tue, 6 Jan 2026 16:00:05 -0500 Subject: [PATCH 08/46] refactor --- selfdrive/ui/mici/onroad/torque_bar.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/selfdrive/ui/mici/onroad/torque_bar.py b/selfdrive/ui/mici/onroad/torque_bar.py index 7653f00510..eb4082d66f 100644 --- a/selfdrive/ui/mici/onroad/torque_bar.py +++ b/selfdrive/ui/mici/onroad/torque_bar.py @@ -154,6 +154,9 @@ def __init__(self, demo: bool = False): self._torque_line_alpha_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps) self.params = Params() + self._update_params() + + def _update_params(self): self.curvature_limit = float(self.params.get("curvature_limit") or 0.0) def update_filter(self, value: float): @@ -165,6 +168,7 @@ def _update_state(self): return # torque line + self._update_params() if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState': if self.curvature_limit > 0: self._torque_filter.update(min(max(ui_state.sm['carControl'].actuators.curvature / self.curvature_limit, -1), 1)) From 00379d542ffde755e7fbbe00fc88d6e722d87e9f Mon Sep 17 00:00:00 2001 From: John Christman Date: Tue, 6 Jan 2026 16:07:53 -0500 Subject: [PATCH 09/46] cleanup --- selfdrive/ui/mici/onroad/torque_bar.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/ui/mici/onroad/torque_bar.py b/selfdrive/ui/mici/onroad/torque_bar.py index eb4082d66f..5f2d507864 100644 --- a/selfdrive/ui/mici/onroad/torque_bar.py +++ b/selfdrive/ui/mici/onroad/torque_bar.py @@ -170,7 +170,7 @@ def _update_state(self): # torque line self._update_params() if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState': - if self.curvature_limit > 0: + if self.curvature_limit > 0.0: self._torque_filter.update(min(max(ui_state.sm['carControl'].actuators.curvature / self.curvature_limit, -1), 1)) else: #incomplete implementation? @@ -192,7 +192,7 @@ def _update_state(self): self._torque_filter.update(-ui_state.sm['carOutput'].actuatorsOutput.torque) def _render(self, rect: rl.Rectangle) -> None: - if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState' and self.curvature_limit == 0.0: + if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState' and self.curvature_limit <= 0.0: return # adjust y pos with torque From d2e0bb1ebc2487634931ec352c49fdc389cbb1e0 Mon Sep 17 00:00:00 2001 From: John Christman Date: Wed, 7 Jan 2026 16:47:11 -0500 Subject: [PATCH 10/46] use constant --- opendbc_repo/opendbc/car/ford/carcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index c015395fec..580b462d02 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -83,7 +83,7 @@ def __init__(self, dbc_names, CP, CP_SP): #IntelligentCruiseButtonManagementInterface.__init__(self, CP, CP_SP) self.params = Params() - self.params.put("curvature_limit", float(0.02)) # Ford system max curvature + self.params.put("curvature_limit", CarControllerParams.CURVATURE_MAX) # Ford system max curvature self.packer = CANPacker(dbc_names[Bus.pt]) self.CAN = fordcan.CanBus(CP) From 8f695724d68030f168f7e599f0b112ba874eaf92 Mon Sep 17 00:00:00 2001 From: John Christman Date: Thu, 8 Jan 2026 16:13:17 -0500 Subject: [PATCH 11/46] testing --- opendbc_repo/opendbc/car/ford/carcontroller.py | 14 ++++++++++++-- opendbc_repo/opendbc/car/lateral.py | 15 +++++++++++++++ 2 files changed, 27 insertions(+), 2 deletions(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index 580b462d02..de8cf49bac 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -52,22 +52,27 @@ def anti_overshoot(apply_curvature, apply_curvature_last, v_ego): return float(np.interp(v_ego, [5, 10], [apply_curvature, output_curvature])) def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_curvature, v_ego_raw, steering_angle, lat_active, CP): + max_curvature = 1 # large initial value # No blending at low speed due to lack of torque wind-up and inaccurate current curvature if v_ego_raw > 9: apply_curvature = np.clip(apply_curvature, current_curvature - CarControllerParams.CURVATURE_ERROR, current_curvature + CarControllerParams.CURVATURE_ERROR) + max_curvature = current_curvature + CarControllerParams.CURVATURE_ERROR # Curvature rate limit after driver torque limit apply_curvature = apply_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, steering_angle, lat_active, CarControllerParams.ANGLE_LIMITS) + max_curvature = np.minimum(max_curvature, get_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, steering_angle, lat_active, CarControllerParams.ANGLE_LIMITS)) + # Ford Q4/CAN FD has more torque available compared to Q3/CAN so we limit it based on lateral acceleration. # Safety is not aware of the road roll so we subtract a conservative amount at all times if CP.flags & FordFlags.CANFD: # Limit curvature to conservative max lateral acceleration curvature_accel_limit = MAX_LATERAL_ACCEL / (max(v_ego_raw, 1) ** 2) apply_curvature = float(np.clip(apply_curvature, -curvature_accel_limit, curvature_accel_limit)) + max_curvature = np.minimum(max_curvature, curvature_accel_limit) - return apply_curvature + return apply_curvature, max_curvature def apply_creep_compensation(accel: float, v_ego: float) -> float: @@ -472,7 +477,7 @@ def update(self, CC, CC_SP, CS, now_nanos): requested_curvature = 0.0 # apply curvature limits - apply_curvature = apply_ford_curvature_limits(requested_curvature, + apply_curvature, max_curvature = apply_ford_curvature_limits(requested_curvature, self.apply_curvature_last, current_curvature, CS.out.vEgoRaw, @@ -480,6 +485,9 @@ def update(self, CC, CC_SP, CS, now_nanos): CC.latActive, self.CP) + lateral_uncertainty = requested_curvature / max_curvature + print(f'lateral_uncertainty: {lateral_uncertainty:.2f}, requested_curvature: {requested_curvature:.5f}, apply_curvature: {apply_curvature:.5f}, max_curvature: {max_curvature:.5f}') + #if reset_steering is 1, set apply_curvature to 0 if reset_steering == 1: apply_curvature = 0.0 @@ -498,6 +506,8 @@ def update(self, CC, CC_SP, CS, now_nanos): # This prevents blocked messages when transitioning out of reset apply_curvature = apply_std_steer_angle_limits(requested_curvature, self.apply_curvature_last, CS.out.vEgoRaw, 0, CC.latActive, CarControllerParams.ANGLE_LIMITS) + max_curvature = get_std_steer_angle_limits(requested_curvature, self.apply_curvature_last, + CS.out.vEgoRaw, 0, CC.latActive, CarControllerParams.ANGLE_LIMITS) # Check if we've ramped close enough to requested curvature (within 10% or 0.001, whichever is larger) curvature_error = abs(requested_curvature - apply_curvature) diff --git a/opendbc_repo/opendbc/car/lateral.py b/opendbc_repo/opendbc/car/lateral.py index 9d120e308b..7d2dd5f1d5 100644 --- a/opendbc_repo/opendbc/car/lateral.py +++ b/opendbc_repo/opendbc/car/lateral.py @@ -90,6 +90,21 @@ def apply_std_steer_angle_limits(apply_angle: float, apply_angle_last: float, v_ return float(np.clip(new_apply_angle, -limits.STEER_ANGLE_MAX, limits.STEER_ANGLE_MAX)) +def get_std_steer_angle_limits(apply_angle: float, apply_angle_last: float, v_ego: float, steering_angle: float, + lat_active: bool, limits: AngleSteeringLimits) -> float: + # angle is current steering wheel angle when inactive on all angle cars + if not lat_active: + return None + + # pick angle rate limits based on wind up/down + steer_up = apply_angle_last * apply_angle >= 0. and abs(apply_angle) > abs(apply_angle_last) + rate_limits = limits.ANGLE_RATE_LIMIT_UP if steer_up else limits.ANGLE_RATE_LIMIT_DOWN + + angle_rate_lim = np.interp(v_ego, rate_limits[0], rate_limits[1]) + apply_angle_max = apply_angle_last + angle_rate_lim + + return float(np.clip(apply_angle_max, -limits.STEER_ANGLE_MAX, limits.STEER_ANGLE_MAX)) + def get_max_angle_delta_vm(v_ego_raw: float, VM: VehicleModel, limits): """Calculate the maximum steering angle rate based on lateral jerk limits.""" From 8374100d1ce33572683f3ae46df26d60f3ff04ca Mon Sep 17 00:00:00 2001 From: John Christman Date: Thu, 8 Jan 2026 16:22:19 -0500 Subject: [PATCH 12/46] include --- opendbc_repo/opendbc/car/ford/carcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index de8cf49bac..e11c3cc220 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -6,7 +6,7 @@ from common.filter_simple import FirstOrderFilter from opendbc.can import CANPacker from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, apply_hysteresis, structs -from opendbc.car.lateral import ISO_LATERAL_ACCEL, apply_std_steer_angle_limits +from opendbc.car.lateral import ISO_LATERAL_ACCEL, apply_std_steer_angle_limits, get_std_steer_angle_limits from opendbc.car.vehicle_model import VehicleModel from opendbc.car.ford import fordcan from opendbc.car.ford.values import CarControllerParams, FordFlags, CAR From a15b67e0325cdb0b002047bd9b955404d0f8f78d Mon Sep 17 00:00:00 2001 From: John Christman Date: Thu, 8 Jan 2026 17:05:16 -0500 Subject: [PATCH 13/46] try 1 --- cereal/custom.capnp | 5 +++++ opendbc_repo/opendbc/car/ford/carcontroller.py | 12 +++++++++++- selfdrive/ui/mici/onroad/torque_bar.py | 13 ++++++++++--- 3 files changed, 26 insertions(+), 4 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 5c0a004fa6..acff19074f 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -45,6 +45,11 @@ struct IntelligentCruiseButtonManagement { } } +#torque meter data +struct TorqueMeter { + lateralUncertainty @0 :Float32; +} + # Same struct as Log.RadarState.LeadData struct LeadData { dRel @0 :Float32; diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index e11c3cc220..603fb83536 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -17,6 +17,8 @@ from openpilot.common.params import Params #from opendbc.sunnypilot.car.ford.icbm import IntelligentCruiseButtonManagementInterface +TORQUE_MESSAGE="TorqueMeter" + LongCtrlState = structs.CarControl.Actuators.LongControlState VisualAlert = structs.CarControl.HUDControl.VisualAlert @@ -93,6 +95,8 @@ def __init__(self, dbc_names, CP, CP_SP): self.packer = CANPacker(dbc_names[Bus.pt]) self.CAN = fordcan.CanBus(CP) + self.pm = messaging.PubMaster([TORQUE_MESSAGE]) + # Initialize control variables self.apply_curvature_last = 0 self.accel = 0.0 @@ -486,7 +490,13 @@ def update(self, CC, CC_SP, CS, now_nanos): self.CP) lateral_uncertainty = requested_curvature / max_curvature - print(f'lateral_uncertainty: {lateral_uncertainty:.2f}, requested_curvature: {requested_curvature:.5f}, apply_curvature: {apply_curvature:.5f}, max_curvature: {max_curvature:.5f}') + + torque_msg = messaging.new_message(TORQUE_MESSAGE) + torque_msg.valid = True + torque_msg.lateralUncertainty = lateral_uncertainty + self.pm.send(TORQUE_MESSAGE, torque_msg) + + #print(f'lateral_uncertainty: {lateral_uncertainty:.2f}, requested_curvature: {requested_curvature:.5f}, apply_curvature: {apply_curvature:.5f}, max_curvature: {max_curvature:.5f}') #if reset_steering is 1, set apply_curvature to 0 if reset_steering == 1: diff --git a/selfdrive/ui/mici/onroad/torque_bar.py b/selfdrive/ui/mici/onroad/torque_bar.py index 5f2d507864..83e30e8436 100644 --- a/selfdrive/ui/mici/onroad/torque_bar.py +++ b/selfdrive/ui/mici/onroad/torque_bar.py @@ -3,6 +3,7 @@ from functools import wraps from collections import OrderedDict from openpilot.common.params import Params +import cereal.messaging as messaging import numpy as np import pyray as rl @@ -14,12 +15,12 @@ from openpilot.system.ui.widgets import Widget from openpilot.common.filter_simple import FirstOrderFilter +TORQUE_MESSAGE="TorqueMeter" # TODO: arc_bar_pts doesn't consider rounded end caps part of the angle span TORQUE_ANGLE_SPAN = 12.7 DEBUG = False - def quantized_lru_cache(maxsize=128): def decorator(func): cache = OrderedDict() @@ -156,6 +157,8 @@ def __init__(self, demo: bool = False): self.params = Params() self._update_params() + self.sm = messaging.SubMaster([TORQUE_MESSAGE]) + def _update_params(self): self.curvature_limit = float(self.params.get("curvature_limit") or 0.0) @@ -170,8 +173,12 @@ def _update_state(self): # torque line self._update_params() if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState': - if self.curvature_limit > 0.0: - self._torque_filter.update(min(max(ui_state.sm['carControl'].actuators.curvature / self.curvature_limit, -1), 1)) + self.sm.update() + if self.sm.updated[TORQUE_MESSAGE]: + torqueValues = self.sm[TORQUE_MESSAGE] + self._torque_filter.update(min(max(torqueValues.lateralUncertainty, -1.2), 1.2)) + elif self.curvature_limit > 0.0: + self._torque_filter.update(min(max(ui_state.sm['carControl'].actuators.curvature / self.curvature_limit, -1.2), 1.2)) else: #incomplete implementation? controls_state = ui_state.sm['controlsState'] From a60f8cb06db3f251f09df03e3158ca552ed79336 Mon Sep 17 00:00:00 2001 From: John Christman Date: Thu, 8 Jan 2026 17:14:06 -0500 Subject: [PATCH 14/46] debug --- cereal/services.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/cereal/services.py b/cereal/services.py index a2fd5f7dce..cbf8dbdf77 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -102,6 +102,9 @@ def __init__(self, should_log: bool, frequency: float, decimation: Optional[int] "modelDataV2SP": (True, 20., None, QueueSize.BIG), "liveLocationKalman": (True, 20.), + # bluepilot + "TorqueMeter": (False, 100., 10), + # debug "uiDebug": (True, 0., 1), "testJoystick": (True, 0.), From b838b2af133755a7288e0315a1d68b65a87a04bd Mon Sep 17 00:00:00 2001 From: John Christman Date: Thu, 8 Jan 2026 17:41:15 -0500 Subject: [PATCH 15/46] elo capn --- cereal/custom.capnp | 9 +++------ cereal/log.capnp | 2 +- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index acff19074f..2df9f82526 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -45,11 +45,6 @@ struct IntelligentCruiseButtonManagement { } } -#torque meter data -struct TorqueMeter { - lateralUncertainty @0 :Float32; -} - # Same struct as Log.RadarState.LeadData struct LeadData { dRel @0 :Float32; @@ -459,7 +454,9 @@ struct ModelDataV2SP @0xa1680744031fdb2d { } } -struct CustomReserved10 @0xcb9fd56c7057593a { +#torque meter data +struct TorqueMeter @0xcb9fd56c7057593a { + lateralUncertainty @0 :Float32; } struct CustomReserved11 @0xc2243c65e0340384 { diff --git a/cereal/log.capnp b/cereal/log.capnp index 55a47cff0a..0bdbc17091 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2635,7 +2635,7 @@ struct Event { carStateSP @114 :Custom.CarStateSP; liveMapDataSP @115 :Custom.LiveMapDataSP; modelDataV2SP @116 :Custom.ModelDataV2SP; - customReserved10 @136 :Custom.CustomReserved10; + torqueMeter @136 :Custom.TorqueMeter; customReserved11 @137 :Custom.CustomReserved11; customReserved12 @138 :Custom.CustomReserved12; customReserved13 @139 :Custom.CustomReserved13; From f92bafccfb7713af4fe3f935d18fdd585c7bc911 Mon Sep 17 00:00:00 2001 From: John Christman Date: Thu, 8 Jan 2026 18:02:04 -0500 Subject: [PATCH 16/46] wrong case? --- opendbc_repo/opendbc/car/ford/carcontroller.py | 2 +- selfdrive/ui/mici/onroad/torque_bar.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index 603fb83536..f0ef1aed24 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -17,7 +17,7 @@ from openpilot.common.params import Params #from opendbc.sunnypilot.car.ford.icbm import IntelligentCruiseButtonManagementInterface -TORQUE_MESSAGE="TorqueMeter" +TORQUE_MESSAGE="torqueMeter" LongCtrlState = structs.CarControl.Actuators.LongControlState VisualAlert = structs.CarControl.HUDControl.VisualAlert diff --git a/selfdrive/ui/mici/onroad/torque_bar.py b/selfdrive/ui/mici/onroad/torque_bar.py index 83e30e8436..cb52534fb9 100644 --- a/selfdrive/ui/mici/onroad/torque_bar.py +++ b/selfdrive/ui/mici/onroad/torque_bar.py @@ -15,7 +15,7 @@ from openpilot.system.ui.widgets import Widget from openpilot.common.filter_simple import FirstOrderFilter -TORQUE_MESSAGE="TorqueMeter" +TORQUE_MESSAGE="torqueMeter" # TODO: arc_bar_pts doesn't consider rounded end caps part of the angle span TORQUE_ANGLE_SPAN = 12.7 From 8f34c00825e89025045e5c6645bf41aedeed0b6b Mon Sep 17 00:00:00 2001 From: John Christman Date: Thu, 8 Jan 2026 18:05:45 -0500 Subject: [PATCH 17/46] wrong case --- cereal/services.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cereal/services.py b/cereal/services.py index cbf8dbdf77..a23f8b2fc5 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -103,7 +103,7 @@ def __init__(self, should_log: bool, frequency: float, decimation: Optional[int] "liveLocationKalman": (True, 20.), # bluepilot - "TorqueMeter": (False, 100., 10), + "torqueMeter": (False, 100., 10), # debug "uiDebug": (True, 0., 1), From ddbc40b16ada1d567b9ad2a46806313892ec95bd Mon Sep 17 00:00:00 2001 From: John Christman Date: Thu, 8 Jan 2026 19:25:23 -0500 Subject: [PATCH 18/46] wip --- cereal/custom.capnp | 4 ++-- cereal/log.capnp | 2 +- opendbc_repo/opendbc/car/ford/carcontroller.py | 15 +++++++-------- selfdrive/ui/mici/onroad/torque_bar.py | 8 ++++---- 4 files changed, 14 insertions(+), 15 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 2df9f82526..2596831913 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -454,8 +454,8 @@ struct ModelDataV2SP @0xa1680744031fdb2d { } } -#torque meter data -struct TorqueMeter @0xcb9fd56c7057593a { +#BluePilot CarController output +struct CarControllerBP @0xcb9fd56c7057593a { lateralUncertainty @0 :Float32; } diff --git a/cereal/log.capnp b/cereal/log.capnp index 0bdbc17091..462a8939d4 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2635,7 +2635,7 @@ struct Event { carStateSP @114 :Custom.CarStateSP; liveMapDataSP @115 :Custom.LiveMapDataSP; modelDataV2SP @116 :Custom.ModelDataV2SP; - torqueMeter @136 :Custom.TorqueMeter; + carControllerBP @136 :Custom.CarControllerBP; customReserved11 @137 :Custom.CustomReserved11; customReserved12 @138 :Custom.CustomReserved12; customReserved13 @139 :Custom.CustomReserved13; diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index f0ef1aed24..8112793c33 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -17,7 +17,7 @@ from openpilot.common.params import Params #from opendbc.sunnypilot.car.ford.icbm import IntelligentCruiseButtonManagementInterface -TORQUE_MESSAGE="torqueMeter" +CARCONTROLLER_MESSAGE="carControllerBP" LongCtrlState = structs.CarControl.Actuators.LongControlState VisualAlert = structs.CarControl.HUDControl.VisualAlert @@ -95,7 +95,7 @@ def __init__(self, dbc_names, CP, CP_SP): self.packer = CANPacker(dbc_names[Bus.pt]) self.CAN = fordcan.CanBus(CP) - self.pm = messaging.PubMaster([TORQUE_MESSAGE]) + self.pm = messaging.PubMaster([CARCONTROLLER_MESSAGE]) # Initialize control variables self.apply_curvature_last = 0 @@ -490,12 +490,6 @@ def update(self, CC, CC_SP, CS, now_nanos): self.CP) lateral_uncertainty = requested_curvature / max_curvature - - torque_msg = messaging.new_message(TORQUE_MESSAGE) - torque_msg.valid = True - torque_msg.lateralUncertainty = lateral_uncertainty - self.pm.send(TORQUE_MESSAGE, torque_msg) - #print(f'lateral_uncertainty: {lateral_uncertainty:.2f}, requested_curvature: {requested_curvature:.5f}, apply_curvature: {apply_curvature:.5f}, max_curvature: {max_curvature:.5f}') #if reset_steering is 1, set apply_curvature to 0 @@ -826,6 +820,11 @@ def update(self, CC, CC_SP, CS, now_nanos): ) ) + carcontroller_msg = messaging.new_message(CARCONTROLLER_MESSAGE) + carcontroller_msg.valid = True + carcontroller_msg.lateralUncertainty = lateral_uncertainty + self.pm.send(CARCONTROLLER_MESSAGE, carcontroller_msg) + self.main_on_last = main_on self.send_ui_last = send_ui self.send_bars_last = send_bars diff --git a/selfdrive/ui/mici/onroad/torque_bar.py b/selfdrive/ui/mici/onroad/torque_bar.py index cb52534fb9..0eae9c7934 100644 --- a/selfdrive/ui/mici/onroad/torque_bar.py +++ b/selfdrive/ui/mici/onroad/torque_bar.py @@ -15,7 +15,7 @@ from openpilot.system.ui.widgets import Widget from openpilot.common.filter_simple import FirstOrderFilter -TORQUE_MESSAGE="torqueMeter" +CARCONTROLLER_MESSAGE="carControllerBP" # TODO: arc_bar_pts doesn't consider rounded end caps part of the angle span TORQUE_ANGLE_SPAN = 12.7 @@ -157,7 +157,7 @@ def __init__(self, demo: bool = False): self.params = Params() self._update_params() - self.sm = messaging.SubMaster([TORQUE_MESSAGE]) + self.sm = messaging.SubMaster([CARCONTROLLER_MESSAGE]) def _update_params(self): self.curvature_limit = float(self.params.get("curvature_limit") or 0.0) @@ -174,8 +174,8 @@ def _update_state(self): self._update_params() if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState': self.sm.update() - if self.sm.updated[TORQUE_MESSAGE]: - torqueValues = self.sm[TORQUE_MESSAGE] + if self.sm.updated[CARCONTROLLER_MESSAGE]: + torqueValues = self.sm[CARCONTROLLER_MESSAGE] self._torque_filter.update(min(max(torqueValues.lateralUncertainty, -1.2), 1.2)) elif self.curvature_limit > 0.0: self._torque_filter.update(min(max(ui_state.sm['carControl'].actuators.curvature / self.curvature_limit, -1.2), 1.2)) From 114b54b2d6a7cef64bdfe7a93bf2797ce6a7f0e8 Mon Sep 17 00:00:00 2001 From: John Christman Date: Thu, 8 Jan 2026 19:30:19 -0500 Subject: [PATCH 19/46] fix --- cereal/services.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cereal/services.py b/cereal/services.py index a23f8b2fc5..cf17ad3024 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -103,7 +103,7 @@ def __init__(self, should_log: bool, frequency: float, decimation: Optional[int] "liveLocationKalman": (True, 20.), # bluepilot - "torqueMeter": (False, 100., 10), + "carControllerBP": (False, 100., 10), # debug "uiDebug": (True, 0., 1), From 48b6dd51d54ace99b96281fec2f5821c107cff12 Mon Sep 17 00:00:00 2001 From: John Christman Date: Thu, 8 Jan 2026 19:58:25 -0500 Subject: [PATCH 20/46] card --- opendbc_repo/opendbc/car/ford/carcontroller.py | 13 +++---------- selfdrive/car/card.py | 5 ++++- 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index 8112793c33..c40c470434 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -17,8 +17,6 @@ from openpilot.common.params import Params #from opendbc.sunnypilot.car.ford.icbm import IntelligentCruiseButtonManagementInterface -CARCONTROLLER_MESSAGE="carControllerBP" - LongCtrlState = structs.CarControl.Actuators.LongControlState VisualAlert = structs.CarControl.HUDControl.VisualAlert @@ -95,8 +93,6 @@ def __init__(self, dbc_names, CP, CP_SP): self.packer = CANPacker(dbc_names[Bus.pt]) self.CAN = fordcan.CanBus(CP) - self.pm = messaging.PubMaster([CARCONTROLLER_MESSAGE]) - # Initialize control variables self.apply_curvature_last = 0 self.accel = 0.0 @@ -489,7 +485,9 @@ def update(self, CC, CC_SP, CS, now_nanos): CC.latActive, self.CP) - lateral_uncertainty = requested_curvature / max_curvature + self.carcontroller_msg = messaging.new_message('carControllerBP') + self.carcontroller_msg.valid = True + self.carcontroller_msg.lateralUncertainty = requested_curvature / max_curvature #print(f'lateral_uncertainty: {lateral_uncertainty:.2f}, requested_curvature: {requested_curvature:.5f}, apply_curvature: {apply_curvature:.5f}, max_curvature: {max_curvature:.5f}') #if reset_steering is 1, set apply_curvature to 0 @@ -820,11 +818,6 @@ def update(self, CC, CC_SP, CS, now_nanos): ) ) - carcontroller_msg = messaging.new_message(CARCONTROLLER_MESSAGE) - carcontroller_msg.valid = True - carcontroller_msg.lateralUncertainty = lateral_uncertainty - self.pm.send(CARCONTROLLER_MESSAGE, carcontroller_msg) - self.main_on_last = main_on self.send_ui_last = send_ui self.send_bars_last = send_bars diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 6758432b68..f1f9f746f8 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -72,7 +72,7 @@ class Car: def __init__(self, CI=None, RI=None) -> None: self.can_sock = messaging.sub_sock('can', timeout=20) self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents'] + ['carControlSP', 'longitudinalPlanSP']) - self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'] + ['carParamsSP', 'carStateSP']) + self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'] + ['carParamsSP', 'carStateSP', 'carControllerBP']) self.can_rcv_cum_timeout_counter = 0 @@ -239,6 +239,9 @@ def state_publish(self, CS: car.CarState, CS_SP: custom.CarStateSP, RD: structs. cp_send.carParams = self.CP self.pm.send('carParams', cp_send) + if hasattr(self.CI.CC, "carcontroller_msg"): + self.pm.send("carControllerBP", self.CI.CC.carcontroller_msg) + # publish new carOutput co_send = messaging.new_message('carOutput') co_send.valid = self.sm.all_checks(['carControl']) From 7b48b8ae033de76800bca9a484d2915cfab0365d Mon Sep 17 00:00:00 2001 From: John Christman Date: Fri, 9 Jan 2026 18:08:25 -0500 Subject: [PATCH 21/46] learnin cap --- opendbc_repo/opendbc/car/ford/carcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index c40c470434..484718611d 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -487,7 +487,7 @@ def update(self, CC, CC_SP, CS, now_nanos): self.carcontroller_msg = messaging.new_message('carControllerBP') self.carcontroller_msg.valid = True - self.carcontroller_msg.lateralUncertainty = requested_curvature / max_curvature + self.carcontroller_msg.lateralUncertainty = float(requested_curvature / max_curvature) #print(f'lateral_uncertainty: {lateral_uncertainty:.2f}, requested_curvature: {requested_curvature:.5f}, apply_curvature: {apply_curvature:.5f}, max_curvature: {max_curvature:.5f}') #if reset_steering is 1, set apply_curvature to 0 From 094341b02183d0d8f414c3cd2f6d8597973605a8 Mon Sep 17 00:00:00 2001 From: John Christman Date: Sat, 10 Jan 2026 12:18:56 -0500 Subject: [PATCH 22/46] lateral uncertainty meter --- cereal/custom.capnp | 5 ++--- cereal/log.capnp | 2 +- cereal/services.py | 2 +- opendbc_repo/opendbc/car/ford/carcontroller.py | 8 ++++---- opendbc_repo/opendbc/car/structs.py | 4 ++++ selfdrive/car/card.py | 14 ++++++++++---- selfdrive/car/helpers.py | 4 +++- selfdrive/ui/mici/onroad/torque_bar.py | 17 +++++++---------- selfdrive/ui/sunnypilot/ui_state.py | 2 +- 9 files changed, 33 insertions(+), 25 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 2596831913..dda9369dee 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -454,9 +454,8 @@ struct ModelDataV2SP @0xa1680744031fdb2d { } } -#BluePilot CarController output -struct CarControllerBP @0xcb9fd56c7057593a { - lateralUncertainty @0 :Float32; +struct ControllerStateBP @0xcb9fd56c7057593a { + lateralUncertainty @0 :Float32; #BluePilot } struct CustomReserved11 @0xc2243c65e0340384 { diff --git a/cereal/log.capnp b/cereal/log.capnp index 462a8939d4..c5801a870f 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2635,7 +2635,7 @@ struct Event { carStateSP @114 :Custom.CarStateSP; liveMapDataSP @115 :Custom.LiveMapDataSP; modelDataV2SP @116 :Custom.ModelDataV2SP; - carControllerBP @136 :Custom.CarControllerBP; + controllerStateBP @136 :Custom.ControllerStateBP; customReserved11 @137 :Custom.CustomReserved11; customReserved12 @138 :Custom.CustomReserved12; customReserved13 @139 :Custom.CustomReserved13; diff --git a/cereal/services.py b/cereal/services.py index cf17ad3024..0b8558fdf7 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -103,7 +103,7 @@ def __init__(self, should_log: bool, frequency: float, decimation: Optional[int] "liveLocationKalman": (True, 20.), # bluepilot - "carControllerBP": (False, 100., 10), + "controllerStateBP": (True, 100., 10), # debug "uiDebug": (True, 0., 1), diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index 484718611d..428c9b1faf 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -111,7 +111,9 @@ def __init__(self, dbc_names, CP, CP_SP): self.steering_wheel_delta_adjusted = 0.0 self.last_button_frame = 0 # Track last ICBM button press frame - ################################## lateral control parameters ############################################## + self.lateralUncertainty = 0.0 + + ################################## lateral control parameters ############################################## # Toggles @@ -485,9 +487,7 @@ def update(self, CC, CC_SP, CS, now_nanos): CC.latActive, self.CP) - self.carcontroller_msg = messaging.new_message('carControllerBP') - self.carcontroller_msg.valid = True - self.carcontroller_msg.lateralUncertainty = float(requested_curvature / max_curvature) + self.lateralUncertainty = float(requested_curvature / max_curvature) #print(f'lateral_uncertainty: {lateral_uncertainty:.2f}, requested_curvature: {requested_curvature:.5f}, apply_curvature: {apply_curvature:.5f}, max_curvature: {max_curvature:.5f}') #if reset_steering is 1, set apply_curvature to 0 diff --git a/opendbc_repo/opendbc/car/structs.py b/opendbc_repo/opendbc/car/structs.py index 7fd7623382..45b5cc44fe 100644 --- a/opendbc_repo/opendbc/car/structs.py +++ b/opendbc_repo/opendbc/car/structs.py @@ -170,3 +170,7 @@ class ParamType(StrEnum): @auto_dataclass class CarStateSP: speedLimit: float = auto_field() + +@auto_dataclass +class ControllerStateBP: + lateralUncertainty: float = auto_field() \ No newline at end of file diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index f1f9f746f8..498bc81946 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -72,7 +72,7 @@ class Car: def __init__(self, CI=None, RI=None) -> None: self.can_sock = messaging.sub_sock('can', timeout=20) self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents'] + ['carControlSP', 'longitudinalPlanSP']) - self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'] + ['carParamsSP', 'carStateSP', 'carControllerBP']) + self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'] + ['carParamsSP', 'carStateSP', 'controllerStateBP']) self.can_rcv_cum_timeout_counter = 0 @@ -239,9 +239,6 @@ def state_publish(self, CS: car.CarState, CS_SP: custom.CarStateSP, RD: structs. cp_send.carParams = self.CP self.pm.send('carParams', cp_send) - if hasattr(self.CI.CC, "carcontroller_msg"): - self.pm.send("carControllerBP", self.CI.CC.carcontroller_msg) - # publish new carOutput co_send = messaging.new_message('carOutput') co_send.valid = self.sm.all_checks(['carControl']) @@ -290,6 +287,15 @@ def controls_update(self, CS: car.CarState, CC: car.CarControl, CC_SP: custom.Ca self.last_actuators_output, can_sends = self.CI.apply(CC, convert_carControlSP(CC_SP), now_nanos) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) + if hasattr(self.CI.CC, "lateralUncertainty"): + cs_bp = structs.ControllerStateBP() + cs_bp.lateralUncertainty = self.CI.CC.lateralUncertainty + cs_bp_capnp = convert_to_capnp(cs_bp) + cs_bp_send = messaging.new_message('controllerStateBP') + cs_bp_send.valid = True + cs_bp_send.controllerStateBP = cs_bp_capnp + self.pm.send('controllerStateBP', cs_bp_send) + self.CC_prev = CC def step(self): diff --git a/selfdrive/car/helpers.py b/selfdrive/car/helpers.py index 384152c71b..3c0cb8d35a 100644 --- a/selfdrive/car/helpers.py +++ b/selfdrive/car/helpers.py @@ -35,13 +35,15 @@ def asdictref(obj) -> dict[str, Any]: return _asdictref_inner(obj) -def convert_to_capnp(struct: structs.CarParamsSP | structs.CarStateSP) -> capnp.lib.capnp._DynamicStructBuilder: +def convert_to_capnp(struct: structs.CarParamsSP | structs.CarStateSP | structs.ControllerStateBP) -> capnp.lib.capnp._DynamicStructBuilder: struct_dict = asdictref(struct) if isinstance(struct, structs.CarParamsSP): struct_capnp = custom.CarParamsSP.new_message(**struct_dict) elif isinstance(struct, structs.CarStateSP): struct_capnp = custom.CarStateSP.new_message(**struct_dict) + elif isinstance(struct, structs.ControllerStateBP): + struct_capnp = custom.ControllerStateBP.new_message(**struct_dict) else: raise ValueError(f"Unsupported struct type: {type(struct)}") diff --git a/selfdrive/ui/mici/onroad/torque_bar.py b/selfdrive/ui/mici/onroad/torque_bar.py index 0eae9c7934..15dfd40cde 100644 --- a/selfdrive/ui/mici/onroad/torque_bar.py +++ b/selfdrive/ui/mici/onroad/torque_bar.py @@ -15,7 +15,6 @@ from openpilot.system.ui.widgets import Widget from openpilot.common.filter_simple import FirstOrderFilter -CARCONTROLLER_MESSAGE="carControllerBP" # TODO: arc_bar_pts doesn't consider rounded end caps part of the angle span TORQUE_ANGLE_SPAN = 12.7 @@ -157,8 +156,6 @@ def __init__(self, demo: bool = False): self.params = Params() self._update_params() - self.sm = messaging.SubMaster([CARCONTROLLER_MESSAGE]) - def _update_params(self): self.curvature_limit = float(self.params.get("curvature_limit") or 0.0) @@ -173,12 +170,11 @@ def _update_state(self): # torque line self._update_params() if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState': - self.sm.update() - if self.sm.updated[CARCONTROLLER_MESSAGE]: - torqueValues = self.sm[CARCONTROLLER_MESSAGE] - self._torque_filter.update(min(max(torqueValues.lateralUncertainty, -1.2), 1.2)) - elif self.curvature_limit > 0.0: - self._torque_filter.update(min(max(ui_state.sm['carControl'].actuators.curvature / self.curvature_limit, -1.2), 1.2)) + if ui_state.sm.updated["controllerStateBP"]: + ctrlr_state = ui_state.sm['controllerStateBP'] + self._torque_filter.update(min(max(ctrlr_state.lateralUncertainty, -1.2), 1.2)) + # elif self.curvature_limit > 0.0: + # self._torque_filter.update(min(max(ui_state.sm['carControl'].actuators.curvature / self.curvature_limit, -1.2), 1.2)) else: #incomplete implementation? controls_state = ui_state.sm['controlsState'] @@ -199,7 +195,8 @@ def _update_state(self): self._torque_filter.update(-ui_state.sm['carOutput'].actuatorsOutput.torque) def _render(self, rect: rl.Rectangle) -> None: - if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState' and self.curvature_limit <= 0.0: + if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState' and self.curvature_limit <= 0.0 \ + and not (ui_state.sm.updated("controllerStateBP") and ui_state.sm.valid("controllerStateBP")): return # adjust y pos with torque diff --git a/selfdrive/ui/sunnypilot/ui_state.py b/selfdrive/ui/sunnypilot/ui_state.py index ca8125512a..940d047955 100644 --- a/selfdrive/ui/sunnypilot/ui_state.py +++ b/selfdrive/ui/sunnypilot/ui_state.py @@ -17,7 +17,7 @@ def __init__(self): self.params = Params() self.sm_services_ext = [ "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP", - "gpsLocation", "liveTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "liveDelay" + "gpsLocation", "liveTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "liveDelay", "carControlSP", "controllerStateBP" ] self.sunnylink_state = SunnylinkState() From 1b1f6939990693ebcf8f0da79b01e2feaf4b3a32 Mon Sep 17 00:00:00 2001 From: John Christman Date: Sun, 11 Jan 2026 14:37:33 -0500 Subject: [PATCH 23/46] cleanup --- common/params_keys.h | 1 - opendbc_repo/opendbc/car/ford/carcontroller.py | 1 - selfdrive/ui/mici/onroad/hud_renderer.py | 1 - selfdrive/ui/mici/onroad/torque_bar.py | 12 +----------- selfdrive/ui/sunnypilot/ui_state.py | 2 +- 5 files changed, 2 insertions(+), 15 deletions(-) diff --git a/common/params_keys.h b/common/params_keys.h index e178dbad5f..534b7b0c34 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -282,5 +282,4 @@ inline static std::unordered_map keys = { {"LC_PID_gain_UI", {PERSISTENT | BACKUP, FLOAT, "3.0"}}, {"disable_BP_lat_UI", {PERSISTENT | BACKUP, BOOL, "0"}}, {"vbatt_pause_charging", {PERSISTENT | BACKUP, FLOAT, "11.8"}}, - {"curvature_limit", {CLEAR_ON_MANAGER_START, FLOAT}}, // for torque bar display }; \ No newline at end of file diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index 428c9b1faf..78c6b81389 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -88,7 +88,6 @@ def __init__(self, dbc_names, CP, CP_SP): #IntelligentCruiseButtonManagementInterface.__init__(self, CP, CP_SP) self.params = Params() - self.params.put("curvature_limit", CarControllerParams.CURVATURE_MAX) # Ford system max curvature self.packer = CANPacker(dbc_names[Bus.pt]) self.CAN = fordcan.CanBus(CP) diff --git a/selfdrive/ui/mici/onroad/hud_renderer.py b/selfdrive/ui/mici/onroad/hud_renderer.py index 2b8569934c..ab02b9c218 100644 --- a/selfdrive/ui/mici/onroad/hud_renderer.py +++ b/selfdrive/ui/mici/onroad/hud_renderer.py @@ -172,7 +172,6 @@ def _update_state(self) -> None: def _render(self, rect: rl.Rectangle) -> None: """Render HUD elements to the screen.""" - #if ui_state.sm['controlsState'].lateralControlState.which() != 'angleState': self._torque_bar.render(rect) if self.is_cruise_set: diff --git a/selfdrive/ui/mici/onroad/torque_bar.py b/selfdrive/ui/mici/onroad/torque_bar.py index 15dfd40cde..dacf161210 100644 --- a/selfdrive/ui/mici/onroad/torque_bar.py +++ b/selfdrive/ui/mici/onroad/torque_bar.py @@ -2,7 +2,6 @@ import time from functools import wraps from collections import OrderedDict -from openpilot.common.params import Params import cereal.messaging as messaging import numpy as np @@ -153,12 +152,6 @@ def __init__(self, demo: bool = False): self._torque_filter = FirstOrderFilter(0, 0.1, 1 / gui_app.target_fps) self._torque_line_alpha_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps) - self.params = Params() - self._update_params() - - def _update_params(self): - self.curvature_limit = float(self.params.get("curvature_limit") or 0.0) - def update_filter(self, value: float): """Update the torque filter value (for demo mode).""" self._torque_filter.update(value) @@ -173,8 +166,6 @@ def _update_state(self): if ui_state.sm.updated["controllerStateBP"]: ctrlr_state = ui_state.sm['controllerStateBP'] self._torque_filter.update(min(max(ctrlr_state.lateralUncertainty, -1.2), 1.2)) - # elif self.curvature_limit > 0.0: - # self._torque_filter.update(min(max(ui_state.sm['carControl'].actuators.curvature / self.curvature_limit, -1.2), 1.2)) else: #incomplete implementation? controls_state = ui_state.sm['controlsState'] @@ -195,8 +186,7 @@ def _update_state(self): self._torque_filter.update(-ui_state.sm['carOutput'].actuatorsOutput.torque) def _render(self, rect: rl.Rectangle) -> None: - if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState' and self.curvature_limit <= 0.0 \ - and not (ui_state.sm.updated("controllerStateBP") and ui_state.sm.valid("controllerStateBP")): + if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState' and not ui_state.sm.updated["controllerStateBP"]: return # adjust y pos with torque diff --git a/selfdrive/ui/sunnypilot/ui_state.py b/selfdrive/ui/sunnypilot/ui_state.py index 940d047955..d81a7093e3 100644 --- a/selfdrive/ui/sunnypilot/ui_state.py +++ b/selfdrive/ui/sunnypilot/ui_state.py @@ -17,7 +17,7 @@ def __init__(self): self.params = Params() self.sm_services_ext = [ "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP", - "gpsLocation", "liveTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "liveDelay", "carControlSP", "controllerStateBP" + "gpsLocation", "liveTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "liveDelay", "controllerStateBP" ] self.sunnylink_state = SunnylinkState() From 963a9c377e190db15a2f7f38880e58130abf5aac Mon Sep 17 00:00:00 2001 From: John Christman Date: Sun, 11 Jan 2026 15:07:22 -0500 Subject: [PATCH 24/46] debug logging --- .../opendbc/car/ford/carcontroller.py | 21 +++++++++++++++++-- opendbc_repo/opendbc/car/lateral.py | 4 ++-- 2 files changed, 21 insertions(+), 4 deletions(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index 78c6b81389..ff72299a20 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -1,4 +1,5 @@ import math +import os import cereal.messaging as messaging import numpy as np from numpy import clip, interp @@ -58,11 +59,15 @@ def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_c apply_curvature = np.clip(apply_curvature, current_curvature - CarControllerParams.CURVATURE_ERROR, current_curvature + CarControllerParams.CURVATURE_ERROR) max_curvature = current_curvature + CarControllerParams.CURVATURE_ERROR + self.lateral_limiter = "CUR + CURVATURE_ERROR" # Curvature rate limit after driver torque limit apply_curvature = apply_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, steering_angle, lat_active, CarControllerParams.ANGLE_LIMITS) - max_curvature = np.minimum(max_curvature, get_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, steering_angle, lat_active, CarControllerParams.ANGLE_LIMITS)) + std_steer_angle_limit = get_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, steering_angle, lat_active, CarControllerParams.ANGLE_LIMITS) + max_curvature = np.minimum(max_curvature, std_steer_angle_limit) + if max_curvature < std_steer_angle_limit: + self.lateral_limiter = "STD STEER ANGLE" # Ford Q4/CAN FD has more torque available compared to Q3/CAN so we limit it based on lateral acceleration. # Safety is not aware of the road roll so we subtract a conservative amount at all times @@ -70,6 +75,10 @@ def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_c # Limit curvature to conservative max lateral acceleration curvature_accel_limit = MAX_LATERAL_ACCEL / (max(v_ego_raw, 1) ** 2) apply_curvature = float(np.clip(apply_curvature, -curvature_accel_limit, curvature_accel_limit)) + + if max_curvature > curvature_accel_limit: + self.lateral_limiter = "MAX LAT ACCEL" + max_curvature = np.minimum(max_curvature, curvature_accel_limit) return apply_curvature, max_curvature @@ -111,6 +120,7 @@ def __init__(self, dbc_names, CP, CP_SP): self.last_button_frame = 0 # Track last ICBM button press frame self.lateralUncertainty = 0.0 + self.lateral_limiter = "MAX" ################################## lateral control parameters ############################################## @@ -487,7 +497,14 @@ def update(self, CC, CC_SP, CS, now_nanos): self.CP) self.lateralUncertainty = float(requested_curvature / max_curvature) - #print(f'lateral_uncertainty: {lateral_uncertainty:.2f}, requested_curvature: {requested_curvature:.5f}, apply_curvature: {apply_curvature:.5f}, max_curvature: {max_curvature:.5f}') + + #debug log + LOG_PATH = "/data/community/logs/" + LOG_FILE = "ford_lateral_log.txt" + if not os.path.exists(LOG_PATH): + os.makedirs(LOG_PATH) + with open(LOG_PATH + LOG_FILE, "a") as f: + f.write(f"f'lat_uncert: {lateral_uncertainty:.2f}, req: {requested_curvature:.5f}, apply: {apply_curvature:.5f}, max: {max_curvature:.5f}:{ self.lateral_limiter}\n") #if reset_steering is 1, set apply_curvature to 0 if reset_steering == 1: diff --git a/opendbc_repo/opendbc/car/lateral.py b/opendbc_repo/opendbc/car/lateral.py index 7d2dd5f1d5..10ef046912 100644 --- a/opendbc_repo/opendbc/car/lateral.py +++ b/opendbc_repo/opendbc/car/lateral.py @@ -94,7 +94,7 @@ def get_std_steer_angle_limits(apply_angle: float, apply_angle_last: float, v_eg lat_active: bool, limits: AngleSteeringLimits) -> float: # angle is current steering wheel angle when inactive on all angle cars if not lat_active: - return None + return apply_angle # pick angle rate limits based on wind up/down steer_up = apply_angle_last * apply_angle >= 0. and abs(apply_angle) > abs(apply_angle_last) @@ -103,7 +103,7 @@ def get_std_steer_angle_limits(apply_angle: float, apply_angle_last: float, v_eg angle_rate_lim = np.interp(v_ego, rate_limits[0], rate_limits[1]) apply_angle_max = apply_angle_last + angle_rate_lim - return float(np.clip(apply_angle_max, -limits.STEER_ANGLE_MAX, limits.STEER_ANGLE_MAX)) + return float(np.minimum(apply_angle_max, limits.STEER_ANGLE_MAX)) def get_max_angle_delta_vm(v_ego_raw: float, VM: VehicleModel, limits): From 9844e77ba24f8b95f4620773daf3357c51c48303 Mon Sep 17 00:00:00 2001 From: John Christman Date: Sun, 11 Jan 2026 15:18:44 -0500 Subject: [PATCH 25/46] fix --- opendbc_repo/opendbc/car/ford/carcontroller.py | 2 +- selfdrive/ui/mici/onroad/torque_bar.py | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index ff72299a20..5574bfaa1f 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -504,7 +504,7 @@ def update(self, CC, CC_SP, CS, now_nanos): if not os.path.exists(LOG_PATH): os.makedirs(LOG_PATH) with open(LOG_PATH + LOG_FILE, "a") as f: - f.write(f"f'lat_uncert: {lateral_uncertainty:.2f}, req: {requested_curvature:.5f}, apply: {apply_curvature:.5f}, max: {max_curvature:.5f}:{ self.lateral_limiter}\n") + f.write(f"f'lat_uncert: {lateral_uncertainty:.2f}, req: {requested_curvature:.5f}, apply: {apply_curvature:.5f}, max: {max_curvature:.5f}:{self.lateral_limiter}\n") #if reset_steering is 1, set apply_curvature to 0 if reset_steering == 1: diff --git a/selfdrive/ui/mici/onroad/torque_bar.py b/selfdrive/ui/mici/onroad/torque_bar.py index dacf161210..bb8bc07d9b 100644 --- a/selfdrive/ui/mici/onroad/torque_bar.py +++ b/selfdrive/ui/mici/onroad/torque_bar.py @@ -161,7 +161,6 @@ def _update_state(self): return # torque line - self._update_params() if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState': if ui_state.sm.updated["controllerStateBP"]: ctrlr_state = ui_state.sm['controllerStateBP'] From 743083d428a4881b2745d6bcfed217b76a2871d9 Mon Sep 17 00:00:00 2001 From: John Christman Date: Sun, 11 Jan 2026 15:23:41 -0500 Subject: [PATCH 26/46] fix --- opendbc_repo/opendbc/car/ford/carcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index 5574bfaa1f..d341788675 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -504,7 +504,7 @@ def update(self, CC, CC_SP, CS, now_nanos): if not os.path.exists(LOG_PATH): os.makedirs(LOG_PATH) with open(LOG_PATH + LOG_FILE, "a") as f: - f.write(f"f'lat_uncert: {lateral_uncertainty:.2f}, req: {requested_curvature:.5f}, apply: {apply_curvature:.5f}, max: {max_curvature:.5f}:{self.lateral_limiter}\n") + f.write(f"f'lat_uncert: {self.lateralUncertainty:.2f}, req: {requested_curvature:.5f}, apply: {apply_curvature:.5f}, max: {max_curvature:.5f}:{self.lateral_limiter}\n") #if reset_steering is 1, set apply_curvature to 0 if reset_steering == 1: From c52798856d1933130d1d036eeedb8ce07c85e4bd Mon Sep 17 00:00:00 2001 From: John Christman Date: Sun, 11 Jan 2026 15:40:07 -0500 Subject: [PATCH 27/46] debugging --- .../opendbc/car/ford/carcontroller.py | 20 ++++++++----------- 1 file changed, 8 insertions(+), 12 deletions(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index d341788675..6321a2a04b 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -52,22 +52,22 @@ def anti_overshoot(apply_curvature, apply_curvature_last, v_ego): return float(np.interp(v_ego, [5, 10], [apply_curvature, output_curvature])) -def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_curvature, v_ego_raw, steering_angle, lat_active, CP): +def apply_ford_curvature_limits(self, apply_curvature, apply_curvature_last, current_curvature, v_ego_raw, steering_angle, lat_active, CP): max_curvature = 1 # large initial value # No blending at low speed due to lack of torque wind-up and inaccurate current curvature if v_ego_raw > 9: apply_curvature = np.clip(apply_curvature, current_curvature - CarControllerParams.CURVATURE_ERROR, current_curvature + CarControllerParams.CURVATURE_ERROR) max_curvature = current_curvature + CarControllerParams.CURVATURE_ERROR - self.lateral_limiter = "CUR + CURVATURE_ERROR" + self.lateral_limiter = "Curvature Error Limit" # Curvature rate limit after driver torque limit apply_curvature = apply_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, steering_angle, lat_active, CarControllerParams.ANGLE_LIMITS) std_steer_angle_limit = get_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, steering_angle, lat_active, CarControllerParams.ANGLE_LIMITS) + if std_steer_angle_limit < max_curvature: + self.lateral_limiter = "Std Steer Angle Limit" max_curvature = np.minimum(max_curvature, std_steer_angle_limit) - if max_curvature < std_steer_angle_limit: - self.lateral_limiter = "STD STEER ANGLE" # Ford Q4/CAN FD has more torque available compared to Q3/CAN so we limit it based on lateral acceleration. # Safety is not aware of the road roll so we subtract a conservative amount at all times @@ -75,10 +75,8 @@ def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_c # Limit curvature to conservative max lateral acceleration curvature_accel_limit = MAX_LATERAL_ACCEL / (max(v_ego_raw, 1) ** 2) apply_curvature = float(np.clip(apply_curvature, -curvature_accel_limit, curvature_accel_limit)) - - if max_curvature > curvature_accel_limit: - self.lateral_limiter = "MAX LAT ACCEL" - + if curvature_accel_limit < max_curvature: + self.lateral_limiter = "CANFD Lat Accel Limit" max_curvature = np.minimum(max_curvature, curvature_accel_limit) return apply_curvature, max_curvature @@ -118,13 +116,11 @@ def __init__(self, dbc_names, CP, CP_SP): self.accel_pitch_compensated = 0.0 self.steering_wheel_delta_adjusted = 0.0 self.last_button_frame = 0 # Track last ICBM button press frame - self.lateralUncertainty = 0.0 - self.lateral_limiter = "MAX" + self.lateral_limiter = "None" # for logging ################################## lateral control parameters ############################################## - # Toggles self.enable_human_turn_detection = True self.enable_lane_positioning = True @@ -504,7 +500,7 @@ def update(self, CC, CC_SP, CS, now_nanos): if not os.path.exists(LOG_PATH): os.makedirs(LOG_PATH) with open(LOG_PATH + LOG_FILE, "a") as f: - f.write(f"f'lat_uncert: {self.lateralUncertainty:.2f}, req: {requested_curvature:.5f}, apply: {apply_curvature:.5f}, max: {max_curvature:.5f}:{self.lateral_limiter}\n") + f.write(f"lat_uncert: {self.lateralUncertainty:.2f}, req: {requested_curvature:.5f}, apply: {apply_curvature:.5f}, max: {max_curvature:.5f}:{self.lateral_limiter}\n") #if reset_steering is 1, set apply_curvature to 0 if reset_steering == 1: From d2d11dcd5acd1ce30f568740250b059caeeb44a8 Mon Sep 17 00:00:00 2001 From: John Christman Date: Sun, 11 Jan 2026 15:43:52 -0500 Subject: [PATCH 28/46] debugging --- opendbc_repo/opendbc/car/ford/carcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index 6321a2a04b..1a284afe4a 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -484,7 +484,7 @@ def update(self, CC, CC_SP, CS, now_nanos): requested_curvature = 0.0 # apply curvature limits - apply_curvature, max_curvature = apply_ford_curvature_limits(requested_curvature, + apply_curvature, max_curvature = apply_ford_curvature_limits(self, requested_curvature, self.apply_curvature_last, current_curvature, CS.out.vEgoRaw, From 78689099d4b79115322297a2510fd8c0cce9f936 Mon Sep 17 00:00:00 2001 From: John Christman Date: Sun, 11 Jan 2026 16:11:52 -0500 Subject: [PATCH 29/46] abs --- .../opendbc/car/ford/carcontroller.py | 20 ++++++++++--------- opendbc_repo/opendbc/car/lateral.py | 9 ++++----- 2 files changed, 15 insertions(+), 14 deletions(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index 1a284afe4a..e00ee0632b 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -58,7 +58,7 @@ def apply_ford_curvature_limits(self, apply_curvature, apply_curvature_last, cur if v_ego_raw > 9: apply_curvature = np.clip(apply_curvature, current_curvature - CarControllerParams.CURVATURE_ERROR, current_curvature + CarControllerParams.CURVATURE_ERROR) - max_curvature = current_curvature + CarControllerParams.CURVATURE_ERROR + max_curvature = abs(current_curvature) + CarControllerParams.CURVATURE_ERROR self.lateral_limiter = "Curvature Error Limit" # Curvature rate limit after driver torque limit @@ -67,7 +67,7 @@ def apply_ford_curvature_limits(self, apply_curvature, apply_curvature_last, cur std_steer_angle_limit = get_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, steering_angle, lat_active, CarControllerParams.ANGLE_LIMITS) if std_steer_angle_limit < max_curvature: self.lateral_limiter = "Std Steer Angle Limit" - max_curvature = np.minimum(max_curvature, std_steer_angle_limit) + max_curvature = np.minimum(max_curvature, abs(std_steer_angle_limit)) # Ford Q4/CAN FD has more torque available compared to Q3/CAN so we limit it based on lateral acceleration. # Safety is not aware of the road roll so we subtract a conservative amount at all times @@ -77,7 +77,9 @@ def apply_ford_curvature_limits(self, apply_curvature, apply_curvature_last, cur apply_curvature = float(np.clip(apply_curvature, -curvature_accel_limit, curvature_accel_limit)) if curvature_accel_limit < max_curvature: self.lateral_limiter = "CANFD Lat Accel Limit" - max_curvature = np.minimum(max_curvature, curvature_accel_limit) + + max_curvature = np.minimum(max_curvature, abs(curvature_accel_limit)) + max_curvature = np.maximum(max_curvature, abs(apply_curvature)) #limit max_curvature is not less than apply_curvature return apply_curvature, max_curvature @@ -495,12 +497,12 @@ def update(self, CC, CC_SP, CS, now_nanos): self.lateralUncertainty = float(requested_curvature / max_curvature) #debug log - LOG_PATH = "/data/community/logs/" - LOG_FILE = "ford_lateral_log.txt" - if not os.path.exists(LOG_PATH): - os.makedirs(LOG_PATH) - with open(LOG_PATH + LOG_FILE, "a") as f: - f.write(f"lat_uncert: {self.lateralUncertainty:.2f}, req: {requested_curvature:.5f}, apply: {apply_curvature:.5f}, max: {max_curvature:.5f}:{self.lateral_limiter}\n") + # LOG_PATH = "/data/community/logs/" + # LOG_FILE = "ford_lateral_log.txt" + # if not os.path.exists(LOG_PATH): + # os.makedirs(LOG_PATH) + # with open(LOG_PATH + LOG_FILE, "a") as f: + # f.write(f"lat_uncert: {self.lateralUncertainty:.2f}, req: {requested_curvature:.5f}, apply: {apply_curvature:.5f}, max: {max_curvature:.5f}:{self.lateral_limiter}\n") #if reset_steering is 1, set apply_curvature to 0 if reset_steering == 1: diff --git a/opendbc_repo/opendbc/car/lateral.py b/opendbc_repo/opendbc/car/lateral.py index 10ef046912..9f0e83d898 100644 --- a/opendbc_repo/opendbc/car/lateral.py +++ b/opendbc_repo/opendbc/car/lateral.py @@ -92,10 +92,6 @@ def apply_std_steer_angle_limits(apply_angle: float, apply_angle_last: float, v_ def get_std_steer_angle_limits(apply_angle: float, apply_angle_last: float, v_ego: float, steering_angle: float, lat_active: bool, limits: AngleSteeringLimits) -> float: - # angle is current steering wheel angle when inactive on all angle cars - if not lat_active: - return apply_angle - # pick angle rate limits based on wind up/down steer_up = apply_angle_last * apply_angle >= 0. and abs(apply_angle) > abs(apply_angle_last) rate_limits = limits.ANGLE_RATE_LIMIT_UP if steer_up else limits.ANGLE_RATE_LIMIT_DOWN @@ -103,7 +99,10 @@ def get_std_steer_angle_limits(apply_angle: float, apply_angle_last: float, v_eg angle_rate_lim = np.interp(v_ego, rate_limits[0], rate_limits[1]) apply_angle_max = apply_angle_last + angle_rate_lim - return float(np.minimum(apply_angle_max, limits.STEER_ANGLE_MAX)) + if not lat_active: + apply_angle_max = steering_angle + + return float(np.minimum(abs(apply_angle_max), limits.STEER_ANGLE_MAX)) def get_max_angle_delta_vm(v_ego_raw: float, VM: VehicleModel, limits): From b91ab3fa65c1e0157cc7c0d3d012d7321b732f14 Mon Sep 17 00:00:00 2001 From: John Christman Date: Sun, 11 Jan 2026 18:05:58 -0500 Subject: [PATCH 30/46] code review --- opendbc_repo/opendbc/car/ford/carcontroller.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index e00ee0632b..b496b32b78 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -75,7 +75,7 @@ def apply_ford_curvature_limits(self, apply_curvature, apply_curvature_last, cur # Limit curvature to conservative max lateral acceleration curvature_accel_limit = MAX_LATERAL_ACCEL / (max(v_ego_raw, 1) ** 2) apply_curvature = float(np.clip(apply_curvature, -curvature_accel_limit, curvature_accel_limit)) - if curvature_accel_limit < max_curvature: + if abs(curvature_accel_limit) < max_curvature: self.lateral_limiter = "CANFD Lat Accel Limit" max_curvature = np.minimum(max_curvature, abs(curvature_accel_limit)) @@ -522,8 +522,6 @@ def update(self, CC, CC_SP, CS, now_nanos): # This prevents blocked messages when transitioning out of reset apply_curvature = apply_std_steer_angle_limits(requested_curvature, self.apply_curvature_last, CS.out.vEgoRaw, 0, CC.latActive, CarControllerParams.ANGLE_LIMITS) - max_curvature = get_std_steer_angle_limits(requested_curvature, self.apply_curvature_last, - CS.out.vEgoRaw, 0, CC.latActive, CarControllerParams.ANGLE_LIMITS) # Check if we've ramped close enough to requested curvature (within 10% or 0.001, whichever is larger) curvature_error = abs(requested_curvature - apply_curvature) From 185ca97ebae62085dbca7de4ca07097014e19333 Mon Sep 17 00:00:00 2001 From: John Christman Date: Mon, 12 Jan 2026 17:05:56 -0500 Subject: [PATCH 31/46] log --- .../opendbc/car/ford/carcontroller.py | 27 ++++++++++--------- opendbc_repo/opendbc/car/lateral.py | 15 ----------- 2 files changed, 15 insertions(+), 27 deletions(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index b496b32b78..ebad9e5843 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -7,7 +7,7 @@ from common.filter_simple import FirstOrderFilter from opendbc.can import CANPacker from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, apply_hysteresis, structs -from opendbc.car.lateral import ISO_LATERAL_ACCEL, apply_std_steer_angle_limits, get_std_steer_angle_limits +from opendbc.car.lateral import ISO_LATERAL_ACCEL, apply_std_steer_angle_limits from opendbc.car.vehicle_model import VehicleModel from opendbc.car.ford import fordcan from opendbc.car.ford.values import CarControllerParams, FordFlags, CAR @@ -64,10 +64,10 @@ def apply_ford_curvature_limits(self, apply_curvature, apply_curvature_last, cur # Curvature rate limit after driver torque limit apply_curvature = apply_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, steering_angle, lat_active, CarControllerParams.ANGLE_LIMITS) - std_steer_angle_limit = get_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, steering_angle, lat_active, CarControllerParams.ANGLE_LIMITS) + std_steer_angle_limit = abs(apply_std_steer_angle_limits(max_curvature * np.sign(apply_curvature), apply_curvature_last, v_ego_raw, steering_angle, lat_active, CarControllerParams.ANGLE_LIMITS)) if std_steer_angle_limit < max_curvature: self.lateral_limiter = "Std Steer Angle Limit" - max_curvature = np.minimum(max_curvature, abs(std_steer_angle_limit)) + max_curvature = np.minimum(max_curvature, std_steer_angle_limit) # Ford Q4/CAN FD has more torque available compared to Q3/CAN so we limit it based on lateral acceleration. # Safety is not aware of the road roll so we subtract a conservative amount at all times @@ -77,9 +77,9 @@ def apply_ford_curvature_limits(self, apply_curvature, apply_curvature_last, cur apply_curvature = float(np.clip(apply_curvature, -curvature_accel_limit, curvature_accel_limit)) if abs(curvature_accel_limit) < max_curvature: self.lateral_limiter = "CANFD Lat Accel Limit" - max_curvature = np.minimum(max_curvature, abs(curvature_accel_limit)) - max_curvature = np.maximum(max_curvature, abs(apply_curvature)) #limit max_curvature is not less than apply_curvature + + max_curvature = np.maximum(max_curvature, abs(apply_curvature)) #limit max_curvature is not less than apply_curvature return apply_curvature, max_curvature @@ -341,6 +341,8 @@ def update(self, CC, CC_SP, CS, now_nanos): self._update_params() + lateralUncertainty = 0.0 + actuators = CC.actuators hud_control = CC.hudControl main_on = CS.out.cruiseState.available @@ -494,15 +496,15 @@ def update(self, CC, CC_SP, CS, now_nanos): CC.latActive, self.CP) - self.lateralUncertainty = float(requested_curvature / max_curvature) + lateralUncertainty = float(requested_curvature / max_curvature) #debug log - # LOG_PATH = "/data/community/logs/" - # LOG_FILE = "ford_lateral_log.txt" - # if not os.path.exists(LOG_PATH): - # os.makedirs(LOG_PATH) - # with open(LOG_PATH + LOG_FILE, "a") as f: - # f.write(f"lat_uncert: {self.lateralUncertainty:.2f}, req: {requested_curvature:.5f}, apply: {apply_curvature:.5f}, max: {max_curvature:.5f}:{self.lateral_limiter}\n") + LOG_PATH = "/data/community/logs/" + LOG_FILE = "ford_lateral_log.csv" + if not os.path.exists(LOG_PATH): + os.makedirs(LOG_PATH) + with open(LOG_PATH + LOG_FILE, "a") as f: + f.write(f"{self.lateralUncertainty:.2f},{CS.out.vEgoRaw},{requested_curvature:.5f},{apply_curvature:.5f},{max_curvature:.5f},{self.lateral_limiter}\n") #if reset_steering is 1, set apply_curvature to 0 if reset_steering == 1: @@ -837,6 +839,7 @@ def update(self, CC, CC_SP, CS, now_nanos): self.steer_alert_last = steer_alert self.fcw_alert_last = fcw_alert self.lead_distance_bars_last = hud_control.leadDistanceBars + self.lateralUncertainty = lateralUncertainty new_actuators = actuators.as_builder() new_actuators.torqueOutputCan = float(self.steer_warning) diff --git a/opendbc_repo/opendbc/car/lateral.py b/opendbc_repo/opendbc/car/lateral.py index 9f0e83d898..986279dca4 100644 --- a/opendbc_repo/opendbc/car/lateral.py +++ b/opendbc_repo/opendbc/car/lateral.py @@ -90,21 +90,6 @@ def apply_std_steer_angle_limits(apply_angle: float, apply_angle_last: float, v_ return float(np.clip(new_apply_angle, -limits.STEER_ANGLE_MAX, limits.STEER_ANGLE_MAX)) -def get_std_steer_angle_limits(apply_angle: float, apply_angle_last: float, v_ego: float, steering_angle: float, - lat_active: bool, limits: AngleSteeringLimits) -> float: - # pick angle rate limits based on wind up/down - steer_up = apply_angle_last * apply_angle >= 0. and abs(apply_angle) > abs(apply_angle_last) - rate_limits = limits.ANGLE_RATE_LIMIT_UP if steer_up else limits.ANGLE_RATE_LIMIT_DOWN - - angle_rate_lim = np.interp(v_ego, rate_limits[0], rate_limits[1]) - apply_angle_max = apply_angle_last + angle_rate_lim - - if not lat_active: - apply_angle_max = steering_angle - - return float(np.minimum(abs(apply_angle_max), limits.STEER_ANGLE_MAX)) - - def get_max_angle_delta_vm(v_ego_raw: float, VM: VehicleModel, limits): """Calculate the maximum steering angle rate based on lateral jerk limits.""" max_curvature_rate_sec = limits.ANGLE_LIMITS.MAX_LATERAL_JERK / (v_ego_raw ** 2) # (1/m)/s From 680234d44ea521c672f81e67f50448f5d88cf8c2 Mon Sep 17 00:00:00 2001 From: John Christman Date: Mon, 12 Jan 2026 17:17:43 -0500 Subject: [PATCH 32/46] log --- opendbc_repo/opendbc/car/ford/carcontroller.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index ebad9e5843..9fa4e0c307 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -64,10 +64,10 @@ def apply_ford_curvature_limits(self, apply_curvature, apply_curvature_last, cur # Curvature rate limit after driver torque limit apply_curvature = apply_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, steering_angle, lat_active, CarControllerParams.ANGLE_LIMITS) - std_steer_angle_limit = abs(apply_std_steer_angle_limits(max_curvature * np.sign(apply_curvature), apply_curvature_last, v_ego_raw, steering_angle, lat_active, CarControllerParams.ANGLE_LIMITS)) - if std_steer_angle_limit < max_curvature: - self.lateral_limiter = "Std Steer Angle Limit" - max_curvature = np.minimum(max_curvature, std_steer_angle_limit) + # std_steer_angle_limit = abs(apply_std_steer_angle_limits(max_curvature * np.sign(apply_curvature), apply_curvature_last, v_ego_raw, steering_angle, lat_active, CarControllerParams.ANGLE_LIMITS)) + # if std_steer_angle_limit < max_curvature: + # self.lateral_limiter = "Std Steer Angle Limit" + # max_curvature = np.minimum(max_curvature, std_steer_angle_limit) # Ford Q4/CAN FD has more torque available compared to Q3/CAN so we limit it based on lateral acceleration. # Safety is not aware of the road roll so we subtract a conservative amount at all times From 36f5cca00d2357896d9e19f17b116dc44359c374 Mon Sep 17 00:00:00 2001 From: John Christman Date: Mon, 12 Jan 2026 17:36:53 -0500 Subject: [PATCH 33/46] lim --- opendbc_repo/opendbc/car/ford/carcontroller.py | 11 ++++++----- opendbc_repo/opendbc/car/lateral.py | 16 ++++++++++------ 2 files changed, 16 insertions(+), 11 deletions(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index 9fa4e0c307..15db6c0dad 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -7,7 +7,7 @@ from common.filter_simple import FirstOrderFilter from opendbc.can import CANPacker from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, apply_hysteresis, structs -from opendbc.car.lateral import ISO_LATERAL_ACCEL, apply_std_steer_angle_limits +from opendbc.car.lateral import ISO_LATERAL_ACCEL, apply_std_steer_angle_limits, get_steer_angle_rate_limit from opendbc.car.vehicle_model import VehicleModel from opendbc.car.ford import fordcan from opendbc.car.ford.values import CarControllerParams, FordFlags, CAR @@ -64,10 +64,11 @@ def apply_ford_curvature_limits(self, apply_curvature, apply_curvature_last, cur # Curvature rate limit after driver torque limit apply_curvature = apply_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, steering_angle, lat_active, CarControllerParams.ANGLE_LIMITS) - # std_steer_angle_limit = abs(apply_std_steer_angle_limits(max_curvature * np.sign(apply_curvature), apply_curvature_last, v_ego_raw, steering_angle, lat_active, CarControllerParams.ANGLE_LIMITS)) - # if std_steer_angle_limit < max_curvature: - # self.lateral_limiter = "Std Steer Angle Limit" - # max_curvature = np.minimum(max_curvature, std_steer_angle_limit) + std_steer_angle_rate_limit = get_steer_angle_rate_limit(apply_curvature, apply_curvature_last, v_ego_raw, CarControllerParams.ANGLE_LIMITS) + std_steer_angle_limit = abs(apply_curvature_last) + abs(std_steer_angle_rate_limit) + if std_steer_angle_limit < max_curvature: + self.lateral_limiter = "Std Steer Angle Limit" + max_curvature = np.minimum(max_curvature, std_steer_angle_limit) # Ford Q4/CAN FD has more torque available compared to Q3/CAN so we limit it based on lateral acceleration. # Safety is not aware of the road roll so we subtract a conservative amount at all times diff --git a/opendbc_repo/opendbc/car/lateral.py b/opendbc_repo/opendbc/car/lateral.py index 986279dca4..e00d59d789 100644 --- a/opendbc_repo/opendbc/car/lateral.py +++ b/opendbc_repo/opendbc/car/lateral.py @@ -75,17 +75,21 @@ def apply_meas_steer_torque_limits(apply_torque, apply_torque_last, motor_torque LIMITS.STEER_ERROR_MAX, LIMITS.STEER_MAX))) -def apply_std_steer_angle_limits(apply_angle: float, apply_angle_last: float, v_ego: float, steering_angle: float, - lat_active: bool, limits: AngleSteeringLimits) -> float: +def get_steer_angle_rate_limit(apply_angle: float, apply_angle_last: float, v_ego: float, limits: AngleSteeringLimits) -> float: # pick angle rate limits based on wind up/down steer_up = apply_angle_last * apply_angle >= 0. and abs(apply_angle) > abs(apply_angle_last) rate_limits = limits.ANGLE_RATE_LIMIT_UP if steer_up else limits.ANGLE_RATE_LIMIT_DOWN - angle_rate_lim = np.interp(v_ego, rate_limits[0], rate_limits[1]) - new_apply_angle = np.clip(apply_angle, apply_angle_last - angle_rate_lim, apply_angle_last + angle_rate_lim) + return np.interp(v_ego, rate_limits[0], rate_limits[1]) - # angle is current steering wheel angle when inactive on all angle cars - if not lat_active: + +def apply_std_steer_angle_limits(apply_angle: float, apply_angle_last: float, v_ego: float, steering_angle: float, + lat_active: bool, limits: AngleSteeringLimits) -> float: + if lat_active: + angle_rate_lim = get_steer_angle_rate_limit(apply_angle, apply_angle_last, v_ego, limits) + new_apply_angle = np.clip(apply_angle, apply_angle_last - angle_rate_lim, apply_angle_last + angle_rate_lim) + else: + # angle is current steering wheel angle when inactive on all angle cars new_apply_angle = steering_angle return float(np.clip(new_apply_angle, -limits.STEER_ANGLE_MAX, limits.STEER_ANGLE_MAX)) From 508519dc7f6072d58e5bc523a2310c759e332a4f Mon Sep 17 00:00:00 2001 From: John Christman Date: Mon, 12 Jan 2026 18:12:18 -0500 Subject: [PATCH 34/46] lim --- opendbc_repo/opendbc/car/ford/carcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index 15db6c0dad..14629f137d 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -505,7 +505,7 @@ def update(self, CC, CC_SP, CS, now_nanos): if not os.path.exists(LOG_PATH): os.makedirs(LOG_PATH) with open(LOG_PATH + LOG_FILE, "a") as f: - f.write(f"{self.lateralUncertainty:.2f},{CS.out.vEgoRaw},{requested_curvature:.5f},{apply_curvature:.5f},{max_curvature:.5f},{self.lateral_limiter}\n") + f.write(f"{lateralUncertainty:.2f},{CS.out.vEgoRaw},{requested_curvature:.5f},{apply_curvature:.5f},{max_curvature:.5f},{self.lateral_limiter}\n") #if reset_steering is 1, set apply_curvature to 0 if reset_steering == 1: From c8086f59a45ff42db1c244d2f9f8484471c9a2ed Mon Sep 17 00:00:00 2001 From: John Christman Date: Mon, 12 Jan 2026 18:23:19 -0500 Subject: [PATCH 35/46] log --- opendbc_repo/opendbc/car/ford/carcontroller.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index 14629f137d..84e3f4f1f8 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -80,7 +80,7 @@ def apply_ford_curvature_limits(self, apply_curvature, apply_curvature_last, cur self.lateral_limiter = "CANFD Lat Accel Limit" max_curvature = np.minimum(max_curvature, abs(curvature_accel_limit)) - max_curvature = np.maximum(max_curvature, abs(apply_curvature)) #limit max_curvature is not less than apply_curvature + #max_curvature = np.maximum(max_curvature, abs(apply_curvature)) #limit max_curvature is not less than apply_curvature return apply_curvature, max_curvature @@ -500,12 +500,12 @@ def update(self, CC, CC_SP, CS, now_nanos): lateralUncertainty = float(requested_curvature / max_curvature) #debug log - LOG_PATH = "/data/community/logs/" - LOG_FILE = "ford_lateral_log.csv" - if not os.path.exists(LOG_PATH): - os.makedirs(LOG_PATH) - with open(LOG_PATH + LOG_FILE, "a") as f: - f.write(f"{lateralUncertainty:.2f},{CS.out.vEgoRaw},{requested_curvature:.5f},{apply_curvature:.5f},{max_curvature:.5f},{self.lateral_limiter}\n") + # LOG_PATH = "/data/community/logs/" + # LOG_FILE = "ford_lateral_log.csv" + # if not os.path.exists(LOG_PATH): + # os.makedirs(LOG_PATH) + # with open(LOG_PATH + LOG_FILE, "a") as f: + # f.write(f"{lateralUncertainty:.2f},{CS.out.vEgoRaw},{requested_curvature:.5f},{apply_curvature:.5f},{max_curvature:.5f},{self.lateral_limiter}\n") #if reset_steering is 1, set apply_curvature to 0 if reset_steering == 1: From 07906865bfcb116d41da32d2449e9125a3c91bc3 Mon Sep 17 00:00:00 2001 From: John Christman Date: Mon, 12 Jan 2026 18:35:15 -0500 Subject: [PATCH 36/46] cleanuo --- opendbc_repo/opendbc/car/ford/carcontroller.py | 6 ++++-- opendbc_repo/opendbc/car/lateral.py | 16 ++++++---------- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index 84e3f4f1f8..f003f17bde 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -7,7 +7,7 @@ from common.filter_simple import FirstOrderFilter from opendbc.can import CANPacker from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, apply_hysteresis, structs -from opendbc.car.lateral import ISO_LATERAL_ACCEL, apply_std_steer_angle_limits, get_steer_angle_rate_limit +from opendbc.car.lateral import ISO_LATERAL_ACCEL, apply_std_steer_angle_limits from opendbc.car.vehicle_model import VehicleModel from opendbc.car.ford import fordcan from opendbc.car.ford.values import CarControllerParams, FordFlags, CAR @@ -64,7 +64,9 @@ def apply_ford_curvature_limits(self, apply_curvature, apply_curvature_last, cur # Curvature rate limit after driver torque limit apply_curvature = apply_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, steering_angle, lat_active, CarControllerParams.ANGLE_LIMITS) - std_steer_angle_rate_limit = get_steer_angle_rate_limit(apply_curvature, apply_curvature_last, v_ego_raw, CarControllerParams.ANGLE_LIMITS) + steer_up = apply_curvature_last * apply_curvature >= 0. and abs(apply_curvature) > abs(apply_curvature_last) + rate_limits = CarControllerParams.ANGLE_LIMITS.ANGLE_RATE_LIMIT_UP if steer_up else CarControllerParams.ANGLE_LIMITS.ANGLE_RATE_LIMIT_DOWN + std_steer_angle_rate_limit = np.interp(v_ego, rate_limits[0], rate_limits[1]) std_steer_angle_limit = abs(apply_curvature_last) + abs(std_steer_angle_rate_limit) if std_steer_angle_limit < max_curvature: self.lateral_limiter = "Std Steer Angle Limit" diff --git a/opendbc_repo/opendbc/car/lateral.py b/opendbc_repo/opendbc/car/lateral.py index e00d59d789..986279dca4 100644 --- a/opendbc_repo/opendbc/car/lateral.py +++ b/opendbc_repo/opendbc/car/lateral.py @@ -75,21 +75,17 @@ def apply_meas_steer_torque_limits(apply_torque, apply_torque_last, motor_torque LIMITS.STEER_ERROR_MAX, LIMITS.STEER_MAX))) -def get_steer_angle_rate_limit(apply_angle: float, apply_angle_last: float, v_ego: float, limits: AngleSteeringLimits) -> float: +def apply_std_steer_angle_limits(apply_angle: float, apply_angle_last: float, v_ego: float, steering_angle: float, + lat_active: bool, limits: AngleSteeringLimits) -> float: # pick angle rate limits based on wind up/down steer_up = apply_angle_last * apply_angle >= 0. and abs(apply_angle) > abs(apply_angle_last) rate_limits = limits.ANGLE_RATE_LIMIT_UP if steer_up else limits.ANGLE_RATE_LIMIT_DOWN - return np.interp(v_ego, rate_limits[0], rate_limits[1]) + angle_rate_lim = np.interp(v_ego, rate_limits[0], rate_limits[1]) + new_apply_angle = np.clip(apply_angle, apply_angle_last - angle_rate_lim, apply_angle_last + angle_rate_lim) - -def apply_std_steer_angle_limits(apply_angle: float, apply_angle_last: float, v_ego: float, steering_angle: float, - lat_active: bool, limits: AngleSteeringLimits) -> float: - if lat_active: - angle_rate_lim = get_steer_angle_rate_limit(apply_angle, apply_angle_last, v_ego, limits) - new_apply_angle = np.clip(apply_angle, apply_angle_last - angle_rate_lim, apply_angle_last + angle_rate_lim) - else: - # angle is current steering wheel angle when inactive on all angle cars + # angle is current steering wheel angle when inactive on all angle cars + if not lat_active: new_apply_angle = steering_angle return float(np.clip(new_apply_angle, -limits.STEER_ANGLE_MAX, limits.STEER_ANGLE_MAX)) From 6538921d679fee2769063364be142d2913c28063 Mon Sep 17 00:00:00 2001 From: John Christman Date: Mon, 12 Jan 2026 18:35:59 -0500 Subject: [PATCH 37/46] cleanup --- opendbc_repo/opendbc/car/lateral.py | 1 + 1 file changed, 1 insertion(+) diff --git a/opendbc_repo/opendbc/car/lateral.py b/opendbc_repo/opendbc/car/lateral.py index 986279dca4..9d120e308b 100644 --- a/opendbc_repo/opendbc/car/lateral.py +++ b/opendbc_repo/opendbc/car/lateral.py @@ -90,6 +90,7 @@ def apply_std_steer_angle_limits(apply_angle: float, apply_angle_last: float, v_ return float(np.clip(new_apply_angle, -limits.STEER_ANGLE_MAX, limits.STEER_ANGLE_MAX)) + def get_max_angle_delta_vm(v_ego_raw: float, VM: VehicleModel, limits): """Calculate the maximum steering angle rate based on lateral jerk limits.""" max_curvature_rate_sec = limits.ANGLE_LIMITS.MAX_LATERAL_JERK / (v_ego_raw ** 2) # (1/m)/s From e4c11fa9a381d581eaca0234f1bafe70f82a16de Mon Sep 17 00:00:00 2001 From: John Christman Date: Mon, 12 Jan 2026 18:38:45 -0500 Subject: [PATCH 38/46] cleanup --- opendbc_repo/opendbc/car/ford/carcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index f003f17bde..52e4baa72c 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -66,7 +66,7 @@ def apply_ford_curvature_limits(self, apply_curvature, apply_curvature_last, cur steer_up = apply_curvature_last * apply_curvature >= 0. and abs(apply_curvature) > abs(apply_curvature_last) rate_limits = CarControllerParams.ANGLE_LIMITS.ANGLE_RATE_LIMIT_UP if steer_up else CarControllerParams.ANGLE_LIMITS.ANGLE_RATE_LIMIT_DOWN - std_steer_angle_rate_limit = np.interp(v_ego, rate_limits[0], rate_limits[1]) + std_steer_angle_rate_limit = np.interp(v_ego_raw, rate_limits[0], rate_limits[1]) std_steer_angle_limit = abs(apply_curvature_last) + abs(std_steer_angle_rate_limit) if std_steer_angle_limit < max_curvature: self.lateral_limiter = "Std Steer Angle Limit" From 40a73ff038670b17a70de4c1f9173b59b4a61023 Mon Sep 17 00:00:00 2001 From: John Christman Date: Mon, 12 Jan 2026 18:59:32 -0500 Subject: [PATCH 39/46] refactor --- opendbc_repo/opendbc/car/ford/carcontroller.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index 52e4baa72c..d8aae9f03c 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -344,8 +344,6 @@ def update(self, CC, CC_SP, CS, now_nanos): self._update_params() - lateralUncertainty = 0.0 - actuators = CC.actuators hud_control = CC.hudControl main_on = CS.out.cruiseState.available @@ -499,6 +497,8 @@ def update(self, CC, CC_SP, CS, now_nanos): CC.latActive, self.CP) + + max_curvature = np.clip(max_curvature, apply_curvature, self.curvature_max) # ensure max_curvature is within reasonable bounds lateralUncertainty = float(requested_curvature / max_curvature) #debug log @@ -713,7 +713,9 @@ def update(self, CC, CC_SP, CS, now_nanos): self.HC_PID_controller.reset() self.LC_PID_controller.reset() ramp_type = 0 + lateralUncertainty = 0.0 + self.lateralUncertainty = lateralUncertainty self.apply_curvature_last = apply_curvature self.curvature_rate_last = desired_curvature_rate self.path_offset_last = path_offset @@ -842,7 +844,6 @@ def update(self, CC, CC_SP, CS, now_nanos): self.steer_alert_last = steer_alert self.fcw_alert_last = fcw_alert self.lead_distance_bars_last = hud_control.leadDistanceBars - self.lateralUncertainty = lateralUncertainty new_actuators = actuators.as_builder() new_actuators.torqueOutputCan = float(self.steer_warning) From 4d0663920515a009ae14f4eab8e4ee7273096e36 Mon Sep 17 00:00:00 2001 From: John Christman Date: Mon, 12 Jan 2026 19:11:54 -0500 Subject: [PATCH 40/46] remove log --- .../opendbc/car/ford/carcontroller.py | 21 ++----------------- 1 file changed, 2 insertions(+), 19 deletions(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index d8aae9f03c..af33e456dd 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -1,5 +1,4 @@ import math -import os import cereal.messaging as messaging import numpy as np from numpy import clip, interp @@ -52,14 +51,13 @@ def anti_overshoot(apply_curvature, apply_curvature_last, v_ego): return float(np.interp(v_ego, [5, 10], [apply_curvature, output_curvature])) -def apply_ford_curvature_limits(self, apply_curvature, apply_curvature_last, current_curvature, v_ego_raw, steering_angle, lat_active, CP): +def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_curvature, v_ego_raw, steering_angle, lat_active, CP): max_curvature = 1 # large initial value # No blending at low speed due to lack of torque wind-up and inaccurate current curvature if v_ego_raw > 9: apply_curvature = np.clip(apply_curvature, current_curvature - CarControllerParams.CURVATURE_ERROR, current_curvature + CarControllerParams.CURVATURE_ERROR) max_curvature = abs(current_curvature) + CarControllerParams.CURVATURE_ERROR - self.lateral_limiter = "Curvature Error Limit" # Curvature rate limit after driver torque limit apply_curvature = apply_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, steering_angle, lat_active, CarControllerParams.ANGLE_LIMITS) @@ -68,8 +66,6 @@ def apply_ford_curvature_limits(self, apply_curvature, apply_curvature_last, cur rate_limits = CarControllerParams.ANGLE_LIMITS.ANGLE_RATE_LIMIT_UP if steer_up else CarControllerParams.ANGLE_LIMITS.ANGLE_RATE_LIMIT_DOWN std_steer_angle_rate_limit = np.interp(v_ego_raw, rate_limits[0], rate_limits[1]) std_steer_angle_limit = abs(apply_curvature_last) + abs(std_steer_angle_rate_limit) - if std_steer_angle_limit < max_curvature: - self.lateral_limiter = "Std Steer Angle Limit" max_curvature = np.minimum(max_curvature, std_steer_angle_limit) # Ford Q4/CAN FD has more torque available compared to Q3/CAN so we limit it based on lateral acceleration. @@ -78,12 +74,8 @@ def apply_ford_curvature_limits(self, apply_curvature, apply_curvature_last, cur # Limit curvature to conservative max lateral acceleration curvature_accel_limit = MAX_LATERAL_ACCEL / (max(v_ego_raw, 1) ** 2) apply_curvature = float(np.clip(apply_curvature, -curvature_accel_limit, curvature_accel_limit)) - if abs(curvature_accel_limit) < max_curvature: - self.lateral_limiter = "CANFD Lat Accel Limit" max_curvature = np.minimum(max_curvature, abs(curvature_accel_limit)) - #max_curvature = np.maximum(max_curvature, abs(apply_curvature)) #limit max_curvature is not less than apply_curvature - return apply_curvature, max_curvature @@ -122,7 +114,6 @@ def __init__(self, dbc_names, CP, CP_SP): self.steering_wheel_delta_adjusted = 0.0 self.last_button_frame = 0 # Track last ICBM button press frame self.lateralUncertainty = 0.0 - self.lateral_limiter = "None" # for logging ################################## lateral control parameters ############################################## @@ -489,7 +480,7 @@ def update(self, CC, CC_SP, CS, now_nanos): requested_curvature = 0.0 # apply curvature limits - apply_curvature, max_curvature = apply_ford_curvature_limits(self, requested_curvature, + apply_curvature, max_curvature = apply_ford_curvature_limits(requested_curvature, self.apply_curvature_last, current_curvature, CS.out.vEgoRaw, @@ -501,14 +492,6 @@ def update(self, CC, CC_SP, CS, now_nanos): max_curvature = np.clip(max_curvature, apply_curvature, self.curvature_max) # ensure max_curvature is within reasonable bounds lateralUncertainty = float(requested_curvature / max_curvature) - #debug log - # LOG_PATH = "/data/community/logs/" - # LOG_FILE = "ford_lateral_log.csv" - # if not os.path.exists(LOG_PATH): - # os.makedirs(LOG_PATH) - # with open(LOG_PATH + LOG_FILE, "a") as f: - # f.write(f"{lateralUncertainty:.2f},{CS.out.vEgoRaw},{requested_curvature:.5f},{apply_curvature:.5f},{max_curvature:.5f},{self.lateral_limiter}\n") - #if reset_steering is 1, set apply_curvature to 0 if reset_steering == 1: apply_curvature = 0.0 From 7a2cbe7296522a7f0aac677ec7180966aa2b8088 Mon Sep 17 00:00:00 2001 From: John Christman Date: Mon, 12 Jan 2026 19:29:47 -0500 Subject: [PATCH 41/46] code review --- selfdrive/car/card.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 498bc81946..420301d013 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -287,6 +287,8 @@ def controls_update(self, CS: car.CarState, CC: car.CarControl, CC_SP: custom.Ca self.last_actuators_output, can_sends = self.CI.apply(CC, convert_carControlSP(CC_SP), now_nanos) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) + self.CC_prev = CC + if hasattr(self.CI.CC, "lateralUncertainty"): cs_bp = structs.ControllerStateBP() cs_bp.lateralUncertainty = self.CI.CC.lateralUncertainty @@ -296,7 +298,6 @@ def controls_update(self, CS: car.CarState, CC: car.CarControl, CC_SP: custom.Ca cs_bp_send.controllerStateBP = cs_bp_capnp self.pm.send('controllerStateBP', cs_bp_send) - self.CC_prev = CC def step(self): CS, CS_SP, RD = self.state_update() From bfa8d422145e219ebbb0b35a24390307de83cc1d Mon Sep 17 00:00:00 2001 From: John Christman Date: Tue, 13 Jan 2026 13:24:12 -0500 Subject: [PATCH 42/46] code review --- opendbc_repo/opendbc/car/ford/carcontroller.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index af33e456dd..36bdb8460f 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -673,8 +673,9 @@ def update(self, CC, CC_SP, CS, now_nanos): current_curvature = -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1) - self.apply_curvature_last = apply_ford_curvature_limits(apply_curvature, self.apply_curvature_last, current_curvature, + self.apply_curvature_last, _ = apply_ford_curvature_limits(apply_curvature, self.apply_curvature_last, current_curvature, CS.out.vEgoRaw, 0., CC.latActive, self.CP) + lateralUncertainty = 0.0 #rem bluepilot sends apply_curvature, and at some point openpilot swapped to sending apply_curvature_last. apply_curvature = self.apply_curvature_last From a1c6f1e121387584f5d62ef9fe31a7be00d5b267 Mon Sep 17 00:00:00 2001 From: John Christman Date: Tue, 13 Jan 2026 14:04:27 -0500 Subject: [PATCH 43/46] handle disable_BP_lat --- opendbc_repo/opendbc/car/ford/carcontroller.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index 36bdb8460f..622c7bfd0a 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -315,6 +315,10 @@ def handle_post_lane_change_transition(self, path_angle, path_offset, desired_cu return (path_angle, path_offset, desired_curvature_rate) + def update_lateral_uncertainty(self, requested_curvature, apply_curvature, max_curvature): + max_curvature = np.clip(max_curvature, apply_curvature, self.curvature_max) # ensure max_curvature is within reasonable bounds + return float(requested_curvature / max_curvature) + def update(self, CC, CC_SP, CS, now_nanos): can_sends = [] self.sm.update(0) @@ -487,10 +491,7 @@ def update(self, CC, CC_SP, CS, now_nanos): 0, CC.latActive, self.CP) - - - max_curvature = np.clip(max_curvature, apply_curvature, self.curvature_max) # ensure max_curvature is within reasonable bounds - lateralUncertainty = float(requested_curvature / max_curvature) + lateralUncertainty = self.update_lateral_uncertainty(requested_curvature, apply_curvature, max_curvature) #if reset_steering is 1, set apply_curvature to 0 if reset_steering == 1: @@ -673,9 +674,9 @@ def update(self, CC, CC_SP, CS, now_nanos): current_curvature = -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1) - self.apply_curvature_last, _ = apply_ford_curvature_limits(apply_curvature, self.apply_curvature_last, current_curvature, + self.apply_curvature_last, max_curvature = apply_ford_curvature_limits(apply_curvature, self.apply_curvature_last, current_curvature, CS.out.vEgoRaw, 0., CC.latActive, self.CP) - lateralUncertainty = 0.0 + lateralUncertainty = self.update_lateral_uncertainty(requested_curvature, apply_curvature, max_curvature) #rem bluepilot sends apply_curvature, and at some point openpilot swapped to sending apply_curvature_last. apply_curvature = self.apply_curvature_last From 19be3aba5928b2253672c911ce1961a088bbcc66 Mon Sep 17 00:00:00 2001 From: John Christman Date: Tue, 13 Jan 2026 14:08:00 -0500 Subject: [PATCH 44/46] cleanup --- opendbc_repo/opendbc/car/ford/carcontroller.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index 622c7bfd0a..8c21ea9e17 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -315,7 +315,7 @@ def handle_post_lane_change_transition(self, path_angle, path_offset, desired_cu return (path_angle, path_offset, desired_curvature_rate) - def update_lateral_uncertainty(self, requested_curvature, apply_curvature, max_curvature): + def calculate_lateral_uncertainty(self, requested_curvature, apply_curvature, max_curvature): max_curvature = np.clip(max_curvature, apply_curvature, self.curvature_max) # ensure max_curvature is within reasonable bounds return float(requested_curvature / max_curvature) @@ -491,7 +491,7 @@ def update(self, CC, CC_SP, CS, now_nanos): 0, CC.latActive, self.CP) - lateralUncertainty = self.update_lateral_uncertainty(requested_curvature, apply_curvature, max_curvature) + lateralUncertainty = self.calculate_lateral_uncertainty(requested_curvature, apply_curvature, max_curvature) #if reset_steering is 1, set apply_curvature to 0 if reset_steering == 1: @@ -676,7 +676,7 @@ def update(self, CC, CC_SP, CS, now_nanos): self.apply_curvature_last, max_curvature = apply_ford_curvature_limits(apply_curvature, self.apply_curvature_last, current_curvature, CS.out.vEgoRaw, 0., CC.latActive, self.CP) - lateralUncertainty = self.update_lateral_uncertainty(requested_curvature, apply_curvature, max_curvature) + lateralUncertainty = self.calculate_lateral_uncertainty(requested_curvature, apply_curvature, max_curvature) #rem bluepilot sends apply_curvature, and at some point openpilot swapped to sending apply_curvature_last. apply_curvature = self.apply_curvature_last From 011943cb088d8585d63537e033057672f4bf3e49 Mon Sep 17 00:00:00 2001 From: John Christman Date: Tue, 13 Jan 2026 16:20:48 -0500 Subject: [PATCH 45/46] code order --- opendbc_repo/opendbc/car/ford/carcontroller.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index 8c21ea9e17..896b696b85 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -676,11 +676,12 @@ def update(self, CC, CC_SP, CS, now_nanos): self.apply_curvature_last, max_curvature = apply_ford_curvature_limits(apply_curvature, self.apply_curvature_last, current_curvature, CS.out.vEgoRaw, 0., CC.latActive, self.CP) - lateralUncertainty = self.calculate_lateral_uncertainty(requested_curvature, apply_curvature, max_curvature) #rem bluepilot sends apply_curvature, and at some point openpilot swapped to sending apply_curvature_last. apply_curvature = self.apply_curvature_last + lateralUncertainty = self.calculate_lateral_uncertainty(requested_curvature, apply_curvature, max_curvature) + # reset steering by setting all values to 0 and ramp_type to immediate if reset_steering == 1: ramp_type = 3 From 6bfe6f711419ec1594b3eb004a32098b0cdf6ff7 Mon Sep 17 00:00:00 2001 From: John Christman Date: Thu, 15 Jan 2026 12:56:11 -0500 Subject: [PATCH 46/46] cleanup --- cereal/custom.capnp | 6 +++--- cereal/log.capnp | 4 ++-- selfdrive/ui/mici/onroad/hud_renderer.py | 3 ++- selfdrive/ui/mici/onroad/torque_bar.py | 3 --- 4 files changed, 7 insertions(+), 9 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index dda9369dee..29bef7ad38 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -454,8 +454,7 @@ struct ModelDataV2SP @0xa1680744031fdb2d { } } -struct ControllerStateBP @0xcb9fd56c7057593a { - lateralUncertainty @0 :Float32; #BluePilot +struct CustomReserved10 @0xcb9fd56c7057593a { } struct CustomReserved11 @0xc2243c65e0340384 { @@ -464,7 +463,8 @@ struct CustomReserved11 @0xc2243c65e0340384 { struct CustomReserved12 @0x9ccdc8676701b412 { } -struct CustomReserved13 @0xcd96dafb67a082d0 { +struct ControllerStateBP @0xcd96dafb67a082d0 { + lateralUncertainty @0 :Float32; #BluePilot } struct CustomReserved14 @0xb057204d7deadf3f { diff --git a/cereal/log.capnp b/cereal/log.capnp index c5801a870f..0c1abbb34d 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2635,10 +2635,10 @@ struct Event { carStateSP @114 :Custom.CarStateSP; liveMapDataSP @115 :Custom.LiveMapDataSP; modelDataV2SP @116 :Custom.ModelDataV2SP; - controllerStateBP @136 :Custom.ControllerStateBP; + customReserved10 @136 :Custom.CustomReserved10; customReserved11 @137 :Custom.CustomReserved11; customReserved12 @138 :Custom.CustomReserved12; - customReserved13 @139 :Custom.CustomReserved13; + controllerStateBP @139 :Custom.ControllerStateBP; customReserved14 @140 :Custom.CustomReserved14; customReserved15 @141 :Custom.CustomReserved15; customReserved16 @142 :Custom.CustomReserved16; diff --git a/selfdrive/ui/mici/onroad/hud_renderer.py b/selfdrive/ui/mici/onroad/hud_renderer.py index ab02b9c218..503fd315de 100644 --- a/selfdrive/ui/mici/onroad/hud_renderer.py +++ b/selfdrive/ui/mici/onroad/hud_renderer.py @@ -172,7 +172,8 @@ def _update_state(self) -> None: def _render(self, rect: rl.Rectangle) -> None: """Render HUD elements to the screen.""" - self._torque_bar.render(rect) + if ui_state.sm['controlsState'].lateralControlState.which() != 'angleState' or ui_state.sm.updated["controllerStateBP"]: + self._torque_bar.render(rect) if self.is_cruise_set: self._draw_set_speed(rect) diff --git a/selfdrive/ui/mici/onroad/torque_bar.py b/selfdrive/ui/mici/onroad/torque_bar.py index bb8bc07d9b..e45e565531 100644 --- a/selfdrive/ui/mici/onroad/torque_bar.py +++ b/selfdrive/ui/mici/onroad/torque_bar.py @@ -185,9 +185,6 @@ def _update_state(self): self._torque_filter.update(-ui_state.sm['carOutput'].actuatorsOutput.torque) def _render(self, rect: rl.Rectangle) -> None: - if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState' and not ui_state.sm.updated["controllerStateBP"]: - return - # adjust y pos with torque torque_line_offset = np.interp(abs(self._torque_filter.x), [0.5, 1], [22, 26]) torque_line_height = np.interp(abs(self._torque_filter.x), [0.5, 1], [14, 56])