Пример #1
0
  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, controls):

    # *** 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)

    # 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

    # 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
    lkas_active = enabled and abs(CS.out.steeringAngle) < 90. and not spas_active

    # 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

    # Disable steering while turning blinker on and speed below 60 kph
    if CS.out.leftBlinker or CS.out.rightBlinker:
      self.turning_signal_timer = 0.5 / DT_CTRL  # Disable for 0.5 Seconds after blinker turned off
    if self.turning_indicator_alert and CS.out.vEgo < 2 * CV.KPH_TO_MS: # set and clear by interface
      lkas_active = 0
    if self.turning_signal_timer > 0:
      self.turning_signal_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)

    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

    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 CS.mdps_bus:  # send clu11 to mdps if it is not on bus 0
      can_sends.append(create_clu11(self.packer, frame % 0x10, CS.mdps_bus, CS.clu11, Buttons.NONE, enabled_speed))

    if pcm_cancel_cmd and self.longcontrol:
      can_sends.append(create_clu11(self.packer, frame % 0x10, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed))

    # fix auto resume - by neokii
    if CS.out.cruiseState.standstill:

      if self.last_lead_distance == 0:
        self.last_lead_distance = CS.lead_distance
        self.resume_cnt = 0
        self.resume_wait_timer = 0

      # scc smoother
      elif self.scc_smoother.is_active(frame):
        pass

      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, self.resume_cnt, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed))
        self.resume_cnt += 1

        if self.resume_cnt >= 8:
          self.resume_cnt = 0
          self.resume_wait_timer = SccSmoother.get_wait_count()

    # reset lead distnce after the car starts moving
    elif self.last_lead_distance != 0:
      self.last_lead_distance = 0

    # scc smoother
    if not self.longcontrol:
      self.scc_smoother.update(enabled, can_sends, self.packer, CC, CS, frame, apply_accel, controls)

    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
Пример #2
0
    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, controls):

        # *** 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 * CarControllerParams.ACCEL_SCALE,
                           CarControllerParams.ACCEL_MIN,
                           CarControllerParams.ACCEL_MAX)

        # Steering Torque
        new_steer = int(round(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.steeringAngleDeg) < 90.

        # 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

        # Disable steering while turning blinker on and speed below 60 kph
        if CS.out.leftBlinker or CS.out.rightBlinker:
            self.turning_signal_timer = 0.5 / DT_CTRL  # Disable for 0.5 Seconds after blinker turned off
        if self.turning_indicator_alert and CS.out.vEgo < 2 * CV.KPH_TO_MS:  # set and clear by interface
            lkas_active = 0
        if self.turning_signal_timer > 0:
            self.turning_signal_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(enabled, self.car_fingerprint, visual_alert,
                            left_lane, right_lane, left_lane_depart, right_lane_depart)

        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

        controls.clu_speed_ms = clu11_speed * CV.KPH_TO_MS

        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

        #    apply_steer = interp(CS.out.vEgo, [0., 3.], [0., 1.])

        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 // 2 % 0x10, CS.mdps_bus,
                             CS.clu11, Buttons.NONE, enabled_speed))

        if pcm_cancel_cmd and (self.longcontrol and not self.mad_mode_enabled):
            can_sends.append(
                create_clu11(self.packer, frame % 0x10, CS.scc_bus, CS.clu11,
                             Buttons.CANCEL, clu11_speed))

        # fix auto resume - by neokii
        if CS.out.cruiseState.standstill:

            if self.last_lead_distance == 0:
                self.last_lead_distance = CS.lead_distance
                self.resume_cnt = 0
                self.resume_wait_timer = 0

            # scc smoother
            elif self.scc_smoother.is_active(frame):
                pass

            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, self.resume_cnt, CS.scc_bus,
                                 CS.clu11, Buttons.RES_ACCEL, clu11_speed))
                self.resume_cnt += 1

                if self.resume_cnt >= 8:
                    self.resume_cnt = 0
                    self.resume_wait_timer = SccSmoother.get_wait_count()

        # reset lead distnce after the car starts moving
        elif self.last_lead_distance != 0:
            self.last_lead_distance = 0

        # scc smoother
        self.scc_smoother.update(enabled, can_sends, self.packer, CC, CS,
                                 frame, apply_accel, controls)

        if CS.mdps_bus:  # send mdps12 to LKAS to prevent LKAS error
            can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))

        controls.apply_accel = apply_accel
        aReqValue = CS.scc12["aReqValue"]
        controls.aReqValue = aReqValue

        if aReqValue < controls.aReqValueMin:
            controls.aReqValueMin = controls.aReqValue

        if aReqValue > controls.aReqValueMax:
            controls.aReqValueMax = controls.aReqValue

        # send scc to car if longcontrol enabled and SCC not on bus 0 or ont live
        if self.longcontrol and CS.cruiseState_enabled and (
                CS.scc_bus or not self.scc_live) and frame % 2 == 0:

            apply_accel, lead_drel = self.scc_smoother.get_fused_accel(
                apply_accel, aReqValue, controls.sm)
            controls.fused_accel = apply_accel
            controls.lead_drel = lead_drel

            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,
                             controls.sm['radarState'], 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_lfahda_mfc(self.packer, enabled))

        return can_sends
Пример #3
0
  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)

    param = self.p

    #self.model_speed = 255 - self.SC.calc_va(sm, CS.out.vEgo)
    #atom model_speed
    #self.model_speed = self.SC.cal_model_speed(sm, CS.out.vEgo)
    #self.curve_speed = self.SC.cal_curve_speed(sm, CS.out.vEgo, frame)

    plan = sm['longitudinalPlan']
    self.dRel = int(plan.dRel1) #EON Lead
    self.yRel = int(plan.yRel1) #EON Lead
    self.vRel = int(plan.vRel1 * 3.6 + 0.5) #EON Lead
    self.dRel2 = int(plan.dRel2) #EON Lead
    self.yRel2 = int(plan.yRel2) #EON Lead
    self.vRel2 = int(plan.vRel2 * 3.6 + 0.5) #EON Lead
    self.lead2_status = plan.status2
    self.target_map_speed_camera = plan.targetSpeedCamera
    
    self.accActive = CS.acc_active

    lateral_plan = sm['lateralPlan']
    self.outScale = lateral_plan.outputScale
    self.vCruiseSet = lateral_plan.vCruiseSet
    
    #self.model_speed = interp(abs(lateral_plan.vCurvature), [0.0002, 0.01], [255, 30])
    #Hoya
    self.model_speed = interp(abs(lateral_plan.vCurvature), [0.0, 0.0002, 0.00074, 0.0025, 0.008, 0.02], [255, 255, 130, 90, 60, 20])

    if CS.out.vEgo > 8:
      if self.variable_steer_max:
        self.steerMax = interp(int(abs(self.model_speed)), self.model_speed_range, self.steerMax_range)
      else:
        self.steerMax = int(self.params.get("SteerMaxBaseAdj", encoding='utf8'))
      if self.variable_steer_delta:
        self.steerDeltaUp = interp(int(abs(self.model_speed)), self.model_speed_range, self.steerDeltaUp_range)
        self.steerDeltaDown = interp(int(abs(self.model_speed)), self.model_speed_range, self.steerDeltaDown_range)
      else:
        self.steerDeltaUp = int(self.params.get("SteerDeltaUpBaseAdj", encoding='utf8'))
        self.steerDeltaDown = int(self.params.get("SteerDeltaDownBaseAdj", encoding='utf8'))
    else:
      self.steerMax = int(self.params.get("SteerMaxBaseAdj", encoding='utf8'))
      self.steerDeltaUp = int(self.params.get("SteerDeltaUpBaseAdj", encoding='utf8'))
      self.steerDeltaDown = int(self.params.get("SteerDeltaDownBaseAdj", encoding='utf8'))

    param.STEER_MAX = min(CarControllerParams.STEER_MAX, self.steerMax) # variable steermax
    param.STEER_DELTA_UP = min(CarControllerParams.STEER_DELTA_UP, self.steerDeltaUp) # variable deltaUp
    param.STEER_DELTA_DOWN = min(CarControllerParams.STEER_DELTA_DOWN, self.steerDeltaDown) # variable deltaDown
    #param.STEER_DELTA_UP = CarControllerParams.STEER_DELTA_UP # fixed deltaUp
    #param.STEER_DELTA_DOWN = CarControllerParams.STEER_DELTA_DOWN # fixed deltaDown

    # Steering Torque
    if 0 <= self.driver_steering_torque_above_timer < 100:
      new_steer = int(round(actuators.steer * self.steerMax * (self.driver_steering_torque_above_timer / 100)))
    else:
      new_steer = int(round(actuators.steer * self.steerMax))
    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

    # SPAS limit angle extremes for safety
    if CS.spas_enabled:
      apply_steer_ang_req = clip(actuators.steeringAngleDeg, -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
    if self.opkr_maxanglelimit >= 90:
      lkas_active = enabled and abs(CS.out.steeringAngleDeg) < self.opkr_maxanglelimit and not spas_active
    else:
      lkas_active = enabled and not spas_active

    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 and self.opkr_turnsteeringdisable:
      self.lanechange_manual_timer = 50
    if CS.out.leftBlinker and CS.out.rightBlinker:
      self.emergency_manual_timer = 50
    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 abs(CS.out.steeringTorque) > 180 and CS.out.vEgo < LANE_CHANGE_SPEED_MIN:
      self.driver_steering_torque_above = True
    else:
      self.driver_steering_torque_above = False

    if self.driver_steering_torque_above == True:
      self.driver_steering_torque_above_timer -= 1
      if self.driver_steering_torque_above_timer <= 0:
        self.driver_steering_torque_above_timer = 0
    elif self.driver_steering_torque_above == False:
      self.driver_steering_torque_above_timer += 5
      if self.driver_steering_torque_above_timer >= 100:
        self.driver_steering_torque_above_timer = 100

    if not lkas_active:
      apply_steer = 0

    self.apply_accel_last = apply_accel
    self.apply_steer_last = apply_steer

    if CS.acc_active and CS.lead_distance > 149 and self.dRel < ((CS.out.vEgo * CV.MS_TO_KPH)+5) < 100 and self.vRel < -5 and CS.out.vEgo > 7 and abs(lateral_plan.steerAngleDesireDeg) < 15:
      self.need_brake_timer += 1
      if self.need_brake_timer > 50:
        self.need_brake = True
    else:
      self.need_brake = False
      self.need_brake_timer = 0

    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 = 'CV={:03.0f}  TQ={:03.0f}  R={:03.0f}  ST={:03.0f}/{:01.0f}/{:01.0f}'.format(abs(self.model_speed), abs(new_steer), self.timer1.sampleTime(), self.steerMax, self.steerDeltaUp, self.steerDeltaDown)

    if self.params.get("OpkrLiveTune", encoding='utf8') == "1":
      if int(self.params.get("LateralControlMethod", encoding='utf8')) == 0:
        self.str_log2 = 'T={:0.2f}/{:0.3f}/{:0.2f}/{:0.5f}'.format(float(int(self.params.get("PidKp", encoding='utf8')) * 0.01), float(int(self.params.get("PidKi", encoding='utf8')) * 0.001), float(int(self.params.get("PidKd", encoding='utf8')) * 0.01), float(int(self.params.get("PidKf", encoding='utf8')) * 0.00001))
      elif int(self.params.get("LateralControlMethod", encoding='utf8')) == 1:
        self.str_log2 = 'T={:03.1f}/{:03.1f}/{:03.1f}/{:03.1f}'.format(float(int(self.params.get("InnerLoopGain", encoding='utf8')) * 0.1), float(int(self.params.get("OuterLoopGain", encoding='utf8')) * 0.1), float(int(self.params.get("TimeConstant", encoding='utf8')) * 0.1), float(int(self.params.get("ActuatorEffectiveness", encoding='utf8')) * 0.1))
      elif int(self.params.get("LateralControlMethod", encoding='utf8')) == 2:
        self.str_log2 = 'T={:04.0f}/{:05.3f}/{:06.4f}'.format(float(int(self.params.get("Scale", encoding='utf8')) * 1.0), float(int(self.params.get("LqrKi", encoding='utf8')) * 0.001), float(int(self.params.get("DcGain", encoding='utf8')) * 0.0001))

    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 = "OpenPilot"
      elif CS.out.cruiseState.modeSel == 1:
        self.steer_mode = "Dist+Curv"
      elif CS.out.cruiseState.modeSel == 2:
        self.steer_mode = "DistOnly"
      elif CS.out.cruiseState.modeSel == 3:
        self.steer_mode = "OneWay"
      if CS.out.steerWarning == 0:
        self.mdps_status = "OK"
      elif CS.out.steerWarning == 1:
        self.mdps_status = "ERR"
      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
      if CS.lead_distance < 149:
        self.leadcar_status = "O"
      else:
        self.leadcar_status = "-"

      str_log2 = 'MODE={:s}  MDPS={:s}  LKAS={:s}  CSG={:1.0f}  LEAD={:s}'.format(self.steer_mode, self.mdps_status, self.lkas_switch, self.cruise_gap, self.leadcar_status)
      trace1.printf2( '{}'.format( str_log2 ) )

    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:
      self.standstill_status = 1
      if self.opkr_autoresume:
        # 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
          self.res_switch_timer = 0
          self.standstill_fault_reduce_timer += 1
        elif self.res_switch_timer > 0:
          self.res_switch_timer -= 1
          self.standstill_fault_reduce_timer += 1
        # at least 1 sec delay after entering the standstill
        elif 100 < self.standstill_fault_reduce_timer and 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.res_switch_timer = randint(10, 15)
          self.standstill_fault_reduce_timer += 1
        # gap save
        elif 160 < self.standstill_fault_reduce_timer and self.cruise_gap_prev == 0 and self.opkr_autoresume: 
          self.cruise_gap_prev = CS.cruiseGapSet
          self.cruise_gap_set_init = 1
        # gap adjust to 1 for fast start
        elif 160 < self.standstill_fault_reduce_timer and CS.cruiseGapSet != 1.0 and self.opkr_autoresume:
          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
        elif self.opkr_autoresume:
          self.standstill_fault_reduce_timer += 1
    # 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
      # gap restore
      if self.dRel > 17 and self.vRel < 5 and self.cruise_gap_prev != CS.cruiseGapSet and self.cruise_gap_set_init == 1 and self.opkr_autoresume:
        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 and self.opkr_autoresume:
        self.cruise_gap_set_init = 0
        self.cruise_gap_prev = 0
    
    if CS.out.brakeLights and CS.out.vEgo == 0 and not CS.acc_active:
      self.standstill_status_timer += 1
      if self.standstill_status_timer > 200:
        self.standstill_status = 1
        self.standstill_status_timer = 0
    if self.standstill_status == 1 and CS.out.vEgo > 1:
      self.standstill_status = 0
      self.standstill_fault_reduce_timer = 0
      self.v_set_dis_prev = 180
      self.last_resume_frame = frame
      self.res_switch_timer = 0
      self.resume_cnt = 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_lfahda_mfa"]:
      can_sends.append(create_lfahda_mfc(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
    def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
               visual_alert, left_lane, right_lane, left_lane_depart,
               right_lane_depart, set_speed, lead_visible):

        # *** 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)

        # 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

        # 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 self.lkas_button_on and not spas_active

        # fix for Genesis hard fault at low speed
        if CS.out.vEgo < 60 * CV.KPH_TO_MS and self.car_fingerprint == CAR.HYUNDAI_GENESIS and not CS.mdps_bus:
            lkas_active = False

        # Disable steering while turning blinker on and speed below 60 kph
        if CS.out.leftBlinker or CS.out.rightBlinker:
            if self.car_fingerprint not in [CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_H]:
                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_indicator_alert:  # set and clear by interface
            lkas_active = 0
        if self.turning_signal_timer > 0:
            self.turning_signal_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 60
        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))

        if pcm_cancel_cmd and self.longcontrol:
            can_sends.append(
                create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
                             Buttons.CANCEL, clu11_speed))
        elif CS.out.cruiseState.standstill:
            # SCC won't resume anyway when the lead distace is less than 3.7m
            # send resume at a max freq of 5Hz
            if CS.lead_distance > 3.7 and (
                    frame - self.last_resume_frame) * DT_CTRL > 0.2:
                can_sends.append(
                    create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
                                 Buttons.RES_ACCEL, clu11_speed))
                self.last_resume_frame = frame

        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))

        # DIY cruise speed code
        sm.update(0)

        lead_data = sm['radarState'].leadOne
        lead_one = sm['radarState'].leadOne
        lead_two = sm['radarState'].leadTwo

        if lead_one.status == True:
            lead_data = lead_one

        if lead_two.status == True and ((lead_one.dRel - lead_two.dRel) > 3.0):
            lead_data = lead_two

        lead_rel_dist = lead_data.dRel
        lead_rel_vel = lead_data.vRel
        lead_vel = lead_data.vLead

        cruise_curr_set_speed = CS.out.cruiseState.speed  #currently set cruise speed in m/s
        d_cru_vego = 1.11  #4km/h diff between cruise/realspeed  #cruise_curr_set_speed - CS.out.vEgo
        min_dist = 10.  #min dist to lead car to engage in meters
        max_dist = 300.  #max dist to lead car to disable acceleration and allow only deceleration
        max_cru_speed = 36.11  #limit set max cruise speed to 130km/h
        press_button_speed = 40  #press buttons at 400 milliseconds interval

        if (
                cruise_curr_set_speed - lead_vel
        ) > 8.33:  #>30km/h faster button presses on deceleration and big cruise speed gap
            press_button_speed = 5
        elif (cruise_curr_set_speed - lead_vel) > 5.55:  #>20km/h
            press_button_speed = 10
        elif (cruise_curr_set_speed - lead_vel) > 2.77:  #>10km/h
            press_button_speed = 20

        if lead_vel > (
                cruise_curr_set_speed - d_cru_vego
        ) and lead_rel_dist > 0. and cruise_curr_set_speed < max_cru_speed and (
                frame % press_button_speed == 0
                or frame % press_button_speed == 1):
            if not CS.out.cruiseState.available:  #prevent pressing up and resume prev speed
                can_sends.append(
                    create_clu11(self.packer, frame, 0, CS.clu11,
                                 Buttons.SET_DECEL, clu11_speed))  #button down
            elif lead_rel_dist > min_dist and lead_rel_dist < max_dist:
                can_sends.append(
                    create_clu11(self.packer, frame, 0, CS.clu11,
                                 Buttons.RES_ACCEL, clu11_speed))  #button up
        elif lead_vel < (cruise_curr_set_speed -
                         d_cru_vego) and lead_rel_dist > 0. and (
                             frame % press_button_speed == 0
                             or frame % press_button_speed == 1):
            can_sends.append(
                create_clu11(self.packer, frame, 0, CS.clu11,
                             Buttons.SET_DECEL, clu11_speed))  #button down

        return can_sends
