From d773ad9025dd65fbb385872b6d70bae3d69ec87e Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 22 Jul 2026 11:38:35 -0500 Subject: [PATCH] fixes --- common/params_keys.h | 1 + docs/MODEL_REBUILD.md | 4 +- opendbc_repo/opendbc/car/gm/carcontroller.py | 4 + opendbc_repo/opendbc/car/gm/gmcan.py | 9 ++ opendbc_repo/opendbc/car/gm/interface.py | 3 + opendbc_repo/opendbc/car/gm/tests/test_gm.py | 8 ++ .../opendbc/car/hyundai/carcontroller.py | 5 +- .../opendbc/car/hyundai/tests/test_hyundai.py | 29 +++++- .../opendbc/safety/modes/hyundai_canfd.h | 4 +- .../safety/tests/test_hyundai_canfd.py | 25 +++++- scripts/model_compiler.py | 3 +- .../controls/lib/latcontrol_vehicle_tunes.py | 19 +++- selfdrive/controls/lib/longcontrol.py | 3 + .../controls/lib/longitudinal_planner.py | 78 +++++++++++++++- .../test_conditional_experimental_mode.py | 43 +++++++++ selfdrive/controls/tests/test_latcontrol.py | 6 +- selfdrive/controls/tests/test_longcontrol.py | 25 ++++++ .../tests/test_longitudinal_planner.py | 82 +++++++++++++++++ .../tests/test_speed_limit_controller.py | 88 +++++++++++++++++-- selfdrive/modeld/compile_modeld.py | 18 ++-- selfdrive/ui/layouts/settings/device.py | 10 +-- .../settings/tests/test_device_reboot.py | 45 ++++++++++ starpilot/assets/tests/test_model_pipeline.py | 29 ++++++ .../lib/conditional_experimental_mode.py | 15 +++- .../controls/lib/speed_limit_controller.py | 12 ++- starpilot/system/speed_limit_vision.py | 3 + .../system/tests/test_speed_limit_vision.py | 21 +++++ system/manager/manager.py | 10 +-- system/manager/test/test_manager.py | 9 +- 29 files changed, 565 insertions(+), 46 deletions(-) create mode 100644 selfdrive/ui/layouts/settings/tests/test_device_reboot.py diff --git a/common/params_keys.h b/common/params_keys.h index db95729b8..3b651a483 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -36,6 +36,7 @@ inline static std::unordered_map keys = { {"DoReboot", {CLEAR_ON_MANAGER_START, BOOL}}, {"DoShutdown", {CLEAR_ON_MANAGER_START, BOOL}}, {"DoUninstall", {CLEAR_ON_MANAGER_START, BOOL}}, + {"DoUserReboot", {CLEAR_ON_MANAGER_START, BOOL}}, {"DriverTooDistracted", {CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON, BOOL}}, {"DriverLockoutCount", {CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON, INT, "0"}}, {"EcuDisableFailed", {CLEAR_ON_MANAGER_START, BOOL}}, diff --git a/docs/MODEL_REBUILD.md b/docs/MODEL_REBUILD.md index 76a1c409c..0631c76c9 100644 --- a/docs/MODEL_REBUILD.md +++ b/docs/MODEL_REBUILD.md @@ -85,9 +85,11 @@ The lower-level device compiler also supports direct use: For a model that cannot run on the device GPU, compile with the USB AMD GPU attached: ```bash -./models --model heavyweight --input-format supercombo --version v15 --external-gpu +./models --model heavyweight --input-format supercombo --version v15 --gpu ``` +`--external-gpu` remains available as a compatibility alias for `--gpu`. + This emits a streaming out-of-band pickle and keeps QCOM available for camera warps. Its manifest entry must include: ```json diff --git a/opendbc_repo/opendbc/car/gm/carcontroller.py b/opendbc_repo/opendbc/car/gm/carcontroller.py index 918bf2300..b5106d712 100644 --- a/opendbc_repo/opendbc/car/gm/carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/carcontroller.py @@ -482,6 +482,8 @@ class CarController(CarControllerBase): self.xt4_cc_button_burst_remaining = 0 self.xt4_cc_button_burst_button = CruiseButtons.INIT self.xt4_cc_button_burst_last_counter = -1 + self.xt4_cc_button_observed_counter = -1 + self.xt4_cc_button_counter_frame = 0 self.lka_steering_cmd_counter = 0 self.lka_icon_status_last = (False, False) @@ -942,6 +944,8 @@ class CarController(CarControllerBase): elif self.CP.carFingerprint == CAR.CADILLAC_XT4_CC: self.xt4_cc_button_burst_remaining = 0 self.xt4_cc_button_burst_button = CruiseButtons.INIT + self.xt4_cc_button_burst_last_counter = -1 + self.xt4_cc_button_observed_counter = -1 if self.CP.openpilotLongitudinalControl: # Gas/regen, brakes, and UI commands - all at 25Hz diff --git a/opendbc_repo/opendbc/car/gm/gmcan.py b/opendbc_repo/opendbc/car/gm/gmcan.py index f190c164c..74eeecc50 100644 --- a/opendbc_repo/opendbc/car/gm/gmcan.py +++ b/opendbc_repo/opendbc/car/gm/gmcan.py @@ -19,6 +19,7 @@ MALIBU_BUTTON_MAP = { ACC_CRUISE_STATE_ADAPTIVE = 2 XT4_CC_BUTTON_BURST_FRAMES = 6 +XT4_CC_BUTTON_COUNTER_DELAY_FRAMES = 1 def malibu_phase_map_for_button(button): @@ -320,6 +321,10 @@ def create_gm_cc_spam_command(packer, controller, CS, actuators, starpilot_toggl controller.apply_speed = speed_setpoint if CS.CP.carFingerprint == CAR.CADILLAC_XT4_CC: + if controller.xt4_cc_button_observed_counter != CS.buttons_counter: + controller.xt4_cc_button_observed_counter = CS.buttons_counter + controller.xt4_cc_button_counter_frame = controller.frame + if cruise_btn == CruiseButtons.INIT: controller.xt4_cc_button_burst_remaining = 0 controller.xt4_cc_button_burst_button = CruiseButtons.INIT @@ -338,6 +343,10 @@ def create_gm_cc_spam_command(packer, controller, CS, actuators, starpilot_toggl controller.xt4_cc_button_burst_last_counter = -1 # XT4 physical taps hold the button for 5-7 consecutive 33 Hz frames. + # Sending immediately after the stock frame is too early for the receiving ECU. + if controller.frame - controller.xt4_cc_button_counter_frame < XT4_CC_BUTTON_COUNTER_DELAY_FRAMES: + return [] + # Send once per observed stock counter so the injected sequence has the same cadence. if controller.xt4_cc_button_burst_last_counter == CS.buttons_counter: return [] diff --git a/opendbc_repo/opendbc/car/gm/interface.py b/opendbc_repo/opendbc/car/gm/interface.py index 7067a11c7..cf75369e1 100755 --- a/opendbc_repo/opendbc/car/gm/interface.py +++ b/opendbc_repo/opendbc/car/gm/interface.py @@ -609,6 +609,9 @@ class CarInterface(CarInterfaceBase): if is_bolt_2022_2023_pedal: # Gen2 Bolt pedal-long should follow the no-ACC panda path. ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.FLAG_GM_NO_ACC.value + ret.startingState = True + ret.startAccel = 0.55 + ret.vEgoStarting = max(ret.vEgoStarting, 0.35) if candidate in (CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL, CAR.CHEVROLET_MALIBU_HYBRID_CC): ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.FLAG_GM_BOLT_2022_PEDAL.value diff --git a/opendbc_repo/opendbc/car/gm/tests/test_gm.py b/opendbc_repo/opendbc/car/gm/tests/test_gm.py index 8aad98485..c5cdf6dc5 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_gm.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_gm.py @@ -362,6 +362,9 @@ class TestGMInterface: assert car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.GM_REMAP_CANCEL_TO_DISTANCE assert car_params.safetyConfigs[0].safetyParam & GMSafetyFlags.FLAG_GM_BOLT_2022_PEDAL.value assert car_params.safetyConfigs[0].safetyParam & GMSafetyFlags.FLAG_GM_PANDA_PADDLE_SCHED.value + assert car_params.startingState + assert car_params.startAccel == pytest.approx(0.55) + assert car_params.vEgoStarting == pytest.approx(0.35) def test_cadillac_xt5_sdgm_sascm_gates_alpha_long(self): CarInterface = interfaces[CAR.CADILLAC_XT5] @@ -564,6 +567,8 @@ class TestGMCarController: xt4_cc_button_burst_remaining=0, xt4_cc_button_burst_button=CruiseButtons.INIT, xt4_cc_button_burst_last_counter=-1, + xt4_cc_button_observed_counter=-1, + xt4_cc_button_counter_frame=0, ) cs = SimpleNamespace( CP=SimpleNamespace( @@ -580,12 +585,15 @@ class TestGMCarController: actuators = SimpleNamespace(accel=1.0) dats = [] + send_counts = [] for counter in (0, 0, 0, 1, 1, 1, 2, 2, 2, 3, 3, 3, 0, 0, 0, 1, 1, 1): cs.buttons_counter = counter msgs = gmcan.create_gm_cc_spam_command(packer, controller, cs, actuators, SimpleNamespace(is_metric=False)) + send_counts.append(len(msgs)) dats.extend(bytes(msg[1]).hex() for msg in msgs) controller.frame += 1 + assert send_counts == [0, 1, 0] * gmcan.XT4_CC_BUTTON_BURST_FRAMES assert dats == [ "000000010125de", "00000001022acd", diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index d6fc68a1f..e79227052 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -773,7 +773,10 @@ class CarController(CarControllerBase): drive_gear = gear == structs.CarState.GearShifter.drive if angle_lkas_alt: steering_msg_active = bool(steering_msg_active and drive_gear) - forward_stock_lkas = angle_lkas_alt and not (drive_gear and (CC.latActive or CC.enabled)) + angle_lkas_alt_standstill_handoff = bool(getattr(CS.out, "standstill", False) and not CC.latActive) + forward_stock_lkas = angle_lkas_alt and ( + angle_lkas_alt_standstill_handoff or not (drive_gear and (CC.latActive or CC.enabled)) + ) if not forward_stock_lkas and not ccnc_angle_long: can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, steering_msg_active, apply_torque, apply_angle, diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index f94ab23cf..568a4d313 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -308,11 +308,13 @@ class TestHyundaiFingerprint: assert direct_angle_request_allowed(8.47, 140.0, 140.0, True, controller.BASELINE_VM, controller.params) assert not direct_angle_request_allowed(8.47, 140.0, 140.0, False, controller.BASELINE_VM, controller.params) - def test_ev9_matches_other_angle_platform_standstill_behavior(self): + def test_angle_platforms_disable_standstill_steering(self): ev9_cp = CarInterface.get_params(CAR.KIA_EV9, gen_empty_fingerprint(), [], False, False, False, None) + ioniq_5_pe_cp = CarInterface.get_params(CAR.HYUNDAI_IONIQ_5_PE, gen_empty_fingerprint(), [], False, False, False, None) sportage_cp = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, gen_empty_fingerprint(), [], False, False, False, None) assert not ev9_cp.steerAtStandstill + assert not ioniq_5_pe_cp.steerAtStandstill assert not sportage_cp.steerAtStandstill @pytest.mark.parametrize("candidate", (CAR.KIA_K4_2025, CAR.KIA_CARNIVAL_2025, CAR.KIA_CARNIVAL_HEV_4TH_GEN)) @@ -1633,6 +1635,31 @@ class TestHyundaiFingerprint: lkas_msgs = [msg for msg in msgs if msg[0] == 0x110] assert len(lkas_msgs) == 0 + @pytest.mark.parametrize(("standstill", "expected_lkas_msgs"), [(False, 1), (True, 0)]) + def test_ioniq_5_pe_standstill_lets_safety_forward_stock_lkas(self, standstill, expected_lkas_msgs): + CP = CarParams.new_message() + CP.carFingerprint = CAR.HYUNDAI_IONIQ_5_PE + CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_ANGLE_STEERING | + HyundaiFlags.CANFD_LKA_STEERING | HyundaiFlags.CANFD_LKA_STEERING_ALT) + CP.openpilotLongitudinalControl = False + + controller = CarController(DBC[CP.carFingerprint], CP) + cc = SimpleNamespace(enabled=True, latActive=False, actuators=SimpleNamespace(longControlState=LongCtrlState.off), + leftBlinker=False, rightBlinker=False, hudControl=SimpleNamespace()) + cs = SimpleNamespace( + stock_lfa_msg=None, + stock_lkas_msg={}, + out=SimpleNamespace( + standstill=standstill, + steeringAngleDeg=0.0, + gearShifter=structs.CarState.GearShifter.drive, + ), + ) + + msgs = controller.create_canfd_msgs(0, False, 0.0, 0.0, 0.0, 0.0, False, cc.hudControl, cs, cc, + get_test_toggles(), lka_icon=1, lfa_icon=1) + assert len([msg for msg in msgs if msg[0] == 0x110]) == expected_lkas_msgs + def test_ev9_inactive_angle_steering_does_not_suppress_stock_lfa(self): CP = CarParams.new_message() CP.carFingerprint = CAR.KIA_EV9 diff --git a/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h b/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h index b81acdd8d..58ca24537 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h @@ -89,7 +89,9 @@ static bool hyundai_canfd_lka_alt_forward_addr(int addr) { } static bool hyundai_canfd_lka_alt_openpilot_allowed(void) { - return (aol_allowed || controls_allowed) && (!hyundai_ev_gas_signal || hyundai_canfd_lka_alt_drive_gear); + const bool angle_steering_allowed = !hyundai_canfd_angle_steering || vehicle_moving; + return (aol_allowed || controls_allowed) && angle_steering_allowed && + (!hyundai_ev_gas_signal || hyundai_canfd_lka_alt_drive_gear); } static bool hyundai_canfd_lka_alt_stock_forwarding(void) { diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py index 2177286a2..4ad6fec44 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py @@ -283,6 +283,7 @@ class TestHyundaiCanfdAngleSteering(HyundaiButtonBase, common.CarSafetyTest): self.safety.get_angle_meas_min, self.safety.get_angle_meas_max) def test_angle_cmd_when_disabled(self): + self._reset_speed_measurement(self.STANDSTILL_THRESHOLD + 1) for controls_allowed in (True, False): self.safety.set_controls_allowed(controls_allowed) for angle_meas in np.arange(-90, 91, 10): @@ -300,7 +301,7 @@ class TestHyundaiCanfdAngleSteering(HyundaiButtonBase, common.CarSafetyTest): speed = round_speed(away_round(speed / 0.03125 * 3.6) * 0.03125 / 3.6) speed = max(speed, 1) self.safety.set_controls_allowed(True) - self._reset_speed_measurement(speed + 1) + self._reset_speed_measurement(max(speed + 1, self.STANDSTILL_THRESHOLD + 1)) max_angle = round(get_max_angle_vm(speed, vm, limits), 1) max_angle = float(np.clip(max_angle, -self.STEER_ANGLE_MAX, self.STEER_ANGLE_MAX)) @@ -315,7 +316,7 @@ class TestHyundaiCanfdAngleSteering(HyundaiButtonBase, common.CarSafetyTest): speed = round_speed(away_round(speed / 0.03125 * 3.6) * 0.03125 / 3.6) speed = max(speed, 1) self.safety.set_controls_allowed(True) - self._reset_speed_measurement(speed + 1) + self._reset_speed_measurement(max(speed + 1, self.STANDSTILL_THRESHOLD + 1)) self.assertTrue(self._tx(self._angle_cmd_msg(0, True))) max_delta = round(get_max_angle_delta_vm(speed, vm, limits), 1) @@ -325,6 +326,7 @@ class TestHyundaiCanfdAngleSteering(HyundaiButtonBase, common.CarSafetyTest): self.assertTrue(self._tx(self._angle_cmd_msg(0, True))) def test_rt_limits(self): + self._reset_speed_measurement(self.STANDSTILL_THRESHOLD + 1) self.safety.set_timer(0) self.safety.set_controls_allowed(True) max_rt_msgs = int(self.LATERAL_FREQUENCY * common.RT_INTERVAL / 1e6 * 1.2 + 1) @@ -682,6 +684,7 @@ class TestHyundaiCanfdLKASteeringAltAngleLongEV(HyundaiLongitudinalBase, TestHyu return self.packer.make_can_msg_safety("ACCELERATOR", self.PT_BUS, values) def test_lka_alt_stock_forwarding_depends_on_controls_allowed(self): + self._reset_speed_measurement(self.STANDSTILL_THRESHOLD + 1) for addr in (0x110, 0x362): self.safety.set_controls_allowed(False) self.assertEqual(0, self.safety.safety_fwd_hook(2, addr)) @@ -690,6 +693,7 @@ class TestHyundaiCanfdLKASteeringAltAngleLongEV(HyundaiLongitudinalBase, TestHyu self.assertEqual(-1, self.safety.safety_fwd_hook(2, addr)) def test_lka_alt_stock_forwarding_blocks_openpilot_tx(self): + self._reset_speed_measurement(self.STANDSTILL_THRESHOLD + 1) self.safety.set_controls_allowed(False) self.assertFalse(self._tx(self._angle_cmd_msg(0, enabled=False))) self.assertFalse(self._tx(common.make_msg(0, 0x362, 32))) @@ -703,16 +707,28 @@ class TestHyundaiCanfdLKASteeringAltAngleLongEV(HyundaiLongitudinalBase, TestHyu self.safety.set_controls_allowed(False) self._toggle_aol(True) self._rx(self._gear_msg(5)) + self._reset_speed_measurement(self.STANDSTILL_THRESHOLD + 1) for addr in (0x110, 0x362): self.assertEqual(-1, self.safety.safety_fwd_hook(2, addr)) self._reset_angle_measurement(0) - self._reset_speed_measurement(1) self._set_prev_desired_angle(0) self.assertTrue(self._tx(self._angle_cmd_msg(0, enabled=True))) self.assertTrue(self._tx(common.make_msg(0, 0x362, 32))) + def test_lka_alt_standstill_forwards_stock_and_blocks_openpilot_tx(self): + self.safety.set_controls_allowed(True) + self._reset_speed_measurement(0) + + for addr in (0x110, 0x362): + self.assertEqual(0, self.safety.safety_fwd_hook(2, addr)) + + self._reset_angle_measurement(0) + self._set_prev_desired_angle(0) + self.assertFalse(self._tx(self._angle_cmd_msg(0, enabled=False))) + self.assertFalse(self._tx(common.make_msg(0, 0x362, 32))) + def test_lka_alt_aol_non_drive_gear_forwards_stock_and_blocks_openpilot_tx(self): self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL) self.safety.set_controls_allowed(False) @@ -725,12 +741,13 @@ class TestHyundaiCanfdLKASteeringAltAngleLongEV(HyundaiLongitudinalBase, TestHyu self.assertEqual(0, self.safety.safety_fwd_hook(2, addr)) self._reset_angle_measurement(0) - self._reset_speed_measurement(1) + self._reset_speed_measurement(self.STANDSTILL_THRESHOLD + 1) self._set_prev_desired_angle(0) self.assertFalse(self._tx(self._angle_cmd_msg(0, enabled=True))) self.assertFalse(self._tx(common.make_msg(0, 0x362, 32))) def test_angle_cmd_when_disabled(self): + self._reset_speed_measurement(self.STANDSTILL_THRESHOLD + 1) for controls_allowed in (True, False): self.safety.set_controls_allowed(controls_allowed) for angle_meas in np.arange(-90, 91, 10): diff --git a/scripts/model_compiler.py b/scripts/model_compiler.py index 1b9cc28cd..6c836a13a 100644 --- a/scripts/model_compiler.py +++ b/scripts/model_compiler.py @@ -80,7 +80,8 @@ def parse_args() -> argparse.Namespace: ) parser.add_argument("--list", action="store_true", help="List staged models and exit.") parser.add_argument("--force", action="store_true", help="Accepted for compatibility; selected outputs are always replaced.") - parser.add_argument("--external-gpu", action="store_true", help="Compile the driving artifact for the USB AMD GPU.") + parser.add_argument("--gpu", "--external-gpu", dest="external_gpu", action="store_true", + help="Compile the driving artifact for the USB AMD GPU.") parser.add_argument("--split-artifact", type=Path, help="Split an existing oversized PKL without compiling.") parser.add_argument("--chunk-size-mib", type=int, default=95, help="Multipart size in MiB; must be below 100.") parser.add_argument( diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index cc2b5808b..0cf571b57 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -613,6 +613,8 @@ IONIQ_6_CURVY_UNWIND_LAT_START = 0.45 IONIQ_6_CURVY_UNWIND_LAT_END = 3.6 IONIQ_6_CURVY_UNWIND_LAT_ONSET_WIDTH = 0.14 IONIQ_6_CURVY_UNWIND_LAT_CUTOFF_WIDTH = 0.55 +IONIQ_6_CURVY_RIGHT_UNWIND_JERK_ONSET = 0.40 +IONIQ_6_CURVY_RIGHT_UNWIND_JERK_WIDTH = 0.22 IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_MIN = 11.5 IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_MAX = 20.5 IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_WIDTH = 1.2 @@ -658,6 +660,8 @@ KIA_EV6_TURN_IN_BOOST_LEFT = 0.62 KIA_EV6_TURN_IN_BOOST_RIGHT = 0.60 KIA_EV6_UNWIND_TAPER_LEFT = 0.56 KIA_EV6_UNWIND_TAPER_RIGHT = 0.54 +KIA_EV6_BASE_UNWIND_TAPER_LEFT = 0.06 +KIA_EV6_BASE_UNWIND_TAPER_RIGHT = 0.05 KIA_EV6_FRICTION_MULT = 1.01 KIA_EV6_FRICTION_LAT_RISE = 0.18 KIA_EV6_FRICTION_JERK_RISE = 0.22 @@ -2263,12 +2267,16 @@ def get_ioniq_6_directional_taper_scale(desired_lateral_accel: float, desired_la curvy_unwind_weight = 0.0 curvy_unwind_floor_relief = 0.0 if v_ego is not None: + curvy_unwind_phase_weight = unwind_weight + if desired_lateral_accel < 0.0: + curvy_unwind_phase_weight = max(-phase, 0.0) * _ioniq_6_sigmoid( + (abs(desired_lateral_jerk) - IONIQ_6_CURVY_RIGHT_UNWIND_JERK_ONSET) / IONIQ_6_CURVY_RIGHT_UNWIND_JERK_WIDTH) curvy_unwind_speed_weight = _ioniq_6_curvy_speed_weight(v_ego) curvy_unwind_lat_onset = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_CURVY_UNWIND_LAT_START) / IONIQ_6_CURVY_UNWIND_LAT_ONSET_WIDTH) curvy_unwind_lat_cutoff = _ioniq_6_sigmoid((IONIQ_6_CURVY_UNWIND_LAT_END - abs_lateral_accel) / IONIQ_6_CURVY_UNWIND_LAT_CUTOFF_WIDTH) - curvy_unwind_weight = curvy_unwind_speed_weight * curvy_unwind_lat_onset * curvy_unwind_lat_cutoff * unwind_weight + curvy_unwind_weight = curvy_unwind_speed_weight * curvy_unwind_lat_onset * curvy_unwind_lat_cutoff * curvy_unwind_phase_weight curvy_unwind_floor_relief = (_ioniq_6_side_value(desired_lateral_accel, _flm_vehicle_knob("hyundai_ioniq_6.curvy_unwind_floor_relief_left", IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_LEFT), _flm_vehicle_knob("hyundai_ioniq_6.curvy_unwind_floor_relief_right", IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_RIGHT)) * @@ -2411,7 +2419,12 @@ def get_kia_ev6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: flo _flm_vehicle_knob("hyundai_kia_ev6.unwind_taper_right", KIA_EV6_UNWIND_TAPER_RIGHT), ) * unwind_weight * (0.35 + 0.65 * low_speed_factor)) - return 1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0)) + base_unwind_taper = 1.0 - (_kia_ev6_side_value( + desired_lateral_accel, + _flm_vehicle_knob("hyundai_kia_ev6.base_unwind_taper_left", KIA_EV6_BASE_UNWIND_TAPER_LEFT), + _flm_vehicle_knob("hyundai_kia_ev6.base_unwind_taper_right", KIA_EV6_BASE_UNWIND_TAPER_RIGHT), + ) * unwind_weight * onset * cutoff) + return base_unwind_taper + (extra_scale * turn_in_boost * max(unwind_taper, 0.0)) def get_kia_ev6_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: @@ -2785,6 +2798,8 @@ FLM_SUPPORTED_VEHICLE_KNOBS = { "hyundai_kia_ev6.turn_in_boost_right": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 1.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_TURN_IN_BOOST_RIGHT}, "hyundai_kia_ev6.unwind_taper_left": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 1.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_UNWIND_TAPER_LEFT}, "hyundai_kia_ev6.unwind_taper_right": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 1.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_UNWIND_TAPER_RIGHT}, + "hyundai_kia_ev6.base_unwind_taper_left": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 0.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_BASE_UNWIND_TAPER_LEFT}, + "hyundai_kia_ev6.base_unwind_taper_right": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 0.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_BASE_UNWIND_TAPER_RIGHT}, "hyundai_kia_ev6.center_taper_max": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 0.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_CENTER_TAPER_MAX}, "hyundai_kia_ev6.turn_in_threshold_reduction_left": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 0.40, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_LEFT}, "hyundai_kia_ev6.turn_in_threshold_reduction_right": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 0.40, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_RIGHT}, diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index ed099ead9..67990576e 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -13,6 +13,7 @@ interp = np.interp STOPPING_RELEASE_HYSTERESIS = 0.35 STOPPING_RELEASE_MIN_ACCEL = 0.15 STOPPING_RELEASE_STRONG_ACCEL = 0.45 +LEAD_GAP_SETTLE_MAX_START_ACCEL = 0.25 MOVING_STOP_FOLLOW_MIN_GAP = 0.25 NEGATIVE_TARGET_CREEP_GUARD_SPEED = 0.35 NEGATIVE_TARGET_CREEP_GUARD_DECEL = 0.40 @@ -257,6 +258,8 @@ class LongControl: output_accel = clip(a_target, 0.0, starpilot_toggles.startAccel) elif getattr(starpilot_toggles, "custom_accel_profile", False): output_accel = clip(a_target, 0.0, starpilot_toggles.startAccel) + elif has_lead and a_target <= LEAD_GAP_SETTLE_MAX_START_ACCEL: + output_accel = clip(a_target, 0.0, starpilot_toggles.startAccel) elif profile_max_accel > 0.0: # Keep the StartAccel friction-overcoming shove, but cap it at the selected # acceleration profile's launch ceiling so Eco launches soft and Sport hard. diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 6841ae6c2..6e62c5ded 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -56,6 +56,14 @@ STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_SPEED = 0.25 STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_ACCEL = 0.08 STANDSTILL_LEAD_CREEP_RELEASE_MIN_GAP_MARGIN = 0.1 STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME = 0.30 +RADAR_STANDSTILL_GAP_SETTLE_ACCEL = 0.18 +RADAR_STANDSTILL_GAP_SETTLE_CONFIRM_TIME = 0.50 +RADAR_STANDSTILL_GAP_SETTLE_ENTRY_MARGIN = 0.60 +RADAR_STANDSTILL_GAP_SETTLE_EXIT_MARGIN = 0.15 +RADAR_STANDSTILL_GAP_SETTLE_MAX_EXTRA_GAP = 1.5 +RADAR_STANDSTILL_GAP_SETTLE_MAX_EGO_SPEED = 0.45 +RADAR_STANDSTILL_GAP_SETTLE_MAX_LEAD_SPEED = 0.15 +RADAR_STANDSTILL_GAP_SETTLE_MAX_LATERAL_OFFSET = 1.0 LEAD_DEPART_CONFIDENT_MIN_GAP = 3.75 LEAD_DEPART_CONFIDENT_MAX_GAP = 5.25 LEAD_DEPART_CONFIDENT_MIN_LEAD_SPEED = 0.3 @@ -640,6 +648,8 @@ class LongitudinalPlanner: self.model_launch_stop_seen = False self.confident_lead_depart_elapsed = 0.0 self.slow_creep_lead_depart_elapsed = 0.0 + self.radar_standstill_gap_settle_elapsed = 0.0 + self.radar_standstill_gap_settle_active = False self.v_desired_trajectory = np.zeros(CONTROL_N) self.a_desired_trajectory = np.zeros(CONTROL_N) @@ -1352,6 +1362,62 @@ class LongitudinalPlanner: lead_accel >= STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_ACCEL ) + @staticmethod + def is_radar_standstill_gap_settle_candidate(lead, v_ego, target_gap, active=False): + if lead is None or not lead.status or not bool(getattr(lead, "radar", False)): + return False + if float(v_ego) > RADAR_STANDSTILL_GAP_SETTLE_MAX_EGO_SPEED: + return False + if abs(float(getattr(lead, "yRel", 0.0))) > RADAR_STANDSTILL_GAP_SETTLE_MAX_LATERAL_OFFSET: + return False + if abs(float(getattr(lead, "vLead", 0.0))) > RADAR_STANDSTILL_GAP_SETTLE_MAX_LEAD_SPEED: + return False + + lead_gap = float(getattr(lead, "dRel", 0.0)) + min_margin = RADAR_STANDSTILL_GAP_SETTLE_EXIT_MARGIN if active else RADAR_STANDSTILL_GAP_SETTLE_ENTRY_MARGIN + return bool( + lead_gap > target_gap + min_margin and + lead_gap <= target_gap + RADAR_STANDSTILL_GAP_SETTLE_MAX_EXTRA_GAP + ) + + def update_radar_standstill_gap_settle(self, sm, target_gap): + vetoed = bool( + getattr(sm["carState"], "brakePressed", False) or + getattr(sm["carState"], "gasPressed", False) or + getattr(sm["starpilotPlan"], "forcingStop", False) or + getattr(sm["starpilotPlan"], "redLight", False) + ) + candidates = [ + lead for lead in (self.lead_one, self.lead_two) + if self.is_radar_standstill_gap_settle_candidate( + lead, + float(sm["carState"].vEgo), + target_gap, + active=self.radar_standstill_gap_settle_active, + ) + ] + + if vetoed or not candidates: + self.radar_standstill_gap_settle_elapsed = 0.0 + self.radar_standstill_gap_settle_active = False + return False + + if self.radar_standstill_gap_settle_active: + return True + + if not bool(sm["carState"].standstill): + self.radar_standstill_gap_settle_elapsed = 0.0 + return False + + self.radar_standstill_gap_settle_elapsed = min( + RADAR_STANDSTILL_GAP_SETTLE_CONFIRM_TIME, + self.radar_standstill_gap_settle_elapsed + self.dt, + ) + self.radar_standstill_gap_settle_active = ( + self.radar_standstill_gap_settle_elapsed + 1e-6 >= RADAR_STANDSTILL_GAP_SETTLE_CONFIRM_TIME + ) + return self.radar_standstill_gap_settle_active + @staticmethod def get_centered_model_lead(model_data): try: @@ -3143,11 +3209,12 @@ class LongitudinalPlanner: slow_creep_depart_detected and self.slow_creep_lead_depart_elapsed >= STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME ) + radar_gap_settle_active = self.update_radar_standstill_gap_settle(sm, standstill_nudge_gap) standstill_stopped_lead_guard_cap = None standstill_guard_lead_present = any(bool(getattr(lead, "status", False)) for lead in (self.lead_one, self.lead_two)) if standstill_guard_lead_present and (bool(sm['carState'].standstill) or float(sm['carState'].vEgo) <= STANDSTILL_STOPPED_LEAD_GUARD_MAX_EGO_SPEED): - release_ready = bool(lead_depart_ready or confident_depart_ready or slow_creep_depart_ready) + release_ready = bool(lead_depart_ready or confident_depart_ready or slow_creep_depart_ready or radar_gap_settle_active) standstill_stopped_lead_guard_caps = [ cap for cap in ( self.get_standstill_stopped_lead_guard_cap( @@ -3196,6 +3263,11 @@ class LongitudinalPlanner: output_a_target = max(output_a_target, STANDSTILL_LEAD_DEPART_MIN_ACCEL) self.post_departure_follow_settle_until = now_t + POST_DEPARTURE_FOLLOW_SETTLE_LATCH_TIME + if radar_gap_settle_active: + vision_low_speed_stop_active = False + output_should_stop = False + output_a_target = RADAR_STANDSTILL_GAP_SETTLE_ACCEL + lead_present = any(bool(getattr(lead, "status", False)) for lead in (self.lead_one, self.lead_two)) confirmed_lead_release = bool(confident_depart_ready or lead_depart_ready or slow_creep_depart_ready) model_launch_allowed = bool( @@ -3588,6 +3660,10 @@ class LongitudinalPlanner: self.a_desired = min(self.a_desired, experimental_release_accel_target) output_a_target = min(output_a_target, experimental_release_accel_target) + if radar_gap_settle_active: + output_a_target = RADAR_STANDSTILL_GAP_SETTLE_ACCEL + output_should_stop = False + self.output_a_target = output_a_target self.output_should_stop = bool(output_should_stop or vision_low_speed_stop_active) diff --git a/selfdrive/controls/tests/test_conditional_experimental_mode.py b/selfdrive/controls/tests/test_conditional_experimental_mode.py index cc967f346..c0eb46890 100644 --- a/selfdrive/controls/tests/test_conditional_experimental_mode.py +++ b/selfdrive/controls/tests/test_conditional_experimental_mode.py @@ -866,6 +866,49 @@ def test_tracked_following_slower_lead_still_triggers_slow_lead(): assert cem.slow_lead_detected +def test_far_radar_slower_lead_waits_for_comfort_range_before_triggering(): + v_ego = 75 * CV.MPH_TO_MS + cem = make_cem( + model_length=v_ego * 5.0, + tracking_lead=True, + lead_status=True, + lead_d_rel=136.0, + lead_v_lead=v_ego - 5.0, + lead_model_prob=1.0, + lead_radar=True, + ) + toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False) + cem.starpilot_planner.starpilot_following.slower_lead = True + + for _ in range(24): + cem.slow_lead(toggles, v_ego) + assert not cem.slow_lead_detected + + cem.starpilot_planner.lead_one.dRel = 80.0 + for _ in range(24): + cem.slow_lead(toggles, v_ego) + assert cem.slow_lead_detected + + +def test_far_vision_slower_lead_keeps_existing_trigger_range(): + v_ego = 75 * CV.MPH_TO_MS + cem = make_cem( + model_length=v_ego * 5.0, + tracking_lead=True, + lead_status=True, + lead_d_rel=110.0, + lead_v_lead=v_ego - 5.0, + lead_model_prob=1.0, + lead_radar=False, + ) + toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False) + cem.starpilot_planner.starpilot_following.slower_lead = True + + for _ in range(24): + cem.slow_lead(toggles, v_ego) + assert cem.slow_lead_detected + + def test_tracked_vision_slow_lead_continues_existing_experimental_mode(): v_ego = 8.0 cem = make_cem( diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index b1d2d4b7b..1695e2489 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -672,7 +672,7 @@ class TestLatControl: assert get_ioniq_6_directional_taper_scale(1.2, -0.40) > get_ioniq_6_directional_taper_scale(1.2, -0.7) assert get_ioniq_6_directional_taper_scale(-1.2, -0.40, 8.0) > get_ioniq_6_directional_taper_scale(-1.2, -0.40, 25.0) assert get_ioniq_6_directional_taper_scale(1.2, 0.40, 8.0) > get_ioniq_6_directional_taper_scale(1.2, 0.40, 25.0) - assert get_ioniq_6_directional_taper_scale(-1.2, 1.6, 8.0) == pytest.approx(get_ioniq_6_directional_taper_scale(-1.2, 1.6, 25.0), abs=0.02) + assert get_ioniq_6_directional_taper_scale(-1.2, 1.6, 8.0) < get_ioniq_6_directional_taper_scale(-1.2, 1.6, 25.0) assert get_ioniq_6_directional_taper_scale(-0.18, -0.40, 3.0) > get_ioniq_6_directional_taper_scale(-0.18, -0.40, 9.0) assert get_ioniq_6_directional_taper_scale(-0.18, -0.40, 9.0) > get_ioniq_6_directional_taper_scale(-0.18, -0.40, 20.0) assert get_ioniq_6_directional_taper_scale(-0.50, -0.40, 3.0) > get_ioniq_6_directional_taper_scale(-0.50, -0.40, 6.0) @@ -681,6 +681,7 @@ class TestLatControl: assert get_ioniq_6_directional_taper_scale(-0.70, -0.70, 6.0) > get_ioniq_6_directional_taper_scale(-0.70, -0.70, 12.0) assert get_ioniq_6_directional_taper_scale(-0.70, -0.70, 12.0) > get_ioniq_6_directional_taper_scale(-0.70, -0.70, 20.0) assert get_ioniq_6_directional_taper_scale(0.30, 0.60, 5.0) > get_ioniq_6_directional_taper_scale(0.30, 0.60, 12.0) + assert get_ioniq_6_directional_taper_scale(-3.0, 0.45, 10.5) < get_ioniq_6_directional_taper_scale(-3.0, 0.45, 3.0) - 0.10 def test_ioniq_6_output_taper_curve(self): assert get_ioniq_6_output_taper_scale(0.0, 0.0, 25.0) < get_ioniq_6_output_taper_scale(0.0, 0.0, 8.0) <= 1.0 @@ -1118,6 +1119,7 @@ class TestLatControl: assert calls == 1 def test_kia_ev6_ff_scale_curve(self): + clear_flm_runtime_overrides() assert get_kia_ev6_ff_scale(0.0, 0.0, 20.0) == 1.0 steady_left = get_kia_ev6_ff_scale(0.45, 0.0, 25.0) steady_right = get_kia_ev6_ff_scale(-0.45, 0.0, 25.0) @@ -1131,6 +1133,8 @@ class TestLatControl: assert turn_in_right > steady_right assert unwind_left < steady_left assert unwind_right < steady_right + assert unwind_left < 1.03 + assert unwind_right < 1.07 def test_kia_ev6_center_taper_curve(self): assert get_kia_ev6_center_taper_scale(0.0, 25.0) < get_kia_ev6_center_taper_scale(0.0, 10.0) diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index aebcec06d..7db0b5f87 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/selfdrive/controls/tests/test_longcontrol.py @@ -146,6 +146,31 @@ def test_starting_accel_unchanged_when_custom_profile_disabled(): assert output_accel == 1.5 +def test_starting_accel_uses_small_planner_target_for_lead_gap_settle(): + CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5) + CP.longitudinalTuning.kpBP = [0.0] + CP.longitudinalTuning.kpV = [0.1] + CP.longitudinalTuning.kiBP = [0.0] + CP.longitudinalTuning.kiV = [0.03] + + lc = LongControl(CP) + CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False) + CS.cruiseState.standstill = False + + output_accel = lc.update( + active=True, + CS=CS, + a_target=0.18, + should_stop=False, + accel_limits=(-3.0, 2.0), + starpilot_toggles=make_toggles(startAccel=1.5), + has_lead=True, + ) + + assert lc.long_control_state == LongCtrlState.starting + assert output_accel == pytest.approx(0.18) + + def test_starting_accel_obeys_a_target_cap_when_custom_profile_enabled(): CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5) CP.longitudinalTuning.kpBP = [0.0] diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index c78c16894..03d63c342 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -1680,6 +1680,88 @@ def test_standstill_slow_creep_depart_does_not_release_on_gap_without_motion_sig assert planner.output_should_stop +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) +def test_stationary_radar_lead_settles_excess_stop_gap_after_confirmation(model_version): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.0) + sm = make_sm( + 0.0, + desired_accel=-0.12, + min_accel=-0.5, + experimental_mode=True, + tracking_lead=False, + lead_one=make_lead(status=True, d_rel=6.4, v_lead=0.0, a_lead=0.0, radar=True, model_prob=1.0), + ) + sm["carState"].standstill = True + sm["controlsState"].longControlState = LongCtrlState.stopping + sm["modelV2"].action.shouldStop = True + + frames = int(round(longitudinal_planner_module.RADAR_STANDSTILL_GAP_SETTLE_CONFIRM_TIME / planner.dt)) + for _ in range(max(frames - 1, 1)): + planner.update(sm, make_toggles(model_version)) + assert planner.output_should_stop + + planner.update(sm, make_toggles(model_version)) + + assert not planner.output_should_stop + assert planner.output_a_target == pytest.approx(longitudinal_planner_module.RADAR_STANDSTILL_GAP_SETTLE_ACCEL) + + +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) +def test_stationary_radar_gap_settle_reholds_at_target_gap(model_version): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.0) + sm = make_sm( + 0.0, + desired_accel=-0.12, + min_accel=-0.5, + experimental_mode=True, + tracking_lead=False, + lead_one=make_lead(status=True, d_rel=6.4, v_lead=0.0, a_lead=0.0, radar=True, model_prob=1.0), + ) + sm["carState"].standstill = True + sm["controlsState"].longControlState = LongCtrlState.stopping + sm["modelV2"].action.shouldStop = True + + frames = int(round(longitudinal_planner_module.RADAR_STANDSTILL_GAP_SETTLE_CONFIRM_TIME / planner.dt)) + for _ in range(frames): + planner.update(sm, make_toggles(model_version)) + assert planner.radar_standstill_gap_settle_active + + sm["carState"].vEgo = 0.15 + sm["carState"].standstill = False + sm["radarState"].leadOne.dRel = 5.6 + planner.update(sm, make_toggles(model_version)) + + assert not planner.radar_standstill_gap_settle_active + assert planner.output_should_stop + assert planner.output_a_target <= 0.0 + + +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) +def test_stationary_gap_settle_never_uses_vision_only_lead(model_version): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.0) + sm = make_sm( + 0.0, + desired_accel=-0.12, + min_accel=-0.5, + experimental_mode=True, + tracking_lead=False, + lead_one=make_lead(status=True, d_rel=6.4, v_lead=0.0, a_lead=0.0, radar=False, model_prob=1.0), + ) + sm["carState"].standstill = True + sm["controlsState"].longControlState = LongCtrlState.stopping + sm["modelV2"].action.shouldStop = True + + frames = int(round(longitudinal_planner_module.RADAR_STANDSTILL_GAP_SETTLE_CONFIRM_TIME / planner.dt)) + 2 + for _ in range(frames): + planner.update(sm, make_toggles(model_version)) + + assert not planner.radar_standstill_gap_settle_active + assert planner.output_should_stop + + @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_moving_lead_applies_resume_floor_once_stop_clears(model_version): v_ego = 0.0 diff --git a/selfdrive/controls/tests/test_speed_limit_controller.py b/selfdrive/controls/tests/test_speed_limit_controller.py index 410294b27..52d5a3781 100644 --- a/selfdrive/controls/tests/test_speed_limit_controller.py +++ b/selfdrive/controls/tests/test_speed_limit_controller.py @@ -67,10 +67,10 @@ def make_toggles(**overrides): return SimpleNamespace(**defaults) -def make_sm(*, gas_pressed, enabled=True, accel_pressed=False, decel_pressed=False): +def make_sm(*, gas_pressed, enabled=True, accel_pressed=False, decel_pressed=False, long_active=True, v_cruise_kph=255.0): return { - "carControl": SimpleNamespace(longActive=True), - "carState": SimpleNamespace(gasPressed=gas_pressed, steeringAngleDeg=0.0), + "carControl": SimpleNamespace(longActive=long_active), + "carState": SimpleNamespace(gasPressed=gas_pressed, steeringAngleDeg=0.0, vCruise=v_cruise_kph), "liveParameters": SimpleNamespace(angleOffsetDeg=0.0), "mapdOut": SimpleNamespace(nextSpeedLimitDistance=0.0, nextSpeedLimit=0.0, speedLimit=0.0, waySelectionType=0), "selfdriveState": SimpleNamespace(enabled=enabled), @@ -104,7 +104,7 @@ def test_large_vision_delta_requires_three_detector_frames(): controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(15)) controller.starpilot_planner.params_memory.put_float("VisionSpeedLimitSupportSpeed", mph(15)) controller.starpilot_planner.params_memory.put_int("VisionSpeedLimitSupportCount", 2) - sm = make_sm(gas_pressed=False) + sm = make_sm(gas_pressed=False, v_cruise_kph=75 * CV.MPH_TO_KPH) controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(75), mph(70), sm) assert controller.target == 0 @@ -124,7 +124,7 @@ def test_normal_vision_delta_keeps_fast_path(): ) try: controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(50)) - sm = make_sm(gas_pressed=False) + sm = make_sm(gas_pressed=False, v_cruise_kph=75 * CV.MPH_TO_KPH) controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(75), mph(70), sm) assert controller.target == pytest.approx(mph(50)) @@ -133,6 +133,84 @@ def test_normal_vision_delta_keeps_fast_path(): controller.shutdown() +def test_inactive_valid_cruise_still_applies_large_delta_guard(): + controller = make_controller( + speed_limit_priority1="Vision", + vision_speed_limit_detection=True, + ) + try: + controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(20)) + sm = make_sm(gas_pressed=False, long_active=False, v_cruise_kph=60 * CV.MPH_TO_KPH) + + controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(60), mph(25), sm) + assert controller.target == 0 + assert controller.source == "None" + finally: + controller.shutdown() + + +def test_unset_active_cruise_uses_vehicle_speed(): + controller = make_controller( + speed_limit_priority1="Vision", + vision_speed_limit_detection=True, + ) + try: + controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(55)) + sm = make_sm(gas_pressed=False) + + controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(90), mph(50), sm) + assert controller.target == pytest.approx(mph(55)) + assert controller.source == "Vision" + finally: + controller.shutdown() + + +def test_unset_cruise_applies_vehicle_speed_large_delta_guard(): + controller = make_controller( + speed_limit_priority1="Vision", + vision_speed_limit_detection=True, + ) + try: + controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(15)) + controller.starpilot_planner.params_memory.put_float("VisionSpeedLimitSupportSpeed", mph(15)) + controller.starpilot_planner.params_memory.put_int("VisionSpeedLimitSupportCount", 2) + sm = make_sm(gas_pressed=False) + + controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(90), mph(75), sm) + assert controller.target == 0 + assert controller.source == "None" + + controller.starpilot_planner.params_memory.put_int("VisionSpeedLimitSupportCount", 3) + controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(90), mph(75), sm) + assert controller.target == pytest.approx(mph(15)) + assert controller.source == "Vision" + finally: + controller.shutdown() + + +def test_display_only_applies_large_delta_guard(): + controller = make_controller( + speed_limit_priority1="Vision", + vision_speed_limit_detection=True, + ) + try: + controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(15)) + controller.starpilot_planner.params_memory.put_float("VisionSpeedLimitSupportSpeed", mph(15)) + controller.starpilot_planner.params_memory.put_int("VisionSpeedLimitSupportCount", 1) + sm = make_sm(gas_pressed=False, v_cruise_kph=75 * CV.MPH_TO_KPH) + + controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(75), mph(70), sm, display_only=True) + assert controller.target == 0 + assert controller.source == "None" + + controller.starpilot_planner.params_memory.put_int("VisionSpeedLimitSupportCount", 3) + controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(75), mph(70), sm, display_only=True) + assert controller.target == pytest.approx(mph(15)) + assert controller.source == "Vision" + finally: + controller.shutdown() + + def test_new_source_limit_clears_override_until_gas_release(): controller = make_controller() try: diff --git a/selfdrive/modeld/compile_modeld.py b/selfdrive/modeld/compile_modeld.py index a2c9a5ce0..2867492bc 100644 --- a/selfdrive/modeld/compile_modeld.py +++ b/selfdrive/modeld/compile_modeld.py @@ -4,6 +4,7 @@ import atexit import math import os import pickle +import shutil import tempfile import time from collections import namedtuple @@ -472,13 +473,12 @@ def _parse_size(value): return int(width), int(height) -def read_file_chunked_to_shm(path): - from openpilot.common.file_chunker import read_file_chunked - from openpilot.system.hardware.hw import Paths +def read_file_chunked_to_disk(path): + from openpilot.common.file_chunker import open_file_chunked - with tempfile.NamedTemporaryFile(prefix="compile_modeld_", dir=Paths.shm_path(), delete=False) as output: - output.write(read_file_chunked(path)) - temporary_path = output.name + temporary_path = f"{path}.unchunked" + with open(temporary_path, "wb") as output, open_file_chunked(path) as source: + shutil.copyfileobj(source, output) atexit.register(lambda: os.path.exists(temporary_path) and os.remove(temporary_path)) return temporary_path @@ -538,7 +538,7 @@ def main(): if args.model_type == "supercombo": if not args.supercombo_onnx: parser.error("--supercombo-onnx is required for supercombo") - model_path = read_file_chunked_to_shm(args.supercombo_onnx) + model_path = read_file_chunked_to_disk(args.supercombo_onnx) model_runner = OnnxRunner(model_path) output["metadata"]["model"] = make_metadata_dict(model_path) validate_metadata(output["metadata"]["model"]) @@ -565,8 +565,8 @@ def main(): if args.model_type == "vision_multi_policy" and not policy_paths: parser.error("vision_multi_policy requires at least one policy ONNX") - vision_path = read_file_chunked_to_shm(args.vision_onnx) - resolved_policy_paths = {key: read_file_chunked_to_shm(path) for key, path in policy_paths.items()} + vision_path = read_file_chunked_to_disk(args.vision_onnx) + resolved_policy_paths = {key: read_file_chunked_to_disk(path) for key, path in policy_paths.items()} vision_runner = OnnxRunner(vision_path) policy_runners = {key: OnnxRunner(path) for key, path in resolved_policy_paths.items()} output["metadata"]["vision"] = make_metadata_dict(vision_path) diff --git a/selfdrive/ui/layouts/settings/device.py b/selfdrive/ui/layouts/settings/device.py index f0a8d6d78..dc9a6c3fe 100644 --- a/selfdrive/ui/layouts/settings/device.py +++ b/selfdrive/ui/layouts/settings/device.py @@ -277,16 +277,12 @@ class DeviceLayout(Widget): self._reset_calib_btn.set_description(desc) def _reboot_prompt(self): - if ui_state.engaged: - gui_app.push_widget(alert_dialog(tr("Disengage to Reboot"))) - return - dialog = ConfirmDialog(tr("Are you sure you want to reboot?"), tr("Reboot"), callback=self._perform_reboot) gui_app.push_widget(dialog) - def _perform_reboot(self, result: int): - if not ui_state.engaged and result == DialogResult.CONFIRM: - self._params.put_bool_nonblocking("DoReboot", True) + def _perform_reboot(self, result: DialogResult): + if result == DialogResult.CONFIRM: + self._params.put_bool("DoUserReboot", True) def _power_off_prompt(self): if ui_state.engaged: diff --git a/selfdrive/ui/layouts/settings/tests/test_device_reboot.py b/selfdrive/ui/layouts/settings/tests/test_device_reboot.py new file mode 100644 index 000000000..137f5cf2f --- /dev/null +++ b/selfdrive/ui/layouts/settings/tests/test_device_reboot.py @@ -0,0 +1,45 @@ +from openpilot.selfdrive.ui.layouts.settings import device +from openpilot.system.ui.widgets import DialogResult + + +class FakeParams: + def __init__(self): + self.writes = [] + + def put_bool(self, key, value): + self.writes.append((key, value)) + + def put_bool_nonblocking(self, *_args, **_kwargs): + raise AssertionError("reboot command must be written synchronously") + + +def _device_layout(): + layout = device.DeviceLayout.__new__(device.DeviceLayout) + layout._params = FakeParams() + return layout + + +def test_confirmed_user_reboot_is_written_synchronously(): + layout = _device_layout() + + layout._perform_reboot(DialogResult.CONFIRM) + + assert layout._params.writes == [("DoUserReboot", True)] + + +def test_confirmed_user_reboot_is_allowed_onroad(monkeypatch): + layout = _device_layout() + + monkeypatch.setattr(device.ui_state, "started", True) + monkeypatch.setattr(device.ui_state, "ignition", True) + layout._perform_reboot(DialogResult.CONFIRM) + + assert layout._params.writes == [("DoUserReboot", True)] + + +def test_cancelled_user_reboot_is_ignored(): + layout = _device_layout() + + layout._perform_reboot(DialogResult.CANCEL) + + assert layout._params.writes == [] diff --git a/starpilot/assets/tests/test_model_pipeline.py b/starpilot/assets/tests/test_model_pipeline.py index 20ad21504..f10a69a29 100644 --- a/starpilot/assets/tests/test_model_pipeline.py +++ b/starpilot/assets/tests/test_model_pipeline.py @@ -1,8 +1,13 @@ import hashlib +import io import json +import sys +from pathlib import Path from scripts import model_compiler from scripts.model_compiler import split_oversized_artifact +from openpilot.common import file_chunker +from openpilot.selfdrive.modeld import compile_modeld from openpilot.starpilot.assets import download_functions from openpilot.starpilot.assets import model_manager from openpilot.starpilot.assets.model_manager import MANIFEST_CANDIDATES, ModelManager @@ -60,6 +65,30 @@ def test_external_gpu_compilation_is_opt_in(tmp_path, monkeypatch): assert all(flag not in external_kwargs["env"] for flag in ("IMAGE", "NOLOCALS", "OPENPILOT_HACKS")) +def test_gpu_is_external_gpu_cli_alias(monkeypatch): + monkeypatch.setattr(sys, "argv", ["models", "--model", "large", "--gpu"]) + args = model_compiler.parse_args() + assert args.external_gpu + + +def test_fat_onnx_is_streamed_to_disk(tmp_path, monkeypatch): + payload = b"fat model" * 1024 + + class StreamingOnly(io.BytesIO): + def read(self, size=-1): + assert size >= 0, "staging must not materialize the whole ONNX" + return super().read(size) + + monkeypatch.setattr(file_chunker, "open_file_chunked", lambda _: StreamingOnly(payload)) + source = tmp_path / "big_driving_supercombo.onnx" + staged = compile_modeld.read_file_chunked_to_disk(source) + try: + assert staged == f"{source}.unchunked" + assert Path(staged).read_bytes() == payload + finally: + Path(staged).unlink(missing_ok=True) + + def test_dropbox_urls_are_direct_downloads(): url = "https://www.dropbox.com/scl/fi/id/model.pkl?rlkey=key&st=value&dl=0" normalized = download_functions.normalize_download_url(url) diff --git a/starpilot/controls/lib/conditional_experimental_mode.py b/starpilot/controls/lib/conditional_experimental_mode.py index 1f3ad5745..b13fec29e 100644 --- a/starpilot/controls/lib/conditional_experimental_mode.py +++ b/starpilot/controls/lib/conditional_experimental_mode.py @@ -58,6 +58,8 @@ class ConditionalExperimentalMode: SLOW_LEAD_MODE_RELEASE_HOLD_TIME = 1.5 SLOW_LEAD_MIN_CLOSING_SPEED = 0.75 SLOW_LEAD_CLEAR_FASTER_FACTOR = 0.5 + SLOW_RADAR_LEAD_TRIGGER_MAX_DISTANCE_TIME = 2.5 + SLOW_RADAR_LEAD_TRIGGER_MIN_DISTANCE = 40.0 POST_STOP_LAUNCH_TRIGGER_SUPPRESS_TIME = 2.0 TURN_STOP_LIGHT_VETO_MAX_SPEED = 15 * CV.MPH_TO_MS TURN_STOP_LIGHT_VETO_STEERING_ANGLE = 45.0 @@ -295,6 +297,7 @@ class ConditionalExperimentalMode: lead_distance = float(getattr(lead, "dRel", float("inf"))) lead_speed = float(getattr(lead, "vLead", float("inf"))) lead_prob = float(getattr(lead, "modelProb", 1.0)) + lead_radar = bool(getattr(lead, "radar", False)) closing_speed = max(0.0, v_ego - lead_speed) min_closing_speed = max(self.SLOW_LEAD_MIN_CLOSING_SPEED, 0.04 * v_ego) @@ -302,7 +305,16 @@ class ConditionalExperimentalMode: self.clear_slow_lead_state(tracking_lead) return - slower_lead = starpilot_toggles.conditional_slower_lead and self.starpilot_planner.starpilot_following.slower_lead + radar_slow_lead_in_range = bool( + not lead_radar or + lead_distance < max(self.SLOW_RADAR_LEAD_TRIGGER_MIN_DISTANCE, + v_ego * self.SLOW_RADAR_LEAD_TRIGGER_MAX_DISTANCE_TIME) + ) + slower_lead = bool( + starpilot_toggles.conditional_slower_lead and + self.starpilot_planner.starpilot_following.slower_lead and + radar_slow_lead_in_range + ) stopped_lead = bool( starpilot_toggles.conditional_stopped_lead and lead_status and @@ -311,6 +323,7 @@ class ConditionalExperimentalMode: ) vision_slow_lead_candidate = bool( lead_status and + not lead_radar and lead_prob >= self.SLOW_LEAD_CONTINUITY_MIN_MODEL_PROB and lead_distance < max(40.0, v_ego * self.SLOW_LEAD_CONTINUITY_MAX_DISTANCE_TIME) and closing_speed >= min_closing_speed and diff --git a/starpilot/controls/lib/speed_limit_controller.py b/starpilot/controls/lib/speed_limit_controller.py index 8c9257013..cb9af6ca9 100644 --- a/starpilot/controls/lib/speed_limit_controller.py +++ b/starpilot/controls/lib/speed_limit_controller.py @@ -9,6 +9,7 @@ from concurrent.futures import ThreadPoolExecutor from openpilot.common.constants import CV from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET from cereal import custom from openpilot.starpilot.common.starpilot_utilities import calculate_bearing_offset, calculate_distance_to_point, is_url_pingable @@ -36,7 +37,7 @@ OFFSET_MAP_METRIC = [ ] SLC_OVERRIDE_DISABLE_CLEAR_TIME = 0.75 -VISION_LARGE_SET_SPEED_DELTA = 30 * CV.MPH_TO_MS +VISION_LARGE_REFERENCE_SPEED_DELTA = 30 * CV.MPH_TO_MS VISION_LARGE_SET_SPEED_MIN_SUPPORT = 3 VISION_SUPPORT_SPEED_TOLERANCE = 0.5 * CV.MPH_TO_MS @@ -332,7 +333,14 @@ class SpeedLimitController: vision_enabled = getattr(self.starpilot_toggles, "vision_speed_limit_detection", False) self.vision_limit = self.starpilot_planner.params_memory.get_float("VisionSpeedLimit") if vision_enabled else 0 usable_vision_limit = self.vision_limit - if usable_vision_limit > 0 and v_cruise > 0 and abs(usable_vision_limit - v_cruise) >= VISION_LARGE_SET_SPEED_DELTA: + # The planner clamps V_CRUISE_UNSET to V_CRUISE_MAX, so plausibility must use the raw selected speed. + raw_set_speed_kph = float(sm["carState"].vCruise) + selected_set_speed = raw_set_speed_kph * CV.KPH_TO_MS if 0 < raw_set_speed_kph < V_CRUISE_UNSET else 0 + reference_speed = selected_set_speed if selected_set_speed > 0 else max(float(v_ego), 0) + if ( + usable_vision_limit > 0 and reference_speed > 0 and + abs(usable_vision_limit - reference_speed) >= VISION_LARGE_REFERENCE_SPEED_DELTA + ): support_count = self.starpilot_planner.params_memory.get_int("VisionSpeedLimitSupportCount") support_speed = self.starpilot_planner.params_memory.get_float("VisionSpeedLimitSupportSpeed") if support_count < VISION_LARGE_SET_SPEED_MIN_SUPPORT or abs(support_speed - usable_vision_limit) > VISION_SUPPORT_SPEED_TOLERANCE: diff --git a/starpilot/system/speed_limit_vision.py b/starpilot/system/speed_limit_vision.py index 012ca235d..3bdc74746 100644 --- a/starpilot/system/speed_limit_vision.py +++ b/starpilot/system/speed_limit_vision.py @@ -159,6 +159,7 @@ SPEED_LIMIT_CLASSES = { VALID_SPEED_LIMITS_MPH = set(range(5, 125, 5)) MIN_PUBLISHABLE_SPEED_LIMIT_MPH = 5 +MAX_IMPERIAL_PUBLISHABLE_SPEED_LIMIT_MPH = 80 LEGACY_MODEL_PATH = Path(__file__).resolve().parents[1] / "assets" / "vision_models" / "speed_limit_vision.onnx" US_DETECTOR_MODEL_PATH = Path(__file__).resolve().parents[1] / "assets" / "vision_models" / "speed_limit_us_detector.onnx" US_CLASSIFIER_MODEL_PATH = Path(__file__).resolve().parents[1] / "assets" / "vision_models" / "speed_limit_us_value_classifier.onnx" @@ -1307,6 +1308,8 @@ class SpeedLimitVisionDaemon: return None if detection.speed_limit_mph < MIN_PUBLISHABLE_SPEED_LIMIT_MPH: return None + if not getattr(self, "is_metric", False) and detection.speed_limit_mph > MAX_IMPERIAL_PUBLISHABLE_SPEED_LIMIT_MPH: + return None return detection def _is_regulatory_speed_sign(self, sign_crop): diff --git a/starpilot/system/tests/test_speed_limit_vision.py b/starpilot/system/tests/test_speed_limit_vision.py index 6e4460a04..8c0385371 100644 --- a/starpilot/system/tests/test_speed_limit_vision.py +++ b/starpilot/system/tests/test_speed_limit_vision.py @@ -140,6 +140,7 @@ def test_extended_classifier_values_require_high_confidence(monkeypatch, confide def test_five_mph_detection_is_publishable(): daemon = SpeedLimitVisionDaemon.__new__(SpeedLimitVisionDaemon) + daemon.is_metric = False detection = daemon._publishable_detection(slv.Detection(5, 0.95)) @@ -147,6 +148,26 @@ def test_five_mph_detection_is_publishable(): assert detection.speed_limit_mph == 5 +@pytest.mark.parametrize(("speed_limit", "expected"), ((80, 80), (90, None), (100, None))) +def test_imperial_detection_blocks_speeds_above_80(speed_limit, expected): + daemon = SpeedLimitVisionDaemon.__new__(SpeedLimitVisionDaemon) + daemon.is_metric = False + + detection = daemon._publishable_detection(slv.Detection(speed_limit, 0.99)) + + assert (detection.speed_limit_mph if detection else None) == expected + + +def test_metric_detection_allows_100(): + daemon = SpeedLimitVisionDaemon.__new__(SpeedLimitVisionDaemon) + daemon.is_metric = True + + detection = daemon._publishable_detection(slv.Detection(100, 0.99)) + + assert detection is not None + assert detection.speed_limit_mph == 100 + + def test_detection_support_counts_independent_frames(): daemon = publishing_daemon(False) daemon.followup_until = 0.0 diff --git a/system/manager/manager.py b/system/manager/manager.py index cea33295b..37e2c15af 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -147,8 +147,8 @@ def update_nav_offroad_clear_state(params, started: bool, tracked_destination, t return tracked_destination, tracked_started_at -def should_defer_reboot(started: bool, ignition: bool) -> bool: - return started or ignition +def should_defer_reboot(param: str, started: bool, ignition: bool) -> bool: + return param == "DoReboot" and (started or ignition) def _merge_starpilot_stat_values(existing, incoming, key=None): @@ -1077,8 +1077,8 @@ def manager_thread() -> None: # Exit main loop when uninstall/shutdown/reboot is needed shutdown = False - for param in ("DoUninstall", "DoShutdown", "DoReboot"): - if param == "DoReboot" and should_defer_reboot(started, ignition): + for param in ("DoUninstall", "DoShutdown", "DoReboot", "DoUserReboot"): + if should_defer_reboot(param, started, ignition): if params.get_bool(param): if not warned_onroad_reboot: cloudlog.warning("ignoring DoReboot while started or ignition is on; deferring until offroad") @@ -1117,7 +1117,7 @@ def main() -> None: if params.get_bool("DoUninstall"): cloudlog.warning("uninstalling") uninstall_starpilot() - elif params.get_bool("DoReboot"): + elif params.get_bool("DoReboot") or params.get_bool("DoUserReboot"): cloudlog.warning("reboot") HARDWARE.reboot() elif params.get_bool("DoShutdown"): diff --git a/system/manager/test/test_manager.py b/system/manager/test/test_manager.py index 084e6d7bc..4d80399fd 100644 --- a/system/manager/test/test_manager.py +++ b/system/manager/test/test_manager.py @@ -106,10 +106,11 @@ class FakeManagedProcess: return SimpleNamespace(name="ui") -def test_reboot_guard_includes_raw_ignition_state(): - assert manager.should_defer_reboot(started=True, ignition=False) - assert manager.should_defer_reboot(started=False, ignition=True) - assert not manager.should_defer_reboot(started=False, ignition=False) +def test_reboot_guard_only_defers_automatic_requests(): + assert manager.should_defer_reboot("DoReboot", started=True, ignition=False) + assert manager.should_defer_reboot("DoReboot", started=False, ignition=True) + assert not manager.should_defer_reboot("DoReboot", started=False, ignition=False) + assert not manager.should_defer_reboot("DoUserReboot", started=True, ignition=True) class TestManager: