def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_lane, right_lane, left_lane_depart, right_lane_depart): # Steering Torque new_steer = int(round(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 when temp fault is active lkas_active = enabled and not CS.out.steerWarning # fix for Genesis hard fault at low speed if CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS: lkas_active = False 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: # send resume at a max freq of 10Hz if (frame - self.last_resume_frame) * DT_CTRL > 0.1: # send 25 messages at a time to increases the likelihood of resume being accepted can_sends.extend([ create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL) ] * 25) self.last_resume_frame = frame # 20 Hz LFA MFA message if frame % 5 == 0 and self.car_fingerprint in [ CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KONA_EV, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021 ]: can_sends.append(create_lfahda_mfc(self.packer, enabled)) return can_sends
def update(self, CC, CS): actuators = CC.actuators hud_control = CC.hudControl # Steering Torque new_steer = int(round(actuators.steer * self.params.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params) self.steer_rate_limited = new_steer != apply_steer if not CC.latActive: apply_steer = 0 self.apply_steer_last = apply_steer sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint, hud_control) can_sends = [] if self.CP.carFingerprint in HDA2_CAR: # steering control can_sends.append(hda2can.create_lkas(self.packer, CC.enabled, self.frame, CC.latActive, apply_steer)) # cruise cancel if (self.frame - self.last_button_frame) * DT_CTRL > 0.25: if CC.cruiseControl.cancel: for _ in range(20): can_sends.append(hda2can.create_buttons(self.packer, CS.buttons_counter+1, True, False)) self.last_button_frame = self.frame # cruise standstill resume elif CC.cruiseControl.resume: can_sends.append(hda2can.create_buttons(self.packer, CS.buttons_counter+1, False, True)) self.last_button_frame = self.frame else: # tester present - w/ no response (keeps radar disabled) if self.CP.openpilotLongitudinalControl: if self.frame % 100 == 0: can_sends.append([0x7D0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0]) can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, CC.latActive, CS.lkas11, sys_warning, sys_state, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible, left_lane_warning, right_lane_warning)) if not self.CP.openpilotLongitudinalControl: if CC.cruiseControl.cancel: can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL)) elif CC.cruiseControl.resume: # send resume at a max freq of 10Hz if (self.frame - self.last_button_frame) * DT_CTRL > 0.1: # send 25 messages at a time to increases the likelihood of resume being accepted can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.RES_ACCEL)] * 25) self.last_button_frame = self.frame if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl: accel = actuators.accel jerk = 0 if CC.longActive: jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7) if accel < 0: accel = interp(accel - CS.out.aEgo, [-1.0, -0.5], [2 * accel, accel]) accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) stopping = actuators.longControlState == LongCtrlState.stopping set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH) can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2), hud_control.leadVisible, set_speed_in_units, stopping, CS.out.gasPressed)) self.accel = accel # 20 Hz LFA MFA message if self.frame % 5 == 0 and self.car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022): can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled)) # 5 Hz ACC options if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl: can_sends.extend(hyundaican.create_acc_opt(self.packer)) # 2 Hz front radar options if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl: can_sends.append(hyundaican.create_frt_radar_opt(self.packer)) new_actuators = actuators.copy() new_actuators.steer = apply_steer / self.params.STEER_MAX new_actuators.accel = self.accel self.frame += 1 return new_actuators, can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed, left_lane, right_lane, left_lane_depart, right_lane_depart): # Steering Torque new_steer = int(round(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 when temp fault is active, or below LKA minimum speed lkas_active = enabled and not CS.out.steerWarning and CS.out.vEgo >= CS.CP.minSteerSpeed 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 = [] # tester present - w/ no response (keeps radar disabled) if CS.CP.openpilotLongitudinalControl: if (frame % 100) == 0: 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)) if not CS.CP.openpilotLongitudinalControl: if pcm_cancel_cmd: can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL)) elif CS.out.cruiseState.standstill: # send resume at a max freq of 10Hz if (frame - self.last_resume_frame) * DT_CTRL > 0.1: # send 25 messages at a time to increases the likelihood of resume being accepted can_sends.extend([create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)] * 25) self.last_resume_frame = frame if frame % 2 == 0 and CS.CP.openpilotLongitudinalControl: lead_visible = False accel = actuators.accel if enabled else 0 jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7) if accel < 0: accel = interp(accel - CS.out.aEgo, [-1.0, -0.5], [2 * accel, accel]) accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) stopping = (actuators.longControlState == LongCtrlState.stopping) set_speed_in_units = hud_speed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH) can_sends.extend(create_acc_commands(self.packer, enabled, accel, jerk, int(frame / 2), lead_visible, set_speed_in_units, stopping)) self.accel = accel # 20 Hz LFA MFA message if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020]: can_sends.append(create_lfahda_mfc(self.packer, enabled)) # 5 Hz ACC options if frame % 20 == 0 and CS.CP.openpilotLongitudinalControl: can_sends.extend(create_acc_opt(self.packer)) # 2 Hz front radar options if frame % 50 == 0 and CS.CP.openpilotLongitudinalControl: can_sends.append(create_frt_radar_opt(self.packer)) new_actuators = actuators.copy() new_actuators.steer = apply_steer / self.p.STEER_MAX new_actuators.accel = self.accel return new_actuators, can_sends
def update(self, enabled, CS, frame, CC, actuators, pcm_cancel_cmd, visual_alert, left_lane, right_lane, left_lane_depart, right_lane_depart, set_speed, lead_visible, sm): # *** compute control surfaces *** # gas and brake apply_accel = actuators.gas - actuators.brake apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady) apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) 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, 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
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 * 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 # 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.steeringAngleDeg ) < CS.CP.maxSteeringAngleDeg 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 = 100 # Disable for 1.0 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_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 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)) 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.clu11_cnt = 0 # reset lead distnce after the car starts moving elif self.last_lead_distance != 0: self.last_lead_distance = 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_lfahda_mfc(self.packer, enabled)) 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, 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 = int(round(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.steeringAngleDeg) < CS.CP.maxSteeringAngleDeg) 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 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 if apply_steer != 0 or CS.out.steeringPressed: self.steer_wind_down = False elif self.apply_steer_last != 0 and apply_steer == 0: self.steer_wind_down = True 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 60 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, self.steer_wind_down, 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 and CS.CP.enableCruise: 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 > 10) and (self.vdiff > .5 or CS.lead_distance > 6.): 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 < 2.: 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_lfahda_mfc(self.packer, enabled)) can_sends.append(create_mdps12(self.packer, frame, CS.mdps12)) 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, 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
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
def update(self, CC, CS, controls): if self.CP.carFingerprint in HDA2_CAR: return self.update_hda2(CC, CS) actuators = CC.actuators hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel # Steering Torque new_steer = int(round(actuators.steer * self.params.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params) # disable when temp fault is active, or below LKA minimum speed lkas_active = CC.latActive # 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( CC.enabled, self.CP.carFingerprint, hud_control) 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: if (self.frame - self.last_active_cam_frame) * DT_CTRL > 10.0: self.active_cam_timer = int(1.5 / DT_CTRL) self.last_active_cam_frame = self.frame 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 self.frame == 0: # initialize counts from last received count signals self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"] self.lkas11_cnt = (self.lkas11_cnt + 1) % 0x10 cut_steer_temp = False if self.steer_fault_max_angle > 0: if lkas_active and abs( CS.out.steeringAngleDeg) > self.steer_fault_max_angle: self.angle_limit_counter += 1 else: self.angle_limit_counter = 0 # stop requesting torque to avoid 90 degree fault and hold torque with induced temporary fault # two cycles avoids race conditions every few minutes if self.angle_limit_counter > self.steer_fault_max_frames: self.cut_steer = True elif self.cut_steer_frames > 1: self.cut_steer_frames = 0 self.cut_steer = False if self.cut_steer: cut_steer_temp = True self.angle_limit_counter = 0 self.cut_steer_frames += 1 can_sends = [] can_sends.append( hyundaican.create_lkas11( self.packer, self.frame, self.CP.carFingerprint, apply_steer, lkas_active, CS.lkas11, sys_warning, sys_state, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible, left_lane_warning, right_lane_warning, 0, self.ldws_opt, cut_steer_temp)) if CS.mdps_bus or CS.scc_bus == 1: # send lkas11 bus 1 if mdps or scc is on bus 1 can_sends.append( hyundaican.create_lkas11( self.packer, self.frame, self.CP.carFingerprint, apply_steer, lkas_active, CS.lkas11, sys_warning, sys_state, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible, left_lane_warning, right_lane_warning, 1, self.ldws_opt, cut_steer_temp)) if self.frame % 2 and CS.mdps_bus: # send clu11 to mdps if it is not on bus 0 can_sends.append( hyundaican.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( hyundaican.create_clu11(self.packer, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed)) if CS.mdps_bus or self.CP.carFingerprint in FEATURES[ "send_mdps12"]: # send mdps12 to LKAS to prevent LKAS error can_sends.append( hyundaican.create_mdps12(self.packer, self.frame, CS.mdps12)) self.update_auto_resume(CC, CS, clu11_speed, can_sends) self.update_scc(CC, CS, actuators, controls, hud_control, can_sends) # 20 Hz LFA MFA message if self.frame % 5 == 0: activated_hda = road_speed_limiter_get_active() # activated_hda: 0 - off, 1 - main road, 2 - highway if self.CP.carFingerprint in FEATURES["send_lfa_mfa"]: can_sends.append( hyundaican.create_lfahda_mfc(self.packer, CC.enabled, activated_hda)) elif CS.has_lfa_hda: can_sends.append( hyundaican.create_hda_mfc(self.packer, activated_hda, CS, hud_control.leftLaneVisible, hud_control.rightLaneVisible)) new_actuators = actuators.copy() new_actuators.steer = apply_steer / self.params.STEER_MAX new_actuators.accel = self.accel self.frame += 1 return new_actuators, can_sends