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)