diff --git a/CREDITS.md b/CREDITS.md index 44c264365..ce6bae302 100644 --- a/CREDITS.md +++ b/CREDITS.md @@ -68,11 +68,11 @@ contributors. | StarPilot area | Upstream lineage | What StarPilot changed | | --- | --- | --- | -| `starpilot/car/ford/lateral.py` | `human_turn.py`, `lateral_curv_ext.py`, `lateral_angle_ext.py`, and `values_ext.py` at the reference snapshot above | Reorganized three extension/mixin implementations into one controller; integrated StarPilot Params; later added live-delay curvature lookahead, active-session driver override, measured-curvature handoff blending, PSCM acknowledgement, and local recovery/tuning behavior. | -| `starpilot/car/ford/fordcan.py` | `fordcan_ext.py` and the angle-mode safety protocol, especially `8f8d6d15f` | Reduced the extension to the Ford lateral CAN constructors used by StarPilot and adapted it to the local controller interface. | +| `starpilot/car/ford/lateral.py` | `human_turn.py`, `lateral_curv_ext.py`, and `values_ext.py` at the reference snapshot above; earlier StarPilot revisions also adapted `lateral_angle_ext.py` | Consolidated the runtime implementation on the extended-curvature strategy, integrated StarPilot Params, and added live-delay curvature lookahead and local tuning behavior. | +| `starpilot/car/ford/fordcan.py` | `fordcan_ext.py` and the extended-lateral protocol, especially `8f8d6d15f` | Reduced the extension to the curvature CAN constructors used by StarPilot and adapted it to the local controller interface. | | `opendbc_repo/opendbc/car/ford/` | Ford controller/state/interface/radar/platform changes in the `bp-7.0` snapshot | Integrated the changes directly into StarPilot's opendbc layout instead of retaining sunnypilot mixins; subsequent fixes and behavior differ by file. | -| `opendbc_repo/opendbc/safety/modes/ford.h` and Ford safety tests | BluePilot panda enforcement for four-signal curvature and angle-primary control, especially `8f8d6d15f`, plus shadow-curvature work | Adapted the flags and checks to StarPilot's smaller mode protocol and continued adding local regression coverage. | -| Ford Params and settings surfaces | BluePilot's mode and tuning concepts | Renamed and implemented in StarPilot's native Params/Galaxy architecture; no BluePilot or sunnypilot UI classes were retained. | +| `opendbc_repo/opendbc/safety/modes/ford.h` and Ford safety tests | BluePilot panda enforcement for four-signal curvature and angle-primary control, especially `8f8d6d15f`, plus shadow-curvature work | Retained the extended-curvature checks, removed runtime path-angle selection, and continued adding local regression coverage. | +| Ford Params and settings surfaces | BluePilot's curvature tuning concepts | Renamed and implemented in StarPilot's native Params/Galaxy architecture; no BluePilot or sunnypilot UI classes were retained. | The local code has materially diverged, but the first four rows remain derivative in design and in parts of their implementation. Future ports should cite the exact upstream commit in the importing diff --git a/common/libcommon.a b/common/libcommon.a index 3c97c905f..356e02b54 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_keys.h b/common/params_keys.h index 564cea176..7d0fa4d6b 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -331,6 +331,12 @@ inline static std::unordered_map keys = { {"DownloadAllModels", {CLEAR_ON_MANAGER_START, BOOL, "0", "0"}}, {"DownloadMaps", {CLEAR_ON_MANAGER_START, BOOL, "0", "0"}}, {"DriverCamera", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}}, + {"ActiveBigModel", {PERSISTENT, STRING}}, + {"ActiveBigModelName", {PERSISTENT, STRING}}, + {"ActiveBigModelVersion", {PERSISTENT, STRING}}, + {"ActiveSmallModel", {PERSISTENT, STRING}}, + {"ActiveSmallModelName", {PERSISTENT, STRING}}, + {"ActiveSmallModelVersion", {PERSISTENT, STRING}}, {"Model", {PERSISTENT, STRING, "rdf43", "rdf43", 1}}, {"ModelVersion", {PERSISTENT, STRING, "v15", "v15", 1}}, {"DrivingModel", {PERSISTENT, STRING, "rdf43", "rdf43", 1}}, @@ -373,19 +379,13 @@ inline static std::unordered_map keys = { {"ForceStandstill", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}}, {"FordLKASButtonControlMigrated", {PERSISTENT, BOOL, "0", "0"}}, {"ForceTorqueController", {PERSISTENT, BOOL, "0", "0", 3}}, - // These Ford lateral tuning concepts descend from BluePilot bp-7.0. StarPilot's key names and + // These Ford curvature tuning concepts descend from BluePilot bp-7.0. StarPilot's key names and // settings integration are local; see /CREDITS.md and /THIRD_PARTY_NOTICES.md for provenance. - {"FordAngleBlend", {PERSISTENT, FLOAT, "0.5", "0.5", 2}}, - {"FordAngleHighSpeedDamping", {PERSISTENT, FLOAT, "1.0", "1.0", 2}}, - {"FordAngleHighSpeedFactor", {PERSISTENT, FLOAT, "1.0", "1.0", 2}}, - {"FordAngleLaneChangeFactor", {PERSISTENT, FLOAT, "1.0", "1.0", 2}}, - {"FordAngleLowSpeedFactor", {PERSISTENT, FLOAT, "1.0", "1.0", 2}}, {"FordCurvatureBlendHigh", {PERSISTENT, FLOAT, "0.4", "0.4", 2}}, {"FordCurvatureBlendLow", {PERSISTENT, FLOAT, "0.4", "0.4", 2}}, {"FordCurvatureLaneChangeFactor", {PERSISTENT, FLOAT, "0.85", "0.85", 2}}, {"FordHandsFreeCluster", {PERSISTENT, BOOL, "0", "0", 2}}, {"FordHumanTurnDetection", {PERSISTENT, BOOL, "1", "1", 2}}, - {"FordLateralMode", {PERSISTENT, INT, "1", "1", 2}}, {"FLMActiveOverrides", {PERSISTENT, JSON, "{}", "{}", 2}}, {"FLMActiveProfileId", {PERSISTENT, STRING, "", "", 2}}, {"FLMSubmittedTune", {CLEAR_ON_MANAGER_START, JSON, "{}", "{}"}}, diff --git a/common/params_pyx.so b/common/params_pyx.so index 0c9e2d84e..19e245202 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/opendbc_repo/opendbc/car/car_helpers.py b/opendbc_repo/opendbc/car/car_helpers.py index 0b2cf0775..686b777f5 100644 --- a/opendbc_repo/opendbc/car/car_helpers.py +++ b/opendbc_repo/opendbc/car/car_helpers.py @@ -10,6 +10,7 @@ from opendbc.car.carlog import carlog from opendbc.car.structs import CarParams, CarParamsT from opendbc.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars from opendbc.car.fw_versions import ObdCallback, get_fw_versions_ordered, get_present_ecus, match_fw_to_car +from opendbc.car.hyundai.values import kia_ray_ev_vin from opendbc.car.mock.values import CAR as MOCK from opendbc.car.toyota.values import ToyotaSafetyFlags from opendbc.car.values import BRANDS @@ -246,8 +247,13 @@ def fingerprint(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_mu set_obd_multiplexing(True) # VIN query only reliably works through OBDII vin_rx_addr, vin_rx_bus, vin = get_vin(can_recv, can_send, (0, 1)) - ecu_rx_addrs = get_present_ecus(can_recv, can_send, set_obd_multiplexing, num_pandas=num_pandas) - car_fw = get_fw_versions_ordered(can_recv, can_send, set_obd_multiplexing, vin, ecu_rx_addrs, num_pandas=num_pandas) + skip_fw_buses = {1} if kia_ray_ev_vin(vin) else set() + if skip_fw_buses: + carlog.warning("Kia Ray EV: skipping CAN1 firmware queries") + ecu_rx_addrs = get_present_ecus(can_recv, can_send, set_obd_multiplexing, + num_pandas=num_pandas, skip_buses=skip_fw_buses) + car_fw = get_fw_versions_ordered(can_recv, can_send, set_obd_multiplexing, vin, ecu_rx_addrs, + num_pandas=num_pandas, skip_buses=skip_fw_buses) cached = False exact_fw_match, fw_candidates = match_fw_to_car(car_fw, vin) diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index 04efbef33..56890bb99 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -1,15 +1,15 @@ import math import numpy as np from opendbc.can import CANPacker -from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, apply_hysteresis, structs +from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, structs from opendbc.car.lateral import ISO_LATERAL_ACCEL, apply_std_steer_angle_limits from opendbc.car.ford import fordcan -from opendbc.car.ford.values import CarControllerParams, FordFlags, CAR +from opendbc.car.ford.values import CarControllerParams, FordFlags from opendbc.car.interfaces import CarControllerBase, V_CRUISE_MAX # This Ford extension boundary substantially adapts BluePilot bp-7.0 work. See the root CREDITS.md # (including Alan Polk's d0aac605f and db2bdff05) and THIRD_PARTY_NOTICES.md. from openpilot.starpilot.car.ford import fordcan as starpilot_fordcan -from openpilot.starpilot.car.ford.lateral import FordLateralController, FordLateralMode, FordLateralResult +from openpilot.starpilot.car.ford.lateral import FordLateralController, FordLateralResult LongCtrlState = structs.CarControl.Actuators.LongControlState VisualAlert = structs.CarControl.HUDControl.VisualAlert @@ -45,22 +45,6 @@ def apply_ford_angle(desired_angle_deg: float, current_angle_deg: float) -> floa return float(np.clip(relative_angle, -5.8, 5.8)) -def anti_overshoot(apply_curvature, apply_curvature_last, v_ego): - diff = 0.1 - tau = 5 # 5s smooths over the overshoot - dt = DT_CTRL * CarControllerParams.STEER_STEP - alpha = 1 - np.exp(-dt / tau) - - lataccel = apply_curvature * (v_ego ** 2) - last_lataccel = apply_curvature_last * (v_ego ** 2) - last_lataccel = apply_hysteresis(lataccel, last_lataccel, diff) - last_lataccel = alpha * lataccel + (1 - alpha) * last_lataccel - - output_curvature = last_lataccel / (max(v_ego, 1) ** 2) - - return float(np.interp(v_ego, [5, 10], [apply_curvature, output_curvature])) - - def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_curvature, v_ego_raw, steering_angle, lat_active, CP): # No blending at low speed due to lack of torque wind-up and inaccurate current curvature if v_ego_raw > 9: @@ -95,7 +79,6 @@ class CarController(CarControllerBase): self.apply_curvature_last = 0 self.apply_angle_last = 0 - self.anti_overshoot_curvature_last = 0 self.accel = 0.0 self.gas = 0.0 self.brake_request = False @@ -105,8 +88,7 @@ class CarController(CarControllerBase): self.lead_distance_bars_last = None self.distance_bar_frame = 0 self.ford_lateral = None if CP.flags & FordFlags.LKA_STEERING else FordLateralController(CP) - self.ford_shadow_curvature = 0.0 - self.ford_lateral_announced_mode = FordLateralMode.native + self.ford_extended_lateral_announced = False self.stock_cruise_button = FordStockCruiseButton() def update(self, CC, CS, now_nanos, starpilot_toggles): @@ -173,70 +155,26 @@ class CarController(CarControllerBase): can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN, active=lka_active, apply_angle=self.apply_angle_last, direction=direction, ramp_type=ramp_type, curvature=-self.apply_curvature_last)) else: - lateral_mode = self.ford_lateral.mode - lateral_mode_ready = lateral_mode == self.ford_lateral_announced_mode - - # Keep the original Ford path available without changing its command behavior. - if lateral_mode == FordLateralMode.native: - if (self.frame % CarControllerParams.STEER_STEP) == 0: - if not lateral_mode_ready: - self.apply_curvature_last = 0.0 - apply_curvature = 0.0 - elif self.CP.carFingerprint in (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_F_150_MK14): - self.anti_overshoot_curvature_last = anti_overshoot( - actuators.curvature, self.anti_overshoot_curvature_last, CS.out.vEgoRaw) - apply_curvature = self.anti_overshoot_curvature_last - else: - apply_curvature = actuators.curvature - - current_curvature = -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1) - self.apply_curvature_last = apply_ford_curvature_limits( - apply_curvature, self.apply_curvature_last, current_curvature, - CS.out.vEgoRaw, 0., CC.latActive and lateral_mode_ready, self.CP) - - if self.CP.flags & FordFlags.CANFD: - mode = 1 if CC.latActive and lateral_mode_ready else 0 - counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10 - can_sends.append(fordcan.create_lat_ctl2_msg( - self.packer, self.CAN, mode, 0., 0., -self.apply_curvature_last, 0., counter)) - else: - can_sends.append(fordcan.create_lat_ctl_msg( - self.packer, self.CAN, CC.latActive and lateral_mode_ready, 0., 0., -self.apply_curvature_last, 0.)) - elif (self.frame % CarControllerParams.STEER_STEP) == 0: - if not lateral_mode_ready: - lateral = FordLateralResult(shadow_curvature=self.ford_lateral._current_curvature(CS)) - elif lateral_mode == FordLateralMode.angle: - lateral = self.ford_lateral.update_angle(CC, CS, actuators) - else: - lateral = self.ford_lateral.update_curvature(CC, CS, actuators) + if (self.frame % CarControllerParams.STEER_STEP) == 0: + lateral = self.ford_lateral.update(CC, CS, actuators) \ + if self.ford_extended_lateral_announced else FordLateralResult() self.apply_curvature_last = lateral.curvature - self.ford_shadow_curvature = lateral.shadow_curvature if self.CP.flags & FordFlags.CANFD: counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10 can_sends.append(starpilot_fordcan.create_lat_ctl2_msg( self.packer, self.CAN, 1 if lateral.active else 0, lateral.ramp_type, lateral.precision_type, - -lateral.path_offset, -lateral.path_angle, -lateral.curvature, -lateral.curvature_rate, counter)) else: can_sends.append(starpilot_fordcan.create_lat_ctl_msg( self.packer, self.CAN, lateral.active, lateral.ramp_type, lateral.precision_type, - -lateral.path_offset, -lateral.path_angle, -lateral.curvature, -lateral.curvature_rate)) if (self.frame % CarControllerParams.LKA_STEP) == 0: - if lateral_mode == FordLateralMode.native: - can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN)) - else: - angle_mode = lateral_mode == FordLateralMode.angle - shadow_curvature = -self.ford_lateral._current_curvature(CS) - if angle_mode: - shadow_curvature = -self.ford_shadow_curvature - can_sends.append(starpilot_fordcan.create_lka_msg( - self.packer, self.CAN, angle_mode=angle_mode, shadow_curvature=shadow_curvature)) - self.ford_lateral_announced_mode = lateral_mode + can_sends.append(starpilot_fordcan.create_lka_msg(self.packer, self.CAN)) + self.ford_extended_lateral_announced = True ### longitudinal control ### # send acc msg at 50Hz @@ -294,8 +232,7 @@ class CarController(CarControllerBase): show_distance_bars = self.frame - self.distance_bar_frame < 400 hands_free_cluster = bool( self.ford_lateral is not None - and self.ford_lateral.mode != FordLateralMode.native - and self.ford_lateral.mode == self.ford_lateral_announced_mode + and self.ford_extended_lateral_announced and self.ford_lateral.hands_free_cluster_enabled) can_sends.append(fordcan.create_acc_ui_msg(self.packer, self.CAN, self.CP, main_on, CC.latActive, fcw_alert, CS.out.cruiseState.standstill, show_distance_bars, diff --git a/opendbc_repo/opendbc/car/fw_versions.py b/opendbc_repo/opendbc/car/fw_versions.py index a9157ccfc..eda01dd30 100644 --- a/opendbc_repo/opendbc/car/fw_versions.py +++ b/opendbc_repo/opendbc/car/fw_versions.py @@ -170,7 +170,9 @@ def match_fw_to_car(fw_versions: list[CarParams.CarFw], vin: str, allow_exact: b return True, set() -def get_present_ecus(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, num_pandas: int = 1) -> set[EcuAddrBusType]: +def get_present_ecus(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, + num_pandas: int = 1, skip_buses: set[int] | None = None) -> set[EcuAddrBusType]: + skip_buses = skip_buses or set() # queries are split by OBD multiplexing mode queries: dict[bool, list[list[EcuAddrBusType]]] = {True: [], False: []} parallel_queries: dict[bool, list[EcuAddrBusType]] = {True: [], False: []} @@ -178,7 +180,7 @@ def get_present_ecus(can_recv: CanRecvCallable, can_send: CanSendCallable, set_o for brand, config, r in REQUESTS: # Skip query if no panda available - if r.bus > num_pandas * 4 - 1: + if r.bus > num_pandas * 4 - 1 or r.bus in skip_buses: continue for ecu_type, addr, sub_addr in config.get_all_ecus(VERSIONS[brand]): @@ -235,7 +237,8 @@ def get_brand_ecu_matches(ecu_rx_addrs: set[EcuAddrBusType]) -> dict[str, list[b def get_fw_versions_ordered(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, vin: str, - ecu_rx_addrs: set[EcuAddrBusType], timeout: float = 0.1, num_pandas: int = 1, progress: bool = False) -> list[CarParams.CarFw]: + ecu_rx_addrs: set[EcuAddrBusType], timeout: float = 0.1, num_pandas: int = 1, + progress: bool = False, skip_buses: set[int] | None = None) -> list[CarParams.CarFw]: """Queries for FW versions ordering brands by likelihood, breaks when exact match is found""" all_car_fw = [] @@ -248,7 +251,8 @@ def get_fw_versions_ordered(can_recv: CanRecvCallable, can_send: CanSendCallable if True not in brand_matches[brand]: continue - car_fw = get_fw_versions(can_recv, can_send, set_obd_multiplexing, query_brand=brand, timeout=timeout, num_pandas=num_pandas, progress=progress) + car_fw = get_fw_versions(can_recv, can_send, set_obd_multiplexing, query_brand=brand, timeout=timeout, + num_pandas=num_pandas, progress=progress, skip_buses=skip_buses) all_car_fw.extend(car_fw) # If there is a match using this brand's FW alone, finish querying early @@ -260,7 +264,9 @@ def get_fw_versions_ordered(can_recv: CanRecvCallable, can_send: CanSendCallable def get_fw_versions(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, query_brand: str = None, - extra: OfflineFwVersions = None, timeout: float = 0.1, num_pandas: int = 1, progress: bool = False) -> list[CarParams.CarFw]: + extra: OfflineFwVersions = None, timeout: float = 0.1, num_pandas: int = 1, progress: bool = False, + skip_buses: set[int] | None = None) -> list[CarParams.CarFw]: + skip_buses = skip_buses or set() versions = VERSIONS.copy() if query_brand is not None: @@ -298,7 +304,7 @@ def get_fw_versions(can_recv: CanRecvCallable, can_send: CanSendCallable, set_ob for addr_chunk in chunks(addr_group): for brand, config, r in requests: # Skip query if no panda available - if r.bus > num_pandas * 4 - 1: + if r.bus > num_pandas * 4 - 1 or r.bus in skip_buses: continue # Toggle OBD multiplexing for each request diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 8f7b2f533..fd484eeab 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -750,6 +750,7 @@ class CarController(CarControllerBase): can_sends = [] can_canfd_blended = bool(self.CP.flags & HyundaiFlags.CAN_CANFD_BLENDED) blended_hda2 = can_canfd_blended and bool(self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING) + longitudinal_active = bool(self.long_active_ecu and getattr(CC, "longActive", False)) # HUD messages sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint, @@ -758,6 +759,7 @@ class CarController(CarControllerBase): if blended_hda2: can_sends.extend(hyundaicanfd.create_steering_messages( self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_torque, 0.0, + longitudinal_active=longitudinal_active, )) if self.long_active_ecu: can_sends.extend(hyundaican.create_lkas11_can_canfd_blended( @@ -849,7 +851,8 @@ class CarController(CarControllerBase): can_sends = [] lka_steering = self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING - lka_steering_long = lka_steering and self.long_active_ecu + longitudinal_active = bool(self.long_active_ecu and getattr(CC, "longActive", False)) + lka_steering_long = lka_steering and longitudinal_active ccnc_non_hda2 = self.CP.flags & HyundaiFlags.CCNC and not lka_steering use_egmp_dynamic_long_tuning = egmp_dynamic_longitudinal_tuning(self.CP) and self.long_active_ecu and \ CC.actuators.longControlState in (LongCtrlState.starting, LongCtrlState.pid, LongCtrlState.stopping) @@ -878,7 +881,7 @@ class CarController(CarControllerBase): if angle_lkas_alt: steering_msg_active = bool(steering_msg_active and drive_gear) angle_lkas_alt_standstill_handoff = bool(getattr(CS.out, "standstill", False) and not CC.latActive) - forward_stock_lkas = angle_lkas_alt and ( + forward_stock_lkas = self.CP.carFingerprint in CANFD_ANGLE_LONGITUDINAL_CAR and angle_lkas_alt and ( angle_lkas_alt_standstill_handoff or not (drive_gear and (CC.latActive or CC.enabled)) ) preserve_stock_lfa_status = preserve_stock_canfd_lfa_status(self.CP.carFingerprint) @@ -887,7 +890,8 @@ class CarController(CarControllerBase): steering_msg_active, apply_torque, apply_angle, CS.stock_lfa_msg if preserve_stock_lfa_status else None, CS.stock_lkas_msg if preserve_stock_lkas else None, - lka_icon=lka_icon)) + lka_icon=lka_icon, + longitudinal_active=longitudinal_active)) direct_steering_active = ccnc_angle_long and drive_gear and CC.latActive and self.direct_angle_request_allowed and not CS.angle_steering_fault inactive_steering_angle = float(np.clip(CS.angle_steering_angle, -self.params.ANGLE_LIMITS.STEER_ANGLE_MAX, diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py index bd06b2895..95de0b03b 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py @@ -99,9 +99,12 @@ def create_angle_adas_cmd(packer, CAN, apply_angle: float, lat_active: bool, tor def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque, apply_angle, - lfa_base_values=None, lkas_base_values=None, lka_icon=None): + lfa_base_values=None, lkas_base_values=None, lka_icon=None, + longitudinal_active=None): if lka_icon is None: lka_icon = 2 if enabled else 1 + if longitudinal_active is None: + longitudinal_active = CP.openpilotLongitudinalControl angle_lkas_alt = CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING and CP.flags & HyundaiFlags.CANFD_LKA_STEERING_ALT @@ -195,7 +198,7 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque, ret = [] if CP.flags & HyundaiFlags.CANFD_LKA_STEERING: lkas_msg = "LKAS_ALT" if CP.flags & HyundaiFlags.CANFD_LKA_STEERING_ALT else "LKAS" - if CP.openpilotLongitudinalControl and not CP.flags & HyundaiFlags.CAN_CANFD_BLENDED: + if longitudinal_active and not CP.flags & HyundaiFlags.CAN_CANFD_BLENDED: ret.append(packer.make_can_msg("LFA", CAN.ECAN, lfa_values)) ret.append(packer.make_can_msg(lkas_msg, CAN.ACAN, lkas_values)) else: diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 5e76e116e..4de27b6a5 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -2520,6 +2520,22 @@ class TestHyundaiFingerprint: assert lfa_parser.can_valid assert lfa_parser.vl["LFA"]["DAMP_FACTOR"] == 100 + controller.long_active_ecu = True + cc.longActive = False + inactive_msgs = controller.create_canfd_msgs(0, True, 0.44, 0.0, 0.0, 0.0, False, + cc.hudControl, cs, cc, get_test_toggles(), lka_icon=2, lfa_icon=2) + steering_names = [(controller.packer.dbc.addr_to_msg[addr].name, bus) for addr, _, bus in inactive_msgs + if controller.packer.dbc.addr_to_msg[addr].name in ("LFA", "LKAS")] + assert steering_names == [("LKAS", can_bus.ACAN)] + + controller.frame = 1 + cc.longActive = True + active_msgs = controller.create_canfd_msgs(0, True, 0.44, 0.0, 0.0, 0.0, False, + cc.hudControl, cs, cc, get_test_toggles(), lka_icon=2, lfa_icon=2) + steering_names = [(controller.packer.dbc.addr_to_msg[addr].name, bus) for addr, _, bus in active_msgs + if controller.packer.dbc.addr_to_msg[addr].name in ("LFA", "LKAS")] + assert steering_names == [("LFA", can_bus.ECAN), ("LKAS", can_bus.ACAN)] + def test_gv70_electrified_longitudinal_uses_hda2_scc_contract(self): CP = CarParams.new_message() CP.carFingerprint = CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN @@ -2651,6 +2667,70 @@ class TestHyundaiFingerprint: get_test_toggles(), lka_icon=1, lfa_icon=1) assert len([msg for msg in msgs if msg[0] == 0x110]) == expected_lkas_msgs + @pytest.mark.parametrize("standstill", [False, True]) + def test_sportage_angle_lkas_alt_keeps_inactive_status_in_drive(self, standstill): + CP = CarParams.new_message() + CP.carFingerprint = CAR.KIA_SPORTAGE_HEV_2026 + CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.HYBRID | HyundaiFlags.CANFD_ANGLE_STEERING | + HyundaiFlags.CANFD_LKA_STEERING | HyundaiFlags.CANFD_LKA_STEERING_ALT) + CP.openpilotLongitudinalControl = False + + controller = CarController(DBC[CP.carFingerprint], CP) + can_bus = CanBus(CP) + parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS_ALT", 0)], can_bus.ACAN) + stock_lkas = { + "CHECKSUM": 1234, + "COUNTER": 42, + "LKA_OptUsmSta": 2, + "LKA_MODE": 2, + "LKA_RcgSta": 3, + "LKA_AVAILABLE": 3, + "LKA_LHLnWrnSta": 3, + "LKA_RHLnWrnSta": 3, + "LKA_WARNING": 1, + "LKA_HndsoffSnd": 1, + "LKA_StrSnd": 1, + "LKA_SysIndReq": 4, + "LKA_ICON": 2, + "FCA_SYSWARN": 1, + "StrTqReqVal": 17, + "TORQUE_REQUEST": 17, + "ActToiSta": 3, + "STEER_REQ": 1, + "ToiFltSta": 3, + "LFA_BUTTON": 1, + "LKA_SysWrn": 15, + "LKA_ASSIST": 1, + "Damping_Gain": 0, + "STEER_MODE": 5, + "NEW_SIGNAL_2": 0, + "LKAS_ANGLE_ACTIVE": 2, + "LKA_UsmMod": 3, + "HAS_LANE_SAFETY": 1, + "ADAS_StrAnglReqVal": 12.3, + "ADAS_ACIAnglTqRedcGainVal": 0.42, + "DAMP_FACTOR": 0, + } + cc = SimpleNamespace(enabled=False, latActive=False, + actuators=SimpleNamespace(longControlState=LongCtrlState.off), + leftBlinker=False, rightBlinker=False, hudControl=SimpleNamespace()) + cs = SimpleNamespace(stock_lfa_msg=None, stock_lkas_msg=stock_lkas, + 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) + lkas_msgs = [msg for msg in msgs if msg[0] == 0x110] + assert len(lkas_msgs) == 1 + + parser.update([(1, lkas_msgs)]) + assert parser.can_valid + assert parser.vl["LKAS_ALT"]["LKA_StrSnd"] == 2 + assert parser.vl["LKAS_ALT"]["LKA_SysIndReq"] == 1 + assert parser.vl["LKAS_ALT"]["LKA_RcgSta"] == 0 + assert parser.vl["LKAS_ALT"]["LKA_AVAILABLE"] == 0 + assert parser.vl["LKAS_ALT"]["LKAS_ANGLE_ACTIVE"] == 1 + 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/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index bb6d7022a..0c74a6c04 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -1000,6 +1000,10 @@ KIA_EV6_GT_LINE_LONG_TUNING_VDS_PREFIXES = frozenset({ }) KIA_EV6_GT_LINE_LONG_TUNING_TESTING_GROUND_ID = "5" +KIA_RAY_EV_VIN_VDS_PREFIXES = frozenset({ + "CG81A", +}) + ALT_BUS_LDA_BUTTON_CARS = frozenset() ALT_BUS_LDA_BUTTON_SWL_STAT_CARS = frozenset() @@ -1014,6 +1018,10 @@ def kia_ev6_gt_line_longitudinal_tuning(car_fingerprint, vin: str, testing_groun return car_fingerprint == CAR.KIA_EV6 and (vin_match or testing_ground_active) +def kia_ray_ev_vin(vin: str) -> bool: + return isinstance(vin, str) and len(vin) == 17 and vin[3:8] in KIA_RAY_EV_VIN_VDS_PREFIXES + + def get_platform_codes(fw_versions: list[bytes]) -> set[tuple[bytes, bytes | None]]: # Returns unique, platform-specific identification codes for a set of versions codes = set() # (code-Optional[part], date) diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index b783021fc..959abdf96 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -11,7 +11,7 @@ from opendbc.car.interfaces import CarControllerBase from opendbc.car.toyota import toyotacan from opendbc.car.toyota.values import CAR, MIN_ACC_SPEED, NO_STOP_TIMER_CAR, PEDAL_TRANSITION, TSS2_CAR, \ CarControllerParams, ToyotaFlags, \ - UNSUPPORTED_DSU_CAR, LEGACY_PRIUS_CAR, RADAR_ACC_CAR, SECOC_CAR + UNSUPPORTED_DSU_CAR, LEGACY_PRIUS_CAR, TOYOTA_AUTO_HOLD_CARS from opendbc.can import CANPacker Ecu = structs.CarParams.Ecu @@ -37,6 +37,9 @@ TOYOTA_COAST_BRAKE_DISABLE_ACCEL = -0.06 # m/s^2 TOYOTA_NO_LEAD_COAST_BRAKE_ACCEL = -0.30 # m/s^2 TOYOTA_INTERCEPTOR_COMFORT_TARGET_ACCEL = 2.0 # m/s^2 TOYOTA_NO_LEAD_CRUISE_SIGN_FLIP_MIN_SET_SPEED_ERROR = 0.35 # m/s +TOYOTA_RAV4_LAUNCH_PEDAL_BLEND_SPEED = 5.0 # m/s +TOYOTA_RAV4_LAUNCH_PEDAL_SCALE = 0.11 +TOYOTA_RAV4_LOW_SPEED_PEDAL_SCALE = 0.23 # LKA limits # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long @@ -49,8 +52,6 @@ MAX_USER_TORQUE = 500 PARK = structs.CarState.GearShifter.park REVERSE = structs.CarState.GearShifter.reverse -TOYOTA_AUTO_HOLD_CARS = TSS2_CAR - RADAR_ACC_CAR - SECOC_CAR - # Lock / unlock door commands - Credit goes to AlexandreSato! LOCK_CMD = b"\x40\x05\x30\x11\x00\x80\x00\x00" UNLOCK_CMD = b"\x40\x05\x30\x11\x00\x40\x00\x00" @@ -82,6 +83,14 @@ def supports_toyota_auto_hold(CP, auto_hold_enabled: bool) -> bool: ) +def get_rav4_interceptor_pedal_scale(v_ego: float) -> float: + return float(np.interp( + max(float(v_ego), 0.0), + [0.0, TOYOTA_RAV4_LAUNCH_PEDAL_BLEND_SPEED, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], + [TOYOTA_RAV4_LAUNCH_PEDAL_SCALE, TOYOTA_RAV4_LOW_SPEED_PEDAL_SCALE, 0.3, 0.0], + )) + + def get_long_tune(CP, params): kiBP = [2., 5.] kiV = [0.5, 0.25] @@ -265,7 +274,7 @@ class CarController(CarControllerBase): max_interceptor_gas = 0.5 if self.CP.carFingerprint == CAR.TOYOTA_RAV4: - pedal_scale = float(np.interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0])) + pedal_scale = get_rav4_interceptor_pedal_scale(CS.out.vEgo) elif self.CP.carFingerprint in (CAR.TOYOTA_COROLLA, CAR.TOYOTA_MATRIX_RETROFIT): pedal_scale = float(np.interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0])) else: diff --git a/opendbc_repo/opendbc/car/toyota/carstate.py b/opendbc_repo/opendbc/car/toyota/carstate.py index 635e29695..6ae944bd1 100644 --- a/opendbc_repo/opendbc/car/toyota/carstate.py +++ b/opendbc_repo/opendbc/car/toyota/carstate.py @@ -314,6 +314,9 @@ class CarState(CarStateBase): if CP.carFingerprint in DISTANCE_BUTTON_CAR: pt_messages.append(("PCM_CRUISE_4", 1)) + if CP.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value: + cam_messages.append(("PRE_COLLISION_2", 50)) + return { Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0), Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, 2), diff --git a/opendbc_repo/opendbc/car/toyota/interface.py b/opendbc_repo/opendbc/car/toyota/interface.py index 61ea6d6a5..618b820d2 100644 --- a/opendbc_repo/opendbc/car/toyota/interface.py +++ b/opendbc_repo/opendbc/car/toyota/interface.py @@ -2,9 +2,9 @@ from opendbc.car import Bus, structs, get_safety_config, uds from opendbc.car.toyota.carstate import CarState from opendbc.car.toyota.carcontroller import CarController from opendbc.car.toyota.radar_interface import RadarInterface -from opendbc.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, SECOC_CAR, NO_DSU_CAR, \ +from opendbc.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \ MIN_ACC_SPEED, EPS_SCALE, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR, \ - ToyotaSafetyFlags, LEGACY_PRIUS_CAR + ToyotaSafetyFlags, LEGACY_PRIUS_CAR, TOYOTA_AUTO_HOLD_CARS from opendbc.car.disable_ecu import disable_ecu from opendbc.car.interfaces import CarInterfaceBase from opendbc.safety import ALTERNATIVE_EXPERIENCE @@ -164,7 +164,7 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.GAS_INTERCEPTOR.value toyota_auto_hold = Params(return_defaults=True).get_bool("ToyotaAutoHold") - if toyota_auto_hold and candidate in (TSS2_CAR - RADAR_ACC_CAR - SECOC_CAR): + if toyota_auto_hold and candidate in TOYOTA_AUTO_HOLD_CARS: ret.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALLOW_AEB ret.flags |= ToyotaFlags.AUTO_BRAKE_HOLD.value diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py index 0bbd1b161..225a997bb 100644 --- a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py +++ b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py @@ -10,6 +10,7 @@ from opendbc.car.fw_versions import build_fw_dict, match_fw_to_car from opendbc.car.toyota import toyotacan from opendbc.car.toyota.carcontroller import CarController, get_camry_hybrid_feedforward, get_long_tune, get_prius_feedforward, \ get_prius_positive_feedforward_scale, \ + get_rav4_interceptor_pedal_scale, \ limit_interceptor_pcm_accel, \ limit_interceptor_stopping_accel, limit_no_lead_cruise_sign_flip, \ limit_prius_stopping_accel, should_bypass_toyota_long_pid, supports_toyota_auto_hold, \ @@ -18,9 +19,10 @@ from opendbc.car.toyota.carstate import CarState, LKAS_BUTTON_CAR, calculate_int from opendbc.car.toyota.fingerprints import FW_VERSIONS from opendbc.car.toyota.interface import CarInterface from opendbc.car.toyota.radar_interface import RadarInterface, TSSP_RADAR_EGO_SPEED_SCALE -from opendbc.car.toyota.values import CAR, DBC, TSS2_CAR, ANGLE_CONTROL_CAR, RADAR_ACC_CAR, SECOC_CAR, \ +from opendbc.car.toyota.values import CAR, DBC, MIN_ACC_SPEED, TSS2_CAR, ANGLE_CONTROL_CAR, RADAR_ACC_CAR, SECOC_CAR, \ FW_QUERY_CONFIG, PLATFORM_CODE_ECUS, FUZZY_EXCLUDED_PLATFORMS, \ - ToyotaFlags, ToyotaSafetyFlags, ToyotaStarPilotFlags, get_platform_codes + ToyotaFlags, ToyotaSafetyFlags, ToyotaStarPilotFlags, TOYOTA_AUTO_HOLD_CARS, \ + get_platform_codes from opendbc.safety import ALTERNATIVE_EXPERIENCE from openpilot.common.params import Params @@ -187,12 +189,13 @@ class TestToyotaInterfaces: if car_model in TSS2_CAR and car_model not in SECOC_CAR: assert dbc[Bus.pt] == "toyota_nodsu_pt_generated" - def test_auto_hold_sets_flag_on_supported_tss2(self): + @pytest.mark.parametrize("candidate", [CAR.TOYOTA_CAMRY_TSS2, CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H]) + def test_auto_hold_sets_flag_on_supported_toyota(self, candidate): params = Params() try: params.put_bool("ToyotaAutoHold", True) car_params = CarInterface.get_params( - CAR.TOYOTA_CAMRY_TSS2, + candidate, {bus: {} for bus in range(8)}, [], alpha_long=False, @@ -206,11 +209,17 @@ class TestToyotaInterfaces: assert car_params.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value assert car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB - def test_auto_hold_is_disabled_by_default(self): + can_parsers = CarState.get_can_parsers(car_params) + car_state = CarState(car_params, SimpleNamespace(flags=0)) + car_state.update(can_parsers, SimpleNamespace(cluster_offset=1.0)) + assert "PRE_COLLISION_2" in can_parsers[Bus.cam].vl + + @pytest.mark.parametrize("candidate", [CAR.TOYOTA_CAMRY_TSS2, CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H]) + def test_auto_hold_is_disabled_by_default(self, candidate): params = Params() params.remove("ToyotaAutoHold") car_params = CarInterface.get_params( - CAR.TOYOTA_CAMRY_TSS2, + candidate, {bus: {} for bus in range(8)}, [], alpha_long=False, @@ -774,6 +783,14 @@ class TestToyotaCarController: ) assert supports_toyota_auto_hold(CP, True) + assert supports_toyota_auto_hold(SimpleNamespace( + carFingerprint=CAR.TOYOTA_RAV4, + flags=ToyotaFlags.AUTO_BRAKE_HOLD.value, + ), True) + assert supports_toyota_auto_hold(SimpleNamespace( + carFingerprint=CAR.TOYOTA_RAV4H, + flags=ToyotaFlags.AUTO_BRAKE_HOLD.value, + ), True) assert not supports_toyota_auto_hold(CP, False) assert not supports_toyota_auto_hold(SimpleNamespace( carFingerprint=CAR.TOYOTA_CAMRY_TSS2, @@ -784,6 +801,8 @@ class TestToyotaCarController: flags=ToyotaFlags.AUTO_BRAKE_HOLD.value, ), True) + assert TOYOTA_AUTO_HOLD_CARS >= {CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H} + def test_toyota_auto_hold_latches_after_brake_press_until_gas(self): controller = self._make_controller() controller.packer = CANPacker(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt]) @@ -1023,6 +1042,27 @@ class TestToyotaCarController: assert 0.0 < gas_cmd <= 0.5 + def test_rav4_interceptor_launch_mapping_is_softer_than_generic_mapping(self): + controller = self._make_controller() + controller.CP.enableGasInterceptorDEPRECATED = True + controller.CP.carFingerprint = CAR.TOYOTA_RAV4 + controller.accel = 1.5 + + rav4_gas = controller._compute_interceptor_gas_cmd( + SimpleNamespace(longActive=True), + SimpleNamespace(out=SimpleNamespace(standstill=False, vEgo=1.0)), + ) + + controller.CP.carFingerprint = CAR.TOYOTA_AVALON_2019 + generic_gas = controller._compute_interceptor_gas_cmd( + SimpleNamespace(longActive=True), + SimpleNamespace(out=SimpleNamespace(standstill=False, vEgo=1.0)), + ) + + assert rav4_gas < generic_gas + assert get_rav4_interceptor_pedal_scale(5.0) == pytest.approx(0.23) + assert get_rav4_interceptor_pedal_scale(MIN_ACC_SPEED) == pytest.approx(0.3) + def test_interceptor_corolla_scales_with_accel_request_when_pedal_enables_sng(self): controller = self._make_controller() controller.CP.enableGasInterceptorDEPRECATED = True diff --git a/opendbc_repo/opendbc/car/toyota/values.py b/opendbc_repo/opendbc/car/toyota/values.py index a02c6bd06..dede289f4 100644 --- a/opendbc_repo/opendbc/car/toyota/values.py +++ b/opendbc_repo/opendbc/car/toyota/values.py @@ -624,6 +624,11 @@ ANGLE_CONTROL_CAR = CAR.with_flags(ToyotaFlags.ANGLE_CONTROL) SECOC_CAR = CAR.with_flags(ToyotaFlags.SECOC) +TOYOTA_AUTO_HOLD_CARS = (TSS2_CAR - RADAR_ACC_CAR - SECOC_CAR) | { + CAR.TOYOTA_RAV4, + CAR.TOYOTA_RAV4H, +} + # no resume button press required NO_STOP_TIMER_CAR = CAR.with_flags(ToyotaFlags.NO_STOP_TIMER) diff --git a/opendbc_repo/opendbc/safety/modes/ford.h b/opendbc_repo/opendbc/safety/modes/ford.h index 241fe993f..49ef7df36 100644 --- a/opendbc_repo/opendbc/safety/modes/ford.h +++ b/opendbc_repo/opendbc/safety/modes/ford.h @@ -2,10 +2,11 @@ #include "opendbc/safety/declarations.h" -// StarPilot's extended Ford curvature/angle enforcement below is substantially adapted from +// StarPilot's extended Ford curvature enforcement below is substantially adapted from // BluePilot bp-7.0 panda work, principally Alan Polk's 8f8d6d15f0a590f42b78de964ffb0d0af7f5d63d -// with shadow-curvature contributions from Jacob Neulight. See /CREDITS.md and -// /THIRD_PARTY_NOTICES.md. This comment does not attribute the surrounding upstream openpilot code. +// See /CREDITS.md and /THIRD_PARTY_NOTICES.md. This comment does not attribute the surrounding +// upstream openpilot code. + // Safety-relevant CAN messages for Ford vehicles. #define FORD_EngBrakeData 0x165U // RX from PCM, for driver brake pedal and cruise state @@ -93,10 +94,8 @@ static bool ford_get_quality_flag_valid(const CANPacket_t *msg) { static bool ford_lka_steering = false; static bool ford_extended_lateral = false; -static bool ford_angle_mode = false; static bool ford_longitudinal = false; static bool ford_cancel_resume_button = false; -static int16_t ford_shadow_curvature = 0; // Curvature rate limits #define FORD_LIMITS(limit_lateral_acceleration) { \ @@ -143,38 +142,6 @@ static const AngleSteeringLimits FORD_STEERING_LIMITS = FORD_LIMITS(false); static const AngleSteeringLimits FORD_EXTENDED_STEERING_LIMITS = FORD_EXTENDED_LIMITS(false); -static int ford_desired_path_angle_last = 0; - -static bool ford_path_angle_checks(int desired_path_angle, bool steer_control_enabled) { - bool violation = false; - if (steer_control_enabled) { - float speed = ((float)vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.0; - const struct lookup_t path_angle_rate = { - .x = {10., 15., 25.}, - .y = {0.0561, 0.04335, 0.00918}, - }; - int max_delta = (safety_interpolate(path_angle_rate, speed) * 2000.0) + 1.0; - violation |= safety_max_limit_check(desired_path_angle, - ford_desired_path_angle_last + max_delta, - ford_desired_path_angle_last - max_delta); - } else { - violation |= desired_path_angle != 0; - } - ford_desired_path_angle_last = violation ? 0 : desired_path_angle; - return violation; -} - -static bool ford_shadow_curvature_check(int desired_curvature, bool steer_control_enabled, - const AngleSteeringLimits limits) { - if (steer_control_enabled && limits.enforce_angle_error && - ((vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR) > limits.angle_error_min_speed)) { - int lowest_allowed = angle_meas.min - limits.max_angle_error - 1; - int highest_allowed = angle_meas.max + limits.max_angle_error + 1; - return safety_max_limit_check(desired_curvature, highest_allowed, lowest_allowed); - } - return false; -} - static void ford_rx_hook(const CANPacket_t *msg) { if (msg->bus == FORD_MAIN_BUS) { // Update in motion state from standstill signal @@ -305,10 +272,8 @@ static bool ford_tx_hook(const CANPacket_t *msg) { } if (!ford_lka_steering) { - ford_angle_mode = (msg->data[4] & 0x1U) != 0U; ford_extended_lateral = (msg->data[4] & 0x2U) != 0U; - ford_shadow_curvature = (int16_t)((msg->data[5] << 8) | msg->data[6]); - if (ford_angle_mode && !ford_extended_lateral) { + if ((msg->data[4] & 0x1U) != 0U) { tx = false; } } @@ -332,20 +297,9 @@ static bool ford_tx_hook(const CANPacket_t *msg) { if (ford_extended_lateral) { violation |= desired_path_offset != 0; violation |= (desired_curvature_rate < -4096) || (desired_curvature_rate > 4095); - violation |= ford_path_angle_checks(desired_path_angle, steer_control_enabled); - if (ford_angle_mode) { - violation |= (desired_path_angle < -1000) || (desired_path_angle > 1047); - violation |= desired_curvature != 0; - violation |= steer_control_enabled && !(aol_allowed || controls_allowed); - int shadow_curvature_can = ROUND((float)ford_shadow_curvature * 0.05); - violation |= ford_shadow_curvature_check(shadow_curvature_can, steer_control_enabled, - FORD_EXTENDED_STEERING_LIMITS); - desired_angle_last = 0; - } else { - violation |= desired_path_angle != 0; - violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled, - FORD_EXTENDED_STEERING_LIMITS); - } + violation |= desired_path_angle != 0; + violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled, + FORD_EXTENDED_STEERING_LIMITS); if (!steer_control_enabled) { violation |= (desired_curvature != 0) || (desired_curvature_rate != 0); } @@ -382,20 +336,9 @@ static bool ford_tx_hook(const CANPacket_t *msg) { if (ford_extended_lateral) { violation |= desired_path_offset != 0; violation |= (desired_curvature_rate < -1024) || (desired_curvature_rate > 1023); - violation |= ford_path_angle_checks(desired_path_angle, steer_control_enabled); - if (ford_angle_mode) { - violation |= (desired_path_angle < -1000) || (desired_path_angle > 1047); - violation |= desired_curvature != 0; - violation |= steer_control_enabled && !(aol_allowed || controls_allowed); - int shadow_curvature_can = ROUND((float)ford_shadow_curvature * 0.05); - violation |= ford_shadow_curvature_check(shadow_curvature_can, steer_control_enabled, - FORD_CANFD_EXTENDED_STEERING_LIMITS); - desired_angle_last = 0; - } else { - violation |= desired_path_angle != 0; - violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled, - FORD_CANFD_EXTENDED_STEERING_LIMITS); - } + violation |= desired_path_angle != 0; + violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled, + FORD_CANFD_EXTENDED_STEERING_LIMITS); if (!steer_control_enabled) { violation |= (desired_curvature != 0) || (desired_curvature_rate != 0); } @@ -466,10 +409,7 @@ static safety_config ford_init(uint16_t param) { const bool ford_canfd = GET_FLAG(param, FORD_PARAM_CANFD); ford_lka_steering = GET_FLAG(param, FORD_PARAM_LKA_STEERING); ford_extended_lateral = false; - ford_angle_mode = false; ford_cancel_resume_button = false; - ford_shadow_curvature = 0; - ford_desired_path_angle_last = 0; ford_longitudinal = false; diff --git a/opendbc_repo/opendbc/safety/modes/tesla_preap.h b/opendbc_repo/opendbc/safety/modes/tesla_preap.h index f70616a20..98f78cd00 100644 --- a/opendbc_repo/opendbc/safety/modes/tesla_preap.h +++ b/opendbc_repo/opendbc/safety/modes/tesla_preap.h @@ -224,10 +224,16 @@ static void tesla_preap_rx_hook(const CANPacket_t *msg) { } if (msg->addr == 0x368U) { - const int cruise_state = (msg->data[1] >> 4) & 0x07U; + const int cruise_state = (msg->data[1] >> 4) & 0x0FU; if (cruise_state == 3) { vehicle_moving = false; } + acc_main_on = (cruise_state == 1) || + (cruise_state == 2) || + (cruise_state == 3) || + (cruise_state == 4) || + (cruise_state == 6) || + (cruise_state == 7); } if (msg->addr == 0x118U) { diff --git a/opendbc_repo/opendbc/safety/tests/test_ford.py b/opendbc_repo/opendbc/safety/tests/test_ford.py index fa27f7462..b8a21a121 100755 --- a/opendbc_repo/opendbc/safety/tests/test_ford.py +++ b/opendbc_repo/opendbc/safety/tests/test_ford.py @@ -170,6 +170,11 @@ class TestFordSafetyBase(common.CarSafetyTest): } return self.packer.make_can_msg_safety("Lane_Assist_Data1", 0, values) + def _extended_lka_msg(self, angle_mode=False): + msg = self._lkas_command_msg(0) + msg[0].data[4] |= 0x2 | int(angle_mode) + return msg + # LCA command def _lat_ctl_msg(self, enabled: bool, path_offset: float, path_angle: float, curvature: float, curvature_rate: float): if self.STEER_MESSAGE == MSG_LateralMotionControl: @@ -376,6 +381,25 @@ class TestFordSafetyBase(common.CarSafetyTest): should_tx |= self.LKA_STEERING and controls_allowed and action in (2, 4) self.assertEqual(should_tx, self._tx(self._lkas_command_msg(action))) + def test_extended_angle_mode_rejected(self): + if self.LKA_STEERING: + return + + self.assertTrue(self._tx(self._extended_lka_msg())) + self.assertFalse(self._tx(self._extended_lka_msg(angle_mode=True))) + + def test_extended_curvature_signals(self): + if self.LKA_STEERING: + return + + speed = 15.0 + self.safety.set_controls_allowed(True) + self._reset_curvature_measurement(0.0, speed) + self.assertTrue(self._tx(self._extended_lka_msg())) + self.assertTrue(self._tx(self._lat_ctl_msg(True, 0.0, 0.0, 0.001, 0.0005))) + self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.1, 0.0, 0.001, 0.0005))) + self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.02, 0.001, 0.0005))) + def test_acc_buttons(self): for allowed in (0, 1): self.safety.set_controls_allowed(allowed) @@ -443,39 +467,6 @@ class TestFordCANFDStockSafety(TestFordSafetyBase): self.safety.set_safety_hooks(CarParams.SafetyModel.ford, FordSafetyFlags.CANFD) self.safety.init_tests() - def _extended_lka_msg(self, angle_mode=False, shadow_curvature=0.0): - msg = self._lkas_command_msg(0) - raw_shadow = int(round(shadow_curvature / 1e-6)) & 0xFFFF - msg[0].data[4] |= 0x2 | int(angle_mode) - msg[0].data[5] = raw_shadow >> 8 - msg[0].data[6] = raw_shadow & 0xFF - return msg - - def test_extended_curvature_signals(self): - speed = 15.0 - self.safety.set_controls_allowed(True) - self._reset_curvature_measurement(0.0, speed) - self.assertTrue(self._tx(self._extended_lka_msg())) - self.assertTrue(self._tx(self._lat_ctl_msg(True, 0.0, 0.0, 0.001, 0.0005))) - - self.assertTrue(self._tx(self._extended_lka_msg())) - self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.1, 0.0, 0.001, 0.0005))) - - def test_extended_angle_signals(self): - speed = 15.0 - curvature = 0.005 - self.safety.set_controls_allowed(True) - self._reset_curvature_measurement(curvature, speed) - self.assertTrue(self._tx(self._extended_lka_msg(angle_mode=True, shadow_curvature=curvature))) - self.assertTrue(self._tx(self._lat_ctl_msg(True, 0.0, 0.02, 0.0, 0.0))) - - self.assertTrue(self._tx(self._extended_lka_msg(angle_mode=True, shadow_curvature=curvature))) - self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.2, 0.0, 0.0))) - - self.assertTrue(self._tx(self._extended_lka_msg(angle_mode=True, shadow_curvature=-curvature))) - self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.01, 0.0, 0.0))) - - class TestFordStockSafety(TestFordSafetyBase): STEER_MESSAGE = MSG_LateralMotionControl STOCK_LONGITUDINAL = True diff --git a/opendbc_repo/opendbc/safety/tests/test_tesla_preap.py b/opendbc_repo/opendbc/safety/tests/test_tesla_preap.py index bc9cbc742..c64d26d19 100644 --- a/opendbc_repo/opendbc/safety/tests/test_tesla_preap.py +++ b/opendbc_repo/opendbc/safety/tests/test_tesla_preap.py @@ -2,6 +2,7 @@ import unittest from opendbc.car.tesla.preap.interface import SAFETY_TESLA_PREAP +from opendbc.safety import ALTERNATIVE_EXPERIENCE from opendbc.safety.tests.libsafety import libsafety_py import opendbc.safety.tests.common as common @@ -36,6 +37,12 @@ class TestTeslaPreAPSafety(common.SafetyTestBase): dat[0] = lever & 0x3F return common.make_msg(0, 0x45, 8, dat) + @staticmethod + def _di_state_msg(cruise_state: int): + dat = bytearray(8) + dat[1] = (cruise_state & 0x0F) << 4 + return common.make_msg(0, 0x368, 8, dat) + @staticmethod def _steering_status_msg(hands_on_level: int = 0, eac_status: int = 1, eac_error_code: int = 0, angle_tenths: int = 0): raw_angle = angle_tenths + 8192 @@ -123,6 +130,30 @@ class TestTeslaPreAPSafety(common.SafetyTestBase): self.assertTrue(self._tx(self._epas_control_msg(1))) self.assertFalse(self._tx(self._epas_control_msg(2))) + def test_always_on_lateral(self): + ENABLED, OFF = 2, 0 + self.safety.set_controls_allowed(False) + + self.safety.set_alternative_experience(0) + self.assertTrue(self._rx(self._di_state_msg(ENABLED))) + self.assertFalse(self._tx(self._steer_cmd_msg(0, 1))) + + self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL) + self.assertTrue(self._rx(self._di_state_msg(OFF))) + self.assertFalse(self.safety.get_controls_allowed()) + self.assertFalse(self._tx(self._steer_cmd_msg(0, 1))) + + self.assertTrue(self._rx(self._di_state_msg(ENABLED))) + self.assertFalse(self.safety.get_controls_allowed()) + self.assertTrue(self._tx(self._steer_cmd_msg(0, 1))) + + self.assertTrue(self._rx(self._di_state_msg(OFF))) + self.assertFalse(self._tx(self._steer_cmd_msg(0, 1))) + + self.assertTrue(self._rx(self._di_state_msg(9))) + self.assertFalse(self.safety.get_aol_allowed()) + self.assertFalse(self._tx(self._steer_cmd_msg(0, 1))) + def test_aeb_is_blocked(self): self.safety.set_controls_allowed(True) self.assertTrue(self._tx(self._long_msg(0))) diff --git a/panda/board/obj/gitversion.h b/panda/board/obj/gitversion.h index a87b22d0d..8c37342cd 100644 --- a/panda/board/obj/gitversion.h +++ b/panda/board/obj/gitversion.h @@ -1,2 +1,2 @@ extern const uint8_t gitversion[19]; -const uint8_t gitversion[19] = "DEV-9cb1b6b1-DEBUG"; +const uint8_t gitversion[19] = "DEV-7a83f8f4-DEBUG"; diff --git a/panda/board/obj/version b/panda/board/obj/version index edc1913db..ebe7e4ab9 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-9cb1b6b1-DEBUG \ No newline at end of file +DEV-7a83f8f4-DEBUG \ No newline at end of file diff --git a/rednose_repo/rednose/helpers/ekf_sym_pyx.so b/rednose_repo/rednose/helpers/ekf_sym_pyx.so index 0c2e6104d..985b6c785 100755 Binary files a/rednose_repo/rednose/helpers/ekf_sym_pyx.so and b/rednose_repo/rednose/helpers/ekf_sym_pyx.so differ diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 8ab7f84c5..6062b4780 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -6,8 +6,6 @@ from openpilot.selfdrive.controls.lib.latcontrol import LatControl # TODO This is speed dependent STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees -FORD_ANGLE_CATCH_UP_HORIZON = 1.25 # Seconds -FORD_ANGLE_RATE_FILTER_TIME_CONSTANT = 0.15 # Seconds _ASCENT_ANGLE_TRACKING_GAIN = 0.25 _ASCENT_ANGLE_TRACKING_MAX_CORRECTION = 8.0 @@ -25,32 +23,12 @@ def _ascent_angle_tracking_target(target_angle: float, steering_angle: float, return target_angle + correction -def _ford_angle_tracking_saturated(angle_error: float, steering_rate: float) -> bool: - """Only call a Ford angle request saturated when the EPS is not on track to catch it.""" - catching_up = angle_error * steering_rate > 0.0 - catching_up &= abs(angle_error) <= abs(steering_rate) * FORD_ANGLE_CATCH_UP_HORIZON - return abs(angle_error) > STEER_ANGLE_SATURATION_THRESHOLD and not catching_up - - class LatControlAngle(LatControl): def __init__(self, CP, CI, dt): super().__init__(CP, CI, dt) self.sat_check_min_speed = 5. self.use_steer_limited_by_safety = CP.brand in ("tesla", "hyundai") self.is_ascent = CP.carFingerprint == SUBARU_CAR.SUBARU_ASCENT_2023 - self.is_ford = CP.brand == "ford" - self.measured_angle_last = None - self.measured_angle_rate = 0.0 - - def _update_measured_angle_rate(self, steering_angle: float, reset: bool) -> float: - if reset or self.measured_angle_last is None: - self.measured_angle_rate = 0.0 - else: - raw_rate = (steering_angle - self.measured_angle_last) / self.dt - alpha = self.dt / (FORD_ANGLE_RATE_FILTER_TIME_CONSTANT + self.dt) - self.measured_angle_rate += alpha * (raw_rate - self.measured_angle_rate) - self.measured_angle_last = steering_angle - return self.measured_angle_rate def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay, calibrated_pose, model_data, starpilot_toggles): angle_log = log.ControlsState.LateralAngleState.new_message() @@ -71,10 +49,6 @@ class LatControlAngle(LatControl): bool(getattr(CS, "steeringPressed", False)), ) - ford_angle_mode = self.is_ford and getattr(starpilot_toggles, "ford_lateral_mode", -1) == 2 - measured_angle_rate = self._update_measured_angle_rate( - float(CS.steeringAngleDeg), not active or bool(CS.steeringPressed) or not ford_angle_mode) - if self.use_steer_limited_by_safety: # these cars' carcontrollers calculate max lateral accel and jerk, so we can rely on carOutput for saturation angle_control_saturated = steer_limited_by_safety @@ -82,10 +56,7 @@ class LatControlAngle(LatControl): # for cars which use a method of limiting torque such as a torque signal (Nissan and Toyota) # or relying on EPS (Ford Q3), carOutput does not capture maxing out torque # TODO: this can be improved angle_error = angle_steers_des - CS.steeringAngleDeg - if ford_angle_mode: - angle_control_saturated = _ford_angle_tracking_saturated(angle_error, measured_angle_rate) - else: - angle_control_saturated = abs(angle_error) > STEER_ANGLE_SATURATION_THRESHOLD + angle_control_saturated = abs(angle_error) > STEER_ANGLE_SATURATION_THRESHOLD angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False, curvature_limited)) angle_log.steeringAngleDeg = float(CS.steeringAngleDeg) angle_log.steeringAngleDesiredDeg = angle_steers_des diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index d9299a490..7688e8cd0 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -520,7 +520,7 @@ class LatControlTorque(LatControl): ) elif genesis_g70_active: vehicle_friction_jerk_deadzone = get_genesis_g70_friction_jerk_deadzone( - CS.vEgo, setpoint, desired_lateral_jerk, + CS.vEgo, setpoint, desired_lateral_jerk, measurement, ) elif self.is_genesis_gv70: vehicle_friction_jerk_deadzone = get_genesis_gv70_friction_jerk_deadzone(CS.vEgo, setpoint) diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 2beaef92c..f179381ef 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -691,7 +691,7 @@ PALISADE_CENTER_TAPER_LAT = 0.28 PALISADE_CENTER_TAPER_LAT_WIDTH = 0.055 PALISADE_CENTER_TAPER_SPEED = 12.0 PALISADE_CENTER_TAPER_SPEED_WIDTH = 2.5 -PALISADE_CENTER_OUTPUT_TAPER_MAX = 0.12 +PALISADE_CENTER_OUTPUT_TAPER_MAX = 0.18 PALISADE_CENTER_OUTPUT_TAPER_LAT = 0.28 PALISADE_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.055 PALISADE_CENTER_OUTPUT_TAPER_SPEED = 15.0 @@ -3199,14 +3199,17 @@ def get_genesis_g70_friction_threshold(v_ego: float, desired_lateral_accel: floa def get_genesis_g70_friction_jerk_deadzone(v_ego: float, desired_lateral_accel: float, - desired_lateral_jerk: float = 0.0) -> float: + desired_lateral_jerk: float = 0.0, + measured_lateral_accel: float = 0.0) -> float: speed_weight = _sigmoid((v_ego - GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED) / GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED_WIDTH) center_weight = _sigmoid((GENESIS_G70_FRICTION_JERK_DEADZONE_LAT - abs(desired_lateral_accel)) / GENESIS_G70_FRICTION_JERK_DEADZONE_LAT_WIDTH) deadzone = GENESIS_G70_FRICTION_JERK_DEADZONE_MAX * speed_weight * center_weight - if desired_lateral_accel * desired_lateral_jerk < 0.0: + overshoot = max(abs(measured_lateral_accel) - abs(desired_lateral_accel), 0.0) + if (desired_lateral_accel * desired_lateral_jerk < 0.0 and + desired_lateral_accel * measured_lateral_accel > 0.0 and overshoot > 0.0): curve_speed_weight = _sigmoid( (max(v_ego, 0.0) - GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_SPEED) / GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_SPEED_WIDTH @@ -3223,8 +3226,9 @@ def get_genesis_g70_friction_jerk_deadzone(v_ego: float, desired_lateral_accel: (abs(desired_lateral_jerk) - GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_JERK) / GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_JERK_WIDTH ) + overshoot_weight = _sigmoid((overshoot - 0.08) / 0.10) deadzone += (GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_MAX * curve_speed_weight * - curve_onset_weight * curve_cutoff_weight * jerk_weight) + curve_onset_weight * curve_cutoff_weight * jerk_weight * overshoot_weight) return deadzone diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index a23b1e8ee..9b1804338 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -1,4 +1,3 @@ -import math import pytest from parameterized import parameterized from types import SimpleNamespace @@ -21,7 +20,6 @@ from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.controls.lib.latcontrol_angle import ( LatControlAngle, _ascent_angle_tracking_target, - _ford_angle_tracking_saturated, ) from openpilot.selfdrive.controls.lib.latcontrol_pid import ( LatControlPID, @@ -205,41 +203,6 @@ class TestLatControl: assert _ascent_angle_tracking_target(10.0, 0.0, 4.0, False) == pytest.approx(10.0) assert _ascent_angle_tracking_target(10.0, 0.0, 20.0, True) == pytest.approx(10.0) - def test_ford_angle_tracking_does_not_report_a_responsive_eps_as_saturated(self): - assert not _ford_angle_tracking_saturated(12.0, 12.0) - assert not _ford_angle_tracking_saturated(-12.0, -12.0) - assert _ford_angle_tracking_saturated(16.0, 12.0) - assert _ford_angle_tracking_saturated(12.0, -12.0) - - def test_ford_angle_tracking_still_reports_a_stalled_eps(self): - assert _ford_angle_tracking_saturated(3.0, 0.0) - assert not _ford_angle_tracking_saturated(2.5, 0.0) - - def test_ford_angle_handoff_saturation_waits_for_eps_response(self): - CP = SimpleNamespace( - steerLimitTimer=1.0, - brand="ford", - carFingerprint="FORD_MUSTANG_MACH_E_MK1", - ) - controller = LatControlAngle(CP, None, DT_CTRL) - target = [12.0] - VM = SimpleNamespace(get_steer_from_curvature=lambda *_args: math.radians(target[0])) - CS = car.CarState.new_message(vEgo=10.0, steeringPressed=False) - params = log.LiveParametersData.new_message(angleOffsetDeg=0.0, roll=0.0) - toggles = SimpleNamespace(ford_lateral_mode=2) - - for frame in range(round(2.0 / DT_CTRL)): - CS.steeringAngleDeg = frame * 12.0 * DT_CTRL - target[0] = CS.steeringAngleDeg + 12.0 - _, _, angle_log = controller.update( - True, CS, VM, params, False, 0.0, False, 0.0, None, None, toggles) - assert not angle_log.saturated - - for _ in range(round(2.0 / DT_CTRL)): - _, _, angle_log = controller.update( - True, CS, VM, params, False, 0.0, False, 0.0, None, None, toggles) - assert angle_log.saturated - def test_torque_log_exposes_friction_controller_state(self): controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(GM.CHEVROLET_BOLT_ACC_2022_2023) @@ -859,7 +822,7 @@ class TestLatControl: assert low_speed_center > highway_center assert highway_center < highway_turn <= 1.0 - assert highway_center > 0.89 + assert highway_center > 0.87 def test_prius_ff_scale_curve(self): assert get_prius_ff_scale(0.0, 0.0, 20.0) == 1.0 @@ -1002,8 +965,8 @@ class TestLatControl: assert get_genesis_g70_angle_output_scale(55.0, 1.0) > get_genesis_g70_angle_output_scale(85.0, 1.0) assert get_genesis_g70_angle_output_scale(85.0, -1.0) == pytest.approx(1.0) assert get_genesis_g70_friction_jerk_deadzone(25.0, 0.0) > 0.25 - hwy_unwind_deadzone = get_genesis_g70_friction_jerk_deadzone(68.0 * 0.44704, 0.8, -0.6) - hwy_turn_in_deadzone = get_genesis_g70_friction_jerk_deadzone(68.0 * 0.44704, 0.8, 0.6) + hwy_unwind_deadzone = get_genesis_g70_friction_jerk_deadzone(68.0 * 0.44704, 0.8, -0.6, 1.0) + hwy_turn_in_deadzone = get_genesis_g70_friction_jerk_deadzone(68.0 * 0.44704, 0.8, 0.6, 0.5) assert hwy_unwind_deadzone > hwy_turn_in_deadzone assert hwy_unwind_deadzone > 0.08 assert get_genesis_g70_unwind_ff_scale(-0.7, -0.95, 0.5, 25.0) < 0.90 diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index b8d294e6f..688a40d82 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -52,11 +52,13 @@ from openpilot.selfdrive.modeld.helpers import get_tg_input_devices, load_oob, t from openpilot.selfdrive.modeld.usbgpu_link import wait_usbgpu_link from openpilot.starpilot.assets.model_manager import ( ModelManager, + get_model_profile, load_model_artifact_metadata, model_accelerator_artifact_available, model_accelerator_artifact_installed, model_accelerator_artifact_path, model_uses_external_gpu, + set_runtime_model_params, ) from openpilot.starpilot.common.model_lab import ( MODEL_LAB_RUNTIME_PARAM, @@ -781,9 +783,16 @@ class ModelState: def _load_model_state(cam_w: int, cam_h: int, selected_model: str, external_gpu_requested: bool, - params: Params) -> ModelState: + params: Params, model_version: str = "", write_model_version: bool = True) -> ModelState: try: - return ModelState(cam_w, cam_h, external_gpu_requested) + return ModelState( + cam_w, + cam_h, + external_gpu_requested, + model_id_override=selected_model, + write_model_version=write_model_version, + model_version_override=model_version, + ) except Exception: if selected_model == BUILTIN_MODEL_KEY: raise @@ -795,10 +804,16 @@ def _load_model_state(cam_w: int, cam_h: int, selected_model: str, external_gpu_ device_config = tinygrad_dev_config(False, TICI) DEV.value = device_config os.environ["DEV"] = device_config - return ModelState(cam_w, cam_h, False) + return ModelState( + cam_w, + cam_h, + False, + model_id_override=BUILTIN_MODEL_KEY, + write_model_version=write_model_version, + ) -def _load_external_gpu_model(cam_w: int, cam_h: int, selected_model: str, +def _load_external_gpu_model(cam_w: int, cam_h: int, selected_model: str, model_version: str = "", CP=None, demo: bool = False) -> ModelState | None: """Load and warm the USB-GPU model without running another tinygrad model concurrently.""" candidate = None @@ -813,6 +828,7 @@ def _load_external_gpu_model(cam_w: int, cam_h: int, selected_model: str, True, model_id_override=selected_model, write_model_version=False, + model_version_override=model_version, ) if not candidate.uses_external_gpu: raise RuntimeError("external GPU model resolved to the builtin model") @@ -987,15 +1003,20 @@ def main(demo=False): config_realtime_process(7, 54) params = Params() - selected_model = _canonical_model_id(_resolve_mirrored_param(params, "Model", "DrivingModel") or BUILTIN_MODEL_KEY) usbgpu_present_now = usbgpu_present() + small_model_id, _, small_model_version = get_model_profile(params, "small") + big_model_id, _, big_model_version = get_model_profile(params, "big") + small_model_id = _canonical_model_id(small_model_id or BUILTIN_MODEL_KEY) + big_model_id = _canonical_model_id(big_model_id) + selected_model = big_model_id if usbgpu_present_now and big_model_id else small_model_id + selected_model_version = big_model_version if selected_model == big_model_id else small_model_version model_lab_config, model_lab_error = _model_lab_runtime_request(params, usbgpu_present_now) model_lab_requested = bool(model_lab_config["enabled"]) model_lab_ready = model_lab_requested and model_lab_error is None - external_model_selected = model_uses_external_gpu(selected_model) - external_artifact = MODELS_PATH / f"{selected_model}_driving_tinygrad.pkl" + external_model_selected = bool(big_model_id) and model_uses_external_gpu(big_model_id) + external_artifact = MODELS_PATH / f"{big_model_id}_driving_tinygrad.pkl" external_artifact_ready = external_model_selected and file_chunked_exists(external_artifact) - external_gpu_requested = usbgpu_present_now and (external_model_selected or model_lab_ready) + external_gpu_requested = usbgpu_present_now and (bool(big_model_id) or model_lab_ready) params.put_bool("UsbGpuPresent", usbgpu_present_now) params.put_bool("UsbGpuCompiled", external_artifact_ready or model_lab_ready) params.put_bool("UsbGpuActive", False) @@ -1045,12 +1066,14 @@ def main(demo=False): CP = get_demo_car_params() else: CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) - small_model = ModelState( + small_model = _load_model_state( vipc_client_main.width, vipc_client_main.height, + small_model_id, + False, + params, + small_model_version, False, - model_id_override=BUILTIN_MODEL_KEY, - write_model_version=False, ) versions = _model_versions() lateral_id = model_lab_config["lateralModel"] @@ -1071,7 +1094,7 @@ def main(demo=False): params.put("ModelVersion", model.policy_generation) params.put("DrivingModelVersion", model.policy_generation) else: - model_lab_error = "one or both precompiled AMD models failed to load; using the built-in model" + model_lab_error = "one or both precompiled AMD models failed to load; using the active small model" cloudlog.error(f"Model Laboratory unavailable: {model_lab_error}") model = small_model params.put("ModelVersion", model.policy_generation) @@ -1087,23 +1110,36 @@ def main(demo=False): vipc_client_main.width, vipc_client_main.height, selected_model, + selected_model_version, CP, demo, ) - small_model = ModelState( + small_model = _load_model_state( vipc_client_main.width, vipc_client_main.height, + small_model_id, + False, + params, + small_model_version, False, - model_id_override=BUILTIN_MODEL_KEY, - write_model_version=False, ) model = big_model if big_model is not None else small_model if big_model is not None: params.put("ModelVersion", model.policy_generation) params.put("DrivingModelVersion", model.policy_generation) else: - model = _load_model_state(vipc_client_main.width, vipc_client_main.height, selected_model, False, params) + model = _load_model_state( + vipc_client_main.width, + vipc_client_main.height, + selected_model, + False, + params, + selected_model_version, + ) + + if not model_lab_active: + set_runtime_model_params(params, model.model_id, model.policy_generation) external_gpu_active = model_lab_active or model.uses_external_gpu params.put_bool("UsbGpuCompiled", external_artifact_ready or model_lab_ready) @@ -1243,14 +1279,15 @@ def main(demo=False): model_lab_active = False model_lab_longitudinal = None if small_model is None: - raise RuntimeError("Model Laboratory has no built-in fallback model") + raise RuntimeError("Model Laboratory has no active small fallback model") model = small_model external_gpu_active = False - model_lab_error = "Chestnut disconnected; using the built-in model" + model_lab_error = "Chestnut disconnected; using the active small model" params.put_bool("UsbGpuPresent", False) params.put_bool("UsbGpuActive", False) params.put("ModelVersion", model.policy_generation) params.put("DrivingModelVersion", model.policy_generation) + set_runtime_model_params(params, model.model_id, model.policy_generation) _set_model_lab_runtime( params, requested=model_lab_requested, @@ -1335,13 +1372,13 @@ def main(demo=False): lateral_model_output = model_output except Exception: if model_lab_active: - cloudlog.exception("Model Laboratory inference failed, falling back to the built-in model") + cloudlog.exception("Model Laboratory inference failed, falling back to the active small model") if small_model is None: - raise RuntimeError("Model Laboratory has no built-in fallback model") from None + raise RuntimeError("Model Laboratory has no active small fallback model") from None model = small_model model_lab_longitudinal = None model_lab_active = False - model_lab_error = "Model Laboratory inference failed; using the built-in model" + model_lab_error = "Model Laboratory inference failed; using the active small model" _set_model_lab_runtime( params, requested=model_lab_requested, @@ -1352,13 +1389,14 @@ def main(demo=False): else: if not external_gpu_active or small_model is None: raise - cloudlog.exception("external GPU model failed, falling back to builtin model") + cloudlog.exception("external GPU model failed, falling back to active small model") model = small_model big_model = None params.put_bool("UsbGpuActive", False) external_gpu_active = False params.put("ModelVersion", model.policy_generation) params.put("DrivingModelVersion", model.policy_generation) + set_runtime_model_params(params, model.model_id, model.policy_generation) params.put_bool("UsbGpuLoading", False) if chestnut_state is not None: chestnut_state.big = False diff --git a/selfdrive/modeld/tests/test_model_fallback.py b/selfdrive/modeld/tests/test_model_fallback.py index 7cf1e8726..6e2db8af1 100644 --- a/selfdrive/modeld/tests/test_model_fallback.py +++ b/selfdrive/modeld/tests/test_model_fallback.py @@ -29,8 +29,8 @@ def test_incompatible_downloaded_model_falls_back_to_builtin(monkeypatch): calls = [] builtin_model = object() - def load_model(cam_w, cam_h, external_gpu_active): - calls.append((cam_w, cam_h, external_gpu_active)) + def load_model(cam_w, cam_h, external_gpu_active, **kwargs): + calls.append((cam_w, cam_h, external_gpu_active, kwargs.get("model_id_override"))) if len(calls) == 1: raise TypeError("incompatible artifact") return builtin_model @@ -40,7 +40,10 @@ def test_incompatible_downloaded_model_falls_back_to_builtin(monkeypatch): monkeypatch.setattr(modeld.cloudlog, "exception", lambda *_args, **_kwargs: None) assert modeld._load_model_state(1928, 1208, "custom-model", False, params) is builtin_model - assert calls == [(1928, 1208, False), (1928, 1208, False)] + assert calls == [ + (1928, 1208, False, "custom-model"), + (1928, 1208, False, modeld.BUILTIN_MODEL_KEY), + ] assert params.values == { "Model": modeld.BUILTIN_MODEL_KEY, "DrivingModel": modeld.BUILTIN_MODEL_KEY, @@ -49,7 +52,7 @@ def test_incompatible_downloaded_model_falls_back_to_builtin(monkeypatch): def test_builtin_model_load_failure_is_not_hidden(monkeypatch): - monkeypatch.setattr(modeld, "ModelState", lambda *_args: (_ for _ in ()).throw(TypeError("bad builtin"))) + monkeypatch.setattr(modeld, "ModelState", lambda *_args, **_kwargs: (_ for _ in ()).throw(TypeError("bad builtin"))) with pytest.raises(TypeError, match="bad builtin"): modeld._load_model_state(1928, 1208, modeld.BUILTIN_MODEL_KEY, False, FakeParams()) diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index eb4c457b4..6041f86ae 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/pandad/pandad.cc b/selfdrive/pandad/pandad.cc index d3f757ff6..0b422aca0 100644 --- a/selfdrive/pandad/pandad.cc +++ b/selfdrive/pandad/pandad.cc @@ -45,8 +45,7 @@ ExitHandler do_exit; static uint64_t last_door_lock_command_time = 0; -static bool is_tesla_preap(Params ¶ms) { - const std::string car_params = params.get("CarParams"); +static bool is_tesla_preap(const std::string &car_params) { if (car_params.empty()) { return false; } @@ -483,6 +482,9 @@ void pandad_run(std::vector &pandas) { Panda *peripheral_panda = pandas[0]; bool engaged = false; bool is_onroad = false; + bool was_onroad = false; + bool tesla_preap = false; + bool tesla_preap_checked = false; // Main loop: receive CAN data and process states while (!do_exit && check_all_connected(pandas)) { @@ -496,13 +498,25 @@ void pandad_run(std::vector &pandas) { // Process panda state at 10 Hz if (rk.frame() % 10 == 0) { sm.update(0); - const bool preap_aol_engaged = is_tesla_preap(params) && + is_onroad = params.getBool("IsOnroad"); + const bool ignore_ignition_line = params.getBool("IgnoreIgnitionLine"); + + if (is_onroad && !was_onroad) { + tesla_preap = false; + tesla_preap_checked = false; + } + was_onroad = is_onroad; + + if (is_onroad && !tesla_preap_checked && params.getBool("ControlsReady")) { + tesla_preap = is_tesla_preap(params.get("CarParams")); + tesla_preap_checked = true; + } + + const bool preap_aol_engaged = tesla_preap && sm["starpilotCarState"].getStarpilotCarState().getAlwaysOnLateralEnabled(); engaged = sm.allAliveAndValid({"selfdriveState", "starpilotCarState"}) && ( sm["selfdriveState"].getSelfdriveState().getEnabled() || preap_aol_engaged ); - is_onroad = params.getBool("IsOnroad"); - const bool ignore_ignition_line = params.getBool("IgnoreIgnitionLine"); process_panda_state(pandas, &pm, engaged, is_onroad, spoofing_started, ignore_ignition_line); panda_safety.configureSafetyMode(is_onroad); } diff --git a/selfdrive/ui/layouts/settings/starpilot/driving_model.py b/selfdrive/ui/layouts/settings/starpilot/driving_model.py index 3f13a3aad..eb9e0e1c1 100644 --- a/selfdrive/ui/layouts/settings/starpilot/driving_model.py +++ b/selfdrive/ui/layouts/settings/starpilot/driving_model.py @@ -23,6 +23,7 @@ from openpilot.starpilot.assets.model_manager import ( is_builtin_model_key, model_uses_external_gpu, model_key_aliases, + set_model_profile, ) from openpilot.starpilot.common.starpilot_variables import MODELS_PATH, update_starpilot_toggles from openpilot.system.ui.lib.application import FontWeight, MouseEvent, MousePos, gui_app @@ -1089,6 +1090,13 @@ class StarPilotDrivingModelLayout(_SettingsPage): resolved_version = resolved_version or entry.version or self._default_model_version() self._params.put("ModelVersion", resolved_version) self._params.put("DrivingModelVersion", resolved_version) + set_model_profile( + self._params, + "big" if entry.requires_external_gpu else "small", + selected_model, + entry.name, + resolved_version, + ) update_starpilot_toggles() self._update_model_metadata() if ui_state.started: diff --git a/selfdrive/ui/mici/layouts/settings/driving_model.py b/selfdrive/ui/mici/layouts/settings/driving_model.py index 749e49c28..eb3270e84 100644 --- a/selfdrive/ui/mici/layouts/settings/driving_model.py +++ b/selfdrive/ui/mici/layouts/settings/driving_model.py @@ -9,7 +9,7 @@ from pathlib import Path from openpilot.common.file_chunker import get_chunk_name, get_manifest_path from openpilot.common.params import Params -from openpilot.starpilot.assets.model_manager import external_gpu_available, model_uses_external_gpu +from openpilot.starpilot.assets.model_manager import external_gpu_available, model_uses_external_gpu, set_model_profile from openpilot.selfdrive.ui.mici.widgets.button import BigButton from openpilot.selfdrive.ui.mici.widgets.dialog import BigDialog, BigDialogBase, BigMultiOptionDialog from openpilot.selfdrive.ui.ui_state import ui_state @@ -568,6 +568,13 @@ class DrivingModelBigButton(BigButton): if version: self._params.put("ModelVersion", version) self._params.put("DrivingModelVersion", version) + set_model_profile( + self._params, + "big" if entry.requires_external_gpu else "small", + entry.key, + entry.name, + version, + ) if ui_state.started: self._params.put_bool("OnroadCycleRequested", True) diff --git a/starpilot/assets/model_manager.py b/starpilot/assets/model_manager.py index c3fe49e77..13912eae2 100644 --- a/starpilot/assets/model_manager.py +++ b/starpilot/assets/model_manager.py @@ -31,6 +31,16 @@ from openpilot.system.hardware.usb import chestnut_firmware_ready MANIFEST_CANDIDATES = ("v25",) MODEL_NAMESPACE_SUFFIX = "3" DEFAULT_MODEL_KEY = "rdf43" +ACTIVE_BIG_MODEL_PARAM = "ActiveBigModel" +ACTIVE_BIG_MODEL_NAME_PARAM = "ActiveBigModelName" +ACTIVE_BIG_MODEL_VERSION_PARAM = "ActiveBigModelVersion" +ACTIVE_SMALL_MODEL_PARAM = "ActiveSmallModel" +ACTIVE_SMALL_MODEL_NAME_PARAM = "ActiveSmallModelName" +ACTIVE_SMALL_MODEL_VERSION_PARAM = "ActiveSmallModelVersion" +MODEL_PROFILE_PARAMS = { + "big": (ACTIVE_BIG_MODEL_PARAM, ACTIVE_BIG_MODEL_NAME_PARAM, ACTIVE_BIG_MODEL_VERSION_PARAM), + "small": (ACTIVE_SMALL_MODEL_PARAM, ACTIVE_SMALL_MODEL_NAME_PARAM, ACTIVE_SMALL_MODEL_VERSION_PARAM), +} LOCAL_MODEL_PREFIX = "local-" LOCAL_MODEL_SERIES = "Local Series" ARTIFACT_URLS_CACHE = ".model_artifact_urls.json" @@ -113,6 +123,105 @@ def model_uses_external_gpu(model_key: str) -> bool: return bool(load_model_artifact_metadata(model_key).get("uses_external_gpu", False)) +def _params_text(params, key: str) -> str: + try: + value = params.get(key) + except Exception: + return "" + if value is None: + return "" + if isinstance(value, bytes): + return value.decode("utf-8", errors="ignore").strip() + return str(value).strip() + + +def _catalog_model_details(params, model_key: str) -> tuple[str, str]: + canonical_key = canonical_model_key(model_key) + models = [canonical_model_key(entry) for entry in _params_text(params, "AvailableModels").split(",")] + names = [entry.strip() for entry in _params_text(params, "AvailableModelNames").split(",")] + versions = [entry.strip() for entry in _params_text(params, "ModelVersions").split(",")] + try: + index = models.index(canonical_key) + except ValueError: + return "", "" + name = names[index] if index < len(names) else "" + version = versions[index] if index < len(versions) else "" + return name, version + + +def get_model_profile(params, profile: str) -> tuple[str, str, str]: + if profile not in MODEL_PROFILE_PARAMS: + raise ValueError(f"Unknown model profile: {profile}") + + key_param, name_param, version_param = MODEL_PROFILE_PARAMS[profile] + model_key = canonical_model_key(_params_text(params, key_param)) + requires_gpu = profile == "big" + if model_key and model_uses_external_gpu(model_key) != requires_gpu: + model_key = "" + + if not model_key: + legacy_key = canonical_model_key(_params_text(params, "DrivingModel") or _params_text(params, "Model")) + if legacy_key and model_uses_external_gpu(legacy_key) == requires_gpu: + model_key = legacy_key + + if not model_key and profile == "small": + model_key = DEFAULT_MODEL_KEY + if not model_key: + return "", "", "" + + stored_key = canonical_model_key(_params_text(params, key_param)) + model_name = _params_text(params, name_param) if stored_key == model_key else "" + model_version = _params_text(params, version_param) if stored_key == model_key else "" + if not model_name and canonical_model_key(_params_text(params, "DrivingModel") or _params_text(params, "Model")) == model_key: + model_name = _params_text(params, "DrivingModelName") + if not model_version and canonical_model_key(_params_text(params, "DrivingModel") or _params_text(params, "Model")) == model_key: + model_version = _params_text(params, "DrivingModelVersion") or _params_text(params, "ModelVersion") + + catalog_name, catalog_version = _catalog_model_details(params, model_key) + model_name = catalog_name or model_name + model_version = catalog_version or model_version + if is_builtin_model_key(model_key): + model_name = model_name or "Regret Driven Framework V4" + model_version = model_version or "v15" + return model_key, model_name, model_version + + +def set_model_profile(params, profile: str, model_key: str, model_name: str = "", model_version: str = "") -> None: + if profile not in MODEL_PROFILE_PARAMS: + raise ValueError(f"Unknown model profile: {profile}") + + canonical_key = canonical_model_key(model_key) + if not canonical_key: + raise ValueError("Model profile cannot be empty") + if model_uses_external_gpu(canonical_key) != (profile == "big"): + raise ValueError(f"Model {canonical_key} is not a {profile} model") + + catalog_name, catalog_version = _catalog_model_details(params, canonical_key) + key_param, name_param, version_param = MODEL_PROFILE_PARAMS[profile] + params.put(key_param, canonical_key) + params.put(name_param, model_name or catalog_name or canonical_key) + params.put(version_param, model_version or catalog_version or ("v15" if is_builtin_model_key(canonical_key) else "")) + + +def set_runtime_model_params(params, model_key: str, model_version: str = "") -> None: + canonical_key = canonical_model_key(model_key) or DEFAULT_MODEL_KEY + profile = "big" if model_uses_external_gpu(canonical_key) else "small" + profile_key, profile_name, profile_version = get_model_profile(params, profile) + catalog_name, catalog_version = _catalog_model_details(params, canonical_key) + model_name = profile_name if profile_key == canonical_key else catalog_name + resolved_version = model_version or (profile_version if profile_key == canonical_key else catalog_version) + if is_builtin_model_key(canonical_key): + model_name = model_name or "Regret Driven Framework V4" + resolved_version = resolved_version or "v15" + + params.put("Model", canonical_key) + params.put("DrivingModel", canonical_key) + params.put("DrivingModelName", model_name or canonical_key) + if resolved_version: + params.put("ModelVersion", resolved_version) + params.put("DrivingModelVersion", resolved_version) + + def model_accelerator_artifact_metadata(model_key: str, accelerator: str = MODEL_LAB_ACCELERATOR) -> dict: metadata = load_model_artifact_metadata(model_key) artifacts = metadata.get("accelerator_artifacts", {}) @@ -168,6 +277,7 @@ class ModelManager: self._load_catalog_from_params() self._ensure_model_params() + self._ensure_model_profiles() if boot_run: self._sync_selected_model_version() @@ -253,6 +363,12 @@ class ModelManager: self._set_model_param_keys(selected_model, selected_name, current_version) + def _ensure_model_profiles(self): + for profile in MODEL_PROFILE_PARAMS: + model_key, model_name, model_version = get_model_profile(self.params, profile) + if model_key: + set_model_profile(self.params, profile, model_key, model_name, model_version) + def _model_key_aliases(self, model_key: str) -> list[str]: return model_key_aliases(model_key) @@ -457,7 +573,7 @@ class ModelManager: return False return True - def _installed_model_choices(self) -> list[tuple[str, str, str]]: + def _installed_model_choices(self, profile: str = "") -> list[tuple[str, str, str]]: self._load_catalog_from_params() version_map = self._model_version_map() artifact_format_map = self._model_artifact_format_map() @@ -472,6 +588,8 @@ class ModelManager: canonical_key = self._canonical_model_key(model_key) if canonical_key in blacklisted_keys or canonical_key in seen_keys: continue + if profile and model_uses_external_gpu(canonical_key) != (profile == "big"): + continue if model_uses_external_gpu(canonical_key) and not external_gpu_available(): continue @@ -496,12 +614,14 @@ class ModelManager: print("Model Randomizer skipped while Model Laboratory is enabled.") return None - choices = self._installed_model_choices() + active_big_model, _, _ = get_model_profile(self.params, "big") + profile = "big" if external_gpu_available() and active_big_model else "small" + choices = self._installed_model_choices(profile) if not choices: print("Model Randomizer skipped: no installed, non-blacklisted models available.") return None - selected = self._selected_model() + selected, _, _ = get_model_profile(self.params, profile) eligible_choices = [choice for choice in choices if self._canonical_model_key(choice[0]) != selected] if not eligible_choices: eligible_choices = choices @@ -510,6 +630,7 @@ class ModelManager: if not model_version: model_version = self._default_param_text("ModelVersion") or self._default_param_text("DrivingModelVersion") or "v11" + set_model_profile(self.params, profile, model_key, model_name, model_version) self._set_model_param_keys(model_key, model_name, model_version) try: self.params_memory.put_bool("StarPilotTogglesUpdated", True) @@ -610,41 +731,33 @@ class ModelManager: return selected = self._selected_model() - if model_uses_external_gpu(selected) and not external_gpu_available(): - default_name = self._default_param_text("DrivingModelName") or "Regret Driven Framework V4" - default_version = self._default_param_text("ModelVersion") or self._default_param_text("DrivingModelVersion") or "v15" - self._set_model_param_keys(DEFAULT_MODEL_KEY, default_name, default_version) - print(f"Model {selected} requires an external GPU; selected built-in model instead.") - return - if is_builtin_model_key(selected): self._sync_selected_model_version() - return + else: + resolved_selected = self._resolve_manifest_model_key(selected) + if resolved_selected != selected: + selected_index = self.available_models.index(resolved_selected) + selected_name = self.available_model_names[selected_index] if selected_index < len(self.available_model_names) else resolved_selected + self._set_model_param_keys(resolved_selected, selected_name, None) + selected = resolved_selected - resolved_selected = self._resolve_manifest_model_key(selected) - if resolved_selected != selected: - selected_index = self.available_models.index(resolved_selected) - selected_name = self.available_model_names[selected_index] if selected_index < len(self.available_model_names) else resolved_selected - self._set_model_param_keys(resolved_selected, selected_name, None) - selected = resolved_selected + aliases = self._model_key_aliases(selected) + if any(alias in self.available_models for alias in aliases): + self._sync_selected_model_version() + else: + try: + default_model = self._default_param_text("Model") or self._default_param_text("DrivingModel") + except Exception: + default_model = DEFAULT_MODEL_KEY - aliases = self._model_key_aliases(selected) - if any(alias in self.available_models for alias in aliases): - self._sync_selected_model_version() - return + candidates = self._model_key_aliases(default_model) + self._model_key_aliases(DEFAULT_MODEL_KEY) + self.available_models + replacement = next((entry for entry in candidates if entry in self.available_models), self.available_models[0]) + replacement_index = self.available_models.index(replacement) + replacement_name = self.available_model_names[replacement_index] if replacement_index < len(self.available_model_names) else replacement + self._set_model_param_keys(replacement, replacement_name, None) + self._sync_selected_model_version() - try: - default_model = self._default_param_text("Model") or self._default_param_text("DrivingModel") - except Exception: - default_model = DEFAULT_MODEL_KEY - - candidates = self._model_key_aliases(default_model) + self._model_key_aliases(DEFAULT_MODEL_KEY) + self.available_models - replacement = next((entry for entry in candidates if entry in self.available_models), self.available_models[0]) - - replacement_index = self.available_models.index(replacement) - replacement_name = self.available_model_names[replacement_index] if replacement_index < len(self.available_model_names) else replacement - self._set_model_param_keys(replacement, replacement_name, None) - self._sync_selected_model_version() + self._ensure_model_profiles() def _discover_local_models(self) -> list[dict]: """Synthesize manifest entries for hand-installed models found in MODELS_PATH. @@ -729,6 +842,7 @@ class ModelManager: self._artifact_metadata_cache_path().write_text(json.dumps(self._build_artifact_metadata_map(model_info))) except Exception as error: print(f"Failed to write model versions cache: {error}") + self._ensure_model_profiles() def check_models(self, boot_run: bool): del boot_run # Not currently needed, retained for call-site parity. diff --git a/starpilot/assets/tests/test_model_pipeline.py b/starpilot/assets/tests/test_model_pipeline.py index bc57e798e..e3ba5a871 100644 --- a/starpilot/assets/tests/test_model_pipeline.py +++ b/starpilot/assets/tests/test_model_pipeline.py @@ -141,6 +141,77 @@ def test_external_gpu_requirement_is_cached_from_manifest(tmp_path, monkeypatch) assert not model_manager.model_uses_external_gpu("missing") +def test_active_small_and_big_profiles_migrate_from_legacy_selection(tmp_path, monkeypatch): + monkeypatch.setattr(model_manager, "MODELS_PATH", tmp_path) + (tmp_path / model_manager.ARTIFACT_METADATA_CACHE).write_text(json.dumps({ + "small-one": {"uses_external_gpu": False}, + "big-one": {"uses_external_gpu": True}, + })) + + class FakeParams: + def __init__(self, selected): + self.values = { + "Model": selected, + "DrivingModel": selected, + "DrivingModelName": selected.title(), + "DrivingModelVersion": "v16", + "AvailableModels": "rdf43,small-one,big-one", + "AvailableModelNames": "Regret Driven Framework V4,Small One,Big One", + "ModelVersions": "v15,v16,v16", + } + + def get(self, key): + return self.values.get(key) + + def put(self, key, value): + self.values[key] = value + + small_params = FakeParams("small-one") + assert model_manager.get_model_profile(small_params, "small") == ("small-one", "Small One", "v16") + assert model_manager.get_model_profile(small_params, "big") == ("", "", "") + + big_params = FakeParams("big-one") + assert model_manager.get_model_profile(big_params, "small") == ( + "rdf43", "Regret Driven Framework V4", "v15", + ) + assert model_manager.get_model_profile(big_params, "big") == ("big-one", "Big One", "v16") + + +def test_runtime_model_metadata_does_not_overwrite_model_profiles(tmp_path, monkeypatch): + monkeypatch.setattr(model_manager, "MODELS_PATH", tmp_path) + (tmp_path / model_manager.ARTIFACT_METADATA_CACHE).write_text(json.dumps({ + "small-one": {"uses_external_gpu": False}, + "big-one": {"uses_external_gpu": True}, + })) + + class FakeParams: + def __init__(self): + self.values = { + "ActiveSmallModel": "small-one", + "ActiveSmallModelName": "Small One", + "ActiveSmallModelVersion": "v15", + "ActiveBigModel": "big-one", + "ActiveBigModelName": "Big One", + "ActiveBigModelVersion": "v16", + } + + def get(self, key): + return self.values.get(key) + + def put(self, key, value): + self.values[key] = value + + params = FakeParams() + model_manager.set_runtime_model_params(params, "big-one", "v16") + assert params.values["DrivingModel"] == "big-one" + assert params.values["DrivingModelName"] == "Big One" + model_manager.set_runtime_model_params(params, "small-one", "v15") + assert params.values["DrivingModel"] == "small-one" + assert params.values["DrivingModelName"] == "Small One" + assert params.values["ActiveBigModel"] == "big-one" + assert params.values["ActiveSmallModel"] == "small-one" + + def test_manifest_metadata_classifies_model_lab_candidates_and_accelerator_artifacts(tmp_path, monkeypatch): monkeypatch.setattr(model_manager, "MODELS_PATH", tmp_path) manager = object.__new__(ModelManager) diff --git a/starpilot/car/ford/fordcan.py b/starpilot/car/ford/fordcan.py index 23a9653c5..cf2f5cc78 100644 --- a/starpilot/car/ford/fordcan.py +++ b/starpilot/car/ford/fordcan.py @@ -1,6 +1,6 @@ """Ford extended-lateral CAN constructors. -Adapted from BluePilot's ``fordcan_ext.py`` and angle-mode protocol at bp-7.0 commit +Adapted from BluePilot's ``fordcan_ext.py`` and extended-lateral protocol at bp-7.0 commit e1d051d7ba270261b4455068bd68f1a58db15a4a, including panda integration developed principally by Alan Polk. See CREDITS.md and THIRD_PARTY_NOTICES.md for detailed provenance and terms. """ @@ -8,32 +8,23 @@ Alan Polk. See CREDITS.md and THIRD_PARTY_NOTICES.md for detailed provenance and from opendbc.car.ford.fordcan import CanBus, calculate_lat_ctl2_checksum -SHADOW_CURVATURE_SCALE = 1e-6 - - -def create_lka_msg(packer, CAN: CanBus, angle_mode: bool = False, shadow_curvature: float = 0.0, - extended_mode: bool = True): +def create_lka_msg(packer, CAN: CanBus): addr, dat, bus = packer.make_can_msg("Lane_Assist_Data1", CAN.main, {}) dat = bytearray(dat) - - shadow_raw = int(round(shadow_curvature / SHADOW_CURVATURE_SCALE)) - shadow_raw = max(-32768, min(32767, shadow_raw)) & 0xFFFF - dat[4] |= int(angle_mode) | (int(extended_mode) << 1) - dat[5] = shadow_raw >> 8 - dat[6] = shadow_raw & 0xFF + dat[4] |= 0x2 return addr, bytes(dat), bus def create_lat_ctl_msg(packer, CAN: CanBus, active: bool, ramp_type: int, precision_type: int, - path_offset: float, path_angle: float, curvature: float, curvature_rate: float): + curvature: float, curvature_rate: float): values = { "LatCtlRng_L_Max": 0, "HandsOffCnfm_B_Rq": 0, "LatCtl_D_Rq": 1 if active else 0, "LatCtlRampType_D_Rq": ramp_type, "LatCtlPrecision_D_Rq": precision_type, - "LatCtlPathOffst_L_Actl": path_offset, - "LatCtlPath_An_Actl": path_angle, + "LatCtlPathOffst_L_Actl": 0.0, + "LatCtlPath_An_Actl": 0.0, "LatCtlCurv_NoRate_Actl": curvature_rate, "LatCtlCurv_No_Actl": curvature, } @@ -41,14 +32,13 @@ def create_lat_ctl_msg(packer, CAN: CanBus, active: bool, ramp_type: int, precis def create_lat_ctl2_msg(packer, CAN: CanBus, mode: int, ramp_type: int, precision_type: int, - path_offset: float, path_angle: float, curvature: float, - curvature_rate: float, counter: int): + curvature: float, curvature_rate: float, counter: int): values = { "LatCtl_D2_Rq": mode, "LatCtlRampType_D_Rq": ramp_type, "LatCtlPrecision_D_Rq": precision_type, - "LatCtlPathOffst_L_Actl": path_offset, - "LatCtlPath_An_Actl": path_angle, + "LatCtlPathOffst_L_Actl": 0.0, + "LatCtlPath_An_Actl": 0.0, "LatCtlCurv_No_Actl": curvature, "LatCtlCrv_NoRate2_Actl": curvature_rate, "HandsOffCnfm_B_Rq": 0, diff --git a/starpilot/car/ford/lateral.py b/starpilot/car/ford/lateral.py index 3bb91a978..42a36f7fd 100644 --- a/starpilot/car/ford/lateral.py +++ b/starpilot/car/ford/lateral.py @@ -1,11 +1,10 @@ """Ford lateral-control extensions. -The four-signal curvature strategy, path-angle-primary strategy, manual-turn detector, platform -gains, and related safety protocol are substantially adapted from BluePilot's Ford work, principally -by Alan Polk and additional contributors. The audited bp-7.0 reference is -e1d051d7ba270261b4455068bd68f1a58db15a4a; the missing original source SHA is reconstructed in -CREDITS.md. StarPilot reorganized that work for its own architecture and has since changed its -tuning, lookahead, driver-override handoff, and recovery behavior. +The extended curvature strategy, manual-turn detector, and related safety protocol are substantially +adapted from BluePilot's Ford work, principally by Alan Polk and additional contributors. The audited +bp-7.0 reference is e1d051d7ba270261b4455068bd68f1a58db15a4a; the missing original source SHA is +reconstructed in CREDITS.md. StarPilot reorganized that work for its own architecture and has since +changed its tuning and lookahead behavior. See CREDITS.md for feature-level authorship and upstream commits, and THIRD_PARTY_NOTICES.md for the published upstream license notices. Upstream contributors do not maintain this adaptation. @@ -13,7 +12,6 @@ published upstream license notices. Upstream contributors do not maintain this a from collections import deque from dataclasses import dataclass -from enum import IntEnum import numpy as np @@ -24,61 +22,30 @@ from openpilot.common.params import Params from openpilot.selfdrive.modeld.constants import ModelConstants -class FordLateralMode(IntEnum): - native = 0 - curvature = 1 - angle = 2 - - # These rate-limit values descend from BluePilot's ``values_ext.py``, which carries the Haibin Wen # and sunnypilot contributors copyright notice reproduced in THIRD_PARTY_NOTICES.md. -FORD_ANGLE_LIMITS = AngleSteeringLimits( +FORD_CURVATURE_LIMITS = AngleSteeringLimits( 0.02, ([5, 16, 25], [0.0025, 0.0012, 0.00008]), ([5, 16, 25], [0.0025, 0.0014, 0.00018]), ) MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL - ACCELERATION_DUE_TO_GRAVITY * 0.06 -PATH_ANGLE_MIN = -0.5 -PATH_ANGLE_MAX = 0.5235 STEER_DT = CarControllerParams.STEER_STEP * DT_CTRL CURVATURE_LOOKAHEAD_MIN = 0.20 CURVATURE_LOOKAHEAD_MAX = 0.40 FORD_CURVATURE_LOOKAHEAD = { CAR.FORD_EXPLORER_MK6: 0.20, } -ANGLE_HANDOFF_RECOVERY_SECONDS = 0.75 -HANDOFF_PAUSE_MIN_FRAMES = 3 -HANDOFF_PAUSE_FRAMES = 6 -HANDOFF_COOLDOWN_SECONDS = 2.0 -HANDOFF_MAX_PATH_ANGLE = 0.10 -LAT_CTL_STATUS_AVAILABLE = 1 -STALL_GAP_MIN = 2.0 * CarControllerParams.CURVATURE_ERROR -STALL_HOLD_SECONDS = 0.5 -STALL_MAX_RECOVERIES = 3 - -CANFD_BODY_ON_FRAME = frozenset({ - CAR.FORD_F_150_MK14, - CAR.FORD_F_150_LIGHTNING_MK1, - CAR.FORD_EXPEDITION_MK4, - CAR.FORD_RANGER_MK2, -}) -CANFD_UNIBODY = frozenset({ - CAR.FORD_MUSTANG_MACH_E_MK1, - CAR.FORD_ESCAPE_MK4_5, -}) @dataclass(frozen=True) class FordLateralResult: curvature: float = 0.0 curvature_rate: float = 0.0 - path_offset: float = 0.0 - path_angle: float = 0.0 ramp_type: int = 0 precision_type: int = 1 active: bool = False - shadow_curvature: float = 0.0 # Adapted from BluePilot HumanTurnDetector (Alan Polk, 97867c1eb57b7472f6fc3de62f0fef576e5a5497). @@ -115,7 +82,6 @@ class HumanTurnDetector: class FordLateralController: - """Ford polynomial lateral strategies kept outside the native car implementation.""" def __init__(self, CP): self.CP = CP @@ -128,52 +94,26 @@ class FordLateralController: self.sm = None self.model = None - self.mode = FordLateralMode.curvature self.hands_free_cluster_enabled = False self.human_turn_enabled = True self.curvature_blend_low = 0.4 self.curvature_blend_high = 0.4 - self.angle_blend = 0.5 self.curvature_lane_change_factor = 0.85 - self.angle_lane_change_factor = 1.0 - self.angle_low_speed_factor = 1.0 - self.angle_high_speed_factor = 1.0 - self.angle_high_speed_damping = 1.0 self.human_turn = HumanTurnDetector() self.curvature_samples = deque(maxlen=max(2, round(0.3 / STEER_DT))) - self.path_angle_last = 0.0 self.curvature_last = 0.0 - self.angle_pause_frames = 0 - self.angle_pause_cooldown = 0.0 - self.angle_handoff_recovery = 0.0 - self.angle_stall_timer = 0.0 - self.angle_stall_recoveries = 0 self._frame = 0 self._update_params() def _update_params(self): - try: - self.mode = FordLateralMode(int(np.clip(self.params.get_int("FordLateralMode", return_default=True), 0, 2))) - except ValueError: - self.mode = FordLateralMode.native - self.hands_free_cluster_enabled = bool( self.CP.flags & FordFlags.CANFD and self.params.get_bool("FordHandsFreeCluster")) self.human_turn_enabled = self.params.get_bool("FordHumanTurnDetection") self.curvature_blend_low = float(np.clip(self.params.get_float("FordCurvatureBlendLow", return_default=True), 0.0, 1.0)) self.curvature_blend_high = float(np.clip(self.params.get_float("FordCurvatureBlendHigh", return_default=True), 0.0, 1.0)) - self.angle_blend = float(np.clip(self.params.get_float("FordAngleBlend", return_default=True), 0.0, 1.0)) self.curvature_lane_change_factor = float(np.clip( self.params.get_float("FordCurvatureLaneChangeFactor", return_default=True), 0.5, 1.25)) - self.angle_lane_change_factor = float(np.clip( - self.params.get_float("FordAngleLaneChangeFactor", return_default=True), 0.5, 1.5)) - self.angle_low_speed_factor = float(np.clip( - self.params.get_float("FordAngleLowSpeedFactor", return_default=True), 0.5, 1.5)) - self.angle_high_speed_factor = float(np.clip( - self.params.get_float("FordAngleHighSpeedFactor", return_default=True), 0.5, 1.5)) - self.angle_high_speed_damping = float(np.clip( - self.params.get_float("FordAngleHighSpeedDamping", return_default=True), 0.25, 1.25)) def update_inputs(self): if self.sm is not None: @@ -211,19 +151,13 @@ class FordLateralController: def _current_curvature(CS) -> float: return -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1) - def _blend_and_scale(self, desired: float, predicted: float, v_ego: float, angle_mode: bool) -> tuple[float, int]: - if angle_mode: - blend = self.angle_blend - high_factor = self.angle_lane_change_factor - else: - blend = float(np.interp(abs(desired), [0.0, 0.001], [self.curvature_blend_low, self.curvature_blend_high])) - high_factor = self.curvature_lane_change_factor - + def _blend_and_scale(self, desired: float, predicted: float, v_ego: float) -> tuple[float, int]: + blend = float(np.interp(abs(desired), [0.0, 0.001], [self.curvature_blend_low, self.curvature_blend_high])) requested = predicted * blend + desired * (1.0 - blend) lane_change, direction = self._lane_change() precision = 1 if lane_change: - factor = float(np.interp(v_ego, [4.4, 40.23], [0.95, high_factor])) + factor = float(np.interp(v_ego, [4.4, 40.23], [0.95, self.curvature_lane_change_factor])) if (direction == 1 and requested < 0.0) or (direction == 2 and requested > 0.0): requested *= factor precision = 0 @@ -236,74 +170,28 @@ class FordLateralController: return self.human_turn.update( self.human_turn_enabled, CS.out.steeringPressed, CS.out.steeringAngleDeg) - def _reset_handoff(self): - self.angle_pause_frames = 0 - self.angle_pause_cooldown = 0.0 - self.angle_handoff_recovery = 0.0 - self.angle_stall_timer = 0.0 - self.angle_stall_recoveries = 0 - - def _update_angle_driver_override(self, steering_pressed: bool) -> bool: - if steering_pressed: - self.angle_handoff_recovery = ANGLE_HANDOFF_RECOVERY_SECONDS - return steering_pressed - - def _angle_stall_pause_active(self, CS) -> bool: - self.angle_pause_cooldown = max(0.0, self.angle_pause_cooldown - STEER_DT) - if self.angle_pause_frames > 0: - pause_frames_sent = HANDOFF_PAUSE_FRAMES - self.angle_pause_frames - pscm_available = getattr(CS, "lateral_control_status", None) == LAT_CTL_STATUS_AVAILABLE - # CAN-FD reports when the mode-0 reset has reached the PSCM. Keep a short minimum - # dwell, then resume immediately on that acknowledgement; retain the full pulse - # as a fallback for platforms without the status signal. - if pause_frames_sent >= HANDOFF_PAUSE_MIN_FRAMES and pscm_available: - self.angle_pause_frames = 0 - self.angle_pause_cooldown = HANDOFF_COOLDOWN_SECONDS - return False - - self.angle_pause_frames -= 1 - if self.angle_pause_frames == 0: - self.angle_pause_cooldown = HANDOFF_COOLDOWN_SECONDS - return True - return False - - def _recover_angle_handoff(self, requested: float, current: float) -> float: - if self.angle_handoff_recovery <= 0.0: - return requested - - authority = 1.0 - self.angle_handoff_recovery / ANGLE_HANDOFF_RECOVERY_SECONDS - recovered = current + float(np.clip(authority, 0.0, 1.0)) * (requested - current) - self.angle_handoff_recovery = max(0.0, self.angle_handoff_recovery - STEER_DT) - return recovered - - def _inactive_angle_result(self, current_curvature: float) -> FordLateralResult: - self.path_angle_last = 0.0 - return FordLateralResult(shadow_curvature=current_curvature) - - def update_curvature(self, CC, CS, actuators) -> FordLateralResult: + def update(self, CC, CS, actuators) -> FordLateralResult: current = self._current_curvature(CS) if not CC.latActive: self.human_turn.reset() - self._reset_handoff() self.curvature_samples.clear() self.curvature_last = 0.0 - return FordLateralResult(shadow_curvature=current) + return FordLateralResult() if self._manual_turn(CC, CS) or CS.out.vEgoRaw < 0.1: - self._reset_handoff() self.curvature_samples.clear() self.curvature_last = 0.0 - return FordLateralResult(active=True, shadow_curvature=current) + return FordLateralResult(active=True) v_ego = float(CS.out.vEgoRaw) predicted = self._predicted_curvature(v_ego, self._curvature_lookahead()) - requested, precision = self._blend_and_scale(float(actuators.curvature), predicted, v_ego, False) + requested, precision = self._blend_and_scale(float(actuators.curvature), predicted, v_ego) if v_ego > 9.0: requested = float(np.clip(requested, current - CarControllerParams.CURVATURE_ERROR, current + CarControllerParams.CURVATURE_ERROR)) applied = float(apply_std_steer_angle_limits( - requested, self.curvature_last, v_ego, CS.out.steeringAngleDeg, True, FORD_ANGLE_LIMITS)) + requested, self.curvature_last, v_ego, CS.out.steeringAngleDeg, True, FORD_CURVATURE_LIMITS)) if self.CP.flags & FordFlags.CANFD: max_curvature = MAX_LATERAL_ACCEL / max(v_ego, 1.0) ** 2 applied = float(np.clip(applied, -max_curvature, max_curvature)) @@ -327,85 +215,3 @@ class FordLateralController: precision_type=precision, active=True, ) - - # Platform grouping and baseline gains descend from Alan Polk's angle-primary implementation - # (d0aac605f99d37e9da205e419f7989c1e9eaa386); StarPilot applies its own runtime factors below. - def _platform_angle_gains(self) -> tuple[float, float]: - if self.CP.carFingerprint in CANFD_BODY_ON_FRAME: - return 0.95, 0.95 - if self.CP.carFingerprint in CANFD_UNIBODY: - return 1.0, 1.05 - return 1.0, 1.15 - - def update_angle(self, CC, CS, actuators) -> FordLateralResult: - current = self._current_curvature(CS) - if not CC.latActive: - self.human_turn.reset() - self._reset_handoff() - return self._inactive_angle_result(current) - - driver_override = self._update_angle_driver_override(bool(CS.out.steeringPressed)) - if self._angle_stall_pause_active(CS): - return self._inactive_angle_result(current) - - v_ego = float(CS.out.vEgoRaw) - live_delay = 0.12 if self.sm is None else float(np.clip(self.sm["liveDelay"].lateralDelay, 0.1, 0.15)) - speed_factor = float(np.interp(v_ego, [11.176, 24.587], [1.0, 0.0])) - curvature_factor = float(np.interp(abs(actuators.curvature), [0.005, 0.02], [1.0, 0.0])) - lookup_time = live_delay + 0.05 + 0.10 * speed_factor * curvature_factor - predicted = self._predicted_curvature(v_ego, lookup_time) - requested, precision = self._blend_and_scale(float(actuators.curvature), predicted, v_ego, True) - - requested_before_deviation_limit = requested - if v_ego > 9.0: - requested = float(np.clip(requested, current - CarControllerParams.CURVATURE_ERROR, - current + CarControllerParams.CURVATURE_ERROR)) - deviation_limited = abs(requested - requested_before_deviation_limit) > 1e-9 - - measured_curvature = float(getattr(CC, "currentCurvature", current)) - if not np.isfinite(measured_curvature): - measured_curvature = current - requested = measured_curvature if driver_override else self._recover_angle_handoff(requested, measured_curvature) - - low_gain_high_speed, high_gain_high_speed = self._platform_angle_gains() - low_gain = float(np.interp(v_ego, [13.5, 26.82], - [1.0, low_gain_high_speed * self.angle_high_speed_damping])) - high_gain = float(np.interp(v_ego, [13.5, 26.82], - [1.30 * self.angle_low_speed_factor, - high_gain_high_speed * self.angle_high_speed_factor])) - gain = float(np.interp(abs(requested), [0.0007, 0.001], [low_gain, high_gain])) - path_angle = float(np.clip(requested * v_ego * gain, PATH_ANGLE_MIN, PATH_ANGLE_MAX)) - - max_delta = float(np.interp(v_ego, [9.0, 10.0, 15.0, 25.0], [0.055, 0.055, 0.0425, 0.009])) - path_angle = float(np.clip(path_angle, self.path_angle_last - max_delta, self.path_angle_last + max_delta)) - self.path_angle_last = path_angle - - lane_change = self._lane_change()[0] - # The anti-stall concept and initial thresholds trace to John Christman's upstream work - # (9012f76666a5c90764fcaec40832b9a607488c27). This active-session recovery is StarPilot-specific. - stall_gap = float(actuators.curvature) - current - stalled = (self.human_turn_enabled and not CS.out.steeringPressed and not lane_change and v_ego > 9.0 - and abs(stall_gap) > STALL_GAP_MIN - and abs(float(actuators.curvature)) > abs(current)) - if stalled: - if deviation_limited and self.angle_pause_cooldown <= 0.0: - self.angle_stall_timer += STEER_DT - if (self.angle_stall_timer + 1e-9 >= STALL_HOLD_SECONDS - and self.angle_stall_recoveries < STALL_MAX_RECOVERIES - and abs(self.path_angle_last) < HANDOFF_MAX_PATH_ANGLE): - self.angle_pause_frames = HANDOFF_PAUSE_FRAMES - self.angle_stall_timer = 0.0 - self.angle_stall_recoveries += 1 - else: - self.angle_stall_timer = 0.0 - if CS.out.steeringPressed or abs(stall_gap) < 0.5 * STALL_GAP_MIN: - self.angle_stall_recoveries = 0 - - shadow = current if CS.out.steeringPressed else requested - return FordLateralResult( - path_angle=path_angle, - ramp_type=2, - precision_type=precision, - active=True, - shadow_curvature=shadow, - ) diff --git a/starpilot/car/ford/tests/test_lateral.py b/starpilot/car/ford/tests/test_lateral.py index 23108fe7f..8529d6e5d 100644 --- a/starpilot/car/ford/tests/test_lateral.py +++ b/starpilot/car/ford/tests/test_lateral.py @@ -3,8 +3,11 @@ from types import SimpleNamespace import pytest -from opendbc.car.ford.values import CAR -from ..lateral import ANGLE_HANDOFF_RECOVERY_SECONDS, HANDOFF_PAUSE_FRAMES, HANDOFF_PAUSE_MIN_FRAMES, STEER_DT, FordLateralController, HumanTurnDetector +from opendbc.can import CANPacker +from opendbc.car.ford.fordcan import CanBus +from opendbc.car.ford.values import CAR, FordFlags +from .. import fordcan +from ..lateral import FordLateralController, HumanTurnDetector class FakeSubMaster(dict): @@ -23,19 +26,19 @@ def controller(monkeypatch): CP = SimpleNamespace(flags=0, carFingerprint="FORD_EDGE_MK2") controller = FordLateralController(CP) controller.sm = FakeSubMaster(["modelV2", "liveDelay"]) + controller.curvature_blend_low = 0.4 + controller.curvature_blend_high = 0.4 + controller.curvature_lane_change_factor = 0.85 return controller -def car_state(speed=15.0, curvature=0.0, steering_pressed=False, steering_angle=0.0, lateral_control_status=None): - state = SimpleNamespace(out=SimpleNamespace( +def car_state(speed=15.0, curvature=0.0, steering_pressed=False, steering_angle=0.0): + return SimpleNamespace(out=SimpleNamespace( vEgoRaw=speed, yawRate=-curvature * speed, steeringPressed=steering_pressed, steeringAngleDeg=steering_angle, )) - if lateral_control_status is not None: - state.lateral_control_status = lateral_control_status - return state def test_human_turn_requires_sustained_input(): @@ -47,8 +50,30 @@ def test_human_turn_requires_sustained_input(): assert not detector.update(True, False, 50.0) +@pytest.mark.parametrize("canfd", (False, True)) +def test_extended_messages_are_curvature_only(canfd): + CP = SimpleNamespace(flags=FordFlags.CANFD if canfd else 0, safetyConfigs=[SimpleNamespace()]) + packer = CANPacker("ford_lincoln_base_pt") + can_bus = CanBus(CP) + + _, lka_data, _ = fordcan.create_lka_msg(packer, can_bus) + assert lka_data[4] & 0x3 == 0x2 + + if canfd: + _, lateral_data, _ = fordcan.create_lat_ctl2_msg(packer, can_bus, 1, 2, 1, 0.001, 0.0, 0) + raw_path_angle = ((lateral_data[3] & 0x1F) << 6) | (lateral_data[4] >> 2) + raw_path_offset = ((lateral_data[4] & 0x3) << 8) | lateral_data[5] + else: + _, lateral_data, _ = fordcan.create_lat_ctl_msg(packer, can_bus, True, 2, 1, 0.001, 0.0) + raw_path_angle = (lateral_data[3] << 3) | (lateral_data[4] >> 5) + raw_path_offset = (lateral_data[5] << 2) | (lateral_data[6] >> 6) + + assert raw_path_angle == 1000 + assert raw_path_offset == 512 + + def test_curvature_strategy_uses_polynomial_signals(controller): - result = controller.update_curvature( + result = controller.update( SimpleNamespace(latActive=True), car_state(), SimpleNamespace(curvature=0.001)) assert result.active assert 0.0 < result.curvature <= 0.001 @@ -83,8 +108,8 @@ def test_curvature_strategy_uses_learned_lookahead(controller, monkeypatch): monkeypatch.setattr(controller, "_predicted_curvature", lambda _v_ego, lookahead: lookaheads.append(lookahead) or 0.0) - controller.update_curvature(SimpleNamespace(latActive=True), car_state(), - SimpleNamespace(curvature=0.001)) + controller.update(SimpleNamespace(latActive=True), car_state(), + SimpleNamespace(curvature=0.001)) assert lookaheads == [pytest.approx(0.38)] @@ -97,34 +122,13 @@ def test_lane_change_accepts_capnp_enum_wrappers(controller): assert controller._lane_change() == (True, 1) -def test_angle_strategy_uses_path_angle_and_shadow(controller): - result = controller.update_angle( - SimpleNamespace(latActive=True), car_state(curvature=0.001), SimpleNamespace(curvature=0.001)) - assert result.active - assert result.curvature == 0.0 - assert result.path_angle > 0.0 - assert result.shadow_curvature == pytest.approx(0.0005) - - -def test_manual_turn_keeps_angle_session_active(controller): - controller.human_turn_enabled = True - measured_curvature = 0.004 - CC = SimpleNamespace(latActive=True, currentCurvature=measured_curvature) - CS = car_state(speed=8.0, curvature=measured_curvature, steering_pressed=True, steering_angle=50.0) - actuators = SimpleNamespace(curvature=-0.005) - for _ in range(61): - result = controller.update_angle(CC, CS, actuators) - assert result.active - assert result.path_angle == pytest.approx(measured_curvature * 8.0 * 1.3) - - def test_curvature_control_stays_active_during_driver_correction(controller): controller.human_turn_enabled = True CC = SimpleNamespace(latActive=True) actuators = SimpleNamespace(curvature=0.001) for _ in range(20): - result = controller.update_curvature( + result = controller.update( CC, car_state(steering_pressed=True, steering_angle=10.0), actuators) assert result.active @@ -134,118 +138,11 @@ def test_curvature_manual_turn_keeps_session_active_with_neutral_command(control CC = SimpleNamespace(latActive=True) actuators = SimpleNamespace(curvature=0.001) - controller.update_curvature( + controller.update( CC, car_state(steering_pressed=True, steering_angle=0.0), actuators) for _ in range(30): - result = controller.update_curvature( + result = controller.update( CC, car_state(steering_pressed=True, steering_angle=50.0), actuators) assert result.active assert result.curvature == 0.0 - assert result.path_angle == 0.0 - - -def test_angle_control_stays_active_after_sustained_driver_correction(controller): - controller.human_turn_enabled = True - measured_curvature = 0.001 - CC = SimpleNamespace(latActive=True, currentCurvature=measured_curvature) - actuators = SimpleNamespace(curvature=-0.001) - - for _ in range(20): - result = controller.update_angle( - CC, car_state(curvature=measured_curvature, steering_pressed=True, steering_angle=10.0), actuators) - assert result.active - assert result.path_angle > 0.0 - - for _ in range(HANDOFF_PAUSE_FRAMES): - assert controller.update_angle(CC, car_state(curvature=measured_curvature), actuators).active - - -def test_angle_driver_override_is_handoff_safe_with_human_turn_detection_disabled(controller): - controller.human_turn_enabled = False - measured_curvature = 0.002 - result = controller.update_angle( - SimpleNamespace(latActive=True, currentCurvature=measured_curvature), - car_state(curvature=measured_curvature, steering_pressed=True), - SimpleNamespace(curvature=-0.002), - ) - - assert result.active - assert result.path_angle > 0.0 - - -def test_short_driver_correction_does_not_pause_angle_control(controller): - controller.human_turn_enabled = True - CC = SimpleNamespace(latActive=True) - actuators = SimpleNamespace(curvature=0.001) - - for _ in range(9): - assert controller.update_angle( - CC, car_state(steering_pressed=True, steering_angle=10.0), actuators).active - - assert controller.update_angle(CC, car_state(), actuators).active - - -def test_angle_driver_handoff_does_not_depend_on_pscm_mode_reset(controller): - controller.human_turn_enabled = True - measured_curvature = 0.001 - CC = SimpleNamespace(latActive=True, currentCurvature=measured_curvature) - actuators = SimpleNamespace(curvature=-0.001) - - for _ in range(10): - assert controller.update_angle( - CC, car_state(curvature=measured_curvature, steering_pressed=True, steering_angle=10.0), actuators).active - - for _ in range(HANDOFF_PAUSE_MIN_FRAMES): - assert controller.update_angle( - CC, car_state(lateral_control_status=1), actuators).active - - -def test_long_manual_turn_hands_angle_control_back_without_disabling(controller): - controller.human_turn_enabled = True - measured_curvature = 0.004 - CC = SimpleNamespace(latActive=True, currentCurvature=measured_curvature) - actuators = SimpleNamespace(curvature=-0.005) - - for _ in range(40): - assert controller.update_angle( - CC, car_state(speed=8.0, curvature=measured_curvature, steering_pressed=True, steering_angle=50.0), actuators).active - - for _ in range(HANDOFF_PAUSE_FRAMES): - assert controller.update_angle( - CC, car_state(speed=8.0, curvature=measured_curvature), actuators).active - - -def test_angle_handoff_reenters_from_measured_curvature(controller): - controller.human_turn_enabled = True - controller.angle_blend = 0.0 - measured_curvature = 0.004 - CC = SimpleNamespace(latActive=True, currentCurvature=measured_curvature) - actuators = SimpleNamespace(curvature=-0.005) - - for _ in range(10): - controller.update_angle( - CC, car_state(speed=8.0, curvature=measured_curvature, steering_pressed=True, steering_angle=10.0), actuators) - - resumed = controller.update_angle(CC, car_state(speed=8.0, curvature=measured_curvature), actuators) - assert resumed.active - assert resumed.path_angle == pytest.approx(measured_curvature * 8.0 * 1.3) - - recovery_frames = round(ANGLE_HANDOFF_RECOVERY_SECONDS / STEER_DT) - for _ in range(recovery_frames + 2): - recovered = controller.update_angle(CC, car_state(speed=8.0, curvature=measured_curvature), actuators) - assert recovered.path_angle < 0.0 - - -def test_angle_control_recovers_from_bounded_tracking_stall(controller): - controller.human_turn_enabled = True - controller.angle_blend = 0.0 - CC = SimpleNamespace(latActive=True) - CS = car_state(speed=15.0, curvature=0.0) - actuators = SimpleNamespace(curvature=0.01) - - for _ in range(10): - assert controller.update_angle(CC, CS, actuators).active - - for _ in range(HANDOFF_PAUSE_FRAMES): - assert not controller.update_angle(CC, CS, actuators).active diff --git a/starpilot/common/assets/device_settings_layout.json b/starpilot/common/assets/device_settings_layout.json index ee2bf452e..7c248e9dc 100644 --- a/starpilot/common/assets/device_settings_layout.json +++ b/starpilot/common/assets/device_settings_layout.json @@ -364,46 +364,12 @@ "parent_key": "QOLLateral", "settings_tier": "simple" }, - { - "key": "FordLateralMode", - "label": "Ford Steering Strategy", - "description": "Choose the Ford lateral controller. Curvature is the tuned default, Angle uses path-angle control, and Native preserves the original Ford controls.", - "picker_description": "Chooses Native, Curvature, or Angle steering on Ford vehicles.", - "data_type": "int", - "ui_type": "dropdown", - "options": [ - { - "value": 0, - "label": "Native" - }, - { - "value": 1, - "label": "Curvature" - }, - { - "value": 2, - "label": "Angle" - } - ], - "is_parent_toggle": true, - "galaxy_only": true, - "vehicle_makes": [ - "Ford" - ], - "settings_tier": "simple" - }, { "key": "FordHumanTurnDetection", "label": "Manual Turn Release", "description": "Yield during an intentional manual turn while keeping the Ford steering session ready.", "data_type": "bool", "ui_type": "toggle", - "parent_key": "FordLateralMode", - "visible_when_key": "FordLateralMode", - "visible_when_values": [ - 1, - 2 - ], "galaxy_only": true, "vehicle_makes": [ "Ford" @@ -416,12 +382,6 @@ "description": "Show the hands-free assistance graphic on supported CAN-FD Ford clusters while lateral control is active. Driver monitoring requirements do not change.", "data_type": "bool", "ui_type": "toggle", - "parent_key": "FordLateralMode", - "visible_when_key": "FordLateralMode", - "visible_when_values": [ - 1, - 2 - ], "galaxy_only": true, "vehicle_makes": [ "Ford" @@ -438,11 +398,6 @@ "max": 1.0, "step": 0.05, "precision": 2, - "parent_key": "FordLateralMode", - "visible_when_key": "FordLateralMode", - "visible_when_values": [ - 1 - ], "galaxy_only": true, "vehicle_makes": [ "Ford" @@ -459,11 +414,6 @@ "max": 1.0, "step": 0.05, "precision": 2, - "parent_key": "FordLateralMode", - "visible_when_key": "FordLateralMode", - "visible_when_values": [ - 1 - ], "galaxy_only": true, "vehicle_makes": [ "Ford" @@ -473,123 +423,13 @@ { "key": "FordCurvatureLaneChangeFactor", "label": "Curvature Lane-Change Factor", - "description": "Scale steering during high-speed lane changes in Curvature mode.", + "description": "Scale steering during high-speed lane changes.", "data_type": "float", "ui_type": "numeric", "min": 0.5, "max": 1.25, "step": 0.05, "precision": 2, - "parent_key": "FordLateralMode", - "visible_when_key": "FordLateralMode", - "visible_when_values": [ - 1 - ], - "galaxy_only": true, - "vehicle_makes": [ - "Ford" - ], - "settings_tier": "simple" - }, - { - "key": "FordAngleBlend", - "label": "Angle Prediction Blend", - "description": "Blend model prediction into the Ford path-angle command. 0 uses planner curvature only; 1 uses model prediction only.", - "data_type": "float", - "ui_type": "numeric", - "min": 0.0, - "max": 1.0, - "step": 0.01, - "precision": 2, - "parent_key": "FordLateralMode", - "visible_when_key": "FordLateralMode", - "visible_when_values": [ - 2 - ], - "galaxy_only": true, - "vehicle_makes": [ - "Ford" - ], - "settings_tier": "simple" - }, - { - "key": "FordAngleLowSpeedFactor", - "label": "Low-Speed Angle Response", - "description": "Adjust path-angle strength at lower speeds and higher curvature.", - "data_type": "float", - "ui_type": "numeric", - "min": 0.5, - "max": 1.5, - "step": 0.01, - "precision": 2, - "parent_key": "FordLateralMode", - "visible_when_key": "FordLateralMode", - "visible_when_values": [ - 2 - ], - "galaxy_only": true, - "vehicle_makes": [ - "Ford" - ], - "settings_tier": "simple" - }, - { - "key": "FordAngleHighSpeedFactor", - "label": "High-Speed Angle Response", - "description": "Adjust path-angle strength through larger highway curves.", - "data_type": "float", - "ui_type": "numeric", - "min": 0.5, - "max": 1.5, - "step": 0.01, - "precision": 2, - "parent_key": "FordLateralMode", - "visible_when_key": "FordLateralMode", - "visible_when_values": [ - 2 - ], - "galaxy_only": true, - "vehicle_makes": [ - "Ford" - ], - "settings_tier": "simple" - }, - { - "key": "FordAngleHighSpeedDamping", - "label": "High-Speed Angle Damping", - "description": "Dampen small steering corrections at highway speed.", - "data_type": "float", - "ui_type": "numeric", - "min": 0.25, - "max": 1.25, - "step": 0.01, - "precision": 2, - "parent_key": "FordLateralMode", - "visible_when_key": "FordLateralMode", - "visible_when_values": [ - 2 - ], - "galaxy_only": true, - "vehicle_makes": [ - "Ford" - ], - "settings_tier": "simple" - }, - { - "key": "FordAngleLaneChangeFactor", - "label": "Angle Lane-Change Factor", - "description": "Scale steering during high-speed lane changes in Angle mode.", - "data_type": "float", - "ui_type": "numeric", - "min": 0.5, - "max": 1.5, - "step": 0.01, - "precision": 2, - "parent_key": "FordLateralMode", - "visible_when_key": "FordLateralMode", - "visible_when_values": [ - 2 - ], "galaxy_only": true, "vehicle_makes": [ "Ford" @@ -3709,8 +3549,8 @@ { "key": "ToyotaAutoHold", "label": "Toyota Auto Hold", - "description": "Hold the brakes at a stop on supported Toyota/Lexus TSS2 vehicles when cruise main is available and cruise is not active.", - "picker_description": "Holds Toyota/Lexus brakes at stops when cruise is available.", + "description": "Hold the brakes at a stop on supported Toyota/Lexus vehicles when cruise main is available and cruise is not active.", + "picker_description": "Holds supported Toyota/Lexus brakes at stops when cruise is available.", "data_type": "bool", "ui_type": "toggle", "galaxy_only": true, diff --git a/starpilot/common/favorite_slots.py b/starpilot/common/favorite_slots.py index eb646fefb..da3da1207 100644 --- a/starpilot/common/favorite_slots.py +++ b/starpilot/common/favorite_slots.py @@ -48,6 +48,12 @@ SETTINGS_CATALOG_PATH = Path(__file__).resolve().parent / "assets" / "device_set BLOCKED_ONROAD_KEYS = { + "ActiveBigModel", + "ActiveBigModelName", + "ActiveBigModelVersion", + "ActiveSmallModel", + "ActiveSmallModelName", + "ActiveSmallModelVersion", "AlphaLongitudinalEnabled", "DrivingModel", "Model", diff --git a/starpilot/common/safe_mode.py b/starpilot/common/safe_mode.py index a6fbb1f7c..a4e170575 100644 --- a/starpilot/common/safe_mode.py +++ b/starpilot/common/safe_mode.py @@ -22,6 +22,12 @@ SAFE_MODE_MANAGED_KEYS = ( "DrivingModelName", "ModelVersion", "DrivingModelVersion", + "ActiveBigModel", + "ActiveBigModelName", + "ActiveBigModelVersion", + "ActiveSmallModel", + "ActiveSmallModelName", + "ActiveSmallModelVersion", "ModelLabConfig", "ModelRandomizer", "LatSmoothSeconds", diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index ac7927901..a3bfeb924 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -635,14 +635,6 @@ class StarPilotVariables: alpha_longitudinal = CP.alphaLongitudinalAvailable toggle.car_make = CP.brand - toggle.ford_lateral_mode = self.get_value( - "FordLateralMode", - cast=int, - condition=toggle.car_make == "ford", - default=1, - min=0, - max=2, - ) migrate_ford_lkas_button_default(toggle.car_make, self.params) toggle.car_model = CP.carFingerprint toggle.disable_openpilot_long = self.get_value("DisableOpenpilotLongitudinal", condition=not alpha_longitudinal) diff --git a/starpilot/common/tests/test_favorite_slots.py b/starpilot/common/tests/test_favorite_slots.py index 66f143116..e4017f6c9 100644 --- a/starpilot/common/tests/test_favorite_slots.py +++ b/starpilot/common/tests/test_favorite_slots.py @@ -91,7 +91,6 @@ def test_shared_settings_catalog_is_common_and_well_formed(): def test_galaxy_only_ford_controls_are_not_available_to_device_favorites(): ford_keys = { - "FordLateralMode", "FordHumanTurnDetection", "FordHandsFreeCluster", } diff --git a/starpilot/controls/lib/starpilot_acceleration.py b/starpilot/controls/lib/starpilot_acceleration.py index d8079fe19..cf516de93 100644 --- a/starpilot/controls/lib/starpilot_acceleration.py +++ b/starpilot/controls/lib/starpilot_acceleration.py @@ -264,7 +264,6 @@ class StarPilotAcceleration: deceleration_profile = normalize_deceleration_profile( getattr(starpilot_toggles, "deceleration_profile", DECELERATION_PROFILES["STANDARD"]) ) - if sm["starpilotCarState"].trafficModeEnabled: self.max_accel = get_max_accel_traffic(v_ego) elif custom_accel_profile: diff --git a/starpilot/system/the_galaxy/assets/components/router.js b/starpilot/system/the_galaxy/assets/components/router.js index e48f2797f..8d69bc1de 100644 --- a/starpilot/system/the_galaxy/assets/components/router.js +++ b/starpilot/system/the_galaxy/assets/components/router.js @@ -20,7 +20,7 @@ import { ScreenRecordings } from "/assets/components/recordings/screen_recording import { Sidebar } from "/assets/components/sidebar.js?v=controllers-nav-1" import { SentryMode } from "/assets/components/tools/sentry.js" import { SpeedLimits } from "/assets/components/tools/speed_limits.js" -import { ModelManager } from "/assets/components/tools/model_manager.js?v=20260825a" +import { ModelManager } from "/assets/components/tools/model_manager.js?v=20260906a" import { ModelLaboratory } from "/assets/components/tools/model_laboratory.js?v=model-lab-5" import { LivePlots } from "/assets/components/tools/plots.js" import { ThemeMaker } from "/assets/components/tools/theme_maker.js" diff --git a/starpilot/system/the_galaxy/assets/components/tools/model_manager.js b/starpilot/system/the_galaxy/assets/components/tools/model_manager.js index e055f8fbc..cd85704f6 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/model_manager.js +++ b/starpilot/system/the_galaxy/assets/components/tools/model_manager.js @@ -11,6 +11,8 @@ const state = reactive({ allowGpuDownloadsWithoutGpu: false, models: [], currentModel: "", + activeSmallModel: "", + activeBigModel: "", summary: { installed: 0, missing: 0, total: 0 }, status: { modelToDownload: "", @@ -158,9 +160,10 @@ function getReleaseOrderedModels() { return getFilteredModels().sort(modelSortCompare); } -function getInstalledModels() { +function getInstalledModels(profile = "") { return state.models .filter(model => model && typeof model === "object" && !!model.installed) + .filter(model => !profile || (!!model.requiresGpu === (profile === "big"))) .sort(modelSortCompare); } @@ -180,6 +183,13 @@ function getCurrentModelName() { return safeText(match.label, current); } +function getModelName(modelKey, fallback = "none selected") { + const key = safeText(modelKey, ""); + if (!key) return fallback; + const match = state.models.find(model => safeText(model?.value, "") === key); + return match ? safeText(match.label, key) : key; +} + async function fetchJson(url, options = {}) { const controller = new AbortController(); const timer = setTimeout(() => controller.abort(), REQUEST_TIMEOUT_MS); @@ -218,6 +228,8 @@ async function fetchStatus() { state.models = models; state.currentModel = safeText(payload.currentModel, ""); + state.activeSmallModel = safeText(payload.activeSmallModel, ""); + state.activeBigModel = safeText(payload.activeBigModel, ""); const summary = payload.summary && typeof payload.summary === "object" ? payload.summary : {}; state.summary = { @@ -241,6 +253,8 @@ async function fetchStatus() { const signature = [ state.models.length, state.currentModel, + state.activeSmallModel, + state.activeBigModel, state.status.downloading, state.status.downloadAll, state.status.modelToDownload, @@ -300,11 +314,13 @@ function ensurePolling() { pollingHandle = setTimeout(poll, ACTIVE_POLL_INTERVAL_MS); } -async function setActiveModel(modelKey) { - const payload = await fetchJson("/api/params", { +async function setActiveModel(modelKey, profile = "") { + const model = state.models.find(entry => safeText(entry?.value, "") === safeText(modelKey, "")); + const resolvedProfile = profile || (model?.requiresGpu ? "big" : "small"); + const payload = await fetchJson("/api/models/active", { method: "PUT", headers: { "Content-Type": "application/json" }, - body: JSON.stringify({ key: "Model", value: modelKey }), + body: JSON.stringify({ profile: resolvedProfile, model: modelKey }), }); notify(payload.message || `Selected "${modelKey}".`); @@ -394,9 +410,10 @@ async function runAction(action, modelKey = "") { return; } - if (action === "select") { + if (action === "select" || action === "select-small" || action === "select-big") { if (!modelKey) return; - await setActiveModel(modelKey); + const profile = action === "select-small" ? "small" : action === "select-big" ? "big" : ""; + await setActiveModel(modelKey, profile); } else if (action === "download") { if (!modelKey) return; await startDownload(modelKey); @@ -454,10 +471,11 @@ function bindDomHandlers() { } if (!(target instanceof HTMLSelectElement)) return; - if (target.id === "mm-active-model-select") { + if (target.id === "mm-active-small-model-select" || target.id === "mm-active-big-model-select") { const modelKey = safeText(target.value, ""); if (!modelKey) return; - runAction("select", modelKey).catch(() => {}); + const profile = target.id === "mm-active-big-model-select" ? "big" : "small"; + runAction(`select-${profile}`, modelKey).catch(() => {}); return; } @@ -498,9 +516,11 @@ function bindDomHandlers() { function renderActions(model) { const modelKey = safeText(model.value, ""); const modelIsDownloading = state.status.downloading && !state.status.downloadAll && state.status.modelToDownload === modelKey; + const profile = model.requiresGpu ? "big" : "small"; + const isActive = profile === "big" ? state.activeBigModel === modelKey : state.activeSmallModel === modelKey; - if (state.currentModel === modelKey) { - return html`Active`; + if (isActive) { + return html`Active ${profile === "big" ? "Big" : "Small"}`; } if (state.status.downloading) { @@ -512,7 +532,7 @@ function renderActions(model) { if (model.installed) { return html` - + ${model.builtin ? "" : html``} @@ -620,30 +640,55 @@ export function ModelManager() {
- Current: ${getCurrentModelName()} + Loaded: ${() => getCurrentModelName()} + Active Small: ${() => getModelName(state.activeSmallModel)} + Active Big: ${() => getModelName(state.activeBigModel)} Progress: ${safeText(state.status.progress, "Idle")} ${() => getUserFavoriteModels(false).length} personal favorites ${() => state.status.isOnroad ? html`Onroad: actions disabled` : ""}
- - + ${() => { + const orderedInstalled = getInstalledModels("small").sort((a, b) => { + const aCurrent = safeText(a.value) === state.activeSmallModel ? 0 : 1; + const bCurrent = safeText(b.value) === state.activeSmallModel ? 0 : 1; if (aCurrent !== bCurrent) return aCurrent - bCurrent; return safeText(a.label, a.value).localeCompare(safeText(b.label, b.value), undefined, { sensitivity: "base" }); }); return orderedInstalled.length > 0 ? orderedInstalled.map(model => html` - `) : html``; - })()} + }} + + + + diff --git a/starpilot/system/the_galaxy/tests/test_dashboard_stats.py b/starpilot/system/the_galaxy/tests/test_dashboard_stats.py index 09581c4ed..2d16a3d64 100644 --- a/starpilot/system/the_galaxy/tests/test_dashboard_stats.py +++ b/starpilot/system/the_galaxy/tests/test_dashboard_stats.py @@ -44,10 +44,25 @@ model_manager = ModuleType("openpilot.starpilot.assets.model_manager") model_manager.MODEL_LAB_DOWNLOAD_PARAM = "ModelLabModelToDownload" model_manager.canonical_model_key = lambda value: str(value or "").strip().lower().replace(" ", "-") model_manager.external_gpu_available = lambda: False +def _stub_get_model_profile(params, profile): + prefix = "ActiveBigModel" if profile == "big" else "ActiveSmallModel" + key = params.get(prefix) or ("rdf43" if profile == "small" else "") + return key, params.get(f"{prefix}Name") or key, params.get(f"{prefix}Version") or "" + + +def _stub_set_model_profile(params, profile, key, name="", version=""): + prefix = "ActiveBigModel" if profile == "big" else "ActiveSmallModel" + params.put(prefix, key) + params.put(f"{prefix}Name", name or key) + params.put(f"{prefix}Version", version) + + +model_manager.get_model_profile = _stub_get_model_profile model_manager.is_builtin_model_key = lambda key: False model_manager.model_accelerator_artifact_filename = lambda key: f"{key}_driving_chestnut_tinygrad.pkl" model_manager.model_key_aliases = lambda key: () model_manager.model_uses_external_gpu = lambda key: False +model_manager.set_model_profile = _stub_set_model_profile sys.modules.setdefault("openpilot.starpilot.assets.model_manager", model_manager) starpilot_variables = ModuleType("openpilot.starpilot.common.starpilot_variables") @@ -1780,6 +1795,70 @@ def _load_server_module(): return module +def test_model_profiles_can_be_selected_without_external_gpu(monkeypatch, tmp_path): + server = _load_server_module() + assert server._import_galaxy_web_symbols() + + class ModelParams(FakeParams): + defaults = { + "Model": "rdf43", + "DrivingModel": "rdf43", + "DrivingModelName": "Regret Driven Framework V4", + "ModelVersion": "v15", + "DrivingModelVersion": "v15", + } + + def get_default_value(self, key): + return self.defaults.get(key) + + params = ModelParams({ + "AvailableModels": "rdf43,small-one,big-one", + "AvailableModelNames": "Regret Driven Framework V4,Small One,Big One", + "AvailableModelSeries": "Built-in,Small,Large", + "AvailableModelArtifactFormats": "tinygrad_single_v1,tinygrad_single_v1,tinygrad_single_v1", + "ModelVersions": "v15,v15,v16", + "ModelReleasedDates": "2026-01-01,2026-01-02,2026-01-03", + "Model": "rdf43", + "DrivingModel": "rdf43", + }) + (tmp_path / "small-one_driving_tinygrad.pkl").write_bytes(b"small") + (tmp_path / "big-one_driving_tinygrad.pkl").write_bytes(b"big") + + app = server.Flask( + "model_profiles_test", + template_folder=str(MODULE_DIR / "templates"), + static_folder=str(MODULE_DIR / "assets"), + ) + server.setup(app) + monkeypatch.setattr(server, "params", params) + monkeypatch.setattr(server, "params_memory", FakeParams()) + monkeypatch.setattr(server, "MODELS_PATH", tmp_path) + monkeypatch.setattr(server, "external_gpu_available", lambda: False) + monkeypatch.setattr(server, "is_builtin_model_key", lambda key: key == "rdf43") + monkeypatch.setattr(server, "model_uses_external_gpu", lambda key: key == "big-one") + client = app.test_client() + + big_response = client.put("/api/models/active", json={"profile": "big", "model": "big-one"}) + assert big_response.status_code == 200 + assert params.values["ActiveBigModel"] == "big-one" + assert params.values["Model"] == "rdf43" + + small_response = client.put("/api/models/active", json={"profile": "small", "model": "small-one"}) + assert small_response.status_code == 200 + assert params.values["ActiveSmallModel"] == "small-one" + + status = client.get("/api/models/status").get_json() + assert status["activeBigModel"] == "big-one" + assert status["activeSmallModel"] == "small-one" + + wrong_profile = client.put("/api/models/active", json={"profile": "small", "model": "big-one"}) + assert wrong_profile.status_code == 409 + + params.put("IsOnroad", True) + onroad = client.put("/api/models/active", json={"profile": "small", "model": "rdf43"}) + assert onroad.status_code == 403 + + def test_model_laboratory_api_uses_installed_models_and_enforces_hardware_size_version_guards(monkeypatch, tmp_path): server = _load_server_module() assert server._import_galaxy_web_symbols() diff --git a/starpilot/system/the_galaxy/tests/test_device_settings_layout.py b/starpilot/system/the_galaxy/tests/test_device_settings_layout.py index 5ec2fd1ad..e97f58066 100644 --- a/starpilot/system/the_galaxy/tests/test_device_settings_layout.py +++ b/starpilot/system/the_galaxy/tests/test_device_settings_layout.py @@ -69,12 +69,14 @@ def test_galaxy_layout_contains_basic_mode_controls(): def test_ford_lateral_controls_are_ford_only_and_galaxy_only(): lateral = _params_by_section(_layout())["Lateral (Steering)"] ford_keys = { - "FordLateralMode", "FordHumanTurnDetection", "FordHandsFreeCluster", "FordCurvatureBlendLow", "FordCurvatureBlendHigh", "FordCurvatureLaneChangeFactor", + } + retired_ford_keys = { + "FordLateralMode", "FordAngleBlend", "FordAngleLowSpeedFactor", "FordAngleHighSpeedFactor", @@ -83,34 +85,12 @@ def test_ford_lateral_controls_are_ford_only_and_galaxy_only(): } assert ford_keys <= lateral.keys() + assert retired_ford_keys.isdisjoint(lateral) assert all(lateral[key]["galaxy_only"] is True for key in ford_keys) assert all(lateral[key]["vehicle_makes"] == ["Ford"] for key in ford_keys) assert all(lateral[key]["settings_tier"] == "simple" for key in ford_keys) - - mode = lateral["FordLateralMode"] - assert mode["ui_type"] == "dropdown" - assert mode["data_type"] == "int" - assert mode["is_parent_toggle"] is True - assert {option["label"]: option["value"] for option in mode["options"]} == { - "Native": 0, - "Curvature": 1, - "Angle": 2, - } - assert _declared_default("FordLateralMode") == "1" - - common_keys = {"FordHumanTurnDetection", "FordHandsFreeCluster"} - curvature_keys = {"FordCurvatureBlendLow", "FordCurvatureBlendHigh", "FordCurvatureLaneChangeFactor"} - angle_keys = { - "FordAngleBlend", - "FordAngleLowSpeedFactor", - "FordAngleHighSpeedFactor", - "FordAngleHighSpeedDamping", - "FordAngleLaneChangeFactor", - } - assert all(lateral[key]["visible_when_values"] == [1, 2] for key in common_keys) - assert all(lateral[key]["visible_when_values"] == [1] for key in curvature_keys) - assert all(lateral[key]["visible_when_values"] == [2] for key in angle_keys) - assert all(lateral[key]["parent_key"] == "FordLateralMode" for key in ford_keys - {"FordLateralMode"}) + assert all("visible_when_key" not in lateral[key] for key in ford_keys) + assert all("parent_key" not in lateral[key] for key in ford_keys) device_ui_root = REPO_ROOT / "selfdrive/ui" for path in device_ui_root.rglob("*.py"): @@ -289,22 +269,6 @@ def test_honda_pid_scale_controls_use_galaxy_fine_granularity(): assert setting["settings_tier"] == "advanced" -def test_ford_angle_controls_use_galaxy_fine_granularity(): - lateral = _params_by_section(_layout())["Lateral (Steering)"] - - for key in ( - "FordAngleBlend", - "FordAngleLowSpeedFactor", - "FordAngleHighSpeedFactor", - "FordAngleHighSpeedDamping", - "FordAngleLaneChangeFactor", - ): - setting = lateral[key] - assert setting["step"] == 0.01 - assert setting["precision"] == 2 - assert setting["galaxy_only"] - - def test_hidden_feature_defaults_remain_enabled(): assert _declared_default("GalaxyDeveloperMode") == "0" assert _declared_default("NavDesiresAllowed") == "1" diff --git a/starpilot/system/the_galaxy/tests/test_navigation_params.py b/starpilot/system/the_galaxy/tests/test_navigation_params.py index 83b57fadd..eac21e62e 100644 --- a/starpilot/system/the_galaxy/tests/test_navigation_params.py +++ b/starpilot/system/the_galaxy/tests/test_navigation_params.py @@ -96,11 +96,10 @@ def _params_client(monkeypatch, values, device_type): the_galaxy, "_get_param_type_info", lambda: ( - {"AlphaLongitudinalEnabled", "ForceOffroad", "FordLateralMode"}, + {"AlphaLongitudinalEnabled", "ForceOffroad"}, { "AlphaLongitudinalEnabled": bool, "ForceOffroad": bool, - "FordLateralMode": int, }, ), ) @@ -558,19 +557,6 @@ def test_params_all_exposes_curve_calibration_readouts(monkeypatch): assert response.get_json()["CalibrationProgress"] == 48.0 -def test_ford_lateral_mode_is_editable_through_galaxy(monkeypatch): - client, fake_params = _params_client(monkeypatch, { - "CarMake": "Ford", - "FordLateralMode": 1, - }, "mici") - - response = client.put("/api/params", json={"key": "FordLateralMode", "value": 2, "label": "Angle"}) - - assert response.status_code == 200 - assert fake_params.values["FordLateralMode"] == "2" - assert ("FordLateralMode", "2") in fake_params.writes - - def test_custom_accel_breakpoint_update_validates_the_complete_curve(monkeypatch): point_count_key = the_galaxy.CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY breakpoint_keys = the_galaxy.CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS diff --git a/starpilot/system/the_galaxy/the_galaxy.py b/starpilot/system/the_galaxy/the_galaxy.py index 2c2a8e9bc..66d52fb39 100644 --- a/starpilot/system/the_galaxy/the_galaxy.py +++ b/starpilot/system/the_galaxy/the_galaxy.py @@ -56,10 +56,12 @@ from openpilot.starpilot.assets.model_manager import ( MODEL_LAB_DOWNLOAD_PARAM, canonical_model_key, external_gpu_available, + get_model_profile, is_builtin_model_key, model_accelerator_artifact_filename, model_key_aliases, model_uses_external_gpu, + set_model_profile, ) from openpilot.starpilot.common.model_lab import ( MODEL_LAB_CONFIG_PARAM, @@ -6066,6 +6068,9 @@ def setup(app): break except Exception: pass + + profile = "big" if model_uses_external_gpu(selected_model) else "small" + set_model_profile(params, profile, selected_model) elif key in ("ModelVersion", "DrivingModelVersion"): params.put("ModelVersion", str_val) params.put("DrivingModelVersion", str_val) @@ -6263,6 +6268,8 @@ def setup(app): return jsonify({ "models": models, "currentModel": _current_model_key(), + "activeSmallModel": _active_model_key("small"), + "activeBigModel": _active_model_key("big"), "summary": { "installed": sum(1 for model in models if model["installed"]), "missing": sum(1 for model in models if not model["installed"]), @@ -6431,6 +6438,43 @@ def setup(app): return jsonify({"message": f"Updated model {' and '.join(changed)}."}), 200 + @app.route("/api/models/active", methods=["PUT"]) + def set_active_model_profile(): + if params.get_bool("IsOnroad"): + return jsonify({"error": "Cannot change active models while driving."}), 403 + + data = request.get_json(silent=True) or {} + profile = str(data.get("profile") or "").strip().lower() + if profile not in ("small", "big"): + return jsonify({"error": "Model profile must be 'small' or 'big'."}), 400 + + model_key = canonical_model_key(str(data.get("model") or "").strip()) + if not model_key: + return jsonify({"error": "Missing model key."}), 400 + + catalog = {model["value"]: model for model in get_model_catalog()} + model = catalog.get(model_key) + if model is None: + return jsonify({"error": f"Unknown model '{model_key}'."}), 404 + if not model["installed"]: + return jsonify({"error": f"Download '{model['label']}' before selecting it."}), 409 + if bool(model["requiresGpu"]) != (profile == "big"): + expected = "an eGPU model" if profile == "big" else "an on-device model" + return jsonify({"error": f"Active {profile.title()} must be {expected}."}), 409 + + lab_config = normalize_model_lab_config(params.get(MODEL_LAB_CONFIG_PARAM, encoding="utf-8") or "") + if lab_config["enabled"]: + lab_config["enabled"] = False + params.put(MODEL_LAB_CONFIG_PARAM, lab_config) + params.remove(MODEL_LAB_RUNTIME_PARAM) + + set_model_profile(params, profile, model_key, model["label"], model["version"]) + return jsonify({ + "message": f"Active {profile.title()} set to '{model['label']}'.", + "profile": profile, + "model": model_key, + }), 200 + @app.route("/api/models/status", methods=["GET"]) def get_models_status(): models = get_model_catalog() @@ -6442,6 +6486,8 @@ def setup(app): downloading = bool(model_to_download or lab_model_to_download) or download_all current_model = _current_model_key() + active_small_model = _active_model_key("small") + active_big_model = _active_model_key("big") sort_mode = read_legacy_param_file(MODEL_SORT_MODE_PARAM, DEFAULT_MODEL_SORT_MODE) terminal = progress in ("Downloaded!", "All models downloaded!") or bool(re.search(r"cancelled|exists|failed|offline|invalid|error", progress, re.IGNORECASE)) summary = { @@ -6462,6 +6508,8 @@ def setup(app): cancelling, progress, current_model, + active_small_model, + active_big_model, sort_mode, terminal, bool(params.get_bool("IsOnroad")), @@ -6498,6 +6546,8 @@ def setup(app): "terminal": terminal, "models": models, "currentModel": current_model, + "activeSmallModel": active_small_model, + "activeBigModel": active_big_model, "summary": summary, "sortMode": sort_mode, }), 200 @@ -6619,7 +6669,8 @@ def setup(app): return jsonify({"error": "Missing model key."}), 400 current_model = _current_model_key() - if model_key == current_model: + active_models = {current_model, _active_model_key("small"), _active_model_key("big")} + if model_key in active_models: return jsonify({"error": "Cannot delete the currently active model."}), 409 catalog = {model["value"]: model for model in get_model_catalog()} @@ -6861,6 +6912,10 @@ def setup(app): current_model = _param_text(params.get("Model", encoding="utf-8") or params.get("DrivingModel", encoding="utf-8")) return canonical_model_key(current_model) or _default_model_key() + def _active_model_key(profile): + model_key, _, _ = get_model_profile(params, profile) + return canonical_model_key(model_key) + def is_model_installed(model_key, model_version, on_disk_files): del model_version if is_builtin_model_key(model_key):