def __init__(self, CP):
super().__init__(CP)
self.cruise_main_button = False
self.cruise_buttons = False
self.lkas_button_on = False
self.lkas_error = False
self.prev_cruise_main_button = False
self.prev_cruise_buttons = False
self.main_on = False
self.acc_active = False
self.cruiseState_modeSel = 0
self.Mdps_ToiUnavail = 0
self.left_blinker_flash = 0
self.right_blinker_flash = 0
self.steerWarning = 0
self.TSigLHSw = 0
self.TSigRHSw = 0
self.driverAcc_time = 0
self.gearShifter = 0
self.leftBlindspot_time = 0
self.rightBlindspot_time = 0
self.SC = SpdController()
def __init__(self, CP):
super().__init__(CP)
#Auto detection for setup
#self.no_radar = CP.sccBus == -1
self.mdps_bus = CP.mdpsBus
self.sas_bus = CP.sasBus
self.scc_bus = CP.sccBus
self.cruise_main_button = False
self.cruise_buttons = False
self.lkas_button_on = False
self.lkas_error = False
self.prev_cruise_main_button = False
self.prev_cruise_buttons = False
self.main_on = False
self.acc_active = False
self.cruiseState_modeSel = 0
self.driverAcc_time = 0
# BSM
self.leftBlindspot_time = 0
self.rightBlindspot_time = 0
# blinker
self.left_blinker_flash = 0
self.right_blinker_flash = 0
self.TSigLHSw = 0
self.TSigRHSw = 0
self.SC = SpdController()
def __init__(self, CP):
super().__init__(CP)
#Auto detection for setup
self.no_radar = CP.sccBus == -1
self.lkas_button_on = True
self.cruise_main_button = 0
self.mdps_error_cnt = 0
self.cruise_unavail_cnt = 0
self.acc_active = False
self.cruiseState_standstill = False
self.cruiseState_modeSel = 0
self.SC = SpdController()
self.driverAcc_time = 0
# blinker
self.left_blinker_flash = 0
self.right_blinker_flash = 0
self.TSigLHSw = 0
self.TSigRHSw = 0
self.apply_steer = 0.
self.steer_anglecorrection = float(
int(Params().get("OpkrSteerAngleCorrection", encoding="utf8")) *
0.1)
self.gear_correction = Params().get_bool("JustDoGearD")
self.steer_wind_down = Params().get_bool("SteerWindDown")
self.brake_check = False
self.cancel_check = False
def __init__(self, CP):
super().__init__(CP)
self.read_distance_lines = 0
#Auto detection for setup
self.cruise_main_button = 0
self.cruise_buttons = 0
self.allow_nonscc_available = False
self.lkasstate = 0
self.lead_distance = 150.
self.radar_obj_valid = 0.
self.vrelative = 0.
self.prev_cruise_buttons = 0
self.prev_gap_button = 0
self.cancel_button_count = 0
self.cancel_button_timer = 0
self.leftblinkerflashdebounce = 0
self.rightblinkerflashdebounce = 0
self.SC = SpdController()
self.driverAcc_time = 0
self.brake_check = 0
self.mainsw_check = 0
self.steer_anglecorrection = int(
Params().get('OpkrSteerAngleCorrection')) * 0.1
self.cruise_gap = int(Params().get('OpkrCruiseGapSet'))
self.pm = messaging.PubMaster(
['liveTrafficData', 'dynamicFollowButton'])
self.sm = messaging.SubMaster(['liveMapData'])
def __init__(self, CP):
super().__init__(CP)
#Auto detection for setup
self.no_radar = CP.sccBus == -1
self.mdps_bus = CP.mdpsBus
self.sas_bus = CP.sasBus
self.scc_bus = CP.sccBus
self.lkas_button_on = True
self.has_scc13 = CP.carFingerprint in FEATURES["has_scc13"]
self.has_scc14 = CP.carFingerprint in FEATURES["has_scc14"]
self.cruise_main_button = 0
self.mdps_error_cnt = 0
self.spas_enabled = CP.spasEnabled
self.acc_active = False
self.cruiseState_standstill = False
self.cruiseState_modeSel = 0
self.SC = SpdController()
self.driverAcc_time = 0
# blinker
self.left_blinker_flash = 0
self.right_blinker_flash = 0
self.TSigLHSw = 0
self.TSigRHSw = 0
self.apply_steer = 0.
self.steer_anglecorrection = float(int(Params().get("OpkrSteerAngleCorrection", encoding='utf8')) * 0.1)
self.gear_correction = Params().get("JustDoGearD", encoding='utf8') == "1"
def __init__(self, dbc_name, car_fingerprint):
self.packer = CANPacker(dbc_name)
self.car_fingerprint = car_fingerprint
self.accel_steady = 0
self.apply_steer_last = 0
self.steer_rate_limited = False
self.lkas11_cnt = 0
self.resume_cnt = 0
self.last_resume_frame = 0
self.last_lead_distance = 0
self.turning_signal_timer = 0
self.lkas_button = 1
self.longcontrol = 0 #TODO: make auto
self.low_speed_car = False
self.streer_angle_over = False
self.turning_indicator = 0
self.hud_timer_left = 0
self.hud_timer_right = 0
self.lkas_active_timer1 = 0
self.lkas_active_timer2 = 0
self.steer_timer = 0
self.steer_torque_over_timer = 0
self.steer_torque_over = False
kegman = kegman_conf()
self.steer_torque_over_max = float(kegman.conf['steerTorqueOver'])
self.timer_curvature = 0
self.SC = SpdController()
self.sc_wait_timer2 = 0
self.sc_active_timer2 = 0
self.sc_btn_type = Buttons.NONE
self.sc_clu_speed = 0
#self.model_speed = 255
self.traceCC = trace1.Loger("CarCtrl")
self.params = Params()
self.lane_change_enabled = self.params.get('LaneChangeEnabled') == b'1'
self.speed_control_enabled = self.params.get(
'SpeedControlEnabled') == b'1'
def __init__(self, CP):
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
if self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
self.shifter_values = can_define.dv["CLU15"]["CF_Clu_Gear"]
elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
self.shifter_values = can_define.dv["TCU12"]["CUR_GR"]
else: # preferred and elect gear methods use same definition
self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"]
#Auto detection for setup
self.no_radar = CP.sccBus == -1
self.lkas_button_on = True
self.cruise_main_button = 0
self.mdps_error_cnt = 0
self.cruise_unavail_cnt = 0
self.acc_active = False
self.cruiseState_standstill = False
self.cruiseState_modeSel = 0
self.SC = SpdController()
self.driverAcc_time = 0
self.apply_steer = 0.
self.steer_anglecorrection = float(
int(Params().get("OpkrSteerAngleCorrection", encoding="utf8")) *
0.1)
self.gear_correction = Params().get_bool("JustDoGearD")
self.steer_wind_down = Params().get_bool("SteerWindDown")
self.brake_check = False
self.cancel_check = False
self.safety_sign_check = 0
self.safety_sign = 0
self.safety_sign_last = 0
self.safety_dist = 0
self.safety_block_remain_dist = 0
self.is_highway = False
self.on_speed_control = False
def __init__(self, CP):
super().__init__(CP)
self.no_radar = CP.sccBus == -1
self.mdps_bus = CP.mdpsBus
self.sas_bus = CP.sasBus
self.scc_bus = CP.sccBus
self.mdps_error_cnt = 0
self.lkas_button_on = True
self.cruise_main_button = False
self.cruise_buttons = False
self.lkas_button_on = False
self.lkas_error = False
self.prev_cruise_main_button = False
self.prev_cruise_buttons = False
self.main_on = False
self.acc_active = False
self.cruiseState_modeSel = 0
self.Mdps_ToiUnavail = 0
self.left_blinker_flash = 0
self.right_blinker_flash = 0
self.steerWarning = 0
self.TSigLHSw = 0
self.TSigRHSw = 0
self.driverAcc_time = 0
self.leftBlindspot_time = 0
self.rightBlindspot_time = 0
self.SC = SpdController()
def __init__(self, CP):
super().__init__(CP)
self.engaged_when_gas_was_pressed = False
self.gas_pressed = False
self.smartspeed = 0
#Auto detection for setup
self.no_radar = CP.sccBus == -1
self.mdps_bus = CP.mdpsBus
self.sas_bus = CP.sasBus
self.scc_bus = CP.sccBus
self.lkas_button_on = True
self.has_scc13 = CP.carFingerprint in FEATURES["has_scc13"]
self.has_scc14 = CP.carFingerprint in FEATURES["has_scc14"]
self.cruise_main_button = 0
self.mdps_error_cnt = 0
self.spas_enabled = CP.spasEnabled
self.acc_active = False
self.cruiseState_standstill = False
self.cruiseState_modeSel = 0
self.SC = SpdController()
self.driverAcc_time = 0
# blinker
self.left_blinker_flash = 0
self.right_blinker_flash = 0
self.TSigLHSw = 0
self.TSigRHSw = 0
self.apply_steer = 0.
self.steer_anglecorrection = int(Params().get('OpkrSteerAngleCorrection')) * 0.1
self.sm = messaging.SubMaster(['liveMapData'])
class CarController():
def __init__(self, dbc_name, car_fingerprint):
self.packer = CANPacker(dbc_name)
self.car_fingerprint = car_fingerprint
self.accel_steady = 0
self.apply_steer_last = 0
self.steer_rate_limited = False
self.lkas11_cnt = 0
self.resume_cnt = 0
self.last_resume_frame = 0
self.last_lead_distance = 0
self.turning_signal_timer = 0
self.lkas_button = 1
self.longcontrol = 0 #TODO: make auto
self.low_speed_car = False
self.streer_angle_over = False
self.turning_indicator = 0
self.hud_timer_left = 0
self.hud_timer_right = 0
self.lkas_active_timer1 = 0
self.lkas_active_timer2 = 0
self.steer_timer = 0
self.steer_torque_over_timer = 0
self.steer_torque_over = False
kegman = kegman_conf()
self.steer_torque_over_max = float(kegman.conf['steerTorqueOver'])
self.timer_curvature = 0
self.SC = SpdController()
self.sc_wait_timer2 = 0
self.sc_active_timer2 = 0
self.sc_btn_type = Buttons.NONE
self.sc_clu_speed = 0
#self.model_speed = 255
self.traceCC = trace1.Loger("CarCtrl")
self.params = Params()
self.lane_change_enabled = self.params.get('LaneChangeEnabled') == b'1'
self.speed_control_enabled = self.params.get(
'SpeedControlEnabled') == b'1'
def limit_ctrl(self, value, limit, offset):
p_limit = offset + limit
m_limit = offset - limit
if value > p_limit:
value = p_limit
elif value < m_limit:
value = m_limit
return value
def limit_dir(self, value, offset, p_l, m_l):
p_limit = offset + p_l
m_limit = offset - m_l
if value > p_limit:
value = p_limit
elif value < m_limit:
value = m_limit
return value
def accel_hysteresis(self, accel, accel_steady):
# for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command
if accel > accel_steady + ACCEL_HYST_GAP:
accel_steady = accel - ACCEL_HYST_GAP
elif accel < accel_steady - ACCEL_HYST_GAP:
accel_steady = accel + ACCEL_HYST_GAP
accel = accel_steady
return accel, accel_steady
def process_hud_alert(self, enabled, button_on, visual_alert, left_line,
right_line, CS):
hud_alert = 0
if visual_alert == VisualAlert.steerRequired:
hud_alert = 3
# initialize to no line visible
lane_visible = 1 # Lkas_LdwsSysState LDWS
if not button_on:
lane_visible = 0
elif left_line and right_line: #or hud_alert: #HUD alert only display when LKAS status is active
if enabled:
lane_visible = 3 # handle icon, lane icon
else:
lane_visible = 4 # lane icon
elif left_line:
lane_visible = 5 # left lan icon
elif right_line:
lane_visible = 6 # right lan icon
if enabled and CS.Navi_HDA >= 1: # highway Area
if CS.v_ego > 40 * CV.KPH_TO_MS:
lane_visible = 4
# 7 : hud can't display, panel : LKA, handle icon.
return hud_alert, lane_visible
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
visual_alert, left_line, right_line, sm, LaC):
path_plan = sm['pathPlan']
# *** compute control surfaces ***
v_ego_kph = CS.v_ego * CV.MS_TO_KPH
# gas and brake
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = self.accel_hysteresis(
apply_accel, self.accel_steady)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
abs_angle_steers = abs(
actuators.steerAngle) # abs(CS.angle_steers) #
param = SteerLimitParams()
if path_plan.laneChangeState != LaneChangeState.off:
#param.STEER_MAX *= 0.99
param.STEER_DELTA_UP = 1
param.STEER_DELTA_DOWN = 5
#v_curvature
elif LaC.v_curvature < 200:
self.timer_curvature = 300
elif abs_angle_steers < 1 and self.timer_curvature <= 0:
xp = [0.5, 1]
fp = [240, 255]
param.STEER_MAX = interp(abs_angle_steers, xp, fp)
if abs_angle_steers < 1 or v_ego_kph < 5:
param.STEER_DELTA_UP = 2
param.STEER_DELTA_DOWN = 4
if self.timer_curvature:
self.timer_curvature -= 1
### Steering Torque
new_steer = actuators.steer * param.STEER_MAX
apply_steer = apply_std_steer_torque_limits(new_steer,
self.apply_steer_last,
CS.steer_torque_driver,
param)
self.steer_rate_limited = new_steer != apply_steer
if abs(CS.steer_torque_driver) > self.steer_torque_over_max: #200:
self.steer_torque_over_timer += 1
if self.steer_torque_over_timer > 5:
self.steer_torque_over = True
self.steer_torque_over_timer = 100
elif self.steer_torque_over_timer:
self.steer_torque_over_timer -= 1
else:
self.steer_torque_over = False
### LKAS button to temporarily disable steering
#if not CS.lkas_error:
# if self.lkas_button != CS.lkas_button_on:
# self.lkas_button = CS.lkas_button_on
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
if self.car_fingerprint == CAR.GENESIS:
lkas_active = enabled and abs(CS.angle_steers) < 90.
else:
lkas_active = enabled and abs(CS.angle_steers) < 100.
# fix for Genesis hard fault at low speed
if v_ego_kph < 60 and self.car_fingerprint == CAR.GENESIS and not CS.mdps_bus:
lkas_active = 0
low_speed = self.low_speed_car
#if not self.lkas_button:
# low_speed = False
#elif not CS.cruiseState.enabled:
# low_speed = False
if CS.stopped:
low_speed = False
elif CS.v_ego > (CS.CP.minSteerSpeed + 0.7):
low_speed = False
elif CS.v_ego < (CS.CP.minSteerSpeed + 0.2):
low_speed = True
if self.low_speed_car != low_speed:
self.low_speed_car = low_speed
# streer over check
if enabled and abs(CS.angle_steers) > 100. or CS.steer_error:
self.streer_angle_over = True
self.steer_timer = 250
elif abs(CS.angle_steers) < 7.5 or not self.steer_timer:
self.streer_angle_over = False
elif self.steer_timer:
self.steer_timer -= 1
# Disable steering while turning blinker on and speed below 60 kph
if CS.left_blinker_on or CS.right_blinker_on:
self.steer_torque_over = False
self.turning_signal_timer = 100 # Disable for 1.0 Seconds after blinker turned off
elif CS.left_blinker_flash or CS.right_blinker_flash:
self.steer_torque_over = False
self.turning_signal_timer = 100
# turning indicator alert logic
if self.lane_change_enabled:
self.turning_indicator = self.turning_signal_timer and CS.v_ego < LaneChangeParms.LANE_CHANGE_SPEED_MIN
else:
self.turning_indicator = self.turning_signal_timer
if self.turning_signal_timer:
self.turning_signal_timer -= 1
if left_line:
self.hud_timer_left = 100
elif self.hud_timer_left:
self.hud_timer_left -= 1
if right_line:
self.hud_timer_right = 100
elif self.hud_timer_right:
self.hud_timer_right -= 1
if path_plan.laneChangeState != LaneChangeState.off:
if LaC.v_curvature > 200:
self.lkas_active_timer1 = 300
apply_steer = self.limit_ctrl(apply_steer, 100, 0)
elif not self.hud_timer_left and not self.hud_timer_right:
self.lkas_active_timer1 = 200 # apply_steer = 70
elif path_plan.laneChangeState != LaneChangeState.off:
self.steer_torque_over = False
# disable lkas
if not CS.main_on:
lkas_active = 0
self.turning_signal_timer = 0
self.turning_indicator = False
self.steer_torque_over = False
elif CS.stopped:
lkas_active = 0
elif self.steer_torque_over:
lkas_active = 0
if self.streer_angle_over:
lkas_active = 0
elif self.turning_indicator:
lkas_active = 0
if not lkas_active:
apply_steer = 0
steer_req = 1 if apply_steer else 0
steer_limit = param.STEER_MAX #param.STEER_MAX
if not lkas_active:
self.lkas_active_timer1 = 200
elif self.lkas_active_timer1 < 400:
self.lkas_active_timer1 += 1
ratio_steer = self.lkas_active_timer1 / 400
if ratio_steer < 1:
steer_limit = ratio_steer * param.STEER_MAX
apply_steer = self.limit_ctrl(apply_steer, steer_limit, 0)
dRel, yRel, vRel = self.SC.get_lead(sm, CS)
vRel = int(vRel * 3.6 + 0.5)
lead_objspd = CS.lead_objspd
str_log1 = 'CV={:03.0f}/{:06.3f} TQ=V:{:04.0f}/S:{:04.0f}'.format(
LaC.v_curvature, LaC.model_sum, apply_steer,
CS.steer_torque_driver)
str_log2 = 'D={:05.1f} V={:03.0f} S_LIM={:03.0f} S_MAX={:03.0f}'.format(
dRel, vRel, steer_limit, param.STEER_MAX)
trace1.printf('{} {}'.format(str_log1, str_log2))
self.apply_accel_last = apply_accel
self.apply_steer_last = apply_steer
hud_alert, lane_visible = self.process_hud_alert(
lkas_active, self.lkas_button, visual_alert, self.hud_timer_left,
self.hud_timer_right, CS)
clu11_speed = CS.clu11["CF_Clu_Vanz"]
enabled_speed = 38 if CS.is_set_speed_in_mph else 60
if clu11_speed > enabled_speed or not lkas_active:
enabled_speed = clu11_speed
can_sends = []
if frame == 0: # initialize counts from last received count signals
self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"] + 1
self.lkas11_cnt %= 0x10
self.clu11_cnt = frame % 0x10
self.mdps12_cnt = frame % 0x100
can_sends.append(
create_lkas11(self.packer,
self.car_fingerprint,
0,
apply_steer,
steer_req,
self.lkas11_cnt,
lkas_active,
CS.lkas11,
hud_alert,
lane_visible,
keep_stock=True))
if CS.mdps_bus or CS.scc_bus == 1: # send lkas12 bus 1 if mdps or scc is on bus 1
can_sends.append(
create_lkas11(self.packer,
self.car_fingerprint,
1,
apply_steer,
steer_req,
self.lkas11_cnt,
lkas_active,
CS.lkas11,
hud_alert,
lane_visible,
keep_stock=True))
if CS.mdps_bus: # send clu11 to mdps if it is not on bus 0
can_sends.append(
create_clu11(self.packer, CS.mdps_bus, CS.clu11, Buttons.NONE,
enabled_speed, self.clu11_cnt))
if pcm_cancel_cmd and self.longcontrol:
can_sends.append(
create_clu11(self.packer, CS.scc_bus, CS.clu11, Buttons.CANCEL,
clu11_speed, self.clu11_cnt))
else: # send mdps12 to LKAS to prevent LKAS error if no cancel cmd
can_sends.append(
create_mdps12(self.packer, self.car_fingerprint,
self.mdps12_cnt, CS.mdps12))
# AVM
#if CS.mdps_bus:
#if not CS.cp_AVM.can_valid:
#can_sends.append(create_AVM(self.packer, self.car_fingerprint, CS.avm, CS ))
if CS.stopped:
#self.model_speed = 300
# run only first time when the car stopped
if self.last_lead_distance == 0:
# get the lead distance from the Radar
self.last_lead_distance = CS.lead_distance
self.resume_cnt = 0
# when lead car starts moving, create 6 RES msgs
elif CS.lead_distance != self.last_lead_distance and (
frame - self.last_resume_frame) > 5:
can_sends.append(
create_clu11(self.packer, CS.scc_bus, CS.clu11,
Buttons.RES_ACCEL, clu11_speed,
self.resume_cnt))
self.resume_cnt += 1
# interval after 6 msgs
if self.resume_cnt > 5:
self.last_resume_frame = frame
self.resume_cnt = 0
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
elif CS.driverOverride == 2 or not CS.pcm_acc_status or CS.clu_CruiseSwState == 1 or CS.clu_CruiseSwState == 2:
#self.model_speed = 300
self.resume_cnt = 0
self.sc_btn_type = Buttons.NONE
self.sc_wait_timer2 = 10
self.sc_active_timer2 = 0
elif self.sc_wait_timer2:
self.sc_wait_timer2 -= 1
elif self.speed_control_enabled:
#acc_mode, clu_speed = self.long_speed_cntrl( v_ego_kph, CS, actuators )
btn_type, clu_speed = self.SC.update(
v_ego_kph, CS, sm, actuators, dRel, yRel, vRel,
LaC.v_curvature) # speed controller spdcontroller.py
if CS.clu_Vanz < 5:
self.sc_btn_type = Buttons.NONE
elif self.sc_btn_type != Buttons.NONE:
pass
elif btn_type != Buttons.NONE:
self.resume_cnt = 0
self.sc_active_timer2 = 0
self.sc_btn_type = btn_type
self.sc_clu_speed = clu_speed
if self.sc_btn_type != Buttons.NONE:
self.sc_active_timer2 += 1
if self.sc_active_timer2 > 10:
self.sc_wait_timer2 = 5
self.resume_cnt = 0
self.sc_active_timer2 = 0
self.sc_btn_type = Buttons.NONE
else:
#self.traceCC.add( 'sc_btn_type={} clu_speed={} set={:.0f} vanz={:.0f}'.format( self.sc_btn_type, self.sc_clu_speed, CS.VSetDis, clu11_speed ) )
can_sends.append(
create_clu11(self.packer, CS.scc_bus, CS.clu11,
self.sc_btn_type, self.sc_clu_speed,
self.resume_cnt))
self.resume_cnt += 1
self.lkas11_cnt += 1
return can_sends
def update(self, enabled, CS, frame, CC, actuators, pcm_cancel_cmd, visual_alert,
left_lane, right_lane, left_lane_depart, right_lane_depart, set_speed, lead_visible, sm):
# *** compute control surfaces ***
# gas and brake
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
self.dRel, self.yRel, self.vRel = SpdController.get_lead(sm)
self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo)
# Steering Torque
steerAngleAbs = abs(actuators.steerAngle)
limitParams = copy.copy(SteerLimitParams)
limitParams.STEER_MAX = int(interp(steerAngleAbs, [5., 10., 20., 30., 70.], [255, SteerLimitParams.STEER_MAX, 500, 600, 800]))
limitParams.STEER_DELTA_UP = int(interp(steerAngleAbs, [5., 20., 50.], [2, 3, 3]))
limitParams.STEER_DELTA_DOWN = int(interp(steerAngleAbs, [5., 20., 50.], [4, 5, 5]))
if self.driver_steering_torque_above_timer:
new_steer = actuators.steer * limitParams.STEER_MAX * (self.driver_steering_torque_above_timer / 100)
else:
new_steer = actuators.steer * limitParams.STEER_MAX
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, limitParams)
self.steer_rate_limited = new_steer != apply_steer
CC.applyAccel = apply_accel
CC.applySteer = apply_steer
# SPAS limit angle extremes for safety
if CS.spas_enabled:
apply_steer_ang_req = clip(actuators.steerAngle, -1*(STEER_ANG_MAX), STEER_ANG_MAX)
# SPAS limit angle rate for safety
if abs(self.apply_steer_ang - apply_steer_ang_req) > STEER_ANG_MAX_RATE:
if apply_steer_ang_req > self.apply_steer_ang:
self.apply_steer_ang += STEER_ANG_MAX_RATE
else:
self.apply_steer_ang -= STEER_ANG_MAX_RATE
else:
self.apply_steer_ang = apply_steer_ang_req
spas_active = CS.spas_enabled and enabled and (self.spas_always or CS.out.vEgo < 7.0) # 25km/h
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
# temporarily disable steering when LKAS button off
#lkas_active = enabled and abs(CS.out.steeringAngle) < 90. and not spas_active
lkas_active = enabled and not spas_active #and abs(CS.out.steeringAngle) < 90.
if (( CS.out.leftBlinker and not CS.out.rightBlinker) or ( CS.out.rightBlinker and not CS.out.leftBlinker)) and CS.out.vEgo < LANE_CHANGE_SPEED_MIN:
self.lanechange_manual_timer = 10
if CS.out.leftBlinker and CS.out.rightBlinker:
self.emergency_manual_timer = 10
if abs(CS.out.steeringTorque) > 200:
self.driver_steering_torque_above_timer = 100
if self.lanechange_manual_timer:
lkas_active = 0
if self.lanechange_manual_timer > 0:
self.lanechange_manual_timer -= 1
if self.emergency_manual_timer > 0:
self.emergency_manual_timer -= 1
if self.driver_steering_torque_above_timer > 0:
self.driver_steering_torque_above_timer -= 1
if not lkas_active:
apply_steer = 0
self.apply_accel_last = apply_accel
self.apply_steer_last = apply_steer
sys_warning, sys_state, left_lane_warning, right_lane_warning =\
process_hud_alert(lkas_active, self.car_fingerprint, visual_alert,
left_lane, right_lane, left_lane_depart, right_lane_depart,
self.lkas_button_on)
clu11_speed = CS.clu11["CF_Clu_Vanz"]
enabled_speed = 38 if CS.is_set_speed_in_mph else 55
if clu11_speed > enabled_speed or not lkas_active:
enabled_speed = clu11_speed
if not(min_set_speed < set_speed < 255 * CV.KPH_TO_MS):
set_speed = min_set_speed
set_speed *= CV.MS_TO_MPH if CS.is_set_speed_in_mph else CV.MS_TO_KPH
if frame == 0: # initialize counts from last received count signals
self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"]
self.scc12_cnt = CS.scc12["CR_VSM_Alive"] + 1 if not CS.no_radar else 0
#TODO: fix this
# self.prev_scc_cnt = CS.scc11["AliveCounterACC"]
# self.scc_update_frame = frame
# check if SCC is alive
# if frame % 7 == 0:
# if CS.scc11["AliveCounterACC"] == self.prev_scc_cnt:
# if frame - self.scc_update_frame > 20 and self.scc_live:
# self.scc_live = False
# else:
# self.scc_live = True
# self.prev_scc_cnt = CS.scc11["AliveCounterACC"]
# self.scc_update_frame = frame
self.prev_scc_cnt = CS.scc11["AliveCounterACC"]
self.lkas11_cnt = (self.lkas11_cnt + 1) % 0x10
self.scc12_cnt %= 0xF
can_sends = []
can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active,
CS.lkas11, sys_warning, sys_state, enabled, left_lane, right_lane,
left_lane_warning, right_lane_warning, 0))
if CS.mdps_bus or CS.scc_bus == 1: # send lkas11 bus 1 if mdps or scc is on bus 1
can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active,
CS.lkas11, sys_warning, sys_state, enabled, left_lane, right_lane,
left_lane_warning, right_lane_warning, 1))
if frame % 2 and CS.mdps_bus: # send clu11 to mdps if it is not on bus 0
can_sends.append(create_clu11(self.packer, frame, CS.mdps_bus, CS.clu11, Buttons.NONE, enabled_speed))
str_log1 = '곡률={:03.0f} 토크={:03.0f} 프레임률={:03.0f} ST={:03.0f}/{:01.0f}/{:01.0f}'.format(abs(self.model_speed), abs(new_steer), self.timer1.sampleTime(), SteerLimitParams.STEER_MAX, SteerLimitParams.STEER_DELTA_UP, SteerLimitParams.STEER_DELTA_DOWN)
trace1.printf1('{} {}'.format(str_log1, self.str_log2))
if CS.out.cruiseState.modeSel == 0 and self.mode_change_switch == 3:
self.mode_change_timer = 50
self.mode_change_switch = 0
elif CS.out.cruiseState.modeSel == 1 and self.mode_change_switch == 0:
self.mode_change_timer = 50
self.mode_change_switch = 1
elif CS.out.cruiseState.modeSel == 2 and self.mode_change_switch == 1:
self.mode_change_timer = 50
self.mode_change_switch = 2
elif CS.out.cruiseState.modeSel == 3 and self.mode_change_switch == 2:
self.mode_change_timer = 50
self.mode_change_switch = 3
if self.mode_change_timer > 0:
self.mode_change_timer -= 1
run_speed_ctrl = self.opkr_variablecruise and CS.acc_active and (CS.out.cruiseState.modeSel == 1 or CS.out.cruiseState.modeSel == 2 or CS.out.cruiseState.modeSel == 3)
if not run_speed_ctrl:
if CS.out.cruiseState.modeSel == 0:
self.steer_mode = "오파모드"
elif CS.out.cruiseState.modeSel == 1:
self.steer_mode = "차간+커브"
elif CS.out.cruiseState.modeSel == 2:
self.steer_mode = "차간ONLY"
elif CS.out.cruiseState.modeSel == 3:
self.steer_mode = "편도1차선"
if CS.out.steerWarning == 0:
self.mdps_status = "정상"
elif CS.out.steerWarning == 1:
self.mdps_status = "오류"
if CS.lkas_button_on == 0:
self.lkas_switch = "OFF"
elif CS.lkas_button_on == 1:
self.lkas_switch = "ON"
else:
self.lkas_switch = "-"
if self.cruise_gap != CS.cruiseGapSet:
self.cruise_gap = CS.cruiseGapSet
str_log3 = '주행모드={:s} MDPS상태={:s} LKAS버튼={:s} 크루즈갭={:1.0f}'.format( self.steer_mode, self.mdps_status, self.lkas_switch, self.cruise_gap)
trace1.printf2( '{}'.format( str_log3 ) )
if pcm_cancel_cmd and self.longcontrol:
can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed))
if CS.out.cruiseState.standstill:
if self.last_lead_distance == 0 or not self.opkr_autoresume:
self.last_lead_distance = CS.lead_distance
self.resume_cnt = 0
self.resume_wait_timer = 0
elif self.resume_wait_timer > 0:
self.resume_wait_timer -= 1
elif CS.lead_distance != self.last_lead_distance:
can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed))
self.resume_cnt += 1
if self.resume_cnt > 5:
self.resume_cnt = 0
self.resume_wait_timer = int(0.25 / DT_CTRL)
elif self.cruise_gap_prev == 0 and run_speed_ctrl:
self.cruise_gap_prev = CS.cruiseGapSet
self.cruise_gap_set_init = 1
elif CS.cruiseGapSet != 1.0 and run_speed_ctrl:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 100:
can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
elif run_speed_ctrl:
is_sc_run = self.SC.update(CS, sm, self)
if is_sc_run:
can_sends.append(create_clu11(self.packer, self.resume_cnt, CS.scc_bus, CS.clu11, self.SC.btn_type, self.SC.sc_clu_speed))
self.resume_cnt += 1
else:
self.resume_cnt = 0
if self.dRel > 17 and self.cruise_gap_prev != CS.cruiseGapSet and self.cruise_gap_set_init == 1:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 50:
can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
elif self.cruise_gap_prev == CS.cruiseGapSet:
self.cruise_gap_set_init = 0
self.cruise_gap_prev = 0
if CS.mdps_bus: # send mdps12 to LKAS to prevent LKAS error
can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
# send scc to car if longcontrol enabled and SCC not on bus 0 or ont live
if self.longcontrol and (CS.scc_bus or not self.scc_live) and frame % 2 == 0:
can_sends.append(create_scc12(self.packer, apply_accel, enabled, self.scc12_cnt, self.scc_live, CS.scc12))
can_sends.append(create_scc11(self.packer, frame, enabled, set_speed, lead_visible, self.scc_live, CS.scc11))
if CS.has_scc13 and frame % 20 == 0:
can_sends.append(create_scc13(self.packer, CS.scc13))
if CS.has_scc14:
can_sends.append(create_scc14(self.packer, enabled, CS.scc14))
self.scc12_cnt += 1
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in FEATURES["send_lfa_mfa"]:
can_sends.append(create_lfa_mfa(self.packer, frame, lkas_active))
if CS.spas_enabled:
if CS.mdps_bus:
can_sends.append(create_ems11(self.packer, CS.ems11, spas_active))
# SPAS11 50hz
if (frame % 2) == 0:
if CS.mdps11_stat == 7 and not self.mdps11_stat_last == 7:
self.en_spas = 7
self.en_cnt = 0
if self.en_spas == 7 and self.en_cnt >= 8:
self.en_spas = 3
self.en_cnt = 0
if self.en_cnt < 8 and spas_active:
self.en_spas = 4
elif self.en_cnt >= 8 and spas_active:
self.en_spas = 5
if not spas_active:
self.apply_steer_ang = CS.mdps11_strang
self.en_spas = 3
self.en_cnt = 0
self.mdps11_stat_last = CS.mdps11_stat
self.en_cnt += 1
can_sends.append(create_spas11(self.packer, self.car_fingerprint, (frame // 2), self.en_spas, self.apply_steer_ang, CS.mdps_bus))
# SPAS12 20Hz
if (frame % 5) == 0:
can_sends.append(create_spas12(CS.mdps_bus))
return can_sends
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
#Auto detection for setup
self.no_radar = CP.sccBus == -1
self.mdps_bus = CP.mdpsBus
self.sas_bus = CP.sasBus
self.scc_bus = CP.sccBus
self.lkas_button_on = True
self.has_scc13 = CP.carFingerprint in FEATURES["has_scc13"]
self.has_scc14 = CP.carFingerprint in FEATURES["has_scc14"]
self.cruise_main_button = 0
self.mdps_error_cnt = 0
self.spas_enabled = CP.spasEnabled
self.acc_active = False
self.cruiseState_standstill = False
self.cruiseState_modeSel = 0
self.SC = SpdController()
self.driverAcc_time = 0
# blinker
self.left_blinker_flash = 0
self.right_blinker_flash = 0
self.TSigLHSw = 0
self.TSigRHSw = 0
self.apply_steer = 0.
self.steer_anglecorrection = float(int(Params().get("OpkrSteerAngleCorrection", encoding='utf8')) * 0.1)
self.gear_correction = Params().get("JustDoGearD", encoding='utf8') == "1"
def update(self, cp, cp2, cp_cam):
cp_mdps = cp2 if self.mdps_bus else cp
cp_sas = cp2 if self.sas_bus else cp
cp_scc = cp2 if self.scc_bus == 1 else cp_cam if self.scc_bus == 2 else cp
self.prev_cruise_buttons = self.cruise_buttons
self.prev_cruise_main_button = self.cruise_main_button
self.prev_lkas_button_on = self.lkas_button_on
ret = car.CarState.new_message()
ret.doorOpen = any([cp.vl["CGW1"]['CF_Gway_DrvDrSw'], cp.vl["CGW1"]['CF_Gway_AstDrSw'],
cp.vl["CGW2"]['CF_Gway_RLDrSw'], cp.vl["CGW2"]['CF_Gway_RRDrSw']])
ret.seatbeltUnlatched = cp.vl["CGW1"]['CF_Gway_DrvSeatBeltSw'] == 0
ret.wheelSpeeds.fl = cp.vl["WHL_SPD11"]['WHL_SPD_FL'] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.vEgo = cp.vl["CLU11"]["CF_Clu_Vanz"] * CV.KPH_TO_MS
ret.standstill = ret.vEgoRaw < 0.1
ret.standStill = self.CP.standStill
ret.steeringAngleDeg = cp_sas.vl["SAS11"]['SAS_Angle'] - self.steer_anglecorrection
ret.steeringRateDeg = cp_sas.vl["SAS11"]['SAS_Speed']
ret.yawRate = cp.vl["ESP12"]['YAW_RATE']
ret.steeringTorque = cp_mdps.vl["MDPS12"]['CR_Mdps_StrColTq']
ret.steeringTorqueEps = cp_mdps.vl["MDPS12"]['CR_Mdps_OutTq']
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
self.mdps_error_cnt += 1 if cp_mdps.vl["MDPS12"]['CF_Mdps_ToiUnavail'] != 0 else -self.mdps_error_cnt
ret.steerWarning = self.mdps_error_cnt > 100 #cp_mdps.vl["MDPS12"]['CF_Mdps_ToiUnavail'] != 0
ret.leftBlinker, ret.rightBlinker = self.update_blinker(cp)
self.VSetDis = cp_scc.vl["SCC11"]['VSetDis']
ret.vSetDis = self.VSetDis
self.clu_Vanz = cp.vl["CLU11"]["CF_Clu_Vanz"]
lead_objspd = cp_scc.vl["SCC11"]['ACC_ObjRelSpd']
self.lead_objspd = lead_objspd * CV.MS_TO_KPH
self.Mdps_ToiUnavail = cp_mdps.vl["MDPS12"]['CF_Mdps_ToiUnavail']
self.driverOverride = cp.vl["TCS13"]["DriverOverride"]
if self.driverOverride == 1:
self.driverAcc_time = 100
elif self.driverAcc_time:
self.driverAcc_time -= 1
# cruise state
ret.cruiseState.enabled = (cp_scc.vl["SCC12"]['ACCMode'] != 0) if not self.no_radar else \
cp.vl["LVR12"]['CF_Lvr_CruiseSet'] != 0
ret.cruiseState.available = (cp_scc.vl["SCC11"]["MainMode_ACC"] != 0) if not self.no_radar else \
cp.vl['EMS16']['CRUISE_LAMP_M'] != 0
ret.cruiseState.standstill = cp_scc.vl["SCC11"]['SCCInfoDisplay'] == 4 if not self.no_radar else False
self.cruiseState_standstill = ret.cruiseState.standstill
self.is_set_speed_in_mph = bool(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"])
self.acc_active = ret.cruiseState.enabled
self.cruiseState_modeSel, speed_kph = self.SC.update_cruiseSW(self)
ret.cruiseState.modeSel = self.cruiseState_modeSel
if ret.cruiseState.enabled:
speed_conv = CV.MPH_TO_MS if self.is_set_speed_in_mph else CV.KPH_TO_MS
ret.cruiseState.speed = speed_kph * speed_conv if not self.no_radar else \
cp.vl["LVR12"]["CF_Lvr_CruiseSet"] * speed_conv
else:
ret.cruiseState.speed = 0
self.cruise_main_button = cp.vl["CLU11"]["CF_Clu_CruiseSwMain"]
self.cruise_buttons = cp.vl["CLU11"]["CF_Clu_CruiseSwState"]
ret.cruiseButtons = self.cruise_buttons
# TODO: Find brake pressure
ret.brake = 0
ret.brakePressed = cp.vl["TCS13"]['DriverBraking'] != 0
# TODO: Check this
ret.brakeLights = bool(cp.vl["TCS13"]['BrakeLight'] or ret.brakePressed)
if self.CP.carFingerprint in FEATURES["use_elect_ems"]:
ret.gas = cp.vl["E_EMS11"]['Accel_Pedal_Pos'] / 256.
ret.gasPressed = ret.gas > 5
else:
ret.gas = cp.vl["EMS12"]['PV_AV_CAN'] / 100
ret.gasPressed = bool(cp.vl["EMS16"]["CF_Ems_AclAct"])
# TPMS code added from OPKR
if cp.vl["TPMS11"]['UNIT'] == 0.0:
ret.tpmsPressureFl = cp.vl["TPMS11"]['PRESSURE_FL']
ret.tpmsPressureFr = cp.vl["TPMS11"]['PRESSURE_FR']
ret.tpmsPressureRl = cp.vl["TPMS11"]['PRESSURE_RL']
ret.tpmsPressureRr = cp.vl["TPMS11"]['PRESSURE_RR']
elif cp.vl["TPMS11"]['UNIT'] == 1.0:
ret.tpmsPressureFl = cp.vl["TPMS11"]['PRESSURE_FL'] * 5 * 0.145038
ret.tpmsPressureFr = cp.vl["TPMS11"]['PRESSURE_FR'] * 5 * 0.145038
ret.tpmsPressureRl = cp.vl["TPMS11"]['PRESSURE_RL'] * 5 * 0.145038
ret.tpmsPressureRr = cp.vl["TPMS11"]['PRESSURE_RR'] * 5 * 0.145038
elif cp.vl["TPMS11"]['UNIT'] == 2.0:
ret.tpmsPressureFl = cp.vl["TPMS11"]['PRESSURE_FL'] / 10 * 14.5038
ret.tpmsPressureFr = cp.vl["TPMS11"]['PRESSURE_FR'] / 10 * 14.5038
ret.tpmsPressureRl = cp.vl["TPMS11"]['PRESSURE_RL'] / 10 * 14.5038
ret.tpmsPressureRr = cp.vl["TPMS11"]['PRESSURE_RR'] / 10 * 14.5038
self.cruiseGapSet = cp_scc.vl["SCC11"]['TauGapSet']
# TODO: refactor gear parsing in function
# Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection,
# as this seems to be standard over all cars, but is not the preferred method.
if self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
if cp.vl["CLU15"]["CF_Clu_InhibitD"] == 1:
ret.gearShifter = GearShifter.drive
elif cp.vl["CLU15"]["CF_Clu_InhibitN"] == 1:
ret.gearShifter = GearShifter.neutral
elif cp.vl["CLU15"]["CF_Clu_InhibitP"] == 1:
ret.gearShifter = GearShifter.park
elif cp.vl["CLU15"]["CF_Clu_InhibitR"] == 1:
ret.gearShifter = GearShifter.reverse
else:
if self.gear_correction:
ret.gearShifter = GearShifter.drive
else:
ret.gearShifter = GearShifter.unknown
# Gear Selecton via TCU12
elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
gear = cp.vl["TCU12"]["CUR_GR"]
if gear == 0:
ret.gearShifter = GearShifter.park
elif gear == 14:
ret.gearShifter = GearShifter.reverse
elif gear > 0 and gear < 9: # unaware of anything over 8 currently
ret.gearShifter = GearShifter.drive
else:
if self.gear_correction:
ret.gearShifter = GearShifter.drive
else:
ret.gearShifter = GearShifter.unknown
# Gear Selecton - This is only compatible with optima hybrid 2017
elif self.CP.carFingerprint in FEATURES["use_elect_gears"]:
gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"]
if gear in (5, 8): # 5: D, 8: sport mode
ret.gearShifter = GearShifter.drive
elif gear == 6:
ret.gearShifter = GearShifter.neutral
elif gear == 0:
ret.gearShifter = GearShifter.park
elif gear == 7:
ret.gearShifter = GearShifter.reverse
else:
if self.gear_correction:
ret.gearShifter = GearShifter.drive
else:
ret.gearShifter = GearShifter.unknown
# Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with
else:
gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
if gear in (5, 8): # 5: D, 8: sport mode
ret.gearShifter = GearShifter.drive
elif gear == 6:
ret.gearShifter = GearShifter.neutral
elif gear == 0:
ret.gearShifter = GearShifter.park
elif gear == 7:
ret.gearShifter = GearShifter.reverse
else:
if self.gear_correction:
ret.gearShifter = GearShifter.drive
else:
ret.gearShifter = GearShifter.unknown
if self.CP.carFingerprint in FEATURES["use_fca"]:
ret.stockAeb = cp.vl["FCA11"]['FCA_CmdAct'] != 0
ret.stockFcw = cp.vl["FCA11"]['CF_VSM_Warn'] == 2
else:
ret.stockAeb = cp.vl["SCC12"]['AEB_CmdAct'] != 0
ret.stockFcw = cp.vl["SCC12"]['CF_VSM_Warn'] == 2
# Blind Spot Detection and Lane Change Assist signals
self.lca_state = cp.vl["LCA11"]["CF_Lca_Stat"]
ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0
ret.rightBlindspot = cp.vl["LCA11"]["CF_Lca_IndRight"] != 0
# save the entire LKAS11, CLU11, SCC12 and MDPS12
self.lkas11 = cp_cam.vl["LKAS11"]
self.clu11 = cp.vl["CLU11"]
self.scc11 = cp_scc.vl["SCC11"]
self.scc12 = cp_scc.vl["SCC12"]
self.mdps12 = cp_mdps.vl["MDPS12"]
#self.park_brake = cp.vl["CGW1"]['CF_Gway_ParkBrakeSw']
self.park_brake = cp.vl["TCS13"]['PBRAKE_ACT'] == 1
self.steer_state = cp_mdps.vl["MDPS12"]['CF_Mdps_ToiActive'] #0 NOT ACTIVE, 1 ACTIVE
self.cruise_unavail = cp.vl["TCS13"]['CF_VSM_Avail'] != 1
self.lead_distance = cp_scc.vl["SCC11"]['ACC_ObjDist'] if not self.no_radar else 0
self.brake_hold = cp.vl["TCS15"]['AVH_LAMP'] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
self.brake_error = cp.vl["TCS13"]['ACCEnable'] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
ret.radarDistance = cp_scc.vl["SCC11"]['ACC_ObjDist'] if not self.no_radar else 0
self.lkas_error = cp_cam.vl["LKAS11"]["CF_Lkas_LdwsSysState"] == 7
if not self.lkas_error:
self.lkas_button_on = cp_cam.vl["LKAS11"]["CF_Lkas_LdwsSysState"]
if self.has_scc13:
self.scc13 = cp_scc.vl["SCC13"]
if self.has_scc14:
self.scc14 = cp_scc.vl["SCC14"]
if self.spas_enabled:
self.ems11 = cp.vl["EMS11"]
self.mdps11_strang = cp_mdps.vl["MDPS11"]["CR_Mdps_StrAng"]
self.mdps11_stat = cp_mdps.vl["MDPS11"]["CF_Mdps_Stat"]
ret.cruiseAccStatus = cp_scc.vl["SCC12"]['ACCMode'] == 1
ret.driverAcc = self.driverOverride == 1
return ret
def update_blinker(self, cp):
self.TSigLHSw = cp.vl["CGW1"]['CF_Gway_TSigLHSw']
self.TSigRHSw = cp.vl["CGW1"]['CF_Gway_TSigRHSw']
leftBlinker = cp.vl["CGW1"]['CF_Gway_TurnSigLh'] != 0
rightBlinker = cp.vl["CGW1"]['CF_Gway_TurnSigRh'] != 0
if leftBlinker and not rightBlinker:
self.left_blinker_flash = 50
self.right_blinker_flash = 0
elif rightBlinker and not leftBlinker:
self.right_blinker_flash = 50
self.left_blinker_flash = 0
elif leftBlinker and rightBlinker:
self.left_blinker_flash = 50
self.right_blinker_flash = 50
if self.left_blinker_flash:
self.left_blinker_flash -= 1
if self.right_blinker_flash:
self.right_blinker_flash -= 1
leftBlinker = self.left_blinker_flash != 0
rightBlinker = self.right_blinker_flash != 0
return leftBlinker, rightBlinker
@staticmethod
def get_can_parser(CP):
signals = [
# sig_name, sig_address, default
("WHL_SPD_FL", "WHL_SPD11", 0),
("WHL_SPD_FR", "WHL_SPD11", 0),
("WHL_SPD_RL", "WHL_SPD11", 0),
("WHL_SPD_RR", "WHL_SPD11", 0),
("YAW_RATE", "ESP12", 0),
("CF_Gway_DrvSeatBeltInd", "CGW4", 1),
("CF_Gway_DrvSeatBeltSw", "CGW1", 0), # Driver Seatbelt
("CF_Gway_DrvDrSw", "CGW1", 0), # Driver Door is open
("CF_Gway_AstDrSw", "CGW1", 0), # Passenger door is open
("CF_Gway_RLDrSw", "CGW2", 0), # Rear reft door is open
("CF_Gway_RRDrSw", "CGW2", 0), # Rear right door is open
("CF_Gway_TSigLHSw", "CGW1", 0),
("CF_Gway_TurnSigLh", "CGW1", 0),
("CF_Gway_TSigRHSw", "CGW1", 0),
("CF_Gway_TurnSigRh", "CGW1", 0),
("CF_Gway_ParkBrakeSw", "CGW1", 0), # Parking Brake
("CYL_PRES", "ESP12", 0),
("CF_Clu_CruiseSwState", "CLU11", 0),
("CF_Clu_CruiseSwMain", "CLU11", 0),
("CF_Clu_SldMainSW", "CLU11", 0),
("CF_Clu_ParityBit1", "CLU11", 0),
("CF_Clu_VanzDecimal" , "CLU11", 0),
("CF_Clu_Vanz", "CLU11", 0),
("CF_Clu_SPEED_UNIT", "CLU11", 0),
("CF_Clu_DetentOut", "CLU11", 0),
("CF_Clu_RheostatLevel", "CLU11", 0),
("CF_Clu_CluInfo", "CLU11", 0),
("CF_Clu_AmpInfo", "CLU11", 0),
("CF_Clu_AliveCnt1", "CLU11", 0),
("ACCEnable", "TCS13", 0),
("ACC_REQ", "TCS13", 0),
("BrakeLight", "TCS13", 0),
("DriverBraking", "TCS13", 0),
("StandStill", "TCS13", 0),
("PBRAKE_ACT", "TCS13", 0),
("DriverOverride", "TCS13", 0),
("CF_VSM_Avail", "TCS13", 0),
("ESC_Off_Step", "TCS15", 0),
("AVH_LAMP", "TCS15", 0),
("CF_Lvr_GearInf", "LVR11", 0), # Transmission Gear (0 = N or P, 1-8 = Fwd, 14 = Rev)
("CF_Lca_Stat", "LCA11", 0),
("CF_Lca_IndLeft", "LCA11", 0),
("CF_Lca_IndRight", "LCA11", 0),
("MainMode_ACC", "SCC11", 1),
("SCCInfoDisplay", "SCC11", 0),
("AliveCounterACC", "SCC11", 0),
("VSetDis", "SCC11", 30),
("ObjValid", "SCC11", 0),
("DriverAlertDisplay", "SCC11", 0),
("TauGapSet", "SCC11", 4),
("ACC_ObjStatus", "SCC11", 0),
("ACC_ObjLatPos", "SCC11", 0),
("ACC_ObjDist", "SCC11", 150), #TK211X value is 204.6
("ACC_ObjRelSpd", "SCC11", 0),
("Navi_SCC_Curve_Status", "SCC11", 0),
("Navi_SCC_Curve_Act", "SCC11", 0),
("Navi_SCC_Camera_Act", "SCC11", 0),
("Navi_SCC_Camera_Status", "SCC11", 2),
("UNIT", "TPMS11", 0),
("PRESSURE_FL", "TPMS11", 0),
("PRESSURE_FR", "TPMS11", 0),
("PRESSURE_RL", "TPMS11", 0),
("PRESSURE_RR", "TPMS11", 0),
("ACCMode", "SCC12", 0),
("CF_VSM_Prefill", "SCC12", 0),
("CF_VSM_DecCmdAct", "SCC12", 0),
("CF_VSM_HBACmd", "SCC12", 0),
("CF_VSM_Warn", "SCC12", 0),
("CF_VSM_Stat", "SCC12", 0),
("CF_VSM_BeltCmd", "SCC12", 0),
("ACCFailInfo", "SCC12", 0),
("StopReq", "SCC12", 0),
("CR_VSM_DecCmd", "SCC12", 0),
("aReqRaw", "SCC12", 0), #aReqMax
("TakeOverReq", "SCC12", 0),
("PreFill", "SCC12", 0),
("aReqValue", "SCC12", 0), #aReqMin
("CF_VSM_ConfMode", "SCC12", 1),
("AEB_Failinfo", "SCC12", 0),
("AEB_Status", "SCC12", 2),
("AEB_CmdAct", "SCC12", 0),
("AEB_StopReq", "SCC12", 0),
("CR_VSM_Alive", "SCC12", 0),
("CR_VSM_ChkSum", "SCC12", 0),
("SCCDrvModeRValue", "SCC13", 2),
("SCC_Equip", "SCC13", 1),
("AebDrvSetStatus", "SCC13", 0),
("JerkUpperLimit", "SCC14", 0),
("JerkLowerLimit", "SCC14", 0),
("SCCMode2", "SCC14", 0),
("ComfortBandUpper", "SCC14", 0),
("ComfortBandLower", "SCC14", 0),
]
checks = [
# address, frequency
("TCS13", 50),
("TCS15", 10),
("CLU11", 50),
("ESP12", 100),
("CGW1", 10),
("CGW4", 5),
("WHL_SPD11", 50),
]
if CP.sccBus == 0 and CP.enableCruise:
checks += [
("SCC11", 50),
("SCC12", 50),
]
if CP.mdpsBus == 0:
signals += [
("CR_Mdps_StrColTq", "MDPS12", 0),
("CF_Mdps_Def", "MDPS12", 0),
("CF_Mdps_ToiActive", "MDPS12", 0),
("CF_Mdps_ToiUnavail", "MDPS12", 0),
("CF_Mdps_MsgCount2", "MDPS12", 0),
("CF_Mdps_Chksum2", "MDPS12", 0),
("CF_Mdps_ToiFlt", "MDPS12", 0),
("CF_Mdps_SErr", "MDPS12", 0),
("CR_Mdps_StrTq", "MDPS12", 0),
("CF_Mdps_FailStat", "MDPS12", 0),
("CR_Mdps_OutTq", "MDPS12", 0)
]
checks += [
("MDPS12", 50)
]
if CP.sasBus == 0:
signals += [
("SAS_Angle", "SAS11", 0),
("SAS_Speed", "SAS11", 0),
]
checks += [
("SAS11", 100)
]
if CP.sccBus == -1:
signals += [
("CRUISE_LAMP_M", "EMS16", 0),
("CF_Lvr_CruiseSet", "LVR12", 0),
]
if CP.carFingerprint in FEATURES["use_cluster_gears"]:
signals += [
("CF_Clu_InhibitD", "CLU15", 0),
("CF_Clu_InhibitP", "CLU15", 0),
("CF_Clu_InhibitN", "CLU15", 0),
("CF_Clu_InhibitR", "CLU15", 0),
]
elif CP.carFingerprint in FEATURES["use_tcu_gears"]:
signals += [
("CUR_GR", "TCU12",0),
]
elif CP.carFingerprint in FEATURES["use_elect_gears"]:
signals += [
("Elect_Gear_Shifter", "ELECT_GEAR", 0),
]
else:
signals += [
("CF_Lvr_Gear","LVR12",0),
]
if CP.carFingerprint not in FEATURES["use_elect_ems"]:
signals += [
("PV_AV_CAN", "EMS12", 0),
("CF_Ems_AclAct", "EMS16", 0),
]
checks += [
("EMS12", 100),
("EMS16", 100),
]
else:
signals += [
("Accel_Pedal_Pos","E_EMS11",0),
("Brake_Pedal_Pos","E_EMS11",0),
]
checks += [
("E_EMS11", 100),
]
if CP.carFingerprint in FEATURES["use_fca"]:
signals += [
("FCA_CmdAct", "FCA11", 0),
("CF_VSM_Warn", "FCA11", 0),
]
checks += [("FCA11", 50)]
if CP.carFingerprint in [CAR.SANTA_FE]:
checks.remove(("TCS13", 50))
if CP.spasEnabled:
if CP.mdpsBus == 1:
signals += [
("SWI_IGK", "EMS11", 0),
("F_N_ENG", "EMS11", 0),
("ACK_TCS", "EMS11", 0),
("PUC_STAT", "EMS11", 0),
("TQ_COR_STAT", "EMS11", 0),
("RLY_AC", "EMS11", 0),
("F_SUB_TQI", "EMS11", 0),
("TQI_ACOR", "EMS11", 0),
("N", "EMS11", 0),
("TQI", "EMS11", 0),
("TQFR", "EMS11", 0),
("VS", "EMS11", 0),
("RATIO_TQI_BAS_MAX_STND", "EMS11", 0),
]
checks += [("EMS11", 100)]
elif CP.mdpsBus == 0:
signals += [
("CR_Mdps_StrAng", "MDPS11", 0),
("CF_Mdps_Stat", "MDPS11", 0),
]
checks += [("MDPS11", 100)]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
@staticmethod
def get_can2_parser(CP):
signals = []
checks = []
if CP.mdpsBus == 1:
signals += [
("CR_Mdps_StrColTq", "MDPS12", 0),
("CF_Mdps_Def", "MDPS12", 0),
("CF_Mdps_ToiActive", "MDPS12", 0),
("CF_Mdps_ToiUnavail", "MDPS12", 0),
("CF_Mdps_MsgCount2", "MDPS12", 0),
("CF_Mdps_Chksum2", "MDPS12", 0),
("CF_Mdps_ToiFlt", "MDPS12", 0),
("CF_Mdps_SErr", "MDPS12", 0),
("CR_Mdps_StrTq", "MDPS12", 0),
("CF_Mdps_FailStat", "MDPS12", 0),
("CR_Mdps_OutTq", "MDPS12", 0)
]
checks += [
("MDPS12", 50)
]
if CP.spasEnabled:
signals += [
("CR_Mdps_StrAng", "MDPS11", 0),
("CF_Mdps_Stat", "MDPS11", 0),
]
checks += [
("MDPS11", 100),
]
if CP.sasBus == 1:
signals += [
("SAS_Angle", "SAS11", 0),
("SAS_Speed", "SAS11", 0),
]
checks += [
("SAS11", 100)
]
if CP.sccBus == 1:
signals += [
("MainMode_ACC", "SCC11", 1),
("SCCInfoDisplay", "SCC11", 0),
("AliveCounterACC", "SCC11", 0),
("VSetDis", "SCC11", 30),
("ObjValid", "SCC11", 0),
("DriverAlertDisplay", "SCC11", 0),
("TauGapSet", "SCC11", 4),
("ACC_ObjStatus", "SCC11", 0),
("ACC_ObjLatPos", "SCC11", 0),
("ACC_ObjDist", "SCC11", 150.),
("ACC_ObjRelSpd", "SCC11", 0),
("Navi_SCC_Curve_Status", "SCC11", 0),
("Navi_SCC_Curve_Act", "SCC11", 0),
("Navi_SCC_Camera_Act", "SCC11", 0),
("Navi_SCC_Camera_Status", "SCC11", 2),
("ACCMode", "SCC12", 0),
("CF_VSM_Prefill", "SCC12", 0),
("CF_VSM_DecCmdAct", "SCC12", 0),
("CF_VSM_HBACmd", "SCC12", 0),
("CF_VSM_Warn", "SCC12", 0),
("CF_VSM_Stat", "SCC12", 0),
("CF_VSM_BeltCmd", "SCC12", 0),
("ACCFailInfo", "SCC12", 0),
("StopReq", "SCC12", 0),
("CR_VSM_DecCmd", "SCC12", 0),
("aReqRaw", "SCC12", 0), #aReqMax
("TakeOverReq", "SCC12", 0),
("PreFill", "SCC12", 0),
("aReqValue", "SCC12", 0), #aReqMin
("CF_VSM_ConfMode", "SCC12", 1),
("AEB_Failinfo", "SCC12", 0),
("AEB_Status", "SCC12", 2),
("AEB_CmdAct", "SCC12", 0),
("AEB_StopReq", "SCC12", 0),
("CR_VSM_Alive", "SCC12", 0),
("CR_VSM_ChkSum", "SCC12", 0),
("SCCDrvModeRValue", "SCC13", 2),
("SCC_Equip", "SCC13", 1),
("AebDrvSetStatus", "SCC13", 0),
("JerkUpperLimit", "SCC14", 0),
("JerkLowerLimit", "SCC14", 0),
("SCCMode2", "SCC14", 0),
("ComfortBandUpper", "SCC14", 0),
("ComfortBandLower", "SCC14", 0),
]
checks += [
("SCC11", 50),
("SCC12", 50),
]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 1)
@staticmethod
def get_cam_can_parser(CP):
signals = [
# sig_name, sig_address, default
("CF_Lkas_LdwsActivemode", "LKAS11", 0),
("CF_Lkas_LdwsSysState", "LKAS11", 0),
("CF_Lkas_SysWarning", "LKAS11", 0),
("CF_Lkas_LdwsLHWarning", "LKAS11", 0),
("CF_Lkas_LdwsRHWarning", "LKAS11", 0),
("CF_Lkas_HbaLamp", "LKAS11", 0),
("CF_Lkas_FcwBasReq", "LKAS11", 0),
("CF_Lkas_ToiFlt", "LKAS11", 0),
("CF_Lkas_HbaSysState", "LKAS11", 0),
("CF_Lkas_FcwOpt", "LKAS11", 0),
("CF_Lkas_HbaOpt", "LKAS11", 0),
("CF_Lkas_FcwSysState", "LKAS11", 0),
("CF_Lkas_FcwCollisionWarning", "LKAS11", 0),
("CF_Lkas_MsgCount", "LKAS11", 0),
("CF_Lkas_FusionState", "LKAS11", 0),
("CF_Lkas_FcwOpt_USM", "LKAS11", 0),
("CF_Lkas_LdwsOpt_USM", "LKAS11", 0)
]
checks = [
("LKAS11", 100)
]
if CP.sccBus == 2:
signals += [
("MainMode_ACC", "SCC11", 1),
("SCCInfoDisplay", "SCC11", 0),
("AliveCounterACC", "SCC11", 0),
("VSetDis", "SCC11", 30),
("ObjValid", "SCC11", 0),
("DriverAlertDisplay", "SCC11", 0),
("TauGapSet", "SCC11", 4),
("ACC_ObjStatus", "SCC11", 0),
("ACC_ObjLatPos", "SCC11", 0),
("ACC_ObjDist", "SCC11", 150.),
("ACC_ObjRelSpd", "SCC11", 0),
("Navi_SCC_Curve_Status", "SCC11", 0),
("Navi_SCC_Curve_Act", "SCC11", 0),
("Navi_SCC_Camera_Act", "SCC11", 0),
("Navi_SCC_Camera_Status", "SCC11", 2),
("ACCMode", "SCC12", 0),
("CF_VSM_Prefill", "SCC12", 0),
("CF_VSM_DecCmdAct", "SCC12", 0),
("CF_VSM_HBACmd", "SCC12", 0),
("CF_VSM_Warn", "SCC12", 0),
("CF_VSM_Stat", "SCC12", 0),
("CF_VSM_BeltCmd", "SCC12", 0),
("ACCFailInfo", "SCC12", 0),
("StopReq", "SCC12", 0),
("CR_VSM_DecCmd", "SCC12", 0),
("aReqRaw", "SCC12", 0), #aReqMax
("TakeOverReq", "SCC12", 0),
("PreFill", "SCC12", 0),
("aReqValue", "SCC12", 0), #aReqMin
("CF_VSM_ConfMode", "SCC12", 1),
("AEB_Failinfo", "SCC12", 0),
("AEB_Status", "SCC12", 2),
("AEB_CmdAct", "SCC12", 0),
("AEB_StopReq", "SCC12", 0),
("CR_VSM_Alive", "SCC12", 0),
("CR_VSM_ChkSum", "SCC12", 0),
("SCCDrvModeRValue", "SCC13", 2),
("SCC_Equip", "SCC13", 1),
("AebDrvSetStatus", "SCC13", 0),
("JerkUpperLimit", "SCC14", 0),
("JerkLowerLimit", "SCC14", 0),
("SCCMode2", "SCC14", 0),
("ComfortBandUpper", "SCC14", 0),
("ComfortBandLower", "SCC14", 0),
]
checks += [
("SCC11", 50),
("SCC12", 50),
]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
def update(self, c, CS, frame, sm, CP):
if self.CP != CP:
self.CP = CP
self.param_load()
enabled = c.enabled
actuators = c.actuators
pcm_cancel_cmd = c.cruiseControl.cancel
path_plan = sm['pathPlan']
self.dRel, self.yRel, self.vRel = SpdController.get_lead(sm)
if self.SC is not None:
self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo)
else:
self.model_speed = self.model_sum = 0
# Steering Torque
param, dst_steer = self.steerParams_torque(CS, c.actuators, path_plan)
new_steer = actuators.steer * param.STEER_MAX
apply_steer = apply_std_steer_torque_limits(new_steer,
self.apply_steer_last,
CS.out.steeringTorque,
param)
self.steer_rate_limited = new_steer != apply_steer
apply_steer_limit = param.STEER_MAX
if self.steer_torque_ratio < 1:
apply_steer_limit = int(self.steer_torque_ratio * param.STEER_MAX)
apply_steer = self.limit_ctrl(apply_steer, apply_steer_limit, 0)
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
lkas_active = enabled and CS.main_on and CS.out.cruiseState.enabled and abs(
CS.out.steeringAngle) < 180. #and self.lkas_button
if not lkas_active:
apply_steer = 0
steer_req = 1 if apply_steer else 0
self.apply_steer_last = apply_steer
sys_warning, self.hud_sys_state = self.process_hud_alert(
lkas_active, c)
can_sends = []
if frame == 0: # initialize counts from last received count signals
self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"] + 1
self.lkas11_cnt %= 0x10
can_sends.append(
create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint,
apply_steer, steer_req, CS.lkas11, sys_warning,
self.hud_sys_state, c))
# send mdps12 to LKAS to prevent LKAS error if no cancel cmd
can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
str_log1 = 'torg:{:5.0f}/{:5.0f}/{:5.0f} CV={:5.1f}'.format(
apply_steer, new_steer, dst_steer, self.model_speed)
str_log2 = 'limit={:.0f} tm={:.1f} '.format(apply_steer_limit,
self.timer1.sampleTime())
trace1.printf('{} {}'.format(str_log1, str_log2))
run_speed_ctrl = self.param_OpkrAccelProfile and CS.acc_active and self.SC != None
if not run_speed_ctrl:
str_log2 = 'LKAS={:.0f} steer={:5.0f} '.format(
CS.lkas_button_on, CS.out.steeringTorque)
trace1.printf2('{}'.format(str_log2))
if pcm_cancel_cmd and self.CP.longcontrolEnabled:
can_sends.append(
create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL))
elif CS.out.cruiseState.standstill:
# run only first time when the car stopped
if self.last_lead_distance == 0 or not self.param_OpkrAutoResume:
# get the lead distance from the Radar
self.last_lead_distance = CS.lead_distance
self.resume_cnt = 0
# when lead car starts moving, create 6 RES msgs
elif CS.lead_distance != self.last_lead_distance and (
frame - self.last_resume_frame) > 5:
can_sends.append(
create_clu11(self.packer, self.resume_cnt, CS.clu11,
Buttons.RES_ACCEL))
self.resume_cnt += 1
# interval after 6 msgs
if self.resume_cnt > 5:
self.last_resume_frame = frame
self.resume_cnt = 0
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
elif run_speed_ctrl and self.SC != None:
is_sc_run = self.SC.update(CS, sm, self)
if is_sc_run:
can_sends.append(
create_clu11(self.packer, self.resume_cnt, CS.clu11,
self.SC.btn_type, self.SC.sc_clu_speed))
self.resume_cnt += 1
else:
self.resume_cnt = 0
str1 = 'run={} cruise_set_mode={} kph={:.1f}/{:.1f} DO={:.0f}/{:.0f} '.format(
is_sc_run, self.SC.cruise_set_mode,
self.SC.cruise_set_speed_kph, CS.VSetDis, CS.driverOverride,
CS.cruise_buttons)
str2 = 'btn_type={:.0f} speed={:.1f} cnt={:.0f}'.format(
self.SC.btn_type, self.SC.sc_clu_speed, self.resume_cnt)
str_log = str1 + str2
self.traceCC.add(str_log)
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in [
CAR.SONATA, CAR.PALISADE
]:
can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
# counter inc
self.lkas11_cnt += 1
return can_sends
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
if self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
self.shifter_values = can_define.dv["CLU15"]["CF_Clu_Gear"]
elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
self.shifter_values = can_define.dv["TCU12"]["CUR_GR"]
else: # preferred and elect gear methods use same definition
self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"]
#Auto detection for setup
self.no_radar = CP.sccBus == -1
self.lkas_button_on = True
self.cruise_main_button = 0
self.mdps_error_cnt = 0
self.cruise_unavail_cnt = 0
self.acc_active = False
self.cruiseState_standstill = False
self.cruiseState_modeSel = 0
self.SC = SpdController()
self.driverAcc_time = 0
self.apply_steer = 0.
self.steer_anglecorrection = float(
int(Params().get("OpkrSteerAngleCorrection", encoding="utf8")) *
0.1)
self.gear_correction = Params().get_bool("JustDoGearD")
self.steer_wind_down = Params().get_bool("SteerWindDown")
self.brake_check = False
self.cancel_check = False
self.safety_sign_check = 0
self.safety_sign = 0
self.safety_sign_last = 0
self.safety_dist = 0
self.safety_block_remain_dist = 0
self.is_highway = False
self.on_speed_control = False
def update(self, cp, cp2, cp_cam):
cp_mdps = cp2 if self.CP.mdpsBus == 1 else cp
cp_sas = cp2 if self.CP.sasBus else cp
cp_scc = cp_cam if ((self.CP.sccBus == 2)
or self.CP.radarOffCan) else cp
cp_fca = cp_cam if (self.CP.fcaBus == 2) else cp
self.prev_cruise_buttons = self.cruise_buttons
self.prev_cruise_main_button = self.cruise_main_button
self.prev_lkas_button_on = self.lkas_button_on
ret = car.CarState.new_message()
ret.doorOpen = any([
cp.vl["CGW1"]["CF_Gway_DrvDrSw"], cp.vl["CGW1"]["CF_Gway_AstDrSw"],
cp.vl["CGW2"]["CF_Gway_RLDrSw"], cp.vl["CGW2"]["CF_Gway_RRDrSw"]
])
ret.seatbeltUnlatched = cp.vl["CGW1"]["CF_Gway_DrvSeatBeltSw"] == 0
ret.wheelSpeeds.fl = cp.vl["WHL_SPD11"]["WHL_SPD_FL"] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = cp.vl["WHL_SPD11"]["WHL_SPD_FR"] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = cp.vl["WHL_SPD11"]["WHL_SPD_RL"] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = cp.vl["WHL_SPD11"]["WHL_SPD_RR"] * CV.KPH_TO_MS
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr +
ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.vEgo = cp.vl["CLU11"]["CF_Clu_Vanz"] * CV.KPH_TO_MS
ret.standstill = ret.vEgoRaw < 0.1
ret.standStill = self.CP.standStill
ret.steeringAngleDeg = cp_sas.vl["SAS11"][
"SAS_Angle"] - self.steer_anglecorrection
ret.steeringRateDeg = cp_sas.vl["SAS11"]["SAS_Speed"]
ret.yawRate = cp.vl["ESP12"]["YAW_RATE"]
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(
50, cp.vl["CGW1"]["CF_Gway_TurnSigLh"],
cp.vl["CGW1"]["CF_Gway_TurnSigRh"])
ret.steeringTorque = cp_mdps.vl["MDPS12"]["CR_Mdps_StrColTq"]
ret.steeringTorqueEps = cp_mdps.vl["MDPS12"]["CR_Mdps_OutTq"]
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
if self.steer_wind_down:
ret.steerWarning = cp_mdps.vl["MDPS12"][
"CF_Mdps_ToiUnavail"] != 0 or cp_mdps.vl["MDPS12"][
"CF_Mdps_ToiFlt"] != 0
else:
self.mdps_error_cnt += 1 if cp_mdps.vl["MDPS12"][
"CF_Mdps_ToiUnavail"] != 0 else -self.mdps_error_cnt
ret.steerWarning = self.mdps_error_cnt > 100 #cp_mdps.vl["MDPS12"]["CF_Mdps_ToiUnavail"] != 0
self.VSetDis = cp_scc.vl["SCC11"]["VSetDis"]
ret.vSetDis = self.VSetDis
self.clu_Vanz = cp.vl["CLU11"]["CF_Clu_Vanz"]
lead_objspd = cp_scc.vl["SCC11"]["ACC_ObjRelSpd"]
self.lead_objspd = lead_objspd * CV.MS_TO_KPH
self.Mdps_ToiUnavail = cp_mdps.vl["MDPS12"]["CF_Mdps_ToiUnavail"]
self.driverOverride = cp.vl["TCS13"]["DriverOverride"]
if self.driverOverride == 1:
self.driverAcc_time = 100
elif self.driverAcc_time:
self.driverAcc_time -= 1
# cruise state
ret.cruiseState.enabled = (cp_scc.vl["SCC12"]["ACCMode"] != 0) if not self.no_radar else \
cp.vl["LVR12"]["CF_Lvr_CruiseSet"] != 0
ret.cruiseState.available = (cp_scc.vl["SCC11"]["MainMode_ACC"] != 0) if not self.no_radar else \
cp.vl["EMS16"]["CRUISE_LAMP_M"] != 0
ret.cruiseState.standstill = cp_scc.vl["SCC11"][
"SCCInfoDisplay"] == 4. if not self.no_radar else False
self.cruiseState_standstill = ret.cruiseState.standstill
self.is_set_speed_in_mph = bool(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"])
self.acc_active = ret.cruiseState.enabled
if self.acc_active:
self.brake_check = False
self.cancel_check = False
self.cruiseState_modeSel, speed_kph = self.SC.update_cruiseSW(self)
ret.cruiseState.modeSel = self.cruiseState_modeSel
if ret.cruiseState.enabled and (self.brake_check == False
or self.cancel_check == False):
speed_conv = CV.MPH_TO_MS if self.is_set_speed_in_mph else CV.KPH_TO_MS
ret.cruiseState.speed = speed_kph * speed_conv if not self.no_radar else \
cp.vl["LVR12"]["CF_Lvr_CruiseSet"] * speed_conv
else:
ret.cruiseState.speed = 0
self.cruise_main_button = cp.vl["CLU11"]["CF_Clu_CruiseSwMain"]
self.cruise_buttons = cp.vl["CLU11"]["CF_Clu_CruiseSwState"]
ret.cruiseButtons = self.cruise_buttons
# TODO: Find brake pressure
ret.brake = 0
ret.brakePressed = cp.vl["TCS13"]["DriverBraking"] != 0
self.brakeUnavailable = cp.vl["TCS13"]["ACCEnable"] == 3
if ret.brakePressed:
self.brake_check = True
if self.cruise_buttons == 4:
self.cancel_check = True
# TODO: Check this
ret.brakeLights = bool(cp.vl["TCS13"]["BrakeLight"]
or ret.brakePressed)
if self.CP.carFingerprint in (HYBRID_CAR | EV_CAR):
if self.CP.carFingerprint in HYBRID_CAR:
ret.gas = cp.vl["E_EMS11"]["CR_Vcu_AccPedDep_Pos"] / 254.
else:
ret.gas = cp.vl["E_EMS11"]["Accel_Pedal_Pos"] / 254.
ret.gasPressed = ret.gas > 0
else:
ret.gas = cp.vl["EMS12"]["PV_AV_CAN"] / 100.
ret.gasPressed = bool(cp.vl["EMS16"]["CF_Ems_AclAct"])
ret.espDisabled = (cp.vl["TCS15"]["ESC_Off_Step"] != 0)
self.parkBrake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1
# TPMS code added from OPKR
if cp.vl["TPMS11"]["UNIT"] == 0.0:
ret.tpmsPressureFl = cp.vl["TPMS11"]["PRESSURE_FL"]
ret.tpmsPressureFr = cp.vl["TPMS11"]["PRESSURE_FR"]
ret.tpmsPressureRl = cp.vl["TPMS11"]["PRESSURE_RL"]
ret.tpmsPressureRr = cp.vl["TPMS11"]["PRESSURE_RR"]
elif cp.vl["TPMS11"]["UNIT"] == 1.0:
ret.tpmsPressureFl = cp.vl["TPMS11"]["PRESSURE_FL"] * 5 * 0.145038
ret.tpmsPressureFr = cp.vl["TPMS11"]["PRESSURE_FR"] * 5 * 0.145038
ret.tpmsPressureRl = cp.vl["TPMS11"]["PRESSURE_RL"] * 5 * 0.145038
ret.tpmsPressureRr = cp.vl["TPMS11"]["PRESSURE_RR"] * 5 * 0.145038
elif cp.vl["TPMS11"]["UNIT"] == 2.0:
ret.tpmsPressureFl = cp.vl["TPMS11"]["PRESSURE_FL"] / 10 * 14.5038
ret.tpmsPressureFr = cp.vl["TPMS11"]["PRESSURE_FR"] / 10 * 14.5038
ret.tpmsPressureRl = cp.vl["TPMS11"]["PRESSURE_RL"] / 10 * 14.5038
ret.tpmsPressureRr = cp.vl["TPMS11"]["PRESSURE_RR"] / 10 * 14.5038
# OPKR
self.safety_dist = cp.vl["NAVI"]["OPKR_S_Dist"]
self.safety_sign_check = cp.vl["NAVI"]["OPKR_S_Sign"]
self.safety_block_remain_dist = cp.vl["NAVI"]["OPKR_SBR_Dist"]
self.is_highway = cp_scc.vl["SCC11"]["Navi_SCC_Camera_Act"] != 0.
if self.safety_sign_check in [25.] and not self.is_highway:
self.safety_sign = 30.
self.safety_sign_last = self.safety_sign
elif self.safety_sign_check in [1.] and not self.is_highway:
self.safety_sign = 40.
self.safety_sign_last = self.safety_sign
elif self.safety_sign_check in [8., 9., 10.] and not self.is_highway:
self.safety_sign = 50.
self.safety_sign_last = self.safety_sign
elif self.safety_sign_check in [16., 17., 18.] and not self.is_highway:
self.safety_sign = 60.
self.safety_sign_last = self.safety_sign
elif self.safety_sign_check in [24.] and not self.is_highway:
self.safety_sign = 70.
self.safety_sign_last = self.safety_sign
elif self.safety_sign_check in [0., 1., 2.]:
self.safety_sign = 80.
self.safety_sign_last = self.safety_sign
elif self.safety_sign_check in [8., 10.]:
self.safety_sign = 90.
self.safety_sign_last = self.safety_sign
elif self.safety_sign_check in [16., 17., 18.] and self.is_highway:
self.safety_sign = 100.
self.safety_sign_last = self.safety_sign
elif self.safety_sign_check in [24., 25., 26.] and self.is_highway:
self.safety_sign = 110.
self.safety_sign_last = self.safety_sign
elif self.safety_block_remain_dist < 255.:
self.safety_sign = self.safety_sign_last
else:
self.safety_sign = 0.
cam_distance_calc = interp(ret.vEgo * CV.MS_TO_KPH, [30, 60, 100, 160],
[3.6, 5.35, 5.8, 6.8])
consider_speed = interp((ret.vEgo * CV.MS_TO_KPH - self.safety_sign),
[10, 30], [1, 1.25])
if self.safety_sign > 29 and self.safety_dist < cam_distance_calc * consider_speed * ret.vEgo * CV.MS_TO_KPH:
ret.safetySign = self.safety_sign
ret.safetyDist = self.safety_dist
self.on_speed_control = True
elif self.safety_sign > 29 and self.safety_block_remain_dist < 255.:
ret.safetySign = self.safety_sign
ret.safetyDist = self.safety_dist
self.on_speed_control = True
elif self.safety_sign > 29 and self.safety_dist < 600.:
ret.safetySign = self.safety_sign
ret.safetyDist = self.safety_dist
self.on_speed_control = False
else:
ret.safetySign = 0
ret.safetyDist = 0
self.on_speed_control = False
self.cruiseGapSet = cp_scc.vl["SCC11"]["TauGapSet"]
ret.cruiseGapSet = self.cruiseGapSet
# TODO: refactor gear parsing in function
# Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection,
# as this seems to be standard over all cars, but is not the preferred method.
if self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
gear = cp.vl["CLU15"]["CF_Clu_Gear"]
elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
gear = cp.vl["TCU12"]["CUR_GR"]
elif self.CP.carFingerprint in FEATURES["use_elect_gears"]:
gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"]
else:
gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
if self.gear_correction:
ret.gearShifter = GearShifter.drive
else:
ret.gearShifter = self.parse_gear_shifter(
self.shifter_values.get(gear))
if self.CP.fcaBus != -1 or self.CP.carFingerprint in FEATURES[
"use_fca"]:
ret.stockAeb = cp_fca.vl["FCA11"]["FCA_CmdAct"] != 0
ret.stockFcw = cp_fca.vl["FCA11"]["CF_VSM_Warn"] == 2
elif not self.CP.radarOffCan:
ret.stockAeb = cp_scc.vl["SCC12"]["AEB_CmdAct"] != 0
ret.stockFcw = cp_scc.vl["SCC12"]["CF_VSM_Warn"] == 2
# Blind Spot Detection and Lane Change Assist signals
if self.CP.bsmAvailable or self.CP.enableBsm:
self.lca_state = cp.vl["LCA11"]["CF_Lca_Stat"]
ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0
ret.rightBlindspot = cp.vl["LCA11"]["CF_Lca_IndRight"] != 0
# save the entire LKAS11, CLU11, SCC12 and MDPS12
self.lkas11 = copy.copy(cp_cam.vl["LKAS11"])
self.clu11 = copy.copy(cp.vl["CLU11"])
self.scc11 = copy.copy(cp_scc.vl["SCC11"])
self.scc12 = copy.copy(cp_scc.vl["SCC12"])
self.scc13 = copy.copy(cp_scc.vl["SCC13"])
self.scc14 = copy.copy(cp_scc.vl["SCC14"])
self.fca11 = copy.copy(cp_fca.vl["FCA11"])
self.mdps12 = copy.copy(cp_mdps.vl["MDPS12"])
self.scc11init = copy.copy(cp.vl["SCC11"])
self.scc12init = copy.copy(cp.vl["SCC12"])
self.fca11init = copy.copy(cp.vl["FCA11"])
ret.brakeHold = cp.vl["TCS15"][
"AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
self.brakeHold = ret.brakeHold
self.brake_error = cp.vl["TCS13"][
"ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
self.steer_state = cp_mdps.vl["MDPS12"][
"CF_Mdps_ToiActive"] #0 NOT ACTIVE, 1 ACTIVE
self.cruise_unavail_cnt += 1 if cp.vl["TCS13"][
"CF_VSM_Avail"] != 1 and cp.vl["TCS13"][
"ACCEnable"] != 0 else -self.cruise_unavail_cnt
self.cruise_unavail = self.cruise_unavail_cnt > 100
self.lead_distance = cp_scc.vl["SCC11"][
"ACC_ObjDist"] if not self.no_radar else 0
ret.radarDistance = cp_scc.vl["SCC11"][
"ACC_ObjDist"] if not self.no_radar else 0
self.lkas_error = cp_cam.vl["LKAS11"]["CF_Lkas_LdwsSysState"] == 7
if not self.lkas_error:
self.lkas_button_on = cp_cam.vl["LKAS11"]["CF_Lkas_LdwsSysState"]
ret.cruiseAccStatus = cp_scc.vl["SCC12"]["ACCMode"] == 1
ret.driverAcc = self.driverOverride == 1
return ret
@staticmethod
def get_can_parser(CP):
signals = [
# sig_name, sig_address, default
("WHL_SPD_FL", "WHL_SPD11", 0),
("WHL_SPD_FR", "WHL_SPD11", 0),
("WHL_SPD_RL", "WHL_SPD11", 0),
("WHL_SPD_RR", "WHL_SPD11", 0),
("YAW_RATE", "ESP12", 0),
("CF_Gway_DrvSeatBeltInd", "CGW4", 1),
("CF_Gway_DrvSeatBeltSw", "CGW1", 0),
("CF_Gway_DrvDrSw", "CGW1", 0), # Driver Door
("CF_Gway_AstDrSw", "CGW1", 0), # Passenger door
("CF_Gway_RLDrSw", "CGW2", 0), # Rear reft door
("CF_Gway_RRDrSw", "CGW2", 0), # Rear right door
("CF_Gway_TurnSigLh", "CGW1", 0),
("CF_Gway_TurnSigRh", "CGW1", 0),
("CF_Gway_ParkBrakeSw", "CGW1", 0),
("CYL_PRES", "ESP12", 0),
("AVH_STAT", "ESP11", 0),
("CF_Clu_CruiseSwState", "CLU11", 0),
("CF_Clu_CruiseSwMain", "CLU11", 0),
("CF_Clu_SldMainSW", "CLU11", 0),
("CF_Clu_ParityBit1", "CLU11", 0),
("CF_Clu_VanzDecimal", "CLU11", 0),
("CF_Clu_Vanz", "CLU11", 0),
("CF_Clu_SPEED_UNIT", "CLU11", 0),
("CF_Clu_DetentOut", "CLU11", 0),
("CF_Clu_RheostatLevel", "CLU11", 0),
("CF_Clu_CluInfo", "CLU11", 0),
("CF_Clu_AmpInfo", "CLU11", 0),
("CF_Clu_AliveCnt1", "CLU11", 0),
("ACCEnable", "TCS13", 0),
("BrakeLight", "TCS13", 0),
("DriverBraking", "TCS13", 0),
("DriverOverride", "TCS13", 0),
("PBRAKE_ACT", "TCS13", 0),
("CF_VSM_Avail", "TCS13", 0),
("ESC_Off_Step", "TCS15", 0),
("AVH_LAMP", "TCS15", 0),
("CF_Lvr_CruiseSet", "LVR12", 0),
("CRUISE_LAMP_M", "EMS16", 0),
("CR_FCA_Alive", "FCA11", 0),
("Supplemental_Counter", "FCA11", 0),
("MainMode_ACC", "SCC11", 1),
("SCCInfoDisplay", "SCC11", 0),
("AliveCounterACC", "SCC11", 0),
("VSetDis", "SCC11", 30),
("ObjValid", "SCC11", 0),
("DriverAlertDisplay", "SCC11", 0),
("TauGapSet", "SCC11", 4),
("ACC_ObjStatus", "SCC11", 0),
("ACC_ObjLatPos", "SCC11", 0),
("ACC_ObjDist", "SCC11", 150), #TK211X value is 204.6
("ACC_ObjRelSpd", "SCC11", 0),
("Navi_SCC_Curve_Status", "SCC11", 0),
("Navi_SCC_Curve_Act", "SCC11", 0),
("Navi_SCC_Camera_Act", "SCC11", 0),
("Navi_SCC_Camera_Status", "SCC11", 2),
("ACCMode", "SCC12", 0),
("CF_VSM_Prefill", "SCC12", 0),
("CF_VSM_DecCmdAct", "SCC12", 0),
("CF_VSM_HBACmd", "SCC12", 0),
("CF_VSM_Warn", "SCC12", 0),
("CF_VSM_Stat", "SCC12", 0),
("CF_VSM_BeltCmd", "SCC12", 0),
("ACCFailInfo", "SCC12", 0),
("StopReq", "SCC12", 0),
("CR_VSM_DecCmd", "SCC12", 0),
("aReqRaw", "SCC12", 0), #aReqMax
("TakeOverReq", "SCC12", 0),
("PreFill", "SCC12", 0),
("aReqValue", "SCC12", 0), #aReqMin
("CF_VSM_ConfMode", "SCC12", 1),
("AEB_Failinfo", "SCC12", 0),
("AEB_Status", "SCC12", 2),
("AEB_CmdAct", "SCC12", 0),
("AEB_StopReq", "SCC12", 0),
("CR_VSM_Alive", "SCC12", 0),
("CR_VSM_ChkSum", "SCC12", 0),
("SCCDrvModeRValue", "SCC13", 2),
("SCC_Equip", "SCC13", 1),
("AebDrvSetStatus", "SCC13", 0),
("JerkUpperLimit", "SCC14", 0),
("JerkLowerLimit", "SCC14", 0),
("SCCMode2", "SCC14", 0),
("ComfortBandUpper", "SCC14", 0),
("ComfortBandLower", "SCC14", 0),
("UNIT", "TPMS11", 0),
("PRESSURE_FL", "TPMS11", 0),
("PRESSURE_FR", "TPMS11", 0),
("PRESSURE_RL", "TPMS11", 0),
("PRESSURE_RR", "TPMS11", 0),
("OPKR_S_Dist", "NAVI", 0),
("OPKR_S_Sign", "NAVI", 31),
("OPKR_SBR_Dist", "NAVI", 0),
]
checks = [
# address, frequency
("TCS13", 50),
("TCS15", 10),
("CLU11", 50),
("ESP12", 100),
("CGW1", 10),
("CGW2", 5),
("CGW4", 5),
("WHL_SPD11", 50),
]
if CP.sccBus == 0 and CP.pcmCruise:
checks += [
("SCC11", 50),
("SCC12", 50),
]
if CP.fcaBus == 0:
signals += [
("FCA_CmdAct", "FCA11", 0),
("CF_VSM_Warn", "FCA11", 0),
]
checks += [("FCA11", 50)]
if CP.mdpsBus == 0:
signals += [("CR_Mdps_StrColTq", "MDPS12", 0),
("CF_Mdps_Def", "MDPS12", 0),
("CF_Mdps_ToiActive", "MDPS12", 0),
("CF_Mdps_ToiUnavail", "MDPS12", 0),
("CF_Mdps_ToiFlt", "MDPS12", 0),
("CF_Mdps_MsgCount2", "MDPS12", 0),
("CF_Mdps_Chksum2", "MDPS12", 0),
("CF_Mdps_SErr", "MDPS12", 0),
("CR_Mdps_StrTq", "MDPS12", 0),
("CF_Mdps_FailStat", "MDPS12", 0),
("CR_Mdps_OutTq", "MDPS12", 0)]
checks += [("MDPS12", 50)]
if CP.sasBus == 0:
signals += [
("SAS_Angle", "SAS11", 0),
("SAS_Speed", "SAS11", 0),
]
checks += [("SAS11", 100)]
if CP.bsmAvailable or CP.enableBsm:
signals += [
("CF_Lca_Stat", "LCA11", 0),
("CF_Lca_IndLeft", "LCA11", 0),
("CF_Lca_IndRight", "LCA11", 0),
]
checks += [("LCA11", 50)]
if CP.carFingerprint in (HYBRID_CAR | EV_CAR):
if CP.carFingerprint in HYBRID_CAR:
signals += [("CR_Vcu_AccPedDep_Pos", "E_EMS11", 0)]
else:
signals += [("Accel_Pedal_Pos", "E_EMS11", 0)]
checks += [
("E_EMS11", 50),
]
else:
signals += [
("PV_AV_CAN", "EMS12", 0),
("CF_Ems_AclAct", "EMS16", 0),
]
checks += [
("EMS12", 100),
("EMS16", 100),
]
if CP.carFingerprint in FEATURES["use_cluster_gears"]:
signals += [
("CF_Clu_Gear", "CLU15", 0),
]
checks += [("CLU15", 5)]
elif CP.carFingerprint in FEATURES["use_tcu_gears"]:
signals += [("CUR_GR", "TCU12", 0)]
checks += [("TCU12", 100)]
elif CP.carFingerprint in FEATURES["use_elect_gears"]:
signals += [("Elect_Gear_Shifter", "ELECT_GEAR", 0)]
checks += [("ELECT_GEAR", 20)]
else:
signals += [("CF_Lvr_Gear", "LVR12", 0)]
checks += [("LVR12", 100)]
return CANParser(DBC[CP.carFingerprint]["pt"],
signals,
checks,
0,
enforce_checks=False)
@staticmethod
def get_can2_parser(CP):
signals = []
checks = []
if CP.mdpsBus == 1:
signals += [("CR_Mdps_StrColTq", "MDPS12", 0),
("CF_Mdps_Def", "MDPS12", 0),
("CF_Mdps_ToiActive", "MDPS12", 0),
("CF_Mdps_ToiUnavail", "MDPS12", 0),
("CF_Mdps_ToiFlt", "MDPS12", 0),
("CF_Mdps_MsgCount2", "MDPS12", 0),
("CF_Mdps_Chksum2", "MDPS12", 0),
("CF_Mdps_SErr", "MDPS12", 0),
("CR_Mdps_StrTq", "MDPS12", 0),
("CF_Mdps_FailStat", "MDPS12", 0),
("CR_Mdps_OutTq", "MDPS12", 0)]
checks += [("MDPS12", 50)]
if CP.sasBus == 1:
signals += [
("SAS_Angle", "SAS11", 0),
("SAS_Speed", "SAS11", 0),
]
checks += [("SAS11", 100)]
return CANParser(DBC[CP.carFingerprint]["pt"],
signals,
checks,
1,
enforce_checks=False)
@staticmethod
def get_cam_can_parser(CP):
signals = [
# sig_name, sig_address, default
("CF_Lkas_LdwsActivemode", "LKAS11", 0),
("CF_Lkas_LdwsSysState", "LKAS11", 0),
("CF_Lkas_SysWarning", "LKAS11", 0),
("CF_Lkas_LdwsLHWarning", "LKAS11", 0),
("CF_Lkas_LdwsRHWarning", "LKAS11", 0),
("CF_Lkas_HbaLamp", "LKAS11", 0),
("CF_Lkas_FcwBasReq", "LKAS11", 0),
("CF_Lkas_ToiFlt", "LKAS11", 0),
("CF_Lkas_HbaSysState", "LKAS11", 0),
("CF_Lkas_FcwOpt", "LKAS11", 0),
("CF_Lkas_HbaOpt", "LKAS11", 0),
("CF_Lkas_FcwSysState", "LKAS11", 0),
("CF_Lkas_FcwCollisionWarning", "LKAS11", 0),
("CF_Lkas_MsgCount", "LKAS11", 0),
("CF_Lkas_FusionState", "LKAS11", 0),
("CF_Lkas_FcwOpt_USM", "LKAS11", 0),
("CF_Lkas_LdwsOpt_USM", "LKAS11", 0)
]
checks = [("LKAS11", 100)]
if CP.sccBus == 2:
signals += [
("MainMode_ACC", "SCC11", 1),
("SCCInfoDisplay", "SCC11", 0),
("AliveCounterACC", "SCC11", 0),
("VSetDis", "SCC11", 30),
("ObjValid", "SCC11", 0),
("DriverAlertDisplay", "SCC11", 0),
("TauGapSet", "SCC11", 4),
("ACC_ObjStatus", "SCC11", 0),
("ACC_ObjLatPos", "SCC11", 0),
("ACC_ObjDist", "SCC11", 150.),
("ACC_ObjRelSpd", "SCC11", 0),
("Navi_SCC_Curve_Status", "SCC11", 0),
("Navi_SCC_Curve_Act", "SCC11", 0),
("Navi_SCC_Camera_Act", "SCC11", 0),
("Navi_SCC_Camera_Status", "SCC11", 2),
("ACCMode", "SCC12", 0),
("CF_VSM_Prefill", "SCC12", 0),
("CF_VSM_DecCmdAct", "SCC12", 0),
("CF_VSM_HBACmd", "SCC12", 0),
("CF_VSM_Warn", "SCC12", 0),
("CF_VSM_Stat", "SCC12", 0),
("CF_VSM_BeltCmd", "SCC12", 0),
("ACCFailInfo", "SCC12", 0),
("StopReq", "SCC12", 0),
("CR_VSM_DecCmd", "SCC12", 0),
("aReqRaw", "SCC12", 0),
("TakeOverReq", "SCC12", 0),
("PreFill", "SCC12", 0),
("aReqValue", "SCC12", 0),
("CF_VSM_ConfMode", "SCC12", 1),
("AEB_Failinfo", "SCC12", 0),
("AEB_Status", "SCC12", 2),
("AEB_CmdAct", "SCC12", 0),
("AEB_StopReq", "SCC12", 0),
("CR_VSM_Alive", "SCC12", 0),
("CR_VSM_ChkSum", "SCC12", 0),
("SCCDrvModeRValue", "SCC13", 2),
("SCC_Equip", "SCC13", 1),
("AebDrvSetStatus", "SCC13", 0),
("JerkUpperLimit", "SCC14", 0),
("JerkLowerLimit", "SCC14", 0),
("SCCMode2", "SCC14", 0),
("ComfortBandUpper", "SCC14", 0),
("ComfortBandLower", "SCC14", 0),
("ACCMode", "SCC14", 0),
("ObjGap", "SCC14", 0),
("CF_VSM_Prefill", "FCA11", 0),
("CF_VSM_HBACmd", "FCA11", 0),
("CF_VSM_Warn", "FCA11", 0),
("CF_VSM_BeltCmd", "FCA11", 0),
("CR_VSM_DecCmd", "FCA11", 0),
("FCA_Status", "FCA11", 2),
("FCA_CmdAct", "FCA11", 0),
("FCA_StopReq", "FCA11", 0),
("FCA_DrvSetStatus", "FCA11", 1),
("CF_VSM_DecCmdAct", "FCA11", 0),
("FCA_Failinfo", "FCA11", 0),
("FCA_RelativeVelocity", "FCA11", 0),
("FCA_TimetoCollision", "FCA11", 2540.),
("CR_FCA_Alive", "FCA11", 0),
("CR_FCA_ChkSum", "FCA11", 0),
("Supplemental_Counter", "FCA11", 0),
("PAINT1_Status", "FCA11", 1),
]
if CP.sccBus == 2:
checks += [
("SCC11", 50),
("SCC12", 50),
]
if CP.fcaBus == 2:
checks += [("FCA11", 50)]
return CANParser(DBC[CP.carFingerprint]["pt"],
signals,
checks,
2,
enforce_checks=False)
def update(self, CC, CS, frame, sm, CP):
if self.CP != CP:
self.CP = CP
self.param_load()
enabled = CC.enabled
actuators = CC.actuators
pcm_cancel_cmd = CC.cruiseControl.cancel
path_plan = sm['pathPlan']
abs_angle_steers = abs(actuators.steerAngle)
self.dRel, self.yRel, self.vRel = SpdController.get_lead(sm)
if self.SC is not None:
self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo)
else:
self.model_speed = self.model_sum = 0
# Steering Torque
param = self.steerParams_torque(CS, abs_angle_steers, path_plan, CC)
new_steer = actuators.steer * param.STEER_MAX
apply_steer = apply_std_steer_torque_limits(new_steer,
self.apply_steer_last,
CS.out.steeringTorque,
param)
self.steer_rate_limited = new_steer != apply_steer
apply_steer_limit = param.STEER_MAX
if self.steer_torque_ratio < 1:
apply_steer_limit = int(self.steer_torque_ratio * param.STEER_MAX)
apply_steer = self.limit_ctrl(apply_steer, apply_steer_limit, 0)
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
lkas_active = enabled and abs(
CS.out.steeringAngle) < 180. #and self.lkas_button
# fix for Genesis hard fault at low speed
#if CS.out.vEgo < 16.666667 and self.car_fingerprint == CAR.GENESIS:
# lkas_active = 0
if not lkas_active:
apply_steer = 0
steer_req = 1 if apply_steer else 0
self.apply_steer_last = apply_steer
sys_warning, sys_state = self.process_hud_alert(lkas_active, CC)
clu11_speed = CS.clu11["CF_Clu_Vanz"]
enabled_speed = 38 if CS.is_set_speed_in_mph else 60
if clu11_speed > enabled_speed or not lkas_active:
enabled_speed = clu11_speed
can_sends = []
if frame == 0: # initialize counts from last received count signals
self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"] + 1
self.lkas11_cnt %= 0x10
can_sends.append(
create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint,
apply_steer, steer_req, CS.lkas11, sys_warning,
sys_state, CC, enabled, 0))
if CS.mdps_bus or CS.scc_bus == 1: # send lkas11 bus 1 if mdps is on bus 1
can_sends.append(
create_lkas11(self.packer, self.lkas11_cnt,
self.car_fingerprint, apply_steer, steer_req,
CS.lkas11, sys_warning, sys_state, CC, enabled,
1))
if CS.mdps_bus: # send clu11 to mdps if it is not on bus 0
can_sends.append(
create_clu11(self.packer, frame, CS.mdps_bus, CS.clu11,
Buttons.NONE, enabled_speed))
#if pcm_cancel_cmd and self.longcontrol:
# can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed))
#else: # send mdps12 to LKAS to prevent LKAS error if no cancel cmd
can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
#str_log1 = 'CV={:5.1f}/{:5.3f} torg:{:5.0f}'.format( self.model_speed, self.model_sum, apply_steer )
str_log1 = 'CV={:5.1f} torg:{:6.1f}'.format(self.model_speed,
apply_steer)
#str_log2 = 'limit={:.0f} tm={:.1f} '.format( apply_steer_limit, self.timer1.sampleTime() )
str_log2 = ' limit={:6.1f}/tm={:3.1f} MAX={:5.1f} UP/DN={:3.1f}/{:3.1f} '.format(
apply_steer_limit, self.timer1.sampleTime(), self.MAX, self.UP,
self.DN)
trace1.printf('{} {}'.format(str_log1, str_log2))
run_speed_ctrl = self.param_OpkrAccelProfile and CS.acc_active and self.SC != None
if not run_speed_ctrl:
str_log2 = 'U={:.0f} LK={:.0f} dir={} steer={:5.0f} '.format(
CS.Mdps_ToiUnavail, CS.lkas_button_on,
self.steer_torque_ratio_dir, CS.out.steeringTorque)
trace1.printf2('{}'.format(str_log2))
if pcm_cancel_cmd and self.CP.longcontrolEnabled:
can_sends.append(
create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL))
elif CS.out.cruiseState.standstill:
# run only first time when the car stopped
if self.last_lead_distance == 0 or not self.param_OpkrAutoResume:
# get the lead distance from the Radar
self.last_lead_distance = CS.lead_distance
self.resume_cnt = 0
# when lead car starts moving, create 6 RES msgs
elif CS.lead_distance != self.last_lead_distance < CS.lead_distance > 4.8 and (
frame - self.last_resume_frame) > 5:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.RES_ACCEL, clu11_speed))
self.resume_cnt += 1
# interval after 10 msgs
if self.resume_cnt > 10:
self.last_resume_frame = frame
self.resume_cnt = 0
self.clu11_cnt = 0
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
elif run_speed_ctrl and self.SC != None:
is_sc_run = self.SC.update(CS, sm, self)
if is_sc_run:
can_sends.append(
create_clu11(self.packer, self.resume_cnt, CS.scc_bus,
CS.clu11, self.SC.btn_type,
self.SC.sc_clu_speed))
self.resume_cnt += 1
else:
self.resume_cnt = 0
str1 = 'run={} cruise_set_mode={} kph={:.1f}/{:.1f} DO={:.0f}/{:.0f} '.format(
is_sc_run, self.SC.cruise_set_mode,
self.SC.cruise_set_speed_kph, CS.VSetDis, CS.driverOverride,
CS.cruise_buttons)
str2 = 'btn_type={:.0f} speed={:.1f} cnt={:.0f}'.format(
self.SC.btn_type, self.SC.sc_clu_speed, self.resume_cnt)
str_log = str1 + str2
self.traceCC.add(str_log)
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in [
CAR.PALISADE, CAR.SELTOS
]:
can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
# counter inc
self.lkas11_cnt += 1
return can_sends
def update(self, CC, CS, frame, sm, CP):
if self.CP != CP:
self.CP = CP
self.param_load()
enabled = CC.enabled
actuators = CC.actuators
pcm_cancel_cmd = CC.cruiseControl.cancel
self.dRel, self.yRel, self.vRel = SpdController.get_lead(sm)
if self.SC is not None:
self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo)
else:
self.model_speed = self.model_sum = 0
# Steering Torque
new_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = apply_std_steer_torque_limits(new_steer,
self.apply_steer_last,
CS.out.steeringTorque,
SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
lkas_active = enabled and abs(CS.out.steeringAngle) < 180. #90
if ((CS.out.leftBlinker and not CS.out.rightBlinker) or
(CS.out.rightBlinker
and not CS.out.leftBlinker)) and CS.out.vEgo < 60 * CV.KPH_TO_MS:
self.lanechange_manual_timer = 10
if CS.out.leftBlinker and CS.out.rightBlinker:
self.emergency_manual_timer = 10
if abs(CS.out.steeringTorque) > 409: #360: #180
self.driver_steering_torque_above_timer = 100
if self.lanechange_manual_timer or self.driver_steering_torque_above_timer:
lkas_active = 0
if self.lanechange_manual_timer > 0:
self.lanechange_manual_timer -= 1
if self.emergency_manual_timer > 0:
self.emergency_manual_timer -= 1
if self.driver_steering_torque_above_timer > 0:
self.driver_steering_torque_above_timer -= 1
if not lkas_active:
apply_steer = 0
steer_req = 1 if apply_steer else 0
self.apply_steer_last = apply_steer
sys_warning, sys_state = self.process_hud_alert(lkas_active, CC)
clu11_speed = CS.clu11["CF_Clu_Vanz"]
enabled_speed = 38 if CS.is_set_speed_in_mph else 55
if clu11_speed > enabled_speed:
enabled_speed = clu11_speed
can_sends = []
if frame == 0: # initialize counts from last received count signals
self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"] + 1
self.lkas11_cnt %= 0x10
can_sends.append(
create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint,
apply_steer, steer_req, CS.lkas11, sys_warning,
sys_state, CC, enabled, 0))
if CS.mdps_bus or CS.scc_bus == 1: # send lkas11 bus 1 if mdps is on bus 1
can_sends.append(
create_lkas11(self.packer, self.lkas11_cnt,
self.car_fingerprint, apply_steer, steer_req,
CS.lkas11, sys_warning, sys_state, CC, enabled,
1))
if CS.mdps_bus: # send clu11 to mdps if it is not on bus 0
#if frame % 2 and CS.mdps_bus == 1: # send clu11 to mdps if it is not on bus 0
can_sends.append(
create_clu11(self.packer, frame, CS.mdps_bus, CS.clu11,
Buttons.NONE, enabled_speed))
#if CS.mdps_bus:
can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
str_log1 = '곡률={:04.1f}/{:=+06.3f} 토크={:=+04.0f}/{:=+04.0f}'.format(
self.model_speed, self.model_sum, new_steer, CS.out.steeringTorque)
str_log2 = '프레임율={:03.0f}'.format(self.timer1.sampleTime())
trace1.printf('{} {}'.format(str_log1, str_log2))
if CS.out.cruiseState.modeSel == 0 and self.mode_change_switch == 4:
self.mode_change_timer = 50
self.mode_change_switch = 0
elif CS.out.cruiseState.modeSel == 1 and self.mode_change_switch == 0:
self.mode_change_timer = 50
self.mode_change_switch = 1
elif CS.out.cruiseState.modeSel == 2 and self.mode_change_switch == 1:
self.mode_change_timer = 50
self.mode_change_switch = 2
elif CS.out.cruiseState.modeSel == 3 and self.mode_change_switch == 2:
self.mode_change_timer = 50
self.mode_change_switch = 3
elif CS.out.cruiseState.modeSel == 4 and self.mode_change_switch == 3:
self.mode_change_timer = 50
self.mode_change_switch = 4
if self.mode_change_timer > 0:
self.mode_change_timer -= 1
run_speed_ctrl = self.param_OpkrAccelProfile and CS.acc_active and self.SC != None
if not run_speed_ctrl:
if CS.out.cruiseState.modeSel == 0:
self.steer_mode = "오파모드"
elif CS.out.cruiseState.modeSel == 1:
self.steer_mode = "차간+커브"
elif CS.out.cruiseState.modeSel == 2:
self.steer_mode = "차간ONLY"
elif CS.out.cruiseState.modeSel == 3:
self.steer_mode = "자동RES"
elif CS.out.cruiseState.modeSel == 4:
self.steer_mode = "순정모드"
if CS.out.steerWarning == 0:
self.mdps_status = "정상"
elif CS.out.steerWarning == 1:
self.mdps_status = "오류"
if CS.lkas_button_on == 0:
self.lkas_switch = "OFF"
elif CS.lkas_button_on == 1:
self.lkas_switch = "ON"
else:
self.lkas_switch = "-"
str_log2 = '주행모드={:s} MDPS상태={:s} LKAS버튼={:s}'.format(
self.steer_mode, self.mdps_status, self.lkas_switch)
trace1.printf2('{}'.format(str_log2))
#print( 'st={} cmd={} long={} steer={} req={}'.format(CS.out.cruiseState.standstill, pcm_cancel_cmd, self.CP.openpilotLongitudinalControl, apply_steer, steer_req ) )
if pcm_cancel_cmd and self.CP.openpilotLongitudinalControl:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.CANCEL, clu11_speed))
elif CS.out.cruiseState.standstill and not self.car_fingerprint == CAR.NIRO_EV:
# run only first time when the car stopped
if self.last_lead_distance == 0 or not self.param_OpkrAutoResume:
# get the lead distance from the Radar
self.last_lead_distance = CS.lead_distance
self.resume_cnt = 0
# when lead car starts moving, create 6 RES msgs
elif CS.lead_distance != self.last_lead_distance and (
frame - self.last_resume_frame) > 5:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.RES_ACCEL, clu11_speed))
self.resume_cnt += 1
# interval after 6 msgs
if self.resume_cnt > 5:
self.last_resume_frame = frame
self.resume_cnt = 0
elif CS.out.cruiseState.standstill and self.car_fingerprint == CAR.NIRO_EV:
if CS.lead_distance > 3.7 and (
frame - self.last_resume_frame
) * DT_CTRL > 0.2 and self.param_OpkrAutoResume:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.RES_ACCEL, clu11_speed))
self.last_resume_frame = frame
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
elif run_speed_ctrl and self.SC != None:
is_sc_run = self.SC.update(CS, sm, self)
if is_sc_run:
can_sends.append(
create_clu11(self.packer, self.resume_cnt, CS.scc_bus,
CS.clu11, self.SC.btn_type,
self.SC.sc_clu_speed))
self.resume_cnt += 1
else:
self.resume_cnt = 0
if CS.out.cruiseState.modeSel == 3:
if CS.out.brakeLights and CS.VSetDis > 30:
self.res_cnt = 0
self.res_delay = 50
elif self.res_delay:
self.res_delay -= 1
elif not self.res_delay and self.res_cnt < 0 and CS.VSetDis > 30 and CS.out.vEgo > 30 * CV.KPH_TO_MS:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.CANCEL, clu11_speed))
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.RES_ACCEL, clu11_speed))
self.res_cnt += 1
else:
self.res_cnt = 7
self.res_delay = 0
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in FEATURES["send_lfa_mfa"]:
can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
self.lkas11_cnt += 1
return can_sends
def update(self, CC, CS, frame, sm, CP ):
if self.CP != CP:
self.CP = CP
self.param_load()
enabled = CC.enabled
actuators = CC.actuators
pcm_cancel_cmd = CC.cruiseControl.cancel
self.dRel, self.yRel, self.vRel = SpdController.get_lead( sm )
if self.SC is not None:
self.model_speed, self.model_sum = self.SC.calc_va( sm, CS.out.vEgo )
else:
self.model_speed = self.model_sum = 0
# Steering Torque
if self.param_OpkrEnableLearner:
new_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
else:
path_plan = sm['pathPlan']
self.cV_tune( CS.out.vEgo, self.model_speed )
param = SteerLimitParams()
param.STEER_MAX = min( param.STEER_MAX, self.MAX )
param.STEER_DELTA_UP = min( param.STEER_DELTA_UP, self.UP )
param.STEER_DELTA_DOWN = min( param.STEER_DELTA_DOWN, self.DN )
new_steer = actuators.steer * param.STEER_MAX
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, param)
self.steer_rate_limited = new_steer != apply_steer
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
lkas_active = enabled and abs(CS.out.steeringAngle) < 90. #TenesiDel -> and self.lkas_button_on
# fix for Genesis hard fault at low speed 테네시 추가
if CS.out.vEgo < 60 * CV.KPH_TO_MS and self.car_fingerprint == CAR.GENESIS and not CS.mdps_bus:
lkas_active = False
# if (( CS.out.leftBlinker and not CS.out.rightBlinker) or ( CS.out.rightBlinker and not CS.out.leftBlinker)) and CS.out.vEgo > 60 * CV.KPH_TO_MS: # 깜빡이 작동시에도 상히조향 유지 수정해보기
# self.lanechange_manual_timer = 10
# if CS.out.leftBlinker and CS.out.rightBlinker:
# self.emergency_manual_timer = 10
# if abs(CS.out.steeringTorque) > self.driver_steering_torque_above and CS.out.vEgo > 60: # 깜빡이 작동시에도 상히조향 유지 수정해보기
# self.driver_steering_torque_above_timer = 30
# if self.lanechange_manual_timer or self.driver_steering_torque_above_timer:
# lkas_active = 0
# if self.lanechange_manual_timer > 0:
# self.lanechange_manual_timer -= 1
# if self.emergency_manual_timer > 0:
# self.emergency_manual_timer -= 1
# if self.driver_steering_torque_above_timer > 0:
# self.driver_steering_torque_above_timer -= 1
# Disable steering while turning blinker on and speed below 60 kph #201011 상시조향 작업 작동성공
if CS.out.leftBlinker or CS.out.rightBlinker:
if self.car_fingerprint not in [CAR.K5, CAR.K5_HEV]: # 테네시 추가 OPTIMA -> K5
self.turning_signal_timer = 100 # Disable for 1.0 Seconds after blinker turned off
elif CS.left_blinker_flash or CS.right_blinker_flash: # Optima has blinker flash signal only
self.turning_signal_timer = 100
if self.turning_signal_timer and CS.out.vEgo > 70 * CV.KPH_TO_MS: # TenesiADD Blinker tune 시속60미만에서는 상시조향
lkas_active = 0
if self.turning_signal_timer: # TenesiADD
self.turning_signal_timer -= 1 #201011 상시조향 작업 작동성공
if not lkas_active:
apply_steer = 0
steer_req = 1 if apply_steer else 0
self.apply_steer_last = apply_steer
sys_warning, sys_state = self.process_hud_alert( lkas_active, CC )
clu11_speed = CS.clu11["CF_Clu_Vanz"]
enabled_speed = 38 if CS.is_set_speed_in_mph else 55
if clu11_speed > enabled_speed:
enabled_speed = clu11_speed
can_sends = []
if frame == 0: # initialize counts from last received count signals
self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"] + 1
self.lkas11_cnt %= 0x10
can_sends.append(create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint, apply_steer, steer_req,
CS.lkas11, sys_warning, sys_state, CC, enabled, 0 ))
if CS.mdps_bus or CS.scc_bus == 1: # send lkas11 bus 1 if mdps is on bus 1
can_sends.append(create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint, apply_steer, steer_req,
CS.lkas11, sys_warning, sys_state, CC, enabled, 1 ))
if CS.mdps_bus: # send clu11 to mdps if it is not on bus 0
#if frame % 2 and CS.mdps_bus == 1: # send clu11 to mdps if it is not on bus 0
can_sends.append(create_clu11(self.packer, frame, CS.mdps_bus, CS.clu11, Buttons.NONE, enabled_speed))
#if CS.mdps_bus:
can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
str_log1 = '곡률={:05.1f}/{:=+06.3f} 토크={:=+04.0f}/{:=+04.0f}'.format( self.model_speed, self.model_sum, new_steer, CS.out.steeringTorque )
if self.param_OpkrEnableLearner:
str_log2 = '프레임율={:03.0f} STMAX={:03.0f}'.format( self.timer1.sampleTime(), SteerLimitParams.STEER_MAX, )
else:
str_log2 = '프레임율={:03.0f} ST={:03.0f}/{:01.0f}/{:01.0f} SR={:05.2f}'.format( self.timer1.sampleTime(), self.MAX, self.UP, self.DN, path_plan.steerRatio )
trace1.printf( '{} {}'.format( str_log1, str_log2 ) )
if CS.out.cruiseState.modeSel == 0 and self.mode_change_switch == 4:
self.mode_change_timer = 50
self.mode_change_switch = 0
elif CS.out.cruiseState.modeSel == 1 and self.mode_change_switch == 0:
self.mode_change_timer = 50
self.mode_change_switch = 1
elif CS.out.cruiseState.modeSel == 2 and self.mode_change_switch == 1:
self.mode_change_timer = 50
self.mode_change_switch = 2
elif CS.out.cruiseState.modeSel == 3 and self.mode_change_switch == 2:
self.mode_change_timer = 50
self.mode_change_switch = 3
elif CS.out.cruiseState.modeSel == 4 and self.mode_change_switch == 3:
self.mode_change_timer = 50
self.mode_change_switch = 4
if self.mode_change_timer > 0:
self.mode_change_timer -= 1
run_speed_ctrl = self.param_OpkrAccelProfile and CS.acc_active and self.SC != None and (CS.out.cruiseState.modeSel == 1 or CS.out.cruiseState.modeSel == 2 or CS.out.cruiseState.modeSel == 3)
if not run_speed_ctrl:
if CS.out.cruiseState.modeSel == 0:
self.steer_mode = "오파모드"
elif CS.out.cruiseState.modeSel == 1:
self.steer_mode = "차간+커브"
elif CS.out.cruiseState.modeSel == 2:
self.steer_mode = "차간ONLY"
elif CS.out.cruiseState.modeSel == 3:
self.steer_mode = "자동RES"
elif CS.out.cruiseState.modeSel == 4:
self.steer_mode = "순정모드"
if CS.out.steerWarning == 0:
self.mdps_status = "정상"
elif CS.out.steerWarning == 1:
self.mdps_status = "오류"
if CS.lkas_button_on == 0:
self.lkas_switch = "OFF"
elif CS.lkas_button_on == 1:
self.lkas_switch = "ON"
else:
self.lkas_switch = "-"
if CS.out.cruiseState.modeSel == 3:
str_log2 = '주행모드={:s} MDPS상태={:s} LKAS버튼={:s} AUTORES=(VS:{:03.0f}/CN:{:01.0f}/RD:{:03.0f}/BK:{})'.format( self.steer_mode, self.mdps_status, self.lkas_switch, CS.VSetDis, self.res_cnt, self.res_delay, CS.out.brakeLights )
else:
str_log2 = '주행모드={:s} MDPS상태={:s} LKAS버튼={:s}'.format( self.steer_mode, self.mdps_status, self.lkas_switch )
trace1.printf2( '{}'.format( str_log2 ) )
#print( 'st={} cmd={} long={} steer={} req={}'.format(CS.out.cruiseState.standstill, pcm_cancel_cmd, self.CP.openpilotLongitudinalControl, apply_steer, steer_req ) )
if pcm_cancel_cmd and self.CP.openpilotLongitudinalControl:
can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed))
elif CS.out.cruiseState.standstill and not self.car_fingerprint == CAR.NIRO_EV:
# run only first time when the car stopped
if self.last_lead_distance == 0 or not self.param_OpkrAutoResume:
# get the lead distance from the Radar
self.last_lead_distance = CS.lead_distance
self.resume_cnt = 0
# when lead car starts moving, create 6 RES msgs
elif CS.lead_distance != self.last_lead_distance and (frame - self.last_resume_frame) > 5:
can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed))
self.resume_cnt += 1
# interval after 6 msgs
if self.resume_cnt > 5:
self.last_resume_frame = frame
self.resume_cnt = 0
elif CS.out.cruiseState.standstill and self.car_fingerprint == CAR.NIRO_EV:
if CS.lead_distance > 3.7 and (frame - self.last_resume_frame)*DT_CTRL > 0.2 and self.param_OpkrAutoResume:
can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed))
self.last_resume_frame = frame
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
elif run_speed_ctrl and self.SC != None:
is_sc_run = self.SC.update( CS, sm, self )
if is_sc_run:
can_sends.append(create_clu11(self.packer, self.resume_cnt, CS.scc_bus, CS.clu11, self.SC.btn_type, self.SC.sc_clu_speed ))
self.resume_cnt += 1
else:
self.resume_cnt = 0
if CS.out.cruiseState.modeSel == 3:
if CS.out.brakeLights and CS.VSetDis > 30:
self.res_cnt = 0
self.res_delay = 50
elif self.res_delay:
self.res_delay -= 1
elif not self.res_delay and self.res_cnt < 6 and CS.VSetDis > 30 and CS.out.vEgo > 30 * CV.KPH_TO_MS:
if self.res_cnt < 1:
can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed))
can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed))
self.res_cnt += 1
else:
self.res_cnt = 7
self.res_delay = 0
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in FEATURES["send_lfa_mfa"]:
can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
self.lkas11_cnt += 1
return can_sends
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
self.cruise_main_button = False
self.cruise_buttons = False
self.lkas_button_on = False
self.lkas_error = False
self.prev_cruise_main_button = False
self.prev_cruise_buttons = False
self.main_on = False
self.acc_active = False
self.cruiseState_modeSel = 0
self.Mdps_ToiUnavail = 0
self.left_blinker_flash = 0
self.right_blinker_flash = 0
self.steerWarning = 0
self.TSigLHSw = 0
self.TSigRHSw = 0
self.driverAcc_time = 0
self.gearShifter = 0
self.leftBlindspot_time = 0
self.rightBlindspot_time = 0
self.SC = SpdController()
def update(self, cp, cp_cam):
self.prev_cruise_main_button = self.cruise_main_button
self.prev_cruise_buttons = self.cruise_buttons
ret = car.CarState.new_message()
ret.doorOpen = any([
cp.vl["CGW1"]['CF_Gway_DrvDrSw'], cp.vl["CGW1"]['CF_Gway_AstDrSw'],
cp.vl["CGW2"]['CF_Gway_RLDrSw'], cp.vl["CGW2"]['CF_Gway_RRDrSw']
])
ret.seatbeltUnlatched = cp.vl["CGW1"]['CF_Gway_DrvSeatBeltSw'] == 0
ret.wheelSpeeds.fl = cp.vl["WHL_SPD11"]['WHL_SPD_FL'] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr +
ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.1
ret.steeringAngle = cp.vl["SAS11"][
'SAS_Angle'] - self.CP.lateralsRatom.steerOffset
ret.steeringRate = cp.vl["SAS11"]['SAS_Speed']
ret.yawRate = cp.vl["ESP12"]['YAW_RATE']
ret.steeringTorque = cp.vl["MDPS12"]['CR_Mdps_StrColTq']
ret.steeringTorqueEps = cp.vl["MDPS12"]['CR_Mdps_OutTq']
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
ret.leftBlinker, ret.rightBlinker = self.update_blinker(cp)
self.lead_distance = cp.vl["SCC11"]['ACC_ObjDist']
lead_objspd = cp.vl["SCC11"]['ACC_ObjRelSpd']
self.lead_objspd = lead_objspd * CV.MS_TO_KPH
self.VSetDis = cp.vl["SCC11"]['VSetDis']
self.Mdps_ToiUnavail = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail']
self.clu_Vanz = cp.vl["CLU11"]["CF_Clu_Vanz"]
ret.vEgo = self.clu_Vanz * CV.KPH_TO_MS
steerWarning = False
if ret.vEgo < 5 or not self.Mdps_ToiUnavail:
self.steerWarning = 0
elif self.steerWarning >= 2:
steerWarning = True
else:
self.steerWarning += 1
ret.steerWarning = steerWarning
# cruise state
#ret.cruiseState.available = True
#ret.cruiseState.enabled = cp.vl["SCC12"]['ACCMode'] != 0
self.main_on = (cp.vl["SCC11"]["MainMode_ACC"] != 0)
self.acc_active = (cp.vl["SCC12"]['ACCMode'] != 0)
self.update_atom(cp, cp_cam)
ret.cruiseState.available = self.main_on and self.cruiseState_modeSel != 3
ret.cruiseState.enabled = ret.cruiseState.available #and self.gearShifter == GearShifter.drive
ret.cruiseState.standstill = cp.vl["SCC11"]['SCCInfoDisplay'] == 4.
# most HKG cars has no long control, it is safer and easier to engage by main on
self.cruiseState_modeSel, speed_kph = self.SC.update_cruiseSW(self)
ret.cruiseState.modeSel = self.cruiseState_modeSel
ret.cruiseState.cruiseSwState = self.cruise_buttons
if self.acc_active:
is_set_speed_in_mph = int(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"])
speed_conv = CV.MPH_TO_MS if is_set_speed_in_mph else CV.KPH_TO_MS
ret.cruiseState.speed = speed_kph * speed_conv
else:
ret.cruiseState.speed = 0
# TODO: Find brake pressure
ret.brake = 0
ret.brakePressed = cp.vl["TCS13"]['DriverBraking'] != 0
# TODO: Check this
ret.brakeLights = bool(cp.vl["TCS13"]['BrakeLight']
or ret.brakePressed)
#TODO: find pedal signal for EV/HYBRID Cars
if self.CP.carFingerprint in EV_HYBRID:
ret.gas = cp.vl["E_EMS11"]['Accel_Pedal_Pos'] / 256.
ret.gasPressed = ret.gas > 0
else:
ret.gas = cp.vl["EMS12"]['PV_AV_CAN'] / 100
ret.gasPressed = bool(cp.vl["EMS16"]["CF_Ems_AclAct"])
# TODO: refactor gear parsing in function
self.gearShifter = self.get_gearShifter(cp)
ret.gearShifter = self.gearShifter
# Blind Spot Detection and Lane Change Assist signals
ret.leftBlindspot, ret.rightBlindspot = self.get_Blindspot(cp)
# save the entire LKAS11 and CLU11
self.lkas11 = cp_cam.vl["LKAS11"]
self.clu11 = cp.vl["CLU11"]
self.mdps12 = cp.vl["MDPS12"]
self.park_brake = cp.vl["CGW1"]['CF_Gway_ParkBrakeSw']
self.steer_state = cp.vl["MDPS12"][
'CF_Mdps_ToiActive'] # 0 NOT ACTIVE, 1 ACTIVE
self.lead_distance = cp.vl["SCC11"]['ACC_ObjDist']
return ret
def update_blinker(self, cp):
self.TSigLHSw = cp.vl["CGW1"]['CF_Gway_TSigLHSw']
self.TSigRHSw = cp.vl["CGW1"]['CF_Gway_TSigRHSw']
leftBlinker = cp.vl["CGW1"]['CF_Gway_TurnSigLh'] != 0
rightBlinker = cp.vl["CGW1"]['CF_Gway_TurnSigRh'] != 0
if leftBlinker:
self.left_blinker_flash = 300
elif self.left_blinker_flash:
self.left_blinker_flash -= 1
if rightBlinker:
self.right_blinker_flash = 300
elif self.right_blinker_flash:
self.right_blinker_flash -= 1
leftBlinker = self.left_blinker_flash != 0
rightBlinker = self.right_blinker_flash != 0
return leftBlinker, rightBlinker
def update_atom(self, cp, cp_cam):
# atom append
self.driverOverride = cp.vl["TCS13"][
"DriverOverride"] # 1 Acc, 2 bracking, 0 Normal
self.cruise_main_button = cp.vl["CLU11"]["CF_Clu_CruiseSwMain"]
self.cruise_buttons = cp.vl["CLU11"][
"CF_Clu_CruiseSwState"] # clu_CruiseSwState
self.Lkas_LdwsSysState = cp_cam.vl["LKAS11"]["CF_Lkas_LdwsSysState"]
self.lkas_error = self.Lkas_LdwsSysState == 7
if not self.lkas_error:
self.lkas_button_on = self.Lkas_LdwsSysState
if self.driverOverride == 1:
self.driverAcc_time = 100
elif self.driverAcc_time:
self.driverAcc_time -= 1
def get_gearShifter(self, cp):
gearShifter = GearShifter.unknown
# TODO: refactor gear parsing in function
# Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection, as this seems to be standard over all cars, but is not the preferred method.
if self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
if cp.vl["CLU15"]["CF_Clu_InhibitD"] == 1:
gearShifter = GearShifter.drive
elif cp.vl["CLU15"]["CF_Clu_InhibitN"] == 1:
gearShifter = GearShifter.neutral
elif cp.vl["CLU15"]["CF_Clu_InhibitP"] == 1:
gearShifter = GearShifter.park
elif cp.vl["CLU15"]["CF_Clu_InhibitR"] == 1:
gearShifter = GearShifter.reverse
# Gear Selecton via TCU12
elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
gear = cp.vl["TCU12"]["CUR_GR"]
if gear == 0:
gearShifter = GearShifter.park
elif gear == 14:
gearShifter = GearShifter.reverse
elif gear > 0 and gear < 9: # unaware of anything over 8 currently
gearShifter = GearShifter.drive
# Gear Selecton - This is only compatible with optima hybrid 2017
elif self.CP.carFingerprint in FEATURES["use_elect_gears"]:
gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"]
if gear in (5, 8): # 5: D, 8: sport mode
gearShifter = GearShifter.drive
elif gear == 6:
gearShifter = GearShifter.neutral
elif gear == 0:
gearShifter = GearShifter.park
elif gear == 7:
gearShifter = GearShifter.reverse
# Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with
else:
gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
if gear in (5, 8): # 5: D, 8: sport mode
gearShifter = GearShifter.drive
elif gear == 6:
gearShifter = GearShifter.neutral
elif gear == 0:
gearShifter = GearShifter.park
elif gear == 7:
gearShifter = GearShifter.reverse
return gearShifter
def get_Blindspot(self, cp):
if cp.vl["LCA11"]['CF_Lca_IndLeft'] != 0:
self.leftBlindspot_time = 200
elif self.leftBlindspot_time:
self.leftBlindspot_time -= 1
if cp.vl["LCA11"]['CF_Lca_IndRight'] != 0:
self.rightBlindspot_time = 200
elif self.rightBlindspot_time:
self.rightBlindspot_time -= 1
leftBlindspot = self.leftBlindspot_time != 0
rightBlindspot = self.rightBlindspot_time != 0
return leftBlindspot, rightBlindspot
@staticmethod
def get_parser_gears(CP, signals, checks):
if CP.carFingerprint in FEATURES["use_cluster_gears"]:
signals += [
("CF_Clu_InhibitD", "CLU15", 0),
("CF_Clu_InhibitP", "CLU15", 0),
("CF_Clu_InhibitN", "CLU15", 0),
("CF_Clu_InhibitR", "CLU15", 0),
]
checks += [("CLU15", 5)]
elif CP.carFingerprint in FEATURES["use_tcu_gears"]:
signals += [("CUR_GR", "TCU12", 0)]
checks += [("TCU12", 100)]
elif CP.carFingerprint in FEATURES["use_elect_gears"]:
signals += [("Elect_Gear_Shifter", "ELECT_GEAR", 0)]
checks += [("ELECT_GEAR", 20)]
else:
signals += [("CF_Lvr_Gear", "LVR12", 0)]
checks += [("LVR12", 100)]
return signals, checks
@staticmethod
def get_parser_ev_hybrid(CP, signals, checks):
if CP.carFingerprint in EV_HYBRID:
signals += [
("Accel_Pedal_Pos", "E_EMS11", 0),
]
checks += [
("E_EMS11", 50),
]
else:
signals += [
("PV_AV_CAN", "EMS12", 0),
("CF_Ems_AclAct", "EMS16", 0),
]
checks += [
("EMS12", 100),
("EMS16", 100), # 608
]
return signals, checks
@staticmethod
def get_can_parser(CP):
signals = [
# sig_name, sig_address, default
("WHL_SPD_FL", "WHL_SPD11", 0),
("WHL_SPD_FR", "WHL_SPD11", 0),
("WHL_SPD_RL", "WHL_SPD11", 0),
("WHL_SPD_RR", "WHL_SPD11", 0),
("YAW_RATE", "ESP12", 0),
("CF_Gway_DrvSeatBeltInd", "CGW4", 1),
("CF_Gway_DrvSeatBeltSw", "CGW1", 0),
("CF_Gway_DrvDrSw", "CGW1", 0), # Driver Door
("CF_Gway_AstDrSw", "CGW1", 0), # Passenger door
("CF_Gway_RLDrSw", "CGW2", 0), # Rear reft door
("CF_Gway_RRDrSw", "CGW2", 0), # Rear right door
("CF_Gway_TSigLHSw", "CGW1", 0),
("CF_Gway_TurnSigLh", "CGW1", 0),
("CF_Gway_TSigRHSw", "CGW1", 0),
("CF_Gway_TurnSigRh", "CGW1", 0),
("CF_Gway_ParkBrakeSw", "CGW1", 0),
("CYL_PRES", "ESP12", 0),
("CF_Clu_CruiseSwState", "CLU11", 0),
("CF_Clu_CruiseSwMain", "CLU11", 0),
("CF_Clu_SldMainSW", "CLU11", 0),
("CF_Clu_ParityBit1", "CLU11", 0),
("CF_Clu_VanzDecimal", "CLU11", 0),
("CF_Clu_Vanz", "CLU11", 0),
("CF_Clu_SPEED_UNIT", "CLU11", 0),
("CF_Clu_DetentOut", "CLU11", 0),
("CF_Clu_RheostatLevel", "CLU11", 0),
("CF_Clu_CluInfo", "CLU11", 0),
("CF_Clu_AmpInfo", "CLU11", 0),
("CF_Clu_AliveCnt1", "CLU11", 0),
("ACCEnable", "TCS13", 0),
("BrakeLight", "TCS13", 0),
("DriverBraking", "TCS13", 0),
("DriverOverride", "TCS13", 0),
("ESC_Off_Step", "TCS15", 0),
("CF_Lvr_GearInf", "LVR11",
0), # Transmission Gear (0 = N or P, 1-8 = Fwd, 14 = Rev)
("CF_Lca_IndLeft", "LCA11", 0),
("CF_Lca_IndRight", "LCA11", 0),
("CR_Mdps_StrColTq", "MDPS12", 0),
("CF_Mdps_Def", "MDPS12", 0), #
("CF_Mdps_ToiActive", "MDPS12", 0),
("CF_Mdps_ToiUnavail", "MDPS12", 0),
("CF_Mdps_MsgCount2", "MDPS12", 0), #
("CF_Mdps_Chksum2", "MDPS12", 0), #
("CF_Mdps_ToiFlt", "MDPS12", 0), #
("CF_Mdps_SErr", "MDPS12", 0), #
("CR_Mdps_StrTq", "MDPS12", 0), #
("CF_Mdps_FailStat", "MDPS12", 0),
("CR_Mdps_OutTq", "MDPS12", 0),
("SAS_Angle", "SAS11", 0),
("SAS_Speed", "SAS11", 0),
("MainMode_ACC", "SCC11", 0),
("VSetDis", "SCC11", 0),
("SCCInfoDisplay", "SCC11", 0),
("ACC_ObjDist", "SCC11", 0),
("ACC_ObjRelSpd", "SCC11", 0),
("ACCMode", "SCC12", 1),
]
checks = [
# address, frequency
("MDPS12", 50), # 593
("TCS13", 50), # 916
("TCS15", 10),
("CLU11", 50),
("ESP12", 100),
("CGW1", 10),
("CGW4", 5),
("WHL_SPD11", 50), # 902
("SAS11", 100),
("SCC11", 50),
("SCC12", 50), # 1057
("LCA11", 50),
]
signals, checks = CarState.get_parser_ev_hybrid(CP, signals, checks)
signals, checks = CarState.get_parser_gears(CP, signals, checks)
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
@staticmethod
def get_cam_can_parser(CP):
signals = [
# sig_name, sig_address, default
("CF_Lkas_Bca_R", "LKAS11", 0),
("CF_Lkas_LdwsSysState", "LKAS11", 0),
("CF_Lkas_SysWarning", "LKAS11", 0),
("CF_Lkas_LdwsLHWarning", "LKAS11", 0),
("CF_Lkas_LdwsRHWarning", "LKAS11", 0),
("CF_Lkas_HbaLamp", "LKAS11", 0),
("CF_Lkas_FcwBasReq", "LKAS11", 0),
("CF_Lkas_ToiFlt", "LKAS11", 0), # append
("CF_Lkas_HbaSysState", "LKAS11", 0),
("CF_Lkas_FcwOpt", "LKAS11", 0),
("CF_Lkas_HbaOpt", "LKAS11", 0),
("CF_Lkas_FcwSysState", "LKAS11", 0),
("CF_Lkas_FcwCollisionWarning", "LKAS11", 0),
("CF_Lkas_MsgCount", "LKAS11", 0), # append
("CF_Lkas_FusionState", "LKAS11", 0),
("CF_Lkas_FcwOpt_USM", "LKAS11", 0),
("CF_Lkas_LdwsOpt_USM", "LKAS11", 0)
]
checks = []
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
def update(self, CC, CS, frame, sm, CP):
if self.CP != CP:
self.CP = CP
enabled = CC.enabled
actuators = CC.actuators
pcm_cancel_cmd = CC.cruiseControl.cancel
self.dRel, self.yRel, self.vRel = SpdController.get_lead(sm)
self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo)
# Steering Torque
new_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = apply_std_steer_torque_limits(new_steer,
self.apply_steer_last,
CS.out.steeringTorque,
SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
lkas_active = enabled and abs(CS.out.steeringAngle) < 90.
if ((CS.out.leftBlinker and not CS.out.rightBlinker) or
(CS.out.rightBlinker
and not CS.out.leftBlinker)) and CS.out.vEgo <= 59 * CV.KPH_TO_MS:
self.lanechange_manual_timer = 10
if CS.out.leftBlinker and CS.out.rightBlinker:
self.emergency_manual_timer = 10
if abs(CS.out.steeringTorque) > 200:
self.driver_steering_torque_above_timer = 15
if self.lanechange_manual_timer or self.driver_steering_torque_above_timer:
lkas_active = 0
if self.lanechange_manual_timer > 0:
self.lanechange_manual_timer -= 1
if self.emergency_manual_timer > 0:
self.emergency_manual_timer -= 1
if self.driver_steering_torque_above_timer > 0:
self.driver_steering_torque_above_timer -= 1
if not lkas_active:
apply_steer = 0
steer_req = 1 if apply_steer else 0
self.apply_steer_last = apply_steer
sys_warning, sys_state = self.process_hud_alert(lkas_active, CC)
clu11_speed = CS.clu11["CF_Clu_Vanz"]
enabled_speed = 38 if CS.is_set_speed_in_mph else 55
if clu11_speed > enabled_speed:
enabled_speed = clu11_speed
can_sends = []
if frame == 0: # initialize counts from last received count signals
self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"] + 1
self.lkas11_cnt %= 0x10
can_sends.append(
create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint,
apply_steer, steer_req, CS.lkas11, sys_warning,
sys_state, CC, enabled, 0))
if CS.mdps_bus or CS.scc_bus == 1: # send lkas11 bus 1 if mdps is on bus 1
can_sends.append(
create_lkas11(self.packer, self.lkas11_cnt,
self.car_fingerprint, apply_steer, steer_req,
CS.lkas11, sys_warning, sys_state, CC, enabled,
1))
if CS.mdps_bus: # send clu11 to mdps if it is not on bus 0
#if frame % 2 and CS.mdps_bus == 1: # send clu11 to mdps if it is not on bus 0
can_sends.append(
create_clu11(self.packer, frame, CS.mdps_bus, CS.clu11,
Buttons.NONE, enabled_speed))
#if CS.mdps_bus:
can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
str_log1 = '곡률={:05.1f}'.format(self.model_speed)
str_log2 = '프레임율={:03.0f} TPMS=FL:{:04.1f}/FR:{:04.1f}/RL:{:04.1f}/RR:{:04.1f}'.format(
self.timer1.sampleTime(), CS.tpmsPressureFl, CS.tpmsPressureFr,
CS.tpmsPressureRl, CS.tpmsPressureRr)
trace1.printf('{} {}'.format(str_log1, str_log2))
if CS.out.cruiseState.modeSel == 0 and self.mode_change_switch == 4:
self.mode_change_timer = 50
self.mode_change_switch = 0
elif CS.out.cruiseState.modeSel == 1 and self.mode_change_switch == 0:
self.mode_change_timer = 50
self.mode_change_switch = 1
elif CS.out.cruiseState.modeSel == 2 and self.mode_change_switch == 1:
self.mode_change_timer = 50
self.mode_change_switch = 2
elif CS.out.cruiseState.modeSel == 3 and self.mode_change_switch == 2:
self.mode_change_timer = 50
self.mode_change_switch = 3
elif CS.out.cruiseState.modeSel == 4 and self.mode_change_switch == 3:
self.mode_change_timer = 50
self.mode_change_switch = 4
if self.mode_change_timer > 0:
self.mode_change_timer -= 1
run_speed_ctrl = self.param_OpkrAccelProfile and CS.acc_active and self.SC != None and (
CS.out.cruiseState.modeSel == 1 or CS.out.cruiseState.modeSel == 2
or CS.out.cruiseState.modeSel == 3)
if not run_speed_ctrl:
if CS.out.cruiseState.modeSel == 0:
self.steer_mode = "오파모드"
elif CS.out.cruiseState.modeSel == 1:
self.steer_mode = "차간+커브"
elif CS.out.cruiseState.modeSel == 2:
self.steer_mode = "차간ONLY"
elif CS.out.cruiseState.modeSel == 3:
self.steer_mode = "정체구간"
elif CS.out.cruiseState.modeSel == 4:
self.steer_mode = "순정모드"
if CS.out.steerWarning == 0:
self.mdps_status = "정상"
elif CS.out.steerWarning == 1:
self.mdps_status = "오류"
if CS.lkas_button_on == 0:
self.lkas_switch = "OFF"
elif CS.lkas_button_on == 1:
self.lkas_switch = "ON"
else:
self.lkas_switch = "-"
if CS.out.cruiseState.modeSel == 3:
str_log2 = '주행모드={:s} MDPS상태={:s} LKAS버튼={:s} CG:{:1.0f}'.format(
self.steer_mode, self.mdps_status, self.lkas_switch,
CS.cruiseGapSet)
else:
str_log2 = '주행모드={:s} MDPS상태={:s} LKAS버튼={:s}'.format(
self.steer_mode, self.mdps_status, self.lkas_switch)
trace1.printf2('{}'.format(str_log2))
#print( 'st={} cmd={} long={} steer={} req={}'.format(CS.out.cruiseState.standstill, pcm_cancel_cmd, self.CP.openpilotLongitudinalControl, apply_steer, steer_req ) )
if pcm_cancel_cmd and self.CP.openpilotLongitudinalControl:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.CANCEL, clu11_speed))
elif CS.out.cruiseState.standstill:
# run only first time when the car stopped
if self.last_lead_distance == 0 or not self.param_OpkrAutoResume:
# get the lead distance from the Radar
self.last_lead_distance = CS.lead_distance
self.resume_cnt = 0
# when lead car starts moving, create 6 RES msgs
elif CS.lead_distance != self.last_lead_distance and (
frame - self.last_resume_frame) > 5:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.RES_ACCEL, clu11_speed))
self.resume_cnt += 1
# interval after 6 msgs
if self.resume_cnt > 5:
self.last_resume_frame = frame
self.resume_cnt = 0
elif self.cruise_gap_prev == 0:
self.cruise_gap_prev = CS.cruiseGapSet
self.cruise_gap_set_init = 1
elif CS.cruiseGapSet != 1.0:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 100:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
self.cruise_gap = 0
elif run_speed_ctrl and self.SC != None:
is_sc_run = self.SC.update(CS, sm, self)
if is_sc_run:
can_sends.append(
create_clu11(self.packer, self.resume_cnt, CS.scc_bus,
CS.clu11, self.SC.btn_type,
self.SC.sc_clu_speed))
self.resume_cnt += 1
else:
self.resume_cnt = 0
if self.dRel > 17 and self.cruise_gap_prev != CS.cruiseGapSet and self.cruise_gap_set_init == 1 and CS.out.cruiseState.modeSel != 3:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 50:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
elif self.cruise_gap_prev == CS.cruiseGapSet:
self.cruise_gap_set_init = 0
self.cruise_gap_prev = 0
if CS.out.cruiseState.modeSel == 3 and CS.acc_active:
if 20 > self.dRel > 18 and self.vRel < 0 and CS.cruiseGapSet != 4.0:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 30:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
elif 16 > self.dRel > 14 and self.vRel < 0 and CS.cruiseGapSet != 3.0:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 30:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
elif 12 > self.dRel > 10 and self.vRel < 0 and CS.cruiseGapSet != 2.0:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 30:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
elif 9 > self.dRel > 7 and self.vRel < 0 and CS.cruiseGapSet != 1.0:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 30:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
elif 15 > self.dRel > 4 and self.vRel > 0 and CS.cruiseGapSet != 1.0:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 30:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
elif 25 > self.dRel > 18 and self.vRel >= 0 and CS.cruiseGapSet != 2.0:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 30:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
# 20 Hz LFA MFA message
#if frame % 5 == 0 and self.car_fingerprint in [CAR.IONIQ]:
# can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
self.lkas11_cnt += 1
return can_sends
from functools import wraps
from selfdrive.config import Conversions as CV
import common.MoveAvg as moveavg1
import cereal.messaging as messaging
import json
import requests
import signal
import subprocess
import time
movAvg = moveavg1.MoveAvg()
MAX_SPEED = 255
sc = SpdController()
path_x = np.arange(192)
def main():
sm = None
if sm is None:
sm = messaging.SubMaster(['plan', 'pathPlan', 'model'])
v_ego = 50 * CV.KPH_TO_MS
print('curvature test ')
while True:
sm.update(0)
value, model_sum = sc.calc_va(sm, v_ego)
print('curvature={:.3f} sum={:.5f}'.format(value, model_sum))
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
self.read_distance_lines = 0
#Auto detection for setup
self.cruise_main_button = 0
self.cruise_buttons = 0
self.allow_nonscc_available = False
self.lkasstate = 0
self.lead_distance = 150.
self.radar_obj_valid = 0.
self.vrelative = 0.
self.prev_cruise_buttons = 0
self.prev_gap_button = 0
self.cancel_button_count = 0
self.cancel_button_timer = 0
self.leftblinkerflashdebounce = 0
self.rightblinkerflashdebounce = 0
self.SC = SpdController()
self.driverAcc_time = 0
self.brake_check = 0
self.mainsw_check = 0
self.steer_anglecorrection = int(
Params().get('OpkrSteerAngleCorrection')) * 0.1
self.cruise_gap = int(Params().get('OpkrCruiseGapSet'))
self.pm = messaging.PubMaster(
['liveTrafficData', 'dynamicFollowButton'])
self.sm = messaging.SubMaster(['liveMapData'])
def update(self, cp, cp2, cp_cam):
cp_mdps = cp2 if self.CP.mdpsHarness else cp
cp_sas = cp2 if self.CP.sasBus else cp
cp_scc = cp_cam if ((self.CP.sccBus == 2)
or self.CP.radarOffCan) else cp
cp_fca = cp_cam if (self.CP.fcaBus == 2) else cp
self.prev_cruise_buttons = self.cruise_buttons
self.prev_cruise_main_button = self.cruise_main_button
self.prev_lkasstate = self.lkasstate
ret = car.CarState.new_message()
ret.doorOpen = any([
cp.vl["CGW1"]['CF_Gway_DrvDrSw'], cp.vl["CGW1"]['CF_Gway_AstDrSw'],
cp.vl["CGW2"]['CF_Gway_RLDrSw'], cp.vl["CGW2"]['CF_Gway_RRDrSw']
])
ret.seatbeltUnlatched = cp.vl["CGW1"]['CF_Gway_DrvSeatBeltSw'] == 0
ret.wheelSpeeds.fl = cp.vl["WHL_SPD11"]['WHL_SPD_FL'] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr +
ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.1
ret.standStill = self.CP.standStill
ret.steeringAngle = cp_sas.vl["SAS11"][
'SAS_Angle'] - self.steer_anglecorrection
ret.steeringRate = cp_sas.vl["SAS11"]['SAS_Speed']
ret.yawRate = cp.vl["ESP12"]['YAW_RATE']
self.leftblinkerflash = cp.vl["CGW1"][
'CF_Gway_TurnSigLh'] != 0 and cp.vl["CGW1"]['CF_Gway_TSigLHSw'] == 0
self.rightblinkerflash = cp.vl["CGW1"][
'CF_Gway_TurnSigRh'] != 0 and cp.vl["CGW1"]['CF_Gway_TSigRHSw'] == 0
if self.leftblinkerflash:
self.leftblinkerflashdebounce = 50
elif self.leftblinkerflashdebounce > 0:
self.leftblinkerflashdebounce -= 1
if self.rightblinkerflash:
self.rightblinkerflashdebounce = 50
elif self.rightblinkerflashdebounce > 0:
self.rightblinkerflashdebounce -= 1
ret.leftBlinker = cp.vl["CGW1"][
'CF_Gway_TSigLHSw'] != 0 or self.leftblinkerflashdebounce > 0
ret.rightBlinker = cp.vl["CGW1"][
'CF_Gway_TSigRHSw'] != 0 or self.rightblinkerflashdebounce > 0
ret.steeringTorque = cp_mdps.vl["MDPS12"]['CR_Mdps_StrColTq']
ret.steeringTorqueEps = cp_mdps.vl["MDPS12"]['CR_Mdps_OutTq']
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
ret.steerWarning = cp_mdps.vl["MDPS12"]['CF_Mdps_ToiUnavail'] != 0
self.VSetDis = cp_scc.vl["SCC11"]['VSetDis']
ret.vSetDis = self.VSetDis
self.clu_Vanz = ret.vEgo * CV.MS_TO_KPH
lead_objspd = cp_scc.vl["SCC11"]['ACC_ObjRelSpd']
self.lead_objspd = lead_objspd * CV.MS_TO_KPH
self.driverOverride = cp.vl["TCS13"]["DriverOverride"]
if self.driverOverride == 1:
self.driverAcc_time = 100
elif self.driverAcc_time:
self.driverAcc_time -= 1
ret.brakeHold = cp.vl["ESP11"]['AVH_STAT'] == 1
self.brakeHold = ret.brakeHold
self.cruise_main_button = cp.vl["CLU11"]["CF_Clu_CruiseSwMain"]
self.cruise_buttons = cp.vl["CLU11"]["CF_Clu_CruiseSwState"]
if not self.cruise_main_button:
if self.cruise_buttons == 4 and self.prev_cruise_buttons != 4 and self.cancel_button_count < 3:
self.cancel_button_count += 1
self.cancel_button_timer = 100
elif self.cancel_button_count == 3:
self.cancel_button_count = 0
if self.cancel_button_timer <= 100 and self.cancel_button_count:
self.cancel_button_timer = max(0, self.cancel_button_timer - 1)
if self.cancel_button_timer == 0:
self.cancel_button_count = 0
else:
self.cancel_button_count = 0
if self.prev_gap_button != self.cruise_buttons:
if self.cruise_buttons == 3:
self.cruise_gap -= 1
if self.cruise_gap < 1:
self.cruise_gap = 4
self.prev_gap_button = self.cruise_buttons
# cruise state
if not self.CP.enableCruise:
if self.cruise_buttons == 1 or self.cruise_buttons == 2:
self.allow_nonscc_available = True
self.brake_check = 0
self.mainsw_check = 0
ret.cruiseState.available = self.allow_nonscc_available != 0
ret.cruiseState.enabled = ret.cruiseState.available
elif not self.CP.radarOffCan:
ret.cruiseState.available = (cp_scc.vl["SCC11"]["MainMode_ACC"] !=
0)
ret.cruiseState.enabled = (cp_scc.vl["SCC12"]['ACCMode'] != 0)
self.lead_distance = cp_scc.vl["SCC11"]['ACC_ObjDist']
self.vrelative = cp_scc.vl["SCC11"]['ACC_ObjRelSpd']
self.radar_obj_valid = cp_scc.vl["SCC11"]['ObjValid']
ret.cruiseState.standstill = cp_scc.vl["SCC11"]['SCCInfoDisplay'] == 4.
self.is_set_speed_in_mph = cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"]
self.acc_active = ret.cruiseState.enabled
speed_kph = self.SC.update_cruiseSW(self)
if ret.cruiseState.enabled and (self.brake_check == 0
or self.mainsw_check == 0):
speed_conv = CV.MPH_TO_MS if self.is_set_speed_in_mph else CV.KPH_TO_MS
if self.CP.radarOffCan:
ret.cruiseState.speed = (cp.vl["LVR12"]["CF_Lvr_CruiseSet"] *
speed_conv) + 1
else:
ret.cruiseState.speed = cp_scc.vl["SCC11"][
'VSetDis'] * speed_conv
else:
ret.cruiseState.speed = 0
self.cruise_main_button = cp.vl["CLU11"]["CF_Clu_CruiseSwMain"]
self.cruise_buttons = cp.vl["CLU11"]["CF_Clu_CruiseSwState"]
ret.cruiseButtons = self.cruise_buttons
if self.cruise_main_button != 0:
self.mainsw_check = 1
# TODO: Find brake pressure
ret.brake = 0
ret.brakePressed = cp.vl["TCS13"]['DriverBraking'] != 0
self.brakeUnavailable = cp.vl["TCS13"]['ACCEnable'] == 3
if ret.brakePressed:
self.brake_check = 1
# TODO: Check this
ret.brakeLights = bool(cp.vl["TCS13"]['BrakeLight']
or ret.brakePressed)
if self.CP.carFingerprint in ELEC_VEH:
ret.gas = cp.vl["E_EMS11"]['Accel_Pedal_Pos'] / 256.
elif self.CP.carFingerprint in HYBRID_VEH:
ret.gas = cp.vl["EV_PC4"]['CR_Vcu_AccPedDep_Pc']
elif self.CP.emsAvailable:
ret.gas = cp.vl["EMS12"]['PV_AV_CAN'] / 100
ret.gasPressed = (cp.vl["TCS13"]["DriverOverride"] == 1)
if self.CP.emsAvailable:
ret.gasPressed = ret.gasPressed or bool(
cp.vl["EMS16"]["CF_Ems_AclAct"])
ret.espDisabled = (cp.vl["TCS15"]['ESC_Off_Step'] != 0)
self.parkBrake = (cp.vl["CGW1"]['CF_Gway_ParkBrakeSw'] != 0)
#TPMS
if cp.vl["TPMS11"]['PRESSURE_FL'] > 43:
ret.tpmsPressureFl = cp.vl["TPMS11"]['PRESSURE_FL'] * 5 * 0.145
else:
ret.tpmsPressureFl = cp.vl["TPMS11"]['PRESSURE_FL']
if cp.vl["TPMS11"]['PRESSURE_FR'] > 43:
ret.tpmsPressureFr = cp.vl["TPMS11"]['PRESSURE_FR'] * 5 * 0.145
else:
ret.tpmsPressureFr = cp.vl["TPMS11"]['PRESSURE_FR']
if cp.vl["TPMS11"]['PRESSURE_RL'] > 43:
ret.tpmsPressureRl = cp.vl["TPMS11"]['PRESSURE_RL'] * 5 * 0.145
else:
ret.tpmsPressureRl = cp.vl["TPMS11"]['PRESSURE_RL']
if cp.vl["TPMS11"]['PRESSURE_RR'] > 43:
ret.tpmsPressureRr = cp.vl["TPMS11"]['PRESSURE_RR'] * 5 * 0.145
else:
ret.tpmsPressureRr = cp.vl["TPMS11"]['PRESSURE_RR']
ret.cruiseGapSet = self.cruise_gap
if self.read_distance_lines != self.cruise_gap:
self.read_distance_lines = self.cruise_gap
msg_df = messaging.new_message('dynamicFollowButton')
msg_df.dynamicFollowButton.status = max(
self.read_distance_lines - 1, 0)
self.pm.send('dynamicFollowButton', msg_df)
# TODO: refactor gear parsing in function
# Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection,
# as this seems to be standard over all cars, but is not the preferred method.
if self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
if cp.vl["CLU15"]["CF_Clu_InhibitD"] == 1:
ret.gearShifter = GearShifter.drive
elif cp.vl["CLU15"]["CF_Clu_InhibitN"] == 1:
ret.gearShifter = GearShifter.neutral
elif cp.vl["CLU15"]["CF_Clu_InhibitP"] == 1:
ret.gearShifter = GearShifter.park
elif cp.vl["CLU15"]["CF_Clu_InhibitR"] == 1:
ret.gearShifter = GearShifter.reverse
else:
ret.gearShifter = GearShifter.unknown
# Gear Selecton via TCU12
elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
gear = cp.vl["TCU12"]["CUR_GR"]
if gear == 0:
ret.gearShifter = GearShifter.park
elif gear == 14:
ret.gearShifter = GearShifter.reverse
elif gear > 0 and gear < 9: # unaware of anything over 8 currently
ret.gearShifter = GearShifter.drive
else:
ret.gearShifter = GearShifter.unknown
# Gear Selecton - This is only compatible with optima hybrid 2017
elif self.CP.evgearAvailable:
gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"]
if gear in (5, 8): # 5: D, 8: sport mode
ret.gearShifter = GearShifter.drive
elif gear == 6:
ret.gearShifter = GearShifter.neutral
elif gear == 0:
ret.gearShifter = GearShifter.park
elif gear == 7:
ret.gearShifter = GearShifter.reverse
else:
ret.gearShifter = GearShifter.unknown
# Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with
elif self.CP.lvrAvailable:
gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
if gear in (5, 8): # 5: D, 8: sport mode
ret.gearShifter = GearShifter.drive
elif gear == 6:
ret.gearShifter = GearShifter.neutral
elif gear == 0:
ret.gearShifter = GearShifter.park
elif gear == 7:
ret.gearShifter = GearShifter.reverse
else:
ret.gearShifter = GearShifter.unknown
if self.CP.fcaBus != -1:
ret.stockAeb = cp_fca.vl["FCA11"]['FCA_CmdAct'] != 0
ret.stockFcw = cp_fca.vl["FCA11"]['CF_VSM_Warn'] == 2
elif not self.CP.radarOffCan:
ret.stockAeb = cp_scc.vl["SCC12"]['AEB_CmdAct'] != 0
ret.stockFcw = cp_scc.vl["SCC12"]['CF_VSM_Warn'] == 2
if self.CP.bsmAvailable:
ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0
ret.rightBlindspot = cp.vl["LCA11"]["CF_Lca_IndRight"] != 0
self.lkasstate = cp_cam.vl["LKAS11"]["CF_Lkas_LdwsSysState"]
self.lkasbutton = self.lkasstate != self.prev_lkasstate and (
self.lkasstate == 0 or self.prev_lkasstate == 0)
# save the entire LKAS11, CLU11, SCC12 and MDPS12
self.lkas11 = copy.copy(cp_cam.vl["LKAS11"])
self.clu11 = copy.copy(cp.vl["CLU11"])
self.scc11 = copy.copy(cp_scc.vl["SCC11"])
self.scc12 = copy.copy(cp_scc.vl["SCC12"])
self.scc13 = copy.copy(cp_scc.vl["SCC13"])
self.scc14 = copy.copy(cp_scc.vl["SCC14"])
self.fca11 = copy.copy(cp_fca.vl["FCA11"])
self.mdps12 = copy.copy(cp_mdps.vl["MDPS12"])
self.scc11init = copy.copy(cp.vl["SCC11"])
self.scc12init = copy.copy(cp.vl["SCC12"])
self.fca11init = copy.copy(cp.vl["FCA11"])
return ret
@staticmethod
def get_can_parser(CP):
checks = []
signals = [
# sig_name, sig_address, default
("WHL_SPD_FL", "WHL_SPD11", 0),
("WHL_SPD_FR", "WHL_SPD11", 0),
("WHL_SPD_RL", "WHL_SPD11", 0),
("WHL_SPD_RR", "WHL_SPD11", 0),
("YAW_RATE", "ESP12", 0),
("CF_Gway_DrvSeatBeltInd", "CGW4", 1),
("CF_Gway_DrvSeatBeltSw", "CGW1", 0),
("CF_Gway_DrvDrSw", "CGW1", 0), # Driver Door
("CF_Gway_AstDrSw", "CGW1", 0), # Passenger door
("CF_Gway_RLDrSw", "CGW2", 0), # Rear reft door
("CF_Gway_RRDrSw", "CGW2", 0), # Rear right door
("CF_Gway_TSigLHSw", "CGW1", 0),
("CF_Gway_TurnSigLh", "CGW1", 0),
("CF_Gway_TSigRHSw", "CGW1", 0),
("CF_Gway_TurnSigRh", "CGW1", 0),
("CF_Gway_ParkBrakeSw", "CGW1", 0),
("CYL_PRES", "ESP12", 0),
("AVH_STAT", "ESP11", 0),
("CF_Clu_CruiseSwState", "CLU11", 0),
("CF_Clu_CruiseSwMain", "CLU11", 0),
("CF_Clu_SldMainSW", "CLU11", 0),
("CF_Clu_ParityBit1", "CLU11", 0),
("CF_Clu_VanzDecimal", "CLU11", 0),
("CF_Clu_Vanz", "CLU11", 0),
("CF_Clu_SPEED_UNIT", "CLU11", 0),
("CF_Clu_DetentOut", "CLU11", 0),
("CF_Clu_RheostatLevel", "CLU11", 0),
("CF_Clu_CluInfo", "CLU11", 0),
("CF_Clu_AmpInfo", "CLU11", 0),
("CF_Clu_AliveCnt1", "CLU11", 0),
("ACCEnable", "TCS13", 0),
("BrakeLight", "TCS13", 0),
("DriverBraking", "TCS13", 0),
("DriverOverride", "TCS13", 0),
("ESC_Off_Step", "TCS15", 0),
("CF_Lvr_CruiseSet", "LVR12", 0),
("CRUISE_LAMP_M", "EMS16", 0),
("CR_VSM_Alive", "SCC12", 0),
("AliveCounterACC", "SCC11", 0),
("CR_FCA_Alive", "FCA11", 0),
("Supplemental_Counter", "FCA11", 0),
("PRESSURE_FL", "TPMS11", 0),
("PRESSURE_FR", "TPMS11", 0),
("PRESSURE_RL", "TPMS11", 0),
("PRESSURE_RR", "TPMS11", 0),
]
checks = [
# address, frequency
("TCS13", 50),
("TCS15", 10),
("CLU11", 50),
("ESP12", 100),
("CGW1", 10),
("CGW4", 5),
("WHL_SPD11", 50),
]
if CP.sccBus == 0:
signals += [
("MainMode_ACC", "SCC11", 0),
("VSetDis", "SCC11", 0),
("SCCInfoDisplay", "SCC11", 0),
("ACC_ObjStatus", "SCC11", 0),
("ACC_ObjDist", "SCC11", 0),
("ObjValid", "SCC11", 0),
("ACC_ObjRelSpd", "SCC11", 0),
("AliveCounterACC", "SCC11", 0),
("ACCMode", "SCC12", 1),
("AEB_CmdAct", "SCC12", 0),
("CF_VSM_Warn", "SCC12", 0),
("CR_VSM_Alive", "SCC12", 0),
]
checks += [
("SCC11", 50),
("SCC12", 50),
]
if CP.fcaBus == 0:
signals += [
("FCA_CmdAct", "FCA11", 0),
("CF_VSM_Warn", "FCA11", 0),
("CR_FCA_Alive", "FCA11", 0),
("Supplemental_Counter", "FCA11", 0),
]
checks += [("FCA11", 50)]
if not CP.mdpsHarness:
signals += [("CR_Mdps_StrColTq", "MDPS12", 0),
("CF_Mdps_Def", "MDPS12", 0),
("CF_Mdps_ToiActive", "MDPS12", 0),
("CF_Mdps_ToiUnavail", "MDPS12", 0),
("CF_Mdps_MsgCount2", "MDPS12", 0),
("CF_Mdps_Chksum2", "MDPS12", 0),
("CF_Mdps_ToiFlt", "MDPS12", 0),
("CF_Mdps_SErr", "MDPS12", 0),
("CR_Mdps_StrTq", "MDPS12", 0),
("CF_Mdps_FailStat", "MDPS12", 0),
("CR_Mdps_OutTq", "MDPS12", 0)]
checks += [("MDPS12", 50)]
if CP.sasBus == 0:
signals += [
("SAS_Angle", "SAS11", 0),
("SAS_Speed", "SAS11", 0),
]
checks += [("SAS11", 100)]
if CP.bsmAvailable:
signals += [
("CF_Lca_IndLeft", "LCA11", 0),
("CF_Lca_IndRight", "LCA11", 0),
]
checks += [("LCA11", 50)]
if CP.carFingerprint in ELEC_VEH:
signals += [
("Accel_Pedal_Pos", "E_EMS11", 0),
]
checks += [
("E_EMS11", 50),
]
elif CP.carFingerprint in HYBRID_VEH:
signals += [
("CR_Vcu_AccPedDep_Pc", "EV_PC4", 0),
]
checks += [
("EV_PC4", 50),
]
elif CP.emsAvailable:
signals += [
("PV_AV_CAN", "EMS12", 0),
("CF_Ems_AclAct", "EMS16", 0),
]
checks += [
("EMS12", 100),
("EMS16", 100),
]
if CP.carFingerprint in FEATURES["use_cluster_gears"]:
signals += [
("CF_Clu_InhibitD", "CLU15", 0),
("CF_Clu_InhibitP", "CLU15", 0),
("CF_Clu_InhibitN", "CLU15", 0),
("CF_Clu_InhibitR", "CLU15", 0),
]
checks += [("CLU15", 5)]
elif CP.carFingerprint in FEATURES["use_tcu_gears"]:
signals += [("CUR_GR", "TCU12", 0)]
checks += [("TCU12", 100)]
elif CP.evgearAvailable:
signals += [("Elect_Gear_Shifter", "ELECT_GEAR", 0)]
checks += [("ELECT_GEAR", 20)]
elif CP.lvrAvailable:
signals += [("CF_Lvr_Gear", "LVR12", 0)]
checks += [("LVR12", 100)]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
@staticmethod
def get_can2_parser(CP):
signals = []
checks = []
if CP.mdpsHarness:
signals += [("CR_Mdps_StrColTq", "MDPS12", 0),
("CF_Mdps_Def", "MDPS12", 0),
("CF_Mdps_ToiActive", "MDPS12", 0),
("CF_Mdps_ToiUnavail", "MDPS12", 0),
("CF_Mdps_MsgCount2", "MDPS12", 0),
("CF_Mdps_Chksum2", "MDPS12", 0),
("CF_Mdps_ToiFlt", "MDPS12", 0),
("CF_Mdps_SErr", "MDPS12", 0),
("CR_Mdps_StrTq", "MDPS12", 0),
("CF_Mdps_FailStat", "MDPS12", 0),
("CR_Mdps_OutTq", "MDPS12", 0)]
checks += [("MDPS12", 50)]
if CP.sasBus == 1:
signals += [
("SAS_Angle", "SAS11", 0),
("SAS_Speed", "SAS11", 0),
]
checks += [("SAS11", 100)]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 1)
@staticmethod
def get_cam_can_parser(CP):
signals = [
# sig_name, sig_address, default
("CF_Lkas_LdwsActivemode", "LKAS11", 0),
("CF_Lkas_LdwsSysState", "LKAS11", 0),
("CF_Lkas_SysWarning", "LKAS11", 0),
("CF_Lkas_LdwsLHWarning", "LKAS11", 0),
("CF_Lkas_LdwsRHWarning", "LKAS11", 0),
("CF_Lkas_HbaLamp", "LKAS11", 0),
("CF_Lkas_FcwBasReq", "LKAS11", 0),
("CF_Lkas_ToiFlt", "LKAS11", 0),
("CF_Lkas_HbaSysState", "LKAS11", 0),
("CF_Lkas_FcwOpt", "LKAS11", 0),
("CF_Lkas_HbaOpt", "LKAS11", 0),
("CF_Lkas_FcwSysState", "LKAS11", 0),
("CF_Lkas_FcwCollisionWarning", "LKAS11", 0),
("CF_Lkas_MsgCount", "LKAS11", 0),
("CF_Lkas_FusionState", "LKAS11", 0),
("CF_Lkas_FcwOpt_USM", "LKAS11", 0),
("CF_Lkas_LdwsOpt_USM", "LKAS11", 0)
]
checks = [("LKAS11", 100)]
if CP.sccBus == 2 or CP.radarOffCan:
signals += [
("MainMode_ACC", "SCC11", 0),
("SCCInfoDisplay", "SCC11", 0),
("AliveCounterACC", "SCC11", 0),
("VSetDis", "SCC11", 0),
("ObjValid", "SCC11", 0),
("DriverAlertDisplay", "SCC11", 0),
("TauGapSet", "SCC11", 4),
("ACC_ObjStatus", "SCC11", 0),
("ACC_ObjLatPos", "SCC11", 0),
("ACC_ObjDist", "SCC11", 150.),
("ACC_ObjRelSpd", "SCC11", 0),
("Navi_SCC_Curve_Status", "SCC11", 0),
("Navi_SCC_Curve_Act", "SCC11", 0),
("Navi_SCC_Camera_Act", "SCC11", 0),
("Navi_SCC_Camera_Status", "SCC11", 2),
("ACCMode", "SCC12", 0),
("CF_VSM_Prefill", "SCC12", 0),
("CF_VSM_DecCmdAct", "SCC12", 0),
("CF_VSM_HBACmd", "SCC12", 0),
("CF_VSM_Warn", "SCC12", 0),
("CF_VSM_Stat", "SCC12", 0),
("CF_VSM_BeltCmd", "SCC12", 0),
("ACCFailInfo", "SCC12", 0),
("ACCMode", "SCC12", 0),
("StopReq", "SCC12", 0),
("CR_VSM_DecCmd", "SCC12", 0),
("aReqRaw", "SCC12", 0),
("TakeOverReq", "SCC12", 0),
("PreFill", "SCC12", 0),
("aReqValue", "SCC12", 0),
("CF_VSM_ConfMode", "SCC12", 1),
("AEB_Failinfo", "SCC12", 0),
("AEB_Status", "SCC12", 2),
("AEB_CmdAct", "SCC12", 0),
("AEB_StopReq", "SCC12", 0),
("CR_VSM_Alive", "SCC12", 0),
("CR_VSM_ChkSum", "SCC12", 0),
("SCCDrvModeRValue", "SCC13", 1),
("SCC_Equip", "SCC13", 1),
("AebDrvSetStatus", "SCC13", 0),
("Lead_Veh_Dep_Alert_USM", "SCC13", 0),
("JerkUpperLimit", "SCC14", 0),
("JerkLowerLimit", "SCC14", 0),
("ComfortBandUpper", "SCC14", 0),
("ComfortBandLower", "SCC14", 0),
("ACCMode", "SCC14", 0),
("ObjGap", "SCC14", 0),
("CF_VSM_Prefill", "FCA11", 0),
("CF_VSM_HBACmd", "FCA11", 0),
("CF_VSM_Warn", "FCA11", 0),
("CF_VSM_BeltCmd", "FCA11", 0),
("CR_VSM_DecCmd", "FCA11", 0),
("FCA_Status", "FCA11", 2),
("FCA_CmdAct", "FCA11", 0),
("FCA_StopReq", "FCA11", 0),
("FCA_DrvSetStatus", "FCA11", 1),
("CF_VSM_DecCmdAct", "FCA11", 0),
("FCA_Failinfo", "FCA11", 0),
("FCA_RelativeVelocity", "FCA11", 0),
("FCA_TimetoCollision", "FCA11", 2540.),
("CR_FCA_Alive", "FCA11", 0),
("CR_FCA_ChkSum", "FCA11", 0),
("Supplemental_Counter", "FCA11", 0),
("PAINT1_Status", "FCA11", 1),
]
if CP.sccBus == 2:
checks += [
("SCC11", 50),
("SCC12", 50),
]
if CP.fcaBus == 2:
checks += [("FCA11", 50)]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
