diff --git a/common/libcommon.a b/common/libcommon.a index 5c5dd0748..7dbad1312 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 b5f178c73..58a303dd2 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -318,6 +318,9 @@ inline static std::unordered_map keys = { {"ForceStopDistanceOffset", {PERSISTENT, INT, "0", "0", 2}}, {"ForceStandstill", {PERSISTENT, BOOL, "0", "0", 2}}, {"ForceTorqueController", {PERSISTENT, BOOL, "0", "0", 3}}, + {"FTMActiveOverrides", {PERSISTENT, JSON, "{}", "{}", 2}}, + {"FTMActiveProfileId", {PERSISTENT, STRING, "", "", 2}}, + {"FTMTrialApplied", {PERSISTENT, BOOL, "0", "0", 2}}, {"FPSCounter", {PERSISTENT, BOOL, "1", "0", 3}}, {"GalaxyDashboardStats", {PERSISTENT | DONT_LOG, JSON, "{}", "{}"}}, {"StarPilotApiToken", {PERSISTENT | DONT_LOG, STRING, "", "", 0}}, diff --git a/common/params_pyx.so b/common/params_pyx.so index a79ba6947..a4539c757 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 7f4625ed7..9d9bc030f 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -90,6 +90,7 @@ class LatControlTorque(LatControl): self.is_silverado = CP.carFingerprint in SILVERADO_CARS self.is_gm = CP.brand == "gm" self.is_hkg_canfd_torque = CP.brand == "hyundai" and bool(CP.flags & HyundaiFlags.CANFD) + self.ftm_surface_profile_key = get_ftm_surface_profile_key(CP.carFingerprint, torque_control=True) if self.is_ioniq_6: self.low_speed_reset_threshold = min(self.low_speed_reset_threshold, IONIQ_6_LOW_SPEED_PID_RESET_SPEED) self.use_bolt_ff_scaling = self.is_bolt_2022_2023 or self.is_bolt_2018_2021 or self.is_bolt_2017 @@ -157,6 +158,7 @@ class LatControlTorque(LatControl): def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay, calibrated_pose, model_data, starpilot_toggles): pid_log = log.ControlsState.LateralTorqueState.new_message() pid_log.version = VERSION + set_ftm_runtime_overrides(getattr(starpilot_toggles, "ftm_active_overrides", None)) if not active: output_torque = 0.0 pid_log.active = False @@ -324,6 +326,15 @@ class LatControlTorque(LatControl): friction_threshold = CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD friction_scale = get_civic_bosch_modified_b_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = 1.0 + ((friction_scale - 1.0) * civic_bosch_modified_a_center_taper) + if self.ftm_surface_profile_key and not ioniq_6_active: + universal_ftm_profile = self.ftm_surface_profile_key == FTM_UNIVERSAL_PROFILE_KEY + ftm_full_surface_center_taper = get_ftm_full_surface_center_taper_scale(self.ftm_surface_profile_key, setpoint, CS.vEgo, + include_base_center=universal_ftm_profile) + ff *= get_ftm_full_surface_ff_scale(self.ftm_surface_profile_key, setpoint, desired_lateral_jerk, CS.vEgo, + include_base_ff=universal_ftm_profile) * ftm_full_surface_center_taper + friction_threshold = get_ftm_full_surface_friction_threshold(self.ftm_surface_profile_key, friction_threshold, CS.vEgo, + setpoint, desired_lateral_jerk, + include_base_threshold=universal_ftm_profile) if trailer_load_kg > 0.0: ff *= get_trailer_lateral_ff_scale(trailer_load_kg, CS.vEgo, setpoint) friction_scale *= get_trailer_lateral_friction_scale(trailer_load_kg, CS.vEgo, setpoint) @@ -353,6 +364,11 @@ class LatControlTorque(LatControl): actual_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetDeg output_torque = get_ioniq_6_low_speed_angle_assist_torque(desired_angle_no_offset, actual_angle_no_offset, output_torque, CS.vEgo) + elif self.ftm_surface_profile_key and not CS.steeringPressed: + desired_angle_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) + actual_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetDeg + output_torque = get_ftm_full_surface_low_speed_angle_assist_torque(self.ftm_surface_profile_key, desired_angle_no_offset, + actual_angle_no_offset, output_torque, CS.vEgo) if ioniq_6_active: output_torque *= get_ioniq_6_highway_output_taper_scale(setpoint, CS.vEgo) output_torque *= get_ioniq_6_highway_transition_output_taper_scale(setpoint, desired_lateral_jerk, CS.vEgo) diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index b343a045b..ae709839d 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -1,3 +1,4 @@ +import json import math import numpy as np @@ -10,6 +11,8 @@ from openpilot.starpilot.common.testing_grounds import testing_ground CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD = 0.30 STANDARD_FRICTION_THRESHOLD = 0.30 HKG_CANFD_BASE_FRICTION_THRESHOLD = 0.39 +FTM_SCHEMA_VERSION = 1 +FTM_FRICTION_SPEED_KNOTS = [0.0, 5.0, 10.0, 15.0, 25.0] CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT = 1.20 CIVIC_BOSCH_MODIFIED_A_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.00 CIVIC_BOSCH_MODIFIED_B_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.75 @@ -692,6 +695,9 @@ TRAILER_LATERAL_LAT_RISE = 0.30 TRAILER_LATERAL_FF_GAIN = 0.05 TRAILER_LATERAL_FRICTION_GAIN = 0.03 +_FTM_ACTIVE_OVERRIDES_TEXT = "" +_FTM_ACTIVE_OVERRIDES = {} + def _sigmoid(x: float) -> float: if x >= 0.0: @@ -702,17 +708,119 @@ def _sigmoid(x: float) -> float: return z / (1.0 + z) -def get_gm_base_friction_threshold(v_ego: float) -> float: - # GM's speed-scaled base friction threshold behavior. +def _gm_base_friction_threshold_default(v_ego: float) -> float: return float(np.interp(v_ego, [1 * CV.MPH_TO_MS, 20 * CV.MPH_TO_MS, 75 * CV.MPH_TO_MS], [0.16, 0.19, 0.27])) +def _standard_friction_threshold_default(v_ego: float) -> float: + return max(_gm_base_friction_threshold_default(v_ego), STANDARD_FRICTION_THRESHOLD) + + +def _hkg_canfd_base_friction_threshold_default(v_ego: float) -> float: + return max(_gm_base_friction_threshold_default(v_ego), HKG_CANFD_BASE_FRICTION_THRESHOLD) + + +def _ftm_copy_json(value): + return json.loads(json.dumps(value)) + + +def normalize_ftm_overrides(overrides) -> dict: + if overrides in (None, "", b""): + return {} + + if isinstance(overrides, bytes): + overrides = overrides.decode("utf-8", errors="replace") + + if isinstance(overrides, str): + stripped = overrides.strip() + if not stripped: + return {} + try: + overrides = json.loads(stripped) + except Exception: + return {} + + if not isinstance(overrides, dict): + return {} + + normalized = { + "schemaVersion": FTM_SCHEMA_VERSION, + "baseFrictionThresholds": {}, + "vehicleKnobs": {}, + } + + for family in ("gm", "standard", "hkg_canfd"): + payload = overrides.get("baseFrictionThresholds", {}).get(family, {}) + values = payload.get("values", payload if isinstance(payload, list) else []) + if not isinstance(values, (list, tuple)) or len(values) != len(FTM_FRICTION_SPEED_KNOTS): + continue + try: + normalized["baseFrictionThresholds"][family] = { + "speedKnots": list(FTM_FRICTION_SPEED_KNOTS), + "values": [float(v) for v in values], + } + except Exception: + continue + + vehicle_knobs = overrides.get("vehicleKnobs", {}) + if isinstance(vehicle_knobs, dict): + for key, value in vehicle_knobs.items(): + try: + normalized["vehicleKnobs"][str(key)] = float(value) + except Exception: + continue + + if not normalized["baseFrictionThresholds"] and not normalized["vehicleKnobs"]: + return {} + + return normalized + + +def set_ftm_runtime_overrides(overrides) -> None: + global _FTM_ACTIVE_OVERRIDES, _FTM_ACTIVE_OVERRIDES_TEXT + + normalized = normalize_ftm_overrides(overrides) + text = json.dumps(normalized, sort_keys=True, separators=(",", ":")) if normalized else "" + if text == _FTM_ACTIVE_OVERRIDES_TEXT: + return + + _FTM_ACTIVE_OVERRIDES_TEXT = text + _FTM_ACTIVE_OVERRIDES = normalized + + +def clear_ftm_runtime_overrides() -> None: + set_ftm_runtime_overrides({}) + + +def get_ftm_runtime_overrides() -> dict: + return _ftm_copy_json(_FTM_ACTIVE_OVERRIDES) if _FTM_ACTIVE_OVERRIDES else {} + + +def _ftm_base_friction_threshold(family: str, v_ego: float, default_fn) -> float: + payload = _FTM_ACTIVE_OVERRIDES.get("baseFrictionThresholds", {}).get(family, {}) + values = payload.get("values", []) + if isinstance(values, list) and len(values) == len(FTM_FRICTION_SPEED_KNOTS): + return float(np.interp(v_ego, FTM_FRICTION_SPEED_KNOTS, values)) + return float(default_fn(v_ego)) + + +def _ftm_vehicle_knob(name: str, default_value: float) -> float: + try: + return float(_FTM_ACTIVE_OVERRIDES.get("vehicleKnobs", {}).get(name, default_value)) + except Exception: + return float(default_value) + + +def get_gm_base_friction_threshold(v_ego: float) -> float: + return _ftm_base_friction_threshold("gm", v_ego, _gm_base_friction_threshold_default) + + def get_standard_friction_threshold(v_ego: float) -> float: - return max(get_gm_base_friction_threshold(v_ego), STANDARD_FRICTION_THRESHOLD) + return _ftm_base_friction_threshold("standard", v_ego, _standard_friction_threshold_default) def get_hkg_canfd_base_friction_threshold(v_ego: float) -> float: - return max(get_gm_base_friction_threshold(v_ego), HKG_CANFD_BASE_FRICTION_THRESHOLD) + return _ftm_base_friction_threshold("hkg_canfd", v_ego, _hkg_canfd_base_friction_threshold_default) def get_trailer_lateral_assist_factor(trailer_load_kg: float, v_ego: float, desired_lateral_accel: float) -> float: @@ -756,7 +864,11 @@ def get_prius_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float if desired_lateral_accel == 0.0: return 1.0 - gain = _prius_side_value(desired_lateral_accel, PRIUS_FF_GAIN_LEFT, PRIUS_FF_GAIN_RIGHT) + gain = _prius_side_value( + desired_lateral_accel, + _ftm_vehicle_knob("toyota_prius.ff_gain_left", PRIUS_FF_GAIN_LEFT), + _ftm_vehicle_knob("toyota_prius.ff_gain_right", PRIUS_FF_GAIN_RIGHT), + ) abs_lateral_accel = abs(desired_lateral_accel) onset = _prius_sigmoid((abs_lateral_accel - PRIUS_FF_ONSET) / PRIUS_FF_ONSET_WIDTH) cutoff = _prius_sigmoid((PRIUS_FF_CUTOFF - abs_lateral_accel) / PRIUS_FF_CUTOFF_WIDTH) @@ -765,9 +877,17 @@ def get_prius_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float turn_in_weight = max(phase, 0.0) unwind_weight = max(-phase, 0.0) low_speed_factor = _prius_low_speed_factor(v_ego) - turn_in_boost = 1.0 + (_prius_side_value(desired_lateral_accel, PRIUS_TURN_IN_BOOST_LEFT, PRIUS_TURN_IN_BOOST_RIGHT) * + turn_in_boost = 1.0 + (_prius_side_value( + desired_lateral_accel, + _ftm_vehicle_knob("toyota_prius.turn_in_boost_left", PRIUS_TURN_IN_BOOST_LEFT), + _ftm_vehicle_knob("toyota_prius.turn_in_boost_right", PRIUS_TURN_IN_BOOST_RIGHT), + ) * turn_in_weight * (0.35 + 0.65 * low_speed_factor)) - unwind_taper = 1.0 - (_prius_side_value(desired_lateral_accel, PRIUS_UNWIND_TAPER_LEFT, PRIUS_UNWIND_TAPER_RIGHT) * + unwind_taper = 1.0 - (_prius_side_value( + desired_lateral_accel, + _ftm_vehicle_knob("toyota_prius.unwind_taper_left", PRIUS_UNWIND_TAPER_LEFT), + _ftm_vehicle_knob("toyota_prius.unwind_taper_right", PRIUS_UNWIND_TAPER_RIGHT), + ) * unwind_weight * (0.35 + 0.65 * low_speed_factor)) return 1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0)) @@ -778,9 +898,17 @@ def get_prius_friction_threshold(v_ego: float, desired_lateral_accel: float = 0. phase = _prius_transition_phase(desired_lateral_accel, desired_lateral_jerk) turn_in_weight = max(phase, 0.0) unwind_weight = max(-phase, 0.0) - threshold_scale = 1.0 - (_prius_side_value(desired_lateral_accel, PRIUS_TURN_IN_THRESHOLD_REDUCTION_LEFT, PRIUS_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * + threshold_scale = 1.0 - (_prius_side_value( + desired_lateral_accel, + _ftm_vehicle_knob("toyota_prius.turn_in_threshold_reduction_left", PRIUS_TURN_IN_THRESHOLD_REDUCTION_LEFT), + _ftm_vehicle_knob("toyota_prius.turn_in_threshold_reduction_right", PRIUS_TURN_IN_THRESHOLD_REDUCTION_RIGHT), + ) * transition_envelope * turn_in_weight) - threshold_scale += (_prius_side_value(desired_lateral_accel, PRIUS_UNWIND_THRESHOLD_INCREASE_LEFT, PRIUS_UNWIND_THRESHOLD_INCREASE_RIGHT) * + threshold_scale += (_prius_side_value( + desired_lateral_accel, + _ftm_vehicle_knob("toyota_prius.unwind_threshold_increase_left", PRIUS_UNWIND_THRESHOLD_INCREASE_LEFT), + _ftm_vehicle_knob("toyota_prius.unwind_threshold_increase_right", PRIUS_UNWIND_THRESHOLD_INCREASE_RIGHT), + ) * transition_envelope * unwind_weight) return base_threshold * min(max(threshold_scale, 0.86), 1.16) @@ -801,7 +929,7 @@ def get_prius_friction_scale(v_ego: float, desired_lateral_accel: float, desired def get_prius_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: speed_weight = _prius_sigmoid((v_ego - PRIUS_CENTER_TAPER_SPEED) / PRIUS_CENTER_TAPER_SPEED_WIDTH) center_weight = _prius_sigmoid((PRIUS_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / PRIUS_CENTER_TAPER_LAT_WIDTH) - reduction = PRIUS_CENTER_TAPER_MAX * speed_weight * center_weight + reduction = _ftm_vehicle_knob("toyota_prius.center_taper_max", PRIUS_CENTER_TAPER_MAX) * speed_weight * center_weight return 1.0 - reduction @@ -1107,23 +1235,35 @@ def get_bolt_2022_2023_ff_scale(desired_lateral_accel: float, desired_lateral_je if desired_lateral_accel == 0.0: return 1.0 - gain = _bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_FF_GAIN_LEFT, BOLT_2022_2023_FF_GAIN_RIGHT) + gain = _bolt_2022_2023_side_value( + desired_lateral_accel, + _ftm_vehicle_knob("gm_bolt_2022_2023.ff_gain_left", BOLT_2022_2023_FF_GAIN_LEFT), + _ftm_vehicle_knob("gm_bolt_2022_2023.ff_gain_right", BOLT_2022_2023_FF_GAIN_RIGHT), + ) abs_lateral_accel = abs(desired_lateral_accel) onset = _bolt_2022_2023_sigmoid((abs_lateral_accel - BOLT_2022_2023_FF_ONSET) / BOLT_2022_2023_FF_ONSET_WIDTH) cutoff = _bolt_2022_2023_sigmoid((BOLT_2022_2023_FF_CUTOFF - abs_lateral_accel) / BOLT_2022_2023_FF_CUTOFF_WIDTH) extra_scale = gain * onset * cutoff speed_weight = _bolt_2022_2023_sigmoid((v_ego - BOLT_2022_2023_CENTER_TAPER_SPEED) / BOLT_2022_2023_CENTER_TAPER_SPEED_WIDTH) center_weight = _bolt_2022_2023_sigmoid((BOLT_2022_2023_CENTER_TAPER_LAT - abs_lateral_accel) / BOLT_2022_2023_CENTER_TAPER_LAT_WIDTH) - center_taper = 1.0 - (BOLT_2022_2023_CENTER_TAPER_MAX * speed_weight * center_weight) + center_taper = 1.0 - (_ftm_vehicle_knob("gm_bolt_2022_2023.center_taper_max", BOLT_2022_2023_CENTER_TAPER_MAX) * speed_weight * center_weight) low_speed_factor = _bolt_2022_2023_low_speed_factor(v_ego) transition_envelope = _bolt_2022_2023_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) phase = _bolt_2022_2023_transition_phase(desired_lateral_accel, desired_lateral_jerk) turn_in_weight = max(phase, 0.0) unwind_weight = max(-phase, 0.0) - turn_in_boost = 1.0 + (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_TURN_IN_BOOST_LEFT, BOLT_2022_2023_TURN_IN_BOOST_RIGHT) * + turn_in_boost = 1.0 + (_bolt_2022_2023_side_value( + desired_lateral_accel, + _ftm_vehicle_knob("gm_bolt_2022_2023.turn_in_boost_left", BOLT_2022_2023_TURN_IN_BOOST_LEFT), + _ftm_vehicle_knob("gm_bolt_2022_2023.turn_in_boost_right", BOLT_2022_2023_TURN_IN_BOOST_RIGHT), + ) * turn_in_weight * low_speed_factor) unwind_envelope = (0.25 + 0.75 * low_speed_factor) * (1.0 + 0.45 * transition_envelope) - unwind_taper = 1.0 - (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_UNWIND_TAPER_LEFT, BOLT_2022_2023_UNWIND_TAPER_RIGHT) * + unwind_taper = 1.0 - (_bolt_2022_2023_side_value( + desired_lateral_accel, + _ftm_vehicle_knob("gm_bolt_2022_2023.unwind_taper_left", BOLT_2022_2023_UNWIND_TAPER_LEFT), + _ftm_vehicle_knob("gm_bolt_2022_2023.unwind_taper_right", BOLT_2022_2023_UNWIND_TAPER_RIGHT), + ) * unwind_weight * unwind_envelope) return 1.0 + (extra_scale * center_taper * turn_in_boost * max(unwind_taper, 0.0)) @@ -1134,9 +1274,17 @@ def get_bolt_2022_2023_friction_threshold(v_ego: float, desired_lateral_accel: f phase = _bolt_2022_2023_transition_phase(desired_lateral_accel, desired_lateral_jerk) turn_in_weight = max(phase, 0.0) unwind_weight = max(-phase, 0.0) - threshold_scale = 1.0 - (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_LEFT, BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * + threshold_scale = 1.0 - (_bolt_2022_2023_side_value( + desired_lateral_accel, + _ftm_vehicle_knob("gm_bolt_2022_2023.turn_in_threshold_reduction_left", BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_LEFT), + _ftm_vehicle_knob("gm_bolt_2022_2023.turn_in_threshold_reduction_right", BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_RIGHT), + ) * transition_envelope * turn_in_weight) - threshold_scale += (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_LEFT, BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_RIGHT) * + threshold_scale += (_bolt_2022_2023_side_value( + desired_lateral_accel, + _ftm_vehicle_knob("gm_bolt_2022_2023.unwind_threshold_increase_left", BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_LEFT), + _ftm_vehicle_knob("gm_bolt_2022_2023.unwind_threshold_increase_right", BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_RIGHT), + ) * transition_envelope * unwind_weight) return base_threshold * min(max(threshold_scale, 0.84), 1.14) @@ -1802,14 +1950,18 @@ def _ioniq_6_transition_envelope(v_ego: float, desired_lateral_accel: float, des def _ioniq_6_curvy_speed_weight(v_ego: float) -> float: - onset = _ioniq_6_sigmoid((max(v_ego, 0.0) - IONIQ_6_CURVY_SPEED_MIN) / IONIQ_6_CURVY_SPEED_MIN_WIDTH) - cutoff = _ioniq_6_sigmoid((IONIQ_6_CURVY_SPEED_MAX - max(v_ego, 0.0)) / IONIQ_6_CURVY_SPEED_MAX_WIDTH) + curvy_speed_min = _ftm_vehicle_knob("hyundai_ioniq_6.curvy_speed_min", IONIQ_6_CURVY_SPEED_MIN) + curvy_speed_max = _ftm_vehicle_knob("hyundai_ioniq_6.curvy_speed_max", IONIQ_6_CURVY_SPEED_MAX) + onset = _ioniq_6_sigmoid((max(v_ego, 0.0) - curvy_speed_min) / IONIQ_6_CURVY_SPEED_MIN_WIDTH) + cutoff = _ioniq_6_sigmoid((curvy_speed_max - max(v_ego, 0.0)) / IONIQ_6_CURVY_SPEED_MAX_WIDTH) return onset * cutoff def _ioniq_6_curvy_turn_in_trim_speed_weight(v_ego: float) -> float: - onset = _ioniq_6_sigmoid((max(v_ego, 0.0) - IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_MIN) / IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_WIDTH) - cutoff = _ioniq_6_sigmoid((IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_MAX - max(v_ego, 0.0)) / IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_WIDTH) + curvy_turn_in_speed_min = _ftm_vehicle_knob("hyundai_ioniq_6.curvy_turn_in_trim_speed_min", IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_MIN) + curvy_turn_in_speed_max = _ftm_vehicle_knob("hyundai_ioniq_6.curvy_turn_in_trim_speed_max", IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_MAX) + onset = _ioniq_6_sigmoid((max(v_ego, 0.0) - curvy_turn_in_speed_min) / IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_WIDTH) + cutoff = _ioniq_6_sigmoid((curvy_turn_in_speed_max - max(v_ego, 0.0)) / IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_WIDTH) return onset * cutoff @@ -1818,7 +1970,11 @@ def get_ioniq_6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: flo if desired_lateral_accel == 0.0: return 1.0 - gain = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_FF_GAIN_LEFT, IONIQ_6_FF_GAIN_RIGHT) + gain = _ioniq_6_side_value( + desired_lateral_accel, + _ftm_vehicle_knob("hyundai_ioniq_6.ff_gain_left", IONIQ_6_FF_GAIN_LEFT), + _ftm_vehicle_knob("hyundai_ioniq_6.ff_gain_right", IONIQ_6_FF_GAIN_RIGHT), + ) abs_lateral_accel = abs(desired_lateral_accel) onset = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_FF_ONSET) / IONIQ_6_FF_ONSET_WIDTH) cutoff = _ioniq_6_sigmoid((IONIQ_6_FF_CUTOFF - abs_lateral_accel) / IONIQ_6_FF_CUTOFF_WIDTH) @@ -1827,9 +1983,17 @@ def get_ioniq_6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: flo turn_in_weight = max(phase, 0.0) unwind_weight = max(-phase, 0.0) low_speed_factor = _ioniq_6_low_speed_factor(v_ego) - turn_in_boost = 1.0 + (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_TURN_IN_BOOST_LEFT, IONIQ_6_TURN_IN_BOOST_RIGHT) * + turn_in_boost = 1.0 + (_ioniq_6_side_value( + desired_lateral_accel, + _ftm_vehicle_knob("hyundai_ioniq_6.turn_in_boost_left", IONIQ_6_TURN_IN_BOOST_LEFT), + _ftm_vehicle_knob("hyundai_ioniq_6.turn_in_boost_right", IONIQ_6_TURN_IN_BOOST_RIGHT), + ) * turn_in_weight * low_speed_factor) - unwind_taper = 1.0 - (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_UNWIND_TAPER_LEFT, IONIQ_6_UNWIND_TAPER_RIGHT) * + unwind_taper = 1.0 - (_ioniq_6_side_value( + desired_lateral_accel, + _ftm_vehicle_knob("hyundai_ioniq_6.unwind_taper_left", IONIQ_6_UNWIND_TAPER_LEFT), + _ftm_vehicle_knob("hyundai_ioniq_6.unwind_taper_right", IONIQ_6_UNWIND_TAPER_RIGHT), + ) * unwind_weight * (0.30 + 0.70 * low_speed_factor)) crawl_turn_in_scale = 0.0 if desired_lateral_accel * desired_lateral_jerk > 0.0: @@ -1837,8 +2001,11 @@ def get_ioniq_6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: flo IONIQ_6_CRAWL_TURN_IN_FF_SPEED_WIDTH) crawl_lat_weight = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_CRAWL_TURN_IN_FF_LAT) / IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH) - crawl_turn_in_scale = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_CRAWL_TURN_IN_FF_BOOST_LEFT, - IONIQ_6_CRAWL_TURN_IN_FF_BOOST_RIGHT) * crawl_speed_weight * crawl_lat_weight + crawl_turn_in_scale = _ioniq_6_side_value( + desired_lateral_accel, + _ftm_vehicle_knob("hyundai_ioniq_6.crawl_turn_in_ff_boost_left", IONIQ_6_CRAWL_TURN_IN_FF_BOOST_LEFT), + _ftm_vehicle_knob("hyundai_ioniq_6.crawl_turn_in_ff_boost_right", IONIQ_6_CRAWL_TURN_IN_FF_BOOST_RIGHT), + ) * crawl_speed_weight * crawl_lat_weight high_speed_right_turn_in_scale = 0.0 if desired_lateral_accel < 0.0 and desired_lateral_accel * desired_lateral_jerk > 0.0: high_speed_weight = _ioniq_6_sigmoid((max(v_ego, 0.0) - IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED) / @@ -1861,9 +2028,17 @@ def get_ioniq_6_friction_threshold(v_ego: float, desired_lateral_accel: float = turn_in_weight = max(phase, 0.0) unwind_weight = max(-phase, 0.0) unwind_speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_UNWIND_HIGH_SPEED_SPEED) / IONIQ_6_UNWIND_HIGH_SPEED_SPEED_WIDTH) - threshold_scale = 1.0 - (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT, IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * + threshold_scale = 1.0 - (_ioniq_6_side_value( + desired_lateral_accel, + _ftm_vehicle_knob("hyundai_ioniq_6.turn_in_threshold_reduction_left", IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT), + _ftm_vehicle_knob("hyundai_ioniq_6.turn_in_threshold_reduction_right", IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT), + ) * transition_envelope * turn_in_weight) - threshold_scale += (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT, IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT) * + threshold_scale += (_ioniq_6_side_value( + desired_lateral_accel, + _ftm_vehicle_knob("hyundai_ioniq_6.unwind_threshold_increase_left", IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT), + _ftm_vehicle_knob("hyundai_ioniq_6.unwind_threshold_increase_right", IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT), + ) * transition_envelope * unwind_weight * unwind_speed_weight) return base_threshold * min(max(threshold_scale, 0.82), 1.18) @@ -1891,12 +2066,12 @@ def get_ioniq_6_friction_center_fade_scale(desired_lateral_accel: float, v_ego: def get_ioniq_6_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_CENTER_TAPER_SPEED) / IONIQ_6_CENTER_TAPER_SPEED_WIDTH) center_weight = _ioniq_6_sigmoid((IONIQ_6_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / IONIQ_6_CENTER_TAPER_LAT_WIDTH) - high_speed_reduction = IONIQ_6_CENTER_TAPER_MAX * speed_weight * center_weight + high_speed_reduction = _ftm_vehicle_knob("hyundai_ioniq_6.center_taper_max", IONIQ_6_CENTER_TAPER_MAX) * speed_weight * center_weight highway_speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED) / IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED_WIDTH) highway_center_weight = _ioniq_6_sigmoid((IONIQ_6_HIGHWAY_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / IONIQ_6_HIGHWAY_CENTER_TAPER_LAT_WIDTH) - highway_center_reduction = IONIQ_6_HIGHWAY_CENTER_TAPER_MAX * highway_speed_weight * highway_center_weight + highway_center_reduction = _ftm_vehicle_knob("hyundai_ioniq_6.highway_center_taper_max", IONIQ_6_HIGHWAY_CENTER_TAPER_MAX) * highway_speed_weight * highway_center_weight low_mid_onset = _ioniq_6_sigmoid((v_ego - IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MIN) / IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_WIDTH) low_mid_cutoff = _ioniq_6_sigmoid((IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MAX - v_ego) / IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_WIDTH) @@ -1944,8 +2119,8 @@ def get_ioniq_6_directional_taper_scale(desired_lateral_accel: float, desired_la reduction = band_weight * (base_reduction + unwind_reduction * unwind_weight) reduction += heavy_band_weight * (heavy_base_reduction + heavy_unwind_reduction * unwind_weight) reduction += (_ioniq_6_side_value(desired_lateral_accel, - IONIQ_6_CURVY_TURN_IN_TRIM_LEFT, - IONIQ_6_CURVY_TURN_IN_TRIM_RIGHT) * + _ftm_vehicle_knob("hyundai_ioniq_6.curvy_turn_in_trim_left", IONIQ_6_CURVY_TURN_IN_TRIM_LEFT), + _ftm_vehicle_knob("hyundai_ioniq_6.curvy_turn_in_trim_right", IONIQ_6_CURVY_TURN_IN_TRIM_RIGHT)) * curvy_turn_in_trim_weight) curvy_unwind_weight = 0.0 curvy_unwind_floor_relief = 0.0 @@ -1957,12 +2132,12 @@ def get_ioniq_6_directional_taper_scale(desired_lateral_accel: float, desired_la IONIQ_6_CURVY_UNWIND_LAT_CUTOFF_WIDTH) curvy_unwind_weight = curvy_unwind_speed_weight * curvy_unwind_lat_onset * curvy_unwind_lat_cutoff * unwind_weight curvy_unwind_floor_relief = (_ioniq_6_side_value(desired_lateral_accel, - IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_LEFT, - IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_RIGHT) * + _ftm_vehicle_knob("hyundai_ioniq_6.curvy_unwind_floor_relief_left", IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_LEFT), + _ftm_vehicle_knob("hyundai_ioniq_6.curvy_unwind_floor_relief_right", IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_RIGHT)) * curvy_unwind_weight) reduction += (_ioniq_6_side_value(desired_lateral_accel, - IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_LEFT, - IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_RIGHT) * + _ftm_vehicle_knob("hyundai_ioniq_6.curvy_unwind_extra_reduction_left", IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_LEFT), + _ftm_vehicle_knob("hyundai_ioniq_6.curvy_unwind_extra_reduction_right", IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_RIGHT)) * curvy_unwind_weight) floor = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_FLOOR_LEFT, IONIQ_6_DIRECTIONAL_TAPER_FLOOR_RIGHT) floor -= _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT) * unwind_weight @@ -2011,7 +2186,11 @@ def get_ioniq_6_low_speed_angle_assist_torque(desired_angle_deg: float, actual_a tracking_taper = _ioniq_6_sigmoid((tracking_ratio - IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_START) / IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_WIDTH) tracking_scale = max(1.0 - tracking_taper, IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_FLOOR) - assist_torque = math.copysign(IONIQ_6_LOW_SPEED_ANGLE_ASSIST_MAX_TORQUE * speed_weight * error_weight * desired_angle_weight * tracking_scale, -angle_error) + assist_torque = math.copysign( + _ftm_vehicle_knob("hyundai_ioniq_6.low_speed_angle_assist_max_torque", IONIQ_6_LOW_SPEED_ANGLE_ASSIST_MAX_TORQUE) * + speed_weight * error_weight * desired_angle_weight * tracking_scale, + -angle_error, + ) if abs(assist_torque) < 1e-4: return current_output_torque @@ -2069,7 +2248,11 @@ def get_kia_ev6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: flo if desired_lateral_accel == 0.0: return 1.0 - gain = _kia_ev6_side_value(desired_lateral_accel, KIA_EV6_FF_GAIN_LEFT, KIA_EV6_FF_GAIN_RIGHT) + gain = _kia_ev6_side_value( + desired_lateral_accel, + _ftm_vehicle_knob("hyundai_kia_ev6.ff_gain_left", KIA_EV6_FF_GAIN_LEFT), + _ftm_vehicle_knob("hyundai_kia_ev6.ff_gain_right", KIA_EV6_FF_GAIN_RIGHT), + ) abs_lateral_accel = abs(desired_lateral_accel) onset = _kia_ev6_sigmoid((abs_lateral_accel - KIA_EV6_FF_ONSET) / KIA_EV6_FF_ONSET_WIDTH) cutoff = _kia_ev6_sigmoid((KIA_EV6_FF_CUTOFF - abs_lateral_accel) / KIA_EV6_FF_CUTOFF_WIDTH) @@ -2078,9 +2261,17 @@ def get_kia_ev6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: flo turn_in_weight = max(phase, 0.0) unwind_weight = max(-phase, 0.0) low_speed_factor = _kia_ev6_low_speed_factor(v_ego) - turn_in_boost = 1.0 + (_kia_ev6_side_value(desired_lateral_accel, KIA_EV6_TURN_IN_BOOST_LEFT, KIA_EV6_TURN_IN_BOOST_RIGHT) * + turn_in_boost = 1.0 + (_kia_ev6_side_value( + desired_lateral_accel, + _ftm_vehicle_knob("hyundai_kia_ev6.turn_in_boost_left", KIA_EV6_TURN_IN_BOOST_LEFT), + _ftm_vehicle_knob("hyundai_kia_ev6.turn_in_boost_right", KIA_EV6_TURN_IN_BOOST_RIGHT), + ) * turn_in_weight * (0.35 + 0.65 * low_speed_factor)) - unwind_taper = 1.0 - (_kia_ev6_side_value(desired_lateral_accel, KIA_EV6_UNWIND_TAPER_LEFT, KIA_EV6_UNWIND_TAPER_RIGHT) * + unwind_taper = 1.0 - (_kia_ev6_side_value( + desired_lateral_accel, + _ftm_vehicle_knob("hyundai_kia_ev6.unwind_taper_left", KIA_EV6_UNWIND_TAPER_LEFT), + _ftm_vehicle_knob("hyundai_kia_ev6.unwind_taper_right", KIA_EV6_UNWIND_TAPER_RIGHT), + ) * unwind_weight * (0.35 + 0.65 * low_speed_factor)) return 1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0)) @@ -2091,9 +2282,17 @@ def get_kia_ev6_friction_threshold(v_ego: float, desired_lateral_accel: float = phase = _kia_ev6_transition_phase(desired_lateral_accel, desired_lateral_jerk) turn_in_weight = max(phase, 0.0) unwind_weight = max(-phase, 0.0) - threshold_scale = 1.0 - (_kia_ev6_side_value(desired_lateral_accel, KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_LEFT, KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * + threshold_scale = 1.0 - (_kia_ev6_side_value( + desired_lateral_accel, + _ftm_vehicle_knob("hyundai_kia_ev6.turn_in_threshold_reduction_left", KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_LEFT), + _ftm_vehicle_knob("hyundai_kia_ev6.turn_in_threshold_reduction_right", KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_RIGHT), + ) * transition_envelope * turn_in_weight) - threshold_scale += (_kia_ev6_side_value(desired_lateral_accel, KIA_EV6_UNWIND_THRESHOLD_INCREASE_LEFT, KIA_EV6_UNWIND_THRESHOLD_INCREASE_RIGHT) * + threshold_scale += (_kia_ev6_side_value( + desired_lateral_accel, + _ftm_vehicle_knob("hyundai_kia_ev6.unwind_threshold_increase_left", KIA_EV6_UNWIND_THRESHOLD_INCREASE_LEFT), + _ftm_vehicle_knob("hyundai_kia_ev6.unwind_threshold_increase_right", KIA_EV6_UNWIND_THRESHOLD_INCREASE_RIGHT), + ) * transition_envelope * unwind_weight) return base_threshold * min(max(threshold_scale, 0.82), 1.16) @@ -2114,7 +2313,7 @@ def get_kia_ev6_friction_scale(v_ego: float, desired_lateral_accel: float, desir def get_kia_ev6_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: speed_weight = _kia_ev6_sigmoid((v_ego - KIA_EV6_CENTER_TAPER_SPEED) / KIA_EV6_CENTER_TAPER_SPEED_WIDTH) center_weight = _kia_ev6_sigmoid((KIA_EV6_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / KIA_EV6_CENTER_TAPER_LAT_WIDTH) - reduction = KIA_EV6_CENTER_TAPER_MAX * speed_weight * center_weight + reduction = _ftm_vehicle_knob("hyundai_kia_ev6.center_taper_max", KIA_EV6_CENTER_TAPER_MAX) * speed_weight * center_weight return 1.0 - reduction @@ -2159,6 +2358,388 @@ def get_volt_plexy_center_taper_scale(desired_lateral_accel: float, v_ego: float return 1.0 - (standard_reduction * VOLT_PLEXY_CENTER_TAPER_REDUCTION_MULT) +FTM_UNIVERSAL_PROFILE_KEY = "torque_universal" + +FTM_FULL_SURFACE_SUFFIX_METADATA = { + "ff_gain_left": {"min": 0.0, "max": 0.60, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "ff_gain_right": {"min": 0.0, "max": 0.60, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "turn_in_boost_left": {"min": -0.10, "max": 2.80, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "turn_in_boost_right": {"min": -0.10, "max": 2.80, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "unwind_taper_left": {"min": 0.0, "max": 12.0, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "unwind_taper_right": {"min": 0.0, "max": 12.0, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "center_taper_max": {"min": 0.0, "max": 0.18, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "highway_center_taper_max": {"min": 0.0, "max": 0.18, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "turn_in_threshold_reduction_left": {"min": 0.0, "max": 2.00, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "turn_in_threshold_reduction_right": {"min": 0.0, "max": 2.00, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "unwind_threshold_increase_left": {"min": 0.0, "max": 12.0, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "unwind_threshold_increase_right": {"min": 0.0, "max": 12.0, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "crawl_turn_in_ff_boost_left": {"min": 0.0, "max": 0.50, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "crawl_turn_in_ff_boost_right": {"min": 0.0, "max": 0.50, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "low_speed_angle_assist_max_torque": {"min": 0.0, "max": 0.80, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "curvy_speed_min": {"min": 4.0, "max": 12.0, "precision": 0.1, "deltaType": "absolute", "safeLiveTrial": True}, + "curvy_speed_max": {"min": 14.0, "max": 25.0, "precision": 0.1, "deltaType": "absolute", "safeLiveTrial": True}, + "curvy_turn_in_trim_speed_min": {"min": 8.0, "max": 16.0, "precision": 0.1, "deltaType": "absolute", "safeLiveTrial": True}, + "curvy_turn_in_trim_speed_max": {"min": 14.0, "max": 25.0, "precision": 0.1, "deltaType": "absolute", "safeLiveTrial": True}, + "curvy_turn_in_trim_left": {"min": 0.0, "max": 0.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "curvy_turn_in_trim_right": {"min": 0.0, "max": 0.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "curvy_unwind_floor_relief_left": {"min": 0.0, "max": 0.45, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "curvy_unwind_floor_relief_right": {"min": 0.0, "max": 0.45, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "curvy_unwind_extra_reduction_left": {"min": 0.0, "max": 0.45, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "curvy_unwind_extra_reduction_right": {"min": 0.0, "max": 0.45, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, +} + +FTM_FULL_SURFACE_NEUTRAL_DEFAULTS = { + "ff_gain_left": 0.0, + "ff_gain_right": 0.0, + "turn_in_boost_left": 0.0, + "turn_in_boost_right": 0.0, + "unwind_taper_left": 0.0, + "unwind_taper_right": 0.0, + "center_taper_max": 0.0, + "highway_center_taper_max": 0.0, + "turn_in_threshold_reduction_left": 0.0, + "turn_in_threshold_reduction_right": 0.0, + "unwind_threshold_increase_left": 0.0, + "unwind_threshold_increase_right": 0.0, + "crawl_turn_in_ff_boost_left": 0.0, + "crawl_turn_in_ff_boost_right": 0.0, + "low_speed_angle_assist_max_torque": 0.0, + "curvy_speed_min": IONIQ_6_CURVY_SPEED_MIN, + "curvy_speed_max": IONIQ_6_CURVY_SPEED_MAX, + "curvy_turn_in_trim_speed_min": IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_MIN, + "curvy_turn_in_trim_speed_max": IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_MAX, + "curvy_turn_in_trim_left": 0.0, + "curvy_turn_in_trim_right": 0.0, + "curvy_unwind_floor_relief_left": 0.0, + "curvy_unwind_floor_relief_right": 0.0, + "curvy_unwind_extra_reduction_left": 0.0, + "curvy_unwind_extra_reduction_right": 0.0, +} + + +def _ftm_profile_symbol(profile_key: str, suffix: str) -> str: + return f"{profile_key}.{suffix}" + + +def ftm_profile_supports_knob(profile_key: str | None, suffix: str) -> bool: + if not profile_key: + return False + return _ftm_profile_symbol(profile_key, suffix) in FTM_SUPPORTED_VEHICLE_KNOBS + + +def _ftm_full_surface_side_value(profile_key: str, desired_lateral_accel: float, + suffix: str, left_default: float = 0.0, right_default: float = 0.0) -> float: + if desired_lateral_accel >= 0.0: + return _ftm_vehicle_knob(_ftm_profile_symbol(profile_key, f"{suffix}_left"), left_default) + return _ftm_vehicle_knob(_ftm_profile_symbol(profile_key, f"{suffix}_right"), right_default) + + +def _ftm_full_surface_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / IONIQ_6_TRANSITION_SPEED) ** 2) + + +def _ftm_full_surface_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / IONIQ_6_PHASE_SCALE) + + +def _ftm_full_surface_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / IONIQ_6_FRICTION_LAT_RISE) + jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / IONIQ_6_FRICTION_JERK_RISE) + return _ftm_full_surface_low_speed_factor(v_ego) * lat_factor * jerk_factor + + +def _ftm_full_surface_curvy_speed_weight(profile_key: str, v_ego: float) -> float: + curvy_speed_min = _ftm_vehicle_knob(_ftm_profile_symbol(profile_key, "curvy_speed_min"), IONIQ_6_CURVY_SPEED_MIN) + curvy_speed_max = _ftm_vehicle_knob(_ftm_profile_symbol(profile_key, "curvy_speed_max"), IONIQ_6_CURVY_SPEED_MAX) + onset = _sigmoid((max(v_ego, 0.0) - curvy_speed_min) / IONIQ_6_CURVY_SPEED_MIN_WIDTH) + cutoff = _sigmoid((curvy_speed_max - max(v_ego, 0.0)) / IONIQ_6_CURVY_SPEED_MAX_WIDTH) + return onset * cutoff + + +def _ftm_full_surface_curvy_turn_in_trim_speed_weight(profile_key: str, v_ego: float) -> float: + curvy_turn_in_speed_min = _ftm_vehicle_knob(_ftm_profile_symbol(profile_key, "curvy_turn_in_trim_speed_min"), IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_MIN) + curvy_turn_in_speed_max = _ftm_vehicle_knob(_ftm_profile_symbol(profile_key, "curvy_turn_in_trim_speed_max"), IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_MAX) + onset = _sigmoid((max(v_ego, 0.0) - curvy_turn_in_speed_min) / IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_WIDTH) + cutoff = _sigmoid((curvy_turn_in_speed_max - max(v_ego, 0.0)) / IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_WIDTH) + return onset * cutoff + + +def get_ftm_full_surface_center_taper_scale(profile_key: str | None, desired_lateral_accel: float, v_ego: float, + include_base_center: bool = False) -> float: + if not profile_key: + return 1.0 + + reduction = 0.0 + if include_base_center and ftm_profile_supports_knob(profile_key, "center_taper_max"): + speed_weight = _sigmoid((v_ego - IONIQ_6_CENTER_TAPER_SPEED) / IONIQ_6_CENTER_TAPER_SPEED_WIDTH) + center_weight = _sigmoid((IONIQ_6_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / IONIQ_6_CENTER_TAPER_LAT_WIDTH) + reduction += _ftm_vehicle_knob(_ftm_profile_symbol(profile_key, "center_taper_max"), 0.0) * speed_weight * center_weight + + if ftm_profile_supports_knob(profile_key, "highway_center_taper_max"): + speed_weight = _sigmoid((v_ego - IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED) / IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED_WIDTH) + center_weight = _sigmoid((IONIQ_6_HIGHWAY_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / IONIQ_6_HIGHWAY_CENTER_TAPER_LAT_WIDTH) + reduction += _ftm_vehicle_knob(_ftm_profile_symbol(profile_key, "highway_center_taper_max"), 0.0) * speed_weight * center_weight + + return 1.0 - min(reduction, 0.20) + + +def get_ftm_full_surface_ff_scale(profile_key: str | None, desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float, + include_base_ff: bool = False) -> float: + if not profile_key or desired_lateral_accel == 0.0: + return 1.0 + + abs_lateral_accel = abs(desired_lateral_accel) + phase = _ftm_full_surface_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + low_speed_factor = _ftm_full_surface_low_speed_factor(v_ego) + + curvy_turn_in_speed_weight = _ftm_full_surface_curvy_turn_in_trim_speed_weight(profile_key, v_ego) + curvy_turn_in_lat_onset = _sigmoid((abs_lateral_accel - IONIQ_6_CURVY_TURN_IN_TRIM_LAT_START) / IONIQ_6_CURVY_TURN_IN_TRIM_LAT_ONSET_WIDTH) + curvy_turn_in_lat_cutoff = _sigmoid((IONIQ_6_CURVY_TURN_IN_TRIM_LAT_END - abs_lateral_accel) / IONIQ_6_CURVY_TURN_IN_TRIM_LAT_CUTOFF_WIDTH) + curvy_turn_in_trim_weight = curvy_turn_in_speed_weight * curvy_turn_in_lat_onset * curvy_turn_in_lat_cutoff * turn_in_weight + + curvy_unwind_speed_weight = _ftm_full_surface_curvy_speed_weight(profile_key, v_ego) + curvy_unwind_lat_onset = _sigmoid((abs_lateral_accel - IONIQ_6_CURVY_UNWIND_LAT_START) / IONIQ_6_CURVY_UNWIND_LAT_ONSET_WIDTH) + curvy_unwind_lat_cutoff = _sigmoid((IONIQ_6_CURVY_UNWIND_LAT_END - abs_lateral_accel) / IONIQ_6_CURVY_UNWIND_LAT_CUTOFF_WIDTH) + curvy_unwind_weight = curvy_unwind_speed_weight * curvy_unwind_lat_onset * curvy_unwind_lat_cutoff * unwind_weight + + scale = 1.0 + if include_base_ff and ftm_profile_supports_knob(profile_key, "ff_gain_left"): + gain = _ftm_full_surface_side_value(profile_key, desired_lateral_accel, "ff_gain") + onset = _sigmoid((abs_lateral_accel - IONIQ_6_FF_ONSET) / IONIQ_6_FF_ONSET_WIDTH) + cutoff = _sigmoid((IONIQ_6_FF_CUTOFF - abs_lateral_accel) / IONIQ_6_FF_CUTOFF_WIDTH) + extra_scale = gain * onset * cutoff + turn_in_boost = 1.0 + (_ftm_full_surface_side_value(profile_key, desired_lateral_accel, "turn_in_boost") * turn_in_weight * low_speed_factor) + unwind_reduction = _ftm_full_surface_side_value(profile_key, desired_lateral_accel, "unwind_taper") * unwind_weight * (0.30 + 0.70 * low_speed_factor) + curvy_unwind_extra = _ftm_full_surface_side_value(profile_key, desired_lateral_accel, "curvy_unwind_extra_reduction") * curvy_unwind_weight + curvy_unwind_floor_relief = _ftm_full_surface_side_value(profile_key, desired_lateral_accel, "curvy_unwind_floor_relief") * curvy_unwind_weight + unwind_floor = 1.0 - (0.55 * unwind_reduction) - curvy_unwind_floor_relief + unwind_scale = max(1.0 - unwind_reduction - curvy_unwind_extra, unwind_floor, 0.0) + scale *= 1.0 + (extra_scale * turn_in_boost * unwind_scale) + else: + curvy_unwind_extra = _ftm_full_surface_side_value(profile_key, desired_lateral_accel, "curvy_unwind_extra_reduction") * curvy_unwind_weight + curvy_unwind_floor_relief = _ftm_full_surface_side_value(profile_key, desired_lateral_accel, "curvy_unwind_floor_relief") * curvy_unwind_weight + scale *= max(1.0 - curvy_unwind_extra - curvy_unwind_floor_relief, 0.55) + + if ftm_profile_supports_knob(profile_key, "curvy_turn_in_trim_left"): + curvy_trim = _ftm_full_surface_side_value(profile_key, desired_lateral_accel, "curvy_turn_in_trim") * curvy_turn_in_trim_weight + scale *= max(1.0 - curvy_trim, 0.55) + + if ftm_profile_supports_knob(profile_key, "crawl_turn_in_ff_boost_left") and desired_lateral_accel * desired_lateral_jerk > 0.0: + crawl_speed_weight = _sigmoid((IONIQ_6_CRAWL_TURN_IN_FF_SPEED - max(v_ego, 0.0)) / IONIQ_6_CRAWL_TURN_IN_FF_SPEED_WIDTH) + crawl_lat_weight = _sigmoid((abs_lateral_accel - IONIQ_6_CRAWL_TURN_IN_FF_LAT) / IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH) + scale += _ftm_full_surface_side_value(profile_key, desired_lateral_accel, "crawl_turn_in_ff_boost") * crawl_speed_weight * crawl_lat_weight + + return scale + + +def get_ftm_full_surface_friction_threshold(profile_key: str | None, base_threshold: float, v_ego: float, + desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0, + include_base_threshold: bool = False) -> float: + if not profile_key or not include_base_threshold: + return base_threshold + + transition_envelope = _ftm_full_surface_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _ftm_full_surface_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + unwind_speed_weight = _sigmoid((v_ego - IONIQ_6_UNWIND_HIGH_SPEED_SPEED) / IONIQ_6_UNWIND_HIGH_SPEED_SPEED_WIDTH) + threshold_scale = 1.0 + threshold_scale -= (_ftm_full_surface_side_value(profile_key, desired_lateral_accel, "turn_in_threshold_reduction") * + transition_envelope * turn_in_weight) + threshold_scale += (_ftm_full_surface_side_value(profile_key, desired_lateral_accel, "unwind_threshold_increase") * + transition_envelope * unwind_weight * unwind_speed_weight) + return base_threshold * min(max(threshold_scale, 0.82), 1.18) + + +def get_ftm_full_surface_low_speed_angle_assist_torque(profile_key: str | None, desired_angle_deg: float, actual_angle_deg: float, + current_output_torque: float, v_ego: float) -> float: + if not profile_key or not ftm_profile_supports_knob(profile_key, "low_speed_angle_assist_max_torque"): + return current_output_torque + + max_torque = _ftm_vehicle_knob(_ftm_profile_symbol(profile_key, "low_speed_angle_assist_max_torque"), 0.0) + if max_torque <= 1e-4: + return current_output_torque + + angle_error = desired_angle_deg - actual_angle_deg + if desired_angle_deg * angle_error > 0.0: + speed_weight = _sigmoid((IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED - max(v_ego, 0.0)) / IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED_WIDTH) + error_weight = _sigmoid((abs(angle_error) - IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR) / IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR_WIDTH) + desired_angle_weight = _sigmoid((abs(desired_angle_deg) - IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE) / IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE_WIDTH) + tracking_ratio = abs(actual_angle_deg) / max(abs(desired_angle_deg), 1e-3) + tracking_taper = _sigmoid((tracking_ratio - IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_START) / IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_WIDTH) + tracking_scale = max(1.0 - tracking_taper, IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_FLOOR) + assist_torque = math.copysign(max_torque * speed_weight * error_weight * desired_angle_weight * tracking_scale, -angle_error) + if abs(assist_torque) < 1e-4: + return current_output_torque + if current_output_torque * assist_torque >= 0.0: + add_scale = float(np.interp(abs(current_output_torque), IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_BP, IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_V)) + return float(np.clip(current_output_torque + (assist_torque * add_scale), -1.0, 1.0)) + return float(np.clip(current_output_torque + assist_torque, -1.0, 1.0)) + + speed_weight = _sigmoid((IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED - max(v_ego, 0.0)) / IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED_WIDTH) + error_weight = _sigmoid((abs(angle_error) - IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR) / IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR_WIDTH) + actual_angle_weight = _sigmoid((abs(actual_angle_deg) - IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ACTUAL_ANGLE) / IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ACTUAL_ANGLE_WIDTH) + assist_torque = math.copysign(IONIQ_6_LOW_SPEED_UNWIND_ASSIST_MAX_TORQUE * speed_weight * error_weight * actual_angle_weight, -angle_error) + if abs(assist_torque) < 1e-4: + return current_output_torque + if current_output_torque * assist_torque >= 0.0: + assist_torque *= IONIQ_6_LOW_SPEED_UNWIND_ASSIST_BLEND + return float(np.clip(current_output_torque + assist_torque, -1.0, 1.0)) + + +FTM_RICH_PROFILE_CARS = { + "gm_bolt_2022_2023": set(BOLT_2022_2023_CARS), + "hyundai_ioniq_6": set(IONIQ_6_CARS), + "hyundai_kia_ev6": set(KIA_EV6_CARS), + "toyota_prius": set(PRIUS_CARS), +} + +FTM_RICH_PROFILE_LABELS = { + "gm_bolt_2022_2023": "Bolt 2022-2023", + "hyundai_ioniq_6": "Ioniq 6", + "hyundai_kia_ev6": "EV6", + "toyota_prius": "Prius", + FTM_UNIVERSAL_PROFILE_KEY: "Torque Controller", +} + +FTM_SUPPORTED_VEHICLE_KNOBS = { + "gm_bolt_2022_2023.ff_gain_left": {"profile": "gm_bolt_2022_2023", "min": 0.0, "max": 0.40, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": BOLT_2022_2023_FF_GAIN_LEFT}, + "gm_bolt_2022_2023.ff_gain_right": {"profile": "gm_bolt_2022_2023", "min": 0.0, "max": 0.40, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": BOLT_2022_2023_FF_GAIN_RIGHT}, + "gm_bolt_2022_2023.turn_in_boost_left": {"profile": "gm_bolt_2022_2023", "min": -0.10, "max": 0.50, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": BOLT_2022_2023_TURN_IN_BOOST_LEFT}, + "gm_bolt_2022_2023.turn_in_boost_right": {"profile": "gm_bolt_2022_2023", "min": -0.10, "max": 0.50, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": BOLT_2022_2023_TURN_IN_BOOST_RIGHT}, + "gm_bolt_2022_2023.unwind_taper_left": {"profile": "gm_bolt_2022_2023", "min": 0.0, "max": 0.80, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": BOLT_2022_2023_UNWIND_TAPER_LEFT}, + "gm_bolt_2022_2023.unwind_taper_right": {"profile": "gm_bolt_2022_2023", "min": 0.0, "max": 0.80, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": BOLT_2022_2023_UNWIND_TAPER_RIGHT}, + "gm_bolt_2022_2023.center_taper_max": {"profile": "gm_bolt_2022_2023", "min": 0.0, "max": 0.25, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": BOLT_2022_2023_CENTER_TAPER_MAX}, + "gm_bolt_2022_2023.turn_in_threshold_reduction_left": {"profile": "gm_bolt_2022_2023", "min": 0.0, "max": 0.40, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_LEFT}, + "gm_bolt_2022_2023.turn_in_threshold_reduction_right": {"profile": "gm_bolt_2022_2023", "min": 0.0, "max": 0.40, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_RIGHT}, + "gm_bolt_2022_2023.unwind_threshold_increase_left": {"profile": "gm_bolt_2022_2023", "min": 0.0, "max": 0.60, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_LEFT}, + "gm_bolt_2022_2023.unwind_threshold_increase_right": {"profile": "gm_bolt_2022_2023", "min": 0.0, "max": 0.60, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_RIGHT}, + "hyundai_ioniq_6.ff_gain_left": {"profile": "hyundai_ioniq_6", "min": 0.0, "max": 0.60, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_FF_GAIN_LEFT}, + "hyundai_ioniq_6.ff_gain_right": {"profile": "hyundai_ioniq_6", "min": 0.0, "max": 0.60, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_FF_GAIN_RIGHT}, + "hyundai_ioniq_6.turn_in_boost_left": {"profile": "hyundai_ioniq_6", "min": 0.40, "max": 2.80, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_TURN_IN_BOOST_LEFT}, + "hyundai_ioniq_6.turn_in_boost_right": {"profile": "hyundai_ioniq_6", "min": 0.40, "max": 2.80, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_TURN_IN_BOOST_RIGHT}, + "hyundai_ioniq_6.unwind_taper_left": {"profile": "hyundai_ioniq_6", "min": 0.0, "max": 12.0, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_UNWIND_TAPER_LEFT}, + "hyundai_ioniq_6.unwind_taper_right": {"profile": "hyundai_ioniq_6", "min": 0.0, "max": 12.0, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_UNWIND_TAPER_RIGHT}, + "hyundai_ioniq_6.center_taper_max": {"profile": "hyundai_ioniq_6", "min": 0.0, "max": 0.18, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_CENTER_TAPER_MAX}, + "hyundai_ioniq_6.highway_center_taper_max": {"profile": "hyundai_ioniq_6", "min": 0.0, "max": 0.18, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_HIGHWAY_CENTER_TAPER_MAX}, + "hyundai_ioniq_6.turn_in_threshold_reduction_left": {"profile": "hyundai_ioniq_6", "min": 0.0, "max": 2.00, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT}, + "hyundai_ioniq_6.turn_in_threshold_reduction_right": {"profile": "hyundai_ioniq_6", "min": 0.0, "max": 2.00, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT}, + "hyundai_ioniq_6.unwind_threshold_increase_left": {"profile": "hyundai_ioniq_6", "min": 0.0, "max": 12.0, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT}, + "hyundai_ioniq_6.unwind_threshold_increase_right": {"profile": "hyundai_ioniq_6", "min": 0.0, "max": 12.0, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT}, + "hyundai_ioniq_6.crawl_turn_in_ff_boost_left": {"profile": "hyundai_ioniq_6", "min": 0.0, "max": 0.50, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_CRAWL_TURN_IN_FF_BOOST_LEFT}, + "hyundai_ioniq_6.crawl_turn_in_ff_boost_right": {"profile": "hyundai_ioniq_6", "min": 0.0, "max": 0.50, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_CRAWL_TURN_IN_FF_BOOST_RIGHT}, + "hyundai_ioniq_6.low_speed_angle_assist_max_torque": {"profile": "hyundai_ioniq_6", "min": 0.0, "max": 0.80, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_LOW_SPEED_ANGLE_ASSIST_MAX_TORQUE}, + "hyundai_ioniq_6.curvy_speed_min": {"profile": "hyundai_ioniq_6", "min": 4.0, "max": 12.0, "precision": 0.1, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_CURVY_SPEED_MIN}, + "hyundai_ioniq_6.curvy_speed_max": {"profile": "hyundai_ioniq_6", "min": 14.0, "max": 25.0, "precision": 0.1, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_CURVY_SPEED_MAX}, + "hyundai_ioniq_6.curvy_turn_in_trim_speed_min": {"profile": "hyundai_ioniq_6", "min": 8.0, "max": 16.0, "precision": 0.1, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_MIN}, + "hyundai_ioniq_6.curvy_turn_in_trim_speed_max": {"profile": "hyundai_ioniq_6", "min": 14.0, "max": 25.0, "precision": 0.1, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_MAX}, + "hyundai_ioniq_6.curvy_turn_in_trim_left": {"profile": "hyundai_ioniq_6", "min": 0.0, "max": 0.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_CURVY_TURN_IN_TRIM_LEFT}, + "hyundai_ioniq_6.curvy_turn_in_trim_right": {"profile": "hyundai_ioniq_6", "min": 0.0, "max": 0.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_CURVY_TURN_IN_TRIM_RIGHT}, + "hyundai_ioniq_6.curvy_unwind_floor_relief_left": {"profile": "hyundai_ioniq_6", "min": 0.0, "max": 0.45, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_LEFT}, + "hyundai_ioniq_6.curvy_unwind_floor_relief_right": {"profile": "hyundai_ioniq_6", "min": 0.0, "max": 0.45, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_RIGHT}, + "hyundai_ioniq_6.curvy_unwind_extra_reduction_left": {"profile": "hyundai_ioniq_6", "min": 0.0, "max": 0.45, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_LEFT}, + "hyundai_ioniq_6.curvy_unwind_extra_reduction_right": {"profile": "hyundai_ioniq_6", "min": 0.0, "max": 0.45, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_RIGHT}, + "hyundai_kia_ev6.ff_gain_left": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 0.60, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_FF_GAIN_LEFT}, + "hyundai_kia_ev6.ff_gain_right": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 0.60, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_FF_GAIN_RIGHT}, + "hyundai_kia_ev6.turn_in_boost_left": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 1.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_TURN_IN_BOOST_LEFT}, + "hyundai_kia_ev6.turn_in_boost_right": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 1.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_TURN_IN_BOOST_RIGHT}, + "hyundai_kia_ev6.unwind_taper_left": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 1.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_UNWIND_TAPER_LEFT}, + "hyundai_kia_ev6.unwind_taper_right": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 1.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_UNWIND_TAPER_RIGHT}, + "hyundai_kia_ev6.center_taper_max": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 0.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_CENTER_TAPER_MAX}, + "hyundai_kia_ev6.turn_in_threshold_reduction_left": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 0.40, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_LEFT}, + "hyundai_kia_ev6.turn_in_threshold_reduction_right": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 0.40, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_RIGHT}, + "hyundai_kia_ev6.unwind_threshold_increase_left": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 0.80, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_UNWIND_THRESHOLD_INCREASE_LEFT}, + "hyundai_kia_ev6.unwind_threshold_increase_right": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 0.80, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_UNWIND_THRESHOLD_INCREASE_RIGHT}, + "toyota_prius.ff_gain_left": {"profile": "toyota_prius", "min": 0.0, "max": 0.25, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": PRIUS_FF_GAIN_LEFT}, + "toyota_prius.ff_gain_right": {"profile": "toyota_prius", "min": 0.0, "max": 0.25, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": PRIUS_FF_GAIN_RIGHT}, + "toyota_prius.turn_in_boost_left": {"profile": "toyota_prius", "min": -0.10, "max": 0.60, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": PRIUS_TURN_IN_BOOST_LEFT}, + "toyota_prius.turn_in_boost_right": {"profile": "toyota_prius", "min": -0.10, "max": 0.60, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": PRIUS_TURN_IN_BOOST_RIGHT}, + "toyota_prius.unwind_taper_left": {"profile": "toyota_prius", "min": 0.0, "max": 1.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": PRIUS_UNWIND_TAPER_LEFT}, + "toyota_prius.unwind_taper_right": {"profile": "toyota_prius", "min": 0.0, "max": 1.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": PRIUS_UNWIND_TAPER_RIGHT}, + "toyota_prius.center_taper_max": {"profile": "toyota_prius", "min": 0.0, "max": 0.25, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": PRIUS_CENTER_TAPER_MAX}, + "toyota_prius.turn_in_threshold_reduction_left": {"profile": "toyota_prius", "min": 0.0, "max": 0.50, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": PRIUS_TURN_IN_THRESHOLD_REDUCTION_LEFT}, + "toyota_prius.turn_in_threshold_reduction_right": {"profile": "toyota_prius", "min": 0.0, "max": 0.50, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": PRIUS_TURN_IN_THRESHOLD_REDUCTION_RIGHT}, + "toyota_prius.unwind_threshold_increase_left": {"profile": "toyota_prius", "min": 0.0, "max": 0.90, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": PRIUS_UNWIND_THRESHOLD_INCREASE_LEFT}, + "toyota_prius.unwind_threshold_increase_right": {"profile": "toyota_prius", "min": 0.0, "max": 0.90, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": PRIUS_UNWIND_THRESHOLD_INCREASE_RIGHT}, +} + + +def _add_ftm_full_surface_profile_knobs(profile_key: str, defaults: dict[str, float] | None = None) -> None: + knob_defaults = dict(FTM_FULL_SURFACE_NEUTRAL_DEFAULTS) + if defaults: + knob_defaults.update(defaults) + + for suffix, meta in FTM_FULL_SURFACE_SUFFIX_METADATA.items(): + symbol = _ftm_profile_symbol(profile_key, suffix) + if symbol in FTM_SUPPORTED_VEHICLE_KNOBS: + continue + FTM_SUPPORTED_VEHICLE_KNOBS[symbol] = { + "profile": profile_key, + "min": meta["min"], + "max": meta["max"], + "precision": meta["precision"], + "deltaType": meta["deltaType"], + "safeLiveTrial": meta["safeLiveTrial"], + "defaultValue": knob_defaults[suffix], + } + + +for _ftm_profile_key in ("gm_bolt_2022_2023", "hyundai_ioniq_6", "hyundai_kia_ev6", "toyota_prius", FTM_UNIVERSAL_PROFILE_KEY): + _add_ftm_full_surface_profile_knobs(_ftm_profile_key) + + +def get_ftm_supported_vehicle_knobs() -> dict: + return _ftm_copy_json(FTM_SUPPORTED_VEHICLE_KNOBS) + + +def get_ftm_rich_profile_key(car_fingerprint) -> str | None: + for profile_key, cars in FTM_RICH_PROFILE_CARS.items(): + if car_fingerprint in cars: + return profile_key + return None + + +def get_ftm_surface_profile_key(car_fingerprint, torque_control: bool = True) -> str | None: + profile_key = get_ftm_rich_profile_key(car_fingerprint) + if profile_key is not None or not torque_control: + return profile_key + return FTM_UNIVERSAL_PROFILE_KEY + + +def get_ftm_capabilities(car_fingerprint, brand: str = "", hyundai_canfd: bool = False, torque_control: bool = True) -> dict: + profile_key = get_ftm_surface_profile_key(car_fingerprint, torque_control=torque_control) + if hyundai_canfd: + friction_family = "hkg_canfd" + elif brand == "gm": + friction_family = "gm" + else: + friction_family = "standard" + + dedicated_friction = car_fingerprint in ( + set(BOLT_2022_2023_CARS) | set(BOLT_2018_2021_CARS) | set(VOLT_STANDARD_CARS) | set(PALISADE_CARS) | + set(PRIUS_CARS) | set(IONIQ_5_CARS) | set(IONIQ_6_CARS) | set(KIA_EV6_CARS) | set(KIA_FORTE_CARS) | + set(KIA_NIRO_PHEV_2022_CARS) | set(GENESIS_G90_CARS) + ) + dedicated_center_taper = car_fingerprint in ( + set(PRIUS_CARS) | set(BOLT_CARS) | set(VOLT_STANDARD_CARS) | set(IONIQ_5_CARS) | + set(IONIQ_EV_OLD_CARS) | set(IONIQ_6_CARS) | set(SONATA_CARS) | set(SONATA_HYBRID_CARS) | + set(KIA_XCEED_CARS) | set(KIA_NIRO_PHEV_2022_CARS) | set(KIA_FORTE_CARS) | set(KIA_EV6_CARS) | + set(SILVERADO_CARS) + ) + rich_knobs = [name for name, meta in FTM_SUPPORTED_VEHICLE_KNOBS.items() if meta["profile"] == profile_key] + return { + "torqueControl": bool(torque_control), + "frictionFamily": friction_family, + "hasDedicatedFrictionThreshold": bool(dedicated_friction), + "hasDedicatedCenterTaper": bool(dedicated_center_taper), + "richProfileKey": profile_key, + "richProfileLabel": FTM_RICH_PROFILE_LABELS.get(profile_key), + "richKnobs": rich_knobs, + } + + __all__ = [name for name in globals() if not name.startswith("_") and name not in { "math", "np", "GM_CAR", "HYUNDAI_CAR", "TOYOTA_CAR", "CV", "testing_ground", }] diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 8fb48421d..274627f0d 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -19,7 +19,13 @@ from openpilot.selfdrive.controls.lib.latcontrol_pid import ( get_civic_bosch_modified_pid_output_alpha, get_civic_bosch_modified_pid_output_scale, ) -from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import get_hkg_canfd_base_friction_threshold +from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import ( + clear_ftm_runtime_overrides, + get_ftm_runtime_overrides, + get_hkg_canfd_base_friction_threshold, + normalize_ftm_overrides, + set_ftm_runtime_overrides, +) from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_civic_bosch_modified_a_center_taper_scale, LatControlTorque, @@ -258,6 +264,42 @@ class TestLatControl: assert unwind_left < steady_left assert unwind_right < steady_right + def test_ftm_standard_friction_curve_override(self): + base = get_standard_friction_threshold(10.0) + overrides = normalize_ftm_overrides({ + "baseFrictionThresholds": { + "standard": { + "values": [0.30, 0.31, 0.32, 0.33, 0.34], + }, + }, + }) + try: + set_ftm_runtime_overrides(overrides) + assert get_ftm_runtime_overrides()["baseFrictionThresholds"]["standard"]["values"] == [0.30, 0.31, 0.32, 0.33, 0.34] + assert get_standard_friction_threshold(10.0) == pytest.approx(0.32) + assert get_standard_friction_threshold(12.5) > get_standard_friction_threshold(10.0) + assert get_standard_friction_threshold(10.0) != pytest.approx(base) + finally: + clear_ftm_runtime_overrides() + assert get_ftm_runtime_overrides() == {} + assert get_standard_friction_threshold(10.0) == pytest.approx(base) + + def test_ftm_vehicle_knob_override_ioniq6_center_taper(self): + baseline = get_ioniq_6_center_taper_scale(0.0, 32.0) + overrides = normalize_ftm_overrides({ + "vehicleKnobs": { + "hyundai_ioniq_6.center_taper_max": 0.0, + "hyundai_ioniq_6.highway_center_taper_max": 0.0, + }, + }) + try: + set_ftm_runtime_overrides(overrides) + adjusted = get_ioniq_6_center_taper_scale(0.0, 32.0) + assert adjusted > baseline + assert adjusted <= 1.0 + finally: + clear_ftm_runtime_overrides() + def test_sonata_hybrid_center_taper_curve(self): assert get_sonata_hybrid_center_taper_scale(0.0, 30.0) < get_sonata_hybrid_center_taper_scale(0.0, 15.0) assert get_sonata_hybrid_center_taper_scale(0.0, 3.0) < get_sonata_hybrid_center_taper_scale(0.0, 10.0) diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index fb91f55dc..e9b41fdc5 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -229,6 +229,9 @@ EXCLUDED_KEYS = { "CommunityFavorites", "CurvatureData", "ExperimentalLongitudinalEnabled", + "FTMActiveOverrides", + "FTMActiveProfileId", + "FTMTrialApplied", "InstallDate", "StarPilotCarParamsPersistent", "KonikMinutes", @@ -677,6 +680,13 @@ class StarPilotVariables: advanced_lateral_tuning = self.get_value("AdvancedLateralTune") toggle.force_auto_tune = self.get_value("ForceAutoTune", condition=advanced_lateral_tuning and not has_auto_tune and is_torque_car and not is_angle_car) toggle.force_auto_tune_off = self.get_value("ForceAutoTuneOff", condition=advanced_lateral_tuning and has_auto_tune and is_torque_car and not is_angle_car) + toggle.ftm_active_profile_id = self.params.get("FTMActiveProfileId", encoding="utf-8") or "" + toggle.ftm_trial_applied = self.params.get_bool("FTMTrialApplied") + ftm_overrides_raw = self.params.get("FTMActiveOverrides", encoding="utf-8") or "" + try: + toggle.ftm_active_overrides = json.loads(ftm_overrides_raw) if ftm_overrides_raw else {} + except Exception: + toggle.ftm_active_overrides = {} toggle.steerActuatorDelay = self.get_value("SteerDelay", cast=float, condition=advanced_lateral_tuning, default=steerActuatorDelay, min=0.01, max=1.0) toggle.use_custom_steerActuatorDelay = bool(round(toggle.steerActuatorDelay, 2) != round(steerActuatorDelay, 2)) toggle.friction = self.get_value("SteerFriction", cast=float, condition=advanced_lateral_tuning, default=friction, min=0, max=1) diff --git a/starpilot/system/the_galaxy/assets/components/router.js b/starpilot/system/the_galaxy/assets/components/router.js index ad6bad678..978a2c976 100644 --- a/starpilot/system/the_galaxy/assets/components/router.js +++ b/starpilot/system/the_galaxy/assets/components/router.js @@ -6,7 +6,6 @@ import { ErrorLogs } from "/assets/components/tools/error_logs.js" import { VehicleFeatures } from "/assets/components/tools/vehicle_features.js" import { GalaxyPairing } from "/assets/components/tools/galaxy.js" import { Home } from "/assets/components/home/home.js" -import { LateralManeuvers } from "/assets/components/tools/lateral_maneuvers.js" import { LongitudinalManeuvers } from "/assets/components/tools/longitudinal_maneuvers.js" import { MapsManager } from "/assets/components/tools/maps.js" import { NavDestination } from "/assets/components/navigation/navigation_destination.js?v=nav-search-context-1" @@ -14,12 +13,13 @@ import { NavKeys } from "/assets/components/navigation/navigation_keys.js?v=app- import { RouteRecordings } from "/assets/components/recordings/dashcam_routes.js" import { SettingsView } from "/assets/components/settings.js" import { ScreenRecordings } from "/assets/components/recordings/screen_recordings.js" -import { Sidebar } from "/assets/components/sidebar.js?v=app-keys-session-1" +import { Sidebar } from "/assets/components/sidebar.js?v=lateral-tuning-1" import { SpeedLimits } from "/assets/components/tools/speed_limits.js" import { ModelManager } from "/assets/components/tools/model_manager.js?v=20260303t" import { LivePlots } from "/assets/components/tools/plots.js" import { ThemeMaker } from "/assets/components/tools/theme_maker.js" import { TestingGround } from "/assets/components/tools/testing_ground.js" +import { Tuning } from "/assets/components/tools/tuning.js?v=ftm-workspace-4" import { Troubleshoot } from "/assets/components/tools/troubleshoot.js" import { TmuxLog } from "/assets/components/tools/tmux.js" import { ToggleControl } from "/assets/components/tools/toggles.js" @@ -72,7 +72,8 @@ function Root() { createRoute("settings", "/settings/:section/:subsection?", SettingsView), createRoute("speed_limits", "/download_speed_limits", SpeedLimits), createRoute("model_manager", "/manage_models", ModelManager), - createRoute("lateral_maneuvers", "/lateral_maneuvers", LateralManeuvers), + createRoute("tuning", "/tuning", Tuning), + createRoute("lateral_maneuvers", "/lateral_maneuvers", Tuning), createRoute("longitudinal_maneuvers", "/longitudinal_maneuvers", LongitudinalManeuvers), createRoute("maps", "/manage_maps", MapsManager), createRoute("plots", "/plots", LivePlots), diff --git a/starpilot/system/the_galaxy/assets/components/sidebar.js b/starpilot/system/the_galaxy/assets/components/sidebar.js index 438f87ee5..4e1339a20 100644 --- a/starpilot/system/the_galaxy/assets/components/sidebar.js +++ b/starpilot/system/the_galaxy/assets/components/sidebar.js @@ -14,7 +14,7 @@ const MENU_ITEMS = { { name: "Download Speed Limits", link: "/download_speed_limits", icon: "bi-download" }, { name: "Error Logs", link: "/manage_error_logs", icon: "bi-exclamation-triangle" }, { name: "Galaxy", link: "/galaxy", icon: "bi-globe2" }, - { name: "Lateral Maneuvers", link: "/lateral_maneuvers", icon: "bi-sign-turn-right" }, + { name: "Lateral Tuning", link: "/tuning", icon: "bi-sign-turn-right" }, { name: "Long Maneuvers", link: "/longitudinal_maneuvers", icon: "bi-signpost-split" }, { name: "Maps", link: "/manage_maps", icon: "bi-map" }, { name: "Navigation", link: "/set_navigation_destination", icon: "bi-geo-alt-fill" }, @@ -33,6 +33,7 @@ const MENU_ITEMS = { function matchesPath(currentPath, link) { if (link === "/") return currentPath === "/"; + if (link === "/tuning" && currentPath === "/lateral_maneuvers") return true; return currentPath === link || currentPath.startsWith(`${link}/`); } diff --git a/starpilot/system/the_galaxy/assets/components/tools/tuning.css b/starpilot/system/the_galaxy/assets/components/tools/tuning.css new file mode 100644 index 000000000..fcbd82d6c --- /dev/null +++ b/starpilot/system/the_galaxy/assets/components/tools/tuning.css @@ -0,0 +1,143 @@ +.ftmTwoColumn { + display: grid; + gap: var(--gap-base); + grid-template-columns: repeat(2, minmax(0, 1fr)); + margin-top: var(--margin-base); +} + +.ftmCard { + background: rgba(255, 255, 255, 0.02); + border: 1px solid rgba(255, 255, 255, 0.08); + border-radius: var(--border-radius-md); + margin-top: var(--margin-base); + padding: var(--padding-base); +} + +.ftmCardHeader { + align-items: flex-start; + display: flex; + gap: var(--gap-sm); + justify-content: space-between; +} + +.ftmCardHeader h3, +.ftmCardHeader h4, +.ftmCardHeader h5 { + margin: 0; +} + +.ftmCardSubsection { + margin-top: var(--margin-base); +} + +.ftmRouteList, +.ftmWorkspaceList, +.ftmFindings { + display: flex; + flex-direction: column; + gap: var(--gap-sm); + margin-top: var(--margin-base); +} + +.ftmWorkspaceRow { + display: flex; + gap: var(--gap-sm); +} + +.ftmRouteList { + max-height: 28rem; + overflow: auto; + padding-right: var(--padding-xs); +} + +.ftmRouteItem, +.ftmWorkspaceItem { + align-items: flex-start; + background: rgba(255, 255, 255, 0.03); + border: 1px solid rgba(255, 255, 255, 0.06); + border-radius: var(--border-radius-sm); + color: var(--text-color); + cursor: pointer; + display: flex; + gap: var(--gap-sm); + padding: var(--padding-sm); + text-align: left; +} + +.ftmRouteItem input { + margin-top: 0.2rem; +} + +.ftmRouteItem span, +.ftmWorkspaceItem { + display: flex; + flex-direction: column; +} + +.ftmWorkspaceItem { + flex: 1 1 auto; +} + +.ftmWorkspaceDelete { + align-self: stretch; + flex: 0 0 auto; +} + +.ftmRouteItem small, +.ftmWorkspaceItem small, +.ftmWorkspaceItem span { + color: var(--text-muted); +} + +.ftmWorkspaceItem:hover, +.ftmRouteItem:hover, +.ftmCard button.selected { + border-color: var(--main-fg); +} + +.ftmProfileGrid { + display: grid; + gap: var(--gap-base); + grid-template-columns: repeat(2, minmax(0, 1fr)); +} + +.ftmFeedbackButtons { + display: flex; + flex-wrap: wrap; + gap: var(--gap-xs); +} + +.ftmDeltaBox { + background: rgba(255, 255, 255, 0.03); + border-radius: var(--border-radius-sm); + margin-top: var(--margin-sm); + padding: var(--padding-sm); +} + +.ftmNotes { + background: var(--secondary-bg); + border: 1px solid rgba(255, 255, 255, 0.08); + border-radius: var(--border-radius-sm); + color: var(--text-color); + margin-top: var(--margin-base); + min-height: 5.5rem; + padding: var(--padding-sm); + resize: vertical; + width: 100%; +} + +.ftmPlotWrap { + margin-top: var(--margin-base); +} + +.ftmPlotWrap svg { + height: 140px; + width: 100%; +} + +@media only screen and (max-width: 900px) { + .ftmTwoColumn, + .ftmProfileGrid { + grid-template-columns: 1fr; + } +} diff --git a/starpilot/system/the_galaxy/assets/components/tools/tuning.js b/starpilot/system/the_galaxy/assets/components/tools/tuning.js new file mode 100644 index 000000000..745f15652 --- /dev/null +++ b/starpilot/system/the_galaxy/assets/components/tools/tuning.js @@ -0,0 +1,846 @@ +import { html, reactive } from "/assets/vendor/arrow-core.js" + +const MAX_RENDERED_ROUTES = 250 +const ROUTE_FLUSH_INTERVAL_MS = 120 +const STATUS_POLL_MS = 3000 + +const state = reactive({ + loadingRoutes: true, + loadingWorkspace: true, + runningAction: false, + error: "", + routes: [], + selectedRoutes: [], + truncatedRoutes: false, + routeProgress: 0, + routeTotal: 0, + workspace: { reports: [], activeTrial: null, status: {} }, + status: {}, + report: null, + feedbackAccepted: [], + feedbackIgnored: [], + feedbackNotes: "", +}) + +let routesAbortController = null +let routesRequestToken = 0 +let pendingRoutes = [] +let flushTimerId = null +let seenRouteNames = new Set() +let initialized = false +let statusPollHandle = null + +function isTuningRouteActive() { + return window.location.pathname === "/tuning" || window.location.pathname === "/lateral_maneuvers" +} + +function formatTimestamp(value) { + if (!value) return "Unknown Route" + const parsed = new Date(value) + if (Number.isNaN(parsed.getTime())) return String(value) + return parsed.toLocaleString() +} + +function safeCount(value) { + const n = Number(value) + return Number.isFinite(n) ? n : 0 +} + +function formatStatusAge(updatedAt) { + const updated = Number(updatedAt) + if (!Number.isFinite(updated) || updated <= 0) return "unknown" + const ageSec = Math.max(0, Math.round(Date.now() / 1000 - updated)) + if (ageSec < 5) return "just now" + if (ageSec < 60) return `${ageSec}s ago` + if (ageSec < 3600) return `${Math.round(ageSec / 60)}m ago` + return `${Math.round(ageSec / 3600)}h ago` +} + +function resetRouteStreamState() { + pendingRoutes = [] + seenRouteNames = new Set() + if (flushTimerId !== null) { + clearTimeout(flushTimerId) + flushTimerId = null + } +} + +function sortedRoutes() { + return [...state.routes].sort((a, b) => { + const aTime = Date.parse(a.timestamp) + const bTime = Date.parse(b.timestamp) + if (Number.isFinite(aTime) && Number.isFinite(bTime)) return bTime - aTime + return String(b.timestamp || "").localeCompare(String(a.timestamp || "")) + }) +} + +function flushPendingRoutes() { + if (!pendingRoutes.length) return + + const availableSlots = Math.max(MAX_RENDERED_ROUTES - state.routes.length, 0) + if (availableSlots <= 0) { + pendingRoutes = [] + state.truncatedRoutes = true + return + } + + const toAppend = pendingRoutes.slice(0, availableSlots) + pendingRoutes = [] + if (toAppend.length > 0) { + state.routes = [...state.routes, ...toAppend] + } + if (state.routes.length >= MAX_RENDERED_ROUTES) { + state.truncatedRoutes = true + } +} + +function enqueueRoutes(rawRoutes) { + if (!Array.isArray(rawRoutes) || rawRoutes.length === 0) return + const nextRoutes = [] + for (const route of rawRoutes) { + const name = String(route?.name || "") + if (!name || seenRouteNames.has(name)) continue + seenRouteNames.add(name) + nextRoutes.push({ + ...route, + timestampLabel: formatTimestamp(route.timestamp), + }) + } + if (!nextRoutes.length) return + + pendingRoutes.push(...nextRoutes) + if (flushTimerId === null) { + flushTimerId = setTimeout(() => { + flushTimerId = null + flushPendingRoutes() + }, ROUTE_FLUSH_INTERVAL_MS) + } +} + +async function fetchRoutes() { + const requestToken = ++routesRequestToken + if (routesAbortController) routesAbortController.abort() + const controller = new AbortController() + routesAbortController = controller + + try { + state.loadingRoutes = true + state.error = "" + const userTimezone = Intl.DateTimeFormat().resolvedOptions().timeZone + const response = await fetch(`/api/routes?timezone=${encodeURIComponent(userTimezone)}`, { signal: controller.signal }) + if (!response.ok) throw new Error("Failed to load local routes.") + + const reader = response.body.getReader() + const decoder = new TextDecoder() + let buffer = "" + + while (true) { + const { value, done } = await reader.read() + if (done) break + if (requestToken !== routesRequestToken) return + + buffer += decoder.decode(value, { stream: true }) + const lines = buffer.split(/\r?\n\r?\n/) + buffer = lines.pop() || "" + + for (const line of lines) { + if (!line.startsWith("data:")) continue + try { + const data = JSON.parse(line.substring(5).trim()) + if (data.progress !== undefined && data.total !== undefined) { + state.routeProgress = safeCount(data.progress) + state.routeTotal = safeCount(data.total) + } + if (Array.isArray(data.routes)) { + enqueueRoutes(data.routes) + } + } catch (error) { + console.error("[ftm] failed to parse route payload", error) + } + } + } + + flushPendingRoutes() + } catch (error) { + if (error?.name !== "AbortError") { + state.error = error?.message || "Failed to load local routes." + } + } finally { + if (requestToken === routesRequestToken) { + flushPendingRoutes() + state.loadingRoutes = false + if (routesAbortController === controller) { + routesAbortController = null + } + } + } +} + +async function fetchWorkspace() { + try { + state.loadingWorkspace = true + const response = await fetch("/api/ftm/workspace") + const payload = await response.json() + if (!response.ok) throw new Error(payload.error || "Failed to load tuning workspace.") + state.workspace = payload + state.status = payload.status || {} + } catch (error) { + state.error = error?.message || "Failed to load tuning workspace." + } finally { + state.loadingWorkspace = false + } +} + +function syncFeedbackState(report) { + const feedback = report?.feedback || {} + state.feedbackAccepted = Array.isArray(feedback.acceptedDimensions) ? [...feedback.acceptedDimensions] : [] + state.feedbackIgnored = Array.isArray(feedback.ignoredDimensions) ? [...feedback.ignoredDimensions] : [] + state.feedbackNotes = typeof feedback.notes === "string" ? feedback.notes : "" +} + +async function loadReport(reportId) { + if (!reportId) return + try { + const response = await fetch(`/api/ftm/report/${encodeURIComponent(reportId)}`) + const payload = await response.json() + if (!response.ok) throw new Error(payload.error || "Failed to load tuning report.") + state.report = payload + syncFeedbackState(payload) + await fetchWorkspace() + } catch (error) { + state.error = error?.message || "Failed to load tuning report." + } +} + +async function deleteReport(reportId) { + if (!reportId || state.runningAction) return + if (!window.confirm("Delete this saved tuning report and its generated trial data?")) return + + state.runningAction = true + try { + const response = await fetch(`/api/ftm/report/${encodeURIComponent(reportId)}`, { method: "DELETE" }) + const payload = await response.json() + if (!response.ok) throw new Error(payload.error || "Failed to delete tuning report.") + + if (state.report?.reportId === reportId) { + state.report = null + syncFeedbackState(null) + } + state.workspace = payload.workspace || state.workspace + state.status = { ...state.status, ...(payload.workspace?.status || {}) } + showSnackbar(payload.message || "Deleted tuning report.") + } catch (error) { + state.error = error?.message || "Failed to delete tuning report." + showSnackbar(state.error, "error") + } finally { + state.runningAction = false + } +} + +async function clearWorkspace() { + if (state.runningAction) return + if (!window.confirm("Clear every saved tuning report, feedback entry, generated profile, and snapshot from the device?")) return + + state.runningAction = true + try { + const response = await fetch("/api/ftm/workspace/clear", { method: "POST" }) + const payload = await response.json() + if (!response.ok) throw new Error(payload.error || "Failed to clear tuning workspace.") + + state.report = null + syncFeedbackState(null) + state.workspace = payload.workspace || { reports: [], activeTrial: null, status: {} } + state.status = { ...state.status, ...(payload.workspace?.status || {}) } + showSnackbar(payload.message || "Cleared tuning workspace.") + } catch (error) { + state.error = error?.message || "Failed to clear tuning workspace." + showSnackbar(state.error, "error") + } finally { + state.runningAction = false + } +} + +async function fetchStatus() { + try { + const response = await fetch("/api/ftm/status") + const payload = await response.json() + if (!response.ok) throw new Error(payload.error || "Failed to load tuning status.") + state.status = { + ...(payload.status || {}), + isOnroad: !!payload.isOnroad, + } + if (payload.activeTrial !== undefined) { + state.workspace = { ...state.workspace, activeTrial: payload.activeTrial, reports: payload.reports || state.workspace.reports } + } + const reportId = state.status.reportId + if (reportId && state.report?.reportId !== reportId) { + await loadReport(reportId) + } + } catch (error) { + state.error = error?.message || "Failed to load tuning status." + } +} + +function stopPolling() { + if (statusPollHandle) { + clearTimeout(statusPollHandle) + statusPollHandle = null + } +} + +function ensurePolling() { + if (statusPollHandle) return + + const poll = async () => { + if (!isTuningRouteActive()) { + stopPolling() + return + } + if (document.visibilityState === "visible") { + await fetchStatus() + } + statusPollHandle = setTimeout(poll, STATUS_POLL_MS) + } + + statusPollHandle = setTimeout(poll, STATUS_POLL_MS) +} + +function toggleRouteSelection(routeName) { + const current = new Set(state.selectedRoutes) + if (current.has(routeName)) { + current.delete(routeName) + } else { + current.add(routeName) + } + state.selectedRoutes = [...current] +} + +function clearSelections() { + state.selectedRoutes = [] +} + +async function runAnalyze() { + if (!state.selectedRoutes.length || state.runningAction) return + state.runningAction = true + try { + const response = await fetch("/api/ftm/analyze", { + method: "POST", + headers: { "Content-Type": "application/json" }, + body: JSON.stringify({ routes: state.selectedRoutes }), + }) + const payload = await response.json() + if (!response.ok) throw new Error(payload.error || "Failed to start tuning analysis.") + state.status = payload.status || {} + showSnackbar(payload.message || "FTM analysis started.") + } catch (error) { + state.error = error?.message || "Failed to start tuning analysis." + showSnackbar(state.error, "error") + } finally { + state.runningAction = false + } +} + +async function stopAnalyze() { + if (state.runningAction) return + state.runningAction = true + try { + const response = await fetch("/api/ftm/analyze/stop", { method: "POST" }) + const payload = await response.json() + if (!response.ok) throw new Error(payload.error || "Failed to stop tuning analysis.") + state.status = payload.status || {} + showSnackbar(payload.message || "FTM analysis stopped.") + } catch (error) { + state.error = error?.message || "Failed to stop tuning analysis." + showSnackbar(state.error, "error") + } finally { + state.runningAction = false + } +} + +async function applyProfile(profileId) { + if (!state.report?.reportId || !profileId || state.runningAction) return + state.runningAction = true + try { + const response = await fetch("/api/ftm/trials/apply", { + method: "POST", + headers: { "Content-Type": "application/json" }, + body: JSON.stringify({ reportId: state.report.reportId, profileId }), + }) + const payload = await response.json() + if (!response.ok) throw new Error(payload.error || "Failed to apply trial profile.") + await fetchWorkspace() + showSnackbar(payload.message || "Trial profile applied.") + } catch (error) { + state.error = error?.message || "Failed to apply trial profile." + showSnackbar(state.error, "error") + } finally { + state.runningAction = false + } +} + +async function revertProfile() { + if (state.runningAction) return + state.runningAction = true + try { + const response = await fetch("/api/ftm/trials/revert", { method: "POST" }) + const payload = await response.json() + if (!response.ok) throw new Error(payload.error || "Failed to revert trial profile.") + await fetchWorkspace() + showSnackbar(payload.message || "Trial profile reverted.") + } catch (error) { + state.error = error?.message || "Failed to revert trial profile." + showSnackbar(state.error, "error") + } finally { + state.runningAction = false + } +} + +function setDimensionFeedback(dimensionId, mode) { + const accepted = new Set(state.feedbackAccepted) + const ignored = new Set(state.feedbackIgnored) + accepted.delete(dimensionId) + ignored.delete(dimensionId) + if (mode === "accepted") accepted.add(dimensionId) + if (mode === "ignored") ignored.add(dimensionId) + state.feedbackAccepted = [...accepted] + state.feedbackIgnored = [...ignored] +} + +async function saveFeedback() { + if (!state.report?.reportId || state.runningAction) return + state.runningAction = true + try { + const response = await fetch("/api/ftm/feedback", { + method: "POST", + headers: { "Content-Type": "application/json" }, + body: JSON.stringify({ + reportId: state.report.reportId, + acceptedDimensions: state.feedbackAccepted, + ignoredDimensions: state.feedbackIgnored, + notes: state.feedbackNotes, + }), + }) + const payload = await response.json() + if (!response.ok) throw new Error(payload.error || "Failed to save tuning feedback.") + state.report = { + ...state.report, + feedback: payload.feedback, + profiles: payload.profiles || state.report.profiles, + } + syncFeedbackState(state.report) + await fetchWorkspace() + showSnackbar(payload.message || "Feedback saved.") + } catch (error) { + state.error = error?.message || "Failed to save tuning feedback." + showSnackbar(state.error, "error") + } finally { + state.runningAction = false + } +} + +function feedbackStateFor(dimensionId) { + if (state.feedbackAccepted.includes(dimensionId)) return "accepted" + if (state.feedbackIgnored.includes(dimensionId)) return "ignored" + return "unset" +} + +function initialize() { + if (initialized) return + initialized = true + resetRouteStreamState() + fetchRoutes() + fetchWorkspace().then(() => { + const latestReport = state.workspace?.reports?.[0]?.reportId + if (latestReport) loadReport(latestReport) + }) + fetchStatus() + ensurePolling() +} + +function renderCurve(values) { + return `[${(values || []).map((value) => Number(value).toFixed(3)).join(", ")}]` +} + +function reportPaths() { + if (Array.isArray(state.report?.paths) && state.report.paths.length) { + return state.report.paths + } + if (!state.report) return [] + return [{ + key: state.report.primaryPathKey || "cleanup_pass", + title: "Recommendations", + description: "", + whenToUse: "", + whySelected: "", + isPrimary: true, + suggestions: state.report.suggestions || [], + profiles: state.report.profiles || [], + }] +} + +function primaryPath() { + const paths = reportPaths() + return paths.find((path) => path.isPrimary) || paths[0] || null +} + +function renderProfile(profile) { + const genericEntries = Object.entries(profile.genericParams || {}).filter(([key]) => key !== "AdvancedLateralTune") + const frictionEntries = Object.entries(profile.ftmOverrides?.baseFrictionThresholds || {}) + const vehicleKnobEntries = Object.entries(profile.ftmOverrides?.vehicleKnobs || {}) + return html` +
+
+
+

