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)