import math import numpy as np from collections import deque from cereal import log from opendbc.car.gm.values import CAR as GM_CAR from opendbc.car.honda.values import CAR as HONDA_CAR, HondaFlags from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR from opendbc.car.toyota.values import CAR as TOYOTA_CAR from opendbc.car.lateral import get_friction from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY, CV from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.pid import PIDController from openpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.starpilot.common.testing_grounds import testing_ground # At higher speeds (25+mph) we can assume: # Lateral acceleration achieved by a specific car correlates to # torque applied to the steering rack. It does not correlate to # wheel slip, or to speed. # This controller applies torque to achieve desired lateral # accelerations. To compensate for the low speed effects the # proportional gain is increased at low speeds by the PID controller. # Additionally, there is friction in the steering wheel that needs # to be overcome to move it at all, this is compensated for too. KP = 0.6 KI = 0.35 INTERP_SPEEDS = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30] KP_INTERP = [250, 120, 65, 30, 11.5, 5.5, 3.5, 2.0, KP] LOW_SPEED_X = [0, 10, 20, 30] LOW_SPEED_Y = [12, 10.5, 8, 5] MAX_LAT_JERK_UP = 2.5 # m/s^3 LP_FILTER_CUTOFF_HZ = 1.2 JERK_LOOKAHEAD_SECONDS = 0.19 JERK_GAIN = 0.22 LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0 VERSION = 2 DEBUG_TORQUE_TUNE = False FF_SCALE_BLEND_LAT_ACCEL = 0.05 DEADZONE_BOOST_LAT_ACCEL = 0.15 UNWIND_D_DES_THRESHOLD = -1.0 UNWIND_LAT_ACCEL_NEAR_ZERO = 0.3 MIN_LATERAL_CONTROL_SPEED = 0.3 CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD = 0.30 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 VOLT_STANDARD_CARS = ( GM_CAR.CHEVROLET_VOLT, 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, ) GENESIS_G90_CARS = ( HYUNDAI_CAR.GENESIS_G90, ) 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, ) 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_XCEED_CARS = ( HYUNDAI_CAR.KIA_XCEED_PHEV, ) KIA_NIRO_PHEV_2022_CARS = ( HYUNDAI_CAR.KIA_NIRO_PHEV_2022, ) KIA_FORTE_CARS = ( HYUNDAI_CAR.KIA_FORTE, HYUNDAI_CAR.KIA_FORTE_2019_NON_SCC, HYUNDAI_CAR.KIA_FORTE_2021_NON_SCC, ) PRIUS_CARS = ( TOYOTA_CAR.TOYOTA_PRIUS, ) 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_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 SONATA_HYBRID_BASE_LAT_ACCEL_FACTOR_MULT = 1.05 SONATA_HYBRID_FF_REDUCTION_LEFT = 0.09 SONATA_HYBRID_FF_REDUCTION_RIGHT = 0.22 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.07 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_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_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 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.12 IONIQ_5_FF_REDUCTION_RIGHT = 0.22 IONIQ_5_TURN_IN_BOOST_LEFT = 0.14 IONIQ_5_TURN_IN_BOOST_RIGHT = 0.06 IONIQ_5_UNWIND_TAPER_LEFT = 0.76 IONIQ_5_UNWIND_TAPER_RIGHT = 0.86 IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.08 IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.05 IONIQ_5_UNWIND_THRESHOLD_INCREASE_LEFT = 0.36 IONIQ_5_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.38 IONIQ_5_TURN_IN_FRICTION_BOOST_LEFT = 0.04 IONIQ_5_TURN_IN_FRICTION_BOOST_RIGHT = 0.03 IONIQ_5_UNWIND_FRICTION_REDUCTION_LEFT = 0.34 IONIQ_5_UNWIND_FRICTION_REDUCTION_RIGHT = 0.34 IONIQ_5_CENTER_TAPER_MAX = 0.14 IONIQ_5_CENTER_TAPER_LAT = 0.12 IONIQ_5_CENTER_TAPER_LAT_WIDTH = 0.03 IONIQ_5_CENTER_TAPER_SPEED = 16.0 IONIQ_5_CENTER_TAPER_SPEED_WIDTH = 2.5 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 = 0.36 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.038 IONIQ_6_HIGHWAY_CENTER_TAPER_LAT = 0.09 IONIQ_6_HIGHWAY_CENTER_TAPER_LAT_WIDTH = 0.03 IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED = 24.5 IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED_WIDTH = 1.8 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 = 2.15 IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 4.25 IONIQ_6_DIRECTIONAL_TAPER_FLOOR_LEFT = 0.48 IONIQ_6_DIRECTIONAL_TAPER_FLOOR_RIGHT = 0.52 IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT = 0.16 IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT = 0.04 IONIQ_6_DIRECTIONAL_TAPER_JERK_ONSET = 0.60 IONIQ_6_DIRECTIONAL_TAPER_JERK_WIDTH = 0.14 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_CRAWL_TURN_IN_FF_BOOST_LEFT = 0.14 IONIQ_6_CRAWL_TURN_IN_FF_BOOST_RIGHT = 0.19 IONIQ_6_CRAWL_TURN_IN_FF_SPEED = 5.0 IONIQ_6_CRAWL_TURN_IN_FF_SPEED_WIDTH = 0.9 IONIQ_6_CRAWL_TURN_IN_FF_LAT = 0.08 IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH = 0.045 IONIQ_6_LOW_SPEED_ANGLE_ASSIST_MAX_TORQUE = 0.42 IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED = 2.95 IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED_WIDTH = 0.35 IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR = 2.4 IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR_WIDTH = 1.35 IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE = 6.5 IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE_WIDTH = 2.6 IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_START = 0.58 IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_WIDTH = 0.14 IONIQ_6_LOW_SPEED_ANGLE_ASSIST_TRACK_RATIO_FLOOR = 0.18 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.82, 0.0] IONIQ_6_LOW_SPEED_UNWIND_ASSIST_MAX_TORQUE = 0.26 IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED = 3.05 IONIQ_6_LOW_SPEED_UNWIND_ASSIST_SPEED_WIDTH = 0.35 IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR = 1.8 IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR_WIDTH = 1.0 IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ACTUAL_ANGLE = 12.0 IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ACTUAL_ANGLE_WIDTH = 5.0 IONIQ_6_LOW_SPEED_UNWIND_ASSIST_BLEND = 0.45 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_LOW_SPEED_PID_RESET_SPEED = 0.1 * CV.MPH_TO_MS 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.06 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_RIGHT = 0.17 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.78 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT = 1.10 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.08 KIA_EV6_FF_GAIN_RIGHT = 0.12 KIA_EV6_FF_ONSET = 0.08 KIA_EV6_FF_ONSET_WIDTH = 0.04 KIA_EV6_FF_CUTOFF = 0.82 KIA_EV6_FF_CUTOFF_WIDTH = 0.18 KIA_EV6_TRANSITION_SPEED = 11.0 KIA_EV6_PHASE_SCALE = 0.09 KIA_EV6_TURN_IN_BOOST_LEFT = 0.22 KIA_EV6_TURN_IN_BOOST_RIGHT = 0.34 KIA_EV6_UNWIND_TAPER_LEFT = 0.48 KIA_EV6_UNWIND_TAPER_RIGHT = 0.46 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.10 KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.24 KIA_EV6_UNWIND_THRESHOLD_INCREASE_LEFT = 0.28 KIA_EV6_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.24 KIA_EV6_TURN_IN_FRICTION_BOOST_LEFT = 0.04 KIA_EV6_TURN_IN_FRICTION_BOOST_RIGHT = 0.12 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_LOW_SPEED_CENTER_TAPER_MAX = 0.24 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 VOLT_PLEXY_LATERAL_TESTING_GROUND_ID = testing_ground.id_7 VOLT_PLEXY_FF_GAIN_LEFT = 0.12 VOLT_PLEXY_FF_GAIN_RIGHT = 0.07 VOLT_PLEXY_FF_ONSET = 0.10 VOLT_PLEXY_FF_ONSET_WIDTH = 0.06 VOLT_PLEXY_FF_CUTOFF = 1.35 VOLT_PLEXY_FF_CUTOFF_WIDTH = 0.24 VOLT_PLEXY_TRANSITION_SPEED = 10.5 VOLT_PLEXY_PHASE_SCALE = 0.10 VOLT_PLEXY_TURN_IN_BOOST_LEFT = 0.22 VOLT_PLEXY_TURN_IN_BOOST_RIGHT = 0.24 VOLT_PLEXY_UNWIND_TAPER_LEFT = 0.22 VOLT_PLEXY_UNWIND_TAPER_RIGHT = 0.56 VOLT_PLEXY_FRICTION_MULT = 1.04 VOLT_PLEXY_FRICTION_LAT_RISE = 0.22 VOLT_PLEXY_FRICTION_JERK_RISE = 0.24 VOLT_PLEXY_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.16 VOLT_PLEXY_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.12 VOLT_PLEXY_UNWIND_THRESHOLD_INCREASE_LEFT = 0.15 VOLT_PLEXY_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.34 VOLT_PLEXY_TURN_IN_FRICTION_BOOST_LEFT = 0.08 VOLT_PLEXY_TURN_IN_FRICTION_BOOST_RIGHT = 0.06 VOLT_PLEXY_UNWIND_FRICTION_REDUCTION_LEFT = 0.16 VOLT_PLEXY_UNWIND_FRICTION_REDUCTION_RIGHT = 0.40 PRIUS_TRANSITION_SPEED = 10.0 PRIUS_PHASE_SCALE = 0.09 PRIUS_FF_GAIN_LEFT = 0.12 PRIUS_FF_GAIN_RIGHT = 0.13 PRIUS_FF_ONSET = 0.12 PRIUS_FF_ONSET_WIDTH = 0.07 PRIUS_FF_CUTOFF = 1.45 PRIUS_FF_CUTOFF_WIDTH = 0.36 PRIUS_FRICTION_LAT_RISE = 0.18 PRIUS_FRICTION_JERK_RISE = 0.22 PRIUS_TURN_IN_BOOST_LEFT = 0.60 PRIUS_TURN_IN_BOOST_RIGHT = 0.56 PRIUS_UNWIND_TAPER_LEFT = 0.58 PRIUS_UNWIND_TAPER_RIGHT = 0.84 PRIUS_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.22 PRIUS_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.22 PRIUS_UNWIND_THRESHOLD_INCREASE_LEFT = 0.36 PRIUS_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.52 PRIUS_TURN_IN_FRICTION_BOOST_LEFT = 0.10 PRIUS_TURN_IN_FRICTION_BOOST_RIGHT = 0.10 PRIUS_UNWIND_FRICTION_REDUCTION_LEFT = 0.22 PRIUS_UNWIND_FRICTION_REDUCTION_RIGHT = 0.32 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 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 get_friction_threshold(v_ego: float) -> float: # Keep the speed-scaled friction threshold behavior. 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 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, PRIUS_FF_GAIN_LEFT, 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, PRIUS_TURN_IN_BOOST_LEFT, PRIUS_TURN_IN_BOOST_RIGHT) * turn_in_weight * (0.35 + 0.65 * low_speed_factor)) unwind_taper = 1.0 - (_prius_side_value(desired_lateral_accel, PRIUS_UNWIND_TAPER_LEFT, PRIUS_UNWIND_TAPER_RIGHT) * unwind_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_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, PRIUS_TURN_IN_THRESHOLD_REDUCTION_LEFT, PRIUS_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * transition_envelope * turn_in_weight) threshold_scale += (_prius_side_value(desired_lateral_accel, PRIUS_UNWIND_THRESHOLD_INCREASE_LEFT, PRIUS_UNWIND_THRESHOLD_INCREASE_RIGHT) * transition_envelope * unwind_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 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_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_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, BOLT_2022_2023_FF_GAIN_LEFT, BOLT_2022_2023_FF_GAIN_RIGHT) abs_lateral_accel = abs(desired_lateral_accel) onset = _bolt_2022_2023_sigmoid((abs_lateral_accel - BOLT_2022_2023_FF_ONSET) / BOLT_2022_2023_FF_ONSET_WIDTH) cutoff = _bolt_2022_2023_sigmoid((BOLT_2022_2023_FF_CUTOFF - abs_lateral_accel) / BOLT_2022_2023_FF_CUTOFF_WIDTH) extra_scale = gain * onset * cutoff speed_weight = _bolt_2022_2023_sigmoid((v_ego - BOLT_2022_2023_CENTER_TAPER_SPEED) / BOLT_2022_2023_CENTER_TAPER_SPEED_WIDTH) center_weight = _bolt_2022_2023_sigmoid((BOLT_2022_2023_CENTER_TAPER_LAT - abs_lateral_accel) / BOLT_2022_2023_CENTER_TAPER_LAT_WIDTH) center_taper = 1.0 - (BOLT_2022_2023_CENTER_TAPER_MAX * speed_weight * center_weight) low_speed_factor = _bolt_2022_2023_low_speed_factor(v_ego) transition_envelope = _bolt_2022_2023_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) phase = _bolt_2022_2023_transition_phase(desired_lateral_accel, desired_lateral_jerk) turn_in_weight = max(phase, 0.0) unwind_weight = max(-phase, 0.0) turn_in_boost = 1.0 + (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_TURN_IN_BOOST_LEFT, BOLT_2022_2023_TURN_IN_BOOST_RIGHT) * turn_in_weight * low_speed_factor) unwind_envelope = (0.25 + 0.75 * low_speed_factor) * (1.0 + 0.45 * transition_envelope) unwind_taper = 1.0 - (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_UNWIND_TAPER_LEFT, BOLT_2022_2023_UNWIND_TAPER_RIGHT) * unwind_weight * unwind_envelope) return 1.0 + (extra_scale * center_taper * turn_in_boost * max(unwind_taper, 0.0)) 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_friction_threshold(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) threshold_scale = 1.0 - (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_LEFT, BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * transition_envelope * turn_in_weight) threshold_scale += (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_LEFT, BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_RIGHT) * 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_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 _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_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_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_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_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 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_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 _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) 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)) return (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_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 _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 get_ioniq_6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: if desired_lateral_accel == 0.0: return 1.0 gain = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_FF_GAIN_LEFT, IONIQ_6_FF_GAIN_RIGHT) 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, IONIQ_6_TURN_IN_BOOST_LEFT, IONIQ_6_TURN_IN_BOOST_RIGHT) * turn_in_weight * low_speed_factor) unwind_taper = 1.0 - (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_UNWIND_TAPER_LEFT, IONIQ_6_UNWIND_TAPER_RIGHT) * unwind_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, IONIQ_6_CRAWL_TURN_IN_FF_BOOST_LEFT, 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 return (1.0 + crawl_turn_in_scale + high_speed_right_turn_in_scale + (extra_scale * turn_in_boost * max(unwind_taper, 0.0))) * get_ioniq_6_directional_taper_scale(desired_lateral_accel, desired_lateral_jerk, v_ego) 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_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) threshold_scale = 1.0 - (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT, IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * transition_envelope * turn_in_weight) threshold_scale += (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT, IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT) * transition_envelope * unwind_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) 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) return min(max(friction_scale, 0.82), 1.08) def get_ioniq_6_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_CENTER_TAPER_SPEED) / IONIQ_6_CENTER_TAPER_SPEED_WIDTH) center_weight = _ioniq_6_sigmoid((IONIQ_6_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / IONIQ_6_CENTER_TAPER_LAT_WIDTH) high_speed_reduction = IONIQ_6_CENTER_TAPER_MAX * speed_weight * center_weight highway_speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED) / IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED_WIDTH) highway_center_weight = _ioniq_6_sigmoid((IONIQ_6_HIGHWAY_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / IONIQ_6_HIGHWAY_CENTER_TAPER_LAT_WIDTH) highway_center_reduction = IONIQ_6_HIGHWAY_CENTER_TAPER_MAX * highway_speed_weight * highway_center_weight 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 = _ioniq_6_transition_phase(desired_lateral_accel, desired_lateral_jerk) 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 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) 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) 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 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_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(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_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, KIA_EV6_FF_GAIN_LEFT, 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, KIA_EV6_TURN_IN_BOOST_LEFT, KIA_EV6_TURN_IN_BOOST_RIGHT) * turn_in_weight * (0.35 + 0.65 * low_speed_factor)) unwind_taper = 1.0 - (_kia_ev6_side_value(desired_lateral_accel, KIA_EV6_UNWIND_TAPER_LEFT, KIA_EV6_UNWIND_TAPER_RIGHT) * unwind_weight * (0.35 + 0.65 * low_speed_factor)) return 1.0 + (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_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, KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_LEFT, KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * transition_envelope * turn_in_weight) threshold_scale += (_kia_ev6_side_value(desired_lateral_accel, KIA_EV6_UNWIND_THRESHOLD_INCREASE_LEFT, KIA_EV6_UNWIND_THRESHOLD_INCREASE_RIGHT) * transition_envelope * unwind_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 = 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 volt_plexy_lateral_testing_ground_active() -> bool: return testing_ground.use(VOLT_PLEXY_LATERAL_TESTING_GROUND_ID) def _volt_plexy_sigmoid(x: float) -> float: return _sigmoid(x) def _volt_plexy_low_speed_factor(v_ego: float) -> float: return 1.0 / (1.0 + (max(v_ego, 0.0) / VOLT_PLEXY_TRANSITION_SPEED) ** 2) def _volt_plexy_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: return math.tanh((desired_lateral_accel * desired_lateral_jerk) / VOLT_PLEXY_PHASE_SCALE) 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 _volt_plexy_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_PLEXY_FRICTION_LAT_RISE) jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / VOLT_PLEXY_FRICTION_JERK_RISE) return _volt_plexy_low_speed_factor(v_ego) * lat_factor * jerk_factor def get_volt_plexy_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_plexy_side_value(desired_lateral_accel, VOLT_PLEXY_FF_GAIN_LEFT, VOLT_PLEXY_FF_GAIN_RIGHT) abs_lateral_accel = abs(desired_lateral_accel) onset = _volt_plexy_sigmoid((abs_lateral_accel - VOLT_PLEXY_FF_ONSET) / VOLT_PLEXY_FF_ONSET_WIDTH) cutoff = _volt_plexy_sigmoid((VOLT_PLEXY_FF_CUTOFF - abs_lateral_accel) / VOLT_PLEXY_FF_CUTOFF_WIDTH) extra_scale = gain * onset * cutoff phase = _volt_plexy_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_plexy_low_speed_factor(v_ego) turn_in_boost = 1.0 + (_volt_plexy_side_value(desired_lateral_accel, VOLT_PLEXY_TURN_IN_BOOST_LEFT, VOLT_PLEXY_TURN_IN_BOOST_RIGHT) * turn_in_weight * low_speed_factor) unwind_taper = 1.0 - (_volt_plexy_side_value(desired_lateral_accel, VOLT_PLEXY_UNWIND_TAPER_LEFT, VOLT_PLEXY_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_volt_plexy_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: base_threshold = get_friction_threshold(v_ego) transition_envelope = _volt_plexy_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) phase = _volt_plexy_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_plexy_side_value(desired_lateral_accel, VOLT_PLEXY_TURN_IN_THRESHOLD_REDUCTION_LEFT, VOLT_PLEXY_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * transition_envelope * turn_in_weight) threshold_scale += (_volt_plexy_side_value(desired_lateral_accel, VOLT_PLEXY_UNWIND_THRESHOLD_INCREASE_LEFT, VOLT_PLEXY_UNWIND_THRESHOLD_INCREASE_RIGHT) * transition_envelope * unwind_weight) return base_threshold * min(max(threshold_scale, 0.84), 1.14) def get_volt_plexy_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: transition_envelope = _volt_plexy_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) phase = _volt_plexy_transition_phase(desired_lateral_accel, desired_lateral_jerk) turn_in_weight = max(phase, 0.0) unwind_weight = max(-phase, 0.0) friction_scale = VOLT_PLEXY_FRICTION_MULT friction_scale += (_volt_plexy_side_value(desired_lateral_accel, VOLT_PLEXY_TURN_IN_FRICTION_BOOST_LEFT, VOLT_PLEXY_TURN_IN_FRICTION_BOOST_RIGHT) * transition_envelope * turn_in_weight) friction_scale -= (_volt_plexy_side_value(desired_lateral_accel, VOLT_PLEXY_UNWIND_FRICTION_REDUCTION_LEFT, VOLT_PLEXY_UNWIND_FRICTION_REDUCTION_RIGHT) * transition_envelope * unwind_weight) return min(max(friction_scale, 0.90), 1.12) class LatControlTorque(LatControl): def __init__(self, CP, CI, dt): super().__init__(CP, CI, dt) self.torque_params = CP.lateralTuning.torque.as_builder() self.torque_from_lateral_accel = CI.torque_from_lateral_accel() self.lateral_accel_from_torque = CI.lateral_accel_from_torque() self.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, rate=1/self.dt) self.update_limits() self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg self.lat_accel_request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt) self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len, maxlen=self.lat_accel_request_buffer_len) self.lookahead_frames = int(JERK_LOOKAHEAD_SECONDS / self.dt) self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt) self.previous_measurement = 0.0 self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt) self.low_speed_reset_threshold = max(CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) self.steer_release_i_decay = 0.8 self.prev_steering_pressed = False self.debug_counter = 0 self.prev_desired_lateral_accel = 0.0 self.is_bolt = CP.carFingerprint in BOLT_CARS self.is_bolt_2022_2023 = CP.carFingerprint in BOLT_2022_2023_CARS self.is_bolt_2018_2021 = CP.carFingerprint in BOLT_2018_2021_CARS self.is_bolt_2017 = CP.carFingerprint in BOLT_2017_CARS self.is_volt_standard = CP.carFingerprint in VOLT_STANDARD_CARS self.is_genesis_g90 = CP.carFingerprint in GENESIS_G90_CARS self.is_palisade = CP.carFingerprint in PALISADE_CARS self.is_prius = CP.carFingerprint in PRIUS_CARS self.is_ioniq_5 = CP.carFingerprint in IONIQ_5_CARS self.is_ioniq_ev_old = CP.carFingerprint in IONIQ_EV_OLD_CARS self.is_ioniq_6 = CP.carFingerprint in IONIQ_6_CARS self.is_sonata = CP.carFingerprint in SONATA_CARS self.is_sonata_hybrid = CP.carFingerprint in SONATA_HYBRID_CARS self.is_elantra_non_scc = CP.carFingerprint in ELANTRA_NON_SCC_CARS self.is_kia_xceed = CP.carFingerprint in KIA_XCEED_CARS self.is_kia_niro_phev_2022 = CP.carFingerprint in KIA_NIRO_PHEV_2022_CARS self.is_kia_forte = CP.carFingerprint in KIA_FORTE_CARS self.is_kia_ev6 = CP.carFingerprint in KIA_EV6_CARS self.is_civic_bosch_modified = CP.carFingerprint == HONDA_CAR.HONDA_CIVIC_BOSCH and bool(CP.flags & HondaFlags.EPS_MODIFIED) self.is_volt_cc = CP.carFingerprint == GM_CAR.CHEVROLET_VOLT_CC self.is_silverado = CP.carFingerprint in SILVERADO_CARS if self.is_ioniq_6: self.low_speed_reset_threshold = min(self.low_speed_reset_threshold, IONIQ_6_LOW_SPEED_PID_RESET_SPEED) self.use_bolt_ff_scaling = self.is_bolt_2022_2023 or self.is_bolt_2018_2021 or self.is_bolt_2017 self.use_bolt_ki_multiplier = self.use_bolt_ff_scaling self.torque_ff_scale_pos = 1.0 self.torque_ff_scale_neg = 1.0 self.torque_deadzone_boost = float(getattr(self.torque_params, "kfDEPRECATED", 0.0)) self.torque_ki_mult = 1.0 if self.is_palisade: self.torque_params.latAccelFactor *= PALISADE_BASE_LAT_ACCEL_FACTOR_MULT if self.is_ioniq_5: self.torque_params.latAccelFactor *= IONIQ_5_BASE_LAT_ACCEL_FACTOR_MULT if self.is_ioniq_ev_old: self.torque_params.latAccelFactor *= IONIQ_EV_OLD_BASE_LAT_ACCEL_FACTOR_MULT if self.is_ioniq_6: self.torque_params.latAccelFactor *= IONIQ_6_BASE_LAT_ACCEL_FACTOR_MULT if self.is_sonata_hybrid: self.torque_params.latAccelFactor *= SONATA_HYBRID_BASE_LAT_ACCEL_FACTOR_MULT if self.is_kia_forte: self.torque_params.latAccelFactor *= KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT if self.is_civic_bosch_modified: self.torque_params.latAccelFactor *= CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT if civic_bosch_modified_a_lateral_testing_ground_active(): self.torque_params.latAccelFactor *= CIVIC_BOSCH_MODIFIED_A_VARIANT_LAT_ACCEL_FACTOR_MULT if civic_bosch_modified_lateral_testing_ground_active(): self.torque_params.latAccelFactor *= CIVIC_BOSCH_MODIFIED_B_VARIANT_LAT_ACCEL_FACTOR_MULT if self.is_bolt: kp_scale = getattr(self.torque_params, "kp", getattr(self.torque_params, "kpDEPRECATED", 1.0)) ki_scale = getattr(self.torque_params, "ki", getattr(self.torque_params, "kiDEPRECATED", 1.0)) kd_scale = getattr(self.torque_params, "kd", getattr(self.torque_params, "kdDEPRECATED", 1.0)) self.torque_ff_scale_pos = float(kp_scale) self.torque_ff_scale_neg = float(ki_scale) self.torque_ki_mult = float(kd_scale) if self.use_bolt_ki_multiplier and self.torque_ki_mult > 0.0 and self.torque_ki_mult != 1.0: self.pid._k_i = [self.pid._k_i[0], [k * self.torque_ki_mult for k in self.pid._k_i[1]]] def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): if self.is_palisade: latAccelFactor *= PALISADE_BASE_LAT_ACCEL_FACTOR_MULT if self.is_ioniq_5: latAccelFactor *= IONIQ_5_BASE_LAT_ACCEL_FACTOR_MULT if self.is_ioniq_ev_old: latAccelFactor *= IONIQ_EV_OLD_BASE_LAT_ACCEL_FACTOR_MULT if self.is_ioniq_6: latAccelFactor *= IONIQ_6_BASE_LAT_ACCEL_FACTOR_MULT if self.is_sonata_hybrid: latAccelFactor *= SONATA_HYBRID_BASE_LAT_ACCEL_FACTOR_MULT if self.is_kia_forte: latAccelFactor *= KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT if self.is_civic_bosch_modified: latAccelFactor *= CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT if civic_bosch_modified_a_lateral_testing_ground_active(): latAccelFactor *= CIVIC_BOSCH_MODIFIED_A_VARIANT_LAT_ACCEL_FACTOR_MULT if civic_bosch_modified_lateral_testing_ground_active(): latAccelFactor *= CIVIC_BOSCH_MODIFIED_B_VARIANT_LAT_ACCEL_FACTOR_MULT self.torque_params.latAccelFactor = latAccelFactor self.torque_params.latAccelOffset = latAccelOffset self.torque_params.friction = friction self.update_limits() def update_limits(self): self.pid.set_limits(self.lateral_accel_from_torque(self.steer_max, self.torque_params), self.lateral_accel_from_torque(-self.steer_max, self.torque_params)) def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay, calibrated_pose, model_data, starpilot_toggles): pid_log = log.ControlsState.LateralTorqueState.new_message() pid_log.version = VERSION if not active: output_torque = 0.0 pid_log.active = False self.pid.reset() self.previous_measurement = 0.0 self.measurement_rate_filter.x = 0.0 self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len, maxlen=self.lat_accel_request_buffer_len) self.prev_desired_lateral_accel = 0.0 else: if self.prev_steering_pressed and not CS.steeringPressed: self.pid.i *= self.steer_release_i_decay measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 delay_frames = int(np.clip(lat_delay / self.dt, 1, self.lat_accel_request_buffer_len)) expected_lateral_accel = self.lat_accel_request_buffer[-delay_frames] future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 self.lat_accel_request_buffer.append(future_desired_lateral_accel) raw_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / max(lat_delay, self.dt) raw_lateral_jerk = np.clip(raw_lateral_jerk, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP) desired_lateral_jerk = np.clip(self.jerk_filter.update(raw_lateral_jerk), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP) gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation setpoint = expected_lateral_accel + desired_lateral_jerk * lat_delay desired_lateral_accel_rate = (setpoint - self.prev_desired_lateral_accel) / self.dt unwind_detected = (desired_lateral_accel_rate < UNWIND_D_DES_THRESHOLD and abs(setpoint) < UNWIND_LAT_ACCEL_NEAR_ZERO) self.prev_desired_lateral_accel = setpoint measurement = measured_curvature * CS.vEgo ** 2 measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt) measurement_rate = np.clip(measurement_rate, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP) self.previous_measurement = measurement low_speed_factor = (np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y) / max(CS.vEgo, MIN_SPEED)) ** 2 current_kp = np.interp(CS.vEgo, self.pid._k_p[0], self.pid._k_p[1]) error = setpoint - measurement error_with_lsf = error * (1 + low_speed_factor / max(current_kp, 1e-3)) # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly pid_log.error = float(error_with_lsf) ff = gravity_adjusted_future_lateral_accel # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll ff -= self.torque_params.latAccelOffset ff_scale = 1.0 if self.use_bolt_ff_scaling: ff_scale = np.interp(ff, [-FF_SCALE_BLEND_LAT_ACCEL, 0.0, FF_SCALE_BLEND_LAT_ACCEL], [self.torque_ff_scale_neg, 1.0, self.torque_ff_scale_pos]) ff *= ff_scale trailer_load_kg = float(max(getattr(starpilot_toggles, "trailer_load_kg", 0.0) or 0.0, 0.0)) bolt_2022_2023_tuned_path_active = self.is_bolt_2022_2023 bolt_2018_2021_tuned_path_active = self.is_bolt_2018_2021 volt_standard_test_active = self.is_volt_standard and volt_standard_lateral_testing_ground_active() genesis_g90_test_active = self.is_genesis_g90 and genesis_g90_lateral_testing_ground_active() palisade_active = self.is_palisade prius_active = self.is_prius ioniq_5_active = self.is_ioniq_5 ioniq_ev_old_active = self.is_ioniq_ev_old ioniq_6_active = self.is_ioniq_6 sonata_active = self.is_sonata sonata_hybrid_active = self.is_sonata_hybrid elantra_non_scc_active = self.is_elantra_non_scc kia_xceed_active = self.is_kia_xceed kia_niro_phev_2022_active = self.is_kia_niro_phev_2022 kia_forte_active = self.is_kia_forte kia_ev6_test_active = self.is_kia_ev6 and kia_ev6_lateral_testing_ground_active() volt_plexy_test_active = self.is_volt_cc and volt_plexy_lateral_testing_ground_active() ioniq_5_center_taper = get_ioniq_5_center_taper_scale(setpoint, CS.vEgo) if ioniq_5_active else 1.0 volt_standard_center_taper = get_volt_standard_center_taper_scale(setpoint, CS.vEgo) if volt_standard_test_active else 1.0 ioniq_ev_old_center_taper = get_ioniq_ev_old_center_taper_scale(setpoint, CS.vEgo) if ioniq_ev_old_active else 1.0 ioniq_6_center_taper = get_ioniq_6_center_taper_scale(setpoint, CS.vEgo) if ioniq_6_active else 1.0 sonata_center_taper = get_sonata_center_taper_scale(setpoint, CS.vEgo) if sonata_active else 1.0 sonata_hybrid_center_taper = get_sonata_hybrid_center_taper_scale(setpoint, CS.vEgo) if sonata_hybrid_active else 1.0 kia_xceed_center_taper = get_kia_xceed_center_taper_scale(setpoint, CS.vEgo) if kia_xceed_active else 1.0 kia_niro_phev_2022_center_taper = get_kia_niro_phev_2022_center_taper_scale(setpoint, CS.vEgo) if kia_niro_phev_2022_active else 1.0 kia_forte_center_taper = get_kia_forte_center_taper_scale(setpoint, CS.vEgo) if kia_forte_active else 1.0 kia_ev6_center_taper = get_kia_ev6_center_taper_scale(setpoint, CS.vEgo) if kia_ev6_test_active else 1.0 kia_ev6_low_speed_center_taper = get_kia_ev6_low_speed_center_taper_scale(setpoint, CS.vEgo) if kia_ev6_test_active else 1.0 silverado_center_taper = get_silverado_center_taper_scale(setpoint, CS.vEgo) if self.is_silverado else 1.0 civic_bosch_modified_a_center_taper = get_civic_bosch_modified_a_center_taper_scale(setpoint, CS.vEgo) if ( self.is_civic_bosch_modified and civic_bosch_modified_a_lateral_testing_ground_active() ) else 1.0 friction_threshold = get_friction_threshold(CS.vEgo) friction_scale = 1.0 if bolt_2022_2023_tuned_path_active: ff *= get_bolt_2022_2023_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) friction_threshold = get_bolt_2022_2023_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_bolt_2022_2023_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) elif bolt_2018_2021_tuned_path_active: friction_threshold = get_bolt_2018_2021_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_bolt_2018_2021_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) elif volt_standard_test_active: ff *= get_volt_standard_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * volt_standard_center_taper friction_threshold = get_volt_standard_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_volt_standard_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = 1.0 + ((friction_scale - 1.0) * volt_standard_center_taper) elif genesis_g90_test_active: ff *= get_genesis_g90_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) friction_threshold = get_genesis_g90_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_genesis_g90_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) elif palisade_active: ff *= get_palisade_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) friction_threshold = get_palisade_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_palisade_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) elif prius_active: ff *= get_prius_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) friction_threshold = get_prius_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_prius_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) elif ioniq_5_active: ff *= get_ioniq_5_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * ioniq_5_center_taper friction_threshold = get_ioniq_5_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_ioniq_5_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = 1.0 + ((friction_scale - 1.0) * ioniq_5_center_taper) elif ioniq_ev_old_active: ff *= get_ioniq_ev_old_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * ioniq_ev_old_center_taper friction_scale = 1.0 + ((friction_scale - 1.0) * ioniq_ev_old_center_taper) elif ioniq_6_active: ff *= get_ioniq_6_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * ioniq_6_center_taper friction_threshold = get_ioniq_6_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) / max(ioniq_6_center_taper, 1e-3) friction_scale = get_ioniq_6_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = 1.0 + ((friction_scale - 1.0) * ioniq_6_center_taper) elif sonata_active: ff *= get_sonata_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * sonata_center_taper elif sonata_hybrid_active: ff *= get_sonata_hybrid_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * sonata_hybrid_center_taper elif elantra_non_scc_active: ff *= get_elantra_non_scc_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) elif kia_xceed_active: ff *= get_kia_xceed_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * kia_xceed_center_taper elif kia_niro_phev_2022_active: friction_threshold = get_kia_niro_phev_2022_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) elif kia_forte_active: ff *= get_kia_forte_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * kia_forte_center_taper friction_threshold = get_kia_forte_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) elif kia_ev6_test_active: ff *= get_kia_ev6_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * kia_ev6_center_taper friction_threshold = get_kia_ev6_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_kia_ev6_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = 1.0 + ((friction_scale - 1.0) * kia_ev6_center_taper) elif self.is_silverado: ff *= silverado_center_taper elif volt_plexy_test_active: ff *= get_volt_plexy_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) friction_threshold = get_volt_plexy_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_volt_plexy_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) elif self.is_civic_bosch_modified: ff *= get_civic_bosch_modified_b_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * civic_bosch_modified_a_center_taper friction_threshold = CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD friction_scale = get_civic_bosch_modified_b_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = 1.0 + ((friction_scale - 1.0) * civic_bosch_modified_a_center_taper) if trailer_load_kg > 0.0: ff *= get_trailer_lateral_ff_scale(trailer_load_kg, CS.vEgo, setpoint) friction_scale *= get_trailer_lateral_friction_scale(trailer_load_kg, CS.vEgo, setpoint) ff += friction_scale * get_friction(error_with_lsf + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, friction_threshold, self.torque_params) deadzone_boost_active = False if self.torque_deadzone_boost > 0.0 and abs(gravity_adjusted_future_lateral_accel) < DEADZONE_BOOST_LAT_ACCEL: boost_scale = np.interp(abs(gravity_adjusted_future_lateral_accel), [0.0, DEADZONE_BOOST_LAT_ACCEL], [1.0, 0.0]) ff += np.sign(gravity_adjusted_future_lateral_accel) * self.torque_deadzone_boost * boost_scale deadzone_boost_active = True if CS.vEgo < self.low_speed_reset_threshold: self.pid.reset() freeze_integrator = (steer_limited_by_safety or CS.steeringPressed or CS.vEgo < self.low_speed_reset_threshold or unwind_detected) output_lataccel = self.pid.update(pid_log.error, error_rate=-measurement_rate, speed=CS.vEgo, feedforward=ff, freeze_integrator=freeze_integrator) output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params) if self.is_bolt_2017: output_torque *= get_bolt_2017_torque_scale(setpoint, desired_lateral_jerk, CS.vEgo) elif bolt_2018_2021_tuned_path_active: output_torque *= get_bolt_2018_2021_dynamic_torque_scale(setpoint, desired_lateral_jerk, CS.vEgo) elif ioniq_6_active and not CS.steeringPressed: desired_angle_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) actual_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetDeg output_torque = get_ioniq_6_low_speed_angle_assist_torque(desired_angle_no_offset, actual_angle_no_offset, output_torque, CS.vEgo) elif volt_standard_test_active: output_torque *= volt_standard_center_taper elif kia_ev6_test_active: output_torque *= kia_ev6_low_speed_center_taper elif self.is_silverado: output_torque *= silverado_center_taper elif kia_niro_phev_2022_active: output_torque *= kia_niro_phev_2022_center_taper elif self.is_civic_bosch_modified and civic_bosch_modified_a_lateral_testing_ground_active(): output_torque *= civic_bosch_modified_a_center_taper pid_log.active = True pid_log.p = float(self.pid.p) pid_log.i = float(self.pid.i) pid_log.d = float(self.pid.d) pid_log.f = float(self.pid.f) pid_log.output = float(-output_torque) # TODO: log lat accel? pid_log.actualLateralAccel = float(measurement) pid_log.desiredLateralAccel = float(setpoint) pid_log.desiredLateralJerk = float(desired_lateral_jerk) pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) if DEBUG_TORQUE_TUNE and self.is_bolt: self.debug_counter += 1 if self.debug_counter % 50 == 0: print(f"bolt_torque ff_scale={ff_scale:.3f} pos={self.torque_ff_scale_pos:.3f} " f"neg={self.torque_ff_scale_neg:.3f} deadzone_boost_active={deadzone_boost_active}") self.prev_steering_pressed = CS.steeringPressed # TODO left is positive in this convention return -output_torque, 0.0, pid_log