${profile.label}

+

${profile.description}

+
+ +
+ +
+
+
Generic Params
+
    + ${genericEntries.length + ? genericEntries.map(([key, value]) => html`
  • ${key}: ${String(value)}
    • `) + : html`
  • None
    • `} +
+
+
+
FTM Overrides
+
    + ${frictionEntries.map(([family, payload]) => html`
  • ${family}: ${renderCurve(payload?.values || [])}
    • `)} + ${vehicleKnobEntries.map(([key, value]) => html`
  • ${key}: ${Number(value).toFixed(3)}
    • `)} + ${!frictionEntries.length && !vehicleKnobEntries.length ? html`
  • None
    • ` : ""} +
+
+
+
+ ` +} + +function renderSuggestion(suggestion) { + const currentVsSuggested = suggestion.currentVsSuggested + const feedbackState = feedbackStateFor(suggestion.dimensionId) + return html` +
+
+
+

${suggestion.bucket.replace(/_/g, " ")}

+

+ ${suggestion.evidence?.speedBand || "mixed"} | + ${suggestion.evidence?.directionBias || "center"} | + ${safeCount(suggestion.evidence?.eventCount)} event(s) +

+
+
+ + +
+
+ +

Observed behavior: ${suggestion.observedBehavior}

+

Likely interpretation: ${suggestion.likelyInterpretation}

