From 4d9df8b14798669a1c272915c8f330f3811ffb82 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 31 Aug 2026 12:31:15 -0500 Subject: [PATCH] foghorn leghorn --- common/params_keys.h | 3 + .../tests/test_lateral_only_experimental.py | 44 +++++++++++++ .../opendbc/car/hyundai/hyundaican.py | 4 +- .../opendbc/car/hyundai/tests/test_hyundai.py | 17 +++++ selfdrive/car/card.py | 13 +++- selfdrive/controls/controlsd.py | 26 +++++++- selfdrive/controls/lib/drive_helpers.py | 10 +-- .../controls/lib/latcontrol_vehicle_tunes.py | 4 +- .../controls/lib/longitudinal_planner.py | 14 +++- .../lib/longitudinal_vehicle_tunes.py | 21 ++++++ .../controls/tests/test_drive_helpers.py | 11 ++++ .../tests/test_longitudinal_planner.py | 17 +++++ .../tests/test_neural_network_feedforward.py | 17 +++++ selfdrive/selfdrived/selfdrived.py | 5 +- selfdrive/ui/layouts/settings/toggles.py | 17 ++++- selfdrive/ui/mici/layouts/home.py | 4 +- selfdrive/ui/mici/layouts/settings/toggles.py | 4 +- selfdrive/ui/onroad/exp_button.py | 2 +- selfdrive/ui/ui_state.py | 8 +++ starpilot/common/lateral_only_experimental.py | 32 +++++++++ starpilot/common/starpilot_variables.py | 56 ++++++++++++---- .../lib/neural_network_feedforward.py | 14 +++- starpilot/controls/starpilot_card.py | 65 ++++++++++++++++++- .../controls/tests/test_starpilot_card.py | 29 +++++++++ .../the_galaxy/tests/test_dashboard_stats.py | 4 ++ .../tests/test_navigation_params.py | 8 +++ starpilot/system/wheel_controls/__init__.py | 10 +++ .../tests/test_wheel_controlsd.py | 31 +++++++++ .../system/wheel_controls/wheel_controlsd.py | 44 +++++++++++++ 29 files changed, 497 insertions(+), 37 deletions(-) create mode 100644 common/tests/test_lateral_only_experimental.py create mode 100644 selfdrive/controls/tests/test_neural_network_feedforward.py create mode 100644 starpilot/common/lateral_only_experimental.py diff --git a/common/params_keys.h b/common/params_keys.h index 92e72ec83..7545f23b8 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -517,6 +517,9 @@ inline static std::unordered_map keys = { {"FavoriteVirtualDecelCruiseCounter", {CLEAR_ON_MANAGER_START, INT, "0", "0"}}, {"FavoriteTrafficModeCounter", {CLEAR_ON_MANAGER_START, INT, "0", "0"}}, {"WheelButtonBookmarkCounter", {CLEAR_ON_MANAGER_START, INT, "0", "0"}}, + {"WheelControlAOLCounter", {CLEAR_ON_MANAGER_START, INT, "0", "0"}}, + {"WheelControlForceCoastCounter", {CLEAR_ON_MANAGER_START, INT, "0", "0"}}, + {"WheelControlPulseGlideCounter", {CLEAR_ON_MANAGER_START, INT, "0", "0"}}, {"openpilotMinutes", {PERSISTENT, INT, "0", "0", 0}}, {"OverpassRequests", {PERSISTENT, JSON, "{}", "{}"}}, {"PathColor", {PERSISTENT, STRING, "", "", 2, SETTINGS_SIMPLE}}, diff --git a/common/tests/test_lateral_only_experimental.py b/common/tests/test_lateral_only_experimental.py new file mode 100644 index 000000000..6d8ffab2c --- /dev/null +++ b/common/tests/test_lateral_only_experimental.py @@ -0,0 +1,44 @@ +from types import SimpleNamespace + +from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR + +from openpilot.starpilot.common.lateral_only_experimental import ( + experimental_mode_available, + lateral_only_experimental_available, +) + + +def test_telluride_platform_allows_lateral_only_experimental_mode(): + CP = SimpleNamespace( + carFingerprint=HYUNDAI_CAR.HYUNDAI_PALISADE_2023, + openpilotLongitudinalControl=False, + ) + + assert lateral_only_experimental_available(CP) + assert experimental_mode_available(CP) + + +def test_lateral_only_mode_does_not_expand_other_stock_acc_cars(): + CP = SimpleNamespace( + carFingerprint=HYUNDAI_CAR.HYUNDAI_SONATA, + openpilotLongitudinalControl=False, + ) + + assert not lateral_only_experimental_available(CP) + assert not experimental_mode_available(CP) + + old_palisade = SimpleNamespace( + carFingerprint=HYUNDAI_CAR.HYUNDAI_PALISADE, + openpilotLongitudinalControl=False, + ) + assert not lateral_only_experimental_available(old_palisade) + + +def test_normal_experimental_mode_remains_available_with_openpilot_long(): + CP = SimpleNamespace( + carFingerprint=HYUNDAI_CAR.HYUNDAI_SONATA, + openpilotLongitudinalControl=True, + ) + + assert not lateral_only_experimental_available(CP) + assert experimental_mode_available(CP) diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaican.py b/opendbc_repo/opendbc/car/hyundai/hyundaican.py index 8afb033bc..c439711fa 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaican.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaican.py @@ -60,7 +60,7 @@ def create_lkas11(packer, frame, CP, apply_torque, steer_req, values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0 # Likely cars lacking the ability to show individual lane lines in the dash - elif CP.carFingerprint in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL): + elif CP.carFingerprint in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.HYUNDAI_KONA_NON_SCC): # SysWarning 4 = keep hands on wheel + beep values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0 @@ -68,7 +68,7 @@ def create_lkas11(packer, frame, CP, apply_torque, steer_req, # SysState 1-2 = white car + lanes # SysState 3 = green car + lanes, green steering wheel # SysState 4 = green car + lanes - values["CF_Lkas_LdwsSysState"] = 3 if enabled else 1 + values["CF_Lkas_LdwsSysState"] = lka_icon if CP.carFingerprint == CAR.HYUNDAI_KONA_NON_SCC else 3 if enabled else 1 values["CF_Lkas_LdwsOpt_USM"] = 2 # non-2 changes above SysState definition # these have no effect diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index e248faa3d..30b60141a 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -692,6 +692,23 @@ class TestHyundaiFingerprint: assert parser.vl["LKAS11"]["CF_Lkas_FcwOpt_USM"] == 2 + def test_kona_non_scc_uses_no_individual_lane_lkas_status(self): + CP = CarInterface.get_params(CAR.HYUNDAI_KONA_NON_SCC, gen_empty_fingerprint(), [], False, False, False, None) + packer = CANPacker(DBC[CP.carFingerprint][Bus.pt]) + parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS11", 0)], 0) + + lkas11 = parser.vl["LKAS11"] + msg = hyundaican.create_lkas11( + packer, 0, CP, 0, True, False, lkas11, False, 4, False, + True, True, 0, 0, 2, + ) + parser.update([(1, [msg])]) + + assert parser.vl["LKAS11"]["CF_Lkas_LdwsSysState"] == 2 + assert parser.vl["LKAS11"]["CF_Lkas_LdwsOpt_USM"] == 2 + assert parser.vl["LKAS11"]["CF_Lkas_LdwsActivemode"] == 0 + assert parser.vl["LKAS11"]["CF_Lkas_FcwOpt_USM"] == 0 + @pytest.mark.parametrize("candidate", (CAR.HYUNDAI_ELANTRA_2024, CAR.HYUNDAI_ELANTRA_HEV_2024)) def test_hyundai_can_refresh_platforms_use_refresh_dbc_and_safety_param(self, candidate): CP = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], False, False, False, None) diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 8f4d6841e..aa49f54df 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -34,6 +34,7 @@ from openpilot.starpilot.common.favorite_slots import ( FAVORITE_ACTION_DECEL_COUNTER, ) from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles, update_starpilot_toggles +from openpilot.starpilot.common.lateral_only_experimental import experimental_mode_available from openpilot.starpilot.controls.starpilot_card import StarPilotCard REPLAY = "REPLAY" in os.environ @@ -203,7 +204,11 @@ class Car: self.is_metric = self.params.get_bool("IsMetric") self.safe_mode = self.params.get_bool("SafeMode") - self.experimental_mode = self.params.get_bool("ExperimentalMode") and not self.safe_mode + self.experimental_mode = ( + self.params.get_bool("ExperimentalMode") and + experimental_mode_available(self.CP) and + not self.safe_mode + ) # card is driven by can recv, expected at 100Hz self.rk = Ratekeeper(100, print_delay_threshold=None) @@ -546,7 +551,11 @@ class Car: while not evt.is_set(): self.safe_mode = self.params.get_bool("SafeMode") self.is_metric = self.params.get_bool("IsMetric") - self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl and not self.safe_mode + self.experimental_mode = ( + self.params.get_bool("ExperimentalMode") and + experimental_mode_available(self.CP) and + not self.safe_mode + ) time.sleep(0.1) def card_thread(self): diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 094199728..ab17bf91d 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -403,6 +403,8 @@ class Controls: self.turn_blinker_swept = 0.0 self.twitch_guard_remaining = 0.0 self.kona_non_scc_lateral_active = False + self.kona_non_scc_lateral_faulted = False + self.elantra_hev_2024_lateral_faulted = False self.pose_calibrator = PoseCalibrator() self.calibrated_pose: Pose | None = None @@ -499,15 +501,37 @@ class Controls: # Check which actuators can be enabled standstill = abs(CS.vEgo) <= max(self.CP.minSteerSpeed, 0.3) or CS.standstill if self.CP.carFingerprint == HYUNDAI_CAR.HYUNDAI_KONA_NON_SCC: + always_on_lateral_enabled = self.sm['starpilotCarState'].alwaysOnLateralEnabled + lateral_requested = (CC.enabled and self.sm['selfdriveState'].active) or always_on_lateral_enabled + if not lateral_requested: + self.kona_non_scc_lateral_faulted = False + elif CS.steerFaultTemporary: + self.kona_non_scc_lateral_faulted = True CC.latActive = get_kona_non_scc_lateral_active( CC.enabled, self.sm['selfdriveState'].active, - self.sm['starpilotCarState'].alwaysOnLateralEnabled, + always_on_lateral_enabled, CS.steerFaultTemporary, CS.steerFaultPermanent, standstill, self.CP.steerAtStandstill, self.sm['starpilotPlan'].lateralCheck, CS.steeringPressed, self.kona_non_scc_lateral_active, + self.kona_non_scc_lateral_faulted, ) self.kona_non_scc_lateral_active = CC.latActive + elif self.CP.carFingerprint == HYUNDAI_CAR.HYUNDAI_ELANTRA_HEV_2024: + always_on_lateral_enabled = self.sm['starpilotCarState'].alwaysOnLateralEnabled + lateral_requested = (CC.enabled and self.sm['selfdriveState'].active) or always_on_lateral_enabled + if not lateral_requested: + self.elantra_hev_2024_lateral_faulted = False + elif CS.steerFaultTemporary: + self.elantra_hev_2024_lateral_faulted = True + CC.latActive = get_lateral_active( + CC.enabled, self.sm['selfdriveState'].active, + always_on_lateral_enabled, + CS.steerFaultTemporary, CS.steerFaultPermanent, + standstill, self.CP.steerAtStandstill, + self.sm['starpilotPlan'].lateralCheck, + self.elantra_hev_2024_lateral_faulted, + ) else: CC.latActive = get_lateral_active(CC.enabled, self.sm['selfdriveState'].active, self.sm['starpilotCarState'].alwaysOnLateralEnabled, diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index cfdd71992..c5b97d02c 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -71,21 +71,23 @@ get_accel_from_plan_tomb_raider = get_accel_from_plan def get_lateral_active(enabled: bool, active: bool, always_on_lateral_enabled: bool, steer_fault_temporary: bool, steer_fault_permanent: bool, - standstill: bool, steer_at_standstill: bool, lateral_check: bool) -> bool: + standstill: bool, steer_at_standstill: bool, lateral_check: bool, + steer_fault_latched: bool = False) -> bool: lateral_allowed = (enabled and active) or always_on_lateral_enabled return lateral_allowed and not steer_fault_temporary and not steer_fault_permanent and \ - (not standstill or steer_at_standstill) and lateral_check + not steer_fault_latched and (not standstill or steer_at_standstill) and lateral_check def get_kona_non_scc_lateral_active(enabled: bool, active: bool, always_on_lateral_enabled: bool, steer_fault_temporary: bool, steer_fault_permanent: bool, standstill: bool, steer_at_standstill: bool, lateral_check: bool, - steering_pressed: bool, previous_lateral_active: bool) -> bool: + steering_pressed: bool, previous_lateral_active: bool, + steer_fault_latched: bool = False) -> bool: """Avoid the Kona EPS torque fault when AOL is enabled over driver steering input.""" lateral_active = get_lateral_active(enabled, active, always_on_lateral_enabled, steer_fault_temporary, steer_fault_permanent, standstill, steer_at_standstill, lateral_check) - if not lateral_active: + if not lateral_active or steer_fault_latched: return False aol_rising_edge = always_on_lateral_enabled and not enabled and not previous_lateral_active diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 7dac4d212..f2cda3c79 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -288,8 +288,8 @@ GENESIS_G70_CURVE_UNWIND_LAT = 0.25 GENESIS_G70_CURVE_UNWIND_LAT_WIDTH = 0.12 GENESIS_G70_CURVE_UNWIND_JERK = 0.08 GENESIS_G70_CURVE_UNWIND_JERK_WIDTH = 0.08 -GENESIS_G70_UNWIND_FF_REDUCTION_MAX = 0.32 -GENESIS_G70_UNWIND_FF_OVERSHOOT = 0.12 +GENESIS_G70_UNWIND_FF_REDUCTION_MAX = 0.36 +GENESIS_G70_UNWIND_FF_OVERSHOOT = 0.10 GENESIS_G70_UNWIND_FF_OVERSHOOT_WIDTH = 0.12 GENESIS_G70_UNWIND_FF_JERK = 0.10 GENESIS_G70_UNWIND_FF_JERK_WIDTH = 0.10 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 76386dae2..f71e98423 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -45,6 +45,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( get_standstill_gap_settle_max_extra_gap, get_standstill_stopped_lead_guard_distance_margin, get_standstill_stopped_lead_guard_max_lead_speed, + is_ford_f150_lightning_stopped_radar_follow_lead, get_tracked_lead_catchup_bias_gain, get_tracked_lead_catchup_bias_cap, get_tracked_lead_catchup_speed_range, @@ -2066,9 +2067,20 @@ class LongitudinalPlanner: any(is_toyota_rav4_tss2_radar_follow_lead(self.CP, lead, scene_v_ego) for lead in (self.lead_one, self.lead_two)) ) + lightning_stopped_radar_follow = ( + experimental_mode and + not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and + not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and + not bool(getattr(sm['starpilotPlan'], 'stopSignConfirmed', False)) and + any(is_ford_f150_lightning_stopped_radar_follow_lead(self.CP, lead, scene_v_ego) + for lead in (self.lead_one, self.lead_two)) + ) # StarPilot trackingLead is debounce/model-length based. Keep a raw close-lead # safety path so ACC/chill does not ignore a visible lead during that debounce. - lead_control_active = tracking_lead or raw_close_lead_control or early_truck_follow or rav4_radar_follow + lead_control_active = ( + tracking_lead or raw_close_lead_control or early_truck_follow or rav4_radar_follow or + lightning_stopped_radar_follow + ) lead_one_active = bool(self.lead_one.status and lead_control_active) effective_t_follow = self.get_dynamic_t_follow(sm['starpilotPlan'].tFollow, self.lead_one if lead_one_active else None, v_ego) diff --git a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py index 1b5a8a628..c9280d06f 100644 --- a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py +++ b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py @@ -33,6 +33,10 @@ FORD_LIGHTNING_FOLLOW_PREBRAKE_MIN_HEADWAY = 0.75 FORD_LIGHTNING_TRACKED_LEAD_CATCHUP_MIN_HEADWAY_MARGIN = 0.10 FORD_LIGHTNING_TRACKED_LEAD_CATCHUP_FULL_HEADWAY_MARGIN = 0.25 FORD_LIGHTNING_TRACKED_LEAD_CATCHUP_BIAS_GAIN = 1.0 +FORD_LIGHTNING_STOPPED_RADAR_FOLLOW_MAX_EGO_SPEED = 4.5 +FORD_LIGHTNING_STOPPED_RADAR_FOLLOW_MAX_DISTANCE = 18.0 +FORD_LIGHTNING_STOPPED_RADAR_FOLLOW_MAX_LEAD_SPEED = 2.0 +FORD_LIGHTNING_STOPPED_RADAR_FOLLOW_MAX_LATERAL_OFFSET = 1.25 HONDA_CRV_5G_TRACKED_LEAD_CATCHUP_MIN_HEADWAY_MARGIN = 0.10 HONDA_CRV_5G_TRACKED_LEAD_CATCHUP_FULL_HEADWAY_MARGIN = 0.35 HONDA_CRV_5G_TRACKED_LEAD_CATCHUP_BIAS_GAIN = 1.25 @@ -341,6 +345,23 @@ def is_ford_f150_lightning(CP): ) +def is_ford_f150_lightning_stopped_radar_follow_lead(CP, lead, v_ego): + """Keep a credible stopped radar lead active after the CEM model horizon collapses.""" + if ( + not is_ford_f150_lightning(CP) or + lead is None or not bool(getattr(lead, "status", False)) or + not bool(getattr(lead, "radar", False)) or + float(v_ego) < 0.0 or + float(v_ego) > FORD_LIGHTNING_STOPPED_RADAR_FOLLOW_MAX_EGO_SPEED or + float(getattr(lead, "dRel", float("inf"))) > FORD_LIGHTNING_STOPPED_RADAR_FOLLOW_MAX_DISTANCE or + max(float(getattr(lead, "vLead", 0.0)), 0.0) > FORD_LIGHTNING_STOPPED_RADAR_FOLLOW_MAX_LEAD_SPEED or + abs(float(getattr(lead, "yRel", 0.0))) > FORD_LIGHTNING_STOPPED_RADAR_FOLLOW_MAX_LATERAL_OFFSET + ): + return False + + return float(getattr(lead, "dRel", 0.0)) > 0.0 + + def is_toyota_rav4_tss2_post_departure_tune(CP): """Identify RAV4 TSS2 variants that need normal catch-up caps after departure.""" return ( diff --git a/selfdrive/controls/tests/test_drive_helpers.py b/selfdrive/controls/tests/test_drive_helpers.py index d2ff69e48..02eaf9623 100644 --- a/selfdrive/controls/tests/test_drive_helpers.py +++ b/selfdrive/controls/tests/test_drive_helpers.py @@ -9,6 +9,11 @@ def test_get_lateral_active_allows_aol_while_disabled(): assert get_lateral_active(False, False, True, False, False, False, False, True) +def test_get_lateral_active_does_not_retry_after_a_latched_temporary_fault(): + assert not get_lateral_active(False, False, True, False, False, False, False, True, True) + assert get_lateral_active(False, False, True, False, False, False, False, True, False) + + def test_kona_non_scc_aol_waits_for_driver_steering_to_release(): assert not get_kona_non_scc_lateral_active( False, False, True, False, False, False, False, True, True, False, @@ -30,5 +35,11 @@ def test_kona_non_scc_aol_gate_does_not_change_fault_or_normal_lateral_gates(): ) +def test_kona_non_scc_does_not_retry_after_a_latched_temporary_fault(): + assert not get_kona_non_scc_lateral_active( + False, False, True, False, False, False, False, True, False, False, True, + ) + + def test_get_lateral_active_honors_manual_pause_while_cruise_is_engaged(): assert not get_lateral_active(True, True, False, False, False, False, False, False) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 9c09f0b2c..f0e90894e 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -40,6 +40,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( get_standstill_stopped_lead_guard_distance_margin, get_standstill_stopped_lead_guard_max_lead_speed, get_stop_sign_low_speed_hold, + is_ford_f150_lightning_stopped_radar_follow_lead, get_tracked_lead_catchup_bias_cap, get_tracked_lead_catchup_bias_gain, get_tracked_lead_catchup_cruise_error_full, @@ -807,6 +808,22 @@ def test_lightning_stopped_lead_guard_tune_is_vehicle_specific(): assert get_tracked_lead_catchup_bias_gain(civic) is None +def test_lightning_stopped_radar_lead_handoff_is_narrow_and_vehicle_specific(): + lightning = FordCarInterface.get_non_essential_params(FORD_CAR.FORD_F_150_LIGHTNING_MK1) + civic = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + lead = make_lead(status=True, d_rel=17.2, v_lead=0.2, radar=True, model_prob=1.0, y_rel=0.1) + + assert is_ford_f150_lightning_stopped_radar_follow_lead(lightning, lead, v_ego=2.6) + assert is_ford_f150_lightning_stopped_radar_follow_lead(lightning, lead, v_ego=0.0) + assert not is_ford_f150_lightning_stopped_radar_follow_lead(lightning, make_lead( + status=True, d_rel=18.1, v_lead=0.2, radar=True, model_prob=1.0, y_rel=0.1, + ), v_ego=2.6) + assert not is_ford_f150_lightning_stopped_radar_follow_lead(lightning, make_lead( + status=True, d_rel=17.2, v_lead=2.1, radar=True, model_prob=1.0, y_rel=0.1, + ), v_ego=2.6) + assert not is_ford_f150_lightning_stopped_radar_follow_lead(civic, lead, v_ego=2.6) + + def test_crv_tracked_lead_catchup_tune_is_vehicle_specific(): crv = CarInterface.get_non_essential_params(CAR.HONDA_CRV_5G) civic = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) diff --git a/selfdrive/controls/tests/test_neural_network_feedforward.py b/selfdrive/controls/tests/test_neural_network_feedforward.py new file mode 100644 index 000000000..0beacf654 --- /dev/null +++ b/selfdrive/controls/tests/test_neural_network_feedforward.py @@ -0,0 +1,17 @@ +from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR + +from openpilot.starpilot.controls.lib.neural_network_feedforward import ( + DEFAULT_NNFF_LAT_JERK_FRICTION_FACTOR, + PALISADE_NNFF_LAT_JERK_FRICTION_FACTOR, + get_nnff_lat_jerk_friction_factor, +) + + +def test_palisade_nnff_jerk_friction_factor_is_damped_for_bumps(): + assert get_nnff_lat_jerk_friction_factor(HYUNDAI_CAR.HYUNDAI_PALISADE_2023) == PALISADE_NNFF_LAT_JERK_FRICTION_FACTOR + assert PALISADE_NNFF_LAT_JERK_FRICTION_FACTOR < DEFAULT_NNFF_LAT_JERK_FRICTION_FACTOR + + +def test_other_nnff_cars_keep_default_jerk_friction_factor(): + assert get_nnff_lat_jerk_friction_factor(HYUNDAI_CAR.HYUNDAI_SONATA) == DEFAULT_NNFF_LAT_JERK_FRICTION_FACTOR + assert get_nnff_lat_jerk_friction_factor(HYUNDAI_CAR.HYUNDAI_PALISADE) == DEFAULT_NNFF_LAT_JERK_FRICTION_FACTOR diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index a2a1e8834..df6b96c1c 100644 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -34,6 +34,7 @@ from openpilot.system.hardware import HARDWARE from openpilot.starpilot.common.starpilot_utilities import contains_event_type from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles +from openpilot.starpilot.common.lateral_only_experimental import experimental_mode_available from openpilot.starpilot.common.vision_bsm import get_fresh_vasm_state REPLAY = "REPLAY" in os.environ @@ -217,7 +218,7 @@ class SelfdriveD: # cleanup old params if not self.CP.alphaLongitudinalAvailable: self.params.remove("AlphaLongitudinalEnabled") - if not self.CP.openpilotLongitudinalControl: + if not experimental_mode_available(self.CP): self.params.remove("ExperimentalMode") self.CS_prev = car.CarState.new_message() @@ -965,7 +966,7 @@ class SelfdriveD: if self.safe_mode: self.experimental_mode = False elif not self.starpilot_toggles.conditional_experimental_mode: - self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl + self.experimental_mode = self.params.get_bool("ExperimentalMode") and experimental_mode_available(self.CP) self.personality = log.LongitudinalPersonality.relaxed if self.safe_mode else self.params.get("LongitudinalPersonality", return_default=True) time.sleep(0.1) diff --git a/selfdrive/ui/layouts/settings/toggles.py b/selfdrive/ui/layouts/settings/toggles.py index 5380960a8..61fd12a3e 100644 --- a/selfdrive/ui/layouts/settings/toggles.py +++ b/selfdrive/ui/layouts/settings/toggles.py @@ -189,10 +189,21 @@ class TogglesLayout(Widget): ) if ui_state.CP is not None: - if ui_state.has_longitudinal_control: + if ui_state.experimental_mode_available: self._toggles["ExperimentalMode"].action_item.set_enabled(not safe_mode) - self._toggles["ExperimentalMode"].set_description(e2e_description) - self._long_personality_setting.action_item.set_enabled(not safe_mode) + if ui_state.has_longitudinal_control: + self._toggles["ExperimentalMode"].set_description(e2e_description) + else: + lateral_only_description = tr( + "Lateral-only experimental mode is enabled for this stock-ACC vehicle for testing. " + "Openpilot will not control gas or brakes; the stock ACC remains responsible for speed." + ) + self._toggles["ExperimentalMode"].set_description( + "" + lateral_only_description + "

" + tr( + "The experimental visualization and driving-model features remain available." + ) + ) + self._long_personality_setting.action_item.set_enabled(not safe_mode and ui_state.has_longitudinal_control) else: # no long for now self._toggles["ExperimentalMode"].action_item.set_enabled(False) diff --git a/selfdrive/ui/mici/layouts/home.py b/selfdrive/ui/mici/layouts/home.py index 21ec72d98..a38f19125 100644 --- a/selfdrive/ui/mici/layouts/home.py +++ b/selfdrive/ui/mici/layouts/home.py @@ -217,8 +217,8 @@ class MiciHomeLayout(Widget): if self._mouse_down_t is not None: if time.monotonic() - self._mouse_down_t > 0.5: - # long gating for experimental mode - only allow toggle if longitudinal control is available - if ui_state.has_longitudinal_control: + # Only allow the toggle when this vehicle exposes Experimental Mode. + if ui_state.experimental_mode_available: self._experimental_mode = not self._experimental_mode ui_state.params.put("ExperimentalMode", self._experimental_mode) self._mode_status_atom.refresh() diff --git a/selfdrive/ui/mici/layouts/settings/toggles.py b/selfdrive/ui/mici/layouts/settings/toggles.py index 77134c7c9..c311949de 100644 --- a/selfdrive/ui/mici/layouts/settings/toggles.py +++ b/selfdrive/ui/mici/layouts/settings/toggles.py @@ -94,9 +94,9 @@ class TogglesLayoutMici(NavScroller): # CP gating for experimental mode if ui_state.CP is not None: - if ui_state.has_longitudinal_control: + if ui_state.experimental_mode_available: self._experimental_btn.set_visible(True) - self._personality_toggle.set_visible(True) + self._personality_toggle.set_visible(ui_state.has_longitudinal_control) else: # no long for now self._experimental_btn.set_visible(False) diff --git a/selfdrive/ui/onroad/exp_button.py b/selfdrive/ui/onroad/exp_button.py index 6e2278cda..f80609787 100644 --- a/selfdrive/ui/onroad/exp_button.py +++ b/selfdrive/ui/onroad/exp_button.py @@ -142,4 +142,4 @@ class ExpButton(Widget): return False # Mirror exp mode toggle using persistent car params - return ui_state.has_longitudinal_control + return ui_state.experimental_mode_available diff --git a/selfdrive/ui/ui_state.py b/selfdrive/ui/ui_state.py index 986de5efe..268e98c00 100644 --- a/selfdrive/ui/ui_state.py +++ b/selfdrive/ui/ui_state.py @@ -12,6 +12,7 @@ from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.ui.lib.prime_state import PrimeState from openpilot.selfdrive.ui.lib.ui_param_cache import shared_ui_params from openpilot.system.ui.lib.application import gui_app +from openpilot.starpilot.common.lateral_only_experimental import lateral_only_experimental_available from openpilot.system.hardware import HARDWARE, PC BACKLIGHT_OFFROAD = 65 if HARDWARE.get_device_type() == "mici" else 50 @@ -97,6 +98,7 @@ class UIState: self.panda_type: log.PandaState.PandaType = log.PandaState.PandaType.unknown self.personality: log.LongitudinalPersonality = log.LongitudinalPersonality.standard self.has_longitudinal_control: bool = False + self.experimental_mode_available: bool = False self.CP: car.CarParams | None = None self.light_sensor: float = -1.0 self._param_update_time: float = 0.0 @@ -285,6 +287,12 @@ class UIState: self.has_longitudinal_control = self.params.get_bool("AlphaLongitudinalEnabled") else: self.has_longitudinal_control = self.CP.openpilotLongitudinalControl + self.experimental_mode_available = ( + self.has_longitudinal_control or + lateral_only_experimental_available(self.CP) + ) + else: + self.experimental_mode_available = False self._param_update_time = time.monotonic() diff --git a/starpilot/common/lateral_only_experimental.py b/starpilot/common/lateral_only_experimental.py new file mode 100644 index 000000000..7de1338ba --- /dev/null +++ b/starpilot/common/lateral_only_experimental.py @@ -0,0 +1,32 @@ +"""Vehicle-scoped availability for experimental lateral-only testing. + +Experimental mode normally implies that openpilot may run its longitudinal +planner. A few stock-ACC platforms can safely use the mode for model/lateral +testing as long as the longitudinal actuator gate remains closed. Keep this +allow-list narrow and explicit so stock-ACC vehicles do not gain an +experimental control path accidentally. +""" + +from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR + + +# The 2023+ Hyundai Palisade platform also identifies Kia Telluride routes. +LATERAL_ONLY_EXPERIMENTAL_CARS = frozenset({ + HYUNDAI_CAR.HYUNDAI_PALISADE_2023, +}) + + +def lateral_only_experimental_available(CP) -> bool: + """Return whether this car may expose Experimental Mode without openpilot long.""" + return ( + not bool(getattr(CP, "openpilotLongitudinalControl", False)) and + getattr(CP, "carFingerprint", None) in LATERAL_ONLY_EXPERIMENTAL_CARS + ) + + +def experimental_mode_available(CP) -> bool: + """Return whether Experimental Mode is valid for the current car.""" + return ( + bool(getattr(CP, "openpilotLongitudinalControl", False)) or + lateral_only_experimental_available(CP) + ) diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index f28dab342..ad06bc942 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -30,6 +30,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import KP from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.starpilot.common.model_versions import is_tinygrad_model_version from openpilot.starpilot.common.lateral_delay import full_lateral_delay +from openpilot.starpilot.common.lateral_only_experimental import lateral_only_experimental_available from openpilot.starpilot.common.accel_profile import ( ACCELERATION_PROFILES, CUSTOM_ACCEL_PROFILE_PARAM_KEYS, @@ -655,6 +656,9 @@ class StarPilotVariables: hyundai_can_use_lkas_for_aol = toggle.car_make == "hyundai" and toggle.lkas_allowed_for_aol longitudinalActuatorDelay = CP.longitudinalActuatorDelay toggle.openpilot_longitudinal = CP.openpilotLongitudinalControl and not toggle.disable_openpilot_long + toggle.experimental_mode_available = ( + toggle.openpilot_longitudinal or lateral_only_experimental_available(CP) + ) if not toggle.redneck_cruise_available or (toggle.openpilot_longitudinal and FPCP.pcmCruiseSpeed): self.params.put_bool("RedneckCruise", False) toggle.redneck_cruise = self.get_value( @@ -1000,7 +1004,9 @@ class StarPilotVariables: ) distance_button_control = self.get_button_function("DistanceButtonControl") - toggle.experimental_mode_via_distance = toggle.openpilot_longitudinal and distance_button_control == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + toggle.experimental_mode_via_distance = ( + toggle.experimental_mode_available and distance_button_control == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + ) toggle.experimental_mode_via_press = toggle.experimental_mode_via_distance toggle.force_coast_via_distance = toggle.openpilot_longitudinal and distance_button_control == BUTTON_FUNCTIONS["FORCE_COAST"] toggle.pulse_and_glide_via_distance = toggle.pulse_and_glide_available and distance_button_control == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] @@ -1013,7 +1019,9 @@ class StarPilotVariables: self.set_favorite_button_flags(toggle, "distance", distance_button_control) distance_button_control_long = self.get_button_function("LongDistanceButtonControl") - toggle.experimental_mode_via_distance_long = toggle.openpilot_longitudinal and distance_button_control_long == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + toggle.experimental_mode_via_distance_long = ( + toggle.experimental_mode_available and distance_button_control_long == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + ) toggle.experimental_mode_via_press |= toggle.experimental_mode_via_distance_long toggle.force_coast_via_distance_long = toggle.openpilot_longitudinal and distance_button_control_long == BUTTON_FUNCTIONS["FORCE_COAST"] toggle.pulse_and_glide_via_distance_long = toggle.pulse_and_glide_available and distance_button_control_long == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] @@ -1026,7 +1034,9 @@ class StarPilotVariables: self.set_favorite_button_flags(toggle, "distance_long", distance_button_control_long) distance_button_control_very_long = self.get_button_function("VeryLongDistanceButtonControl") - toggle.experimental_mode_via_distance_very_long = toggle.openpilot_longitudinal and distance_button_control_very_long == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + toggle.experimental_mode_via_distance_very_long = ( + toggle.experimental_mode_available and distance_button_control_very_long == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + ) toggle.experimental_mode_via_press |= toggle.experimental_mode_via_distance_very_long toggle.force_coast_via_distance_very_long = toggle.openpilot_longitudinal and distance_button_control_very_long == BUTTON_FUNCTIONS["FORCE_COAST"] toggle.pulse_and_glide_via_distance_very_long = toggle.pulse_and_glide_available and distance_button_control_very_long == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] @@ -1039,7 +1049,9 @@ class StarPilotVariables: self.set_favorite_button_flags(toggle, "distance_very_long", distance_button_control_very_long) cancel_button_control = self.get_button_function("CancelButtonControl", condition=toggle.remap_cancel_to_distance) - toggle.experimental_mode_via_cancel = toggle.openpilot_longitudinal and cancel_button_control == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + toggle.experimental_mode_via_cancel = ( + toggle.experimental_mode_available and cancel_button_control == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + ) toggle.experimental_mode_via_press |= toggle.experimental_mode_via_cancel toggle.force_coast_via_cancel = toggle.openpilot_longitudinal and cancel_button_control == BUTTON_FUNCTIONS["FORCE_COAST"] toggle.pulse_and_glide_via_cancel = toggle.pulse_and_glide_available and cancel_button_control == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] @@ -1052,7 +1064,9 @@ class StarPilotVariables: self.set_favorite_button_flags(toggle, "cancel", cancel_button_control) cancel_button_control_long = self.get_button_function("LongCancelButtonControl", condition=toggle.remap_cancel_to_distance) - toggle.experimental_mode_via_cancel_long = toggle.openpilot_longitudinal and cancel_button_control_long == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + toggle.experimental_mode_via_cancel_long = ( + toggle.experimental_mode_available and cancel_button_control_long == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + ) toggle.experimental_mode_via_press |= toggle.experimental_mode_via_cancel_long toggle.force_coast_via_cancel_long = toggle.openpilot_longitudinal and cancel_button_control_long == BUTTON_FUNCTIONS["FORCE_COAST"] toggle.pulse_and_glide_via_cancel_long = toggle.pulse_and_glide_available and cancel_button_control_long == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] @@ -1065,7 +1079,9 @@ class StarPilotVariables: self.set_favorite_button_flags(toggle, "cancel_long", cancel_button_control_long) cancel_button_control_very_long = self.get_button_function("VeryLongCancelButtonControl", condition=toggle.remap_cancel_to_distance) - toggle.experimental_mode_via_cancel_very_long = toggle.openpilot_longitudinal and cancel_button_control_very_long == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + toggle.experimental_mode_via_cancel_very_long = ( + toggle.experimental_mode_available and cancel_button_control_very_long == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + ) toggle.experimental_mode_via_press |= toggle.experimental_mode_via_cancel_very_long toggle.force_coast_via_cancel_very_long = toggle.openpilot_longitudinal and cancel_button_control_very_long == BUTTON_FUNCTIONS["FORCE_COAST"] toggle.pulse_and_glide_via_cancel_very_long = toggle.pulse_and_glide_available and cancel_button_control_very_long == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] @@ -1122,7 +1138,9 @@ class StarPilotVariables: toggle.use_turn_desires = self.get_value("TurnDesires", condition=lateral_tuning) lkas_button_control = self.get_button_function("LKASButtonControl", condition=toggle.car_make != "subaru") - toggle.experimental_mode_via_lkas = toggle.openpilot_longitudinal and lkas_button_control == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + toggle.experimental_mode_via_lkas = ( + toggle.experimental_mode_available and lkas_button_control == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + ) toggle.experimental_mode_via_press |= toggle.experimental_mode_via_lkas toggle.force_coast_via_lkas = toggle.openpilot_longitudinal and lkas_button_control == BUTTON_FUNCTIONS["FORCE_COAST"] toggle.pulse_and_glide_via_lkas = toggle.pulse_and_glide_available and lkas_button_control == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] @@ -1136,7 +1154,9 @@ class StarPilotVariables: has_canfd_media_buttons = toggle.car_make == "hyundai" and bool(CP.flags & HyundaiFlags.CANFD) mode_button_control = self.get_button_function("ModeButtonControl", condition=has_canfd_media_buttons) - toggle.experimental_mode_via_mode = toggle.openpilot_longitudinal and mode_button_control == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + toggle.experimental_mode_via_mode = ( + toggle.experimental_mode_available and mode_button_control == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + ) toggle.experimental_mode_via_press |= toggle.experimental_mode_via_mode toggle.force_coast_via_mode = toggle.openpilot_longitudinal and mode_button_control == BUTTON_FUNCTIONS["FORCE_COAST"] toggle.pulse_and_glide_via_mode = toggle.pulse_and_glide_available and mode_button_control == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] @@ -1149,7 +1169,9 @@ class StarPilotVariables: self.set_favorite_button_flags(toggle, "mode", mode_button_control) mode_button_control_long = self.get_button_function("LongModeButtonControl", condition=has_canfd_media_buttons) - toggle.experimental_mode_via_mode_long = toggle.openpilot_longitudinal and mode_button_control_long == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + toggle.experimental_mode_via_mode_long = ( + toggle.experimental_mode_available and mode_button_control_long == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + ) toggle.experimental_mode_via_press |= toggle.experimental_mode_via_mode_long toggle.force_coast_via_mode_long = toggle.openpilot_longitudinal and mode_button_control_long == BUTTON_FUNCTIONS["FORCE_COAST"] toggle.pulse_and_glide_via_mode_long = toggle.pulse_and_glide_available and mode_button_control_long == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] @@ -1162,7 +1184,9 @@ class StarPilotVariables: self.set_favorite_button_flags(toggle, "mode_long", mode_button_control_long) mode_button_control_very_long = self.get_button_function("VeryLongModeButtonControl", condition=has_canfd_media_buttons) - toggle.experimental_mode_via_mode_very_long = toggle.openpilot_longitudinal and mode_button_control_very_long == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + toggle.experimental_mode_via_mode_very_long = ( + toggle.experimental_mode_available and mode_button_control_very_long == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + ) toggle.experimental_mode_via_press |= toggle.experimental_mode_via_mode_very_long toggle.force_coast_via_mode_very_long = toggle.openpilot_longitudinal and mode_button_control_very_long == BUTTON_FUNCTIONS["FORCE_COAST"] toggle.pulse_and_glide_via_mode_very_long = toggle.pulse_and_glide_available and mode_button_control_very_long == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] @@ -1175,7 +1199,9 @@ class StarPilotVariables: self.set_favorite_button_flags(toggle, "mode_very_long", mode_button_control_very_long) star_button_control = self.get_button_function("StarButtonControl", condition=has_canfd_media_buttons) - toggle.experimental_mode_via_star = toggle.openpilot_longitudinal and star_button_control == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + toggle.experimental_mode_via_star = ( + toggle.experimental_mode_available and star_button_control == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + ) toggle.experimental_mode_via_press |= toggle.experimental_mode_via_star toggle.force_coast_via_star = toggle.openpilot_longitudinal and star_button_control == BUTTON_FUNCTIONS["FORCE_COAST"] toggle.pulse_and_glide_via_star = toggle.pulse_and_glide_available and star_button_control == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] @@ -1188,7 +1214,9 @@ class StarPilotVariables: self.set_favorite_button_flags(toggle, "star", star_button_control) star_button_control_long = self.get_button_function("LongStarButtonControl", condition=has_canfd_media_buttons) - toggle.experimental_mode_via_star_long = toggle.openpilot_longitudinal and star_button_control_long == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + toggle.experimental_mode_via_star_long = ( + toggle.experimental_mode_available and star_button_control_long == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + ) toggle.experimental_mode_via_press |= toggle.experimental_mode_via_star_long toggle.force_coast_via_star_long = toggle.openpilot_longitudinal and star_button_control_long == BUTTON_FUNCTIONS["FORCE_COAST"] toggle.pulse_and_glide_via_star_long = toggle.pulse_and_glide_available and star_button_control_long == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] @@ -1201,7 +1229,9 @@ class StarPilotVariables: self.set_favorite_button_flags(toggle, "star_long", star_button_control_long) star_button_control_very_long = self.get_button_function("VeryLongStarButtonControl", condition=has_canfd_media_buttons) - toggle.experimental_mode_via_star_very_long = toggle.openpilot_longitudinal and star_button_control_very_long == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + toggle.experimental_mode_via_star_very_long = ( + toggle.experimental_mode_available and star_button_control_very_long == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] + ) toggle.experimental_mode_via_press |= toggle.experimental_mode_via_star_very_long toggle.force_coast_via_star_very_long = toggle.openpilot_longitudinal and star_button_control_very_long == BUTTON_FUNCTIONS["FORCE_COAST"] toggle.pulse_and_glide_via_star_very_long = toggle.pulse_and_glide_available and star_button_control_very_long == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] diff --git a/starpilot/controls/lib/neural_network_feedforward.py b/starpilot/controls/lib/neural_network_feedforward.py index 207356da3..3b8e76e88 100644 --- a/starpilot/controls/lib/neural_network_feedforward.py +++ b/starpilot/controls/lib/neural_network_feedforward.py @@ -9,6 +9,7 @@ from collections import deque from difflib import SequenceMatcher from cereal import log +from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.params import Params @@ -33,6 +34,17 @@ from openpilot.starpilot.common.starpilot_variables import NNFF_MODELS_PATH, get # dict used to rename activation functions whose names aren't valid python identifiers ACTIVATION_FUNCTION_NAMES = {"σ": "sigmoid"} +PALISADE_NNFF_LAT_JERK_FRICTION_FACTOR = 0.25 +DEFAULT_NNFF_LAT_JERK_FRICTION_FACTOR = 0.4 + + +def get_nnff_lat_jerk_friction_factor(car_fingerprint) -> float: + return ( + PALISADE_NNFF_LAT_JERK_FRICTION_FACTOR + if car_fingerprint == HYUNDAI_CAR.HYUNDAI_PALISADE_2023 + else DEFAULT_NNFF_LAT_JERK_FRICTION_FACTOR + ) + LOW_SPEED_X = [0, 10, 20, 30] LOW_SPEED_Y = [12, 3, 1, 0] @@ -190,7 +202,7 @@ class LatControlNNFF(LatControl): # Scaling the lateral acceleration "friction response" could be helpful for some. # Increase for a stronger response, decrease for a weaker response. self.lat_accel_friction_factor = 0.7 # in [0, 3], in 0.05 increments. 3 is arbitrary safety limit - self.lat_jerk_friction_factor = 0.4 + self.lat_jerk_friction_factor = get_nnff_lat_jerk_friction_factor(CP.carFingerprint) # precompute time differences between ModelConstants.T_IDXS self.t_diffs = np.diff(ModelConstants.T_IDXS) diff --git a/starpilot/controls/starpilot_card.py b/starpilot/controls/starpilot_card.py index 8d865cca3..75225aa06 100644 --- a/starpilot/controls/starpilot_card.py +++ b/starpilot/controls/starpilot_card.py @@ -17,6 +17,13 @@ from openpilot.starpilot.common.experimental_state import ( ) from openpilot.starpilot.common.favorite_slots import FAVORITE_ACTION_TRAFFIC_MODE_COUNTER, toggle_favorite_slot from openpilot.starpilot.common.starpilot_variables import ERROR_LOGS_PATH, GearShifter, NON_DRIVING_GEARS +from openpilot.starpilot.common.lateral_only_experimental import experimental_mode_available +from openpilot.starpilot.system.wheel_controls import ( + CONTROLLER_ACTION_COUNTERS, + CONTROLLER_ACTION_FORCE_COAST, + CONTROLLER_ACTION_PULSE_AND_GLIDE, + CONTROLLER_ACTION_TOGGLE_AOL, +) HYUNDAI_MAIN_CRUISE_AOL_CONFIRM_TIMEOUT_FRAMES = 100 @@ -57,6 +64,11 @@ class StarPilotCard: self.distancePressed_previously = False self.force_coast = False self.pulse_and_glide = False + self._controller_action_counters = { + key: self._get_controller_action_counter(counter) + for key, counter in CONTROLLER_ACTION_COUNTERS.items() + if counter != "WheelButtonBookmarkCounter" + } self.modePressed_previously = False self.mode_counter = 0 self.customPressed_previously = False @@ -79,7 +91,13 @@ class StarPilotCard: self.error_log = ERROR_LOGS_PATH / "error.txt" def handle_button_event(self, key, sm, starpilot_toggles): - if sm["carControl"].longActive and getattr(starpilot_toggles, f"experimental_mode_via_{key}"): + experimental_active = bool(getattr(sm["carControl"], "longActive", False) or + getattr(sm["carControl"], "latActive", False)) + mode_available = getattr(starpilot_toggles, "experimental_mode_available", + bool(getattr(sm["carControl"], "longActive", False) or + experimental_mode_available(self.CP))) + if (experimental_active and mode_available and + getattr(starpilot_toggles, f"experimental_mode_via_{key}")): self.handle_experimental_mode(sm, starpilot_toggles) elif getattr(starpilot_toggles, f"bookmark_via_{key}"): self.handle_bookmark() @@ -108,6 +126,49 @@ class StarPilotCard: counter = self.params_memory.get_int("WheelButtonBookmarkCounter") self.params_memory.put_int("WheelButtonBookmarkCounter", counter + 1) + def _get_controller_action_counter(self, key): + try: + return self.params_memory.get_int(key) + except Exception: + return 0 + + def _pending_controller_action_count(self, key): + counter_key = CONTROLLER_ACTION_COUNTERS[key] + current = self._get_controller_action_counter(counter_key) + previous = self._controller_action_counters[key] + self._controller_action_counters[key] = current + return max(0, current - previous) + + def _toggle_controller_aol(self, carState, starpilot_toggles, button_aol_supported): + if not button_aol_supported or not getattr(starpilot_toggles, "always_on_lateral", False): + return False + if self.hyundai_aol_needs_engagement: + self.hyundai_aol_ready = True + self.always_on_lateral_allowed = not self.always_on_lateral_allowed + if carState.cruiseState.enabled or self.pause_lateral: + self.pause_lateral = not self.always_on_lateral_allowed + return True + + def _handle_controller_actions(self, carState, sm, starpilot_toggles, button_aol_supported): + force_coast_count = self._pending_controller_action_count( + CONTROLLER_ACTION_FORCE_COAST + ) + if force_coast_count % 2 and getattr(starpilot_toggles, "openpilot_longitudinal", False): + self.force_coast = not self.force_coast + + pulse_and_glide_count = self._pending_controller_action_count( + CONTROLLER_ACTION_PULSE_AND_GLIDE + ) + if (pulse_and_glide_count % 2 and getattr(starpilot_toggles, "pulse_and_glide_available", False) and + (getattr(sm["carControl"], "longActive", False) or self.pulse_and_glide)): + self.pulse_and_glide = not self.pulse_and_glide + + aol_count = self._pending_controller_action_count( + CONTROLLER_ACTION_TOGGLE_AOL + ) + if aol_count % 2: + self._toggle_controller_aol(carState, starpilot_toggles, button_aol_supported) + def _handle_favorite_traffic_mode_action(self, sm): counter = self.params_memory.get_int(FAVORITE_ACTION_TRAFFIC_MODE_COUNTER) pending = counter - self._favorite_traffic_mode_counter @@ -339,6 +400,8 @@ class StarPilotCard: else: self.handle_button_event("lkas", sm, starpilot_toggles) + self._handle_controller_actions(carState, sm, starpilot_toggles, button_aol_supported) + if getattr(starpilot_toggles, "has_canfd_media_buttons", False): if starpilotCarState.modePressed: self.mode_counter += 1 diff --git a/starpilot/controls/tests/test_starpilot_card.py b/starpilot/controls/tests/test_starpilot_card.py index 022a8f39b..caef51d14 100644 --- a/starpilot/controls/tests/test_starpilot_card.py +++ b/starpilot/controls/tests/test_starpilot_card.py @@ -337,6 +337,35 @@ def test_honda_lkas_button_can_toggle_always_on_lateral(monkeypatch, tmp_path): assert ret.pauseLateral is False +def test_controller_actions_match_vehicle_button_behaviors(monkeypatch, tmp_path): + monkeypatch.setattr(spc, "Params", FakeParams) + monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) + + card = spc.StarPilotCard(SimpleNamespace(brand="honda"), SimpleNamespace(alternativeExperience=0)) + sm = make_sm() + sm["carControl"].longActive = True + toggles = make_toggles( + always_on_lateral=True, + lkas_allowed_for_aol=True, + openpilot_longitudinal=True, + pulse_and_glide_available=True, + ) + for _key, counter in spc.CONTROLLER_ACTION_COUNTERS.items(): + if counter != "WheelButtonBookmarkCounter": + card.params_memory.put_int(counter, 1) + + ret = card.update(make_car_state(), SimpleNamespace(distancePressed=False), sm, toggles) + + assert card.force_coast is True + assert card.pulse_and_glide is True + assert ret.alwaysOnLateralAllowed is True + + ret = card.update(make_car_state(), SimpleNamespace(distancePressed=False), sm, toggles) + assert card.force_coast is True + assert card.pulse_and_glide is True + assert ret.alwaysOnLateralAllowed is True + + def test_hyundai_lkas_button_can_start_aol_before_normal_engagement(monkeypatch, tmp_path): monkeypatch.setattr(spc, "Params", FakeParams) monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) diff --git a/starpilot/system/the_galaxy/tests/test_dashboard_stats.py b/starpilot/system/the_galaxy/tests/test_dashboard_stats.py index 0034daf72..23ee5a4fc 100644 --- a/starpilot/system/the_galaxy/tests/test_dashboard_stats.py +++ b/starpilot/system/the_galaxy/tests/test_dashboard_stats.py @@ -323,6 +323,10 @@ def _install_server_import_stubs(): CONTROLLER_ACTION_OPTIONS=( {"key": "__starpilot_controller_action__:set_speed", "label": "Set Speed To", "section": "Controller Actions", "value_type": "speed"}, {"key": "__starpilot_controller_action__:selfie", "label": "Take Comma Selfie", "section": "Controller Actions"}, + {"key": "__starpilot_controller_action__:bookmark", "label": "Bookmark", "section": "Controller Actions"}, + {"key": "__starpilot_controller_action__:pulse_and_glide", "label": "Pulse and Glide", "section": "Controller Actions"}, + {"key": "__starpilot_controller_action__:force_coast", "label": "Force Coasting", "section": "Controller Actions"}, + {"key": "__starpilot_controller_action__:toggle_aol", "label": "Toggle AOL", "section": "Controller Actions"}, ), CONTROLLER_ACTION_SET_SPEED="__starpilot_controller_action__:set_speed", CONTROLLER_ACTION_SLOT_COUNT=10, diff --git a/starpilot/system/the_galaxy/tests/test_navigation_params.py b/starpilot/system/the_galaxy/tests/test_navigation_params.py index c93360016..7eeb10138 100644 --- a/starpilot/system/the_galaxy/tests/test_navigation_params.py +++ b/starpilot/system/the_galaxy/tests/test_navigation_params.py @@ -224,6 +224,10 @@ def test_wheel_controls_status_includes_favorite_slots(monkeypatch): "ForceOffroad", "__starpilot_controller_action__:set_speed", "__starpilot_controller_action__:selfie", + "__starpilot_controller_action__:bookmark", + "__starpilot_controller_action__:pulse_and_glide", + "__starpilot_controller_action__:force_coast", + "__starpilot_controller_action__:toggle_aol", } assert response.get_json()["speed_unit"] == "mph" @@ -241,6 +245,10 @@ def test_wheel_controls_configures_a_controller_only_action(monkeypatch): "ForceOffroad", "__starpilot_controller_action__:set_speed", "__starpilot_controller_action__:selfie", + "__starpilot_controller_action__:bookmark", + "__starpilot_controller_action__:pulse_and_glide", + "__starpilot_controller_action__:force_coast", + "__starpilot_controller_action__:toggle_aol", } assert calls == [((9, "ForceOffroad", "Force Offroad", the_galaxy.params), {"value": None, "eligible_keys": expected_keys})] diff --git a/starpilot/system/wheel_controls/__init__.py b/starpilot/system/wheel_controls/__init__.py index 4ac67e220..139ea8d01 100644 --- a/starpilot/system/wheel_controls/__init__.py +++ b/starpilot/system/wheel_controls/__init__.py @@ -1,7 +1,12 @@ from .wheel_controlsd import ( CONTROLLER_ACTION_SLOT_COUNT, CONTROLLER_ACTION_OPTIONS, + CONTROLLER_ACTION_BOOKMARK, + CONTROLLER_ACTION_COUNTERS, + CONTROLLER_ACTION_FORCE_COAST, + CONTROLLER_ACTION_PULSE_AND_GLIDE, CONTROLLER_ACTION_SET_SPEED, + CONTROLLER_ACTION_TOGGLE_AOL, FAVORITE_SLOT_COUNT, LEARN_TIMEOUT_SECONDS, cancel_learning, @@ -23,7 +28,12 @@ from .wheel_controlsd import ( __all__ = [ "CONTROLLER_ACTION_SLOT_COUNT", "CONTROLLER_ACTION_OPTIONS", + "CONTROLLER_ACTION_BOOKMARK", + "CONTROLLER_ACTION_COUNTERS", + "CONTROLLER_ACTION_FORCE_COAST", + "CONTROLLER_ACTION_PULSE_AND_GLIDE", "CONTROLLER_ACTION_SET_SPEED", + "CONTROLLER_ACTION_TOGGLE_AOL", "FAVORITE_SLOT_COUNT", "LEARN_TIMEOUT_SECONDS", "cancel_learning", diff --git a/starpilot/system/wheel_controls/tests/test_wheel_controlsd.py b/starpilot/system/wheel_controls/tests/test_wheel_controlsd.py index 16ad89eaf..ffaf05d86 100644 --- a/starpilot/system/wheel_controls/tests/test_wheel_controlsd.py +++ b/starpilot/system/wheel_controls/tests/test_wheel_controlsd.py @@ -77,6 +77,15 @@ def test_controller_action_slots_are_separate_and_fixed_at_ten(): assert wheel_controlsd.CONTROLLER_ACTIONS_PARAM != "StarPilotFavoriteSlots" +def test_controller_action_options_include_vehicle_controls(): + options = {option["key"]: option for option in wheel_controlsd.CONTROLLER_ACTION_OPTIONS} + + assert options[wheel_controlsd.CONTROLLER_ACTION_BOOKMARK]["label"] == "Bookmark" + assert options[wheel_controlsd.CONTROLLER_ACTION_PULSE_AND_GLIDE]["label"] == "Pulse and Glide" + assert options[wheel_controlsd.CONTROLLER_ACTION_FORCE_COAST]["label"] == "Force Coasting" + assert options[wheel_controlsd.CONTROLLER_ACTION_TOGGLE_AOL]["label"] == "Toggle AOL" + + def test_joystick_selection_is_explicit_and_exclusive(): params = FakeParams() @@ -204,6 +213,28 @@ def test_controller_custom_actions_dispatch_their_own_payload(monkeypatch): assert selfies == [True] +def test_controller_actions_trigger_runtime_counters(): + params = FakeParams({ + wheel_controlsd.CONTROLLER_ACTIONS_PARAM: [ + {"enabled": True, "key": wheel_controlsd.CONTROLLER_ACTION_BOOKMARK, "label": "Bookmark"}, + {"enabled": True, "key": wheel_controlsd.CONTROLLER_ACTION_PULSE_AND_GLIDE, "label": "Pulse and Glide"}, + {"enabled": True, "key": wheel_controlsd.CONTROLLER_ACTION_FORCE_COAST, "label": "Force Coasting"}, + {"enabled": True, "key": wheel_controlsd.CONTROLLER_ACTION_TOGGLE_AOL, "label": "Toggle AOL"}, + ], + }) + memory = FakeParams() + + for index in range(4): + assert wheel_controlsd.execute_controller_action(index, params, memory) + + assert memory.values == { + "WheelButtonBookmarkCounter": 1, + "WheelControlPulseGlideCounter": 1, + "WheelControlForceCoastCounter": 1, + "WheelControlAOLCounter": 1, + } + + def test_learning_accepts_the_tenth_controller_action(): params = FakeParams({"IsOffroad": True}) memory = FakeParams() diff --git a/starpilot/system/wheel_controls/wheel_controlsd.py b/starpilot/system/wheel_controls/wheel_controlsd.py index b942009af..3acc39868 100644 --- a/starpilot/system/wheel_controls/wheel_controlsd.py +++ b/starpilot/system/wheel_controls/wheel_controlsd.py @@ -33,6 +33,16 @@ CONTROLLER_ACTION_SLOT_COUNT = 10 MAPPING_SLOT_COUNT = FAVORITE_SLOT_COUNT + CONTROLLER_ACTION_SLOT_COUNT CONTROLLER_ACTION_SET_SPEED = "__starpilot_controller_action__:set_speed" CONTROLLER_ACTION_SELFIE = "__starpilot_controller_action__:selfie" +CONTROLLER_ACTION_BOOKMARK = "__starpilot_controller_action__:bookmark" +CONTROLLER_ACTION_PULSE_AND_GLIDE = "__starpilot_controller_action__:pulse_and_glide" +CONTROLLER_ACTION_FORCE_COAST = "__starpilot_controller_action__:force_coast" +CONTROLLER_ACTION_TOGGLE_AOL = "__starpilot_controller_action__:toggle_aol" +CONTROLLER_ACTION_COUNTERS = { + CONTROLLER_ACTION_BOOKMARK: "WheelButtonBookmarkCounter", + CONTROLLER_ACTION_PULSE_AND_GLIDE: "WheelControlPulseGlideCounter", + CONTROLLER_ACTION_FORCE_COAST: "WheelControlForceCoastCounter", + CONTROLLER_ACTION_TOGGLE_AOL: "WheelControlAOLCounter", +} CONTROLLER_ACTION_OPTIONS = ( { "key": CONTROLLER_ACTION_SET_SPEED, @@ -48,6 +58,30 @@ CONTROLLER_ACTION_OPTIONS = ( "description": "Captures the driver camera and saves it in Sentry history.", "section": "Controller Actions", }, + { + "key": CONTROLLER_ACTION_BOOKMARK, + "label": "Bookmark", + "description": "Creates a driving bookmark without changing the on-screen Favorites.", + "section": "Controller Actions", + }, + { + "key": CONTROLLER_ACTION_PULSE_AND_GLIDE, + "label": "Pulse and Glide", + "description": "Toggles Pulse and Glide using the same transient control as a mapped vehicle button.", + "section": "Controller Actions", + }, + { + "key": CONTROLLER_ACTION_FORCE_COAST, + "label": "Force Coasting", + "description": "Toggles forced coasting using the same transient control as a mapped vehicle button.", + "section": "Controller Actions", + }, + { + "key": CONTROLLER_ACTION_TOGGLE_AOL, + "label": "Toggle AOL", + "description": "Toggles Always On Lateral like the vehicle LKAS button; it does not change the AOL setting.", + "section": "Controller Actions", + }, ) CONTROLLER_ACTION_KEYS = {option["key"] for option in CONTROLLER_ACTION_OPTIONS} LEARN_TIMEOUT_SECONDS = 20.0 @@ -271,6 +305,14 @@ def request_comma_selfie() -> bool: return True +def trigger_controller_action(key: str, params_memory: Params) -> bool: + counter_key = CONTROLLER_ACTION_COUNTERS.get(key) + if counter_key is None: + return False + params_memory.put_int(counter_key, params_memory.get_int(counter_key) + 1) + return True + + def normalize_mappings(value: Any) -> list[dict[str, Any]]: if isinstance(value, bytes): value = value.decode("utf-8", errors="replace") @@ -443,6 +485,8 @@ def execute_controller_action(index: int, params: Params, params_memory: Params) return set_controller_cruise_speed(slot.get("value"), params, params_memory) if slot.get("key") == CONTROLLER_ACTION_SELFIE: return request_comma_selfie() + if slot.get("key") in CONTROLLER_ACTION_COUNTERS: + return trigger_controller_action(slot["key"], params_memory) return execute_favorite_key(slot.get("key"), params, params_memory)