def param_load(self): self.command_cnt += 1 if self.command_cnt > 100: self.command_cnt = 0 if self.command_cnt % 10: return self.command_load += 1 if self.command_load == 1: self.param_OpkrAccelProfile = int( self.params.get('OpkrAccelProfile')) elif self.command_load == 2: self.param_OpkrAutoResume = int(self.params.get('OpkrAutoResume')) elif self.command_load == 3: self.param_OpkrWhoisDriver = int( self.params.get('OpkrWhoisDriver')) else: self.command_load = 0 # speed controller if self.param_preOpkrAccelProfile != self.param_OpkrAccelProfile: self.param_preOpkrAccelProfile = self.param_OpkrAccelProfile if self.param_OpkrAccelProfile == 1: self.SC = SpdctrlSlow() elif self.param_OpkrAccelProfile == 2: self.SC = SpdctrlNormal() else: self.SC = SpdctrlNormal()
def __init__(self, dbc_name, CP, VM): self.CP = CP self.apply_steer_last = 0 self.car_fingerprint = CP.carFingerprint self.packer = CANPacker(dbc_name) self.steer_rate_limited = False self.resume_cnt = 0 self.last_resume_frame = 0 self.last_lead_distance = 0 self.lanechange_manual_timer = 0 self.emergency_manual_timer = 0 self.driver_steering_torque_above_timer = 0 self.mode_change_timer = 0 self.steer_mode = "" self.mdps_status = "" self.lkas_switch = "" self.lkas11_cnt = 0 self.nBlinker = 0 self.dRel = 0 self.yRel = 0 self.vRel = 0 self.timer1 = tm.CTime1000("time") self.model_speed = 0 self.model_sum = 0 # hud self.hud_timer_left = 0 self.hud_timer_right = 0 self.traceCC = trace1.Loger("CarController") self.cruise_gap = 0 self.cruise_gap_prev = 0 self.cruise_gap_set_init = 0 self.cruise_gap_switch_timer = 0 self.params = Params() self.mode_change_switch = int( self.params.get('CruiseStatemodeSelInit')) self.param_OpkrAccelProfile = int(self.params.get('OpkrAccelProfile')) self.param_OpkrAutoResume = int(self.params.get('OpkrAutoResume')) if self.param_OpkrAccelProfile == 1: self.SC = SpdctrlSlow() elif self.param_OpkrAccelProfile == 2: self.SC = SpdctrlNormal() else: self.SC = SpdctrlFast()
class CarController(): def __init__(self, dbc_name, CP, VM): self.CP = CP self.apply_steer_last = 0 self.car_fingerprint = CP.carFingerprint self.packer = CANPacker(dbc_name) self.steer_rate_limited = False self.resume_cnt = 0 self.lkas11_cnt = 0 self.last_resume_frame = 0 self.last_lead_distance = 0 self.nBlinker = 0 self.lane_change_torque_lower = 0 self.steer_torque_over_timer = 0 self.steer_torque_ratio = 1 self.steer_torque_ratio_dir = 1 self.dRel = 0 self.yRel = 0 self.vRel = 0 self.timer1 = tm.CTime1000("time") self.model_speed = 0 self.model_sum = 0 # hud self.hud_timer_left = 0 self.hud_timer_right = 0 self.hud_sys_state = 0 self.command_cnt = 0 self.command_load = 0 self.params = Params() # param self.param_preOpkrAccelProfile = -1 self.param_OpkrAccelProfile = 0 self.param_OpkrAutoResume = 0 self.param_OpkrWhoisDriver = 0 self.SC = None self.traceCC = trace1.Loger("CarController") 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 process_hud_alert(self, enabled, c): visual_alert = c.hudControl.visualAlert left_lane = c.hudControl.leftLaneVisible right_lane = c.hudControl.rightLaneVisible sys_warning = (visual_alert == VisualAlert.steerRequired) if left_lane: self.hud_timer_left = 100 if right_lane: self.hud_timer_right = 100 if self.hud_timer_left: self.hud_timer_left -= 1 if self.hud_timer_right: self.hud_timer_right -= 1 # initialize to no line visible sys_state = 1 if self.hud_timer_left and self.hud_timer_right or sys_warning: # HUD alert only display when LKAS status is active if (self.steer_torque_ratio > 0.7) and (enabled or sys_warning): sys_state = 3 else: sys_state = 4 elif self.hud_timer_left: sys_state = 5 elif self.hud_timer_right: sys_state = 6 return sys_warning, sys_state def atom_tune(self, v_ego, cv_value): # cV(곡률에 의한 변화) self.cv_KPH = self.CP.atomTuning.cvKPH self.cv_BPV = self.CP.atomTuning.cvBPV self.cv_sMaxV = self.CP.atomTuning.cvsMaxV self.cv_sdUpV = self.CP.atomTuning.cvsdUpV self.cv_sdDnV = self.CP.atomTuning.cvsdDnV self.steerMAX = [] self.steerdUP = [] self.steerdDN = [] # Max nPos = 0 for sCV in self.cv_BPV: self.steerMAX.append(interp(cv_value, sCV, self.cv_sMaxV[nPos])) self.steerdUP.append(interp(cv_value, sCV, self.cv_sdUpV[nPos])) self.steerdDN.append(interp(cv_value, sCV, self.cv_sdDnV[nPos])) nPos += 1 if nPos > 10: break MAX = interp(v_ego, self.cv_KPH, self.steerMAX) UP = interp(v_ego, self.cv_KPH, self.steerdUP) DN = interp(v_ego, self.cv_KPH, self.steerdDN) #str_log1 = 'ego={:.1f} /{:.1f}/{:.1f}/{:.1f} {}'.format(v_ego, MAX, UP, DN, self.steerMAX ) #trace1.printf2( '{}'.format( str_log1 ) ) return MAX, UP, DN def steerParams_torque(self, CS, actuators, path_plan): param = SteerLimitParams() v_ego_kph = CS.out.vEgo * CV.MS_TO_KPH dst_steer = actuators.steer * param.STEER_MAX abs_angle_steers = abs(actuators.steerAngle) self.enable_time = self.timer1.sampleTime() if self.enable_time < 50: self.steer_torque_over_timer = 0 self.steer_torque_ratio = 1 return param, dst_steer nMAX, nUP, nDN = self.atom_tune(CS.out.vEgo, self.model_speed) param.STEER_MAX = min(param.STEER_MAX, nMAX) param.STEER_DELTA_UP = min(param.STEER_DELTA_UP, nUP) param.STEER_DELTA_DOWN = min(param.STEER_DELTA_DOWN, nDN) sec_pval = 3 # 0.5 sec 운전자 => 오파 (sec) sec_mval = 10.0 # 오파 => 운전자. (sec) # streer over check if path_plan.laneChangeState != LaneChangeState.off: self.steer_torque_over_timer = 0 elif CS.out.leftBlinker or CS.out.rightBlinker: sec_mval = 2 # 오파 => 운전자. sec_pval = 10 if v_ego_kph > 5 and CS.out.steeringPressed: #사용자 핸들 토크 if abs_angle_steers > 5 and CS.out.steeringTorque < -STEER_THRESHOLD: #right if dst_steer < 0: param.STEER_MAX = SteerLimitParams.STEER_MAX self.steer_torque_over_timer = 0 else: sec_mval = 1 self.steer_torque_over_timer = 50 elif abs_angle_steers > 5 and CS.out.steeringTorque > STEER_THRESHOLD: #left if dst_steer > 0: param.STEER_MAX = SteerLimitParams.STEER_MAX self.steer_torque_over_timer = 0 else: sec_mval = 1 self.steer_torque_over_timer = 50 else: self.steer_torque_over_timer = 50 elif self.steer_torque_over_timer: self.steer_torque_over_timer -= 1 ratio_pval = 1 / (100 * sec_pval) ratio_mval = 1 / (100 * sec_mval) if self.steer_torque_over_timer: self.steer_torque_ratio -= ratio_mval else: self.steer_torque_ratio += ratio_pval if self.steer_torque_ratio < 0: self.steer_torque_ratio = 0 elif self.steer_torque_ratio > 1: self.steer_torque_ratio = 1 return param, dst_steer def param_load(self): self.command_cnt += 1 if self.command_cnt > 100: self.command_cnt = 0 if self.command_cnt % 10: return self.command_load += 1 if self.command_load == 1: self.param_OpkrAccelProfile = int( self.params.get('OpkrAccelProfile')) elif self.command_load == 2: self.param_OpkrAutoResume = int(self.params.get('OpkrAutoResume')) elif self.command_load == 3: self.param_OpkrWhoisDriver = int( self.params.get('OpkrWhoisDriver')) else: self.command_load = 0 # speed controller if self.param_preOpkrAccelProfile != self.param_OpkrAccelProfile: self.param_preOpkrAccelProfile = self.param_OpkrAccelProfile if self.param_OpkrAccelProfile == 1: self.SC = SpdctrlSlow() elif self.param_OpkrAccelProfile == 2: self.SC = SpdctrlNormal() else: self.SC = SpdctrlNormal() # c:car.CarControl(car.capnp), CS:CarState CP:CarInterface.get_params 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 CarController(): def __init__(self, dbc_name, CP, VM): self.CP = CP self.apply_steer_last = 0 self.car_fingerprint = CP.carFingerprint self.packer = CANPacker(dbc_name) self.steer_rate_limited = False self.resume_cnt = 0 self.lkas11_cnt = 0 self.last_resume_frame = 0 self.last_lead_distance = 0 self.longcontrol = CP.openpilotLongitudinalControl self.nBlinker = 0 self.lane_change_torque_lower = 0 self.steer_torque_over_timer = 0 self.steer_torque_ratio = 1 self.steer_torque_ratio_dir = 1 self.dRel = 0 self.yRel = 0 self.vRel = 0 self.timer1 = tm.CTime1000("time") self.model_speed = 0 self.model_sum = 0 # hud self.hud_timer_left = 0 self.hud_timer_right = 0 self.command_cnt = 0 self.command_load = 0 self.params = Params() # param self.param_preOpkrAccelProfile = -1 self.param_OpkrAccelProfile = 0 self.param_OpkrAutoResume = 0 self.param_OpkrWhoisDriver = 0 self.SC = None self.traceCC = trace1.Loger("CarController") 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 process_hud_alert(self, enabled, CC): visual_alert = CC.hudControl.visualAlert left_lane = CC.hudControl.leftLaneVisible right_lane = CC.hudControl.rightLaneVisible sys_warning = (visual_alert == VisualAlert.steerRequired) if left_lane: self.hud_timer_left = 100 if right_lane: self.hud_timer_right = 100 if self.hud_timer_left: self.hud_timer_left -= 1 if self.hud_timer_right: self.hud_timer_right -= 1 # initialize to no line visible sys_state = 1 if self.hud_timer_left and self.hud_timer_right or sys_warning: # HUD alert only display when LKAS status is active if (self.steer_torque_ratio > 0.7) and (enabled or sys_warning): sys_state = 3 else: sys_state = 4 elif self.hud_timer_left: sys_state = 5 elif self.hud_timer_right: sys_state = 6 return sys_warning, sys_state def cV_tune(self, v_ego, cv_value): # cV(곡률에 의한 변화) self.sRKPHV = self.CP.lateralPIDatom.sRKPHV self.cVBPV = self.CP.lateralCVatom.cvBPV self.cvSteerMaxV1 = self.CP.lateralCVatom.cvSteerMaxV1 self.cvSteerDeltaUpV1 = self.CP.lateralCVatom.cvSteerDeltaUpV1 self.cvSteerDeltaDnV1 = self.CP.lateralCVatom.cvSteerDeltaDnV1 self.cvSteerMaxV2 = self.CP.lateralCVatom.cvSteerMaxV2 self.cvSteerDeltaUpV2 = self.CP.lateralCVatom.cvSteerDeltaUpV2 self.cvSteerDeltaDnV2 = self.CP.lateralCVatom.cvSteerDeltaDnV2 cv_BPV = self.cVBPV # 곡률 # Max self.steerMax1 = interp(cv_value, cv_BPV, self.cvSteerMaxV1) self.steerMax2 = interp(cv_value, cv_BPV, self.cvSteerMaxV2) self.steerMaxV = [float(self.steerMax1), float(self.steerMax2)] self.MAX = interp(v_ego, self.sRKPHV, self.steerMaxV) # Up self.steerUP1 = interp(cv_value, cv_BPV, self.cvSteerDeltaUpV1) self.steerUP2 = interp(cv_value, cv_BPV, self.cvSteerDeltaUpV2) self.steerUPV = [float(self.steerUP1), float(self.steerUP2)] self.UP = interp(v_ego, self.sRKPHV, self.steerUPV) # dn self.steerDN1 = interp(cv_value, cv_BPV, self.cvSteerDeltaDnV1) self.steerDN2 = interp(cv_value, cv_BPV, self.cvSteerDeltaDnV2) self.steerDNV = [float(self.steerDN1), float(self.steerDN2)] self.DN = interp(v_ego, self.sRKPHV, self.steerDNV) def steerParams_torque(self, CS, abs_angle_steers, path_plan, CC): param = SteerLimitParams() v_ego_kph = CS.out.vEgo * CV.MS_TO_KPH self.cV_tune(CS.out.vEgo, self.model_speed) 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) # streer over check if v_ego_kph > 5 and abs(CS.out.steeringTorque) > 180: #사용자 핸들 토크 self.steer_torque_over_timer = 1 else: self.steer_torque_over_timer = 0 if CS.out.leftBlinker or CS.out.rightBlinker: self.nBlinker += 1 elif self.nBlinker: self.nBlinker = 0 # 차선이 없고 앞차량이 없으면. steer_angle_lower = self.dRel > 20 and ( not CC.hudControl.leftLaneVisible and not CC.hudControl.rightLaneVisible) if v_ego_kph < 1: self.steer_torque_over_timer = 0 self.steer_torque_ratio_dir = 1 elif path_plan.laneChangeState != LaneChangeState.off: self.steer_torque_ratio_dir = 1 self.steer_torque_over_timer = 0 self.nBlinker = 0 elif self.steer_torque_over_timer: #or CS.out.steerWarning: self.steer_torque_ratio_dir = -1 elif steer_angle_lower: param.STEER_MAX *= 0.5 param.STEER_DELTA_UP = 1 param.STEER_DELTA_DOWN = 2 self.steer_torque_ratio_dir = 1 else: self.steer_torque_ratio_dir = 1 lane_change_torque_lower = 0 if self.nBlinker > 10 and v_ego_kph > 1: lane_change_torque_lower = int( CS.out.leftBlinker) + int(CS.out.rightBlinker) * 2 if CS.out.steeringPressed and self.param_OpkrWhoisDriver: self.steer_torque_ratio = 0.05 self.lane_change_torque_lower = lane_change_torque_lower # smoth torque enable or disable ratio_pval = 0.001 # 10 sec ratio_mval = 0.001 # 10 sec if self.param_OpkrWhoisDriver == 1: # 민감 ratio_pval = 0.005 # 2 sec ratio_mval = 0.01 # 1 sec else: # 보통. ratio_pval = 0.002 # 5 sec ratio_mval = 0.005 # 2 sec if self.param_OpkrWhoisDriver == 0: self.steer_torque_ratio = 1 elif self.steer_torque_ratio_dir >= 1: if self.steer_torque_ratio < 1: self.steer_torque_ratio += ratio_pval elif self.steer_torque_ratio_dir <= -1: if self.steer_torque_ratio > 0: self.steer_torque_ratio -= ratio_mval if self.steer_torque_ratio < 0: self.steer_torque_ratio = 0 elif self.steer_torque_ratio > 1: self.steer_torque_ratio = 1 return param def param_load(self): self.command_cnt += 1 if self.command_cnt > 100: self.command_cnt = 0 if self.command_cnt % 10: return self.command_load += 1 if self.command_load == 1: self.param_OpkrAccelProfile = int( self.params.get('OpkrAccelProfile')) elif self.command_load == 2: self.param_OpkrAutoResume = int(self.params.get('OpkrAutoResume')) elif self.command_load == 3: self.param_OpkrWhoisDriver = int( self.params.get('OpkrWhoisDriver')) else: self.command_load = 0 # speed controller if self.param_preOpkrAccelProfile != self.param_OpkrAccelProfile: self.param_preOpkrAccelProfile = self.param_OpkrAccelProfile if self.param_OpkrAccelProfile == 1: self.SC = SpdctrlSlow() elif self.param_OpkrAccelProfile == 2: self.SC = SpdctrlNormal() else: self.SC = SpdctrlNormal() # CC:car.CarControl(car.capnp), CS:CarState CP:CarInterface.get_params 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) < 90. #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={:.1f}/{:.3f} torg:{:5.0f}'.format( self.model_speed, self.model_sum, apply_steer) 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 = '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 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 # 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