class LatControlPID():
def __init__(self, CP):
self.trPID = trace1.Loger("pid")
self.angle_steers_des = 0.
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)
def reset(self):
self.pid.reset()
def atom_tune(self, v_ego_kph, sr_value, CP): # 조향각에 따른 변화.
self.sr_KPH = CP.atomTuning.sRKPH
self.sr_BPV = CP.atomTuning.sRBPV
self.sR_pid_KiV = CP.atomTuning.sRpidKiV
self.sR_pid_KpV = CP.atomTuning.sRpidKpV
self.Ki = []
self.Kp = []
self.Ms = []
nPos = 0
for angle in self.sr_BPV: # angle
self.Ki.append(interp(sr_value, angle, self.sR_pid_KiV[nPos]))
self.Kp.append(interp(sr_value, angle, self.sR_pid_KpV[nPos]))
nPos += 1
if nPos > 10:
break
for kph in self.sr_KPH:
self.Ms.append(kph * CV.KPH_TO_MS)
#rt_Ki = interp( v_ego_kph, self.sr_KPH, self.Ki )
#rt_Kp = interp( v_ego_kph, self.sr_KPH, self.Kp )
return self.Ms, self.Ki, self.Kp
def linear2_tune(self, CS, CP): # angle(조향각에 의한 변화)
v_ego_kph = CS.vEgo * CV.MS_TO_KPH
sr_value = self.angle_steers_des
MsV, KiV, KpV = self.atom_tune(v_ego_kph, sr_value, CP)
self.pid.gain((MsV, KpV), (MsV, KiV), k_f=self.steerKf)
def update(self, active, CS, CP, path_plan):
self.angle_steers_des = path_plan.angleSteers
self.linear2_tune(CS, CP)
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steerAngle = float(CS.steeringAngle)
pid_log.steerRate = float(CS.steeringRate)
if CS.vEgo < 0.3 or not active:
output_steer = 0.0
pid_log.active = False
self.reset()
else:
steers_max = get_steer_max(CP, CS.vEgo)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
steer_feedforward = self.angle_steers_des # feedforward desired angle
if CP.steerControlType == car.CarParams.SteerControlType.torque:
# TODO: feedforward something based on path_plan.rateSteers
steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque
steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel)
deadzone = self.deadzone #0.1
check_saturation = (
CS.vEgo >
10) and not CS.steeringRateLimited and not CS.steeringPressed
output_steer = self.pid.update(self.angle_steers_des,
CS.steeringAngle,
check_saturation=check_saturation,
override=CS.steeringPressed,
feedforward=steer_feedforward,
speed=CS.vEgo,
deadzone=deadzone)
pid_log.active = True
pid_log.p = self.pid.p
pid_log.i = self.pid.i
pid_log.f = self.pid.f
pid_log.output = output_steer
pid_log.saturated = bool(self.pid.saturated)
return output_steer, float(self.angle_steers_des), pid_log
class LatControlPID():
def __init__(self, CP):
self.kegman = kegman_conf(CP)
self.deadzone = float(self.kegman.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 = 500
self.BP0 = 4
self.steer_Kf1 = [0.00003, 0.00003]
self.steer_Ki1 = [0.02, 0.04]
self.steer_Kp1 = [0.18, 0.20]
self.steer_Kf2 = [0.00005, 0.00005]
self.steer_Ki2 = [0.04, 0.05]
self.steer_Kp2 = [0.20, 0.25]
self.pid_change_flag = 0
self.pre_pid_change_flag = 0
self.pid_BP0_time = 0
self.movAvg = moveavg1.MoveAvg()
self.v_curvature = 256
self.model_sum = 0
self.path_x = np.arange(192)
def calc_va(self, sm, v_ego):
md = sm['model']
if len(md.path.poly):
path = list(md.path.poly)
self.l_poly = np.array(md.leftLane.poly)
self.r_poly = np.array(md.rightLane.poly)
self.p_poly = np.array(md.path.poly)
# Curvature of polynomial https://en.wikipedia.org/wiki/Curvature#Curvature_of_the_graph_of_a_function
# y = a x^3 + b x^2 + c x + d, y' = 3 a x^2 + 2 b x + c, y'' = 6 a x + 2 b
# k = y'' / (1 + y'^2)^1.5
# TODO: compute max speed without using a list of points and without numpy
y_p = 3 * path[0] * self.path_x**2 + 2 * path[
1] * self.path_x + path[2]
y_pp = 6 * path[0] * self.path_x + 2 * path[1]
curv = y_pp / (1. + y_p**2)**1.5
#print( 'curv={}'.format( curv ) )
a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None))
model_speed = np.min(v_curvature)
model_speed = max(30.0 * CV.KPH_TO_MS,
model_speed) # Don't slow down below 20mph
model_sum = curv[2] * 1000. #np.sum( curv, 0 )
model_speed = model_speed * CV.MS_TO_KPH
if model_speed > MAX_SPEED:
model_speed = MAX_SPEED
else:
model_speed = MAX_SPEED
model_sum = 0
#following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
#following = CS.lead_distance < 100.0
#accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)]
#jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] # TODO: make a separate lookup for jerk tuning
#accel_limits_turns = limit_accel_in_turns(v_ego, CS.angle_steers, accel_limits, self.steerRatio, self.wheelbase )
model_speed = self.movAvg.get_min(model_speed, 10)
return model_speed, model_sum
def update_state(self, sm, CS):
self.v_curvature, self.model_sum = self.calc_va(sm, CS.vEgo)
def reset(self):
self.pid.reset()
def linear2_tune(self, CP, v_ego): # angle(조향각에 의한 변화)
cv_angle = abs(self.angle_steers_des)
cv = [2, 15] # angle
# Kp
fKp1 = [float(self.steer_Kp1[0]), float(self.steer_Kp1[1])]
fKp2 = [float(self.steer_Kp2[0]), float(self.steer_Kp2[1])]
self.steerKp1 = interp(cv_angle, cv, fKp1)
self.steerKp2 = interp(cv_angle, cv, fKp2)
self.steerKpV = [float(self.steerKp1), float(self.steerKp2)]
# Ki
fKi1 = [float(self.steer_Ki1[0]), float(self.steer_Ki1[1])]
fKi2 = [float(self.steer_Ki2[0]), float(self.steer_Ki2[1])]
self.steerKi1 = interp(cv_angle, cv, fKi1)
self.steerKi2 = interp(cv_angle, cv, fKi2)
self.steerKiV = [float(self.steerKi1), float(self.steerKi2)]
# kf
fKf1 = [float(self.steer_Kf1[0]), float(self.steer_Kf1[1])]
fKf2 = [float(self.steer_Kf2[0]), float(self.steer_Kf2[1])]
self.steerKf1 = interp(cv_angle, cv, fKf1)
self.steerKf2 = interp(cv_angle, cv, fKf2)
xp = CP.lateralTuning.pid.kpBP
fp = [float(self.steerKf1), float(self.steerKf2)]
self.steerKfV = interp(v_ego, xp, fp)
self.pid.gain((CP.lateralTuning.pid.kpBP, self.steerKpV),
(CP.lateralTuning.pid.kiBP, self.steerKiV),
k_f=self.steerKfV)
def linear_tune(self, CP, v_ego): # 곡률에 의한 변화.
cv_value = self.v_curvature
cv = [100, 200] # 곡률
# Kp
fKp1 = [float(self.steer_Kp1[1]), float(self.steer_Kp1[0])]
fKp2 = [float(self.steer_Kp2[1]), float(self.steer_Kp2[0])]
self.steerKp1 = interp(cv_value, cv, fKp1)
self.steerKp2 = interp(cv_value, cv, fKp2)
self.steerKpV = [float(self.steerKp1), float(self.steerKp2)]
# Ki
fKi1 = [float(self.steer_Ki1[1]), float(self.steer_Ki1[0])]
fKi2 = [float(self.steer_Ki2[1]), float(self.steer_Ki2[0])]
self.steerKi1 = interp(cv_value, cv, fKi1)
self.steerKi2 = interp(cv_value, cv, fKi2)
self.steerKiV = [float(self.steerKi1), float(self.steerKi2)]
# kf
fKf1 = [float(self.steer_Kf1[1]), float(self.steer_Kf1[0])]
fKf2 = [float(self.steer_Kf2[1]), float(self.steer_Kf2[0])]
self.steerKf1 = interp(cv_value, cv, fKf1)
self.steerKf2 = interp(cv_value, cv, fKf2)
xp = CP.lateralTuning.pid.kpBP
fp = [float(self.steerKf1), float(self.steerKf2)]
self.steerKfV = interp(v_ego, xp, fp)
self.pid.gain((CP.lateralTuning.pid.kpBP, self.steerKpV),
(CP.lateralTuning.pid.kiBP, self.steerKiV),
k_f=self.steerKfV)
def sR_tune(self, CP, v_ego, path_plan):
kBP0 = 0
if self.pid_change_flag == 0:
pass
elif abs(path_plan.angleSteers) > self.BP0 or self.v_curvature < 200:
kBP0 = 1
self.pid_change_flag = 2
##
self.pid_BP0_time = 300
elif self.pid_BP0_time:
kBP0 = 1
self.pid_BP0_time -= 1
else:
kBP0 = 0
self.pid_change_flag = 3
self.steerKpV = [
float(self.steer_Kp1[kBP0]),
float(self.steer_Kp2[kBP0])
]
self.steerKiV = [
float(self.steer_Ki1[kBP0]),
float(self.steer_Ki2[kBP0])
]
xp = CP.lateralTuning.pid.kpBP
fp = [float(self.steer_Kf1[kBP0]), float(self.steer_Kf2[kBP0])]
self.steerKfV = interp(v_ego, xp, fp)
if self.pid_change_flag != self.pre_pid_change_flag:
self.pre_pid_change_flag = self.pid_change_flag
self.pid.gain((CP.lateralTuning.pid.kpBP, self.steerKpV),
(CP.lateralTuning.pid.kiBP, self.steerKiV),
k_f=self.steerKfV)
#self.pid = PIController((CP.lateralTuning.pid.kpBP, self.steerKpV),
# (CP.lateralTuning.pid.kiBP, self.steerKiV),
# k_f=self.steerKfV, pos_limit=1.0)
def live_tune(self, CP, path_plan, v_ego):
self.mpc_frame += 1
if self.mpc_frame % 600 == 0:
# live tuning through /data/openpilot/tune.py overrides interface.py settings
self.kegman = kegman_conf()
if self.kegman.conf['tuneGernby'] == "1":
self.steerKf = float(self.kegman.conf['Kf'])
self.BP0 = float(self.kegman.conf['sR_BP0'])
self.steer_Kp1 = [
float(self.kegman.conf['Kp']),
float(self.kegman.conf['sR_Kp'])
]
self.steer_Ki1 = [
float(self.kegman.conf['Ki']),
float(self.kegman.conf['sR_Ki'])
]
self.steer_Kf1 = [
float(self.kegman.conf['Kf']),
float(self.kegman.conf['sR_Kf'])
]
self.steer_Kp2 = [
float(self.kegman.conf['Kp2']),
float(self.kegman.conf['sR_Kp2'])
]
self.steer_Ki2 = [
float(self.kegman.conf['Ki2']),
float(self.kegman.conf['sR_Ki2'])
]
self.steer_Kf2 = [
float(self.kegman.conf['Kf2']),
float(self.kegman.conf['sR_Kf2'])
]
self.deadzone = float(self.kegman.conf['deadzone'])
self.mpc_frame = 0
if not self.pid_change_flag:
self.pid_change_flag = 1
self.linear2_tune(CP, v_ego)
#self.linear_tune( CP, v_ego )
#self.sR_tune( CP, v_ego, path_plan )
def update(self, active, v_ego, angle_steers, angle_steers_rate,
eps_torque, steer_override, rate_limited, CP, path_plan):
self.angle_steers_des = path_plan.angleSteers
self.live_tune(CP, path_plan, v_ego)
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steerAngle = float(angle_steers)
pid_log.steerRate = float(angle_steers_rate)
if v_ego < 0.3 or not active:
output_steer = 0.0
pid_log.active = False
#self.angle_steers_des = 0.0
self.pid.reset()
#self.angle_steers_des = path_plan.angleSteers
else:
#self.angle_steers_des = path_plan.angleSteers
steers_max = get_steer_max(CP, v_ego)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
steer_feedforward = self.angle_steers_des # feedforward desired angle
if CP.steerControlType == car.CarParams.SteerControlType.torque:
# TODO: feedforward something based on path_plan.rateSteers
steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque
steer_feedforward *= v_ego**2 # proportional to realigning tire momentum (~ lateral accel)
if abs(self.angle_steers_des) > self.BP0:
deadzone = 0
else:
deadzone = self.deadzone
check_saturation = (v_ego >
10) and not rate_limited and not steer_override
output_steer = self.pid.update(self.angle_steers_des,
angle_steers,
check_saturation=check_saturation,
override=steer_override,
feedforward=steer_feedforward,
speed=v_ego,
deadzone=deadzone)
pid_log.active = True
pid_log.p = self.pid.p
pid_log.i = self.pid.i
pid_log.f = self.pid.f
pid_log.output = output_steer
pid_log.saturated = bool(self.pid.saturated)
return output_steer, float(self.angle_steers_des), pid_log
class LatControlPID():
def __init__(self, CP):
self.trPID = trace1.Loger("pid")
self.angle_steers_des = 0.
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)
def reset(self):
self.pid.reset()
def linear2_tune(self, CS, CP): # angle(조향각에 의한 변화)
v_ego = CS.vEgo
self.sRKPHV = CP.lateralPIDatom.sRKPHV
self.sRkBPV = CP.lateralPIDatom.sRkBPV
self.sRBoostV = CP.lateralPIDatom.sRBoostV
self.sRkpV1 = CP.lateralPIDatom.sRkpV1
self.sRkiV1 = CP.lateralPIDatom.sRkiV1
self.sRkdV1 = CP.lateralPIDatom.sRkdV1
self.sRkfV1 = CP.lateralPIDatom.sRkfV1
self.sRkpV2 = CP.lateralPIDatom.sRkpV2
self.sRkiV2 = CP.lateralPIDatom.sRkiV2
self.sRkdV2 = CP.lateralPIDatom.sRkdV2
self.sRkfV2 = CP.lateralPIDatom.sRkfV2
self.deadzone = CP.lateralsRatom.deadzone
str1 = 'bp={} srBP={} sRBoost={} sRkpV={},{} sRkiV={},{} sRkdV={},{} sRkfV={},{}'.format(
self.sRKPHV, self.sRkBPV, self.sRBoostV, self.sRkpV1, self.sRkpV2,
self.sRkiV1, self.sRkiV2, self.sRkdV1, self.sRkdV2, self.sRkfV1,
self.sRkfV2)
self.trPID.add(str1)
cv_angle = abs(self.angle_steers_des)
cv = self.sRkBPV # angle
# Kp
fKp1 = self.sRkpV1
fKp2 = self.sRkpV2
self.steerKp1 = interp(cv_angle, cv, fKp1)
self.steerKp2 = interp(cv_angle, cv, fKp2)
self.steerKpV = [float(self.steerKp1), float(self.steerKp2)]
# Ki
fKi1 = self.sRkiV1
fKi2 = self.sRkiV2
self.steerKi1 = interp(cv_angle, cv, fKi1)
self.steerKi2 = interp(cv_angle, cv, fKi2)
self.steerKiV = [float(self.steerKi1), float(self.steerKi2)]
# Kd
fKd1 = self.sRkdV1
fKd2 = self.sRkdV2
self.steerKd1 = interp(cv_angle, cv, fKd1)
self.steerKd2 = interp(cv_angle, cv, fKd2)
self.steerKdV = [float(self.steerKd1), float(self.steerKd2)]
# kf
fKf1 = self.sRkfV1
fKf2 = self.sRkfV2
self.steerKf1 = interp(cv_angle, cv, fKf1)
self.steerKf2 = interp(cv_angle, cv, fKf2)
sRKPHV = self.sRKPHV
fp = [float(self.steerKf1), float(self.steerKf2)]
self.steerKf = interp(v_ego, sRKPHV, fp)
self.pid.gain((sRKPHV, self.steerKpV), (sRKPHV, self.steerKiV),
k_f=self.steerKf,
k_d=(sRKPHV, self.steerKdV))
def update(self, active, CS, CP, path_plan):
self.angle_steers_des = path_plan.angleSteers
self.linear2_tune(CS, CP)
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steerAngle = float(CS.steeringAngle)
pid_log.steerRate = float(CS.steeringRate)
if CS.vEgo < 0.3 or not active:
output_steer = 0.0
pid_log.active = False
self.reset()
else:
steers_max = get_steer_max(CP, CS.vEgo)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
steer_feedforward = self.angle_steers_des # feedforward desired angle
if CP.steerControlType == car.CarParams.SteerControlType.torque:
# TODO: feedforward something based on path_plan.rateSteers
steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque
steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel)
deadzone = self.deadzone #0.1
check_saturation = (
CS.vEgo >
10) and not CS.steeringRateLimited and not CS.steeringPressed
output_steer = self.pid.update(self.angle_steers_des,
CS.steeringAngle,
check_saturation=check_saturation,
override=CS.steeringPressed,
feedforward=steer_feedforward,
speed=CS.vEgo,
deadzone=deadzone)
pid_log.active = True
pid_log.p = self.pid.p
pid_log.i = self.pid.i
pid_log.f = self.pid.f
pid_log.output = output_steer
pid_log.saturated = bool(self.pid.saturated)
return output_steer, float(self.angle_steers_des), pid_log
