mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-06-13 02:54:37 +08:00
Ioniq 6 Long Tune
This commit is contained in:
firestar5683
@@ -1,3 +1,5 @@
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
from opendbc.can import CANPacker
|
||||
from opendbc.car import Bus, DT_CTRL, make_tester_present_msg, structs
|
||||
@@ -19,6 +21,88 @@ MAX_ANGLE = 85
|
||||
MAX_ANGLE_FRAMES = 89
|
||||
MAX_ANGLE_CONSECUTIVE_FRAMES = 2
|
||||
CANFD_BLINDSPOT_STATUS_STALE_NS = 200_000_000
|
||||
HYUNDAI_CANFD_SCC_ACCEL_STEP = 5.0 / 50.0
|
||||
HYUNDAI_CANFD_SCC_DECEL_STEP = 12.5 / 50.0
|
||||
IONIQ_6_LONG_MIN_JERK = 0.5
|
||||
IONIQ_6_LONG_JERK_LIMIT = 4.0
|
||||
IONIQ_6_LONG_LOOKAHEAD_JERK_BP = [2.0, 5.0, 20.0]
|
||||
IONIQ_6_LONG_LOOKAHEAD_JERK_V = [0.3, 0.45, 0.6]
|
||||
IONIQ_6_DYNAMIC_LOWER_JERK_BP = [-2.0, -1.5, -1.0, -0.25, -0.1, -0.025, -0.01, -0.005]
|
||||
IONIQ_6_DYNAMIC_LOWER_JERK_V = [3.3, 1.5, 1.0, 0.8, 0.7, 0.65, 0.55, 0.5]
|
||||
|
||||
|
||||
@dataclass
|
||||
class Ioniq6LongitudinalTuningState:
|
||||
desired_accel: float = 0.0
|
||||
actual_accel: float = 0.0
|
||||
accel_last: float = 0.0
|
||||
jerk_upper: float = 0.0
|
||||
jerk_lower: float = 0.0
|
||||
stopping: bool = False
|
||||
stopping_count: int = 0
|
||||
long_control_state_last: LongCtrlState = LongCtrlState.off
|
||||
|
||||
|
||||
def _jerk_limited_integrator(desired_accel: float, last_accel: float, jerk_upper: float, jerk_lower: float) -> float:
|
||||
step = (jerk_upper if desired_accel >= last_accel else jerk_lower) * DT_CTRL * 5.0
|
||||
return float(np.clip(desired_accel, last_accel - step, last_accel + step))
|
||||
|
||||
|
||||
def _calculate_ioniq_6_dynamic_lower_jerk(accel_error: float) -> float:
|
||||
if accel_error < 0.0:
|
||||
scaled_values = np.array(IONIQ_6_DYNAMIC_LOWER_JERK_V) * (IONIQ_6_LONG_JERK_LIMIT / IONIQ_6_DYNAMIC_LOWER_JERK_V[0])
|
||||
return float(np.interp(accel_error, IONIQ_6_DYNAMIC_LOWER_JERK_BP, scaled_values))
|
||||
return IONIQ_6_LONG_MIN_JERK
|
||||
|
||||
|
||||
def update_ioniq_6_longitudinal_tuning(state: Ioniq6LongitudinalTuningState, accel_cmd: float, v_ego: float, a_ego: float,
|
||||
long_control_state: LongCtrlState, long_active: bool) -> Ioniq6LongitudinalTuningState:
|
||||
stopping = long_control_state == LongCtrlState.stopping
|
||||
|
||||
if not long_active or not stopping:
|
||||
state.stopping = False
|
||||
state.stopping_count = 0
|
||||
elif state.long_control_state_last == LongCtrlState.off:
|
||||
state.stopping = True
|
||||
else:
|
||||
if state.stopping_count > 1 / (DT_CTRL * 5):
|
||||
state.stopping = True
|
||||
state.stopping_count += 1
|
||||
|
||||
if not long_active:
|
||||
state.desired_accel = 0.0
|
||||
state.actual_accel = 0.0
|
||||
state.accel_last = 0.0
|
||||
state.jerk_upper = 0.0
|
||||
state.jerk_lower = 0.0
|
||||
state.long_control_state_last = long_control_state
|
||||
return state
|
||||
|
||||
upper_speed_limit = float(np.interp(v_ego, [0.0, 5.0, 20.0], [2.0, 3.0, 2.0])) if long_control_state == LongCtrlState.pid else IONIQ_6_LONG_MIN_JERK
|
||||
lower_speed_limit = float(np.interp(v_ego, [0.0, 5.0, 20.0], [5.0, 3.5, 3.0]))
|
||||
|
||||
future_t_upper = float(np.interp(v_ego, IONIQ_6_LONG_LOOKAHEAD_JERK_BP, IONIQ_6_LONG_LOOKAHEAD_JERK_V))
|
||||
future_t_lower = float(np.interp(v_ego, IONIQ_6_LONG_LOOKAHEAD_JERK_BP, IONIQ_6_LONG_LOOKAHEAD_JERK_V))
|
||||
|
||||
accel_error = accel_cmd - state.accel_last
|
||||
j_ego_upper = float(np.clip(accel_error / future_t_upper, -IONIQ_6_LONG_JERK_LIMIT, IONIQ_6_LONG_JERK_LIMIT))
|
||||
j_ego_lower = float(np.clip(accel_error / future_t_lower, -IONIQ_6_LONG_JERK_LIMIT, IONIQ_6_LONG_JERK_LIMIT))
|
||||
desired_jerk_upper = min(max(j_ego_upper, IONIQ_6_LONG_MIN_JERK), upper_speed_limit)
|
||||
|
||||
dynamic_accel_error = a_ego - state.accel_last
|
||||
dynamic_lower_jerk = _calculate_ioniq_6_dynamic_lower_jerk(dynamic_accel_error)
|
||||
state.jerk_upper = desired_jerk_upper
|
||||
state.jerk_lower = min(dynamic_lower_jerk, lower_speed_limit)
|
||||
|
||||
if state.stopping:
|
||||
state.desired_accel = 0.0
|
||||
else:
|
||||
state.desired_accel = float(np.clip(accel_cmd, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
|
||||
|
||||
state.actual_accel = _jerk_limited_integrator(state.desired_accel, state.accel_last, state.jerk_upper, state.jerk_lower)
|
||||
state.accel_last = state.actual_accel
|
||||
state.long_control_state_last = long_control_state
|
||||
return state
|
||||
|
||||
|
||||
def process_hud_alert(enabled, fingerprint, hud_control):
|
||||
@@ -65,6 +149,7 @@ class CarController(CarControllerBase):
|
||||
self.long_active_ecu = self.CP.openpilotLongitudinalControl
|
||||
self._ioniq_6_lane_change_ui_side = None
|
||||
self._ioniq_6_lane_change_ui_trigger_frames = 0
|
||||
self._ioniq_6_long_tuning = Ioniq6LongitudinalTuningState()
|
||||
|
||||
def update(self, CC, CS, now_nanos, starpilot_toggles):
|
||||
actuators = CC.actuators
|
||||
@@ -110,7 +195,8 @@ class CarController(CarControllerBase):
|
||||
self.apply_torque_last = apply_torque
|
||||
|
||||
# accel + longitudinal
|
||||
accel = float(np.clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
|
||||
accel_cmd = float(np.clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
|
||||
accel = accel_cmd
|
||||
stopping = actuators.longControlState == LongCtrlState.stopping
|
||||
set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH)
|
||||
|
||||
@@ -125,6 +211,29 @@ class CarController(CarControllerBase):
|
||||
# longitudinal messages - stock ECU is still active and these would conflict
|
||||
self.long_active_ecu = self.CP.openpilotLongitudinalControl and not self.ecu_disable_failed
|
||||
|
||||
use_ioniq_6_dynamic_long_tuning = self.CP.carFingerprint == CAR.HYUNDAI_IONIQ_6 and self.long_active_ecu and \
|
||||
actuators.longControlState == LongCtrlState.pid
|
||||
if use_ioniq_6_dynamic_long_tuning and self.frame % 5 == 0:
|
||||
self._ioniq_6_long_tuning = update_ioniq_6_longitudinal_tuning(self._ioniq_6_long_tuning, accel_cmd,
|
||||
CS.out.vEgo, CS.out.aEgo,
|
||||
actuators.longControlState, self.long_active_ecu)
|
||||
use_ioniq_6_smoothed_accel = use_ioniq_6_dynamic_long_tuning and accel_cmd >= self._ioniq_6_long_tuning.actual_accel
|
||||
if self.CP.carFingerprint == CAR.HYUNDAI_IONIQ_6 and self.long_active_ecu:
|
||||
if use_ioniq_6_smoothed_accel:
|
||||
accel = self._ioniq_6_long_tuning.actual_accel
|
||||
stopping = self._ioniq_6_long_tuning.stopping
|
||||
elif use_ioniq_6_dynamic_long_tuning:
|
||||
accel = float(np.clip(accel_cmd,
|
||||
self.accel_last - HYUNDAI_CANFD_SCC_DECEL_STEP,
|
||||
self.accel_last + HYUNDAI_CANFD_SCC_ACCEL_STEP))
|
||||
self._ioniq_6_long_tuning.desired_accel = accel_cmd
|
||||
self._ioniq_6_long_tuning.actual_accel = accel
|
||||
self._ioniq_6_long_tuning.accel_last = accel
|
||||
self._ioniq_6_long_tuning.jerk_upper = 3.0
|
||||
self._ioniq_6_long_tuning.jerk_lower = 5.0 if CC.enabled else 1.0
|
||||
self._ioniq_6_long_tuning.stopping = stopping
|
||||
self._ioniq_6_long_tuning.long_control_state_last = actuators.longControlState
|
||||
|
||||
# *** common hyundai stuff ***
|
||||
|
||||
# tester present - w/ no response (keeps relevant ECU disabled)
|
||||
@@ -211,6 +320,9 @@ class CarController(CarControllerBase):
|
||||
|
||||
lka_steering = self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING
|
||||
lka_steering_long = lka_steering and self.long_active_ecu
|
||||
use_ioniq_6_dynamic_long_tuning = self.CP.carFingerprint == CAR.HYUNDAI_IONIQ_6 and self.long_active_ecu and \
|
||||
CC.actuators.longControlState == LongCtrlState.pid
|
||||
use_ioniq_6_smoothed_accel = use_ioniq_6_dynamic_long_tuning and CC.actuators.accel >= self._ioniq_6_long_tuning.actual_accel
|
||||
|
||||
# steering control
|
||||
can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled,
|
||||
@@ -271,8 +383,20 @@ class CarController(CarControllerBase):
|
||||
CC.leftBlinker,
|
||||
CC.rightBlinker))
|
||||
if self.frame % 2 == 0:
|
||||
acc_kwargs = {}
|
||||
if self.CP.carFingerprint == CAR.HYUNDAI_IONIQ_6:
|
||||
acc_kwargs = {
|
||||
"main_mode_acc": int(CS.out.cruiseState.available),
|
||||
"direct_accel": True,
|
||||
}
|
||||
if use_ioniq_6_smoothed_accel:
|
||||
acc_kwargs["jerk_lower"] = self._ioniq_6_long_tuning.jerk_lower
|
||||
acc_kwargs["jerk_upper"] = self._ioniq_6_long_tuning.jerk_upper
|
||||
else:
|
||||
acc_kwargs["jerk_lower"] = 5.0
|
||||
acc_kwargs["jerk_upper"] = 3.0
|
||||
can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
|
||||
set_speed_in_units, hud_control))
|
||||
set_speed_in_units, hud_control, **acc_kwargs))
|
||||
self.accel_last = accel
|
||||
else:
|
||||
# button presses
|
||||
|
||||
@@ -314,24 +314,28 @@ def create_ioniq_6_cluster_lane_change_messages(CAN, frame, side=None, trigger=F
|
||||
return []
|
||||
|
||||
|
||||
def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control):
|
||||
def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control,
|
||||
main_mode_acc=1, jerk_lower=None, jerk_upper=None, direct_accel=False):
|
||||
jerk = 5
|
||||
jn = jerk / 50
|
||||
if not enabled or gas_override:
|
||||
a_val, a_raw = 0, 0
|
||||
elif direct_accel:
|
||||
a_raw = accel
|
||||
a_val = accel
|
||||
else:
|
||||
a_raw = accel
|
||||
a_val = np.clip(accel, accel_last - jn, accel_last + jn)
|
||||
|
||||
values = {
|
||||
"ACCMode": 0 if not enabled else (2 if gas_override else 1),
|
||||
"MainMode_ACC": 1,
|
||||
"MainMode_ACC": main_mode_acc,
|
||||
"StopReq": 1 if stopping else 0,
|
||||
"aReqValue": a_val,
|
||||
"aReqRaw": a_raw,
|
||||
"VSetDis": set_speed,
|
||||
"JerkLowerLimit": jerk if enabled else 1,
|
||||
"JerkUpperLimit": 3.0,
|
||||
"JerkLowerLimit": jerk_lower if jerk_lower is not None else (jerk if enabled else 1),
|
||||
"JerkUpperLimit": jerk_upper if jerk_upper is not None else 3.0,
|
||||
|
||||
"ACC_ObjDist": 1,
|
||||
"ObjValid": 0,
|
||||
|
||||
@@ -160,6 +160,13 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.startAccel = 1.0
|
||||
ret.longitudinalActuatorDelay = 0.5
|
||||
|
||||
if candidate == CAR.HYUNDAI_IONIQ_6:
|
||||
ret.startingState = True
|
||||
ret.vEgoStopping = 0.3
|
||||
ret.vEgoStarting = 0.1
|
||||
ret.stoppingDecelRate = 0.4
|
||||
ret.longitudinalActuatorDelay = 0.5
|
||||
|
||||
if ret.openpilotLongitudinalControl:
|
||||
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.LONG.value
|
||||
if ret.flags & HyundaiFlags.HYBRID:
|
||||
|
||||
@@ -7,6 +7,7 @@ from opendbc.can import CANPacker, CANParser
|
||||
from opendbc.car import Bus, ButtonType, gen_empty_fingerprint
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.fw_versions import build_fw_dict, match_fw_to_car
|
||||
from opendbc.car.hyundai.carcontroller import Ioniq6LongitudinalTuningState, update_ioniq_6_longitudinal_tuning
|
||||
from opendbc.car.hyundai.carstate import CarState, decode_ioniq_6_blindspot_radar_state
|
||||
from opendbc.car.hyundai.interface import CarInterface
|
||||
from opendbc.car.hyundai import hyundaicanfd
|
||||
@@ -16,6 +17,8 @@ from opendbc.car.hyundai.values import CAMERA_SCC_CAR, CANFD_CAR, CAN_GEARS, CAR
|
||||
HYBRID_CAR, EV_CAR, FW_QUERY_CONFIG, LEGACY_SAFETY_MODE_CAR, CANFD_FUZZY_WHITELIST, \
|
||||
UNSUPPORTED_LONGITUDINAL_CAR, PLATFORM_CODE_ECUS, HYUNDAI_VERSION_REQUEST_LONG, \
|
||||
CarControllerParams, DBC, HyundaiFlags, get_platform_codes, HyundaiSafetyFlags
|
||||
|
||||
LongCtrlState = CarParams.Actuators.LongControlState
|
||||
from opendbc.car.hyundai.fingerprints import FW_VERSIONS
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
@@ -219,6 +222,49 @@ class TestHyundaiFingerprint:
|
||||
ret = update(0, 3)
|
||||
assert any(be.type == ButtonType.altButton2 and not be.pressed for be in ret.buttonEvents)
|
||||
|
||||
def test_ioniq_6_longitudinal_params_match_canfd_tune(self):
|
||||
toggles = get_test_toggles()
|
||||
CP = CarInterface.get_params(CAR.HYUNDAI_IONIQ_6, gen_empty_fingerprint(), [], True, False, False, toggles)
|
||||
|
||||
assert CP.vEgoStopping == pytest.approx(0.3)
|
||||
assert CP.vEgoStarting == pytest.approx(0.1)
|
||||
assert CP.stoppingDecelRate == pytest.approx(0.4)
|
||||
assert CP.longitudinalActuatorDelay == pytest.approx(0.5)
|
||||
assert CP.startingState
|
||||
|
||||
def test_ioniq_6_longitudinal_tuning_helper_matches_dynamic_profile(self):
|
||||
state = Ioniq6LongitudinalTuningState()
|
||||
|
||||
state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=1.5, v_ego=10.0, a_ego=0.0,
|
||||
long_control_state=LongCtrlState.pid, long_active=True)
|
||||
assert state.desired_accel == pytest.approx(1.5)
|
||||
assert state.jerk_upper == pytest.approx(2.6666666667)
|
||||
assert state.jerk_lower == pytest.approx(0.5)
|
||||
assert state.actual_accel == pytest.approx(0.1333333333)
|
||||
|
||||
state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=1.0, v_ego=10.0, a_ego=0.0,
|
||||
long_control_state=LongCtrlState.stopping, long_active=True)
|
||||
assert not state.stopping
|
||||
assert state.desired_accel == pytest.approx(1.0)
|
||||
|
||||
for _ in range(25):
|
||||
state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=1.0, v_ego=10.0, a_ego=0.0,
|
||||
long_control_state=LongCtrlState.stopping, long_active=True)
|
||||
assert state.stopping
|
||||
assert state.desired_accel == pytest.approx(0.0)
|
||||
actual_accel_after_stop = state.actual_accel
|
||||
|
||||
state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=1.0, v_ego=10.0, a_ego=0.0,
|
||||
long_control_state=LongCtrlState.stopping, long_active=True)
|
||||
assert state.actual_accel < actual_accel_after_stop
|
||||
|
||||
state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=1.0, v_ego=10.0, a_ego=0.0,
|
||||
long_control_state=LongCtrlState.pid, long_active=False)
|
||||
assert state.desired_accel == pytest.approx(0.0)
|
||||
assert state.actual_accel == pytest.approx(0.0)
|
||||
assert state.jerk_upper == pytest.approx(0.0)
|
||||
assert state.jerk_lower == pytest.approx(0.0)
|
||||
|
||||
def test_sportage_angle_steering_uses_adas_cmd_with_send_lfa(self):
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
cam_can = CanBus(None, fingerprint).CAM
|
||||
|
||||
@@ -226,18 +226,22 @@ IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.05
|
||||
IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.04
|
||||
IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 0.42
|
||||
IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 0.96
|
||||
IONIQ_6_CENTER_TAPER_MAX = 0.020
|
||||
IONIQ_6_CENTER_TAPER_LAT = 0.08
|
||||
IONIQ_6_CENTER_TAPER_MAX = 0.026
|
||||
IONIQ_6_CENTER_TAPER_LAT = 0.11
|
||||
IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.02
|
||||
IONIQ_6_CENTER_TAPER_SPEED = 18.0
|
||||
IONIQ_6_CENTER_TAPER_SPEED_WIDTH = 2.5
|
||||
IONIQ_6_DIRECTIONAL_TAPER_LAT_START = 0.22
|
||||
IONIQ_6_DIRECTIONAL_TAPER_LAT_START = 0.18
|
||||
IONIQ_6_DIRECTIONAL_TAPER_LAT_END = 0.90
|
||||
IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH = 0.08
|
||||
IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT = 0.03
|
||||
IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT = 0.10
|
||||
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.10
|
||||
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 0.30
|
||||
IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT = 0.05
|
||||
IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT = 0.18
|
||||
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.18
|
||||
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 0.52
|
||||
IONIQ_6_OUTPUT_TAPER_SPEED = 8.5
|
||||
IONIQ_6_OUTPUT_TAPER_SPEED_WIDTH = 2.5
|
||||
IONIQ_6_OUTPUT_CENTER_TAPER_BLEND = 0.62
|
||||
IONIQ_6_OUTPUT_DIRECTIONAL_TAPER_BLEND = 0.78
|
||||
|
||||
KIA_EV6_LATERAL_TESTING_GROUND_ID = testing_ground.id_6
|
||||
KIA_EV6_LATERAL_TESTING_GROUND_VARIANT = "C"
|
||||
@@ -765,6 +769,15 @@ def get_ioniq_6_directional_taper_scale(desired_lateral_accel: float, desired_la
|
||||
return max(1.0 - reduction, 0.70)
|
||||
|
||||
|
||||
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)
|
||||
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 kia_ev6_lateral_testing_ground_active() -> bool:
|
||||
return testing_ground.use(KIA_EV6_LATERAL_TESTING_GROUND_ID, KIA_EV6_LATERAL_TESTING_GROUND_VARIANT)
|
||||
|
||||
@@ -1079,6 +1092,8 @@ class LatControlTorque(LatControl):
|
||||
output_torque *= get_bolt_2018_2021_dynamic_torque_scale(setpoint, desired_lateral_jerk, CS.vEgo)
|
||||
elif volt_standard_test_active:
|
||||
output_torque *= volt_standard_center_taper
|
||||
elif ioniq_6_test_active:
|
||||
output_torque *= get_ioniq_6_output_taper_scale(setpoint, desired_lateral_jerk, CS.vEgo)
|
||||
|
||||
pid_log.active = True
|
||||
pid_log.p = float(self.pid.p)
|
||||
|
||||
@@ -32,6 +32,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
|
||||
get_genesis_g90_friction_threshold,
|
||||
get_ioniq_6_center_taper_scale,
|
||||
get_ioniq_6_directional_taper_scale,
|
||||
get_ioniq_6_output_taper_scale,
|
||||
get_ioniq_6_ff_scale,
|
||||
get_ioniq_6_friction_scale,
|
||||
get_ioniq_6_friction_threshold,
|
||||
@@ -182,7 +183,7 @@ class TestLatControl:
|
||||
|
||||
def test_genesis_g90_ff_scale_curve(self):
|
||||
assert get_genesis_g90_ff_scale(0.0, 0.0, 20.0) == 1.0
|
||||
assert get_genesis_g90_ff_scale(-0.5, 0.0, 20.0) > get_genesis_g90_ff_scale(0.5, 0.0, 20.0)
|
||||
assert get_genesis_g90_ff_scale(0.5, 0.0, 20.0) > get_genesis_g90_ff_scale(-0.5, 0.0, 20.0)
|
||||
assert get_genesis_g90_ff_scale(0.6, 0.7, 8.0) > get_genesis_g90_ff_scale(0.6, 0.0, 8.0) > get_genesis_g90_ff_scale(0.6, -0.7, 8.0)
|
||||
assert get_genesis_g90_ff_scale(-0.6, -0.7, 8.0) > get_genesis_g90_ff_scale(-0.6, 0.0, 8.0) > get_genesis_g90_ff_scale(-0.6, 0.7, 8.0)
|
||||
assert get_genesis_g90_ff_scale(2.0, 0.0, 20.0) < get_genesis_g90_ff_scale(0.8, 0.0, 20.0)
|
||||
@@ -218,7 +219,13 @@ class TestLatControl:
|
||||
assert get_ioniq_6_directional_taper_scale(0.0, 0.0) == 1.0
|
||||
assert get_ioniq_6_directional_taper_scale(-0.5, 0.0) < get_ioniq_6_directional_taper_scale(0.5, 0.0) < 1.0
|
||||
assert get_ioniq_6_directional_taper_scale(-0.5, 0.7) < get_ioniq_6_directional_taper_scale(-0.5, 0.0)
|
||||
assert get_ioniq_6_directional_taper_scale(1.2, 0.0) > 0.98
|
||||
assert get_ioniq_6_directional_taper_scale(1.2, 0.0) > 0.96
|
||||
|
||||
def test_ioniq_6_output_taper_curve(self):
|
||||
assert get_ioniq_6_output_taper_scale(0.0, 0.0, 25.0) < get_ioniq_6_output_taper_scale(0.0, 0.0, 8.0) <= 1.0
|
||||
assert get_ioniq_6_output_taper_scale(-0.5, 0.0, 25.0) < get_ioniq_6_output_taper_scale(0.5, 0.0, 25.0) < 1.0
|
||||
assert get_ioniq_6_output_taper_scale(-0.5, 0.7, 25.0) < get_ioniq_6_output_taper_scale(-0.5, 0.0, 25.0)
|
||||
assert get_ioniq_6_output_taper_scale(1.2, 0.0, 25.0) > 0.94
|
||||
|
||||
def test_ioniq_6_friction_threshold_curve(self):
|
||||
base = get_friction_threshold(6.0)
|
||||
|
||||
Reference in New Issue
Block a user