Пример #5
0
    def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
               visual_alert, left_lane, right_lane, left_lane_depart,
               right_lane_depart, set_speed, lead_visible, lead_dist,
               lead_vrel, lead_yrel):

        self.enabled = enabled
        # gas and brake
        self.accel_lim_prev = self.accel_lim
        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.accel_lim = apply_accel
        apply_accel = accel_rate_limit(self.accel_lim, self.accel_lim_prev)

        # Steering Torque
        new_steer = actuators.steer * self.p.STEER_MAX
        apply_steer = apply_std_steer_torque_limits(new_steer,
                                                    self.apply_steer_last,
                                                    CS.out.steeringTorque,
                                                    self.p)
        self.steer_rate_limited = new_steer != apply_steer

        # disable if steer angle reach 90 deg, otherwise mdps fault in some models
        self.high_steer_allowed = True if self.car_fingerprint in FEATURES[
            "allow_high_steer"] else False
        lkas_active = enabled and ((abs(CS.out.steeringAngle) < 90.)
                                   or self.high_steer_allowed)

        # fix for Genesis hard fault at low speed
        if CS.out.vEgo < 55 * CV.KPH_TO_MS and self.car_fingerprint == CAR.HYUNDAI_GENESIS and CS.CP.minSteerSpeed > 0.:
            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 <= 39 * 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

        if CS.CP.radarOffCan:
            self.usestockscc = not self.cp_oplongcontrol
        elif (CS.cancel_button_count == 3) and self.cp_oplongcontrol:
            self.usestockscc = not self.usestockscc

        if not self.usestockscc:
            self.gapcount += 1
            if self.gapcount == 50 and self.gapsettingdance == 2:
                self.gapsettingdance = 1
                self.gapcount = 0
            elif self.gapcount == 50 and self.gapsettingdance == 1:
                self.gapsettingdance = 4
                self.gapcount = 0
            elif self.gapcount == 50 and self.gapsettingdance == 4:
                self.gapsettingdance = 3
                self.gapcount = 0
            elif self.gapcount == 50 and self.gapsettingdance == 3:
                self.gapsettingdance = 2
                self.gapcount = 0

        self.apply_steer_last = apply_steer

        sys_warning, sys_state, left_lane_warning, right_lane_warning =\
          process_hud_alert(enabled, self.car_fingerprint, visual_alert,
                            left_lane, right_lane, left_lane_depart, right_lane_depart)

        speed_conv = CV.MS_TO_MPH if CS.is_set_speed_in_mph else CV.MS_TO_KPH

        self.clu11_speed = CS.clu11["CF_Clu_Vanz"]

        enabled_speed = 38 if CS.is_set_speed_in_mph else 55

        if self.clu11_speed > enabled_speed or not lkas_active or CS.out.gearShifter != GearShifter.drive:
            enabled_speed = self.clu11_speed

        self.current_veh_speed = int(CS.out.vEgo * speed_conv)

        self.clu11_cnt = frame % 0x10

        can_sends = []

        self.lfa_available = True if self.lfainFingerprint or self.car_fingerprint in FEATURES[
            "send_lfa_mfa"] else False

        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,
                          self.lfa_available, 0))

        if CS.CP.mdpsHarness:  # send lkas11 bus 1 if mdps
            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,
                              self.lfa_available, 1))

            can_sends.append(
                create_clu11(self.packer, 1, CS.clu11, Buttons.NONE,
                             enabled_speed, self.clu11_cnt))

        if pcm_cancel_cmd and CS.scc12[
                "ACCMode"] != 0 and not CS.out.standstill:
            self.vdiff = 0.
            self.resumebuttoncnt = 0
            can_sends.append(
                create_clu11(self.packer, CS.CP.sccBus, CS.clu11,
                             Buttons.CANCEL, self.current_veh_speed,
                             self.clu11_cnt))
        elif CS.out.cruiseState.standstill and CS.scc12[
                "ACCMode"] != 0 and CS.vrelative > 0:
            self.vdiff += (CS.vrelative - self.vdiff)
            if (frame - self.lastresumeframe > 5) and (self.vdiff > .2 or
                                                       CS.lead_distance > 5.5):
                can_sends.append(
                    create_clu11(self.packer, CS.CP.sccBus, CS.clu11,
                                 Buttons.RES_ACCEL, self.current_veh_speed,
                                 self.resumebuttoncnt))
                self.resumebuttoncnt += 1
                if self.resumebuttoncnt > 5:
                    self.lastresumeframe = frame
                    self.resumebuttoncnt = 0
        else:
            self.vdiff = 0.
            self.resumebuttoncnt = 0

        if CS.out.vEgo < 5.:
            self.sm.update(0)
            long_control_state = self.sm['controlsState'].longControlState
            self.acc_standstill = True if long_control_state == LongCtrlState.stopping else False
        else:
            self.acc_standstill = False

        if lead_visible:
            self.lead_visible = True
            self.lead_debounce = 50
        elif self.lead_debounce > 0:
            self.lead_debounce -= 1
        else:
            self.lead_visible = lead_visible

        self.setspeed = set_speed * speed_conv

        if enabled:
            self.sendaccmode = enabled

        if CS.CP.radarDisablePossible:
            self.radarDisableOverlapTimer += 1
            self.radarDisableResetTimer = 0
            if self.radarDisableOverlapTimer >= 30:
                self.radarDisableActivated = True
                if 200 > self.radarDisableOverlapTimer > 36:
                    if frame % 41 == 0 or self.radarDisableOverlapTimer == 37:
                        can_sends.append(
                            create_scc7d0(b'\x02\x10\x03\x00\x00\x00\x00\x00'))
                    elif frame % 43 == 0 or self.radarDisableOverlapTimer == 37:
                        can_sends.append(
                            create_scc7d0(b'\x03\x28\x03\x01\x00\x00\x00\x00'))
                    elif frame % 19 == 0 or self.radarDisableOverlapTimer == 37:
                        can_sends.append(
                            create_scc7d0(b'\x02\x10\x85\x00\x00\x00\x00\x00')
                        )  # this disables RADAR for
            else:
                self.counter_init = False
                can_sends.append(
                    create_scc7d0(b'\x02\x10\x90\x00\x00\x00\x00\x00')
                )  # this enables RADAR
                can_sends.append(
                    create_scc7d0(b'\x03\x29\x03\x01\x00\x00\x00\x00'))
        elif self.radarDisableActivated:
            can_sends.append(create_scc7d0(
                b'\x02\x10\x90\x00\x00\x00\x00\x00'))  # this enables RADAR
            can_sends.append(
                create_scc7d0(b'\x03\x29\x03\x01\x00\x00\x00\x00'))
            self.radarDisableOverlapTimer = 0
            if frame % 50 == 0:
                self.radarDisableResetTimer += 1
                if self.radarDisableResetTimer > 2:
                    self.radarDisableActivated = False
                    self.counter_init = True
        else:
            self.radarDisableOverlapTimer = 0
            self.radarDisableResetTimer = 0

        if (frame % 50 == 0 or self.radarDisableOverlapTimer == 37) and \
                CS.CP.radarDisablePossible and self.radarDisableOverlapTimer >= 30:
            can_sends.append(
                create_scc7d0(b'\x02\x3E\x00\x00\x00\x00\x00\x00'))

        if self.lead_visible:
            self.objdiststat = 1 if lead_dist < 25 else 2 if lead_dist < 40 else \
                               3 if lead_dist < 60 else 4 if lead_dist < 80 else 5
        else:
            self.objdiststat = 0

        # send scc to car if longcontrol enabled and SCC not on bus 0 or ont live
        if (CS.CP.sccBus == 2 or not self.usestockscc
                or self.radarDisableActivated) and self.counter_init:
            if frame % 2 == 0:
                self.scc12cnt += 1
                self.scc12cnt %= 0xF
                self.scc11cnt += 1
                self.scc11cnt %= 0x10
                self.fca11supcnt += 1
                self.fca11supcnt %= 0xF

                if self.fca11alivecnt == 1:
                    self.fca11inc = 0
                    if self.fca11cnt13 == 3:
                        self.fca11maxcnt = 0x9
                        self.fca11cnt13 = 0
                    else:
                        self.fca11maxcnt = 0xD
                        self.fca11cnt13 += 1
                else:
                    self.fca11inc += 4

                self.fca11alivecnt = self.fca11maxcnt - self.fca11inc

                can_sends.append(
                    create_scc11(self.packer, enabled, self.setspeed,
                                 self.lead_visible, lead_dist, lead_vrel,
                                 lead_yrel, self.gapsettingdance,
                                 CS.out.standstill, CS.scc11, self.usestockscc,
                                 CS.CP.radarOffCan, self.scc11cnt,
                                 self.sendaccmode))

                can_sends.append(
                    create_scc12(self.packer, apply_accel, enabled,
                                 self.acc_standstill, CS.out.gasPressed,
                                 CS.out.brakePressed, CS.out.stockAeb,
                                 CS.scc12, self.usestockscc, CS.CP.radarOffCan,
                                 self.scc12cnt))

                can_sends.append(
                    create_scc14(self.packer, enabled, self.usestockscc,
                                 CS.out.stockAeb, apply_accel, CS.scc14,
                                 self.objdiststat, CS.out.gasPressed,
                                 self.acc_standstill, CS.out.vEgo))
                if CS.CP.fcaBus == -1:
                    can_sends.append(
                        create_fca11(self.packer, CS.fca11, self.fca11alivecnt,
                                     self.fca11supcnt))

            if frame % 20 == 0:
                can_sends.append(create_scc13(self.packer, CS.scc13))
                if CS.CP.fcaBus == -1:
                    can_sends.append(create_fca12(self.packer))
            if frame % 50 == 0:
                can_sends.append(create_scc42a(self.packer))
        else:
            self.counter_init = True
            self.scc12cnt = CS.scc12init["CR_VSM_Alive"]
            self.scc11cnt = CS.scc11init["AliveCounterACC"]
            self.fca11alivecnt = CS.fca11init["CR_FCA_Alive"]
            self.fca11supcnt = CS.fca11init["Supplemental_Counter"]

        # 20 Hz LFA MFA message
        if frame % 5 == 0 and self.lfa_available:
            can_sends.append(create_lfa_mfa(self.packer, frame, enabled))

        return can_sends
