Merge branch 'hyundai-custom-button' into ccnc-port-custom-button

openpilot/common/params_keys.h

@@ -186,6 +186,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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"}},

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])

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)

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))

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()

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)
 
 

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()

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

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",

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