+

Primary adjustment: ${suggestion.primaryAdjustment}

+

What not to touch yet: ${suggestion.whatNotToTouchYet}

+

If that was wrong, next thing to try: ${suggestion.ifThatWasWrong}

+

Strongest segments: ${(suggestion.evidence?.segments || []).map((segment) => segment.label).join(", ") || "none"}

+ + ${currentVsSuggested + ? html` +
+ Current vs suggested: + ${currentVsSuggested.type === "friction_curve" + ? html` +

${currentVsSuggested.family} current: ${renderCurve(currentVsSuggested.current)}

+

${currentVsSuggested.family} suggested: ${renderCurve(currentVsSuggested.suggested)}

+ ` + : html` +

+ ${currentVsSuggested.paramKey || currentVsSuggested.symbol}: + ${Number(currentVsSuggested.current).toFixed(3)} -> ${Number(currentVsSuggested.suggested).toFixed(3)} +

+ `} +
+ ` + : html`

No trial adjustment suggested for this dimension.

`} + + ${suggestion.plotSvg + ? html`

Saved report includes an inline event plot for this finding.

` + : ""} +
+ ` +} + +function renderPathSummary(path) { + return html` +
+
+
+

${path.title}

+

${path.isPrimary ? "Recommended path" : "Alternate path"}