Пример #6
0
    def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
               visual_alert, left_lane, right_lane, left_lane_depart,
               right_lane_depart, set_speed, lead_visible):
        # Steering Torque
        self.scc11_cnt %= 16
        self.scc12_cnt %= 0xF
        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

        apply_accel = actuators.gas - actuators.brake
        apply_accel, self.accel_steady = accel_hysteresis(
            apply_accel, self.accel_steady, enabled)
        apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
        # disable if steer angle reach 90 deg, otherwise mdps fault in some models
        lkas_active = enabled and abs(CS.out.steeringAngle) < 90.

        # fix for Genesis hard fault at low speed
        if CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS:
            lkas_active = 0

        if not lkas_active:
            apply_steer = 0

        self.apply_steer_last = apply_steer

        sys_warning, sys_state, left_lane_warning, right_lane_warning =\
          process_hud_alert(enabled, self.car_fingerprint, visual_alert,
                            left_lane, right_lane, left_lane_depart, right_lane_depart)

        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))

        if pcm_cancel_cmd:
            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:
                # 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.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

        if frame % 2 == 0:
            #cloudlog.info("create_scc11(self.packer, %d, %d)" % (frame, self.scc11_cnt))
            can_sends.append(
                create_scc11(self.packer, enabled, self.scc11_cnt, set_speed,
                             lead_visible))
            self.scc11_cnt += 1
            self.scc12_cnt += 1
            #cloudlog.info("create_scc12(self.packer, %d, %d, %d)" % (apply_accel, enabled, self.scc12_cnt))
            can_sends.append(
                create_scc12(self.packer, apply_accel, enabled,
                             self.scc12_cnt))
            can_sends.append(create_scc14(self.packer, enabled))
            #cloudlog.info("create_scc14(self.packer, %d)" % (enabled))

        if frame % 20 == 0:
            can_sends.append(create_scc13(self.packer))
            #cloudlog.info("create_scc13(self.packer)")
        # if frame % 50 == 0:
        #   can_sends.append(create_4a2SCC(self.packer))
        #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))

        return can_sends
