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

F

Browse Source
This commit is contained in:
firestar5683
2026-05-11 21:17:43 -05:00
parent 02717ebe73
commit 4d0f65a6d1
3 changed files with 8 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/hyundai/carcontroller.py
+5 -16
View File
@@ -57,15 +57,6 @@ IONIQ_6_STOP_RELEASE_JERK_BP = [0.0, 0.15, 0.5]
IONIQ_6_STOP_RELEASE_JERK_V = [3.6 * IONIQ_6_RESPONSE_MULTIPLIER,
4.2 * IONIQ_6_RESPONSE_MULTIPLIER,
4.8 * IONIQ_6_RESPONSE_MULTIPLIER]
def get_canfd_lead_distance_setting(lead_distance: float | None, default_setting: int) -> int:
default = int(np.clip(default_setting, 0, 3))
if lead_distance is None or lead_distance <= CANFD_LEAD_MIN_DISTANCE:
return default
if lead_distance < 20.0:
return 1
if lead_distance < 30.0:
return 2
return 3
@dataclass
@@ -266,7 +257,6 @@ class CarController(CarControllerBase):
return lka_icon, lfa_icon
def _get_canfd_scc_lead_state(self, CC, CS, now_nanos):
default_distance_setting = int(np.clip(getattr(CC.hudControl, "leadDistanceBars", 0), 0, 3))
openpilot_lead_visible = bool(getattr(CS, "openpilot_lead_visible", False) or CC.hudControl.leadVisible)
openpilot_lead_distance = float(np.clip(getattr(CS, "openpilot_lead_distance", 0.0), 0.0, 204.7))
openpilot_lead_rel_speed = float(np.clip(getattr(CS, "openpilot_lead_rel_speed", 0.0), -16.4, 34.7))
@@ -274,15 +264,15 @@ class CarController(CarControllerBase):
stock_camera_lead_visible = stock_camera_lead_fresh and getattr(CS, "stock_camera_lead_visible", False)
if openpilot_lead_visible and openpilot_lead_distance > CANFD_LEAD_MIN_DISTANCE:
return True, openpilot_lead_distance, openpilot_lead_rel_speed, get_canfd_lead_distance_setting(openpilot_lead_distance, default_distance_setting)
return True, openpilot_lead_distance, openpilot_lead_rel_speed
if stock_camera_lead_visible:
lead_distance = float(np.clip(getattr(CS, "stock_camera_lead_distance", 0.0), 0.0, 204.7))
lead_rel_speed = float(np.clip(getattr(CS, "stock_camera_lead_rel_speed", 0.0), -16.4, 34.7))
return True, lead_distance, lead_rel_speed, get_canfd_lead_distance_setting(lead_distance, default_distance_setting)
return True, lead_distance, lead_rel_speed
if openpilot_lead_visible:
return True, CANFD_FALLBACK_LEAD_DISTANCE, 0.0, default_distance_setting
return True, CANFD_FALLBACK_LEAD_DISTANCE, 0.0
return False, 0.0, 0.0, default_distance_setting
return False, 0.0, 0.0
def update(self, CC, CS, now_nanos, starpilot_toggles):
actuators = CC.actuators
@@ -559,13 +549,12 @@ class CarController(CarControllerBase):
CC.leftBlinker,
CC.rightBlinker))
if self.frame % 2 == 0:
lead_visible, lead_distance, lead_rel_speed, lead_distance_setting = self._get_canfd_scc_lead_state(CC, CS, now_nanos)
lead_visible, lead_distance, lead_rel_speed = self._get_canfd_scc_lead_state(CC, CS, now_nanos)
acc_kwargs = {
"main_mode_acc": int(CS.out.cruiseState.available),
"direct_accel": True,
"jerk_lower": 5.0,
"jerk_upper": 3.0 if CC.actuators.longControlState == LongCtrlState.pid else 1.0,
"distance_setting": lead_distance_setting,
"lead_distance": lead_distance,
"lead_rel_speed": lead_rel_speed,
"lead_visible": lead_visible,
opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py
+1 -2
View File
@@ -333,7 +333,6 @@ def create_ioniq_6_cluster_lane_change_messages(CAN, frame, side=None, trigger=F
def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control,
main_mode_acc=1, jerk_lower=None, jerk_upper=None, direct_accel=False,
distance_setting=None,
lead_distance=None, lead_rel_speed=None, lead_visible=None):
jerk = 5
jn = jerk / 50
@@ -375,7 +374,7 @@ def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_ov
"SET_ME_2": 0x4,
"SET_ME_3": 0x3,
"SET_ME_TMP_64": 0x64,
"DISTANCE_SETTING": int(np.clip(hud_control.leadDistanceBars if distance_setting is None else distance_setting, 0, 3)),
"DISTANCE_SETTING": hud_control.leadDistanceBars,
}
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py
+2 -31
View File
@@ -8,7 +8,6 @@ from opendbc.car import Bus, ButtonType, gen_empty_fingerprint, structs
from opendbc.car.structs import CarControl, CarParams
from opendbc.car.fw_versions import build_fw_dict, match_fw_to_car
from opendbc.car.hyundai.carcontroller import CarController, Ioniq6LongitudinalTuningState, GenesisG90LongitudinalTuningState, \
get_canfd_lead_distance_setting, \
update_ioniq_6_longitudinal_tuning, \
update_genesis_g90_longitudinal_tuning
from opendbc.car.hyundai.carstate import CarState, decode_canfd_camera_lead, decode_ioniq_6_blindspot_radar_state
@@ -508,32 +507,6 @@ class TestHyundaiFingerprint:
assert parser.vl["SCC_CONTROL"]["ObjValid"] == 1
assert parser.vl["SCC_CONTROL"]["OBJ_STATUS"] == 0
def test_canfd_acc_control_allows_distance_setting_override(self):
CP = CarParams.new_message()
CP.carFingerprint = CAR.KIA_EV6
CP.flags = int(HyundaiFlags.CANFD)
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
can_bus = CanBus(CP)
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("SCC_CONTROL", 0)], can_bus.ECAN)
msg = hyundaicanfd.create_acc_control(packer, can_bus, enabled=True, accel_last=0.0, accel=0.1, stopping=False,
gas_override=False, set_speed=42, hud_control=SimpleNamespace(leadDistanceBars=1),
direct_accel=True, distance_setting=3,
lead_distance=41.0, lead_rel_speed=-0.5, lead_visible=True)
parser.update([(1, [msg])])
assert parser.can_valid
assert parser.vl["SCC_CONTROL"]["DISTANCE_SETTING"] == 3
assert parser.vl["SCC_CONTROL"]["ACC_ObjDist"] == pytest.approx(41.0)
def test_canfd_lead_distance_setting_uses_detected_range(self):
assert get_canfd_lead_distance_setting(None, 2) == 2
assert get_canfd_lead_distance_setting(0.0, 2) == 2
assert get_canfd_lead_distance_setting(12.0, 3) == 1
assert get_canfd_lead_distance_setting(24.0, 1) == 2
assert get_canfd_lead_distance_setting(38.0, 1) == 3
def test_canfd_scc_lead_state_prefers_openpilot_lead_distance(self):
CP = CarParams.new_message()
CP.carFingerprint = CAR.KIA_EV6
@@ -551,12 +524,11 @@ class TestHyundaiFingerprint:
stock_camera_lead_rel_speed=0.0,
)
lead_visible, lead_distance, lead_rel_speed, distance_setting = controller._get_canfd_scc_lead_state(cc, cs, now_nanos=1_000_000_000)
lead_visible, lead_distance, lead_rel_speed = controller._get_canfd_scc_lead_state(cc, cs, now_nanos=1_000_000_000)
assert lead_visible
assert lead_distance == pytest.approx(37.5)
assert lead_rel_speed == pytest.approx(-1.3)
assert distance_setting == 3
def test_canfd_scc_lead_state_falls_back_to_hud_lead_when_no_distance_available(self):
CP = CarParams.new_message()
@@ -575,12 +547,11 @@ class TestHyundaiFingerprint:
stock_camera_lead_rel_speed=0.0,
)
lead_visible, lead_distance, lead_rel_speed, distance_setting = controller._get_canfd_scc_lead_state(cc, cs, now_nanos=1_000_000_000)
lead_visible, lead_distance, lead_rel_speed = controller._get_canfd_scc_lead_state(cc, cs, now_nanos=1_000_000_000)
assert lead_visible
assert lead_distance == pytest.approx(20.0)
assert lead_rel_speed == pytest.approx(0.0)
assert distance_setting == 2
def test_can_acc_commands_use_default_values(self):
CP = CarParams.new_message()