diff --git a/opendbc_repo/opendbc/car/fingerprints.py b/opendbc_repo/opendbc/car/fingerprints.py index 25150d1c3..70e13cae6 100644 --- a/opendbc_repo/opendbc/car/fingerprints.py +++ b/opendbc_repo/opendbc/car/fingerprints.py @@ -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, diff --git a/opendbc_repo/opendbc/car/hyundai/fingerprints.py b/opendbc_repo/opendbc/car/hyundai/fingerprints.py index bd464b07a..d183a48a1 100644 --- a/opendbc_repo/opendbc/car/hyundai/fingerprints.py +++ b/opendbc_repo/opendbc/car/hyundai/fingerprints.py @@ -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', diff --git a/opendbc_repo/opendbc/car/hyundai/interface.py b/opendbc_repo/opendbc/car/hyundai/interface.py index 03a6022f0..e5e8045a2 100644 --- a/opendbc_repo/opendbc/car/hyundai/interface.py +++ b/opendbc_repo/opendbc/car/hyundai/interface.py @@ -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: diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index ca3f6ac89..df59cdccf 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -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} diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index 2cf0c8c7b..ae48f23d9 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -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 diff --git a/opendbc_repo/opendbc/car/tests/routes.py b/opendbc_repo/opendbc/car/tests/routes.py index 32b3356b4..4ebca2099 100644 --- a/opendbc_repo/opendbc/car/tests/routes.py +++ b/opendbc_repo/opendbc/car/tests/routes.py @@ -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), diff --git a/opendbc_repo/opendbc/car/torque_data/override.toml b/opendbc_repo/opendbc/car/torque_data/override.toml index 1e1ef2999..05f1522ad 100644 --- a/opendbc_repo/opendbc/car/torque_data/override.toml +++ b/opendbc_repo/opendbc/car/torque_data/override.toml @@ -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] diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index b30500f4a..062ee26b2 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -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)) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 8a0c21301..cec5a3399 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -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) diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index a1e7e3c6f..8ba40d945 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -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, diff --git a/selfdrive/ui/layouts/home.py b/selfdrive/ui/layouts/home.py index b3fb84e32..f064dfcb7 100644 --- a/selfdrive/ui/layouts/home.py +++ b/selfdrive/ui/layouts/home.py @@ -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 diff --git a/selfdrive/ui/lib/starpilot_version.py b/selfdrive/ui/lib/starpilot_version.py index bd192f40a..a79202eee 100644 --- a/selfdrive/ui/lib/starpilot_version.py +++ b/selfdrive/ui/lib/starpilot_version.py @@ -1,4 +1,4 @@ -STARPILOT_DISPLAY_VERSION = "6.7.1" +STARPILOT_DISPLAY_VERSION = "6.7.2" def starpilot_display_description(description: str | None) -> str: diff --git a/selfdrive/ui/widgets/setup.py b/selfdrive/ui/widgets/setup.py index 31af5074f..d01db1424 100644 --- a/selfdrive/ui/widgets/setup.py +++ b/selfdrive/ui/widgets/setup.py @@ -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(): diff --git a/starpilot/controls/lib/curve_speed_controller.py b/starpilot/controls/lib/curve_speed_controller.py index a5fa45be6..e4f96ec0a 100644 --- a/starpilot/controls/lib/curve_speed_controller.py +++ b/starpilot/controls/lib/curve_speed_controller.py @@ -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 diff --git a/starpilot/system/the_galaxy/flm_workspace.py b/starpilot/system/the_galaxy/flm_workspace.py index dce5e0015..7b6ad14bd 100644 --- a/starpilot/system/the_galaxy/flm_workspace.py +++ b/starpilot/system/the_galaxy/flm_workspace.py @@ -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, diff --git a/starpilot/system/the_galaxy/tests/test_flm_workspace.py b/starpilot/system/the_galaxy/tests/test_flm_workspace.py index a1efaabd2..bdd001cdf 100644 --- a/starpilot/system/the_galaxy/tests/test_flm_workspace.py +++ b/starpilot/system/the_galaxy/tests/test_flm_workspace.py @@ -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 = [