cleanups / BUMP

This commit is contained in:
firestar5683
2026-07-14 22:03:41 -05:00
parent 73485ba76c
commit eb7d4212fd
16 changed files with 449 additions and 26 deletions
+2
View File
@@ -258,6 +258,8 @@ MIGRATION = {
"KIA EV6 2025": HYUNDAI.KIA_EV6_2025,
"KIA EV9 2025": HYUNDAI.KIA_EV9,
"KIA CARNIVAL 4TH GEN": HYUNDAI.KIA_CARNIVAL_4TH_GEN,
"KIA CARNIVAL 2025": HYUNDAI.KIA_CARNIVAL_2025,
"KIA CARNIVAL HYBRID 4TH GEN": HYUNDAI.KIA_CARNIVAL_HEV_4TH_GEN,
"GENESIS GV60 ELECTRIC 1ST GEN": HYUNDAI.GENESIS_GV60_EV_1ST_GEN,
"GENESIS G70 2018": HYUNDAI.GENESIS_G70,
"GENESIS G70 2020": HYUNDAI.GENESIS_G70_2020,
@@ -1512,6 +1512,25 @@ FW_VERSIONS = {
b'\xf1\x00KA4c SCC FHCUP 1.00 1.01 99110-I4000 ',
],
},
CAR.KIA_CARNIVAL_2025: {
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00KA4 MFC AT USA LHD 1.00 1.05 99210-R0500 240305',
],
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00KA4_ SCC FHCUP 1.00 1.01 99110-R0510 ',
b'\xf1\x00KA4_ RDR ----- 1.00 1.01 99110-R0510 ',
],
},
CAR.KIA_CARNIVAL_HEV_4TH_GEN: {
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00KA4HMFC AT USA LHD 1.00 1.05 99210-R0500 240305',
b'\xf1\x00KA4HMFC AT KOR LHD 1.00 1.00 99210-R0600 240924',
b'\xf1\x00KA4HMFC AT USA LHD 1.00 1.00 99210-R0700 250324',
],
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00KAhe RDR ----- 1.00 1.01 99110-ES500 ',
],
},
CAR.KIA_K8_HEV_1ST_GEN: {
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00GL3HMFC AT KOR LHD 1.00 1.03 99211-L8000 210907',
@@ -106,8 +106,8 @@ class CarInterface(CarInterfaceBase):
ret.enableBsm = 0x1ba in fingerprint[CAN.ECAN]
# Check if the car is hybrid. Only HEV/PHEV cars have 0xFA on E-CAN.
if 0xFA in fingerprint[CAN.ECAN]:
# Carnival HEV can fingerprint with too little E-CAN traffic to see 0xFA.
if 0xFA in fingerprint[CAN.ECAN] or candidate == CAR.KIA_CARNIVAL_HEV_4TH_GEN:
ret.flags |= HyundaiFlags.HYBRID.value
if lka_steering:
@@ -90,6 +90,8 @@ CCNC_NON_HDA2_CARS = (
CAR.HYUNDAI_SANTA_CRUZ_2025,
CAR.KIA_K4_2025,
CAR.KIA_K5_2025,
CAR.KIA_CARNIVAL_2025,
CAR.KIA_CARNIVAL_HEV_4TH_GEN,
CAR.KIA_SPORTAGE_2026,
CAR.KIA_SORENTO_2024,
)
@@ -332,17 +334,24 @@ class TestHyundaiFingerprint:
assert ev9_cp.steerAtStandstill
assert not sportage_cp.steerAtStandstill
def test_ccnc_hda2_lka_layout_does_not_set_ccnc_safety_param(self):
@pytest.mark.parametrize("candidate", (CAR.KIA_K4_2025, CAR.KIA_CARNIVAL_2025, CAR.KIA_CARNIVAL_HEV_4TH_GEN))
def test_ccnc_hda2_lka_layout_does_not_set_ccnc_safety_param(self, candidate):
fingerprint = gen_empty_fingerprint()
cam_can = CanBus(None, fingerprint).CAM
fingerprint[cam_can] = {0x50: 16}
CP = CarInterface.get_params(CAR.KIA_K4_2025, fingerprint, [], False, False, False, None)
CP = CarInterface.get_params(candidate, fingerprint, [], False, False, False, None)
assert CP.flags & HyundaiFlags.CCNC
assert CP.flags & HyundaiFlags.CANFD_LKA_STEERING
assert not (CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CCNC)
def test_carnival_hev_sets_hybrid_gas_safety(self):
CP = CarInterface.get_params(CAR.KIA_CARNIVAL_HEV_4TH_GEN, gen_empty_fingerprint(), [], False, False, False, None)
assert CP.flags & HyundaiFlags.HYBRID
assert CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.HYBRID_GAS
def test_ioniq_6_hda1_layout_stays_non_lka(self):
fingerprint = gen_empty_fingerprint()
fingerprint[1] = {0x100: 8, 0x110: 8}
+20 -2
View File
@@ -788,6 +788,24 @@ class CAR(Platforms):
CarSpecs(mass=2087, wheelbase=3.09, steerRatio=14.23),
flags=HyundaiFlags.RADAR_SCC,
)
KIA_CARNIVAL_2025 = HyundaiCanFDPlatformConfig(
[
HyundaiCarDocs("Kia Carnival 2025", car_parts=CarParts.common([CarHarness.hyundai_k])),
HyundaiCarDocs("Kia Carnival (with HDA II) 2025", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_q])),
],
KIA_CARNIVAL_4TH_GEN.specs,
flags=HyundaiFlags.CCNC,
)
KIA_CARNIVAL_HEV_4TH_GEN = HyundaiCanFDPlatformConfig(
[
HyundaiCarDocs("Kia Carnival Hybrid 2025", car_parts=CarParts.common([CarHarness.hyundai_k])),
HyundaiCarDocs("Kia Carnival Hybrid 2026", car_parts=CarParts.common([CarHarness.hyundai_a])),
HyundaiCarDocs("Kia Carnival Hybrid (with HDA II) 2025-26", "Highway Driving Assist II",
car_parts=CarParts.common([CarHarness.hyundai_q])),
],
CarSpecs(mass=2253, wheelbase=3.09, steerRatio=14.23),
flags=HyundaiFlags.CCNC,
)
# Genesis
GENESIS_GV60_EV_1ST_GEN = HyundaiCanFDPlatformConfig(
@@ -1051,8 +1069,8 @@ PART_NUMBER_FW_PATTERN = re.compile(b'(?<=[0-9][.,][0-9]{2} )([0-9]{5}[-/]?[A-Z]
# We've seen both ICE and hybrid for these platforms, and they have hybrid descriptors (e.g. MQ4 vs MQ4H)
CANFD_FUZZY_WHITELIST = {CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN, CAR.KIA_K8_HEV_1ST_GEN,
# TODO: the hybrid variant is not out yet
CAR.KIA_CARNIVAL_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN_LFA2}
CAR.KIA_CARNIVAL_4TH_GEN, CAR.KIA_CARNIVAL_2025, CAR.KIA_CARNIVAL_HEV_4TH_GEN,
CAR.KIA_SORENTO_HEV_4TH_GEN_LFA2}
# List of ECUs expected to have platform codes, camera and radar should exist on all cars
# TODO: use abs, it has the platform code and part number on many platforms
+2
View File
@@ -36,6 +36,7 @@ non_tested_cars = [
HYUNDAI.HYUNDAI_TUCSON_PHEV_2025,
HYUNDAI.KIA_EV6_2025,
HYUNDAI.KIA_EV9,
HYUNDAI.KIA_CARNIVAL_2025,
HYUNDAI.KIA_SPORTAGE_2026,
HYUNDAI.KIA_SORENTO_2024,
HYUNDAI.KIA_SORENTO_HEV_4TH_GEN_LFA2,
@@ -164,6 +165,7 @@ routes = [
CarTestRoute("656ac0d830792fcc/2021-12-28--14-45-56", HYUNDAI.HYUNDAI_SANTA_FE_PHEV_2022, segment=1),
CarTestRoute("de59124955b921d8/2023-06-24--00-12-50", HYUNDAI.KIA_CARNIVAL_4TH_GEN),
CarTestRoute("409c9409979a8abc/2023-07-11--09-06-44", HYUNDAI.KIA_CARNIVAL_4TH_GEN), # Chinese model
CarTestRoute("6c0069dcd5bbb6c1/00000020--6b95507969", HYUNDAI.KIA_CARNIVAL_HEV_4TH_GEN), # HDA II
CarTestRoute("e0e98335f3ebc58f/2021-03-07--16-38-29", HYUNDAI.KIA_CEED),
CarTestRoute("22703002ddbe2a08/0000000e--083b76c42c", HYUNDAI.KIA_XCEED_PHEV, segment=0),
CarTestRoute("7653b2bce7bcfdaa/2020-03-04--15-34-32", HYUNDAI.KIA_OPTIMA_G4),
@@ -90,6 +90,8 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
"KIA_NIRO_EV_2ND_GEN" = [2.05, 2.5, 0.14]
"GENESIS_GV80" = [2.5, 2.5, 0.1]
"KIA_CARNIVAL_4TH_GEN" = [1.75, 1.75, 0.15]
"KIA_CARNIVAL_2025" = [1.75, 1.75, 0.15]
"KIA_CARNIVAL_HEV_4TH_GEN" = [1.75, 1.75, 0.15]
"GMC_ACADIA" = [1.6, 1.6, 0.2]
"LEXUS_IS_TSS2" = [2.0, 2.0, 0.1]
"HYUNDAI_KONA_EV_2ND_GEN" = [2.5, 2.5, 0.1]
@@ -167,6 +167,16 @@ VISION_UNTRACKED_SLOW_LEAD_CONFIRM_TIME = 0.30
VISION_UNTRACKED_SLOW_LEAD_IMMEDIATE_DECEL = 0.55
VISION_UNTRACKED_SLOW_LEAD_IMMEDIATE_DISTANCE = 45.0
VISION_UNTRACKED_SLOW_LEAD_IMMEDIATE_LEAD_BRAKE = 0.10
VISION_UNTRACKED_APPROACH_LIFT_MIN_EGO_SPEED = 18.0
VISION_UNTRACKED_APPROACH_LIFT_MIN_MODEL_PROB = 0.95
VISION_UNTRACKED_APPROACH_LIFT_MAX_LATERAL_OFFSET = 1.2
VISION_UNTRACKED_APPROACH_LIFT_MIN_CLOSING_SPEED = 0.75
VISION_UNTRACKED_APPROACH_LIFT_MAX_DISTANCE = 130.0
VISION_UNTRACKED_APPROACH_LIFT_MIN_GAP_EXCESS = 6.0
VISION_UNTRACKED_APPROACH_LIFT_TRIGGER_TIME = 20.0
VISION_UNTRACKED_APPROACH_LIFT_FULL_TIME = 6.0
VISION_UNTRACKED_APPROACH_LIFT_MAX_ACCEL = 0.22
VISION_UNTRACKED_APPROACH_LIFT_CONFIRM_TIME = 0.30
VISION_SLOW_LEAD_MAX_SPEED = 5.0
VISION_SLOW_LEAD_MIN_CLOSING_SPEED = 1.5
VISION_SLOW_LEAD_TRIGGER_TTC = 4.5
@@ -368,6 +378,16 @@ MILD_FOLLOW_ZERO_CROSS_GUARD_FULL_HEADWAY_MARGIN = 0.85
MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_DELTA_A = 0.08
MILD_FOLLOW_ZERO_CROSS_GUARD_MIN_DEADBAND = 0.04
MILD_FOLLOW_ZERO_CROSS_GUARD_MAX_DEADBAND = 0.08
FAR_OPENING_RADAR_BRAKE_GUARD_MIN_SPEED = 20.0
FAR_OPENING_RADAR_BRAKE_GUARD_MIN_DISTANCE = 80.0
FAR_OPENING_RADAR_BRAKE_GUARD_MIN_MODEL_PROB = 0.85
FAR_OPENING_RADAR_BRAKE_GUARD_MIN_LEAD_DELTA = 1.0
FAR_OPENING_RADAR_BRAKE_GUARD_MIN_HEADWAY_MARGIN = 0.75
FAR_OPENING_RADAR_BRAKE_GUARD_MAX_LEAD_BRAKE = 0.25
FAR_OPENING_RADAR_BRAKE_GUARD_MIN_PREV_TARGET = -0.08
FAR_OPENING_RADAR_BRAKE_GUARD_MAX_NEW_TARGET = -0.15
FAR_OPENING_RADAR_BRAKE_GUARD_MAX_CRUISE_DEFICIT = 0.25
FAR_OPENING_RADAR_BRAKE_GUARD_MIN_MODEL_ACCEL = -0.50
NEAR_DUPLICATE_LEAD_TRANSITION_MIN_SPEED = 20.0
NEAR_DUPLICATE_LEAD_TRANSITION_MIN_MODEL_PROB = 0.95
NEAR_DUPLICATE_LEAD_TRANSITION_MAX_LEAD_BRAKE = 0.35
@@ -605,6 +625,7 @@ class LongitudinalPlanner:
self.vision_low_speed_stop_hold_until = 0.0
self.vision_lead_approach_confirm_t = 0.0
self.untracked_slow_lead_confirm_t = 0.0
self.untracked_vision_approach_lift_confirm_t = 0.0
self.manual_stop_resume_override_until = 0.0
self.lead_depart_accel_hold_until = 0.0
self.lead_depart_accel_hold_floor = None
@@ -882,6 +903,43 @@ class LongitudinalPlanner:
return max(accel_min, -approach_decel)
@staticmethod
def get_vision_untracked_approach_lift_cap(lead, v_ego, t_follow):
"""Trim throttle before a confident vision lead reaches the tracking window."""
if lead is None or not lead.status or bool(getattr(lead, "radar", False)):
return None
if float(v_ego) < VISION_UNTRACKED_APPROACH_LIFT_MIN_EGO_SPEED:
return None
lead_prob = float(getattr(lead, "modelProb", 0.0))
if lead_prob < VISION_UNTRACKED_APPROACH_LIFT_MIN_MODEL_PROB:
return None
if abs(float(getattr(lead, "yRel", 0.0))) > VISION_UNTRACKED_APPROACH_LIFT_MAX_LATERAL_OFFSET:
return None
if float(lead.dRel) > VISION_UNTRACKED_APPROACH_LIFT_MAX_DISTANCE:
return None
closing_speed = float(v_ego) - float(lead.vLead)
if closing_speed < VISION_UNTRACKED_APPROACH_LIFT_MIN_CLOSING_SPEED:
return None
desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow))
gap_excess = float(lead.dRel) - desired_gap
if gap_excess < VISION_UNTRACKED_APPROACH_LIFT_MIN_GAP_EXCESS:
return 0.0
time_to_desired_gap = gap_excess / max(closing_speed, 0.1)
if time_to_desired_gap > VISION_UNTRACKED_APPROACH_LIFT_TRIGGER_TIME:
return None
# This path only removes positive acceleration. Braking remains exclusively
# owned by lead tracking and the existing close-lead safety caps.
return float(np.interp(
time_to_desired_gap,
[VISION_UNTRACKED_APPROACH_LIFT_FULL_TIME, VISION_UNTRACKED_APPROACH_LIFT_TRIGGER_TIME],
[0.0, VISION_UNTRACKED_APPROACH_LIFT_MAX_ACCEL],
))
def get_vision_slow_stopped_lead_cap(self, lead, v_ego, accel_min, t_follow):
if lead is None or not lead.status or bool(getattr(lead, "radar", False)):
return None
@@ -2116,6 +2174,46 @@ class LongitudinalPlanner:
return None
@staticmethod
def get_far_opening_radar_brake_guard_target(lead, v_ego, base_t_follow,
prev_output_a_target, output_a_target,
v_cruise, model_desired_accel,
current_source, tracking_lead_active):
if lead is None or not lead.status or not bool(getattr(lead, "radar", False)):
return None
if current_source != "cruise" or not tracking_lead_active:
return None
if float(v_ego) < FAR_OPENING_RADAR_BRAKE_GUARD_MIN_SPEED:
return None
if float(lead.dRel) < FAR_OPENING_RADAR_BRAKE_GUARD_MIN_DISTANCE:
return None
lead_prob = float(getattr(lead, "modelProb", 1.0))
lead_delta = float(lead.vLead) - float(v_ego)
lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3)
headway_margin = actual_headway - float(base_t_follow)
if (
lead_prob < FAR_OPENING_RADAR_BRAKE_GUARD_MIN_MODEL_PROB or
lead_delta < FAR_OPENING_RADAR_BRAKE_GUARD_MIN_LEAD_DELTA or
lead_brake > FAR_OPENING_RADAR_BRAKE_GUARD_MAX_LEAD_BRAKE or
headway_margin < FAR_OPENING_RADAR_BRAKE_GUARD_MIN_HEADWAY_MARGIN
):
return None
if float(prev_output_a_target) < FAR_OPENING_RADAR_BRAKE_GUARD_MIN_PREV_TARGET:
return None
if float(output_a_target) > FAR_OPENING_RADAR_BRAKE_GUARD_MAX_NEW_TARGET:
return None
if float(v_cruise) < float(v_ego) - FAR_OPENING_RADAR_BRAKE_GUARD_MAX_CRUISE_DEFICIT:
return None
if float(model_desired_accel) < FAR_OPENING_RADAR_BRAKE_GUARD_MIN_MODEL_ACCEL:
return None
# A far lead that is pulling away cannot justify a one-frame braking pulse.
# Preserve real speed-target, model, and closing-lead deceleration above.
return 0.0
def get_duplicate_slow_lead_brake_hold_target(self, lead, v_ego, base_t_follow,
prev_output_a_target, output_a_target,
current_source, tracking_lead_active):
@@ -2651,6 +2749,24 @@ class LongitudinalPlanner:
model_desired_accel = float(sm['modelV2'].action.desiredAcceleration)
if not tracking_lead:
approach_lift_caps = [
cap for cap in (
self.get_vision_untracked_approach_lift_cap(self.lead_one, v_ego, effective_t_follow),
self.get_vision_untracked_approach_lift_cap(self.lead_two, v_ego, effective_t_follow),
) if cap is not None
]
if approach_lift_caps:
self.untracked_vision_approach_lift_confirm_t = min(
self.untracked_vision_approach_lift_confirm_t + self.dt,
VISION_UNTRACKED_APPROACH_LIFT_CONFIRM_TIME,
)
if self.untracked_vision_approach_lift_confirm_t >= VISION_UNTRACKED_APPROACH_LIFT_CONFIRM_TIME:
approach_lift_cap = min(approach_lift_caps)
self.a_desired = min(self.a_desired, approach_lift_cap)
output_a_target = min(output_a_target, approach_lift_cap)
else:
self.untracked_vision_approach_lift_confirm_t = 0.0
pretracking_vision_caps = []
for lead in (self.lead_one, self.lead_two):
if lead.status and not bool(getattr(lead, "radar", False)):
@@ -2682,6 +2798,7 @@ class LongitudinalPlanner:
else:
self.untracked_slow_lead_confirm_t = 0.0
else:
self.untracked_vision_approach_lift_confirm_t = 0.0
self.untracked_slow_lead_confirm_t = 0.0
close_lead_caps = []
@@ -3128,6 +3245,21 @@ class LongitudinalPlanner:
if mild_follow_zero_cross_guard_target is not None:
output_a_target = mild_follow_zero_cross_guard_target
far_opening_radar_brake_guard_target = self.get_far_opening_radar_brake_guard_target(
comfort_follow_lead,
scene_v_ego,
effective_t_follow,
prev_output_a_target,
output_a_target,
v_cruise,
model_desired_accel,
self.mpc.source,
tracking_lead,
)
if far_opening_radar_brake_guard_target is not None:
self.a_desired = max(self.a_desired, far_opening_radar_brake_guard_target)
output_a_target = max(output_a_target, far_opening_radar_brake_guard_target)
output_accel_max = no_throttle_output_max if not self.allow_throttle else accel_limits_turns[1]
output_a_target = float(np.clip(output_a_target, output_accel_min, output_accel_max))
@@ -382,6 +382,62 @@ def test_vision_untracked_slow_lead_cap_keeps_low_confidence_floor_for_less_thre
assert planner.get_vision_untracked_slow_lead_cap(less_threatening_lead, v_ego, -1.0) is None
def test_vision_untracked_approach_lift_eases_throttle_without_braking():
v_ego = 30.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead = make_lead(status=True, d_rel=110.0, v_lead=27.0, radar=False, model_prob=0.98)
aggressive_cap = planner.get_vision_untracked_approach_lift_cap(lead, v_ego, 1.2)
standard_cap = planner.get_vision_untracked_approach_lift_cap(lead, v_ego, 1.4)
assert aggressive_cap is not None
assert standard_cap is not None
assert 0.0 <= standard_cap <= aggressive_cap < 0.22
@pytest.mark.parametrize("lead", [
make_lead(status=True, d_rel=110.0, v_lead=27.0, radar=True, model_prob=0.98),
make_lead(status=True, d_rel=110.0, v_lead=27.0, radar=False, model_prob=0.90),
make_lead(status=True, d_rel=110.0, v_lead=27.0, radar=False, model_prob=0.98, y_rel=1.5),
make_lead(status=True, d_rel=110.0, v_lead=30.0, radar=False, model_prob=0.98),
])
def test_vision_untracked_approach_lift_ignores_unqualified_leads(lead):
v_ego = 30.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
assert planner.get_vision_untracked_approach_lift_cap(lead, v_ego, 1.4) is None
def test_far_opening_radar_brake_guard_removes_only_harmless_pulse():
v_ego = 28.9
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
opening_lead = make_lead(status=True, d_rel=96.5, v_lead=32.0, a_lead=-0.15, radar=True, model_prob=0.93)
guard_target = planner.get_far_opening_radar_brake_guard_target(
opening_lead, v_ego, 1.25, 0.0, -0.41, 29.0, 0.0, "cruise", True,
)
assert guard_target == 0.0
def test_far_opening_radar_brake_guard_preserves_close_or_requested_braking():
v_ego = 28.9
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
close_slower_lead = make_lead(status=True, d_rel=36.9, v_lead=20.9, a_lead=-0.18, radar=True, model_prob=0.99)
far_opening_lead = make_lead(status=True, d_rel=96.5, v_lead=32.0, a_lead=-0.15, radar=True, model_prob=0.93)
assert planner.get_far_opening_radar_brake_guard_target(
close_slower_lead, v_ego, 1.25, 0.0, -3.5, 29.0, 0.0, "lead0", True,
) is None
assert planner.get_far_opening_radar_brake_guard_target(
far_opening_lead, v_ego, 1.25, 0.0, -0.41, 27.0, 0.0, "cruise", True,
) is None
def test_vision_slow_stopped_lead_cap_brakes_earlier_for_confident_stop():
v_ego = 13.207
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
@@ -3,6 +3,8 @@ import datetime
import pytest
from openpilot.common.constants import CV
from openpilot.common.realtime import DT_MDL
from openpilot.starpilot.controls.lib.curve_speed_controller import CSC_MAX_DECEL_RATE, CurveSpeedController
from openpilot.starpilot.controls.lib.starpilot_vcruise import (
StarPilotVCruise,
get_active_slc_control_target,
@@ -148,6 +150,41 @@ def test_curve_speed_controller_releases_immediately_when_disabled():
assert not vcruise.csc_controlling_speed
def test_curve_speed_controller_ramps_toward_curve_speed_at_bounded_rate():
planner = SimpleNamespace(
params=FakeParams(),
road_curvature=0.004,
time_to_curve=2.0,
starpilot_weather=SimpleNamespace(weather_id=0, reduce_lateral_acceleration=0.0),
)
controller = CurveSpeedController(SimpleNamespace(starpilot_planner=planner))
controller.lateral_acceleration = 2.0
controller.target_set = True
controller.target = 30.0
controller.update_target(30.0)
assert controller.target == pytest.approx(30.0 - CSC_MAX_DECEL_RATE * DT_MDL)
assert controller.target > (controller.lateral_acceleration / planner.road_curvature) ** 0.5
def test_curve_speed_controller_does_not_slow_for_curve_speed_above_ego():
planner = SimpleNamespace(
params=FakeParams(),
road_curvature=0.001,
time_to_curve=2.0,
starpilot_weather=SimpleNamespace(weather_id=0, reduce_lateral_acceleration=0.0),
)
controller = CurveSpeedController(SimpleNamespace(starpilot_planner=planner))
controller.lateral_acceleration = 2.0
controller.target_set = True
controller.target = 28.0
controller.update_target(30.0)
assert controller.target == pytest.approx(30.0)
def test_active_slc_control_target_applies_offset_and_cluster_diff():
target = get_active_slc_control_target(
speed_limit_controller=True,
+3 -4
View File
@@ -5,8 +5,7 @@ from enum import IntEnum
from openpilot.common.params import Params
from openpilot.selfdrive.ui.widgets.offroad_alerts import UpdateAlert, OffroadAlert
from openpilot.selfdrive.ui.widgets.exp_mode_button import ExperimentalModeButton
from openpilot.selfdrive.ui.widgets.prime import PrimeWidget
from openpilot.selfdrive.ui.widgets.setup import SetupWidget
from openpilot.selfdrive.ui.widgets.setup import SetupWidget, StarPilotLogoWidget
from openpilot.selfdrive.ui.lib.starpilot_version import starpilot_display_description
from openpilot.system.ui.lib.text_measure import measure_text_cached
from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos
@@ -56,7 +55,7 @@ class HomeLayout(Widget):
self.update_notif_rect = rl.Rectangle(0, 0, 200, HEADER_HEIGHT - 10)
self.alert_notif_rect = rl.Rectangle(0, 0, 220, HEADER_HEIGHT - 10)
self._prime_widget = PrimeWidget()
self._starpilot_logo_widget = StarPilotLogoWidget()
self._setup_widget = SetupWidget()
self._exp_mode_button = ExperimentalModeButton()
@@ -191,7 +190,7 @@ class HomeLayout(Widget):
self.offroad_alert.render(self.content_rect)
def _render_left_column(self):
self._prime_widget.render(self.left_column_rect)
self._starpilot_logo_widget.render(self.left_column_rect)
def _render_right_column(self):
exp_height = 125
+1 -1
View File
@@ -1,4 +1,4 @@
STARPILOT_DISPLAY_VERSION = "6.7.1"
STARPILOT_DISPLAY_VERSION = "6.7.2"
def starpilot_display_description(description: str | None) -> str:
+16 -6
View File
@@ -14,12 +14,26 @@ LOGO_WIDTH = 750
LOGO_HEIGHT = 770
class StarPilotLogoWidget(Widget):
def __init__(self):
super().__init__()
self._logo_texture = gui_app.texture("images/StarPilotLogo.png", LOGO_WIDTH, LOGO_HEIGHT)
def _render(self, rect: rl.Rectangle):
scale = min(1.0, rect.width / self._logo_texture.width, rect.height / self._logo_texture.height)
width = self._logo_texture.width * scale
height = self._logo_texture.height * scale
x = rect.x + (rect.width - width) / 2
y = rect.y + (rect.height - height) / 2
rl.draw_texture_ex(self._logo_texture, rl.Vector2(x, y), 0.0, scale, rl.WHITE)
class SetupWidget(Widget):
def __init__(self):
super().__init__()
self._pairing_dialog: PairingDialog | None = None
self._pair_device_btn = Button(lambda: tr("Pair device"), self._show_pairing, button_style=ButtonStyle.PRIMARY)
self._logo_texture = gui_app.texture("images/StarPilotLogo.png", LOGO_WIDTH, LOGO_HEIGHT)
self._logo_widget = StarPilotLogoWidget()
def _render(self, rect: rl.Rectangle):
if not ui_state.prime_state.is_paired():
@@ -53,11 +67,7 @@ class SetupWidget(Widget):
self._pair_device_btn.render(button_rect)
def _render_logo(self, rect: rl.Rectangle):
tex_w = self._logo_texture.width
tex_h = self._logo_texture.height
x = rect.x + (rect.width - tex_w) / 2
y = rect.y + (rect.height - tex_h) / 2
rl.draw_texture(self._logo_texture, int(x), int(y), rl.WHITE)
self._logo_widget.render(rect)
def _show_pairing(self):
if not system_time_valid():
@@ -8,6 +8,7 @@ from openpilot.starpilot.common.starpilot_variables import CITY_SPEED_LIMIT, CRU
CALIBRATION_PROGRESS_THRESHOLD = 10 / DT_MDL
CSC_MIN_SPEED = CITY_SPEED_LIMIT * CV.MPH_TO_MS
CSC_MAX_DECEL_RATE = 1.5
MAX_CURVATURE = 0.1
MIN_CURVATURE = 0.001
PERCENTILE = 90
@@ -139,10 +140,13 @@ class CurveSpeedController:
if self.target_set:
csc_speed = (lateral_acceleration / abs(self.starpilot_planner.road_curvature))**0.5
decel_rate = (v_ego - csc_speed) / self.starpilot_planner.time_to_curve
self.target -= decel_rate * DT_MDL
self.target = float(np.clip(self.target, CSC_MIN_SPEED, csc_speed))
csc_speed = max(float(csc_speed), CSC_MIN_SPEED)
if csc_speed >= v_ego:
self.target = v_ego
else:
time_to_curve = max(float(self.starpilot_planner.time_to_curve), DT_MDL)
decel_rate = float(np.clip((v_ego - csc_speed) / time_to_curve, 0.0, CSC_MAX_DECEL_RATE))
self.target = float(np.clip(self.target - decel_rate * DT_MDL, csc_speed, v_ego))
else:
self.target_set = True
self.target = v_ego
+106 -5
View File
@@ -44,6 +44,7 @@ FLM_STATUS_MAX_AGE_SECONDS = 3600.0
FLM_ANALYZER_ROUTE_LIMIT = 8
FLM_ANALYZER_PROCESS = None
FLM_ANALYZER_LOCK = threading.Lock()
FLM_PROGRESS_FILENAME = "progress.json"
TRIAL_PARAM_SPECS = {
"AdvancedLateralTune": "bool",
@@ -277,6 +278,56 @@ def _write_json(path: Path, payload) -> None:
tmp_path.replace(path)
def _progress_path() -> Path:
return get_flm_workspace_root() / FLM_PROGRESS_FILENAME
def _record_cleanup_progress(car_fingerprint: str, source_report_id: str = "") -> None:
fingerprint = str(car_fingerprint or "").strip()
if not fingerprint:
return
progress = _read_json(_progress_path(), {})
if not isinstance(progress, dict):
progress = {}
vehicles = progress.get("vehicles", {})
if not isinstance(vehicles, dict):
vehicles = {}
vehicles[fingerprint] = {
"minimumPathKey": "cleanup_pass",
"sourceReportId": str(source_report_id or ""),
"updatedAt": time.time(),
}
_write_json(_progress_path(), {"version": 1, "vehicles": vehicles})
def _cleanup_progress_locked(car_fingerprint: str) -> bool:
fingerprint = str(car_fingerprint or "").strip()
if not fingerprint:
return False
progress = _read_json(_progress_path(), {})
vehicle_progress = progress.get("vehicles", {}).get(fingerprint, {}) if isinstance(progress, dict) else {}
if isinstance(vehicle_progress, dict) and vehicle_progress.get("minimumPathKey") == "cleanup_pass":
return True
# Bootstrap workspaces created before progression tracking was added.
for path in _workspace_paths()["reports"].glob("*.json"):
report = _read_json(path, {})
if not isinstance(report, dict):
continue
car_info = report.get("car", {})
if (
isinstance(car_info, dict)
and str(car_info.get("carFingerprint", "")) == fingerprint
and car_info.get("controlPath") == "torque"
and report.get("primaryPathKey") == "cleanup_pass"
):
_record_cleanup_progress(fingerprint, str(report.get("reportId", path.stem)))
return True
return False
def _worker_env(repo_root: Path) -> dict[str, str]:
env = os.environ.copy()
pythonpath = [
@@ -1592,7 +1643,7 @@ def _bucket_tuning_family(bucket: str) -> str:
return "other"
def select_primary_tuning_path(summaries: list[dict[str, Any]], summary_stats: dict[str, Any]) -> dict[str, Any]:
def _select_primary_tuning_path_unlocked(summaries: list[dict[str, Any]], summary_stats: dict[str, Any]) -> dict[str, Any]:
actionable = [
summary for summary in summaries
if summary.get("bucket") not in ("model_limited", "angle_control_diagnostic")
@@ -1669,6 +1720,34 @@ def select_primary_tuning_path(summaries: list[dict[str, Any]], summary_stats: d
}
def select_primary_tuning_path(summaries: list[dict[str, Any]], summary_stats: dict[str, Any],
cleanup_progress_locked: bool = False) -> dict[str, Any]:
decision = _select_primary_tuning_path_unlocked(summaries, summary_stats)
if not cleanup_progress_locked:
return decision
raw_primary_path = decision["primaryPathKey"]
if raw_primary_path == "baseline_fix":
return {
**decision,
"primaryPathKey": "cleanup_pass",
"alternatePathKey": "baseline_fix",
"reason": (
"This vehicle already progressed to Cleanup Pass. This route contains broader misses, but FLM will not automatically "
"reset a tune that already reached fine adjustment. Review Baseline Fix manually if the regression is real and repeatable."
),
"rawPrimaryPathKey": raw_primary_path,
"automaticBaselineDemotionBlocked": True,
"cleanupProgressLocked": True,
}
return {
**decision,
"rawPrimaryPathKey": raw_primary_path,
"cleanupProgressLocked": True,
}
def build_trial_profiles(report_id: str, suggestions: list[dict[str, Any]], feedback: dict[str, Any], capabilities: dict[str, Any],
path_key: str = "cleanup_pass", path_label: str = "Cleanup Pass") -> list[dict[str, Any]]:
ignored = set(str(item) for item in feedback.get("ignoredDimensions", []))
@@ -1738,8 +1817,8 @@ def _add_parameters_start_here(capabilities: dict[str, Any], suggestions: list[d
def build_recommendation_paths(report_id: str, summaries: list[dict[str, Any]], summary_stats: dict[str, Any],
capabilities: dict[str, Any], current: dict[str, Any],
feedback: dict[str, Any]) -> tuple[list[dict[str, Any]], dict[str, Any]]:
decision = select_primary_tuning_path(summaries, summary_stats)
feedback: dict[str, Any], cleanup_progress_locked: bool = False) -> tuple[list[dict[str, Any]], dict[str, Any]]:
decision = select_primary_tuning_path(summaries, summary_stats, cleanup_progress_locked)
all_suggestions = {
"baseline_fix": build_suggestions(summaries, capabilities, current, strategy="baseline"),
"cleanup_pass": build_suggestions(summaries, capabilities, current, strategy="cleanup"),
@@ -1930,11 +2009,21 @@ def analyze_routes(route_names: list[str], footage_paths: list[str], feedback: d
capabilities["nonlinearTorqueMap"] = _nonlinear_torque_map(car_params)
current_params = _current_param_state(car_params, params)
stock_params = _stock_param_state(car_params, capabilities)
car_fingerprint = str(car_params.carFingerprint)
if torque_control:
raw_summaries, summary_stats = classify_torque_samples(all_samples)
summaries = _resolve_conflicting_actionable_suggestions(raw_summaries)
paths_payload, path_decision = build_recommendation_paths(report_id, summaries, summary_stats, capabilities, current_params, feedback)
cleanup_progress_locked = _cleanup_progress_locked(car_fingerprint)
paths_payload, path_decision = build_recommendation_paths(
report_id,
summaries,
summary_stats,
capabilities,
current_params,
feedback,
cleanup_progress_locked=cleanup_progress_locked,
)
primary_path = next((path for path in paths_payload if path.get("isPrimary")), paths_payload[0] if paths_payload else {})
suggestions = list(primary_path.get("suggestions", []))
profiles = [profile for path in paths_payload for profile in path.get("profiles", [])]
@@ -1991,7 +2080,7 @@ def analyze_routes(route_names: list[str], footage_paths: list[str], feedback: d
"warnings": warnings,
"feedback": feedback,
"car": {
"carFingerprint": str(car_params.carFingerprint),
"carFingerprint": car_fingerprint,
"brand": str(getattr(car_params, "brand", "") or ""),
"controlPath": "torque" if torque_control else "angle",
"gitBranch": init_data.get("gitBranch", ""),
@@ -2025,6 +2114,8 @@ def analyze_routes(route_names: list[str], footage_paths: list[str], feedback: d
(paths["reports"] / f"{report_id}.html").write_text(html, encoding="utf-8")
_write_json(paths["reports"] / f"{report_id}.json", report)
_write_json(paths["profiles"] / f"{report_id}.json", profiles)
if torque_control and path_decision["primaryPathKey"] == "cleanup_pass":
_record_cleanup_progress(car_fingerprint, report_id)
_write_flm_status({
"pid": os.getpid(),
"startedAt": time.time(),
@@ -2068,6 +2159,8 @@ def select_report_path(report_id: str, path_key: str) -> dict[str, Any]:
report.pop("html", None)
(paths["reports"] / f"{report_id}.html").write_text(_render_report_html(report), encoding="utf-8")
_write_json(paths["reports"] / f"{report_id}.json", report)
if path_key == "cleanup_pass" and report.get("car", {}).get("controlPath") == "torque":
_record_cleanup_progress(str(report.get("car", {}).get("carFingerprint", "")), report_id)
return {
"message": f"Using {selected_path.get('title', path_key)} for this report.",
"report": load_report(report_id),
@@ -2208,6 +2301,11 @@ def clear_workspace() -> dict[str, Any]:
path.unlink()
removed.append(str(path))
progress_path = _progress_path()
if progress_path.is_file():
progress_path.unlink()
removed.append(str(progress_path))
_clear_persistent_trial_baseline(params)
_clear_flm_status()
@@ -2453,6 +2551,9 @@ def apply_trial_profile(report_id: str, profile_id: str) -> dict[str, Any]:
)
bundle["FLMTrialApplied"] = True
_apply_param_bundle(params, bundle)
if profile.get("pathKey") == "cleanup_pass":
report = _read_json(paths["reports"] / f"{report_id}.json", {})
_record_cleanup_progress(str(report.get("car", {}).get("carFingerprint", "")), report_id)
return {
"message": f"Applied {profile.get('label', 'FLM')} profile.",
"profile": profile,
@@ -465,6 +465,38 @@ def test_select_primary_tuning_path_prefers_baseline_for_broad_mismatch(tmp_path
assert decision["alternatePathKey"] == "cleanup_pass"
def test_select_primary_tuning_path_does_not_automatically_demote_cleanup_progress(tmp_path):
module, _ = _load_flm_workspace_module(tmp_path)
summaries = [
{"bucket": "understeer", "severity": 1.0},
{"bucket": "center_chatter", "severity": 0.9},
{"bucket": "unwind_too_slow", "severity": 0.85},
{"bucket": "saturation_limited", "severity": 0.8},
]
decision = module.select_primary_tuning_path(summaries, {"meanErrorAbs": 0.16}, cleanup_progress_locked=True)
assert decision["primaryPathKey"] == "cleanup_pass"
assert decision["alternatePathKey"] == "baseline_fix"
assert decision["rawPrimaryPathKey"] == "baseline_fix"
assert decision["automaticBaselineDemotionBlocked"] is True
def test_cleanup_progress_bootstraps_from_existing_vehicle_report(tmp_path):
module, _ = _load_flm_workspace_module(tmp_path)
workspace = module.ensure_flm_workspace()
report = {
"reportId": "existing-cleanup",
"primaryPathKey": "cleanup_pass",
"car": {"carFingerprint": "TEST_TRUCK", "controlPath": "torque"},
}
(workspace["reports"] / "existing-cleanup.json").write_text(json.dumps(report), encoding="utf-8")
assert module._cleanup_progress_locked("TEST_TRUCK") is True
progress = json.loads((workspace["root"] / module.FLM_PROGRESS_FILENAME).read_text(encoding="utf-8"))
assert progress["vehicles"]["TEST_TRUCK"]["minimumPathKey"] == "cleanup_pass"
def test_select_primary_tuning_path_prefers_cleanup_for_localized_issue(tmp_path):
module, _ = _load_flm_workspace_module(tmp_path)
summaries = [