def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd,
hud_alert, audible_alert):
# this seems needed to avoid steering faults and to force the sync with the EPS counter
if self.prev_frame == frame:
return
# *** compute control surfaces ***
# steer torque
apply_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = apply_toyota_steer_torque_limits(apply_steer,
self.apply_steer_last,
CS.steer_torque_motor,
SteerLimitParams)
moving_fast = CS.v_ego > CS.CP.minSteerSpeed # for status message
lkas_active = moving_fast and enabled
if not lkas_active:
apply_steer = 0
self.apply_steer_last = apply_steer
if audible_alert in LOUD_ALERTS:
self.send_chime = True
can_sends = []
#*** control msgs ***
if self.send_chime:
new_msg = create_chimes(AudibleAlert)
can_sends.append(new_msg)
if audible_alert not in LOUD_ALERTS:
self.send_chime = False
if pcm_cancel_cmd:
# TODO: would be better to start from frame_2b3
new_msg = create_wheel_buttons(self.ccframe)
can_sends.append(new_msg)
# frame is 100Hz (0.01s period)
if (self.ccframe % 10 == 0): # 0.1s period
new_msg = create_lkas_heartbit(self.car_fingerprint)
can_sends.append(new_msg)
if (self.ccframe % 25 == 0): # 0.25s period
new_msg = create_lkas_hud(self.packer, CS.gear_shifter,
lkas_active, hud_alert,
self.car_fingerprint, self.hud_count)
can_sends.append(new_msg)
self.hud_count += 1
new_msg = create_lkas_command(self.packer, int(apply_steer), frame)
can_sends.append(new_msg)
self.ccframe += 1
self.prev_frame = frame
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, sendcan, enabled, CS, frame, actuators,
pcm_cancel_cmd, hud_alert, audible_alert):
# this seems needed to avoid steering faults and to force the sync with the EPS counter
frame = CS.lkas_counter
if self.prev_frame == frame:
return
# *** compute control surfaces ***
# steer torque
apply_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = apply_toyota_steer_torque_limits(apply_steer, self.apply_steer_last,
CS.steer_torque_motor, SteerLimitParams)
moving_fast = CS.v_ego > CS.CP.minSteerSpeed # for status message
if CS.v_ego > (CS.CP.minSteerSpeed - 0.5): # for command high bit
self.gone_fast_yet = True
elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.JEEP_CHEROKEE_2019):
if CS.v_ego < (CS.CP.minSteerSpeed - 3.0):
self.gone_fast_yet = False # < 14.5m/s stock turns off this bit, but fine down to 13.5
lkas_active = moving_fast and enabled
if not lkas_active:
apply_steer = 0
self.apply_steer_last = apply_steer
if audible_alert in LOUD_ALERTS:
self.send_chime = True
can_sends = []
#*** control msgs ***
if self.send_chime:
new_msg = create_chimes(AudibleAlert)
can_sends.append(new_msg)
if audible_alert not in LOUD_ALERTS:
self.send_chime = False
if pcm_cancel_cmd:
# TODO: would be better to start from frame_2b3
new_msg = create_wheel_buttons(self.ccframe)
can_sends.append(new_msg)
# LKAS_HEARTBIT is forwarded by Panda so no need to send it here.
# frame is 100Hz (0.01s period)
if (self.ccframe % 25 == 0): # 0.25s period
if (CS.lkas_car_model != -1):
new_msg = create_lkas_hud(
self.packer, CS.gear_shifter, lkas_active, hud_alert,
self.hud_count, CS.lkas_car_model)
can_sends.append(new_msg)
self.hud_count += 1
new_msg = create_lkas_command(self.packer, int(apply_steer), self.gone_fast_yet, frame)
can_sends.append(new_msg)
self.ccframe += 1
self.prev_frame = frame
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
# this seems needed to avoid steering faults and to force the sync with the EPS counter
frame = CS.lkas_counter
if self.prev_frame == frame:
return car.CarControl.Actuators.new_message(), []
# steer torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorqueEps,
CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
moving_fast = CS.out.vEgo > CS.CP.minSteerSpeed # for status message
if CS.out.vEgo > (CS.CP.minSteerSpeed - 0.5): # for command high bit
self.gone_fast_yet = True
elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID,
CAR.PACIFICA_2020,
CAR.JEEP_CHEROKEE_2019):
if CS.out.vEgo < (CS.CP.minSteerSpeed - 3.0):
self.gone_fast_yet = False # < 14.5m/s stock turns off this bit, but fine down to 13.5
lkas_active = moving_fast and enabled
if not lkas_active:
apply_steer = 0
self.apply_steer_last = apply_steer
can_sends = []
#*** control msgs ***
if pcm_cancel_cmd:
# TODO: would be better to start from frame_2b3
new_msg = create_wheel_buttons(self.packer,
self.ccframe,
cancel=True)
can_sends.append(new_msg)
# LKAS_HEARTBIT is forwarded by Panda so no need to send it here.
# frame is 100Hz (0.01s period)
if (self.ccframe % 25 == 0): # 0.25s period
if (CS.lkas_car_model != -1):
new_msg = create_lkas_hud(self.packer, CS.out.gearShifter,
lkas_active, hud_alert,
self.hud_count, CS.lkas_car_model)
can_sends.append(new_msg)
self.hud_count += 1
new_msg = create_lkas_command(self.packer, int(apply_steer),
self.gone_fast_yet, frame)
can_sends.append(new_msg)
self.ccframe += 1
self.prev_frame = frame
new_actuators = actuators.copy()
new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
return new_actuators, can_sends
def update(self, CC, CS, low_speed_alert):
can_sends = []
# EPS faults if LKAS re-enables too quickly
lkas_active = CC.latActive and not low_speed_alert and (
self.frame - self.last_lkas_falling_edge > 200)
# *** control msgs ***
das_bus = 2 if self.CP.carFingerprint in RAM_CARS else 0
if CC.cruiseControl.cancel:
can_sends.append(
create_cruise_buttons(self.packer,
CS.button_counter + 1,
das_bus,
cancel=True))
elif CC.enabled and CS.out.cruiseState.standstill:
can_sends.append(
create_cruise_buttons(self.packer,
CS.button_counter + 1,
das_bus,
resume=True))
# HUD alerts
if self.frame % 25 == 0:
if CS.lkas_car_model != -1:
can_sends.append(
create_lkas_hud(self.packer, self.CP, lkas_active,
CC.hudControl.visualAlert, self.hud_count,
CS.lkas_car_model, CS.auto_high_beam))
self.hud_count += 1
# steering
if self.frame % 2 == 0:
# steer torque
new_steer = int(
round(CC.actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorqueEps,
CarControllerParams)
if not lkas_active:
apply_steer = 0
self.steer_rate_limited = new_steer != apply_steer
self.apply_steer_last = apply_steer
idx = self.frame // 2
can_sends.append(
create_lkas_command(self.packer, self.CP, int(apply_steer),
lkas_active, idx))
self.frame += 1
if not lkas_active and self.lkas_active_prev:
self.last_lkas_falling_edge = self.frame
self.lkas_active_prev = lkas_active
new_actuators = CC.actuators.copy()
new_actuators.steer = self.apply_steer_last / CarControllerParams.STEER_MAX
return new_actuators, can_sends
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, 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, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
# this seems needed to avoid steering faults and to force the sync with the EPS counter
frame = CS.lkas_counter
if self.prev_frame == frame:
return car.CarControl.Actuators.new_message(), []
# Steering Torque
new_steer = int(round(actuators.steer * 1024))
apply_steer = apply_toyota_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorqueEps,
CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
# disable when temp fault is active, or below LKA minimum speed
#lkas_active = enabled and not CS.out.steerFaultTemporary and CS.out.vEgo >= CS.CP.minSteerSpeed
lkas_active = enabled
if CS.out.leftTurn or CS.out.rightTurn:
apply_steer = 0
if not lkas_active:
apply_steer = 0
self.apply_steer_last = apply_steer
can_sends = []
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
if (frame % fr_step == 0): # 25 0.25s period
can_sends.append([addr, bus, vl, 0])
#if (self.ccframe % 20 == 0):
# #new_msg = create_lkas_hud(self.packer, CS.lkas_status, left_lane, right_lane, left_lane_depart, right_lane_depart )
# #can_sends.append(new_msg)
# self.hud_count += 1
new_msg = create_lkas_command(self.packer, CS.lkas_run, self.lkascnt,
int(apply_steer))
can_sends.append(new_msg)
self.lkascnt += 1
#cloudlog.warning("LANDROVER LKAS (%s)", new_msg)
self.ccframe += 1
self.prev_frame = frame
new_actuators = actuators.copy()
new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
return new_actuators, can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart,
right_lane_depart):
#if not enabled:
# self.sm.update(0)
#if self.sm.updated['pathPlan']:
# blinker = CS.out.leftBlinker or CS.out.rightBlinker
# ldw_allowed = CS.out.vEgo > 12.5 and not blinker
# CAMERA_OFFSET = op_params.get('camera_offset', 0.06)
# right_lane_visible = self.sm['pathPlan'].rProb > 0.5
# left_lane_visible = self.sm['pathPlan'].lProb > 0.5
# self.rightLaneDepart = bool(ldw_allowed and self.sm['pathPlan'].rPoly[3] > -(0.93 + CAMERA_OFFSET) and right_lane_visible)
# self.leftLaneDepart = bool(ldw_allowed and self.sm['pathPlan'].lPoly[3] < (0.93 - CAMERA_OFFSET) and left_lane_visible)
# print("blinker")
# print(blinker)
# print("ldw_allowed")
# print(ldw_allowed)
# print("CAMERA_OFFSET")
# print(CAMERA_OFFSET)
# print("right_lane_visible")
# print(right_lane_visible)
# print("left_lane_visible")
# print(left_lane_visible)
# print("self.rightLaneDepart")
# print(self.rightLaneDepart)
# print("self.leftLaneDepart")
# print(self.leftLaneDepart)
# *** compute control surfaces ***
# gas and brake
apply_gas = clip(actuators.gas, 0., 1.)
if CS.CP.enableGasInterceptor:
# send only negative accel if interceptor is detected. otherwise, send the regular value
# +0.06 offset to reduce ABS pump usage when OP is engaged
apply_accel = 0.06 - actuators.brake
else:
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(
apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
if CS.CP.enableGasInterceptor:
if CS.out.gasPressed:
apply_accel = max(apply_accel, 0.06)
if CS.out.brakePressed:
apply_gas = 0.0
apply_accel = min(apply_accel, 0.00)
else:
if CS.out.gasPressed:
apply_accel = max(apply_accel, 0.0)
if CS.out.brakePressed and CS.out.vEgo > 1:
apply_accel = min(apply_accel, 0.0)
# steer torque
new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
# only cut torque when steer state is a known fault
if CS.steer_state in [9, 25]:
self.last_fault_frame = frame
# Cut steering for 1s after fault
if (frame - self.last_fault_frame < 100) or abs(
CS.out.steeringRate) > 100 or (abs(CS.out.steeringAngle) > 100
and CS.CP.carFingerprint
in [CAR.RAV4H, CAR.PRIUS]):
new_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
if not enabled and right_lane_depart and CS.out.vEgo > 12.5 and not CS.out.rightBlinker:
new_steer = self.last_steer + 3
new_steer = min(new_steer, 800)
print("right")
print(new_steer)
apply_steer_req = 1
if not enabled and left_lane_depart and CS.out.vEgo > 12.5 and not CS.out.leftBlinker:
new_steer = self.last_steer - 3
new_steer = max(new_steer, -800)
print("left")
print(new_steer)
apply_steer_req = 1
apply_steer = apply_toyota_steer_torque_limits(
new_steer, self.last_steer, CS.out.steeringTorqueEps,
SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
if not enabled and abs(apply_steer) > 800 and not (right_lane_depart or
left_lane_depart):
apply_steer = 0
apply_steer_req = 0
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if CS.out.standstill and not self.last_standstill:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_accel = apply_accel
self.last_standstill = CS.out.standstill
can_sends = []
#*** control msgs ***
#print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
if Ecu.fwdCamera in self.fake_ecus:
can_sends.append(
create_steer_command(self.packer, apply_steer, apply_steer_req,
frame))
# we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (
pcm_cancel_cmd and Ecu.fwdCamera in self.fake_ecus):
lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged
# Lexus IS uses a different cancellation message
if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
can_sends.append(create_acc_cancel_command(self.packer))
elif CS.CP.openpilotLongitudinalControl:
can_sends.append(
create_accel_command(self.packer, apply_accel,
pcm_cancel_cmd, self.standstill_req,
lead))
else:
can_sends.append(
create_accel_command(self.packer, 0, pcm_cancel_cmd, False,
lead))
if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(
create_gas_command(self.packer, apply_gas, frame // 2))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
fcw_alert = hud_alert == VisualAlert.fcw
steer_alert = hud_alert == VisualAlert.steerRequired
send_ui = False
if ((fcw_alert or steer_alert) and not self.alert_active) or \
(not (fcw_alert or steer_alert) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
elif pcm_cancel_cmd:
# forcing the pcm to disengage causes a bad fault sound so play a good sound instead
send_ui = True
if (frame % 100 == 0 or send_ui) and Ecu.fwdCamera in self.fake_ecus:
can_sends.append(
create_ui_command(self.packer, steer_alert, pcm_cancel_cmd,
left_line, right_line, left_lane_depart,
right_lane_depart))
if frame % 100 == 0 and (Ecu.dsu in self.fake_ecus
or Ecu.unknown in self.fake_ecus):
can_sends.append(create_fcw_command(self.packer, fcw_alert))
#*** static msgs ***
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
if frame % fr_step == 0 and ecu in self.fake_ecus and CS.CP.carFingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus))
# Enable blindspot debug mode once
if frame > 1000 and not (
CS.CP.carFingerprint in TSS2_CAR or CS.CP.carFingerprint
== CAR.CAMRY): # 10 seconds after start and not a tss2 car
if BLINDSPOTALWAYSON:
self.blindspot_blink_counter_left += 1
self.blindspot_blink_counter_right += 1
#print("debug blindspot alwayson!")
elif CS.out.leftBlinker:
self.blindspot_blink_counter_left += 1
#print("debug Left Blinker on")
elif CS.out.rightBlinker:
self.blindspot_blink_counter_right += 1
#print("debug Right Blinker on")
else:
self.blindspot_blink_counter_left = 0
self.blindspot_blink_counter_right = 0
if self.blindspot_debug_enabled_left:
can_sends.append(
set_blindspot_debug_mode(LEFT_BLINDSPOT, False))
#can_sends.append(make_can_msg(0x750, b'\x41\x02\x10\x01\x00\x00\x00\x00', 0))
self.blindspot_debug_enabled_left = False
#print ("debug Left blindspot debug disabled")
if self.blindspot_debug_enabled_right:
can_sends.append(
set_blindspot_debug_mode(RIGHT_BLINDSPOT, False))
#can_sends.append(make_can_msg(0x750, b'\x42\x02\x10\x01\x00\x00\x00\x00', 0))
self.blindspot_debug_enabled_right = False
#print("debug Right blindspot debug disabled")
if self.blindspot_blink_counter_left > 9 and not self.blindspot_debug_enabled_left: #check blinds
can_sends.append(set_blindspot_debug_mode(
LEFT_BLINDSPOT, True))
#can_sends.append(make_can_msg(0x750, b'\x41\x02\x10\x60\x00\x00\x00\x00', 0))
#print("debug Left blindspot debug enabled")
self.blindspot_debug_enabled_left = True
if self.blindspot_blink_counter_right > 5 and not self.blindspot_debug_enabled_right: #enable blindspot debug mode
if CS.out.vEgo > 6: #polling at low speeds switches camera off
#can_sends.append(make_can_msg(0x750, b'\x42\x02\x10\x60\x00\x00\x00\x00', 0))
can_sends.append(
set_blindspot_debug_mode(RIGHT_BLINDSPOT, True))
#print("debug Right blindspot debug enabled")
self.blindspot_debug_enabled_right = True
if CS.out.vEgo < 6 and self.blindspot_debug_enabled_right: # if enabled and speed falls below 6m/s
#can_sends.append(make_can_msg(0x750, b'\x42\x02\x10\x01\x00\x00\x00\x00', 0))
can_sends.append(
set_blindspot_debug_mode(RIGHT_BLINDSPOT, False))
self.blindspot_debug_enabled_right = False
#print("debug Right blindspot debug disabled")
if self.blindspot_debug_enabled_left:
if frame % 20 == 0 and frame > 1001: # Poll blindspots at 5 Hz
can_sends.append(poll_blindspot_status(LEFT_BLINDSPOT))
#can_sends.append(make_can_msg(0x750, b'\x41\x02\x21\x69\x00\x00\x00\x00', 0))
#print("debug Left blindspot poll")
if self.blindspot_debug_enabled_right:
if frame % 20 == 10 and frame > 1005: # Poll blindspots at 5 Hz
#can_sends.append(make_can_msg(0x750, b'\x42\x02\x21\x69\x00\x00\x00\x00', 0))
can_sends.append(poll_blindspot_status(RIGHT_BLINDSPOT))
#print("debug Right blindspot poll")
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart, right_lane_depart):
# *** compute control surfaces ***
# gas and brake
apply_gas = clip(actuators.gas, 0., 1.)
if CS.CP.enableGasInterceptor:
# send only negative accel if interceptor is detected. otherwise, send the regular value
# +0.06 offset to reduce ABS pump usage when OP is engaged
apply_accel = 0.06 - actuators.brake
else:
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# Get the angle from ALCA.
alca_enabled = False
alca_steer = 0.
alca_angle = 0.
turn_signal_needed = 0
# Update ALCA status and custom button every 0.1 sec.
if self.ALCA.pid == None:
self.ALCA.set_pid(CS)
if (frame % 10 == 0):
self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0)
# steer torque
new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
# only cut torque when steer state is a known fault
if CS.steer_state in [9, 25]:
self.last_fault_frame = frame
# Cut steering for 2s after fault
if not enabled or (frame - self.last_fault_frame < 200):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if CS.out.standstill and not self.last_standstill:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_accel = apply_accel
self.last_standstill = CS.out.standstill
can_sends = []
#*** control msgs ***
#print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
if Ecu.fwdCamera in self.fake_ecus:
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
# we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (pcm_cancel_cmd and Ecu.fwdCamera in self.fake_ecus):
lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged
# Lexus IS uses a different cancellation message
if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
can_sends.append(create_acc_cancel_command(self.packer))
elif CS.CP.openpilotLongitudinalControl:
can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead))
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead))
if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(create_gas_command(self.packer, apply_gas, frame//2))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
alert_out = process_hud_alert(hud_alert)
steer, fcw = alert_out
if (any(alert_out) and not self.alert_active) or \
(not any(alert_out) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
else:
send_ui = False
# disengage msg causes a bad fault sound so play a good sound instead
if pcm_cancel_cmd:
send_ui = True
if (frame % 100 == 0 or send_ui) and Ecu.fwdCamera in self.fake_ecus:
can_sends.append(create_ui_command(self.packer, steer, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart))
if frame % 100 == 0 and Ecu.dsu in self.fake_ecus:
can_sends.append(create_fcw_command(self.packer, fcw))
#*** static msgs ***
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus))
return can_sends
def update(self, CC, CS):
can_sends = []
lkas_active = CC.latActive and self.lkas_control_bit_prev
# cruise buttons
if (self.frame - self.last_button_frame) * DT_CTRL > 0.05:
das_bus = 2 if self.CP.carFingerprint in RAM_CARS else 0
# ACC cancellation
if CC.cruiseControl.cancel:
self.last_button_frame = self.frame
can_sends.append(
create_cruise_buttons(self.packer,
CS.button_counter + 1,
das_bus,
cancel=True))
# ACC resume from standstill
elif CC.cruiseControl.resume:
self.last_button_frame = self.frame
can_sends.append(
create_cruise_buttons(self.packer,
CS.button_counter + 1,
das_bus,
resume=True))
# HUD alerts
if self.frame % 25 == 0:
if CS.lkas_car_model != -1:
can_sends.append(
create_lkas_hud(self.packer, self.CP, lkas_active,
CC.hudControl.visualAlert, self.hud_count,
CS.lkas_car_model, CS.auto_high_beam))
self.hud_count += 1
# steering
if self.frame % 2 == 0:
# TODO: can we make this more sane? why is it different for all the cars?
lkas_control_bit = self.lkas_control_bit_prev
if CS.out.vEgo > self.CP.minSteerSpeed:
lkas_control_bit = True
elif self.CP.carFingerprint in (CAR.PACIFICA_2019_HYBRID,
CAR.PACIFICA_2020,
CAR.JEEP_CHEROKEE_2019):
if CS.out.vEgo < (self.CP.minSteerSpeed - 3.0):
lkas_control_bit = False
elif self.CP.carFingerprint in RAM_CARS:
if CS.out.vEgo < (self.CP.minSteerSpeed - 0.5):
lkas_control_bit = False
# EPS faults if LKAS re-enables too quickly
lkas_control_bit = lkas_control_bit and (
self.frame - self.last_lkas_falling_edge > 200)
if not lkas_control_bit and self.lkas_control_bit_prev:
self.last_lkas_falling_edge = self.frame
self.lkas_control_bit_prev = lkas_control_bit
# steer torque
new_steer = int(round(CC.actuators.steer * self.params.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorqueEps,
self.params)
if not lkas_active or not lkas_control_bit:
apply_steer = 0
self.apply_steer_last = apply_steer
can_sends.append(
create_lkas_command(self.packer, self.CP, int(apply_steer),
lkas_control_bit))
self.frame += 1
new_actuators = CC.actuators.copy()
new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX
return new_actuators, can_sends
def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd,
hud_alert, audible_alert, forwarding_camera, left_line,
right_line, lead, left_lane_depart, right_lane_depart):
#update custom UI buttons and alerts
CS.UE.update_custom_ui()
if (frame % 1000 == 0):
CS.cstm_btns.send_button_info()
CS.UE.uiSetCarEvent(CS.cstm_btns.car_folder, CS.cstm_btns.car_name)
# *** compute control surfaces ***
# gas and brake
apply_gas = clip(actuators.gas, 0., 1.)
if CS.CP.enableGasInterceptor:
# send only negative accel if interceptor is detected. otherwise, send the regular value
# +0.06 offset to reduce ABS pump usage when OP is engaged
apply_accel = 0.06 - actuators.brake
else:
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(
apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# Get the angle from ALCA.
alca_enabled = False
alca_steer = 0.
alca_angle = 0.
turn_signal_needed = 0
# Update ALCA status and custom button every 0.1 sec.
if self.ALCA.pid == None and not CS.indi_toggle:
self.ALCA.set_pid(CS)
if (frame % 10 == 0):
self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0)
# steer torque
alca_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(
enabled, CS, frame, actuators)
#apply_steer = int(round(alca_steer * STEER_MAX))
self.phantom.update()
# steer torque
if self.phantom.data["status"]:
apply_steer = int(round(self.phantom.data["angle"]))
if abs(CS.angle_steers) > 400:
apply_steer = 0
else:
apply_steer = int(round(alca_steer * SteerLimitParams.STEER_MAX))
if abs(CS.angle_steers) > 100:
apply_steer = 0
if not CS.lane_departure_toggle_on:
apply_steer = 0
# only cut torque when steer state is a known fault
if CS.steer_state in [3, 7, 9, 11, 25]:
self.last_fault_frame = frame
# Cut steering for 2s after fault
cutout_time = 100 if self.phantom.data["status"] else 200
if not enabled or (frame - self.last_fault_frame < cutout_time):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
apply_steer = apply_toyota_steer_torque_limits(apply_steer,
self.last_steer,
CS.steer_torque_motor,
SteerLimitParams)
if apply_steer == 0 and self.last_steer == 0:
apply_steer_req = 0
if not enabled and right_lane_depart and CS.v_ego > 12.5 and not CS.right_blinker_on:
apply_steer = self.last_steer + 3
apply_steer = min(apply_steer, 800)
#print "right"
#print apply_steer
apply_steer_req = 1
if not enabled and left_lane_depart and CS.v_ego > 12.5 and not CS.left_blinker_on:
apply_steer = self.last_steer - 3
apply_steer = max(apply_steer, -800)
#print "left"
#print apply_steer
apply_steer_req = 1
self.steer_angle_enabled, self.ipas_reset_counter = \
ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
#print("{0} {1} {2}".format(self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active))
# steer angle
if self.steer_angle_enabled and CS.ipas_active:
apply_angle = alca_angle
#apply_angle = actuators.steerAngle
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(
self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP,
ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP,
ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim,
self.last_angle + angle_rate_lim)
else:
apply_angle = CS.angle_steers
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
#if CS.standstill and not self.last_standstill:
# self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_angle = apply_angle
self.last_accel = apply_accel
self.last_standstill = CS.standstill
can_sends = []
# Enable blindspot debug mode once
if BLINDSPOTDEBUG:
self.blindspot_poll_counter += 1
if self.blindspot_poll_counter > 1000: # 10 seconds after start
if CS.left_blinker_on:
self.blindspot_blink_counter_left += 1
#print "debug Left Blinker on"
elif CS.right_blinker_on:
self.blindspot_blink_counter_right += 1
else:
self.blindspot_blink_counter_left = 0
self.blindspot_blink_counter_right = 0
#print "debug Left Blinker off"
if self.blindspot_debug_enabled_left:
can_sends.append(
set_blindspot_debug_mode(LEFT_BLINDSPOT, False))
self.blindspot_debug_enabled_left = False
#print "debug Left blindspot debug disabled"
if self.blindspot_debug_enabled_right:
can_sends.append(
set_blindspot_debug_mode(RIGHT_BLINDSPOT, False))
self.blindspot_debug_enabled_right = False
#print "debug Right blindspot debug disabled"
if self.blindspot_blink_counter_left > 9 and not self.blindspot_debug_enabled_left: #check blinds
can_sends.append(set_blindspot_debug_mode(
LEFT_BLINDSPOT, True))
#print "debug Left blindspot debug enabled"
self.blindspot_debug_enabled_left = True
if self.blindspot_blink_counter_right > 5 and not self.blindspot_debug_enabled_right: #enable blindspot debug mode
if CS.v_ego > 6: #polling at low speeds switches camera off
can_sends.append(
set_blindspot_debug_mode(RIGHT_BLINDSPOT, True))
#print "debug Right blindspot debug enabled"
self.blindspot_debug_enabled_right = True
if self.blindspot_debug_enabled_left:
if self.blindspot_poll_counter % 20 == 0 and self.blindspot_poll_counter > 1001: # Poll blindspots at 5 Hz
can_sends.append(poll_blindspot_status(LEFT_BLINDSPOT))
if self.blindspot_debug_enabled_right:
if self.blindspot_poll_counter % 20 == 10 and self.blindspot_poll_counter > 1005: # Poll blindspots at 5 Hz
can_sends.append(poll_blindspot_status(RIGHT_BLINDSPOT))
#*** control msgs ***
#print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
if ECU.CAM in self.fake_ecus:
if self.angle_control:
can_sends.append(
create_steer_command(self.packer, 0., 0, frame))
else:
can_sends.append(
create_steer_command(self.packer, apply_steer,
apply_steer_req, frame))
if self.angle_control:
can_sends.append(
create_ipas_steer_command(self.packer, apply_angle,
self.steer_angle_enabled, ECU.APGS
in self.fake_ecus))
elif ECU.APGS in self.fake_ecus:
can_sends.append(create_ipas_steer_command(self.packer, 0, 0,
True))
if CS.cstm_btns.get_button_status("tr") > 0:
distance = 1
else:
distance = 0
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (
pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged
if ECU.DSU in self.fake_ecus:
can_sends.append(
create_accel_command(self.packer, apply_accel,
pcm_cancel_cmd, self.standstill_req,
lead, distance))
else:
can_sends.append(
create_accel_command(self.packer, 0, pcm_cancel_cmd, False,
lead, distance))
if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(
create_gas_command(self.packer, apply_gas, frame // 2))
if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera:
for addr in TARGET_IDS:
can_sends.append(create_video_target(frame // 10, addr))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
alert_out = process_hud_alert(hud_alert, audible_alert)
steer, fcw, sound1, sound2 = alert_out
if (any(alert_out) and not self.alert_active) or \
(not any(alert_out) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
else:
send_ui = False
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
can_sends.append(
create_ui_command(self.packer, steer, sound1, sound2,
left_line, right_line, left_lane_depart,
right_lane_depart))
if frame % 100 == 0 and ECU.DSU in self.fake_ecus and self.car_fingerprint not in TSSP2_CAR:
can_sends.append(create_fcw_command(self.packer, fcw))
#*** static msgs ***
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars and not (
ecu == ECU.CAM and forwarding_camera):
# special cases
if fr_step == 5 and ecu == ECU.CAM and bus == 1:
cnt = (((frame // 5) % 7) + 1) << 5
vl = chr(cnt) + vl
elif addr in (0x489, 0x48a) and bus == 0:
# add counter for those 2 messages (last 4 bits)
cnt = ((frame // 100) % 0xf) + 1
if addr == 0x48a:
# 0x48a has a 8 preceding the counter
cnt += 1 << 7
vl += chr(cnt)
can_sends.append(make_can_msg(addr, vl, bus, False))
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
def update(self, enabled, CS, frame, actuators,
pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera,
left_line, right_line, lead, left_lane_depart, right_lane_depart):
# *** compute control surfaces ***
# gas and brake
apply_gas = clip(actuators.gas, 0., 1.)
if CS.CP.enableGasInterceptor:
# send only negative accel if interceptor is detected. otherwise, send the regular value
# +0.06 offset to reduce ABS pump usage when OP is engaged
apply_accel = 0.06 - actuators.brake
else:
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# steer torque
apply_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(apply_steer, self.last_steer, CS.steer_torque_motor, SteerLimitParams)
# only cut torque when steer state is a known fault
if CS.steer_state in [9, 25]:
self.last_fault_frame = frame
# Cut steering for 2s after fault
if not enabled or (frame - self.last_fault_frame < 200):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
self.steer_angle_enabled, self.ipas_reset_counter = \
ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
#print("{0} {1} {2}".format(self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active))
# steer angle
if self.steer_angle_enabled and CS.ipas_active:
apply_angle = actuators.steerAngle
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
else:
apply_angle = CS.angle_steers
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if CS.standstill and not self.last_standstill:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_angle = apply_angle
self.last_accel = apply_accel
self.last_standstill = CS.standstill
can_sends = []
# dragonpilot
if enabled and (CS.left_blinker_on or CS.right_blinker_on) and params.get("DragonEnableSteeringOnSignal") == "1":
self.turning_signal_timer = 100
if self.turning_signal_timer > 0:
self.turning_signal_timer -= 1
apply_steer_req = 0
#*** control msgs ***
#print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
if ECU.CAM in self.fake_ecus:
if self.angle_control:
can_sends.append(create_steer_command(self.packer, 0., 0, frame))
else:
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
if self.angle_control:
can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled,
ECU.APGS in self.fake_ecus))
elif ECU.APGS in self.fake_ecus:
can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True))
# DragonAllowGas
# if we detect gas pedal pressed, we do not want OP to apply gas or brake
# gasPressed code from interface.py
if CS.CP.enableGasInterceptor:
# use interceptor values to disengage on pedal press
gasPressed = CS.pedal_gas > 15
else:
gasPressed = CS.pedal_gas > 0
if params.get("DragonAllowGas") == "1" and gasPressed:
apply_accel = 0
apply_gas = 0
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged
if ECU.DSU in self.fake_ecus:
can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead))
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead))
if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(create_gas_command(self.packer, apply_gas, frame//2))
if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera:
for addr in TARGET_IDS:
can_sends.append(create_video_target(frame//10, addr))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
alert_out = process_hud_alert(hud_alert, audible_alert)
steer, fcw, sound1, sound2 = alert_out
if (any(alert_out) and not self.alert_active) or \
(not any(alert_out) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
else:
send_ui = False
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
can_sends.append(create_ui_command(self.packer, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart))
if frame % 100 == 0 and ECU.DSU in self.fake_ecus and self.car_fingerprint not in TSS2_CAR:
can_sends.append(create_fcw_command(self.packer, fcw))
#*** static msgs ***
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars and not (ecu == ECU.CAM and forwarding_camera):
# special cases
if fr_step == 5 and ecu == ECU.CAM and bus == 1:
cnt = (((frame // 5) % 7) + 1) << 5
vl = chr(cnt) + vl
elif addr in (0x489, 0x48a) and bus == 0:
# add counter for those 2 messages (last 4 bits)
cnt = ((frame // 100) % 0xf) + 1
if addr == 0x48a:
# 0x48a has a 8 preceding the counter
cnt += 1 << 7
vl += chr(cnt)
can_sends.append(make_can_msg(addr, vl, bus, False))
return can_sends
def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert,
dragonconf):
# this seems needed to avoid steering faults and to force the sync with the EPS counter
frame = CS.lkas_counter
if self.prev_frame == frame:
return []
# *** compute control surfaces ***
# steer torque
new_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = apply_toyota_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorqueEps,
SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
moving_fast = CS.out.vEgo > CS.CP.minSteerSpeed # for status message
if CS.out.vEgo > (CS.CP.minSteerSpeed - 0.5): # for command high bit
self.gone_fast_yet = True
elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID,
CAR.JEEP_CHEROKEE_2019):
if CS.out.vEgo < (CS.CP.minSteerSpeed - 3.0):
self.gone_fast_yet = False # < 14.5m/s stock turns off this bit, but fine down to 13.5
lkas_active = moving_fast and enabled
if not lkas_active:
apply_steer = 0
# dp
blinker_on = CS.out.leftBlinker or CS.out.rightBlinker
if not enabled:
self.blinker_end_frame = 0
if self.last_blinker_on and not blinker_on:
self.blinker_end_frame = frame + dragonconf.dpSignalOffDelay
apply_steer = common_controller_ctrl(
enabled, dragonconf.dpLatCtrl, dragonconf.dpSteeringOnSignal,
blinker_on or frame < self.blinker_end_frame, apply_steer)
self.last_blinker_on = blinker_on
self.apply_steer_last = apply_steer
can_sends = []
#*** control msgs ***
if pcm_cancel_cmd:
# TODO: would be better to start from frame_2b3
new_msg = create_wheel_buttons(self.packer,
self.ccframe,
cancel=True)
can_sends.append(new_msg)
# LKAS_HEARTBIT is forwarded by Panda so no need to send it here.
# frame is 100Hz (0.01s period)
if (self.ccframe % 25 == 0): # 0.25s period
if (CS.lkas_car_model != -1):
new_msg = create_lkas_hud(self.packer, CS.out.gearShifter,
lkas_active, hud_alert,
self.hud_count, CS.lkas_car_model)
can_sends.append(new_msg)
self.hud_count += 1
new_msg = create_lkas_command(self.packer, int(apply_steer),
self.gone_fast_yet, frame)
can_sends.append(new_msg)
self.ccframe += 1
self.prev_frame = frame
return can_sends
def lkas_control(self, CS, actuators, can_sends, enabled, hud_alert,
jvepilot_state):
if self.prev_frame == CS.frame:
return
self.prev_frame = CS.frame
self.lkas_frame += 1
lkas_counter = CS.lkas_counter
if self.prev_lkas_counter == lkas_counter:
lkas_counter = (self.prev_lkas_counter +
1) % 16 # Predict the next frame
self.prev_lkas_counter = lkas_counter
# *** compute control surfaces ***
# steer torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorqueEps,
CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
low_steer_models = self.car_fingerprint in (CAR.JEEP_CHEROKEE,
CAR.PACIFICA_2017_HYBRID,
CAR.PACIFICA_2018,
CAR.PACIFICA_2018_HYBRID)
if not self.min_steer_check:
self.moving_fast = True
self.torq_enabled = enabled or low_steer_models
elif low_steer_models:
self.moving_fast = not CS.out.steerError and CS.lkas_active
self.torq_enabled = self.torq_enabled or CS.torq_status > 1
else:
self.moving_fast = CS.out.vEgo > CS.CP.minSteerSpeed # for status message
if CS.out.vEgo > (CS.CP.minSteerSpeed -
0.5): # for command high bit
self.torq_enabled = True
elif CS.out.vEgo < (CS.CP.minSteerSpeed - 3.0):
self.torq_enabled = False # < 14.5m/s stock turns off this bit, but fine down to 13.5
lkas_active = self.moving_fast and enabled
if not lkas_active:
apply_steer = 0
self.apply_steer_last = apply_steer
if self.lkas_frame % 10 == 0: # 0.1s period
new_msg = create_lkas_heartbit(
self.packer,
0 if jvepilot_state.carControl.useLaneLines else 1,
CS.lkasHeartbit)
can_sends.append(new_msg)
if self.lkas_frame % 25 == 0: # 0.25s period
if CS.lkas_car_model != -1:
new_msg = create_lkas_hud(self.packer, CS.out.gearShifter,
lkas_active, hud_alert,
self.hud_count, CS.lkas_car_model)
can_sends.append(new_msg)
self.hud_count += 1
new_msg = create_lkas_command(self.packer, int(apply_steer),
self.torq_enabled, lkas_counter)
can_sends.append(new_msg)
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
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart,
right_lane_depart):
# *** compute control surfaces ***
# gas and brake
interceptor_gas_cmd = 0.
pcm_accel_cmd = actuators.gas - actuators.brake
if CS.CP.enableGasInterceptor:
# handle hysteresis when around the minimum acc speed
if CS.out.vEgo < MIN_ACC_SPEED:
self.use_interceptor = True
elif CS.out.vEgo > MIN_ACC_SPEED + PEDAL_HYST_GAP:
self.use_interceptor = False
if self.use_interceptor and enabled:
# only send negative accel when using interceptor. gas handles acceleration
# +0.06 offset to reduce ABS pump usage when OP is engaged
interceptor_gas_cmd = clip(actuators.gas, 0., 1.)
pcm_accel_cmd = 0.06 - actuators.brake
pcm_accel_cmd, self.accel_steady = accel_hysteresis(
pcm_accel_cmd, self.accel_steady, enabled)
pcm_accel_cmd = clip(pcm_accel_cmd * CarControllerParams.ACCEL_SCALE,
CarControllerParams.ACCEL_MIN,
CarControllerParams.ACCEL_MAX)
# steer torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(
new_steer, self.last_steer, CS.out.steeringTorqueEps,
CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
# Cut steering while we're in a known fault state (2s)
if not enabled or CS.steer_state in [9, 25]:
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_accel = pcm_accel_cmd
self.last_standstill = CS.out.standstill
can_sends = []
#*** control msgs ***
#print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
can_sends.append(
create_steer_command(self.packer, apply_steer, apply_steer_req,
frame))
if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR:
can_sends.append(
create_lta_steer_command(self.packer, 0, 0, frame // 2))
# LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
# if frame % 2 == 0:
# can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2))
# can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, frame // 2))
# we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0
and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged
# Lexus IS uses a different cancellation message
if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
can_sends.append(create_acc_cancel_command(self.packer))
elif CS.CP.openpilotLongitudinalControl:
can_sends.append(
create_accel_command(self.packer, pcm_accel_cmd,
pcm_cancel_cmd, self.standstill_req,
lead, CS.acc_type))
else:
can_sends.append(
create_accel_command(self.packer, 0, pcm_cancel_cmd, False,
lead, CS.acc_type))
if frame % 2 == 0 and CS.CP.enableGasInterceptor:
# send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
# This prevents unexpected pedal range rescaling
can_sends.append(
create_gas_command(self.packer, interceptor_gas_cmd,
frame // 2))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
fcw_alert = hud_alert == VisualAlert.fcw
steer_alert = hud_alert in [VisualAlert.steerRequired, VisualAlert.ldw]
send_ui = False
if ((fcw_alert or steer_alert) and not self.alert_active) or \
(not (fcw_alert or steer_alert) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
elif pcm_cancel_cmd:
# forcing the pcm to disengage causes a bad fault sound so play a good sound instead
send_ui = True
if (frame % 100 == 0 or send_ui):
can_sends.append(
create_ui_command(self.packer, steer_alert, pcm_cancel_cmd,
left_line, right_line, left_lane_depart,
right_lane_depart))
if frame % 100 == 0 and CS.CP.enableDsu:
can_sends.append(create_fcw_command(self.packer, fcw_alert))
#*** static msgs ***
for (addr, cars, bus, fr_step, vl) in STATIC_DSU_MSGS:
if frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus))
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart, right_lane_depart):
# *** compute control surfaces ***
# gas and brake
apply_gas = clip(actuators.gas, 0., 1.)
if CS.CP.enableGasInterceptor:
# send only negative accel if interceptor is detected. otherwise, send the regular value
# +0.06 offset to reduce ABS pump usage when OP is engaged
apply_accel = 0.06 - actuators.brake
else:
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# steer torque
apply_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(apply_steer, self.last_steer, CS.steer_torque_motor, SteerLimitParams)
# only cut torque when steer state is a known fault
if CS.steer_state in [9, 25]:
self.last_fault_frame = frame
# Cut steering for 2s after fault
if not enabled or (frame - self.last_fault_frame < 200):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
self.steer_angle_enabled, self.ipas_reset_counter = \
ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
#print("{0} {1} {2}".format(self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active))
# steer angle
if self.steer_angle_enabled and CS.ipas_active:
apply_angle = actuators.steerAngle
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
else:
apply_angle = CS.angle_steers
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if CS.standstill and not self.last_standstill:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_angle = apply_angle
self.last_accel = apply_accel
self.last_standstill = CS.standstill
can_sends = []
#*** control msgs ***
#print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
if ECU.CAM in self.fake_ecus:
if self.angle_control:
can_sends.append(create_steer_command(self.packer, 0., 0, frame))
else:
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
if self.angle_control:
can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled,
ECU.APGS in self.fake_ecus))
elif ECU.APGS in self.fake_ecus:
can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True))
if CS.cstm_btns_tr > 0:
distance = 1
else:
distance = 0
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged
# Lexus IS uses a different cancellation message
if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
can_sends.append(create_acc_cancel_command(self.packer))
elif ECU.DSU in self.fake_ecus:
can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead, distance))
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, distance))
if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(create_gas_command(self.packer, apply_gas, frame//2))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
alert_out = process_hud_alert(hud_alert)
steer, fcw = alert_out
if (any(alert_out) and not self.alert_active) or \
(not any(alert_out) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
else:
send_ui = False
# disengage msg causes a bad fault sound so play a good sound instead
if pcm_cancel_cmd:
send_ui = True
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
can_sends.append(create_ui_command(self.packer, steer, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart))
if frame % 100 == 0 and ECU.DSU in self.fake_ecus and self.car_fingerprint not in TSS2_CAR:
can_sends.append(create_fcw_command(self.packer, fcw))
#*** static msgs ***
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus, False))
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart,
right_lane_depart):
# *** compute control surfaces ***
self.sm.update(0)
if self.sm.updated['radarState']:
self.lead_v = self.sm['radarState'].leadOne.vRel
self.lead_a = self.sm['radarState'].leadOne.aRel
self.lead_d = self.sm['radarState'].leadOne.dRel
if self.sm.updated['controlsState']:
self.LCS = self.sm['controlsState'].longControlState
if frame % 500 == 0: # *** 5 sec interval? ***
print(self.LCS)
# gas and brake
# apply_gas = 0.
# apply_accel = actuators.gas - actuators.brake
# if CS.CP.enableGasInterceptor and enabled and CS.out.vEgo < MIN_ACC_SPEED:
# apply_gas = clip(actuators.gas, 0., 1.)
# # converts desired acceleration to gas percentage for pedal
# # +0.06 offset to reduce ABS pump usage when applying very small gas
# if apply_accel * CarControllerParams.ACCEL_SCALE > coast_accel(CS.out.vEgo):
# apply_gas = clip(compute_gb_pedal(apply_accel * CarControllerParams.ACCEL_SCALE, CS.out.vEgo), 0., 1.)
# apply_accel += 0.06
apply_gas = clip(actuators.gas, 0., 1.)
if CS.CP.enableGasInterceptor:
# send only negative accel if interceptor is detected. otherwise, send the regular value
# +0.06 offset to reduce ABS pump usage when OP is engaged
#apply_accel = 0.06 - actuators.brake # Original
if lead:
apply_accel = 0.06 - actuators.brake
#apply_accel = 0.06
else:
apply_accel = 0.06
# End new
else:
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(
apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * CarControllerParams.ACCEL_SCALE,
CarControllerParams.ACCEL_MIN,
CarControllerParams.ACCEL_MAX)
# steer torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(
new_steer, self.last_steer, CS.out.steeringTorqueEps,
CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
# Cut steering while we're in a known fault state (2s)
if not enabled or abs(CS.out.steeringRateDeg) > 100:
#if not enabled or CS.steer_state in [9, 25] or CS.out.epsDisabled==1 or abs(CS.out.steeringRateDeg) > 100: #Original statement
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
#if not enabled and CS.pcm_acc_status: # Original
if not enabled:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR and not self.standstill_hack:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_accel = apply_accel
self.last_standstill = CS.out.standstill
can_sends = []
if (frame % 2 == 0):
can_sends.append(
create_lead_command(self.packer, self.lead_v, self.lead_a,
self.lead_d))
#*** control msgs ***
#print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
if Ecu.fwdCamera in self.fake_ecus:
can_sends.append(
create_steer_command(self.packer, apply_steer, apply_steer_req,
frame))
if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR:
can_sends.append(
create_lta_steer_command(self.packer, 0, 0, frame // 2))
# LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
# if frame % 2 == 0:
# can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2))
# can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, frame // 2))
# we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (
pcm_cancel_cmd and Ecu.fwdCamera in self.fake_ecus):
lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged
# Lexus IS uses a different cancellation message
if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
can_sends.append(create_acc_cancel_command(self.packer))
elif CS.CP.openpilotLongitudinalControl:
can_sends.append(
create_accel_command(self.packer, apply_accel,
pcm_cancel_cmd, self.standstill_req,
lead))
else:
#can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead)) # Original value
can_sends.append(
create_accel_command(self.packer, 500, pcm_cancel_cmd,
False, lead))
if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(
create_gas_command(self.packer, apply_gas, frame // 2))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
fcw_alert = hud_alert == VisualAlert.fcw
steer_alert = hud_alert == VisualAlert.steerRequired
send_ui = False
if ((fcw_alert or steer_alert) and not self.alert_active) or \
(not (fcw_alert or steer_alert) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
elif pcm_cancel_cmd:
# forcing the pcm to disengage causes a bad fault sound so play a good sound instead
send_ui = True
if (frame % 100 == 0 or send_ui) and Ecu.fwdCamera in self.fake_ecus:
can_sends.append(
create_ui_command(self.packer, steer_alert, pcm_cancel_cmd,
left_line, right_line, left_lane_depart,
right_lane_depart))
if frame % 100 == 0 and Ecu.dsu in self.fake_ecus:
can_sends.append(create_fcw_command(self.packer, fcw_alert))
#*** static msgs ***
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
if frame % fr_step == 0 and ecu in self.fake_ecus and CS.CP.carFingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus))
return can_sends
def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart, right_lane_depart):
# gas and brake
if CS.CP.enableGasInterceptor and active:
MAX_INTERCEPTOR_GAS = 0.5
# RAV4 has very sensitive gas pedal
if CS.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH):
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0])
elif CS.CP.carFingerprint in (CAR.COROLLA,):
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0])
else:
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0])
# offset for creep and windbrake
pedal_offset = interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2])
pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset)
interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS)
else:
interceptor_gas_cmd = 0.
pcm_accel_cmd = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
# steer torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
# TODO: probably can delete this. CS.pcm_acc_status uses a different signal
# than CS.cruiseState.enabled. confirm they're not meaningfully different
if not enabled and CS.pcm_acc_status:
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_standstill = CS.out.standstill
can_sends = []
if CS.CP.steerControlType == car.CarParams.SteerControlType.angle:
can_sends.append(create_steer_command(self.packer, 0, 0, frame))
# On TSS2 cars, the LTA and STEER_TORQUE_SENSOR messages use relative steering angle signals that start
# at 0 degrees, so we need to offset the commanded angle as well.
# Also need to pulse steer_req to prevent 5 second steering lockout in some cases
steer_req = active # TODO(thinh): Check and cut out steering while we are in a known fault state
percentage = interp(abs(CS.out.steeringTorque), [50, 100], [100, 0])
commanded_angle = interp(percentage, [-10, 100], [CS.out.steeringAngleDeg + CS.out.steeringAngleOffsetDeg,
actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg])
can_sends.append(create_lta_steer_command(self.packer, commanded_angle, steer_req, frame))
else:
# Cut steering while we're in a known fault state (2s)
if not active or CS.steer_state in (9, 25):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR:
can_sends.append(create_lta_steer_command(self.packer, 0, 0, frame // 2))
# we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged
# Lexus IS uses a different cancellation message
if pcm_cancel_cmd and CS.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
can_sends.append(create_acc_cancel_command(self.packer))
elif CS.CP.openpilotLongitudinalControl:
can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, CS.distance_btn))
self.accel = pcm_accel_cmd
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, CS.distance_btn))
if frame % 2 == 0 and CS.CP.enableGasInterceptor and CS.CP.openpilotLongitudinalControl:
# send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
# This prevents unexpected pedal range rescaling
can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, frame // 2))
self.gas = interceptor_gas_cmd
# ui mesg is at 1Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
fcw_alert = hud_alert == VisualAlert.fcw
steer_alert = hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw)
send_ui = False
if ((fcw_alert or steer_alert) and not self.alert_active) or \
(not (fcw_alert or steer_alert) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
elif pcm_cancel_cmd:
# forcing the pcm to disengage causes a bad fault sound so play a good sound instead
send_ui = True
if (frame % 100 == 0 or send_ui):
can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart, enabled))
if frame % 100 == 0 and CS.CP.enableDsu:
can_sends.append(create_fcw_command(self.packer, fcw_alert))
# *** static msgs ***
for (addr, cars, bus, fr_step, vl) in STATIC_DSU_MSGS:
if frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus))
new_actuators = actuators.copy()
new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
new_actuators.accel = self.accel
new_actuators.gas = self.gas
return new_actuators, can_sends
def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd,
hud_alert, audible_alert, forwarding_camera, left_line,
right_line, lead):
#update custom UI buttons and alerts
CS.UE.update_custom_ui()
if (frame % 1000 == 0):
CS.cstm_btns.send_button_info()
CS.UE.uiSetCarEvent(CS.cstm_btns.car_folder, CS.cstm_btns.car_name)
# *** compute control surfaces ***
# gas and brake
apply_gas = clip(actuators.gas, 0., 1.)
if CS.CP.enableGasInterceptor:
# send only negative accel if interceptor is detected. otherwise, send the regular value
# +0.06 offset to reduce ABS pump usage when OP is engaged
apply_accel = 0.06 - actuators.brake
else:
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(
apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# Get the angle from ALCA.
alca_enabled = False
alca_steer = 0.
alca_angle = 0.
turn_signal_needed = 0
# Update ALCA status and custom button every 0.1 sec.
if self.ALCA.pid == None:
self.ALCA.set_pid(CS)
if (frame % 10 == 0):
self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0)
# steer torque
alca_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(
enabled, CS, frame, actuators)
apply_steer = int(round(alca_steer * SteerLimitParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(apply_steer,
self.last_steer,
CS.steer_torque_motor,
SteerLimitParams)
# only cut torque when steer state is a known fault
if CS.steer_state in [9, 25]:
self.last_fault_frame = frame
# Cut steering for 2s after fault
if not enabled or (frame - self.last_fault_frame < 200):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
self.steer_angle_enabled, self.ipas_reset_counter = \
ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
#print self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active
# steer angle
if self.steer_angle_enabled and CS.ipas_active:
apply_angle = alca_angle
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(
self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP,
ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP,
ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim,
self.last_angle + angle_rate_lim)
else:
apply_angle = CS.angle_steers
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if CS.standstill and not self.last_standstill:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_angle = apply_angle
self.last_accel = apply_accel
self.last_standstill = CS.standstill
can_sends = []
#*** control msgs ***
#print "steer", apply_steer, min_lim, max_lim, CS.steer_torque_motor
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
if ECU.CAM in self.fake_ecus:
if self.angle_control:
can_sends.append(
create_steer_command(self.packer, 0., 0, frame))
else:
can_sends.append(
create_steer_command(self.packer, apply_steer,
apply_steer_req, frame))
if self.angle_control:
can_sends.append(
create_ipas_steer_command(self.packer, apply_angle,
self.steer_angle_enabled, ECU.APGS
in self.fake_ecus))
elif ECU.APGS in self.fake_ecus:
can_sends.append(create_ipas_steer_command(self.packer, 0, 0,
True))
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (
pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged
if ECU.DSU in self.fake_ecus:
can_sends.append(
create_accel_command(self.packer, apply_accel,
pcm_cancel_cmd, self.standstill_req,
lead))
else:
can_sends.append(
create_accel_command(self.packer, 0, pcm_cancel_cmd, False,
lead))
if CS.CP.enableGasInterceptor:
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(create_gas_command(self.packer, apply_gas))
if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera:
for addr in TARGET_IDS:
can_sends.append(create_video_target(frame / 10, addr))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
alert_out = process_hud_alert(hud_alert, audible_alert)
steer, fcw, sound1, sound2 = alert_out
if (any(alert_out) and not self.alert_active) or \
(not any(alert_out) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
else:
send_ui = False
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
can_sends.append(
create_ui_command(self.packer, steer, sound1, sound2,
left_line, right_line))
if frame % 100 == 0 and ECU.DSU in self.fake_ecus:
can_sends.append(create_fcw_command(self.packer, fcw))
#*** static msgs ***
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars and not (
ecu == ECU.CAM and forwarding_camera):
# special cases
if fr_step == 5 and ecu == ECU.CAM and bus == 1:
cnt = (((frame / 5) % 7) + 1) << 5
vl = chr(cnt) + vl
elif addr in (0x489, 0x48a) and bus == 0:
# add counter for those 2 messages (last 4 bits)
cnt = ((frame / 100) % 0xf) + 1
if addr == 0x48a:
# 0x48a has a 8 preceding the counter
cnt += 1 << 7
vl += chr(cnt)
can_sends.append(make_can_msg(addr, vl, bus, False))
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart, right_lane_depart, dragonconf):
# *** compute control surfaces ***
# gas and brake
apply_gas = clip(actuators.gas, 0., 1.)
if CS.CP.enableGasInterceptor:
# send only negative accel if interceptor is detected. otherwise, send the regular value
# +0.06 offset to reduce ABS pump usage when OP is engaged
apply_accel = 0.06 - actuators.brake
else:
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# steer torque
new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
# Cut steering while we're in a known fault state (2s)
if not enabled or CS.steer_state in [9, 25] or abs(CS.out.steeringRate) > 100 or (abs(CS.out.steeringAngle) > 150 and CS.CP.carFingerprint in [CAR.RAV4H, CAR.PRIUS]):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if not dragonconf.dpToyotaSng and CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
# dp
blinker_on = CS.out.leftBlinker or CS.out.rightBlinker
if not enabled:
self.blinker_end_frame = 0
if self.last_blinker_on and not blinker_on:
self.blinker_end_frame = frame + dragonconf.dpSignalOffDelay
apply_steer = common_controller_ctrl(enabled,
dragonconf,
blinker_on or frame < self.blinker_end_frame,
apply_steer, CS.out.vEgo)
self.last_blinker_on = blinker_on
self.last_steer = apply_steer
self.last_accel = apply_accel
self.last_standstill = CS.out.standstill
can_sends = []
#*** control msgs ***
#print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
if Ecu.fwdCamera in self.fake_ecus:
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
# LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
# if frame % 2 == 0:
# can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2))
# can_sends.append(create_lta_steer_command(self.packer, actuators.steerAngle, apply_steer_req, frame // 2))
# we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (pcm_cancel_cmd and Ecu.fwdCamera in self.fake_ecus):
lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged
# Lexus IS uses a different cancellation message
if not dragonconf.dpAtl:
if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
can_sends.append(create_acc_cancel_command(self.packer))
elif CS.CP.openpilotLongitudinalControl:
can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead))
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead))
if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(create_gas_command(self.packer, apply_gas, frame//2))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
fcw_alert = hud_alert == VisualAlert.fcw
steer_alert = hud_alert == VisualAlert.steerRequired
send_ui = False
if ((fcw_alert or steer_alert) and not self.alert_active) or \
(not (fcw_alert or steer_alert) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
elif pcm_cancel_cmd:
# forcing the pcm to disengage causes a bad fault sound so play a good sound instead
send_ui = True
# dp
if dragonconf.dpToyotaLdw:
dragon_left_lane_depart = left_lane_depart
dragon_right_lane_depart = right_lane_depart
else:
dragon_left_lane_depart = False
dragon_right_lane_depart = False
if (frame % 100 == 0 or send_ui) and Ecu.fwdCamera in self.fake_ecus:
can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, dragon_left_lane_depart, dragon_right_lane_depart))
if frame % 100 == 0 and Ecu.dsu in self.fake_ecus:
can_sends.append(create_fcw_command(self.packer, fcw_alert))
#*** static msgs ***
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
if frame % fr_step == 0 and ecu in self.fake_ecus and CS.CP.carFingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus))
return can_sends
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, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd,
hud_alert, audible_alert):
#update custom UI buttons and alerts
CS.UE.update_custom_ui()
if (frame % 1000 == 0):
CS.cstm_btns.send_button_info()
CS.UE.uiSetCarEvent(CS.cstm_btns.car_folder, CS.cstm_btns.car_name)
# Get the angle from ALCA.
alca_enabled = False
alca_steer = 0.
alca_angle = 0.
turn_signal_needed = 0
# Update ALCA status and custom button every 0.1 sec.
if self.ALCA.pid == None:
self.ALCA.set_pid(CS)
if (frame % 10 == 0):
self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0)
# steer torque
alca_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(
enabled, CS, frame, actuators)
# this seems needed to avoid steering faults and to force the sync with the EPS counter
frame = CS.lkas_counter
if self.prev_frame == frame:
return
# *** compute control surfaces ***
# steer torque
apply_steer = alca_steer * SteerLimitParams.STEER_MAX
apply_steer = apply_toyota_steer_torque_limits(apply_steer,
self.apply_steer_last,
CS.steer_torque_motor,
SteerLimitParams)
moving_fast = CS.v_ego > CS.CP.minSteerSpeed # for status message
if CS.v_ego > (CS.CP.minSteerSpeed - 0.5): # for command high bit
self.gone_fast_yet = True
elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID,
CAR.JEEP_CHEROKEE_2019):
if CS.v_ego < (CS.CP.minSteerSpeed - 3.0):
self.gone_fast_yet = False # < 14.5m/s stock turns off this bit, but fine down to 13.5
lkas_active = moving_fast and enabled
if not lkas_active:
apply_steer = 0
if not CS.lane_departure_toggle_on:
apply_steer = 0
self.apply_steer_last = apply_steer
if audible_alert in LOUD_ALERTS:
self.send_chime = True
can_sends = []
#*** control msgs ***
if self.send_chime:
new_msg = create_chimes(AudibleAlert)
can_sends.append(new_msg)
if audible_alert not in LOUD_ALERTS:
self.send_chime = False
if pcm_cancel_cmd:
# TODO: would be better to start from frame_2b3
new_msg = create_wheel_buttons(self.ccframe)
can_sends.append(new_msg)
# frame is 100Hz (0.01s period)
if (self.ccframe % 10 == 0): # 0.1s period
new_msg = create_lkas_heartbit(self.packer, CS.lkas_status_ok)
can_sends.append(new_msg)
if (self.ccframe % 25 == 0): # 0.25s period
if (CS.lkas_car_model != -1):
new_msg = create_lkas_hud(self.packer, CS.gear_shifter,
lkas_active, hud_alert,
self.hud_count, CS.lkas_car_model)
can_sends.append(new_msg)
self.hud_count += 1
new_msg = create_lkas_command(self.packer, int(apply_steer),
self.gone_fast_yet, frame)
can_sends.append(new_msg)
self.ccframe += 1
self.prev_frame = frame
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart, right_lane_depart):
# dp
if frame % 500 == 0:
modified = get_last_modified()
if self.dp_last_modified != modified:
self.dragon_lane_departure_warning = False if params.get("DragonToyotaLaneDepartureWarning", encoding='utf8') == "0" else True
self.dragon_toyota_sng_mod = True if params.get("DragonToyotaSnGMod", encoding='utf8') == "1" else False
self.dragon_lat_ctrl, \
self.lane_change_enabled, \
self.dragon_enable_steering_on_signal = common_controller_update(self.lane_change_enabled)
self.dp_last_modified = modified
# *** compute control surfaces ***
# gas and brake
apply_gas = clip(actuators.gas, 0., 1.)
if CS.CP.enableGasInterceptor:
# send only negative accel if interceptor is detected. otherwise, send the regular value
# +0.06 offset to reduce ABS pump usage when OP is engaged
apply_accel = 0.06 - actuators.brake
else:
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# steer torque
new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
# only cut torque when steer state is a known fault
if CS.steer_state in [9, 25]:
self.last_fault_frame = frame
# # Cut steering for 2s after fault
if not enabled: # or (frame - self.last_fault_frame < 200):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if not self.dragon_toyota_sng_mod and CS.out.standstill and not self.last_standstill:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_accel = apply_accel
self.last_standstill = CS.out.standstill
# dp
apply_steer_req = common_controller_ctrl(enabled,
self.dragon_lat_ctrl,
self.dragon_enable_steering_on_signal,
CS.out.leftBlinker,
CS.out.rightBlinker,
apply_steer_req)
can_sends = []
#*** control msgs ***
#print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
if Ecu.fwdCamera in self.fake_ecus:
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
# we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (pcm_cancel_cmd and Ecu.fwdCamera in self.fake_ecus):
lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged
# Lexus IS uses a different cancellation message
if pcm_cancel_cmd and CS.CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_ISH, CAR.LEXUS_GSH]:
can_sends.append(create_acc_cancel_command(self.packer))
elif CS.CP.openpilotLongitudinalControl:
can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead))
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead))
if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(create_gas_command(self.packer, apply_gas, frame//2))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
fcw_alert = hud_alert == VisualAlert.fcw
steer_alert = hud_alert == VisualAlert.steerRequired
send_ui = False
if ((fcw_alert or steer_alert) and not self.alert_active) or \
(not (fcw_alert or steer_alert) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
elif pcm_cancel_cmd:
# forcing the pcm to disengage causes a bad fault sound so play a good sound instead
send_ui = True
# dp
if self.dragon_lane_departure_warning:
dragon_left_lane_depart = left_lane_depart
dragon_right_lane_depart = right_lane_depart
else:
dragon_left_lane_depart = False
dragon_right_lane_depart = False
if (frame % 100 == 0 or send_ui) and Ecu.fwdCamera in self.fake_ecus:
can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, dragon_left_lane_depart, dragon_right_lane_depart))
if frame % 100 == 0 and Ecu.dsu in self.fake_ecus:
can_sends.append(create_fcw_command(self.packer, fcw_alert))
#*** static msgs ***
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
if frame % fr_step == 0 and ecu in self.fake_ecus and CS.CP.carFingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus))
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart,
right_lane_depart, dragonconf):
# *** compute control surfaces ***
# gas and brake
interceptor_gas_cmd = 0.
pcm_accel_cmd = actuators.accel
if CS.CP.enableGasInterceptor:
# handle hysteresis when around the minimum acc speed
if CS.out.vEgo < MIN_ACC_SPEED:
self.use_interceptor = True
elif CS.out.vEgo > MIN_ACC_SPEED + PEDAL_HYST_GAP:
self.use_interceptor = False
if self.use_interceptor and enabled:
# only send negative accel when using interceptor. gas handles acceleration
# +0.18 m/s^2 offset to reduce ABS pump usage when OP is engaged
MAX_INTERCEPTOR_GAS = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED],
[0.2, 0.5])
interceptor_gas_cmd = clip(actuators.accel / PEDAL_SCALE, 0.,
MAX_INTERCEPTOR_GAS)
pcm_accel_cmd = 0.18 - max(0, -actuators.accel)
pcm_accel_cmd, self.accel_steady = accel_hysteresis(
pcm_accel_cmd, self.accel_steady, enabled)
pcm_accel_cmd = clip(pcm_accel_cmd, CarControllerParams.ACCEL_MIN,
CarControllerParams.ACCEL_MAX)
# steer torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(
new_steer, self.last_steer, CS.out.steeringTorqueEps,
CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
# Cut steering while we're in a known fault state (2s)
if not enabled or CS.steer_state in [
9, 25
] or abs(CS.out.steeringRateDeg) > 100 or (
abs(CS.out.steeringAngleDeg) > 150
and CS.CP.carFingerprint in [CAR.RAV4H, CAR.PRIUS]):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
# TODO: probably can delete this. CS.pcm_acc_status uses a different signal
# than CS.cruiseState.enabled. confirm they're not meaningfully different
if not enabled and CS.pcm_acc_status:
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if not dragonconf.dpToyotaSng and CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
# dp
blinker_on = CS.out.leftBlinker or CS.out.rightBlinker
if not enabled:
self.blinker_end_frame = 0
if self.last_blinker_on and not blinker_on:
self.blinker_end_frame = frame + dragonconf.dpSignalOffDelay
apply_steer = common_controller_ctrl(
enabled, dragonconf, blinker_on or frame < self.blinker_end_frame,
apply_steer, CS.out.vEgo)
self.last_blinker_on = blinker_on
self.last_steer = apply_steer
self.last_accel = pcm_accel_cmd
self.last_standstill = CS.out.standstill
can_sends = []
#*** control msgs ***
#print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
can_sends.append(
create_steer_command(self.packer, apply_steer, apply_steer_req,
frame))
if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR:
can_sends.append(
create_lta_steer_command(self.packer, 0, 0, frame // 2))
# LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
# if frame % 2 == 0:
# can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2))
# can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, frame // 2))
if dragonconf.dpAtl and dragonconf.dpAtlOpLong and not CS.out.cruiseActualEnabled:
pcm_accel_cmd = 0.
if CS.CP.enableGasInterceptor:
interceptor_gas_cmd = 0.
# we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0
and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged
if dragonconf.dpAtl and not dragonconf.dpAtlOpLong:
pass
# Lexus IS uses a different cancellation message
elif pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
can_sends.append(create_acc_cancel_command(self.packer))
elif CS.CP.openpilotLongitudinalControl:
can_sends.append(
create_accel_command(self.packer, pcm_accel_cmd,
pcm_cancel_cmd, self.standstill_req,
lead, CS.acc_type, CS.distance))
else:
can_sends.append(
create_accel_command(self.packer, 0, pcm_cancel_cmd, False,
lead, CS.acc_type, CS.distance))
if frame % 2 == 0 and CS.CP.enableGasInterceptor:
# send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
# This prevents unexpected pedal range rescaling
can_sends.append(
create_gas_command(self.packer, interceptor_gas_cmd,
frame // 2))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
fcw_alert = hud_alert == VisualAlert.fcw
steer_alert = hud_alert in [VisualAlert.steerRequired, VisualAlert.ldw]
send_ui = False
if ((fcw_alert or steer_alert) and not self.alert_active) or \
(not (fcw_alert or steer_alert) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
elif pcm_cancel_cmd:
# forcing the pcm to disengage causes a bad fault sound so play a good sound instead
send_ui = True
# dp
if not dragonconf.dpToyotaLdw:
left_lane_depart = False
right_lane_depart = False
if (frame % 100 == 0 or send_ui):
can_sends.append(
create_ui_command(self.packer, steer_alert, pcm_cancel_cmd,
left_line, right_line, left_lane_depart,
right_lane_depart, enabled))
if frame % 100 == 0 and CS.CP.enableDsu:
can_sends.append(create_fcw_command(self.packer, fcw_alert))
#*** static msgs ***
for (addr, cars, bus, fr_step, vl) in STATIC_DSU_MSGS:
if frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus))
return can_sends
def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart, right_lane_depart):
# *** compute control surfaces ***
# gas and brake
if CS.CP.enableGasInterceptor and enabled:
MAX_INTERCEPTOR_GAS = 0.5
# RAV4 has very sensitive gas pedal
if CS.CP.carFingerprint in [CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH]:
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0])
elif CS.CP.carFingerprint in [CAR.COROLLA]:
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0])
else:
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0])
# offset for creep and windbrake
pedal_offset = interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2])
pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset)
interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS)
else:
interceptor_gas_cmd = 0.
pcm_accel_cmd = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
# steer torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
# Cut steering while we're in a known fault state (2s)
if not enabled or CS.steer_state in [9, 25]:
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
# TODO: probably can delete this. CS.pcm_acc_status uses a different signal
# than CS.cruiseState.enabled. confirm they're not meaningfully different
if not enabled and CS.pcm_acc_status:
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_standstill = CS.out.standstill
can_sends = []
#*** control msgs ***
#print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR:
can_sends.append(create_lta_steer_command(self.packer, 0, 0, frame // 2))
# LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
# if frame % 2 == 0:
# can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2))
# can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, frame // 2))
# we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged
# Lexus IS uses a different cancellation message
if pcm_cancel_cmd and CS.CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_RC]:
can_sends.append(create_acc_cancel_command(self.packer))
elif CS.CP.openpilotLongitudinalControl:
can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type))
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type))
if frame % 2 == 0 and CS.CP.enableGasInterceptor:
# send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
# This prevents unexpected pedal range rescaling
can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, frame // 2))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
fcw_alert = hud_alert == VisualAlert.fcw
steer_alert = hud_alert in [VisualAlert.steerRequired, VisualAlert.ldw]
send_ui = False
if ((fcw_alert or steer_alert) and not self.alert_active) or \
(not (fcw_alert or steer_alert) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
elif pcm_cancel_cmd:
# forcing the pcm to disengage causes a bad fault sound so play a good sound instead
send_ui = True
if (frame % 100 == 0 or send_ui):
can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart, enabled))
if frame % 100 == 0 and CS.CP.enableDsu:
can_sends.append(create_fcw_command(self.packer, fcw_alert))
#*** static msgs ***
for (addr, cars, bus, fr_step, vl) in STATIC_DSU_MSGS:
if frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus))
return can_sends
def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
# this seems needed to avoid steering faults and to force the sync with the EPS counter
frame = CS.lkas_counter
if self.prev_frame == frame:
return []
# *** compute control surfaces ***
if self.on_timer < 200 and CS.veh_on:
self.on_timer += 1
wp_type = int(0)
if Params().get('LkasFullRangeAvailable') == b'1':
wp_type = int(1)
if Params().get('ChryslerMangoMode') == b'1':
wp_type = int(2)
if enabled:
if CS.out.steeringAngleDeg > 50. and wp_type == 1 and self.timer > 97:
self.timer = 97
if self.timer < 99 and wp_type == 1 and CS.out.vEgo < 65:
self.timer += 1
else:
self.timer = 99
else:
self.timer = 0
lkas_active = self.timer == 99
# steer torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorqueEps,
CarControllerParams)
#self.steer_rate_limited = new_steer != apply_steer
#moving_fast = CS.out.vEgo > CS.CP.minSteerSpeed # for status message
#if CS.out.vEgo > (CS.CP.minSteerSpeed - 0.5): # for command high bit
# self.gone_fast_yet = True
#elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_CHEROKEE_2019):
# if CS.out.vEgo < (CS.CP.minSteerSpeed - 3.0):
# self.gone_fast_yet = False # < 14.5m/s stock turns off this bit, but fine down to 13.5
#lkas_active = moving_fast and enabled
if not lkas_active:
apply_steer = 0
self.steer_rate_limited = new_steer != apply_steer
self.apply_steer_last = apply_steer
if CS.out.standstill:
self.steer_type = wp_type
if wp_type != 2:
self.steerErrorMod = CS.steerError
if self.steerErrorMod:
self.steer_type = int(0)
elif CS.apaFault or CS.out.gearShifter not in (GearShifter.drive, GearShifter.low) or \
abs(CS.out.steeringAngleDeg) > 330. or self.on_timer < 200 or CS.apa_steer_status:
self.steer_type = int(0)
self.apaActive = CS.apasteerOn and self.steer_type == 2
can_sends = []
#*** control msgs ***
# if pcm_cancel_cmd:
# # TODO: would be better to start from frame_2b3
# new_msg = create_wheel_buttons(self.packer, self.ccframe, cancel=True)
# can_sends.append(new_msg)
# LKAS_HEARTBIT is forwarded by Panda so no need to send it here.
# frame is 100Hz (0.01s period)
if (self.ccframe % 2 == 0) and wp_type == 2: # 0.02s period
if (CS.lkas_car_model != -1):
new_msg = create_apa_hud(self.packer, CS.out.gearShifter,
self.apaActive, CS.apaFault,
hud_alert, lkas_active,
self.hud_count, CS.lkas_car_model,
self.steer_type)
can_sends.append(new_msg)
self.hud_count += 1
if (self.ccframe % 2 == 0) and wp_type != 2: # 0.25s period
if (CS.lkas_car_model != -1):
new_msg = create_lkas_hud(self.packer, CS.out.gearShifter,
lkas_active, hud_alert,
self.hud_count, CS.lkas_car_model,
self.steer_type)
can_sends.append(new_msg)
self.hud_count += 1
#new_msg = create_lkas_command(self.packer, int(apply_steer), self.gone_fast_yet, frame)
new_msg = create_lkas_command(self.packer, int(apply_steer),
lkas_active, frame)
can_sends.append(new_msg)
self.ccframe += 1
self.prev_frame = frame
return can_sends
