Squilliam Fancyson

common/params_keys.h

@@ -62,6 +62,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"GsmRoaming", {PERSISTENT, BOOL}},
     {"HardwareSerial", {PERSISTENT, STRING}},
     {"HasAcceptedTerms", {PERSISTENT, STRING, "0"}},
+    {"HyundaiLkasAolPending", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL, "0", "0"}},
     {"HondaGasFactorParams", {PERSISTENT, FLOAT}},
     {"HondaLateralPidKiScale", {PERSISTENT, FLOAT, "1.0", "1.0", 3}},
     {"HondaLateralPidKpScale", {PERSISTENT, FLOAT, "1.0", "1.0", 3}},

opendbc_repo/opendbc/car/hyundai/values.py

@@ -111,6 +111,7 @@ class HyundaiSafetyFlags(IntFlag):
 
 
 class HyundaiStarPilotSafetyFlags(IntFlag):
+  AOL_LKAS_ON_INIT = 128
   AOL_MAIN_LKAS_SYNC = 32
   HAS_LDA_BUTTON = 1024
   AOL_LKAS_ON_ENGAGE = 2048

opendbc_repo/opendbc/safety/modes/hyundai_common.h

@@ -77,6 +77,7 @@ void hyundai_common_init(uint16_t param) {
   const uint16_t HYUNDAI_PARAM_FCEV_GAS = 256;
   const uint16_t HYUNDAI_PARAM_ALT_LIMITS_2 = 512;
 
+  const uint16_t HYUNDAI_PARAM_AOL_LKAS_ON_INIT = 128;
   const int HYUNDAI_PARAM_HAS_LDA_BUTTON = 1024;
   const uint16_t HYUNDAI_PARAM_AOL_LKAS_ON_ENGAGE = 2048;
   const uint16_t HYUNDAI_PARAM_NON_SCC = 4096;
@@ -100,6 +101,10 @@ void hyundai_common_init(uint16_t param) {
   hyundai_can_refresh_msgs = GET_FLAG(param, HYUNDAI_PARAM_CAN_REFRESH_MSGS);
   hyundai_aol_main_lkas_sync = false;
 
+  if (GET_FLAG(param, HYUNDAI_PARAM_AOL_LKAS_ON_INIT)) {
+    lkas_on = true;
+  }
+
   hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES;
   acc_main_on_prev = false;
   acc_main_on_tx = false;

opendbc_repo/opendbc/safety/tests/test_hyundai.py

@@ -572,6 +572,21 @@ class TestHyundaiAolLkasOnEngageStockSafety(HyundaiAolLkasOnEngageStockBase, Tes
     self.safety.init_tests()
 
 
+class TestHyundaiAolLkasOnInitSafety(TestHyundaiSafety):
+  def setUp(self):
+    self.packer = CANPackerSafety("hyundai_kia_generic")
+    self.safety = libsafety_py.libsafety
+    self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_INIT)
+    self.safety.init_tests()
+
+  def test_pending_lkas_press_allows_aol_after_safety_init(self):
+    self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL)
+    self._rx(self._button_msg(Buttons.NONE))
+
+    self.assertTrue(self.safety.get_lkas_on())
+    self.assertTrue(self.safety.get_aol_allowed())
+
+
 class TestHyundaiAolMainLkasSyncSafety(TestHyundaiSafety):
   def setUp(self):
     self.packer = CANPackerSafety("hyundai_kia_generic")

selfdrive/car/card.py

@@ -39,6 +39,11 @@ from openpilot.starpilot.controls.starpilot_card import StarPilotCard
 REPLAY = "REPLAY" in os.environ
 OPENPILOT_LEAD_MIN_DISTANCE = 0.1
 REDNECK_DECREASE_LOOKAHEAD_POINTS = 10
+HYUNDAI_SAFETY_MODELS = {
+  int(structs.CarParams.SafetyModel.hyundai),
+  int(structs.CarParams.SafetyModel.hyundaiCanfd),
+  int(structs.CarParams.SafetyModel.hyundaiLegacy),
+}
 
 EventName = log.OnroadEvent.EventName
 
@@ -200,6 +205,7 @@ class Car:
     self.rk = Ratekeeper(100, print_delay_threshold=None)
 
     self.resume_prev_button = False
+    self.hyundai_lkas_aol_pending = False
 
     self.starpilot_toggles = get_starpilot_toggles(read_persisted_force_params=True)
 
@@ -320,9 +326,31 @@ class Car:
       self.resume_prev_button = False
 
     FPCS = self.starpilot_card.update(CS, FPCS, self.sm, self.starpilot_toggles)
+    self._latch_pre_safety_hyundai_lkas_button(CS)
 
     return CS, RD, FPCS
 
+  def _hyundai_safety_active(self) -> bool:
+    if not self.sm.seen['pandaStates'] or not self.sm.valid['pandaStates']:
+      return False
+
+    panda_states = self.sm['pandaStates'].pandaStates
+    return any(
+      int(getattr(state.safetyModel, "raw", state.safetyModel)) in HYUNDAI_SAFETY_MODELS
+      for state in panda_states
+    )
+
+  def _latch_pre_safety_hyundai_lkas_button(self, CS: car.CarState) -> None:
+    if self.CP.brand != "hyundai" or not getattr(self.starpilot_toggles, "always_on_lateral_lkas", False):
+      return
+    if self.hyundai_lkas_aol_pending or self._hyundai_safety_active() or not any(
+      button.type == ButtonType.lkas and button.pressed for button in CS.buttonEvents
+    ):
+      return
+
+    self.hyundai_lkas_aol_pending = True
+    self.params.put_bool_nonblocking("HyundaiLkasAolPending", True)
+
   def state_publish(self, CS: car.CarState, RD: structs.RadarDataT | None, FPCS: custom.StarPilotCarState):
     """carState and carParams publish loop"""
 

selfdrive/controls/lib/latcontrol_angle.py

@@ -1,17 +1,34 @@
 import math
 
 from cereal import log
+from opendbc.car.subaru.values import CAR as SUBARU_CAR
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl
 
 # TODO This is speed dependent
 STEER_ANGLE_SATURATION_THRESHOLD = 2.5  # Degrees
 
+_ASCENT_ANGLE_TRACKING_GAIN = 0.25
+_ASCENT_ANGLE_TRACKING_MAX_CORRECTION = 8.0
+_ASCENT_ANGLE_TRACKING_MIN_SPEED = 5.0
+
+
+def _ascent_angle_tracking_target(target_angle: float, steering_angle: float,
+                                  v_ego: float, steering_pressed: bool) -> float:
+  if steering_pressed or v_ego < _ASCENT_ANGLE_TRACKING_MIN_SPEED:
+    return target_angle
+
+  correction = (target_angle - steering_angle) * _ASCENT_ANGLE_TRACKING_GAIN
+  correction = max(-_ASCENT_ANGLE_TRACKING_MAX_CORRECTION,
+                   min(_ASCENT_ANGLE_TRACKING_MAX_CORRECTION, correction))
+  return target_angle + correction
+
 
 class LatControlAngle(LatControl):
   def __init__(self, CP, CI, dt):
     super().__init__(CP, CI, dt)
     self.sat_check_min_speed = 5.
     self.use_steer_limited_by_safety = CP.brand in ("tesla", "hyundai")
+    self.is_ascent = CP.carFingerprint == SUBARU_CAR.SUBARU_ASCENT_2023
 
   def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay, calibrated_pose, model_data, starpilot_toggles):
     angle_log = log.ControlsState.LateralAngleState.new_message()
