def __init__(self, CP):
self.angle_steers_des = 0.
A = np.matrix([[1.0, DT_CTRL, 0.0], [0.0, 1.0, DT_CTRL],
[0.0, 0.0, 1.0]])
C = np.matrix([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0]])
# Q = np.matrix([[1e-2, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 10.0]])
# R = np.matrix([[1e-2, 0.0], [0.0, 1e3]])
# (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R)
# K = np.transpose(K)
K = np.matrix([[7.30262179e-01, 2.07003658e-04],
[7.29394177e+00, 1.39159419e-02],
[1.71022442e+01, 3.38495381e-02]])
self.K = K
self.A_K = A - np.dot(K, C)
self.x = np.matrix([[0.], [0.], [0.]])
self.enforce_rate_limit = CP.carName == "toyota"
self.kyd = kyd_conf(CP)
self.mpc_frame = 0
self.RC = CP.lateralTuning.indi.timeConstant
self.G = CP.lateralTuning.indi.actuatorEffectiveness
self.outer_loop_gain = CP.lateralTuning.indi.outerLoopGain
self.inner_loop_gain = CP.lateralTuning.indi.innerLoopGain
self.alpha = 1. - DT_CTRL / (self.RC + DT_CTRL)
self.sat_count_rate = 1.0 * DT_CTRL
self.sat_limit = CP.steerLimitTimer
self.reset()
def cV_tune(self, v_ego, cv_value): # cV(곡률에 의한 변화)
kyd = kyd_conf()
self.sRKPHV = [9., 22.]
self.cVBPV = kyd.conf['cvBPV'] # 곡률
self.cvSteerMaxV1 = kyd.conf['cvSteerMaxV1']
self.cvSteerDeltaUpV1 = kyd.conf['cvSteerDeltaUpV1']
self.cvSteerDeltaDnV1 = kyd.conf['cvSteerDeltaDnV1']
self.cvSteerMaxV2 = kyd.conf['cvSteerMaxV2']
self.cvSteerDeltaUpV2 = kyd.conf['cvSteerDeltaUpV2']
self.cvSteerDeltaDnV2 = kyd.conf['cvSteerDeltaDnV2']
cv_BPV = self.cVBPV # 곡률
# Max
self.steerMax1 = interp(cv_value, cv_BPV, self.cvSteerMaxV1)
self.steerMax2 = interp(cv_value, cv_BPV, self.cvSteerMaxV2)
self.steerMaxV = [float(self.steerMax1), float(self.steerMax2)]
self.MAX = interp(v_ego, self.sRKPHV, self.steerMaxV)
# Up
self.steerUP1 = interp(cv_value, cv_BPV, self.cvSteerDeltaUpV1)
self.steerUP2 = interp(cv_value, cv_BPV, self.cvSteerDeltaUpV2)
self.steerUPV = [float(self.steerUP1), float(self.steerUP2)]
self.UP = interp(v_ego, self.sRKPHV, self.steerUPV)
# Dn
self.steerDN1 = interp(cv_value, cv_BPV, self.cvSteerDeltaDnV1)
self.steerDN2 = interp(cv_value, cv_BPV, self.cvSteerDeltaDnV2)
self.steerDNV = [float(self.steerDN1), float(self.steerDN2)]
self.DN = interp(v_ego, self.sRKPHV, self.steerDNV)
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.apply_steer_last = 0
self.car_fingerprint = CP.carFingerprint
self.packer = CANPacker(dbc_name)
self.steer_rate_limited = False
self.resume_cnt = 0
self.last_resume_frame = 0
self.last_lead_distance = 0
self.lanechange_manual_timer = 0
self.emergency_manual_timer = 0
self.driver_steering_torque_above_timer = 0
self.mode_change_timer = 0
self.steer_mode = ""
self.mdps_status = ""
self.lkas_switch = ""
self.lkas11_cnt = 0
self.nBlinker = 0
self.dRel = 0
self.yRel = 0
self.vRel = 0
self.timer1 = tm.CTime1000("time")
self.model_speed = 0
self.model_sum = 0
# hud
self.hud_timer_left = 0
self.hud_timer_right = 0
self.command_cnt = 0
self.command_load = 0
self.params = Params()
# param
self.param_preOpkrAccelProfile = -1
self.param_OpkrAccelProfile = 0
self.param_OpkrAutoResume = 0
self.param_OpkrEnableLearner = 0
self.SC = None
self.traceCC = trace1.Loger("CarController")
self.res_cnt = 7
self.res_delay = 0
kyd = kyd_conf()
self.driver_steering_torque_above = float(
kyd.conf['driverSteeringTorqueAbove'])
self.params = Params()
self.mode_change_switch = int(
self.params.get('CruiseStatemodeSelInit'))
def __init__(self, CP):
self.kyd = kyd_conf(CP)
self.deadzone = float(self.kyd.conf['deadzone'])
self.pid = PIController(
(CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
k_f=CP.lateralTuning.pid.kf,
pos_limit=1.0,
sat_limit=CP.steerLimitTimer)
self.angle_steers_des = 0.
self.mpc_frame = 0
def live_tune(self, CP):
self.mpc_frame += 1
if self.mpc_frame % 300 == 0:
# live tuning through /data/openpilot/tune.py overrides interface.py settings
self.kyd = kyd_conf()
if self.kyd.conf['EnableLiveTune'] == "1":
self.scale_ = float(self.kyd.conf['scale'])
self.ki_ = float(self.kyd.conf['ki'])
self.dc_gain_ = float(self.kyd.conf['dc_gain'])
self.scale = self.scale_
self.ki = self.ki_
self.dc_gain = self.dc_gain_
self.mpc_frame = 0
def live_tune(self, CP):
self.mpc_frame += 1
if self.mpc_frame % 300 == 0:
# live tuning through /data/openpilot/tune.py overrides interface.py settings
self.kyd = kyd_conf()
if self.kyd.conf['EnableLiveTune'] == "1":
self.steerKpV = [float(self.kyd.conf['Kp'])]
self.steerKiV = [float(self.kyd.conf['Ki'])]
self.steerKf = float(self.kyd.conf['Kf'])
self.pid = PIController(
(CP.lateralTuning.pid.kpBP, self.steerKpV),
(CP.lateralTuning.pid.kiBP, self.steerKiV),
k_f=self.steerKf,
pos_limit=1.0)
self.deadzone = float(self.kyd.conf['deadzone'])
self.mpc_frame = 0
def live_tune(self, CP):
self.mpc_frame += 1
if self.mpc_frame % 300 == 0:
# live tuning through /data/openpilot/tune.py overrides interface.py settings
self.kyd = kyd_conf()
if self.kyd.conf['EnableLiveTune'] == "1":
self.outerLoopGain = float(self.kyd.conf['outerLoopGain'])
self.innerLoopGain = float(self.kyd.conf['innerLoopGain'])
self.timeConstant = float(self.kyd.conf['timeConstant'])
self.actuatorEffectiveness = float(
self.kyd.conf['actuatorEffectiveness'])
self.RC = self.timeConstant
self.G = self.actuatorEffectiveness
self.outer_loop_gain = self.outerLoopGain
self.inner_loop_gain = self.innerLoopGain
self.mpc_frame = 0
def __init__(self, CP):
super().__init__(CP)
#Auto detection for setup
#self.no_radar = CP.sccBus == -1
self.mdps_bus = CP.mdpsBus
self.sas_bus = CP.sasBus
self.scc_bus = CP.sccBus
self.cruise_main_button = False
self.cruise_buttons = False
self.lkas_button_on = False
self.lkas_error = False
self.prev_cruise_main_button = False
self.prev_cruise_buttons = False
self.main_on = False
self.acc_active = False
self.cruiseState_modeSel = 0
self.driverAcc_time = 0
self.kyd = kyd_conf()
self.steer_Angle_Correction = float(
self.kyd.conf['steerAngleCorrection'])
# BSM
self.leftBlindspot_time = 0
self.rightBlindspot_time = 0
# blinker
self.left_blinker_flash = 0
self.right_blinker_flash = 0
self.TSigLHSw = 0
self.TSigRHSw = 0
self.SC = SpdController()
def __init__(self, CP):
self.kyd = kyd_conf(CP)
self.mpc_frame = 0
self.scale = CP.lateralTuning.lqr.scale
self.ki = CP.lateralTuning.lqr.ki
self.A = np.array(CP.lateralTuning.lqr.a).reshape((2, 2))
self.B = np.array(CP.lateralTuning.lqr.b).reshape((2, 1))
self.C = np.array(CP.lateralTuning.lqr.c).reshape((1, 2))
self.K = np.array(CP.lateralTuning.lqr.k).reshape((1, 2))
self.L = np.array(CP.lateralTuning.lqr.l).reshape((2, 1))
self.dc_gain = CP.lateralTuning.lqr.dcGain
self.x_hat = np.array([[0], [0]])
self.i_unwind_rate = 0.3 * DT_CTRL
self.i_rate = 1.0 * DT_CTRL
self.sat_count_rate = 1.0 * DT_CTRL
self.sat_limit = CP.steerLimitTimer
self.reset()
def __init__(self, CP):
self.LP = LanePlanner()
self.last_cloudlog_t = 0
self.steer_rate_cost = CP.steerRateCost
self.setup_mpc()
self.solution_invalid_cnt = 0
self.params = Params()
kyd = kyd_conf(CP)
if kyd.conf['steerRatio'] == "-1":
self.steerRatio = CP.steerRatio
else:
self.steerRatio = float(kyd.conf['steerRatio'])
if kyd.conf['steerRateCost'] == "-1":
self.steer_rate_cost = CP.steerRateCost
else:
self.steer_rate_cost = float(kyd.conf['steerRateCost'])
self.kyd_steerRatio = None
self.sRBP = [0., 0.]
self.sRBoost = [0., 0.]
# Lane change
self.lane_change_enabled = self.params.get('LaneChangeEnabled') == b'1'
self.lane_change_auto_delay = self.params.get_OpkrAutoLanechangedelay(
) #int( self.params.get('OpkrAutoLanechangedelay') )
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
self.lane_change_run_timer = 0.0
self.lane_change_wait_timer = 0.0
self.lane_change_ll_prob = 1.0
self.prev_one_blinker = False
self.param_OpkrEnableLearner = int(
self.params.get('OpkrEnableLearner'))
def update(self, sm, pm, CP, VM):
v_ego = sm['carState'].vEgo
angle_steers = sm['carState'].steeringAngle
active = sm['controlsState'].active
angle_offset = sm['liveParameters'].angleOffset
if not self.param_OpkrEnableLearner:
kyd = kyd_conf()
#self.steer_rate_cost = float(kyd.conf['steerRateCost'])
self.sRBP = kyd.conf['sR_BP']
self.sRBoost = kyd.conf['sR_Boost']
boost_rate = interp(abs(angle_steers), self.sRBP, self.sRBoost)
self.kyd_steerRatio = self.steerRatio + boost_rate
self.sR_Cost = [
1.0, 0.90, 0.81, 0.73, 0.66, 0.60, 0.54, 0.48, 0.36, 0.275,
0.20, 0.175, 0.15, 0.11, 0.05
]
#self.sR_Cost = [0.75,0.67,0.60,0.54,0.48,0.425,0.37,0.32,0.24,0.19,0.15,0.14,0.13,0.11,0.05]
#self.sR_Cost = [0.50,0.46,0.425,0.395,0.37,0.34,0.315,0.29,0.23,0.185,0.15,0.14,0.13,0.11,0.05]
#steerRatio = 10.0
#"sR_BP": [0.0,2.0,4.0,6.0,8.0,10.0,12.0,14.0,20.0,27.0,35.0,40.0,45.0,60.0,100],
#"sR_Boost": [0.0,0.7,1.3,1.9,2.5,3.05,3.55,4.0,5.0,5.7,6.2,6.35,6.4,6.5,8.0],
self.steer_rate_cost = interp(abs(angle_steers), self.sRBP,
self.sR_Cost)
# Run MPC
self.angle_steers_des_prev = self.angle_steers_des_mpc
# Update vehicle model
x = max(sm['liveParameters'].stiffnessFactor, 0.1)
sr = max(sm['liveParameters'].steerRatio, 0.1)
VM.update_params(x, sr)
curvature_factor = VM.curvature_factor(v_ego)
self.LP.parse_model(sm['model'])
# Lane change logic
one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
if sm['carState'].leftBlinker:
self.lane_change_direction = LaneChangeDirection.left
elif sm['carState'].rightBlinker:
self.lane_change_direction = LaneChangeDirection.right
if (not active) or (self.lane_change_run_timer >
LANE_CHANGE_TIME_MAX) or (not one_blinker) or (
not self.lane_change_enabled):
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
else:
torque_applied = sm['carState'].steeringPressed and \
((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
(sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
blindspot_detected = (
(sm['carState'].leftBlindspot
and self.lane_change_direction == LaneChangeDirection.left) or
(sm['carState'].rightBlindspot
and self.lane_change_direction == LaneChangeDirection.right))
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
# State transitions
# off
if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
self.lane_change_state = LaneChangeState.preLaneChange
self.lane_change_ll_prob = 1.0
self.lane_change_wait_timer = 0
# pre
elif self.lane_change_state == LaneChangeState.preLaneChange:
self.lane_change_wait_timer += DT_MDL
if not one_blinker or below_lane_change_speed:
self.lane_change_state = LaneChangeState.off
elif not blindspot_detected and (
torque_applied or (self.lane_change_auto_delay
and self.lane_change_wait_timer >
self.lane_change_auto_delay)):
self.lane_change_state = LaneChangeState.laneChangeStarting
# starting
elif self.lane_change_state == LaneChangeState.laneChangeStarting:
# fade out over .5s
self.lane_change_ll_prob = max(
self.lane_change_ll_prob - 1.5 * DT_MDL, 0.0)
# 98% certainty
if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
self.lane_change_state = LaneChangeState.laneChangeFinishing
# finishing
elif self.lane_change_state == LaneChangeState.laneChangeFinishing:
# fade in laneline over 1s
self.lane_change_ll_prob = min(
self.lane_change_ll_prob + DT_MDL, 1.0)
if one_blinker and self.lane_change_ll_prob > 0.99:
self.lane_change_state = LaneChangeState.laneChangeDone
# done
elif self.lane_change_state == LaneChangeState.laneChangeDone:
if not one_blinker:
self.lane_change_state = LaneChangeState.off
if self.lane_change_state in [
LaneChangeState.off, LaneChangeState.preLaneChange
]:
self.lane_change_run_timer = 0.0
else:
self.lane_change_run_timer += DT_MDL
self.prev_one_blinker = one_blinker
desire = DESIRES[self.lane_change_direction][self.lane_change_state]
# Turn off lanes during lane change
if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft:
self.LP.l_prob *= self.lane_change_ll_prob
self.LP.r_prob *= self.lane_change_ll_prob
self.LP.update_d_poly(v_ego)
# account for actuation delay
if self.param_OpkrEnableLearner:
self.cur_state = calc_states_after_delay(
self.cur_state, v_ego, angle_steers - angle_offset,
curvature_factor, VM.sR, CP.steerActuatorDelay)
else:
self.cur_state = calc_states_after_delay(
self.cur_state, v_ego, angle_steers - angle_offset,
curvature_factor, self.kyd_steerRatio, CP.steerActuatorDelay)
v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
list(self.LP.l_poly), list(self.LP.r_poly),
list(self.LP.d_poly), self.LP.l_prob,
self.LP.r_prob, curvature_factor, v_ego_mpc,
self.LP.lane_width)
# reset to current steer angle if not active or overriding
if active:
delta_desired = self.mpc_solution[0].delta[1]
if self.param_OpkrEnableLearner:
rate_desired = math.degrees(self.mpc_solution[0].rate[0] *
VM.sR)
else:
rate_desired = math.degrees(self.mpc_solution[0].rate[0] *
self.kyd_steerRatio)
else:
if self.param_OpkrEnableLearner:
delta_desired = math.radians(angle_steers -
angle_offset) / VM.sR
else:
delta_desired = math.radians(
angle_steers - angle_offset) / self.kyd_steerRatio
rate_desired = 0.0
self.cur_state[0].delta = delta_desired
if self.param_OpkrEnableLearner:
self.angle_steers_des_mpc = float(
math.degrees(delta_desired * VM.sR) + angle_offset)
else:
self.angle_steers_des_mpc = float(
math.degrees(delta_desired * self.kyd_steerRatio) +
angle_offset)
# Check for infeasable MPC solution
mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta)
t = sec_since_boot()
if mpc_nans:
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE,
MPC_COST_LAT.HEADING, self.steer_rate_cost)
if self.param_OpkrEnableLearner:
self.cur_state[0].delta = math.radians(angle_steers -
angle_offset) / VM.sR
else:
self.cur_state[0].delta = math.radians(
angle_steers - angle_offset) / self.kyd_steerRatio
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning("Lateral mpc - nan: True")
if self.mpc_solution[
0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge
self.solution_invalid_cnt += 1
else:
self.solution_invalid_cnt = 0
plan_solution_valid = self.solution_invalid_cnt < 3
plan_send = messaging.new_message('pathPlan')
plan_send.valid = sm.all_alive_and_valid(service_list=[
'carState', 'controlsState', 'liveParameters', 'model'
])
plan_send.pathPlan.laneWidth = float(self.LP.lane_width)
plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly]
plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly]
plan_send.pathPlan.lProb = float(self.LP.l_prob)
plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly]
plan_send.pathPlan.rProb = float(self.LP.r_prob)
plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc)
plan_send.pathPlan.rateSteers = float(rate_desired)
plan_send.pathPlan.angleOffset = float(
sm['liveParameters'].angleOffsetAverage)
plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid)
plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid)
plan_send.pathPlan.desire = desire
plan_send.pathPlan.laneChangeState = self.lane_change_state
plan_send.pathPlan.laneChangeDirection = self.lane_change_direction
if not self.param_OpkrEnableLearner:
plan_send.pathPlan.steerRatio = float(self.kyd_steerRatio)
pm.send('pathPlan', plan_send)
if LOG_MPC:
dat = messaging.new_message('liveMpc')
dat.liveMpc.x = list(self.mpc_solution[0].x)
dat.liveMpc.y = list(self.mpc_solution[0].y)
dat.liveMpc.psi = list(self.mpc_solution[0].psi)
dat.liveMpc.delta = list(self.mpc_solution[0].delta)
dat.liveMpc.cost = self.mpc_solution[0].cost
pm.send('liveMpc', dat)
def getch():
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
try:
tty.setraw(sys.stdin.fileno())
ch = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
return ch
button_delay = 0.2
kyd = kyd_conf()
kyd.conf['EnableLiveTune'] = "1"
#kyd.write_config(kyd.conf)
#param = ["cameraOffset", "Kp", "Ki", "Kf", "steerRatio", "sR_boost", "sR_BP0", \
# "sR_BP1", "sR_time", "steerRateCost"]
param = [
"cameraOffset", "outerLoopGain", "innerLoopGain", "timeConstant",
"actuatorEffectiveness"
]
# param = ["cameraOffset", "scale", "ki", "dc_gain", "steerRatio", "sR_boost", "sR_BP0", \
# "sR_BP1", "sR_time", "steerRateCost"]
#param = ["Kp", "Ki", "Kf", "steerRatio", "sR_boost", "sR_BP0", \
# "sR_BP1", "sR_time", "steerRateCost", "deadzone", "slowOnCurves", \
def get_params(candidate,
fingerprint=gen_empty_fingerprint(),
has_relay=False,
car_fw=[]): # pylint: disable=dangerous-default-value
ret = CarInterfaceBase.get_std_params(candidate, fingerprint,
has_relay)
ret.carName = "hyundai"
ret.safetyModel = car.CarParams.SafetyModel.hyundai
params = Params()
if int(params.get('LateralControlPriority')) == 0:
ret.radarOffCan = False #False(선행차우선) #True(차선우선) #선행차량 인식 마크 유무.
else:
ret.radarOffCan = True
# Most Hyundai car ports are community features for now
ret.communityFeature = False
kyd = kyd_conf()
tire_stiffness_factor = 1.
ret.steerActuatorDelay = float(kyd.conf['SteerActuatorDelay']) # 0.3
ret.steerRateCost = 0.5
ret.steerLimitTimer = float(kyd.conf['SteerLimitTimer']) # 0.4
if int(params.get('LateralControlMethod')) == 0:
if candidate == CAR.SANTAFE:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1830. + STD_CARGO_KG
ret.wheelbase = 2.765
# Values from optimizer
ret.steerRatio = 13.8 # 13.8 is spec end-to-end
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.SORENTO:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1950. + STD_CARGO_KG
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.15
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.GENESIS:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 2060. + STD_CARGO_KG
ret.wheelbase = 3.01
ret.steerRatio = 16.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16],
[0.01]]
elif candidate in [CAR.K5, CAR.SONATA]:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1470. + STD_CARGO_KG
ret.wheelbase = 2.80
ret.steerRatio = 12.75
ret.steerRateCost = 0.4
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.SONATA_TURBO:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1565. + STD_CARGO_KG
ret.wheelbase = 2.80
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate in [CAR.K5_HEV, CAR.SONATA_HEV]:
ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1595. + STD_CARGO_KG
ret.wheelbase = 2.80
ret.steerRatio = 12.75
ret.steerRateCost = 0.4
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate in [CAR.GRANDEUR, CAR.K7]:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1570. + STD_CARGO_KG
ret.wheelbase = 2.885
ret.steerRatio = 12.5
ret.steerRateCost = 0.4
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate in [CAR.GRANDEUR_HEV, CAR.K7_HEV]:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1675. + STD_CARGO_KG
ret.wheelbase = 2.885
ret.steerRatio = 12.5
ret.steerRateCost = 0.4
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.STINGER:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1825. + STD_CARGO_KG
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.KONA:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1330. + STD_CARGO_KG
ret.wheelbase = 2.6
ret.steerRatio = 13.5 #Spec
ret.steerRateCost = 0.4
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.KONA_HEV:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1330. + STD_CARGO_KG
ret.wheelbase = 2.6
ret.steerRatio = 13.5 #Spec
ret.steerRateCost = 0.4
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.KONA_EV:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1330. + STD_CARGO_KG
ret.wheelbase = 2.6
ret.steerRatio = 13.5 #Spec
ret.steerRateCost = 0.4
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.NIRO_HEV:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1425. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73 #Spec
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.NIRO_EV:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1425. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73 #Spec
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.IONIQ_HEV:
ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1275. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73 #Spec
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.IONIQ_EV:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1490. + STD_CARGO_KG #weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx
ret.wheelbase = 2.7
ret.steerRatio = 13.25 #Spec
ret.steerRateCost = 0.4
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.NEXO:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1885. + STD_CARGO_KG
ret.wheelbase = 2.79
ret.steerRatio = 12.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.MOHAVE:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 2250. + STD_CARGO_KG
ret.wheelbase = 2.895
ret.steerRatio = 14.1
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.I30:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1380. + STD_CARGO_KG
ret.wheelbase = 2.65
ret.steerRatio = 13.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.AVANTE:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1275. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.5 # 14 is Stock | Settled Params Learner values are steerRatio: 15.401566348670535
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.SELTOS:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1470. + STD_CARGO_KG
ret.wheelbase = 2.63
ret.steerRatio = 13.0
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.PALISADE:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1955. + STD_CARGO_KG
ret.wheelbase = 2.90
ret.steerRatio = 13.0
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif int(params.get('LateralControlMethod')) == 1:
if candidate == CAR.SANTAFE:
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 3.0
ret.lateralTuning.indi.outerLoopGain = 2.0
ret.lateralTuning.indi.timeConstant = 1.0
ret.lateralTuning.indi.actuatorEffectiveness = 1.5
ret.mass = 1830. + STD_CARGO_KG
ret.wheelbase = 2.765
ret.steerRatio = 13.8 # 13.8 is spec end-to-end
elif candidate == CAR.SORENTO:
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 3.0
ret.lateralTuning.indi.outerLoopGain = 2.0
ret.lateralTuning.indi.timeConstant = 1.0
ret.lateralTuning.indi.actuatorEffectiveness = 1.5
ret.mass = 1950. + STD_CARGO_KG
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.15
elif candidate == CAR.GENESIS:
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 3.0
ret.lateralTuning.indi.outerLoopGain = 2.0
ret.lateralTuning.indi.timeConstant = 1.0
ret.lateralTuning.indi.actuatorEffectiveness = 1.5
ret.mass = 2060. + STD_CARGO_KG
ret.wheelbase = 3.01
ret.steerRatio = 16.5
elif candidate in [CAR.K5, CAR.SONATA]:
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 3.0
ret.lateralTuning.indi.outerLoopGain = 2.0
ret.lateralTuning.indi.timeConstant = 1.0
ret.lateralTuning.indi.actuatorEffectiveness = 1.5
ret.mass = 1470. + STD_CARGO_KG
ret.wheelbase = 2.80
ret.steerRatio = 12.75
ret.steerRateCost = 0.4
elif candidate == CAR.SONATA_TURBO:
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 3.0
ret.lateralTuning.indi.outerLoopGain = 2.0
ret.lateralTuning.indi.timeConstant = 1.0
ret.lateralTuning.indi.actuatorEffectiveness = 1.5
ret.mass = 1565. + STD_CARGO_KG
ret.wheelbase = 2.80
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
elif candidate in [CAR.K5_HEV, CAR.SONATA_HEV]:
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 3.0
ret.lateralTuning.indi.outerLoopGain = 2.0
ret.lateralTuning.indi.timeConstant = 1.0
ret.lateralTuning.indi.actuatorEffectiveness = 1.5
ret.mass = 1595. + STD_CARGO_KG
ret.wheelbase = 2.80
ret.steerRatio = 12.75
ret.steerRateCost = 0.4
elif candidate in [CAR.GRANDEUR, CAR.K7]:
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 3.0
ret.lateralTuning.indi.outerLoopGain = 2.0
ret.lateralTuning.indi.timeConstant = 1.0
ret.lateralTuning.indi.actuatorEffectiveness = 1.5
ret.mass = 1570. + STD_CARGO_KG
ret.wheelbase = 2.885
ret.steerRatio = 12.5
ret.steerRateCost = 0.4
elif candidate in [CAR.GRANDEUR_HEV, CAR.K7_HEV]:
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 3.0
ret.lateralTuning.indi.outerLoopGain = 2.0
ret.lateralTuning.indi.timeConstant = 1.0
ret.lateralTuning.indi.actuatorEffectiveness = 1.5
ret.mass = 1675. + STD_CARGO_KG
ret.wheelbase = 2.885
ret.steerRatio = 12.5
ret.steerRateCost = 0.4
elif candidate == CAR.STINGER:
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 3.0
ret.lateralTuning.indi.outerLoopGain = 2.0
ret.lateralTuning.indi.timeConstant = 1.0
ret.lateralTuning.indi.actuatorEffectiveness = 1.5
ret.mass = 1825. + STD_CARGO_KG
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
elif candidate == CAR.KONA:
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 3.0
ret.lateralTuning.indi.outerLoopGain = 2.0
ret.lateralTuning.indi.timeConstant = 1.0
ret.lateralTuning.indi.actuatorEffectiveness = 1.5
ret.mass = 1330. + STD_CARGO_KG
ret.wheelbase = 2.6
ret.steerRatio = 13.5 #Spec
ret.steerRateCost = 0.4
elif candidate == CAR.KONA_HEV:
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 3.0
ret.lateralTuning.indi.outerLoopGain = 2.0
ret.lateralTuning.indi.timeConstant = 1.0
ret.lateralTuning.indi.actuatorEffectiveness = 1.5
ret.mass = 1330. + STD_CARGO_KG
ret.wheelbase = 2.6
ret.steerRatio = 13.5 #Spec
ret.steerRateCost = 0.4
elif candidate == CAR.KONA_EV:
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 3.0
ret.lateralTuning.indi.outerLoopGain = 2.0
ret.lateralTuning.indi.timeConstant = 1.0
ret.lateralTuning.indi.actuatorEffectiveness = 1.5
ret.mass = 1330. + STD_CARGO_KG
ret.wheelbase = 2.6
ret.steerRatio = 13.5 #Spec
ret.steerRateCost = 0.4
elif candidate == CAR.NIRO_HEV:
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 3.0
ret.lateralTuning.indi.outerLoopGain = 2.0
ret.lateralTuning.indi.timeConstant = 1.0
ret.lateralTuning.indi.actuatorEffectiveness = 1.5
ret.mass = 1425. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73 #Spec
elif candidate == CAR.NIRO_EV:
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 3.0
ret.lateralTuning.indi.outerLoopGain = 2.0
ret.lateralTuning.indi.timeConstant = 1.0
ret.lateralTuning.indi.actuatorEffectiveness = 1.5
ret.mass = 1425. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73 #Spec
elif candidate == CAR.IONIQ_HEV:
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 3.0
ret.lateralTuning.indi.outerLoopGain = 2.0
ret.lateralTuning.indi.timeConstant = 1.0
ret.lateralTuning.indi.actuatorEffectiveness = 1.5
ret.mass = 1275. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73 #Spec
elif candidate == CAR.IONIQ_EV:
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 3.0
ret.lateralTuning.indi.outerLoopGain = 2.0
ret.lateralTuning.indi.timeConstant = 1.0
ret.lateralTuning.indi.actuatorEffectiveness = 1.5
ret.mass = 1490. + STD_CARGO_KG #weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx
ret.wheelbase = 2.7
ret.steerRatio = 13.25 #Spec
ret.steerRateCost = 0.4
elif candidate == CAR.NEXO:
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 3.0
ret.lateralTuning.indi.outerLoopGain = 2.0
ret.lateralTuning.indi.timeConstant = 1.0
ret.lateralTuning.indi.actuatorEffectiveness = 1.5
ret.mass = 1885. + STD_CARGO_KG
ret.wheelbase = 2.79
ret.steerRatio = 12.5
elif candidate == CAR.MOHAVE:
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 3.0
ret.lateralTuning.indi.outerLoopGain = 2.0
ret.lateralTuning.indi.timeConstant = 1.0
ret.lateralTuning.indi.actuatorEffectiveness = 1.5
ret.mass = 2250. + STD_CARGO_KG
ret.wheelbase = 2.895
ret.steerRatio = 14.1
elif candidate == CAR.I30:
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 3.0
ret.lateralTuning.indi.outerLoopGain = 2.0
ret.lateralTuning.indi.timeConstant = 1.0
ret.lateralTuning.indi.actuatorEffectiveness = 1.5
ret.mass = 1380. + STD_CARGO_KG
ret.wheelbase = 2.65
ret.steerRatio = 13.5
elif candidate == CAR.AVANTE:
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 3.0
ret.lateralTuning.indi.outerLoopGain = 2.0
ret.lateralTuning.indi.timeConstant = 1.0
ret.lateralTuning.indi.actuatorEffectiveness = 1.5
ret.mass = 1275. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.5 # 14 is Stock | Settled Params Learner values are steerRatio: 15.401566348670535
elif candidate == CAR.SELTOS:
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 3.0
ret.lateralTuning.indi.outerLoopGain = 2.0
ret.lateralTuning.indi.timeConstant = 1.0
ret.lateralTuning.indi.actuatorEffectiveness = 1.5
ret.mass = 1470. + STD_CARGO_KG
ret.wheelbase = 2.63
ret.steerRatio = 13.0
elif candidate == CAR.PALISADE:
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 3.0
ret.lateralTuning.indi.outerLoopGain = 2.0
ret.lateralTuning.indi.timeConstant = 1.0
ret.lateralTuning.indi.actuatorEffectiveness = 1.5
ret.mass = 1955. + STD_CARGO_KG
ret.wheelbase = 2.90
ret.steerRatio = 13.0
elif int(params.get('LateralControlMethod')) == 2:
if candidate == CAR.SANTAFE:
ret.lateralTuning.init('lqr')
ret.lateralTuning.lqr.scale = 1750.0
ret.lateralTuning.lqr.ki = 0.03
ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
ret.lateralTuning.lqr.c = [1., 0.]
ret.lateralTuning.lqr.k = [-100., 450.]
ret.lateralTuning.lqr.l = [0.22, 0.318]
ret.lateralTuning.lqr.dcGain = 0.003
ret.mass = 1830. + STD_CARGO_KG
ret.wheelbase = 2.765
ret.steerRatio = 13.8 # 13.8 is spec end-to-end
elif candidate == CAR.SORENTO:
ret.lateralTuning.init('lqr')
ret.lateralTuning.lqr.scale = 1750.0
ret.lateralTuning.lqr.ki = 0.03
ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
ret.lateralTuning.lqr.c = [1., 0.]
ret.lateralTuning.lqr.k = [-100., 450.]
ret.lateralTuning.lqr.l = [0.22, 0.318]
ret.lateralTuning.lqr.dcGain = 0.003
ret.mass = 1950. + STD_CARGO_KG
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.15
elif candidate == CAR.GENESIS:
ret.lateralTuning.init('lqr')
ret.lateralTuning.lqr.scale = 1750.0
ret.lateralTuning.lqr.ki = 0.03
ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
ret.lateralTuning.lqr.c = [1., 0.]
ret.lateralTuning.lqr.k = [-100., 450.]
ret.lateralTuning.lqr.l = [0.22, 0.318]
ret.lateralTuning.lqr.dcGain = 0.003
ret.mass = 2060. + STD_CARGO_KG
ret.wheelbase = 3.01
ret.steerRatio = 16.5
elif candidate in [CAR.K5, CAR.SONATA]:
ret.lateralTuning.init('lqr')
ret.lateralTuning.lqr.scale = 1750.0
ret.lateralTuning.lqr.ki = 0.03
ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
ret.lateralTuning.lqr.c = [1., 0.]
ret.lateralTuning.lqr.k = [-100., 450.]
ret.lateralTuning.lqr.l = [0.22, 0.318]
ret.lateralTuning.lqr.dcGain = 0.003
ret.mass = 1470. + STD_CARGO_KG
ret.wheelbase = 2.80
ret.steerRatio = 12.75
ret.steerRateCost = 0.4
elif candidate == CAR.SONATA_TURBO:
ret.lateralTuning.init('lqr')
ret.lateralTuning.lqr.scale = 1750.0
ret.lateralTuning.lqr.ki = 0.03
ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
ret.lateralTuning.lqr.c = [1., 0.]
ret.lateralTuning.lqr.k = [-100., 450.]
ret.lateralTuning.lqr.l = [0.22, 0.318]
ret.lateralTuning.lqr.dcGain = 0.003
ret.mass = 1565. + STD_CARGO_KG
ret.wheelbase = 2.80
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
elif candidate in [CAR.K5_HEV, CAR.SONATA_HEV]:
ret.lateralTuning.init('lqr')
ret.lateralTuning.lqr.scale = 1750.0
ret.lateralTuning.lqr.ki = 0.03
ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
ret.lateralTuning.lqr.c = [1., 0.]
ret.lateralTuning.lqr.k = [-100., 450.]
ret.lateralTuning.lqr.l = [0.22, 0.318]
ret.lateralTuning.lqr.dcGain = 0.003
ret.mass = 1595. + STD_CARGO_KG
ret.wheelbase = 2.80
ret.steerRatio = 12.75
ret.steerRateCost = 0.4
elif candidate in [CAR.GRANDEUR, CAR.K7]:
ret.lateralTuning.init('lqr')
ret.lateralTuning.lqr.scale = 1750.0
ret.lateralTuning.lqr.ki = 0.03
ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
ret.lateralTuning.lqr.c = [1., 0.]
ret.lateralTuning.lqr.k = [-100., 450.]
ret.lateralTuning.lqr.l = [0.22, 0.318]
ret.lateralTuning.lqr.dcGain = 0.003
ret.mass = 1570. + STD_CARGO_KG
ret.wheelbase = 2.885
ret.steerRatio = 12.5
ret.steerRateCost = 0.4
elif candidate in [CAR.GRANDEUR_HEV, CAR.K7_HEV]:
ret.lateralTuning.init('lqr')
ret.lateralTuning.lqr.scale = 1750.0
ret.lateralTuning.lqr.ki = 0.03
ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
ret.lateralTuning.lqr.c = [1., 0.]
ret.lateralTuning.lqr.k = [-100., 450.]
ret.lateralTuning.lqr.l = [0.22, 0.318]
ret.lateralTuning.lqr.dcGain = 0.003
ret.mass = 1675. + STD_CARGO_KG
ret.wheelbase = 2.885
ret.steerRatio = 12.5
ret.steerRateCost = 0.4
elif candidate == CAR.STINGER:
ret.lateralTuning.init('lqr')
ret.lateralTuning.lqr.scale = 1750.0
ret.lateralTuning.lqr.ki = 0.03
ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
ret.lateralTuning.lqr.c = [1., 0.]
ret.lateralTuning.lqr.k = [-100., 450.]
ret.lateralTuning.lqr.l = [0.22, 0.318]
ret.lateralTuning.lqr.dcGain = 0.003
ret.mass = 1825. + STD_CARGO_KG
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
elif candidate == CAR.KONA:
ret.lateralTuning.init('lqr')
ret.lateralTuning.lqr.scale = 1750.0
ret.lateralTuning.lqr.ki = 0.03
ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
ret.lateralTuning.lqr.c = [1., 0.]
ret.lateralTuning.lqr.k = [-100., 450.]
ret.lateralTuning.lqr.l = [0.22, 0.318]
ret.lateralTuning.lqr.dcGain = 0.003
ret.mass = 1330. + STD_CARGO_KG
ret.wheelbase = 2.6
ret.steerRatio = 13.5 #Spec
ret.steerRateCost = 0.4
elif candidate == CAR.KONA_HEV:
ret.lateralTuning.init('lqr')
ret.lateralTuning.lqr.scale = 1750.0
ret.lateralTuning.lqr.ki = 0.03
ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
ret.lateralTuning.lqr.c = [1., 0.]
ret.lateralTuning.lqr.k = [-100., 450.]
ret.lateralTuning.lqr.l = [0.22, 0.318]
ret.lateralTuning.lqr.dcGain = 0.003
ret.mass = 1330. + STD_CARGO_KG
ret.wheelbase = 2.6
ret.steerRatio = 13.5 #Spec
ret.steerRateCost = 0.4
elif candidate == CAR.KONA_EV:
ret.lateralTuning.init('lqr')
ret.lateralTuning.lqr.scale = 1750.0
ret.lateralTuning.lqr.ki = 0.03
ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
ret.lateralTuning.lqr.c = [1., 0.]
ret.lateralTuning.lqr.k = [-100., 450.]
ret.lateralTuning.lqr.l = [0.22, 0.318]
ret.lateralTuning.lqr.dcGain = 0.003
ret.mass = 1330. + STD_CARGO_KG
ret.wheelbase = 2.6
ret.steerRatio = 13.5 #Spec
ret.steerRateCost = 0.4
elif candidate == CAR.NIRO_HEV:
ret.lateralTuning.init('lqr')
ret.lateralTuning.lqr.scale = 1750.0
ret.lateralTuning.lqr.ki = 0.03
ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
ret.lateralTuning.lqr.c = [1., 0.]
ret.lateralTuning.lqr.k = [-100., 450.]
ret.lateralTuning.lqr.l = [0.22, 0.318]
ret.lateralTuning.lqr.dcGain = 0.003
ret.mass = 1425. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73 #Spec
elif candidate == CAR.NIRO_EV:
ret.lateralTuning.init('lqr')
ret.lateralTuning.lqr.scale = 1750.0
ret.lateralTuning.lqr.ki = 0.03
ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
ret.lateralTuning.lqr.c = [1., 0.]
ret.lateralTuning.lqr.k = [-100., 450.]
ret.lateralTuning.lqr.l = [0.22, 0.318]
ret.lateralTuning.lqr.dcGain = 0.003
ret.mass = 1425. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73 #Spec
elif candidate == CAR.IONIQ_HEV:
ret.lateralTuning.init('lqr')
ret.lateralTuning.lqr.scale = 1750.0
ret.lateralTuning.lqr.ki = 0.03
ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
ret.lateralTuning.lqr.c = [1., 0.]
ret.lateralTuning.lqr.k = [-100., 450.]
ret.lateralTuning.lqr.l = [0.22, 0.318]
ret.lateralTuning.lqr.dcGain = 0.003
ret.mass = 1275. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73 #Spec
elif candidate == CAR.IONIQ_EV:
ret.lateralTuning.init('lqr')
ret.lateralTuning.lqr.scale = 1750.0
ret.lateralTuning.lqr.ki = 0.03
ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
ret.lateralTuning.lqr.c = [1., 0.]
ret.lateralTuning.lqr.k = [-100., 450.]
ret.lateralTuning.lqr.l = [0.22, 0.318]
ret.lateralTuning.lqr.dcGain = 0.003
ret.mass = 1490. + STD_CARGO_KG #weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx
ret.wheelbase = 2.7
ret.steerRatio = 13.25 #Spec
ret.steerRateCost = 0.4
elif candidate == CAR.NEXO:
ret.lateralTuning.init('lqr')
ret.lateralTuning.lqr.scale = 1750.0
ret.lateralTuning.lqr.ki = 0.03
ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
ret.lateralTuning.lqr.c = [1., 0.]
ret.lateralTuning.lqr.k = [-100., 450.]
ret.lateralTuning.lqr.l = [0.22, 0.318]
ret.lateralTuning.lqr.dcGain = 0.003
ret.mass = 1885. + STD_CARGO_KG
ret.wheelbase = 2.79
ret.steerRatio = 12.5
elif candidate == CAR.MOHAVE:
ret.lateralTuning.init('lqr')
ret.lateralTuning.lqr.scale = 1750.0
ret.lateralTuning.lqr.ki = 0.03
ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
ret.lateralTuning.lqr.c = [1., 0.]
ret.lateralTuning.lqr.k = [-100., 450.]
ret.lateralTuning.lqr.l = [0.22, 0.318]
ret.lateralTuning.lqr.dcGain = 0.003
ret.mass = 2250. + STD_CARGO_KG
ret.wheelbase = 2.895
ret.steerRatio = 14.1
elif candidate == CAR.I30:
ret.lateralTuning.init('lqr')
ret.lateralTuning.lqr.scale = 1750.0
ret.lateralTuning.lqr.ki = 0.03
ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
ret.lateralTuning.lqr.c = [1., 0.]
ret.lateralTuning.lqr.k = [-100., 450.]
ret.lateralTuning.lqr.l = [0.22, 0.318]
ret.lateralTuning.lqr.dcGain = 0.003
ret.mass = 1380. + STD_CARGO_KG
ret.wheelbase = 2.65
ret.steerRatio = 13.5
elif candidate == CAR.AVANTE:
ret.lateralTuning.init('lqr')
ret.lateralTuning.lqr.scale = 1750.0
ret.lateralTuning.lqr.ki = 0.03
ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
ret.lateralTuning.lqr.c = [1., 0.]
ret.lateralTuning.lqr.k = [-100., 450.]
ret.lateralTuning.lqr.l = [0.22, 0.318]
ret.lateralTuning.lqr.dcGain = 0.003
ret.mass = 1275. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.5 # 14 is Stock | Settled Params Learner values are steerRatio: 15.401566348670535
elif candidate == CAR.SELTOS:
ret.lateralTuning.init('lqr')
ret.lateralTuning.lqr.scale = 1750.0
ret.lateralTuning.lqr.ki = 0.03
ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
ret.lateralTuning.lqr.c = [1., 0.]
ret.lateralTuning.lqr.k = [-100., 450.]
ret.lateralTuning.lqr.l = [0.22, 0.318]
ret.lateralTuning.lqr.dcGain = 0.003
ret.mass = 1470. + STD_CARGO_KG
ret.wheelbase = 2.63
ret.steerRatio = 13.0
elif candidate == CAR.PALISADE:
ret.lateralTuning.init('lqr')
ret.lateralTuning.lqr.scale = 1750.0
ret.lateralTuning.lqr.ki = 0.03
ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
ret.lateralTuning.lqr.c = [1., 0.]
ret.lateralTuning.lqr.k = [-100., 450.]
ret.lateralTuning.lqr.l = [0.22, 0.318]
ret.lateralTuning.lqr.dcGain = 0.003
ret.mass = 1955. + STD_CARGO_KG
ret.wheelbase = 2.90
ret.steerRatio = 13.0
# these cars require a special panda safety mode due to missing counters and checksums in the messages
if candidate in [
CAR.GENESIS, CAR.IONIQ_EV, CAR.IONIQ_HEV, CAR.KONA_EV
]:
ret.safetyModel = car.CarParams.SafetyModel.hyundaiLegacy
ret.centerToFront = ret.wheelbase * 0.4
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(
ret.mass,
ret.wheelbase,
ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS,
ECU_FINGERPRINT, candidate,
Ecu.fwdCamera) or has_relay
# ignore CAN2 address if L-CAN on the same BUS
ret.mdpsBus = 1 if 593 in fingerprint[1] and 1296 not in fingerprint[
1] else 0 # MDPS12
ret.sasBus = 1 if 688 in fingerprint[1] and 1296 not in fingerprint[
1] else 0 # SAS11
ret.sccBus = 0 if 1056 in fingerprint[
0] else 1 if 1056 in fingerprint[1] and 1296 not in fingerprint[
1] else 2 if 1056 in fingerprint[2] else -1 # SCC11
return ret