def update(self, enabled, CS, frame, actuators, hud_v_cruise,
hud_show_lanes, hud_show_car, hud_alert, dragonconf):
P = self.params
# Send CAN commands.
can_sends = []
# STEER
if (frame % P.STEER_STEP) == 0:
lkas_enabled = enabled and not CS.out.steerWarning and CS.out.vEgo > P.MIN_STEER_SPEED
if lkas_enabled:
new_steer = actuators.steer * P.STEER_MAX
apply_steer = apply_std_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
self.steer_rate_limited = new_steer != apply_steer
else:
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, blinker_on
or frame < self.blinker_end_frame, apply_steer, CS.out.vEgo)
self.last_blinker_on = blinker_on
self.apply_steer_last = apply_steer
idx = (frame // P.STEER_STEP) % 4
can_sends.append(
gmcan.create_steering_control(self.packer_pt,
CanBus.POWERTRAIN, apply_steer,
idx, lkas_enabled))
# GAS/BRAKE
# no output if not enabled, but keep sending keepalive messages
# treat pedals as one
final_pedal = actuators.gas - actuators.brake
if not enabled:
# Stock ECU sends max regen when not enabled.
apply_gas = P.MAX_ACC_REGEN
apply_brake = 0
else:
apply_gas = int(
round(interp(final_pedal, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V)))
apply_brake = int(
round(interp(final_pedal, P.BRAKE_LOOKUP_BP,
P.BRAKE_LOOKUP_V)))
# Gas/regen and brakes - all at 25Hz
if (frame % 4) == 0:
idx = (frame // 4) % 4
at_full_stop = enabled and CS.out.standstill
near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE)
can_sends.append(
gmcan.create_friction_brake_command(self.packer_ch,
CanBus.CHASSIS,
apply_brake, idx,
near_stop, at_full_stop))
can_sends.append(
gmcan.create_gas_regen_command(self.packer_pt,
CanBus.POWERTRAIN, apply_gas,
idx, enabled, at_full_stop))
# Send dashboard UI commands (ACC status), 25hz
if (frame % 4) == 0:
send_fcw = hud_alert == VisualAlert.fcw
can_sends.append(
gmcan.create_acc_dashboard_command(self.packer_pt,
CanBus.POWERTRAIN, enabled,
hud_v_cruise * CV.MS_TO_KPH,
hud_show_car, send_fcw))
# Radar needs to know current speed and yaw rate (50hz),
# and that ADAS is alive (10hz)
time_and_headlights_step = 10
tt = frame * DT_CTRL
if frame % time_and_headlights_step == 0:
idx = (frame // time_and_headlights_step) % 4
can_sends.append(
gmcan.create_adas_time_status(CanBus.OBSTACLE,
int((tt - self.start_time) * 60),
idx))
can_sends.append(
gmcan.create_adas_headlights_status(self.packer_obj,
CanBus.OBSTACLE))
speed_and_accelerometer_step = 2
if frame % speed_and_accelerometer_step == 0:
idx = (frame // speed_and_accelerometer_step) % 4
can_sends.append(
gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx))
can_sends.append(
gmcan.create_adas_accelerometer_speed_status(
CanBus.OBSTACLE, CS.out.vEgo, idx))
if frame % P.ADAS_KEEPALIVE_STEP == 0:
can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN)
# Show green icon when LKA torque is applied, and
# alarming orange icon when approaching torque limit.
# If not sent again, LKA icon disappears in about 5 seconds.
# Conveniently, sending camera message periodically also works as a keepalive.
lka_active = CS.lkas_status == 1
lka_critical = lka_active and abs(actuators.steer) > 0.9
lka_icon_status = (lka_active, lka_critical)
if frame % P.CAMERA_KEEPALIVE_STEP == 0 or lka_icon_status != self.lka_icon_status_last:
steer_alert = hud_alert == VisualAlert.steerRequired
can_sends.append(
gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active,
lka_critical, steer_alert))
self.lka_icon_status_last = lka_icon_status
return can_sends
def update(self, enabled, CS, frame, actuators, \
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
P = self.params
# Send CAN commands.
can_sends = []
canbus = self.canbus
alert_out = process_hud_alert(hud_alert)
steer = alert_out
### STEER ###
if (frame % P.STEER_STEP) == 0:
lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > P.MIN_STEER_SPEED
if lkas_enabled:
apply_steer = actuators.steer * P.STEER_MAX
apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P)
else:
apply_steer = 0
self.apply_steer_last = apply_steer
idx = (frame // P.STEER_STEP) % 4
if self.car_fingerprint in SUPERCRUISE_CARS:
can_sends += gmcan.create_steering_control_ct6(self.packer_pt,
canbus, apply_steer, CS.v_ego, idx, lkas_enabled)
else:
can_sends.append(gmcan.create_steering_control(self.packer_pt,
canbus.powertrain, apply_steer, idx, lkas_enabled))
### GAS/BRAKE ###
if self.car_fingerprint not in SUPERCRUISE_CARS:
# no output if not enabled, but keep sending keepalive messages
# treat pedals as one
final_pedal = actuators.gas - actuators.brake
# *** apply pedal hysteresis ***
final_brake, self.brake_steady = actuator_hystereses(
final_pedal, self.pedal_steady)
if not enabled:
# Stock ECU sends max regen when not enabled.
apply_gas = P.MAX_ACC_REGEN
apply_brake = 0
else:
apply_gas = int(round(interp(final_pedal, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V)))
apply_brake = int(round(interp(final_pedal, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V)))
# Gas/regen and brakes - all at 25Hz
if (frame % 4) == 0:
idx = (frame // 4) % 4
at_full_stop = enabled and CS.standstill
near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE)
can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, canbus.chassis, apply_brake, idx, near_stop, at_full_stop))
at_full_stop = enabled and CS.standstill
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, canbus.powertrain, apply_gas, idx, enabled, at_full_stop))
# Send dashboard UI commands (ACC status), 25hz
if (frame % 4) == 0:
can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, canbus.powertrain, enabled, hud_v_cruise * CV.MS_TO_KPH, hud_show_car))
# Radar needs to know current speed and yaw rate (50hz),
# and that ADAS is alive (10hz)
time_and_headlights_step = 10
tt = frame * DT_CTRL
if frame % time_and_headlights_step == 0:
idx = (frame // time_and_headlights_step) % 4
can_sends.append(gmcan.create_adas_time_status(canbus.obstacle, int((tt - self.start_time) * 60), idx))
can_sends.append(gmcan.create_adas_headlights_status(canbus.obstacle))
speed_and_accelerometer_step = 2
if frame % speed_and_accelerometer_step == 0:
idx = (frame // speed_and_accelerometer_step) % 4
can_sends.append(gmcan.create_adas_steering_status(canbus.obstacle, idx))
can_sends.append(gmcan.create_adas_accelerometer_speed_status(canbus.obstacle, CS.v_ego, idx))
if frame % P.ADAS_KEEPALIVE_STEP == 0:
can_sends += gmcan.create_adas_keepalive(canbus.powertrain)
# Show green icon when LKA torque is applied, and
# alarming orange icon when approaching torque limit.
# If not sent again, LKA icon disappears in about 5 seconds.
# Conveniently, sending camera message periodically also works as a keepalive.
lka_active = CS.lkas_status == 1
lka_critical = lka_active and abs(actuators.steer) > 0.9
lka_icon_status = (lka_active, lka_critical)
if frame % P.CAMERA_KEEPALIVE_STEP == 0 \
or lka_icon_status != self.lka_icon_status_last:
can_sends.append(gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active, lka_critical, steer))
self.lka_icon_status_last = lka_icon_status
return can_sends
def update(self, sendcan, enabled, CS, frame, actuators, \
hud_v_cruise, hud_show_lanes, hud_show_car, chime, chime_cnt):
""" Controls thread """
P = self.params
# Send CAN commands.
can_sends = []
canbus = self.canbus
### STEER ###
if (frame % P.STEER_STEP) == 0:
final_steer = actuators.steer if enabled else 0.
apply_steer = final_steer * P.STEER_MAX
apply_steer = apply_std_steer_torque_limits(
apply_steer, self.apply_steer_last, CS.steer_torque_driver, P)
lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > 3.
if not lkas_enabled:
apply_steer = 0
self.apply_steer_last = apply_steer
idx = (frame / P.STEER_STEP) % 4
if self.car_fingerprint == CAR.VOLT:
can_sends.append(
gmcan.create_steering_control(self.packer_pt,
canbus.powertrain,
apply_steer, idx,
lkas_enabled))
if self.car_fingerprint == CAR.CADILLAC_CT6:
can_sends += gmcan.create_steering_control_ct6(
self.packer_pt, canbus, apply_steer, CS.v_ego, idx,
lkas_enabled)
### GAS/BRAKE ###
if self.car_fingerprint == CAR.VOLT:
# no output if not enabled, but keep sending keepalive messages
# threat pedals as one
final_pedal = actuators.gas - actuators.brake
# *** apply pedal hysteresis ***
final_brake, self.brake_steady = actuator_hystereses(
final_pedal, self.pedal_steady)
if not enabled:
apply_gas = P.MAX_ACC_REGEN # TODO: do we really need to send max regen when not enabled?
apply_brake = 0
else:
apply_gas = int(
round(interp(final_pedal, P.GAS_LOOKUP_BP,
P.GAS_LOOKUP_V)))
apply_brake = int(
round(
interp(final_pedal, P.BRAKE_LOOKUP_BP,
P.BRAKE_LOOKUP_V)))
# Gas/regen and brakes - all at 25Hz
if (frame % 4) == 0:
idx = (frame / 4) % 4
at_full_stop = enabled and CS.standstill
near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE)
can_sends.append(
gmcan.create_friction_brake_command(
self.packer_ch, canbus.chassis, apply_brake, idx,
near_stop, at_full_stop))
at_full_stop = enabled and CS.standstill
can_sends.append(
gmcan.create_gas_regen_command(self.packer_pt,
canbus.powertrain,
apply_gas, idx, enabled,
at_full_stop))
# Send dashboard UI commands (ACC status), 25hz
if (frame % 4) == 0:
can_sends.append(
gmcan.create_acc_dashboard_command(
canbus.powertrain, enabled,
hud_v_cruise / CV.MS_TO_KPH, hud_show_car))
# Radar needs to know current speed and yaw rate (50hz),
# and that ADAS is alive (10hz)
time_and_headlights_step = 10
tt = sec_since_boot()
if frame % time_and_headlights_step == 0:
idx = (frame / time_and_headlights_step) % 4
can_sends.append(
gmcan.create_adas_time_status(
canbus.obstacle, int((tt - self.start_time) * 60),
idx))
can_sends.append(
gmcan.create_adas_headlights_status(canbus.obstacle))
speed_and_accelerometer_step = 2
if frame % speed_and_accelerometer_step == 0:
idx = (frame / speed_and_accelerometer_step) % 4
can_sends.append(
gmcan.create_adas_steering_status(canbus.obstacle, idx))
can_sends.append(
gmcan.create_adas_accelerometer_speed_status(
canbus.obstacle, CS.v_ego, idx))
# Send ADAS keepalive, 10hz
if frame % P.ADAS_KEEPALIVE_STEP == 0:
can_sends += gmcan.create_adas_keepalive(canbus.powertrain)
# Send chimes
if self.chime != chime:
duration = 0x3c
# There is no 'repeat forever' chime command
# TODO: Manage periodic re-issuing of chime command
# and chime cancellation
if chime_cnt == -1:
chime_cnt = 10
if chime != 0:
can_sends.append(
gmcan.create_chime_command(canbus.sw_gmlan, chime,
duration, chime_cnt))
# If canceling a repeated chime, cancel command must be
# issued for the same chime type and duration
self.chime = chime
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, sendcan, enabled, CS, frame, actuators, \
hud_v_cruise, hud_show_lanes, hud_show_car, chime, chime_cnt):
""" Controls thread """
# Sanity check.
if not self.allow_controls:
return
P = self.params
# Send CAN commands.
can_sends = []
canbus = self.canbus
### STEER ###
if (frame % P.STEER_STEP) == 0:
lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > 3.
if lkas_enabled:
apply_steer = actuators.steer * P.STEER_MAX
apply_steer = apply_std_steer_torque_limits(
apply_steer, self.apply_steer_last, CS.steer_torque_driver,
P)
else:
apply_steer = 0
self.apply_steer_last = apply_steer
idx = (frame / P.STEER_STEP) % 4
if self.car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA,
CAR.ACADIA, CAR.CADILLAC_ATS):
can_sends.append(
gmcan.create_steering_control(self.packer_pt,
canbus.powertrain,
apply_steer, idx,
lkas_enabled))
if self.car_fingerprint == CAR.CADILLAC_CT6:
can_sends += gmcan.create_steering_control_ct6(
self.packer_pt, canbus, apply_steer, CS.v_ego, idx,
lkas_enabled)
### GAS/BRAKE ###
if self.car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA,
CAR.ACADIA, CAR.CADILLAC_ATS):
# no output if not enabled, but keep sending keepalive messages
# treat pedals as one
final_pedal = actuators.gas - actuators.brake
# *** apply pedal hysteresis ***
final_brake, self.brake_steady = actuator_hystereses(
final_pedal, self.pedal_steady)
if not enabled:
# Stock ECU sends max regen when not enabled.
apply_gas = P.MAX_ACC_REGEN
apply_brake = 0
else:
apply_gas = int(
round(interp(final_pedal, P.GAS_LOOKUP_BP,
P.GAS_LOOKUP_V)))
apply_brake = int(
round(
interp(final_pedal, P.BRAKE_LOOKUP_BP,
P.BRAKE_LOOKUP_V)))
# Gas/regen and brakes - all at 25Hz
if (frame % 4) == 0:
idx = (frame / 4) % 4
at_full_stop = enabled and CS.standstill
near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE)
can_sends.append(
gmcan.create_friction_brake_command(
self.packer_ch, canbus.chassis, apply_brake, idx,
near_stop, at_full_stop))
at_full_stop = enabled and CS.standstill
can_sends.append(
gmcan.create_gas_regen_command(self.packer_pt,
canbus.powertrain,
apply_gas, idx, enabled,
at_full_stop))
# Send dashboard UI commands (ACC status), 25hz
if (frame % 4) == 0:
can_sends.append(
gmcan.create_acc_dashboard_command(
self.packer_pt, canbus.powertrain, enabled,
hud_v_cruise * CV.MS_TO_KPH, hud_show_car))
# Radar needs to know current speed and yaw rate (50hz),
# and that ADAS is alive (10hz)
time_and_headlights_step = 10
tt = sec_since_boot()
if frame % time_and_headlights_step == 0:
idx = (frame / time_and_headlights_step) % 4
can_sends.append(
gmcan.create_adas_time_status(
canbus.obstacle, int((tt - self.start_time) * 60),
idx))
can_sends.append(
gmcan.create_adas_headlights_status(canbus.obstacle))
speed_and_accelerometer_step = 2
if frame % speed_and_accelerometer_step == 0:
idx = (frame / speed_and_accelerometer_step) % 4
can_sends.append(
gmcan.create_adas_steering_status(canbus.obstacle, idx))
can_sends.append(
gmcan.create_adas_accelerometer_speed_status(
canbus.obstacle, CS.v_ego, idx))
if frame % P.ADAS_KEEPALIVE_STEP == 0:
can_sends += gmcan.create_adas_keepalive(canbus.powertrain)
# Show green icon when LKA torque is applied, and
# alarming orange icon when approaching torque limit.
# If not sent again, LKA icon disappears in about 5 seconds.
# Conveniently, sending camera message periodically also works as a keepalive.
lka_active = CS.lkas_status == 1
lka_critical = lka_active and abs(actuators.steer) > 0.9
lka_icon_status = (lka_active, lka_critical)
if frame % P.CAMERA_KEEPALIVE_STEP == 0 \
or lka_icon_status != self.lka_icon_status_last:
can_sends.append(
gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active,
lka_critical))
self.lka_icon_status_last = lka_icon_status
# Send chimes
if self.chime != chime:
duration = 0x3c
# There is no 'repeat forever' chime command
# TODO: Manage periodic re-issuing of chime command
# and chime cancellation
if chime_cnt == -1:
chime_cnt = 10
if chime != 0:
can_sends.append(
gmcan.create_chime_command(canbus.sw_gmlan, chime,
duration, chime_cnt))
# If canceling a repeated chime, cancel command must be
# issued for the same chime type and duration
self.chime = chime
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, enabled, CS, frame, actuators, hud_v_cruise,
hud_show_lanes, hud_show_car, hud_alert):
P = self.params
# Send CAN commands.
can_sends = []
# STEER
if (frame % P.STEER_STEP) == 0:
lkas_enabled = enabled and not CS.out.steerWarning and CS.lkMode and CS.out.vEgo > P.MIN_STEER_SPEED
if lkas_enabled:
new_steer = int(round(actuators.steer * P.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
self.steer_rate_limited = new_steer != apply_steer
else:
apply_steer = 0
self.apply_steer_last = apply_steer
idx = (frame // P.STEER_STEP) % 4
can_sends.append(
gmcan.create_steering_control(self.packer_pt,
CanBus.POWERTRAIN, apply_steer,
idx, lkas_enabled))
# GAS/BRAKE
# no output if not enabled, but keep sending keepalive messages
# treat pedals as one
final_pedal = actuators.gas - actuators.brake
if not enabled:
# Stock ECU sends max regen when not enabled.
apply_gas = P.MAX_ACC_REGEN
apply_brake = 0
else:
apply_gas = int(
round(interp(final_pedal, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V)))
apply_brake = int(
round(interp(final_pedal, P.BRAKE_LOOKUP_BP,
P.BRAKE_LOOKUP_V)))
# Gas/regen and brakes - all at 25Hz
if (frame % 4) == 0:
idx = (frame // 4) % 4
if CS.cruiseMain and not enabled and CS.autoHold and CS.autoHoldActive and not CS.out.gasPressed and CS.out.gearShifter == 'drive' and CS.out.vEgo < 0.01 and not CS.regenPaddlePressed:
# Auto Hold State
car_stopping = apply_gas < P.ZERO_GAS
standstill = CS.pcm_acc_status == AccState.STANDSTILL
at_full_stop = standstill and car_stopping
near_stop = (CS.out.vEgo <
P.NEAR_STOP_BRAKE_PHASE) and car_stopping
can_sends.append(
gmcan.create_friction_brake_command(
self.packer_ch, CanBus.CHASSIS, apply_brake, idx,
near_stop, at_full_stop))
CS.autoHoldActivated = True
else:
car_stopping = apply_gas < P.ZERO_GAS
standstill = CS.pcm_acc_status == AccState.STANDSTILL
at_full_stop = enabled and standstill and car_stopping
near_stop = enabled and (
CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE) and car_stopping
can_sends.append(
gmcan.create_friction_brake_command(
self.packer_ch, CanBus.CHASSIS, apply_brake, idx,
near_stop, at_full_stop))
CS.autoHoldActivated = False
# Auto-resume from full stop by resetting ACC control
acc_enabled = enabled
if standstill and not car_stopping:
acc_enabled = False
can_sends.append(
gmcan.create_gas_regen_command(self.packer_pt,
CanBus.POWERTRAIN,
apply_gas, idx, acc_enabled,
at_full_stop))
follow_level = CS.get_follow_level()
# Send dashboard UI commands (ACC status), 25hz
if (frame % 4) == 0:
send_fcw = hud_alert == VisualAlert.fcw
can_sends.append(
gmcan.create_acc_dashboard_command(self.packer_pt,
CanBus.POWERTRAIN, enabled,
hud_v_cruise * CV.MS_TO_KPH,
hud_show_car, send_fcw,
follow_level))
# Radar needs to know current speed and yaw rate (50hz),
# and that ADAS is alive (10hz)
time_and_headlights_step = 10
tt = frame * DT_CTRL
if frame % time_and_headlights_step == 0:
idx = (frame // time_and_headlights_step) % 4
can_sends.append(
gmcan.create_adas_time_status(CanBus.OBSTACLE,
int((tt - self.start_time) * 60),
idx))
can_sends.append(
gmcan.create_adas_headlights_status(self.packer_obj,
CanBus.OBSTACLE))
speed_and_accelerometer_step = 2
if frame % speed_and_accelerometer_step == 0:
idx = (frame // speed_and_accelerometer_step) % 4
can_sends.append(
gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx))
can_sends.append(
gmcan.create_adas_accelerometer_speed_status(
CanBus.OBSTACLE, CS.out.vEgo, idx))
if frame % P.ADAS_KEEPALIVE_STEP == 0:
can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN)
# Show green icon when LKA torque is applied, and
# alarming orange icon when approaching torque limit.
# If not sent again, LKA icon disappears in about 5 seconds.
# Conveniently, sending camera message periodically also works as a keepalive.
lka_active = CS.lkas_status == 1
lka_critical = lka_active and abs(actuators.steer) > 0.9
lka_icon_status = (lka_active, lka_critical)
if frame % P.CAMERA_KEEPALIVE_STEP == 0 or lka_icon_status != self.lka_icon_status_last:
steer_alert = hud_alert == VisualAlert.steerRequired
can_sends.append(
gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active,
lka_critical, steer_alert))
self.lka_icon_status_last = lka_icon_status
return can_sends
def update(self, sendcan, enabled, CS, frame, actuators, \
hud_v_cruise, hud_show_lanes, hud_show_car, chime, chime_cnt):
""" Controls thread """
#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)
# Sanity check.
if not self.allow_controls:
return
P = self.params
# Send CAN commands.
can_sends = []
canbus = self.canbus
### STEER ###
if (frame % P.STEER_STEP) == 0:
lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > P.MIN_STEER_SPEED
if lkas_enabled:
apply_steer = alca_steer * P.STEER_MAX
apply_steer = apply_std_steer_torque_limits(
apply_steer, self.apply_steer_last, CS.steer_torque_driver,
P)
else:
apply_steer = 0
self.apply_steer_last = apply_steer
idx = (frame / P.STEER_STEP) % 4
if CS.cstm_btns.get_button_status("lka") == 0:
apply_steer = 0
if self.car_fingerprint in SUPERCRUISE_CARS:
can_sends += gmcan.create_steering_control_ct6(
self.packer_pt, canbus, apply_steer, CS.v_ego, idx,
lkas_enabled)
else:
can_sends.append(
gmcan.create_steering_control(self.packer_pt,
canbus.powertrain,
apply_steer, idx,
lkas_enabled))
### GAS/BRAKE ###
if self.car_fingerprint not in SUPERCRUISE_CARS:
# no output if not enabled, but keep sending keepalive messages
# treat pedals as one
final_pedal = actuators.gas - actuators.brake
# *** apply pedal hysteresis ***
final_brake, self.brake_steady = actuator_hystereses(
final_pedal, self.pedal_steady)
if not enabled:
# Stock ECU sends max regen when not enabled.
apply_gas = P.MAX_ACC_REGEN
apply_brake = 0
else:
apply_gas = int(
round(interp(final_pedal, P.GAS_LOOKUP_BP,
P.GAS_LOOKUP_V)))
apply_brake = int(
round(
interp(final_pedal, P.BRAKE_LOOKUP_BP,
P.BRAKE_LOOKUP_V)))
# Gas/regen and brakes - all at 25Hz
if (frame % 4) == 0:
idx = (frame / 4) % 4
car_stopping = apply_gas < P.ZERO_GAS
standstill = CS.pcm_acc_status == AccState.STANDSTILL
at_full_stop = enabled and standstill and car_stopping
near_stop = enabled and (
CS.v_ego < P.NEAR_STOP_BRAKE_PHASE) and car_stopping
can_sends.append(
gmcan.create_friction_brake_command(
self.packer_ch, canbus.chassis, apply_brake, idx,
near_stop, at_full_stop))
acc_enabled = enabled
if CS.cstm_btns.get_button_status("stop") > 0:
# Auto-resume from full stop by resetting ACC control
if standstill and not car_stopping:
acc_enabled = False
can_sends.append(
gmcan.create_gas_regen_command(self.packer_pt,
canbus.powertrain,
apply_gas, idx, acc_enabled,
at_full_stop))
# Send dashboard UI commands (ACC status), 25hz
follow_level = CS.get_follow_level()
if (frame % 4) == 0:
can_sends.append(
gmcan.create_acc_dashboard_command(
self.packer_pt, canbus.powertrain, enabled,
hud_v_cruise * CV.MS_TO_KPH, hud_show_car,
follow_level))
# Radar needs to know current speed and yaw rate (50hz),
# and that ADAS is alive (10hz)
time_and_headlights_step = 10
tt = sec_since_boot()
if frame % time_and_headlights_step == 0:
idx = (frame / time_and_headlights_step) % 4
can_sends.append(
gmcan.create_adas_time_status(
canbus.obstacle, int((tt - self.start_time) * 60),
idx))
can_sends.append(
gmcan.create_adas_headlights_status(canbus.obstacle))
speed_and_accelerometer_step = 2
if frame % speed_and_accelerometer_step == 0:
idx = (frame / speed_and_accelerometer_step) % 4
can_sends.append(
gmcan.create_adas_steering_status(canbus.obstacle, idx))
can_sends.append(
gmcan.create_adas_accelerometer_speed_status(
canbus.obstacle, CS.v_ego, idx))
if frame % P.ADAS_KEEPALIVE_STEP == 0:
can_sends += gmcan.create_adas_keepalive(canbus.powertrain)
# Show green icon when LKA torque is applied, and
# alarming orange icon when approaching torque limit.
# If not sent again, LKA icon disappears in about 5 seconds.
# Conveniently, sending camera message periodically also works as a keepalive.
lka_active = CS.lkas_status == 1
lka_critical = lka_active and abs(actuators.steer) > 0.9
lka_icon_status = (lka_active, lka_critical)
if frame % P.CAMERA_KEEPALIVE_STEP == 0 \
or lka_icon_status != self.lka_icon_status_last:
can_sends.append(
gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active,
lka_critical))
self.lka_icon_status_last = lka_icon_status
# Send chimes
if self.chime != chime:
duration = 0x3c
# There is no 'repeat forever' chime command
# TODO: Manage periodic re-issuing of chime command
# and chime cancellation
if chime_cnt == -1:
chime_cnt = 10
if chime != 0:
can_sends.append(
gmcan.create_chime_command(canbus.sw_gmlan, chime,
duration, chime_cnt))
# If canceling a repeated chime, cancel command must be
# issued for the same chime type and duration
self.chime = chime
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, CC, CS):
actuators = CC.actuators
hud_control = CC.hudControl
hud_alert = hud_control.visualAlert
hud_v_cruise = hud_control.setSpeed
if hud_v_cruise > 70:
hud_v_cruise = 0
# Send CAN commands.
can_sends = []
# Steering (50Hz)
# Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we just received the
# next Panda loopback confirmation in the current CS frame.
if CS.lka_steering_cmd_counter != self.lka_steering_cmd_counter_last:
self.lka_steering_cmd_counter_last = CS.lka_steering_cmd_counter
elif (self.frame % self.params.STEER_STEP) == 0:
lkas_enabled = CC.latActive and CS.out.vEgo > self.params.MIN_STEER_SPEED
if lkas_enabled:
new_steer = int(round(actuators.steer * self.params.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params)
self.steer_rate_limited = new_steer != apply_steer
else:
apply_steer = 0
self.apply_steer_last = apply_steer
# GM EPS faults on any gap in received message counters. To handle transient OP/Panda safety sync issues at the
# moment of disengaging, increment the counter based on the last message known to pass Panda safety checks.
idx = (CS.lka_steering_cmd_counter + 1) % 4
can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled))
# Gas/regen and brakes - all at 25Hz
if (self.frame % 4) == 0:
if not CC.longActive:
# Stock ECU sends max regen when not enabled.
self.apply_gas = self.params.MAX_ACC_REGEN
self.apply_brake = 0
else:
self.apply_gas = int(round(interp(actuators.accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V)))
self.apply_brake = int(round(interp(actuators.accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V)))
idx = (self.frame // 4) % 4
at_full_stop = CC.longActive and CS.out.standstill
near_stop = CC.longActive and (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE)
can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, self.apply_brake, idx, near_stop, at_full_stop))
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, CC.longActive, at_full_stop))
# Send dashboard UI commands (ACC status), 25hz
if (self.frame % 4) == 0:
send_fcw = hud_alert == VisualAlert.fcw
can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.longActive,
hud_v_cruise * CV.MS_TO_KPH, hud_control.leadVisible, send_fcw))
# Radar needs to know current speed and yaw rate (50hz),
# and that ADAS is alive (10hz)
time_and_headlights_step = 10
tt = self.frame * DT_CTRL
if self.frame % time_and_headlights_step == 0:
idx = (self.frame // time_and_headlights_step) % 4
can_sends.append(gmcan.create_adas_time_status(CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx))
can_sends.append(gmcan.create_adas_headlights_status(self.packer_obj, CanBus.OBSTACLE))
speed_and_accelerometer_step = 2
if self.frame % speed_and_accelerometer_step == 0:
idx = (self.frame // speed_and_accelerometer_step) % 4
can_sends.append(gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx))
can_sends.append(gmcan.create_adas_accelerometer_speed_status(CanBus.OBSTACLE, CS.out.vEgo, idx))
if self.frame % self.params.ADAS_KEEPALIVE_STEP == 0:
can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN)
# Show green icon when LKA torque is applied, and
# alarming orange icon when approaching torque limit.
# If not sent again, LKA icon disappears in about 5 seconds.
# Conveniently, sending camera message periodically also works as a keepalive.
lka_active = CS.lkas_status == 1
lka_critical = lka_active and abs(actuators.steer) > 0.9
lka_icon_status = (lka_active, lka_critical)
if self.frame % self.params.CAMERA_KEEPALIVE_STEP == 0 or lka_icon_status != self.lka_icon_status_last:
steer_alert = hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw)
can_sends.append(gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active, lka_critical, steer_alert))
self.lka_icon_status_last = lka_icon_status
new_actuators = actuators.copy()
new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX
new_actuators.gas = self.apply_gas
new_actuators.brake = self.apply_brake
self.frame += 1
return new_actuators, can_sends
def update(self, enabled, CS, frame, actuators, \
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
P = self.params
# Send CAN commands.
can_sends = []
# FCW: trigger FCWAlert for 100 frames (4 seconds)
if hud_alert == VisualAlert.fcw:
self.fcw_frames = 100
### STEER ###
if (frame % P.STEER_STEP) == 0:
lkas_enabled = enabled and not CS.out.steerWarning and CS.out.vEgo > P.MIN_STEER_SPEED
if lkas_enabled:
if CS.out.vEgo < 8.0:
self.steer_max = 180
elif CS.out.vEgo < 12.5:
self.steer_max = 220
elif CS.out.vEgo < 16.6:
self.steer_max = 250
elif CS.out.vEgo < 20.0:
self.steer_max = 260
else:
self.steer_max = P.STEER_MAX * 0.9
new_steer = actuators.steer * self.steer_max
apply_steer = apply_std_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
self.steer_rate_limited = new_steer != apply_steer
else:
apply_steer = 0
self.apply_steer_last = apply_steer
idx = (frame // P.STEER_STEP) % 4
can_sends.append(
gmcan.create_steering_control(self.packer_pt,
CanBus.POWERTRAIN, apply_steer,
idx, lkas_enabled))
### GAS/BRAKE ###
# no output if not enabled, but keep sending keepalive messages
# treat pedals as one
final_pedal = actuators.gas - actuators.brake
# *** apply pedal hysteresis ***
final_brake, self.brake_steady = actuator_hystereses(
final_pedal, self.pedal_steady)
if not enabled:
# Stock ECU sends max regen when not enabled.
apply_gas = P.MAX_ACC_REGEN
apply_brake = 0
else:
apply_gas = int(
round(interp(final_pedal, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V)))
apply_brake = int(
round(interp(final_pedal, P.BRAKE_LOOKUP_BP,
P.BRAKE_LOOKUP_V)))
# Gas/regen and brakes - all at 25Hz
if (frame % 4) == 0:
idx = (frame // 4) % 4
#at_full_stop = enabled and CS.out.standstill
#near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE)
car_stopping = apply_gas < P.ZERO_GAS
standstill = CS.pcm_acc_status == AccState.STANDSTILL
at_full_stop = enabled and standstill and car_stopping
near_stop = enabled and (CS.out.vEgo <
P.NEAR_STOP_BRAKE_PHASE) and car_stopping
can_sends.append(
gmcan.create_friction_brake_command(self.packer_ch,
CanBus.CHASSIS,
apply_brake, idx,
near_stop, at_full_stop))
#at_full_stop = enabled and CS.out.standstill
#can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, apply_gas, idx, enabled, at_full_stop))
# Send dashboard UI commands (ACC status), 25hz
if (frame % 4) == 0:
# Send FCW if applicable
send_fcw = 0
if self.fcw_frames > 0:
send_fcw = 0x3
self.fcw_frames -= 1
can_sends.append(
gmcan.create_acc_dashboard_command(self.packer_pt,
CanBus.POWERTRAIN, enabled,
hud_v_cruise * CV.MS_TO_KPH,
hud_show_car, send_fcw))
# Radar needs to know current speed and yaw rate (50hz),
# and that ADAS is alive (10hz)
time_and_headlights_step = 10
tt = frame * DT_CTRL
if frame % time_and_headlights_step == 0:
idx = (frame // time_and_headlights_step) % 4
can_sends.append(
gmcan.create_adas_time_status(CanBus.OBSTACLE,
int((tt - self.start_time) * 60),
idx))
can_sends.append(
gmcan.create_adas_headlights_status(CanBus.OBSTACLE))
speed_and_accelerometer_step = 2
if frame % speed_and_accelerometer_step == 0:
idx = (frame // speed_and_accelerometer_step) % 4
can_sends.append(
gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx))
can_sends.append(
gmcan.create_adas_accelerometer_speed_status(
CanBus.OBSTACLE, CS.out.vEgo, idx))
if frame % P.ADAS_KEEPALIVE_STEP == 0:
can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN)
# Show green icon when LKA torque is applied, and
# alarming orange icon when approaching torque limit.
# If not sent again, LKA icon disappears in about 5 seconds.
# Conveniently, sending camera message periodically also works as a keepalive.
lka_active = CS.lkas_status == 1
lka_critical = lka_active and abs(actuators.steer) > 0.9
lka_icon_status = (lka_active, lka_critical)
if frame % P.CAMERA_KEEPALIVE_STEP == 0 \
or lka_icon_status != self.lka_icon_status_last:
steer_alert = hud_alert == VisualAlert.steerRequired
can_sends.append(
gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active,
lka_critical, steer_alert))
self.lka_icon_status_last = lka_icon_status
return can_sends