@@ -24,6 +41,14 @@ class LatControlAngle(LatControl):
       angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
       angle_steers_des += params.angleOffsetDeg
 
+      if self.is_ascent:
+        angle_steers_des = _ascent_angle_tracking_target(
+          angle_steers_des,
+          CS.steeringAngleDeg,
+          CS.vEgo,
+          bool(getattr(CS, "steeringPressed", False)),
+        )
+
     if self.use_steer_limited_by_safety:
       # these cars' carcontrollers calculate max lateral accel and jerk, so we can rely on carOutput for saturation
       angle_control_saturated = steer_limited_by_safety

selfdrive/controls/tests/test_latcontrol.py

@@ -17,7 +17,7 @@ from opendbc.car.hyundai.values import CAR as HYUNDAI
 from opendbc.car.subaru.values import CAR as SUBARU
 from opendbc.car.vehicle_model import VehicleModel
 from openpilot.common.realtime import DT_CTRL
-from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle
+from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, _ascent_angle_tracking_target
 from openpilot.selfdrive.controls.lib.latcontrol_pid import (
   LatControlPID,
   get_civic_bosch_modified_pid_output_alpha,
@@ -169,6 +169,12 @@ class TestLatControl:
   def test_center_chatter_friction_jerk_deadzone_preserves_vehicle_override(self):
     assert get_center_chatter_friction_jerk_deadzone(25.0, 0.6, 0.30) == pytest.approx(0.30)
 
+  def test_ascent_angle_tracking_correction_is_bounded_and_handoff_safe(self):
+    assert _ascent_angle_tracking_target(10.0, 0.0, 20.0, False) == pytest.approx(12.5)
+    assert _ascent_angle_tracking_target(40.0, 0.0, 20.0, False) == pytest.approx(48.0)
+    assert _ascent_angle_tracking_target(10.0, 0.0, 4.0, False) == pytest.approx(10.0)
+    assert _ascent_angle_tracking_target(10.0, 0.0, 20.0, True) == pytest.approx(10.0)
+
   def test_torque_log_exposes_friction_controller_state(self):
     controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(GM.CHEVROLET_BOLT_ACC_2022_2023)
 

selfdrive/pandad/panda_safety.cc

@@ -2,6 +2,8 @@
 #include "cereal/messaging/messaging.h"
 #include "common/swaglog.h"
 
+static constexpr uint16_t HYUNDAI_AOL_LKAS_ON_INIT = 128U;
+
 void PandaSafety::configureSafetyMode(bool is_onroad) {
   if (is_onroad && !safety_configured_) {
     updateMultiplexingMode();
@@ -73,6 +75,10 @@ void PandaSafety::setSafetyMode(const std::string &params_string) {
 
   auto starpilot_safety_configs = starpilot_car_params.getSafetyConfigs();
   alternative_experience |= starpilot_car_params.getAlternativeExperience();
+  const bool hyundai_lkas_aol_pending = params_.getBool("HyundaiLkasAolPending");
+  if (hyundai_lkas_aol_pending) {
+    params_.putBool("HyundaiLkasAolPending", false);
+  }
 
   for (int i = 0; i < pandas_.size(); ++i) {
     // Default to SILENT safety model if not specified
@@ -87,6 +93,13 @@ void PandaSafety::setSafetyMode(const std::string &params_string) {
       safety_param |= starpilot_safety_configs[i].getSafetyParam();
     }
 
+    const bool hyundai_safety = safety_model == cereal::CarParams::SafetyModel::HYUNDAI ||
+                                safety_model == cereal::CarParams::SafetyModel::HYUNDAI_CANFD ||
+                                safety_model == cereal::CarParams::SafetyModel::HYUNDAI_LEGACY;
+    if (hyundai_lkas_aol_pending && hyundai_safety) {
+      safety_param |= HYUNDAI_AOL_LKAS_ON_INIT;
+    }
+
     LOGW("Panda %d: setting safety model: %d, param: %d, alternative experience: %d", i, (int)safety_model, safety_param, alternative_experience);
     pandas_[i]->set_alternative_experience(alternative_experience);
     pandas_[i]->set_safety_model(safety_model, safety_param);