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
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, 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)
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
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
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()
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
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
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.dampened_angle_steers = 0.
self.angle_steers_des = 0.
self.sat_time = 0.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 # 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)
# Reset sat_flat always, set it only if needed
self.sat_flag = False
# If PID is saturated, set time which it was saturated
if self.pid.saturated and self.sat_time < 0.5:
self.sat_time = sec_since_boot()
# To save cycles, nest in sat_time check
if self.sat_time > 0.5:
# If its been saturated for 1.5 seconds then set flag
if (sec_since_boot() - self.sat_time) > 0.7:
self.sat_flag = True
# If it is no longer saturated, clear the sat flag and timer
if not self.pid.saturated:
self.sat_time = 0.0
return output_steer, float(self.angle_steers_des)
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
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.
# TODO: add the feedforward parameters to LiveParameters
self.angle_ff_gain = 2.0
self.rate_ff_gain = 0.2
self.angle_ff_bp = [[0.5, 5.0],[0.0, 1.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 # 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:
angle_feedforward = steer_feedforward - path_plan.angleOffset
angle_ff_ratio = interp(abs(angle_feedforward), self.angle_ff_bp[0], self.angle_ff_bp[1])
angle_feedforward *= angle_ff_ratio * self.angle_ff_gain
rate_feedforward = (1.0 - angle_ff_ratio) * self.rate_ff_gain * path_plan.rateSteers
steer_feedforward = v_ego**2 * (rate_feedforward + angle_feedforward)
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)
class LongControl():
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 reset(self, v_pid):
"""Reset PID controller and change setpoint"""
self.pid.reset()
self.v_pid = v_pid
def update(self, active, CS, CP, long_plan, accel_limits):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Interp control trajectory
# TODO estimate car specific lag, use .15s for now
speeds = long_plan.speeds
if len(speeds) == CONTROL_N:
v_target_lower = interp(CP.longitudinalActuatorDelayLowerBound,
T_IDXS[:CONTROL_N], speeds)
a_target_lower = 2 * (
v_target_lower - speeds[0]
) / CP.longitudinalActuatorDelayLowerBound - long_plan.accels[0]
v_target_upper = interp(CP.longitudinalActuatorDelayUpperBound,
T_IDXS[:CONTROL_N], speeds)
a_target_upper = 2 * (
v_target_upper - speeds[0]
) / CP.longitudinalActuatorDelayUpperBound - long_plan.accels[0]
a_target = min(a_target_lower, a_target_upper)
v_target = speeds[0]
v_target_future = speeds[-1]
else:
v_target = 0.0
v_target_future = 0.0
a_target = 0.0
# TODO: This check is not complete and needs to be enforced by MPC
a_target = clip(a_target, ACCEL_MIN_ISO, ACCEL_MAX_ISO)
self.pid.neg_limit = accel_limits[0]
self.pid.pos_limit = accel_limits[1]
# Update state machine
output_accel = self.last_output_accel
self.long_control_state = long_control_state_trans(
CP, active, self.long_control_state, CS.vEgo, v_target_future,
CS.brakePressed, CS.cruiseState.standstill)
if self.long_control_state == LongCtrlState.off or CS.gasPressed:
self.reset(CS.vEgo)
output_accel = 0.
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target
# Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7 and v_target_future < self.v_pid
deadzone = interp(CS.vEgo, CP.longitudinalTuning.deadzoneBP,
CP.longitudinalTuning.deadzoneV)
freeze_integrator = prevent_overshoot
output_accel = self.pid.update(self.v_pid,
CS.vEgo,
speed=CS.vEgo,
deadzone=deadzone,
feedforward=a_target,
freeze_integrator=freeze_integrator)
if prevent_overshoot:
output_accel = min(output_accel, 0.0)
# Intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping:
# Keep applying brakes until the car is stopped
if not CS.standstill or output_accel > CP.stopAccel:
output_accel -= CP.stoppingDecelRate * DT_CTRL
output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
self.reset(CS.vEgo)
self.last_output_accel = output_accel
final_accel = clip(output_accel, accel_limits[0], accel_limits[1])
return final_accel
class LongControl():
def __init__(self, CP, compute_gb):
self.long_control_state = LongCtrlState.off # initialized to off
self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
CP.longitudinalTuning.kf,
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
def reset(self, v_pid):
"""Reset PID controller and change setpoint"""
self.pid.reset()
self.v_pid = v_pid
def update(self, active, CS, v_target, v_target_future, a_target, CP, radarState = None):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Actuation limits
gas_max = interp(CS.vEgo, CP.gasMaxBP, CP.gasMaxV)
brake_max = interp(CS.vEgo, CP.brakeMaxBP, CP.brakeMaxV)
# Update state machine
output_gb = self.last_output_gb
d_rel = -1.
if radarState is not None and radarState.leadOne.status:
lead = radarState.leadOne
following = lead.dRel < 45.0 and lead.vLeadK > CS.vEgo and lead.aLeadK > 0.0
d_rel = lead.dRel
else:
following = False
if not following:
gas_max *= 0.9
self.long_control_state = long_control_state_trans(active, self.long_control_state, CS.vEgo,
v_target_future, self.v_pid, output_gb,
CS.brakePressed, CS.cruiseState.standstill, CP.minSpeedCan, d_rel)
v_ego_pid = max(CS.vEgo, CP.minSpeedCan) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
if self.long_control_state == LongCtrlState.off or CS.gasPressed:
self.reset(v_ego_pid)
output_gb = 0.
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target
self.pid.pos_limit = gas_max
self.pid.neg_limit = - brake_max
# Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7
deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV)
if d_rel > 0.:
a_target *= interp(d_rel, [10., 25., 70.], [1.0, 0.8, 0.6])
output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot)
if prevent_overshoot:
output_gb = min(output_gb, 0.0)
# Intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping:
# Keep applying brakes until the car is stopped
if not CS.standstill or output_gb > -BRAKE_STOPPING_TARGET:
output_gb -= CP.stoppingBrakeRate / RATE
output_gb = clip(output_gb, -brake_max, gas_max)
self.reset(CS.vEgo)
# Intention is to move again, release brake fast before handing control to PID
elif self.long_control_state == LongCtrlState.starting:
if output_gb < -0.2:
output_gb += CP.startingBrakeRate / RATE
self.reset(CS.vEgo)
self.last_output_gb = output_gb
final_gas = clip(output_gb, 0., gas_max)
final_brake = -clip(output_gb, -brake_max, 0.)
return final_gas, final_brake
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 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,
neg_limit=-1.0,
sat_limit=CP.steerLimitTimer)
self.angle_steers_des = 0.
self.Mid_Smoother = St_Smoother.Steer_Mid_Smoother()
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:
if CS.vEgo < 7.0: # ristrict to 25km
self.angle_steers_des = self.Mid_Smoother.get_data(
path_plan.angleSteers, 0.3)
elif CS.vEgo < 15.0: # ristrict to 54km
self.angle_steers_des = self.Mid_Smoother.get_data(
path_plan.angleSteers, 0.45)
elif CS.vEgo < 20.0: # ristrict to 72km
self.angle_steers_des = self.Mid_Smoother.get_data(
path_plan.angleSteers, 0.6)
elif CS.vEgo < 25.0: # ristrict to 90km
self.angle_steers_des = self.Mid_Smoother.get_data(
path_plan.angleSteers, 0.8)
elif CS.vEgo < 35.0: # ristrict to 125km
self.angle_steers_des = self.Mid_Smoother.get_data(
path_plan.angleSteers, 0.9)
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 = 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(object):
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 live_tune(self, CP):
if self.frame % 300 == 0:
(mode, ino, dev, nlink, uid, gid, size, atime, mtime, self.kegtime) = os.stat(os.path.expanduser('~/kegman.json'))
if self.kegtime != self.kegtime_prev:
try:
time.sleep(0.0001)
self.kegman = kegman_conf()
except:
print(" Kegman error")
self.pid._k_i = ([0.], [float(self.kegman.conf['Ki'])])
self.pid._k_p = ([0.], [float(self.kegman.conf['Kp'])])
self.pid.k_f = (float(self.kegman.conf['Kf']))
self.damp_steer = (float(self.kegman.conf['dampSteer']))
self.react_steer = (float(self.kegman.conf['reactSteer']))
self.react_mpc = (float(self.kegman.conf['reactMPC']))
self.damp_mpc = (float(self.kegman.conf['dampMPC']))
self.deadzone = float(self.kegman.conf['deadzone'])
self.polyReact = min(11, max(0, int(10 * float(self.kegman.conf['polyReact']))))
self.poly_damp = min(1, max(0, float(self.kegman.conf['polyDamp'])))
self.poly_factor = max(0.0, float(self.kegman.conf['polyFactor']) * 0.001)
self.require_blinker = bool(int(self.kegman.conf['requireBlinker']))
self.require_nudge = bool(int(self.kegman.conf['requireNudge']))
self.react_center = [max(0, float(self.kegman.conf['reactCenter0'])),max(0, float(self.kegman.conf['reactCenter1'])),max(0, float(self.kegman.conf['reactCenter2'])), 0]
self.kegtime_prev = self.kegtime
def update_lane_state(self, angle_steers, driver_opposing_lane, blinker_on, path_plan):
if self.require_nudge:
if self.lane_changing > 0.0: # and path_plan.cProb > 0:
self.lane_changing += 0.01 # max(self.lane_changing + 0.01, 0.005 * abs(path_plan.lPoly[5] + path_plan.rPoly[5]))
if self.lane_changing > 2.75 or (not blinker_on and self.lane_changing < 1.0 and abs(path_plan.cPoly[5]) < 100 and min(abs(self.starting_angle - angle_steers), abs(self.angle_steers_des - angle_steers)) < 1.5 and path_plan.cPoly[14] * path_plan.cPoly[0] > 0):
self.lane_changing = 0.0
self.stage = "4"
elif 2.25 <= self.lane_changing < 2.5 and path_plan.cPoly[14] * path_plan.cPoly[4] > 0: # abs(path_plan.lPoly[5] + path_plan.rPoly[5]) < abs(path_plan.cPoly[5]):
self.lane_changing = 2.5
self.stage = "3"
elif 2.0 <= self.lane_changing < 2.25 and path_plan.cPoly[14] * path_plan.cPoly[9] > 0: # (path_plan.lPoly[5] + path_plan.rPoly[5]) * path_plan.cPoly[0] < 0:
self.lane_changing = 2.25
self.stage = "2"
elif self.lane_changing < 2.0 and path_plan.cPoly[14] * path_plan.cPoly[0] < 0: #path_plan.laneWidth < 1.2 * abs(path_plan.lPoly[5] + path_plan.rPoly[5]):
self.lane_changing = 2.0
self.stage = "1"
elif self.lane_changing < 1.0 and abs(path_plan.cPoly[14]) > abs(path_plan.cPoly[7]): #path_plan.laneWidth < 1.2 * abs(path_plan.lPoly[5] + path_plan.rPoly[5]):
self.lane_changing = 0.98
self.stage = "0"
#else:
#self.lane_changing = max(self.lane_changing + 0.01, 0.005 * abs(path_plan.lPoly[5] + path_plan.rPoly[5]))
#if blinker_on:
# self.lane_change_adjustment = 0.0
#else:
self.lane_change_adjustment = interp(self.lane_changing, [0.0, 1.0, 2.0, 2.25, 2.5, 2.75], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
#print("%0.2f lane_changing %0.2f adjustment %0.2f p_poly %0.2f avg_poly stage = %s blinker %d opposing %d width_1 %0.2f width_2 %0.2f center_1 %0.2f center_2 %0.2f" % (self.lane_changing, self.lane_change_adjustment, path_plan.cPoly[5], path_plan.lPoly[5] + path_plan.rPoly[5], self.stage, blinker_on, driver_opposing_lane, path_plan.laneWidth, 0.6 * abs(path_plan.lPoly[5] - path_plan.rPoly[5]), path_plan.cPoly[0], path_plan.lPoly[5] + path_plan.rPoly[5]))
elif (blinker_on or not self.require_blinker) and driver_opposing_lane and path_plan.rProb > 0 and path_plan.lProb > 0 and (abs(path_plan.cPoly[14]) > 100 or min(abs(self.starting_angle - angle_steers), abs(self.angle_steers_des - angle_steers)) > 1.0): # and path_plan.cPoly[14] * path_plan.cPoly[0] > 0:
print('starting lane change @ %0.2f' % self.lane_changing)
self.lane_changing = 0.01
self.lane_change_adjustment = 1.0
else:
if self.lane_changing != 0: print('terminating lane change @ %0.2f' % self.lane_changing)
self.lane_changing = 0
self.stage = "0"
self.starting_angle = angle_steers
self.lane_change_adjustment = 1.0
elif blinker_on:
self.lane_change_adjustment = 0.0
else:
self.lane_change_adjustment = 1.0
def reset(self):
self.pid.reset()
def adjust_angle_gain(self):
if (self.pid.f > 0) == (self.pid.i > 0) and abs(self.pid.i) >= abs(self.previous_integral):
if not abs(self.pid.f + self.pid.i) > 1: self.angle_ff_gain *= 1.0001
elif self.angle_ff_gain > 1.0:
self.angle_ff_gain *= 0.9999
self.previous_integral = self.pid.i
def update(self, active, brake_pressed, v_ego, angle_steers, angle_steers_rate, steer_override, CP, path_plan, canTime, blinker_on):
self.profiler.checkpoint('controlsd')
pid_log = car.CarState.LateralPIDState.new_message()
path_age = (time.time() * 1000 - path_plan.sysTime) * 1e-3
if (angle_steers - path_plan.angleOffset >= 0) == (self.prev_angle_steers < 0):
self.zero_steer_crossed = time.time()
self.prev_angle_steers = angle_steers - path_plan.angleOffset
if path_plan.canTime != self.last_plan_time and len(path_plan.fastAngles) > 1:
time.sleep(0.00001)
if path_age > 0.23: self.old_plan_count += 1
if self.path_index is None:
self.avg_plan_age = path_age
self.path_index = np.arange((len(path_plan.fastAngles)))*100.0/15.0
self.last_plan_time = path_plan.canTime
self.avg_plan_age += 0.01 * (path_age - self.avg_plan_age)
self.c_prob = path_plan.cProb
self.projected_lane_error = float(min(0.5, max(-0.5, self.c_prob * self.poly_factor * sum(np.array(path_plan.cPoly)))))
self.center_angles.append(float(self.projected_lane_error))
if len(self.center_angles) > 15: self.center_angles.pop(0)
if (self.projected_lane_error >= 0) == (self.prev_projected_lane_error < 0):
self.zero_poly_crossed = time.time()
self.prev_projected_lane_error = self.projected_lane_error
if time.time() - min(self.zero_poly_crossed, self.zero_steer_crossed) < 4:
self.projected_lane_error -= (float(self.c_prob * self.poly_damp * self.center_angles[0]))
self.fast_angles = np.array(path_plan.fastAngles)
self.profiler.checkpoint('path_plan')
if path_plan.paramsValid: self.angle_index = max(0., 100. * (self.react_mpc + path_age))
self.min_index = min(self.min_index, self.angle_index)
self.max_index = max(self.max_index, self.angle_index)
if self.frame % 300 == 0 and self.frame > 0:
print("old plans: %d avg plan age: %0.3f min index: %d max_index: %d center_steer: %0.2f" % (self.old_plan_count, self.avg_plan_age, self.min_index, self.max_index, self.path_error_comp))
self.min_index = 100
self.max_index = 0
self.profiler.checkpoint('update')
self.frame += 1
self.live_tune(CP)
self.profiler.checkpoint('live_tune')
if v_ego < 0.3 or not path_plan.paramsValid:
if self.frame > self.next_params_put and v_ego == 0 and brake_pressed:
self.next_params_put = self.frame + 36000
put_nonblocking("LateralGain", json.dumps({'angle_ff_gain': self.angle_ff_gain}))
self.profiler.checkpoint('params_put')
output_steer = 0.0
self.stage = "0"
self.lane_changing = 0.0
self.previous_integral = 0.0
self.previous_lane_error = 0.0
self.path_error_comp = 0.0
self.damp_angle_steers= 0.0
self.damp_rate_steers_des = 0.0
self.damp_angle_steers_des = 0.0
pid_log.active = False
self.pid.reset()
self.profiler.checkpoint('pid_reset')
else:
try:
pid_log.active = True
if False and blinker_on and steer_override:
self.path_error_comp *= 0.9
self.damp_angle_steers = angle_steers
self.angle_steers_des = angle_steers
self.damp_angle_steers_des = angle_steers
self.limited_damp_angle_steers_des = angle_steers
self.angle_rate_des = 0
else:
react_steer = self.react_steer + self.react_center[min(len(self.react_center)-1, int(abs(angle_steers - path_plan.angleOffset)))]
self.damp_angle_steers += (angle_steers + angle_steers_rate * (self.damp_steer + float(react_steer)) - self.damp_angle_steers) / max(1.0, self.damp_steer * 100.)
self.angle_steers_des = interp(self.angle_index, self.path_index, self.fast_angles[min(len(self.fast_angles)-1, int(self.polyReact))])
self.damp_angle_steers_des += (self.angle_steers_des - self.damp_angle_steers_des + self.projected_lane_error) / max(1.0, self.damp_mpc * 100.)
if (self.damp_angle_steers - self.damp_angle_steers_des) * (angle_steers - self.damp_angle_steers_des) < 0:
self.damp_angle_steers = self.damp_angle_steers_des
angle_feedforward = float(self.damp_angle_steers_des - path_plan.angleOffset) + float(self.path_error_comp)
self.angle_ff_ratio = float(gernterp(abs(angle_feedforward), self.angle_ff_bp[0], self.angle_ff_bp[1]))
rate_feedforward = (1.0 - self.angle_ff_ratio) * self.rate_ff_gain * self.angle_rate_des
steer_feedforward = float(v_ego)**2 * (rate_feedforward + angle_feedforward * self.angle_ff_ratio * self.angle_ff_gain)
if not steer_override and v_ego > 10.0:
if abs(angle_steers) > (self.angle_ff_bp[0][1] / 2.0):
self.adjust_angle_gain()
else:
self.previous_integral = self.pid.i
if path_plan.cProb == 0 or (angle_feedforward > 0) == (self.pid.p > 0) or (path_plan.cPoly[-1] > 0) == (self.pid.p > 0):
p_scale = 1.0
else:
p_scale = max(0.2, min(1.0, 1 / abs(angle_feedforward)))
self.profiler.checkpoint('pre-pid')
output_steer = self.pid.update(self.damp_angle_steers_des, self.damp_angle_steers, check_saturation=(v_ego > 10), override=steer_override, p_scale=p_scale,
add_error=0, feedforward=steer_feedforward, speed=v_ego, deadzone=self.deadzone if abs(angle_feedforward) < 1 else 0.0)
self.profiler.checkpoint('pid_update')
except:
output_steer = 0
print(" angle error!")
pass
#driver_opposing_op = steer_override and (angle_steers - self.prev_angle_steers) * output_steer < 0
#self.update_lane_state(angle_steers, driver_opposing_op, blinker_on, path_plan)
#self.profiler.checkpoint('lane_change')
output_factor = self.lane_change_adjustment if pid_log.active else 0
if self.lane_change_adjustment < 1 and self.lane_changing > 0:
self.damp_angle_steers_des = self.angle_steers_des
self.limit_damp_angle_steers_des = self.angle_steers_des
self.prev_override = steer_override
self.pid.f *= output_factor
self.pid.i *= output_factor
self.pid.p *= output_factor
output_steer *= output_factor
pid_log.p = float(self.pid.p)
pid_log.i = float(self.pid.i)
pid_log.f = float(self.pid.f)
pid_log.output = float(output_steer)
pid_log.p2 = float(self.projected_lane_error)
pid_log.saturated = bool(self.pid.saturated)
pid_log.angleFFRatio = self.angle_ff_ratio
pid_log.steerAngle = float(self.damp_angle_steers)
pid_log.steerAngleDes = float(self.damp_angle_steers_des)
self.sat_flag = self.pid.saturated
self.profiler.checkpoint('post_update')
if self.frame % 5000 == 1000 and self.profiler.enabled:
self.profiler.display()
self.profiler.reset(True)
return output_steer, float(self.angle_steers_des), pid_log
class LongControl():
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 reset(self, v_pid):
"""Reset PID controller and change setpoint"""
self.pid.reset()
self.v_pid = v_pid
def update(self, active, CS, CP, long_plan, accel_limits):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Interp control trajectory
# TODO estimate car specific lag, use .15s for now
if len(long_plan.speeds) == CONTROL_N:
v_target = interp(DEFAULT_LONG_LAG, T_IDXS[:CONTROL_N],
long_plan.speeds)
v_target_future = long_plan.speeds[-1]
a_target = 2 * (v_target - long_plan.speeds[0]
) / DEFAULT_LONG_LAG - long_plan.accels[0]
else:
v_target = 0.0
v_target_future = 0.0
a_target = 0.0
self.pid.neg_limit = accel_limits[0]
self.pid.pos_limit = accel_limits[1]
# Update state machine
output_accel = self.last_output_accel
self.long_control_state = long_control_state_trans(
active, self.long_control_state, CS.vEgo, v_target_future,
self.v_pid, output_accel, CS.brakePressed,
CS.cruiseState.standstill, CP.minSpeedCan)
v_ego_pid = max(
CS.vEgo, CP.minSpeedCan
) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
if self.long_control_state == LongCtrlState.off or CS.gasPressed:
self.reset(v_ego_pid)
output_accel = 0.
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target
# Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7
deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP,
CP.longitudinalTuning.deadzoneV)
freeze_integrator = prevent_overshoot
output_accel = self.pid.update(self.v_pid,
v_ego_pid,
speed=v_ego_pid,
deadzone=deadzone,
feedforward=a_target,
freeze_integrator=freeze_integrator)
if prevent_overshoot:
output_accel = min(output_accel, 0.0)
# Intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping:
# Keep applying brakes until the car is stopped
if not CS.standstill or output_accel > -DECEL_STOPPING_TARGET:
output_accel -= CP.stoppingDecelRate / RATE
output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
self.reset(CS.vEgo)
# Intention is to move again, release brake fast before handing control to PID
elif self.long_control_state == LongCtrlState.starting:
if output_accel < -DECEL_THRESHOLD_TO_PID:
output_accel += CP.startingAccelRate / RATE
self.reset(CS.vEgo)
self.last_output_accel = output_accel
final_accel = clip(output_accel, accel_limits[0], accel_limits[1])
return final_accel, v_target, a_target
class LongControl(object):
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=100.0,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
def reset(self, v_pid):
self.pid.reset()
self.v_pid = v_pid
def update(self, active, v_ego, brake_pressed, standstill,
cruise_standstill, v_cruise, v_target, v_target_future,
a_target, CP, lead_1):
# actuation limits
gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)
output_gb = self.last_output_gb
rate = 100.0
self.long_control_state = long_control_state_trans(
active, self.long_control_state, v_ego, v_target_future,
self.v_pid, output_gb, brake_pressed, cruise_standstill)
v_ego_pid = max(
v_ego, MIN_CAN_SPEED
) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
# *** long control behavior based on state
if self.long_control_state == LongCtrlState.off:
self.v_pid = v_ego_pid # do nothing
output_gb = 0.
self.pid.reset()
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
prevent_overshoot = not CP.stoppingControl and v_ego < 1.5 and v_target_future < 0.7
self.v_pid = v_target
self.pid.pos_limit = gas_max
self.pid.neg_limit = -brake_max
deadzone = interp(v_ego_pid, CP.longPidDeadzoneBP,
CP.longPidDeadzoneV)
output_gb = self.pid.update(self.v_pid,
v_ego_pid,
speed=v_ego_pid,
deadzone=deadzone,
feedforward=a_target,
freeze_integrator=prevent_overshoot)
if prevent_overshoot:
output_gb = min(output_gb, 0.0)
# intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping:
# TODO: use the standstill bit from CAN to detect movements, usually more accurate than looking at v_ego
if not standstill or output_gb > -brake_stopping_target:
output_gb -= stopping_brake_rate / rate
output_gb = clip(output_gb, -brake_max, gas_max)
self.v_pid = v_ego
self.pid.reset()
# intention is to move again, release brake fast before handling control to PID
elif self.long_control_state == LongCtrlState.starting:
if output_gb < -0.2:
output_gb += starting_brake_rate / rate
self.v_pid = v_ego
self.pid.reset()
self.last_output_gb = output_gb
final_gas = clip(output_gb, 0., gas_max)
final_brake = -clip(output_gb, -brake_max, 0.)
return final_gas, final_brake
class LatControl(object):
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)
def setup_mpc(self, steer_rate_cost):
self.libmpc = libmpc_py.libmpc
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, steer_rate_cost)
self.mpc_solution = libmpc_py.ffi.new("log_t *")
self.cur_state = libmpc_py.ffi.new("state_t *")
self.mpc_updated = False
self.mpc_nans = 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.0
self.angle_steers_des_mpc = 0.0
self.angle_steers_des_prev = 0.0
self.angle_steers_des_time = 0.0
self.blindspot_blink_counter_left_check = 0
self.blindspot_blink_counter_right_check = 0
def reset(self):
self.pid.reset()
def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, VM, PL,blindspot,leftBlinker,rightBlinker):
cur_time = sec_since_boot()
self.mpc_updated = False
# TODO: this creates issues in replay when rewinding time: mpc won't run
if self.last_mpc_ts < PL.last_md_ts:
self.last_mpc_ts = PL.last_md_ts
self.angle_steers_des_prev = self.angle_steers_des_mpc
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.steerRatio, VM.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,
l_poly, r_poly, p_poly,
PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, curvature_factor, v_ego_mpc, PL.PP.lane_width)
# reset to current steer angle if not active or overriding
if active:
delta_desired = self.mpc_solution[0].delta[1]
else:
delta_desired = math.radians(angle_steers - angle_offset) / VM.CP.steerRatio
self.cur_state[0].delta = delta_desired
self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.CP.steerRatio) + angle_offset)
if leftBlinker:
if self.blindspot_blink_counter_left_check > 150:
self.angle_steers_des_mpc += 0#15
if rightBlinker:
if self.blindspot_blink_counter_right_check > 150:
self.angle_steers_des_mpc -= 0#15
self.angle_steers_des_time = cur_time
self.mpc_updated = True
# Check for infeasable MPC solution
self.mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
t = sec_since_boot()
if self.mpc_nans:
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, VM.CP.steerRateCost)
self.cur_state[0].delta = math.radians(angle_steers) / VM.CP.steerRatio
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning("Lateral mpc - nan: True")
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 = self.angle_steers_des_mpc
steers_max = get_steer_max(VM.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 VM.CP.steerControlType == car.CarParams.SteerControlType.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)
if rightBlinker:
if blindspot:
self.blindspot_blink_counter_right_check = 0
print "debug: blindspot detected"
self.blindspot_blink_counter_right_check += 1
if self.blindspot_blink_counter_right_check > 150:
self.angle_steers_des -= 0#15
else:
self.blindspot_blink_counter_right_check = 0
if leftBlinker:
if blindspot:
self.blindspot_blink_counter_left_check = 0
print "debug: blindspot detected"
self.blindspot_blink_counter_left_check += 1
if self.blindspot_blink_counter_left_check > 150:
self.angle_steers_des += 0#15
else:
self.blindspot_blink_counter_left_check = 0
self.sat_flag = self.pid.saturated
return output_steer, float(self.angle_steers_des)
class LatControl(object):
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(VM.CP.steerRateCost)
def setup_mpc(self, steer_rate_cost):
self.libmpc = libmpc_py.libmpc
self.libmpc.init(steer_rate_cost)
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.0
self.angle_steers_des_mpc = 0.0
self.angle_steers_des_prev = 0.0
self.angle_steers_des_time = 0.0
def reset(self):
self.pid.reset()
def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, VM, PL):
cur_time = sec_since_boot()
self.mpc_updated = False
if self.last_mpc_ts < PL.last_md_ts:
self.last_mpc_ts = PL.last_md_ts
self.angle_steers_des_prev = self.angle_steers_des_mpc
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.steerRatio)
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,
l_poly, r_poly, p_poly,
PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, curvature_factor, v_ego_mpc, PL.PP.lane_width)
delta_desired = self.mpc_solution[0].delta[1]
self.cur_state[0].delta = delta_desired
self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.CP.steerRatio) + angle_offset)
self.angle_steers_des_time = cur_time
self.mpc_updated = True
# Check for infeasable MPC solution
nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
t = sec_since_boot()
if nans:
self.libmpc.init(VM.CP.steerRateCost)
self.cur_state[0].delta = math.radians(angle_steers) / VM.CP.steerRatio
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning("Lateral mpc - nan: True")
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 = self.angle_steers_des_mpc
steers_max = get_steer_max(VM.CP, v_ego)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
steer_feedforward = self.angle_steers_des * v_ego**2 # proportional to realigning tire momentum (~ lateral accel)
output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, feedforward=steer_feedforward)
self.sat_flag = self.pid.saturated
return output_steer
class LongControl(object):
def __init__(self, CP, compute_gb):
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=1.0,
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
self.lastdecelForTurn = False
self.last_lead = None
self.freeze = False
self.none_count = 0
def reset(self, v_pid):
"""Reset PID controller and change setpoint"""
self.pid.reset()
self.v_pid = v_pid
def dynamic_gas(self, v_ego, gas_interceptor, gas_button_status):
dynamic = False
if gas_interceptor:
if gas_button_status == 0:
dynamic = True
x = [
0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781,
8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703,
24.46614, 29.05805, 32.71015, 35.76326
]
y = [
0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229,
0.34784, 0.36765, 0.38, 0.396, 0.409, 0.425, 0.478, 0.55,
0.621, 0.7
]
elif gas_button_status == 1:
y = [0.25, 0.9, 0.9]
elif gas_button_status == 2:
y = [0.2, 0.5, 0.7]
else:
if gas_button_status == 0:
dynamic = True
x = [
0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781,
8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703,
24.46614, 29.05805, 32.71015, 35.76326
]
y = [
0.35, 0.47, 0.43, 0.35, 0.3, 0.3, 0.3229, 0.34784, 0.36765,
0.38, 0.396, 0.409, 0.425, 0.478, 0.55, 0.621, 0.7
]
elif gas_button_status == 1:
y = [0.9, 0.95, 0.99]
elif gas_button_status == 2:
y = [0.25, 0.2, 0.2]
if not dynamic:
x = [0., 9., 35.] # default BP values
accel = interp(v_ego, x, y)
if self.none_count < 10 and self.last_lead is not None and self.last_lead.status: # if returned nones is less than 10, last lead is not none, and last lead's status is true assume lead
v_rel = self.last_lead.vRel
#a_lead = self.last_lead.aLeadK # to use later
#x_lead = self.last_lead.dRel
else:
v_rel = None
#a_lead = None
#x_lead = None
if dynamic and v_rel is not None: # dynamic gas profile specific operations, and if lead
if v_ego < 6.7056: # if under 15 mph
x = [1.61479, 1.99067, 2.28537, 2.49888, 2.6312, 2.68224]
y = [
-accel, -(accel / 1.06), -(accel / 1.2), -(accel / 1.8),
-(accel / 4.4), 0
] # array that matches current chosen accel value
accel += interp(v_rel, x, y)
else:
x = [-0.89408, 0, 0.89408, 4.4704]
y = [-.15, -.05, .005, .05]
accel += interp(v_rel, x, y)
min_return = 0.0
max_return = 1.0
return round(max(min(accel, max_return), min_return),
5) # ensure we return a value between range
def update(self, active, v_ego, brake_pressed, standstill,
cruise_standstill, v_cruise, v_target, v_target_future,
a_target, CP, gas_interceptor, gas_button_status, decelForTurn,
longitudinalPlanSource, lead_one, gas_pressed):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Actuation limits
if lead_one is not None:
self.last_lead = lead_one
self.none_count = 0
else:
self.none_count = clip(self.none_count + 1, 0, 10)
#gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
gas_max = self.dynamic_gas(v_ego, gas_interceptor, gas_button_status)
brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)
# Update state machine
output_gb = self.last_output_gb
self.long_control_state = long_control_state_trans(
active, self.long_control_state, v_ego, v_target_future,
self.v_pid, output_gb, brake_pressed, cruise_standstill)
v_ego_pid = max(
v_ego, MIN_CAN_SPEED
) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
if self.long_control_state == LongCtrlState.off:
self.v_pid = v_ego_pid
self.pid.reset()
output_gb = 0.
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target
self.pid.pos_limit = gas_max
self.pid.neg_limit = -brake_max
# Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
prevent_overshoot = not CP.stoppingControl and v_ego < 1.5 and v_target_future < 0.7
deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP,
CP.longitudinalTuning.deadzoneV)
if longitudinalPlanSource == 'cruise':
if decelForTurn and not self.lastdecelForTurn:
self.lastdecelForTurn = True
self.pid._k_p = (CP.longitudinalTuning.kpBP, [
x * 0 for x in CP.longitudinalTuning.kpV
])
self.pid._k_i = (CP.longitudinalTuning.kiBP, [
x * 0 for x in CP.longitudinalTuning.kiV
])
self.pid.i = 0.0
self.pid.k_f = 1.0
if self.lastdecelForTurn and not decelForTurn:
self.lastdecelForTurn = False
self.pid._k_p = (CP.longitudinalTuning.kpBP,
CP.longitudinalTuning.kpV)
self.pid._k_i = (CP.longitudinalTuning.kiBP,
CP.longitudinalTuning.kiV)
self.pid.k_f = 1.0
else:
self.lastdecelForTurn = False
self.pid._k_p = (CP.longitudinalTuning.kpBP,
CP.longitudinalTuning.kpV)
self.pid._k_i = (CP.longitudinalTuning.kiBP,
CP.longitudinalTuning.kiV)
self.pid.k_f = 1.0
if gas_pressed or brake_pressed:
if not self.freeze:
self.pid.i = 0.0
self.freeze = True
else:
if self.freeze:
self.freeze = False
output_gb = self.pid.update(self.v_pid,
v_ego_pid,
speed=v_ego_pid,
deadzone=deadzone,
feedforward=a_target,
freeze_integrator=(prevent_overshoot
or gas_pressed
or brake_pressed))
if prevent_overshoot:
output_gb = min(output_gb, 0.0)
# Intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping:
# Keep applying brakes until the car is stopped
if not standstill or output_gb > -BRAKE_STOPPING_TARGET:
output_gb -= STOPPING_BRAKE_RATE / RATE
output_gb = clip(output_gb, -brake_max, gas_max)
self.v_pid = v_ego
self.pid.reset()
# Intention is to move again, release brake fast before handing control to PID
elif self.long_control_state == LongCtrlState.starting:
if output_gb < -0.2:
output_gb += STARTING_BRAKE_RATE / RATE
self.v_pid = v_ego
self.pid.reset()
self.last_output_gb = output_gb
final_gas = clip(output_gb, 0., gas_max)
final_brake = -clip(output_gb, -brake_max, 0.)
return final_gas, final_brake
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.setup_mpc(CP.steerRateCost)
def setup_mpc(self, steer_rate_cost):
self.libmpc = libmpc_py.libmpc
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE,
MPC_COST_LAT.HEADING, steer_rate_cost)
self.mpc_solution = libmpc_py.ffi.new("log_t *")
self.cur_state = libmpc_py.ffi.new("state_t *")
self.mpc_updated = False
self.mpc_nans = 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.0
self.angle_steers_des_mpc = 0.0
self.angle_steers_des_prev = 0.0
self.angle_steers_des_time = 0.0
# For Variable Steering Ratio
self.lowSteerRatio = 9.0 # Set the lowest possible steering ratio allowed
self.vsrWindowLow = 0.1 # Set the tire/car angle low-end used for VSR (vsrWindowLow - is same as lowSteerRatio)
self.vsrWindowHigh = 0.65 # Set the tire/car angle high-end (vsrWindowHigh + is same as CP.steerRatio / interface.py)
self.manual_Steering_Offset = 0.0 # Set a steering wheel offset. (Should this be * steering ratio to get the steering wheel angle?)
self.variableSteerRatio = 0.0 # Used to store the calculated steering ratio
self.angle_Check = 0.0 # Used for desired tire/car angle
self.vsrSlope = 0.0 # Used for slope intercept formula
self.vsrYIntercept = 0.0 # Used for slope intercept formula
def reset(self):
self.pid.reset()
def update(self, active, v_ego, angle_steers, steer_override, d_poly,
angle_offset, CP, VM, PL):
cur_time = sec_since_boot()
self.mpc_updated = False
# TODO: this creates issues in replay when rewinding time: mpc won't run
if self.last_mpc_ts < PL.last_md_ts:
self.last_mpc_ts = PL.last_md_ts
self.angle_steers_des_prev = self.angle_steers_des_mpc
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))
# Prius (and Prime) appears to have a variable steering ratio. Try to account for that
# Random maths:
# https://www.desmos.com/calculator
# https://www.calculator.net/slope-calculator.html
# (steering wheel angle / steering ratio) = tire angle ..
# So, if the calculation below is determining the tire angle, look for values under about 1.5 degrees
self.angle_Check = angle_steers - angle_offset
if abs(self.angle_Check
) < self.vsrWindowLow: # 0.3 degrees, for example
self.variableSteerRatio = self.lowSteerRatio # Use the lower ratio
elif self.vsrWindowLow < abs(
self.angle_Check
) < self.vsrWindowHigh: # The VSR transition zone
# Begin the _variable_ part
# Find the slope of the line from the start of the VSR window to the end of the window - ( m = (y1 - y) / (x1 - x) )
self.vsrSlope = (self.lowSteerRatio - CP.steerRatio) / (
self.vsrWindowLow - self.vsrWindowHigh)
# Solve for b (y-intercept) - (b = y - mx)
self.vsrYIntercept = (CP.steerRatio -
self.vsrSlope) * self.vsrWindowHigh
# Use b to find y - (y = mx + b)
self.variableSteerRatio = (
self.vsrSlope * self.angle_Check) + self.vsrYIntercept
if not self.lowSteerRatio <= self.variableSteerRatio <= CP.steerRatio: # Sanity/safety check
if self.variableSteerRatio < self.lowSteerRatio:
self.variableSteerRatio = self.lowSteerRatio # Reset to the low ratio
elif self.variableSteerRatio > CP.steerRatio:
self.variableSteerRatio = CP.steerRatio # Reset to steerRatio from interface.py
else: # The angle is in the quick zone so do nothing
self.variableSteerRatio = CP.steerRatio # Use steerRatio from interface.py
# account for actuation delay
self.cur_state = calc_states_after_delay(self.cur_state, v_ego,
angle_steers,
curvature_factor,
self.variableSteerRatio,
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, l_poly,
r_poly, p_poly, PL.PP.l_prob, PL.PP.r_prob,
PL.PP.p_prob, curvature_factor, v_ego_mpc,
PL.PP.lane_width)
# reset to current steer angle if not active or overriding
if active:
delta_desired = self.mpc_solution[0].delta[1]
else: # Add a steering offset vs recalibrating steering sensor so it reads near 0
delta_desired = math.radians(
self.angle_Check) / self.variableSteerRatio
self.cur_state[0].delta = delta_desired
self.angle_steers_des_mpc = float(
math.degrees(delta_desired * self.variableSteerRatio) +
angle_offset + self.manual_Steering_Offset)
self.angle_steers_des_time = cur_time
self.mpc_updated = True
# Check for infeasable MPC solution
self.mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
t = sec_since_boot()
if self.mpc_nans:
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE,
MPC_COST_LAT.HEADING, CP.steerRateCost)
self.cur_state[0].delta = math.radians(
angle_steers) / self.variableSteerRatio
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning("Lateral mpc - nan: True")
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 = self.angle_steers_des_mpc
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:
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)
class LatControlPID():
def __init__(self, CP):
self.factor = 1.
self.lowpid = 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.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.
self.increasing = False
self.dualpids = False
def reset(self):
self.pid.reset()
self.lowpid.reset()
self.increasing = False
def dualPIDinit(self, CP):
self.factor = CP.lateralParams.torqueBP[1] / CP.lateralParams.torqueBP[
-1]
self.highkpV = CP.lateralTuning.pid.kpV[1]
self.highkiV = CP.lateralTuning.pid.kiV[1]
self.lowpid = PIController(
(CP.lateralTuning.pid.kpBP, [CP.lateralTuning.pid.kpV[0]]),
(CP.lateralTuning.pid.kiBP, [CP.lateralTuning.pid.kiV[0]]),
k_f=CP.lateralTuning.pid.kf,
pos_limit=1.0,
sat_limit=CP.steerLimitTimer)
self.pid = PIController((CP.lateralTuning.pid.kpBP, [self.highkpV]),
(CP.lateralTuning.pid.kiBP, [self.highkiV]),
k_f=CP.lateralTuning.pid.kf,
pos_limit=1.0,
sat_limit=CP.steerLimitTimer)
self.angle_steers_des = 0.
self.increasing = False
self.dualpids = True
def pidset(self, pid, descontrol, setpoint, measurement, feedforward):
error = float(setpoint - measurement)
p = error * pid._k_p[1][0]
f = feedforward * pid.k_f
i = descontrol - p - f
pid.p = p
pid.i = i
pid.f = f
pid.sat_count = 0.0
pid.saturated = False
pid.control = descontrol
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
if len(CP.lateralTuning.pid.kpV) > 1 and not self.dualpids:
self.dualPIDinit(CP)
self.pid.reset()
self.lowpid.reset()
self.increasing = False
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
self.lowpid.pos_limit = steers_max
self.lowpid.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 = 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)
output_steer = 0.0
if not self.dualpids:
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)
else:
if not self.increasing:
raw_low_output_steer = self.lowpid.update(
self.angle_steers_des,
CS.steeringAngle,
check_saturation=check_saturation,
override=CS.steeringPressed,
feedforward=steer_feedforward,
speed=CS.vEgo,
deadzone=deadzone)
output_steer = raw_low_output_steer * self.factor
#print("Lo - " + str(output_steer))
if abs(output_steer) > (self.factor * 0.99):
self.pidset(self.pid, output_steer,
self.angle_steers_des, CS.steeringAngle,
steer_feedforward)
#self.pid.p = self.lowpid.p * self.factor
#self.pid.i = self.lowpid.i * self.factor
#self.pid.f = self.lowpid.f * self.factor
#self.pid.sat_count = 0.0
#self.pid.saturated = False
#self.pid.control = self.lowpid.control * self.factor
self.increasing = True
else:
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)
#print("Hi - " + str(output_steer))
if abs(output_steer) < (self.factor * 0.1) and abs(
self.pid.i) < (self.factor * 0.2):
self.pidset(self.lowpid, (output_steer / self.factor),
self.angle_steers_des, CS.steeringAngle,
steer_feedforward)
#self.lowpid.p = self.pid.p / self.factor
#self.lowpid.i = 0
#self.lowpid.f = self.pid.f / self.factor
#self.lowpid.sat_count = 0.0
#self.lowpid.saturated = False
#self.lowpid.control = self.pid.control / self.factor
self.increasing = False
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 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.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 = 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.ratioDelayExp = 2.0 # Exponential coefficient for variable steering ratio (delay)
self.ratioDelayScale = 0.0 # Multiplier for variable steering ratio (delay)
self.ratioScale = 6.0 # Multiplier for variable steering ratio
self.ratioExp = 2.0 # Exponential coefficient for variable steering assist (torque)
self.ratioAdjust = 0.85 # Fudge factor to preserve existing tuning parameters
self.prev_angle_rate = 0
self.feed_forward = 0.0
self.steerActuatorDelay = CP.steerActuatorDelay
self.angle_rate_desired = 0.0
self.last_mpc_ts = 0.0
self.angle_steers_des = 0.0
self.angle_steers_des_mpc = 0.0
self.angle_steers_des_prev = 0.0
self.angle_steers_des_time = 0.0
self.avg_angle_steers = 0.0
self.last_y = 0.0
self.new_y = 0.0
self.angle_sample_count = 0.0
self.projected_angle_steers = 0.0
self.lateral_error = 0.0
# variables for dashboarding
self.context = zmq.Context()
self.steerpub = self.context.socket(zmq.PUB)
self.steerpub.bind("tcp://*:8594")
self.influxString = 'steerData3,testName=none,active=%s,ff_type=%s ff_type_a=%s,ff_type_r=%s,steer_status=%s,steer_torque_motor=%s,' \
'steering_control_active=%s,steer_parameter1=%s,steer_parameter2=%s,steer_parameter3=%s,steer_parameter4=%s,steer_parameter5=%s,' \
'steer_parameter6=%s,steer_stock_torque=%s,steer_stock_torque_request=%s,x=%s,y=%s,lateral_error=%s,y0=%s,y1=%s,y2=%s,y3=%s,y4=%s,psi=%s,delta=%s,t=%s,' \
'curvature_factor=%s,slip_factor=%s,resonant_period=%s,accel_limit=%s,restricted_steer_rate=%s,ff_angle_factor=%s,ff_rate_factor=%s,' \
'pCost=%s,lCost=%s,rCost=%s,hCost=%s,srCost=%s,torque_motor=%s,driver_torque=%s,angle_rate_count=%s,angle_rate_desired=%s,' \
'avg_angle_rate=%s,future_angle_steers=%s,angle_rate=%s,steer_zero_crossing=%s,center_angle=%s,angle_steers=%s,angle_steers_des=%s,' \
'angle_offset=%s,self.angle_steers_des_mpc=%s,steerRatio=%s,steerKf=%s,steerKpV[0]=%s,steerKiV[0]=%s,steerRateCost=%s,l_prob=%s,' \
'r_prob=%s,c_prob=%s,p_prob=%s,l_poly[0]=%s,l_poly[1]=%s,l_poly[2]=%s,l_poly[3]=%s,r_poly[0]=%s,r_poly[1]=%s,r_poly[2]=%s,r_poly[3]=%s,' \
'p_poly[0]=%s,p_poly[1]=%s,p_poly[2]=%s,p_poly[3]=%s,c_poly[0]=%s,c_poly[1]=%s,c_poly[2]=%s,c_poly[3]=%s,d_poly[0]=%s,d_poly[1]=%s,' \
'd_poly[2]=%s,lane_width=%s,lane_width_estimate=%s,lane_width_certainty=%s,v_ego=%s,p=%s,i=%s,f=%s %s\n~'
self.steerdata = self.influxString
self.frames = 0
self.curvature_factor = 0.0
self.slip_factor = 0.0
self.isActive = 0
def setup_mpc(self, steer_rate_cost):
self.libmpc = libmpc_py.libmpc
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE,
MPC_COST_LAT.HEADING, steer_rate_cost)
self.mpc_solution = libmpc_py.ffi.new("log_t *")
self.cur_state = libmpc_py.ffi.new("state_t *")
self.mpc_updated = False
self.mpc_nans = 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
def reset(self):
self.pid.reset()
def update(self, active, v_ego, angle_steers, angle_rate, steer_override,
d_poly, angle_offset, CP, VM, PL):
cur_time = sec_since_boot()
self.mpc_updated = False
# TODO: this creates issues in replay when rewinding time: mpc won't run
if self.last_mpc_ts < PL.last_md_ts:
self.last_mpc_ts = PL.last_md_ts
self.angle_steers_des_prev = self.angle_steers_des_mpc
# Use the model's solve time instead of cur_time
self.angle_steers_des_time = float(self.last_mpc_ts / 1000000000.0)
self.curvature_factor = VM.curvature_factor(v_ego)
# This is currently disabled, but it is used to compensate for variable steering rate
ratioDelayFactor = 1. + self.ratioDelayScale * abs(
angle_steers / 100.)**self.ratioDelayExp
# Determine a proper delay time that includes the model's processing time, which is variable
plan_age = _DT_MPC + cur_time - float(
self.last_mpc_ts / 1000000000.0)
total_delay = ratioDelayFactor * CP.steerActuatorDelay + plan_age
# Use steering rate from the last 2 samples to estimate acceleration for a more realistic future steering rate
accelerated_angle_rate = 2.0 * angle_rate - self.prev_angle_rate
# Project the future steering angle for the actuator delay only (not model delay)
self.projected_angle_steers = ratioDelayFactor * CP.steerActuatorDelay * accelerated_angle_rate + angle_steers
self.l_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.l_poly))
self.r_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.r_poly))
self.p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.p_poly))
# account for actuation delay and the age of the plan
self.cur_state = calc_states_after_delay(
self.cur_state, v_ego, self.projected_angle_steers,
self.curvature_factor, CP.steerRatio, total_delay)
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, self.l_poly,
self.r_poly, self.p_poly, PL.PP.l_prob,
PL.PP.r_prob, PL.PP.p_prob,
self.curvature_factor, v_ego_mpc,
PL.PP.lane_width)
# reset to current steer angle if not active or overriding
if active:
self.isActive = 1
delta_desired = self.mpc_solution[0].delta[1]
else:
self.isActive = 0
delta_desired = math.radians(angle_steers -
angle_offset) / CP.steerRatio
self.cur_state[0].delta = delta_desired
self.angle_steers_des_mpc = float(
math.degrees(delta_desired * CP.steerRatio) + angle_offset)
# Use last 2 desired angles to determine the model's desired steer rate
self.angle_rate_desired = (self.angle_steers_des_mpc -
self.angle_steers_des_prev) / _DT_MPC
self.mpc_updated = True
# Check for infeasable MPC solution
self.mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
t = sec_since_boot()
if self.mpc_nans:
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE,
MPC_COST_LAT.HEADING, CP.steerRateCost)
self.cur_state[0].delta = math.radians(
angle_steers) / CP.steerRatio
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning("Lateral mpc - nan: True")
elif self.frames > 20:
self.steerpub.send(self.steerdata)
self.frames = 0
self.steerdata = self.influxString
if v_ego < 0.3 or not active:
output_steer = 0.0
self.feed_forward = 0.0
self.pid.reset()
ff_type = "r"
projected_angle_steers_des = 0.0
projected_angle_steers = 0.0
restricted_steer_rate = 0.0
else:
# Interpolate desired angle between MPC updates
self.angle_steers_des = self.angle_steers_des_prev + self.angle_rate_desired * (
cur_time - self.angle_steers_des_time)
self.avg_angle_steers = (4.0 * self.avg_angle_steers +
angle_steers) / 5.0
# Determine the target steer rate for desired angle, but prevent the acceleration limit from being exceeded
# Restricting the steer rate creates the resistive component needed for resonance
restricted_steer_rate = np.clip(
self.angle_steers_des - float(angle_steers),
float(angle_rate) - self.accel_limit,
float(angle_rate) + self.accel_limit)
# Determine projected desired angle that is within the acceleration limit (prevent the steering wheel from jerking)
projected_angle_steers_des = self.angle_steers_des + self.projection_factor * restricted_steer_rate
# Determine project angle steers using current steer rate
projected_angle_steers = float(
angle_steers) + self.projection_factor * float(angle_rate)
steers_max = get_steer_max(CP, v_ego)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
if CP.steerControlType == car.CarParams.SteerControlType.torque:
# Decide which feed forward mode should be used (angle or rate). Use more dominant mode, and only if conditions are met
# Spread feed forward out over a period of time to make it more inductive (for resonance)
if abs(self.ff_rate_factor * float(restricted_steer_rate)) > abs(self.ff_angle_factor * float(self.angle_steers_des) - float(angle_offset)) - 0.5 \
and (abs(float(restricted_steer_rate)) > abs(angle_rate) or (float(restricted_steer_rate) < 0) != (angle_rate < 0)) \
and (float(restricted_steer_rate) < 0) == (float(self.angle_steers_des) - float(angle_offset) - 0.5 < 0):
ff_type = "r"
self.feed_forward = (
((self.smooth_factor - 1.) * self.feed_forward) +
self.ff_rate_factor * v_ego**2 *
float(restricted_steer_rate)) / self.smooth_factor
elif abs(self.angle_steers_des - float(angle_offset)) > 0.5:
ff_type = "a"
self.feed_forward = ((
(self.smooth_factor - 1.) * self.feed_forward
) + self.ff_angle_factor * v_ego**2 * float(
apply_deadzone(
float(self.angle_steers_des) - float(angle_offset),
0.5))) / self.smooth_factor
else:
ff_type = "r"
self.feed_forward = ((
(self.smooth_factor - 1.) * self.feed_forward) +
0.0) / self.smooth_factor
else:
self.feed_forward = self.angle_steers_des # feedforward desired angle
deadzone = 0.0
# Use projected desired and actual angles instead of "current" values, in order to make PI more reactive (for resonance)
output_steer = self.pid.update(projected_angle_steers_des,
projected_angle_steers,
check_saturation=(v_ego > 10),
override=steer_override,
feedforward=self.feed_forward,
speed=v_ego,
deadzone=deadzone)
# All but the last 3 lines after here are for real-time dashboarding
self.pCost = 0.0
self.lCost = 0.0
self.rCost = 0.0
self.hCost = 0.0
self.srCost = 0.0
self.last_ff_a = 0.0
self.last_ff_r = 0.0
self.center_angle = 0.0
self.steer_zero_crossing = 0.0
self.steer_rate_cost = 0.0
self.avg_angle_rate = 0.0
self.angle_rate_count = 0.0
steer_motor = 0.0
self.frames += 1
steer_parameter1 = 0.0
steer_parameter2 = 0.0
steer_parameter3 = 0.0
steer_parameter4 = 0.0
steer_parameter5 = 0.0
steer_parameter6 = 0.0
steering_control_active = 0.0
steer_torque_motor = 0.0
driver_torque = 0.0
steer_status = 0.0
steer_stock_torque = 0.0
steer_stock_torque_request = 0.0
self.steerdata += ("%d,%s,%d,%d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%d|" % (self.isActive, \
ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, steer_status, steer_torque_motor, steering_control_active, steer_parameter1, steer_parameter2, steer_parameter3, steer_parameter4, steer_parameter5, steer_parameter6, steer_stock_torque, steer_stock_torque_request, self.cur_state[0].x, self.cur_state[0].y, self.lateral_error, self.mpc_solution[0].y[0], self.mpc_solution[0].y[1], self.mpc_solution[0].y[2], self.mpc_solution[0].y[3], self.mpc_solution[0].y[4], self.cur_state[0].psi, self.cur_state[0].delta, self.cur_state[0].t, self.curvature_factor, self.slip_factor ,self.smooth_factor, self.accel_limit, float(restricted_steer_rate) ,self.ff_angle_factor, self.ff_rate_factor, self.pCost, self.lCost, self.rCost, self.hCost, self.srCost, steer_motor, float(driver_torque), \
self.angle_rate_count, self.angle_rate_desired, self.avg_angle_rate, projected_angle_steers, float(angle_rate), self.steer_zero_crossing, self.center_angle, angle_steers, self.angle_steers_des, angle_offset, \
self.angle_steers_des_mpc, CP.steerRatio, CP.steerKf, CP.steerKpV[0], CP.steerKiV[0], CP.steerRateCost, PL.PP.l_prob, \
PL.PP.r_prob, PL.PP.c_prob, PL.PP.p_prob, self.l_poly[0], self.l_poly[1], self.l_poly[2], self.l_poly[3], self.r_poly[0], self.r_poly[1], self.r_poly[2], self.r_poly[3], \
self.p_poly[0], self.p_poly[1], self.p_poly[2], self.p_poly[3], PL.PP.c_poly[0], PL.PP.c_poly[1], PL.PP.c_poly[2], PL.PP.c_poly[3], PL.PP.d_poly[0], PL.PP.d_poly[1], \
PL.PP.d_poly[2], PL.PP.lane_width, PL.PP.lane_width_estimate, PL.PP.lane_width_certainty, v_ego, self.pid.p, self.pid.i, self.pid.f, int(time.time() * 100) * 10000000))
self.sat_flag = self.pid.saturated
self.prev_angle_rate = angle_rate
return output_steer, float(self.angle_steers_des)
class ALCAController(object):
def __init__(self, carcontroller, alcaEnabled, steerByAngle):
#import settings
self.CC = carcontroller # added to start, will see if we need it actually
# variables for lane change
self.alcaEnabled = alcaEnabled
self.laneChange_strStartFactor = 2.
self.laneChange_strStartMultiplier = 1.5
self.laneChange_steerByAngle = steerByAngle # steer only by angle; do not call PID
self.laneChange_last_actuator_angle = 0.
self.laneChange_last_actuator_delta = 0.
self.laneChange_last_sent_angle = 0.
self.laneChange_over_the_line = 0 # did we cross the line?
self.laneChange_avg_angle = 0. # used if we do average entry angle over x frames
self.laneChange_avg_count = 0. # used if we do average entry angle over x frames
self.laneChange_enabled = 1 # set to zero for no lane change
self.laneChange_counter = 0 # used to count frames during lane change
self.laneChange_min_duration = 1. # min time to wait before looking for next lane
self.laneChange_duration = 5 # how many max seconds to actually do the move; if lane not found after this then send error
self.laneChange_after_lane_duration_mult = 1. # multiplier for time after we cross the line before we let OP take over; multiplied with CL_TIMEA_T
self.laneChange_wait = 2 # how many seconds to wait before it starts the change
self.laneChange_lw = 3.0 # lane width in meters
self.laneChange_angle = 0. # saves the last angle from actuators before lane change starts
self.laneChange_angled = 0. # angle delta
self.laneChange_steerr = 16.00 # steer ratio for lane change
self.laneChange_direction = 0 # direction of the lane change
self.prev_right_blinker_on = False # local variable for prev position
self.prev_left_blinker_on = False # local variable for prev position
self.keep_angle = False #local variable to keep certain angle delta vs. actuator
#self.blindspot_blink_counter = 0
self.pid = None
self.last10delta = []
def last10delta_reset(self):
self.last10delta = []
for i in range(0, 10):
self.last10delta.append(0.)
def last10delta_add(self, angle):
self.last10delta.pop(0)
self.last10delta.append(angle)
n = 0
a = 0.
for i in self.last10delta:
if i != 0:
n += 1
a += i
return n, a
def last10delta_correct(self, c):
for i in self.last10delta:
j = self.last10delta.index(i)
n = self.last10delta.pop(j)
n += c
self.last10delta.insert(j, n)
def update_status(self, alcaEnabled):
self.alcaEnabled = alcaEnabled
def set_pid(self, CS):
self.laneChange_steerr = CS.CP.steerRatio
self.pid = PIController((CS.CP.steerKpBP, CS.CP.steerKpV),
(CS.CP.steerKiBP, CS.CP.steerKiV),
k_f=CS.CP.steerKf,
pos_limit=1.0)
def update_angle(self, enabled, CS, frame, actuators):
alca_m1 = 1.
alca_m2 = 1.
alca_m3 = 1.
if CS.alcaMode == 1:
alca_m1 = 0.95
alca_m2 = 1.
alca_m3 = 0.5
if CS.alcaMode == 2:
alca_m1 = 0.9
alca_m2 = 1.7
alca_m3 = 0.5
# speed variable parameters
cl_max_angle_delta = interp(CS.v_ego, CS.CL_MAX_ANGLE_DELTA_BP,
CS.CL_MAX_ANGLE_DELTA) * alca_m1
cl_maxd_a = interp(CS.v_ego, CS.CL_MAXD_BP, CS.CL_MAXD_A) * alca_m3
cl_lane_pass_time = interp(CS.v_ego, CS.CL_LANE_PASS_BP,
CS.CL_LANE_PASS_TIME) * alca_m2
cl_timea_t = interp(CS.v_ego, CS.CL_TIMEA_BP, CS.CL_TIMEA_T) * alca_m2
cl_lane_detect_factor = interp(CS.v_ego, CS.CL_LANE_DETECT_BP,
CS.CL_LANE_DETECT_FACTOR)
cl_max_a = interp(CS.v_ego, CS.CL_MAX_A_BP, CS.CL_MAX_A)
cl_adjust_factor = interp(CS.v_ego, CS.CL_ADJUST_FACTOR_BP,
CS.CL_ADJUST_FACTOR)
cl_reentry_angle = interp(CS.v_ego, CS.CL_REENTRY_ANGLE_BP,
CS.CL_REENTRY_ANGLE)
cl_correction_factor = interp(CS.v_ego, CS.CL_CORRECTION_FACTOR_BP,
CS.CL_CORRECTION_FACTOR)
cl_min_v = CS.CL_MIN_V
self.laneChange_wait = CS.CL_WAIT_BEFORE_START
# Basic highway lane change logic
blindspot = CS.blind_spot_on
#if changing_lanes:
#if blindspot:
#self.blindspot_blink_counter = 0
# print "debug: blindspot detected in alca"
#CS.UE.custom_alert_message(3,"Auto Lane Change Canceled! (s)",200,5)
#self.laneChange_enabled = 1
# self.laneChange_counter = 0
#self.laneChange_direction = 0
# self.blindspot_blink_counter += 1
#if self.blindspot_blink_counter < 150:
#self.laneChange_enabled = 0
#self.laneChange_counter = 0
#else:
#self.laneChange_enabled = 1
#self.laneChange_counter = self.laneChange_counter
#self.laneChange_steerr = CS.CP.steerRatio
actuator_delta = 0.
laneChange_angle = 0.
turn_signal_needed = 0 # send 1 for left, 2 for right 0 for not needed
if (not CS.right_blinker_on) and (not CS.left_blinker_on) and \
(self.laneChange_enabled ==7):
self.laneChange_enabled = 1
self.laneChange_counter = 0
self.laneChange_direction = 0
CS.UE.custom_alert_message(-1, "", 0)
if (not CS.right_blinker_on) and (not CS.left_blinker_on) and \
(self.laneChange_enabled > 1):
# no blinkers on but we are still changing lane, so we need to send blinker command
if self.laneChange_direction == -1:
turn_signal_needed = 1
elif self.laneChange_direction == 1:
turn_signal_needed = 2
else:
turn_signal_needed = 0
if (CS.cstm_btns.get_button_status("alca") >
0) and self.alcaEnabled and (self.laneChange_enabled == 1):
if ((CS.v_ego < cl_min_v) or (abs(actuators.steerAngle) >= cl_max_a) or \
(abs(CS.angle_steers)>= cl_max_a) or (not enabled)):
CS.cstm_btns.set_button_status("alca", 9)
else:
CS.cstm_btns.set_button_status("alca", 1)
if self.alcaEnabled and enabled and (((not self.prev_right_blinker_on) and CS.right_blinker_on) or \
((not self.prev_left_blinker_on) and CS.left_blinker_on)) and \
((CS.v_ego < cl_min_v) or (abs(actuators.steerAngle) >= cl_max_a) or (abs(CS.angle_steers) >=cl_max_a)):
# something is not right, the speed or angle is limitting
CS.UE.custom_alert_message(3, "Auto Lane Change Unavailable!", 500,
3)
CS.cstm_btns.set_button_status("alca", 9)
if self.alcaEnabled and enabled and (((not self.prev_right_blinker_on) and CS.right_blinker_on) or \
((not self.prev_left_blinker_on) and CS.left_blinker_on)) and \
(CS.v_ego >= cl_min_v) and (abs(actuators.steerAngle) < cl_max_a):
# start blinker, speed and angle is within limits, let's go
laneChange_direction = 1
# changing lanes
if CS.left_blinker_on:
laneChange_direction = -1
if (self.laneChange_enabled > 1) and (self.laneChange_direction <>
laneChange_direction):
# something is not right; signal in oposite direction; cancel
CS.UE.custom_alert_message(3, "Auto Lane Change Canceled! (s)",
200, 5)
self.laneChange_enabled = 1
self.laneChange_counter = 0
self.laneChange_direction = 0
CS.cstm_btns.set_button_status("alca", 1)
elif (self.laneChange_enabled == 1):
# compute angle delta for lane change
CS.UE.custom_alert_message(2, "Auto Lane Change Engaged! (1)",
100)
self.laneChange_enabled = 5
self.laneChange_counter = 1
self.laneChange_direction = laneChange_direction
self.laneChange_avg_angle = 0.
self.laneChange_avg_count = 0.
self.laneChange_angled = self.laneChange_direction * self.laneChange_steerr * cl_maxd_a
self.laneChange_last_actuator_angle = 0.
self.laneChange_last_actuator_delta = 0.
self.laneChange_over_the_line = 0
CS.cstm_btns.set_button_status("alca", 2)
# reset PID for torque
self.pid.reset()
if (not self.alcaEnabled) and self.laneChange_enabled > 1:
self.laneChange_enabled = 1
self.laneChange_counter = 0
self.laneChange_direction = 0
# lane change in process
if self.laneChange_enabled > 1:
if (CS.steer_override or (CS.v_ego < cl_min_v)
or (not (CS.right_blinker_on or CS.left_blinker_on))):
CS.UE.custom_alert_message(4, "Auto Lane Change Canceled! (u)",
200, 3)
# if any steer override cancel process or if speed less than min speed
self.laneChange_counter = 0
self.laneChange_enabled = 1
self.laneChange_direction = 0
CS.cstm_btns.set_button_status("alca", 1)
if blindspot:
CS.UE.custom_alert_message(4, "Auto Lane Change Held! (b)",
200, 3)
# if blindspot detected cancel process
self.laneChange_counter = 0
self.laneChange_enabled = 5
#CS.cstm_btns.set_button_status("alca",1)
# now that we crossed the line, we need to get far enough from the line and then engage OP
# 1. we wait 0.05s to ensure we don't have back and forth adjustments
# 2. we check to see if the delta between our steering and the actuator continues to decrease or not
# 3. we check to see if the delta between our steering and the actuator is less than 5 deg
# 4. we disengage after 0.5s and let OP take over
if self.laneChange_enabled == 2:
laneChange_angle = self.laneChange_angled
if self.laneChange_counter == 1:
CS.UE.custom_alert_message(
2, "Auto Lane Change Engaged! (5)", 800)
self.laneChange_counter += 1
# check if angle continues to decrease
current_delta = abs(self.laneChange_angle + laneChange_angle +
(-actuators.steerAngle))
previous_delta = abs(self.laneChange_last_sent_angle -
self.laneChange_last_actuator_angle)
if (self.laneChange_counter > cl_lane_pass_time):
# continue to half the angle between our angle and actuator
delta_angle = (-actuators.steerAngle -
self.laneChange_angle -
self.laneChange_angled) / cl_adjust_factor
self.laneChange_angle += delta_angle
# wait 0.05 sec before starting to check if angle increases or if we are within X deg of actuator.angleSteer
if ((current_delta > previous_delta) or
(current_delta <= cl_reentry_angle)) and (
self.laneChange_counter > cl_lane_pass_time):
self.laneChange_enabled = 7
self.laneChange_counter = 1
self.laneChange_direction = 0
# we crossed the line, so x sec later give control back to OP
laneChange_after_lane_duration = cl_timea_t * self.laneChange_after_lane_duration_mult
if (self.laneChange_counter >
laneChange_after_lane_duration * 100):
self.laneChange_enabled = 7
self.laneChange_counter = 1
self.laneChange_direction = 0
# this is the main stage once we start turning
# we have to detect when to let go control back to OP or raise alarm if max timer passed
# there are three conditions we look for:
# 1. we can detect when we cross the lane, and then we let go control to OP
# 2. we passed the min timer to cross the line and the delta between actuator and our angle
# is less than the release angle, then we let go control to OP
# 3. the delta between our angle and the actuator is higher than the previous one
# (we cross the optimal path), then we let go control to OP
# 4. max time is achieved: alert and disengage
# CONTROL: during this time we use ALCA angle to steer (ALCA Control)
if self.laneChange_enabled == 3:
if self.laneChange_counter == 1:
CS.UE.custom_alert_message(
2, "Auto Lane Change Engaged! (4)", 800)
self.laneChange_over_the_line = 0
self.last10delta_reset()
self.keep_angle = False
self.laneChange_counter += 1
laneChange_angle = self.laneChange_angled
if (self.laneChange_over_the_line == 0):
# we didn't cross the line, so keep computing the actuator delta until it flips
actuator_delta = self.laneChange_direction * (
-actuators.steerAngle -
self.laneChange_last_actuator_angle)
actuator_ratio = (-actuators.steerAngle
) / self.laneChange_last_actuator_angle
if (actuator_ratio < 1) and (abs(actuator_delta) >
0.5 * cl_lane_detect_factor):
# sudden change in actuator angle or sign means we are on the other side of the line
self.laneChange_over_the_line = 1
self.laneChange_enabled = 2
self.laneChange_counter = 1
# didn't change the lane yet, check that we are not eversteering or understeering based on road curvature
"""
# code for v3.1 designed to turn more if lane moves away
n,a = self.last10delta_add(actuator_delta)
c = 5.
if a == 0:
a = 0.00001
if (abs(a) < CL_MIN_ACTUATOR_DELTA) and (self.keep_angle == False):
c = (a/abs(a)) * (CL_MIN_ACTUATOR_DELTA - abs(a)) / 10
self.laneChange_angle = self.laneChange_angle -self.laneChange_direction * CL_CORRECTION_FACTOR * c * 10
self.last10delta_correct(c)
#print a, c, actuator_delta, self.laneChange_angle
"""
#print a, c, actuator_delta, self.laneChange_angle
# code for v3.2 and above
a_delta = self.laneChange_direction * (self.laneChange_angle +
laneChange_angle -
(-actuators.steerAngle))
if (self.laneChange_over_the_line == 0) and (
(abs(a_delta) > cl_max_angle_delta *
self.laneChange_steerr) or (self.keep_angle)):
#steering more than what we wanted, need to adjust
self.keep_angle = True
self.laneChange_angle = -actuators.steerAngle + self.laneChange_direction * cl_max_angle_delta * self.laneChange_steerr - laneChange_angle
self.laneChange_angle = self.laneChange_angle * (
1 - self.laneChange_direction *
(1 - cl_correction_factor))
if self.laneChange_counter > (self.laneChange_duration) * 100:
self.laneChange_enabled = 1
self.laneChange_counter = 0
CS.UE.custom_alert_message(
4, "Auto Lane Change Canceled! (t)", 200, 5)
self.laneChange_counter = 0
CS.cstm_btns.set_button_status("alca", 1)
# this is the critical start of the turn
# here we will detect the angle to move; it is based on a speed determined angle but we have to also
# take in consideration what's happening with the delta of consecutive actuators
# if they go in the same direction with our turn we have to reset our turn angle because the actuator either
# is compensating for a turn in the road OR for same lane correction for the car
# CONTROL: during this time we use ALCA angle to steer (ALCA Control)
# TODO: - when actuator moves in the same direction with lane change, correct starting angle
if self.laneChange_enabled == 4:
if self.laneChange_counter == 1:
self.laneChange_angle = -actuators.steerAngle
CS.UE.custom_alert_message(
2, "Auto Lane Change Engaged! (3)", 100)
self.laneChange_angled = self.laneChange_direction * self.laneChange_steerr * cl_maxd_a
# if angle more than max angle allowed cancel; last chance to cancel on road curvature
if self.laneChange_counter == 2:
CS.UE.custom_alert_message(
2, "Auto Lane Change Engaged! (3)", 100)
if (abs(self.laneChange_angle) >
cl_max_a) and (self.laneChange_counter == 1):
CS.UE.custom_alert_message(
4, "Auto Lane Change Canceled! (a)", 200, 5)
self.laneChange_enabled = 1
self.laneChange_counter = 0
self.laneChange_direction = 0
CS.cstm_btns.set_button_status("alca", 1)
laneChange_angle = self.laneChange_strStartFactor * self.laneChange_angled * self.laneChange_counter / 50
self.laneChange_counter += 1
#laneChange_angle = self.laneChange_strStartFactor * self.laneChange_angled * self.laneChange_counter / 50
#delta_change = abs(self.laneChange_angle+ laneChange_angle + actuators.steerAngle) - self.laneChange_strStartMultiplier * abs(self.laneChange_angled)
laneChange_angle = self.laneChange_strStartFactor * self.laneChange_angled * self.laneChange_counter / 100
delta_change = abs(
self.laneChange_angle + laneChange_angle + actuators.
steerAngle) - cl_max_angle_delta * self.laneChange_steerr
if (self.laneChange_counter == 100) or (delta_change >= 0):
if (delta_change < 0):
# didn't achieve desired angle yet, so repeat
self.laneChange_counter = 2
self.laneChange_angle += laneChange_angle
laneChange_angle = 0.
else:
self.laneChange_enabled = 3
self.laneChange_counter = 1
self.laneChange_angled = laneChange_angle
# this is the first stage of the ALCAS
# here we wait for x seconds before we start the turn
# if at any point we detect hand on wheel, we let go of control and notify user
# the same test for hand on wheel is done at ANY stage throughout the lane change process
# CONTROL: during this stage we use the actuator angle to steer (OP Control)
if self.laneChange_enabled == 5:
if self.laneChange_counter == 1:
CS.UE.custom_alert_message(
2, "Auto Lane Change Engaged! (2)",
self.laneChange_wait * 100)
self.laneChange_counter += 1
if self.laneChange_counter > (self.laneChange_wait - 1) * 100:
self.laneChange_avg_angle += -actuators.steerAngle
self.laneChange_avg_count += 1
if self.laneChange_counter == self.laneChange_wait * 100:
self.laneChange_enabled = 4
self.laneChange_counter = 1
if self.laneChange_counter == 0:
if not blindspot:
if CS.right_blinker_on or CS.left_blinker_on:
self.laneChange_counter += 1
# this is the final stage of the ALCAS
# this just shows a message that we completed the lane change
# CONTROL: during this time we use the actuator angle to steer (OP Control)
if self.laneChange_enabled == 7:
if self.laneChange_counter == 1:
CS.UE.custom_alert_message(2, "Auto Lane Change Complete!",
300, 4)
CS.cstm_btns.set_button_status("alca", 1)
self.laneChange_counter += 1
alca_enabled = (self.laneChange_enabled > 1)
apply_angle = 0.
# save position of blinker stalk
self.prev_right_blinker_on = CS.right_blinker_on
self.prev_left_blinker_on = CS.left_blinker_on
# Angle if 0 we need to save it as a very small nonzero with the same sign as the prev one
self.laneChange_last_actuator_delta = -actuators.steerAngle - self.laneChange_last_actuator_angle
last_angle_sign = 1
if (self.laneChange_last_actuator_angle <> 0):
last_angle_sign = self.laneChange_last_actuator_angle / abs(
self.laneChange_last_actuator_angle)
if actuators.steerAngle == 0:
self.laneChange_last_actuator_angle = last_angle_sign * 0.00001
else:
self.laneChange_last_actuator_angle = -actuators.steerAngle
# determine what angle to send and send it
if (self.laneChange_enabled > 1) and (self.laneChange_enabled < 5):
apply_angle = self.laneChange_angle + laneChange_angle
else:
apply_angle = -actuators.steerAngle
self.laneChange_last_sent_angle = apply_angle
return [-apply_angle, alca_enabled, turn_signal_needed]
def update(self, enabled, CS, frame, actuators):
if self.alcaEnabled:
# ALCA enabled
new_angle = 0.
new_ALCA_Enabled = False
new_turn_signal = 0
new_angle, new_ALCA_Enabled, new_turn_signal = self.update_angle(
enabled, CS, frame, actuators)
output_steer = 0.
if new_ALCA_Enabled and (self.laneChange_enabled <
5) and not self.laneChange_steerByAngle:
#self.angle_steers_des = self.angle_steers_des_mpc # not declared
steers_max = interp(CS.v_ego, CS.CP.steerMaxBP,
CS.CP.steerMaxV)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
output_steer = self.pid.update(
new_angle,
CS.angle_steers,
check_saturation=(CS.v_ego > 10),
override=CS.steer_override,
feedforward=new_angle * (CS.v_ego**2),
speed=CS.v_ego,
deadzone=0.0)
else:
output_steer = actuators.steer
return [new_angle, output_steer, new_ALCA_Enabled, new_turn_signal]
else:
# ALCA disabled
return [actuators.steerAngle, actuators.steer, False, 0]
class LongControl(object):
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 reset(self, v_pid):
self.pid.reset()
self.v_pid = v_pid
def update(self, active, v_ego, brake_pressed, standstill, v_cruise,
v_target_lead, a_target, jerk_factor, CP):
# actuation limits
gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)
overshoot_allowance = 2.0 # overshoot allowed when changing accel sign
output_gb = self.last_output_gb
# limit max target speed based on cruise setting
v_target = min(v_target_lead, v_cruise * CV.KPH_TO_MS)
rate = 100.0
max_speed_delta_up = a_target[1] * 1.0 / rate
max_speed_delta_down = a_target[0] * 1.0 / rate
self.long_control_state = long_control_state_trans(active, self.long_control_state, v_ego,\
v_target, self.v_pid, output_gb, brake_pressed)
v_ego_pid = max(
v_ego, 0.3
) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
# *** long control behavior based on state
if self.long_control_state == LongCtrlState.off:
self.v_pid = v_ego_pid # do nothing
output_gb = 0.
self.pid.reset()
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
#reset v_pid close to v_ego if it was too far and new v_target is closer to v_ego
if ((self.v_pid > v_ego + overshoot_allowance)
and (v_target < self.v_pid)):
self.v_pid = max(v_target, v_ego + overshoot_allowance)
elif ((self.v_pid < v_ego - overshoot_allowance)
and (v_target > self.v_pid)):
self.v_pid = min(v_target, v_ego - overshoot_allowance)
# move v_pid no faster than allowed accel limits
if (v_target > self.v_pid + max_speed_delta_up):
self.v_pid += max_speed_delta_up
elif (v_target < self.v_pid + max_speed_delta_down):
self.v_pid += max_speed_delta_down
else:
self.v_pid = v_target
# to avoid too much wind up on acceleration, limit positive speed error
if CP.enableGas:
max_speed_error = interp(v_ego, _MAX_SPEED_ERROR_BP,
_MAX_SPEED_ERROR_V)
self.v_pid = min(self.v_pid, v_ego + max_speed_error)
self.pid.pos_limit = gas_max
self.pid.neg_limit = -brake_max
deadzone = interp(v_ego_pid, CP.longPidDeadzoneBP,
CP.longPidDeadzoneV)
output_gb = self.pid.update(self.v_pid,
v_ego_pid,
speed=v_ego_pid,
jerk_factor=jerk_factor,
deadzone=deadzone)
# intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping:
# TODO: use the standstill bit from CAN to detect movements, usually more accurate than looking at v_ego
if not standstill or output_gb > -brake_stopping_target:
output_gb -= stopping_brake_rate / rate
output_gb = clip(output_gb, -brake_max, gas_max)
self.v_pid = v_ego
self.pid.reset()
# intention is to move again, release brake fast before handling control to PID
elif self.long_control_state == LongCtrlState.starting:
if output_gb < -0.2:
output_gb += starting_brake_rate / rate
self.v_pid = v_ego
self.pid.reset()
self.pid.i = starting_Ui
self.last_output_gb = output_gb
final_gas = clip(output_gb, 0., gas_max)
final_brake = -clip(output_gb, -brake_max, 0.)
return final_gas, final_brake
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.
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
def reset(self):
self.pid.reset()
def adjust_angle_gain(self):
if (self.pid.f > 0) == (self.pid.i > 0) and abs(self.pid.i) >= abs(self.previous_integral):
self.angle_ff_gain *= 1.0001
else:
self.angle_ff_gain *= 0.9999
self.angle_ff_gain = max(1.0, self.angle_ff_gain)
self.previous_integral = self.pid.i
def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, CP, VM, path_plan):
pid_log = log.Live100Data.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()
self.previous_integral = 0.0
else:
self.angle_steers_des = interp(sec_since_boot(), path_plan.mpcTimes, path_plan.mpcAngles)
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:
angle_feedforward = steer_feedforward - path_plan.angleOffset
self.angle_ff_ratio = interp(abs(angle_feedforward), self.angle_ff_bp[0], self.angle_ff_bp[1])
angle_feedforward *= self.angle_ff_ratio * self.angle_ff_gain
rate_feedforward = (1.0 - self.angle_ff_ratio) * self.rate_ff_gain * path_plan.rateSteers
steer_feedforward = v_ego**2 * (rate_feedforward + angle_feedforward)
if v_ego > 10.0:
if abs(angle_steers) > (self.angle_ff_bp[0][1] / 2.0):
self.adjust_angle_gain()
else:
self.previous_integral = self.pid.i
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)
# Reset sat_flat always, set it only if needed
self.sat_flag = False
# If PID is saturated, set time which it was saturated
if self.pid.saturated and self.sat_time < 0.5:
self.sat_time = sec_since_boot()
# To save cycles, nest in sat_time check
if self.sat_time > 0.5:
# If its been saturated for 0.7 seconds then set flag
if (sec_since_boot() - self.sat_time) > 0.7:
self.sat_flag = True
# If it is no longer saturated, clear the sat flag and timer
if not self.pid.saturated:
self.sat_time = 0.0
if CP.steerControlType == car.CarParams.SteerControlType.torque:
return output_steer, path_plan.angleSteers, pid_log
else:
return self.angle_steers_des, path_plan.angleSteers
class LongControl(object):
def __init__(self, CP, compute_gb):
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,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
def reset(self, v_pid):
"""Reset PID controller and change setpoint"""
self.pid.reset()
self.v_pid = v_pid
def update(self, active, v_ego, brake_pressed, standstill,
cruise_standstill, v_cruise, v_target, v_target_future,
a_target, CP):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Actuation limits
gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)
# Update state machine
output_gb = self.last_output_gb
self.long_control_state = long_control_state_trans(
active, self.long_control_state, v_ego, v_target_future,
self.v_pid, output_gb, brake_pressed, cruise_standstill)
v_ego_pid = max(
v_ego, MIN_CAN_SPEED
) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
if self.long_control_state == LongCtrlState.off:
self.v_pid = v_ego_pid
self.pid.reset()
output_gb = 0.
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target
self.pid.pos_limit = gas_max
self.pid.neg_limit = -brake_max
# Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
prevent_overshoot = not CP.stoppingControl and v_ego < 1.5 and v_target_future < 0.7
deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP,
CP.longitudinalTuning.deadzoneV)
output_gb = self.pid.update(self.v_pid,
v_ego_pid,
speed=v_ego_pid,
deadzone=deadzone,
feedforward=a_target,
freeze_integrator=prevent_overshoot)
if prevent_overshoot:
output_gb = min(output_gb, 0.0)
# Intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping:
# Keep applying brakes until the car is stopped
if not standstill or output_gb > -BRAKE_STOPPING_TARGET:
output_gb -= STOPPING_BRAKE_RATE / RATE
output_gb = clip(output_gb, -brake_max, gas_max)
self.v_pid = v_ego
self.pid.reset()
# Intention is to move again, release brake fast before handing control to PID
elif self.long_control_state == LongCtrlState.starting:
if output_gb < -0.2:
output_gb += STARTING_BRAKE_RATE / RATE
self.v_pid = v_ego
self.pid.reset()
self.last_output_gb = output_gb
final_gas = clip(output_gb, 0., gas_max)
final_brake = -clip(output_gb, -brake_max, 0.)
return final_gas, final_brake
class LatControlPID(object):
def __init__(self, CP):
kegman_conf(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.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
kegman = kegman_conf()
if kegman.conf['tuneGernby'] == "1":
self.steerKpV = [float(kegman.conf['Kp'])]
self.steerKiV = [float(kegman.conf['Ki'])]
self.pid = PIController(
(CP.lateralTuning.pid.kpBP, self.steerKpV),
(CP.lateralTuning.pid.kiBP, self.steerKiV),
k_f=CP.lateralTuning.pid.kf,
pos_limit=1.0)
self.mpc_frame = 0
def update(self, active, v_ego, angle_steers, angle_steers_rate,
eps_torque, steer_override, CP, VM, path_plan):
self.live_tune(CP)
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 # 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