From 27f73a6f08295284823f9da54fdeea6e2df8541b Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 20 Jul 2026 11:26:07 -0500 Subject: [PATCH] Kirkland Rotisserie --- common/params_keys.h | 2 +- docs/contributing/contribute.md | 102 ++++++++++++++++++ docs/how-to/laptop-device-build.md | 2 +- mkdocs.yml | 4 +- opendbc_repo/opendbc/car/gm/carcontroller.py | 35 ++++-- opendbc_repo/opendbc/car/gm/gmcan.py | 29 +++++ opendbc_repo/opendbc/car/gm/tests/test_gm.py | 42 ++++++++ selfdrive/controls/lib/latcontrol_torque.py | 5 + .../controls/lib/latcontrol_vehicle_tunes.py | 31 ++++-- selfdrive/controls/tests/test_latcontrol.py | 36 ++++++- selfdrive/locationd/paramsd.py | 15 ++- selfdrive/locationd/test/test_paramsd.py | 27 ++++- .../ui/layouts/settings/starpilot/lateral.py | 6 +- .../ui/onroad/starpilot/developer_sidebar.py | 14 ++- starpilot/common/starpilot_variables.py | 6 +- starpilot/navigation/mapd_wrapper.py | 38 +++++-- starpilot/navigation/test_mapd_wrapper.py | 77 ++++++++++++- 17 files changed, 427 insertions(+), 44 deletions(-) create mode 100644 docs/contributing/contribute.md diff --git a/common/params_keys.h b/common/params_keys.h index c5f0f41a1..fee0be79e 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -367,7 +367,7 @@ inline static std::unordered_map keys = { {"KonikDongleId", {PERSISTENT, STRING, "", "", 0}}, {"KonikMinutes", {PERSISTENT, INT, "0", "0", 0}}, {"LaneChanges", {PERSISTENT, BOOL, "1", "1", 0, SETTINGS_SIMPLE}}, - {"LaneChangeSmoothing", {PERSISTENT, INT, "10", "10", 1, SETTINGS_SIMPLE}}, + {"LaneChangeSmoothing", {PERSISTENT, INT, "5", "10", 1, SETTINGS_SIMPLE}}, {"LaneChangeTime", {PERSISTENT, FLOAT, "1.0", "0.0", 1, SETTINGS_SIMPLE}}, {"LaneDetectionWidth", {PERSISTENT, FLOAT, "0.0", "0.0", 1, SETTINGS_SIMPLE}}, {"LaneLinesColor", {PERSISTENT, STRING, "", "", 2, SETTINGS_SIMPLE}}, diff --git a/docs/contributing/contribute.md b/docs/contributing/contribute.md new file mode 100644 index 000000000..42712d950 --- /dev/null +++ b/docs/contributing/contribute.md @@ -0,0 +1,102 @@ +# Contribute to StarPilot + +Contributions are welcome. StarPilot is driver-assistance software, so changes should be focused, testable, and safe for vehicles outside the change's intended scope. + +## Pull requests + +Open all pull requests against the **`Dom` branch**. `Dom` is StarPilot's testing and integration branch; do not target the release branch directly. + +A pull request should: + +* have one clear purpose and contain only changes needed for that purpose; +* explain what changed, why it changed, and how it was tested; +* link the relevant issue or feedback item when one exists; +* avoid unrelated cleanup, refactors, dependency changes, and formatting churn; and +* pass the existing tests and checks. + +Keep commits reviewable and update documentation when behavior or configuration changes. If a change needs a large refactor, separate that work from the behavior change when practical. + +### Vehicle-specific changes + +Do your best to ensure a vehicle-specific change affects only the intended vehicle, platform, or brand. Prefer the narrowest appropriate condition instead of changing shared behavior for every vehicle. + +Tests should prove both sides of that boundary: + +* the affected vehicle receives the new or corrected behavior; and +* an unaffected vehicle, platform, brand, or configuration retains the existing behavior. + +Include the vehicle and hardware used for on-road or bench testing in the pull request. Hardware testing is valuable, but it does not replace an automated regression test when the behavior can be tested in code. + +## Development environment + +StarPilot keeps host-native development tools separate from device-target builds. Run the setup and development commands from the repository root. + +Install the Python dependencies before starting: + +```bash +tools/install_python_dependencies.sh +``` + +On Ubuntu, `tools/ubuntu_setup.sh` installs both the system and Python dependencies. The development tools also require `uv`. + +Use `./dev` for host-native tools. It creates and reuses an isolated environment under `.host_runtime/`, keeping host build artifacts out of the working tree: + +```bash +./dev replay +./dev cabana +./dev plotjuggler +./dev shell +``` + +For desktop UI work, use `./c3`, `./c4`, or `./raybig`. These commands use the same isolated host environment. + +Use `./build` when you need comma-compatible device artifacts. This is the expected validation for changes that affect compiled device code or runtime behavior: + +```bash +./build +``` + +Device builds require Docker Desktop or Podman with Linux/aarch64 support and a configured comma sysroot. See the [laptop device-build guide](../how-to/laptop-device-build.md) for setup instructions and the [complete StarPilot development workflow](https://github.com/firestar5683/StarPilot/blob/Dom/tools/STARPILOT_DEVELOPMENT.md) for all host commands and troubleshooting. + +## Code formatting + +Match the style of the code around your change and do not reformat unrelated files. Python formatting and lint rules are defined in `pyproject.toml`; the project uses two-space indentation and Ruff. + +Run Ruff on changed Python files while developing: + +```bash +ruff check path/to/changed_file.py +ruff format --check path/to/changed_file.py +``` + +Before submitting, run the repository lint checks from the project root: + +```bash +./scripts/lint/lint.sh +``` + +## Testing standards + +Every behavior change or bug fix should include focused automated tests when practical. Recent StarPilot tests favor small regression cases that construct the relevant state, exercise one behavior, and assert the exact result. + +At minimum: + +1. Add or update a test that would fail without the change. +2. Cover important boundaries, modes, and disabled states related to the change. +3. For vehicle-specific behavior, add a negative case showing the change does not bleed into an unaffected vehicle or configuration. +4. Run the directly affected test module and the existing tests for the affected subsystem. +5. Ensure the pull request passes all existing CI checks before it is ready to merge. + +Run a focused test module with pytest: + +```bash +pytest path/to/test_file.py +``` + +Run the full test suite when your development environment supports it: + +```bash +pytest +``` + +If a test requires special hardware or cannot run in your environment, say so in the pull request and document the closest validation you completed. diff --git a/docs/how-to/laptop-device-build.md b/docs/how-to/laptop-device-build.md index 8948d9b11..8bddc4f26 100644 --- a/docs/how-to/laptop-device-build.md +++ b/docs/how-to/laptop-device-build.md @@ -2,7 +2,7 @@ This flow builds **device-target (`larch64`) binaries on your laptop** using a Linux/aarch64 container and a synced comma sysroot. -For the full StarPilot branch workflow, including host-native shorthand tools such as `./dev`, `./c3`, `./c4`, and `./raybig`, see [tools/STARPILOT_DEVELOPMENT.md](../../tools/STARPILOT_DEVELOPMENT.md). +For the full StarPilot branch workflow, including host-native shorthand tools such as `./dev`, `./c3`, `./c4`, and `./raybig`, see the [StarPilot development guide](https://github.com/firestar5683/StarPilot/blob/Dom/tools/STARPILOT_DEVELOPMENT.md). ## Prerequisites diff --git a/mkdocs.yml b/mkdocs.yml index 550f807ac..9ede5c8b1 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -32,10 +32,10 @@ nav: - What is a car port?: car-porting/what-is-a-car-port.md - Porting a car brand: car-porting/brand-port.md - Porting a car model: car-porting/model-port.md - - Contributing: + - Contribute: + - Contributing Guide: contributing/contribute.md - Roadmap: contributing/roadmap.md #- Architecture: contributing/architecture.md - - Contributing Guide →: https://github.com/commaai/openpilot/blob/master/docs/CONTRIBUTING.md - Links: - Blog →: https://blog.comma.ai - Bounties →: https://comma.ai/bounties diff --git a/opendbc_repo/opendbc/car/gm/carcontroller.py b/opendbc_repo/opendbc/car/gm/carcontroller.py index 4857d9863..918bf2300 100644 --- a/opendbc_repo/opendbc/car/gm/carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/carcontroller.py @@ -479,6 +479,9 @@ class CarController(CarControllerBase): self.last_steer_frame = 0 self.last_button_frame = 0 self.cancel_counter = 0 + self.xt4_cc_button_burst_remaining = 0 + self.xt4_cc_button_burst_button = CruiseButtons.INIT + self.xt4_cc_button_burst_last_counter = -1 self.lka_steering_cmd_counter = 0 self.lka_icon_status_last = (False, False) @@ -930,6 +933,16 @@ class CarController(CarControllerBase): can_sends.append(gmcan.create_ecm_cruise_control_command( self.packer_pt, CanBus.POWERTRAIN, True, hud_v_cruise * CV.MS_TO_KPH)) + xt4_cc_button_spam = ( + self.CP.carFingerprint == CAR.CADILLAC_XT4_CC and + should_send_cc_button_spam(self.CP, CC, CS) + ) + if xt4_cc_button_spam: + can_sends.extend(gmcan.create_gm_cc_spam_command(self.packer_pt, self, CS, actuators, starpilot_toggles)) + elif self.CP.carFingerprint == CAR.CADILLAC_XT4_CC: + self.xt4_cc_button_burst_remaining = 0 + self.xt4_cc_button_burst_button = CruiseButtons.INIT + if self.CP.openpilotLongitudinalControl: # Gas/regen, brakes, and UI commands - all at 25Hz if self.frame % 4 == 0: @@ -1087,17 +1100,19 @@ class CarController(CarControllerBase): if self.CP.flags & GMFlags.CC_LONG.value: if should_send_cc_button_spam(self.CP, CC, CS): - # Using extend instead of append since the message is only sent intermittently - can_sends.extend(gmcan.create_gm_cc_spam_command(self.packer_pt, self, CS, actuators, starpilot_toggles)) - elif (CS.out.cruiseState.enabled and CC.enabled and self.frame % 52 == 0 and + if self.CP.carFingerprint != CAR.CADILLAC_XT4_CC: + # Using extend instead of append since the message is only sent intermittently + can_sends.extend(gmcan.create_gm_cc_spam_command(self.packer_pt, self, CS, actuators, starpilot_toggles)) + else: + if (CS.out.cruiseState.enabled and CC.enabled and self.frame % 52 == 0 and CS.cruise_buttons == CruiseButtons.UNPRESS and CS.out.gasPressed and CS.out.cruiseState.speed < CS.out.vEgo < hud_v_cruise): - if self.CP.carFingerprint == CAR.CHEVROLET_MALIBU_HYBRID_CC: - can_sends.append(gmcan.create_buttons_malibu( - self.packer_pt, CanBus.POWERTRAIN, CruiseButtons.DECEL_SET, - self.malibu_button_phase, CS.steering_button_prefix)) - self.malibu_button_phase = (self.malibu_button_phase + 1) % 4 - else: - can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.POWERTRAIN, (CS.buttons_counter + 1) % 4, CruiseButtons.DECEL_SET)) + if self.CP.carFingerprint == CAR.CHEVROLET_MALIBU_HYBRID_CC: + can_sends.append(gmcan.create_buttons_malibu( + self.packer_pt, CanBus.POWERTRAIN, CruiseButtons.DECEL_SET, + self.malibu_button_phase, CS.steering_button_prefix)) + self.malibu_button_phase = (self.malibu_button_phase + 1) % 4 + else: + can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.POWERTRAIN, (CS.buttons_counter + 1) % 4, CruiseButtons.DECEL_SET)) if self.CP.enableGasInterceptorDEPRECATED: can_sends.append(create_gas_interceptor_command(self.packer_pt, interceptor_gas_cmd, idx)) if bolt_acc_pedal_friction_experiment: diff --git a/opendbc_repo/opendbc/car/gm/gmcan.py b/opendbc_repo/opendbc/car/gm/gmcan.py index d0a70caef..f190c164c 100644 --- a/opendbc_repo/opendbc/car/gm/gmcan.py +++ b/opendbc_repo/opendbc/car/gm/gmcan.py @@ -18,6 +18,7 @@ MALIBU_BUTTON_MAP = { } ACC_CRUISE_STATE_ADAPTIVE = 2 +XT4_CC_BUTTON_BURST_FRAMES = 6 def malibu_phase_map_for_button(button): @@ -318,6 +319,34 @@ def create_gm_cc_spam_command(packer, controller, CS, actuators, starpilot_toggl cruise_btn = CruiseButtons.INIT controller.apply_speed = speed_setpoint + if CS.CP.carFingerprint == CAR.CADILLAC_XT4_CC: + if cruise_btn == CruiseButtons.INIT: + controller.xt4_cc_button_burst_remaining = 0 + controller.xt4_cc_button_burst_button = CruiseButtons.INIT + return [] + + if (controller.xt4_cc_button_burst_remaining > 0 and + controller.xt4_cc_button_burst_button != cruise_btn): + controller.xt4_cc_button_burst_remaining = 0 + + if controller.xt4_cc_button_burst_remaining == 0: + if (controller.frame - controller.last_button_frame) * DT_CTRL <= rate: + return [] + controller.last_button_frame = controller.frame + controller.xt4_cc_button_burst_button = cruise_btn + controller.xt4_cc_button_burst_remaining = XT4_CC_BUTTON_BURST_FRAMES + controller.xt4_cc_button_burst_last_counter = -1 + + # XT4 physical taps hold the button for 5-7 consecutive 33 Hz frames. + # 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 [] + + controller.xt4_cc_button_burst_last_counter = CS.buttons_counter + controller.xt4_cc_button_burst_remaining -= 1 + idx = (CS.buttons_counter + 1) % 4 + return [create_buttons(packer, CanBus.POWERTRAIN, idx, controller.xt4_cc_button_burst_button)] + # Check rlogs closely - our message shouldn't show up on the pt bus for us # Or bus 2, since we're forwarding... but I think it does if (cruise_btn != CruiseButtons.INIT) and ((controller.frame - controller.last_button_frame) * DT_CTRL > rate): diff --git a/opendbc_repo/opendbc/car/gm/tests/test_gm.py b/opendbc_repo/opendbc/car/gm/tests/test_gm.py index bf957b102..8aad98485 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_gm.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_gm.py @@ -554,6 +554,48 @@ class TestGMCarController: assert [msg[2] for msg in msgs] == [0] + def test_xt4_cc_redneck_spam_matches_physical_button_burst(self): + packer = CANPacker(DBC[CAR.CADILLAC_XT4_CC][Bus.pt]) + controller = SimpleNamespace( + frame=int(0.3 / DT_CTRL), + last_button_frame=0, + apply_speed=0, + malibu_button_phase=0, + xt4_cc_button_burst_remaining=0, + xt4_cc_button_burst_button=CruiseButtons.INIT, + xt4_cc_button_burst_last_counter=-1, + ) + cs = SimpleNamespace( + CP=SimpleNamespace( + carFingerprint=CAR.CADILLAC_XT4_CC, + flags=GMFlags.CC_LONG.value, + minEnableSpeed=24 * CV.MPH_TO_MS, + ), + buttons_counter=0, + out=SimpleNamespace( + vEgo=25.0, + cruiseState=SimpleNamespace(speed=20.0), + ), + ) + actuators = SimpleNamespace(accel=1.0) + + dats = [] + 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)) + dats.extend(bytes(msg[1]).hex() for msg in msgs) + controller.frame += 1 + + assert dats == [ + "000000010125de", + "00000001022acd", + "00000001032fbc", + "000000010020ef", + "000000010125de", + "00000001022acd", + ] + assert controller.xt4_cc_button_burst_remaining == 0 + def test_acc_dashboard_command_preserves_raw_fcw_alert_level(self): packer = CANPacker(DBC[CAR.CHEVROLET_BOLT_ACC_2022_2023][Bus.pt]) parser = CANParser(DBC[CAR.CHEVROLET_BOLT_ACC_2022_2023][Bus.pt], [("ASCMActiveCruiseControlStatus", 0)], 0) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 08ac0b7a6..20caa74a0 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -98,6 +98,7 @@ class LatControlTorque(LatControl): self.is_kia_forte = CP.carFingerprint in KIA_FORTE_CARS self.is_kia_ev6 = CP.carFingerprint in KIA_EV6_CARS self.is_kia_carnival = CP.carFingerprint in KIA_CARNIVAL_CARS + self.is_tucson_4th_gen = CP.carFingerprint in TUCSON_4TH_GEN_CARS self.is_civic_bosch_modified = CP.carFingerprint == HONDA_CAR.HONDA_CIVIC_BOSCH and bool(CP.flags & HondaFlags.EPS_MODIFIED) self.is_silverado = CP.carFingerprint in SILVERADO_CARS self.is_gm = CP.brand == "gm" @@ -251,6 +252,7 @@ class LatControlTorque(LatControl): kia_forte_active = self.is_kia_forte kia_ev6_test_active = self.is_kia_ev6 and kia_ev6_lateral_testing_ground_active() kia_carnival_active = self.is_kia_carnival + tucson_4th_gen_active = self.is_tucson_4th_gen volt_plexy_test_active = self.is_volt_standard and volt_plexy_lateral_testing_ground_active() ioniq_5_center_taper = get_ioniq_5_center_taper_scale(setpoint, CS.vEgo) if ioniq_5_active else 1.0 prius_center_taper = get_prius_center_taper_scale(setpoint, CS.vEgo) if prius_active else 1.0 @@ -266,6 +268,7 @@ class LatControlTorque(LatControl): kia_ev6_center_taper = get_kia_ev6_center_taper_scale(setpoint, CS.vEgo) if kia_ev6_test_active else 1.0 kia_ev6_low_speed_center_taper = get_kia_ev6_low_speed_center_taper_scale(setpoint, CS.vEgo) if kia_ev6_test_active else 1.0 kia_carnival_center_taper = get_kia_carnival_center_taper_scale(setpoint, CS.vEgo) if kia_carnival_active else 1.0 + tucson_4th_gen_center_taper = get_tucson_4th_gen_center_taper_scale(setpoint, CS.vEgo) if tucson_4th_gen_active else 1.0 silverado_center_taper = get_silverado_center_taper_scale(setpoint, CS.vEgo) if self.is_silverado else 1.0 civic_bosch_modified_a_center_taper = get_civic_bosch_modified_a_center_taper_scale(setpoint, CS.vEgo) if ( self.is_civic_bosch_modified and civic_bosch_modified_a_lateral_testing_ground_active() @@ -409,6 +412,8 @@ class LatControlTorque(LatControl): output_torque *= kia_ev6_low_speed_center_taper elif kia_carnival_active: output_torque *= kia_carnival_center_taper + elif tucson_4th_gen_active: + output_torque *= tucson_4th_gen_center_taper elif self.is_silverado: output_torque *= silverado_center_taper elif kia_niro_phev_2022_active: diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 2fcd603d4..8cbf91941 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -114,6 +114,9 @@ KIA_EV6_CARS = ( KIA_CARNIVAL_CARS = ( HYUNDAI_CAR.KIA_CARNIVAL_2025, ) +TUCSON_4TH_GEN_CARS = ( + HYUNDAI_CAR.HYUNDAI_TUCSON_4TH_GEN, +) KIA_XCEED_CARS = ( HYUNDAI_CAR.KIA_XCEED_PHEV, ) @@ -333,15 +336,21 @@ KIA_NIRO_PHEV_2022_FRICTION_CALM_JERK = 0.22 KIA_NIRO_PHEV_2022_FRICTION_CALM_JERK_WIDTH = 0.06 KIA_NIRO_PHEV_2022_FRICTION_THRESHOLD_GAIN = 0.12 -KIA_CARNIVAL_CENTER_TAPER_MAX = 0.10 -KIA_CARNIVAL_CENTER_TAPER_LAT = 0.16 -KIA_CARNIVAL_CENTER_TAPER_LAT_WIDTH = 0.05 +KIA_CARNIVAL_CENTER_TAPER_MAX = 0.20 +KIA_CARNIVAL_CENTER_TAPER_LAT = 0.20 +KIA_CARNIVAL_CENTER_TAPER_LAT_WIDTH = 0.055 KIA_CARNIVAL_CENTER_TAPER_SPEED = 3.5 KIA_CARNIVAL_CENTER_TAPER_SPEED_WIDTH = 1.8 -KIA_CARNIVAL_CENTER_TAPER_SPEED_MAX = 16.0 -KIA_CARNIVAL_CENTER_TAPER_SPEED_MAX_WIDTH = 2.5 -KIA_CARNIVAL_FRICTION_THRESHOLD_GAIN = 0.18 -KIA_CARNIVAL_FRICTION_CENTER_FADE_MAX = 0.25 +KIA_CARNIVAL_CENTER_TAPER_SPEED_MAX = 14.5 +KIA_CARNIVAL_CENTER_TAPER_SPEED_MAX_WIDTH = 2.0 +KIA_CARNIVAL_FRICTION_THRESHOLD_GAIN = 0.24 +KIA_CARNIVAL_FRICTION_CENTER_FADE_MAX = 0.34 + +TUCSON_4TH_GEN_CENTER_TAPER_MAX = 0.36 +TUCSON_4TH_GEN_CENTER_TAPER_LAT = 0.28 +TUCSON_4TH_GEN_CENTER_TAPER_LAT_WIDTH = 0.055 +TUCSON_4TH_GEN_CENTER_TAPER_SPEED_MAX = 14.0 +TUCSON_4TH_GEN_CENTER_TAPER_SPEED_WIDTH = 1.5 KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT = 1.05 KIA_FORTE_FF_REDUCTION_LEFT = 0.05 @@ -1636,6 +1645,12 @@ def get_kia_carnival_friction_center_fade_scale(desired_lateral_accel: float, v_ return 1.0 - (KIA_CARNIVAL_FRICTION_CENTER_FADE_MAX * speed_weight * center_weight) +def get_tucson_4th_gen_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _sigmoid((TUCSON_4TH_GEN_CENTER_TAPER_SPEED_MAX - v_ego) / TUCSON_4TH_GEN_CENTER_TAPER_SPEED_WIDTH) + center_weight = _sigmoid((TUCSON_4TH_GEN_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / TUCSON_4TH_GEN_CENTER_TAPER_LAT_WIDTH) + return 1.0 - (TUCSON_4TH_GEN_CENTER_TAPER_MAX * speed_weight * center_weight) + + def _kia_forte_sigmoid(x: float) -> float: return _sigmoid(x) @@ -2769,7 +2784,7 @@ def get_flm_capabilities(car_fingerprint, brand: str = "", hyundai_canfd: bool = set(PRIUS_CARS) | set(BOLT_CARS) | set(VOLT_STANDARD_CARS) | set(IONIQ_5_CARS) | set(IONIQ_EV_OLD_CARS) | set(IONIQ_6_CARS) | set(SONATA_CARS) | set(SONATA_HYBRID_CARS) | set(KIA_XCEED_CARS) | set(KIA_NIRO_PHEV_2022_CARS) | set(KIA_FORTE_CARS) | set(KIA_EV6_CARS) | - set(KIA_CARNIVAL_CARS) | set(SILVERADO_CARS) + set(KIA_CARNIVAL_CARS) | set(TUCSON_4TH_GEN_CARS) | set(SILVERADO_CARS) ) rich_knobs = [name for name, meta in FLM_SUPPORTED_VEHICLE_KNOBS.items() if meta["profile"] == profile_key] return { diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 080cfd454..3d4868ee1 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -75,6 +75,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_kia_carnival_center_taper_scale, get_kia_carnival_friction_center_fade_scale, get_kia_carnival_friction_threshold, + get_tucson_4th_gen_center_taper_scale, get_kia_ev6_center_taper_scale, get_kia_ev6_ff_scale, get_kia_ev6_friction_scale, @@ -416,6 +417,7 @@ class TestLatControl: assert center_taper < turn_taper <= 1.0 assert center_taper < low_speed_taper <= 1.0 assert center_taper < highway_taper <= 1.0 + assert center_taper < 0.84 assert neighborhood_taper < 0.94 assert neighborhood_turn_taper > 0.99 @@ -425,7 +427,7 @@ class TestLatControl: center_fade = get_kia_carnival_friction_center_fade_scale(0.04, 8.5) turn_fade = get_kia_carnival_friction_center_fade_scale(0.35, 8.5) - assert center_fade < turn_fade <= 1.0 + assert center_fade < 0.75 < turn_fade <= 1.0 def test_genesis_g90_ff_scale_curve(self): assert get_genesis_g90_ff_scale(0.0, 0.0, 20.0) == 1.0 @@ -826,6 +828,38 @@ class TestLatControl: assert controller.is_kia_carnival assert lac_log.active + def test_tucson_4th_gen_low_speed_center_taper_curve(self): + low_speed_center = get_tucson_4th_gen_center_taper_scale(0.0, 8.5) + low_speed_moderate = get_tucson_4th_gen_center_taper_scale(0.30, 8.5) + low_speed_turn = get_tucson_4th_gen_center_taper_scale(0.50, 8.5) + high_speed_center = get_tucson_4th_gen_center_taper_scale(0.0, 20.0) + + assert low_speed_center < 0.70 + assert low_speed_center < low_speed_moderate < low_speed_turn + assert low_speed_turn > 0.98 + assert high_speed_center > 0.98 + + def test_tucson_4th_gen_default_update_path(self): + controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_TUCSON_4TH_GEN) + CS.vEgo = 8.5 + + _, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) + + assert controller.is_tucson_4th_gen + assert lac_log.active + + def test_tucson_4th_gen_tapers_low_speed_output(self, monkeypatch): + tapered_controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_TUCSON_4TH_GEN) + CS.vEgo = 8.5 + tapered_output, _, _ = tapered_controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) + + monkeypatch.setattr(latcontrol_torque, "get_tucson_4th_gen_center_taper_scale", lambda *_args: 1.0) + base_controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_TUCSON_4TH_GEN) + CS.vEgo = 8.5 + base_output, _, _ = base_controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) + + assert abs(tapered_output) < abs(base_output) + def test_ioniq_6_update_path_does_not_post_taper_output(self, monkeypatch): base_controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_IONIQ_6) base_output, _, _ = base_controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 4e73a67fb..32e7bcecb 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -26,6 +26,14 @@ MIN_ACTIVE_SPEED = 1.0 LOW_ACTIVE_SPEED = 10.0 +def resolve_vehicle_model_params(learned_steer_ratio: float, learned_stiffness: float, starpilot_toggles) -> tuple[float, float]: + if getattr(starpilot_toggles, "force_auto_tune_off", False): + return float(starpilot_toggles.steerRatio), 1.0 + + steer_ratio = starpilot_toggles.steerRatio if getattr(starpilot_toggles, "use_custom_steerRatio", False) else learned_steer_ratio + return float(steer_ratio), float(learned_stiffness) + + class VehicleParamsLearner: def __init__(self, CP: car.CarParams, steer_ratio: float, stiffness_factor: float, angle_offset: float, P_initial: np.ndarray | None = None): self.kf = CarKalman(GENERATED_DIR) @@ -165,8 +173,11 @@ class VehicleParamsLearner: liveParameters = msg.liveParameters liveParameters.posenetValid = True liveParameters.sensorValid = sensors_valid - liveParameters.steerRatio = float(x[States.STEER_RATIO].item() if not self.starpilot_toggles.use_custom_steerRatio else self.starpilot_toggles.steerRatio) - liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item()) + liveParameters.steerRatio, liveParameters.stiffnessFactor = resolve_vehicle_model_params( + float(x[States.STEER_RATIO].item()), + float(x[States.STIFFNESS].item()), + self.starpilot_toggles, + ) liveParameters.roll = float(self.roll) liveParameters.angleOffsetAverageDeg = float(self.avg_angle_offset) liveParameters.angleOffsetDeg = float(self.angle_offset) diff --git a/selfdrive/locationd/test/test_paramsd.py b/selfdrive/locationd/test/test_paramsd.py index dd496b767..5e9feb369 100644 --- a/selfdrive/locationd/test/test_paramsd.py +++ b/selfdrive/locationd/test/test_paramsd.py @@ -1,8 +1,9 @@ import random +from types import SimpleNamespace import numpy as np from cereal import messaging -from openpilot.selfdrive.locationd.paramsd import retrieve_initial_vehicle_params, migrate_cached_vehicle_params_if_needed +from openpilot.selfdrive.locationd.paramsd import resolve_vehicle_model_params, retrieve_initial_vehicle_params, migrate_cached_vehicle_params_if_needed from openpilot.selfdrive.locationd.models.car_kf import CarKalman from openpilot.selfdrive.locationd.test.test_locationd_scenarios import TEST_ROUTE from openpilot.selfdrive.test.process_replay.migration import migrate, migrate_carParams @@ -20,6 +21,30 @@ def get_random_live_parameters(CP): class TestParamsd: + def test_force_auto_tune_off_locks_vehicle_model_params(self): + toggles = SimpleNamespace(force_auto_tune_off=True, use_custom_steerRatio=True, steerRatio=16.8) + + steer_ratio, stiffness = resolve_vehicle_model_params(17.2, 0.84, toggles) + + assert steer_ratio == 16.8 + assert stiffness == 1.0 + + def test_custom_steer_ratio_keeps_learned_stiffness(self): + toggles = SimpleNamespace(force_auto_tune_off=False, use_custom_steerRatio=True, steerRatio=16.8) + + steer_ratio, stiffness = resolve_vehicle_model_params(17.2, 0.84, toggles) + + assert steer_ratio == 16.8 + assert stiffness == 0.84 + + def test_vehicle_model_learning_remains_default(self): + toggles = SimpleNamespace(force_auto_tune_off=False, use_custom_steerRatio=False, steerRatio=16.8) + + steer_ratio, stiffness = resolve_vehicle_model_params(17.2, 0.84, toggles) + + assert steer_ratio == 17.2 + assert stiffness == 0.84 + def test_read_saved_params(self): params = Params() diff --git a/selfdrive/ui/layouts/settings/starpilot/lateral.py b/selfdrive/ui/layouts/settings/starpilot/lateral.py index 736867839..8edecd006 100644 --- a/selfdrive/ui/layouts/settings/starpilot/lateral.py +++ b/selfdrive/ui/layouts/settings/starpilot/lateral.py @@ -247,12 +247,12 @@ class StarPilotLateralLayout(_SettingsPage): ), SettingRow( "ForceAutoTuneOff", "toggle", tr_noop("Force Auto-Tune Off"), - subtitle=tr_noop("Force-disable auto-tuning and use your set values."), + subtitle=tr_noop("Force-disable learned lateral values and use your set values."), get_state=lambda: p.get_bool("ForceAutoTuneOff"), set_state=lambda s: (p.put_bool("ForceAutoTuneOff", s), s and p.put_bool("ForceAutoTune", False), _sync_parent(p, "AdvancedLateralTune", _ADVANCED_LATERAL_KEYS)), - enabled=lambda: cs.hasAutoTune and cs.isTorqueCar and not cs.isAngleCar, + enabled=lambda: cs.isTorqueCar and not cs.isAngleCar, disabled_label=tr_noop("Not Available"), visible=alt_on, ), @@ -379,6 +379,6 @@ class StarPilotLateralLayout(_SettingsPage): def on_close(res, val): if res == DialogResult.CONFIRM: self._params.put_int("LaneChangeSmoothing", int(val)) - current = self._params.get_int("LaneChangeSmoothing") if self._params.get_int("LaneChangeSmoothing") > 0 else 10 + current = self._params.get_int("LaneChangeSmoothing") if self._params.get_int("LaneChangeSmoothing") > 0 else 5 gui_app.push_widget(AetherSliderDialog(tr("Lane Change Smoothing"), 1, 10, 1, current, on_close, color=self.SLIDER_COLOR)) diff --git a/selfdrive/ui/onroad/starpilot/developer_sidebar.py b/selfdrive/ui/onroad/starpilot/developer_sidebar.py index eace8d09f..724dfce00 100644 --- a/selfdrive/ui/onroad/starpilot/developer_sidebar.py +++ b/selfdrive/ui/onroad/starpilot/developer_sidebar.py @@ -316,12 +316,13 @@ class DeveloperSidebar: fallback_use_custom_steer_ratio = force_auto_tune_off or (_setting_changed(self._cached_ratio, self._cached_ratio_stock) and not force_auto_tune) use_custom_steer_ratio = self._toggle_bool(toggles, "use_custom_steerRatio", fallback_use_custom_steer_ratio) custom_steer_ratio = self._toggle_float(toggles, "steerRatio", self._cached_ratio) - if live_parameters: + if use_custom_steer_ratio: + steer_ratio = custom_steer_ratio + elif live_parameters: steer_ratio = live_parameters.steerRatio - stiff_factor = live_parameters.stiffnessFactor else: - steer_ratio = custom_steer_ratio if use_custom_steer_ratio else (self._cached_ratio_stock if self._cached_ratio_stock != 0.0 else self._cached_ratio) - stiff_factor = 0.0 + steer_ratio = self._cached_ratio_stock if self._cached_ratio_stock != 0.0 else self._cached_ratio + stiff_factor = 1.0 if force_auto_tune_off else (live_parameters.stiffnessFactor if live_parameters else 1.0) force_auto_tune_flm = self._flm_changed("ForceAutoTune", "ForceAutoTuneOff") self._metric_colors = { @@ -341,7 +342,10 @@ class DeveloperSidebar: auto=live_parameters is not None and not use_custom_steer_ratio, flm=self._flm_changed("SteerRatio") or (force_auto_tune_flm and use_custom_steer_ratio), ), - 7: self._tuning_color(auto=live_parameters is not None), + 7: self._tuning_color( + auto=live_parameters is not None and not force_auto_tune_off, + flm=force_auto_tune_flm and force_auto_tune_off, + ), } model_name = ui_state.starpilot_toggles.get("model_name", "N/A") diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index f88c0a784..f0e7474ca 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -717,7 +717,9 @@ class StarPilotVariables: advanced_lateral_tuning = self.get_value("AdvancedLateralTune") toggle.force_auto_tune = self.get_value("ForceAutoTune", condition=advanced_lateral_tuning and not has_auto_tune and is_torque_car and not is_angle_car) - toggle.force_auto_tune_off = self.get_value("ForceAutoTuneOff", condition=advanced_lateral_tuning and has_auto_tune and is_torque_car and not is_angle_car) + # Force-off is also meaningful on manually tuned torque cars: it locks the + # vehicle-model parameters instead of allowing paramsd to learn over them. + toggle.force_auto_tune_off = self.get_value("ForceAutoTuneOff", condition=advanced_lateral_tuning and is_torque_car and not is_angle_car) toggle.flm_active_profile_id = self.params.get("FLMActiveProfileId", encoding="utf-8") or "" toggle.flm_trial_applied = self.params.get_bool("FLMTrialApplied") flm_overrides_raw = self.params.get("FLMActiveOverrides", encoding="utf-8") or "" @@ -1052,7 +1054,7 @@ class StarPilotVariables: # The jerk factor is derived from a sinusoidal lane-change profile: j = pi^3 * W / T^3, # with 1.3x headroom. Only jerk (curvature rate) is shaped; lateral accel stays at the # stock envelope so the end-of-maneuver arrest is never starved of authority. - pace = self.get_value("LaneChangeSmoothing", cast=int, condition=toggle.lane_changes) or 10 + pace = self.get_value("LaneChangeSmoothing", cast=int, condition=toggle.lane_changes) or 5 pace = max(1, min(10, pace)) lane_w = 3.5 t_target = 3.0 + (10 - pace) * 5.0 / 9.0 diff --git a/starpilot/navigation/mapd_wrapper.py b/starpilot/navigation/mapd_wrapper.py index ad2fc4163..55a93c902 100644 --- a/starpilot/navigation/mapd_wrapper.py +++ b/starpilot/navigation/mapd_wrapper.py @@ -1,14 +1,13 @@ #!/usr/bin/env python3 import json -import os import signal import subprocess -import sys import time from collections import defaultdict, deque from pathlib import Path +from cereal import messaging from openpilot.common.basedir import BASEDIR from openpilot.common.params import Params from openpilot.common.swaglog import cloudlog @@ -21,6 +20,7 @@ MISSING_TILE_BACKOFF_S = 30.0 FAILURE_WINDOW_S = 3.0 FAILURE_THRESHOLD = 3 MISSING_COVERAGE_EXIT_CODE = 3 +WAIT_FOR_GPS_EXIT_CODE = 4 ROAD_STATE_POLL_S = 1.0 @@ -46,6 +46,15 @@ def is_offline_read_error(line: str) -> bool: return payload.get("msg") == "could not unmarshal offline data" +def is_null_island_tile(filename: str) -> bool: + try: + min_lat, min_lon, max_lat, max_lon = (float(value) for value in Path(filename).name.split("_")) + except (TypeError, ValueError): + return False + + return min_lat <= 0 <= max_lat and min_lon <= 0 <= max_lon + + class CorruptTileMonitor: def __init__(self, threshold: int = FAILURE_THRESHOLD, window_s: float = FAILURE_WINDOW_S): self.threshold = threshold @@ -86,7 +95,7 @@ def quarantine_offline_tile(filename: str) -> Path | None: if not tile_path.is_file(): return None - quarantined = tile_path.with_name(f"{tile_path.name}.corrupt.{int(time.time())}") + quarantined = tile_path.with_name(f"{tile_path.name}.corrupt.{time.monotonic_ns()}") try: tile_path.rename(quarantined) except OSError: @@ -147,6 +156,11 @@ def run_mapd_once() -> int: # Stop its resulting hot loop until the next onroad process cycle. missing_tile = monitor.current_filename if is_offline_read_error(line) and missing_tile is not None and not Path(missing_tile).is_file(): + if is_null_island_tile(missing_tile): + cloudlog.info(f"mapd_wrapper received a location before GPS fix; waiting to restart mapd: {missing_tile}") + terminate_child(proc) + return WAIT_FOR_GPS_EXIT_CODE + cloudlog.info(f"mapd_wrapper has no offline tile for {missing_tile}; stopping mapd until the next drive") terminate_child(proc) return MISSING_COVERAGE_EXIT_CODE @@ -157,10 +171,7 @@ def run_mapd_once() -> int: quarantined = quarantine_offline_tile(bad_tile) if quarantined is None: if not OFFLINE_ROOT.exists(): - cloudlog.warning( - f"mapd_wrapper detected repeated offline read failures for {bad_tile}, " - f"but {OFFLINE_ROOT} does not exist; backing off mapd restarts" - ) + cloudlog.warning(f"mapd_wrapper detected repeated offline read failures for {bad_tile}, but {OFFLINE_ROOT} does not exist; backing off mapd restarts") terminate_child(proc) return 2 @@ -182,6 +193,16 @@ def wait_for_road_state_change(params: Params) -> None: time.sleep(ROAD_STATE_POLL_S) +def wait_for_gps_fix_or_road_state_change(params: Params, sm=None) -> None: + initial_onroad = params.get_bool("IsOnroad") + sm = sm or messaging.SubMaster(["gpsLocationExternal"]) + + while params.get_bool("IsOnroad") == initial_onroad: + sm.update(1000) + if sm.updated["gpsLocationExternal"] and sm["gpsLocationExternal"].hasFix: + return + + def main() -> None: params = Params() while True: @@ -195,6 +216,9 @@ def main() -> None: if exit_code == MISSING_COVERAGE_EXIT_CODE: wait_for_road_state_change(params) continue + if exit_code == WAIT_FOR_GPS_EXIT_CODE: + wait_for_gps_fix_or_road_state_change(params) + continue raise SystemExit(exit_code) diff --git a/starpilot/navigation/test_mapd_wrapper.py b/starpilot/navigation/test_mapd_wrapper.py index a3ce7a16b..f278179eb 100644 --- a/starpilot/navigation/test_mapd_wrapper.py +++ b/starpilot/navigation/test_mapd_wrapper.py @@ -4,7 +4,16 @@ import subprocess from pathlib import Path -from openpilot.starpilot.navigation.mapd_wrapper import CorruptTileMonitor, quarantine_offline_tile, run_mapd_once, terminate_child, wait_for_road_state_change +from openpilot.starpilot.navigation.mapd_wrapper import ( + WAIT_FOR_GPS_EXIT_CODE, + CorruptTileMonitor, + is_null_island_tile, + quarantine_offline_tile, + run_mapd_once, + terminate_child, + wait_for_gps_fix_or_road_state_change, + wait_for_road_state_change, +) def _loading_line(filename: str) -> str: @@ -52,6 +61,12 @@ def test_quarantine_offline_tile_ignores_missing_file(tmp_path, monkeypatch): assert quarantine_offline_tile(missing_tile.as_posix()) is None +def test_null_island_tile_detection(): + assert is_null_island_tile("/data/media/0/osm/offline/-2/-2/-0.250000_-0.250000_0.000000_0.000000") + assert not is_null_island_tile("/data/media/0/osm/offline/42/-72/42.500000_-71.750000_42.750000_-71.500000") + assert not is_null_island_tile("not-a-tile") + + def test_run_mapd_once_stops_for_missing_offline_coverage(tmp_path, monkeypatch): missing_tile = tmp_path / "offline/36/-98/37.500000_-98.000000_37.750000_-97.750000" output = [] @@ -83,6 +98,35 @@ def test_run_mapd_once_stops_for_missing_offline_coverage(tmp_path, monkeypatch) assert proc.terminated +def test_run_mapd_once_waits_for_gps_after_null_island_lookup(tmp_path, monkeypatch): + missing_tile = tmp_path / "offline/-2/-2/-0.250000_-0.250000_0.000000_0.000000" + output = (_loading_line(missing_tile.as_posix()), _error_line()) + + class CompletedProcess: + pid = 123 + + def __init__(self): + self.stdout = iter(f"{line}\n" for line in output) + self.terminated = False + + def poll(self): + return 0 if self.terminated else None + + def terminate(self): + self.terminated = True + + def wait(self, timeout=None): + return 0 + + proc = CompletedProcess() + monkeypatch.setitem(run_mapd_once.__globals__, "OFFLINE_ROOT", tmp_path / "offline") + monkeypatch.setattr(subprocess, "Popen", lambda *args, **kwargs: proc) + monkeypatch.setattr("signal.signal", lambda *args: None) + + assert run_mapd_once() == WAIT_FOR_GPS_EXIT_CODE + assert proc.terminated + + def test_missing_coverage_waits_for_road_state_change(monkeypatch): class Params: states = iter((True, True, False)) @@ -99,6 +143,37 @@ def test_missing_coverage_waits_for_road_state_change(monkeypatch): assert sleeps == [1.0] +def test_null_island_wait_ends_when_gps_gets_a_fix(): + class Params: + def get_bool(self, key): + assert key == "IsOnroad" + return True + + class GpsLocation: + hasFix = False + + class SubMaster: + def __init__(self): + self.updated = {"gpsLocationExternal": False} + self.location = GpsLocation() + self.update_count = 0 + + def update(self, timeout): + assert timeout == 1000 + self.update_count += 1 + self.updated["gpsLocationExternal"] = True + self.location.hasFix = self.update_count == 2 + + def __getitem__(self, key): + assert key == "gpsLocationExternal" + return self.location + + sm = SubMaster() + wait_for_gps_fix_or_road_state_change(Params(), sm) + + assert sm.update_count == 2 + + def test_terminate_child_tolerates_wedged_process(): class WedgedProcess: pid = 123