+
+
+

${path.description || ""}

+

Why this path: ${path.whySelected || "No path note available."}

+

When to use it: ${path.whenToUse || "Use the path that best matches the spread of the problem."}

+
+ ` +} + +export function Tuning() { + initialize() + + return html` +
+

Lateral Tuning

+ +
+

+ Analyze one or more local routes, review deterministic lateral findings, apply a bounded trial, drive, then revert or refine. +

+ +
+ + + + +
+ + ${() => state.error ? html`

${state.error}

` : ""} + +
+

Status: ${() => state.status?.state || "idle"}

+

Running: ${() => state.status?.running ? "Yes" : "No"}

+

Onroad: ${() => state.status?.isOnroad ? "Yes" : "No"}

+

Updated: ${() => formatStatusAge(state.status?.updatedAt)}

+

Selected Routes: ${() => state.selectedRoutes.length}

+

Progress: ${() => `${safeCount(state.status?.progress)}/${safeCount(state.status?.total)}`}

+

Active Trial: ${() => state.workspace?.activeTrial?.profileId || "None"}

+
+ + ${() => state.status?.isOnroad ? html` +

FTM analysis is offroad-only. Stop the car and go offroad before starting a run.

+ ` : ""} + + ${() => state.status?.currentSegment ? html` +
+

Current Segment: ${state.status.currentSegment}

+
+ ` : ""} + +
+
+
+
+