Пример #7
0
    def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
               visual_alert, left_lane, right_lane, left_lane_depart,
               right_lane_depart, set_speed, lead_visible, lead_dist,
               lead_vrel, lead_yrel, sm):

        # *** compute control surfaces ***

        # gas and brake
        self.accel_lim_prev = self.accel_lim
        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.accel_lim = apply_accel
        apply_accel = accel_rate_limit(self.accel_lim, self.accel_lim_prev)

        param = self.p

        #self.model_speed = 255 - self.SC.calc_va(sm, CS.out.vEgo)
        #atom model_speed
        #self.model_speed = self.SC.cal_model_speed(sm, CS.out.vEgo)
        if frame % 10 == 0:
            self.curve_speed = self.SC.cal_curve_speed(sm, CS.out.vEgo)

        plan = sm['longitudinalPlan']
        self.dRel = int(plan.dRel1)  #EON Lead
        self.yRel = int(plan.yRel1)  #EON Lead
        self.vRel = int(plan.vRel1 * 3.6 + 0.5)  #EON Lead
        self.dRel2 = int(plan.dRel2)  #EON Lead
        self.yRel2 = int(plan.yRel2)  #EON Lead
        self.vRel2 = int(plan.vRel2 * 3.6 + 0.5)  #EON Lead
        self.lead2_status = plan.status2

        lateral_plan = sm['lateralPlan']
        self.outScale = lateral_plan.outputScale
        self.vCruiseSet = lateral_plan.vCruiseSet

        #self.model_speed = interp(abs(lateral_plan.vCurvature), [0.0002, 0.01], [255, 30])
        #Hoya
        self.model_speed = interp(abs(lateral_plan.vCurvature),
                                  [0.0, 0.0002, 0.00074, 0.0025, 0.008, 0.02],
                                  [255, 255, 130, 90, 60, 20])

        if CS.out.vEgo > 8:
            if self.variable_steer_max:
                self.steerMax = interp(int(abs(self.model_speed)),
                                       self.model_speed_range,
                                       self.steerMax_range)
            else:
                self.steerMax = self.steerMax_base
            if self.variable_steer_delta:
                self.steerDeltaUp = interp(int(abs(self.model_speed)),
                                           self.model_speed_range,
                                           self.steerDeltaUp_range)
                self.steerDeltaDown = interp(int(abs(self.model_speed)),
                                             self.model_speed_range,
                                             self.steerDeltaDown_range)
            else:
                self.steerDeltaUp = self.steerDeltaUp_base
                self.steerDeltaDown = self.steerDeltaDown_base
        else:
            self.steerMax = self.steerMax_base
            self.steerDeltaUp = self.steerDeltaUp_base
            self.steerDeltaDown = self.steerDeltaDown_base

        param.STEER_MAX = min(CarControllerParams.STEER_MAX,
                              self.steerMax)  # variable steermax
        param.STEER_DELTA_UP = min(CarControllerParams.STEER_DELTA_UP,
                                   self.steerDeltaUp)  # variable deltaUp
        param.STEER_DELTA_DOWN = min(CarControllerParams.STEER_DELTA_DOWN,
                                     self.steerDeltaDown)  # variable deltaDown

        # Steering Torque
        if 0 <= self.driver_steering_torque_above_timer < 100:
            new_steer = int(
                round(actuators.steer * self.steerMax *
                      (self.driver_steering_torque_above_timer / 100)))
        else:
            new_steer = int(round(actuators.steer * self.steerMax))
        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
        if self.opkr_maxanglelimit >= 90 and not self.steer_wind_down_enabled:
            lkas_active = enabled and abs(
                CS.out.steeringAngleDeg
            ) < self.opkr_maxanglelimit and CS.out.gearShifter == GearShifter.drive
        else:
            lkas_active = enabled and not CS.out.steerWarning and CS.out.gearShifter == GearShifter.drive

        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 and self.opkr_turnsteeringdisable:
            self.lanechange_manual_timer = 50
        if CS.out.leftBlinker and CS.out.rightBlinker:
            self.emergency_manual_timer = 50
        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 abs(CS.out.steeringTorque
               ) > 180 and CS.out.vEgo < LANE_CHANGE_SPEED_MIN:
            self.driver_steering_torque_above = True
        else:
            self.driver_steering_torque_above = False

        if self.driver_steering_torque_above == True:
            self.driver_steering_torque_above_timer -= 1
            if self.driver_steering_torque_above_timer <= 0:
                self.driver_steering_torque_above_timer = 0
        elif self.driver_steering_torque_above == False:
            self.driver_steering_torque_above_timer += 5
            if self.driver_steering_torque_above_timer >= 100:
                self.driver_steering_torque_above_timer = 100

        if not lkas_active:
            apply_steer = 0
            if self.apply_steer_last != 0:
                self.steer_wind_down = 1
        if lkas_active or CS.out.steeringPressed:
            self.steer_wind_down = 0

        self.apply_accel_last = apply_accel
        self.apply_steer_last = apply_steer

        if CS.acc_active and CS.lead_distance > 149 and self.dRel < ((CS.out.vEgo * CV.MS_TO_KPH)+5) < 100 and \
         self.vRel < -(CS.out.vEgo * CV.MS_TO_KPH * 0.16) and CS.out.vEgo > 7 and abs(CS.out.steeringAngleDeg) < 10 and not self.longcontrol:
            self.need_brake_timer += 1
            if self.need_brake_timer > 50:
                self.need_brake = True
        else:
            self.need_brake = False
            self.need_brake_timer = 0

        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)

        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 or CS.out.gearShifter != GearShifter.drive:
            enabled_speed = clu11_speed

        can_sends = []
        can_sends.append(
            create_lkas11(self.packer, frame, self.car_fingerprint,
                          apply_steer, lkas_active, self.steer_wind_down,
                          CS.lkas11, sys_warning, sys_state, enabled,
                          left_lane, right_lane, left_lane_warning,
                          right_lane_warning, 0, self.ldws_fix,
                          self.steer_wind_down_enabled))

        if CS.CP.mdpsBus:  # send lkas11 bus 1 if mdps is bus 1
            can_sends.append(
                create_lkas11(self.packer, frame, self.car_fingerprint,
                              apply_steer, lkas_active, self.steer_wind_down,
                              CS.lkas11, sys_warning, sys_state, enabled,
                              left_lane, right_lane, left_lane_warning,
                              right_lane_warning, 1, self.ldws_fix,
                              self.steer_wind_down_enabled))
            if frame % 2:  # send clu11 to mdps if it is not on bus 0
                can_sends.append(
                    create_clu11(self.packer, frame, CS.clu11, Buttons.NONE,
                                 enabled_speed, CS.CP.mdpsBus))

        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.opkr_variablecruise and CS.acc_active and (
            CS.out.cruiseState.modeSel > 0)
        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차선"
            elif CS.out.cruiseState.modeSel == 4:
                self.steer_mode = "맵감속ONLY"
            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
            if CS.lead_distance < 149:
                self.leadcar_status = "O"
            else:
                self.leadcar_status = "-"

            str_log2 = 'MODE={:s}  MDPS={:s}  LKAS={:s}  CSG={:1.0f}  LEAD={:s}  FR={:03.0f}'.format(self.steer_mode, \
             self.mdps_status, self.lkas_switch, self.cruise_gap, self.leadcar_status, self.timer1.sampleTime())
            trace1.printf2('{}'.format(str_log2))

        if pcm_cancel_cmd and self.longcontrol:
            can_sends.append(
                create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL,
                             clu11_speed, CS.CP.sccBus))

        if CS.out.cruiseState.standstill:
            self.standstill_status = 1
            if self.opkr_autoresume:
                # 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
                    self.res_switch_timer = 0
                    self.standstill_fault_reduce_timer += 1
                elif self.res_switch_timer > 0:
                    self.res_switch_timer -= 1
                    self.standstill_fault_reduce_timer += 1
                # at least 1 sec delay after entering the standstill
                elif 100 < self.standstill_fault_reduce_timer and CS.lead_distance != self.last_lead_distance:
                    self.acc_standstill_timer = 0
                    self.acc_standstill = False
                    can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)) if not self.longcontrol \
                     else can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL, clu11_speed, CS.CP.sccBus))
                    self.resume_cnt += 1
                    if self.resume_cnt > 5:
                        self.resume_cnt = 0
                        self.res_switch_timer = randint(10, 15)
                    self.standstill_fault_reduce_timer += 1
                # gap save
                elif 160 < self.standstill_fault_reduce_timer and self.cruise_gap_prev == 0 and self.opkr_autoresume and self.opkr_cruisegap_auto_adj:
                    self.cruise_gap_prev = CS.cruiseGapSet
                    self.cruise_gap_set_init = 1
                # gap adjust to 1 for fast start
                elif 160 < self.standstill_fault_reduce_timer and CS.cruiseGapSet != 1.0 and self.opkr_autoresume and self.opkr_cruisegap_auto_adj:
                    self.cruise_gap_switch_timer += 1
                    if self.cruise_gap_switch_timer > 100:
                        can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.GAP_DIST)) if not self.longcontrol \
                         else can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.GAP_DIST, clu11_speed, CS.CP.sccBus))
                        self.cruise_gap_switch_timer = 0
                elif self.opkr_autoresume:
                    self.standstill_fault_reduce_timer += 1
        # 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.clu11, self.SC.btn_type, self.SC.sc_clu_speed)) if not self.longcontrol \
                 else can_sends.append(create_clu11(self.packer, self.resume_cnt, CS.clu11, self.SC.btn_type, self.SC.sc_clu_speed, CS.CP.sccBus))
                self.resume_cnt += 1
            else:
                self.resume_cnt = 0
            if self.opkr_cruisegap_auto_adj:
                # gap restore
                if self.dRel > 17 and self.vRel < 5 and self.cruise_gap_prev != CS.cruiseGapSet and self.cruise_gap_set_init == 1 and self.opkr_autoresume:
                    self.cruise_gap_switch_timer += 1
                    if self.cruise_gap_switch_timer > 50:
                        can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.GAP_DIST)) if not self.longcontrol \
                         else can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.GAP_DIST, clu11_speed, CS.CP.sccBus))
                        self.cruise_gap_switch_timer = 0
                elif self.cruise_gap_prev == CS.cruiseGapSet and self.opkr_autoresume:
                    self.cruise_gap_set_init = 0
                    self.cruise_gap_prev = 0

        if CS.cruise_buttons == 4:
            self.cancel_counter += 1
        elif CS.acc_active:
            self.cancel_counter = 0
            if self.res_speed_timer > 0:
                self.res_speed_timer -= 1
            else:
                self.v_cruise_kph_auto_res = 0
                self.res_speed = 0

        if self.model_speed > 95 and self.cancel_counter == 0 and not CS.acc_active and not CS.out.brakeLights and int(CS.VSetDis) > 30 and \
         (CS.lead_distance < 149 or int(CS.clu_Vanz) > 30) and int(CS.clu_Vanz) >= 3 and self.auto_res_timer <= 0 and self.opkr_cruise_auto_res:
            if self.opkr_cruise_auto_res_option == 0:
                can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)) if not self.longcontrol \
                 else can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL, clu11_speed, CS.CP.sccBus))  # auto res
                self.res_speed = int(CS.clu_Vanz * 1.1)
                self.res_speed_timer = 350
            elif self.opkr_cruise_auto_res_option == 1:
                can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.SET_DECEL)) if not self.longcontrol \
                 else can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.SET_DECEL, clu11_speed, CS.CP.sccBus)) # auto res but set_decel to set current speed
                self.v_cruise_kph_auto_res = int(CS.clu_Vanz)
                self.res_speed_timer = 50
            if self.auto_res_timer <= 0:
                self.auto_res_timer = randint(10, 15)
        elif self.auto_res_timer > 0 and self.opkr_cruise_auto_res:
            self.auto_res_timer -= 1

        if CS.out.brakeLights and CS.out.vEgo == 0 and not CS.acc_active:
            self.standstill_status_timer += 1
            if self.standstill_status_timer > 200:
                self.standstill_status = 1
                self.standstill_status_timer = 0
        if self.standstill_status == 1 and CS.out.vEgo > 1:
            self.standstill_status = 0
            self.standstill_fault_reduce_timer = 0
            self.last_resume_frame = frame
            self.res_switch_timer = 0
            self.resume_cnt = 0

        if CS.out.vEgo <= 1:
            self.sm.update(0)
            long_control_state = self.sm['controlsState'].longControlState
            if long_control_state == LongCtrlState.stopping and CS.out.vEgo < 0.1 and not CS.out.gasPressed:
                self.acc_standstill_timer += 1
                if self.acc_standstill_timer >= 200:
                    self.acc_standstill_timer = 200
                    self.acc_standstill = True
            else:
                self.acc_standstill_timer = 0
                self.acc_standstill = False
        elif CS.out.gasPressed or CS.out.vEgo > 1:
            self.acc_standstill = False
            self.acc_standstill_timer = 0
        else:
            self.acc_standstill = False
            self.acc_standstill_timer = 0

        if CS.CP.mdpsBus:  # send mdps12 to LKAS to prevent LKAS error
            can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))

        if CS.CP.sccBus != 0 and self.counter_init and self.longcontrol:
            if frame % 2 == 0:
                self.scc12cnt += 1
                self.scc12cnt %= 0xF
                self.scc11cnt += 1
                self.scc11cnt %= 0x10
                self.fca11supcnt += 1
                self.fca11supcnt %= 0xF
                if self.fca11alivecnt == 1:
                    self.fca11inc = 0
                    if self.fca11cnt13 == 3:
                        self.fca11maxcnt = 0x9
                        self.fca11cnt13 = 0
                    else:
                        self.fca11maxcnt = 0xD
                        self.fca11cnt13 += 1
                else:
                    self.fca11inc += 4
                self.fca11alivecnt = self.fca11maxcnt - self.fca11inc
                lead_objspd = CS.lead_objspd  # vRel (km/h)
                aReqValue = CS.scc12["aReqValue"]
                if 0 < CS.out.radarDistance <= 149:
                    if aReqValue > 0.:
                        stock_weight = interp(CS.out.radarDistance, [3., 25.],
                                              [0.8, 0.])
                    elif aReqValue < 0.:
                        stock_weight = interp(CS.out.radarDistance, [3., 25.],
                                              [1., 0.])
                        if lead_objspd < 0:
                            vRel_weight = interp(abs(lead_objspd), [0, 25],
                                                 [1, 2])
                            stock_weight = interp(
                                CS.out.radarDistance,
                                [3.**vRel_weight, 25. * vRel_weight], [1., 0.])
                    else:
                        stock_weight = 0.
                    apply_accel = apply_accel * (
                        1. - stock_weight) + aReqValue * stock_weight
                else:
                    stock_weight = 0.
                can_sends.append(
                    create_scc11(self.packer, frame, set_speed, lead_visible,
                                 self.scc_live, lead_dist, lead_vrel,
                                 lead_yrel, self.car_fingerprint,
                                 CS.out.vEgo * CV.MS_TO_KPH,
                                 self.acc_standstill, CS.scc11))
                if (CS.brake_check
                        or CS.cancel_check) and self.car_fingerprint not in [
                            CAR.NIRO_EV
                        ]:
                    can_sends.append(
                        create_scc12(self.packer, apply_accel, enabled,
                                     self.scc_live, CS.out.gasPressed, 1,
                                     CS.out.stockAeb, self.car_fingerprint,
                                     CS.out.vEgo * CV.MS_TO_KPH, CS.scc12))
                else:
                    can_sends.append(
                        create_scc12(self.packer, apply_accel, enabled,
                                     self.scc_live, CS.out.gasPressed,
                                     CS.out.brakePressed, CS.out.stockAeb,
                                     self.car_fingerprint,
                                     CS.out.vEgo * CV.MS_TO_KPH, CS.scc12))
                can_sends.append(
                    create_scc14(self.packer, enabled, CS.scc14,
                                 CS.out.stockAeb, lead_visible, lead_dist,
                                 CS.out.vEgo, self.acc_standstill,
                                 self.car_fingerprint))
                if CS.CP.fcaBus == -1:
                    can_sends.append(
                        create_fca11(self.packer, CS.fca11, self.fca11alivecnt,
                                     self.fca11supcnt))
            if frame % 20 == 0:
                can_sends.append(create_scc13(self.packer, CS.scc13))
                if CS.CP.fcaBus == -1:
                    can_sends.append(create_fca12(self.packer))
            if frame % 50 == 0:
                can_sends.append(create_scc42a(self.packer))
        elif CS.CP.sccBus == 2 and self.longcontrol:
            self.counter_init = True
            self.scc12cnt = CS.scc12init["CR_VSM_Alive"]
            self.scc11cnt = CS.scc11init["AliveCounterACC"]
            self.fca11alivecnt = CS.fca11init["CR_FCA_Alive"]
            self.fca11supcnt = CS.fca11init["Supplemental_Counter"]

        aq_value = CS.scc12["aReqValue"] if CS.CP.sccBus == 0 else apply_accel
        if self.apks_enabled:
            str_log1 = 'M/C={:03.0f}/{:03.0f}  TQ={:03.0f}  ST={:03.0f}/{:01.0f}/{:01.0f}  AQ={:+04.2f}'.format(abs(self.model_speed), self.curve_speed, \
             abs(new_steer), max(self.steerMax, abs(new_steer)), self.steerDeltaUp, self.steerDeltaDown, aq_value)
        else:
            str_log1 = 'M/C={:03.0f}/{:03.0f}  TQ={:03.0f}  ST={:03.0f}/{:01.0f}/{:01.0f}  AQ={:+04.2f}  S={:.0f}/{:.0f}'.format(abs(self.model_speed), self.curve_speed, \
             abs(new_steer), max(self.steerMax, abs(new_steer)), self.steerDeltaUp, self.steerDeltaDown, aq_value, int(CS.is_highway), CS.safety_sign_check)

        trace1.printf1('{}  {}'.format(str_log1, self.str_log2))

        # 20 Hz LFA MFA message
        if frame % 5 == 0 and self.car_fingerprint in FEATURES[
                "send_lfahda_mfa"]:
            can_sends.append(create_lfahda_mfc(self.packer, frame,
                                               lkas_active))

        return can_sends
