mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-07 06:32:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

steering arc: enable for angle cars (#37078)

Browse Source 
* enable for angle cars

* use carparams

* less roll at low speed, it's too pronounced

* clean up
This commit is contained in:
Shane Smiskol
2026-03-03 21:45:49 -08:00
committed by GitHub
parent 0c452dbafe
commit 7e1a8d41a1
2 changed files with 16 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/ui/mici/onroad/hud_renderer.py
+1 -2
View File
@@ -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)
selfdrive/ui/mici/onroad/torque_bar.py
+15 -5
View File
@@ -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)