Local Routes

+

+ Pick up to 8 routes from the device. The analyzer prefers rlogs and falls back to qlogs when needed. +

+
+ +
+ + ${() => state.loadingRoutes ? html`

Loading local routes...

` : ""} + ${() => state.routeTotal ? html`

Route index: ${state.routeProgress}/${state.routeTotal}

` : ""} + ${() => state.truncatedRoutes ? html`

Showing the first ${MAX_RENDERED_ROUTES} routes only.

` : ""} + +
+ ${() => sortedRoutes().map((route) => html` + + `)} +
+
+ +
+
+
+

Workspace

+
+ +
+ ${() => state.loadingWorkspace ? html`

Loading workspace...

` : ""} +

+ Recent reports stay on-device under /data/galaxy/ftm. Loading a report refreshes the suggestion and trial view below. +

+
+ ${() => (state.workspace?.reports || []).length + ? state.workspace.reports.map((report) => html` +
+ + +
+ `) + : html`

No tuning reports yet.

`} +
+
+
+ + ${() => state.report ? html` +
+
+
+

Report Summary

+
+ +
+
+

Car: ${state.report.car?.carFingerprint || "Unknown"}

+

Control Path: ${state.report.car?.controlPath || "unknown"}

+

Friction Family: ${state.report.capabilities?.frictionFamily || "standard"}

+

Recommended Path: ${primaryPath()?.title || "Recommendations"}

+

Processed Segments: ${safeCount(state.report.summary?.processedSegments)}

+

qlog Fallback: ${state.report.summary?.usedQlogFallback ? "Yes" : "No"}

+

Samples: ${safeCount(state.report.summary?.sampleCount)}

+
+ +
+ ${reportPaths().map((path) => renderPathSummary(path))} +
+ + ${() => (state.report.warnings || []).length ? html` +
+

Warnings

+
    + ${(state.report.warnings || []).map((warning) => html`
  • ${warning}
    • `)} +
+
+ ` : ""} + + ${() => (state.report.addTheseParametersAndStartHere || []).length ? html` +
+

Add These Parameters And Start Here

+
    + ${(state.report.addTheseParametersAndStartHere || []).map((line) => html`
  • ${line}
    • `)} +
+
+ ` : ""} +
+ +
+
+
+

Recommended Findings: ${primaryPath()?.title || "Recommendations"}

+

+ ${primaryPath()?.whySelected || "Mark the dimensions that match what the driver felt."} +

+
+ +
+ + + +
+ ${((primaryPath()?.suggestions) || []).map((suggestion) => renderSuggestion(suggestion))} +
+
+ +
+

Trial Profiles

+

+ Apply one bounded profile at a time. Revert restores the exact advanced-lateral and FTM state that existed before the trial. +

+
+ ${reportPaths().length + ? reportPaths().map((path) => html` +
+

${path.title} Profiles

+

${path.whenToUse || ""}

+ ${(path.profiles || []).length + ? (path.profiles || []).map((profile) => renderProfile(profile)) + : html`

No trial profiles generated for this path.

`} +
+ `) + : html`

No trial profiles generated for this report.

`} +
+
+ ` : html` +
+

No Active Report

+

+ Select local routes, run analysis, or open one of the saved reports from the workspace panel. +

+
+ `} +
+
+ ` +} diff --git a/starpilot/system/the_galaxy/ftm_workspace.py b/starpilot/system/the_galaxy/ftm_workspace.py new file mode 100644 index 000000000..2dc20351a --- /dev/null +++ b/starpilot/system/the_galaxy/ftm_workspace.py @@ -0,0 +1,1997 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import json +import math +import os +import signal +import subprocess +import sys +import threading +import time + +from dataclasses import asdict, dataclass +from pathlib import Path +from typing import Any + +import numpy as np + +from cereal import car +from opendbc.car.hyundai.values import HyundaiFlags +from openpilot.common.params import Params +from openpilot.selfdrive.controls.lib.latcontrol_torque import KP +from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import ( + FTM_FRICTION_SPEED_KNOTS, + get_ftm_capabilities, + get_ftm_rich_profile_key, + get_ftm_supported_vehicle_knobs, + get_gm_base_friction_threshold, + get_hkg_canfd_base_friction_threshold, + get_standard_friction_threshold, + normalize_ftm_overrides, +) +from openpilot.system.hardware import PC +from openpilot.system.hardware.hw import Paths +from openpilot.tools.lib.logreader import LogReader +from openpilot.starpilot.system.the_galaxy import utilities + + +FTM_STATUS_PATH = Path("/tmp/galaxy_ftm_status.json") +FTM_LOG_PATH = Path("/tmp/galaxy_ftm.log") +FTM_STATUS_MAX_AGE_SECONDS = 3600.0 +FTM_ANALYZER_ROUTE_LIMIT = 8 +FTM_ANALYZER_PROCESS = None +FTM_ANALYZER_LOCK = threading.Lock() + +TRIAL_PARAM_SPECS = { + "AdvancedLateralTune": "bool", + "ForceAutoTune": "bool", + "ForceAutoTuneOff": "bool", + "SteerDelay": "float", + "SteerFriction": "float", + "SteerKP": "float", + "SteerLatAccel": "float", + "SteerRatio": "float", + "FTMActiveProfileId": "string", + "FTMActiveOverrides": "json", + "FTMTrialApplied": "bool", +} + +GENERIC_PARAM_METADATA = { + "SteerDelay": {"min": 0.01, "max": 1.0, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "SteerFriction": {"min": 0.0, "max": 1.0, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "SteerKP": {"min": 0.1, "max": 1.5, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "SteerLatAccel": {"min": 0.5, "max": 5.0, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "SteerRatio": {"min": 5.0, "max": 25.0, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, +} + +FTM_REFERENCE_MODEL = { + "version": 1, + "families": { + "turn_in_boost": { + "reason": "Used when the car waits too long to initiate torque even though desired lateral accel is already rising.", + "too_low": "Turn starts late or feels unwilling.", + "too_high": "Car dives into the curve too early or spikes past the plan on entry.", + }, + "unwind_taper": { + "reason": "Used when the car holds steering too long or drops it too quickly on exit.", + "too_low": "Unwind drags and the car keeps steering past the intended release.", + "too_high": "Unwind snaps back and the wheel releases too aggressively.", + }, + "friction_threshold_curve": { + "reason": "Used to calm chatter or wake up low-speed response without pretending the whole torque map is wrong.", + "too_low": "Tiny errors create twitch and correction chatter.", + "too_high": "Controller feels reluctant near center and can hesitate at low speed.", + }, + "center_taper": { + "reason": "Used only when the problem is calm-road or near-center nibbling, not general curve response.", + "too_low": "Straight-road wheel activity stays busy.", + "too_high": "Car feels lazy around center and can miss light corrections.", + }, + }, +} + +FTM_PATH_SPECS = { + "baseline_fix": { + "title": "Baseline Fix", + "description": "Use the broad knobs first to get the car into the right zip code before touching narrower cleanup layers.", + "whenToUse": "Use this when the car is broadly wrong: repeated line riding, multi-band under/oversteer, saturation, or obvious whole-car mismatch.", + "alternateHint": "If the car is already mostly good and only one band is bothering you, switch to Cleanup Pass instead.", + }, + "cleanup_pass": { + "title": "Cleanup Pass", + "description": "Use the narrow band-specific knobs first so you can clean up one behavior without disturbing the rest of the tune.", + "whenToUse": "Use this when the car is already mostly in the right zip code and the misses are localized to one phase or speed band.", + "alternateHint": "If the car is still broadly wrong after this, step back and run Baseline Fix first.", + }, +} + + +@dataclass +class RouteSource: + route: str + footage_path: str + segment: str + segment_num: int + log_path: str + used_qlog: bool + + +@dataclass +class FTMSample: + route: str + segment: int + t: float + v_ego: float + lat_active: bool + steering_pressed: bool + saturated: bool + actual_la: float + desired_la: float + desired_jerk: float + error: float + error_rate: float + p: float + i: float + d: float + f: float + output: float + steering_angle_deg: float + steering_torque: float + cmd_torque: float + out_torque: float + roll_deg: float + + +def _get_galaxy_dir() -> Path: + return Path(Paths.comma_home()) / "starpilot" / "data" / "galaxy" if PC else Path("/data/galaxy") + + +def get_ftm_workspace_root() -> Path: + return _get_galaxy_dir() / "ftm" + + +def _workspace_paths() -> dict[str, Path]: + root = get_ftm_workspace_root() + return { + "root": root, + "reports": root / "reports", + "profiles": root / "profiles", + "feedback": root / "feedback", + "snapshots": root / "snapshots", + "reference": root / "reference", + } + + +def ensure_ftm_workspace() -> dict[str, Path]: + paths = _workspace_paths() + for key, path in paths.items(): + if key != "root": + path.mkdir(parents=True, exist_ok=True) + reference_path = paths["reference"] / "ftm_reference.json" + if not reference_path.exists(): + reference_path.write_text(json.dumps(FTM_REFERENCE_MODEL, indent=2, sort_keys=True), encoding="utf-8") + return paths + + +def _read_json(path: Path, default): + try: + return json.loads(path.read_text(encoding="utf-8")) + except Exception: + return default + + +def _write_json(path: Path, payload) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + tmp_path = path.with_suffix(path.suffix + ".tmp") + tmp_path.write_text(json.dumps(payload, indent=2, sort_keys=True), encoding="utf-8") + tmp_path.replace(path) + + +def _worker_env(repo_root: Path) -> dict[str, str]: + env = os.environ.copy() + pythonpath = [ + "/usr/local/venv/lib/python3.12/site-packages", + str(repo_root / "starpilot" / "third_party"), + str(repo_root), + ] + if env.get("PYTHONPATH"): + pythonpath.append(env["PYTHONPATH"]) + env["PYTHONPATH"] = os.pathsep.join(pythonpath) + env.setdefault("OPENBLAS_NUM_THREADS", "1") + env.setdefault("OMP_NUM_THREADS", "1") + env.setdefault("MKL_NUM_THREADS", "1") + env.setdefault("NUMEXPR_NUM_THREADS", "1") + return env + + +def read_ftm_status() -> dict[str, Any]: + data = _read_json(FTM_STATUS_PATH, {}) + return data if isinstance(data, dict) else {} + + +def _write_ftm_status(payload: dict[str, Any]) -> None: + payload = dict(payload) + payload["updatedAt"] = time.time() + tmp_path = FTM_STATUS_PATH.with_suffix(".tmp") + tmp_path.write_text(json.dumps(payload, separators=(",", ":")), encoding="utf-8") + tmp_path.replace(FTM_STATUS_PATH) + + +def clear_ftm_status() -> None: + try: + FTM_STATUS_PATH.unlink() + except FileNotFoundError: + pass + except OSError: + pass + + +def ftm_analyzer_running() -> bool: + process = FTM_ANALYZER_PROCESS + if process is not None and process.poll() is None: + return True + + status = read_ftm_status() + pid = int(status.get("pid") or 0) + started_at = float(status.get("startedAt") or 0.0) + if pid <= 0 or started_at <= 0: + return False + if (time.time() - started_at) > FTM_STATUS_MAX_AGE_SECONDS: + clear_ftm_status() + return False + try: + os.kill(pid, 0) + except ProcessLookupError: + clear_ftm_status() + return False + except PermissionError: + return True + except OSError: + return False + return True + + +def stop_ftm_background_analysis() -> bool: + global FTM_ANALYZER_PROCESS + + with FTM_ANALYZER_LOCK: + process = FTM_ANALYZER_PROCESS + status = read_ftm_status() + pid = int(status.get("pid") or 0) + + if process is not None and process.poll() is None: + process.terminate() + try: + process.wait(timeout=2.0) + except subprocess.TimeoutExpired: + process.kill() + FTM_ANALYZER_PROCESS = None + clear_ftm_status() + return True + + if pid > 0: + try: + os.kill(pid, signal.SIGTERM) + except ProcessLookupError: + pass + except OSError: + return False + clear_ftm_status() + return True + + return False + + +def start_ftm_background_analysis(route_names: list[str], footage_paths: list[str]) -> bool: + global FTM_ANALYZER_PROCESS + + route_names = [str(route) for route in route_names if str(route).strip()] + if not route_names: + return False + + ensure_ftm_workspace() + with FTM_ANALYZER_LOCK: + if ftm_analyzer_running(): + return True + + repo_root = Path(__file__).resolve().parents[3] + command = [ + "nice", + "-n", + "19", + sys.executable or "python3", + str(Path(__file__).resolve()), + "worker", + json.dumps({ + "routes": route_names[:FTM_ANALYZER_ROUTE_LIMIT], + "footagePaths": [str(path) for path in footage_paths], + }), + ] + log_file = None + try: + log_file = open(FTM_LOG_PATH, "ab") + FTM_ANALYZER_PROCESS = subprocess.Popen( + command, + cwd=str(repo_root), + env=_worker_env(repo_root), + stdout=log_file, + stderr=log_file, + start_new_session=True, + ) + _write_ftm_status({ + "pid": FTM_ANALYZER_PROCESS.pid, + "startedAt": time.time(), + "running": True, + "state": "queued", + "routes": route_names[:FTM_ANALYZER_ROUTE_LIMIT], + "progress": 0, + "total": len(route_names[:FTM_ANALYZER_ROUTE_LIMIT]), + }) + except Exception: + FTM_ANALYZER_PROCESS = None + return False + finally: + if log_file is not None: + log_file.close() + + return ftm_analyzer_running() + + +def _parse_segment_num(segment_name: str) -> int: + try: + return int(str(segment_name).rsplit("--", 1)[1]) + except Exception: + return 0 + + +def resolve_route_sources(route_names: list[str], footage_paths: list[str]) -> tuple[list[RouteSource], list[str]]: + sources: list[RouteSource] = [] + warnings: list[str] = [] + for route in route_names: + route_added = False + for footage_path in footage_paths: + segments = utilities.get_segments_in_route(route, footage_path) + if not segments: + continue + for segment in segments: + segment_path = Path(footage_path) / segment + rlog_path = None + qlog_path = None + for candidate in ("rlog.zst", "rlog.bz2", "rlog"): + candidate_path = segment_path / candidate + if candidate_path.exists(): + rlog_path = candidate_path + break + for candidate in ("qlog.zst", "qlog.bz2", "qlog"): + candidate_path = segment_path / candidate + if candidate_path.exists(): + qlog_path = candidate_path + break + log_path = rlog_path or qlog_path + if log_path is None: + continue + if rlog_path is None and qlog_path is not None: + warnings.append(f"{route} segment {segment} fell back to qlog.") + sources.append(RouteSource( + route=route, + footage_path=str(footage_path), + segment=segment, + segment_num=_parse_segment_num(segment), + log_path=str(log_path), + used_qlog=rlog_path is None, + )) + route_added = True + if route_added: + break + if not route_added: + warnings.append(f"{route} could not be resolved to a local route with logs.") + sources.sort(key=lambda source: (source.route, source.segment_num)) + return sources, warnings + + +def _speed_band_label(v_ego: float) -> str: + if v_ego < 6.0: + return "low" + if v_ego < 15.0: + return "mid" + if v_ego < 25.0: + return "fast" + return "highway" + + +def _route_label(route: str, segment: int) -> str: + return f"{route}/{segment}" + + +def _event_direction(samples: list[FTMSample]) -> str: + mean_desired = float(np.mean([sample.desired_la for sample in samples])) + if mean_desired > 0.02: + return "left" + if mean_desired < -0.02: + return "right" + return "center" + + +def _group_masked_events(samples: list[FTMSample], mask: list[bool], score_series: list[float], min_points: int = 5) -> list[dict[str, Any]]: + events: list[dict[str, Any]] = [] + start_idx = None + for idx, active in enumerate(mask + [False]): + if active and start_idx is None: + start_idx = idx + continue + if active: + continue + if start_idx is None: + continue + end_idx = idx - 1 + event_samples = samples[start_idx:end_idx + 1] + if len(event_samples) >= min_points: + event_scores = score_series[start_idx:end_idx + 1] + peak_offset = int(np.argmax(event_scores)) + peak_idx = start_idx + peak_offset + peak_sample = samples[peak_idx] + direction = _event_direction(event_samples) + events.append({ + "startIdx": start_idx, + "endIdx": end_idx, + "peakIdx": peak_idx, + "peakScore": float(event_scores[peak_offset]), + "route": peak_sample.route, + "segment": peak_sample.segment, + "speedBand": _speed_band_label(float(np.mean([sample.v_ego for sample in event_samples]))), + "direction": direction, + "supportCount": len(event_samples), + }) + start_idx = None + return events + + +def _build_plot_svg(samples: list[FTMSample], event: dict[str, Any]) -> str: + start_idx = max(0, event["startIdx"] - 12) + end_idx = min(len(samples) - 1, event["endIdx"] + 12) + window = samples[start_idx:end_idx + 1] + if len(window) < 2: + return "" + + times = np.array([sample.t for sample in window], dtype=float) + desired = np.array([sample.desired_la for sample in window], dtype=float) + actual = np.array([sample.actual_la for sample in window], dtype=float) + + time_min = float(times.min()) + time_span = max(float(times.max() - time_min), 1e-3) + y_min = float(min(np.min(desired), np.min(actual))) + y_max = float(max(np.max(desired), np.max(actual))) + y_pad = max((y_max - y_min) * 0.10, 0.1) + y_min -= y_pad + y_max += y_pad + y_span = max(y_max - y_min, 1e-3) + + def _points(series): + coords = [] + for t_val, y_val in zip(times, series, strict=True): + x = ((float(t_val) - time_min) / time_span) * 380.0 + y = 120.0 - (((float(y_val) - y_min) / y_span) * 120.0) + coords.append(f"{x:.1f},{y:.1f}") + return " ".join(coords) + + return ( + "" + "" + "" + f"" + f"" + "" + ) + + +def _segment_samples(segment_source: RouteSource) -> tuple[list[FTMSample], car.CarParams | None, dict[str, str]]: + samples: list[FTMSample] = [] + car_params = None + init_data: dict[str, str] = {} + latest: dict[str, Any] = {} + + for msg in LogReader(segment_source.log_path, sort_by_time=True): + which = msg.which() + if which == "carParams" and car_params is None: + car_params = msg.carParams + continue + if which == "initData": + init = msg.initData + init_data = { + "gitCommit": str(getattr(init, "gitCommit", "") or ""), + "gitBranch": str(getattr(init, "gitBranch", "") or ""), + } + continue + if which == "carState": + latest["carState"] = msg.carState + continue + if which == "carControl": + latest["carControl"] = msg.carControl + continue + if which == "carOutput": + latest["carOutput"] = msg.carOutput + continue + if which == "liveParameters": + latest["liveParameters"] = msg.liveParameters + continue + if which != "controlsState" or "carState" not in latest or "carControl" not in latest: + continue + + controls_state = msg.controlsState + lateral_state = controls_state.lateralControlState + if lateral_state.which() != "torqueState": + continue + + torque_state = lateral_state.torqueState + car_state = latest["carState"] + car_control = latest["carControl"] + live_parameters = latest.get("liveParameters") + car_output = latest.get("carOutput") + roll_deg = math.degrees(float(getattr(live_parameters, "roll", 0.0) or 0.0)) if live_parameters is not None else 0.0 + out_torque = float(getattr(getattr(car_output, "actuatorsOutput", None), "torque", 0.0) or 0.0) if car_output is not None else 0.0 + + samples.append(FTMSample( + route=segment_source.route, + segment=segment_source.segment_num, + t=float(msg.logMonoTime) / 1e9, + v_ego=float(getattr(car_state, "vEgo", 0.0) or 0.0), + lat_active=bool(getattr(car_control, "latActive", False)), + steering_pressed=bool(getattr(car_state, "steeringPressed", False)), + saturated=bool(getattr(torque_state, "saturated", False)), + actual_la=float(getattr(torque_state, "actualLateralAccel", 0.0) or 0.0), + desired_la=float(getattr(torque_state, "desiredLateralAccel", 0.0) or 0.0), + desired_jerk=float(getattr(torque_state, "desiredLateralJerk", 0.0) or 0.0), + error=float(getattr(torque_state, "error", 0.0) or 0.0), + error_rate=float(getattr(torque_state, "errorRate", 0.0) or 0.0), + p=float(getattr(torque_state, "p", 0.0) or 0.0), + i=float(getattr(torque_state, "i", 0.0) or 0.0), + d=float(getattr(torque_state, "d", 0.0) or 0.0), + f=float(getattr(torque_state, "f", 0.0) or 0.0), + output=float(getattr(torque_state, "output", 0.0) or 0.0), + steering_angle_deg=float(getattr(car_state, "steeringAngleDeg", 0.0) or 0.0), + steering_torque=float(getattr(car_state, "steeringTorque", 0.0) or 0.0), + cmd_torque=float(getattr(getattr(car_control, "actuators", None), "torque", 0.0) or 0.0), + out_torque=out_torque, + roll_deg=roll_deg, + )) + + return samples, car_params, init_data + + +def _current_param_state(CP, params: Params) -> dict[str, Any]: + advanced_enabled = params.get_bool("AdvancedLateralTune") + torque_tune = CP.lateralTuning.torque if CP.lateralTuning.which() == "torque" else None + stock_delay = float(getattr(CP, "steerActuatorDelay", 0.0) or 0.0) + stock_ratio = float(getattr(CP, "steerRatio", 0.0) or 0.0) + stock_friction = float(getattr(torque_tune, "friction", 0.0) or 0.0) if torque_tune is not None else 0.0 + stock_lat_accel = float(getattr(torque_tune, "latAccelFactor", 0.0) or 0.0) if torque_tune is not None else 0.0 + return { + "AdvancedLateralTune": advanced_enabled, + "ForceAutoTune": params.get_bool("ForceAutoTune"), + "ForceAutoTuneOff": params.get_bool("ForceAutoTuneOff"), + "SteerDelay": params.get_float("SteerDelay", return_default=True, default=stock_delay) if advanced_enabled else stock_delay, + "SteerFriction": params.get_float("SteerFriction", return_default=True, default=stock_friction) if advanced_enabled else stock_friction, + "SteerKP": params.get_float("SteerKP", return_default=True, default=KP) if advanced_enabled else KP, + "SteerLatAccel": params.get_float("SteerLatAccel", return_default=True, default=stock_lat_accel) if advanced_enabled else stock_lat_accel, + "SteerRatio": params.get_float("SteerRatio", return_default=True, default=stock_ratio) if advanced_enabled else stock_ratio, + "FTMActiveProfileId": params.get("FTMActiveProfileId", encoding="utf-8") or "", + "FTMActiveOverrides": normalize_ftm_overrides(params.get("FTMActiveOverrides", encoding="utf-8") or "{}"), + "FTMTrialApplied": params.get_bool("FTMTrialApplied"), + } + + +def _baseline_family_curve(family: str) -> list[float]: + getter = { + "gm": get_gm_base_friction_threshold, + "standard": get_standard_friction_threshold, + "hkg_canfd": get_hkg_canfd_base_friction_threshold, + }.get(family, get_standard_friction_threshold) + return [round(float(getter(knot)), 4) for knot in FTM_FRICTION_SPEED_KNOTS] + + +def _current_family_curve(family: str, current: dict[str, Any]) -> list[float]: + active_overrides = current.get("FTMActiveOverrides", {}) if isinstance(current, dict) else {} + payload = active_overrides.get("baseFrictionThresholds", {}).get(family, {}) if isinstance(active_overrides, dict) else {} + values = payload.get("values", []) if isinstance(payload, dict) else [] + if isinstance(values, list) and len(values) == len(FTM_FRICTION_SPEED_KNOTS): + try: + return [round(float(value), 4) for value in values] + except Exception: + pass + return _baseline_family_curve(family) + + +def _clamp(value: float, lower: float, upper: float) -> float: + return min(max(float(value), lower), upper) + + +def _round_to_precision(value: float, precision: float) -> float: + if precision <= 0: + return float(value) + steps = round(float(value) / precision) + return round(steps * precision, 6) + + +def _current_vehicle_knob_value(symbol: str, current: dict[str, Any]) -> float | None: + knob = get_ftm_supported_vehicle_knobs().get(symbol) + if knob is None: + return None + + active_overrides = current.get("FTMActiveOverrides", {}) if isinstance(current, dict) else {} + vehicle_knobs = active_overrides.get("vehicleKnobs", {}) if isinstance(active_overrides, dict) else {} + try: + return float(vehicle_knobs.get(symbol, knob["defaultValue"])) + except Exception: + return float(knob["defaultValue"]) + + +def _vehicle_knob_adjustment(symbol: str, delta: float, current: dict[str, Any] | None = None) -> dict[str, Any] | None: + knob = get_ftm_supported_vehicle_knobs().get(symbol) + if knob is None: + return None + current_value = _current_vehicle_knob_value(symbol, current or {}) + if current_value is None: + return None + suggested_value = _round_to_precision(_clamp(current_value + delta, knob["min"], knob["max"]), knob["precision"]) + if math.isclose(current_value, suggested_value, abs_tol=max(float(knob["precision"]) / 2.0, 1e-6)): + return None + return { + "type": "vehicle_knob", + "symbol": symbol, + "current": current_value, + "suggested": suggested_value, + "delta": round(suggested_value - current_value, 4), + } + + +def _rich_profile_supports_knob(capabilities: dict[str, Any], suffix: str) -> bool: + rich_profile = capabilities.get("richProfileKey") + if not rich_profile: + return False + return f"{rich_profile}.{suffix}" in get_ftm_supported_vehicle_knobs() + + +def _build_event_summaries(samples: list[FTMSample]) -> tuple[list[dict[str, Any]], dict[str, Any]]: + active_samples = [sample for sample in samples if sample.lat_active and not sample.steering_pressed] + if not active_samples: + return [], {"sampleCount": 0} + + over_error = [abs(sample.actual_la) - abs(sample.desired_la) for sample in active_samples] + v_ego = [sample.v_ego for sample in active_samples] + desired = [sample.desired_la for sample in active_samples] + jerk = [sample.desired_jerk for sample in active_samples] + angle = [sample.steering_angle_deg for sample in active_samples] + output = [sample.output for sample in active_samples] + + entry_phase = [(abs(d) > 0.30 and abs(j) > 0.25 and d * j > 0.0) for d, j in zip(desired, jerk, strict=True)] + unwind_phase = [(abs(d) > 0.25 and abs(j) > 0.20 and d * j < 0.0) for d, j in zip(desired, jerk, strict=True)] + steady_curve_phase = [(abs(d) > 0.35 and abs(j) < 0.18) for d, j in zip(desired, jerk, strict=True)] + saturation_phase = [ + bool(sample.saturated and abs(sample.desired_la) > 0.30 and (abs(sample.desired_jerk) > 0.16 or abs(sample.actual_la) > 0.45)) + for sample in active_samples + ] + + base_masks = { + "understeer": [(phase and (ov < -0.20)) for phase, ov in zip(steady_curve_phase, over_error, strict=True)], + "oversteer": [(phase and (ov > 0.20)) for phase, ov in zip(steady_curve_phase, over_error, strict=True)], + "late_turn_in": [(phase and ov < -0.16) for phase, ov in zip(entry_phase, over_error, strict=True)], + "early_turn_in": [(phase and ov > 0.16) for phase, ov in zip(entry_phase, over_error, strict=True)], + "unwind_too_slow": [(phase and ov > 0.14) for phase, ov in zip(unwind_phase, over_error, strict=True)], + "unwind_too_fast": [(phase and ov < -0.14) for phase, ov in zip(unwind_phase, over_error, strict=True)], + "low_speed_unwillingness": [(s.v_ego < 6.0 and abs(s.desired_la) > 0.30 and abs(s.desired_jerk) > 0.18 and (abs(s.actual_la) + 0.18) < abs(s.desired_la)) for s in active_samples], + "saturation_limited": saturation_phase, + } + score_map = { + "understeer": [max((-ov), 0.0) for ov in over_error], + "oversteer": [max(ov, 0.0) for ov in over_error], + "late_turn_in": [max((-ov), 0.0) + abs(j) * 0.1 for ov, j in zip(over_error, jerk, strict=True)], + "early_turn_in": [max(ov, 0.0) + abs(j) * 0.1 for ov, j in zip(over_error, jerk, strict=True)], + "unwind_too_slow": [max(ov, 0.0) for ov in over_error], + "unwind_too_fast": [max((-ov), 0.0) for ov in over_error], + "low_speed_unwillingness": [max(abs(d) - abs(a), 0.0) for d, a in zip(desired, [sample.actual_la for sample in active_samples], strict=True)], + "saturation_limited": [1.0 if sample.saturated else 0.0 for sample in active_samples], + } + + # Straight-road chatter detection uses a simple 4-second window. + straight_windows = [] + for start_idx in range(0, max(len(active_samples) - 20, 1), 10): + window = active_samples[start_idx:start_idx + 40] + if len(window) < 20: + continue + if float(np.mean([sample.v_ego for sample in window])) < 20.0: + continue + if float(np.mean([abs(sample.desired_la) for sample in window])) > 0.12: + continue + centered_angles = np.array([sample.steering_angle_deg for sample in window]) - float(np.mean([sample.steering_angle_deg for sample in window])) + sign_changes = int(np.sum(np.sign(centered_angles[1:]) != np.sign(centered_angles[:-1]))) + amplitude = float(np.max(centered_angles) - np.min(centered_angles)) + chatter_score = (amplitude * 0.25) + (sign_changes * 0.04) + if amplitude > 0.45 and sign_changes >= 6: + straight_windows.append({ + "startIdx": start_idx, + "endIdx": start_idx + len(window) - 1, + "peakIdx": start_idx + int(len(window) / 2), + "peakScore": chatter_score, + "route": window[0].route, + "segment": window[0].segment, + "speedBand": "highway", + "direction": "center", + "supportCount": len(window), + }) + + curve_windows = [] + for start_idx in range(0, max(len(active_samples) - 20, 1), 8): + window = active_samples[start_idx:start_idx + 36] + if len(window) < 18: + continue + if float(np.mean([sample.v_ego for sample in window])) < 15.0: + continue + desired_sign = float(np.mean([sample.desired_la for sample in window])) + if abs(desired_sign) < 0.35: + continue + if any((sample.desired_la * desired_sign) < 0.0 for sample in window): + continue + error_series = np.array([sample.actual_la - sample.desired_la for sample in window]) + sign_changes = int(np.sum(np.sign(error_series[1:]) != np.sign(error_series[:-1]))) + amplitude = float(np.max(error_series) - np.min(error_series)) + if amplitude > 0.22 and sign_changes >= 4: + curve_windows.append({ + "startIdx": start_idx, + "endIdx": start_idx + len(window) - 1, + "peakIdx": start_idx + int(np.argmax(np.abs(error_series))), + "peakScore": amplitude + sign_changes * 0.03, + "route": window[0].route, + "segment": window[0].segment, + "speedBand": _speed_band_label(float(np.mean([sample.v_ego for sample in window]))), + "direction": "left" if desired_sign > 0.0 else "right", + "supportCount": len(window), + }) + + summaries: list[dict[str, Any]] = [] + for bucket, mask in base_masks.items(): + events = _group_masked_events(active_samples, mask, score_map[bucket]) + if events: + summaries.extend(_summaries_from_events(bucket, active_samples, events)) + if straight_windows: + summaries.extend(_summaries_from_events("center_chatter", active_samples, straight_windows)) + if curve_windows: + summaries.extend(_summaries_from_events("notchy_mid_curve", active_samples, curve_windows)) + + left_errors = [abs(sample.actual_la) - abs(sample.desired_la) for sample in active_samples if sample.desired_la > 0.25] + right_errors = [abs(sample.actual_la) - abs(sample.desired_la) for sample in active_samples if sample.desired_la < -0.25] + summary_stats = { + "sampleCount": len(active_samples), + "qlogFallback": False, + "meanDesiredAbs": round(float(np.mean(np.abs(desired))), 4), + "meanErrorAbs": round(float(np.mean(np.abs([sample.actual_la - sample.desired_la for sample in active_samples]))), 4), + "leftBias": round(float(np.mean(left_errors)), 4) if left_errors else 0.0, + "rightBias": round(float(np.mean(right_errors)), 4) if right_errors else 0.0, + "highwayStraightAngleP2P": round(float(np.percentile(np.abs(angle), 95) - np.percentile(np.abs(angle), 5)), 4) if angle else 0.0, + "meanOutputAbs": round(float(np.mean(np.abs(output))), 4) if output else 0.0, + } + + if summary_stats["meanErrorAbs"] < 0.08 and not any(summary["severity"] > 0.65 for summary in summaries): + summaries.append({ + "bucket": "model_limited", + "dimensionId": "model_limited:overall", + "direction": "center", + "speedBand": "mixed", + "count": 1, + "severity": 0.25, + "evidence": { + "speedBand": "mixed", + "directionBias": "center", + "eventCount": 1, + "segments": [], + }, + "events": [], + "plotSvg": "", + }) + + return sorted(summaries, key=lambda item: item["severity"], reverse=True), summary_stats + + +def _summaries_from_events(bucket: str, samples: list[FTMSample], events: list[dict[str, Any]]) -> list[dict[str, Any]]: + grouped: dict[tuple[str, str], list[dict[str, Any]]] = {} + for event in events: + key = (bucket, event["direction"]) + grouped.setdefault(key, []).append(event) + + summaries = [] + for (bucket_name, direction), grouped_events in grouped.items(): + grouped_events.sort(key=lambda item: item["peakScore"], reverse=True) + strongest = grouped_events[:3] + strongest_labels = [ + { + "route": event["route"], + "segment": event["segment"], + "label": _route_label(event["route"], event["segment"]), + "score": round(float(event["peakScore"]), 3), + } + for event in strongest + ] + top_event = strongest[0] + top_speed_band = top_event["speedBand"] + summaries.append({ + "bucket": bucket_name, + "dimensionId": f"{bucket_name}:{direction}:{top_speed_band}", + "direction": direction, + "speedBand": top_speed_band, + "count": len(grouped_events), + "severity": round(float(min(1.5, np.mean([event["peakScore"] for event in strongest]))), 3), + "evidence": { + "speedBand": top_speed_band, + "directionBias": direction, + "eventCount": len(grouped_events), + "segments": strongest_labels, + }, + "events": grouped_events, + "plotSvg": _build_plot_svg(samples, top_event), + }) + return summaries + + +def classify_torque_samples(samples: list[FTMSample]) -> tuple[list[dict[str, Any]], dict[str, Any]]: + return _build_event_summaries(samples) + + +def _primary_delta_from_summary(summary: dict[str, Any], capabilities: dict[str, Any], current: dict[str, Any], + strategy: str = "cleanup") -> dict[str, Any] | None: + bucket = summary["bucket"] + direction = summary["direction"] + severity = max(float(summary["severity"]), 0.2) + speed_band = str(summary.get("speedBand", "mixed")) + rich_profile = capabilities.get("richProfileKey") + family = capabilities.get("frictionFamily", "standard") + side = "left" if direction != "right" else "right" + curvy_band = speed_band in ("mid", "fast") + supports_low_speed_assist = _rich_profile_supports_knob(capabilities, "low_speed_angle_assist_max_torque") + supports_crawl_turn_in = _rich_profile_supports_knob(capabilities, f"crawl_turn_in_ff_boost_{side}") + supports_turn_in_boost = _rich_profile_supports_knob(capabilities, f"turn_in_boost_{side}") + supports_unwind_taper = _rich_profile_supports_knob(capabilities, f"unwind_taper_{side}") + supports_curvy_turn_in_trim = _rich_profile_supports_knob(capabilities, f"curvy_turn_in_trim_{side}") + supports_curvy_turn_in_speed = _rich_profile_supports_knob(capabilities, "curvy_turn_in_trim_speed_max") + supports_curvy_speed_max = _rich_profile_supports_knob(capabilities, "curvy_speed_max") + supports_curvy_unwind_extra = _rich_profile_supports_knob(capabilities, f"curvy_unwind_extra_reduction_{side}") + supports_curvy_unwind_floor = _rich_profile_supports_knob(capabilities, f"curvy_unwind_floor_relief_{side}") + + if bucket == "model_limited": + return None + + if strategy == "baseline": + if bucket in ("center_chatter", "notchy_mid_curve"): + current_curve = _current_family_curve(family, current) + deltas = [0.0, 0.01, 0.02, 0.025, 0.03] if bucket == "center_chatter" else [0.0, 0.0, 0.015, 0.02, 0.02] + scale = min(max(severity, 0.4), 1.2) + suggested = [round(current_curve[idx] + (delta * scale), 4) for idx, delta in enumerate(deltas)] + return { + "type": "friction_curve", + "symbol": f"base_friction_threshold.{family}", + "family": family, + "current": current_curve, + "suggested": suggested, + "delta": [round(suggested[idx] - current_curve[idx], 4) for idx in range(len(current_curve))], + } + + if bucket == "low_speed_unwillingness": + current_curve = _current_family_curve(family, current) + deltas = [-0.03, -0.025, -0.015, -0.005, 0.0] + scale = min(max(severity, 0.5), 1.2) + suggested = [round(max(0.05, current_curve[idx] + (delta * scale)), 4) for idx, delta in enumerate(deltas)] + return { + "type": "friction_curve", + "symbol": f"base_friction_threshold.{family}", + "family": family, + "current": current_curve, + "suggested": suggested, + "delta": [round(suggested[idx] - current_curve[idx], 4) for idx in range(len(current_curve))], + } + + if bucket in ("understeer", "late_turn_in", "saturation_limited"): + current_value = float(current["SteerLatAccel"]) + scale = 0.04 if bucket == "saturation_limited" else 0.03 + suggested_value = round(_clamp(current_value + max(scale, current_value * scale * severity), 0.5, 5.0), 4) + return {"type": "generic_param", "paramKey": "SteerLatAccel", "current": current_value, "suggested": suggested_value, "delta": round(suggested_value - current_value, 4)} + + if bucket in ("oversteer", "early_turn_in"): + current_value = float(current["SteerLatAccel"]) + suggested_value = round(_clamp(current_value - max(0.03, current_value * 0.03 * severity), 0.5, 5.0), 4) + return {"type": "generic_param", "paramKey": "SteerLatAccel", "current": current_value, "suggested": suggested_value, "delta": round(suggested_value - current_value, 4)} + + if bucket in ("unwind_too_slow", "unwind_too_fast"): + current_value = float(current["SteerFriction"]) + direction_mult = -1.0 if bucket == "unwind_too_slow" else 1.0 + suggested_value = round(_clamp(current_value + (0.015 * severity * direction_mult), 0.0, 1.0), 4) + return {"type": "generic_param", "paramKey": "SteerFriction", "current": current_value, "suggested": suggested_value, "delta": round(suggested_value - current_value, 4)} + + if bucket in ("center_chatter", "notchy_mid_curve"): + current_curve = _current_family_curve(family, current) + if bucket == "center_chatter": + deltas = [0.0, 0.01, 0.02, 0.025, 0.03] + else: + deltas = [0.0, 0.0, 0.015, 0.02, 0.02] + scale = min(max(severity, 0.4), 1.2) + suggested = [round(current_curve[idx] + (delta * scale), 4) for idx, delta in enumerate(deltas)] + return { + "type": "friction_curve", + "symbol": f"base_friction_threshold.{family}", + "family": family, + "current": current_curve, + "suggested": suggested, + "delta": [round(suggested[idx] - current_curve[idx], 4) for idx in range(len(current_curve))], + } + + if bucket == "low_speed_unwillingness": + current_curve = _current_family_curve(family, current) + deltas = [-0.03, -0.025, -0.015, -0.005, 0.0] + scale = min(max(severity, 0.5), 1.2) + if supports_low_speed_assist: + adjustment = _vehicle_knob_adjustment(f"{rich_profile}.low_speed_angle_assist_max_torque", 0.04 * scale, current) + if adjustment is not None: + return adjustment + if supports_crawl_turn_in: + adjustment = _vehicle_knob_adjustment(f"{rich_profile}.crawl_turn_in_ff_boost_{side}", 0.03 * scale, current) + if adjustment is not None: + return adjustment + if rich_profile and supports_turn_in_boost: + adjustment = _vehicle_knob_adjustment(f"{rich_profile}.turn_in_boost_{side}", 0.025 * scale, current) + if adjustment is not None: + return adjustment + suggested = [round(max(0.05, current_curve[idx] + (delta * scale)), 4) for idx, delta in enumerate(deltas)] + return { + "type": "friction_curve", + "symbol": f"base_friction_threshold.{family}", + "family": family, + "current": current_curve, + "suggested": suggested, + "delta": [round(suggested[idx] - current_curve[idx], 4) for idx in range(len(current_curve))], + } + + if bucket in ("understeer", "late_turn_in"): + if curvy_band and speed_band == "fast" and bucket == "late_turn_in" and supports_curvy_turn_in_speed: + adjustment = _vehicle_knob_adjustment(f"{rich_profile}.curvy_turn_in_trim_speed_max", 1.6 * severity, current) + if adjustment is not None: + return adjustment + if curvy_band and supports_curvy_turn_in_trim: + adjustment = _vehicle_knob_adjustment(f"{rich_profile}.curvy_turn_in_trim_{side}", -0.018 * severity, current) + if adjustment is not None: + return adjustment + if rich_profile and supports_turn_in_boost: + adjustment = _vehicle_knob_adjustment(f"{rich_profile}.turn_in_boost_{side}", 0.02 * severity, current) + if adjustment is not None: + return adjustment + current_value = float(current["SteerLatAccel"]) + suggested_value = round(_clamp(current_value + max(0.03, current_value * 0.03 * severity), 0.5, 5.0), 4) + return {"type": "generic_param", "paramKey": "SteerLatAccel", "current": current_value, "suggested": suggested_value, "delta": round(suggested_value - current_value, 4)} + + if bucket in ("oversteer", "early_turn_in"): + if curvy_band and supports_curvy_turn_in_trim: + adjustment = _vehicle_knob_adjustment(f"{rich_profile}.curvy_turn_in_trim_{side}", 0.018 * severity, current) + if adjustment is not None: + return adjustment + if rich_profile and supports_turn_in_boost: + adjustment = _vehicle_knob_adjustment(f"{rich_profile}.turn_in_boost_{side}", -0.02 * severity, current) + if adjustment is not None: + return adjustment + current_value = float(current["SteerLatAccel"]) + suggested_value = round(_clamp(current_value - max(0.03, current_value * 0.03 * severity), 0.5, 5.0), 4) + return {"type": "generic_param", "paramKey": "SteerLatAccel", "current": current_value, "suggested": suggested_value, "delta": round(suggested_value - current_value, 4)} + + if bucket in ("unwind_too_slow", "unwind_too_fast"): + if curvy_band and supports_curvy_unwind_extra: + if bucket == "unwind_too_slow" and speed_band == "fast" and supports_curvy_speed_max: + adjustment = _vehicle_knob_adjustment(f"{rich_profile}.curvy_speed_max", 1.8 * severity, current) + if adjustment is not None: + return adjustment + direction_mult = 1.0 if bucket == "unwind_too_slow" else -1.0 + adjustment = _vehicle_knob_adjustment(f"{rich_profile}.curvy_unwind_extra_reduction_{side}", 0.03 * severity * direction_mult, current) + if adjustment is not None: + return adjustment + if rich_profile and supports_unwind_taper: + direction_mult = 1.0 if bucket == "unwind_too_slow" else -1.0 + adjustment = _vehicle_knob_adjustment(f"{rich_profile}.unwind_taper_{side}", 0.08 * severity * direction_mult, current) + if adjustment is not None: + return adjustment + current_value = float(current["SteerFriction"]) + direction_mult = -1.0 if bucket == "unwind_too_slow" else 1.0 + suggested_value = round(_clamp(current_value + (0.015 * severity * direction_mult), 0.0, 1.0), 4) + return {"type": "generic_param", "paramKey": "SteerFriction", "current": current_value, "suggested": suggested_value, "delta": round(suggested_value - current_value, 4)} + + if bucket == "saturation_limited": + if curvy_band and supports_curvy_unwind_floor: + adjustment = _vehicle_knob_adjustment(f"{rich_profile}.curvy_unwind_floor_relief_{side}", 0.04 * severity, current) + if adjustment is not None: + return adjustment + current_value = float(current["SteerLatAccel"]) + suggested_value = round(_clamp(current_value + max(0.04, current_value * 0.04 * severity), 0.5, 5.0), 4) + return {"type": "generic_param", "paramKey": "SteerLatAccel", "current": current_value, "suggested": suggested_value, "delta": round(suggested_value - current_value, 4)} + + return None + + +def _observed_behavior(summary: dict[str, Any]) -> str: + bucket = summary["bucket"] + direction = summary["direction"] + speed_band = summary["speedBand"] + direction_text = "" if direction == "center" else f" on {direction} {speed_band} inputs" + mapping = { + "understeer": f"The car is not matching requested lateral accel{direction_text}; it stays wider than plan before recovery.", + "oversteer": f"The car is exceeding requested lateral accel{direction_text}; it is stepping past plan before correcting back.", + "late_turn_in": f"Turn-in is late{direction_text}; desired lateral accel is already building while actual response lags.", + "early_turn_in": f"Turn-in is too eager{direction_text}; actual response jumps ahead of the plan during entry.", + "unwind_too_slow": f"Unwind is hanging on too long{direction_text}; the car keeps steering after the plan starts releasing.", + "unwind_too_fast": f"Unwind is releasing too quickly{direction_text}; the wheel gives back steering sooner than the plan wants.", + "center_chatter": "The car is doing repeated micro-corrections on straights or very light highway arcs.", + "notchy_mid_curve": "Mid-curve tracking is correcting in steps instead of flowing through the same steering band cleanly.", + "low_speed_unwillingness": "At low speed the controller is slow to wake up even though the turn request is already there.", + "saturation_limited": "The controller is spending meaningful time at or near its steering authority ceiling.", + "model_limited": "The controller is largely matching the commanded path; this sample does not show a strong tuning mismatch.", + } + return mapping.get(bucket, "The controller is showing a repeatable mismatch against the requested path.") + + +def _likely_interpretation(summary: dict[str, Any], adjustment: dict[str, Any]) -> str: + bucket = summary["bucket"] + if adjustment["type"] == "friction_curve": + if bucket == "low_speed_unwillingness": + return "The near-center friction threshold is too high in the crawl-speed band, so small requests are being muted." + return "This looks more like a friction-threshold problem than a whole-tune problem; the controller is busy around center and needs a calmer deadzone slope." + if adjustment["type"] == "vehicle_knob": + symbol = adjustment["symbol"] + if "low_speed_angle_assist_max_torque" in symbol: + return "The main torque path is waking up too late below about 8 mph, so the low-speed assist layer needs a little more authority." + if "crawl_turn_in_ff_boost" in symbol: + return "The crawl-speed turn-in band is still too lazy, even before the broader tune needs to move." + if "curvy_speed_max" in symbol: + return "The curvy unwind band is dropping out too early at higher speed, so the curve-specific release cleanup is not staying active long enough." + if "curvy_turn_in_trim_speed_max" in symbol: + return "The fast-curve entry trim band is fading out too early, so the controller is falling back to the base path before the curve is done." + if "curvy_turn_in_trim" in symbol: + return "This is a curve-band entry problem, not a whole-car turn-in problem; the mid-speed trim needs to move without touching the rest of the tune." + if "curvy_unwind" in symbol: + return "This is a curve-band release problem, not a global unwind problem; the mid-speed unwind cleanup needs to move on its own." + if bucket in ("understeer", "late_turn_in", "low_speed_unwillingness", "saturation_limited"): + return "Primary turn-in authority is too low for the way this car is reacting in that band." + if bucket in ("oversteer", "early_turn_in"): + return "Entry authority is too aggressive for the speed band being hit here." + if bucket == "unwind_too_slow": + return "Exit steering is being held too long after the plan starts backing out of the curve." + if bucket == "unwind_too_fast": + return "Exit steering is tapering away too quickly once unwind starts." + return "The mismatch is consistent enough to justify a direct tuning pass." + + +def _why_this_knob(adjustment: dict[str, Any]) -> str: + if adjustment["type"] == "friction_curve": + return "This changes the threshold that maps small lateral-accel error into friction compensation without pretending the whole torque slope is wrong." + if adjustment["type"] == "vehicle_knob": + symbol = adjustment["symbol"] + if "low_speed_angle_assist_max_torque" in symbol: + return "This directly raises the crawl-speed assist ceiling that fills the gap before the normal torque path wakes up." + if "crawl_turn_in_ff_boost" in symbol: + return "This only touches the crawl-speed turn-in band instead of disturbing normal-speed behavior." + if "curvy_speed_max" in symbol: + return "This keeps the dedicated curvy unwind helper alive deeper into faster curves instead of globally changing the whole unwind map." + if "curvy_turn_in_trim_speed_max" in symbol: + return "This extends the fast-curve trim band instead of making the whole car more eager to turn everywhere." + if "curvy_turn_in_trim" in symbol: + return "This trims entry only in the dedicated curvy speed band instead of flattening turn-in everywhere." + if "curvy_unwind" in symbol: + return "This cleans up release only in the dedicated curvy speed band instead of changing global unwind behavior." + if "turn_in_boost" in symbol: + return "This targets entry behavior directly instead of disturbing the whole tune." + if "unwind_taper" in symbol: + return "This targets release behavior directly instead of flattening the whole response." + if "threshold" in symbol: + return "This adjusts the transition deadzone for the specific phase that is misbehaving." + return "This is the closest car-specific knob to the symptom being shown." + return "This is the smallest generic user-facing change that moves the car in the right direction without inventing a new code path." + + +def _render_adjustment_line(adjustment: dict[str, Any]) -> str: + if adjustment["type"] == "friction_curve": + curve = ", ".join(f"{value:.3f}" for value in adjustment["suggested"]) + return f"Adjust {adjustment['family']} friction threshold curve at {FTM_FRICTION_SPEED_KNOTS} m/s to [{curve}]." + if adjustment["type"] == "vehicle_knob": + return f"Move `{adjustment['symbol']}` from {adjustment['current']:.3f} to {adjustment['suggested']:.3f}." + return f"Move `{adjustment['paramKey']}` from {adjustment['current']:.3f} to {adjustment['suggested']:.3f}." + + +def _what_not_to_touch_yet(summary: dict[str, Any], adjustment: dict[str, Any] | None, strategy: str) -> str: + if strategy == "baseline": + if adjustment and adjustment.get("type") in ("generic_param", "friction_curve"): + return "Do not jump straight into phase-specific cleanup knobs yet. Get the broad authority and friction behavior into the right zip code first." + return "Do not start layering narrow cleanup knobs onto a car that is still broadly wrong." + if adjustment and adjustment.get("type") == "generic_param": + return "Do not widen this into a whole-car ratio or delay change first. This symptom can usually be cleaned up without global geometry edits." + return "Do not change unrelated center-taper or steer-ratio behavior first. This symptom has a narrower cause than that." + + +def _if_that_was_wrong(summary: dict[str, Any], adjustment: dict[str, Any], strategy: str) -> str: + if strategy == "baseline": + return f"If this gets the car broadly closer but leaves one specific phase ugly, stop here and switch to Cleanup Pass for that band. {_why_this_knob(adjustment)}" + return f"If this cleans up the main symptom but introduces the opposite behavior, keep half the change and move to the next phase-specific knob. {_why_this_knob(adjustment)}" + + +def build_suggestions(summaries: list[dict[str, Any]], capabilities: dict[str, Any], current: dict[str, Any], + strategy: str = "cleanup") -> list[dict[str, Any]]: + suggestions = [] + for summary in summaries: + adjustment = _primary_delta_from_summary(summary, capabilities, current, strategy=strategy) + evidence = summary.get("evidence", {}) + if adjustment is None: + suggestions.append({ + "dimensionId": summary["dimensionId"], + "bucket": summary["bucket"], + "severity": float(summary.get("severity", 0.0)), + "evidence": evidence, + "currentVsSuggested": None, + "observedBehavior": _observed_behavior(summary), + "likelyInterpretation": _likely_interpretation(summary, {"type": "generic_param", "paramKey": "none"}), + "primaryAdjustment": "Do not change the tune yet.", + "whatNotToTouchYet": "Do not start cutting or adding turn-in. This sample does not show a clean controller-side miss.", + "ifThatWasWrong": "If a stronger sample later shows actual lateral accel lagging or overshooting the plan, revisit with that route.", + "strategy": strategy, + }) + continue + + if adjustment["type"] == "friction_curve": + current_vs_suggested = { + "type": "friction_curve", + "family": adjustment["family"], + "current": adjustment["current"], + "suggested": adjustment["suggested"], + } + elif adjustment["type"] == "vehicle_knob": + current_vs_suggested = { + "type": "vehicle_knob", + "symbol": adjustment["symbol"], + "current": adjustment["current"], + "suggested": adjustment["suggested"], + } + else: + current_vs_suggested = { + "type": "generic_param", + "paramKey": adjustment["paramKey"], + "current": adjustment["current"], + "suggested": adjustment["suggested"], + } + + suggestions.append({ + "dimensionId": summary["dimensionId"], + "bucket": summary["bucket"], + "severity": float(summary.get("severity", 0.0)), + "evidence": evidence, + "currentVsSuggested": current_vs_suggested, + "primaryAdjustmentRaw": adjustment, + "strategy": strategy, + "observedBehavior": _observed_behavior(summary), + "likelyInterpretation": _likely_interpretation(summary, adjustment), + "primaryAdjustment": _render_adjustment_line(adjustment), + "whatNotToTouchYet": _what_not_to_touch_yet(summary, adjustment, strategy), + "ifThatWasWrong": _if_that_was_wrong(summary, adjustment, strategy), + "driverFeel": _observed_behavior(summary), + "logSupport": f"Matched in {evidence.get('eventCount', 0)} event(s); strongest samples: {', '.join(item['label'] for item in evidence.get('segments', [])[:3]) or 'none'}", + "whyThisKnob": _why_this_knob(adjustment), + "plotSvg": summary.get("plotSvg", ""), + }) + return suggestions + + +def _clamp_generic_param(param_key: str, value: float) -> float: + meta = GENERIC_PARAM_METADATA[param_key] + return _round_to_precision(_clamp(value, meta["min"], meta["max"]), meta["precision"]) + + +def _merge_primary_adjustments(suggestions: list[dict[str, Any]], multiplier: float) -> tuple[dict[str, Any], dict[str, Any], bool]: + params_delta: dict[str, Any] = {"AdvancedLateralTune": True} + requires_force_auto_tune_off = False + generic_targets: dict[str, dict[str, Any]] = {} + vehicle_targets: dict[str, dict[str, Any]] = {} + friction_targets: dict[str, dict[str, Any]] = {} + + for suggestion in suggestions: + adjustment = suggestion.get("primaryAdjustmentRaw") + if not isinstance(adjustment, dict): + continue + weight = max(float(suggestion.get("severity", 0.0)), 0.25) + if adjustment["type"] == "generic_param": + param_key = adjustment["paramKey"] + bucket = generic_targets.setdefault(param_key, { + "current": float(adjustment["current"]), + "weightedDelta": 0.0, + "weight": 0.0, + }) + bucket["weightedDelta"] += float(adjustment["delta"]) * weight + bucket["weight"] += weight + if param_key in ("SteerFriction", "SteerLatAccel", "SteerKP", "SteerDelay", "SteerRatio"): + requires_force_auto_tune_off = True + elif adjustment["type"] == "vehicle_knob": + symbol = adjustment["symbol"] + bucket = vehicle_targets.setdefault(symbol, { + "current": float(adjustment["current"]), + "weightedDelta": 0.0, + "weight": 0.0, + }) + bucket["weightedDelta"] += float(adjustment["delta"]) * weight + bucket["weight"] += weight + requires_force_auto_tune_off = True + elif adjustment["type"] == "friction_curve": + family = adjustment["family"] + delta_curve = [float(value) for value in adjustment["delta"]] + bucket = friction_targets.setdefault(family, { + "current": [float(value) for value in adjustment["current"]], + "weightedDelta": [0.0] * len(delta_curve), + "weight": 0.0, + }) + for idx, value in enumerate(delta_curve): + bucket["weightedDelta"][idx] += value * weight + bucket["weight"] += weight + requires_force_auto_tune_off = True + + overrides: dict[str, Any] = {"schemaVersion": 1, "baseFrictionThresholds": {}, "vehicleKnobs": {}} + for param_key, bucket in generic_targets.items(): + avg_delta = (bucket["weightedDelta"] / bucket["weight"]) * multiplier if bucket["weight"] > 0 else 0.0 + next_value = _clamp_generic_param(param_key, float(bucket["current"]) + avg_delta) + precision = float(GENERIC_PARAM_METADATA[param_key]["precision"]) + if not math.isclose(float(bucket["current"]), next_value, abs_tol=max(precision / 2.0, 1e-6)): + params_delta[param_key] = next_value + + supported_knobs = get_ftm_supported_vehicle_knobs() + for symbol, bucket in vehicle_targets.items(): + meta = supported_knobs.get(symbol) + if meta is None or bucket["weight"] <= 0: + continue + avg_delta = (bucket["weightedDelta"] / bucket["weight"]) * multiplier + next_value = _round_to_precision(_clamp(float(bucket["current"]) + avg_delta, meta["min"], meta["max"]), meta["precision"]) + if not math.isclose(float(bucket["current"]), next_value, abs_tol=max(float(meta["precision"]) / 2.0, 1e-6)): + overrides["vehicleKnobs"][symbol] = next_value + + for family, bucket in friction_targets.items(): + if bucket["weight"] <= 0: + continue + avg_delta_curve = [value / bucket["weight"] for value in bucket["weightedDelta"]] + values = [ + round(max(0.05, float(bucket["current"][idx]) + (avg_delta_curve[idx] * multiplier)), 4) + for idx in range(len(bucket["current"])) + ] + if any(not math.isclose(float(bucket["current"][idx]), values[idx], abs_tol=1e-6) for idx in range(len(values))): + overrides["baseFrictionThresholds"][family] = {"speedKnots": list(FTM_FRICTION_SPEED_KNOTS), "values": values} + + overrides = normalize_ftm_overrides(overrides) + return params_delta, overrides, requires_force_auto_tune_off + + +def _resolve_conflicting_actionable_suggestions(suggestions: list[dict[str, Any]]) -> list[dict[str, Any]]: + families = { + "understeer": ("turn_in", "more"), + "late_turn_in": ("turn_in", "more"), + "oversteer": ("turn_in", "less"), + "early_turn_in": ("turn_in", "less"), + "unwind_too_slow": ("unwind", "more"), + "unwind_too_fast": ("unwind", "less"), + } + grouped: dict[tuple[str, str, str], dict[str, list[dict[str, Any]]]] = {} + passthrough: list[dict[str, Any]] = [] + + for suggestion in suggestions: + bucket = str(suggestion.get("bucket", "")) + family_info = families.get(bucket) + if family_info is None: + passthrough.append(suggestion) + continue + evidence = suggestion.get("evidence", {}) + key = ( + family_info[0], + str(evidence.get("directionBias", "center")), + str(evidence.get("speedBand", "mixed")), + ) + grouped.setdefault(key, {"more": [], "less": []})[family_info[1]].append(suggestion) + + resolved = list(passthrough) + for polarities in grouped.values(): + more = polarities["more"] + less = polarities["less"] + if not more or not less: + resolved.extend(more or less) + continue + + def score(items: list[dict[str, Any]]) -> float: + total = 0.0 + for item in items: + severity = max(float(item.get("severity", 0.0)), 0.25) + event_count = max(int(item.get("evidence", {}).get("eventCount", 0)), 1) + total += severity * math.log1p(event_count) + return total + + more_score = score(more) + less_score = score(less) + if more_score >= less_score * 1.2: + resolved.extend(more) + elif less_score >= more_score * 1.2: + resolved.extend(less) + + return sorted(resolved, key=lambda item: float(item.get("severity", 0.0)), reverse=True) + + +def _bucket_tuning_family(bucket: str) -> str: + if bucket in ("understeer", "late_turn_in", "oversteer", "early_turn_in", "saturation_limited", "low_speed_unwillingness"): + return "authority" + if bucket in ("unwind_too_slow", "unwind_too_fast"): + return "release" + if bucket in ("center_chatter", "notchy_mid_curve"): + return "stability" + return "other" + + +def select_primary_tuning_path(summaries: list[dict[str, Any]], summary_stats: dict[str, Any]) -> dict[str, Any]: + actionable = [ + summary for summary in summaries + if summary.get("bucket") not in ("model_limited", "angle_control_diagnostic") + ] + if not actionable: + return { + "primaryPathKey": "cleanup_pass", + "alternatePathKey": "baseline_fix", + "reason": "This sample does not show a broad controller-side miss. Start with the narrower cleanup path if you test anything.", + "baselineScore": 0, + } + + mean_error = float(summary_stats.get("meanErrorAbs", 0.0) or 0.0) + families = {_bucket_tuning_family(str(summary.get("bucket", ""))) for summary in actionable} + major_events = [summary for summary in actionable if float(summary.get("severity", 0.0)) >= 0.8] + severe_global = [ + summary for summary in actionable + if summary.get("bucket") in ("understeer", "oversteer", "late_turn_in", "early_turn_in", "saturation_limited") + and float(summary.get("severity", 0.0)) >= 0.85 + ] + + baseline_score = 0 + if mean_error >= 0.14: + baseline_score += 2 + elif mean_error >= 0.11: + baseline_score += 1 + if len(actionable) >= 4: + baseline_score += 1 + if len(families - {"other"}) >= 3: + baseline_score += 1 + if len(major_events) >= 2: + baseline_score += 1 + if severe_global: + baseline_score += 1 + + if baseline_score >= 3: + return { + "primaryPathKey": "baseline_fix", + "alternatePathKey": "cleanup_pass", + "reason": "This route looks broadly wrong across enough bands that the right first move is to fix base authority and friction behavior before touching narrower cleanup layers.", + "baselineScore": baseline_score, + } + + return { + "primaryPathKey": "cleanup_pass", + "alternatePathKey": "baseline_fix", + "reason": "This route is already close enough overall that the better first move is a narrow cleanup pass instead of a broad whole-car reset.", + "baselineScore": baseline_score, + } + + +def build_trial_profiles(report_id: str, suggestions: list[dict[str, Any]], feedback: dict[str, Any], capabilities: dict[str, Any], + path_key: str = "cleanup_pass", path_label: str = "Cleanup Pass") -> list[dict[str, Any]]: + ignored = set(str(item) for item in feedback.get("ignoredDimensions", [])) + accepted = set(str(item) for item in feedback.get("acceptedDimensions", [])) + + considered = [ + suggestion for suggestion in suggestions + if suggestion.get("dimensionId") not in ignored and ( + not accepted or suggestion.get("dimensionId") in accepted + ) + ] + if not considered: + considered = [suggestion for suggestion in suggestions if suggestion.get("primaryAdjustmentRaw")] + actionable = [ + suggestion for suggestion in considered + if suggestion.get("primaryAdjustmentRaw") + ] + actionable = _resolve_conflicting_actionable_suggestions(actionable) + + profiles = [] + profile_defs = [ + ("conservative", "Conservative", 0.6), + ("recommended", "Recommended", 1.0), + ("assertive", "Assertive", 1.35), + ] + for suffix, label, multiplier in profile_defs: + params_delta, overrides, force_auto_tune_off = _merge_primary_adjustments(actionable, multiplier) + if not overrides and len(params_delta) <= 1: + continue + profile = { + "id": f"{report_id}:{path_key}:{suffix}", + "reportId": report_id, + "label": label, + "pathKey": path_key, + "pathLabel": path_label, + "description": f"{label} {path_label.lower()} trial generated from {len(actionable)} confirmed symptom dimension(s).", + "genericParams": params_delta, + "ftmOverrides": overrides, + "requiresForceAutoTuneOff": bool(force_auto_tune_off), + "capabilities": capabilities, + } + if force_auto_tune_off: + profile["genericParams"]["ForceAutoTuneOff"] = True + profile["genericParams"]["ForceAutoTune"] = False + profiles.append(profile) + + return profiles[:3] + + +def _add_parameters_start_here(capabilities: dict[str, Any], suggestions: list[dict[str, Any]], primary_path_key: str) -> list[str]: + lines = ["Turn on Advanced Lateral Tune before trying any suggested profile."] + if primary_path_key == "baseline_fix": + lines.append("This route looks broadly wrong enough that the first move should be a baseline fix, not a surgical cleanup pass.") + lines.append("Start with the broad knobs this report suggests. Once the car is in the right zip code, re-run analysis and switch to Cleanup Pass for the leftovers.") + else: + lines.append("This route is already mostly in the right zip code, so start with cleanup changes before reaching for broader whole-car adjustments.") + + if any(suggestion.get("primaryAdjustmentRaw", {}).get("type") == "generic_param" for suggestion in suggestions if suggestion.get("primaryAdjustmentRaw")): + lines.append("Generic advanced lateral params are in play on this pass, so apply those first before deciding you need deeper code-level changes.") + if any(suggestion.get("primaryAdjustmentRaw", {}).get("type") == "friction_curve" for suggestion in suggestions if suggestion.get("primaryAdjustmentRaw")): + lines.append("Friction-threshold changes are active in this pass because the logs point to small-signal steering behavior, not just whole-tune authority.") + if not capabilities.get("richProfileKey"): + lines.append("This car does not expose richer live FTM knobs yet, so generic advanced params and friction-threshold trials are the first code-level moves to test.") + return lines + + +def build_recommendation_paths(report_id: str, summaries: list[dict[str, Any]], summary_stats: dict[str, Any], + capabilities: dict[str, Any], current: dict[str, Any], + feedback: dict[str, Any]) -> tuple[list[dict[str, Any]], dict[str, Any]]: + decision = select_primary_tuning_path(summaries, summary_stats) + all_suggestions = { + "baseline_fix": build_suggestions(summaries, capabilities, current, strategy="baseline"), + "cleanup_pass": build_suggestions(summaries, capabilities, current, strategy="cleanup"), + } + + ordered_keys = [decision["primaryPathKey"], decision["alternatePathKey"]] + paths = [] + for path_key in ordered_keys: + spec = FTM_PATH_SPECS[path_key] + suggestions = all_suggestions[path_key] + profiles = build_trial_profiles(report_id, suggestions, feedback, capabilities, path_key=path_key, path_label=spec["title"]) + paths.append({ + "key": path_key, + "title": spec["title"], + "description": spec["description"], + "whenToUse": spec["whenToUse"], + "alternateHint": spec["alternateHint"], + "isPrimary": path_key == decision["primaryPathKey"], + "whySelected": decision["reason"] if path_key == decision["primaryPathKey"] else spec["alternateHint"], + "suggestions": suggestions, + "profiles": profiles, + }) + return paths, decision + + +def _render_report_html(report: dict[str, Any]) -> str: + report_paths = [path for path in report.get("paths", []) if isinstance(path, dict)] + primary_path = next((path for path in report_paths if path.get("isPrimary")), report_paths[0] if report_paths else {}) + findings_html = [] + for suggestion in report.get("suggestions", []): + evidence = suggestion.get("evidence", {}) + current_vs_suggested = suggestion.get("currentVsSuggested") + if current_vs_suggested is None: + delta_html = "

No trial adjustment suggested for this dimension.

" + elif current_vs_suggested["type"] == "friction_curve": + delta_html = ( + f"

Current: {current_vs_suggested['current']}

" + f"

Suggested: {current_vs_suggested['suggested']}

" + ) + else: + label = current_vs_suggested.get("paramKey") or current_vs_suggested.get("symbol") + delta_html = ( + f"

{label}: {float(current_vs_suggested['current']):.3f} -> {float(current_vs_suggested['suggested']):.3f}

" + ) + findings_html.append( + "
" + f"

{suggestion['bucket'].replace('_', ' ').title()}

" + f"

Observed behavior: {suggestion['observedBehavior']}

" + f"

Likely interpretation: {suggestion['likelyInterpretation']}

" + f"

Primary adjustment: {suggestion['primaryAdjustment']}

" + f"

What not to touch yet: {suggestion['whatNotToTouchYet']}

" + f"

If that was wrong, next thing to try: {suggestion['ifThatWasWrong']}

" + f"

Evidence: speed={evidence.get('speedBand', 'mixed')}, direction={evidence.get('directionBias', 'center')}, events={evidence.get('eventCount', 0)}

" + f"

Strongest segments: {', '.join(item['label'] for item in evidence.get('segments', [])[:3]) or 'none'}

" + f"{delta_html}" + f"{suggestion.get('plotSvg', '')}" + "
" + ) + + path_html = [] + for path in report_paths: + badge = "Recommended" if path.get("isPrimary") else "Alternate" + path_html.append( + "
" + f"

{path.get('title', 'Path')}

" + f"

{badge}: {path.get('description', '')}

" + f"

Why this path: {path.get('whySelected', '')}

" + f"

When to use it: {path.get('whenToUse', '')}

" + "
" + ) + + profile_html = [] + for path in report_paths: + path_profiles = path.get("profiles", []) + cards = [] + for profile in path_profiles: + generic_lines = [] + for key, value in profile.get("genericParams", {}).items(): + if key == "AdvancedLateralTune": + continue + generic_lines.append(f"
  • {key}: {value}
    • ") + override_lines = [] + for family, payload in profile.get("ftmOverrides", {}).get("baseFrictionThresholds", {}).items(): + override_lines.append(f"
  • {family} curve: {payload.get('values', [])}
    • ") + for key, value in profile.get("ftmOverrides", {}).get("vehicleKnobs", {}).items(): + override_lines.append(f"
  • {key}: {value}
    • ") + cards.append( + "
    " + f"

    {profile['label']}

    " + f"

    {profile['description']}

    " + f"

    Generic params:

    " + f"

    FTM overrides:

    " + "
    " + ) + path_profiles_html = "".join(cards) or "

    No trial profiles generated for this path.

    " + profile_html.append( + f"

    {path.get('title', 'Path')} Profiles

    " + f"{path_profiles_html}" + ) + + start_here_lines = "".join(f"
  • {line}
    • " for line in report.get("addTheseParametersAndStartHere", [])) + start_here_html = f"

    Add These Parameters And Start Here

    " if start_here_lines else "" + findings_block = "".join(findings_html) or "

    No strong findings.

    " + profiles_block = "".join(profile_html) or "

    No trial profiles generated.

    " + return ( + "" + "FTM Tuning Report" + "" + f"

    FTM Tuning Report

    " + f"

    {report['car']['carFingerprint']} | {report['car'].get('gitBranch', '')} {report['car'].get('gitCommit', '')}

    " + f"

    Routes

    {', '.join(report.get('routeNames', []))}

    " + f"

    Control Path

    {report['car'].get('controlPath', 'unknown')}

    " + f"

    Friction Family

    {report['capabilities'].get('frictionFamily', 'standard')}

    " + f"{''.join(path_html)}" + f"{start_here_html}" + f"

    Recommended Findings: {primary_path.get('title', 'Recommendations')}

    " + f"{findings_block}" + "

    Trial Profiles

    " + f"{profiles_block}" + "" + ) + + +def analyze_routes(route_names: list[str], footage_paths: list[str], feedback: dict[str, Any] | None = None, report_id: str | None = None) -> dict[str, Any]: + ensure_ftm_workspace() + params = Params(return_defaults=True) + report_id = report_id or f"ftm-{int(time.time())}" + feedback = feedback or {} + + sources, warnings = resolve_route_sources(route_names, footage_paths) + if not sources: + raise RuntimeError("No local routes with qlogs or rlogs were found for the selected routes.") + + all_samples: list[FTMSample] = [] + car_params = None + init_data: dict[str, str] = {} + used_qlog = False + processed_segments = 0 + for idx, source in enumerate(sources, start=1): + _write_ftm_status({ + "pid": os.getpid(), + "startedAt": time.time(), + "running": True, + "state": "analyzing", + "routes": route_names, + "progress": idx - 1, + "total": len(sources), + "currentSegment": source.segment, + }) + segment_samples, segment_car_params, segment_init = _segment_samples(source) + if car_params is None and segment_car_params is not None: + car_params = segment_car_params + if segment_init and not init_data: + init_data = segment_init + if source.used_qlog: + used_qlog = True + all_samples.extend(segment_samples) + processed_segments += 1 + + if car_params is None: + raise RuntimeError("No carParams were found in the selected routes.") + + torque_control = car_params.lateralTuning.which() == "torque" + hyundai_canfd = bool(getattr(car_params, "flags", 0) & HyundaiFlags.CANFD) + capabilities = get_ftm_capabilities( + car_params.carFingerprint, + brand=str(getattr(car_params, "brand", "") or ""), + hyundai_canfd=hyundai_canfd, + torque_control=torque_control, + ) + current_params = _current_param_state(car_params, params) + + if torque_control: + summaries, summary_stats = classify_torque_samples(all_samples) + paths_payload, path_decision = build_recommendation_paths(report_id, summaries, summary_stats, capabilities, current_params, feedback) + primary_path = next((path for path in paths_payload if path.get("isPrimary")), paths_payload[0] if paths_payload else {}) + suggestions = list(primary_path.get("suggestions", [])) + profiles = [profile for path in paths_payload for profile in path.get("profiles", [])] + else: + summary_stats = {"sampleCount": len(all_samples), "qlogFallback": used_qlog} + summaries = [{ + "bucket": "angle_control_diagnostic", + "dimensionId": "angle_control_diagnostic:overall", + "direction": "center", + "speedBand": "mixed", + "count": 1, + "severity": 0.0, + "evidence": {"speedBand": "mixed", "directionBias": "center", "eventCount": 1, "segments": []}, + "events": [], + "plotSvg": "", + }] + suggestions = [{ + "dimensionId": "angle_control_diagnostic:overall", + "bucket": "angle_control_diagnostic", + "evidence": summaries[0]["evidence"], + "currentVsSuggested": None, + "observedBehavior": "This route is using an angle-control path, so the torque-specific FTM trial system stays in diagnostic mode.", + "likelyInterpretation": "You can still inspect lane behavior here, but torque-controller trial profiles do not apply.", + "primaryAdjustment": "Do not apply an FTM torque profile to this car.", + "whatNotToTouchYet": "Do not write torque-controller override blobs for an angle-control path.", + "ifThatWasWrong": "If the car later moves to torque control, re-run FTM on a fresh route.", + "plotSvg": "", + }] + path_decision = { + "primaryPathKey": "cleanup_pass", + "alternatePathKey": "baseline_fix", + "reason": "Angle-control diagnostic mode does not participate in the torque trial workflow.", + "baselineScore": 0, + } + paths_payload = [{ + "key": "cleanup_pass", + "title": "Diagnostic Only", + "description": "This route is using an angle-control path, so torque-controller trial profiles do not apply.", + "whenToUse": "Use this report only for diagnostic review.", + "alternateHint": "", + "isPrimary": True, + "whySelected": path_decision["reason"], + "suggestions": suggestions, + "profiles": [], + }] + profiles = [] + + report = { + "reportId": report_id, + "createdAt": time.time(), + "routeNames": route_names, + "warnings": warnings, + "feedback": feedback, + "car": { + "carFingerprint": str(car_params.carFingerprint), + "brand": str(getattr(car_params, "brand", "") or ""), + "controlPath": "torque" if torque_control else "angle", + "gitBranch": init_data.get("gitBranch", ""), + "gitCommit": init_data.get("gitCommit", ""), + "steerControlType": str(getattr(car_params, "steerControlType", car.CarParams.SteerControlType.torque)), + }, + "capabilities": capabilities, + "currentParams": current_params, + "summary": { + **summary_stats, + "processedSegments": processed_segments, + "usedQlogFallback": used_qlog, + }, + "primaryPathKey": path_decision["primaryPathKey"], + "pathDecision": path_decision, + "paths": paths_payload, + "findings": summaries, + "suggestions": suggestions, + "profiles": profiles, + "addTheseParametersAndStartHere": _add_parameters_start_here(capabilities, suggestions, path_decision["primaryPathKey"]), + } + + paths = ensure_ftm_workspace() + html = _render_report_html(report) + report["htmlPath"] = str(paths["reports"] / f"{report_id}.html") + report["jsonPath"] = str(paths["reports"] / f"{report_id}.json") + (paths["reports"] / f"{report_id}.html").write_text(html, encoding="utf-8") + _write_json(paths["reports"] / f"{report_id}.json", report) + _write_json(paths["profiles"] / f"{report_id}.json", profiles) + _write_ftm_status({ + "pid": os.getpid(), + "startedAt": time.time(), + "running": False, + "state": "complete", + "routes": route_names, + "progress": processed_segments, + "total": processed_segments, + "reportId": report_id, + }) + return report + + +def load_report(report_id: str) -> dict[str, Any]: + paths = ensure_ftm_workspace() + report_path = paths["reports"] / f"{report_id}.json" + report = _read_json(report_path, {}) + if not isinstance(report, dict) or not report: + raise FileNotFoundError(report_id) + html_path = paths["reports"] / f"{report_id}.html" + report["html"] = html_path.read_text(encoding="utf-8") if html_path.exists() else "" + return report + + +def list_workspace() -> dict[str, Any]: + paths = ensure_ftm_workspace() + reports = [] + for path in sorted(paths["reports"].glob("*.json"), reverse=True): + payload = _read_json(path, {}) + if not isinstance(payload, dict) or not payload: + continue + reports.append({ + "reportId": payload.get("reportId", path.stem), + "createdAt": payload.get("createdAt", path.stat().st_mtime), + "carFingerprint": payload.get("car", {}).get("carFingerprint", ""), + "routeNames": payload.get("routeNames", []), + "controlPath": payload.get("car", {}).get("controlPath", ""), + }) + feedback_files = sorted(paths["feedback"].glob("*.json"), reverse=True) + active_snapshot = _read_json(paths["snapshots"] / "active.json", {}) + return { + "reports": reports[:20], + "feedbackCount": len(feedback_files), + "activeTrial": active_snapshot if isinstance(active_snapshot, dict) and active_snapshot else None, + "status": read_ftm_status(), + } + + +def delete_report(report_id: str) -> dict[str, Any]: + paths = ensure_ftm_workspace() + active_snapshot = _read_json(paths["snapshots"] / "active.json", {}) + if isinstance(active_snapshot, dict) and active_snapshot.get("reportId") == report_id: + raise RuntimeError("Revert the active FTM trial before deleting its source report.") + + removed = [] + direct_paths = [ + paths["reports"] / f"{report_id}.json", + paths["reports"] / f"{report_id}.html", + paths["profiles"] / f"{report_id}.json", + paths["feedback"] / f"{report_id}.json", + ] + for path in direct_paths: + if path.exists(): + path.unlink() + removed.append(str(path)) + + for path in paths["snapshots"].glob(f"{report_id}-*.json"): + path.unlink() + removed.append(str(path)) + + if not removed: + raise FileNotFoundError(report_id) + + status = read_ftm_status() + if not status.get("running") and status.get("reportId") == report_id: + _clear_ftm_status() + + return { + "message": f"Deleted tuning report {report_id}.", + "removed": removed, + "workspace": list_workspace(), + } + + +def clear_workspace() -> dict[str, Any]: + paths = ensure_ftm_workspace() + status = read_ftm_status() + if status.get("running"): + raise RuntimeError("Stop the active FTM analysis before clearing the workspace.") + + active_snapshot = _read_json(paths["snapshots"] / "active.json", {}) + if isinstance(active_snapshot, dict) and active_snapshot.get("params"): + raise RuntimeError("Revert the active FTM trial before clearing the workspace.") + + removed = [] + for key in ("reports", "profiles", "feedback", "snapshots"): + for path in paths[key].glob("*"): + if path.is_file(): + path.unlink() + removed.append(str(path)) + + _clear_ftm_status() + + return { + "message": "Cleared saved tuning reports, feedback, profiles, and snapshots.", + "removedCount": len(removed), + "workspace": list_workspace(), + } + + +def _snapshot_current_trial_state(params: Params) -> dict[str, Any]: + snapshot = {} + for key, kind in TRIAL_PARAM_SPECS.items(): + if kind == "bool": + snapshot[key] = params.get_bool(key) + elif kind == "float": + snapshot[key] = params.get_float(key, return_default=True) + elif kind == "json": + snapshot[key] = normalize_ftm_overrides(params.get(key, encoding="utf-8") or "{}") + else: + snapshot[key] = params.get(key, encoding="utf-8") or "" + return snapshot + + +def _apply_param_bundle(params: Params, bundle: dict[str, Any]) -> None: + for key, value in bundle.items(): + kind = TRIAL_PARAM_SPECS.get(key) + if kind == "bool": + params.put_bool(key, bool(value)) + elif kind == "float": + params.put_float(key, float(value)) + elif kind == "json": + params.put(key, normalize_ftm_overrides(value)) + elif kind == "string": + params.put(key, str(value or "")) + + +def apply_trial_profile(report_id: str, profile_id: str) -> dict[str, Any]: + paths = ensure_ftm_workspace() + params = Params(return_defaults=True) + profiles = _read_json(paths["profiles"] / f"{report_id}.json", []) + if not isinstance(profiles, list): + raise FileNotFoundError(profile_id) + profile = next((item for item in profiles if isinstance(item, dict) and item.get("id") == profile_id), None) + if profile is None: + raise FileNotFoundError(profile_id) + + snapshot = { + "reportId": report_id, + "profileId": profile_id, + "capturedAt": time.time(), + "params": _snapshot_current_trial_state(params), + } + _write_json(paths["snapshots"] / "active.json", snapshot) + _write_json(paths["snapshots"] / f"{report_id}-{profile_id.replace(':', '_')}.json", snapshot) + + bundle = dict(profile.get("genericParams", {})) + bundle["FTMActiveProfileId"] = profile_id + bundle["FTMActiveOverrides"] = profile.get("ftmOverrides", {}) + bundle["FTMTrialApplied"] = True + _apply_param_bundle(params, bundle) + return { + "message": f"Applied {profile.get('label', 'FTM')} profile.", + "profile": profile, + } + + +def revert_trial_profile() -> dict[str, Any]: + paths = ensure_ftm_workspace() + snapshot_path = paths["snapshots"] / "active.json" + snapshot = _read_json(snapshot_path, {}) + if not isinstance(snapshot, dict) or "params" not in snapshot: + raise FileNotFoundError("active trial snapshot") + params = Params(return_defaults=True) + _apply_param_bundle(params, snapshot["params"]) + try: + snapshot_path.unlink() + except FileNotFoundError: + pass + return { + "message": "Reverted FTM trial state.", + "snapshot": snapshot, + } + + +def record_feedback(report_id: str, feedback: dict[str, Any]) -> dict[str, Any]: + paths = ensure_ftm_workspace() + normalized = { + "acceptedDimensions": [str(item) for item in feedback.get("acceptedDimensions", [])], + "ignoredDimensions": [str(item) for item in feedback.get("ignoredDimensions", [])], + "notes": str(feedback.get("notes", "") or "").strip(), + "updatedAt": time.time(), + } + _write_json(paths["feedback"] / f"{report_id}.json", normalized) + report = load_report(report_id) + report["feedback"] = normalized + if isinstance(report.get("paths"), list) and report.get("paths"): + flattened_profiles = [] + for path in report["paths"]: + if not isinstance(path, dict): + continue + profiles = build_trial_profiles( + report_id, + path.get("suggestions", []), + normalized, + report.get("capabilities", {}), + path_key=str(path.get("key", "cleanup_pass")), + path_label=str(path.get("title", "Cleanup Pass")), + ) + path["profiles"] = profiles + flattened_profiles.extend(profiles) + if path.get("isPrimary"): + report["suggestions"] = list(path.get("suggestions", [])) + report["profiles"] = flattened_profiles + else: + report["profiles"] = build_trial_profiles(report_id, report.get("suggestions", []), normalized, report.get("capabilities", {})) + (paths["reports"] / f"{report_id}.html").write_text(_render_report_html(report), encoding="utf-8") + report.pop("html", None) + _write_json(paths["reports"] / f"{report_id}.json", report) + _write_json(paths["profiles"] / f"{report_id}.json", report["profiles"]) + return { + "message": "Saved FTM feedback.", + "feedback": normalized, + "profiles": report["profiles"], + } + + +def run_worker(payload_json: str) -> None: + payload = json.loads(payload_json) + routes = [str(route) for route in payload.get("routes", [])] + footage_paths = [str(path) for path in payload.get("footagePaths", [])] + ensure_ftm_workspace() + _write_ftm_status({ + "pid": os.getpid(), + "startedAt": time.time(), + "running": True, + "state": "starting", + "routes": routes, + "progress": 0, + "total": len(routes), + }) + try: + report = analyze_routes(routes, footage_paths) + _write_ftm_status({ + "pid": os.getpid(), + "startedAt": time.time(), + "running": False, + "state": "complete", + "routes": routes, + "progress": len(routes), + "total": len(routes), + "reportId": report["reportId"], + }) + except Exception as error: + _write_ftm_status({ + "pid": os.getpid(), + "startedAt": time.time(), + "running": False, + "state": "failed", + "routes": routes, + "progress": 0, + "total": len(routes), + "error": str(error), + }) + raise + + +def main() -> None: + if len(sys.argv) >= 3 and sys.argv[1] == "worker": + run_worker(sys.argv[2]) + return + if len(sys.argv) >= 2 and sys.argv[1] == "analyze": + routes = sys.argv[2:] + footage_paths = [str(Paths.log_root(HD=True, raw=True)), str(Paths.log_root(konik=True, raw=True)), str(Paths.log_root(raw=True))] + report = analyze_routes(routes, footage_paths) + print(json.dumps({"reportId": report["reportId"], "htmlPath": report["htmlPath"], "jsonPath": report["jsonPath"]}, indent=2)) + return + print("Usage: ftm_workspace.py analyze [...]") + + +if __name__ == "__main__": + main() diff --git a/starpilot/system/the_galaxy/templates/index.html b/starpilot/system/the_galaxy/templates/index.html index 9df2cf344..24176b351 100644 --- a/starpilot/system/the_galaxy/templates/index.html +++ b/starpilot/system/the_galaxy/templates/index.html @@ -34,6 +34,7 @@ + @@ -44,7 +45,7 @@