Пример #8
0
  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
Пример #9
0
    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)

        param = self.p

        self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo)

        plan = sm['plan']
        self.dRel = int(plan.dRel1)  #EON Lead
        self.yRel = int(plan.yRel1)  #EON Lead
        self.vRel = int(plan.vRel1 * 3.6 + 0.5)  #EON Lead
        self.dRel2 = int(plan.dRel2)  #EON Lead
        self.yRel2 = int(plan.yRel2)  #EON Lead
        self.vRel2 = int(plan.vRel2 * 3.6 + 0.5)  #EON Lead
        self.lead2_status = plan.status2
        self.target_map_speed = plan.targetSpeed
        self.target_map_speed_camera = plan.targetSpeedCamera

        self.accActive = CS.acc_active

        path_plan = sm['pathPlan']
        self.outScale = path_plan.outputScale
        self.vCruiseSet = path_plan.vCruiseSet

        self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset
        self.angle_steers = CS.out.steeringAngle
        self.angle_diff = abs(self.angle_steers_des) - abs(self.angle_steers)

        if abs(self.outScale) >= 1 and CS.out.vEgo > 8:
            self.steerMax_prev = interp(self.angle_diff,
                                        self.angle_differ_range,
                                        self.steerMax_range)
            if self.steerMax_prev > self.steerMax:
                self.steerMax = self.steerMax_prev
            self.steerDeltaUp_prev = interp(self.angle_diff,
                                            self.angle_differ_range,
                                            self.steerDeltaUp_range)
            if self.steerDeltaUp_prev > self.steerDeltaUp:
                self.steerDeltaUp = self.steerDeltaUp_prev
            self.steerDeltaDown_prev = interp(self.angle_diff,
                                              self.angle_differ_range,
                                              self.steerDeltaDown_range)
            if self.steerDeltaDown_prev > self.steerDeltaDown:
                self.steerDeltaDown = self.steerDeltaDown_prev

        #if abs(self.outScale) >= 0.9 and CS.out.vEgo > 8:
        #  self.steerMax_timer += 1
        #  self.steerDeltaUp_timer += 1
        #  self.steerDeltaDown_timer += 1
        #  if self.steerMax_timer > 5:
        #    self.steerMax += int(CS.out.vEgo//2)
        #    self.steerMax_timer = 0
        #    if self.steerMax >= int(self.params.get('SteerMaxAdj')):
        #      self.steerMax = int(self.params.get('SteerMaxAdj'))
        #  if self.steerDeltaUp_timer > 50:
        #    self.steerDeltaUp += 1
        #    self.steerDeltaUp_timer = 0
        #    if self.steerDeltaUp >= 7:
        #      self.steerDeltaUp = 7
        #  if self.steerDeltaDown_timer > 25:
        #    self.steerDeltaDown += 1
        #    self.steerDeltaDown_timer = 0
        #    if self.steerDeltaDown >= 15:
        #      self.steerDeltaDown = 15
        else:
            self.steerMax_timer += 1
            self.steerDeltaUp_timer += 1
            self.steerDeltaDown_timer += 1
            if self.steerMax_timer > 20:
                self.steerMax -= 5
                self.steerMax_timer = 0
                if self.steerMax < int(self.params.get('SteerMaxBaseAdj')):
                    self.steerMax = int(self.params.get('SteerMaxBaseAdj'))
            if self.steerDeltaUp_timer > 100:
                self.steerDeltaUp -= 1
                self.steerDeltaUp_timer = 0
                if self.steerDeltaUp <= int(
                        self.params.get('SteerDeltaUpAdj')):
                    self.steerDeltaUp = int(self.params.get('SteerDeltaUpAdj'))
            if self.steerDeltaDown_timer > 50:
                self.steerDeltaDown -= 1
                self.steerDeltaDown_timer = 0
                if self.steerDeltaDown <= int(
                        self.params.get('SteerDeltaDownAdj')):
                    self.steerDeltaDown = int(
                        self.params.get('SteerDeltaDownAdj'))

        param.STEER_MAX = min(SteerLimitParams.STEER_MAX, self.steerMax)
        param.STEER_DELTA_UP = max(int(self.params.get('SteerDeltaUpAdj')),
                                   self.steerDeltaUp)
        param.STEER_DELTA_DOWN = max(int(self.params.get('SteerDeltaDownAdj')),
                                     self.steerDeltaDown)

        # Steering Torque
        if 0 <= self.driver_steering_torque_above_timer < 100:
            new_steer = actuators.steer * self.steerMax * (
                self.driver_steering_torque_above_timer / 100)
        else:
            new_steer = actuators.steer * self.steerMax
        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

        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
        if self.opkr_maxanglelimit >= 90:
            lkas_active = enabled and abs(
                CS.out.steeringAngle
            ) < self.opkr_maxanglelimit and not spas_active
        else:
            lkas_active = enabled and not spas_active

        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 = 50
        if CS.out.leftBlinker and CS.out.rightBlinker:
            self.emergency_manual_timer = 50
        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 abs(CS.out.steeringTorque
               ) > 200 and CS.out.vEgo < LANE_CHANGE_SPEED_MIN:
            self.driver_steering_torque_above = True
        else:
            self.driver_steering_torque_above = False

        if self.driver_steering_torque_above == True:
            self.driver_steering_torque_above_timer -= 1
            if self.driver_steering_torque_above_timer <= 0:
                self.driver_steering_torque_above_timer = 0
        elif self.driver_steering_torque_above == False:
            self.driver_steering_torque_above_timer += 5
            if self.driver_steering_torque_above_timer >= 100:
                self.driver_steering_torque_above_timer = 100

        if not lkas_active:
            apply_steer = 0

        self.apply_accel_last = apply_accel
        self.apply_steer_last = apply_steer

        if CS.acc_active and CS.lead_distance > 149 and self.dRel < (
            (CS.out.vEgo * CV.MS_TO_KPH) +
                3) and self.vRel < -5 and CS.out.vEgo > 7:
            self.need_brake_timer += 1
            if self.need_brake_timer > 50:
                self.need_brake = True
        else:
            self.need_brake = False
            self.need_brake_timer = 0

        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 = 'CV={:03.0f}  TQ={:03.0f}  R={:03.0f}  ST={:03.0f}/{:01.0f}/{:01.0f}'.format(
            abs(self.model_speed), abs(new_steer), self.timer1.sampleTime(),
            self.steerMax, self.steerDeltaUp, self.steerDeltaDown)
        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
            if CS.lead_distance < 149:
                self.leadcar_status = "O"
            else:
                self.leadcar_status = "-"

            str_log2 = 'MODE={:s}  MDPS={:s}  LKAS={:s}  CSG={:1.0f}  LEAD={:s}'.format(
                self.steer_mode, self.mdps_status, self.lkas_switch,
                self.cruise_gap, self.leadcar_status)
            trace1.printf2('{}'.format(str_log2))

        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:
            self.standstill_status = 1
            if self.opkr_autoresumeoption == 1 and self.opkr_autoresume:
                # 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
                    self.res_switch_timer = 0
                    self.standstill_fault_reduce_timer += 1
                elif self.res_switch_timer > 0:
                    self.res_switch_timer -= 1
                    self.standstill_fault_reduce_timer += 1
                # standstill 진입하자마자 바로 누르지 말고 최소 1초의 딜레이를 주기 위함
                elif 100 < self.standstill_fault_reduce_timer and 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.res_switch_timer = randint(10, 15)
                    self.standstill_fault_reduce_timer += 1
                # gap save
                elif 160 < self.standstill_fault_reduce_timer and self.cruise_gap_prev == 0 and self.opkr_autoresume:
                    self.cruise_gap_prev = CS.cruiseGapSet
                    self.cruise_gap_set_init = 1
                # gap adjust to 1 for fast start
                elif 160 < self.standstill_fault_reduce_timer and CS.cruiseGapSet != 1.0 and self.opkr_autoresume:
                    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
                elif self.opkr_autoresume:
                    self.standstill_fault_reduce_timer += 1
            elif self.opkr_autoresumeoption == 0 and self.opkr_autoresume:
                # 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 and self.opkr_autoresume:
                    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 and self.opkr_autoresume:
                    self.cruise_gap_prev = CS.cruiseGapSet
                    self.cruise_gap_set_init = 1
                elif CS.cruiseGapSet != 1.0 and self.opkr_autoresume:
                    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
            # gap restore
            if self.dRel > 17 and self.vRel < 5 and self.cruise_gap_prev != CS.cruiseGapSet and self.cruise_gap_set_init == 1 and self.opkr_autoresume:
                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 and self.opkr_autoresume:
                self.cruise_gap_set_init = 0
                self.cruise_gap_prev = 0
            #if CS.out.vEgo > 8 and self.lead2_status and self.dRel - self.dRel2 > 3 and self.cut_in_detection == 0 and self.cruise_gap_prev2 == 0:
            #  self.cut_in_detection = 1
            #  self.cruise_gap_prev2 = CS.cruiseGapSet
            #elif CS.out.vEgo > 8 and self.cut_in_detection == 1 and CS.cruiseGapSet != 1.0:
            #  self.cruise_gap_switch_timer2 += 1
            #  if self.cruise_gap_switch_timer2 > 150:
            #    can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.GAP_DIST, clu11_speed))
            #    self.cruise_gap_switch_timer2 = 0
            #elif CS.out.vEgo > 8 and self.cut_in_detection == 1 and CS.cruiseGapSet == 1.0:
            #  self.cruise_gap_switch_timer2 += 1
            #  if self.cruise_gap_switch_timer2 > 600:
            #    if self.cruise_gap_prev2 != CS.cruiseGapSet:
            #      self.cruise_gap_switch_timer3 += 1
            #      if self.cruise_gap_switch_timer3 > 50:
            #        can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.GAP_DIST, clu11_speed))
            #        self.cruise_gap_switch_timer3 = 0
            #elif self.cruise_gap_prev2 == CS.cruiseGapSet:
            #  self.cut_in_detection == 0
            #  self.cruise_gap_prev2 = 0
            #  self.cruise_gap_switch_timer2 = 0
            #  self.cruise_gap_switch_timer3 = 0

        if CS.out.brakeLights and CS.out.vEgo == 0 and not CS.acc_active:
            self.standstill_status_timer += 1
            if self.standstill_status_timer > 200:
                self.standstill_status = 1
                self.standstill_status_timer = 0
        if self.standstill_status == 1 and CS.out.vEgo > 1:
            self.standstill_status = 0
            self.standstill_fault_reduce_timer = 0
            self.v_set_dis_prev = 180
            self.last_resume_frame = frame
            self.res_switch_timer = 0
            self.resume_cnt = 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
