def update(self, active, CS, CP, lat_plan):
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
if CS.vEgo < 0.3 or not active:
output_steer = 0.0
pid_log.active = False
self.pid.reset()
else:
self.angle_steers_des = lat_plan.steeringAngleDeg # get from MPC/LateralPlanner
steers_max = get_steer_max(CP, CS.vEgo)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
steer_feedforward = self.angle_steers_des # feedforward desired angle
if CP.steerControlType == car.CarParams.SteerControlType.torque:
# TODO: feedforward something based on lat_plan.rateSteers
steer_feedforward -= lat_plan.angleOffsetDeg # subtract the offset, since it does not contribute to resistive torque
steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel)
deadzone = 0.0
check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
output_steer = self.pid.update(self.angle_steers_des, CS.steeringAngleDeg, check_saturation=check_saturation, override=CS.steeringPressed,
feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone)
pid_log.active = True
pid_log.p = self.pid.p
pid_log.i = self.pid.i
pid_log.f = self.pid.f
pid_log.output = output_steer
pid_log.saturated = bool(self.pid.saturated)
return output_steer, float(self.angle_steers_des), pid_log
def 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:
self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner
steers_max = get_steer_max(CP, v_ego)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
steer_feedforward = self.angle_steers_des # feedforward desired angle
if CP.steerControlType == car.CarParams.SteerControlType.torque:
# TODO: feedforward something based on path_plan.rateSteers
steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque
steer_feedforward *= v_ego**2 # proportional to realigning tire momentum (~ lateral accel)
deadzone = 0.0
output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override,
feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone)
pid_log.active = True
pid_log.p = self.pid.p
pid_log.i = self.pid.i
pid_log.f = self.pid.f
pid_log.output = output_steer
pid_log.saturated = bool(self.pid.saturated)
self.sat_flag = self.pid.saturated
return output_steer, float(self.angle_steers_des), pid_log
def 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
# offset does not contribute to resistive torque
steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
deadzone = 0.0
check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
output_steer = self.pid.update(angle_steers_des, CS.steeringAngleDeg, check_saturation=check_saturation, override=CS.steeringPressed,
feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone)
pid_log.active = True
pid_log.p = self.pid.p
pid_log.i = self.pid.i
pid_log.f = self.pid.f
pid_log.output = output_steer
pid_log.saturated = bool(self.pid.saturated)
return output_steer, angle_steers_des, pid_log
def update(self, active, CS, CP, path_plan):
lqr_log = log.ControlsState.LateralLQRState.new_message()
steering_angle = CS.steeringAngle
# if CS.v_ego > 8:
# self.new_scale = interp(abs(steering_angle), self.angle_differ_range, self.scale_range)
steers_max = get_steer_max(CP, CS.vEgo)
torque_scale = (0.45 +
CS.vEgo / 60.0)**2 # Scale actuator model with speed
# Subtract offset. Zero angle should correspond to zero torque
self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset
steering_angle -= path_plan.angleOffset
# Update Kalman filter
angle_steers_k = float(self.C.dot(self.x_hat))
e = steering_angle - angle_steers_k
self.x_hat = self.A.dot(self.x_hat) + self.B.dot(
CS.steeringTorqueEps / torque_scale) + self.L.dot(e)
if CS.vEgo < 0.3 or not active:
lqr_log.active = False
lqr_output = 0.
self.reset()
else:
lqr_log.active = True
# LQR
u_lqr = float(self.angle_steers_des / self.dc_gain -
self.K.dot(self.x_hat))
lqr_output = torque_scale * u_lqr / self.scale #new_scale #
# Integrator
if CS.steeringPressed:
self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
else:
error = self.angle_steers_des - angle_steers_k
i = self.i_lqr + self.ki * self.i_rate * error
control = lqr_output + i
if (error >= 0 and (control <= steers_max or i < 0.0)) or \
(error <= 0 and (control >= -steers_max or i > 0.0)):
self.i_lqr = i
self.output_steer = lqr_output + self.i_lqr
self.output_steer = clip(self.output_steer, -steers_max,
steers_max)
check_saturation = (
CS.vEgo >
10) and not CS.steeringRateLimited and not CS.steeringPressed
saturated = self._check_saturation(self.output_steer, check_saturation,
steers_max)
lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset
lqr_log.i = self.i_lqr
lqr_log.output = self.output_steer
lqr_log.lqrOutput = lqr_output
lqr_log.saturated = saturated
return self.output_steer, float(self.angle_steers_des), lqr_log
def update(self, active, CS, CP, VM, params, last_actuators,
desired_curvature, desired_curvature_rate):
lqr_log = log.ControlsState.LateralLQRState.new_message()
steers_max = get_steer_max(CP, CS.vEgo)
torque_scale = (0.45 +
CS.vEgo / 60.0)**2 # Scale actuator model with speed
# Subtract offset. Zero angle should correspond to zero torque
steering_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetAverageDeg
desired_angle = math.degrees(
VM.get_steer_from_curvature(-desired_curvature, CS.vEgo,
params.roll))
instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg
desired_angle += instant_offset # Only add offset that originates from vehicle model errors
lqr_log.steeringAngleDesiredDeg = desired_angle
# Update Kalman filter
angle_steers_k = float(self.C.dot(self.x_hat))
e = steering_angle_no_offset - angle_steers_k
self.x_hat = self.A.dot(self.x_hat) + self.B.dot(
CS.steeringTorqueEps / torque_scale) + self.L.dot(e)
if CS.vEgo < MIN_STEER_SPEED or not active:
lqr_log.active = False
lqr_output = 0.
output_steer = 0.
self.reset()
else:
lqr_log.active = True
# LQR
u_lqr = float(desired_angle / self.dc_gain -
self.K.dot(self.x_hat))
lqr_output = torque_scale * u_lqr / self.scale
# Integrator
if CS.steeringPressed:
self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
else:
error = desired_angle - angle_steers_k
i = self.i_lqr + self.ki * self.i_rate * error
control = lqr_output + i
if (error >= 0 and (control <= steers_max or i < 0.0)) or \
(error <= 0 and (control >= -steers_max or i > 0.0)):
self.i_lqr = i
output_steer = lqr_output + self.i_lqr
output_steer = clip(output_steer, -steers_max, steers_max)
lqr_log.steeringAngleDeg = angle_steers_k
lqr_log.i = self.i_lqr
lqr_log.output = output_steer
lqr_log.lqrOutput = lqr_output
lqr_log.saturated = self._check_saturation(
steers_max - abs(output_steer) < 1e-3, CS)
return output_steer, desired_angle, lqr_log
def update(self, active, v_ego, angle_steers, angle_steers_rate,
eps_torque, steer_override, rate_limited, CP, path_plan):
self.tune.check() # Added for utune
lqr_log = log.ControlsState.LateralLQRState.new_message()
steers_max = get_steer_max(CP, v_ego)
torque_scale = (0.45 +
v_ego / 60.0)**2 # Scale actuator model with speed
torque_scale = min(torque_scale, 0.69)
# Subtract offset. Zero angle should correspond to zero torque
self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset
angle_steers -= path_plan.angleOffset
# Update Kalman filter
angle_steers_k = float(self.C.dot(self.x_hat))
e = angle_steers - angle_steers_k
self.x_hat = self.A.dot(self.x_hat) + self.B.dot(
eps_torque / torque_scale) + self.L.dot(e)
if v_ego < 0.3 or not active:
lqr_log.active = False
lqr_output = 0.
self.reset()
else:
lqr_log.active = True
# LQR
u_lqr = float(self.angle_steers_des / self.dc_gain -
self.K.dot(self.x_hat))
lqr_output = torque_scale * u_lqr / self.scale
# Integrator
if steer_override:
self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
else:
error = self.angle_steers_des - angle_steers_k
i = self.i_lqr + self.ki * self.i_rate * error
control = lqr_output + i
if ((error >= 0 and (control <= steers_max or i < 0.0)) or \
(error <= 0 and (control >= -steers_max or i > 0.0))):
self.i_lqr = i
self.output_steer = lqr_output + self.i_lqr
self.output_steer = clip(self.output_steer, -steers_max,
steers_max)
check_saturation = (v_ego >
10) and not rate_limited and not steer_override
saturated = self._check_saturation(self.output_steer, check_saturation,
steers_max)
lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset
lqr_log.i = self.i_lqr
lqr_log.output = self.output_steer
lqr_log.lqrOutput = lqr_output
lqr_log.saturated = saturated
return self.output_steer, float(self.angle_steers_des), lqr_log
def update(self, active, CS, CP, path_plan):
# Update Kalman filter
y = np.array([[math.radians(CS.steeringAngle)], [math.radians(CS.steeringRate)]])
self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)
indi_log = log.ControlsState.LateralINDIState.new_message()
indi_log.steerAngle = math.degrees(self.x[0])
indi_log.steerRate = math.degrees(self.x[1])
indi_log.steerAccel = math.degrees(self.x[2])
if CS.vEgo < 0.3 or not active:
indi_log.active = False
self.output_steer = 0.0
self.delayed_output = 0.0
self.angle_steers_des = path_plan.angleSteers
else:
self.angle_steers_des = path_plan.angleSteers
self.rate_steers_des = path_plan.rateSteers
steers_des = math.radians(self.angle_steers_des)
rate_des = math.radians(self.rate_steers_des)
# Expected actuator value
self.delayed_output = self.delayed_output * self.alpha + self.output_steer * (1. - self.alpha)
# Compute acceleration error
rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des
accel_sp = self.inner_loop_gain * (rate_sp - self.x[1])
accel_error = accel_sp - self.x[2]
# Compute change in actuator
g_inv = 1. / self.G
delta_u = g_inv * accel_error
# Enforce rate limit
if self.enforce_rate_limit:
steer_max = float(SteerLimitParams.STEER_MAX)
new_output_steer_cmd = steer_max * (self.delayed_output + delta_u)
prev_output_steer_cmd = steer_max * self.output_steer
new_output_steer_cmd = apply_toyota_steer_torque_limits(new_output_steer_cmd, prev_output_steer_cmd, prev_output_steer_cmd, SteerLimitParams)
self.output_steer = new_output_steer_cmd / steer_max
else:
self.output_steer = self.delayed_output + delta_u
steers_max = get_steer_max(CP, CS.vEgo)
self.output_steer = clip(self.output_steer, -steers_max, steers_max)
indi_log.active = True
indi_log.rateSetPoint = float(rate_sp)
indi_log.accelSetPoint = float(accel_sp)
indi_log.accelError = float(accel_error)
indi_log.delayedOutput = float(self.delayed_output)
indi_log.delta = float(delta_u)
indi_log.output = float(self.output_steer)
check_saturation = (CS.vEgo > 10.) and not CS.steeringRateLimited and not CS.steeringPressed
indi_log.saturated = self._check_saturation(self.output_steer, check_saturation, steers_max)
return float(self.output_steer), float(self.angle_steers_des), indi_log
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
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,
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 < MIN_STEER_SPEED 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)
# torque for steer rate. ~0 angle, steer rate ~= steer command.
steer_rate_actual = CS.steeringRateDeg
steer_rate_desired = math.degrees(
VM.get_steer_from_curvature(-desired_curvature_rate, CS.vEgo,
0))
speed_mph = CS.vEgo * CV.MS_TO_MPH
steer_rate_max = 0.0389837 * speed_mph**2 - 5.34858 * speed_mph + 223.831
steer_feedforward += ((steer_rate_desired - steer_rate_actual) /
steer_rate_max)
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 = self._check_saturation(
steers_max - abs(output_steer) < 1e-3, CS)
return output_steer, angle_steers_des, pid_log
def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, CP, VM, path_plan):
# Update Kalman filter
y = np.matrix([[math.radians(angle_steers)], [math.radians(angle_steers_rate)]])
self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)
indi_log = log.Live100Data.LateralINDIState.new_message()
indi_log.steerAngle = math.degrees(self.x[0])
indi_log.steerRate = math.degrees(self.x[1])
indi_log.steerAccel = math.degrees(self.x[2])
if v_ego < 0.3 or not active:
indi_log.active = False
self.output_steer = 0.0
self.delayed_output = 0.0
else:
self.angle_steers_des = path_plan.angleSteers
self.rate_steers_des = path_plan.rateSteers
steers_des = math.radians(self.angle_steers_des)
rate_des = math.radians(self.rate_steers_des)
# Expected actuator value
self.delayed_output = self.delayed_output * self.alpha + self.output_steer * (1. - self.alpha)
# Compute acceleration error
rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des
accel_sp = self.inner_loop_gain * (rate_sp - self.x[1])
accel_error = accel_sp - self.x[2]
# Compute change in actuator
g_inv = 1. / self.G
delta_u = g_inv * accel_error
# Enforce rate limit
if self.enfore_rate_limit:
steer_max = float(SteerLimitParams.STEER_MAX)
new_output_steer_cmd = steer_max * (self.delayed_output + delta_u)
prev_output_steer_cmd = steer_max * self.output_steer
new_output_steer_cmd = apply_toyota_steer_torque_limits(new_output_steer_cmd, prev_output_steer_cmd, prev_output_steer_cmd, SteerLimitParams)
self.output_steer = new_output_steer_cmd / steer_max
else:
self.output_steer = self.delayed_output + delta_u
steers_max = get_steer_max(CP, v_ego)
self.output_steer = clip(self.output_steer, -steers_max, steers_max)
indi_log.active = True
indi_log.rateSetPoint = float(rate_sp)
indi_log.accelSetPoint = float(accel_sp)
indi_log.accelError = float(accel_error)
indi_log.delayedOutput = float(self.delayed_output)
indi_log.delta = float(delta_u)
indi_log.output = float(self.output_steer)
self.sat_flag = False
return float(self.output_steer), float(self.angle_steers_des), indi_log
def update(self, active, CS, CP, path_plan):
self.live_tune(CP)
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steerAngle = float(CS.steeringAngle)
pid_log.steerRate = float(CS.steeringRate)
if CS.vEgo < 0.3 or not active:
output_steer = 0.0
pid_log.active = False
self.pid.reset()
else:
self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner
self.angle_steer_new = interp(CS.vEgo, self.angleBP,
self.angle_steer_rate)
check_pingpong = abs(self.angle_steers_des -
self.angle_steers_des_last) > 4.0
if check_pingpong:
self.angle_steers_des = path_plan.angleSteers * self.angle_steer_new
steers_max = get_steer_max(CP, CS.vEgo)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
steer_feedforward = self.angle_steers_des # feedforward desired angle
self.angle_steers_des_last = self.angle_steers_des
if CP.steerControlType == car.CarParams.SteerControlType.torque:
# TODO: feedforward something based on path_plan.rateSteers
steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque
#steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel)
_c1, _c2, _c3 = [
0.35189607550172824, 7.506201251644202, 69.226826411091
]
steer_feedforward *= _c1 * CS.vEgo**2 + _c2 * CS.vEgo + _c3
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
def update(self, active, CS, CP, VM, params, lat_plan):
if self.params.get_bool("OpkrLiveTune"):
self.live_tune(CP)
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
angle_steers_des_no_offset = math.degrees(
VM.get_steer_from_curvature(-lat_plan.curvature, CS.vEgo))
angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
if CS.vEgo < 0.3 or not active:
output_steer = 0.0
pid_log.active = False
self.pid.reset()
else:
steers_max = get_steer_max(CP, CS.vEgo)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
# TODO: feedforward something based on lat_plan.rateSteers
steer_feedforward = angle_steers_des_no_offset # offset does not contribute to resistive torque
if self.new_kf_tuned:
_c1, _c2, _c3 = 0.35189607550172824, 7.506201251644202, 69.226826411091
steer_feedforward *= _c1 * CS.vEgo**2 + _c2 * CS.vEgo + _c3
else:
steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel)
deadzone = float(
int(self.params.get("IgnoreZone", encoding="utf8")) *
0.1) if self.params.get("IgnoreZone",
encoding="utf8") is not None else 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, 0, pid_log
def update(self, active, v_ego, angle_steers, angle_steers_rate,
eps_torque, steer_override, CP, VM, path_plan):
lqr_log = log.ControlsState.LateralLQRState.new_message()
torque_scale = (0.45 +
v_ego / 60.0)**2 # Scale actuator model with speed
# Subtract offset. Zero angle should correspond to zero torque
self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset
angle_steers -= path_plan.angleOffset
# Update Kalman filter
angle_steers_k = float(self.C.dot(self.x_hat))
e = angle_steers - angle_steers_k
self.x_hat = self.A.dot(self.x_hat) + self.B.dot(
eps_torque / torque_scale) + self.L.dot(e)
if v_ego < 0.3 or not active:
lqr_log.active = False
self.reset()
else:
lqr_log.active = True
# LQR
u_lqr = float(self.angle_steers_des / self.dc_gain -
self.K.dot(self.x_hat))
# Integrator
if steer_override:
self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
else:
self.i_lqr += self.ki * self.i_rate * (self.angle_steers_des -
angle_steers_k)
lqr_output = torque_scale * u_lqr / self.scale
self.i_lqr = clip(
self.i_lqr, -1.0 - lqr_output,
1.0 - lqr_output) # (LQR + I) has to be between -1 and 1
self.output_steer = lqr_output + self.i_lqr
# Clip output
steers_max = get_steer_max(CP, v_ego)
self.output_steer = clip(self.output_steer, -steers_max,
steers_max)
lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset
lqr_log.i = self.i_lqr
lqr_log.output = self.output_steer
return self.output_steer, float(self.angle_steers_des), lqr_log
def update(self, active, CS, CP, path_plan):
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steerAngle = float(CS.steeringAngle)
pid_log.steerRate = float(CS.steeringRate)
if CS.vEgo < 0.3 or not active:
output_steer = 0.0
pid_log.active = False
self.pid.reset()
else:
self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner
steers_max = get_steer_max(CP, CS.vEgo)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
steer_feedforward = self.angle_steers_des # feedforward desired angle
if CP.steerControlType == car.CarParams.SteerControlType.torque:
# TODO: feedforward something based on path_plan.rateSteers
steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque
_c1, _c2, _c3 = [
0.34365576041121065, 12.845373070976711, 51.63304088261174
]
steer_feedforward *= _c1 * CS.vEgo**2 + _c2 * CS.vEgo + _c3
deadzone = 0.0
check_saturation = (
CS.vEgo >
10) and not CS.steeringRateLimited and not CS.steeringPressed
output_steer = self.pid.update(self.angle_steers_des,
CS.steeringAngle,
check_saturation=check_saturation,
override=CS.steeringPressed,
feedforward=steer_feedforward,
speed=CS.vEgo,
deadzone=deadzone)
pid_log.active = True
pid_log.p = self.pid.p
pid_log.i = self.pid.i
pid_log.f = self.pid.f
pid_log.output = output_steer
pid_log.saturated = bool(self.pid.saturated)
return output_steer, float(self.angle_steers_des), pid_log
def update(self, active, v_ego, angle_steers, angle_steers_rate,
steer_override, CP, VM, path_plan):
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steerAngle = float(angle_steers)
pid_log.steerRate = float(angle_steers_rate)
if v_ego < 0.3 or not active:
output_steer = 0.0
pid_log.active = False
self.pid.reset()
else:
# TODO: ideally we should interp, but for tuning reasons we keep the mpc solution
# constant for 0.05s.
#dt = min(cur_time - self.angle_steers_des_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps
#self.angle_steers_des = self.angle_steers_des_prev + (dt / _DT_MPC) * (self.angle_steers_des_mpc - self.angle_steers_des_prev)
self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner
steers_max = get_steer_max(CP, v_ego)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
steer_feedforward = self.angle_steers_des # feedforward desired angle
if CP.steerControlType == car.CarParams.SteerControlType.torque:
# TODO: feedforward something based on path_plan.rateSteers
steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque
steer_feedforward *= v_ego**2 # proportional to realigning tire momentum (~ lateral accel)
deadzone = 0.0
output_steer = self.pid.update(self.angle_steers_des,
angle_steers,
check_saturation=(v_ego > 10),
override=steer_override,
feedforward=steer_feedforward,
speed=v_ego,
deadzone=deadzone)
pid_log.active = True
pid_log.p = self.pid.p
pid_log.i = self.pid.i
pid_log.f = self.pid.f
pid_log.output = output_steer
pid_log.saturated = bool(self.pid.saturated)
self.sat_flag = self.pid.saturated
return output_steer, float(self.angle_steers_des), pid_log
def update(self, active, CS, CP, VM, params, lat_plan):
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(-lat_plan.curvature, CS.vEgo))
angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
if CS.vEgo < 0.3 or not active:
output_steer = 0.0
pid_log.active = False
self.pid.reset()
else:
steers_max = get_steer_max(CP, CS.vEgo)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
# TODO: feedforward something based on lat_plan.rateSteers
steer_feedforward = angle_steers_des_no_offset # offset does not contribute to resistive torque
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, 0, pid_log
def update(self, active, CS, CP, VM, params, lat_plan):
self.speed = CS.vEgo
self.RC = interp(self.speed, self._RC[0], self._RC[1])
self.G = interp(self.speed, self._G[0], self._G[1])
self.outer_loop_gain = interp(self.speed, self._outer_loop_gain[0], self._outer_loop_gain[1])
self.inner_loop_gain = interp(self.speed, self._inner_loop_gain[0], self._inner_loop_gain[1])
if self.params.get_bool("OpkrLiveTune"):
self.live_tune(CP)
# Update Kalman filter
y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]])
self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)
indi_log = log.ControlsState.LateralINDIState.new_message()
indi_log.steeringAngleDeg = math.degrees(self.x[0])
indi_log.steeringRateDeg = math.degrees(self.x[1])
indi_log.steeringAccelDeg = math.degrees(self.x[2])
if CS.vEgo < 0.3 or not active:
indi_log.active = False
self.output_steer = 0.0
self.delayed_output = 0.0
else:
steers_des = VM.get_steer_from_curvature(-lat_plan.curvature, CS.vEgo)
steers_des += math.radians(params.angleOffsetDeg)
rate_des = VM.get_steer_from_curvature(-lat_plan.curvatureRate, CS.vEgo)
# Expected actuator value
alpha = 1. - DT_CTRL / (self.RC + DT_CTRL)
self.delayed_output = self.delayed_output * alpha + self.output_steer * (1. - alpha)
# Compute acceleration error
rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des
accel_sp = self.inner_loop_gain * (rate_sp - self.x[1])
accel_error = accel_sp - self.x[2]
# Compute change in actuator
g_inv = 1. / self.G
delta_u = g_inv * accel_error
# If steering pressed, only allow wind down
if CS.steeringPressed and (delta_u * self.output_steer > 0):
delta_u = 0
# Enforce rate limit
if self.enforce_rate_limit:
steer_max = float(CarControllerParams.STEER_MAX)
new_output_steer_cmd = steer_max * (self.delayed_output + delta_u)
prev_output_steer_cmd = steer_max * self.output_steer
new_output_steer_cmd = apply_toyota_steer_torque_limits(new_output_steer_cmd, prev_output_steer_cmd, prev_output_steer_cmd, CarControllerParams)
self.output_steer = new_output_steer_cmd / steer_max
else:
self.output_steer = self.delayed_output + delta_u
steers_max = get_steer_max(CP, CS.vEgo)
self.output_steer = clip(self.output_steer, -steers_max, steers_max)
indi_log.active = True
indi_log.rateSetPoint = float(rate_sp)
indi_log.accelSetPoint = float(accel_sp)
indi_log.accelError = float(accel_error)
indi_log.delayedOutput = float(self.delayed_output)
indi_log.delta = float(delta_u)
indi_log.output = float(self.output_steer)
check_saturation = (CS.vEgo > 10.) and not CS.steeringRateLimited and not CS.steeringPressed
indi_log.saturated = self._check_saturation(self.output_steer, check_saturation, steers_max)
return float(self.output_steer), 0, indi_log
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
def update(self, active, CS, CP, VM, params, last_actuators,
desired_curvature, desired_curvature_rate):
self.speed = CS.vEgo
# Update Kalman filter
y = np.array([[math.radians(CS.steeringAngleDeg)],
[math.radians(CS.steeringRateDeg)]])
self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)
indi_log = log.ControlsState.LateralINDIState.new_message()
indi_log.steeringAngleDeg = math.degrees(self.x[0])
indi_log.steeringRateDeg = math.degrees(self.x[1])
indi_log.steeringAccelDeg = math.degrees(self.x[2])
steers_des = VM.get_steer_from_curvature(-desired_curvature, CS.vEgo,
params.roll)
steers_des += math.radians(params.angleOffsetDeg)
indi_log.steeringAngleDesiredDeg = math.degrees(steers_des)
rate_des = VM.get_steer_from_curvature(-desired_curvature_rate,
CS.vEgo, 0)
indi_log.steeringRateDesiredDeg = math.degrees(rate_des)
if CS.vEgo < MIN_STEER_SPEED or not active:
indi_log.active = False
self.steer_filter.x = 0.0
output_steer = 0
else:
# Expected actuator value
self.steer_filter.update_alpha(self.RC)
self.steer_filter.update(last_actuators.steer)
# Compute acceleration error
rate_sp = self.outer_loop_gain * (steers_des -
self.x[0]) + rate_des
accel_sp = self.inner_loop_gain * (rate_sp - self.x[1])
accel_error = accel_sp - self.x[2]
# Compute change in actuator
g_inv = 1. / self.G
delta_u = g_inv * accel_error
# If steering pressed, only allow wind down
if CS.steeringPressed and (delta_u * last_actuators.steer > 0):
delta_u = 0
output_steer = self.steer_filter.x + delta_u
steers_max = get_steer_max(CP, CS.vEgo)
output_steer = clip(output_steer, -steers_max, steers_max)
indi_log.active = True
indi_log.rateSetPoint = float(rate_sp)
indi_log.accelSetPoint = float(accel_sp)
indi_log.accelError = float(accel_error)
indi_log.delayedOutput = float(self.steer_filter.x)
indi_log.delta = float(delta_u)
indi_log.output = float(output_steer)
indi_log.saturated = self._check_saturation(
steers_max - abs(output_steer) < 1e-3, CS)
return float(output_steer), float(steers_des), indi_log
def update(self, active, CS, CP, path_plan):
lqr_log = log.ControlsState.LateralLQRState.new_message()
steers_max = get_steer_max(CP, CS.vEgo)
torque_scale = (0.45 +
CS.vEgo / 60.0)**2 # Scale actuator model with speed
#neokii
torque_scale = min(torque_scale, 0.65)
steering_angle = CS.steeringAngle
steeringTQ = CS.steeringTorqueEps
v_ego_kph = CS.vEgo * CV.MS_TO_KPH
self.ki, self.scale = self.atom_tune(v_ego_kph, CS.steeringAngle, CP)
# ### 설정값 최적화 분석을 위한 랜덤화 임시 코드
#now = datetime.datetime.now() # current date and time
#micro_S = int(now.microsecond)
#if micro_S < 10000 : #1초에 한번만 랜덤변환
# self.ki = random.uniform(0.015, 0.025) #self.ki - (self.ki*0.5), self.ki + (self.ki*0.5) )
# self.scale = random.uniform(1750, 1950) #int(self.scale) - int(self.scale*0.055), int(self.scale) + int(self.scale*0.055) ) )
# self.dc_gain = random.uniform(0.0028, 0.0032) #self.dc_gain - (self.dc_gain*0.1), self.dc_gain + (self.dc_gain*0.1) )
# steers_max = random.uniform(1.0, 1.2)
# ###########################
log_ki = self.ki
log_scale = self.scale
log_dc_gain = self.dc_gain
# Subtract offset. Zero angle should correspond to zero torque
self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset
steering_angle -= path_plan.angleOffset
# Update Kalman filter
angle_steers_k = float(self.C.dot(self.x_hat))
e = steering_angle - angle_steers_k
self.x_hat = self.A.dot(self.x_hat) + self.B.dot(
CS.steeringTorqueEps / torque_scale) + self.L.dot(e)
error = self.angle_steers_des - angle_steers_k
u_lqr = float(self.angle_steers_des / self.dc_gain -
self.K.dot(self.x_hat))
if CS.vEgo < 0.3 or not active:
lqr_log.active = False
lqr_output = 0.
self.reset()
else:
lqr_log.active = True
# LQR
#u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat))
lqr_output = torque_scale * u_lqr / self.scale
# Integrator
if CS.steeringPressed:
self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
else:
#error = self.angle_steers_des - angle_steers_k
i = self.i_lqr + self.ki * self.i_rate * error
control = lqr_output + i
if (error >= 0 and (control <= steers_max or i < 0.0)) or \
(error <= 0 and (control >= -steers_max or i > 0.0)):
self.i_lqr = i
self.output_steer = lqr_output + self.i_lqr
self.output_steer = clip(self.output_steer, -steers_max,
steers_max)
check_saturation = (
CS.vEgo >
10) and not CS.steeringRateLimited and not CS.steeringPressed
saturated = self._check_saturation(self.output_steer, check_saturation,
steers_max)
if not CS.steeringPressed:
str2 = '/{} /{} /{} /{} /{} /{} /{} /{} /{} /{}'.format(
v_ego_kph, steering_angle, self.angle_steers_des,
angle_steers_k, steeringTQ, torque_scale, log_scale, log_ki,
log_dc_gain, self.output_steer)
self.trLQR.add(str2)
str5 = 'LQR_Set:dc_gain={:06.4f}/scale={:06.1f}/ki={:05.3f}/OutputSteer={:5.3f}/Angle={:5.1f}|{:5.1f}'.format(
self.scale, self.dc_gain, self.ki, self.output_steer,
steering_angle, angle_steers_k)
trace1.printf2(str5)
lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset
lqr_log.i = self.i_lqr
lqr_log.output = self.output_steer
lqr_log.lqrOutput = lqr_output
lqr_log.saturated = saturated
return self.output_steer, float(self.angle_steers_des), lqr_log
def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, blinkers_on, CP, VM, path_plan, live_params):
if angle_steers_rate == 0.0 and self.calculate_rate:
if angle_steers != self.prev_angle_steers:
self.steer_counter_prev = self.steer_counter
self.rough_steers_rate = (self.rough_steers_rate + 100.0 * (angle_steers - self.prev_angle_steers) / self.steer_counter_prev) / 2.0
self.steer_counter = 0.0
elif self.steer_counter >= self.steer_counter_prev:
self.rough_steers_rate = (self.steer_counter * self.rough_steers_rate) / (self.steer_counter + 1.0)
self.steer_counter += 1.0
angle_steers_rate = self.rough_steers_rate
else:
# If non-zero angle_rate is provided, stop calculating angle rate
self.calculate_rate = False
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steerAngle = float(angle_steers)
pid_log.steerRate = float(angle_steers_rate)
max_bias_change = 0.0002 / (abs(self.angle_bias) + 0.0001)
self.angle_bias = float(clip(live_params.angleOffset - live_params.angleOffsetAverage, self.angle_bias - max_bias_change, self.angle_bias + max_bias_change))
self.live_tune(CP)
if v_ego < 0.3 or not active:
output_steer = 0.0
self.lane_changing = 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
pid_log.active = False
self.pid.reset()
else:
self.angle_steers_des = path_plan.angleSteers
if not self.driver_assist_hold:
self.damp_angle_steers_des += (interp(sec_since_boot() + self.damp_mpc + self.react_mpc, path_plan.mpcTimes, path_plan.mpcAngles) - self.damp_angle_steers_des) / max(1.0, self.damp_mpc * 100.)
self.damp_rate_steers_des += (interp(sec_since_boot() + self.damp_mpc + self.react_mpc, path_plan.mpcTimes, path_plan.mpcRates) - self.damp_rate_steers_des) / max(1.0, self.damp_mpc * 100.)
self.damp_angle_steers += (angle_steers - self.angle_bias + self.damp_time * angle_steers_rate - self.damp_angle_steers) / max(1.0, self.damp_time * 100.)
else:
self.damp_angle_steers = angle_steers
self.damp_angle_steers_des = self.damp_angle_steers + self.driver_assist_offset
if steer_override and abs(self.damp_angle_steers) > abs(self.damp_angle_steers_des) and self.pid.saturated:
self.damp_angle_steers_des = self.damp_angle_steers
steers_max = get_steer_max(CP, v_ego)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
angle_feedforward = float(self.damp_angle_steers_des - path_plan.angleOffset)
self.angle_ff_ratio = interp(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.damp_rate_steers_des
steer_feedforward = float(v_ego)**2 * (rate_feedforward + angle_feedforward * self.angle_ff_ratio * self.angle_ff_gain)
if len(self.poly_scale) > 0:
if abs(self.damp_angle_steers_des) > abs(self.damp_angle_steers):
self.cur_poly_scale += 0.05 * (interp(abs(self.damp_rate_steers_des), self.poly_scale[0], self.poly_scale[1]) - self.cur_poly_scale)
else:
self.cur_poly_scale += 0.05 * (interp(abs(self.damp_rate_steers_des), self.poly_scale[0], self.poly_scale[2]) - self.cur_poly_scale)
else:
self.cur_poly_scale = 1.0
if len(self.steer_p_scale) > 0:
if abs(self.damp_angle_steers_des) > abs(self.damp_angle_steers):
p_scale = interp(abs(angle_feedforward), self.steer_p_scale[0], self.steer_p_scale[1])
else:
p_scale = interp(abs(angle_feedforward), self.steer_p_scale[0], self.steer_p_scale[2])
else:
p_scale = 1.0
if CP.carName == "honda" and steer_override and not self.prev_override and not self.driver_assist_hold and self.pid.saturated and abs(angle_steers) < abs(self.damp_angle_steers_des) and not blinkers_on:
self.driver_assist_hold = True
self.driver_assist_offset = self.damp_angle_steers_des - self.damp_angle_steers
else:
self.driver_assist_hold = steer_override and self.driver_assist_hold
self.path_error = float(v_ego) * float(self.get_projected_path_error(v_ego, angle_feedforward, angle_steers, live_params, path_plan, VM)) * self.poly_factor * self.cur_poly_scale * self.angle_ff_gain
if self.driver_assist_hold and not steer_override and abs(angle_steers) > abs(self.damp_angle_steers_des):
#self.angle_bias = 0.0
driver_opposing_i = False
elif (steer_override and self.pid.saturated) or self.driver_assist_hold or self.lane_changing > 0.0 or blinkers_on:
#self.angle_bias = 0.0
self.path_error = 0.0
if self.gernbySteer and 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
driver_opposing_i = steer_override and self.pid.i * self.pid.p > 0 and not self.pid.saturated and not self.driver_assist_hold
deadzone = 0.0
output_steer = self.pid.update(self.damp_angle_steers_des, self.damp_angle_steers, check_saturation=(v_ego > 10), override=driver_opposing_i,
add_error=float(self.path_error), feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone, p_scale=p_scale)
driver_opposing_op = steer_override and (angle_steers - self.prev_angle_steers) * output_steer < 0
self.update_lane_state(angle_steers, driver_opposing_op, blinkers_on, path_plan)
output_steer *= self.lane_change_adjustment
pid_log.active = True
pid_log.p = float(self.pid.p)
pid_log.i = float(self.pid.i)
pid_log.f = float(self.pid.f)
pid_log.p2 = float(self.pid.p2)
pid_log.output = float(output_steer)
pid_log.saturated = bool(self.pid.saturated)
pid_log.angleFFRatio = self.angle_ff_ratio
pid_log.angleBias = self.angle_bias
self.prev_angle_steers = angle_steers
self.prev_override = steer_override
self.sat_flag = self.pid.saturated
return output_steer, float(self.angle_steers_des), pid_log
def update(self, active, v_ego, angle_steers, angle_steers_rate,
eps_torque, steer_override, rate_limited, CP, 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)
v_ego_kph = v_ego * CV.MS_TO_KPH
if v_ego < 0.3 or not active:
output_steer = 0.0
pid_log.active = False
self.pid.reset()
self.angle_steers_des = self.movAvg.get_data(
path_plan.angleSteers, 500)
else:
if v_ego_kph < 10:
self.angle_steers_des = self.movAvg.get_data(
path_plan.angleSteers, 200)
elif v_ego_kph < 20:
self.angle_steers_des = self.movAvg.get_data(
path_plan.angleSteers, 100)
elif v_ego_kph < 30:
self.angle_steers_des = self.movAvg.get_data(
path_plan.angleSteers, 50)
elif v_ego_kph < 40:
self.angle_steers_des = self.movAvg.get_data(
path_plan.angleSteers, 10)
else:
self.angle_steers_des = self.movAvg.get_data(
path_plan.angleSteers, 5)
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 = 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
def update(self, active, v_ego, angle_steers, angle_steers_advance,
angle_steers_rate, steer_override, blinkers_on, CP, VM,
path_plan, live_mpc, logMonoTime):
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steerAngle = float(angle_steers)
if logMonoTime['pathPlan'] != self.last_plan_time:
if self.path_age > 0.23: self.old_plan_count += 1
self.last_plan_time = logMonoTime['pathPlan']
path_age = sec_since_boot() - logMonoTime['pathPlan'] * 1e-9
self.avg_plan_age += 0.01 * (path_age - self.avg_plan_age)
self.plan_index = max(
0, min(24, int(100 * (self.react_mpc + path_age))))
#self.lane_error = self.poly_factor * path_plan.cPoly[0] + self.polyReact*(path_plan.cPoly[len(path_plan.cPoly)-1]-path_plan.cPoly[3])
#if path_plan.cPoly[0] > 0:
# self.lane_error += self.poly_factor
#elif path_plan.cPoly[0] < 0:
# self.lane_error -= self.poly_factor
self.projected_lane_error = self.polyReact * 0.001 * sum(
self.poly_range * self.poly_range *
path_plan.cPoly) #-path_plan.cPoly[3])
else:
self.plan_index += 1
self.min_index = min(self.min_index, self.plan_index)
self.max_index = max(self.max_index, self.plan_index)
if self.frame % 300 == 0:
#print(path_plan.mpcAngles)
#print(path_plan.mpcRates)
print(
"old plans: %d avg plan age: %0.3f min index: %d max_index: %d angles: %d poly_react: %d"
% (self.old_plan_count, self.avg_plan_age, self.min_index,
self.max_index, len(path_plan.cPoly), self.polyReact))
self.min_index = 100
self.max_index = 0
self.frame += 1
self.live_tune(CP)
if (v_ego < 0.3 or
not active): # or self.plan_index > len(path_plan.mpcAngles)):
output_steer = 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.angle_bias = 0.0
pid_log.active = False
self.pid.reset()
else:
try:
self.angle_steers_des = path_plan.mpcAngles[self.plan_index //
7]
#self.angle_steers_des += ((path_plan.mpcAngles[self.plan_index//7] - path_plan.mpcAngles[(self.plan_index//7)-1])/7) * self.plan_index % 7
#self.damp_angle_steers_des = self.angle_steers_des
#print(" %0.1f %0.1f %0.1f %0.1f %d %d" % (self.damp_rate_steers_des, self.angle_steers_des, self.damp_angle_steers_des, path_plan.mpcAngles[self.plan_index//7], self.plan_index,self.plan_index//7 ))
#self.angle_steers_des = path_plan.mpcAngles[self.plan_index]
self.damp_angle_steers_des += (
self.angle_steers_des - self.damp_angle_steers_des) / max(
1.0, self.damp_mpc * 100.)
#self.damp_rate_steers_des += ((path_plan.mpcAngles[self.plan_index//7] - path_plan.mpcAngles[(self.plan_index//7)-1]) - self.damp_rate_steers_des) / max(1.0, self.damp_mpc * 100.)
except:
pass
self.damp_angle_steers = angle_steers # += (angle_steers + angle_steers_rate * self.damp_time - self.damp_angle_steers) / max(1.0, self.damp_time * 100.)
self.damp_angle_rate = angle_steers_rate #+= (angle_steers_rate - self.damp_angle_rate) / max(1.0, self.damp_time * 100.)
steers_max = get_steer_max(CP, v_ego)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
angle_feedforward = float(self.damp_angle_steers_des -
path_plan.angleOffset)
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.damp_rate_steers_des
steer_feedforward = float(v_ego)**2 * (
rate_feedforward +
angle_feedforward * self.angle_ff_ratio * self.angle_ff_gain)
#self.lane_compensation += self.poly_factor * path_plan.cPoly[0] * 1 if self.lane_compensation * path_plan.cPoly[0] > 1 else 10
self.path_error_comp += (
v_ego * self.projected_lane_error * self.angle_ff_gain -
self.path_error_comp) / self.poly_smoothing
#self.path_error_comp += ((self.projected_lane_error + self.lane_compensation) - self.path_error_comp) / self.poly_smoothing
if self.gernbySteer and 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
p_scale = 1.0
deadzone = 0.0
driver_opposing_i = steer_override
#output_steer = self.pid.update(self.angle_steers_des + self.path_error_comp, self.damp_angle_steers, check_saturation=(v_ego > 10), override=driver_opposing_i,
# feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone, p_scale=p_scale)
print("%0.3f %0.3f %0.3f " %
(self.path_error_comp, self.angle_steers_des,
self.damp_angle_steers))
output_steer = self.pid.update(self.damp_angle_steers_des,
self.damp_angle_steers,
check_saturation=(v_ego > 10),
override=driver_opposing_i,
add_error=float(
self.path_error_comp),
feedforward=steer_feedforward,
speed=v_ego,
deadzone=deadzone,
p_scale=p_scale)
pid_log.active = True
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.pid.p2
) #float(self.path_error_comp) * float(self.pid._k_p[1][0])
pid_log.saturated = bool(self.pid.saturated)
pid_log.angleFFRatio = self.angle_ff_ratio
pid_log.angleBias = self.angle_bias
#if self.frame % 100 == 0:
# print(path_plan)
self.prev_angle_steers = angle_steers
self.prev_override = steer_override
self.sat_flag = self.pid.saturated
return output_steer, float(self.angle_steers_des), pid_log
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
def update(self, active, CS, CP, path_plan):
lqr_log = log.ControlsState.LateralLQRState.new_message()
steers_max = get_steer_max(CP, CS.vEgo)
torque_scale = (0.45 + CS.vEgo / 60.0)**2 # Scale actuator model with speed
#neokii
#torque_scale = min(torque_scale, 0.65)
steering_angle = CS.steeringAngle
steeringTQ = CS.steeringTorqueEps
v_ego_kph = CS.vEgo * CV.MS_TO_KPH
self.ki, self.scale = self.atom_tune( v_ego_kph, CS.steeringAngle, CP )
self.tune.check() # neokii 추가
# Subtract offset. Zero angle should correspond to zero torque
self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset
steering_angle -= path_plan.angleOffset
# Update Kalman filter
angle_steers_k = float(self.C.dot(self.x_hat))
e = steering_angle - angle_steers_k
self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e)
error = self.angle_steers_des - angle_steers_k
u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat))
if CS.vEgo < 0.3 or not active:
lqr_log.active = False
lqr_output = 0.
self.reset()
else:
lqr_log.active = True
# LQR
#u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat))
lqr_output = torque_scale * u_lqr / self.scale
# Integrator
if CS.steeringPressed:
self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
else:
#error = self.angle_steers_des - angle_steers_k
i = self.i_lqr + self.ki * self.i_rate * error
control = lqr_output + i
if (error >= 0 and (control <= steers_max or i < 0.0)) or \
(error <= 0 and (control >= -steers_max or i > 0.0)):
self.i_lqr = i
self.output_steer = lqr_output + self.i_lqr
self.output_steer = clip(self.output_steer, -steers_max, steers_max)
check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
saturated = self._check_saturation(self.output_steer, check_saturation, steers_max)
if CS.steeringPressed:
whoissteering = "Driver"
else:
whoissteering = "Openpilot"
# str2 = '/{} /{} /{} /{} /{} /{} /{} /{} /{} /{} /{}'.format(
# v_ego_kph, steering_angle, self.angle_steers_des, angle_steers_k, steeringTQ, torque_scale, self.dc_gain, self.scale, self.ki, self.output_steer, whoissteering)
# self.trLQR.add( str2 )
# str5 = 'LQR_Set:dcgain={:06.4f}/scale={:06.1f}/ki={:05.3f}/tq={:4.1f}/u={:5.1f}/i={:5.3f}/O={:5.3f}'.format(
# self.dc_gain, self.scale, self.ki, steeringTQ, u_lqr, self.i_lqr, self.output_steer )
# trace1.printf2( str5 )
lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset
lqr_log.i = self.i_lqr
lqr_log.output = self.output_steer
lqr_log.lqrOutput = lqr_output
lqr_log.saturated = saturated
return self.output_steer, float(self.angle_steers_des), lqr_log
def update(self, active, CS, CP, VM, params, last_actuators, curvature, curvature_rate):
self.speed = CS.vEgo
# Update Kalman filter
y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]])
self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)
indi_log = log.ControlsState.LateralINDIState.new_message()
indi_log.steeringAngleDeg = math.degrees(self.x[0])
indi_log.steeringRateDeg = math.degrees(self.x[1])
indi_log.steeringAccelDeg = math.degrees(self.x[2])
steers_des = VM.get_steer_from_curvature(-curvature, CS.vEgo, params.roll)
steers_des += math.radians(params.angleOffsetDeg)
indi_log.steeringAngleDesiredDeg = math.degrees(steers_des)
rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo, 0)
indi_log.steeringRateDesiredDeg = math.degrees(rate_des)
if CS.vEgo < 0.3 or not active:
indi_log.active = False
self.output_steer = 0.0
self.steer_filter.x = 0.0
else:
# Expected actuator value
self.steer_filter.update_alpha(self.RC)
self.steer_filter.update(self.output_steer)
# Compute acceleration error
rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des
accel_sp = self.inner_loop_gain * (rate_sp - self.x[1])
accel_error = accel_sp - self.x[2]
# Compute change in actuator
g_inv = 1. / self.G
delta_u = g_inv * accel_error
# If steering pressed, only allow wind down
if CS.steeringPressed and (delta_u * self.output_steer > 0):
delta_u = 0
# Enforce rate limit
if self.enforce_rate_limit:
steer_max = float(CarControllerParams.STEER_MAX)
new_output_steer_cmd = steer_max * (self.steer_filter.x + delta_u)
prev_output_steer_cmd = steer_max * self.output_steer
new_output_steer_cmd = apply_toyota_steer_torque_limits(new_output_steer_cmd, prev_output_steer_cmd, prev_output_steer_cmd, CarControllerParams)
self.output_steer = new_output_steer_cmd / steer_max
else:
self.output_steer = self.steer_filter.x + delta_u
steers_max = get_steer_max(CP, CS.vEgo)
self.output_steer = clip(self.output_steer, -steers_max, steers_max)
indi_log.active = True
indi_log.rateSetPoint = float(rate_sp)
indi_log.accelSetPoint = float(accel_sp)
indi_log.accelError = float(accel_error)
indi_log.delayedOutput = float(self.steer_filter.x)
indi_log.delta = float(delta_u)
indi_log.output = float(self.output_steer)
check_saturation = (CS.vEgo > 10.) and not CS.steeringRateLimited and not CS.steeringPressed
indi_log.saturated = self._check_saturation(self.output_steer, check_saturation, steers_max)
return float(self.output_steer), float(steers_des), indi_log
