Esempio n. 1
0
  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( v_ego_kph, 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_mval = 10.0  # 오파 => 운전자.  (sec)
    sec_pval = 3  #  운전자 => 오파  (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 = 0.5  # 오파 => 운전자.
      sec_pval = 10 # 운전자 => 오파  (sec)

    if v_ego_kph > 5 and CS.out.steeringPressed:  #사용자 핸들 토크
      if abs_angle_steers > 5 and CS.out.steeringTorque < -10: #-STEER_THRESHOLD:   #right
        if dst_steer < 0:
          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 > 10: #STEER_THRESHOLD:  #left
        if dst_steer > 0:
          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
Esempio n. 2
0
    def steerParams_torque(self, CS, abs_angle_steers, path_plan):
        param = SteerLimitParams()
        v_ego_kph = CS.out.vEgo * CV.MS_TO_KPH

        self.enable_time = self.timer1.sampleTime()
        if self.enable_time < 50:
            self.steer_torque_over_timer = 0
            self.steer_torque_ratio = 1
            return param

        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 = 0.5  # 0.5 sec 운전자 => 오파  (sec)
        sec_mval = 30.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 = 0.2  # 오파 => 운전자.
            sec_pval = 10

        if v_ego_kph > 5 and CS.out.steeringPressed:  #사용자 핸들 토크
            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
Esempio n. 3
0
    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
Esempio n. 4
0
    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
Esempio n. 5
0
    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.

        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

        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