class LatControlPID():
def __init__(self, CP):
self.pid = LatPIDController(
(CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
(CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV),
k_f=CP.lateralTuning.pid.kf,
pos_limit=1.0,
sat_limit=CP.steerLimitTimer)
self.new_kf_tuned = CP.lateralTuning.pid.newKfTuned
self.angle_steers_des = 0.
def reset(self):
self.pid.reset()
def update(self, active, CS, CP, lat_plan):
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
if CS.vEgo < 0.3 or not active:
output_steer = 0.0
pid_log.active = False
self.pid.reset()
else:
self.angle_steers_des = lat_plan.steeringAngleDeg # get from MPC/LateralPlanner
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 lat_plan.rateSteers
steer_feedforward -= lat_plan.angleOffsetDeg # subtract the offset, since it does not contribute to resistive torque
if self.new_kf_tuned:
_c1, _c2, _c3 = 0.35189607550172824, 7.506201251644202, 69.226826411091
steer_feedforward *= _c1 * CS.vEgo**2 + _c2 * CS.vEgo + _c3
else:
steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel)
deadzone = 0.0
check_saturation = (
CS.vEgo >
10) and not CS.steeringRateLimited and not CS.steeringPressed
output_steer = self.pid.update(self.angle_steers_des,
CS.steeringAngleDeg,
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
def __init__(self, CP):
self.pid = LatPIDController(
(CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
(CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV),
k_f=CP.lateralTuning.pid.kf,
pos_limit=1.0,
sat_limit=CP.steerLimitTimer)
self.angle_steers_des = 0.
def __init__(self, CP):
self.pid = LatPIDController(
(CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
(CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV),
k_f=CP.lateralTuning.pid.kf,
pos_limit=1.0,
sat_limit=CP.steerLimitTimer)
self.new_kf_tuned = CP.lateralTuning.pid.newKfTuned
self.angle_steers_des = 0.
self.mpc_frame = 0
self.params = Params()
def __init__(self, CP):
self.kegman = kegman_conf(CP)
self.deadzone = float(self.kegman.conf['deadzone'])
self.pid = LatPIDController(
(CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
(CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV),
k_f=CP.lateralTuning.pid.kf,
pos_limit=1.0,
neg_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:
self.steerKpV = float(int(self.params.get('PidKp')) * 0.01)
self.steerKiV = float(int(self.params.get('PidKi')) * 0.001)
self.steerKdV = float(int(self.params.get('PidKd')) * 0.01)
self.steerKf = float(int(self.params.get('PidKf')) * 0.00001)
self.pid = LatPIDController(
(CP.lateralTuning.pid.kpBP, [0.1, self.steerKpV]),
(CP.lateralTuning.pid.kiBP, [0.01, self.steerKiV]),
(CP.lateralTuning.pid.kdBP, [self.steerKdV]),
k_f=self.steerKf,
pos_limit=1.0)
self.mpc_frame = 0
def __init__(self, CP):
self.pid = LatPIDController(
(CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
(CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV),
k_f=CP.lateralTuning.pid.kf,
pos_limit=1.0,
neg_limit=-1.0,
sat_limit=CP.steerLimitTimer)
self.angle_steers_des = 0.
self.angle_steers_des_last = 0.
self.angle_steer_rate = [0.6, 1.0]
self.angleBP = [10., 25.]
self.angle_steer_new = 0.0
class LatControlPID():
def __init__(self, CP):
self.pid = LatPIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
(CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV),
k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0,
sat_limit=CP.steerLimitTimer)
def reset(self):
self.pid.reset()
def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvature_rate):
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo))
angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
if CS.vEgo < 0.3 or not active or not CS.lkasEnable:
output_steer = 0.0
pid_log.active = False
self.pid.reset()
else:
steers_max = get_steer_max(CP, CS.vEgo)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
# TODO: feedforward something based on lat_plan.rateSteers
steer_feedforward = angle_steers_des_no_offset # offset does not contribute to resistive torque
steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel)
#_c1, _c2, _c3 = [0.35189607550172824, 7.506201251644202, 69.226826411091]
#steer_feedforward *= _c1 * CS.vEgo ** 2 + _c2 * CS.vEgo + _c3
deadzone = 0.0
check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
output_steer = self.pid.update(angle_steers_des, CS.steeringAngleDeg, 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, angle_steers_des, pid_log
def __init__(self, CP):
self.pid = LatPIDController(
(CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
(CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV),
k_f=CP.lateralTuning.pid.kf,
pos_limit=1.0,
neg_limit=-1.0,
sat_limit=CP.steerLimitTimer)
self.new_kf_tuned = CP.lateralTuning.pid.newKfTuned
self.mpc_frame = 0
self.params = Params()
self.live_tune_enabled = self.params.get_bool("OpkrLiveTune")
self.dead_zone = float(
Decimal(self.params.get("IgnoreZone", encoding="utf8")) *
Decimal('0.1'))
def live_tune(self, CP):
self.mpc_frame += 1
if self.mpc_frame % 300 == 0:
self.steerKpV = float(
Decimal(self.params.get("PidKp", encoding="utf8")) *
Decimal('0.01'))
self.steerKiV = float(
Decimal(self.params.get("PidKi", encoding="utf8")) *
Decimal('0.001'))
self.steerKdV = float(
Decimal(self.params.get("PidKd", encoding="utf8")) *
Decimal('0.01'))
self.steerKf = float(
Decimal(self.params.get("PidKf", encoding="utf8")) *
Decimal('0.00001'))
self.pid = LatPIDController(
(CP.lateralTuning.pid.kpBP, [0.1, self.steerKpV]),
(CP.lateralTuning.pid.kiBP, [0.01, self.steerKiV]),
(CP.lateralTuning.pid.kdBP, [self.steerKdV]),
k_f=self.steerKf,
pos_limit=1.0)
self.mpc_frame = 0
class LatControlPID():
def __init__(self, CP):
self.kegman = kegman_conf(CP)
self.deadzone = float(self.kegman.conf['deadzone'])
self.pid = LatPIDController(
(CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
(CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV),
k_f=CP.lateralTuning.pid.kf,
pos_limit=1.0,
neg_limit=-1.0,
sat_limit=CP.steerLimitTimer)
self.angle_steers_des = 0.
self.angle_steers_des_last = 0.
self.mpc_frame = 0
def reset(self):
self.pid.reset()
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.kegman = kegman_conf()
if self.kegman.conf['tuneGernby'] == "1":
#self.steerKpV = [float(self.kegman.conf['Kp'])]
#self.steerKiV = [float(self.kegman.conf['Ki'])]
#self.steerKf = float(self.kegman.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.kegman.conf['deadzone'])
self.mpc_frame = 0
def update(self, active, CS, CP, path_plan):
self.live_tune(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.pid.reset()
else:
self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner
check_pingpong = abs(self.angle_steers_des -
self.angle_steers_des_last) > 3.0
if CS.vEgo < 10.0 and check_pingpong:
self.angle_steers_des = path_plan.angleSteers * 0.3
elif CS.vEgo < 15.0 and check_pingpong:
self.angle_steers_des = path_plan.angleSteers * 0.45
elif CS.vEgo < 20.0 and check_pingpong:
self.angle_steers_des = path_plan.angleSteers * 0.6
elif CS.vEgo < 25.0 and check_pingpong:
self.angle_steers_des = path_plan.angleSteers * 0.8
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
self.angle_steers_des_last = self.angle_steers_des
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
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.pid = LatPIDController(
(CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
(CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV),
k_f=CP.lateralTuning.pid.kf,
pos_limit=1.0,
sat_limit=CP.steerLimitTimer)
self.new_kf_tuned = CP.lateralTuning.pid.newKfTuned
self.angle_steers_des = 0.
self.mpc_frame = 0
self.params = Params()
def reset(self):
self.pid.reset()
# live tune referred to kegman's
def live_tune(self, CP):
self.mpc_frame += 1
if self.mpc_frame % 300 == 0:
self.steerKpV = float(int(self.params.get('PidKp')) * 0.01)
self.steerKiV = float(int(self.params.get('PidKi')) * 0.001)
self.steerKdV = float(int(self.params.get('PidKd')) * 0.01)
self.steerKf = float(int(self.params.get('PidKf')) * 0.00001)
self.pid = LatPIDController(
(CP.lateralTuning.pid.kpBP, [0.1, self.steerKpV]),
(CP.lateralTuning.pid.kiBP, [0.01, self.steerKiV]),
(CP.lateralTuning.pid.kdBP, [self.steerKdV]),
k_f=self.steerKf,
pos_limit=1.0)
self.mpc_frame = 0
def update(self, active, CS, CP, path_plan):
if int(self.params.get('OpkrLiveTune')) == 1:
self.live_tune(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.pid.reset()
else:
self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner
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
if self.new_kf_tuned:
_c1, _c2, _c3 = 0.35189607550172824, 7.506201251644202, 69.226826411091
steer_feedforward *= _c1 * CS.vEgo**2 + _c2 * CS.vEgo + _c3
else:
steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel)
deadzone = float(int(self.params.get('IgnoreZone')) * 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.pid = LatPIDController(
(CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
(CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV),
k_f=CP.lateralTuning.pid.kf,
pos_limit=1.0,
neg_limit=-1.0,
sat_limit=CP.steerLimitTimer)
self.angle_steers_des = 0.
self.angle_steers_des_last = 0.
self.angle_steer_rate = [0.6, 1.0]
self.angleBP = [10., 25.]
self.angle_steer_new = 0.0
def reset(self):
self.pid.reset()
def update(self, active, CS, CP, path_plan):
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.pid.reset()
else:
self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner
self.angle_steer_new = interp(CS.vEgo, self.angleBP,
self.angle_steer_rate)
check_pingpong = abs(self.angle_steers_des -
self.angle_steers_des_last) > 4.0
if check_pingpong:
self.angle_steers_des = path_plan.angleSteers * self.angle_steer_new
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)
_c1, _c2, _c3 = [
0.35189607550172824, 7.506201251644202, 69.226826411091
]
steer_feedforward *= _c1 * CS.vEgo**2 + _c2 * CS.vEgo + _c3
deadzone = 0.0
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)
self.angle_steers_des_last = self.angle_steers_des
return output_steer, float(self.angle_steers_des), pid_log
class LatControlPID():
def __init__(self, CP):
self.pid = LatPIDController(
(CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
(CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV),
k_f=CP.lateralTuning.pid.kf,
pos_limit=1.0,
neg_limit=-1.0,
sat_limit=CP.steerLimitTimer)
self.new_kf_tuned = CP.lateralTuning.pid.newKfTuned
self.mpc_frame = 0
self.params = Params()
self.live_tune_enabled = self.params.get_bool("OpkrLiveTune")
self.dead_zone = float(
Decimal(self.params.get("IgnoreZone", encoding="utf8")) *
Decimal('0.1'))
def reset(self):
self.pid.reset()
# live tune referred to kegman's
def live_tune(self, CP):
self.mpc_frame += 1
if self.mpc_frame % 300 == 0:
self.steerKpV = float(
Decimal(self.params.get("PidKp", encoding="utf8")) *
Decimal('0.01'))
self.steerKiV = float(
Decimal(self.params.get("PidKi", encoding="utf8")) *
Decimal('0.001'))
self.steerKdV = float(
Decimal(self.params.get("PidKd", encoding="utf8")) *
Decimal('0.01'))
self.steerKf = float(
Decimal(self.params.get("PidKf", encoding="utf8")) *
Decimal('0.00001'))
self.pid = LatPIDController(
(CP.lateralTuning.pid.kpBP, [0.1, self.steerKpV]),
(CP.lateralTuning.pid.kiBP, [0.01, self.steerKiV]),
(CP.lateralTuning.pid.kdBP, [self.steerKdV]),
k_f=self.steerKf,
pos_limit=1.0)
self.mpc_frame = 0
def update(self, active, CS, CP, VM, params, desired_curvature,
desired_curvature_rate):
if self.live_tune_enabled:
self.live_tune(CP)
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
angle_steers_des_no_offset = math.degrees(
VM.get_steer_from_curvature(-desired_curvature, CS.vEgo))
angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
if CS.vEgo < 0.3 or not active:
output_steer = 0.0
pid_log.active = False
self.pid.reset()
else:
steers_max = get_steer_max(CP, CS.vEgo)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
# TODO: feedforward something based on lat_plan.rateSteers
steer_feedforward = angle_steers_des_no_offset # offset does not contribute to resistive torque
if self.new_kf_tuned:
_c1, _c2, _c3 = 0.35189607550172824, 7.506201251644202, 69.226826411091
steer_feedforward *= _c1 * CS.vEgo**2 + _c2 * CS.vEgo + _c3
else:
steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel)
deadzone = self.dead_zone
check_saturation = (
CS.vEgo >
10) and not CS.steeringRateLimited and not CS.steeringPressed
output_steer = self.pid.update(angle_steers_des,
CS.steeringAngleDeg,
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, angle_steers_des, pid_log