diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index bb7989381b..44f21b9761 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -186,6 +186,7 @@ inline static std::unordered_map keys = { {"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}}, {"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}}, {"TrueVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"CustomButtonAction", {PERSISTENT | BACKUP, INT, "0"}}, // MADS params {"Mads", {PERSISTENT | BACKUP, BOOL, "1"}}, diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index 12b4f9da61..8b62808dc0 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -139,23 +139,16 @@ class LongitudinalPlanner(LongitudinalPlannerSP): output_a_target_e2e = sm['modelV2'].action.desiredAcceleration output_should_stop_e2e = sm['modelV2'].action.shouldStop - if self.is_e2e(sm): - output_a_target = min(output_a_target_e2e, output_a_target_mpc) - self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc - if output_a_target < output_a_target_mpc: - self.mpc.source = LongitudinalPlanSource.e2e - else: - output_a_target = output_a_target_mpc - self.output_should_stop = output_should_stop_mpc + is_e2e = self.is_e2e(sm) - self.a_cruise = get_cruise_accel(sm['selfdriveState'].experimentalMode, v_cruise, v_ego, + self.a_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego, self.a_cruise, steer_angle_without_offset, self.CP, self.dt, accel_coast, self.allow_throttle) cruise_should_stop = should_stop(v_ego, self.a_cruise) candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc), (self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop)] - if sm['selfdriveState'].experimentalMode: + if is_e2e: candidates.append((output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e)) output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0]) diff --git a/openpilot/selfdrive/ui/layouts/main.py b/openpilot/selfdrive/ui/layouts/main.py index c4224d6467..52de91d906 100644 --- a/openpilot/selfdrive/ui/layouts/main.py +++ b/openpilot/selfdrive/ui/layouts/main.py @@ -3,6 +3,7 @@ from enum import IntEnum import openpilot.cereal.messaging as messaging from openpilot.system.ui.lib.application import gui_app from openpilot.system.ui.widgets import Widget +from openpilot.selfdrive.ui.sunnypilot.custom_button import CustomButtonAction, handle_custom_button from openpilot.selfdrive.ui.layouts.sidebar import Sidebar, SIDEBAR_WIDTH from openpilot.selfdrive.ui.layouts.home import HomeLayout from openpilot.selfdrive.ui.layouts.settings.settings import SettingsLayout, PanelType @@ -40,6 +41,10 @@ class MainLayout(Widget): MainState.SETTINGS: SettingsLayout(), MainState.ONROAD: AugmentedRoadView(), } + self._custom_button_callbacks = { + CustomButtonAction.BOOKMARK: self._on_bookmark_clicked, + CustomButtonAction.CYCLE_UI: self._cycle_ui, + } self._sidebar_rect = rl.Rectangle(0, 0, 0, 0) self._content_rect = rl.Rectangle(0, 0, 0, 0) @@ -55,6 +60,7 @@ class MainLayout(Widget): gui_app.push_widget(self._onboarding_window) def _render(self, _): + handle_custom_button(ui_state.sm, ui_state.params, self._custom_button_callbacks) self._handle_onroad_transition() self._render_main_content() @@ -114,6 +120,18 @@ class MainLayout(Widget): def _on_settings_clicked(self): self.open_settings(PanelType.DEVICE) + def _show_onroad(self): + self._set_current_layout(MainState.ONROAD) + self._sidebar.set_visible(False) + + def _cycle_ui(self): + if self._current_mode == MainState.ONROAD and not self._sidebar.is_visible: + self._sidebar.set_visible(True) + elif self._current_mode == MainState.SETTINGS: + self._show_onroad() + else: + self._on_settings_clicked() + def _on_bookmark_clicked(self): for service in ('bookmarkButton', 'userBookmark'): msg = messaging.new_message(service, valid=True) diff --git a/openpilot/selfdrive/ui/mici/layouts/main.py b/openpilot/selfdrive/ui/mici/layouts/main.py index 7b96366894..9f1168f103 100644 --- a/openpilot/selfdrive/ui/mici/layouts/main.py +++ b/openpilot/selfdrive/ui/mici/layouts/main.py @@ -4,6 +4,7 @@ from openpilot.selfdrive.ui.mici.layouts.home import MiciHomeLayout from openpilot.selfdrive.ui.mici.layouts.settings.settings import SettingsLayout from openpilot.selfdrive.ui.mici.layouts.offroad_alerts import MiciOffroadAlerts from openpilot.selfdrive.ui.mici.onroad.augmented_road_view import AugmentedRoadView +from openpilot.selfdrive.ui.sunnypilot.custom_button import CustomButtonAction, handle_custom_button from openpilot.selfdrive.ui.ui_state import device, ui_state from openpilot.selfdrive.ui.mici.layouts.onboarding import OnboardingWindow from openpilot.selfdrive.ui.body.layouts.onroad import BodyLayout @@ -35,6 +36,10 @@ class MiciMainLayout(Scroller): self._settings_layout = SettingsLayout() self._car_onroad_layout = AugmentedRoadView(bookmark_callback=self._on_bookmark_clicked) self._body_onroad_layout = BodyLayout() + self._custom_button_callbacks = { + CustomButtonAction.BOOKMARK: self._on_bookmark_clicked, + CustomButtonAction.CYCLE_UI: self._cycle_ui, + } # Initialize widget rects for widget in (self._home_layout, self._alerts_layout, self._settings_layout, @@ -95,6 +100,8 @@ class MiciMainLayout(Scroller): self._alerts_layout._update_state() def _render(self, _): + handle_custom_button(ui_state.sm, ui_state.params, self._custom_button_callbacks) + if not self._setup: if self._alerts_layout.active_alerts() > 0: self._scroller.scroll_to(self._alerts_layout.rect.x) @@ -150,6 +157,23 @@ class MiciMainLayout(Scroller): msg = messaging.new_message(service, valid=True) self._pm.send(service, msg) + def _show_layout(self, layout: Widget): + if gui_app.widget_in_stack(self._onboarding_window): + return + gui_app.pop_widgets_to(self, lambda: self._scroll_to(layout)) + + def _layout_visible(self, layout: Widget) -> bool: + return abs(layout.rect.x - self._rect.x) < self._rect.width / 2 + + def _cycle_ui(self): + if gui_app.widget_in_stack(self._settings_layout): + self._show_layout(self._onroad_layout) + elif gui_app.get_active_widget() is self and self._layout_visible(self._home_layout): + if not gui_app.widget_in_stack(self._onboarding_window): + gui_app.push_widget(self._settings_layout) + else: + self._show_layout(self._home_layout) + def _on_body_changed(self): self._car_onroad_layout.set_visible(not ui_state.is_body) self._body_onroad_layout.set_visible(bool(ui_state.is_body)) diff --git a/openpilot/selfdrive/ui/sunnypilot/custom_button.py b/openpilot/selfdrive/ui/sunnypilot/custom_button.py new file mode 100644 index 0000000000..9f8374cb75 --- /dev/null +++ b/openpilot/selfdrive/ui/sunnypilot/custom_button.py @@ -0,0 +1,23 @@ +from enum import IntEnum + +from opendbc.car.structs import car + + +class CustomButtonAction(IntEnum): + NONE = 0 + BOOKMARK = 1 + CYCLE_UI = 3 + + +def handle_custom_button(sm, params, callbacks): + if not sm.updated['carState']: + return + + custom_pressed = any(be.type == car.CarState.ButtonEvent.Type.altButton2 and be.pressed + for be in sm['carState'].buttonEvents) + if not custom_pressed: + return + + action = CustomButtonAction(params.get('CustomButtonAction', return_default=True)) + if callback := callbacks.get(action): + callback() diff --git a/openpilot/selfdrive/ui/sunnypilot/onroad/developer_ui/elements.py b/openpilot/selfdrive/ui/sunnypilot/onroad/developer_ui/elements.py index f89edef48b..a8ecb5f8ab 100644 --- a/openpilot/selfdrive/ui/sunnypilot/onroad/developer_ui/elements.py +++ b/openpilot/selfdrive/ui/sunnypilot/onroad/developer_ui/elements.py @@ -252,7 +252,7 @@ class FrictionCoefficientElement: ltp = sm['lateralTorqueParameters'] value = f"{ltp.frictionCoefficientFiltered:.3f}" - color = rl.Color(0, 255, 0, 255) if ltp.liveValid else rl.WHITE + color = rl.Color(0, 255, 0, 255) if ltp.valid else rl.WHITE return UiElement(value, "FRIC.", self.unit, color) @@ -266,7 +266,7 @@ class LatAccelFactorElement: ltp = sm['lateralTorqueParameters'] value = f"{ltp.latAccelFactorFiltered:.3f}" - color = rl.Color(0, 255, 0, 255) if ltp.liveValid else rl.WHITE + color = rl.Color(0, 255, 0, 255) if ltp.valid else rl.WHITE return UiElement(value, "L.A.F.", self.unit, color) diff --git a/openpilot/selfdrive/ui/tests/test_custom_button.py b/openpilot/selfdrive/ui/tests/test_custom_button.py new file mode 100644 index 0000000000..9398d18039 --- /dev/null +++ b/openpilot/selfdrive/ui/tests/test_custom_button.py @@ -0,0 +1,31 @@ +from types import SimpleNamespace +from unittest.mock import Mock + +from opendbc.car.structs import car + +from openpilot.selfdrive.ui.sunnypilot.custom_button import CustomButtonAction, handle_custom_button + + +class FakeSubMaster: + def __init__(self, messages): + self.messages = messages + self.updated = {'carState': True} + + def __getitem__(self, key): + return self.messages[key] + + +def test_custom_button_actions(): + params = Mock() + sm = FakeSubMaster({ + 'carState': SimpleNamespace(buttonEvents=[SimpleNamespace( + type=car.CarState.ButtonEvent.Type.altButton2, + pressed=True, + )]), + }) + callbacks = {action: Mock() for action in CustomButtonAction if action != CustomButtonAction.NONE} + + for action, callback in callbacks.items(): + params.get.return_value = action + handle_custom_button(sm, params, callbacks) + callback.assert_called_once() diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dec_planner_gate.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dec_planner_gate.py new file mode 100644 index 0000000000..1f5c577028 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dec_planner_gate.py @@ -0,0 +1,112 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +from typing import cast + +from openpilot.cereal import custom, messaging +from opendbc.car import structs +from openpilot.common.test import OpenpilotTestCase +from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, LongitudinalPlanSource +from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController + +V_EGO = 20.0 +E2E_ACCEL = -3.0 # low enough that e2e wins the min() whenever it is a candidate + + +class MockDec: + def __init__(self, active: bool, mode: str): + self._active = active + self._mode = mode + + def update(self, sm): + pass + + def active(self) -> bool: + return self._active + + def mode(self) -> str: + return self._mode + + def enabled(self) -> bool: + return True + + +class MockSubMaster(dict): + def __init__(self, services: dict): + super().__init__(services) + self.valid = dict.fromkeys(services, True) + self.logMonoTime = dict.fromkeys(services, 0) + self.updated = dict.fromkeys(services, True) + self.recv_frame = dict.fromkeys(services, 1) + + def all_checks(self, service_list=None) -> bool: + return True + + +def build_sm(experimental_mode: bool) -> MockSubMaster: + services = {} + for service in ("radarState", "controlsState", "vehicleParameters", "carStateSP", + "liveMapDataSP", "gpsLocationExternal", "gpsLocation"): + services[service] = getattr(messaging.new_message(service), service) + + car_state = messaging.new_message('carState') + car_state.carState.vEgo = V_EGO + car_state.carState.vCruise = 100.0 + car_state.carState.vCruiseCluster = 100.0 + services['carState'] = car_state.carState.as_reader() + + selfdrive_state = messaging.new_message('selfdriveState') + selfdrive_state.selfdriveState.experimentalMode = experimental_mode + selfdrive_state.selfdriveState.enabled = True + services['selfdriveState'] = selfdrive_state.selfdriveState.as_reader() + + car_control = messaging.new_message('carControl') + car_control.carControl.enabled = True + services['carControl'] = car_control.carControl.as_reader() + + model = messaging.new_message('modelV2') + model.modelV2.orientationRate.z = [0.01] * 33 # nonzero: a straight path divides by zero in SCC vision + model.modelV2.velocity.x = [V_EGO] * 33 + model.modelV2.position.x = [float(i) for i in range(33)] + model.modelV2.action.desiredAcceleration = E2E_ACCEL + services['modelV2'] = model.modelV2.as_reader() + + return MockSubMaster(services) + + +def build_planner(dec_active: bool, dec_mode: str) -> LongitudinalPlanner: + CP = structs.CarParams() + CP.steerRatio = 15.0 + CP.wheelbase = 2.7 + CP.longitudinalActuatorDelay = 0.2 + CP_SP = custom.CarParamsSP.new_message().as_reader() + + planner = LongitudinalPlanner(CP, CP_SP, init_v=V_EGO) + planner.dec = cast(DynamicExperimentalController, MockDec(dec_active, dec_mode)) + return planner + + +class TestDecPlannerGate(OpenpilotTestCase): + """The e2e candidate must be gated on is_e2e(), not raw experimentalMode.""" + + def _source(self, experimental_mode: bool, dec_active: bool, dec_mode: str) -> LongitudinalPlanSource: + planner = build_planner(dec_active, dec_mode) + planner.update(build_sm(experimental_mode)) + return planner.mpc.source + + def test_no_e2e_when_experimental_mode_off(self): + assert self._source(False, False, 'acc') != LongitudinalPlanSource.e2e + + def test_e2e_when_dec_inactive(self): + # DEC off: behavior must match upstream + assert self._source(True, False, 'acc') == LongitudinalPlanSource.e2e + + def test_e2e_when_dec_blended(self): + assert self._source(True, True, 'blended') == LongitudinalPlanSource.e2e + + def test_no_e2e_when_dec_holds_acc(self): + # the regression + assert self._source(True, True, 'acc') != LongitudinalPlanSource.e2e diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index 63bf342fcb..c2d8a9b695 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -2172,6 +2172,26 @@ "title": "Hyundai / Kia / Genesis Settings", "description": "", "items": [ + { + "key": "CustomButtonAction", + "widget": "multiple_button", + "title": "Steering Custom Button", + "description": "Choose the openpilot action for the steering wheel custom/star button. OEM functionality is unchanged.", + "options": [ + { + "value": 0, + "label": "None" + }, + { + "value": 1, + "label": "Bookmark" + }, + { + "value": 3, + "label": "Cycle UI" + } + ] + }, { "key": "HyundaiLongitudinalTuning", "widget": "multiple_button", diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml index 7dd23256b1..31eaed5675 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml @@ -10,6 +10,17 @@ sections: title: Hyundai / Kia / Genesis Settings description: '' items: + - key: CustomButtonAction + widget: multiple_button + title: Steering Custom Button + description: Choose the openpilot action for the steering wheel custom/star button. OEM functionality is unchanged. + options: + - value: 0 + label: None + - value: 1 + label: Bookmark + - value: 3 + label: Cycle UI - key: HyundaiLongitudinalTuning widget: multiple_button title: Custom Longitudinal Tuning