Пример #10
0
    def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
               visual_alert, left_lane, right_lane, left_lane_depart,
               right_lane_depart, set_speed, lead_visible, lead_dist,
               lead_vrel, lead_yrel, sm):

        self.enabled = enabled

        param = self.p

        self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo)

        plan = sm['plan']
        self.dRel = int(plan.dRel1)  #EON Lead
        self.yRel = int(plan.yRel1)  #EON Lead
        self.vRel = int(plan.vRel1 * 3.6 + 0.5)  #EON Lead
        self.dRel2 = int(plan.dRel2)  #EON Lead
        self.yRel2 = int(plan.yRel2)  #EON Lead
        self.vRel2 = int(plan.vRel2 * 3.6 + 0.5)  #EON Lead
        self.lead2_status = plan.status2
        self.target_map_speed = plan.targetSpeed
        self.target_map_speed_camera = plan.targetSpeedCamera
        path_plan = sm['pathPlan']
        self.outScale = path_plan.outputScale
        self.angle_steers = CS.out.steeringAngle
        self.vCruiseSet = path_plan.vCruiseSet

        if CS.out.vEgo > 8:
            if self.variable_steer_max:
                #self.steerMax = interp(int(abs(self.model_speed)), self.model_speed_range, self.steerMax_range)
                self.steerMax = interp(abs(self.angle_steers),
                                       self.angle_range, self.steerMax_range)
            else:
                self.steerMax = int(self.params.get('SteerMaxBaseAdj'))
            if self.variable_steer_delta:
                #self.steerDeltaUp = interp(int(abs(self.model_speed)), self.model_speed_range, self.steerDeltaUp_range)
                #self.steerDeltaDown = interp(int(abs(self.model_speed)), self.model_speed_range, self.steerDeltaDown_range)
                self.steerDeltaUp = interp(abs(self.angle_steers),
                                           self.angle_range,
                                           self.steerDeltaUp_range)
                self.steerDeltaDown = interp(abs(self.angle_steers),
                                             self.angle_range,
                                             self.steerDeltaDown_range)
            else:
                self.steerDeltaUp = int(self.params.get('SteerDeltaUpAdj'))
                self.steerDeltaDown = int(self.params.get('SteerDeltaDownAdj'))
        else:
            self.steerMax = int(self.params.get('SteerMaxBaseAdj'))
            self.steerDeltaUp = int(self.params.get('SteerDeltaUpAdj'))
            self.steerDeltaDown = int(self.params.get('SteerDeltaDownAdj'))

        param.STEER_MAX = min(SteerLimitParams.STEER_MAX,
                              self.steerMax)  # variable steermax
        param.STEER_DELTA_UP = max(int(self.params.get('SteerDeltaUpAdj')),
                                   self.steerDeltaUp)  # variable deltaUp
        param.STEER_DELTA_DOWN = max(int(self.params.get('SteerDeltaDownAdj')),
                                     self.steerDeltaDown)  # variable deltaDown
        #param.STEER_DELTA_UP = SteerLimitParams.STEER_DELTA_UP # fixed deltaUp
        #param.STEER_DELTA_DOWN = SteerLimitParams.STEER_DELTA_DOWN # fixed deltaDown

        # Steering Torque
        if 0 <= self.driver_steering_torque_above_timer < 100:
            new_steer = actuators.steer * self.steerMax * (
                self.driver_steering_torque_above_timer / 100)
        else:
            new_steer = actuators.steer * self.steerMax
        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
        self.high_steer_allowed = True if self.car_fingerprint in FEATURES[
            "allow_high_steer"] else False
        if self.opkr_maxanglelimit >= 90:
            lkas_active = enabled and abs(
                CS.out.steeringAngle) < self.opkr_maxanglelimit
        else:
            lkas_active = enabled

        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 = 50
        if CS.out.leftBlinker and CS.out.rightBlinker:
            self.emergency_manual_timer = 50
        #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 abs(CS.out.steeringTorque
               ) > 200 and CS.out.vEgo < LANE_CHANGE_SPEED_MIN:
            self.driver_steering_torque_above = True
        else:
            self.driver_steering_torque_above = False

        if self.driver_steering_torque_above == True:
            self.driver_steering_torque_above_timer -= 1
            if self.driver_steering_torque_above_timer <= 0:
                self.driver_steering_torque_above_timer = 0
        elif self.driver_steering_torque_above == False:
            self.driver_steering_torque_above_timer += 5
            if self.driver_steering_torque_above_timer >= 100:
                self.driver_steering_torque_above_timer = 100

        if not lkas_active:
            apply_steer = 0

        if CS.CP.radarOffCan:
            self.usestockscc = not self.cp_oplongcontrol
        elif (CS.cancel_button_count == 3) and self.cp_oplongcontrol:
            self.usestockscc = not self.usestockscc

        if self.prev_gapButton != CS.cruise_buttons:  # gap change.
            if CS.cruise_buttons == 3:
                self.gapsettingdance -= 1
            if self.gapsettingdance < 1:
                self.gapsettingdance = 4
            self.prev_gapButton = CS.cruise_buttons

        self.apply_steer_last = apply_steer

        sys_warning, sys_state, left_lane_warning, right_lane_warning =\
          process_hud_alert(enabled, self.car_fingerprint, visual_alert,
                            left_lane, right_lane, left_lane_depart, right_lane_depart)

        speed_conv = CV.MS_TO_MPH if CS.is_set_speed_in_mph else CV.MS_TO_KPH

        self.clu11_speed = CS.clu11["CF_Clu_Vanz"]

        enabled_speed = 38 if CS.is_set_speed_in_mph else 55

        if self.clu11_speed > enabled_speed or not lkas_active or CS.out.gearShifter != GearShifter.drive:
            enabled_speed = self.clu11_speed

        self.current_veh_speed = int(CS.out.vEgo * speed_conv)

        self.clu11_cnt = frame % 0x10

        can_sends = []

        self.lfa_available = True if self.lfainFingerprint or self.car_fingerprint in FEATURES[
            "send_lfa_mfa"] else False

        if (frame % 10) == 0:
            # tester present - w/ no response (keeps radar disabled)
            can_sends.append(
                [0x7D0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0])

        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,
                          self.lfa_available, 0))

        if CS.CP.mdpsHarness:  # send lkas11 bus 1 if mdps
            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,
                              self.lfa_available, 1))

            can_sends.append(
                create_clu11(self.packer, 1, CS.clu11, Buttons.NONE,
                             enabled_speed, self.clu11_cnt))

        str_log1 = 'CV={:03.0f}  TQ={:03.0f}  R={:03.0f}  ST={:03.0f}/{:01.0f}/{:01.0f}  G={:01.0f}'.format(
            abs(self.model_speed), abs(new_steer), self.timer1.sampleTime(),
            self.steerMax, self.steerDeltaUp, self.steerDeltaDown,
            CS.out.cruiseGapSet)

        if int(self.params.get('OpkrLiveTune')) == 1:
            if int(self.params.get('LateralControlMethod')) == 0:
                self.str_log2 = 'T={:0.2f}/{:0.3f}/{:0.2f}/{:0.5f}'.format(
                    float(int(self.params.get('PidKp')) * 0.01),
                    float(int(self.params.get('PidKi')) * 0.001),
                    float(int(self.params.get('PidKd')) * 0.01),
                    float(int(self.params.get('PidKf')) * 0.00001))
            elif int(self.params.get('LateralControlMethod')) == 1:
                self.str_log2 = 'T={:03.1f}/{:03.1f}/{:03.1f}/{:03.1f}'.format(
                    float(int(self.params.get('InnerLoopGain')) * 0.1),
                    float(int(self.params.get('OuterLoopGain')) * 0.1),
                    float(int(self.params.get('TimeConstant')) * 0.1),
                    float(int(self.params.get('ActuatorEffectiveness')) * 0.1))
            elif int(self.params.get('LateralControlMethod')) == 2:
                self.str_log2 = 'T={:04.0f}/{:05.3f}/{:06.4f}'.format(
                    float(int(self.params.get('Scale')) * 1.0),
                    float(int(self.params.get('LqrKi')) * 0.001),
                    float(int(self.params.get('DcGain')) * 0.0001))

        trace1.printf1('{}  {}'.format(str_log1, self.str_log2))

        run_speed_ctrl = self.opkr_variablecruise and CS.acc_active

        if pcm_cancel_cmd and CS.scc12[
                "ACCMode"] != 0 and not CS.out.standstill:
            self.vdiff = 0.
            self.resumebuttoncnt = 0
            can_sends.append(
                create_clu11(self.packer, CS.CP.sccBus, CS.clu11,
                             Buttons.CANCEL, self.current_veh_speed,
                             self.clu11_cnt))
        elif CS.out.cruiseState.standstill and CS.scc12[
                "ACCMode"] != 0 and CS.vrelative > 0:
            self.vdiff += (CS.vrelative - self.vdiff)
            if (frame - self.lastresumeframe > 5) and (self.vdiff > .1 or
                                                       CS.lead_distance > 4.5):
                can_sends.append(
                    create_clu11(self.packer, CS.CP.sccBus, CS.clu11,
                                 Buttons.RES_ACCEL, self.current_veh_speed,
                                 self.resumebuttoncnt))
                self.resumebuttoncnt += 1
                if self.resumebuttoncnt > 5:
                    self.lastresumeframe = frame
                    self.resumebuttoncnt = 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, CS.CP.sccBus, CS.clu11,
                                 self.SC.btn_type, self.SC.sc_clu_speed,
                                 self.clu11_cnt))
        else:
            self.vdiff = 0.
            self.resumebuttoncnt = 0

        if frame % 2 == 0 and self.cp_oplongcontrol:
            accel_target = clip(actuators.gas - actuators.brake, -3.5, 2.0)
            self.apply_accel += 0.02 if accel_target > self.apply_accel else -0.02
            stopping = accel_target < 0 and CS.out.vEgo < 0.05
            self.apply_accel = clip(self.apply_accel,
                                    accel_target if accel_target < 0 else 0,
                                    accel_target if accel_target > 0 else 0)
            if not enabled:
                self.apply_accel = 0

        if CS.out.vEgo <= 1:
            self.sm.update(0)
            long_control_state = self.sm['controlsState'].longControlState
            self.acc_standstill = True if long_control_state == LongCtrlState.stopping else False
            if self.acc_standstill == True and not CS.out.gasPressed:
                self.acc_standstill_timer += 1
                if self.acc_standstill_timer >= 200:
                    self.acc_standstill_timer = 200
            elif CS.out.gasPressed:
                self.acc_standstill_timer = 0
            else:
                self.acc_standstill_timer = 0
        elif CS.out.gasPressed or CS.out.vEgo > 1:
            self.acc_standstill = False
            self.acc_standstill_timer = 0
        else:
            self.acc_standstill = False
            self.acc_standstill_timer = 0

        if lead_visible:
            self.lead_visible = True
            self.lead_debounce = 50
        elif self.lead_debounce > 0:
            self.lead_debounce -= 1
        else:
            self.lead_visible = lead_visible

        self.setspeed = set_speed * speed_conv

        if enabled:
            self.sendaccmode = enabled

        if CS.CP.radarDisablePossible:
            self.radarDisableOverlapTimer += 1
            self.radarDisableResetTimer = 0
            if self.radarDisableOverlapTimer >= 30:
                self.radarDisableActivated = True
                if 200 > self.radarDisableOverlapTimer > 36:
                    if frame % 41 == 0 or self.radarDisableOverlapTimer == 37:
                        can_sends.append(
                            create_scc7d0(b'\x02\x10\x03\x00\x00\x00\x00\x00'))
                    elif frame % 43 == 0 or self.radarDisableOverlapTimer == 37:
                        can_sends.append(
                            create_scc7d0(b'\x03\x28\x03\x01\x00\x00\x00\x00'))
                    elif frame % 19 == 0 or self.radarDisableOverlapTimer == 37:
                        can_sends.append(
                            create_scc7d0(b'\x02\x10\x85\x00\x00\x00\x00\x00'))
            else:
                self.counter_init = False
                can_sends.append(
                    create_scc7d0(b'\x02\x10\x90\x00\x00\x00\x00\x00'))
                can_sends.append(
                    create_scc7d0(b'\x03\x29\x03\x01\x00\x00\x00\x00'))
        elif self.radarDisableActivated:
            can_sends.append(
                create_scc7d0(b'\x02\x10\x90\x00\x00\x00\x00\x00'))
            can_sends.append(
                create_scc7d0(b'\x03\x29\x03\x01\x00\x00\x00\x00'))
            self.radarDisableOverlapTimer = 0
            if frame % 50 == 0:
                self.radarDisableResetTimer += 1
                if self.radarDisableResetTimer > 2:
                    self.radarDisableActivated = False
                    self.counter_init = True
        else:
            self.radarDisableOverlapTimer = 0
            self.radarDisableResetTimer = 0

        if (frame % 50 == 0 or self.radarDisableOverlapTimer == 37) and \
                CS.CP.radarDisablePossible and self.radarDisableOverlapTimer >= 30:
            can_sends.append(
                create_scc7d0(b'\x02\x3E\x00\x00\x00\x00\x00\x00'))

        if self.lead_visible:
            self.objdiststat = 1 if lead_dist < 25 else 2 if lead_dist < 40 else \
                               3 if lead_dist < 60 else 4 if lead_dist < 80 else 5
        else:
            self.objdiststat = 0

        # send scc to car if longcontrol enabled and SCC not on bus 0 or ont live
        if (CS.CP.sccBus == 2 or not self.usestockscc
                or self.radarDisableActivated) and self.counter_init:
            if frame % 2 == 0:
                self.scc12cnt += 1
                self.scc12cnt %= 0xF
                self.scc11cnt += 1
                self.scc11cnt %= 0x10
                self.fca11supcnt += 1
                self.fca11supcnt %= 0xF

                if self.fca11alivecnt == 1:
                    self.fca11inc = 0
                    if self.fca11cnt13 == 3:
                        self.fca11maxcnt = 0x9
                        self.fca11cnt13 = 0
                    else:
                        self.fca11maxcnt = 0xD
                        self.fca11cnt13 += 1
                else:
                    self.fca11inc += 4

                self.fca11alivecnt = self.fca11maxcnt - self.fca11inc

                can_sends.append(
                    create_scc11(self.packer, enabled, self.setspeed,
                                 self.lead_visible, lead_dist, lead_vrel,
                                 lead_yrel, self.gapsettingdance,
                                 CS.out.standstill, CS.scc11, self.usestockscc,
                                 CS.CP.radarOffCan, self.scc11cnt,
                                 self.sendaccmode))

                if CS.brake_check == 1 or CS.mainsw_check == 1:
                    can_sends.append(
                        create_scc12(self.packer, accel_target, accel_target,
                                     enabled, self.acc_standstill,
                                     CS.out.gasPressed, 1, CS.out.stockAeb,
                                     CS.scc12, self.usestockscc,
                                     CS.CP.radarOffCan, self.scc12cnt))
                else:
                    can_sends.append(
                        create_scc12(self.packer, accel_target, accel_target,
                                     enabled, self.acc_standstill,
                                     CS.out.gasPressed, CS.out.brakePressed,
                                     CS.out.stockAeb, CS.scc12,
                                     self.usestockscc, CS.CP.radarOffCan,
                                     self.scc12cnt))

                can_sends.append(
                    create_scc14(self.packer, enabled, self.usestockscc,
                                 CS.out.stockAeb, self.apply_accel, CS.scc14,
                                 self.objdiststat, CS.out.gasPressed,
                                 self.acc_standstill, CS.out.vEgo))
                if CS.CP.fcaBus == -1:
                    can_sends.append(
                        create_fca11(self.packer, CS.fca11, self.fca11alivecnt,
                                     self.fca11supcnt))

            if frame % 20 == 0:
                can_sends.append(create_scc13(self.packer, CS.scc13))
                if CS.CP.fcaBus == -1:
                    can_sends.append(create_fca12(self.packer))
            if frame % 50 == 0:
                can_sends.append(create_scc42a(self.packer))
        else:
            self.counter_init = True
            self.scc12cnt = CS.scc12init["CR_VSM_Alive"]
            self.scc11cnt = CS.scc11init["AliveCounterACC"]
            self.fca11alivecnt = CS.fca11init["CR_FCA_Alive"]
            self.fca11supcnt = CS.fca11init["Supplemental_Counter"]

        # 20 Hz LFA MFA message
        if frame % 5 == 0 and self.lfa_available:
            can_sends.append(create_lfa_mfa(self.packer, frame, enabled))

        can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))

        return can_sends
