From 7e1a8d41a1b83e53ef483956513f7e2974ac042d Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 3 Mar 2026 21:45:49 -0800 Subject: [PATCH] steering arc: enable for angle cars (#37078) * enable for angle cars * use carparams * less roll at low speed, it's too pronounced * clean up --- selfdrive/ui/mici/onroad/hud_renderer.py | 3 +-- selfdrive/ui/mici/onroad/torque_bar.py | 20 +++++++++++++++----- 2 files changed, 16 insertions(+), 7 deletions(-) diff --git a/selfdrive/ui/mici/onroad/hud_renderer.py b/selfdrive/ui/mici/onroad/hud_renderer.py index 3b056c3e0..21e30e742 100644 --- a/selfdrive/ui/mici/onroad/hud_renderer.py +++ b/selfdrive/ui/mici/onroad/hud_renderer.py @@ -172,8 +172,7 @@ class HudRenderer(Widget): 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) + 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 d7c9f27a9..1338c8dfb 100644 --- a/selfdrive/ui/mici/onroad/torque_bar.py +++ b/selfdrive/ui/mici/onroad/torque_bar.py @@ -145,6 +145,9 @@ def arc_bar_pts(cx: float, cy: float, return pts +DEFAULT_MAX_LAT_ACCEL = 3.0 # m/s^2 + + class TorqueBar(Widget): def __init__(self, demo: bool = False): super().__init__() @@ -165,16 +168,23 @@ class TorqueBar(Widget): 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 + car_control = ui_state.sm['carControl'] - # from selfdrived + # Include lateral accel error in estimated torque utilization 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)) + # Include road roll in estimated torque utilization + # Roll is less accurate near standstill, so reduce its effect at low speed + roll_compensation = live_parameters.roll * ACCELERATION_DUE_TO_GRAVITY * np.interp(car_state.vEgo, [5, 15], [0.0, 1.0]) + lateral_acceleration = actual_lateral_accel - roll_compensation + max_lateral_acceleration = ui_state.CP.maxLateralAccel if ui_state.CP else DEFAULT_MAX_LAT_ACCEL + + if not car_control.latActive: + self._torque_filter.update(0.0) + else: + self._torque_filter.update(np.clip((lateral_acceleration + accel_diff) / max_lateral_acceleration, -1, 1)) else: self._torque_filter.update(-ui_state.sm['carOutput'].actuatorsOutput.torque)