def __init__(self, CP):
self.pid = PIController((CP.steerKpBP, CP.steerKpV),
(CP.steerKiBP, CP.steerKiV),
k_f=CP.steerKf,
pos_limit=1.0)
self.last_cloudlog_t = 0.0
self.angle_steers_des = 0.
def __init__(self, VM):
self.pid = PIController(VM.CP.steerKp,
VM.CP.steerKi,
k_f=VM.CP.steerKf,
pos_limit=1.0)
self.last_cloudlog_t = 0.0
self.setup_mpc()
class LatControl(object):
def __init__(self, CP):
self.pid = PIController((CP.steerKpBP, CP.steerKpV),
(CP.steerKiBP, CP.steerKiV),
k_f=CP.steerKf, pos_limit=1.0)
self.last_cloudlog_t = 0.0
self.angle_steers_des = 0.
def reset(self):
self.pid.reset()
def update(self, active, v_ego, angle_steers, steer_override, CP, VM, path_plan):
if v_ego < 0.3 or not active:
output_steer = 0.0
self.pid.reset()
else:
# TODO: ideally we should interp, but for tuning reasons we keep the mpc solution
# constant for 0.05s.
#dt = min(cur_time - self.angle_steers_des_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps
#self.angle_steers_des = self.angle_steers_des_prev + (dt / _DT_MPC) * (self.angle_steers_des_mpc - self.angle_steers_des_prev)
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)
deadzone = 0.0
output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override,
feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone)
self.sat_flag = self.pid.saturated
return output_steer, float(self.angle_steers_des)
def __init__(self, CP):
self.pid = PIController((CP.steerKpBP, CP.steerKpV),
(CP.steerKiBP, CP.steerKiV),
k_f=CP.steerKf,
pos_limit=1.0)
self.last_cloudlog_t = 0.0
self.setup_mpc(CP.steerRateCost)
self.smooth_factor = 2.0 * CP.steerActuatorDelay / _DT # Multiplier for inductive component (feed forward)
self.projection_factor = CP.steerActuatorDelay # Mutiplier for reactive component (PI)
self.accel_limit = 2.0 # Desired acceleration limit to prevent "whip steer" (resistive component)
self.ff_angle_factor = 0.5 # Kf multiplier for angle-based feed forward
self.ff_rate_factor = 5.0 # Kf multiplier for rate-based feed forward
self.prev_angle_rate = 0
self.feed_forward = 0.0
self.steerActuatorDelay = CP.steerActuatorDelay
self.last_mpc_ts = 0.0
self.angle_steers_des = 0.0
self.angle_steers_des_mpc = 0.0
self.angle_steers_des_time = 0.0
self.avg_angle_steers = 0.0
self.projected_angle_steers = 0.0
self.frames = 0
self.curvature_factor = 0.0
self.mpc_frame = 0
class LatControlPID():
def __init__(self, CP, CI):
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,
neg_limit=-1.0,
sat_limit=CP.steerLimitTimer)
self.get_steer_feedforward = CI.get_steer_feedforward_function()
def reset(self):
self.pid.reset()
def update(self, active, CS, CP, VM, params, last_actuators,
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,
params.roll))
angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
pid_log.steeringAngleDesiredDeg = angle_steers_des
pid_log.angleError = angle_steers_des - CS.steeringAngleDeg
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
# offset does not contribute to resistive torque
steer_feedforward = self.get_steer_feedforward(
angle_steers_des_no_offset, CS.vEgo)
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 = 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)
self.angle_steers_des = 0.
def __init__(self, CP):
self.long_control_state = LongCtrlState.off # initialized to off
self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
k_f = CP.longitudinalTuning.kf, rate=1 / DT_CTRL)
self.v_pid = 0.0
self.last_output_accel = 0.0
def __init__(self, VM):
self.pid = PIController((VM.CP.steerKpBP, VM.CP.steerKpV),
(VM.CP.steerKiBP, VM.CP.steerKiV),
k_f=VM.CP.steerKf,
pos_limit=1.0)
self.last_cloudlog_t = 0.0
self.setup_mpc(VM.CP.steerRateCost)
class TiciFanController(BaseFanController):
def __init__(self) -> None:
super().__init__()
cloudlog.info("Setting up TICI fan handler")
self.last_ignition = False
self.controller = PIController(k_p=0,
k_i=2e-3,
k_f=1,
neg_limit=-80,
pos_limit=0,
rate=(1 / DT_TRML))
def update(self, max_cpu_temp: float, ignition: bool) -> int:
self.controller.neg_limit = -(80 if ignition else 30)
self.controller.pos_limit = -(30 if ignition else 0)
if ignition != self.last_ignition:
self.controller.reset()
fan_pwr_out = -int(
self.controller.update(setpoint=75,
measurement=max_cpu_temp,
feedforward=interp(
max_cpu_temp, [60.0, 100.0], [0, -80])))
self.last_ignition = ignition
return fan_pwr_out
class LatControl(object):
def __init__(self, VM):
self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, pos_limit=1.0)
self.setup_mpc()
self.y_des = -1 # Legacy
def setup_mpc(self):
self.libmpc = libmpc_py.libmpc
self.libmpc.init()
self.mpc_solution = libmpc_py.ffi.new("log_t *")
self.cur_state = libmpc_py.ffi.new("state_t *")
self.mpc_updated = False
self.cur_state[0].x = 0.0
self.cur_state[0].y = 0.0
self.cur_state[0].psi = 0.0
self.cur_state[0].delta = 0.0
self.last_mpc_ts = 0.0
self.angle_steers_des = 0
def reset(self):
self.pid.reset()
def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, VM, PL):
self.mpc_updated = False
if self.last_mpc_ts + 0.001 < PL.last_md_ts:
self.last_mpc_ts = PL.last_md_ts
curvature_factor = VM.curvature_factor(v_ego)
l_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.l_poly))
r_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.r_poly))
p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.p_poly))
# account for actuation delay
self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, VM.CP.sR)
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
l_poly, r_poly, p_poly,
PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, curvature_factor, v_ego, PL.PP.lane_width)
delta_desired = self.mpc_solution[0].delta[1]
self.cur_state[0].delta = delta_desired
self.angle_steers_des = math.degrees(delta_desired * VM.CP.sR) + angle_offset
self.mpc_updated = True
if v_ego < 0.3 or not active:
output_steer = 0.0
self.pid.reset()
else:
steer_max = get_steer_max(VM.CP, v_ego)
self.pid.pos_limit = steer_max
self.pid.neg_limit = -steer_max
output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override)
self.sat_flag = self.pid.saturated
return output_steer
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 __init__(self, CP):
self.kegman = kegman_conf(CP)
self.kegtime_prev = 0
self.profiler = Profiler(False, 'LaC')
self.frame = 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)
self.angle_steers_des = 0.
self.polyReact = 1.
self.poly_damp = 0
self.poly_factor = CP.lateralTuning.pid.polyFactor
self.poly_scale = CP.lateralTuning.pid.polyScale
self.path_error_comp = 0.0
self.damp_angle_steers = 0.
self.damp_angle_rate = 0.
self.damp_steer = 0.1
self.react_steer = 0.01
self.react_mpc = 0.0
self.damp_mpc = 0.25
self.angle_ff_ratio = 0.0
self.angle_ff_gain = 1.0
self.rate_ff_gain = CP.lateralTuning.pid.rateFFGain
self.angle_ff_bp = [[0.5, 5.0],[0.0, 1.0]]
self.lateral_offset = 0.0
self.previous_integral = 0.0
self.damp_angle_steers= 0.0
self.damp_rate_steers_des = 0.0
self.damp_angle_steers_des = 0.0
self.limited_damp_angle_steers_des = 0.0
self.old_plan_count = 0
self.last_plan_time = 0
self.lane_change_adjustment = 1.0
self.angle_index = 0.
self.avg_plan_age = 0.
self.min_index = 0
self.max_index = 0
self.prev_angle_steers = 0.
self.c_prob = 0.
self.deadzone = 0.
self.starting_angle = 0.
self.projected_lane_error = 0.
self.prev_projected_lane_error = 0.
self.path_index = None #np.arange((30.))*100.0/15.0
self.angle_rate_des = 0.0 # degrees/sec, rate dynamically limited by accel_limit
self.fast_angles = [[]]
self.center_angles = []
self.live_tune(CP)
self.react_index = 0.0
self.next_params_put = 36000
self.zero_poly_crossed = 0
self.zero_steer_crossed = 0
try:
params = Params()
lateral_params = params.get("LateralGain")
lateral_params = json.loads(lateral_params)
self.angle_ff_gain = max(1.0, float(lateral_params['angle_ff_gain']))
except:
self.angle_ff_gain = 1.0
def __init__(self, CP):
self.pid = PIController((CP.steerKpBP, CP.steerKpV),
(CP.steerKiBP, CP.steerKiV),
k_f=CP.steerKf, pos_limit=1.0)
self.last_cloudlog_t = 0.0
self.setup_mpc(CP.steerRateCost)
self.smooth_factor = 2.0 * CP.steerActuatorDelay / _DT # Multiplier for inductive component (feed forward)
self.projection_factor = 5.0 * _DT # Mutiplier for reactive component (PI)
self.accel_limit = 5.0 # Desired acceleration limit to prevent "whip steer" (resistive component)
self.ff_angle_factor = 0.5 # Kf multiplier for angle-based feed forward
self.ff_rate_factor = 5.0 # Kf multiplier for rate-based feed forward
self.ratioDelayExp = 2.0 # Exponential coefficient for variable steering rate (delay)
self.ratioDelayScale = 0.0 # Multiplier for variable steering rate (delay)
self.prev_angle_rate = 0
self.feed_forward = 0.0
self.angle_rate_desired = 0.0
# variables for dashboarding
self.context = zmq.Context()
self.steerpub = self.context.socket(zmq.PUB)
self.steerpub.bind("tcp://*:8594")
self.steerdata = ""
self.frames = 0
self.curvature_factor = 0.0
self.slip_factor = 0.0
def __init__(self, CP, compute_gb):
self.gas_interceptor = CP.enableGasInterceptor
self.long_control_state_stock = LongCtrlState.off # initialized to off
self.long_control_state_pedal = LongCtrlState.off # initialized to off
self.pid_stock = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), # stock toyota tune, no pedal. handles braking if pedal
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.pid_pedal = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpVPedal), # if pedal, handles gas
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiVPedal),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid_stock = 0.0
self.v_pid_pedal = 0.0
self.last_output_gb_stock = 0.0
self.last_output_gb_pedal = 0.0
self.lead_data = {'v_rel': None, 'a_lead': None, 'x_lead': None, 'status': False}
self.v_ego = 0.0
self.gas_pressed = False
def __init__(self, compute_gb):
self.long_control_state = LongCtrlState.off # initialized to off
self.pid = PIController((_KP_BP, _KP_V), (_KI_BP, _KI_V),
rate=100.0,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
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 = 0
def __init__(self, CP):
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,
neg_limit=-1.0,
sat_limit=CP.steerLimitTimer)
def __init__(self, CP):
self.long_control_state = LongCtrlState.off # initialized to off
self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
rate=RATE,
sat_limit=0.8)
self.v_pid = 0.0
self.last_output_accel = 0.0
def __init__(self, CP):
self.trPID = trace1.Loger("pid")
self.angle_steers_des = 0.
self.deadzone = CP.lateralsRatom.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)
def __init__(self, CP, compute_gb):
self.long_control_state = LongCtrlState.off # initialized to off
self.pid = PIController((CP.longitudinalKpBP, CP.longitudinalKpV),
(CP.longitudinalKiBP, CP.longitudinalKiV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
class LatControlPID():
def __init__(self, CP):
self.pid = PIController(
(CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
([0.], [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
pid_log.angleError = angle_steers_des - CS.steeringAngleDeg
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
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(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
class LatControlPID():
def __init__(self, CP):
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,
neg_limit=-1.0,
sat_limit=CP.steerLimitTimer)
self.angle_steers_des = 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
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
_c1, _c2, _c3 = [
0.34365576041121065, 12.845373070976711, 51.63304088261174
]
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)
return output_steer, float(self.angle_steers_des), pid_log
def __init__(self, CP):
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,
k_d=CP.lateralTuning.pid.kd,
pos_limit=1.0,
sat_limit=CP.steerLimitTimer)
self.angle_steers_des = 0.
def __init__(self, CP, CI):
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,
neg_limit=-1.0,
sat_limit=CP.steerLimitTimer)
self.get_steer_feedforward = CI.get_steer_feedforward_function()
class LatControlPID():
def __init__(self, CP):
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,
neg_limit=-1.0,
sat_limit=CP.steerLimitTimer)
self.angle_steers_des = 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()
elif CS.vEgo < 7: #7*3.6km
self.angle_steers_des = path_plan.angleSteers * 0.5
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
steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel)
deadzone = int(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
def __init__(self, CP):
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)
self.angle_steers_des = 0.
self.angle_ff_ratio = 0.0
self.angle_ff_gain = 1.0
self.rate_ff_gain = 0.01
self.angle_ff_bp = [[0.5, 5.0],[0.0, 1.0]]
self.sat_time = 0.0
class LatControlPID(object):
def __init__(self, CP):
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)
self.angle_steers_des = 0.
def reset(self):
self.pid.reset()
def update(self, active, v_ego, angle_steers, angle_steers_rate,
steer_override, CP, VM, path_plan):
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.pid.reset()
else:
# TODO: ideally we should interp, but for tuning reasons we keep the mpc solution
# constant for 0.05s.
#dt = min(cur_time - self.angle_steers_des_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps
#self.angle_steers_des = self.angle_steers_des_prev + (dt / _DT_MPC) * (self.angle_steers_des_mpc - self.angle_steers_des_prev)
self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner
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)
deadzone = 0.0
output_steer = self.pid.update(self.angle_steers_des,
angle_steers,
check_saturation=(v_ego > 10),
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)
self.sat_flag = self.pid.saturated
return output_steer, float(self.angle_steers_des), pid_log
def __init__(self, CP):
self.pid = PIController((CP.steerKpBP, CP.steerKpV),
(CP.steerKiBP, CP.steerKiV),
k_f=CP.steerKf,
pos_limit=1.0)
self.last_cloudlog_t = 0.0
self.angle_steers_des = 0.
self.angle_ff_ratio = 0.0
self.angle_ff_gain = 1.0
self.rate_ff_gain = 0.01
self.angle_ff_bp = [[0.5, 5.0], [0.0, 1.0]]
class LatControlPID():
def __init__(self, CP):
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.
def reset(self):
self.pid.reset()
def update(self, active, v_ego, angle_steers, angle_steers_rate,
eps_torque, steer_override, rate_limited, CP, path_plan):
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.pid.reset()
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)
deadzone = 0.0
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)
# we only deal with Tesla, so we return two angles, no torque info
return float(self.angle_steers_des), path_plan.angleSteers, pid_log
def __init__(self, CP):
self.params = Params(CP)
self.deadzone = float(self.params.get('IgnoreZone'))
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