Пример #11
0
    def update(self, c, enabled, CS, frame, CC, actuators, pcm_cancel_cmd,
               visual_alert, left_lane, right_lane, left_lane_depart,
               right_lane_depart, set_speed, lead_visible, controls):

        # Steering Torque
        new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
        apply_steer = apply_std_steer_torque_limits(new_steer,
                                                    self.apply_steer_last,
                                                    CS.out.steeringTorque,
                                                    CarControllerParams)

        # disable when temp fault is active, or below LKA minimum speed
        lkas_active = c.active and not CS.out.steerWarning and abs(
            CS.out.steeringAngleDeg) < CS.CP.maxSteeringAngleDeg

        # Disable steering while turning blinker on and speed below 60 kph
        if CS.out.leftBlinker or CS.out.rightBlinker:
            self.turning_signal_timer = 0.5 / DT_CTRL  # Disable for 0.5 Seconds after blinker turned off
        if self.turning_indicator_alert:  # set and clear by interface
            lkas_active = 0
        if self.turning_signal_timer > 0:
            self.turning_signal_timer -= 1

        if not lkas_active:
            apply_steer = 0

        self.apply_steer_last = apply_steer

        sys_warning, sys_state, left_lane_warning, right_lane_warning = \
          process_hud_alert(enabled, self.car_fingerprint, visual_alert,
                            left_lane, right_lane, left_lane_depart, right_lane_depart)

        if self.haptic_feedback_speed_camera:
            if self.prev_active_cam != self.scc_smoother.active_cam:
                self.prev_active_cam = self.scc_smoother.active_cam
                if self.scc_smoother.active_cam:
                    self.active_cam_timer = int(1.5 / DT_CTRL)

            if self.active_cam_timer > 0:
                self.active_cam_timer -= 1
                left_lane_warning = right_lane_warning = 1

        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

        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"]

            # 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

        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,
                          self.ldws_opt))

        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,
                              self.ldws_opt))

        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, CS.mdps_bus, CS.clu11, Buttons.NONE,
                             enabled_speed))

        if pcm_cancel_cmd and (self.longcontrol and not self.mad_mode_enabled):
            can_sends.append(
                create_clu11(self.packer, CS.scc_bus, CS.clu11, Buttons.CANCEL,
                             clu11_speed))
        else:
            can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))

        # fix auto resume - by neokii
        if CS.out.cruiseState.standstill and not CS.out.gasPressed:

            if self.last_lead_distance == 0:
                self.last_lead_distance = CS.lead_distance
                self.resume_cnt = 0
                self.resume_wait_timer = 0

            # scc smoother
            elif self.scc_smoother.is_active(frame):
                pass

            elif self.resume_wait_timer > 0:
                self.resume_wait_timer -= 1

            elif abs(CS.lead_distance - self.last_lead_distance) > 0.1:
                can_sends.append(
                    create_clu11(self.packer, CS.scc_bus, CS.clu11,
                                 Buttons.RES_ACCEL, clu11_speed))
                self.resume_cnt += 1

                if self.resume_cnt >= randint(6, 8):
                    self.resume_cnt = 0
                    self.resume_wait_timer = randint(30, 36)

        # reset lead distnce after the car starts moving
        elif self.last_lead_distance != 0:
            self.last_lead_distance = 0

        # scc smoother
        self.scc_smoother.update(enabled, can_sends, self.packer, CC, CS,
                                 frame, controls)

        # send scc to car if longcontrol enabled and SCC not on bus 0 or ont live
        if self.longcontrol and CS.cruiseState_enabled and (CS.scc_bus or
                                                            not self.scc_live):

            if frame % 2 == 0:

                stopping = controls.LoC.long_control_state == LongCtrlState.stopping
                apply_accel = clip(actuators.accel if c.active else 0,
                                   CarControllerParams.ACCEL_MIN,
                                   CarControllerParams.ACCEL_MAX)
                apply_accel = self.scc_smoother.get_apply_accel(
                    CS, controls.sm, apply_accel, stopping)

                self.accel = apply_accel

                controls.apply_accel = apply_accel
                aReqValue = CS.scc12["aReqValue"]
                controls.aReqValue = aReqValue

                if aReqValue < controls.aReqValueMin:
                    controls.aReqValueMin = controls.aReqValue

                if aReqValue > controls.aReqValueMax:
                    controls.aReqValueMax = controls.aReqValue

                if self.stock_navi_decel_enabled:
                    controls.sccStockCamAct = CS.scc11["Navi_SCC_Camera_Act"]
                    controls.sccStockCamStatus = CS.scc11[
                        "Navi_SCC_Camera_Status"]
                    apply_accel, stock_cam = self.scc_smoother.get_stock_cam_accel(
                        apply_accel, aReqValue, CS.scc11)
                else:
                    controls.sccStockCamAct = 0
                    controls.sccStockCamStatus = 0
                    stock_cam = False

                if self.scc12_cnt < 0:
                    self.scc12_cnt = CS.scc12[
                        "CR_VSM_Alive"] if not CS.no_radar else 0

                self.scc12_cnt += 1
                self.scc12_cnt %= 0xF

                can_sends.append(
                    create_scc12(self.packer, apply_accel, enabled,
                                 self.scc12_cnt, self.scc_live, CS.scc12,
                                 CS.out.gasPressed, CS.out.brakePressed,
                                 CS.out.cruiseState.standstill,
                                 self.car_fingerprint))

                can_sends.append(
                    create_scc11(self.packer, frame, enabled, set_speed,
                                 lead_visible, self.scc_live, CS.scc11,
                                 self.scc_smoother.active_cam, stock_cam))

                if frame % 20 == 0 and CS.has_scc13:
                    can_sends.append(create_scc13(self.packer, CS.scc13))

                if CS.has_scc14:
                    acc_standstill = stopping if CS.out.vEgo < 2. else False

                    lead = self.scc_smoother.get_lead(controls.sm)

                    if lead is not None:
                        d = lead.dRel
                        obj_gap = 1 if d < 25 else 2 if d < 40 else 3 if d < 60 else 4 if d < 80 else 5
                    else:
                        obj_gap = 0

                    can_sends.append(
                        create_scc14(self.packer, enabled, CS.out.vEgo,
                                     acc_standstill, apply_accel,
                                     CS.out.gasPressed, obj_gap, CS.scc14))
        else:
            self.scc12_cnt = -1

        # 20 Hz LFA MFA message
        if frame % 5 == 0:
            activated_hda = road_speed_limiter_get_active()
            # activated_hda: 0 - off, 1 - main road, 2 - highway
            if self.car_fingerprint in FEATURES["send_lfa_mfa"]:
                can_sends.append(
                    create_lfahda_mfc(self.packer, enabled, activated_hda))
            elif CS.has_lfa_hda:
                can_sends.append(
                    create_hda_mfc(self.packer, activated_hda, CS, left_lane,
                                   right_lane))

        new_actuators = actuators.copy()
        new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
        new_actuators.accel = self.accel

        return new_actuators, can_sends
