mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-04 15:13:58 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
2db8d959052d1829d61d6bcaa451a845cd2a9bf1
StarPilot/selfdrive/controls/lib/latcontrol_vehicle_tunes.py
T
firestar5683 cfd221b89c fine day, Sunday.
2026-08-23 10:40:20 -05:00

4243 lines
234 KiB
Python
Raw Blame History

import ast
import json
import math
import numpy as np
from opendbc.car.chrysler.values import CAR as CHRYSLER_CAR
from opendbc.car.gm.values import CAR as GM_CAR
from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR
from opendbc.car.subaru.values import CAR as SUBARU_CAR
from opendbc.car.toyota.values import CAR as TOYOTA_CAR
from openpilot.common.constants import CV
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
FLM_SCHEMA_VERSION = 1
FLM_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
CIVIC_BOSCH_MODIFIED_B_TRANSITION_SPEED = 12.0
CIVIC_BOSCH_MODIFIED_B_PHASE_SCALE = 0.08
CIVIC_BOSCH_MODIFIED_B_FF_ONSET = 0.18
CIVIC_BOSCH_MODIFIED_B_FF_ONSET_WIDTH = 0.07
CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF = 1.35
CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF_WIDTH = 0.38
CIVIC_BOSCH_MODIFIED_B_FF_REDUCTION_LEFT = 0.14
CIVIC_BOSCH_MODIFIED_B_FF_REDUCTION_RIGHT = 0.24
CIVIC_BOSCH_MODIFIED_B_TURN_IN_BOOST_LEFT = 0.04
CIVIC_BOSCH_MODIFIED_B_TURN_IN_BOOST_RIGHT = 0.00
CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_LEFT = 0.40
CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_RIGHT = 0.60
CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_LEFT = 0.02
CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_RIGHT = 0.00
CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_LEFT = 0.26
CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_RIGHT = 0.40
CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_LEFT = -0.10
CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_RIGHT = 0.02
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_LEFT = -0.05
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_RIGHT = 0.02
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_LEFT = 0.24
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_RIGHT = 0.06
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = -0.02
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.01
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.22
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.05
CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_MAX = 0.12
CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT = 0.24
CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT_WIDTH = 0.05
CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_SPEED = 18.0
CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_SPEED_WIDTH = 2.5
CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT = 0.50
CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT = 0.82
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT = 0.00
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_RIGHT = 0.00
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT = 4.40
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_RIGHT = 6.20
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.00
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.00
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 2.80
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 4.20
BOLT_2022_2023_CARS = (
GM_CAR.CHEVROLET_BOLT_ACC_2022_2023,
GM_CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL,
GM_CAR.CHEVROLET_BOLT_CC_2022_2023,
)
BOLT_2018_2021_CARS = (
GM_CAR.CHEVROLET_BOLT_CC_2018_2021,
)
BOLT_2017_CARS = (
GM_CAR.CHEVROLET_BOLT_CC_2017,
)
BOLT_CARS = BOLT_2022_2023_CARS + BOLT_2018_2021_CARS + BOLT_2017_CARS
HONDA_ACCORD_STEER_RATIO_SCALE = 14.0 / 16.33
HONDA_ACCORD_TORQUE_KP = 0.8
HONDA_ACCORD_TORQUE_KI = 0.15
HONDA_ACCORD_TURN_FF_REDUCTION_MAX = 0.10
HONDA_ACCORD_TURN_FF_ONSET = 0.45
HONDA_ACCORD_TURN_FF_WIDTH = 0.12
VOLT_STANDARD_CARS = (
GM_CAR.CHEVROLET_VOLT,
GM_CAR.CHEVROLET_VOLT_2019,
GM_CAR.CHEVROLET_VOLT_ASCM,
GM_CAR.CHEVROLET_VOLT_CAMERA,
GM_CAR.CHEVROLET_VOLT_CC,
)
SILVERADO_CARS = (
GM_CAR.CHEVROLET_SILVERADO,
GM_CAR.CHEVROLET_SILVERADO_CC,
)
GMC_YUKON_CC_CARS = (
GM_CAR.GMC_YUKON_CC,
)
GENESIS_G90_CARS = (
HYUNDAI_CAR.GENESIS_G90,
)
GENESIS_G70_CARS = (
HYUNDAI_CAR.GENESIS_G70_2020,
)
GENESIS_GV70_CARS = (
HYUNDAI_CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN,
)
PALISADE_CARS = (
HYUNDAI_CAR.HYUNDAI_PALISADE,
HYUNDAI_CAR.HYUNDAI_PALISADE_2023,
)
IONIQ_5_CARS = (
HYUNDAI_CAR.HYUNDAI_IONIQ_5,
)
IONIQ_EV_OLD_CARS = (
HYUNDAI_CAR.HYUNDAI_IONIQ_EV_LTD,
HYUNDAI_CAR.HYUNDAI_IONIQ_EV_2020,
)
IONIQ_6_CARS = (
HYUNDAI_CAR.HYUNDAI_IONIQ_6,
)
def is_ioniq_6_2025_model(CP) -> bool:
"""Identify the newer Ioniq 6 firmware without changing the legacy 2023 path."""
if getattr(CP, "carFingerprint", None) not in IONIQ_6_CARS:
return False
versions = []
try:
for fw in CP.carFw:
value = fw.fwVersion
versions.append(value.decode("ascii", errors="ignore") if isinstance(value, bytes) else str(value))
except (AttributeError, TypeError, ValueError):
return False
return any("230915" in version for version in versions) and any("240206" in version for version in versions)
SONATA_HYBRID_CARS = (
HYUNDAI_CAR.HYUNDAI_SONATA_HYBRID,
)
SONATA_CARS = (
HYUNDAI_CAR.HYUNDAI_SONATA,
)
ELANTRA_NON_SCC_CARS = (
HYUNDAI_CAR.HYUNDAI_ELANTRA_2022_NON_SCC,
HYUNDAI_CAR.HYUNDAI_ELANTRA_HEV_2022_NON_SCC,
)
KIA_EV6_CARS = (
HYUNDAI_CAR.KIA_EV6,
)
KIA_CARNIVAL_CARS = (
HYUNDAI_CAR.KIA_CARNIVAL_2025,
)
TUCSON_4TH_GEN_CARS = (
HYUNDAI_CAR.HYUNDAI_TUCSON_4TH_GEN,
)
KIA_XCEED_CARS = (
HYUNDAI_CAR.KIA_XCEED_PHEV,
)
KIA_NIRO_PHEV_2022_CARS = (
HYUNDAI_CAR.KIA_NIRO_PHEV_2022,
)
KIA_STINGER_2022_CARS = (
HYUNDAI_CAR.KIA_STINGER_2022,
)
KIA_FORTE_CARS = (
HYUNDAI_CAR.KIA_FORTE,
HYUNDAI_CAR.KIA_FORTE_2019_NON_SCC,
HYUNDAI_CAR.KIA_FORTE_2021_NON_SCC,
)
KONA_NON_SCC_CARS = (
HYUNDAI_CAR.HYUNDAI_KONA_NON_SCC,
)
PRIUS_CARS = (
TOYOTA_CAR.TOYOTA_PRIUS,
TOYOTA_CAR.TOYOTA_PRIUS_RETROFIT,
)
CAMRY_CARS = (
TOYOTA_CAR.TOYOTA_CAMRY,
)
RAV4_PRIME_CARS = (
TOYOTA_CAR.TOYOTA_RAV4_PRIME,
)
SIENNA_4TH_GEN_CARS = (
TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN,
)
TOYOTA_HIGHLANDER_TSS2_CARS = (
TOYOTA_CAR.TOYOTA_HIGHLANDER_TSS2,
)
TOYOTA_COROLLA_TSS2_CARS = (
TOYOTA_CAR.TOYOTA_COROLLA_TSS2,
)
LEXUS_IS_CARS = (
TOYOTA_CAR.LEXUS_IS,
)
SUBARU_IMPREZA_CARS = (
SUBARU_CAR.SUBARU_IMPREZA,
)
RAM_1500_CARS = (
CHRYSLER_CAR.RAM_1500_5TH_GEN,
)
RAM_1500_BASE_LAT_ACCEL_FACTOR_MULT = 1.0
GENESIS_GV70_FRICTION_THRESHOLD_GAIN = 0.12
GENESIS_GV70_FRICTION_SPEED_ONSET = 8.0 * CV.MPH_TO_MS
GENESIS_GV70_FRICTION_SPEED_ONSET_WIDTH = 4.0 * CV.MPH_TO_MS
GENESIS_GV70_FRICTION_SPEED_CUTOFF = 60.0 * CV.MPH_TO_MS
GENESIS_GV70_FRICTION_SPEED_CUTOFF_WIDTH = 10.0 * CV.MPH_TO_MS
GENESIS_GV70_FRICTION_CENTER_LAT = 0.28
GENESIS_GV70_FRICTION_CENTER_LAT_WIDTH = 0.12
GENESIS_GV70_FRICTION_CALM_JERK = 0.35
GENESIS_GV70_FRICTION_CALM_JERK_WIDTH = 0.10
GENESIS_GV70_UNWIND_FF_REDUCTION_MAX = 0.35
GENESIS_GV70_UNWIND_FF_OVERSHOOT = 0.15
GENESIS_GV70_UNWIND_FF_OVERSHOOT_WIDTH = 0.18
GENESIS_GV70_UNWIND_FF_JERK = 0.10
GENESIS_GV70_UNWIND_FF_JERK_WIDTH = 0.10
GENESIS_GV70_UNWIND_FF_SPEED = 10.0 * CV.MPH_TO_MS
GENESIS_GV70_UNWIND_FF_SPEED_WIDTH = 4.0 * CV.MPH_TO_MS
GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_MAX = 0.18
GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_SPEED = 50.0 * CV.MPH_TO_MS
GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_SPEED_WIDTH = 8.0 * CV.MPH_TO_MS
GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_ERROR = 0.18
GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_ERROR_WIDTH = 0.15
GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_JERK = 0.15
GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_JERK_WIDTH = 0.10
GENESIS_G70_FRICTION_THRESHOLD_GAIN = 0.10
GENESIS_G70_FRICTION_SPEED_ONSET = 10.0
GENESIS_G70_FRICTION_SPEED_ONSET_WIDTH = 3.0
GENESIS_G70_FRICTION_SPEED_CUTOFF = 35.0
GENESIS_G70_FRICTION_SPEED_CUTOFF_WIDTH = 6.0
GENESIS_G70_FRICTION_CENTER_LAT = 0.28
GENESIS_G70_FRICTION_CENTER_LAT_WIDTH = 0.10
GENESIS_G70_FRICTION_CALM_JERK = 0.35
GENESIS_G70_FRICTION_CALM_JERK_WIDTH = 0.10
GENESIS_G70_FRICTION_JERK_DEADZONE_MAX = 0.30
GENESIS_G70_FRICTION_JERK_DEADZONE_LAT = 0.30
GENESIS_G70_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.08
GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED = 12.0
GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 3.5
GENESIS_G70_CENTER_OUTPUT_TAPER_MAX = 0.14
GENESIS_G70_CENTER_OUTPUT_TAPER_LAT = 0.30
GENESIS_G70_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.10
GENESIS_G70_CENTER_OUTPUT_TAPER_SPEED = 18.0
GENESIS_G70_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 3.0
GENESIS_G70_LOW_SPEED_CENTER_TAPER_MAX = 0.06
GENESIS_G70_LOW_SPEED_CENTER_TAPER_LAT = 0.14
GENESIS_G70_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.05
GENESIS_G70_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 7.5
GENESIS_G70_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.2
GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_MAX = 0.24
GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_SPEED = 6.0
GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_SPEED_WIDTH = 1.5
GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_ERROR = 7.0
GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_ERROR_WIDTH = 3.0
GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_ACTUAL = 8.0
GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_ACTUAL_WIDTH = 4.0
GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_BLEND = 0.50
GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_REDUCTION = 0.85
GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT = 0.14
GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT_WIDTH = 0.05
GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED = 6.0
GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH = 1.5
GENESIS_G70_CURVE_UNWIND_OUTPUT_BOOST = 0.00
GENESIS_G70_CURVE_UNWIND_SPEED = 18.0
GENESIS_G70_CURVE_UNWIND_SPEED_WIDTH = 3.0
GENESIS_G70_CURVE_UNWIND_LAT = 0.25
GENESIS_G70_CURVE_UNWIND_LAT_WIDTH = 0.12
GENESIS_G70_CURVE_UNWIND_JERK = 0.08
GENESIS_G70_CURVE_UNWIND_JERK_WIDTH = 0.08
GENESIS_G70_UNWIND_FF_REDUCTION_MAX = 0.28
GENESIS_G70_UNWIND_FF_OVERSHOOT = 0.12
GENESIS_G70_UNWIND_FF_OVERSHOOT_WIDTH = 0.12
GENESIS_G70_UNWIND_FF_JERK = 0.10
GENESIS_G70_UNWIND_FF_JERK_WIDTH = 0.10
GENESIS_G70_UNWIND_FF_SPEED = 18.0
GENESIS_G70_UNWIND_FF_SPEED_WIDTH = 3.0
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_MAX = 0.15
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_SPEED = 50.0 * CV.MPH_TO_MS
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_SPEED_WIDTH = 8.0 * CV.MPH_TO_MS
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR = 0.18
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR_WIDTH = 0.15
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK = 0.15
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK_WIDTH = 0.10
BOLT_2017_LATERAL_TESTING_GROUND_ID = testing_ground.id_3
BOLT_2017_STEER_RATIO_TEST_SCALE = 1.045
BOLT_2017_STEER_RATIO_ONSET_SPEED = 20.0 * CV.MPH_TO_MS
BOLT_2017_STEER_RATIO_ONSET_WIDTH = 4.0 * CV.MPH_TO_MS
BOLT_2017_CENTER_TAPER_LAT = 0.10
BOLT_2017_CENTER_TAPER_WIDTH = 0.03
BOLT_2017_CENTER_TAPER_GAIN = 0.055
BOLT_2017_TORQUE_SCALE_BP = [0.0, 0.2, 0.5, 1.0, 1.5, 2.5]
BOLT_2017_TORQUE_SCALE_LEFT = [1.0, 1.0, 1.065, 1.060, 1.055, 1.045]
BOLT_2017_TORQUE_SCALE_RIGHT = [1.0, 1.0, 1.035, 1.020, 0.995, 0.985]
BOLT_2017_TRANSITION_SPEED = 10.0
BOLT_2017_PHASE_SCALE = 0.12
BOLT_2017_TURN_IN_BOOST_LEFT = 0.28
BOLT_2017_TURN_IN_BOOST_RIGHT = 0.18
BOLT_2017_UNWIND_TAPER_LEFT = 0.08
BOLT_2017_UNWIND_TAPER_RIGHT = 0.28
BOLT_2018_2021_LATERAL_TESTING_GROUND_ID = testing_ground.id_4
BOLT_2018_2021_STEER_RATIO_TEST_SCALE = 1.01
BOLT_2018_2021_TORQUE_GAIN_LEFT = 0.090
BOLT_2018_2021_TORQUE_GAIN_RIGHT = 0.050
BOLT_2018_2021_TORQUE_ONSET = 0.18
BOLT_2018_2021_TORQUE_ONSET_WIDTH = 0.08
BOLT_2018_2021_TORQUE_CUTOFF = 1.05
BOLT_2018_2021_TORQUE_CUTOFF_WIDTH = 0.24
BOLT_2018_2021_JERK_TAPER_CUTOFF = 0.42
BOLT_2018_2021_CENTER_TAPER_LAT = 0.12
BOLT_2018_2021_CENTER_TAPER_WIDTH = 0.04
BOLT_2018_2021_CENTER_TAPER_GAIN = 0.35
BOLT_2018_2021_TRANSITION_SPEED = 8.5
BOLT_2018_2021_PHASE_SCALE = 0.10
BOLT_2018_2021_TURN_IN_BOOST_LEFT = 0.22
BOLT_2018_2021_TURN_IN_BOOST_RIGHT = 0.12
BOLT_2018_2021_UNWIND_TAPER_GAIN_LEFT = 0.80
BOLT_2018_2021_UNWIND_TAPER_GAIN_RIGHT = 1.04
BOLT_2018_2021_FRICTION_MULT = 1.01
BOLT_2018_2021_FRICTION_LAT_RISE = 0.24
BOLT_2018_2021_FRICTION_JERK_RISE = 0.28
BOLT_2018_2021_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.16
BOLT_2018_2021_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.16
BOLT_2018_2021_UNWIND_THRESHOLD_INCREASE_LEFT = 0.15
BOLT_2018_2021_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.25
BOLT_2018_2021_TURN_IN_FRICTION_BOOST_LEFT = 0.08
BOLT_2018_2021_TURN_IN_FRICTION_BOOST_RIGHT = 0.08
BOLT_2018_2021_UNWIND_FRICTION_REDUCTION_LEFT = 0.17
BOLT_2018_2021_UNWIND_FRICTION_REDUCTION_RIGHT = 0.27
BOLT_2022_2023_LATERAL_TESTING_GROUND_ID = testing_ground.id_5
BOLT_2022_2023_FF_GAIN_LEFT = 0.11
BOLT_2022_2023_FF_GAIN_RIGHT = 0.06
BOLT_2022_2023_FF_ONSET = 0.12
BOLT_2022_2023_FF_ONSET_WIDTH = 0.07
BOLT_2022_2023_FF_CUTOFF = 1.35
BOLT_2022_2023_FF_CUTOFF_WIDTH = 0.28
BOLT_2022_2023_TRANSITION_SPEED = 9.0
BOLT_2022_2023_PHASE_SCALE = 0.12
BOLT_2022_2023_TURN_IN_BOOST_LEFT = 0.18
BOLT_2022_2023_TURN_IN_BOOST_RIGHT = 0.13
BOLT_2022_2023_UNWIND_TAPER_LEFT = 0.38
BOLT_2022_2023_UNWIND_TAPER_RIGHT = 0.40
BOLT_2022_2023_FRICTION_MULT = 1.09
BOLT_2022_2023_FRICTION_LAT_RISE = 0.22
BOLT_2022_2023_FRICTION_JERK_RISE = 0.26
BOLT_2022_2023_CENTER_TAPER_MAX = 0.11
BOLT_2022_2023_CENTER_TAPER_LAT = 0.18
BOLT_2022_2023_CENTER_TAPER_LAT_WIDTH = 0.03
BOLT_2022_2023_CENTER_TAPER_SPEED = 25.0
BOLT_2022_2023_CENTER_TAPER_SPEED_WIDTH = 2.5
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_MAX = 0.12
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_LAT = 0.14
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.04
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED = 4.0
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.5
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_FLOOR = 2.0
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_FLOOR_WIDTH = 0.7
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 16.5
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_MAX_WIDTH = 2.0
BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_LIMIT = 0.38
BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_LAT = 0.17
BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_LAT_WIDTH = 0.04
BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED = 2.5
BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_WIDTH = 0.7
BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_MAX = 7.2
BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_MAX_WIDTH = 0.5
BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_BUMP = 0.080
BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_LAT = 0.18
BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.06
BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_SPEED = 6.7
BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 1.5
BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.16
BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.12
BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_LEFT = 0.26
BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.28
BOLT_2022_2023_TURN_IN_FRICTION_BOOST_LEFT = 0.10
BOLT_2022_2023_TURN_IN_FRICTION_BOOST_RIGHT = 0.07
BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_LEFT = 0.27
BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_RIGHT = 0.28
VOLT_STANDARD_LATERAL_TESTING_GROUND_ID = testing_ground.id_3
VOLT_STANDARD_FF_GAIN_LEFT = 0.13
VOLT_STANDARD_FF_GAIN_RIGHT = 0.06
VOLT_STANDARD_FF_ONSET = 0.10
VOLT_STANDARD_FF_ONSET_WIDTH = 0.05
VOLT_STANDARD_FF_CUTOFF = 1.38
VOLT_STANDARD_FF_CUTOFF_WIDTH = 0.28
VOLT_STANDARD_TRANSITION_SPEED = 10.0
VOLT_STANDARD_PHASE_SCALE = 0.10
VOLT_STANDARD_TURN_IN_BOOST_LEFT = 0.20
VOLT_STANDARD_TURN_IN_BOOST_RIGHT = 0.20
VOLT_STANDARD_UNWIND_TAPER_LEFT = 0.20
VOLT_STANDARD_UNWIND_TAPER_RIGHT = 0.20
VOLT_STANDARD_FRICTION_MULT = 1.00
VOLT_STANDARD_FRICTION_LAT_RISE = 0.20
VOLT_STANDARD_FRICTION_JERK_RISE = 0.20
VOLT_STANDARD_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.05
VOLT_STANDARD_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.05
VOLT_STANDARD_UNWIND_THRESHOLD_INCREASE_LEFT = 0.05
VOLT_STANDARD_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.05
VOLT_STANDARD_TURN_IN_FRICTION_BOOST_LEFT = 0.00
VOLT_STANDARD_TURN_IN_FRICTION_BOOST_RIGHT = 0.00
VOLT_STANDARD_UNWIND_FRICTION_REDUCTION_LEFT = 0.05
VOLT_STANDARD_UNWIND_FRICTION_REDUCTION_RIGHT = 0.05
VOLT_STANDARD_CENTER_TAPER_MAX = 0.12
VOLT_STANDARD_CENTER_TAPER_LAT = 0.10
VOLT_STANDARD_CENTER_TAPER_LAT_WIDTH = 0.018
VOLT_STANDARD_CENTER_TAPER_SPEED = 20.0
VOLT_STANDARD_CENTER_TAPER_SPEED_WIDTH = 2.5
SILVERADO_CENTER_TAPER_MAX = 0.22
SILVERADO_CENTER_TAPER_LAT = 0.18
SILVERADO_CENTER_TAPER_LAT_WIDTH = 0.05
SILVERADO_CENTER_TAPER_SPEED = 12.0
SILVERADO_CENTER_TAPER_SPEED_WIDTH = 2.5
GMC_YUKON_CC_PHASE_SCALE = 0.14
GMC_YUKON_CC_PHASE_SPEED_ONSET = 12.0
GMC_YUKON_CC_PHASE_SPEED_FULL = 30.0
GMC_YUKON_CC_PHASE_LAT_ONSET = 0.35
GMC_YUKON_CC_PHASE_LAT_WIDTH = 0.18
GMC_YUKON_CC_TURN_IN_FF_BOOST = 0.08
GMC_YUKON_CC_UNWIND_FF_REDUCTION = 0.12
SONATA_HYBRID_BASE_LAT_ACCEL_FACTOR_MULT = 1.05
SONATA_HYBRID_FF_REDUCTION_LEFT = 0.09
SONATA_HYBRID_FF_REDUCTION_RIGHT = 0.26
SONATA_HYBRID_FF_ONSET = 0.18
SONATA_HYBRID_FF_ONSET_WIDTH = 0.08
SONATA_HYBRID_FF_CUTOFF = 1.35
SONATA_HYBRID_FF_CUTOFF_WIDTH = 0.40
SONATA_HYBRID_TRANSITION_SPEED = 8.0
SONATA_HYBRID_PHASE_SCALE = 0.12
SONATA_HYBRID_TURN_IN_BOOST_LEFT = 0.12
SONATA_HYBRID_TURN_IN_BOOST_RIGHT = 0.02
SONATA_HYBRID_UNWIND_TAPER_LEFT = 0.18
SONATA_HYBRID_UNWIND_TAPER_RIGHT = 0.10
SONATA_HYBRID_CENTER_TAPER_MAX = 0.10
SONATA_HYBRID_CENTER_TAPER_LAT = 0.16
SONATA_HYBRID_CENTER_TAPER_LAT_WIDTH = 0.025
SONATA_HYBRID_CENTER_TAPER_SPEED = 22.0
SONATA_HYBRID_CENTER_TAPER_SPEED_WIDTH = 2.5
SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_MAX = 0.14
SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_LAT = 0.10
SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.02
SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 7.5
SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.0
SONATA_HYBRID_CENTER_OUTPUT_TAPER_MAX = 0.08
SONATA_HYBRID_CENTER_OUTPUT_TAPER_LAT = 0.18
SONATA_HYBRID_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.05
SONATA_HYBRID_CENTER_OUTPUT_TAPER_SPEED = 12.5
SONATA_HYBRID_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 2.5
SONATA_FF_REDUCTION_LEFT = 0.04
SONATA_FF_REDUCTION_RIGHT = 0.26
SONATA_FF_ONSET = 0.18
SONATA_FF_ONSET_WIDTH = 0.08
SONATA_FF_CUTOFF = 1.40
SONATA_FF_CUTOFF_WIDTH = 0.42
SONATA_TRANSITION_SPEED = 8.5
SONATA_PHASE_SCALE = 0.12
SONATA_TURN_IN_BOOST_LEFT = 0.18
SONATA_TURN_IN_BOOST_RIGHT = 0.00
SONATA_UNWIND_TAPER_LEFT = 0.28
SONATA_UNWIND_TAPER_RIGHT = 0.00
SONATA_CENTER_TAPER_MAX = 0.04
SONATA_CENTER_TAPER_LAT = 0.15
SONATA_CENTER_TAPER_LAT_WIDTH = 0.025
SONATA_CENTER_TAPER_SPEED = 22.0
SONATA_CENTER_TAPER_SPEED_WIDTH = 2.5
SONATA_LOW_SPEED_CENTER_TAPER_MAX = 0.08
SONATA_LOW_SPEED_CENTER_TAPER_LAT = 0.10
SONATA_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.02
SONATA_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 7.0
SONATA_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.0
ELANTRA_NON_SCC_FF_ADJUST_LEFT = 0.02
ELANTRA_NON_SCC_FF_ADJUST_RIGHT = -0.02
ELANTRA_NON_SCC_FF_ONSET = 0.14
ELANTRA_NON_SCC_FF_ONSET_WIDTH = 0.06
ELANTRA_NON_SCC_FF_CUTOFF = 1.10
ELANTRA_NON_SCC_FF_CUTOFF_WIDTH = 0.34
ELANTRA_NON_SCC_TRANSITION_SPEED = 8.0
ELANTRA_NON_SCC_PHASE_SCALE = 0.10
ELANTRA_NON_SCC_TURN_IN_BOOST_LEFT = 0.10
ELANTRA_NON_SCC_TURN_IN_BOOST_RIGHT = 0.12
ELANTRA_NON_SCC_UNWIND_TAPER_LEFT = 0.22
ELANTRA_NON_SCC_UNWIND_TAPER_RIGHT = 0.12
KIA_XCEED_FF_REDUCTION_LEFT = 0.04
KIA_XCEED_FF_REDUCTION_RIGHT = 0.10
KIA_XCEED_FF_ONSET = 0.16
KIA_XCEED_FF_ONSET_WIDTH = 0.06
KIA_XCEED_FF_CUTOFF = 1.30
KIA_XCEED_FF_CUTOFF_WIDTH = 0.36
KIA_XCEED_TRANSITION_SPEED = 11.5
KIA_XCEED_PHASE_SCALE = 0.10
KIA_XCEED_TURN_IN_BOOST_LEFT = 0.26
KIA_XCEED_TURN_IN_BOOST_RIGHT = 0.06
KIA_XCEED_UNWIND_TAPER_LEFT = 0.20
KIA_XCEED_UNWIND_TAPER_RIGHT = 0.14
KIA_XCEED_CENTER_TAPER_MAX = 0.06
KIA_XCEED_CENTER_TAPER_LAT = 0.12
KIA_XCEED_CENTER_TAPER_LAT_WIDTH = 0.03
KIA_XCEED_CENTER_TAPER_SPEED = 17.5
KIA_XCEED_CENTER_TAPER_SPEED_WIDTH = 2.5
KIA_NIRO_PHEV_2022_CENTER_TAPER_MAX = 0.06
KIA_NIRO_PHEV_2022_CENTER_TAPER_LAT = 0.12
KIA_NIRO_PHEV_2022_CENTER_TAPER_LAT_WIDTH = 0.03
KIA_NIRO_PHEV_2022_CENTER_TAPER_SPEED = 23.0
KIA_NIRO_PHEV_2022_CENTER_TAPER_SPEED_WIDTH = 2.5
KIA_NIRO_PHEV_2022_FRICTION_CENTER_LAT = 0.12
KIA_NIRO_PHEV_2022_FRICTION_CENTER_LAT_WIDTH = 0.03
KIA_NIRO_PHEV_2022_FRICTION_SPEED = 23.0
KIA_NIRO_PHEV_2022_FRICTION_SPEED_WIDTH = 2.5
KIA_NIRO_PHEV_2022_FRICTION_CALM_JERK = 0.22
KIA_NIRO_PHEV_2022_FRICTION_CALM_JERK_WIDTH = 0.06
KIA_NIRO_PHEV_2022_FRICTION_THRESHOLD_GAIN = 0.12
KIA_STINGER_2022_CENTER_TAPER_MAX = 0.12
KIA_STINGER_2022_CENTER_TAPER_LAT = 0.30
KIA_STINGER_2022_CENTER_TAPER_LAT_WIDTH = 0.05
KIA_STINGER_2022_CENTER_TAPER_SPEED = 10.0
KIA_STINGER_2022_CENTER_TAPER_SPEED_WIDTH = 2.5
KIA_STINGER_2022_FRICTION_THRESHOLD_GAIN = 0.10
KIA_CARNIVAL_CENTER_TAPER_MAX = 0.20
KIA_CARNIVAL_CENTER_TAPER_LAT = 0.20
KIA_CARNIVAL_CENTER_TAPER_LAT_WIDTH = 0.055
KIA_CARNIVAL_CENTER_TAPER_SPEED = 3.5
KIA_CARNIVAL_CENTER_TAPER_SPEED_WIDTH = 1.8
KIA_CARNIVAL_CENTER_TAPER_SPEED_MAX = 14.5
KIA_CARNIVAL_CENTER_TAPER_SPEED_MAX_WIDTH = 2.0
KIA_CARNIVAL_FRICTION_THRESHOLD_GAIN = 0.24
KIA_CARNIVAL_FRICTION_CENTER_FADE_MAX = 0.34
KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_MAX = 0.14
KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_LAT = 0.24
KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_LAT_WIDTH = 0.06
KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_SPEED = 28.0
KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_SPEED_WIDTH = 2.0
KIA_CARNIVAL_HIGHWAY_FRICTION_THRESHOLD_GAIN = 0.14
KIA_CARNIVAL_HIGHWAY_FRICTION_CENTER_FADE_MAX = 0.20
KIA_CARNIVAL_HIGHWAY_TRANSITION_TAPER_MAX = 0.28
KIA_CARNIVAL_HIGHWAY_TRANSITION_JERK = 0.45
KIA_CARNIVAL_HIGHWAY_TRANSITION_JERK_WIDTH = 0.15
KIA_CARNIVAL_HIGHWAY_TRANSITION_LAT_CUTOFF = 1.20
KIA_CARNIVAL_HIGHWAY_TRANSITION_LAT_WIDTH = 0.20
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_MAX = 0.34
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_SPEED = 15.0
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 2.0
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_SPEED_CUTOFF = 23.0
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_SPEED_CUTOFF_WIDTH = 2.0
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_LAT = 0.35
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.18
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_JERK = 0.65
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_JERK_WIDTH = 0.25
KIA_CARNIVAL_UNWIND_FF_REDUCTION_MAX = 0.45
KIA_CARNIVAL_UNWIND_FF_SPEED = 15.0
KIA_CARNIVAL_UNWIND_FF_SPEED_WIDTH = 2.0
KIA_CARNIVAL_UNWIND_FF_SPEED_CUTOFF = 23.0
KIA_CARNIVAL_UNWIND_FF_SPEED_CUTOFF_WIDTH = 2.0
KIA_CARNIVAL_UNWIND_FF_OVERSHOOT = 0.20
KIA_CARNIVAL_UNWIND_FF_OVERSHOOT_WIDTH = 0.12
KIA_CARNIVAL_UNWIND_FF_JERK = 0.65
KIA_CARNIVAL_UNWIND_FF_JERK_WIDTH = 0.25
TUCSON_4TH_GEN_CENTER_TAPER_MAX = 0.44
TUCSON_4TH_GEN_CENTER_TAPER_LAT = 0.28
TUCSON_4TH_GEN_CENTER_TAPER_LAT_WIDTH = 0.055
TUCSON_4TH_GEN_CENTER_TAPER_SPEED_MAX = 14.0
TUCSON_4TH_GEN_CENTER_TAPER_SPEED_WIDTH = 1.5
TUCSON_4TH_GEN_FRICTION_THRESHOLD_GAIN = 0.28
KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT = 1.05
KIA_FORTE_FF_REDUCTION_LEFT = 0.05
KIA_FORTE_FF_REDUCTION_RIGHT = 0.10
KIA_FORTE_FF_ONSET = 0.16
KIA_FORTE_FF_ONSET_WIDTH = 0.06
KIA_FORTE_FF_CUTOFF = 1.20
KIA_FORTE_FF_CUTOFF_WIDTH = 0.36
KIA_FORTE_TRANSITION_SPEED = 9.0
KIA_FORTE_PHASE_SCALE = 0.10
KIA_FORTE_TURN_IN_BOOST_LEFT = 0.10
KIA_FORTE_TURN_IN_BOOST_RIGHT = 0.05
KIA_FORTE_UNWIND_TAPER_LEFT = 0.18
KIA_FORTE_UNWIND_TAPER_RIGHT = 0.02
KIA_FORTE_CRAWL_TURN_IN_FF_BOOST_LEFT = 0.10
KIA_FORTE_CRAWL_TURN_IN_FF_BOOST_RIGHT = 0.14
KIA_FORTE_CRAWL_TURN_IN_FF_SPEED = 4.5
KIA_FORTE_CRAWL_TURN_IN_FF_SPEED_WIDTH = 0.8
KIA_FORTE_CRAWL_TURN_IN_FF_LAT = 0.10
KIA_FORTE_CRAWL_TURN_IN_FF_LAT_WIDTH = 0.05
KIA_FORTE_CENTER_TAPER_MAX = 0.12
KIA_FORTE_CENTER_TAPER_LAT = 0.18
KIA_FORTE_CENTER_TAPER_LAT_WIDTH = 0.04
KIA_FORTE_CENTER_TAPER_SPEED = 22.5
KIA_FORTE_CENTER_TAPER_SPEED_WIDTH = 3.0
KIA_FORTE_FRICTION_CENTER_LAT = 0.18
KIA_FORTE_FRICTION_CENTER_LAT_WIDTH = 0.04
KIA_FORTE_FRICTION_SPEED = 24.0
KIA_FORTE_FRICTION_SPEED_WIDTH = 3.0
KIA_FORTE_FRICTION_CALM_JERK = 0.24
KIA_FORTE_FRICTION_CALM_JERK_WIDTH = 0.08
KIA_FORTE_FRICTION_THRESHOLD_GAIN = 0.10
PALISADE_BASE_LAT_ACCEL_FACTOR_MULT = 0.98
PALISADE_FF_GAIN_LEFT = 0.14
PALISADE_FF_GAIN_RIGHT = 0.12
PALISADE_FF_ONSET = 0.08
PALISADE_FF_ONSET_WIDTH = 0.04
PALISADE_FF_CUTOFF = 1.25
PALISADE_FF_CUTOFF_WIDTH = 0.36
PALISADE_TRANSITION_SPEED = 9.0
PALISADE_PHASE_SCALE = 0.11
PALISADE_TURN_IN_BOOST_LEFT = 0.34
PALISADE_TURN_IN_BOOST_RIGHT = 0.24
PALISADE_UNWIND_TAPER_LEFT = 0.18
PALISADE_UNWIND_TAPER_RIGHT = 0.30
PALISADE_FRICTION_MULT = 1.02
PALISADE_FRICTION_LAT_RISE = 0.20
PALISADE_FRICTION_JERK_RISE = 0.24
PALISADE_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.18
PALISADE_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.14
PALISADE_UNWIND_THRESHOLD_INCREASE_LEFT = 0.14
PALISADE_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.22
PALISADE_TURN_IN_FRICTION_BOOST_LEFT = 0.08
PALISADE_TURN_IN_FRICTION_BOOST_RIGHT = 0.06
PALISADE_UNWIND_FRICTION_REDUCTION_LEFT = 0.12
PALISADE_UNWIND_FRICTION_REDUCTION_RIGHT = 0.20
PALISADE_CENTER_TAPER_MAX = 0.12
PALISADE_CENTER_TAPER_LAT = 0.28
PALISADE_CENTER_TAPER_LAT_WIDTH = 0.055
PALISADE_CENTER_TAPER_SPEED = 12.0
PALISADE_CENTER_TAPER_SPEED_WIDTH = 2.5
PALISADE_CENTER_OUTPUT_TAPER_MAX = 0.10
PALISADE_CENTER_OUTPUT_TAPER_LAT = 0.28
PALISADE_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.055
PALISADE_CENTER_OUTPUT_TAPER_SPEED = 18.0
PALISADE_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 2.5
GENESIS_G90_LATERAL_TESTING_GROUND_ID = testing_ground.id_4
GENESIS_G90_FF_GAIN_LEFT = 0.32
GENESIS_G90_FF_GAIN_RIGHT = 0.16
GENESIS_G90_FF_ONSET = 0.10
GENESIS_G90_FF_ONSET_WIDTH = 0.05
GENESIS_G90_FF_CUTOFF = 2.35
GENESIS_G90_FF_CUTOFF_WIDTH = 0.48
GENESIS_G90_TRANSITION_SPEED = 10.0
GENESIS_G90_PHASE_SCALE = 0.12
GENESIS_G90_TURN_IN_BOOST_LEFT = 0.66
GENESIS_G90_TURN_IN_BOOST_RIGHT = 0.50
GENESIS_G90_UNWIND_TAPER_LEFT = 0.10
GENESIS_G90_UNWIND_TAPER_RIGHT = 0.55
GENESIS_G90_FRICTION_MULT = 1.02
GENESIS_G90_FRICTION_LAT_RISE = 0.22
GENESIS_G90_FRICTION_JERK_RISE = 0.24
GENESIS_G90_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.28
GENESIS_G90_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.22
GENESIS_G90_UNWIND_THRESHOLD_INCREASE_LEFT = 0.06
GENESIS_G90_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.32
GENESIS_G90_TURN_IN_FRICTION_BOOST_LEFT = 0.16
GENESIS_G90_TURN_IN_FRICTION_BOOST_RIGHT = 0.14
GENESIS_G90_UNWIND_FRICTION_REDUCTION_LEFT = 0.04
GENESIS_G90_UNWIND_FRICTION_REDUCTION_RIGHT = 0.30
IONIQ_5_BASE_LAT_ACCEL_FACTOR_MULT = 1.22
IONIQ_5_FF_ONSET = 0.10
IONIQ_5_FF_ONSET_WIDTH = 0.05
IONIQ_5_FF_CUTOFF = 1.20
IONIQ_5_FF_CUTOFF_WIDTH = 0.30
IONIQ_5_TRANSITION_SPEED = 12.5
IONIQ_5_PHASE_SCALE = 0.10
IONIQ_5_FF_REDUCTION_LEFT = 0.15
IONIQ_5_FF_REDUCTION_RIGHT = 0.25
IONIQ_5_TURN_IN_BOOST_LEFT = 0.10
IONIQ_5_TURN_IN_BOOST_RIGHT = 0.04
IONIQ_5_UNWIND_TAPER_LEFT = 0.92
IONIQ_5_UNWIND_TAPER_RIGHT = 1.04
IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.05
IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.03
IONIQ_5_UNWIND_THRESHOLD_INCREASE_LEFT = 0.46
IONIQ_5_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.50
IONIQ_5_TURN_IN_FRICTION_BOOST_LEFT = 0.02
IONIQ_5_TURN_IN_FRICTION_BOOST_RIGHT = 0.01
IONIQ_5_UNWIND_FRICTION_REDUCTION_LEFT = 0.42
IONIQ_5_UNWIND_FRICTION_REDUCTION_RIGHT = 0.44
IONIQ_5_CENTER_TAPER_MAX = 0.18
IONIQ_5_CENTER_TAPER_LAT = 0.16
IONIQ_5_CENTER_TAPER_LAT_WIDTH = 0.04
IONIQ_5_CENTER_TAPER_SPEED = 15.0
IONIQ_5_CENTER_TAPER_SPEED_WIDTH = 2.2
IONIQ_5_SUSTAINED_TURN_IN_FF_BOOST_LEFT = 0.10
IONIQ_5_SUSTAINED_TURN_IN_FF_BOOST_RIGHT = 0.16
IONIQ_5_SUSTAINED_TURN_IN_FF_SPEED = 13.5
IONIQ_5_SUSTAINED_TURN_IN_FF_SPEED_WIDTH = 1.8
IONIQ_5_SUSTAINED_TURN_IN_FF_LAT_START = 1.10
IONIQ_5_SUSTAINED_TURN_IN_FF_LAT_END = 3.60
IONIQ_5_SUSTAINED_TURN_IN_FF_LAT_WIDTH = 0.30
IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_BASE = 0.05
IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_TURN_RELIEF = 0.95
IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_SPEED = 8.0
IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH = 0.8
IONIQ_5_LOW_SPEED_CENTER_LAT = 0.40
IONIQ_5_LOW_SPEED_CENTER_LAT_WIDTH = 0.10
IONIQ_5_LOW_SPEED_CENTER_JERK = 0.40
IONIQ_5_LOW_SPEED_CENTER_JERK_WIDTH = 0.12
IONIQ_5_FRICTION_JERK_DEADZONE_MAX = 0.36
IONIQ_5_FRICTION_JERK_DEADZONE_LAT = 1.25
IONIQ_5_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.35
IONIQ_5_FRICTION_JERK_DEADZONE_SPEED = 18.0
IONIQ_5_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 3.0
IONIQ_EV_OLD_BASE_LAT_ACCEL_FACTOR_MULT = 1.16
IONIQ_EV_OLD_FF_REDUCTION_LEFT = 0.16
IONIQ_EV_OLD_FF_REDUCTION_RIGHT = 0.30
IONIQ_EV_OLD_FF_ONSET = 0.14
IONIQ_EV_OLD_FF_ONSET_WIDTH = 0.05
IONIQ_EV_OLD_FF_CUTOFF = 1.10
IONIQ_EV_OLD_FF_CUTOFF_WIDTH = 0.30
IONIQ_EV_OLD_TRANSITION_SPEED = 10.0
IONIQ_EV_OLD_PHASE_SCALE = 0.10
IONIQ_EV_OLD_TURN_IN_BOOST_LEFT = 0.01
IONIQ_EV_OLD_TURN_IN_BOOST_RIGHT = 0.00
IONIQ_EV_OLD_UNWIND_TAPER_LEFT = 0.26
IONIQ_EV_OLD_UNWIND_TAPER_RIGHT = 0.06
IONIQ_EV_OLD_CENTER_TAPER_MAX = 0.14
IONIQ_EV_OLD_CENTER_TAPER_LAT = 0.12
IONIQ_EV_OLD_CENTER_TAPER_LAT_WIDTH = 0.03
IONIQ_EV_OLD_CENTER_TAPER_SPEED = 22.0
IONIQ_EV_OLD_CENTER_TAPER_SPEED_WIDTH = 2.2
IONIQ_6_FF_GAIN_LEFT = 0.045
IONIQ_6_FF_GAIN_RIGHT = 0.015
IONIQ_6_BASE_LAT_ACCEL_FACTOR_MULT = 1.22
IONIQ_6_BASE_FRICTION_THRESHOLD = HKG_CANFD_BASE_FRICTION_THRESHOLD
IONIQ_6_FF_ONSET = 0.10
IONIQ_6_FF_ONSET_WIDTH = 0.04
IONIQ_6_FF_CUTOFF = 0.48
IONIQ_6_FF_CUTOFF_WIDTH = 0.12
IONIQ_6_TRANSITION_SPEED = 10.0
IONIQ_6_PHASE_SCALE = 0.10
IONIQ_6_TURN_IN_BOOST_LEFT = 1.64
IONIQ_6_TURN_IN_BOOST_RIGHT = 2.10
IONIQ_6_UNWIND_TAPER_LEFT = 3.18
IONIQ_6_UNWIND_TAPER_RIGHT = 8.20
IONIQ_6_FRICTION_MULT = 0.928
IONIQ_6_FRICTION_LAT_RISE = 0.20
IONIQ_6_FRICTION_JERK_RISE = 0.24
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.78
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 1.42
IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 3.90
IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 10.20
IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.44
IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.94
IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 3.55
IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 9.10
IONIQ_6_CENTER_TAPER_MAX = 0.082
IONIQ_6_CENTER_TAPER_LAT = 0.24
IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.025
IONIQ_6_CENTER_TAPER_SPEED = 18.0
IONIQ_6_CENTER_TAPER_SPEED_WIDTH = 2.5
IONIQ_6_HIGHWAY_CENTER_TAPER_MAX = 0.046
IONIQ_6_HIGHWAY_CENTER_TAPER_LAT = 0.10
IONIQ_6_HIGHWAY_CENTER_TAPER_LAT_WIDTH = 0.035
IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED = 24.5
IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED_WIDTH = 1.8
IONIQ_6_HIGHWAY_OUTPUT_TAPER_MAX = 0.10
IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT = 0.14
IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT_WIDTH = 0.04
IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED = 23.5
IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED_WIDTH = 2.0
IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_MAX = 0.18
IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT = 1.05
IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT_WIDTH = 0.22
IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK = 0.24
IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK_WIDTH = 0.14
IONIQ_6_LOW_MID_CENTER_TAPER_MAX = 0.088
IONIQ_6_LOW_MID_CENTER_TAPER_LAT = 0.28
IONIQ_6_LOW_MID_CENTER_TAPER_LAT_WIDTH = 0.06
IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MIN = 8.5
IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MAX = 16.5
IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_WIDTH = 1.5
IONIQ_6_DIRECTIONAL_TAPER_LAT_START = 0.19
IONIQ_6_DIRECTIONAL_TAPER_LAT_END = 0.90
IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH = 0.06
IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT = 0.11
IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT = 0.45
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 1.10
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 2.10
IONIQ_6_DIRECTIONAL_TAPER_FLOOR_LEFT = 0.48
IONIQ_6_DIRECTIONAL_TAPER_FLOOR_RIGHT = 0.52
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT = 0.20
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT = 0.10
IONIQ_6_DIRECTIONAL_TAPER_JERK_ONSET = 1.00
IONIQ_6_DIRECTIONAL_TAPER_JERK_WIDTH = 0.30
# Unwind detection needs a softer phase transition and time smoothing than the shared
# PHASE_SCALE: desired lateral jerk noise in a sustained curve (~+/-1-2 m/s^3) otherwise
# chatters the taper between its base value and its floor at ~0.5 Hz (felt as notchy
# steering in highway sweepers).
IONIQ_6_DIRECTIONAL_TAPER_PHASE_SCALE = 0.45
IONIQ_6_DIRECTIONAL_TAPER_FILTER_RC = 0.4
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF = 0.98
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED = 11.2
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED_WIDTH = 1.5
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT = 0.10
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT_WIDTH = 0.06
IONIQ_6_UNWIND_HIGH_SPEED_SPEED = 23.2
IONIQ_6_UNWIND_HIGH_SPEED_SPEED_WIDTH = 1.7
IONIQ_6_CRAWL_TURN_IN_FF_BOOST_LEFT = 0.18
IONIQ_6_CRAWL_TURN_IN_FF_BOOST_RIGHT = 0.24
IONIQ_6_CRAWL_TURN_IN_FF_SPEED = 5.3
IONIQ_6_CRAWL_TURN_IN_FF_SPEED_WIDTH = 1.0
IONIQ_6_CRAWL_TURN_IN_FF_LAT = 0.06
IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH = 0.035
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_MAX_TORQUE = 0.46
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED = 3.25
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED_WIDTH = 0.45
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR = 1.9
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR_WIDTH = 1.20
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE = 5.5
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE_WIDTH = 2.4
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_START = 0.66
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_WIDTH = 0.12
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_FLOOR = 0.26
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_BP = [0.0, 0.35, 0.65, 1.0]
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_V = [1.0, 1.0, 0.88, 0.08]
IONIQ_6_LOW_SPEED_UNWIND_ASSIST_MAX_TORQUE = 0.30
IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED = 3.35
IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED_WIDTH = 0.50
IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR = 1.6
IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR_WIDTH = 0.95
IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ACTUAL_ANGLE = 10.5
IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ACTUAL_ANGLE_WIDTH = 4.0
IONIQ_6_LOW_SPEED_UNWIND_ASSIST_BLEND = 0.52
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_BOOST = 0.10
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED = 18.0
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED_WIDTH = 2.5
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_START = 0.06
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_END = 0.22
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH = 0.035
IONIQ_6_CURVY_SPEED_MIN = 7.2
IONIQ_6_CURVY_SPEED_MAX = 21.5
IONIQ_6_CURVY_SPEED_MIN_WIDTH = 1.1
IONIQ_6_CURVY_SPEED_MAX_WIDTH = 1.8
IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_LEFT = 0.26
IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_RIGHT = 0.30
IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_LEFT = 0.22
IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_RIGHT = 0.28
IONIQ_6_CURVY_UNWIND_LAT_START = 0.45
IONIQ_6_CURVY_UNWIND_LAT_END = 3.6
IONIQ_6_CURVY_UNWIND_LAT_ONSET_WIDTH = 0.14
IONIQ_6_CURVY_UNWIND_LAT_CUTOFF_WIDTH = 0.55
IONIQ_6_CURVY_RIGHT_UNWIND_JERK_ONSET = 0.40
IONIQ_6_CURVY_RIGHT_UNWIND_JERK_WIDTH = 0.22
IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_MIN = 11.5
IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_MAX = 20.5
IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_WIDTH = 1.2
IONIQ_6_CURVY_TURN_IN_TRIM_LEFT = 0.08
IONIQ_6_CURVY_TURN_IN_TRIM_RIGHT = 0.09
IONIQ_6_CURVY_TURN_IN_TRIM_LAT_START = 1.0
IONIQ_6_CURVY_TURN_IN_TRIM_LAT_END = 2.5
IONIQ_6_CURVY_TURN_IN_TRIM_LAT_ONSET_WIDTH = 0.18
IONIQ_6_CURVY_TURN_IN_TRIM_LAT_CUTOFF_WIDTH = 0.30
IONIQ_6_2023_UNWIND_FF_REDUCTION_MAX = 0.24
IONIQ_6_2023_UNWIND_FF_OVERSHOOT = 0.15
IONIQ_6_2023_UNWIND_FF_OVERSHOOT_WIDTH = 0.18
IONIQ_6_2023_UNWIND_FF_JERK = 0.10
IONIQ_6_2023_UNWIND_FF_JERK_WIDTH = 0.10
IONIQ_6_2023_UNWIND_FF_SPEED_ONSET = 8.0
IONIQ_6_2023_UNWIND_FF_SPEED_ONSET_WIDTH = 2.5
IONIQ_6_2023_UNWIND_FF_SPEED_CUTOFF = 23.5
IONIQ_6_2023_UNWIND_FF_SPEED_CUTOFF_WIDTH = 2.0
IONIQ_6_LOW_SPEED_PID_RESET_SPEED = 0.1 * CV.MPH_TO_MS
# Friction compensation near zero lateral accel amplifies planner jerk noise into a slow
# (~0.5 Hz) weave on straights: the 0.09/0.39 small-signal slope plus the jerk feed acts as
# extra P/D gain right where there is no breakaway torque to overcome. Deadzone the jerk
# feed below straight-line noise levels and fade friction near center at highway speed.
IONIQ_6_FRICTION_JERK_DEADZONE = 0.30
IONIQ_6_FRICTION_CENTER_FADE_MAX = 0.50
IONIQ_6_FRICTION_CENTER_FADE_LAT = 0.15
IONIQ_6_FRICTION_CENTER_FADE_LAT_WIDTH = 0.06
IONIQ_6_FRICTION_CENTER_FADE_SPEED = 18.0
IONIQ_6_FRICTION_CENTER_FADE_SPEED_WIDTH = 2.5
# Newer Ioniq 6 highway center-chatter correction; activation is firmware-gated.
IONIQ_6_2025_FRICTION_SCALE_MULT = 0.80
IONIQ_6_2025_FRICTION_JERK_DEADZONE = 0.45
IONIQ_6_2025_CENTER_OUTPUT_TAPER_MAX = 0.32
IONIQ_6_2025_CENTER_OUTPUT_TAPER_LAT = 0.35
IONIQ_6_2025_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.10
IONIQ_6_2025_CENTER_OUTPUT_TAPER_SPEED = 22.0
IONIQ_6_2025_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 2.5
IONIQ_6_2025_LOW_SPEED_CENTER_ERROR_SCALE = 0.68
IONIQ_6_2025_LOW_SPEED_CENTER_FRICTION_SCALE = 0.72
IONIQ_6_2025_LOW_SPEED_CENTER_SPEED = 5.0
IONIQ_6_2025_LOW_SPEED_CENTER_SPEED_WIDTH = 1.3
IONIQ_6_2025_LOW_SPEED_CENTER_LAT = 0.22
IONIQ_6_2025_LOW_SPEED_CENTER_LAT_WIDTH = 0.10
IONIQ_6_2025_LOW_SPEED_CENTER_JERK = 0.30
IONIQ_6_2025_LOW_SPEED_CENTER_JERK_WIDTH = 0.13
IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_BASE = 0.22
IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_TURN_RELIEF = 0.50
IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_SPEED_RELIEF = 0.20
IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_SPEED = 6.5
IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH = 1.5
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START = 0.90
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH = 0.18
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT = 0.03
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_RIGHT = 0.11
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.40
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT = 0.55
IONIQ_6_OUTPUT_TAPER_SPEED = 8.5
IONIQ_6_OUTPUT_TAPER_SPEED_WIDTH = 2.5
IONIQ_6_OUTPUT_CENTER_TAPER_BLEND = 0.90
IONIQ_6_OUTPUT_DIRECTIONAL_TAPER_BLEND = 0.97
KIA_EV6_LATERAL_TESTING_GROUND_ID = testing_ground.id_6
KIA_EV6_LATERAL_TESTING_GROUND_VARIANT = "C"
KIA_EV6_FF_GAIN_LEFT = 0.12
KIA_EV6_FF_GAIN_RIGHT = 0.17
KIA_EV6_FF_ONSET = 0.08
KIA_EV6_FF_ONSET_WIDTH = 0.04
KIA_EV6_FF_CUTOFF = 1.90
KIA_EV6_FF_CUTOFF_WIDTH = 0.40
KIA_EV6_TRANSITION_SPEED = 14.5
KIA_EV6_PHASE_SCALE = 0.09
KIA_EV6_TURN_IN_BOOST_LEFT = 0.54
KIA_EV6_TURN_IN_BOOST_RIGHT = 0.60
KIA_EV6_UNWIND_TAPER_LEFT = 0.56
KIA_EV6_UNWIND_TAPER_RIGHT = 0.54
KIA_EV6_BASE_UNWIND_TAPER_LEFT = 0.10
KIA_EV6_BASE_UNWIND_TAPER_RIGHT = 0.13
KIA_EV6_JWARM_BASE_TURN_IN_BOOST_LEFT = 0.12
KIA_EV6_JWARM_BASE_TURN_IN_BOOST_RIGHT = 0.14
KIA_EV6_JWARM_BASE_UNWIND_TAPER_LEFT = 0.15
KIA_EV6_JWARM_BASE_UNWIND_TAPER_RIGHT = 0.16
KIA_EV6_JWARM_PHASE_STABILITY_MAX_REDUCTION = 0.25
KIA_EV6_JWARM_PHASE_STABILITY_SPEED = 10.0
KIA_EV6_JWARM_PHASE_STABILITY_SPEED_WIDTH = 1.8
KIA_EV6_JWARM_PHASE_STABILITY_JERK = 0.70
KIA_EV6_JWARM_PHASE_STABILITY_JERK_WIDTH = 0.18
KIA_EV6_FRICTION_MULT = 1.01
KIA_EV6_FRICTION_LAT_RISE = 0.18
KIA_EV6_FRICTION_JERK_RISE = 0.22
KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.34
KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.40
KIA_EV6_UNWIND_THRESHOLD_INCREASE_LEFT = 0.28
KIA_EV6_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.24
KIA_EV6_TURN_IN_FRICTION_BOOST_LEFT = 0.18
KIA_EV6_TURN_IN_FRICTION_BOOST_RIGHT = 0.24
KIA_EV6_UNWIND_FRICTION_REDUCTION_LEFT = 0.28
KIA_EV6_UNWIND_FRICTION_REDUCTION_RIGHT = 0.22
KIA_EV6_CENTER_TAPER_MAX = 0.12
KIA_EV6_CENTER_TAPER_LAT = 0.16
KIA_EV6_CENTER_TAPER_LAT_WIDTH = 0.04
KIA_EV6_CENTER_TAPER_SPEED = 17.0
KIA_EV6_CENTER_TAPER_SPEED_WIDTH = 2.8
KIA_EV6_CENTER_FRICTION_THRESHOLD_GAIN = 0.14
KIA_EV6_CENTER_FRICTION_THRESHOLD_LAT = 0.30
KIA_EV6_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.07
KIA_EV6_CENTER_FRICTION_THRESHOLD_SPEED = 18.0
KIA_EV6_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 2.5
KIA_EV6_LOW_SPEED_CENTER_TAPER_MAX = 0.19
KIA_EV6_LOW_SPEED_CENTER_TAPER_LAT = 0.08
KIA_EV6_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.02
KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 8.5
KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.4
KIA_EV6_CENTER_OUTPUT_TAPER_MAX = 0.14
KIA_EV6_CENTER_OUTPUT_TAPER_LAT = 0.30
KIA_EV6_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.08
KIA_EV6_CENTER_OUTPUT_TAPER_SPEED = 12.0
KIA_EV6_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 2.4
VOLT_PLEXY_LATERAL_TESTING_GROUND_ID = testing_ground.id_7
VOLT_PLEXY_FF_EXTRA_MULT_LEFT = 1.07
VOLT_PLEXY_FF_EXTRA_MULT_RIGHT = 1.12
VOLT_PLEXY_FRICTION_SCALE_MULT_LEFT = 1.04
VOLT_PLEXY_FRICTION_SCALE_MULT_RIGHT = 1.05
VOLT_PLEXY_FRICTION_THRESHOLD_MULT_LEFT = 0.97
VOLT_PLEXY_FRICTION_THRESHOLD_MULT_RIGHT = 0.96
VOLT_PLEXY_CENTER_TAPER_REDUCTION_MULT = 1.02
PRIUS_TRANSITION_SPEED = 11.0
PRIUS_PHASE_SCALE = 0.09
PRIUS_FF_GAIN_LEFT = 0.06
PRIUS_FF_GAIN_RIGHT = 0.07
PRIUS_FF_ONSET = 0.15
PRIUS_FF_ONSET_WIDTH = 0.08
PRIUS_FF_CUTOFF = 1.30
PRIUS_FF_CUTOFF_WIDTH = 0.32
PRIUS_FRICTION_LAT_RISE = 0.18
PRIUS_FRICTION_JERK_RISE = 0.22
PRIUS_TURN_IN_BOOST_LEFT = 0.24
PRIUS_TURN_IN_BOOST_RIGHT = 0.22
PRIUS_UNWIND_TAPER_LEFT = 0.42
PRIUS_UNWIND_TAPER_RIGHT = 0.62
PRIUS_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.14
PRIUS_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.14
PRIUS_UNWIND_THRESHOLD_INCREASE_LEFT = 0.40
PRIUS_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.56
PRIUS_TURN_IN_FRICTION_BOOST_LEFT = 0.06
PRIUS_TURN_IN_FRICTION_BOOST_RIGHT = 0.06
PRIUS_UNWIND_FRICTION_REDUCTION_LEFT = 0.22
PRIUS_UNWIND_FRICTION_REDUCTION_RIGHT = 0.30
PRIUS_CENTER_TAPER_MAX = 0.155
PRIUS_CENTER_TAPER_LAT = 0.24
PRIUS_CENTER_TAPER_LAT_WIDTH = 0.035
PRIUS_CENTER_TAPER_SPEED = 18.0
PRIUS_CENTER_TAPER_SPEED_WIDTH = 2.2
PRIUS_CENTER_FRICTION_THRESHOLD_GAIN = 0.08
PRIUS_CENTER_FRICTION_THRESHOLD_LAT = 0.30
PRIUS_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.07
PRIUS_CENTER_FRICTION_THRESHOLD_SPEED = 18.0
PRIUS_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 2.2
PRIUS_FRICTION_JERK_DEADZONE_MAX = 0.24
PRIUS_FRICTION_JERK_DEADZONE_LAT = 0.30
PRIUS_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.07
PRIUS_FRICTION_JERK_DEADZONE_SPEED = 18.0
PRIUS_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 2.2
PRIUS_HIGH_SPEED_OUTPUT_TAPER_MAX = 0.06
PRIUS_HIGH_SPEED_OUTPUT_TAPER_LAT = 0.30
PRIUS_HIGH_SPEED_OUTPUT_TAPER_LAT_WIDTH = 0.35
PRIUS_HIGH_SPEED_OUTPUT_TAPER_SPEED = 22.0
PRIUS_HIGH_SPEED_OUTPUT_TAPER_SPEED_WIDTH = 2.5
CAMRY_CENTER_FRICTION_THRESHOLD_GAIN = 0.09
CAMRY_CENTER_FRICTION_THRESHOLD_LAT = 0.22
CAMRY_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.06
CAMRY_CENTER_FRICTION_THRESHOLD_SPEED = 18.0
CAMRY_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 3.0
CAMRY_UNWIND_FF_REDUCTION = 0.08
CAMRY_UNWIND_LAT_ONSET = 0.18
CAMRY_UNWIND_LAT_WIDTH = 0.07
CAMRY_UNWIND_SPEED_ONSET = 15.0
CAMRY_UNWIND_SPEED_WIDTH = 3.0
RAV4_PRIME_PHASE_SCALE = 0.12
RAV4_PRIME_TURN_IN_FF_BOOST_LEFT = 0.055
RAV4_PRIME_TURN_IN_FF_BOOST_RIGHT = 0.040
RAV4_PRIME_UNWIND_FF_REDUCTION_LEFT = 0.18
RAV4_PRIME_UNWIND_FF_REDUCTION_RIGHT = 0.19
RAV4_PRIME_UNWIND_FRICTION_REDUCTION_LEFT = 0.19
RAV4_PRIME_UNWIND_FRICTION_REDUCTION_RIGHT = 0.20
RAV4_PRIME_UNWIND_OUTPUT_REDUCTION_LEFT = 0.185
RAV4_PRIME_UNWIND_OUTPUT_REDUCTION_RIGHT = 0.225
RAV4_PRIME_HARD_UNWIND_OUTPUT_REDUCTION_LEFT = 0.04
RAV4_PRIME_HARD_UNWIND_OUTPUT_REDUCTION_RIGHT = 0.025
RAV4_PRIME_HARD_UNWIND_LAT = 1.25
RAV4_PRIME_HARD_UNWIND_LAT_WIDTH = 0.20
RAV4_PRIME_FRICTION_THRESHOLD_GAIN = 0.30
RAV4_PRIME_FRICTION_CENTER_LAT = 0.30
RAV4_PRIME_FRICTION_CENTER_LAT_WIDTH = 0.07
RAV4_PRIME_SPEED_ONSET = 5.0
RAV4_PRIME_SPEED_ONSET_WIDTH = 1.5
RAV4_PRIME_SPEED_MAX = 20.0
RAV4_PRIME_SPEED_MAX_WIDTH = 2.5
SIENNA_4TH_GEN_PHASE_SCALE = 0.12
SIENNA_4TH_GEN_TURN_IN_FF_BOOST = 0.06
SIENNA_4TH_GEN_TURN_IN_LAT = 0.24
SIENNA_4TH_GEN_TURN_IN_LAT_WIDTH = 0.08
SIENNA_4TH_GEN_TURN_IN_SPEED_ONSET = 3.0
SIENNA_4TH_GEN_TURN_IN_SPEED_WIDTH = 1.5
SIENNA_4TH_GEN_TURN_IN_SPEED_MAX = 20.0
SIENNA_4TH_GEN_TURN_IN_SPEED_MAX_WIDTH = 2.0
SIENNA_4TH_GEN_FRICTION_THRESHOLD_GAIN = 0.30
SIENNA_4TH_GEN_FRICTION_CENTER_LAT = 0.30
SIENNA_4TH_GEN_FRICTION_CENTER_LAT_WIDTH = 0.07
SIENNA_4TH_GEN_FRICTION_SPEED_ONSET = 3.0
SIENNA_4TH_GEN_FRICTION_SPEED_WIDTH = 1.5
SIENNA_4TH_GEN_FRICTION_SPEED_MAX = 14.0
SIENNA_4TH_GEN_FRICTION_SPEED_MAX_WIDTH = 2.0
SIENNA_4TH_GEN_HIGH_SPEED_FRICTION_THRESHOLD_GAIN = 0.14
SIENNA_4TH_GEN_HIGH_SPEED_FRICTION_SPEED_ONSET = 18.0
SIENNA_4TH_GEN_HIGH_SPEED_FRICTION_SPEED_WIDTH = 2.5
SIENNA_4TH_GEN_CENTER_TAPER_MAX = 0.12
SIENNA_4TH_GEN_CENTER_TAPER_LAT = 0.20
SIENNA_4TH_GEN_CENTER_TAPER_LAT_WIDTH = 0.06
SIENNA_4TH_GEN_CENTER_TAPER_SPEED_MAX = 13.0
SIENNA_4TH_GEN_CENTER_TAPER_SPEED_WIDTH = 2.0
SIENNA_4TH_GEN_HIGH_SPEED_CENTER_TAPER_MAX = 0.16
SIENNA_4TH_GEN_HIGH_SPEED_CENTER_TAPER_ONSET = 15.0
SIENNA_4TH_GEN_HIGH_SPEED_CENTER_TAPER_ONSET_WIDTH = 2.0
SIENNA_4TH_GEN_HIGH_SPEED_CENTER_TAPER_MAX_SPEED = 27.0
SIENNA_4TH_GEN_HIGH_SPEED_CENTER_TAPER_MAX_SPEED_WIDTH = 3.0
SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_MAX = 0.10
SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_ONSET = 18.0
SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_ONSET_WIDTH = 2.0
SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_MAX_SPEED = 27.0
SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_MAX_SPEED_WIDTH = 3.0
TOYOTA_COROLLA_TSS2_PHASE_SCALE = 0.12
TOYOTA_COROLLA_TSS2_TURN_IN_FF_BOOST = 0.035
TOYOTA_COROLLA_TSS2_UNWIND_FF_REDUCTION = 0.06
TOYOTA_COROLLA_TSS2_CURVE_LAT_ONSET = 0.24
TOYOTA_COROLLA_TSS2_CURVE_LAT_WIDTH = 0.10
TOYOTA_COROLLA_TSS2_SPEED_ONSET = 4.0
TOYOTA_COROLLA_TSS2_SPEED_ONSET_WIDTH = 1.5
TOYOTA_COROLLA_TSS2_SPEED_CUTOFF = 24.0
TOYOTA_COROLLA_TSS2_SPEED_CUTOFF_WIDTH = 3.0
TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_MAX = 0.30
TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_LAT = 0.18
TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.08
TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED = 4.5
TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 1.5
TOYOTA_HIGHLANDER_TSS2_PHASE_SCALE = 0.12
TOYOTA_HIGHLANDER_TSS2_UNWIND_FF_REDUCTION = 0.10
TOYOTA_HIGHLANDER_TSS2_UNWIND_FRICTION_THRESHOLD_GAIN = 0.16
TOYOTA_HIGHLANDER_TSS2_UNWIND_FRICTION_SCALE_REDUCTION = 0.10
TOYOTA_HIGHLANDER_TSS2_UNWIND_OUTPUT_REDUCTION = 0.15
TOYOTA_HIGHLANDER_TSS2_UNWIND_LAT_ONSET = 0.20
TOYOTA_HIGHLANDER_TSS2_UNWIND_LAT_WIDTH = 0.08
TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_ONSET = 3.0
TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_WIDTH = 1.5
TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_MAX = 15.0
TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_MAX_WIDTH = 2.0
LEXUS_IS_PHASE_SCALE = 0.10
LEXUS_IS_TURN_IN_FF_BOOST_LEFT = 0.06
LEXUS_IS_TURN_IN_FF_BOOST_RIGHT = 0.06
LEXUS_IS_UNWIND_FF_REDUCTION_LEFT = 0.10
LEXUS_IS_UNWIND_FF_REDUCTION_RIGHT = 0.16
LEXUS_IS_UNWIND_LAT_ONSET = 0.18
LEXUS_IS_UNWIND_LAT_WIDTH = 0.07
LEXUS_IS_UNWIND_SPEED_ONSET = 9.0
LEXUS_IS_UNWIND_SPEED_WIDTH = 2.0
SUBARU_IMPREZA_PID_TAPER_START_DEG = 0.75
SUBARU_IMPREZA_PID_TAPER_FULL_DEG = 4.0
SUBARU_IMPREZA_PID_TAPER_MIN = 0.58
RAV4_TSS2_CARS = (
TOYOTA_CAR.TOYOTA_RAV4_TSS2,
)
RAV4_TSS2_PID_LOW_SPEED = 12.0 * CV.MPH_TO_MS
RAV4_TSS2_PID_LOW_SPEED_WIDTH = 2.0 * CV.MPH_TO_MS
RAV4_TSS2_PID_CENTER_ANGLE = 14.0
RAV4_TSS2_PID_CENTER_ANGLE_WIDTH = 3.0
RAV4_TSS2_PID_OUTPUT_SCALE_MIN = 0.62
RAV4_TSS2_PID_OUTPUT_ALPHA_MIN = 0.28
RAV4_TSS2_CENTER_FRICTION_THRESHOLD_GAIN = 0.14
RAV4_TSS2_CENTER_FRICTION_LAT = 0.30
RAV4_TSS2_CENTER_FRICTION_LAT_WIDTH = 0.08
RAV4_TSS2_CENTER_SPEED = 13.0
RAV4_TSS2_CENTER_SPEED_WIDTH = 3.0
RAV4_TSS2_CENTER_OUTPUT_TAPER_MAX = 0.12
RAV4_TSS2_CENTER_OUTPUT_LAT = 0.30
RAV4_TSS2_CENTER_OUTPUT_LAT_WIDTH = 0.08
RAM_1500_TRANSITION_TAPER_MAX = 0.34
RAM_1500_TRANSITION_SPEED_ONSET = 10.0
RAM_1500_TRANSITION_SPEED_FULL = 15.0
RAM_1500_TRANSITION_JERK_ONSET = 0.35
RAM_1500_TRANSITION_JERK_FULL = 1.10
RAM_1500_TRANSITION_LAT_FADE_START = 0.65
RAM_1500_TRANSITION_LAT_FADE_END = 1.85
RAM_1500_MAX_LAT_JERK_UP = 2.10
RAM_1500_PHASE_SCALE = 0.12
RAM_1500_PHASE_SPEED_ONSET = 8.0
RAM_1500_PHASE_SPEED_FULL = 15.0
RAM_1500_PHASE_LAT_ONSET = 0.25
RAM_1500_PHASE_LAT_WIDTH = 0.12
RAM_1500_TURN_IN_FF_BOOST = 0.06
RAM_1500_UNWIND_FF_REDUCTION = 0.05
RAM_1500_CENTER_OUTPUT_TAPER_MAX = 0.12
RAM_1500_CENTER_OUTPUT_TAPER_LAT = 0.24
RAM_1500_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.08
RAM_1500_CENTER_OUTPUT_TAPER_SPEED_ONSET = 5.5
RAM_1500_CENTER_OUTPUT_TAPER_SPEED_ONSET_WIDTH = 1.5
RAM_1500_CENTER_OUTPUT_TAPER_SPEED_MAX = 16.0
RAM_1500_CENTER_OUTPUT_TAPER_SPEED_MAX_WIDTH = 2.0
RAM_1500_UNWIND_OUTPUT_TAPER_MAX = 0.18
RAM_1500_UNWIND_OUTPUT_SPEED_ONSET = 20.0
RAM_1500_UNWIND_OUTPUT_SPEED_FULL = 29.0
RAM_1500_UNWIND_OUTPUT_JERK_ONSET = 0.50
RAM_1500_UNWIND_OUTPUT_JERK_FULL = 1.80
RAM_1500_UNWIND_OUTPUT_LAT_ONSET = 0.65
RAM_1500_UNWIND_OUTPUT_LAT_WIDTH = 0.30
# The Kona route is exceptionally accurate below highway speed, but Pop V2
# reverses the requested lateral acceleration roughly once per second at
# 29-30 m/s. Fade only rapid, high-speed turn-building torque so the EPS has
# less stored torque to unwind while leaving steady curves and counter-torque.
KONA_NON_SCC_TRANSITION_TURN_IN_TAPER_MAX = 0.24
KONA_NON_SCC_TRANSITION_UNWIND_TAPER_MAX = 0.42
KONA_NON_SCC_TRANSITION_SPEED_ONSET = 22.0
KONA_NON_SCC_TRANSITION_SPEED_FULL = 29.0
KONA_NON_SCC_TRANSITION_JERK_ONSET = 0.35
KONA_NON_SCC_TRANSITION_JERK_FULL = 1.20
KONA_NON_SCC_TRANSITION_LAT_FADE_START = 0.45
KONA_NON_SCC_TRANSITION_LAT_FADE_END = 1.60
KONA_NON_SCC_CENTER_TAPER_MAX = 0.14
KONA_NON_SCC_CENTER_TAPER_LAT = 0.28
KONA_NON_SCC_CENTER_TAPER_SPEED_ONSET = 12.0
KONA_NON_SCC_CENTER_TAPER_SPEED_FULL = 24.0
KONA_NON_SCC_CENTER_FRICTION_THRESHOLD_GAIN = 0.14
KONA_NON_SCC_CENTER_FRICTION_THRESHOLD_LAT = 0.28
KONA_NON_SCC_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.07
KONA_NON_SCC_CENTER_FRICTION_THRESHOLD_SPEED_ONSET = 11.0
KONA_NON_SCC_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 2.5
TRAILER_LOAD_FULL_ASSIST_KG = 15000.0 * CV.LB_TO_KG
TRAILER_LATERAL_MIN_SPEED = 15.0 * CV.MPH_TO_MS
TRAILER_LATERAL_FULL_SPEED = 35.0 * CV.MPH_TO_MS
TRAILER_LATERAL_LAT_RISE = 0.30
TRAILER_LATERAL_FF_GAIN = 0.05
TRAILER_LATERAL_FRICTION_GAIN = 0.03
_FLM_ACTIVE_OVERRIDES_TEXT = ""
_FLM_ACTIVE_OVERRIDES = {}
def _sigmoid(x: float) -> float:
if x >= 0.0:
z = math.exp(-x)
return 1.0 / (1.0 + z)
z = math.exp(x)
return z / (1.0 + z)
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 _flm_copy_json(value):
return json.loads(json.dumps(value))
def normalize_flm_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:
try:
overrides = ast.literal_eval(stripped)
except (SyntaxError, ValueError):
return {}
if not isinstance(overrides, dict):
return {}
normalized = {
"schemaVersion": FLM_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(FLM_FRICTION_SPEED_KNOTS):
continue
try:
normalized["baseFrictionThresholds"][family] = {
"speedKnots": list(FLM_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_flm_runtime_overrides(overrides) -> None:
global _FLM_ACTIVE_OVERRIDES, _FLM_ACTIVE_OVERRIDES_TEXT
normalized = normalize_flm_overrides(overrides)
text = json.dumps(normalized, sort_keys=True, separators=(",", ":")) if normalized else ""
if text == _FLM_ACTIVE_OVERRIDES_TEXT:
return
_FLM_ACTIVE_OVERRIDES_TEXT = text
_FLM_ACTIVE_OVERRIDES = normalized
def clear_flm_runtime_overrides() -> None:
set_flm_runtime_overrides({})
def get_flm_runtime_overrides() -> dict:
return _flm_copy_json(_FLM_ACTIVE_OVERRIDES) if _FLM_ACTIVE_OVERRIDES else {}
def flm_runtime_overrides_active() -> bool:
return bool(_FLM_ACTIVE_OVERRIDES)
def _flm_base_friction_threshold(family: str, v_ego: float, default_fn) -> float:
payload = _FLM_ACTIVE_OVERRIDES.get("baseFrictionThresholds", {}).get(family, {})
values = payload.get("values", [])
if isinstance(values, list) and len(values) == len(FLM_FRICTION_SPEED_KNOTS):
return float(np.interp(v_ego, FLM_FRICTION_SPEED_KNOTS, values))
return float(default_fn(v_ego))
def _flm_vehicle_knob(name: str, default_value: float) -> float:
try:
return float(_FLM_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 _flm_base_friction_threshold("gm", v_ego, _gm_base_friction_threshold_default)
def get_standard_friction_threshold(v_ego: float) -> float:
return _flm_base_friction_threshold("standard", v_ego, _standard_friction_threshold_default)
def get_hkg_canfd_base_friction_threshold(v_ego: float) -> float:
return _flm_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:
load_factor = np.clip(trailer_load_kg / TRAILER_LOAD_FULL_ASSIST_KG, 0.0, 1.0)
speed_factor = np.interp(v_ego, [TRAILER_LATERAL_MIN_SPEED, TRAILER_LATERAL_FULL_SPEED], [0.0, 1.0])
lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / TRAILER_LATERAL_LAT_RISE)
return float(load_factor * speed_factor * lat_factor)
def get_trailer_lateral_ff_scale(trailer_load_kg: float, v_ego: float, desired_lateral_accel: float) -> float:
return 1.0 + TRAILER_LATERAL_FF_GAIN * get_trailer_lateral_assist_factor(trailer_load_kg, v_ego, desired_lateral_accel)
def get_trailer_lateral_friction_scale(trailer_load_kg: float, v_ego: float, desired_lateral_accel: float) -> float:
return 1.0 + TRAILER_LATERAL_FRICTION_GAIN * get_trailer_lateral_assist_factor(trailer_load_kg, v_ego, desired_lateral_accel)
def _prius_sigmoid(x: float) -> float:
return _sigmoid(x)
def _prius_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / PRIUS_TRANSITION_SPEED) ** 2)
def _prius_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
return math.tanh((desired_lateral_accel * desired_lateral_jerk) / PRIUS_PHASE_SCALE)
def _prius_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def _prius_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / PRIUS_FRICTION_LAT_RISE)
jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / PRIUS_FRICTION_JERK_RISE)
return _prius_low_speed_factor(v_ego) * lat_factor * jerk_factor
def get_prius_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
gain = _prius_side_value(
desired_lateral_accel,
_flm_vehicle_knob("toyota_prius.ff_gain_left", PRIUS_FF_GAIN_LEFT),
_flm_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)
extra_scale = gain * onset * cutoff
phase = _prius_transition_phase(desired_lateral_accel, desired_lateral_jerk)
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,
_flm_vehicle_knob("toyota_prius.turn_in_boost_left", PRIUS_TURN_IN_BOOST_LEFT),
_flm_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,
_flm_vehicle_knob("toyota_prius.unwind_taper_left", PRIUS_UNWIND_TAPER_LEFT),
_flm_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))
def get_prius_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_gm_base_friction_threshold(v_ego)
transition_envelope = _prius_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
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,
_flm_vehicle_knob("toyota_prius.turn_in_threshold_reduction_left", PRIUS_TURN_IN_THRESHOLD_REDUCTION_LEFT),
_flm_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,
_flm_vehicle_knob("toyota_prius.unwind_threshold_increase_left", PRIUS_UNWIND_THRESHOLD_INCREASE_LEFT),
_flm_vehicle_knob("toyota_prius.unwind_threshold_increase_right", PRIUS_UNWIND_THRESHOLD_INCREASE_RIGHT),
) *
transition_envelope * unwind_weight)
center_speed_weight = _prius_sigmoid((v_ego - PRIUS_CENTER_FRICTION_THRESHOLD_SPEED) /
PRIUS_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH)
center_lat_weight = _prius_sigmoid((PRIUS_CENTER_FRICTION_THRESHOLD_LAT - abs(desired_lateral_accel)) /
PRIUS_CENTER_FRICTION_THRESHOLD_LAT_WIDTH)
threshold_scale += (_flm_vehicle_knob("toyota_prius.center_friction_threshold_gain",
PRIUS_CENTER_FRICTION_THRESHOLD_GAIN) *
center_speed_weight * center_lat_weight)
return base_threshold * min(max(threshold_scale, 0.86), 1.16)
def get_prius_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
transition_envelope = _prius_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _prius_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
friction_scale = 1.0
friction_scale += (_prius_side_value(desired_lateral_accel, PRIUS_TURN_IN_FRICTION_BOOST_LEFT, PRIUS_TURN_IN_FRICTION_BOOST_RIGHT) *
transition_envelope * turn_in_weight)
friction_scale -= (_prius_side_value(desired_lateral_accel, PRIUS_UNWIND_FRICTION_REDUCTION_LEFT, PRIUS_UNWIND_FRICTION_REDUCTION_RIGHT) *
transition_envelope * unwind_weight)
return min(max(friction_scale, 0.90), 1.14)
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 = _flm_vehicle_knob("toyota_prius.center_taper_max", PRIUS_CENTER_TAPER_MAX) * speed_weight * center_weight
return 1.0 - reduction
def get_prius_friction_jerk_deadzone(v_ego: float, desired_lateral_accel: float) -> float:
speed_weight = _prius_sigmoid((v_ego - PRIUS_FRICTION_JERK_DEADZONE_SPEED) /
PRIUS_FRICTION_JERK_DEADZONE_SPEED_WIDTH)
center_weight = _prius_sigmoid((PRIUS_FRICTION_JERK_DEADZONE_LAT - abs(desired_lateral_accel)) /
PRIUS_FRICTION_JERK_DEADZONE_LAT_WIDTH)
return PRIUS_FRICTION_JERK_DEADZONE_MAX * speed_weight * center_weight
def get_prius_high_speed_output_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _prius_sigmoid((v_ego - PRIUS_HIGH_SPEED_OUTPUT_TAPER_SPEED) /
PRIUS_HIGH_SPEED_OUTPUT_TAPER_SPEED_WIDTH)
curve_weight = _prius_sigmoid((abs(desired_lateral_accel) - PRIUS_HIGH_SPEED_OUTPUT_TAPER_LAT) /
PRIUS_HIGH_SPEED_OUTPUT_TAPER_LAT_WIDTH)
return 1.0 - PRIUS_HIGH_SPEED_OUTPUT_TAPER_MAX * speed_weight * curve_weight
def get_camry_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0,
desired_lateral_jerk: float = 0.0) -> float:
del desired_lateral_jerk
speed_weight = _sigmoid((v_ego - CAMRY_CENTER_FRICTION_THRESHOLD_SPEED) /
CAMRY_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH)
center_weight = _sigmoid((CAMRY_CENTER_FRICTION_THRESHOLD_LAT - abs(desired_lateral_accel)) /
CAMRY_CENTER_FRICTION_THRESHOLD_LAT_WIDTH)
gain = _flm_vehicle_knob("toyota_camry.center_friction_threshold_gain",
CAMRY_CENTER_FRICTION_THRESHOLD_GAIN)
return get_standard_friction_threshold(v_ego) * (1.0 + gain * speed_weight * center_weight)
def get_camry_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
phase = math.tanh((desired_lateral_accel * desired_lateral_jerk) / 0.10)
unwind_weight = max(-phase, 0.0)
lat_weight = _sigmoid((abs(desired_lateral_accel) - CAMRY_UNWIND_LAT_ONSET) / CAMRY_UNWIND_LAT_WIDTH)
speed_weight = _sigmoid((v_ego - CAMRY_UNWIND_SPEED_ONSET) / CAMRY_UNWIND_SPEED_WIDTH)
reduction = _flm_vehicle_knob("toyota_camry.unwind_ff_reduction", CAMRY_UNWIND_FF_REDUCTION)
return 1.0 - reduction * unwind_weight * lat_weight * speed_weight
def _rav4_prime_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def _rav4_prime_speed_weight(v_ego: float) -> float:
onset = _sigmoid((v_ego - RAV4_PRIME_SPEED_ONSET) / RAV4_PRIME_SPEED_ONSET_WIDTH)
cutoff = _sigmoid((RAV4_PRIME_SPEED_MAX - v_ego) / RAV4_PRIME_SPEED_MAX_WIDTH)
return onset * cutoff
def _rav4_prime_unwind_weight(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
phase = math.tanh((desired_lateral_accel * desired_lateral_jerk) / RAV4_PRIME_PHASE_SCALE)
lat_weight = _sigmoid((abs(desired_lateral_accel) - 0.20) / 0.07)
return max(-phase, 0.0) * lat_weight
def _rav4_prime_turn_in_weight(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
phase = math.tanh((desired_lateral_accel * desired_lateral_jerk) / RAV4_PRIME_PHASE_SCALE)
lat_weight = _sigmoid((abs(desired_lateral_accel) - 0.20) / 0.07)
return max(phase, 0.0) * lat_weight
def get_rav4_prime_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
turn_in_boost = _rav4_prime_side_value(desired_lateral_accel,
RAV4_PRIME_TURN_IN_FF_BOOST_LEFT,
RAV4_PRIME_TURN_IN_FF_BOOST_RIGHT)
reduction = _rav4_prime_side_value(desired_lateral_accel,
RAV4_PRIME_UNWIND_FF_REDUCTION_LEFT,
RAV4_PRIME_UNWIND_FF_REDUCTION_RIGHT)
speed_weight = _rav4_prime_speed_weight(v_ego)
return (1.0 + (turn_in_boost * _rav4_prime_turn_in_weight(desired_lateral_accel, desired_lateral_jerk) * speed_weight) -
(reduction * _rav4_prime_unwind_weight(desired_lateral_accel, desired_lateral_jerk) * speed_weight))
def get_rav4_prime_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0,
desired_lateral_jerk: float = 0.0) -> float:
del desired_lateral_jerk
center_weight = _sigmoid((RAV4_PRIME_FRICTION_CENTER_LAT - abs(desired_lateral_accel)) /
RAV4_PRIME_FRICTION_CENTER_LAT_WIDTH)
scale = 1.0 + (RAV4_PRIME_FRICTION_THRESHOLD_GAIN * center_weight * _rav4_prime_speed_weight(v_ego))
return get_standard_friction_threshold(v_ego) * scale
def get_rav4_prime_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
reduction = _rav4_prime_side_value(desired_lateral_accel,
RAV4_PRIME_UNWIND_FRICTION_REDUCTION_LEFT,
RAV4_PRIME_UNWIND_FRICTION_REDUCTION_RIGHT)
return 1.0 - (reduction * _rav4_prime_unwind_weight(desired_lateral_accel, desired_lateral_jerk) *
_rav4_prime_speed_weight(v_ego))
def get_rav4_prime_output_taper_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
reduction = _rav4_prime_side_value(desired_lateral_accel,
RAV4_PRIME_UNWIND_OUTPUT_REDUCTION_LEFT,
RAV4_PRIME_UNWIND_OUTPUT_REDUCTION_RIGHT)
hard_reduction = _rav4_prime_side_value(desired_lateral_accel,
RAV4_PRIME_HARD_UNWIND_OUTPUT_REDUCTION_LEFT,
RAV4_PRIME_HARD_UNWIND_OUTPUT_REDUCTION_RIGHT)
hard_curve_weight = _sigmoid((abs(desired_lateral_accel) - RAV4_PRIME_HARD_UNWIND_LAT) /
RAV4_PRIME_HARD_UNWIND_LAT_WIDTH)
reduction += hard_reduction * hard_curve_weight
return 1.0 - (reduction * _rav4_prime_unwind_weight(desired_lateral_accel, desired_lateral_jerk) *
_rav4_prime_speed_weight(v_ego))
def _sienna_4th_gen_turn_in_speed_weight(v_ego: float) -> float:
onset = _sigmoid((v_ego - SIENNA_4TH_GEN_TURN_IN_SPEED_ONSET) / SIENNA_4TH_GEN_TURN_IN_SPEED_WIDTH)
cutoff = _sigmoid((SIENNA_4TH_GEN_TURN_IN_SPEED_MAX - v_ego) / SIENNA_4TH_GEN_TURN_IN_SPEED_MAX_WIDTH)
return onset * cutoff
def _sienna_4th_gen_friction_speed_weight(v_ego: float) -> float:
onset = _sigmoid((v_ego - SIENNA_4TH_GEN_FRICTION_SPEED_ONSET) / SIENNA_4TH_GEN_FRICTION_SPEED_WIDTH)
cutoff = _sigmoid((SIENNA_4TH_GEN_FRICTION_SPEED_MAX - v_ego) / SIENNA_4TH_GEN_FRICTION_SPEED_MAX_WIDTH)
return onset * cutoff
def _sienna_4th_gen_turn_in_weight(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
phase = math.tanh((desired_lateral_accel * desired_lateral_jerk) / SIENNA_4TH_GEN_PHASE_SCALE)
lat_weight = _sigmoid((abs(desired_lateral_accel) - SIENNA_4TH_GEN_TURN_IN_LAT) /
SIENNA_4TH_GEN_TURN_IN_LAT_WIDTH)
return max(phase, 0.0) * lat_weight
def get_sienna_4th_gen_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
return 1.0 + (SIENNA_4TH_GEN_TURN_IN_FF_BOOST *
_sienna_4th_gen_turn_in_weight(desired_lateral_accel, desired_lateral_jerk) *
_sienna_4th_gen_turn_in_speed_weight(v_ego))
def get_sienna_4th_gen_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0,
desired_lateral_jerk: float = 0.0) -> float:
del desired_lateral_jerk
center_weight = _sigmoid((SIENNA_4TH_GEN_FRICTION_CENTER_LAT - abs(desired_lateral_accel)) /
SIENNA_4TH_GEN_FRICTION_CENTER_LAT_WIDTH)
high_speed_weight = _sigmoid((v_ego - SIENNA_4TH_GEN_HIGH_SPEED_FRICTION_SPEED_ONSET) /
SIENNA_4TH_GEN_HIGH_SPEED_FRICTION_SPEED_WIDTH)
return get_standard_friction_threshold(v_ego) * (
1.0 + center_weight * (
SIENNA_4TH_GEN_FRICTION_THRESHOLD_GAIN * _sienna_4th_gen_friction_speed_weight(v_ego) +
SIENNA_4TH_GEN_HIGH_SPEED_FRICTION_THRESHOLD_GAIN * high_speed_weight
)
)
def get_sienna_4th_gen_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
center_weight = _sigmoid((SIENNA_4TH_GEN_CENTER_TAPER_LAT - abs(desired_lateral_accel)) /
SIENNA_4TH_GEN_CENTER_TAPER_LAT_WIDTH)
low_speed_weight = _sigmoid((SIENNA_4TH_GEN_CENTER_TAPER_SPEED_MAX - v_ego) /
SIENNA_4TH_GEN_CENTER_TAPER_SPEED_WIDTH)
high_speed_onset = _sigmoid((v_ego - SIENNA_4TH_GEN_HIGH_SPEED_CENTER_TAPER_ONSET) /
SIENNA_4TH_GEN_HIGH_SPEED_CENTER_TAPER_ONSET_WIDTH)
high_speed_cutoff = _sigmoid((SIENNA_4TH_GEN_HIGH_SPEED_CENTER_TAPER_MAX_SPEED - v_ego) /
SIENNA_4TH_GEN_HIGH_SPEED_CENTER_TAPER_MAX_SPEED_WIDTH)
reduction = (SIENNA_4TH_GEN_CENTER_TAPER_MAX * low_speed_weight +
SIENNA_4TH_GEN_HIGH_SPEED_CENTER_TAPER_MAX * high_speed_onset * high_speed_cutoff)
return 1.0 - (reduction * center_weight)
def get_sienna_4th_gen_high_speed_output_taper_scale(v_ego: float) -> float:
onset = _sigmoid((v_ego - SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_ONSET) /
SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_ONSET_WIDTH)
cutoff = _sigmoid((SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_MAX_SPEED - v_ego) /
SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_MAX_SPEED_WIDTH)
return 1.0 - SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_MAX * onset * cutoff
def get_toyota_corolla_tss2_ff_scale(desired_lateral_accel: float,
desired_lateral_jerk: float,
v_ego: float) -> float:
"""Add a small, transition-only turn-in correction for Corolla TSS2 torque EPS."""
if desired_lateral_accel == 0.0:
return 1.0
phase = math.tanh((desired_lateral_accel * desired_lateral_jerk) /
TOYOTA_COROLLA_TSS2_PHASE_SCALE)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
curve_weight = _sigmoid((abs(desired_lateral_accel) - TOYOTA_COROLLA_TSS2_CURVE_LAT_ONSET) /
TOYOTA_COROLLA_TSS2_CURVE_LAT_WIDTH)
speed_weight = (_sigmoid((v_ego - TOYOTA_COROLLA_TSS2_SPEED_ONSET) /
TOYOTA_COROLLA_TSS2_SPEED_ONSET_WIDTH) *
_sigmoid((TOYOTA_COROLLA_TSS2_SPEED_CUTOFF - v_ego) /
TOYOTA_COROLLA_TSS2_SPEED_CUTOFF_WIDTH))
boost = _flm_vehicle_knob("toyota_corolla_tss2.turn_in_ff_boost",
TOYOTA_COROLLA_TSS2_TURN_IN_FF_BOOST)
unwind_reduction = _flm_vehicle_knob("toyota_corolla_tss2.unwind_ff_reduction",
TOYOTA_COROLLA_TSS2_UNWIND_FF_REDUCTION)
return 1.0 + curve_weight * speed_weight * (boost * turn_in_weight - unwind_reduction * unwind_weight)
def get_toyota_corolla_tss2_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
"""Taper only near-center crawl-speed torque during manual handoff."""
center_weight = _sigmoid((TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) /
TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_LAT_WIDTH)
low_speed_weight = _sigmoid((TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED - v_ego) /
TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED_WIDTH)
reduction = _flm_vehicle_knob("toyota_corolla_tss2.center_output_taper_max",
TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_MAX) * center_weight * low_speed_weight
return max(1.0 - reduction, 0.65)
def _toyota_highlander_tss2_unwind_weight(desired_lateral_accel: float,
desired_lateral_jerk: float,
v_ego: float) -> float:
phase = math.tanh((desired_lateral_accel * desired_lateral_jerk) /
TOYOTA_HIGHLANDER_TSS2_PHASE_SCALE)
curve_weight = _sigmoid((abs(desired_lateral_accel) - TOYOTA_HIGHLANDER_TSS2_UNWIND_LAT_ONSET) /
TOYOTA_HIGHLANDER_TSS2_UNWIND_LAT_WIDTH)
speed_weight = (_sigmoid((v_ego - TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_ONSET) /
TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_WIDTH) *
_sigmoid((TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_MAX - v_ego) /
TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_MAX_WIDTH))
return max(-phase, 0.0) * curve_weight * speed_weight
def get_toyota_highlander_tss2_ff_scale(desired_lateral_accel: float,
desired_lateral_jerk: float,
v_ego: float) -> float:
reduction = _flm_vehicle_knob("toyota_highlander_tss2.unwind_ff_reduction",
TOYOTA_HIGHLANDER_TSS2_UNWIND_FF_REDUCTION)
return 1.0 - reduction * _toyota_highlander_tss2_unwind_weight(
desired_lateral_accel, desired_lateral_jerk, v_ego,
)
def get_toyota_highlander_tss2_friction_threshold(v_ego: float,
desired_lateral_accel: float = 0.0,
desired_lateral_jerk: float = 0.0) -> float:
gain = _flm_vehicle_knob("toyota_highlander_tss2.unwind_friction_threshold_gain",
TOYOTA_HIGHLANDER_TSS2_UNWIND_FRICTION_THRESHOLD_GAIN)
return get_standard_friction_threshold(v_ego) * (
1.0 + gain * _toyota_highlander_tss2_unwind_weight(
desired_lateral_accel, desired_lateral_jerk, v_ego,
)
)
def get_toyota_highlander_tss2_friction_scale(v_ego: float,
desired_lateral_accel: float,
desired_lateral_jerk: float) -> float:
reduction = _flm_vehicle_knob("toyota_highlander_tss2.unwind_friction_scale_reduction",
TOYOTA_HIGHLANDER_TSS2_UNWIND_FRICTION_SCALE_REDUCTION)
return 1.0 - reduction * _toyota_highlander_tss2_unwind_weight(
desired_lateral_accel, desired_lateral_jerk, v_ego,
)
def get_toyota_highlander_tss2_output_taper_scale(desired_lateral_accel: float,
desired_lateral_jerk: float,
v_ego: float) -> float:
"""Slow low-speed unwind reversals without reducing turn-in authority."""
reduction = _flm_vehicle_knob("toyota_highlander_tss2.unwind_output_reduction",
TOYOTA_HIGHLANDER_TSS2_UNWIND_OUTPUT_REDUCTION)
return 1.0 - reduction * _toyota_highlander_tss2_unwind_weight(
desired_lateral_accel, desired_lateral_jerk, v_ego,
)
def get_lexus_is_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
phase = math.tanh((desired_lateral_accel * desired_lateral_jerk) / LEXUS_IS_PHASE_SCALE)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
lat_weight = _sigmoid((abs(desired_lateral_accel) - LEXUS_IS_UNWIND_LAT_ONSET) / LEXUS_IS_UNWIND_LAT_WIDTH)
speed_weight = _sigmoid((v_ego - LEXUS_IS_UNWIND_SPEED_ONSET) / LEXUS_IS_UNWIND_SPEED_WIDTH)
turn_in_boost = LEXUS_IS_TURN_IN_FF_BOOST_LEFT if desired_lateral_accel >= 0.0 else LEXUS_IS_TURN_IN_FF_BOOST_RIGHT
reduction = LEXUS_IS_UNWIND_FF_REDUCTION_LEFT if desired_lateral_accel >= 0.0 else LEXUS_IS_UNWIND_FF_REDUCTION_RIGHT
return (1.0 + (turn_in_boost * turn_in_weight * lat_weight * speed_weight) -
(reduction * unwind_weight * lat_weight * speed_weight))
def get_subaru_impreza_pid_output_scale(angle_error_deg: float) -> float:
error_weight = min(max((abs(angle_error_deg) - SUBARU_IMPREZA_PID_TAPER_START_DEG) /
(SUBARU_IMPREZA_PID_TAPER_FULL_DEG - SUBARU_IMPREZA_PID_TAPER_START_DEG), 0.0), 1.0)
return 1.0 - ((1.0 - SUBARU_IMPREZA_PID_TAPER_MIN) * error_weight)
def get_rav4_tss2_pid_output(output_torque: float, prev_output_torque: float,
desired_angle_deg: float, v_ego: float) -> float:
"""Damp low-speed RAV4 center reversals without blunting real turns."""
speed_weight = _sigmoid((RAV4_TSS2_PID_LOW_SPEED - max(v_ego, 0.0)) /
RAV4_TSS2_PID_LOW_SPEED_WIDTH)
center_weight = _sigmoid((RAV4_TSS2_PID_CENTER_ANGLE - abs(desired_angle_deg)) /
RAV4_TSS2_PID_CENTER_ANGLE_WIDTH)
envelope = speed_weight * center_weight
output_scale = 1.0 - ((1.0 - RAV4_TSS2_PID_OUTPUT_SCALE_MIN) * envelope)
output_alpha = 1.0 - ((1.0 - RAV4_TSS2_PID_OUTPUT_ALPHA_MIN) * envelope)
limited_output = output_torque * output_scale
return float(prev_output_torque + output_alpha * (limited_output - prev_output_torque))
def _rav4_tss2_center_envelope(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _sigmoid((RAV4_TSS2_CENTER_SPEED - max(v_ego, 0.0)) /
RAV4_TSS2_CENTER_SPEED_WIDTH)
center_weight = _sigmoid((RAV4_TSS2_CENTER_FRICTION_LAT - abs(desired_lateral_accel)) /
RAV4_TSS2_CENTER_FRICTION_LAT_WIDTH)
return speed_weight * center_weight
def get_rav4_tss2_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0,
desired_lateral_jerk: float = 0.0) -> float:
del desired_lateral_jerk
gain = _flm_vehicle_knob("toyota_rav4_tss2.center_friction_threshold_gain",
RAV4_TSS2_CENTER_FRICTION_THRESHOLD_GAIN)
return get_standard_friction_threshold(v_ego) * (
1.0 + gain * _rav4_tss2_center_envelope(desired_lateral_accel, v_ego)
)
def get_rav4_tss2_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
reduction = _flm_vehicle_knob("toyota_rav4_tss2.center_output_taper_max",
RAV4_TSS2_CENTER_OUTPUT_TAPER_MAX)
return 1.0 - reduction * _rav4_tss2_center_envelope(desired_lateral_accel, v_ego)
def get_ram_1500_transition_output_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
speed_weight = float(np.interp(v_ego, [RAM_1500_TRANSITION_SPEED_ONSET, RAM_1500_TRANSITION_SPEED_FULL], [0.0, 1.0]))
jerk_weight = float(np.interp(abs(desired_lateral_jerk),
[RAM_1500_TRANSITION_JERK_ONSET, RAM_1500_TRANSITION_JERK_FULL], [0.0, 1.0]))
lat_weight = 1.0 - float(np.interp(abs(desired_lateral_accel),
[RAM_1500_TRANSITION_LAT_FADE_START, RAM_1500_TRANSITION_LAT_FADE_END], [0.0, 1.0]))
return 1.0 - (RAM_1500_TRANSITION_TAPER_MAX * speed_weight * jerk_weight * lat_weight)
def get_ram_1500_unwind_output_scale(desired_lateral_accel: float, desired_lateral_jerk: float,
v_ego: float) -> float:
"""Soften only rapid high-speed unwind reversals on the RAM 1500."""
if desired_lateral_accel * desired_lateral_jerk >= 0.0:
return 1.0
speed_weight = float(np.interp(v_ego,
[RAM_1500_UNWIND_OUTPUT_SPEED_ONSET, RAM_1500_UNWIND_OUTPUT_SPEED_FULL],
[0.0, 1.0]))
jerk_weight = float(np.interp(abs(desired_lateral_jerk),
[RAM_1500_UNWIND_OUTPUT_JERK_ONSET, RAM_1500_UNWIND_OUTPUT_JERK_FULL],
[0.0, 1.0]))
curve_weight = _sigmoid((abs(desired_lateral_accel) - RAM_1500_UNWIND_OUTPUT_LAT_ONSET) /
RAM_1500_UNWIND_OUTPUT_LAT_WIDTH)
return 1.0 - (RAM_1500_UNWIND_OUTPUT_TAPER_MAX * speed_weight * jerk_weight * curve_weight)
def get_ram_1500_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
"""Damp only center corrections at low/mid speed, not turn authority."""
center_weight = _sigmoid((RAM_1500_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) /
RAM_1500_CENTER_OUTPUT_TAPER_LAT_WIDTH)
speed_onset = _sigmoid((v_ego - RAM_1500_CENTER_OUTPUT_TAPER_SPEED_ONSET) /
RAM_1500_CENTER_OUTPUT_TAPER_SPEED_ONSET_WIDTH)
speed_cutoff = _sigmoid((RAM_1500_CENTER_OUTPUT_TAPER_SPEED_MAX - v_ego) /
RAM_1500_CENTER_OUTPUT_TAPER_SPEED_MAX_WIDTH)
return 1.0 - (RAM_1500_CENTER_OUTPUT_TAPER_MAX * center_weight * speed_onset * speed_cutoff)
def get_ram_1500_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
phase = math.tanh((desired_lateral_accel * desired_lateral_jerk) / RAM_1500_PHASE_SCALE)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
speed_weight = float(np.interp(v_ego, [RAM_1500_PHASE_SPEED_ONSET, RAM_1500_PHASE_SPEED_FULL], [0.0, 1.0]))
lat_weight = _sigmoid((abs(desired_lateral_accel) - RAM_1500_PHASE_LAT_ONSET) / RAM_1500_PHASE_LAT_WIDTH)
return 1.0 + ((RAM_1500_TURN_IN_FF_BOOST * turn_in_weight -
RAM_1500_UNWIND_FF_REDUCTION * unwind_weight) * speed_weight * lat_weight)
def get_gmc_yukon_cc_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
"""Add turn-in authority and soften the high-speed unwind transient on Yukon CC."""
phase = math.tanh((desired_lateral_accel * desired_lateral_jerk) / GMC_YUKON_CC_PHASE_SCALE)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
speed_weight = float(np.interp(v_ego,
[GMC_YUKON_CC_PHASE_SPEED_ONSET, GMC_YUKON_CC_PHASE_SPEED_FULL],
[0.0, 1.0]))
lat_weight = _sigmoid((abs(desired_lateral_accel) - GMC_YUKON_CC_PHASE_LAT_ONSET) /
GMC_YUKON_CC_PHASE_LAT_WIDTH)
return 1.0 + ((GMC_YUKON_CC_TURN_IN_FF_BOOST * turn_in_weight -
GMC_YUKON_CC_UNWIND_FF_REDUCTION * unwind_weight) * speed_weight * lat_weight)
def get_kona_non_scc_highway_transition_output_scale(desired_lateral_accel: float, desired_lateral_jerk: float,
v_ego: float) -> float:
speed_weight = float(np.interp(v_ego, [KONA_NON_SCC_TRANSITION_SPEED_ONSET, KONA_NON_SCC_TRANSITION_SPEED_FULL], [0.0, 1.0]))
jerk_weight = float(np.interp(abs(desired_lateral_jerk),
[KONA_NON_SCC_TRANSITION_JERK_ONSET, KONA_NON_SCC_TRANSITION_JERK_FULL], [0.0, 1.0]))
lat_weight = 1.0 - float(np.interp(abs(desired_lateral_accel),
[KONA_NON_SCC_TRANSITION_LAT_FADE_START, KONA_NON_SCC_TRANSITION_LAT_FADE_END], [0.0, 1.0]))
taper_max = (KONA_NON_SCC_TRANSITION_UNWIND_TAPER_MAX
if desired_lateral_accel * desired_lateral_jerk < 0.0
else KONA_NON_SCC_TRANSITION_TURN_IN_TAPER_MAX)
return 1.0 - (taper_max * speed_weight * jerk_weight * lat_weight)
def get_kona_non_scc_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0,
desired_lateral_jerk: float = 0.0) -> float:
del desired_lateral_jerk
speed_weight = _sigmoid((v_ego - KONA_NON_SCC_CENTER_FRICTION_THRESHOLD_SPEED_ONSET) /
KONA_NON_SCC_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH)
center_weight = _sigmoid((KONA_NON_SCC_CENTER_FRICTION_THRESHOLD_LAT - abs(desired_lateral_accel)) /
KONA_NON_SCC_CENTER_FRICTION_THRESHOLD_LAT_WIDTH)
return get_standard_friction_threshold(v_ego) * (
1.0 + KONA_NON_SCC_CENTER_FRICTION_THRESHOLD_GAIN * speed_weight * center_weight
)
def get_kona_non_scc_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = float(np.interp(v_ego, [KONA_NON_SCC_CENTER_TAPER_SPEED_ONSET, KONA_NON_SCC_CENTER_TAPER_SPEED_FULL], [0.0, 1.0]))
center_weight = float(np.interp(abs(desired_lateral_accel), [0.0, KONA_NON_SCC_CENTER_TAPER_LAT], [1.0, 0.0]))
return 1.0 - (KONA_NON_SCC_CENTER_TAPER_MAX * speed_weight * center_weight)
def civic_bosch_modified_lateral_testing_ground_active() -> bool:
return testing_ground.use("8", "B")
def civic_bosch_modified_a_lateral_testing_ground_active() -> bool:
return testing_ground.use("8", "A")
def get_civic_bosch_modified_a_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _sigmoid((v_ego - CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_SPEED) /
CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_SPEED_WIDTH)
center_weight = _sigmoid((CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT - abs(desired_lateral_accel)) /
CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT_WIDTH)
reduction = CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_MAX * speed_weight * center_weight
return 1.0 - reduction
def _civic_bosch_modified_b_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / CIVIC_BOSCH_MODIFIED_B_TRANSITION_SPEED) ** 2)
def _civic_bosch_modified_b_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
return math.tanh((desired_lateral_accel * desired_lateral_jerk) / CIVIC_BOSCH_MODIFIED_B_PHASE_SCALE)
def _civic_bosch_modified_b_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def get_civic_bosch_modified_b_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
abs_lateral_accel = abs(desired_lateral_accel)
onset = _sigmoid((abs_lateral_accel - CIVIC_BOSCH_MODIFIED_B_FF_ONSET) / CIVIC_BOSCH_MODIFIED_B_FF_ONSET_WIDTH)
cutoff = _sigmoid((CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF - abs_lateral_accel) / CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF_WIDTH)
base_reduction = _civic_bosch_modified_b_side_value(desired_lateral_accel,
CIVIC_BOSCH_MODIFIED_B_FF_REDUCTION_LEFT,
CIVIC_BOSCH_MODIFIED_B_FF_REDUCTION_RIGHT) * onset * cutoff
phase = _civic_bosch_modified_b_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
low_speed_factor = _civic_bosch_modified_b_low_speed_factor(v_ego)
a_variant_active = civic_bosch_modified_a_lateral_testing_ground_active()
variant_active = civic_bosch_modified_lateral_testing_ground_active()
if a_variant_active:
base_reduction -= (_civic_bosch_modified_b_side_value(desired_lateral_accel,
CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_LEFT,
CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_RIGHT) * onset * cutoff)
if variant_active:
base_reduction += (_civic_bosch_modified_b_side_value(desired_lateral_accel,
CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT,
CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT) * onset * cutoff)
turn_in_boost = 1.0 + (_civic_bosch_modified_b_side_value(desired_lateral_accel,
CIVIC_BOSCH_MODIFIED_B_TURN_IN_BOOST_LEFT,
CIVIC_BOSCH_MODIFIED_B_TURN_IN_BOOST_RIGHT) *
turn_in_weight * (0.40 + 0.60 * low_speed_factor))
if a_variant_active:
turn_in_boost *= 1.0 + (_civic_bosch_modified_b_side_value(desired_lateral_accel,
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_LEFT,
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_RIGHT) *
turn_in_weight * (0.40 + 0.60 * low_speed_factor))
if variant_active:
turn_in_boost *= 1.0 + (_civic_bosch_modified_b_side_value(desired_lateral_accel,
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT,
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_RIGHT) *
turn_in_weight * (0.40 + 0.60 * low_speed_factor))
unwind_taper = 1.0 - (_civic_bosch_modified_b_side_value(desired_lateral_accel,
CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_LEFT,
CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_RIGHT) *
unwind_weight * (0.35 + 0.65 * low_speed_factor))
if a_variant_active:
unwind_taper *= 1.0 - (_civic_bosch_modified_b_side_value(desired_lateral_accel,
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_LEFT,
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_RIGHT) *
unwind_weight * (0.35 + 0.65 * low_speed_factor))
if variant_active:
unwind_taper *= 1.0 - (_civic_bosch_modified_b_side_value(desired_lateral_accel,
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT,
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_RIGHT) *
unwind_weight * (0.35 + 0.65 * low_speed_factor))
return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0)
def get_civic_bosch_modified_b_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
if desired_lateral_accel == 0.0 or desired_lateral_jerk == 0.0:
return 1.0
abs_lateral_accel = abs(desired_lateral_accel)
onset = _sigmoid((abs_lateral_accel - CIVIC_BOSCH_MODIFIED_B_FF_ONSET) / CIVIC_BOSCH_MODIFIED_B_FF_ONSET_WIDTH)
cutoff = _sigmoid((CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF - abs_lateral_accel) / CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF_WIDTH)
envelope = onset * cutoff * _civic_bosch_modified_b_low_speed_factor(v_ego)
phase = _civic_bosch_modified_b_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
a_variant_active = civic_bosch_modified_a_lateral_testing_ground_active()
variant_active = civic_bosch_modified_lateral_testing_ground_active()
friction_scale = 1.0
friction_scale += (_civic_bosch_modified_b_side_value(desired_lateral_accel,
CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_LEFT,
CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_RIGHT) *
envelope * turn_in_weight)
if a_variant_active:
friction_scale += (_civic_bosch_modified_b_side_value(desired_lateral_accel,
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_LEFT,
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT) *
envelope * turn_in_weight)
if variant_active:
friction_scale += (_civic_bosch_modified_b_side_value(desired_lateral_accel,
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_LEFT,
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT) *
envelope * turn_in_weight)
friction_scale -= (_civic_bosch_modified_b_side_value(desired_lateral_accel,
CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_LEFT,
CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_RIGHT) *
envelope * unwind_weight)
if a_variant_active:
friction_scale -= (_civic_bosch_modified_b_side_value(desired_lateral_accel,
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT,
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT) *
envelope * unwind_weight)
if variant_active:
friction_scale -= (_civic_bosch_modified_b_side_value(desired_lateral_accel,
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT,
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT) *
envelope * unwind_weight)
return min(max(friction_scale, 0.82), 1.06)
def bolt_2017_lateral_testing_ground_active() -> bool:
return testing_ground.use(BOLT_2017_LATERAL_TESTING_GROUND_ID)
def _bolt_2017_sigmoid(x: float) -> float:
return _sigmoid(x)
def _bolt_2017_high_speed_factor(v_ego: float) -> float:
return _bolt_2017_sigmoid((max(v_ego, 0.0) - BOLT_2017_STEER_RATIO_ONSET_SPEED) / BOLT_2017_STEER_RATIO_ONSET_WIDTH)
def get_bolt_2017_steer_ratio_scale(v_ego: float) -> float:
return 1.0 + ((BOLT_2017_STEER_RATIO_TEST_SCALE - 1.0) * _bolt_2017_high_speed_factor(v_ego))
def get_honda_accord_steer_ratio_scale(_v_ego: float) -> float:
return HONDA_ACCORD_STEER_RATIO_SCALE
def get_honda_accord_ff_scale(desired_lateral_accel: float) -> float:
"""Taper only sharp-turn feedforward where the Accord carries excess curvature."""
turn_weight = _sigmoid((abs(desired_lateral_accel) - HONDA_ACCORD_TURN_FF_ONSET) /
HONDA_ACCORD_TURN_FF_WIDTH)
return 1.0 - (HONDA_ACCORD_TURN_FF_REDUCTION_MAX * turn_weight)
def get_bolt_2017_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
center_window = _bolt_2017_sigmoid((BOLT_2017_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / BOLT_2017_CENTER_TAPER_WIDTH)
return 1.0 - (BOLT_2017_CENTER_TAPER_GAIN * _bolt_2017_high_speed_factor(v_ego) * center_window)
def _bolt_2017_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / BOLT_2017_TRANSITION_SPEED) ** 2)
def _bolt_2017_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
return math.tanh((desired_lateral_accel * desired_lateral_jerk) / BOLT_2017_PHASE_SCALE)
def _bolt_2017_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def get_bolt_2017_base_torque_scale(desired_lateral_accel: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
scale_values = BOLT_2017_TORQUE_SCALE_LEFT if desired_lateral_accel > 0.0 else BOLT_2017_TORQUE_SCALE_RIGHT
return float(np.interp(abs(desired_lateral_accel), BOLT_2017_TORQUE_SCALE_BP, scale_values))
def get_bolt_2017_torque_scale(desired_lateral_accel: float, desired_lateral_jerk: float = 0.0, v_ego: float = 30.0) -> float:
base_scale = get_bolt_2017_base_torque_scale(desired_lateral_accel)
scale = base_scale
if base_scale > 1.0 and desired_lateral_jerk != 0.0:
low_speed_factor = _bolt_2017_low_speed_factor(v_ego)
phase = _bolt_2017_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_2017_side_value(desired_lateral_accel, BOLT_2017_TURN_IN_BOOST_LEFT, BOLT_2017_TURN_IN_BOOST_RIGHT) *
turn_in_weight * (0.35 + 0.65 * low_speed_factor))
unwind_taper = 1.0 - (_bolt_2017_side_value(desired_lateral_accel, BOLT_2017_UNWIND_TAPER_LEFT, BOLT_2017_UNWIND_TAPER_RIGHT) *
unwind_weight * (0.45 + 0.55 * low_speed_factor))
scale = 1.0 + ((base_scale - 1.0) * turn_in_boost * max(unwind_taper, 0.0))
return scale * get_bolt_2017_center_taper_scale(desired_lateral_accel, v_ego)
def bolt_2018_2021_lateral_testing_ground_active() -> bool:
return testing_ground.use(BOLT_2018_2021_LATERAL_TESTING_GROUND_ID)
def _bolt_2018_2021_sigmoid(x: float) -> float:
return _sigmoid(x)
def _bolt_2018_2021_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / BOLT_2018_2021_TRANSITION_SPEED) ** 2)
def _bolt_2018_2021_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
return math.tanh((desired_lateral_accel * desired_lateral_jerk) / BOLT_2018_2021_PHASE_SCALE)
def _bolt_2018_2021_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def _bolt_2018_2021_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / BOLT_2018_2021_FRICTION_LAT_RISE)
jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / BOLT_2018_2021_FRICTION_JERK_RISE)
return _bolt_2018_2021_low_speed_factor(v_ego) * lat_factor * jerk_factor
def get_bolt_2018_2021_torque_scale(desired_lateral_accel: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
gain = BOLT_2018_2021_TORQUE_GAIN_LEFT if desired_lateral_accel > 0.0 else BOLT_2018_2021_TORQUE_GAIN_RIGHT
abs_lateral_accel = abs(desired_lateral_accel)
onset = _bolt_2018_2021_sigmoid((abs_lateral_accel - BOLT_2018_2021_TORQUE_ONSET) / BOLT_2018_2021_TORQUE_ONSET_WIDTH)
cutoff = _bolt_2018_2021_sigmoid((BOLT_2018_2021_TORQUE_CUTOFF - abs_lateral_accel) / BOLT_2018_2021_TORQUE_CUTOFF_WIDTH)
return 1.0 + gain * onset * cutoff
def get_bolt_2018_2021_dynamic_torque_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
base_scale = get_bolt_2018_2021_torque_scale(desired_lateral_accel)
extra_scale = max(base_scale - 1.0, 0.0)
abs_lateral_accel = abs(desired_lateral_accel)
low_speed_factor = _bolt_2018_2021_low_speed_factor(v_ego)
high_speed_factor = 1.0 - low_speed_factor
center_window = _bolt_2018_2021_sigmoid((BOLT_2018_2021_CENTER_TAPER_LAT - abs_lateral_accel) / BOLT_2018_2021_CENTER_TAPER_WIDTH)
center_taper = 1.0 - (BOLT_2018_2021_CENTER_TAPER_GAIN * high_speed_factor * center_window)
phase = _bolt_2018_2021_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
jerk_taper = 1.0 / (1.0 + (abs(desired_lateral_jerk) / BOLT_2018_2021_JERK_TAPER_CUTOFF) ** 2)
turn_in_boost = 1.0 + (_bolt_2018_2021_side_value(desired_lateral_accel, BOLT_2018_2021_TURN_IN_BOOST_LEFT, BOLT_2018_2021_TURN_IN_BOOST_RIGHT) *
turn_in_weight * low_speed_factor)
unwind_weight = max(-phase, 0.0)
unwind_taper = 1.0 - (_bolt_2018_2021_side_value(desired_lateral_accel, BOLT_2018_2021_UNWIND_TAPER_GAIN_LEFT, BOLT_2018_2021_UNWIND_TAPER_GAIN_RIGHT) *
unwind_weight * (0.55 + 0.45 * low_speed_factor))
return 1.0 + (extra_scale * center_taper * jerk_taper * turn_in_boost * max(unwind_taper, 0.0))
def get_bolt_2018_2021_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_gm_base_friction_threshold(v_ego)
transition_envelope = _bolt_2018_2021_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _bolt_2018_2021_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_2018_2021_side_value(desired_lateral_accel, BOLT_2018_2021_TURN_IN_THRESHOLD_REDUCTION_LEFT, BOLT_2018_2021_TURN_IN_THRESHOLD_REDUCTION_RIGHT) *
transition_envelope * turn_in_weight)
threshold_scale += (_bolt_2018_2021_side_value(desired_lateral_accel, BOLT_2018_2021_UNWIND_THRESHOLD_INCREASE_LEFT, BOLT_2018_2021_UNWIND_THRESHOLD_INCREASE_RIGHT) *
transition_envelope * unwind_weight)
return base_threshold * min(max(threshold_scale, 0.82), 1.12)
def get_bolt_2018_2021_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
transition_envelope = _bolt_2018_2021_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _bolt_2018_2021_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
friction_scale = BOLT_2018_2021_FRICTION_MULT
friction_scale += (_bolt_2018_2021_side_value(desired_lateral_accel, BOLT_2018_2021_TURN_IN_FRICTION_BOOST_LEFT, BOLT_2018_2021_TURN_IN_FRICTION_BOOST_RIGHT) *
transition_envelope * turn_in_weight)
friction_scale -= (_bolt_2018_2021_side_value(desired_lateral_accel, BOLT_2018_2021_UNWIND_FRICTION_REDUCTION_LEFT, BOLT_2018_2021_UNWIND_FRICTION_REDUCTION_RIGHT) *
transition_envelope * unwind_weight)
return min(max(friction_scale, 0.88), 1.10)
def bolt_2022_2023_lateral_testing_ground_active() -> bool:
return testing_ground.use(BOLT_2022_2023_LATERAL_TESTING_GROUND_ID)
def _bolt_2022_2023_sigmoid(x: float) -> float:
return _sigmoid(x)
def _bolt_2022_2023_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / BOLT_2022_2023_TRANSITION_SPEED) ** 2)
def _bolt_2022_2023_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
return math.tanh((desired_lateral_accel * desired_lateral_jerk) / BOLT_2022_2023_PHASE_SCALE)
def _bolt_2022_2023_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def _bolt_2022_2023_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / BOLT_2022_2023_FRICTION_LAT_RISE)
jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / BOLT_2022_2023_FRICTION_JERK_RISE)
return _bolt_2022_2023_low_speed_factor(v_ego) * lat_factor * jerk_factor
def get_bolt_2022_2023_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
gain = _bolt_2022_2023_side_value(
desired_lateral_accel,
_flm_vehicle_knob("gm_bolt_2022_2023.ff_gain_left", BOLT_2022_2023_FF_GAIN_LEFT),
_flm_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 - (_flm_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,
_flm_vehicle_knob("gm_bolt_2022_2023.turn_in_boost_left", BOLT_2022_2023_TURN_IN_BOOST_LEFT),
_flm_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,
_flm_vehicle_knob("gm_bolt_2022_2023.unwind_taper_left", BOLT_2022_2023_UNWIND_TAPER_LEFT),
_flm_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))
def get_bolt_2022_2023_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
highway_speed_weight = _bolt_2022_2023_sigmoid((v_ego - BOLT_2022_2023_CENTER_TAPER_SPEED) /
BOLT_2022_2023_CENTER_TAPER_SPEED_WIDTH)
highway_center_weight = _bolt_2022_2023_sigmoid((BOLT_2022_2023_CENTER_TAPER_LAT - abs(desired_lateral_accel)) /
BOLT_2022_2023_CENTER_TAPER_LAT_WIDTH)
low_speed_onset = _bolt_2022_2023_sigmoid((v_ego - BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED) /
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH)
low_speed_cutoff = _bolt_2022_2023_sigmoid((BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_MAX - v_ego) /
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_MAX_WIDTH)
low_speed_center_weight = _bolt_2022_2023_sigmoid((BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) /
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_LAT_WIDTH)
low_speed_floor = _bolt_2022_2023_sigmoid(
(v_ego - BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_FLOOR) /
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_FLOOR_WIDTH
)
highway_reduction = (_flm_vehicle_knob("gm_bolt_2022_2023.center_taper_max", BOLT_2022_2023_CENTER_TAPER_MAX) *
highway_speed_weight * highway_center_weight)
low_speed_reduction = (BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_MAX * low_speed_onset * low_speed_cutoff *
low_speed_center_weight * low_speed_floor)
return 1.0 - min(highway_reduction + low_speed_reduction, 0.95)
def get_bolt_2022_2023_low_speed_center_output_limit(desired_lateral_accel: float, v_ego: float) -> float:
"""Limit small-signal output while the Bolt is in its low-speed chatter band."""
speed_onset = _bolt_2022_2023_sigmoid(
(v_ego - BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED) /
BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_WIDTH
)
speed_cutoff = _bolt_2022_2023_sigmoid(
(BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_MAX - v_ego) /
BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_MAX_WIDTH
)
center_weight = _bolt_2022_2023_sigmoid(
(BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_LAT - abs(desired_lateral_accel)) /
BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_LAT_WIDTH
)
speed_weight = speed_onset * speed_cutoff
reduction = (1.0 - BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_LIMIT) * speed_weight * center_weight
return 1.0 - reduction
def get_bolt_2022_2023_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_gm_base_friction_threshold(v_ego)
center_weight = _bolt_2022_2023_sigmoid(
(BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_LAT - abs(desired_lateral_accel)) /
BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_LAT_WIDTH
)
low_speed_weight = _bolt_2022_2023_sigmoid(
(BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_SPEED - v_ego) /
BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH
)
base_threshold += (BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_BUMP * center_weight * low_speed_weight)
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)
threshold_scale = 1.0 - (_bolt_2022_2023_side_value(
desired_lateral_accel,
_flm_vehicle_knob("gm_bolt_2022_2023.turn_in_threshold_reduction_left", BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_LEFT),
_flm_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,
_flm_vehicle_knob("gm_bolt_2022_2023.unwind_threshold_increase_left", BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_LEFT),
_flm_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)
def get_bolt_2022_2023_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
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)
friction_scale = BOLT_2022_2023_FRICTION_MULT
friction_scale += (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_TURN_IN_FRICTION_BOOST_LEFT, BOLT_2022_2023_TURN_IN_FRICTION_BOOST_RIGHT) *
transition_envelope * turn_in_weight)
friction_scale -= (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_LEFT, BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_RIGHT) *
transition_envelope * unwind_weight)
return min(max(friction_scale, 0.92), 1.22)
def volt_standard_lateral_testing_ground_active() -> bool:
return testing_ground.use(VOLT_STANDARD_LATERAL_TESTING_GROUND_ID)
def _volt_standard_sigmoid(x: float) -> float:
return _sigmoid(x)
def _volt_standard_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / VOLT_STANDARD_TRANSITION_SPEED) ** 2)
def _volt_standard_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
return math.tanh((desired_lateral_accel * desired_lateral_jerk) / VOLT_STANDARD_PHASE_SCALE)
def _volt_standard_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def _volt_standard_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / VOLT_STANDARD_FRICTION_LAT_RISE)
jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / VOLT_STANDARD_FRICTION_JERK_RISE)
return _volt_standard_low_speed_factor(v_ego) * lat_factor * jerk_factor
def get_volt_standard_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
gain = _volt_standard_side_value(desired_lateral_accel, VOLT_STANDARD_FF_GAIN_LEFT, VOLT_STANDARD_FF_GAIN_RIGHT)
abs_lateral_accel = abs(desired_lateral_accel)
onset = _volt_standard_sigmoid((abs_lateral_accel - VOLT_STANDARD_FF_ONSET) / VOLT_STANDARD_FF_ONSET_WIDTH)
cutoff = _volt_standard_sigmoid((VOLT_STANDARD_FF_CUTOFF - abs_lateral_accel) / VOLT_STANDARD_FF_CUTOFF_WIDTH)
extra_scale = gain * onset * cutoff
phase = _volt_standard_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
low_speed_factor = _volt_standard_low_speed_factor(v_ego)
turn_in_boost = 1.0 + (_volt_standard_side_value(desired_lateral_accel, VOLT_STANDARD_TURN_IN_BOOST_LEFT, VOLT_STANDARD_TURN_IN_BOOST_RIGHT) *
turn_in_weight * low_speed_factor)
unwind_taper = 1.0 - (_volt_standard_side_value(desired_lateral_accel, VOLT_STANDARD_UNWIND_TAPER_LEFT, VOLT_STANDARD_UNWIND_TAPER_RIGHT) *
unwind_weight * (0.30 + 0.70 * low_speed_factor))
return 1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0))
def get_volt_standard_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_gm_base_friction_threshold(v_ego)
transition_envelope = _volt_standard_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _volt_standard_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 - (_volt_standard_side_value(desired_lateral_accel, VOLT_STANDARD_TURN_IN_THRESHOLD_REDUCTION_LEFT, VOLT_STANDARD_TURN_IN_THRESHOLD_REDUCTION_RIGHT) *
transition_envelope * turn_in_weight)
threshold_scale += (_volt_standard_side_value(desired_lateral_accel, VOLT_STANDARD_UNWIND_THRESHOLD_INCREASE_LEFT, VOLT_STANDARD_UNWIND_THRESHOLD_INCREASE_RIGHT) *
transition_envelope * unwind_weight)
return base_threshold * min(max(threshold_scale, 0.84), 1.12)
def get_volt_standard_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
transition_envelope = _volt_standard_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _volt_standard_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
friction_scale = VOLT_STANDARD_FRICTION_MULT
friction_scale += (_volt_standard_side_value(desired_lateral_accel, VOLT_STANDARD_TURN_IN_FRICTION_BOOST_LEFT, VOLT_STANDARD_TURN_IN_FRICTION_BOOST_RIGHT) *
transition_envelope * turn_in_weight)
friction_scale -= (_volt_standard_side_value(desired_lateral_accel, VOLT_STANDARD_UNWIND_FRICTION_REDUCTION_LEFT, VOLT_STANDARD_UNWIND_FRICTION_REDUCTION_RIGHT) *
transition_envelope * unwind_weight)
return min(max(friction_scale, 0.90), 1.14)
def get_volt_standard_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _volt_standard_sigmoid((v_ego - VOLT_STANDARD_CENTER_TAPER_SPEED) / VOLT_STANDARD_CENTER_TAPER_SPEED_WIDTH)
center_weight = _volt_standard_sigmoid((VOLT_STANDARD_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / VOLT_STANDARD_CENTER_TAPER_LAT_WIDTH)
reduction = VOLT_STANDARD_CENTER_TAPER_MAX * speed_weight * center_weight
return 1.0 - reduction
def get_silverado_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _sigmoid((v_ego - SILVERADO_CENTER_TAPER_SPEED) / SILVERADO_CENTER_TAPER_SPEED_WIDTH)
center_weight = _sigmoid((SILVERADO_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / SILVERADO_CENTER_TAPER_LAT_WIDTH)
reduction = SILVERADO_CENTER_TAPER_MAX * speed_weight * center_weight
return 1.0 - reduction
def _sonata_hybrid_sigmoid(x: float) -> float:
return _sigmoid(x)
def _sonata_hybrid_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / SONATA_HYBRID_TRANSITION_SPEED) ** 2)
def _sonata_hybrid_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
return math.tanh((desired_lateral_accel * desired_lateral_jerk) / SONATA_HYBRID_PHASE_SCALE)
def _sonata_hybrid_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def get_sonata_hybrid_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
abs_lateral_accel = abs(desired_lateral_accel)
onset = _sonata_hybrid_sigmoid((abs_lateral_accel - SONATA_HYBRID_FF_ONSET) / SONATA_HYBRID_FF_ONSET_WIDTH)
cutoff = _sonata_hybrid_sigmoid((SONATA_HYBRID_FF_CUTOFF - abs_lateral_accel) / SONATA_HYBRID_FF_CUTOFF_WIDTH)
base_reduction = _sonata_hybrid_side_value(desired_lateral_accel,
SONATA_HYBRID_FF_REDUCTION_LEFT,
SONATA_HYBRID_FF_REDUCTION_RIGHT) * onset * cutoff
phase = _sonata_hybrid_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
low_speed_factor = _sonata_hybrid_low_speed_factor(v_ego)
turn_in_boost = 1.0 + (_sonata_hybrid_side_value(desired_lateral_accel,
SONATA_HYBRID_TURN_IN_BOOST_LEFT,
SONATA_HYBRID_TURN_IN_BOOST_RIGHT) *
turn_in_weight * low_speed_factor)
unwind_taper = 1.0 - (_sonata_hybrid_side_value(desired_lateral_accel,
SONATA_HYBRID_UNWIND_TAPER_LEFT,
SONATA_HYBRID_UNWIND_TAPER_RIGHT) *
unwind_weight * (0.35 + 0.65 * low_speed_factor))
return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0)
def get_sonata_hybrid_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _sonata_hybrid_sigmoid((v_ego - SONATA_HYBRID_CENTER_TAPER_SPEED) / SONATA_HYBRID_CENTER_TAPER_SPEED_WIDTH)
center_weight = _sonata_hybrid_sigmoid((SONATA_HYBRID_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / SONATA_HYBRID_CENTER_TAPER_LAT_WIDTH)
reduction = SONATA_HYBRID_CENTER_TAPER_MAX * speed_weight * center_weight
low_speed_weight = _sonata_hybrid_sigmoid((SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_MAX - v_ego) /
SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH)
low_speed_center_weight = _sonata_hybrid_sigmoid((SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) /
SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_LAT_WIDTH)
reduction += SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_MAX * low_speed_weight * low_speed_center_weight
return 1.0 - reduction
def get_sonata_hybrid_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _sonata_hybrid_sigmoid((v_ego - SONATA_HYBRID_CENTER_OUTPUT_TAPER_SPEED) /
SONATA_HYBRID_CENTER_OUTPUT_TAPER_SPEED_WIDTH)
center_weight = _sonata_hybrid_sigmoid((SONATA_HYBRID_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) /
SONATA_HYBRID_CENTER_OUTPUT_TAPER_LAT_WIDTH)
return 1.0 - SONATA_HYBRID_CENTER_OUTPUT_TAPER_MAX * speed_weight * center_weight
def _sonata_sigmoid(x: float) -> float:
return _sigmoid(x)
def _sonata_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / SONATA_TRANSITION_SPEED) ** 2)
def _sonata_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
return math.tanh((desired_lateral_accel * desired_lateral_jerk) / SONATA_PHASE_SCALE)
def _sonata_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def get_sonata_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
abs_lateral_accel = abs(desired_lateral_accel)
onset = _sonata_sigmoid((abs_lateral_accel - SONATA_FF_ONSET) / SONATA_FF_ONSET_WIDTH)
cutoff = _sonata_sigmoid((SONATA_FF_CUTOFF - abs_lateral_accel) / SONATA_FF_CUTOFF_WIDTH)
base_reduction = _sonata_side_value(desired_lateral_accel, SONATA_FF_REDUCTION_LEFT, SONATA_FF_REDUCTION_RIGHT) * onset * cutoff
phase = _sonata_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
low_speed_factor = _sonata_low_speed_factor(v_ego)
turn_in_boost = 1.0 + (_sonata_side_value(desired_lateral_accel, SONATA_TURN_IN_BOOST_LEFT, SONATA_TURN_IN_BOOST_RIGHT) *
turn_in_weight * low_speed_factor)
unwind_taper = 1.0 - (_sonata_side_value(desired_lateral_accel, SONATA_UNWIND_TAPER_LEFT, SONATA_UNWIND_TAPER_RIGHT) *
unwind_weight * (0.35 + 0.65 * low_speed_factor))
return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0)
def get_sonata_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _sonata_sigmoid((v_ego - SONATA_CENTER_TAPER_SPEED) / SONATA_CENTER_TAPER_SPEED_WIDTH)
center_weight = _sonata_sigmoid((SONATA_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / SONATA_CENTER_TAPER_LAT_WIDTH)
reduction = SONATA_CENTER_TAPER_MAX * speed_weight * center_weight
low_speed_weight = _sonata_sigmoid((SONATA_LOW_SPEED_CENTER_TAPER_SPEED_MAX - v_ego) /
SONATA_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH)
low_speed_center_weight = _sonata_sigmoid((SONATA_LOW_SPEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) /
SONATA_LOW_SPEED_CENTER_TAPER_LAT_WIDTH)
reduction += SONATA_LOW_SPEED_CENTER_TAPER_MAX * low_speed_weight * low_speed_center_weight
return 1.0 - reduction
def _elantra_non_scc_sigmoid(x: float) -> float:
return _sigmoid(x)
def _elantra_non_scc_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / ELANTRA_NON_SCC_TRANSITION_SPEED) ** 2)
def _elantra_non_scc_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
return math.tanh((desired_lateral_accel * desired_lateral_jerk) / ELANTRA_NON_SCC_PHASE_SCALE)
def _elantra_non_scc_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def get_elantra_non_scc_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
abs_lateral_accel = abs(desired_lateral_accel)
onset = _elantra_non_scc_sigmoid((abs_lateral_accel - ELANTRA_NON_SCC_FF_ONSET) / ELANTRA_NON_SCC_FF_ONSET_WIDTH)
cutoff = _elantra_non_scc_sigmoid((ELANTRA_NON_SCC_FF_CUTOFF - abs_lateral_accel) / ELANTRA_NON_SCC_FF_CUTOFF_WIDTH)
low_speed_factor = _elantra_non_scc_low_speed_factor(v_ego)
envelope = onset * cutoff * low_speed_factor
base_scale = 1.0 - (_elantra_non_scc_side_value(desired_lateral_accel,
ELANTRA_NON_SCC_FF_ADJUST_LEFT,
ELANTRA_NON_SCC_FF_ADJUST_RIGHT) * envelope)
phase = _elantra_non_scc_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 + (_elantra_non_scc_side_value(desired_lateral_accel,
ELANTRA_NON_SCC_TURN_IN_BOOST_LEFT,
ELANTRA_NON_SCC_TURN_IN_BOOST_RIGHT) *
turn_in_weight * (0.35 + 0.65 * low_speed_factor))
unwind_taper = 1.0 - (_elantra_non_scc_side_value(desired_lateral_accel,
ELANTRA_NON_SCC_UNWIND_TAPER_LEFT,
ELANTRA_NON_SCC_UNWIND_TAPER_RIGHT) *
unwind_weight * (0.35 + 0.65 * low_speed_factor))
return base_scale * turn_in_boost * max(unwind_taper, 0.0)
def _kia_xceed_sigmoid(x: float) -> float:
return _sigmoid(x)
def _kia_xceed_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / KIA_XCEED_TRANSITION_SPEED) ** 2)
def _kia_xceed_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
return math.tanh((desired_lateral_accel * desired_lateral_jerk) / KIA_XCEED_PHASE_SCALE)
def _kia_xceed_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def get_kia_xceed_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
abs_lateral_accel = abs(desired_lateral_accel)
onset = _kia_xceed_sigmoid((abs_lateral_accel - KIA_XCEED_FF_ONSET) / KIA_XCEED_FF_ONSET_WIDTH)
cutoff = _kia_xceed_sigmoid((KIA_XCEED_FF_CUTOFF - abs_lateral_accel) / KIA_XCEED_FF_CUTOFF_WIDTH)
base_reduction = _kia_xceed_side_value(desired_lateral_accel,
KIA_XCEED_FF_REDUCTION_LEFT,
KIA_XCEED_FF_REDUCTION_RIGHT) * onset * cutoff
phase = _kia_xceed_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
low_speed_factor = _kia_xceed_low_speed_factor(v_ego)
turn_in_boost = 1.0 + (_kia_xceed_side_value(desired_lateral_accel,
KIA_XCEED_TURN_IN_BOOST_LEFT,
KIA_XCEED_TURN_IN_BOOST_RIGHT) *
turn_in_weight * low_speed_factor)
unwind_taper = 1.0 - (_kia_xceed_side_value(desired_lateral_accel,
KIA_XCEED_UNWIND_TAPER_LEFT,
KIA_XCEED_UNWIND_TAPER_RIGHT) *
unwind_weight * (0.35 + 0.65 * low_speed_factor))
return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0)
def get_kia_xceed_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _kia_xceed_sigmoid((v_ego - KIA_XCEED_CENTER_TAPER_SPEED) / KIA_XCEED_CENTER_TAPER_SPEED_WIDTH)
center_weight = _kia_xceed_sigmoid((KIA_XCEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / KIA_XCEED_CENTER_TAPER_LAT_WIDTH)
reduction = KIA_XCEED_CENTER_TAPER_MAX * speed_weight * center_weight
return 1.0 - reduction
def get_kia_niro_phev_2022_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _sigmoid((v_ego - KIA_NIRO_PHEV_2022_CENTER_TAPER_SPEED) / KIA_NIRO_PHEV_2022_CENTER_TAPER_SPEED_WIDTH)
center_weight = _sigmoid((KIA_NIRO_PHEV_2022_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / KIA_NIRO_PHEV_2022_CENTER_TAPER_LAT_WIDTH)
reduction = KIA_NIRO_PHEV_2022_CENTER_TAPER_MAX * speed_weight * center_weight
return 1.0 - reduction
def get_kia_niro_phev_2022_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_gm_base_friction_threshold(v_ego)
speed_weight = _sigmoid((v_ego - KIA_NIRO_PHEV_2022_FRICTION_SPEED) / KIA_NIRO_PHEV_2022_FRICTION_SPEED_WIDTH)
center_weight = _sigmoid((KIA_NIRO_PHEV_2022_FRICTION_CENTER_LAT - abs(desired_lateral_accel)) / KIA_NIRO_PHEV_2022_FRICTION_CENTER_LAT_WIDTH)
calm_jerk_weight = _sigmoid((KIA_NIRO_PHEV_2022_FRICTION_CALM_JERK - abs(desired_lateral_jerk)) / KIA_NIRO_PHEV_2022_FRICTION_CALM_JERK_WIDTH)
threshold_scale = 1.0 + (KIA_NIRO_PHEV_2022_FRICTION_THRESHOLD_GAIN * speed_weight * center_weight * calm_jerk_weight)
return base_threshold * min(max(threshold_scale, 1.0), 1.18)
def _kia_stinger_2022_center_weights(desired_lateral_accel: float, v_ego: float) -> tuple[float, float]:
speed_weight = _sigmoid((v_ego - KIA_STINGER_2022_CENTER_TAPER_SPEED) / KIA_STINGER_2022_CENTER_TAPER_SPEED_WIDTH)
center_weight = _sigmoid((KIA_STINGER_2022_CENTER_TAPER_LAT - abs(desired_lateral_accel)) /
KIA_STINGER_2022_CENTER_TAPER_LAT_WIDTH)
return speed_weight, center_weight
def get_kia_stinger_2022_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight, center_weight = _kia_stinger_2022_center_weights(desired_lateral_accel, v_ego)
return 1.0 - (KIA_STINGER_2022_CENTER_TAPER_MAX * speed_weight * center_weight)
def get_kia_stinger_2022_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0,
desired_lateral_jerk: float = 0.0) -> float:
del desired_lateral_jerk
speed_weight, center_weight = _kia_stinger_2022_center_weights(desired_lateral_accel, v_ego)
return get_standard_friction_threshold(v_ego) * (1.0 + KIA_STINGER_2022_FRICTION_THRESHOLD_GAIN * speed_weight * center_weight)
def _kia_carnival_center_weights(desired_lateral_accel: float, v_ego: float) -> tuple[float, float]:
speed_onset = _sigmoid((v_ego - KIA_CARNIVAL_CENTER_TAPER_SPEED) / KIA_CARNIVAL_CENTER_TAPER_SPEED_WIDTH)
speed_cutoff = _sigmoid((KIA_CARNIVAL_CENTER_TAPER_SPEED_MAX - v_ego) / KIA_CARNIVAL_CENTER_TAPER_SPEED_MAX_WIDTH)
speed_weight = speed_onset * speed_cutoff
center_weight = _sigmoid((KIA_CARNIVAL_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / KIA_CARNIVAL_CENTER_TAPER_LAT_WIDTH)
return speed_weight, center_weight
def _kia_carnival_highway_center_weights(desired_lateral_accel: float, v_ego: float) -> tuple[float, float]:
speed_weight = _sigmoid((v_ego - KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_SPEED) /
KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_SPEED_WIDTH)
center_weight = _sigmoid((KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_LAT - abs(desired_lateral_accel)) /
KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_LAT_WIDTH)
return speed_weight, center_weight
def get_kia_carnival_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight, center_weight = _kia_carnival_center_weights(desired_lateral_accel, v_ego)
highway_speed_weight, highway_center_weight = _kia_carnival_highway_center_weights(desired_lateral_accel, v_ego)
reduction = KIA_CARNIVAL_CENTER_TAPER_MAX * speed_weight * center_weight
reduction += KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_MAX * highway_speed_weight * highway_center_weight
return 1.0 - min(reduction, 0.95)
def get_kia_carnival_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
del desired_lateral_jerk
speed_weight, center_weight = _kia_carnival_center_weights(desired_lateral_accel, v_ego)
highway_speed_weight, highway_center_weight = _kia_carnival_highway_center_weights(desired_lateral_accel, v_ego)
gain = KIA_CARNIVAL_FRICTION_THRESHOLD_GAIN * speed_weight * center_weight
gain += KIA_CARNIVAL_HIGHWAY_FRICTION_THRESHOLD_GAIN * highway_speed_weight * highway_center_weight
return get_hkg_canfd_base_friction_threshold(v_ego) * (1.0 + gain)
def get_kia_carnival_friction_center_fade_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight, center_weight = _kia_carnival_center_weights(desired_lateral_accel, v_ego)
highway_speed_weight, highway_center_weight = _kia_carnival_highway_center_weights(desired_lateral_accel, v_ego)
reduction = KIA_CARNIVAL_FRICTION_CENTER_FADE_MAX * speed_weight * center_weight
reduction += KIA_CARNIVAL_HIGHWAY_FRICTION_CENTER_FADE_MAX * highway_speed_weight * highway_center_weight
return 1.0 - min(reduction, 0.95)
def get_kia_carnival_highway_transition_output_scale(desired_lateral_accel: float, desired_lateral_jerk: float,
v_ego: float) -> float:
speed_weight = _sigmoid((v_ego - KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_SPEED) /
KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_SPEED_WIDTH)
jerk_weight = _sigmoid((abs(desired_lateral_jerk) - KIA_CARNIVAL_HIGHWAY_TRANSITION_JERK) /
KIA_CARNIVAL_HIGHWAY_TRANSITION_JERK_WIDTH)
lat_weight = _sigmoid((KIA_CARNIVAL_HIGHWAY_TRANSITION_LAT_CUTOFF - abs(desired_lateral_accel)) /
KIA_CARNIVAL_HIGHWAY_TRANSITION_LAT_WIDTH)
return 1.0 - (KIA_CARNIVAL_HIGHWAY_TRANSITION_TAPER_MAX * speed_weight * jerk_weight * lat_weight)
def get_kia_carnival_friction_jerk_deadzone(v_ego: float, desired_lateral_accel: float,
desired_lateral_jerk: float) -> float:
"""Reduce abrupt friction reversals during mid-speed curve exits only."""
speed_weight = _sigmoid((v_ego - KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_SPEED) /
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_SPEED_WIDTH)
speed_cutoff = _sigmoid((KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_SPEED_CUTOFF - v_ego) /
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_SPEED_CUTOFF_WIDTH)
center_weight = _sigmoid((KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_LAT - abs(desired_lateral_accel)) /
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_LAT_WIDTH)
jerk_weight = _sigmoid((abs(desired_lateral_jerk) - KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_JERK) /
KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_JERK_WIDTH)
return KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_MAX * speed_weight * speed_cutoff * center_weight * jerk_weight
def get_kia_carnival_unwind_ff_scale(setpoint: float, measured_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float) -> float:
"""Remove stale turn feedforward when the measured response carries through an unwind."""
if setpoint * desired_lateral_jerk >= 0.0:
return 1.0
overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0)
if overshoot <= 0.0:
return 1.0
speed_weight = (_sigmoid((v_ego - KIA_CARNIVAL_UNWIND_FF_SPEED) /
KIA_CARNIVAL_UNWIND_FF_SPEED_WIDTH) *
_sigmoid((KIA_CARNIVAL_UNWIND_FF_SPEED_CUTOFF - v_ego) /
KIA_CARNIVAL_UNWIND_FF_SPEED_CUTOFF_WIDTH))
overshoot_weight = _sigmoid((overshoot - KIA_CARNIVAL_UNWIND_FF_OVERSHOOT) /
KIA_CARNIVAL_UNWIND_FF_OVERSHOOT_WIDTH)
jerk_weight = _sigmoid((abs(desired_lateral_jerk) - KIA_CARNIVAL_UNWIND_FF_JERK) /
KIA_CARNIVAL_UNWIND_FF_JERK_WIDTH)
return 1.0 - (KIA_CARNIVAL_UNWIND_FF_REDUCTION_MAX * speed_weight * overshoot_weight * jerk_weight)
def _tucson_4th_gen_center_weights(desired_lateral_accel: float, v_ego: float) -> tuple[float, float]:
speed_weight = _sigmoid((TUCSON_4TH_GEN_CENTER_TAPER_SPEED_MAX - v_ego) / TUCSON_4TH_GEN_CENTER_TAPER_SPEED_WIDTH)
center_weight = _sigmoid((TUCSON_4TH_GEN_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / TUCSON_4TH_GEN_CENTER_TAPER_LAT_WIDTH)
return speed_weight, center_weight
def get_tucson_4th_gen_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight, center_weight = _tucson_4th_gen_center_weights(desired_lateral_accel, v_ego)
return 1.0 - (TUCSON_4TH_GEN_CENTER_TAPER_MAX * speed_weight * center_weight)
def get_tucson_4th_gen_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0,
desired_lateral_jerk: float = 0.0) -> float:
del desired_lateral_jerk
speed_weight, center_weight = _tucson_4th_gen_center_weights(desired_lateral_accel, v_ego)
return get_hkg_canfd_base_friction_threshold(v_ego) * (1.0 + TUCSON_4TH_GEN_FRICTION_THRESHOLD_GAIN * speed_weight * center_weight)
def _kia_forte_sigmoid(x: float) -> float:
return _sigmoid(x)
def _kia_forte_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / KIA_FORTE_TRANSITION_SPEED) ** 2)
def _kia_forte_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
return math.tanh((desired_lateral_accel * desired_lateral_jerk) / KIA_FORTE_PHASE_SCALE)
def _kia_forte_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def get_kia_forte_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
abs_lateral_accel = abs(desired_lateral_accel)
onset = _kia_forte_sigmoid((abs_lateral_accel - KIA_FORTE_FF_ONSET) / KIA_FORTE_FF_ONSET_WIDTH)
cutoff = _kia_forte_sigmoid((KIA_FORTE_FF_CUTOFF - abs_lateral_accel) / KIA_FORTE_FF_CUTOFF_WIDTH)
base_reduction = _kia_forte_side_value(desired_lateral_accel, KIA_FORTE_FF_REDUCTION_LEFT, KIA_FORTE_FF_REDUCTION_RIGHT) * onset * cutoff
phase = _kia_forte_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
low_speed_factor = _kia_forte_low_speed_factor(v_ego)
turn_in_boost = 1.0 + (_kia_forte_side_value(desired_lateral_accel, KIA_FORTE_TURN_IN_BOOST_LEFT, KIA_FORTE_TURN_IN_BOOST_RIGHT) *
turn_in_weight * (0.35 + 0.65 * low_speed_factor))
unwind_taper = 1.0 - (_kia_forte_side_value(desired_lateral_accel, KIA_FORTE_UNWIND_TAPER_LEFT, KIA_FORTE_UNWIND_TAPER_RIGHT) *
unwind_weight * (0.35 + 0.65 * low_speed_factor))
crawl_turn_in_scale = 0.0
if desired_lateral_accel * desired_lateral_jerk > 0.0:
crawl_speed_weight = _kia_forte_sigmoid((KIA_FORTE_CRAWL_TURN_IN_FF_SPEED - max(v_ego, 0.0)) /
KIA_FORTE_CRAWL_TURN_IN_FF_SPEED_WIDTH)
crawl_lat_weight = _kia_forte_sigmoid((abs_lateral_accel - KIA_FORTE_CRAWL_TURN_IN_FF_LAT) /
KIA_FORTE_CRAWL_TURN_IN_FF_LAT_WIDTH)
crawl_turn_in_scale = _kia_forte_side_value(desired_lateral_accel, KIA_FORTE_CRAWL_TURN_IN_FF_BOOST_LEFT,
KIA_FORTE_CRAWL_TURN_IN_FF_BOOST_RIGHT) * crawl_speed_weight * crawl_lat_weight
return ((1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0)) + crawl_turn_in_scale
def get_kia_forte_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _kia_forte_sigmoid((v_ego - KIA_FORTE_CENTER_TAPER_SPEED) / KIA_FORTE_CENTER_TAPER_SPEED_WIDTH)
center_weight = _kia_forte_sigmoid((KIA_FORTE_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / KIA_FORTE_CENTER_TAPER_LAT_WIDTH)
reduction = KIA_FORTE_CENTER_TAPER_MAX * speed_weight * center_weight
return 1.0 - reduction
def get_kia_forte_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_gm_base_friction_threshold(v_ego)
speed_weight = _kia_forte_sigmoid((v_ego - KIA_FORTE_FRICTION_SPEED) / KIA_FORTE_FRICTION_SPEED_WIDTH)
center_weight = _kia_forte_sigmoid((KIA_FORTE_FRICTION_CENTER_LAT - abs(desired_lateral_accel)) / KIA_FORTE_FRICTION_CENTER_LAT_WIDTH)
calm_jerk_weight = _kia_forte_sigmoid((KIA_FORTE_FRICTION_CALM_JERK - abs(desired_lateral_jerk)) / KIA_FORTE_FRICTION_CALM_JERK_WIDTH)
threshold_scale = 1.0 + (KIA_FORTE_FRICTION_THRESHOLD_GAIN * speed_weight * center_weight * calm_jerk_weight)
return base_threshold * min(max(threshold_scale, 1.0), 1.18)
def _palisade_sigmoid(x: float) -> float:
return _sigmoid(x)
def _palisade_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / PALISADE_TRANSITION_SPEED) ** 2)
def _palisade_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
return math.tanh((desired_lateral_accel * desired_lateral_jerk) / PALISADE_PHASE_SCALE)
def _palisade_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def _palisade_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / PALISADE_FRICTION_LAT_RISE)
jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / PALISADE_FRICTION_JERK_RISE)
return _palisade_low_speed_factor(v_ego) * lat_factor * jerk_factor
def get_palisade_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
gain = _palisade_side_value(desired_lateral_accel, PALISADE_FF_GAIN_LEFT, PALISADE_FF_GAIN_RIGHT)
abs_lateral_accel = abs(desired_lateral_accel)
onset = _palisade_sigmoid((abs_lateral_accel - PALISADE_FF_ONSET) / PALISADE_FF_ONSET_WIDTH)
cutoff = _palisade_sigmoid((PALISADE_FF_CUTOFF - abs_lateral_accel) / PALISADE_FF_CUTOFF_WIDTH)
extra_scale = gain * onset * cutoff
phase = _palisade_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
low_speed_factor = _palisade_low_speed_factor(v_ego)
turn_in_boost = 1.0 + (_palisade_side_value(desired_lateral_accel, PALISADE_TURN_IN_BOOST_LEFT, PALISADE_TURN_IN_BOOST_RIGHT) *
turn_in_weight * (0.35 + 0.65 * low_speed_factor))
unwind_taper = 1.0 - (_palisade_side_value(desired_lateral_accel, PALISADE_UNWIND_TAPER_LEFT, PALISADE_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))
def get_palisade_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_gm_base_friction_threshold(v_ego)
transition_envelope = _palisade_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _palisade_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 - (_palisade_side_value(desired_lateral_accel, PALISADE_TURN_IN_THRESHOLD_REDUCTION_LEFT, PALISADE_TURN_IN_THRESHOLD_REDUCTION_RIGHT) *
transition_envelope * turn_in_weight)
threshold_scale += (_palisade_side_value(desired_lateral_accel, PALISADE_UNWIND_THRESHOLD_INCREASE_LEFT, PALISADE_UNWIND_THRESHOLD_INCREASE_RIGHT) *
transition_envelope * unwind_weight)
return base_threshold * min(max(threshold_scale, 0.84), 1.14)
def get_palisade_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
transition_envelope = _palisade_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _palisade_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
friction_scale = PALISADE_FRICTION_MULT
friction_scale += (_palisade_side_value(desired_lateral_accel, PALISADE_TURN_IN_FRICTION_BOOST_LEFT, PALISADE_TURN_IN_FRICTION_BOOST_RIGHT) *
transition_envelope * turn_in_weight)
friction_scale -= (_palisade_side_value(desired_lateral_accel, PALISADE_UNWIND_FRICTION_REDUCTION_LEFT, PALISADE_UNWIND_FRICTION_REDUCTION_RIGHT) *
transition_envelope * unwind_weight)
return min(max(friction_scale, 0.92), 1.12)
def get_palisade_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _palisade_sigmoid((v_ego - PALISADE_CENTER_TAPER_SPEED) / PALISADE_CENTER_TAPER_SPEED_WIDTH)
center_weight = _palisade_sigmoid((PALISADE_CENTER_TAPER_LAT - abs(desired_lateral_accel)) /
PALISADE_CENTER_TAPER_LAT_WIDTH)
return 1.0 - (PALISADE_CENTER_TAPER_MAX * speed_weight * center_weight)
def get_palisade_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
"""Reduce high-speed center corrections without reducing normal turn authority."""
speed_weight = _palisade_sigmoid((v_ego - PALISADE_CENTER_OUTPUT_TAPER_SPEED) /
PALISADE_CENTER_OUTPUT_TAPER_SPEED_WIDTH)
center_weight = _palisade_sigmoid((PALISADE_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) /
PALISADE_CENTER_OUTPUT_TAPER_LAT_WIDTH)
return 1.0 - (PALISADE_CENTER_OUTPUT_TAPER_MAX * speed_weight * center_weight)
def genesis_g90_lateral_testing_ground_active() -> bool:
return testing_ground.use(GENESIS_G90_LATERAL_TESTING_GROUND_ID)
def _genesis_g90_sigmoid(x: float) -> float:
return _sigmoid(x)
def _genesis_g90_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / GENESIS_G90_TRANSITION_SPEED) ** 2)
def _genesis_g90_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
return math.tanh((desired_lateral_accel * desired_lateral_jerk) / GENESIS_G90_PHASE_SCALE)
def _genesis_g90_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def _genesis_g90_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / GENESIS_G90_FRICTION_LAT_RISE)
jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / GENESIS_G90_FRICTION_JERK_RISE)
return _genesis_g90_low_speed_factor(v_ego) * lat_factor * jerk_factor
def get_genesis_g90_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
gain = _genesis_g90_side_value(desired_lateral_accel, GENESIS_G90_FF_GAIN_LEFT, GENESIS_G90_FF_GAIN_RIGHT)
abs_lateral_accel = abs(desired_lateral_accel)
onset = _genesis_g90_sigmoid((abs_lateral_accel - GENESIS_G90_FF_ONSET) / GENESIS_G90_FF_ONSET_WIDTH)
cutoff = _genesis_g90_sigmoid((GENESIS_G90_FF_CUTOFF - abs_lateral_accel) / GENESIS_G90_FF_CUTOFF_WIDTH)
extra_scale = gain * onset * cutoff
phase = _genesis_g90_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
low_speed_factor = _genesis_g90_low_speed_factor(v_ego)
turn_in_boost = 1.0 + (_genesis_g90_side_value(desired_lateral_accel, GENESIS_G90_TURN_IN_BOOST_LEFT, GENESIS_G90_TURN_IN_BOOST_RIGHT) *
turn_in_weight * (0.35 + 0.65 * low_speed_factor))
unwind_taper = 1.0 - (_genesis_g90_side_value(desired_lateral_accel, GENESIS_G90_UNWIND_TAPER_LEFT, GENESIS_G90_UNWIND_TAPER_RIGHT) *
unwind_weight * (0.30 + 0.70 * low_speed_factor))
return 1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0))
def get_genesis_g90_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_gm_base_friction_threshold(v_ego)
transition_envelope = _genesis_g90_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _genesis_g90_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 - (_genesis_g90_side_value(desired_lateral_accel, GENESIS_G90_TURN_IN_THRESHOLD_REDUCTION_LEFT, GENESIS_G90_TURN_IN_THRESHOLD_REDUCTION_RIGHT) *
transition_envelope * turn_in_weight)
threshold_scale += (_genesis_g90_side_value(desired_lateral_accel, GENESIS_G90_UNWIND_THRESHOLD_INCREASE_LEFT, GENESIS_G90_UNWIND_THRESHOLD_INCREASE_RIGHT) *
transition_envelope * unwind_weight)
return base_threshold * min(max(threshold_scale, 0.86), 1.12)
def get_genesis_g90_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
transition_envelope = _genesis_g90_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _genesis_g90_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
friction_scale = GENESIS_G90_FRICTION_MULT
friction_scale += (_genesis_g90_side_value(desired_lateral_accel, GENESIS_G90_TURN_IN_FRICTION_BOOST_LEFT, GENESIS_G90_TURN_IN_FRICTION_BOOST_RIGHT) *
transition_envelope * turn_in_weight)
friction_scale -= (_genesis_g90_side_value(desired_lateral_accel, GENESIS_G90_UNWIND_FRICTION_REDUCTION_LEFT, GENESIS_G90_UNWIND_FRICTION_REDUCTION_RIGHT) *
transition_envelope * unwind_weight)
return min(max(friction_scale, 0.92), 1.12)
def get_genesis_gv70_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0,
desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_hkg_canfd_base_friction_threshold(v_ego)
speed_onset = _sigmoid((v_ego - GENESIS_GV70_FRICTION_SPEED_ONSET) / GENESIS_GV70_FRICTION_SPEED_ONSET_WIDTH)
speed_cutoff = _sigmoid((GENESIS_GV70_FRICTION_SPEED_CUTOFF - v_ego) / GENESIS_GV70_FRICTION_SPEED_CUTOFF_WIDTH)
center_weight = _sigmoid((GENESIS_GV70_FRICTION_CENTER_LAT - abs(desired_lateral_accel)) /
GENESIS_GV70_FRICTION_CENTER_LAT_WIDTH)
calm_jerk_weight = _sigmoid((GENESIS_GV70_FRICTION_CALM_JERK - abs(desired_lateral_jerk)) /
GENESIS_GV70_FRICTION_CALM_JERK_WIDTH)
gain = (GENESIS_GV70_FRICTION_THRESHOLD_GAIN * speed_onset * speed_cutoff *
center_weight * calm_jerk_weight)
return base_threshold * (1.0 + gain)
def get_genesis_gv70_unwind_ff_scale(setpoint: float, measured_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float) -> float:
"""Remove old-turn feedforward when the GV70 has already over-rotated."""
if setpoint * desired_lateral_jerk >= 0.0 or setpoint * measured_lateral_accel <= 0.0:
return 1.0
overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0)
if overshoot <= 0.0:
return 1.0
overshoot_weight = _sigmoid((overshoot - GENESIS_GV70_UNWIND_FF_OVERSHOOT) /
GENESIS_GV70_UNWIND_FF_OVERSHOOT_WIDTH)
jerk_weight = _sigmoid((abs(desired_lateral_jerk) - GENESIS_GV70_UNWIND_FF_JERK) /
GENESIS_GV70_UNWIND_FF_JERK_WIDTH)
speed_weight = _sigmoid((v_ego - GENESIS_GV70_UNWIND_FF_SPEED) /
GENESIS_GV70_UNWIND_FF_SPEED_WIDTH)
return 1.0 - GENESIS_GV70_UNWIND_FF_REDUCTION_MAX * overshoot_weight * jerk_weight * speed_weight
def get_genesis_gv70_high_speed_error_scale(setpoint: float, measured_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float) -> float:
tracking_error = abs(measured_lateral_accel - setpoint)
if tracking_error <= 0.0:
return 1.0
speed_weight = _sigmoid((v_ego - GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_SPEED) /
GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_SPEED_WIDTH)
error_weight = _sigmoid((tracking_error - GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_ERROR) /
GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_ERROR_WIDTH)
jerk_weight = _sigmoid((abs(desired_lateral_jerk) - GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_JERK) /
GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_JERK_WIDTH)
phase_weight = 1.0 if setpoint * desired_lateral_jerk < 0.0 else 0.45
reduction = (GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_MAX * speed_weight * error_weight *
(0.35 + (0.65 * jerk_weight)) * phase_weight)
return 1.0 - reduction
def get_genesis_g70_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0,
desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_standard_friction_threshold(v_ego)
speed_onset = _sigmoid((v_ego - GENESIS_G70_FRICTION_SPEED_ONSET) / GENESIS_G70_FRICTION_SPEED_ONSET_WIDTH)
speed_cutoff = _sigmoid((GENESIS_G70_FRICTION_SPEED_CUTOFF - v_ego) / GENESIS_G70_FRICTION_SPEED_CUTOFF_WIDTH)
center_weight = _sigmoid((GENESIS_G70_FRICTION_CENTER_LAT - abs(desired_lateral_accel)) /
GENESIS_G70_FRICTION_CENTER_LAT_WIDTH)
calm_jerk_weight = _sigmoid((GENESIS_G70_FRICTION_CALM_JERK - abs(desired_lateral_jerk)) /
GENESIS_G70_FRICTION_CALM_JERK_WIDTH)
gain = (GENESIS_G70_FRICTION_THRESHOLD_GAIN * speed_onset * speed_cutoff *
center_weight * calm_jerk_weight)
return base_threshold * (1.0 + gain)
def get_genesis_g70_friction_jerk_deadzone(v_ego: float, desired_lateral_accel: float) -> float:
speed_weight = _sigmoid((v_ego - GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED) /
GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED_WIDTH)
center_weight = _sigmoid((GENESIS_G70_FRICTION_JERK_DEADZONE_LAT - abs(desired_lateral_accel)) /
GENESIS_G70_FRICTION_JERK_DEADZONE_LAT_WIDTH)
return GENESIS_G70_FRICTION_JERK_DEADZONE_MAX * speed_weight * center_weight
def get_genesis_g70_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _sigmoid((v_ego - GENESIS_G70_CENTER_OUTPUT_TAPER_SPEED) /
GENESIS_G70_CENTER_OUTPUT_TAPER_SPEED_WIDTH)
center_weight = _sigmoid((GENESIS_G70_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) /
GENESIS_G70_CENTER_OUTPUT_TAPER_LAT_WIDTH)
reduction = GENESIS_G70_CENTER_OUTPUT_TAPER_MAX * speed_weight * center_weight
low_speed_weight = _sigmoid((GENESIS_G70_LOW_SPEED_CENTER_TAPER_SPEED_MAX - v_ego) /
GENESIS_G70_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH)
low_speed_center_weight = _sigmoid((GENESIS_G70_LOW_SPEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) /
GENESIS_G70_LOW_SPEED_CENTER_TAPER_LAT_WIDTH)
reduction += GENESIS_G70_LOW_SPEED_CENTER_TAPER_MAX * low_speed_weight * low_speed_center_weight
return 1.0 - reduction
def get_genesis_g70_low_speed_angle_damping(desired_angle_deg: float, actual_angle_deg: float,
current_output_torque: float, v_ego: float) -> float:
angle_error = desired_angle_deg - actual_angle_deg
speed_weight = _sigmoid((GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_SPEED - max(v_ego, 0.0)) /
GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_SPEED_WIDTH)
error_weight = _sigmoid((abs(angle_error) - GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_ERROR) /
GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_ERROR_WIDTH)
actual_angle_weight = _sigmoid((abs(actual_angle_deg) - GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_ACTUAL) /
GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_ACTUAL_WIDTH)
damping_torque = math.copysign(
GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_MAX * speed_weight * error_weight * actual_angle_weight,
-angle_error,
)
if abs(damping_torque) < 1e-4:
return current_output_torque
if current_output_torque * damping_torque >= 0.0:
damping_torque *= GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_BLEND
return float(np.clip(current_output_torque + damping_torque, -1.0, 1.0))
def get_genesis_g70_low_speed_output_limit(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _sigmoid((GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED - max(v_ego, 0.0)) /
GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH)
center_weight = _sigmoid((GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT - abs(desired_lateral_accel)) /
GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT_WIDTH)
return max(0.05, 1.0 - GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_REDUCTION * speed_weight * center_weight)
def get_genesis_g70_curve_unwind_output_scale(desired_lateral_accel: float, desired_lateral_jerk: float,
v_ego: float) -> float:
if desired_lateral_accel * desired_lateral_jerk >= 0.0:
return 1.0
speed_weight = _sigmoid((max(v_ego, 0.0) - GENESIS_G70_CURVE_UNWIND_SPEED) /
GENESIS_G70_CURVE_UNWIND_SPEED_WIDTH)
lateral_weight = _sigmoid((abs(desired_lateral_accel) - GENESIS_G70_CURVE_UNWIND_LAT) /
GENESIS_G70_CURVE_UNWIND_LAT_WIDTH)
jerk_weight = _sigmoid((abs(desired_lateral_jerk) - GENESIS_G70_CURVE_UNWIND_JERK) /
GENESIS_G70_CURVE_UNWIND_JERK_WIDTH)
return 1.0 + GENESIS_G70_CURVE_UNWIND_OUTPUT_BOOST * speed_weight * lateral_weight * jerk_weight
def get_genesis_g70_unwind_ff_scale(setpoint: float, measured_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float) -> float:
if setpoint * desired_lateral_jerk >= 0.0 or setpoint * measured_lateral_accel <= 0.0:
return 1.0
overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0)
if overshoot <= 0.0:
return 1.0
overshoot_weight = _sigmoid((overshoot - GENESIS_G70_UNWIND_FF_OVERSHOOT) /
GENESIS_G70_UNWIND_FF_OVERSHOOT_WIDTH)
jerk_weight = _sigmoid((abs(desired_lateral_jerk) - GENESIS_G70_UNWIND_FF_JERK) /
GENESIS_G70_UNWIND_FF_JERK_WIDTH)
speed_weight = _sigmoid((v_ego - GENESIS_G70_UNWIND_FF_SPEED) /
GENESIS_G70_UNWIND_FF_SPEED_WIDTH)
return 1.0 - GENESIS_G70_UNWIND_FF_REDUCTION_MAX * overshoot_weight * jerk_weight * speed_weight
def get_genesis_g70_high_speed_error_scale(setpoint: float, measured_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float) -> float:
tracking_error = abs(measured_lateral_accel - setpoint)
if tracking_error <= 0.0:
return 1.0
speed_weight = _sigmoid((v_ego - GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_SPEED) /
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_SPEED_WIDTH)
error_weight = _sigmoid((tracking_error - GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR) /
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR_WIDTH)
jerk_weight = _sigmoid((abs(desired_lateral_jerk) - GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK) /
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK_WIDTH)
phase_weight = 1.0 if setpoint * desired_lateral_jerk < 0.0 else 0.45
reduction = (GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_MAX * speed_weight * error_weight *
(0.35 + (0.65 * jerk_weight)) * phase_weight)
return 1.0 - reduction
def _ioniq_5_sigmoid(x: float) -> float:
return _sigmoid(x)
def _ioniq_5_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / IONIQ_5_TRANSITION_SPEED) ** 2)
def _ioniq_5_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
return math.tanh((desired_lateral_accel * desired_lateral_jerk) / IONIQ_5_PHASE_SCALE)
def _ioniq_5_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def _ioniq_5_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
abs_lateral_accel = abs(desired_lateral_accel)
onset = _ioniq_5_sigmoid((abs_lateral_accel - IONIQ_5_FF_ONSET) / IONIQ_5_FF_ONSET_WIDTH)
cutoff = _ioniq_5_sigmoid((IONIQ_5_FF_CUTOFF - abs_lateral_accel) / IONIQ_5_FF_CUTOFF_WIDTH)
return onset * cutoff * _ioniq_5_low_speed_factor(v_ego)
def get_ioniq_5_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
envelope = _ioniq_5_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _ioniq_5_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
low_speed_factor = _ioniq_5_low_speed_factor(v_ego)
abs_lateral_accel = abs(desired_lateral_accel)
base_reduction = _ioniq_5_side_value(desired_lateral_accel, IONIQ_5_FF_REDUCTION_LEFT, IONIQ_5_FF_REDUCTION_RIGHT) * envelope
turn_in_boost = 1.0 + (_ioniq_5_side_value(desired_lateral_accel, IONIQ_5_TURN_IN_BOOST_LEFT, IONIQ_5_TURN_IN_BOOST_RIGHT) *
turn_in_weight * (0.35 + 0.65 * low_speed_factor))
unwind_taper = 1.0 - (_ioniq_5_side_value(desired_lateral_accel, IONIQ_5_UNWIND_TAPER_LEFT, IONIQ_5_UNWIND_TAPER_RIGHT) *
unwind_weight * (0.35 + 0.65 * low_speed_factor))
sustained_turn_in_scale = 0.0
if desired_lateral_accel * desired_lateral_jerk > 0.0:
sustained_speed_weight = _ioniq_5_sigmoid((max(v_ego, 0.0) - IONIQ_5_SUSTAINED_TURN_IN_FF_SPEED) /
IONIQ_5_SUSTAINED_TURN_IN_FF_SPEED_WIDTH)
sustained_lat_onset = _ioniq_5_sigmoid((abs_lateral_accel - IONIQ_5_SUSTAINED_TURN_IN_FF_LAT_START) /
IONIQ_5_SUSTAINED_TURN_IN_FF_LAT_WIDTH)
sustained_lat_cutoff = _ioniq_5_sigmoid((IONIQ_5_SUSTAINED_TURN_IN_FF_LAT_END - abs_lateral_accel) /
IONIQ_5_SUSTAINED_TURN_IN_FF_LAT_WIDTH)
sustained_turn_in_scale = (_ioniq_5_side_value(desired_lateral_accel,
IONIQ_5_SUSTAINED_TURN_IN_FF_BOOST_LEFT,
IONIQ_5_SUSTAINED_TURN_IN_FF_BOOST_RIGHT) *
sustained_speed_weight * sustained_lat_onset * sustained_lat_cutoff)
return (1.0 + sustained_turn_in_scale) * (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0)
def get_ioniq_5_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_hkg_canfd_base_friction_threshold(v_ego)
envelope = _ioniq_5_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _ioniq_5_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 - (_ioniq_5_side_value(desired_lateral_accel, IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_LEFT, IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_RIGHT) *
envelope * turn_in_weight)
threshold_scale += (_ioniq_5_side_value(desired_lateral_accel, IONIQ_5_UNWIND_THRESHOLD_INCREASE_LEFT, IONIQ_5_UNWIND_THRESHOLD_INCREASE_RIGHT) *
envelope * unwind_weight)
return base_threshold * min(max(threshold_scale, 0.86), 1.18)
def get_ioniq_5_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
if desired_lateral_accel == 0.0 or desired_lateral_jerk == 0.0:
return 1.0
envelope = _ioniq_5_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _ioniq_5_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
friction_scale = 1.0
friction_scale += (_ioniq_5_side_value(desired_lateral_accel, IONIQ_5_TURN_IN_FRICTION_BOOST_LEFT, IONIQ_5_TURN_IN_FRICTION_BOOST_RIGHT) *
envelope * turn_in_weight)
friction_scale -= (_ioniq_5_side_value(desired_lateral_accel, IONIQ_5_UNWIND_FRICTION_REDUCTION_LEFT, IONIQ_5_UNWIND_FRICTION_REDUCTION_RIGHT) *
envelope * unwind_weight)
return min(max(friction_scale, 0.86), 1.04)
def get_ioniq_5_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _ioniq_5_sigmoid((v_ego - IONIQ_5_CENTER_TAPER_SPEED) / IONIQ_5_CENTER_TAPER_SPEED_WIDTH)
center_weight = _ioniq_5_sigmoid((IONIQ_5_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / IONIQ_5_CENTER_TAPER_LAT_WIDTH)
reduction = IONIQ_5_CENTER_TAPER_MAX * speed_weight * center_weight
return 1.0 - reduction
def get_ioniq_5_low_speed_output_limit(desired_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float) -> float:
"""Bound stop-transition center chatter without blunting actual turns."""
speed_weight = _ioniq_5_sigmoid((IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_SPEED - max(v_ego, 0.0)) /
IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH)
center_weight = _ioniq_5_sigmoid((IONIQ_5_LOW_SPEED_CENTER_LAT - abs(desired_lateral_accel)) /
IONIQ_5_LOW_SPEED_CENTER_LAT_WIDTH)
calm_weight = _ioniq_5_sigmoid((IONIQ_5_LOW_SPEED_CENTER_JERK - abs(desired_lateral_jerk)) /
IONIQ_5_LOW_SPEED_CENTER_JERK_WIDTH)
center_weight *= calm_weight
center_limit = (IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_BASE +
IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_TURN_RELIEF * (1.0 - center_weight))
limit = 1.0 - speed_weight * (1.0 - center_limit)
return float(np.clip(limit, IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_BASE, 1.0))
def get_ioniq_5_friction_jerk_deadzone(v_ego: float, desired_lateral_accel: float) -> float:
"""Suppress high-speed friction reversals without reducing steady-turn torque."""
speed_weight = _ioniq_5_sigmoid((max(v_ego, 0.0) - IONIQ_5_FRICTION_JERK_DEADZONE_SPEED) /
IONIQ_5_FRICTION_JERK_DEADZONE_SPEED_WIDTH)
curve_weight = _ioniq_5_sigmoid((IONIQ_5_FRICTION_JERK_DEADZONE_LAT - abs(desired_lateral_accel)) /
IONIQ_5_FRICTION_JERK_DEADZONE_LAT_WIDTH)
return IONIQ_5_FRICTION_JERK_DEADZONE_MAX * speed_weight * curve_weight
def _ioniq_ev_old_sigmoid(x: float) -> float:
return _sigmoid(x)
def _ioniq_ev_old_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / IONIQ_EV_OLD_TRANSITION_SPEED) ** 2)
def _ioniq_ev_old_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
return math.tanh((desired_lateral_accel * desired_lateral_jerk) / IONIQ_EV_OLD_PHASE_SCALE)
def _ioniq_ev_old_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def get_ioniq_ev_old_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
abs_lateral_accel = abs(desired_lateral_accel)
onset = _ioniq_ev_old_sigmoid((abs_lateral_accel - IONIQ_EV_OLD_FF_ONSET) / IONIQ_EV_OLD_FF_ONSET_WIDTH)
cutoff = _ioniq_ev_old_sigmoid((IONIQ_EV_OLD_FF_CUTOFF - abs_lateral_accel) / IONIQ_EV_OLD_FF_CUTOFF_WIDTH)
base_reduction = _ioniq_ev_old_side_value(desired_lateral_accel, IONIQ_EV_OLD_FF_REDUCTION_LEFT, IONIQ_EV_OLD_FF_REDUCTION_RIGHT) * onset * cutoff
phase = _ioniq_ev_old_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
low_speed_factor = _ioniq_ev_old_low_speed_factor(v_ego)
turn_in_boost = 1.0 + (_ioniq_ev_old_side_value(desired_lateral_accel, IONIQ_EV_OLD_TURN_IN_BOOST_LEFT, IONIQ_EV_OLD_TURN_IN_BOOST_RIGHT) *
turn_in_weight * (0.35 + 0.65 * low_speed_factor))
unwind_taper = 1.0 - (_ioniq_ev_old_side_value(desired_lateral_accel, IONIQ_EV_OLD_UNWIND_TAPER_LEFT, IONIQ_EV_OLD_UNWIND_TAPER_RIGHT) *
unwind_weight * (0.35 + 0.65 * low_speed_factor))
return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0)
def get_ioniq_ev_old_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _ioniq_ev_old_sigmoid((v_ego - IONIQ_EV_OLD_CENTER_TAPER_SPEED) / IONIQ_EV_OLD_CENTER_TAPER_SPEED_WIDTH)
center_weight = _ioniq_ev_old_sigmoid((IONIQ_EV_OLD_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / IONIQ_EV_OLD_CENTER_TAPER_LAT_WIDTH)
reduction = IONIQ_EV_OLD_CENTER_TAPER_MAX * speed_weight * center_weight
return 1.0 - reduction
def _ioniq_6_sigmoid(x: float) -> float:
return _sigmoid(x)
def _ioniq_6_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / IONIQ_6_TRANSITION_SPEED) ** 2)
def _ioniq_6_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 _ioniq_6_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def _ioniq_6_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 _ioniq_6_low_speed_factor(v_ego) * lat_factor * jerk_factor
def _ioniq_6_curvy_speed_weight(v_ego: float) -> float:
curvy_speed_min = _flm_vehicle_knob("hyundai_ioniq_6.curvy_speed_min", IONIQ_6_CURVY_SPEED_MIN)
curvy_speed_max = _flm_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:
curvy_turn_in_speed_min = _flm_vehicle_knob("hyundai_ioniq_6.curvy_turn_in_trim_speed_min", IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_MIN)
curvy_turn_in_speed_max = _flm_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
def get_ioniq_6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float,
directional_taper_scale: float | None = None) -> float:
if desired_lateral_accel == 0.0:
return 1.0
gain = _ioniq_6_side_value(
desired_lateral_accel,
_flm_vehicle_knob("hyundai_ioniq_6.ff_gain_left", IONIQ_6_FF_GAIN_LEFT),
_flm_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)
extra_scale = gain * onset * cutoff
phase = _ioniq_6_transition_phase(desired_lateral_accel, desired_lateral_jerk)
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,
_flm_vehicle_knob("hyundai_ioniq_6.turn_in_boost_left", IONIQ_6_TURN_IN_BOOST_LEFT),
_flm_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,
_flm_vehicle_knob("hyundai_ioniq_6.unwind_taper_left", IONIQ_6_UNWIND_TAPER_LEFT),
_flm_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:
crawl_speed_weight = _ioniq_6_sigmoid((IONIQ_6_CRAWL_TURN_IN_FF_SPEED - max(v_ego, 0.0)) /
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,
_flm_vehicle_knob("hyundai_ioniq_6.crawl_turn_in_ff_boost_left", IONIQ_6_CRAWL_TURN_IN_FF_BOOST_LEFT),
_flm_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) /
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED_WIDTH)
high_speed_lat_onset = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_START) /
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH)
high_speed_lat_cutoff = _ioniq_6_sigmoid((IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_END - abs_lateral_accel) /
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH)
high_speed_right_turn_in_scale = IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_BOOST * high_speed_weight * high_speed_lat_onset * high_speed_lat_cutoff
if directional_taper_scale is None:
directional_taper_scale = get_ioniq_6_directional_taper_scale(desired_lateral_accel, desired_lateral_jerk, v_ego)
return (1.0 + crawl_turn_in_scale + high_speed_right_turn_in_scale +
(extra_scale * turn_in_boost * max(unwind_taper, 0.0))) * directional_taper_scale
def get_ioniq_6_2023_unwind_ff_scale(setpoint: float, measured_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float) -> float:
"""Trim residual curve feedforward when the 2023 car is already over-rotated."""
if setpoint * desired_lateral_jerk >= 0.0 or setpoint * measured_lateral_accel <= 0.0:
return 1.0
overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0)
if overshoot <= 0.0:
return 1.0
overshoot_weight = _ioniq_6_sigmoid((overshoot - IONIQ_6_2023_UNWIND_FF_OVERSHOOT) /
IONIQ_6_2023_UNWIND_FF_OVERSHOOT_WIDTH)
jerk_weight = _ioniq_6_sigmoid((abs(desired_lateral_jerk) - IONIQ_6_2023_UNWIND_FF_JERK) /
IONIQ_6_2023_UNWIND_FF_JERK_WIDTH)
speed_onset = _ioniq_6_sigmoid((v_ego - IONIQ_6_2023_UNWIND_FF_SPEED_ONSET) /
IONIQ_6_2023_UNWIND_FF_SPEED_ONSET_WIDTH)
speed_cutoff = _ioniq_6_sigmoid((IONIQ_6_2023_UNWIND_FF_SPEED_CUTOFF - v_ego) /
IONIQ_6_2023_UNWIND_FF_SPEED_CUTOFF_WIDTH)
reduction = (IONIQ_6_2023_UNWIND_FF_REDUCTION_MAX * overshoot_weight * jerk_weight *
speed_onset * speed_cutoff)
return 1.0 - reduction
def get_ioniq_6_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = max(get_hkg_canfd_base_friction_threshold(v_ego), IONIQ_6_BASE_FRICTION_THRESHOLD)
transition_envelope = _ioniq_6_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _ioniq_6_transition_phase(desired_lateral_accel, desired_lateral_jerk)
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,
_flm_vehicle_knob("hyundai_ioniq_6.turn_in_threshold_reduction_left", IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT),
_flm_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,
_flm_vehicle_knob("hyundai_ioniq_6.unwind_threshold_increase_left", IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT),
_flm_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)
def get_ioniq_6_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
transition_envelope = _ioniq_6_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _ioniq_6_transition_phase(desired_lateral_accel, desired_lateral_jerk)
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)
friction_scale = IONIQ_6_FRICTION_MULT
friction_scale += (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT, IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT) *
transition_envelope * turn_in_weight)
friction_scale -= (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT, IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT) *
transition_envelope * unwind_weight * unwind_speed_weight)
return min(max(friction_scale, 0.82), 1.08)
def get_ioniq_6_friction_center_fade_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_FRICTION_CENTER_FADE_SPEED) / IONIQ_6_FRICTION_CENTER_FADE_SPEED_WIDTH)
center_weight = _ioniq_6_sigmoid((IONIQ_6_FRICTION_CENTER_FADE_LAT - abs(desired_lateral_accel)) / IONIQ_6_FRICTION_CENTER_FADE_LAT_WIDTH)
return 1.0 - IONIQ_6_FRICTION_CENTER_FADE_MAX * speed_weight * center_weight
def get_ioniq_6_2025_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_2025_CENTER_OUTPUT_TAPER_SPEED) /
IONIQ_6_2025_CENTER_OUTPUT_TAPER_SPEED_WIDTH)
center_weight = _ioniq_6_sigmoid((IONIQ_6_2025_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) /
IONIQ_6_2025_CENTER_OUTPUT_TAPER_LAT_WIDTH)
return 1.0 - IONIQ_6_2025_CENTER_OUTPUT_TAPER_MAX * speed_weight * center_weight
def get_ioniq_6_2025_low_speed_output_limit(desired_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float) -> float:
"""Limit small-signal torque at crawl speed while leaving real turn commands open."""
speed_weight = _ioniq_6_sigmoid((IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_SPEED - max(v_ego, 0.0)) /
IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH)
center_weight = _ioniq_6_sigmoid((IONIQ_6_2025_LOW_SPEED_CENTER_LAT - abs(desired_lateral_accel)) /
IONIQ_6_2025_LOW_SPEED_CENTER_LAT_WIDTH)
calm_weight = _ioniq_6_sigmoid((IONIQ_6_2025_LOW_SPEED_CENTER_JERK - abs(desired_lateral_jerk)) /
IONIQ_6_2025_LOW_SPEED_CENTER_JERK_WIDTH)
center_weight *= calm_weight
limit = (IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_BASE +
IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_TURN_RELIEF * (1.0 - center_weight) +
IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_SPEED_RELIEF * (1.0 - speed_weight))
return float(np.clip(limit, IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_BASE, 1.0))
def _ioniq_6_2025_low_speed_center_envelope(desired_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float) -> float:
speed_weight = _ioniq_6_sigmoid((IONIQ_6_2025_LOW_SPEED_CENTER_SPEED - max(v_ego, 0.0)) /
IONIQ_6_2025_LOW_SPEED_CENTER_SPEED_WIDTH)
center_weight = _ioniq_6_sigmoid((IONIQ_6_2025_LOW_SPEED_CENTER_LAT - abs(desired_lateral_accel)) /
IONIQ_6_2025_LOW_SPEED_CENTER_LAT_WIDTH)
calm_weight = _ioniq_6_sigmoid((IONIQ_6_2025_LOW_SPEED_CENTER_JERK - abs(desired_lateral_jerk)) /
IONIQ_6_2025_LOW_SPEED_CENTER_JERK_WIDTH)
return speed_weight * center_weight * calm_weight
def get_ioniq_6_2025_low_speed_center_error_scale(desired_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float) -> float:
envelope = _ioniq_6_2025_low_speed_center_envelope(desired_lateral_accel, desired_lateral_jerk, v_ego)
return 1.0 - (1.0 - IONIQ_6_2025_LOW_SPEED_CENTER_ERROR_SCALE) * envelope
def get_ioniq_6_2025_low_speed_center_friction_scale(desired_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float) -> float:
envelope = _ioniq_6_2025_low_speed_center_envelope(desired_lateral_accel, desired_lateral_jerk, v_ego)
return 1.0 - (1.0 - IONIQ_6_2025_LOW_SPEED_CENTER_FRICTION_SCALE) * envelope
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 = _flm_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 = _flm_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)
low_mid_speed_weight = low_mid_onset * low_mid_cutoff
low_mid_center_weight = _ioniq_6_sigmoid((IONIQ_6_LOW_MID_CENTER_TAPER_LAT - abs(desired_lateral_accel)) /
IONIQ_6_LOW_MID_CENTER_TAPER_LAT_WIDTH)
low_mid_reduction = IONIQ_6_LOW_MID_CENTER_TAPER_MAX * low_mid_speed_weight * low_mid_center_weight
return 1.0 - min(high_speed_reduction + highway_center_reduction + low_mid_reduction, 0.12)
def get_ioniq_6_directional_taper_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float | None = None) -> float:
if desired_lateral_accel == 0.0:
return 1.0
abs_lateral_accel = abs(desired_lateral_accel)
onset = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_DIRECTIONAL_TAPER_LAT_START) / IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH)
cutoff = _ioniq_6_sigmoid((IONIQ_6_DIRECTIONAL_TAPER_LAT_END - abs_lateral_accel) / IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH)
band_weight = onset * cutoff
heavy_band_weight = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START) / IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH)
phase = math.tanh((desired_lateral_accel * desired_lateral_jerk) / IONIQ_6_DIRECTIONAL_TAPER_PHASE_SCALE)
unwind_weight = max(-phase, 0.0) * _ioniq_6_sigmoid((abs(desired_lateral_jerk) - IONIQ_6_DIRECTIONAL_TAPER_JERK_ONSET) /
IONIQ_6_DIRECTIONAL_TAPER_JERK_WIDTH)
low_speed_relief_weight = 0.0
curvy_turn_in_trim_weight = 0.0
if v_ego is not None:
low_speed_weight = _ioniq_6_sigmoid((IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED - max(v_ego, 0.0)) /
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED_WIDTH)
tight_turn_weight = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT) /
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT_WIDTH)
low_speed_relief_weight = IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF * low_speed_weight * tight_turn_weight * (1.0 - unwind_weight)
turn_in_weight = max(phase, 0.0)
curvy_turn_in_speed_weight = _ioniq_6_curvy_turn_in_trim_speed_weight(v_ego)
curvy_turn_in_lat_onset = _ioniq_6_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 = _ioniq_6_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
base_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT, IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT)
unwind_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT)
heavy_base_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_RIGHT)
heavy_unwind_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT)
base_reduction *= 1.0 - low_speed_relief_weight
heavy_base_reduction *= 1.0 - low_speed_relief_weight
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,
_flm_vehicle_knob("hyundai_ioniq_6.curvy_turn_in_trim_left", IONIQ_6_CURVY_TURN_IN_TRIM_LEFT),
_flm_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
if v_ego is not None:
curvy_unwind_phase_weight = unwind_weight
if desired_lateral_accel < 0.0:
curvy_unwind_phase_weight = max(-phase, 0.0) * _ioniq_6_sigmoid(
(abs(desired_lateral_jerk) - IONIQ_6_CURVY_RIGHT_UNWIND_JERK_ONSET) / IONIQ_6_CURVY_RIGHT_UNWIND_JERK_WIDTH)
curvy_unwind_speed_weight = _ioniq_6_curvy_speed_weight(v_ego)
curvy_unwind_lat_onset = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_CURVY_UNWIND_LAT_START) /
IONIQ_6_CURVY_UNWIND_LAT_ONSET_WIDTH)
curvy_unwind_lat_cutoff = _ioniq_6_sigmoid((IONIQ_6_CURVY_UNWIND_LAT_END - abs_lateral_accel) /
IONIQ_6_CURVY_UNWIND_LAT_CUTOFF_WIDTH)
curvy_unwind_weight = curvy_unwind_speed_weight * curvy_unwind_lat_onset * curvy_unwind_lat_cutoff * curvy_unwind_phase_weight
curvy_unwind_floor_relief = (_ioniq_6_side_value(desired_lateral_accel,
_flm_vehicle_knob("hyundai_ioniq_6.curvy_unwind_floor_relief_left", IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_LEFT),
_flm_vehicle_knob("hyundai_ioniq_6.curvy_unwind_floor_relief_right", IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_RIGHT)) *
curvy_unwind_weight)
reduction += (_ioniq_6_side_value(desired_lateral_accel,
_flm_vehicle_knob("hyundai_ioniq_6.curvy_unwind_extra_reduction_left", IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_LEFT),
_flm_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
floor -= curvy_unwind_floor_relief
return max(1.0 - reduction, floor)
def get_ioniq_6_output_taper_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_OUTPUT_TAPER_SPEED) / IONIQ_6_OUTPUT_TAPER_SPEED_WIDTH)
center_taper = get_ioniq_6_center_taper_scale(desired_lateral_accel, v_ego)
directional_taper = get_ioniq_6_directional_taper_scale(desired_lateral_accel, desired_lateral_jerk, v_ego)
center_scale = 1.0 - ((1.0 - center_taper) * IONIQ_6_OUTPUT_CENTER_TAPER_BLEND * speed_weight)
directional_scale = 1.0 - ((1.0 - directional_taper) * IONIQ_6_OUTPUT_DIRECTIONAL_TAPER_BLEND * speed_weight)
return center_scale * directional_scale
def get_ioniq_6_highway_output_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED) / IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED_WIDTH)
center_weight = _ioniq_6_sigmoid((IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) /
IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT_WIDTH)
reduction = IONIQ_6_HIGHWAY_OUTPUT_TAPER_MAX * speed_weight * center_weight
return 1.0 - reduction
def get_ioniq_6_highway_transition_output_taper_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED) / IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED_WIDTH)
center_weight = _ioniq_6_sigmoid((IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) /
IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT_WIDTH)
jerk_weight = _ioniq_6_sigmoid((abs(desired_lateral_jerk) - IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK) /
IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK_WIDTH)
reduction = IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_MAX * speed_weight * center_weight * jerk_weight
return 1.0 - reduction
def get_ioniq_6_low_speed_angle_assist_torque(desired_angle_deg: float, actual_angle_deg: float,
current_output_torque: float, v_ego: float) -> float:
angle_error = desired_angle_deg - actual_angle_deg
if desired_angle_deg * angle_error > 0.0:
speed_weight = _ioniq_6_sigmoid((IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED - max(v_ego, 0.0)) /
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED_WIDTH)
error_weight = _ioniq_6_sigmoid((abs(angle_error) - IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR) /
IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR_WIDTH)
desired_angle_weight = _ioniq_6_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 = _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(
_flm_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
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 = _ioniq_6_sigmoid((IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED - max(v_ego, 0.0)) /
IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED_WIDTH)
error_weight = _ioniq_6_sigmoid((abs(angle_error) - IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR) /
IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR_WIDTH)
actual_angle_weight = _ioniq_6_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))
def kia_ev6_lateral_testing_ground_active() -> bool:
return testing_ground.use(KIA_EV6_LATERAL_TESTING_GROUND_ID, KIA_EV6_LATERAL_TESTING_GROUND_VARIANT)
def _kia_ev6_sigmoid(x: float) -> float:
return _sigmoid(x)
def _kia_ev6_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / KIA_EV6_TRANSITION_SPEED) ** 2)
def _kia_ev6_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
return math.tanh((desired_lateral_accel * desired_lateral_jerk) / KIA_EV6_PHASE_SCALE)
def _kia_ev6_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def _kia_ev6_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / KIA_EV6_FRICTION_LAT_RISE)
jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / KIA_EV6_FRICTION_JERK_RISE)
return _kia_ev6_low_speed_factor(v_ego) * lat_factor * jerk_factor
def get_kia_ev6_jwarm_phase_confidence(v_ego: float, desired_lateral_jerk: float) -> float:
low_speed_weight = _kia_ev6_sigmoid(
(KIA_EV6_JWARM_PHASE_STABILITY_SPEED - v_ego) / KIA_EV6_JWARM_PHASE_STABILITY_SPEED_WIDTH
)
abrupt_transition_weight = _kia_ev6_sigmoid(
(abs(desired_lateral_jerk) - KIA_EV6_JWARM_PHASE_STABILITY_JERK) / KIA_EV6_JWARM_PHASE_STABILITY_JERK_WIDTH
)
return 1.0 - (KIA_EV6_JWARM_PHASE_STABILITY_MAX_REDUCTION * low_speed_weight * abrupt_transition_weight)
def get_kia_ev6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
gain = _kia_ev6_side_value(
desired_lateral_accel,
_flm_vehicle_knob("hyundai_kia_ev6.ff_gain_left", KIA_EV6_FF_GAIN_LEFT),
_flm_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)
extra_scale = gain * onset * cutoff
phase = _kia_ev6_transition_phase(desired_lateral_accel, desired_lateral_jerk)
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,
_flm_vehicle_knob("hyundai_kia_ev6.turn_in_boost_left", KIA_EV6_TURN_IN_BOOST_LEFT),
_flm_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,
_flm_vehicle_knob("hyundai_kia_ev6.unwind_taper_left", KIA_EV6_UNWIND_TAPER_LEFT),
_flm_vehicle_knob("hyundai_kia_ev6.unwind_taper_right", KIA_EV6_UNWIND_TAPER_RIGHT),
) *
unwind_weight * (0.35 + 0.65 * low_speed_factor))
jwarm_tune = kia_ev6_lateral_testing_ground_active()
jwarm_phase_confidence = get_kia_ev6_jwarm_phase_confidence(v_ego, desired_lateral_jerk) if jwarm_tune else 0.0
base_turn_in_boost = 1.0 + ((_kia_ev6_side_value(
desired_lateral_accel,
KIA_EV6_JWARM_BASE_TURN_IN_BOOST_LEFT,
KIA_EV6_JWARM_BASE_TURN_IN_BOOST_RIGHT,
) if jwarm_tune else 0.0) *
jwarm_phase_confidence * turn_in_weight * onset * cutoff)
base_unwind_taper_left = _flm_vehicle_knob("hyundai_kia_ev6.base_unwind_taper_left", KIA_EV6_BASE_UNWIND_TAPER_LEFT)
base_unwind_taper_right = _flm_vehicle_knob("hyundai_kia_ev6.base_unwind_taper_right", KIA_EV6_BASE_UNWIND_TAPER_RIGHT)
if jwarm_tune:
base_unwind_taper_left += (KIA_EV6_JWARM_BASE_UNWIND_TAPER_LEFT - base_unwind_taper_left) * jwarm_phase_confidence
base_unwind_taper_right += (KIA_EV6_JWARM_BASE_UNWIND_TAPER_RIGHT - base_unwind_taper_right) * jwarm_phase_confidence
base_unwind_taper = 1.0 - (_kia_ev6_side_value(
desired_lateral_accel,
base_unwind_taper_left,
base_unwind_taper_right,
) * unwind_weight * onset * cutoff)
return (base_unwind_taper * base_turn_in_boost) + (extra_scale * turn_in_boost * max(unwind_taper, 0.0))
def get_kia_ev6_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_hkg_canfd_base_friction_threshold(v_ego)
transition_envelope = _kia_ev6_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
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,
_flm_vehicle_knob("hyundai_kia_ev6.turn_in_threshold_reduction_left", KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_LEFT),
_flm_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,
_flm_vehicle_knob("hyundai_kia_ev6.unwind_threshold_increase_left", KIA_EV6_UNWIND_THRESHOLD_INCREASE_LEFT),
_flm_vehicle_knob("hyundai_kia_ev6.unwind_threshold_increase_right", KIA_EV6_UNWIND_THRESHOLD_INCREASE_RIGHT),
) *
transition_envelope * unwind_weight)
center_speed_weight = _kia_ev6_sigmoid((v_ego - KIA_EV6_CENTER_FRICTION_THRESHOLD_SPEED) /
KIA_EV6_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH)
center_lat_weight = _kia_ev6_sigmoid((KIA_EV6_CENTER_FRICTION_THRESHOLD_LAT - abs(desired_lateral_accel)) /
KIA_EV6_CENTER_FRICTION_THRESHOLD_LAT_WIDTH)
threshold_scale += (_flm_vehicle_knob("hyundai_kia_ev6.center_friction_threshold_gain",
KIA_EV6_CENTER_FRICTION_THRESHOLD_GAIN) *
center_speed_weight * center_lat_weight)
return base_threshold * min(max(threshold_scale, 0.82), 1.16)
def get_kia_ev6_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
transition_envelope = _kia_ev6_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _kia_ev6_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
friction_scale = KIA_EV6_FRICTION_MULT
friction_scale += (_kia_ev6_side_value(desired_lateral_accel, KIA_EV6_TURN_IN_FRICTION_BOOST_LEFT, KIA_EV6_TURN_IN_FRICTION_BOOST_RIGHT) *
transition_envelope * turn_in_weight)
friction_scale -= (_kia_ev6_side_value(desired_lateral_accel, KIA_EV6_UNWIND_FRICTION_REDUCTION_LEFT, KIA_EV6_UNWIND_FRICTION_REDUCTION_RIGHT) *
transition_envelope * unwind_weight)
return min(max(friction_scale, 0.90), 1.10)
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 = _flm_vehicle_knob("hyundai_kia_ev6.center_taper_max", KIA_EV6_CENTER_TAPER_MAX) * speed_weight * center_weight
return 1.0 - reduction
def get_kia_ev6_low_speed_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _kia_ev6_sigmoid((KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_MAX - v_ego) / KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH)
center_weight = _kia_ev6_sigmoid((KIA_EV6_LOW_SPEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / KIA_EV6_LOW_SPEED_CENTER_TAPER_LAT_WIDTH)
reduction = KIA_EV6_LOW_SPEED_CENTER_TAPER_MAX * speed_weight * center_weight
return 1.0 - reduction
def get_kia_ev6_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _kia_ev6_sigmoid((v_ego - KIA_EV6_CENTER_OUTPUT_TAPER_SPEED) /
KIA_EV6_CENTER_OUTPUT_TAPER_SPEED_WIDTH)
center_weight = _kia_ev6_sigmoid((KIA_EV6_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) /
KIA_EV6_CENTER_OUTPUT_TAPER_LAT_WIDTH)
reduction = _flm_vehicle_knob("hyundai_kia_ev6.center_output_taper_max", KIA_EV6_CENTER_OUTPUT_TAPER_MAX) * speed_weight * center_weight
return 1.0 - reduction
def volt_plexy_lateral_testing_ground_active() -> bool:
return testing_ground.use(VOLT_PLEXY_LATERAL_TESTING_GROUND_ID)
def _volt_plexy_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def get_volt_plexy_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
standard_scale = get_volt_standard_ff_scale(desired_lateral_accel, desired_lateral_jerk, v_ego)
extra_scale = standard_scale - 1.0
extra_mult = _volt_plexy_side_value(desired_lateral_accel, VOLT_PLEXY_FF_EXTRA_MULT_LEFT, VOLT_PLEXY_FF_EXTRA_MULT_RIGHT)
return 1.0 + (extra_scale * extra_mult)
def get_volt_plexy_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
standard_threshold = get_volt_standard_friction_threshold(v_ego, desired_lateral_accel, desired_lateral_jerk)
threshold_mult = _volt_plexy_side_value(desired_lateral_accel, VOLT_PLEXY_FRICTION_THRESHOLD_MULT_LEFT, VOLT_PLEXY_FRICTION_THRESHOLD_MULT_RIGHT)
return standard_threshold * threshold_mult
def get_volt_plexy_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
standard_scale = get_volt_standard_friction_scale(v_ego, desired_lateral_accel, desired_lateral_jerk)
friction_extra = standard_scale - 1.0
friction_mult = _volt_plexy_side_value(desired_lateral_accel, VOLT_PLEXY_FRICTION_SCALE_MULT_LEFT, VOLT_PLEXY_FRICTION_SCALE_MULT_RIGHT)
return 1.0 + (friction_extra * friction_mult)
def get_volt_plexy_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
standard_scale = get_volt_standard_center_taper_scale(desired_lateral_accel, v_ego)
standard_reduction = 1.0 - standard_scale
return 1.0 - (standard_reduction * VOLT_PLEXY_CENTER_TAPER_REDUCTION_MULT)
FLM_UNIVERSAL_PROFILE_KEY = "torque_universal"
FLM_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},
"center_deadband_crawl_deg": {"min": 0.0, "max": 0.30, "precision": 0.005, "deltaType": "absolute", "safeLiveTrial": True},
"center_deadband_low_deg": {"min": 0.0, "max": 0.30, "precision": 0.005, "deltaType": "absolute", "safeLiveTrial": True},
"center_deadband_mid_deg": {"min": 0.0, "max": 0.20, "precision": 0.005, "deltaType": "absolute", "safeLiveTrial": True},
"center_deadband_fast_deg": {"min": 0.0, "max": 0.12, "precision": 0.005, "deltaType": "absolute", "safeLiveTrial": True},
"center_deadband_highway_deg": {"min": 0.0, "max": 0.08, "precision": 0.005, "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},
}
FLM_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,
"center_deadband_crawl_deg": 0.0,
"center_deadband_low_deg": 0.0,
"center_deadband_mid_deg": 0.0,
"center_deadband_fast_deg": 0.0,
"center_deadband_highway_deg": 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 _flm_profile_symbol(profile_key: str, suffix: str) -> str:
return f"{profile_key}.{suffix}"
def flm_profile_supports_knob(profile_key: str | None, suffix: str) -> bool:
if not profile_key:
return False
return _flm_profile_symbol(profile_key, suffix) in FLM_SUPPORTED_VEHICLE_KNOBS
def _flm_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 _flm_vehicle_knob(_flm_profile_symbol(profile_key, f"{suffix}_left"), left_default)
return _flm_vehicle_knob(_flm_profile_symbol(profile_key, f"{suffix}_right"), right_default)
def _flm_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 _flm_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 _flm_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 _flm_full_surface_low_speed_factor(v_ego) * lat_factor * jerk_factor
def _flm_full_surface_curvy_speed_weight(profile_key: str, v_ego: float) -> float:
curvy_speed_min = _flm_vehicle_knob(_flm_profile_symbol(profile_key, "curvy_speed_min"), IONIQ_6_CURVY_SPEED_MIN)
curvy_speed_max = _flm_vehicle_knob(_flm_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 _flm_full_surface_curvy_turn_in_trim_speed_weight(profile_key: str, v_ego: float) -> float:
curvy_turn_in_speed_min = _flm_vehicle_knob(_flm_profile_symbol(profile_key, "curvy_turn_in_trim_speed_min"), IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_MIN)
curvy_turn_in_speed_max = _flm_vehicle_knob(_flm_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_flm_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 flm_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 += _flm_vehicle_knob(_flm_profile_symbol(profile_key, "center_taper_max"), 0.0) * speed_weight * center_weight
if flm_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 += _flm_vehicle_knob(_flm_profile_symbol(profile_key, "highway_center_taper_max"), 0.0) * speed_weight * center_weight
return 1.0 - min(reduction, 0.20)
def get_flm_full_surface_center_deadband_deg(profile_key: str | None, v_ego: float) -> float:
if not profile_key:
return 0.0
suffixes = (
"center_deadband_crawl_deg",
"center_deadband_low_deg",
"center_deadband_mid_deg",
"center_deadband_fast_deg",
"center_deadband_highway_deg",
)
values = [
_flm_vehicle_knob(_flm_profile_symbol(profile_key, suffix), 0.0)
for suffix in suffixes
]
return float(np.interp(max(v_ego, 0.0), FLM_FRICTION_SPEED_KNOTS, values))
def get_flm_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 = _flm_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 = _flm_full_surface_low_speed_factor(v_ego)
curvy_turn_in_speed_weight = _flm_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 = _flm_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 flm_profile_supports_knob(profile_key, "ff_gain_left"):
gain = _flm_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 + (_flm_full_surface_side_value(profile_key, desired_lateral_accel, "turn_in_boost") * turn_in_weight * low_speed_factor)
unwind_reduction = _flm_full_surface_side_value(profile_key, desired_lateral_accel, "unwind_taper") * unwind_weight * (0.30 + 0.70 * low_speed_factor)
curvy_unwind_extra = _flm_full_surface_side_value(profile_key, desired_lateral_accel, "curvy_unwind_extra_reduction") * curvy_unwind_weight
curvy_unwind_floor_relief = _flm_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 = _flm_full_surface_side_value(profile_key, desired_lateral_accel, "curvy_unwind_extra_reduction") * curvy_unwind_weight
curvy_unwind_floor_relief = _flm_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 flm_profile_supports_knob(profile_key, "curvy_turn_in_trim_left"):
curvy_trim = _flm_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 flm_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 += _flm_full_surface_side_value(profile_key, desired_lateral_accel, "crawl_turn_in_ff_boost") * crawl_speed_weight * crawl_lat_weight
return scale
def get_flm_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 = _flm_full_surface_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _flm_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 -= (_flm_full_surface_side_value(profile_key, desired_lateral_accel, "turn_in_threshold_reduction") *
transition_envelope * turn_in_weight)
threshold_scale += (_flm_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_flm_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 flm_profile_supports_knob(profile_key, "low_speed_angle_assist_max_torque"):
return current_output_torque
max_torque = _flm_vehicle_knob(_flm_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))
FLM_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),
"toyota_camry": set(CAMRY_CARS),
}
FLM_RICH_PROFILE_LABELS = {
"gm_bolt_2022_2023": "Bolt 2022-2023",
"hyundai_ioniq_6": "Ioniq 6",
"hyundai_kia_ev6": "EV6",
"toyota_prius": "Prius",
"toyota_camry": "Camry",
FLM_UNIVERSAL_PROFILE_KEY: "Torque Controller",
}
FLM_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.base_unwind_taper_left": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 0.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_BASE_UNWIND_TAPER_LEFT},
"hyundai_kia_ev6.base_unwind_taper_right": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 0.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_BASE_UNWIND_TAPER_RIGHT},
"hyundai_kia_ev6.center_taper_max": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 0.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_CENTER_TAPER_MAX},
"hyundai_kia_ev6.turn_in_threshold_reduction_left": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 0.40, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_LEFT},
"hyundai_kia_ev6.turn_in_threshold_reduction_right": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 0.40, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_RIGHT},
"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},
"hyundai_kia_ev6.center_friction_threshold_gain": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 0.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_CENTER_FRICTION_THRESHOLD_GAIN},
"hyundai_kia_ev6.center_output_taper_max": {"profile": "hyundai_kia_ev6", "min": 0.0, "max": 0.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": KIA_EV6_CENTER_OUTPUT_TAPER_MAX},
"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},
"toyota_prius.center_friction_threshold_gain": {"profile": "toyota_prius", "min": 0.0, "max": 0.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": PRIUS_CENTER_FRICTION_THRESHOLD_GAIN},
"toyota_camry.center_friction_threshold_gain": {"profile": "toyota_camry", "min": 0.0, "max": 0.15, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": CAMRY_CENTER_FRICTION_THRESHOLD_GAIN},
"toyota_camry.unwind_ff_reduction": {"profile": "toyota_camry", "min": 0.0, "max": 0.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": CAMRY_UNWIND_FF_REDUCTION},
}
def _add_flm_full_surface_profile_knobs(profile_key: str, defaults: dict[str, float] | None = None) -> None:
knob_defaults = dict(FLM_FULL_SURFACE_NEUTRAL_DEFAULTS)
if defaults:
knob_defaults.update(defaults)
for suffix, meta in FLM_FULL_SURFACE_SUFFIX_METADATA.items():
symbol = _flm_profile_symbol(profile_key, suffix)
if symbol in FLM_SUPPORTED_VEHICLE_KNOBS:
continue
minimum = -0.40 if profile_key == FLM_UNIVERSAL_PROFILE_KEY and suffix in ("ff_gain_left", "ff_gain_right") else meta["min"]
FLM_SUPPORTED_VEHICLE_KNOBS[symbol] = {
"profile": profile_key,
"min": minimum,
"max": meta["max"],
"precision": meta["precision"],
"deltaType": meta["deltaType"],
"safeLiveTrial": meta["safeLiveTrial"],
"defaultValue": knob_defaults[suffix],
}
for _flm_profile_key in ("gm_bolt_2022_2023", "hyundai_ioniq_6", "hyundai_kia_ev6", "toyota_prius", "toyota_camry", FLM_UNIVERSAL_PROFILE_KEY):
_add_flm_full_surface_profile_knobs(_flm_profile_key)
def get_flm_supported_vehicle_knobs() -> dict:
return _flm_copy_json(FLM_SUPPORTED_VEHICLE_KNOBS)
def get_flm_rich_profile_key(car_fingerprint) -> str | None:
for profile_key, cars in FLM_RICH_PROFILE_CARS.items():
if car_fingerprint in cars:
return profile_key
return None
def get_flm_surface_profile_key(car_fingerprint, torque_control: bool = True) -> str | None:
profile_key = get_flm_rich_profile_key(car_fingerprint)
if profile_key is not None or not torque_control:
return profile_key
return FLM_UNIVERSAL_PROFILE_KEY
def get_flm_capabilities(car_fingerprint, brand: str = "", hyundai_canfd: bool = False, torque_control: bool = True) -> dict:
profile_key = get_flm_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(RAV4_PRIME_CARS) | set(SIENNA_4TH_GEN_CARS) | set(IONIQ_5_CARS) | set(IONIQ_6_CARS) | set(KIA_EV6_CARS) | set(KIA_FORTE_CARS) |
set(KIA_NIRO_PHEV_2022_CARS) | set(KIA_CARNIVAL_CARS) | set(GENESIS_G90_CARS) | set(GENESIS_G70_CARS) | set(CAMRY_CARS)
)
dedicated_center_taper = car_fingerprint in (
set(PRIUS_CARS) | set(SIENNA_4TH_GEN_CARS) | set(BOLT_CARS) | set(VOLT_STANDARD_CARS) | set(IONIQ_5_CARS) |
set(IONIQ_EV_OLD_CARS) | set(IONIQ_6_CARS) | set(SONATA_CARS) | set(SONATA_HYBRID_CARS) |
set(KIA_XCEED_CARS) | set(KIA_NIRO_PHEV_2022_CARS) | set(KIA_FORTE_CARS) | set(KIA_EV6_CARS) |
set(KIA_CARNIVAL_CARS) | set(TUCSON_4TH_GEN_CARS) | set(GENESIS_G70_CARS) | set(SILVERADO_CARS)
)
rich_knobs = [name for name, meta in FLM_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": FLM_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",
}]
Reference in New Issue View Git Blame Copy Permalink