mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-12 03:03:51 +08:00
Code Issues Packages Projects Releases Wiki Activity

weevil

Browse Source
This commit is contained in:
firestar5683
2026-09-01 09:26:29 -05:00
parent eef1d0513e
commit d0fc9f9f46
30 changed files with 350 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/car/car_specific.py
+10 -3
View File
@@ -3,6 +3,7 @@ import cereal.messaging as messaging
from opendbc.car import DT_CTRL, structs
from opendbc.car.chrysler.values import RAM_DT
from opendbc.car.gm.values import CAR as GM_CAR, GMFlags, SDGM_CAR
from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR
from opendbc.car.interfaces import MAX_CTRL_SPEED
from opendbc.car.rivian.values import RivianFlags
@@ -198,7 +199,13 @@ class CarSpecificEvents:
events = self.create_common_events(CS, CS_prev, extra_gears=extra_gears)
elif self.CP.brand == 'hyundai':
events = self.create_common_events(CS, CS_prev, extra_gears=extra_gears, pcm_enable=self.CP.pcmCruise, allow_button_cancel=False)
ray_ev = self.CP.carFingerprint == HYUNDAI_CAR.KIA_RAY_EV
events = self.create_common_events(
CS, CS_prev, extra_gears=extra_gears,
pcm_enable=self.CP.pcmCruise and not ray_ev,
allow_button_cancel=False,
ignore_cruise_state=ray_ev,
)
elif self.CP.brand == 'nissan':
events = self.create_common_events(CS, CS_prev, extra_gears=extra_gears, pcm_enable=self.CP.pcmCruise)
@@ -221,7 +228,7 @@ class CarSpecificEvents:
return events
def create_common_events(self, CS: structs.CarState, CS_prev: car.CarState, extra_gears: list | None = None, pcm_enable=True,
allow_button_cancel=True, suppress_low_speed_alert=False):
allow_button_cancel=True, suppress_low_speed_alert=False, ignore_cruise_state=False):
events = Events()
preap_software_cruise = (self.CP.brand == "tesla" and self.CP.carFingerprint == "TESLA_MODEL_S_PREAP" and
self.CP.openpilotLongitudinalControl and not self.CP.pcmCruise)
@@ -236,7 +243,7 @@ class CarSpecificEvents:
events.add(EventName.wrongGear)
if CS.gearShifter == GearShifter.reverse:
events.add(EventName.reverseGear)
if not CS.cruiseState.available:
if not CS.cruiseState.available and not ignore_cruise_state:
events.add(EventName.wrongCarMode)
if CS.espDisabled:
events.add(EventName.espDisabled)
selfdrive/car/card.py
+7 -2
View File
@@ -33,7 +33,7 @@ from openpilot.starpilot.common.favorite_slots import (
FAVORITE_ACTION_ACCEL_COUNTER,
FAVORITE_ACTION_DECEL_COUNTER,
)
from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles, update_starpilot_toggles
from openpilot.starpilot.common.starpilot_variables import always_on_lateral_available, 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
@@ -145,7 +145,10 @@ class Car:
if car_gps_supported:
self.gps_pm = messaging.PubMaster(['gpsLocationExternal'])
aol_available = always_on_lateral_available(self.CP)
interface_alternative_experience = self.CP.alternativeExperience
if not aol_available:
interface_alternative_experience &= ~ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL
self.CP.alternativeExperience = interface_alternative_experience
openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle")
controller_available = self.CI.CC is not None and openpilot_enabled_toggle
@@ -217,8 +220,10 @@ class Car:
self.starpilot_toggles = get_starpilot_toggles(read_persisted_force_params=True)
self.FPCP.alternativeExperience |= interface_alternative_experience
if not aol_available:
self.FPCP.alternativeExperience &= ~ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL
if self.starpilot_toggles.always_on_lateral:
if self.starpilot_toggles.always_on_lateral and aol_available:
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL
self.FPCP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL
if getattr(self.starpilot_toggles, "remap_cancel_to_distance", False):
selfdrive/controls/controlsd.py
+4 -1
View File
@@ -405,6 +405,7 @@ class Controls:
self.kona_non_scc_lateral_active = False
self.kona_non_scc_lateral_faulted = False
self.elantra_hev_2024_lateral_faulted = False
self.elantra_hev_2024_previous_cruise_enabled = False
self.pose_calibrator = PoseCalibrator()
self.calibrated_pose: Pose | None = None
@@ -520,7 +521,8 @@ class Controls:
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:
cruise_reenabled = CS.cruiseState.enabled and not self.elantra_hev_2024_previous_cruise_enabled
if not lateral_requested or cruise_reenabled:
self.elantra_hev_2024_lateral_faulted = False
elif CS.steerFaultTemporary:
self.elantra_hev_2024_lateral_faulted = True
@@ -532,6 +534,7 @@ class Controls:
self.sm['starpilotPlan'].lateralCheck,
self.elantra_hev_2024_lateral_faulted,
)
self.elantra_hev_2024_previous_cruise_enabled = CS.cruiseState.enabled
else:
CC.latActive = get_lateral_active(CC.enabled, self.sm['selfdriveState'].active,
self.sm['starpilotCarState'].alwaysOnLateralEnabled,
selfdrive/controls/lib/latcontrol_torque.py
+5 -1
View File
@@ -513,7 +513,11 @@ class LatControlTorque(LatControl):
elif ioniq_5_active:
vehicle_friction_jerk_deadzone = get_ioniq_5_friction_jerk_deadzone(CS.vEgo, setpoint)
elif prius_active:
vehicle_friction_jerk_deadzone = get_prius_friction_jerk_deadzone(CS.vEgo, setpoint)
prius_deadzone_max = (PRIUS_STANDARD_FRICTION_JERK_DEADZONE_MAX if self.is_standard_prius
else PRIUS_FRICTION_JERK_DEADZONE_MAX)
vehicle_friction_jerk_deadzone = get_prius_friction_jerk_deadzone(
CS.vEgo, setpoint, prius_deadzone_max,
)
elif genesis_g70_active:
vehicle_friction_jerk_deadzone = get_genesis_g70_friction_jerk_deadzone(CS.vEgo, setpoint)
elif self.is_genesis_gv70:
selfdrive/controls/lib/latcontrol_vehicle_tunes.py
+8 -3
View File
@@ -219,7 +219,7 @@ GENESIS_GV70_FRICTION_CENTER_LAT = 0.28
GENESIS_GV70_FRICTION_CENTER_LAT_WIDTH = 0.12
GENESIS_GV70_FRICTION_CALM_JERK = 0.35
GENESIS_GV70_FRICTION_CALM_JERK_WIDTH = 0.10
GENESIS_GV70_FRICTION_JERK_DEADZONE_MAX = 0.36
GENESIS_GV70_FRICTION_JERK_DEADZONE_MAX = 0.55
GENESIS_GV70_FRICTION_JERK_DEADZONE_LAT = 0.30
GENESIS_GV70_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.08
GENESIS_GV70_FRICTION_JERK_DEADZONE_SPEED = 12.0 * CV.MPH_TO_MS
@@ -302,6 +302,7 @@ GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR = 0.18
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR_WIDTH = 0.15
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK = 0.15
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK_WIDTH = 0.10
GENESIS_G70_HIGH_SPEED_OVERSHOOT_PHASE_WEIGHT = 0.60
GENESIS_G70_ANGLE_OUTPUT_TAPER_MIN = 0.45
GENESIS_G70_ANGLE_OUTPUT_TAPER_START = 70.0
GENESIS_G70_ANGLE_OUTPUT_TAPER_WIDTH = 6.0
@@ -1044,6 +1045,7 @@ PRIUS_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.07
PRIUS_CENTER_FRICTION_THRESHOLD_SPEED = 18.0
PRIUS_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 2.2
PRIUS_FRICTION_JERK_DEADZONE_MAX = 0.24
PRIUS_STANDARD_FRICTION_JERK_DEADZONE_MAX = 0.30
PRIUS_FRICTION_JERK_DEADZONE_LAT = 0.30
PRIUS_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.07
PRIUS_FRICTION_JERK_DEADZONE_SPEED = 18.0
@@ -1508,12 +1510,13 @@ def get_prius_center_taper_scale(desired_lateral_accel: float, v_ego: float) ->
return 1.0 - reduction
def get_prius_friction_jerk_deadzone(v_ego: float, desired_lateral_accel: float) -> float:
def get_prius_friction_jerk_deadzone(v_ego: float, desired_lateral_accel: float,
deadzone_max: float = PRIUS_FRICTION_JERK_DEADZONE_MAX) -> float:
speed_weight = _prius_sigmoid((v_ego - PRIUS_FRICTION_JERK_DEADZONE_SPEED) /
PRIUS_FRICTION_JERK_DEADZONE_SPEED_WIDTH)
center_weight = _prius_sigmoid((PRIUS_FRICTION_JERK_DEADZONE_LAT - abs(desired_lateral_accel)) /
PRIUS_FRICTION_JERK_DEADZONE_LAT_WIDTH)
return PRIUS_FRICTION_JERK_DEADZONE_MAX * speed_weight * center_weight
return deadzone_max * speed_weight * center_weight
def get_prius_high_speed_output_taper_scale(desired_lateral_accel: float, v_ego: float,
@@ -3235,6 +3238,8 @@ def get_genesis_g70_high_speed_error_scale(setpoint: float, measured_lateral_acc
jerk_weight = _sigmoid((abs(desired_lateral_jerk) - GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK) /
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK_WIDTH)
phase_weight = 1.0 if setpoint * desired_lateral_jerk < 0.0 else 0.45
if setpoint * measured_lateral_accel > 0.0 and abs(measured_lateral_accel) > abs(setpoint):
phase_weight = max(phase_weight, GENESIS_G70_HIGH_SPEED_OVERSHOOT_PHASE_WEIGHT)
reduction = (GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_MAX * speed_weight * error_weight *
(0.35 + (0.65 * jerk_weight)) * phase_weight)
return 1.0 - reduction
selfdrive/controls/tests/test_latcontrol.py
+6
View File
@@ -42,6 +42,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import (
get_sonata_hybrid_center_output_scale,
get_sonata_hybrid_friction_threshold,
get_prius_center_taper_scale,
PRIUS_STANDARD_FRICTION_JERK_DEADZONE_MAX,
KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT,
HONDA_ACCORD_TORQUE_KI,
HONDA_ACCORD_TORQUE_KP,
@@ -900,6 +901,8 @@ class TestLatControl:
assert base_scale > left_unwind_scale == right_unwind_scale
assert get_prius_friction_jerk_deadzone(30.0, 0.0) > get_prius_friction_jerk_deadzone(30.0, 0.8)
assert get_prius_friction_jerk_deadzone(30.0, 0.0, PRIUS_STANDARD_FRICTION_JERK_DEADZONE_MAX) > \
get_prius_friction_jerk_deadzone(30.0, 0.0)
assert get_prius_friction_jerk_deadzone(8.0, 0.0) < 0.05
assert get_prius_center_taper_scale(0.0, 30.0) < get_prius_center_taper_scale(0.8, 30.0)
assert get_prius_center_taper_scale(0.0, 8.0) > 0.99
@@ -944,6 +947,7 @@ class TestLatControl:
assert turn_scale > center_scale
assert highway_center_deadzone > highway_turn_deadzone
assert highway_turn_deadzone < 0.05
assert latcontrol_vehicle_tunes.get_genesis_gv70_friction_jerk_deadzone(60.0 * 0.44704, 0.2) > 0.40
def test_genesis_gv70_high_speed_error_damping(self):
assert get_genesis_gv70_high_speed_error_scale(0.2, 0.2, 0.8, 20.0) == 1.0
@@ -981,6 +985,8 @@ class TestLatControl:
assert get_genesis_g70_high_speed_error_scale(0.2, 0.2, 0.8, 20.0) == 1.0
assert get_genesis_g70_high_speed_error_scale(0.2, 0.9, 0.8, 20.0) < 1.0
assert get_genesis_g70_high_speed_error_scale(0.2, 0.9, 0.8, 10.0) > get_genesis_g70_high_speed_error_scale(0.2, 0.9, 0.8, 20.0)
assert get_genesis_g70_high_speed_error_scale(0.7, 0.95, 0.8, 30.0) < \
get_genesis_g70_high_speed_error_scale(0.7, 0.45, 0.8, 30.0)
def test_sonata_hybrid_center_output_taper_is_mid_speed_and_center_gated(self):
low_speed = get_sonata_hybrid_center_output_scale(0.0, 8.0)