Пример #12
0
    def update_scc(self, CC, CS, actuators, controls, hud_control, can_sends):

        # scc smoother
        self.scc_smoother.update(CC.enabled, can_sends, self.packer, CC, CS,
                                 self.frame, controls)

        # send scc to car if longcontrol enabled and SCC not on bus 0 or ont live
        if self.longcontrol and CS.cruiseState_enabled and (CS.scc_bus or
                                                            not self.scc_live):

            if self.frame % 2 == 0:

                set_speed = hud_control.setSpeed
                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

                stopping = controls.LoC.long_control_state == LongCtrlState.stopping
                apply_accel = self.scc_smoother.get_apply_accel(
                    CS, controls.sm, actuators.accel, stopping)
                apply_accel = clip(apply_accel if CC.longActive else 0,
                                   CarControllerParams.ACCEL_MIN,
                                   CarControllerParams.ACCEL_MAX)

                self.accel = apply_accel

                controls.apply_accel = apply_accel
                aReqValue = CS.scc12["aReqValue"]
                controls.aReqValue = aReqValue

                if aReqValue < controls.aReqValueMin:
                    controls.aReqValueMin = controls.aReqValue

                if aReqValue > controls.aReqValueMax:
                    controls.aReqValueMax = controls.aReqValue

                if self.stock_navi_decel_enabled:
                    controls.sccStockCamAct = CS.scc11["Navi_SCC_Camera_Act"]
                    controls.sccStockCamStatus = CS.scc11[
                        "Navi_SCC_Camera_Status"]
                    apply_accel, stock_cam = self.scc_smoother.get_stock_cam_accel(
                        apply_accel, aReqValue, CS.scc11)
                else:
                    controls.sccStockCamAct = 0
                    controls.sccStockCamStatus = 0
                    stock_cam = False

                if self.scc12_cnt < 0:
                    self.scc12_cnt = CS.scc12[
                        "CR_VSM_Alive"] if not CS.no_radar else 0

                self.scc12_cnt += 1
                self.scc12_cnt %= 0xF

                can_sends.append(
                    hyundaican.create_scc12(
                        self.packer, apply_accel, CC.enabled, self.scc12_cnt,
                        self.scc_live, CS.scc12, CS.out.gasPressed,
                        CS.out.brakePressed, CS.out.cruiseState.standstill,
                        self.CP.carFingerprint))

                can_sends.append(
                    hyundaican.create_scc11(self.packer, self.frame,
                                            CC.enabled, set_speed,
                                            hud_control.leadVisible,
                                            self.scc_live, CS.scc11,
                                            self.scc_smoother.active_cam,
                                            stock_cam))

                if self.frame % 20 == 0 and CS.has_scc13:
                    can_sends.append(
                        hyundaican.create_scc13(self.packer, CS.scc13))

                if CS.has_scc14:
                    acc_standstill = stopping if CS.out.vEgo < 2. else False

                    lead = self.scc_smoother.get_lead(controls.sm)

                    if lead is not None:
                        d = lead.dRel
                        obj_gap = 1 if d < 25 else 2 if d < 40 else 3 if d < 60 else 4 if d < 80 else 5
                    else:
                        obj_gap = 0

                    can_sends.append(
                        hyundaican.create_scc14(self.packer, CC.enabled,
                                                CS.out.vEgo, acc_standstill,
                                                apply_accel, CS.out.gasPressed,
                                                obj_gap, CS.scc14))
        else:
            self.scc12_cnt = -1
Пример #13
0
    def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
               visual_alert, left_lane, right_lane, left_lane_depart,
               right_lane_depart, set_speed, lead_visible):

        self.lfa_available = True if self.lfainFingerprint or self.car_fingerprint in FEATURES[
            "send_lfa_mfa"] else False

        self.high_steer_allowed = True if self.car_fingerprint in FEATURES[
            "allow_high_steer"] else False

        if lead_visible:
            self.lead_visible = True
            self.lead_debounce = 50
        elif self.lead_debounce > 0:
            self.lead_debounce -= 1
        else:
            self.lead_visible = lead_visible

        # 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)

        # 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.)
                                   or self.high_steer_allowed)

        # fix for Genesis hard fault at low speed
        if CS.out.vEgo < 55 * CV.KPH_TO_MS and self.car_fingerprint == CAR.HYUNDAI_GENESIS and not CS.mdpsHarness:
            lkas_active = False

        if not lkas_active:
            apply_steer = 0

        if CS.nosccradar:
            self.usestockscc = not self.cp_oplongcontrol
        elif (CS.cancel_button_count == 3) and self.cp_oplongcontrol:
            self.usestockscc = not self.usestockscc

        if not self.usestockscc:
            self.gapcount += 1
            if self.gapcount == 50 and self.gapsettingdance == 2:
                self.gapsettingdance = 1
                self.gapcount = 0
            elif self.gapcount == 50 and self.gapsettingdance == 1:
                self.gapsettingdance = 4
                self.gapcount = 0
            elif self.gapcount == 50 and self.gapsettingdance == 4:
                self.gapsettingdance = 3
                self.gapcount = 0
            elif self.gapcount == 50 and self.gapsettingdance == 3:
                self.gapsettingdance = 2
                self.gapcount = 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(enabled, self.car_fingerprint, visual_alert,
                            left_lane, right_lane, left_lane_depart, right_lane_depart)

        speed_conv = CV.MS_TO_MPH if CS.is_set_speed_in_mph else CV.MS_TO_KPH

        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 or CS.out.gearShifter != GearShifter.drive:
            enabled_speed = clu11_speed

        self.current_veh_speed = int(CS.out.vEgo * speed_conv)

        self.clu11_cnt = frame % 0x10
        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,
                          self.lfa_available, 0))

        if CS.mdpsHarness:  # send lkas11 bus 1 if mdps
            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,
                              self.lfa_available, 1))

            can_sends.append(
                create_clu11(self.packer, 1, CS.clu11, Buttons.NONE,
                             enabled_speed, self.clu11_cnt))

        if pcm_cancel_cmd and not CS.nosccradar and self.usestockscc and CS.scc12[
                "ACCMode"] and not CS.out.standstill:
            self.vdiff = 0.
            self.resumebuttoncnt = 0
            can_sends.append(
                create_clu11(self.packer, CS.scc_bus, CS.clu11, Buttons.CANCEL,
                             self.current_veh_speed, self.clu11_cnt))
        elif CS.out.cruiseState.standstill and not CS.nosccradar and self.usestockscc and CS.vrelative > 0:
            self.vdiff += (CS.vrelative - self.vdiff)
            if (frame - self.lastresumeframe > 10) and (self.vdiff > .5 or
                                                        CS.lead_distance > 6.):
                can_sends.append(
                    create_clu11(self.packer, CS.scc_bus, CS.clu11,
                                 Buttons.RES_ACCEL, self.current_veh_speed,
                                 self.resumebuttoncnt))
                self.resumebuttoncnt += 1
                if self.resumebuttoncnt > 5:
                    self.lastresumeframe = frame
                    self.resumebuttoncnt = 0
        else:
            self.vdiff = 0.
            self.resumebuttoncnt = 0

        self.acc_standstill = False  #True if (enabled and not self.acc_paused and CS.out.standstill) else False

        set_speed *= speed_conv

        # send scc to car if longcontrol enabled and SCC not on bus 0 or ont live
        if (CS.scc_bus == 2 or not self.usestockscc) and frame % 2 == 0:
            self.scc12cnt += 1
            self.scc12cnt %= 0xF
            can_sends.append(
                create_scc11(self.packer, enabled, set_speed,
                             self.lead_visible, self.gapsettingdance,
                             CS.out.standstill, CS.scc11, self.usestockscc,
                             CS.nosccradar, frame))

            can_sends.append(
                create_scc12(self.packer, apply_accel, enabled,
                             self.acc_standstill, CS.out.gasPressed,
                             CS.out.brakePressed, CS.scc11["MainMode_ACC"],
                             CS.out.stockAeb, CS.scc12, self.usestockscc,
                             CS.nosccradar, self.scc12cnt))

            can_sends.append(create_scc13(self.packer, CS.scc13))
            can_sends.append(
                create_scc14(self.packer, enabled, self.usestockscc,
                             CS.out.stockAeb, CS.scc14))

        # 20 Hz LFA MFA message
        if frame % 5 == 0 and self.lfa_available:
            can_sends.append(create_lfa_mfa(self.packer, frame, enabled))

        #detect stopped vehicle logic -> object with relative velocity -ve current vehicle speed + some hysteresis
        #Once detected allow maturation and calculate appropriate distance for decelration target

        self.sm.update(0)

        dat = self.sm['radarState'].leadOne
        if CS.is_set_speed_in_mph:
            self.setspeed = CS.out.cruiseState.speed * speed_conv
        else:
            self.setspeed = CS.out.cruiseState.speed * speed_conv

        if not CS.radar_obj_valid and dat.status and dat.vLead < 3. \
                and  CS.out.cruiseState.enabled and not CS.out.gasPressed:
            aRel = (dat.vLead**2 - CS.out.vEgo**2) / (2 * dat.dRel)
            print("aRel", aRel)
            if aRel < -.5 or self.stopcontrolupdate:
                self.stopcontrolupdate = True
                print("STOPPED VEHICLE")
                self.smartspeed = 20 if CS.is_set_speed_in_mph else 30
                if not self.stopcontrolupdate:
                    self.button_cnt = 0
                    self.recordsetspeed = self.setspeed
        else:
            if self.setspeed != self.recordsetspeed and self.stopcontrolupdate and  CS.out.cruiseState.enabled \
                    and CS.radar_obj_valid and not CS.out.gasPressed:
                self.smartspeed = self.recordsetspeed
            else:
                self.stopcontrolupdate = False
                self.recordsetspeed = 0

        framestoskip = 10

        if (frame - self.last_button_frame
            ) > framestoskip and self.stopcontrolupdate:
            if self.setspeed > (self.smartspeed * 1.005):
                can_sends.append(
                    create_clu11(self.packer, CS.scc_bus, CS.clu11,
                                 Buttons.SET_DECEL, self.current_veh_speed,
                                 self.button_cnt))
                print("AUTO SLOW DOWN")
                if CS.cruise_buttons == 1:
                    self.button_res_stop += 2
                else:
                    self.button_res_stop -= 1
            elif self.setspeed < (self.smartspeed / 1.005):
                can_sends.append(
                    create_clu11(self.packer, CS.scc_bus, CS.clu11,
                                 Buttons.RES_ACCEL, self.current_veh_speed,
                                 self.button_cnt))
                print("AUTO SPEED UP")
                if CS.cruise_buttons == 2:
                    self.button_set_stop += 2
                else:
                    self.button_set_stop -= 1
            else:
                self.button_res_stop = self.button_set_stop = 0

            if (abs(self.smartspeed - self.setspeed) <
                    0.5) or (self.button_res_stop >=
                             50) or (self.button_set_stop >= 50):
                self.stopcontrolupdate = False

            self.button_cnt += 1
            if self.button_cnt > 5:
                self.last_button_frame = frame
                self.button_cnt = 0
        else:
            self.button_set_stop = self.button_res_stop = 0

        return can_sends
Пример #14
0
    def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
               visual_alert, left_lane, right_lane, left_lane_depart,
               right_lane_depart, set_speed, lead_visible):

        # *** 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)

        # 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
        # temporarily disable steering when LKAS button off
        lkas_active = enabled and abs(
            CS.out.steeringAngle) < 90. and self.lkas_button_on

        # fix for Genesis hard fault at low speed
        if CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS and not CS.mdps_bus:
            lkas_active = 0

        # Disable steering while turning blinker on and speed below 60 kph
        if CS.out.leftBlinker or CS.out.rightBlinker:
            if self.car_fingerprint not in [CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_H]:
                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 < 16.7:
            lkas_active = 0
        if self.turning_signal_timer:
            self.turning_signal_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 60
        if clu11_speed > enabled_speed or not lkas_active:
            enabled_speed = clu11_speed
        set_speed *= 2.237 if CS.is_set_speed_in_mph else 3.6

        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

        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))

        if pcm_cancel_cmd and self.longcontrol:
            can_sends.append(
                create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
                             Buttons.CANCEL, clu11_speed))
        elif CS.mdps_bus:  # send mdps12 to LKAS to prevent LKAS error if no cancel cmd
            can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))

        if CS.scc_bus and self.longcontrol and frame % 2 == 0:  # send scc12 to car if SCC not on bus 0 and longcontrol enabled
            can_sends.append(
                create_scc12(self.packer, apply_accel, enabled, self.scc12_cnt,
                             CS.scc12))
            can_sends.append(
                create_scc11(self.packer, frame, enabled, set_speed,
                             lead_visible, 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

        if CS.out.cruiseState.standstill:
            # 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, 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

        # 20 Hz LFA MFA message
        if frame % 5 == 0 and self.car_fingerprint in [
                CAR.SONATA, CAR.PALISADE, CAR.SONATA_H
        ]:
            can_sends.append(create_lfa_mfa(self.packer, frame, enabled))

        return can_sends