def update_log(self, CS, set_speed, target_set_speed, long_wait_cmd):
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 self.cruise_gap != CS.cruiseGapSet:
self.cruise_gap = CS.cruiseGapSet
str3 = 'MODE={:s} BS={:1.0f}/{:1.0f} VL={:03.0f}/{:03.0f} TM={:03.0f}/{:03.0f} TS={:03.0f}'.format(
self.steer_mode, CS.CP.mdpsBus, CS.CP.sccBus, set_speed,
CS.VSetDis, long_wait_cmd, self.long_curv_timer,
int(round(self.target_speed)))
str4 = ' RD=D:{:03.0f}/V:{:03.0f} CG={:1.0f} DG={:s}'.format(
CS.lead_distance, CS.lead_objspd, self.cruise_gap,
self.seq_step_debug)
str5 = str3 + str4
trace1.printf2(str5)
def update_log(self, CS, set_speed, target_set_speed, long_wait_cmd ):
str3 = 'SS={:03.0f}/{:03.0f} DAt={:03.0f}/{:03.0f}/{:03.0f} DG={:02.0f}'.format(
set_speed, CS.VSetDis, CS.driverAcc_time, long_wait_cmd, self.long_curv_timer, self.seq_step_debug )
str4 = ' LD/LS={:03.0f}/{:03.0f} '.format( CS.lead_distance, CS.lead_objspd )
str5 = str3 + str4
trace1.printf2( str5 )
def update_log(self, CS, set_speed, target_set_speed, long_wait_cmd):
str3 = 'M={:3.0f} DST={:3.0f} VSD={:.0f} DA={:.0f}/{:.0f}/{:.0f} DG={:s} DO={:.0f}'.format(
CS.out.cruiseState.modeSel, target_set_speed, CS.VSetDis,
CS.driverAcc_time, long_wait_cmd, self.long_curv_timer,
self.seq_step_debug, CS.driverOverride)
str4 = ' CS={:.1f}/{:.1f} '.format(CS.lead_distance, CS.lead_objspd)
str5 = str3 + str4
trace1.printf2(str5)
def update_log(self, CS, set_speed, target_set_speed, long_wait_cmd ):
if CS.out.cruiseState.modeSel == 0:
self.steer_mode = "오파모드"
elif CS.out.cruiseState.modeSel == 1:
self.steer_mode = "차간+커브"
elif CS.out.cruiseState.modeSel == 2:
self.steer_mode = "차간ONLY"
elif CS.out.cruiseState.modeSel == 3:
self.steer_mode = "자동RES"
elif CS.out.cruiseState.modeSel == 4:
self.steer_mode = "순정모드"
str3 = '주행모드={:s} 설정속도={:03.0f}/{:03.0f} 타이머={:03.0f}/{:03.0f}/{:03.0f}'.format( self.steer_mode,
set_speed, CS.VSetDis, CS.driverAcc_time, long_wait_cmd, self.long_curv_timer )
str4 = ' 거리차/속도차={:03.0f}/{:03.0f} 구분={:s}'.format( CS.lead_distance, CS.lead_objspd, self.seq_step_debug )
str5 = str3 + str4
trace1.printf2( str5 )
def update_log(self, CS, set_speed, target_set_speed, long_wait_cmd ):
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 self.cruise_gap != CS.cruiseGapSet:
self.cruise_gap = CS.cruiseGapSet
str3 = '주행모드={:s} 설정속도={:03.0f}/{:03.0f} 타이머={:03.0f}/{:03.0f}'.format( self.steer_mode, set_speed, CS.VSetDis, long_wait_cmd, self.long_curv_timer )
str4 = ' 레이더=D:{:04.1f}/V:{:05.1f} CG={:1.0f} 구분={:s}'.format( CS.lead_distance, CS.lead_objspd, self.cruise_gap, self.seq_step_debug )
str5 = str3 + str4
trace1.printf2( str5 )
def update_log(self, CS, set_speed, target_set_speed, long_wait_cmd):
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 self.cruise_gap != CS.cruiseGapSet:
self.cruise_gap = CS.cruiseGapSet
str3 = 'MODE={:s} VL={:03.0f}/{:03.0f} TM={:03.0f}/{:03.0f} TS={:03.0f}'.format(
self.steer_mode, set_speed, CS.VSetDis, long_wait_cmd,
self.long_curv_timer, int(round(self.target_speed)))
str4 = ' RD=D:{:03.0f}/V:{:03.0f} CG={:1.0f} DG={:s}'.format(
CS.lead_distance, CS.lead_objspd, self.cruise_gap,
self.seq_step_debug)
str5 = str3 + str4
trace1.printf2(str5)
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
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, active, CS, v_target, v_target_future, a_target, CP,
hasLead, radarState, decelForTurn, longitudinalPlanSource,
extras):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Actuation limits
gas_max = interp(CS.vEgo, CP.gasMaxBP, CP.gasMaxV)
brake_max = interp(CS.vEgo, CP.brakeMaxBP, CP.brakeMaxV)
multiplier = 0
multiplier2 = 0
multiplier3 = 0
vRel = 0
if self.enable_dg:
gas_max = self.dynamic_gas.update(CS, extras)
# Update state machine
output_gb = self.last_output_gb
if radarState is None:
dRel = 200
vRel = 0
else:
dRel = radarState.leadOne.dRel
vRel = radarState.leadOne.vRel
# 앞차와 거리가 3.5m이하일때 상태를 강제로 STOP으로 만듬
if hasLead:
stop = True if (dRel < 3.5
and radarState.leadOne.status) else False
else:
stop = False
self.long_control_state = long_control_state_trans(
active, self.long_control_state, CS.vEgo, v_target_future,
self.v_pid, output_gb, CS.brakePressed, CS.cruiseState.standstill,
stop, CS.gasPressed, CP.minSpeedCan)
v_ego_pid = max(
CS.vEgo, CP.minSpeedCan
) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
if self.long_control_state == LongCtrlState.off or (CS.brakePressed
or CS.gasPressed):
self.v_pid = v_ego_pid
self.pid.reset()
output_gb = 0.
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target
self.pid.pos_limit = gas_max
self.pid.neg_limit = -brake_max
# Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7
deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP,
CP.longitudinalTuning.deadzoneV)
if longitudinalPlanSource == 'cruise':
if decelForTurn and not self.lastdecelForTurn:
self.lastdecelForTurn = True
self.pid._k_p = (CP.longitudinalTuning.kpBP, [
x * 0 for x in CP.longitudinalTuning.kpV
])
self.pid._k_i = (CP.longitudinalTuning.kiBP, [
x * 0 for x in CP.longitudinalTuning.kiV
])
self.pid.i = 0.0
self.pid.k_f = 1.0
self.v_pid = CS.vEgo
self.pid.reset()
if self.lastdecelForTurn and not decelForTurn:
self.lastdecelForTurn = False
self.pid._k_p = (CP.longitudinalTuning.kpBP,
CP.longitudinalTuning.kpV)
self.pid._k_i = (CP.longitudinalTuning.kiBP,
CP.longitudinalTuning.kiV)
self.pid.k_f = 1.0
self.v_pid = CS.vEgo
self.pid.reset()
else:
if self.lastdecelForTurn:
self.v_pid = CS.vEgo
self.pid.reset()
self.lastdecelForTurn = False
self.pid._k_p = (CP.longitudinalTuning.kpBP,
[x * 1 for x in CP.longitudinalTuning.kpV])
self.pid._k_i = (CP.longitudinalTuning.kiBP,
[x * 1 for x in CP.longitudinalTuning.kiV])
self.pid.k_f = 1.0
output_gb = self.pid.update(self.v_pid,
v_ego_pid,
speed=v_ego_pid,
deadzone=deadzone,
feedforward=a_target,
freeze_integrator=prevent_overshoot)
## 감속 보충을 위해 out_gb -값일 때 임의의 값을 더 곱해줌
#if hasLead and radarState.leadOne.status and 4 < dRel <= 55 and output_gb < 0 and vRel < 0:
# multiplier = max((self.v_pid/(max(v_target_future, 1))), 1)
# multiplier = clip(multiplier, 1.2, 3.5)
# output_gb *= multiplier
# #20m 간격 이하에서 거리보다 속도가 2배 이상인경우 조금더 감속 보충
# if dRel*2 < CS.vEgo*3.6 and dRel <= 20:
# multiplier2 = interp(dRel, [4, 20], [2, 1])
# elif dRel*1.5 < CS.vEgo*3.6 and dRel <= 20:
# multiplier2 = interp(dRel, [4, 20], [1.5, 1])
# output_gb *= multiplier3
# output_gb = clip(output_gb, -brake_max, gas_max)
## 앞차 감속시 가속하는것을 완화해줌
#elif hasLead and radarState.leadOne.status and 4 < dRel <= 55 and output_gb > 0 and vRel < 0:
# multiplier3 = interp(abs(vRel*3.6), [0, 1, 2], [1.0, 0.5, 0.0])
# output_gb *= multiplier3
# output_gb = clip(output_gb, -brake_max, gas_max)
if prevent_overshoot:
output_gb = min(output_gb, 0.0)
# Intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping:
# Keep applying brakes until the car is stopped
factor = 1
if hasLead:
factor = interp(dRel, [2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0],
[6, 3.5, 1.5, 0.8, 0.5, 0.3, 0.2])
if not CS.standstill or output_gb > -BRAKE_STOPPING_TARGET:
output_gb -= CP.stoppingBrakeRate / RATE * factor
output_gb = clip(output_gb, -brake_max, gas_max)
self.reset(CS.vEgo)
# Intention is to move again, release brake fast before handing control to PID
elif self.long_control_state == LongCtrlState.starting:
factor = 1
if hasLead:
factor = interp(dRel, [0.0, 2.0, 3.0, 4.0, 5.0],
[0.0, 0.5, 0.75, 1.0, 1000.0])
if output_gb < -0.2:
output_gb += CP.startingBrakeRate / RATE * factor
self.reset(CS.vEgo)
self.last_output_gb = output_gb
final_gas = clip(output_gb, 0., gas_max)
final_brake = -clip(output_gb, -brake_max, 0.)
if self.long_control_state == LongCtrlState.stopping:
self.long_stat = "STP"
elif self.long_control_state == LongCtrlState.starting:
self.long_stat = "STR"
elif self.long_control_state == LongCtrlState.pid:
self.long_stat = "PID"
elif self.long_control_state == LongCtrlState.off:
self.long_stat = "OFF"
else:
self.long_stat = "---"
str_log3 = 'LS={:s} GS={:01.2f}/{:01.2f} BK={:01.2f}/{:01.2f} GB={:+04.2f} TG=P:{:05.2f}/F:{:05.2f}/m:{:.1f}/m2:{:.1f}/m3:{:.1f}/vr:{:+04.1f}'.format(
self.long_stat, final_gas, gas_max,
abs(final_brake), abs(brake_max), output_gb, abs(self.v_pid),
abs(v_target_future), multiplier, multiplier2, multiplier3, vRel)
trace1.printf2('{}'.format(str_log3))
return final_gas, final_brake
def update(self, CC, CS, frame, sm, CP):
if self.CP != CP:
self.CP = CP
self.param_load()
enabled = CC.enabled
actuators = CC.actuators
pcm_cancel_cmd = CC.cruiseControl.cancel
path_plan = sm['pathPlan']
abs_angle_steers = abs(actuators.steerAngle)
self.dRel, self.yRel, self.vRel = SpdController.get_lead(sm)
if self.SC is not None:
self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo)
else:
self.model_speed = self.model_sum = 0
# Steering Torque
param = self.steerParams_torque(CS, abs_angle_steers, path_plan, CC)
new_steer = actuators.steer * param.STEER_MAX
apply_steer = apply_std_steer_torque_limits(new_steer,
self.apply_steer_last,
CS.out.steeringTorque,
param)
self.steer_rate_limited = new_steer != apply_steer
apply_steer_limit = param.STEER_MAX
if self.steer_torque_ratio < 1:
apply_steer_limit = int(self.steer_torque_ratio * param.STEER_MAX)
apply_steer = self.limit_ctrl(apply_steer, apply_steer_limit, 0)
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
lkas_active = enabled and abs(
CS.out.steeringAngle) < 180. #and self.lkas_button
# fix for Genesis hard fault at low speed
#if CS.out.vEgo < 16.666667 and self.car_fingerprint == CAR.GENESIS:
# lkas_active = 0
if not lkas_active:
apply_steer = 0
steer_req = 1 if apply_steer else 0
self.apply_steer_last = apply_steer
sys_warning, sys_state = self.process_hud_alert(lkas_active, CC)
clu11_speed = CS.clu11["CF_Clu_Vanz"]
enabled_speed = 38 if CS.is_set_speed_in_mph else 60
if clu11_speed > enabled_speed or not lkas_active:
enabled_speed = clu11_speed
can_sends = []
if frame == 0: # initialize counts from last received count signals
self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"] + 1
self.lkas11_cnt %= 0x10
can_sends.append(
create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint,
apply_steer, steer_req, CS.lkas11, sys_warning,
sys_state, CC, enabled, 0))
if CS.mdps_bus or CS.scc_bus == 1: # send lkas11 bus 1 if mdps is on bus 1
can_sends.append(
create_lkas11(self.packer, self.lkas11_cnt,
self.car_fingerprint, apply_steer, steer_req,
CS.lkas11, sys_warning, sys_state, CC, enabled,
1))
if CS.mdps_bus: # send clu11 to mdps if it is not on bus 0
can_sends.append(
create_clu11(self.packer, frame, CS.mdps_bus, CS.clu11,
Buttons.NONE, enabled_speed))
#if pcm_cancel_cmd and self.longcontrol:
# can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed))
#else: # send mdps12 to LKAS to prevent LKAS error if no cancel cmd
can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
#str_log1 = 'CV={:5.1f}/{:5.3f} torg:{:5.0f}'.format( self.model_speed, self.model_sum, apply_steer )
str_log1 = 'CV={:5.1f} torg:{:6.1f}'.format(self.model_speed,
apply_steer)
#str_log2 = 'limit={:.0f} tm={:.1f} '.format( apply_steer_limit, self.timer1.sampleTime() )
str_log2 = ' limit={:6.1f}/tm={:3.1f} MAX={:5.1f} UP/DN={:3.1f}/{:3.1f} '.format(
apply_steer_limit, self.timer1.sampleTime(), self.MAX, self.UP,
self.DN)
trace1.printf('{} {}'.format(str_log1, str_log2))
run_speed_ctrl = self.param_OpkrAccelProfile and CS.acc_active and self.SC != None
if not run_speed_ctrl:
str_log2 = 'U={:.0f} LK={:.0f} dir={} steer={:5.0f} '.format(
CS.Mdps_ToiUnavail, CS.lkas_button_on,
self.steer_torque_ratio_dir, CS.out.steeringTorque)
trace1.printf2('{}'.format(str_log2))
if pcm_cancel_cmd and self.CP.longcontrolEnabled:
can_sends.append(
create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL))
elif CS.out.cruiseState.standstill:
# run only first time when the car stopped
if self.last_lead_distance == 0 or not self.param_OpkrAutoResume:
# get the lead distance from the Radar
self.last_lead_distance = CS.lead_distance
self.resume_cnt = 0
# when lead car starts moving, create 6 RES msgs
elif CS.lead_distance != self.last_lead_distance < CS.lead_distance > 4.8 and (
frame - self.last_resume_frame) > 5:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.RES_ACCEL, clu11_speed))
self.resume_cnt += 1
# interval after 10 msgs
if self.resume_cnt > 10:
self.last_resume_frame = frame
self.resume_cnt = 0
self.clu11_cnt = 0
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
elif run_speed_ctrl and self.SC != None:
is_sc_run = self.SC.update(CS, sm, self)
if is_sc_run:
can_sends.append(
create_clu11(self.packer, self.resume_cnt, CS.scc_bus,
CS.clu11, self.SC.btn_type,
self.SC.sc_clu_speed))
self.resume_cnt += 1
else:
self.resume_cnt = 0
str1 = 'run={} cruise_set_mode={} kph={:.1f}/{:.1f} DO={:.0f}/{:.0f} '.format(
is_sc_run, self.SC.cruise_set_mode,
self.SC.cruise_set_speed_kph, CS.VSetDis, CS.driverOverride,
CS.cruise_buttons)
str2 = 'btn_type={:.0f} speed={:.1f} cnt={:.0f}'.format(
self.SC.btn_type, self.SC.sc_clu_speed, self.resume_cnt)
str_log = str1 + str2
self.traceCC.add(str_log)
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in [
CAR.PALISADE, CAR.SELTOS
]:
can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
# counter inc
self.lkas11_cnt += 1
return can_sends
def update(self, c, CS, frame, sm, CP):
if self.CP != CP:
self.CP = CP
self.param_load()
enabled = c.enabled
actuators = c.actuators
pcm_cancel_cmd = c.cruiseControl.cancel
path_plan = sm['pathPlan']
self.dRel, self.yRel, self.vRel = SpdController.get_lead(sm)
if self.SC is not None:
self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo)
else:
self.model_speed = self.model_sum = 0
# Steering Torque
param, dst_steer = self.steerParams_torque(CS, c.actuators, path_plan)
new_steer = actuators.steer * param.STEER_MAX
apply_steer = apply_std_steer_torque_limits(new_steer,
self.apply_steer_last,
CS.out.steeringTorque,
param)
self.steer_rate_limited = new_steer != apply_steer
apply_steer_limit = param.STEER_MAX
if self.steer_torque_ratio < 1:
apply_steer_limit = int(self.steer_torque_ratio * param.STEER_MAX)
apply_steer = self.limit_ctrl(apply_steer, apply_steer_limit, 0)
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
lkas_active = enabled and CS.main_on and CS.out.cruiseState.enabled and abs(
CS.out.steeringAngle) < 180. #and self.lkas_button
if not lkas_active:
apply_steer = 0
steer_req = 1 if apply_steer else 0
self.apply_steer_last = apply_steer
sys_warning, self.hud_sys_state = self.process_hud_alert(
lkas_active, c)
can_sends = []
if frame == 0: # initialize counts from last received count signals
self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"] + 1
self.lkas11_cnt %= 0x10
can_sends.append(
create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint,
apply_steer, steer_req, CS.lkas11, sys_warning,
self.hud_sys_state, c))
# send mdps12 to LKAS to prevent LKAS error if no cancel cmd
can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
str_log1 = 'torg:{:5.0f}/{:5.0f}/{:5.0f} CV={:5.1f}'.format(
apply_steer, new_steer, dst_steer, self.model_speed)
str_log2 = 'limit={:.0f} tm={:.1f} '.format(apply_steer_limit,
self.timer1.sampleTime())
trace1.printf('{} {}'.format(str_log1, str_log2))
run_speed_ctrl = self.param_OpkrAccelProfile and CS.acc_active and self.SC != None
if not run_speed_ctrl:
str_log2 = 'LKAS={:.0f} steer={:5.0f} '.format(
CS.lkas_button_on, CS.out.steeringTorque)
trace1.printf2('{}'.format(str_log2))
if pcm_cancel_cmd and self.CP.longcontrolEnabled:
can_sends.append(
create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL))
elif CS.out.cruiseState.standstill:
# run only first time when the car stopped
if self.last_lead_distance == 0 or not self.param_OpkrAutoResume:
# get the lead distance from the Radar
self.last_lead_distance = CS.lead_distance
self.resume_cnt = 0
# when lead car starts moving, create 6 RES msgs
elif CS.lead_distance != self.last_lead_distance and (
frame - self.last_resume_frame) > 5:
can_sends.append(
create_clu11(self.packer, self.resume_cnt, CS.clu11,
Buttons.RES_ACCEL))
self.resume_cnt += 1
# interval after 6 msgs
if self.resume_cnt > 5:
self.last_resume_frame = frame
self.resume_cnt = 0
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
elif run_speed_ctrl and self.SC != None:
is_sc_run = self.SC.update(CS, sm, self)
if is_sc_run:
can_sends.append(
create_clu11(self.packer, self.resume_cnt, CS.clu11,
self.SC.btn_type, self.SC.sc_clu_speed))
self.resume_cnt += 1
else:
self.resume_cnt = 0
str1 = 'run={} cruise_set_mode={} kph={:.1f}/{:.1f} DO={:.0f}/{:.0f} '.format(
is_sc_run, self.SC.cruise_set_mode,
self.SC.cruise_set_speed_kph, CS.VSetDis, CS.driverOverride,
CS.cruise_buttons)
str2 = 'btn_type={:.0f} speed={:.1f} cnt={:.0f}'.format(
self.SC.btn_type, self.SC.sc_clu_speed, self.resume_cnt)
str_log = str1 + str2
self.traceCC.add(str_log)
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in [
CAR.SONATA, CAR.PALISADE
]:
can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
# counter inc
self.lkas11_cnt += 1
return can_sends
def update(self, v_ego_kph, CS, sm, actuators, dRel, yRel, vRel,
model_speed):
btn_type = Buttons.NONE
#lead_1 = sm['radarState'].leadOne
long_wait_cmd = 500
set_speed = CS.cruise_set_speed_kph
dec_step_cmd = 0
if self.long_curv_timer < 600:
self.long_curv_timer += 1
# 선행 차량 거리유지
lead_wait_cmd, lead_set_speed = self.update_lead(CS, dRel, yRel, vRel)
# 커브 감속.
curv_wait_cmd, curv_set_speed = self.update_curv(CS, sm, model_speed)
if curv_wait_cmd != 0:
if lead_set_speed > curv_set_speed:
dec_step_cmd = 1
set_speed = curv_set_speed
long_wait_cmd = curv_wait_cmd
else:
set_speed = lead_set_speed
long_wait_cmd = lead_wait_cmd
else:
set_speed = lead_set_speed
long_wait_cmd = lead_wait_cmd
if set_speed > CS.cruise_set_speed_kph:
set_speed = CS.cruise_set_speed_kph
elif set_speed < 30:
set_speed = 30
# control process
target_set_speed = set_speed
delta = int(set_speed) - int(CS.VSetDis)
if dec_step_cmd == 0 and delta < -1:
if delta < -3:
dec_step_cmd = 4
elif delta < -2:
dec_step_cmd = 3
else:
dec_step_cmd = 2
else:
dec_step_cmd = 1
if self.long_curv_timer < long_wait_cmd:
pass
elif CS.driverOverride == 1: # 가속패달에 의한 속도 설정.
if CS.cruise_set_speed_kph > CS.clu_Vanz:
delta = int(CS.clu_Vanz) - int(CS.VSetDis)
if delta > 1:
set_speed = CS.clu_Vanz
self.seq_step_debug = 97
btn_type = Buttons.SET_DECEL
elif delta <= -1:
set_speed = CS.VSetDis - dec_step_cmd
self.seq_step_debug = 98
btn_type = Buttons.SET_DECEL
self.long_curv_timer = 0
elif delta >= 1 and (model_speed > 200 or CS.clu_Vanz < 70):
set_speed = CS.VSetDis + dec_step_cmd
self.seq_step_debug = 99
btn_type = Buttons.RES_ACCEL
self.long_curv_timer = 0
if set_speed > CS.cruise_set_speed_kph:
set_speed = CS.cruise_set_speed_kph
if CS.cruise_set_mode == 0:
btn_type = Buttons.NONE
str3 = 'SS={:03.0f} TSS={:03.0f} VSD={:03.0f} DAt={:03.0f}/{:03.0f}/{:03.0f} DG/dec={:02.0f}/{:02.0f}'.format(
set_speed, target_set_speed, CS.VSetDis, CS.driverAcc_time,
long_wait_cmd, self.long_curv_timer, self.seq_step_debug,
dec_step_cmd)
str4 = ' LD/LS={:03.0f}/{:03.0f} '.format(CS.lead_distance,
CS.lead_objspd)
str5 = str3 + str4
trace1.printf2(str5)
return btn_type, set_speed
def update(self, active, CS, CP, long_plan, radarState):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Interp control trajectory
# TODO estimate car specific lag, use .5s for now
if len(long_plan.speeds) == CONTROL_N:
v_target = interp(DEFAULT_LONG_LAG, T_IDXS[:CONTROL_N], long_plan.speeds)
v_target_future = long_plan.speeds[-1]
a_target = interp(DEFAULT_LONG_LAG, T_IDXS[:CONTROL_N], long_plan.accels)
else:
v_target = 0.0
v_target_future = 0.0
a_target = 0.0
# Actuation limits
gas_max = interp(CS.vEgo, CP.gasMaxBP, CP.gasMaxV)
brake_max = interp(CS.vEgo, CP.brakeMaxBP, CP.brakeMaxV)
# Update state machine
output_gb = self.last_output_gb
if radarState is None:
dRel = 200
vRel = 0
else:
dRel = radarState.leadOne.dRel
vRel = radarState.leadOne.vRel
if long_plan.hasLead:
stop = True if (dRel < 4.0 and radarState.leadOne.status) else False
else:
stop = False
self.long_control_state = long_control_state_trans(active, self.long_control_state, CS.vEgo,
v_target_future, self.v_pid, output_gb,
CS.brakePressed, CS.cruiseState.standstill, stop, CS.gasPressed, CP.minSpeedCan)
v_ego_pid = max(CS.vEgo, CP.minSpeedCan) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
if (self.long_control_state == LongCtrlState.off or (CS.brakePressed or CS.gasPressed)) and self.candidate not in [CAR.NIRO_EV]:
self.v_pid = v_ego_pid
self.pid.reset()
output_gb = 0.
elif self.long_control_state == LongCtrlState.off or CS.gasPressed:
self.reset(v_ego_pid)
output_gb = 0.
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target
self.pid.pos_limit = gas_max
self.pid.neg_limit = - brake_max
# Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7
deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV)
# opkr
if self.vRel_prev != vRel and vRel <= 0 and CS.vEgo > 13. and self.damping_timer <= 0: # decel mitigation for a while
if (vRel - self.vRel_prev)*3.6 < -4:
self.damping_timer = 45
self.decel_damping2 = interp(abs((vRel - self.vRel_prev)*3.6), [0, 10], [1, 0.1])
self.vRel_prev = vRel
elif self.damping_timer > 0:
self.damping_timer -= 1
self.decel_damping = interp(self.damping_timer, [0, 45], [1, self.decel_damping2])
output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot)
output_gb *= self.decel_damping
if prevent_overshoot or CS.brakeHold:
output_gb = min(output_gb, 0.0)
# Intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping:
# Keep applying brakes until the car is stopped
factor = 1
if long_plan.hasLead:
factor = interp(dRel,[2.0,4.0,5.0,6.0,7.0,8.0], [2.0,1.0,0.7,0.5,0.3,0.0])
if not CS.standstill or output_gb > -BRAKE_STOPPING_TARGET:
output_gb -= CP.stoppingBrakeRate / RATE * factor
elif CS.cruiseState.standstill and output_gb < -BRAKE_STOPPING_TARGET:
output_gb += CP.stoppingBrakeRate / RATE
output_gb = clip(output_gb, -brake_max, gas_max)
self.reset(CS.vEgo)
# Intention is to move again, release brake fast before handing control to PID
elif self.long_control_state == LongCtrlState.starting:
factor = 1
if long_plan.hasLead:
factor = interp(dRel,[0.0,2.0,3.0,4.0,5.0], [0.0,0.5,1,250.0,500.0])
if output_gb < -0.2:
output_gb += CP.startingBrakeRate / RATE * factor
self.reset(CS.vEgo)
self.last_output_gb = output_gb
final_gas = clip(output_gb, 0., gas_max)
final_brake = -clip(output_gb, -brake_max, 0.)
if self.long_control_state == LongCtrlState.stopping:
self.long_stat = "STP"
elif self.long_control_state == LongCtrlState.starting:
self.long_stat = "STR"
elif self.long_control_state == LongCtrlState.pid:
self.long_stat = "PID"
elif self.long_control_state == LongCtrlState.off:
self.long_stat = "OFF"
else:
self.long_stat = "---"
if long_plan.longitudinalPlanSource == LongitudinalPlanSource.cruise:
self.long_plan_source = "cruise"
elif long_plan.longitudinalPlanSource == LongitudinalPlanSource.lead0:
self.long_plan_source = "lead0"
elif long_plan.longitudinalPlanSource == LongitudinalPlanSource.lead1:
self.long_plan_source = "lead1"
elif long_plan.longitudinalPlanSource == LongitudinalPlanSource.lead2:
self.long_plan_source = "lead2"
elif long_plan.longitudinalPlanSource == LongitudinalPlanSource.e2e:
self.long_plan_source = "e2e"
else:
self.long_plan_source = "---"
if CP.sccBus != 0 and self.long_log:
str_log3 = 'BUS={:1.0f}/{:1.0f} LS={:s} LP={:s} GS={:01.2f}/{:01.2f} BK={:01.2f}/{:01.2f} GB={:+04.2f} G={:1.0f} GS={} TG={:04.2f}/{:+04.2f}'.format(CP.mdpsBus, CP.sccBus, self.long_stat, self.long_plan_source, final_gas, gas_max, abs(final_brake), abs(brake_max), output_gb, CS.cruiseGapSet, int(CS.gasPressed), v_target, a_target)
trace1.printf2('{}'.format(str_log3))
return final_gas, final_brake, v_target, a_target
def update(self, c, CS, frame, sm):
enabled = c.enabled
actuators = c.actuators
pcm_cancel_cmd = c.cruiseControl.cancel
abs_angle_steers = abs(actuators.steerAngle)
self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo)
# Steering Torque
path_plan = sm['pathPlan']
param = self.steerParams_torque(CS, abs_angle_steers, path_plan)
new_steer = actuators.steer * param.STEER_MAX
apply_steer = apply_std_steer_torque_limits(new_steer,
self.apply_steer_last,
CS.out.steeringTorque,
param)
self.steer_rate_limited = new_steer != apply_steer
apply_steer_limit = param.STEER_MAX
if self.steer_torque_ratio < 1:
apply_steer_limit = int(self.steer_torque_ratio * param.STEER_MAX)
apply_steer = self.limit_ctrl(apply_steer, apply_steer_limit, 0)
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
lkas_active = enabled and abs(CS.out.steeringAngle) < 120.
# 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
steer_req = 1 if apply_steer else 0
self.apply_steer_last = apply_steer
sys_warning, sys_state = self.process_hud_alert(lkas_active, c)
if 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
can_sends = []
can_sends.append(
create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint,
apply_steer, steer_req, CS.lkas11, sys_warning,
sys_state, c))
can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
str_log1 = 'torg:{:>4.0f}/{:>4.0f} CV:{:>5.0f}'.format(
apply_steer, new_steer, self.model_speed)
str_log2 = 'max={:>4.0f} tm={:>5.1f} '.format(apply_steer_limit,
self.timer1.sampleTime())
trace1.printf('{} {}'.format(str_log1, str_log2))
lfa_usm = CS.lfahda["LFA_USM"]
lfa_warn = CS.lfahda["LFA_SysWarning"]
lfa_active = CS.lfahda["ACTIVE2"]
hda_usm = CS.lfahda["HDA_USM"]
hda_active = CS.lfahda["ACTIVE"]
str_log1 = 'hda={:.0f},{:.0f}'.format(hda_usm, hda_active)
str_log2 = 'lfa={:.0f},{:.0f},{:.0f}'.format(lfa_usm, lfa_warn,
lfa_active)
trace1.printf2('{} {}'.format(str_log1, str_log2))
if pcm_cancel_cmd and self.CP.openpilotLongitudinalControl:
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, self.resume_cnt, CS.clu11,
Buttons.RES_ACCEL))
self.resume_cnt += 1
# interval after 6 msgs
if self.resume_cnt > 5:
self.last_resume_frame = frame
# 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.IONIQ
]:
can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
return can_sends
def update(self, active, CS, v_target, v_target_future, a_target_raw,
a_target, CP, hasLead, radarState, longitudinalPlanSource,
extras):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Actuation limits
gas_max = interp(CS.vEgo, CP.gasMaxBP, CP.gasMaxV)
brake_max = interp(CS.vEgo, CP.brakeMaxBP, CP.brakeMaxV)
# Update state machine
output_gb = self.last_output_gb
if radarState is None:
dRel = 200
vRel = 0
else:
dRel = radarState.leadOne.dRel
vRel = radarState.leadOne.vRel
if hasLead:
stop = True if (dRel < 4.0
and radarState.leadOne.status) else False
else:
stop = False
self.long_control_state = long_control_state_trans(
active, self.long_control_state, CS.vEgo, v_target_future,
self.v_pid, output_gb, CS.brakePressed, CS.cruiseState.standstill,
stop, CS.gasPressed, CP.minSpeedCan)
v_ego_pid = max(
CS.vEgo, CP.minSpeedCan
) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
if self.long_control_state == LongCtrlState.off or (CS.brakePressed
or CS.gasPressed):
self.v_pid = v_ego_pid
self.pid.reset()
output_gb = 0.
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target
self.pid.pos_limit = gas_max
self.pid.neg_limit = -brake_max
afactor = 1
vfactor = 1
dfactor = 1
dvfactor = 1
# Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7
deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP,
CP.longitudinalTuning.deadzoneV)
output_gb = self.pid.update(self.v_pid,
v_ego_pid,
speed=v_ego_pid,
deadzone=deadzone,
feedforward=a_target,
freeze_integrator=prevent_overshoot)
# added by opkr
afactor = interp(CS.vEgo, [0, 1, 2, 3, 4, 8, 12, 16, 20],
[4.5, 4.2, 3.65, 3.375, 3.1, 2.3, 2.1, 2, 2])
vfactor = interp(dRel, [1, 30, 50], [15, 7, 4])
dfactor = interp(dRel, [4, 10], [1.6, 1])
dvfactor = interp(((CS.vEgo * 3.6) / (max(3, dRel))), [1, 2, 3],
[1, 3, 5])
# if abs(output_gb) < abs(a_target_raw)/afactor and a_target_raw < 0 and dRel >= 4.2:
# output_gb = (-abs(a_target_raw)/afactor)*dfactor
if output_gb > 0 and a_target_raw < 0 and dRel >= 4.2:
output_gb = output_gb / vfactor
elif output_gb > 0 and a_target_raw > 0 and dRel >= 4.2 and (
CS.vEgo * 3.6) < 65:
output_gb = output_gb / dvfactor
if prevent_overshoot or CS.brakeHold:
output_gb = min(output_gb, 0.0)
# Intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping:
# Keep applying brakes until the car is stopped
factor = 1
if hasLead:
factor = interp(dRel, [2.0, 4.2, 5.0, 6.0, 7.0, 8.0],
[2.5, 1, 0.7, 0.5, 0.3, 0.0])
if not CS.standstill or output_gb > -BRAKE_STOPPING_TARGET:
output_gb -= CP.stoppingBrakeRate / RATE * factor
elif CS.cruiseState.standstill and output_gb < -BRAKE_STOPPING_TARGET:
output_gb += CP.stoppingBrakeRate / RATE
output_gb = clip(output_gb, -brake_max, gas_max)
self.reset(CS.vEgo)
# Intention is to move again, release brake fast before handing control to PID
elif self.long_control_state == LongCtrlState.starting:
factor = 1
if hasLead:
factor = interp(dRel, [0.0, 2.0, 3.0, 4.2, 5.0],
[0.0, 0.5, 1, 500.0, 1500.0])
if output_gb < -0.2:
output_gb += CP.startingBrakeRate / RATE * factor
self.reset(CS.vEgo)
self.last_output_gb = output_gb
final_gas = clip(output_gb, 0., gas_max)
final_brake = -clip(output_gb, -brake_max, 0.)
if self.long_control_state == LongCtrlState.stopping:
self.long_stat = "STP"
elif self.long_control_state == LongCtrlState.starting:
self.long_stat = "STR"
elif self.long_control_state == LongCtrlState.pid:
self.long_stat = "PID"
elif self.long_control_state == LongCtrlState.off:
self.long_stat = "OFF"
else:
self.long_stat = "---"
if Params().get_bool("OpenpilotLongitudinalControl") and Params(
).get_bool("LongLogDisplay"):
str_log3 = 'MDPS={:1.0f} SCC={:1.0f} LS={:s} GS={:01.2f}/{:01.2f} BK={:01.2f}/{:01.2f} GB={:+04.2f} TG={:+04.2f} G={:1.0f} GS={}'.format(
CP.mdpsBus, CP.sccBus, self.long_stat, final_gas, gas_max,
abs(final_brake), abs(brake_max), output_gb, a_target_raw,
CS.cruiseGapSet, CS.gasPressed)
trace1.printf2('{}'.format(str_log3))
return final_gas, final_brake
def update(self, CC, CS, frame, sm, CP):
if self.CP != CP:
self.CP = CP
enabled = CC.enabled
actuators = CC.actuators
pcm_cancel_cmd = CC.cruiseControl.cancel
self.dRel, self.yRel, self.vRel = SpdController.get_lead(sm)
self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo)
# Steering Torque
new_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = apply_std_steer_torque_limits(new_steer,
self.apply_steer_last,
CS.out.steeringTorque,
SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
lkas_active = enabled and abs(CS.out.steeringAngle) < 90.
if ((CS.out.leftBlinker and not CS.out.rightBlinker) or
(CS.out.rightBlinker
and not CS.out.leftBlinker)) and CS.out.vEgo <= 59 * CV.KPH_TO_MS:
self.lanechange_manual_timer = 10
if CS.out.leftBlinker and CS.out.rightBlinker:
self.emergency_manual_timer = 10
if abs(CS.out.steeringTorque) > 200:
self.driver_steering_torque_above_timer = 15
if self.lanechange_manual_timer or self.driver_steering_torque_above_timer:
lkas_active = 0
if self.lanechange_manual_timer > 0:
self.lanechange_manual_timer -= 1
if self.emergency_manual_timer > 0:
self.emergency_manual_timer -= 1
if self.driver_steering_torque_above_timer > 0:
self.driver_steering_torque_above_timer -= 1
if not lkas_active:
apply_steer = 0
steer_req = 1 if apply_steer else 0
self.apply_steer_last = apply_steer
sys_warning, sys_state = self.process_hud_alert(lkas_active, CC)
clu11_speed = CS.clu11["CF_Clu_Vanz"]
enabled_speed = 38 if CS.is_set_speed_in_mph else 55
if clu11_speed > enabled_speed:
enabled_speed = clu11_speed
can_sends = []
if frame == 0: # initialize counts from last received count signals
self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"] + 1
self.lkas11_cnt %= 0x10
can_sends.append(
create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint,
apply_steer, steer_req, CS.lkas11, sys_warning,
sys_state, CC, enabled, 0))
if CS.mdps_bus or CS.scc_bus == 1: # send lkas11 bus 1 if mdps is on bus 1
can_sends.append(
create_lkas11(self.packer, self.lkas11_cnt,
self.car_fingerprint, apply_steer, steer_req,
CS.lkas11, sys_warning, sys_state, CC, enabled,
1))
if CS.mdps_bus: # send clu11 to mdps if it is not on bus 0
#if frame % 2 and CS.mdps_bus == 1: # send clu11 to mdps if it is not on bus 0
can_sends.append(
create_clu11(self.packer, frame, CS.mdps_bus, CS.clu11,
Buttons.NONE, enabled_speed))
#if CS.mdps_bus:
can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
str_log1 = '곡률={:05.1f}'.format(self.model_speed)
str_log2 = '프레임율={:03.0f} TPMS=FL:{:04.1f}/FR:{:04.1f}/RL:{:04.1f}/RR:{:04.1f}'.format(
self.timer1.sampleTime(), CS.tpmsPressureFl, CS.tpmsPressureFr,
CS.tpmsPressureRl, CS.tpmsPressureRr)
trace1.printf('{} {}'.format(str_log1, str_log2))
if CS.out.cruiseState.modeSel == 0 and self.mode_change_switch == 4:
self.mode_change_timer = 50
self.mode_change_switch = 0
elif CS.out.cruiseState.modeSel == 1 and self.mode_change_switch == 0:
self.mode_change_timer = 50
self.mode_change_switch = 1
elif CS.out.cruiseState.modeSel == 2 and self.mode_change_switch == 1:
self.mode_change_timer = 50
self.mode_change_switch = 2
elif CS.out.cruiseState.modeSel == 3 and self.mode_change_switch == 2:
self.mode_change_timer = 50
self.mode_change_switch = 3
elif CS.out.cruiseState.modeSel == 4 and self.mode_change_switch == 3:
self.mode_change_timer = 50
self.mode_change_switch = 4
if self.mode_change_timer > 0:
self.mode_change_timer -= 1
run_speed_ctrl = self.param_OpkrAccelProfile and CS.acc_active and self.SC != None and (
CS.out.cruiseState.modeSel == 1 or CS.out.cruiseState.modeSel == 2
or CS.out.cruiseState.modeSel == 3)
if not run_speed_ctrl:
if CS.out.cruiseState.modeSel == 0:
self.steer_mode = "오파모드"
elif CS.out.cruiseState.modeSel == 1:
self.steer_mode = "차간+커브"
elif CS.out.cruiseState.modeSel == 2:
self.steer_mode = "차간ONLY"
elif CS.out.cruiseState.modeSel == 3:
self.steer_mode = "정체구간"
elif CS.out.cruiseState.modeSel == 4:
self.steer_mode = "순정모드"
if CS.out.steerWarning == 0:
self.mdps_status = "정상"
elif CS.out.steerWarning == 1:
self.mdps_status = "오류"
if CS.lkas_button_on == 0:
self.lkas_switch = "OFF"
elif CS.lkas_button_on == 1:
self.lkas_switch = "ON"
else:
self.lkas_switch = "-"
if CS.out.cruiseState.modeSel == 3:
str_log2 = '주행모드={:s} MDPS상태={:s} LKAS버튼={:s} CG:{:1.0f}'.format(
self.steer_mode, self.mdps_status, self.lkas_switch,
CS.cruiseGapSet)
else:
str_log2 = '주행모드={:s} MDPS상태={:s} LKAS버튼={:s}'.format(
self.steer_mode, self.mdps_status, self.lkas_switch)
trace1.printf2('{}'.format(str_log2))
#print( 'st={} cmd={} long={} steer={} req={}'.format(CS.out.cruiseState.standstill, pcm_cancel_cmd, self.CP.openpilotLongitudinalControl, apply_steer, steer_req ) )
if pcm_cancel_cmd and self.CP.openpilotLongitudinalControl:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.CANCEL, clu11_speed))
elif CS.out.cruiseState.standstill:
# run only first time when the car stopped
if self.last_lead_distance == 0 or not self.param_OpkrAutoResume:
# get the lead distance from the Radar
self.last_lead_distance = CS.lead_distance
self.resume_cnt = 0
# when lead car starts moving, create 6 RES msgs
elif CS.lead_distance != self.last_lead_distance and (
frame - self.last_resume_frame) > 5:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.RES_ACCEL, clu11_speed))
self.resume_cnt += 1
# interval after 6 msgs
if self.resume_cnt > 5:
self.last_resume_frame = frame
self.resume_cnt = 0
elif self.cruise_gap_prev == 0:
self.cruise_gap_prev = CS.cruiseGapSet
self.cruise_gap_set_init = 1
elif CS.cruiseGapSet != 1.0:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 100:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
self.cruise_gap = 0
elif run_speed_ctrl and self.SC != None:
is_sc_run = self.SC.update(CS, sm, self)
if is_sc_run:
can_sends.append(
create_clu11(self.packer, self.resume_cnt, CS.scc_bus,
CS.clu11, self.SC.btn_type,
self.SC.sc_clu_speed))
self.resume_cnt += 1
else:
self.resume_cnt = 0
if self.dRel > 17 and self.cruise_gap_prev != CS.cruiseGapSet and self.cruise_gap_set_init == 1 and CS.out.cruiseState.modeSel != 3:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 50:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
elif self.cruise_gap_prev == CS.cruiseGapSet:
self.cruise_gap_set_init = 0
self.cruise_gap_prev = 0
if CS.out.cruiseState.modeSel == 3 and CS.acc_active:
if 20 > self.dRel > 18 and self.vRel < 0 and CS.cruiseGapSet != 4.0:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 30:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
elif 16 > self.dRel > 14 and self.vRel < 0 and CS.cruiseGapSet != 3.0:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 30:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
elif 12 > self.dRel > 10 and self.vRel < 0 and CS.cruiseGapSet != 2.0:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 30:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
elif 9 > self.dRel > 7 and self.vRel < 0 and CS.cruiseGapSet != 1.0:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 30:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
elif 15 > self.dRel > 4 and self.vRel > 0 and CS.cruiseGapSet != 1.0:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 30:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
elif 25 > self.dRel > 18 and self.vRel >= 0 and CS.cruiseGapSet != 2.0:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 30:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
# 20 Hz LFA MFA message
#if frame % 5 == 0 and self.car_fingerprint in [CAR.IONIQ]:
# can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
self.lkas11_cnt += 1
return can_sends
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, 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
def update(self, active, CS, CP, path_plan):
lqr_log = log.ControlsState.LateralLQRState.new_message()
steers_max = get_steer_max(CP, CS.vEgo)
torque_scale = (0.45 +
CS.vEgo / 60.0)**2 # Scale actuator model with speed
#neokii
torque_scale = min(torque_scale, 0.65)
steering_angle = CS.steeringAngle
steeringTQ = CS.steeringTorqueEps
v_ego_kph = CS.vEgo * CV.MS_TO_KPH
self.ki, self.scale = self.atom_tune(v_ego_kph, CS.steeringAngle, CP)
# ### 설정값 최적화 분석을 위한 랜덤화 임시 코드
#now = datetime.datetime.now() # current date and time
#micro_S = int(now.microsecond)
#if micro_S < 10000 : #1초에 한번만 랜덤변환
# self.ki = random.uniform(0.015, 0.025) #self.ki - (self.ki*0.5), self.ki + (self.ki*0.5) )
# self.scale = random.uniform(1750, 1950) #int(self.scale) - int(self.scale*0.055), int(self.scale) + int(self.scale*0.055) ) )
# self.dc_gain = random.uniform(0.0028, 0.0032) #self.dc_gain - (self.dc_gain*0.1), self.dc_gain + (self.dc_gain*0.1) )
# steers_max = random.uniform(1.0, 1.2)
# ###########################
log_ki = self.ki
log_scale = self.scale
log_dc_gain = self.dc_gain
# Subtract offset. Zero angle should correspond to zero torque
self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset
steering_angle -= path_plan.angleOffset
# Update Kalman filter
angle_steers_k = float(self.C.dot(self.x_hat))
e = steering_angle - angle_steers_k
self.x_hat = self.A.dot(self.x_hat) + self.B.dot(
CS.steeringTorqueEps / torque_scale) + self.L.dot(e)
error = self.angle_steers_des - angle_steers_k
u_lqr = float(self.angle_steers_des / self.dc_gain -
self.K.dot(self.x_hat))
if CS.vEgo < 0.3 or not active:
lqr_log.active = False
lqr_output = 0.
self.reset()
else:
lqr_log.active = True
# LQR
#u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat))
lqr_output = torque_scale * u_lqr / self.scale
# Integrator
if CS.steeringPressed:
self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
else:
#error = self.angle_steers_des - angle_steers_k
i = self.i_lqr + self.ki * self.i_rate * error
control = lqr_output + i
if (error >= 0 and (control <= steers_max or i < 0.0)) or \
(error <= 0 and (control >= -steers_max or i > 0.0)):
self.i_lqr = i
self.output_steer = lqr_output + self.i_lqr
self.output_steer = clip(self.output_steer, -steers_max,
steers_max)
check_saturation = (
CS.vEgo >
10) and not CS.steeringRateLimited and not CS.steeringPressed
saturated = self._check_saturation(self.output_steer, check_saturation,
steers_max)
if not CS.steeringPressed:
str2 = '/{} /{} /{} /{} /{} /{} /{} /{} /{} /{}'.format(
v_ego_kph, steering_angle, self.angle_steers_des,
angle_steers_k, steeringTQ, torque_scale, log_scale, log_ki,
log_dc_gain, self.output_steer)
self.trLQR.add(str2)
str5 = 'LQR_Set:dc_gain={:06.4f}/scale={:06.1f}/ki={:05.3f}/OutputSteer={:5.3f}/Angle={:5.1f}|{:5.1f}'.format(
self.scale, self.dc_gain, self.ki, self.output_steer,
steering_angle, angle_steers_k)
trace1.printf2(str5)
lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset
lqr_log.i = self.i_lqr
lqr_log.output = self.output_steer
lqr_log.lqrOutput = lqr_output
lqr_log.saturated = saturated
return self.output_steer, float(self.angle_steers_des), lqr_log
def update(self, CC, CS, frame, sm, CP):
if self.CP != CP:
self.CP = CP
self.param_load()
enabled = CC.enabled
actuators = CC.actuators
pcm_cancel_cmd = CC.cruiseControl.cancel
self.dRel, self.yRel, self.vRel = SpdController.get_lead(sm)
if self.SC is not None:
self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo)
else:
self.model_speed = self.model_sum = 0
# Steering Torque
new_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = apply_std_steer_torque_limits(new_steer,
self.apply_steer_last,
CS.out.steeringTorque,
SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
lkas_active = enabled and abs(CS.out.steeringAngle) < 180. #90
if ((CS.out.leftBlinker and not CS.out.rightBlinker) or
(CS.out.rightBlinker
and not CS.out.leftBlinker)) and CS.out.vEgo < 60 * CV.KPH_TO_MS:
self.lanechange_manual_timer = 10
if CS.out.leftBlinker and CS.out.rightBlinker:
self.emergency_manual_timer = 10
if abs(CS.out.steeringTorque) > 409: #360: #180
self.driver_steering_torque_above_timer = 100
if self.lanechange_manual_timer or self.driver_steering_torque_above_timer:
lkas_active = 0
if self.lanechange_manual_timer > 0:
self.lanechange_manual_timer -= 1
if self.emergency_manual_timer > 0:
self.emergency_manual_timer -= 1
if self.driver_steering_torque_above_timer > 0:
self.driver_steering_torque_above_timer -= 1
if not lkas_active:
apply_steer = 0
steer_req = 1 if apply_steer else 0
self.apply_steer_last = apply_steer
sys_warning, sys_state = self.process_hud_alert(lkas_active, CC)
clu11_speed = CS.clu11["CF_Clu_Vanz"]
enabled_speed = 38 if CS.is_set_speed_in_mph else 55
if clu11_speed > enabled_speed:
enabled_speed = clu11_speed
can_sends = []
if frame == 0: # initialize counts from last received count signals
self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"] + 1
self.lkas11_cnt %= 0x10
can_sends.append(
create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint,
apply_steer, steer_req, CS.lkas11, sys_warning,
sys_state, CC, enabled, 0))
if CS.mdps_bus or CS.scc_bus == 1: # send lkas11 bus 1 if mdps is on bus 1
can_sends.append(
create_lkas11(self.packer, self.lkas11_cnt,
self.car_fingerprint, apply_steer, steer_req,
CS.lkas11, sys_warning, sys_state, CC, enabled,
1))
if CS.mdps_bus: # send clu11 to mdps if it is not on bus 0
#if frame % 2 and CS.mdps_bus == 1: # send clu11 to mdps if it is not on bus 0
can_sends.append(
create_clu11(self.packer, frame, CS.mdps_bus, CS.clu11,
Buttons.NONE, enabled_speed))
#if CS.mdps_bus:
can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
str_log1 = '곡률={:04.1f}/{:=+06.3f} 토크={:=+04.0f}/{:=+04.0f}'.format(
self.model_speed, self.model_sum, new_steer, CS.out.steeringTorque)
str_log2 = '프레임율={:03.0f}'.format(self.timer1.sampleTime())
trace1.printf('{} {}'.format(str_log1, str_log2))
if CS.out.cruiseState.modeSel == 0 and self.mode_change_switch == 4:
self.mode_change_timer = 50
self.mode_change_switch = 0
elif CS.out.cruiseState.modeSel == 1 and self.mode_change_switch == 0:
self.mode_change_timer = 50
self.mode_change_switch = 1
elif CS.out.cruiseState.modeSel == 2 and self.mode_change_switch == 1:
self.mode_change_timer = 50
self.mode_change_switch = 2
elif CS.out.cruiseState.modeSel == 3 and self.mode_change_switch == 2:
self.mode_change_timer = 50
self.mode_change_switch = 3
elif CS.out.cruiseState.modeSel == 4 and self.mode_change_switch == 3:
self.mode_change_timer = 50
self.mode_change_switch = 4
if self.mode_change_timer > 0:
self.mode_change_timer -= 1
run_speed_ctrl = self.param_OpkrAccelProfile and CS.acc_active and self.SC != None
if not run_speed_ctrl:
if CS.out.cruiseState.modeSel == 0:
self.steer_mode = "오파모드"
elif CS.out.cruiseState.modeSel == 1:
self.steer_mode = "차간+커브"
elif CS.out.cruiseState.modeSel == 2:
self.steer_mode = "차간ONLY"
elif CS.out.cruiseState.modeSel == 3:
self.steer_mode = "자동RES"
elif CS.out.cruiseState.modeSel == 4:
self.steer_mode = "순정모드"
if CS.out.steerWarning == 0:
self.mdps_status = "정상"
elif CS.out.steerWarning == 1:
self.mdps_status = "오류"
if CS.lkas_button_on == 0:
self.lkas_switch = "OFF"
elif CS.lkas_button_on == 1:
self.lkas_switch = "ON"
else:
self.lkas_switch = "-"
str_log2 = '주행모드={:s} MDPS상태={:s} LKAS버튼={:s}'.format(
self.steer_mode, self.mdps_status, self.lkas_switch)
trace1.printf2('{}'.format(str_log2))
#print( 'st={} cmd={} long={} steer={} req={}'.format(CS.out.cruiseState.standstill, pcm_cancel_cmd, self.CP.openpilotLongitudinalControl, apply_steer, steer_req ) )
if pcm_cancel_cmd and self.CP.openpilotLongitudinalControl:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.CANCEL, clu11_speed))
elif CS.out.cruiseState.standstill and not self.car_fingerprint == CAR.NIRO_EV:
# run only first time when the car stopped
if self.last_lead_distance == 0 or not self.param_OpkrAutoResume:
# get the lead distance from the Radar
self.last_lead_distance = CS.lead_distance
self.resume_cnt = 0
# when lead car starts moving, create 6 RES msgs
elif CS.lead_distance != self.last_lead_distance and (
frame - self.last_resume_frame) > 5:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.RES_ACCEL, clu11_speed))
self.resume_cnt += 1
# interval after 6 msgs
if self.resume_cnt > 5:
self.last_resume_frame = frame
self.resume_cnt = 0
elif CS.out.cruiseState.standstill and self.car_fingerprint == CAR.NIRO_EV:
if CS.lead_distance > 3.7 and (
frame - self.last_resume_frame
) * DT_CTRL > 0.2 and self.param_OpkrAutoResume:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.RES_ACCEL, clu11_speed))
self.last_resume_frame = frame
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
elif run_speed_ctrl and self.SC != None:
is_sc_run = self.SC.update(CS, sm, self)
if is_sc_run:
can_sends.append(
create_clu11(self.packer, self.resume_cnt, CS.scc_bus,
CS.clu11, self.SC.btn_type,
self.SC.sc_clu_speed))
self.resume_cnt += 1
else:
self.resume_cnt = 0
if CS.out.cruiseState.modeSel == 3:
if CS.out.brakeLights and CS.VSetDis > 30:
self.res_cnt = 0
self.res_delay = 50
elif self.res_delay:
self.res_delay -= 1
elif not self.res_delay and self.res_cnt < 0 and CS.VSetDis > 30 and CS.out.vEgo > 30 * CV.KPH_TO_MS:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.CANCEL, clu11_speed))
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.RES_ACCEL, clu11_speed))
self.res_cnt += 1
else:
self.res_cnt = 7
self.res_delay = 0
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in FEATURES["send_lfa_mfa"]:
can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
self.lkas11_cnt += 1
return can_sends
def update(self, CC, CS, frame, sm, CP ):
if self.CP != CP:
self.CP = CP
self.param_load()
enabled = CC.enabled
actuators = CC.actuators
pcm_cancel_cmd = CC.cruiseControl.cancel
self.dRel, self.yRel, self.vRel = SpdController.get_lead( sm )
if self.SC is not None:
self.model_speed, self.model_sum = self.SC.calc_va( sm, CS.out.vEgo )
else:
self.model_speed = self.model_sum = 0
# Steering Torque
if self.param_OpkrEnableLearner:
new_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
else:
path_plan = sm['pathPlan']
self.cV_tune( CS.out.vEgo, self.model_speed )
param = SteerLimitParams()
param.STEER_MAX = min( param.STEER_MAX, self.MAX )
param.STEER_DELTA_UP = min( param.STEER_DELTA_UP, self.UP )
param.STEER_DELTA_DOWN = min( param.STEER_DELTA_DOWN, self.DN )
new_steer = actuators.steer * param.STEER_MAX
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, param)
self.steer_rate_limited = new_steer != apply_steer
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
lkas_active = enabled and abs(CS.out.steeringAngle) < 90. #TenesiDel -> and self.lkas_button_on
# fix for Genesis hard fault at low speed 테네시 추가
if CS.out.vEgo < 60 * CV.KPH_TO_MS and self.car_fingerprint == CAR.GENESIS and not CS.mdps_bus:
lkas_active = False
# if (( CS.out.leftBlinker and not CS.out.rightBlinker) or ( CS.out.rightBlinker and not CS.out.leftBlinker)) and CS.out.vEgo > 60 * CV.KPH_TO_MS: # 깜빡이 작동시에도 상히조향 유지 수정해보기
# self.lanechange_manual_timer = 10
# if CS.out.leftBlinker and CS.out.rightBlinker:
# self.emergency_manual_timer = 10
# if abs(CS.out.steeringTorque) > self.driver_steering_torque_above and CS.out.vEgo > 60: # 깜빡이 작동시에도 상히조향 유지 수정해보기
# self.driver_steering_torque_above_timer = 30
# if self.lanechange_manual_timer or self.driver_steering_torque_above_timer:
# lkas_active = 0
# if self.lanechange_manual_timer > 0:
# self.lanechange_manual_timer -= 1
# if self.emergency_manual_timer > 0:
# self.emergency_manual_timer -= 1
# if self.driver_steering_torque_above_timer > 0:
# self.driver_steering_torque_above_timer -= 1
# Disable steering while turning blinker on and speed below 60 kph #201011 상시조향 작업 작동성공
if CS.out.leftBlinker or CS.out.rightBlinker:
if self.car_fingerprint not in [CAR.K5, CAR.K5_HEV]: # 테네시 추가 OPTIMA -> K5
self.turning_signal_timer = 100 # Disable for 1.0 Seconds after blinker turned off
elif CS.left_blinker_flash or CS.right_blinker_flash: # Optima has blinker flash signal only
self.turning_signal_timer = 100
if self.turning_signal_timer and CS.out.vEgo > 70 * CV.KPH_TO_MS: # TenesiADD Blinker tune 시속60미만에서는 상시조향
lkas_active = 0
if self.turning_signal_timer: # TenesiADD
self.turning_signal_timer -= 1 #201011 상시조향 작업 작동성공
if not lkas_active:
apply_steer = 0
steer_req = 1 if apply_steer else 0
self.apply_steer_last = apply_steer
sys_warning, sys_state = self.process_hud_alert( lkas_active, CC )
clu11_speed = CS.clu11["CF_Clu_Vanz"]
enabled_speed = 38 if CS.is_set_speed_in_mph else 55
if clu11_speed > enabled_speed:
enabled_speed = clu11_speed
can_sends = []
if frame == 0: # initialize counts from last received count signals
self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"] + 1
self.lkas11_cnt %= 0x10
can_sends.append(create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint, apply_steer, steer_req,
CS.lkas11, sys_warning, sys_state, CC, enabled, 0 ))
if CS.mdps_bus or CS.scc_bus == 1: # send lkas11 bus 1 if mdps is on bus 1
can_sends.append(create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint, apply_steer, steer_req,
CS.lkas11, sys_warning, sys_state, CC, enabled, 1 ))
if CS.mdps_bus: # send clu11 to mdps if it is not on bus 0
#if frame % 2 and CS.mdps_bus == 1: # send clu11 to mdps if it is not on bus 0
can_sends.append(create_clu11(self.packer, frame, CS.mdps_bus, CS.clu11, Buttons.NONE, enabled_speed))
#if CS.mdps_bus:
can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
str_log1 = '곡률={:05.1f}/{:=+06.3f} 토크={:=+04.0f}/{:=+04.0f}'.format( self.model_speed, self.model_sum, new_steer, CS.out.steeringTorque )
if self.param_OpkrEnableLearner:
str_log2 = '프레임율={:03.0f} STMAX={:03.0f}'.format( self.timer1.sampleTime(), SteerLimitParams.STEER_MAX, )
else:
str_log2 = '프레임율={:03.0f} ST={:03.0f}/{:01.0f}/{:01.0f} SR={:05.2f}'.format( self.timer1.sampleTime(), self.MAX, self.UP, self.DN, path_plan.steerRatio )
trace1.printf( '{} {}'.format( str_log1, str_log2 ) )
if CS.out.cruiseState.modeSel == 0 and self.mode_change_switch == 4:
self.mode_change_timer = 50
self.mode_change_switch = 0
elif CS.out.cruiseState.modeSel == 1 and self.mode_change_switch == 0:
self.mode_change_timer = 50
self.mode_change_switch = 1
elif CS.out.cruiseState.modeSel == 2 and self.mode_change_switch == 1:
self.mode_change_timer = 50
self.mode_change_switch = 2
elif CS.out.cruiseState.modeSel == 3 and self.mode_change_switch == 2:
self.mode_change_timer = 50
self.mode_change_switch = 3
elif CS.out.cruiseState.modeSel == 4 and self.mode_change_switch == 3:
self.mode_change_timer = 50
self.mode_change_switch = 4
if self.mode_change_timer > 0:
self.mode_change_timer -= 1
run_speed_ctrl = self.param_OpkrAccelProfile and CS.acc_active and self.SC != None and (CS.out.cruiseState.modeSel == 1 or CS.out.cruiseState.modeSel == 2 or CS.out.cruiseState.modeSel == 3)
if not run_speed_ctrl:
if CS.out.cruiseState.modeSel == 0:
self.steer_mode = "오파모드"
elif CS.out.cruiseState.modeSel == 1:
self.steer_mode = "차간+커브"
elif CS.out.cruiseState.modeSel == 2:
self.steer_mode = "차간ONLY"
elif CS.out.cruiseState.modeSel == 3:
self.steer_mode = "자동RES"
elif CS.out.cruiseState.modeSel == 4:
self.steer_mode = "순정모드"
if CS.out.steerWarning == 0:
self.mdps_status = "정상"
elif CS.out.steerWarning == 1:
self.mdps_status = "오류"
if CS.lkas_button_on == 0:
self.lkas_switch = "OFF"
elif CS.lkas_button_on == 1:
self.lkas_switch = "ON"
else:
self.lkas_switch = "-"
if CS.out.cruiseState.modeSel == 3:
str_log2 = '주행모드={:s} MDPS상태={:s} LKAS버튼={:s} AUTORES=(VS:{:03.0f}/CN:{:01.0f}/RD:{:03.0f}/BK:{})'.format( self.steer_mode, self.mdps_status, self.lkas_switch, CS.VSetDis, self.res_cnt, self.res_delay, CS.out.brakeLights )
else:
str_log2 = '주행모드={:s} MDPS상태={:s} LKAS버튼={:s}'.format( self.steer_mode, self.mdps_status, self.lkas_switch )
trace1.printf2( '{}'.format( str_log2 ) )
#print( 'st={} cmd={} long={} steer={} req={}'.format(CS.out.cruiseState.standstill, pcm_cancel_cmd, self.CP.openpilotLongitudinalControl, apply_steer, steer_req ) )
if pcm_cancel_cmd and self.CP.openpilotLongitudinalControl:
can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed))
elif CS.out.cruiseState.standstill and not self.car_fingerprint == CAR.NIRO_EV:
# run only first time when the car stopped
if self.last_lead_distance == 0 or not self.param_OpkrAutoResume:
# get the lead distance from the Radar
self.last_lead_distance = CS.lead_distance
self.resume_cnt = 0
# when lead car starts moving, create 6 RES msgs
elif CS.lead_distance != self.last_lead_distance and (frame - self.last_resume_frame) > 5:
can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed))
self.resume_cnt += 1
# interval after 6 msgs
if self.resume_cnt > 5:
self.last_resume_frame = frame
self.resume_cnt = 0
elif CS.out.cruiseState.standstill and self.car_fingerprint == CAR.NIRO_EV:
if CS.lead_distance > 3.7 and (frame - self.last_resume_frame)*DT_CTRL > 0.2 and self.param_OpkrAutoResume:
can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed))
self.last_resume_frame = frame
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
elif run_speed_ctrl and self.SC != None:
is_sc_run = self.SC.update( CS, sm, self )
if is_sc_run:
can_sends.append(create_clu11(self.packer, self.resume_cnt, CS.scc_bus, CS.clu11, self.SC.btn_type, self.SC.sc_clu_speed ))
self.resume_cnt += 1
else:
self.resume_cnt = 0
if CS.out.cruiseState.modeSel == 3:
if CS.out.brakeLights and CS.VSetDis > 30:
self.res_cnt = 0
self.res_delay = 50
elif self.res_delay:
self.res_delay -= 1
elif not self.res_delay and self.res_cnt < 6 and CS.VSetDis > 30 and CS.out.vEgo > 30 * CV.KPH_TO_MS:
if self.res_cnt < 1:
can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed))
can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed))
self.res_cnt += 1
else:
self.res_cnt = 7
self.res_delay = 0
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in FEATURES["send_lfa_mfa"]:
can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
self.lkas11_cnt += 1
return can_sends
