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, 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,
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 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())
