diff --git a/selfdrive/ui/sunnypilot/onroad/developer_ui/__init__.py b/selfdrive/ui/sunnypilot/onroad/developer_ui/__init__.py index 441a169d2..f74f1f4c3 100644 --- a/selfdrive/ui/sunnypilot/onroad/developer_ui/__init__.py +++ b/selfdrive/ui/sunnypilot/onroad/developer_ui/__init__.py @@ -10,7 +10,7 @@ from openpilot.selfdrive.ui.sunnypilot.onroad.developer_ui.elements import ( UiElement, RelDistElement, RelSpeedElement, SteeringAngleElement, DesiredLateralAccelElement, ActualLateralAccelElement, DesiredSteeringAngleElement, AEgoElement, LeadSpeedElement, FrictionCoefficientElement, LatAccelFactorElement, - SteeringTorqueEpsElement, BearingDegElement, AltitudeElement + SteeringTorqueEpsElement, BearingDegElement, AltitudeElement, DesiredSteeringPIDElement ) from openpilot.system.ui.lib.application import gui_app, FontWeight from openpilot.system.ui.lib.text_measure import measure_text_cached @@ -36,6 +36,7 @@ class DeveloperUiRenderer(Widget): self.desired_lat_accel_elem = DesiredLateralAccelElement() self.actual_lat_accel_elem = ActualLateralAccelElement() self.desired_steer_elem = DesiredSteeringAngleElement() + self.desired_pid_steer_elem = DesiredSteeringPIDElement() self.a_ego_elem = AEgoElement() self.lead_speed_elem = LeadSpeedElement() self.friction_elem = FrictionCoefficientElement() @@ -85,8 +86,10 @@ class DeveloperUiRenderer(Widget): ] if controls_state.lateralControlState.which() == 'torqueState': elements.append(self.desired_lat_accel_elem.update(sm, ui_state.is_metric)) - else: + elif controls_state.lateralControlState.which() == 'angleState': elements.append(self.desired_steer_elem.update(sm, ui_state.is_metric)) + elif controls_state.lateralControlState.which() == 'pidState': + elements.append(self.desired_pid_steer_elem.update(sm, ui_state.is_metric)) elements.append(self.actual_lat_accel_elem.update(sm, ui_state.is_metric)) diff --git a/selfdrive/ui/sunnypilot/onroad/developer_ui/elements.py b/selfdrive/ui/sunnypilot/onroad/developer_ui/elements.py index 652469bdd..94e3af42e 100644 --- a/selfdrive/ui/sunnypilot/onroad/developer_ui/elements.py +++ b/selfdrive/ui/sunnypilot/onroad/developer_ui/elements.py @@ -191,6 +191,31 @@ class DesiredLateralAccelElement(LateralControlElement): return UiElement(value, "DESIRED L.A.", self.unit, color) +class DesiredSteeringPIDElement(LateralControlElement): + def __init__(self): + self.unit = "" + + def update(self, sm, is_metric: bool) -> UiElement: + car_state = sm['carState'] + controls_state = sm['controlsState'] + lat_active = sm['carControl'].latActive + angle_steers = car_state.steeringAngleDeg + steer_angle_desired = controls_state.lateralControlState.pidState.steeringAngleDesiredDeg + + value = f"{steer_angle_desired:.1f}°" if lat_active else "-" + + color = rl.WHITE + if lat_active: + if abs(angle_steers) > 180: + color = rl.RED + elif abs(angle_steers) > 90: + color = rl.Color(255, 188, 0, 255) + else: + color = rl.Color(0, 255, 0, 255) + + return UiElement(value, "DESIRED STEER", self.unit, color) + + class AEgoElement: def __init__(self): self.unit = "m/s^2"