def test_correctness(self):
# Test all cars' commands, randomize the params.
for _ in range(1000):
bus = random.randint(0, 65536)
apply_steer = (random.randint(0, 2) % 2 == 0)
idx = random.randint(0, 65536)
lkas_active = (random.randint(0, 2) % 2 == 0)
m_old = gmcan.create_steering_control(self.gm_cp_old, bus,
apply_steer, idx,
lkas_active)
m = gmcan.create_steering_control(self.gm_cp, bus, apply_steer,
idx, lkas_active)
self.assertEqual(m_old, m)
canbus = GMCanBus()
apply_steer = (random.randint(0, 2) % 2 == 0)
v_ego = random.randint(0, 65536)
idx = random.randint(0, 65536)
enabled = (random.randint(0, 2) % 2 == 0)
m_old = gmcan.create_steering_control_ct6(self.ct6_cp_old, canbus,
apply_steer, v_ego, idx,
enabled)
m = gmcan.create_steering_control_ct6(self.ct6_cp, canbus,
apply_steer, v_ego, idx,
enabled)
self.assertEqual(m_old, m)
bus = random.randint(0, 65536)
throttle = random.randint(0, 65536)
idx = random.randint(0, 65536)
acc_engaged = (random.randint(0, 2) % 2 == 0)
at_full_stop = (random.randint(0, 2) % 2 == 0)
m_old = gmcan.create_gas_regen_command(self.gm_cp_old, bus,
throttle, idx, acc_engaged,
at_full_stop)
m = gmcan.create_gas_regen_command(self.gm_cp, bus, throttle, idx,
acc_engaged, at_full_stop)
self.assertEqual(m_old, m)
bus = random.randint(0, 65536)
apply_brake = (random.randint(0, 2) % 2 == 0)
idx = random.randint(0, 65536)
near_stop = (random.randint(0, 2) % 2 == 0)
at_full_stop = (random.randint(0, 2) % 2 == 0)
m_old = gmcan.create_friction_brake_command(
self.ct6_cp_old, bus, apply_brake, idx, near_stop,
at_full_stop)
m = gmcan.create_friction_brake_command(self.ct6_cp, bus,
apply_brake, idx,
near_stop, at_full_stop)
self.assertEqual(m_old, m)
bus = random.randint(0, 65536)
acc_engaged = (random.randint(0, 2) % 2 == 0)
#target_speed_kph = random.randint(0, 65536)
#lead_car_in_sight = (random.randint(0, 2) % 2 == 0)
#m_old = gmcan.create_acc_dashboard_command(self.gm_cp_old, bus, acc_engaged, target_speed_kph, lead_car_in_sight)
#m = gmcan.create_acc_dashboard_command(self.gm_cp, bus, acc_engaged, target_speed_kph, lead_car_in_sight)
self.assertEqual(m_old, m)
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.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
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))
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 = 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, paddleFrame):
##print("paddleFrame %d", paddleFrame)
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
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
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
## 인게이지 상태 아니고 오토홀드 스위치 들어가 있고 엑셀 페달 밟는 상태가 아니고 기어가 d,l에 있으며 속도가 1.8kph 이하일때 동작
##if not enabled and CS.autoHold and not CS.out.gasPressed and CS.out.gearShifter == 'drive' and CS.out.vEgo < 0.01 and not CS.regenPaddlePressed:
if 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:
car_stopping = apply_gas < P.ZERO_GAS
standstill = CS.out.vEgo < 0.01 ## neokii test
at_full_stop = standstill and car_stopping
near_stop = (CS.out.vEgo <
P.NEAR_STOP_BRAKE_PHASE) and car_stopping
##if at_full_stop :
##print("A------------------")
##print("apply_brake", apply_brake)
##print("car_stopping", car_stopping)
##print("apply_gas ", apply_gas)
##print("P.ZERO_GAS ", P.ZERO_GAS)
##print("standstill ", standstill)
##print("CS.pcm_acc_status ", CS.pcm_acc_status)
##print("AccState.STANDSTILL ", AccState.STANDSTILL)
##print("at_full_stop", at_full_stop)
##print("near_stop ", near_stop)
##print("CS.out.vEgo ", CS.out.vEgo)
##print(" P.NEAR_STOP_BRAKE_PHASE ", P.NEAR_STOP_BRAKE_PHASE)
##print("enabled ", enabled)
#print("CS.autoHold ", CS.autoHold)
#print("CS.autoHoldActive ", CS.autoHoldActive)
##print("CS.gas", CS.out.gas)
##print("CS.gasPressed", CS.out.gasPressed)
##print("gearShifter ", CS.out.gearShifter)
####at_full_stop = standstill and car_stopping
####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_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
##print("B------------------")
##print("apply_brake", apply_brake)
##print("car_stopping", car_stopping)
##print("apply_gas ", apply_gas)
##print("P.ZERO_GAS ", P.ZERO_GAS)
##print("standstill ", standstill)
##print("CS.pcm_acc_status ", CS.pcm_acc_status)
##print("AccState.STANDSTILL ", AccState.STANDSTILL)
##print("at_full_stop", at_full_stop)
##print("near_stop ", near_stop)
##print("CS.out.vEgo ", CS.out.vEgo)
##print(" P.NEAR_STOP_BRAKE_PHASE ", P.NEAR_STOP_BRAKE_PHASE)
##print("enabled ", enabled)
#print("CS.autoHold ", CS.autoHold)
#print("CS.autoHoldActive ", CS.autoHoldActive)
##print("CS.gas", CS.out.gas)
##print("CS.gasPressed", CS.out.gasPressed)
##print("gearShifter ", CS.out.gearShifter)
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)
# 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 CS.cstm_btns.get_button_status("lka") == 0:
apply_steer = 0
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
# 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))
# 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))
# 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, 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)
# GasRegenCmdActive needs to be 1 to avoid cruise faults. It describes the ACC state, not actuation
can_sends.append(
gmcan.create_gas_regen_command(self.packer_pt,
CanBus.POWERTRAIN,
self.apply_gas, idx, CC.enabled,
at_full_stop))
can_sends.append(
gmcan.create_friction_brake_command(self.packer_ch,
CanBus.CHASSIS,
self.apply_brake, idx,
near_stop, 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.enabled,
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 = []
# STEER
lkas_enabled = enabled and not CS.out.steerWarning and CS.out.vEgo > P.MIN_STEER_SPEED and CS.enable_lkas
if (frame % P.STEER_STEP) == 0:
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))
# Pedal
if CS.CP.enableGasInterceptor:
if (frame % 2) == 0:
idx = (frame // 2) % 4
zero = 0.15625 * 2 #40/256
accel = (1 -
zero) * actuators.gas + self.apply_pedal_last * zero
regen_brake = zero * actuators.brake
final_accel = accel - regen_brake
if not enabled or not CS.adaptive_Cruise:
final_accel = 0.
final_accel, self.accel_steady = accel_hysteresis(
final_accel, self.accel_steady)
final_pedal = clip(final_accel, 0., 1.)
self.apply_pedal_last = final_pedal
can_sends.append(
create_gas_command(self.packer_pt, final_pedal, idx))
# 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) - Delete
# and that ADAS is alive (10hz)
#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 = lkas_enabled == 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 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
return can_sends
def update(self, c, enabled, CS, frame, actuators, hud_v_cruise,
hud_show_lanes, hud_show_car, hud_alert):
P = self.params
# 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
if (frame % P.STEER_STEP) == 0:
lkas_enabled = c.active and not (
CS.out.steerWarning
or CS.out.steerError) 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
# 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))
# TODO: All three conditions should not be required - really only last two?
if CS.CP.carFingerprint not in NO_ASCM and CS.CP.openpilotLongitudinalControl and not CS.CP.pcmCruise:
# Gas/regen and brakes - all at 25Hz
if (frame % 4) == 0:
if not c.active:
# Stock ECU sends max regen when not enabled.
self.apply_gas = P.MAX_ACC_REGEN
self.apply_brake = 0
else:
self.apply_gas = int(
round(
interp(actuators.accel, P.GAS_LOOKUP_BP,
P.GAS_LOOKUP_V)))
self.apply_brake = int(
round(
interp(actuators.accel, P.BRAKE_LOOKUP_BP,
P.BRAKE_LOOKUP_V)))
idx = (frame // 4) % 4
# TODO: Should all instances of "enabled" be replaced with c.active?
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, 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,
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)
elif CS.CP.openpilotLongitudinalControl:
# Gas/regen and brakes - all at 25Hz
if (frame % 4) == 0:
if not c.active:
# Stock ECU sends max regen when not enabled.
self.apply_gas = P.MAX_ACC_REGEN
self.apply_brake = 0
else:
self.apply_gas = int(
round(
interp(actuators.accel, P.GAS_LOOKUP_BP,
P.GAS_LOOKUP_V)))
self.apply_brake = int(
round(
interp(actuators.accel, P.BRAKE_LOOKUP_BP,
P.BRAKE_LOOKUP_V)))
idx = (frame // 4) % 4
at_full_stop = enabled and CS.out.standstill
# near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE)
# VOACC based cars have brakes on PT bus - OP won't be doing VOACC for a while
# can_sends.append(gmcan.create_friction_brake_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_brake, idx, near_stop, at_full_stop))
if CS.CP.enableGasInterceptor:
# TODO: JJS Unsure if low is single pedal mode in any non-electric cars
singlePedalMode = CS.out.gearShifter == GearShifter.low and CS.CP.carFingerprint in EV_CAR
# TODO: JJS Detect saturated battery and fallback to D mode (until regen is available
if singlePedalMode:
# In L Mode, Pedal applies regen at a fixed coast-point (TODO: max regen in L mode may be different per car)
# This will apply to EVs in L mode.
# accel values below zero down to a cutoff point
# that approximates the percentage of braking regen can handle should be scaled between 0 and the coast-point
# accell values below this point will need to be add-on future hijacked AEB
# TODO: Determine (or guess) at regen precentage
# From Felger's Bolt Bort
#It seems in L mode, accel / decel point is around 1/5
#-1-------AEB------0----regen---0.15-------accel----------+1
# Shrink gas request to 0.85, have it start at 0.2
# Shrink brake request to 0.85, first 0.15 gives regen, rest gives AEB
zero = 40 / 256
if (actuators.accel > 0.):
pedal_gas = clip(
((1 - zero) * actuators.accel + zero), 0., 1.)
else:
pedal_gas = clip(
actuators.accel, 0., zero
) # Make brake the same size as gas, but clip to regen
# aeb = actuators.brake*(1-zero)-regen # For use later, braking more than regen
else:
# In D Mode, Pedal is close to coast at 0, 100% at 1.
# This will apply to non-EVs and EVs in D mode.
# accel values below zero will need to be handled by future hijacked AEB
# TODO: Determine if this clipping is correct
pedal_gas = clip(actuators.accel, 0., 1.)
can_sends.append(
create_gas_interceptor_command(self.packer_pt,
pedal_gas, idx))
else:
can_sends.append(
gmcan.create_gas_regen_command(self.packer_pt,
CanBus.POWERTRAIN,
self.apply_gas, idx,
enabled, at_full_stop))
# 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 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 / P.STEER_MAX
new_actuators.gas = self.apply_gas
new_actuators.brake = self.apply_brake
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 = []
# STEER
if (frame % P.STEER_STEP) == 0:
lkas_enabled = (
enabled or CS.pause_long_on_gas_press
) and CS.lkMode and not (
CS.out.steerWarning or CS.out.steerError
) and CS.out.vEgo > P.MIN_STEER_SPEED and CS.lane_change_steer_factor > 0.
if lkas_enabled:
new_steer = int(
round(actuators.steer * P.STEER_MAX *
CS.lane_change_steer_factor))
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/regen prep
if not enabled or CS.pause_long_on_gas_press:
# Stock ECU sends max regen when not enabled.
apply_gas = P.MAX_ACC_REGEN
apply_brake = 0
else:
apply_gas = interp(actuators.accel, P.GAS_LOOKUP_BP,
P.GAS_LOOKUP_V)
apply_brake = interp(actuators.accel, P.BRAKE_LOOKUP_BP,
P.BRAKE_LOOKUP_V)
t = sec_since_boot()
v_rel = CS.coasting_lead_v - CS.vEgo
ttc = min(
-CS.coasting_lead_d / v_rel if
(CS.coasting_lead_d > 0. and v_rel < 0.) else 100., 100.)
d_time = CS.coasting_lead_d / CS.vEgo if (CS.coasting_lead_d > 0.
and CS.vEgo > 0.
and CS.tr > 0.) else 10.
if CS.coasting_lead_d > 0. and (ttc < CS.lead_ttc_long_gas_lockout_bp[-1] \
or v_rel < CS.lead_v_rel_long_gas_lockout_bp[-1] \
or CS.coasting_lead_v < CS.lead_v_long_gas_lockout_bp[-1] \
or d_time < CS.tr * CS.lead_tr_long_gas_lockout_bp[-1]\
or CS.coasting_lead_d < CS.lead_d_long_gas_lockout_bp[-1]):
lead_long_gas_lockout_factor = max([
interp(v_rel, CS.lead_v_rel_long_gas_lockout_bp,
CS.lead_v_rel_long_gas_lockout_v),
interp(CS.coasting_lead_v, CS.lead_v_long_gas_lockout_bp,
CS.lead_v_long_gas_lockout_v),
interp(ttc, CS.lead_ttc_long_gas_lockout_bp,
CS.lead_ttc_long_gas_lockout_v),
interp(d_time / CS.tr, CS.lead_tr_long_gas_lockout_bp,
CS.lead_tr_long_gas_lockout_v),
interp(CS.coasting_lead_d, CS.lead_d_long_gas_lockout_bp,
CS.lead_d_long_gas_lockout_v)
])
if CS.coasting_lead_d > 0. and (ttc < CS.lead_ttc_long_brake_lockout_bp[-1] \
or v_rel < CS.lead_v_rel_long_brake_lockout_bp[-1] \
or CS.coasting_lead_v < CS.lead_v_long_brake_lockout_bp[-1] \
or d_time < CS.tr * CS.lead_tr_long_brake_lockout_bp[-1]\
or CS.coasting_lead_d < CS.lead_d_long_brake_lockout_bp[-1]):
lead_long_brake_lockout_factor = max([
interp(v_rel, CS.lead_v_rel_long_brake_lockout_bp,
CS.lead_v_rel_long_brake_lockout_v),
interp(CS.coasting_lead_v,
CS.lead_v_long_brake_lockout_bp,
CS.lead_v_long_brake_lockout_v),
interp(ttc, CS.lead_ttc_long_brake_lockout_bp,
CS.lead_ttc_long_brake_lockout_v),
interp(d_time / CS.tr,
CS.lead_tr_long_brake_lockout_bp,
CS.lead_tr_long_brake_lockout_v),
interp(CS.coasting_lead_d,
CS.lead_d_long_brake_lockout_bp,
CS.lead_d_long_brake_lockout_v)
])
else:
lead_long_brake_lockout_factor = 0. # 1.0 means regular braking logic is completely unaltered, 0.0 means no cruise braking
else:
lead_long_gas_lockout_factor = 0. # 1.0 means regular braking logic is completely unaltered, 0.0 means no cruise braking
lead_long_brake_lockout_factor = 0. # 1.0 means regular braking logic is completely unaltered, 0.0 means no cruise braking
# debug logging
do_log = self.debug_logging and (t - self.last_debug_log_t >
self.debug_log_time_step)
if do_log:
self.last_debug_log_t = t
f = open("/data/openpilot/coast_debug.csv", "a")
f.write(",".join([
f"{i:.1f}" if i == float else str(i) for i in [
t - CS.sessionInitTime, CS.coasting_long_plan,
CS.coasting_lead_d, CS.coasting_lead_v, CS.vEgo,
CS.v_cruise_kph * CV.KPH_TO_MS, CS.coasting_lead_v *
CV.MS_TO_MPH, CS.vEgo * CV.MS_TO_MPH, CS.v_cruise_kph *
CV.KPH_TO_MPH, ttc, lead_long_gas_lockout_factor,
lead_long_brake_lockout_factor,
int(apply_gas),
int(apply_brake),
(CS.one_pedal_mode_active
or CS.coast_one_pedal_mode_active
), CS.coasting_enabled, CS.no_friction_braking
]
]) + ",")
if (CS.one_pedal_mode_active or CS.coast_one_pedal_mode_active):
apply_gas = apply_gas * lead_long_gas_lockout_factor + float(
P.MAX_ACC_REGEN) * (1. - lead_long_gas_lockout_factor)
time_since_brake = t - CS.one_pedal_mode_last_gas_press_t
if CS.one_pedal_mode_active:
if abs(CS.angle_steers
) > CS.one_pedal_angle_steers_cutoff_bp[0]:
one_pedal_apply_brake = interp(
CS.vEgo, CS.one_pedal_mode_stop_apply_brake_bp[
CS.one_pedal_brake_mode],
CS.one_pedal_mode_stop_apply_brake_v[
CS.one_pedal_brake_mode])
one_pedal_apply_brake_minus1 = interp(
CS.vEgo, CS.one_pedal_mode_stop_apply_brake_bp[max(
0, CS.one_pedal_brake_mode - 1)],
CS.one_pedal_mode_stop_apply_brake_v[max(
0, CS.one_pedal_brake_mode - 1)])
one_pedal_apply_brake = interp(
abs(CS.angle_steers),
CS.one_pedal_angle_steers_cutoff_bp, [
one_pedal_apply_brake,
one_pedal_apply_brake_minus1
])
else:
one_pedal_apply_brake = interp(
CS.vEgo, CS.one_pedal_mode_stop_apply_brake_bp[
CS.one_pedal_brake_mode],
CS.one_pedal_mode_stop_apply_brake_v[
CS.one_pedal_brake_mode])
one_pedal_apply_brake *= interp(
CS.pitch, CS.one_pedal_pitch_brake_adjust_bp,
CS.one_pedal_pitch_brake_adjust_v[
CS.one_pedal_brake_mode])
one_pedal_apply_brake = min(one_pedal_apply_brake,
float(P.BRAKE_LOOKUP_V[0]))
one_pedal_apply_brake *= interp(
time_since_brake, CS.one_pedal_mode_ramp_time_bp,
CS.one_pedal_mode_ramp_time_v
) if CS.one_pedal_brake_mode < 2 else 1.
else:
one_pedal_apply_brake = 0.
# ramp braking
if CS.one_pedal_mode_active_last and time_since_brake > CS.one_pedal_mode_ramp_time_bp[
-1]:
if CS.one_pedal_mode_apply_brake != one_pedal_apply_brake:
if CS.one_pedal_mode_ramp_mode_last != CS.one_pedal_brake_mode:
# brake mode changed, so need to calculate new step based on the old and new modes
old_apply_brake = interp(
CS.vEgo, CS.one_pedal_mode_stop_apply_brake_bp[
CS.one_pedal_mode_ramp_mode_last],
CS.one_pedal_mode_stop_apply_brake_v[
CS.one_pedal_mode_ramp_mode_last])
CS.one_pedal_mode_ramp_time_step = (
one_pedal_apply_brake - old_apply_brake
) / CS.one_pedal_mode_ramp_duration
if CS.one_pedal_mode_apply_brake < one_pedal_apply_brake:
if CS.one_pedal_mode_ramp_time_step < 0.:
CS.one_pedal_mode_ramp_time_step *= -1.
CS.one_pedal_mode_apply_brake = max(
one_pedal_apply_brake,
CS.one_pedal_mode_apply_brake +
CS.one_pedal_mode_ramp_time_step *
(t - CS.one_pedal_mode_ramp_t_last))
else:
if CS.one_pedal_mode_ramp_time_step > 0.:
CS.one_pedal_mode_ramp_time_step *= -1.
CS.one_pedal_mode_apply_brake = min(
one_pedal_apply_brake,
CS.one_pedal_mode_apply_brake +
CS.one_pedal_mode_ramp_time_step *
(t - CS.one_pedal_mode_ramp_t_last))
one_pedal_apply_brake = CS.one_pedal_mode_apply_brake
else:
CS.one_pedal_mode_apply_brake = one_pedal_apply_brake
CS.one_pedal_mode_active_last = True
CS.one_pedal_mode_ramp_t_last = t
CS.one_pedal_mode_ramp_mode_last = CS.one_pedal_brake_mode
if CS.one_pedal_mode_op_braking_allowed and CS.coasting_long_plan not in [
'cruise', 'limit'
]:
apply_brake = max(
one_pedal_apply_brake,
apply_brake * lead_long_brake_lockout_factor)
else:
apply_brake = one_pedal_apply_brake
elif CS.coasting_enabled and lead_long_brake_lockout_factor < 1.:
if CS.coasting_long_plan in [
'cruise', 'limit'
] and apply_gas < P.ZERO_GAS or apply_brake > 0.:
check_speed_ms = (CS.speed_limit if CS.speed_limit_active
and CS.speed_limit < CS.v_cruise_kph else
CS.v_cruise_kph) * CV.KPH_TO_MS
if apply_brake > 0.:
coasting_over_speed_vEgo_BP = [
interp(CS.vEgo, CS.coasting_over_speed_vEgo_BP_BP,
CS.coasting_over_speed_vEgo_BP[0]),
interp(CS.vEgo, CS.coasting_over_speed_vEgo_BP_BP,
CS.coasting_over_speed_vEgo_BP[1])
]
over_speed_factor = interp(
CS.vEgo / check_speed_ms,
coasting_over_speed_vEgo_BP, [0., 1.]) if (
check_speed_ms > 0. and
CS.coasting_brake_over_speed_enabled) else 0.
over_speed_brake = apply_brake * over_speed_factor
apply_brake = max([
apply_brake * lead_long_brake_lockout_factor,
over_speed_brake
])
if apply_gas < P.ZERO_GAS and lead_long_gas_lockout_factor < 1.:
coasting_over_speed_vEgo_BP = [
interp(CS.vEgo, CS.coasting_over_speed_vEgo_BP_BP,
CS.coasting_over_speed_regen_vEgo_BP[0]),
interp(CS.vEgo, CS.coasting_over_speed_vEgo_BP_BP,
CS.coasting_over_speed_regen_vEgo_BP[1])
]
over_speed_factor = interp(
CS.vEgo / check_speed_ms,
coasting_over_speed_vEgo_BP, [0., 1.]) if (
check_speed_ms > 0 and
CS.coasting_brake_over_speed_enabled) else 0.
coast_apply_gas = int(
round(
float(P.ZERO_GAS) - over_speed_factor *
(P.ZERO_GAS - apply_gas)))
apply_gas = apply_gas * lead_long_gas_lockout_factor + coast_apply_gas * (
1. - lead_long_gas_lockout_factor)
elif CS.no_friction_braking and lead_long_brake_lockout_factor < 1.:
if CS.coasting_long_plan in ['cruise', 'limit'
] and apply_brake > 0.:
apply_brake *= lead_long_brake_lockout_factor
apply_gas = int(round(apply_gas))
apply_brake = int(round(apply_brake))
CS.one_pedal_mode_active_last = CS.one_pedal_mode_active
if do_log:
f.write(",".join([str(i)
for i in [apply_gas, apply_brake]]) + "\n")
f.close()
if CS.showBrakeIndicator:
CS.apply_brake_percent = int(
interp(apply_brake, [
float(P.BRAKE_LOOKUP_V[-1]),
float(P.BRAKE_LOOKUP_V[0])
], [0., 100.])) if CS.vEgo > 0.1 else 0
# 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:
if CS.pause_long_on_gas_press:
at_full_stop = False
near_stop = False
car_stopping = False
standstill = False
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))
# Send dashboard UI commands (ACC status), 25hz
if (frame % 4) == 0:
send_fcw = hud_alert == VisualAlert.fcw
follow_level = CS.get_follow_level()
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,
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 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
return can_sends
def update(self, enabled, CS, frame, actuators, \
hud_v_cruise, hud_show_lanes, hud_show_car, chime, chime_cnt, 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 = 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, steer))
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
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 = []
alert_out = process_hud_alert(hud_alert)
steer = alert_out
### STEER ###
if (frame % P.STEER_STEP) == 0:
lkas_enabled = enabled and not CS.steer_warning and CS.lkMode 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
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.out.vEgo, 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.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))
# 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))
# Send dashboard UI commands (ACC status), 25hz
follow_level = CS.get_follow_level()
if (frame % 4) == 0:
send_fcw = 0
if self.fcw_count > 0:
self.fcw_count -= 1
send_fcw = 0x3
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, 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:
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())
