def __init__(self, dbc_name, CP, VM):
self.car_fingerprint = CP.carFingerprint
self.packer = CANPacker(dbc_name)
self.accel_steady = 0
self.apply_steer_last = 0
self.steer_rate_limited = False
self.lkas11_cnt = 0
self.scc12_cnt = 0
self.resume_cnt = 0
self.last_lead_distance = 0
self.resume_wait_timer = 0
self.lanechange_manual_timer = 0
self.emergency_manual_timer = 0
self.driver_steering_torque_above_timer = 0
self.mode_change_timer = 0
self.params = Params()
self.mode_change_switch = int(self.params.get('CruiseStatemodeSelInit'))
self.opkr_variablecruise = int(self.params.get('OpkrVariableCruise'))
self.opkr_autoresume = int(self.params.get('OpkrAutoResume'))
self.steer_mode = ""
self.mdps_status = ""
self.lkas_switch = ""
self.timer1 = tm.CTime1000("time")
self.SC = Spdctrl()
self.model_speed = 0
self.model_sum = 0
self.dRel = 0
self.yRel = 0
self.vRel = 0
self.cruise_gap = 0.0
self.cruise_gap_prev = 0
self.cruise_gap_set_init = 0
self.cruise_gap_switch_timer = 0
self.lkas_button_on = True
self.longcontrol = CP.openpilotLongitudinalControl
self.scc_live = not CP.radarOffCan
if CP.lateralTuning.which() == 'pid':
self.str_log2 = 'TUNE={:0.2f}/{:0.3f}/{:0.5f}'.format(CP.lateralTuning.pid.kpV[1], CP.lateralTuning.pid.kiV[1], CP.lateralTuning.pid.kf)
elif CP.lateralTuning.which() == 'indi':
self.str_log2 = 'TUNE={:03.1f}/{:03.1f}/{:03.1f}/{:03.1f}'.format(CP.lateralTuning.indi.innerLoopGain, CP.lateralTuning.indi.outerLoopGain, CP.lateralTuning.indi.timeConstant, CP.lateralTuning.indi.actuatorEffectiveness)
elif CP.lateralTuning.which() == 'lqr':
self.str_log2 = 'TUNE={:04.0f}/{:05.3f}/{:06.4f}'.format(CP.lateralTuning.lqr.scale, CP.lateralTuning.lqr.ki, CP.lateralTuning.lqr.dcGain)
if CP.spasEnabled:
self.en_cnt = 0
self.apply_steer_ang = 0.0
self.en_spas = 3
self.mdps11_stat_last = 0
self.spas_always = False
def __init__(self):
self.long_control_state = 0 # initialized to off
self.seq_step_debug = 0
self.long_curv_timer = 0
self.path_x = np.arange(192)
self.traceSC = trace1.Loger("SPD_CTRL")
self.wheelbase = 2.845
self.steerRatio = 12.5 # 12.5
self.v_model = 0
self.a_model = 0
self.v_cruise = 0
self.a_cruise = 0
self.l_poly = []
self.r_poly = []
self.movAvg = moveavg1.MoveAvg()
self.Timer1 = tm.CTime1000("SPD")
self.time_no_lean = 0
self.SC = trace1.Loger("spd")
def __init__(self, CP=None):
self.long_control_state = 0 # initialized to off
self.seq_step_debug = ""
self.long_curv_timer = 0
self.path_x = np.arange(192)
self.traceSC = trace1.Loger("SPD_CTRL")
self.v_model = 0
self.a_model = 0
self.v_cruise = 0
self.a_cruise = 0
self.l_poly = []
self.r_poly = []
self.movAvg = moveavg1.MoveAvg()
self.Timer1 = tm.CTime1000("SPD")
self.time_no_lean = 0
self.wait_timer2 = 0
self.cruise_set_speed_kph = 0
self.curise_set_first = 0
self.curise_sw_check = 0
self.prev_clu_CruiseSwState = 0
self.prev_VSetDis = 0
self.sc_clu_speed = 0
self.btn_type = Buttons.NONE
self.active_time = 0
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.apply_steer_last = 0
self.car_fingerprint = CP.carFingerprint
self.packer = CANPacker(dbc_name)
self.steer_rate_limited = False
# resume
self.resume_cnt = 0
self.last_resume_frame = 0
self.last_lead_distance = 0
self.lkas11_cnt = 0
# hud
self.hud_timer_left = 0
self.hud_timer_right = 0
self.enable_time = 0
self.steer_torque_over_timer = 0
self.steer_torque_ratio = 1
self.timer1 = tm.CTime1000("time")
self.SC = SpdctrlSlow()
self.model_speed = 0
self.model_sum = 0
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.apply_steer_last = 0
self.car_fingerprint = CP.carFingerprint
self.packer = CANPacker(dbc_name)
self.steer_rate_limited = False
self.resume_cnt = 0
self.last_resume_frame = 0
self.last_lead_distance = 0
self.lanechange_manual_timer = 0
self.emergency_manual_timer = 0
self.driver_steering_torque_above_timer = 0
self.mode_change_timer = 0
self.steer_mode = ""
self.mdps_status = ""
self.lkas_switch = ""
self.lkas11_cnt = 0
self.nBlinker = 0
self.dRel = 0
self.yRel = 0
self.vRel = 0
self.timer1 = tm.CTime1000("time")
self.model_speed = 0
self.model_sum = 0
# hud
self.hud_timer_left = 0
self.hud_timer_right = 0
self.command_cnt = 0
self.command_load = 0
self.params = Params()
# param
self.param_preOpkrAccelProfile = -1
self.param_OpkrAccelProfile = 0
self.param_OpkrAutoResume = 0
self.param_OpkrEnableLearner = 0
self.SC = None
self.traceCC = trace1.Loger("CarController")
self.res_cnt = 7
self.res_delay = 0
kyd = kyd_conf()
self.driver_steering_torque_above = float(
kyd.conf['driverSteeringTorqueAbove'])
self.params = Params()
self.mode_change_switch = int(
self.params.get('CruiseStatemodeSelInit'))
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.apply_steer_last = 0
self.car_fingerprint = CP.carFingerprint
self.packer = CANPacker(dbc_name)
self.steer_rate_limited = False
self.resume_cnt = 0
self.last_resume_frame = 0
self.last_lead_distance = 0
self.lanechange_manual_timer = 0
self.emergency_manual_timer = 0
self.driver_steering_torque_above_timer = 0
self.mode_change_timer = 0
self.steer_mode = ""
self.mdps_status = ""
self.lkas_switch = ""
self.lkas11_cnt = 0
self.nBlinker = 0
self.dRel = 0
self.yRel = 0
self.vRel = 0
self.timer1 = tm.CTime1000("time")
self.model_speed = 0
self.model_sum = 0
# hud
self.hud_timer_left = 0
self.hud_timer_right = 0
self.traceCC = trace1.Loger("CarController")
self.cruise_gap = 0
self.cruise_gap_prev = 0
self.cruise_gap_set_init = 0
self.cruise_gap_switch_timer = 0
self.params = Params()
self.mode_change_switch = int(
self.params.get('CruiseStatemodeSelInit'))
self.param_OpkrAccelProfile = int(self.params.get('OpkrAccelProfile'))
self.param_OpkrAutoResume = int(self.params.get('OpkrAutoResume'))
if self.param_OpkrAccelProfile == 1:
self.SC = SpdctrlSlow()
elif self.param_OpkrAccelProfile == 2:
self.SC = SpdctrlNormal()
else:
self.SC = SpdctrlFast()
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.apply_steer_last = 0
self.car_fingerprint = CP.carFingerprint
self.packer = CANPacker(dbc_name)
self.steer_rate_limited = False
self.last_resume_frame = 0
self.last_lead_distance = 0
self.lkas11_cnt = 0
self.timer1 = tm.CTime1000("time")
def __init__(self, CP=None):
self.long_control_state = 0 # initialized to off
self.seq_step_debug = ""
self.long_curv_timer = 0
self.path_x = np.arange(192)
self.traceSC = trace1.Loger("SPD_CTRL")
self.wheelbase = 2.8
self.steerRatio = 13.5 # 13.5
self.v_model = 0
self.a_model = 0
self.v_cruise = 0
self.a_cruise = 0
self.l_poly = []
self.r_poly = []
self.movAvg = moveavg1.MoveAvg()
self.Timer1 = tm.CTime1000("SPD")
self.time_no_lean = 0
self.wait_timer2 = 0
self.cruise_set_speed_kph = 0
self.curise_set_first = 0
self.curise_sw_check = 0
self.prev_clu_CruiseSwState = 0
self.prev_VSetDis = 0
self.sc_clu_speed = 0
self.btn_type = Buttons.NONE
self.active_time = 0
self.old_model_speed = 0
self.old_model_init = 0
self.curve_speed = 0
self.curvature_gain = 1
self.params = Params()
self.cruise_set_mode = int(
self.params.get("CruiseStatemodeSelInit", encoding='utf8'))
self.map_spd_limit_offset = int(
self.params.get("OpkrSpeedLimitOffset", encoding='utf8'))
self.map_spd_enable = False
self.map_spd_camera = 0
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.apply_steer_last = 0
self.car_fingerprint = CP.carFingerprint
self.packer = CANPacker(dbc_name)
self.steer_rate_limited = False
self.resume_cnt = 0
self.lkas11_cnt = 0
self.last_resume_frame = 0
self.last_lead_distance = 0
self.nBlinker = 0
self.lane_change_torque_lower = 0
self.steer_torque_over_timer = 0
self.steer_torque_ratio = 1
self.steer_torque_ratio_dir = 1
self.dRel = 0
self.yRel = 0
self.vRel = 0
self.timer1 = tm.CTime1000("time")
self.model_speed = 0
self.model_sum = 0
# hud
self.hud_timer_left = 0
self.hud_timer_right = 0
self.hud_sys_state = 0
self.command_cnt = 0
self.command_load = 0
self.params = Params()
# param
self.param_preOpkrAccelProfile = -1
self.param_OpkrAccelProfile = 0
self.param_OpkrAutoResume = 0
self.param_OpkrWhoisDriver = 0
self.SC = None
self.traceCC = trace1.Loger("CarController")
def __init__(self, CP=None):
self.long_control_state = 0 # initialized to off
self.seq_step_debug = ""
self.long_curv_timer = 0
self.path_x = np.arange(192)
self.traceSC = trace1.Loger("SPD_CTRL")
self.wheelbase = 2.845
self.steerRatio = 12.5 # 12.5
self.v_model = 0
self.a_model = 0
self.v_cruise = 0
self.a_cruise = 0
self.l_poly = []
self.r_poly = []
self.movAvg = moveavg1.MoveAvg()
self.Timer1 = tm.CTime1000("SPD")
self.time_no_lean = 0
self.wait_timer2 = 0
self.cruise_set_speed_kph = 0
self.curise_set_first = 0
self.curise_sw_check = 0
self.prev_clu_CruiseSwState = 0
self.prev_VSetDis = 0
self.sc_clu_speed = 0
self.btn_type = Buttons.NONE
self.active_time = 0
self.params = Params()
self.param_OpkrAccelProfile = int(self.params.get('OpkrAccelProfile'))
self.cruise_set_mode = int(self.params.get('CruiseStatemodeSelInit'))
def __init__(self, dbc_name, CP, VM):
self.packer = CANPacker(dbc_name)
self.apply_steer_last = 0
self.car_fingerprint = CP.carFingerprint
self.steer_rate_limited = False
self.accel_steady = 0
self.lkas11_cnt = 0
self.scc12_cnt = 0
self.resume_cnt = 0
self.last_lead_distance = 0
self.resume_wait_timer = 0
self.last_resume_frame = 0
self.lanechange_manual_timer = 0
self.emergency_manual_timer = 0
self.driver_steering_torque_above = False
self.driver_steering_torque_above_timer = 100
self.mode_change_timer = 0
self.need_brake = False
self.need_brake_timer = 0
self.params = Params()
self.mode_change_switch = int(self.params.get("CruiseStatemodeSelInit", encoding='utf8'))
self.opkr_variablecruise = self.params.get("OpkrVariableCruise", encoding='utf8') == "1"
self.opkr_autoresume = self.params.get("OpkrAutoResume", encoding='utf8') == "1"
self.opkr_turnsteeringdisable = self.params.get("OpkrTurnSteeringDisable", encoding='utf8') == "1"
self.opkr_maxanglelimit = float(int(self.params.get("OpkrMaxAngleLimit", encoding='utf8')))
self.steer_mode = ""
self.mdps_status = ""
self.lkas_switch = ""
self.leadcar_status = ""
self.timer1 = tm.CTime1000("time")
if self.params.get("OpkrVariableCruiseProfile", encoding='utf8') == "0":
self.SC = Spdctrl()
elif self.params.get("OpkrVariableCruiseProfile", encoding='utf8') == "1":
self.SC = SpdctrlRelaxed()
else:
self.SC = Spdctrl()
self.model_speed = 0
self.curve_speed = 0
self.dRel = 0
self.yRel = 0
self.vRel = 0
self.dRel2 = 0
self.yRel2 = 0
self.vRel2 = 0
self.lead2_status = False
self.cut_in_detection = 0
self.target_map_speed_camera = 0
self.v_set_dis_prev = 180
self.cruise_gap = 0.0
self.cruise_gap_prev = 0
self.cruise_gap_set_init = 0
self.cruise_gap_switch_timer = 0
self.standstill_fault_reduce_timer = 0
self.cruise_gap_prev2 = 0
self.cruise_gap_switch_timer2 = 0
self.cruise_gap_switch_timer3 = 0
self.standstill_status = 0
self.standstill_status_timer = 0
self.res_switch_timer = 0
self.lkas_button_on = True
self.longcontrol = CP.openpilotLongitudinalControl
self.scc_live = not CP.radarOffCan
self.accActive = False
self.safety_camera_timer = 0
self.model_speed_range = [30, 90, 255]
self.steerMax_range = [CarControllerParams.STEER_MAX, int(self.params.get("SteerMaxBaseAdj", encoding='utf8')), int(self.params.get("SteerMaxBaseAdj", encoding='utf8'))]
self.steerDeltaUp_range = [CarControllerParams.STEER_DELTA_UP, int(self.params.get("SteerDeltaUpBaseAdj", encoding='utf8')), int(self.params.get("SteerDeltaUpBaseAdj", encoding='utf8'))]
self.steerDeltaDown_range = [CarControllerParams.STEER_DELTA_DOWN, int(self.params.get("SteerDeltaDownBaseAdj", encoding='utf8')), int(self.params.get("SteerDeltaDownBaseAdj", encoding='utf8'))]
#self.model_speed_range = [0, 30, 255]
#self.steerMax_range = [int(self.params.get('SteerMaxBaseAdj')), int(self.params.get('SteerMaxBaseAdj')), CarControllerParams.STEER_MAX]
#self.steerDeltaUp_range = [int(self.params.get('SteerDeltaUpAdj')), int(self.params.get('SteerDeltaUpAdj')), 5]
#self.steerDeltaDown_range = [int(self.params.get('SteerDeltaDownAdj')), int(self.params.get('SteerDeltaDownAdj')), 10]
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'))
self.variable_steer_max = self.params.get('OpkrVariableSteerMax', encoding='utf8') == "1"
self.variable_steer_delta = self.params.get('OpkrVariableSteerDelta', encoding='utf8') == "1"
if CP.lateralTuning.which() == 'pid':
self.str_log2 = 'T={:0.2f}/{:0.3f}/{:0.2f}/{:0.5f}'.format(CP.lateralTuning.pid.kpV[1], CP.lateralTuning.pid.kiV[1], CP.lateralTuning.pid.kdV[0], CP.lateralTuning.pid.kf)
elif CP.lateralTuning.which() == 'indi':
self.str_log2 = 'T={:03.1f}/{:03.1f}/{:03.1f}/{:03.1f}'.format(CP.lateralTuning.indi.innerLoopGainV[1], CP.lateralTuning.indi.outerLoopGainV[1], CP.lateralTuning.indi.timeConstantV[1], CP.lateralTuning.indi.actuatorEffectivenessV[1])
elif CP.lateralTuning.which() == 'lqr':
self.str_log2 = 'T={:04.0f}/{:05.3f}/{:06.4f}'.format(CP.lateralTuning.lqr.scale, CP.lateralTuning.lqr.ki, CP.lateralTuning.lqr.dcGain)
if CP.spasEnabled:
self.en_cnt = 0
self.apply_steer_ang = 0.0
self.en_spas = 3
self.mdps11_stat_last = 0
self.spas_always = False
self.p = CarControllerParams
def __init__(self, txt_msg="defualt", time_val = 0. ):
self.name = txt_msg
self.nTime = time_val * 0.001
self.old_txt = ""
self.debug_Timer = 0
self.Timer1 = tm.CTime1000("LOG")
def __init__(self, dbc_name, CP, VM):
self.packer = CANPacker(dbc_name)
self.apply_steer_last = 0
self.car_fingerprint = CP.carFingerprint
self.steer_rate_limited = False
self.steer_wind_down = 0
self.accel_steady = 0
self.accel_lim_prev = 0.
self.accel_lim = 0.
self.lastresumeframe = 0
self.fca11supcnt = self.fca11inc = self.fca11alivecnt = self.fca11cnt13 = self.scc11cnt = self.scc12cnt = 0
self.counter_init = False
self.fca11maxcnt = 0xD
self.resume_cnt = 0
self.last_lead_distance = 0
self.resume_wait_timer = 0
self.last_resume_frame = 0
self.lanechange_manual_timer = 0
self.emergency_manual_timer = 0
self.driver_steering_torque_above = False
self.driver_steering_torque_above_timer = 100
self.mode_change_timer = 0
self.acc_standstill_timer = 0
self.acc_standstill = False
self.need_brake = False
self.need_brake_timer = 0
self.cancel_counter = 0
self.v_cruise_kph_auto_res = 0
self.params = Params()
self.mode_change_switch = int(
self.params.get("CruiseStatemodeSelInit", encoding="utf8"))
self.opkr_variablecruise = self.params.get_bool("OpkrVariableCruise")
self.opkr_autoresume = self.params.get_bool("OpkrAutoResume")
self.opkr_cruisegap_auto_adj = self.params.get_bool("CruiseGapAdjust")
self.opkr_cruise_auto_res = self.params.get_bool("CruiseAutoRes")
self.opkr_cruise_auto_res_option = int(
self.params.get("AutoResOption", encoding="utf8"))
self.opkr_turnsteeringdisable = self.params.get_bool(
"OpkrTurnSteeringDisable")
self.steer_wind_down_enabled = self.params.get_bool("SteerWindDown")
self.opkr_maxanglelimit = float(
int(self.params.get("OpkrMaxAngleLimit", encoding="utf8")))
self.mad_mode_enabled = self.params.get_bool("MadModeEnabled")
self.ldws_fix = self.params.get_bool("LdwsCarFix")
self.apks_enabled = self.params.get_bool("OpkrApksEnable")
self.steer_mode = ""
self.mdps_status = ""
self.lkas_switch = ""
self.leadcar_status = ""
self.longcontrol = CP.openpilotLongitudinalControl
#self.scc_live is true because CP.radarOffCan is False
self.scc_live = not CP.radarOffCan
self.timer1 = tm.CTime1000("time")
if int(self.params.get("OpkrVariableCruiseProfile",
encoding="utf8")) == 0 and not self.longcontrol:
self.SC = Spdctrl()
elif int(self.params.get(
"OpkrVariableCruiseProfile",
encoding="utf8")) == 1 and not self.longcontrol:
self.SC = SpdctrlRelaxed()
elif self.longcontrol:
self.SC = SpdctrlLong()
self.model_speed = 0
self.curve_speed = 0
self.dRel = 0
self.yRel = 0
self.vRel = 0
self.dRel2 = 0
self.yRel2 = 0
self.vRel2 = 0
self.lead2_status = False
self.cut_in_detection = 0
self.cruise_gap = 0.0
self.cruise_gap_prev = 0
self.cruise_gap_set_init = 0
self.cruise_gap_switch_timer = 0
self.standstill_fault_reduce_timer = 0
self.cruise_gap_prev2 = 0
self.cruise_gap_switch_timer2 = 0
self.cruise_gap_switch_timer3 = 0
self.standstill_status = 0
self.standstill_status_timer = 0
self.res_switch_timer = 0
self.auto_res_timer = 0
self.res_speed = 0
self.res_speed_timer = 0
self.steerMax_base = int(
self.params.get("SteerMaxBaseAdj", encoding="utf8"))
self.steerDeltaUp_base = int(
self.params.get("SteerDeltaUpBaseAdj", encoding="utf8"))
self.steerDeltaDown_base = int(
self.params.get("SteerDeltaDownBaseAdj", encoding="utf8"))
self.model_speed_range = [30, 100, 255]
self.steerMax_range = [
CarControllerParams.STEER_MAX, self.steerMax_base,
self.steerMax_base
]
self.steerDeltaUp_range = [
CarControllerParams.STEER_DELTA_UP, self.steerDeltaUp_base,
self.steerDeltaUp_base
]
self.steerDeltaDown_range = [
CarControllerParams.STEER_DELTA_DOWN, self.steerDeltaDown_base,
self.steerDeltaDown_base
]
self.steerMax = 0
self.steerDeltaUp = 0
self.steerDeltaDown = 0
self.variable_steer_max = self.params.get_bool("OpkrVariableSteerMax")
self.variable_steer_delta = self.params.get_bool(
"OpkrVariableSteerDelta")
if CP.lateralTuning.which() == 'pid':
self.str_log2 = 'T={:0.2f}/{:0.3f}/{:0.2f}/{:0.5f}'.format(
CP.lateralTuning.pid.kpV[1], CP.lateralTuning.pid.kiV[1],
CP.lateralTuning.pid.kdV[0], CP.lateralTuning.pid.kf)
elif CP.lateralTuning.which() == 'indi':
self.str_log2 = 'T={:03.1f}/{:03.1f}/{:03.1f}/{:03.1f}'.format(CP.lateralTuning.indi.innerLoopGainV[0], CP.lateralTuning.indi.outerLoopGainV[0], \
CP.lateralTuning.indi.timeConstantV[0], CP.lateralTuning.indi.actuatorEffectivenessV[0])
elif CP.lateralTuning.which() == 'lqr':
self.str_log2 = 'T={:04.0f}/{:05.3f}/{:06.4f}'.format(
CP.lateralTuning.lqr.scale, CP.lateralTuning.lqr.ki,
CP.lateralTuning.lqr.dcGain)
self.p = CarControllerParams
self.sm = messaging.SubMaster(['controlsState'])
def __init__(self, dbc_name, CP, VM):
self.car_fingerprint = CP.carFingerprint
self.packer = CANPacker(dbc_name)
self.accel_steady = 0
self.apply_steer_last = 0
self.steer_rate_limited = False
self.lkas11_cnt = 0
self.scc12_cnt = 0
self.resume_cnt = 0
self.last_lead_distance = 0
self.resume_wait_timer = 0
self.last_resume_frame = 0
self.lanechange_manual_timer = 0
self.emergency_manual_timer = 0
self.driver_steering_torque_above = False
self.driver_steering_torque_above_timer = 100
self.mode_change_timer = 0
self.need_brake = False
self.need_brake_timer = 0
self.params = Params()
self.mode_change_switch = int(
self.params.get('CruiseStatemodeSelInit'))
self.opkr_variablecruise = int(self.params.get('OpkrVariableCruise'))
self.opkr_autoresume = int(self.params.get('OpkrAutoResume'))
self.opkr_autoresumeoption = int(
self.params.get('OpkrAutoResumeOption'))
self.opkr_maxanglelimit = int(self.params.get('OpkrMaxAngleLimit'))
self.steer_mode = ""
self.mdps_status = ""
self.lkas_switch = ""
self.leadcar_status = ""
self.timer1 = tm.CTime1000("time")
self.SC = Spdctrl()
self.model_speed = 0
self.model_sum = 0
self.dRel = 0
self.yRel = 0
self.vRel = 0
self.dRel2 = 0
self.yRel2 = 0
self.vRel2 = 0
self.lead2_status = False
self.cut_in_detection = 0
self.target_map_speed = 0
self.target_map_speed_camera = 0
self.v_set_dis_prev = 180
self.cruise_gap = 0.0
self.cruise_gap_prev = 0
self.cruise_gap_set_init = 0
self.cruise_gap_switch_timer = 0
self.standstill_fault_reduce_timer = 0
self.cruise_gap_prev2 = 0
self.cruise_gap_switch_timer2 = 0
self.cruise_gap_switch_timer3 = 0
self.standstill_status = 0
self.standstill_status_timer = 0
self.res_switch_timer = 0
self.lkas_button_on = True
self.longcontrol = CP.openpilotLongitudinalControl
self.scc_live = not CP.radarOffCan
self.accActive = False
self.angle_differ_range = [0, 45]
self.steerMax_range = [
int(self.params.get('SteerMaxBaseAdj')), SteerLimitParams.STEER_MAX
]
self.steerDeltaUp_range = [int(self.params.get('SteerDeltaUpAdj')), 5]
self.steerDeltaDown_range = [
int(self.params.get('SteerDeltaDownAdj')), 10
]
self.steerMax = int(self.params.get('SteerMaxBaseAdj'))
self.steerDeltaUp = int(self.params.get('SteerDeltaUpAdj'))
self.steerDeltaDown = int(self.params.get('SteerDeltaDownAdj'))
self.steerMax_prev = int(self.params.get('SteerMaxBaseAdj'))
self.steerDeltaUp_prev = int(self.params.get('SteerDeltaUpAdj'))
self.steerDeltaDown_prev = int(self.params.get('SteerDeltaDownAdj'))
self.steerMax_timer = 0
self.steerDeltaUp_timer = 0
self.steerDeltaDown_timer = 0
if CP.lateralTuning.which() == 'pid':
self.str_log2 = 'T={:0.2f}/{:0.3f}/{:0.5f}'.format(
CP.lateralTuning.pid.kpV[1], CP.lateralTuning.pid.kiV[1],
CP.lateralTuning.pid.kf)
elif CP.lateralTuning.which() == 'indi':
self.str_log2 = 'T={:03.1f}/{:03.1f}/{:03.1f}/{:03.1f}'.format(
CP.lateralTuning.indi.innerLoopGain,
CP.lateralTuning.indi.outerLoopGain,
CP.lateralTuning.indi.timeConstant,
CP.lateralTuning.indi.actuatorEffectiveness)
elif CP.lateralTuning.which() == 'lqr':
self.str_log2 = 'T={:04.0f}/{:05.3f}/{:06.4f}'.format(
CP.lateralTuning.lqr.scale, CP.lateralTuning.lqr.ki,
CP.lateralTuning.lqr.dcGain)
if CP.spasEnabled:
self.en_cnt = 0
self.apply_steer_ang = 0.0
self.en_spas = 3
self.mdps11_stat_last = 0
self.spas_always = False
self.p = SteerLimitParams
def __init__(self, dbc_name, CP, VM):
self.apply_steer_last = 0
self.car_fingerprint = CP.carFingerprint
self.cp_oplongcontrol = CP.openpilotLongitudinalControl
self.packer = CANPacker(dbc_name)
self.accel_steady = 0
self.accel_lim_prev = 0.
self.accel_lim = 0.
self.steer_rate_limited = False
self.apply_accel = 0
self.usestockscc = True
self.lead_visible = False
self.lead_debounce = 0
self.prev_gapButton = 0
self.current_veh_speed = 0
self.lfainFingerprint = CP.lfaAvailable
self.vdiff = 0
self.resumebuttoncnt = 0
self.lastresumeframe = 0
self.fca11supcnt = self.fca11inc = self.fca11alivecnt = self.fca11cnt13 = self.scc11cnt = self.scc12cnt = 0
self.counter_init = False
self.fca11maxcnt = 0xD
self.radarDisableActivated = False
self.radarDisableResetTimer = 0
self.radarDisableOverlapTimer = 0
self.sendaccmode = not CP.radarDisablePossible
self.enabled = False
self.sm = messaging.SubMaster(['controlsState'])
self.lanechange_manual_timer = 0
self.emergency_manual_timer = 0
self.driver_steering_torque_above = False
self.driver_steering_torque_above_timer = 100
self.acc_standstill_timer = 0
self.acc_standstill = False
self.params = Params()
self.gapsettingdance = int(self.params.get('OpkrCruiseGapSet'))
self.opkr_variablecruise = int(self.params.get('OpkrVariableCruise'))
self.opkr_autoresume = int(self.params.get('OpkrAutoResume'))
self.opkr_maxanglelimit = int(self.params.get('OpkrMaxAngleLimit'))
self.timer1 = tm.CTime1000("time")
if int(self.params.get('OpkrVariableCruiseProfile')) == 0:
self.SC = Spdctrl()
elif int(self.params.get('OpkrVariableCruiseProfile')) == 1:
self.SC = SpdctrlRelaxed()
else:
self.SC = Spdctrl()
self.model_speed = 0
self.model_sum = 0
self.dRel = 0
self.yRel = 0
self.vRel = 0
self.dRel2 = 0
self.yRel2 = 0
self.vRel2 = 0
self.lead2_status = False
self.cut_in_detection = 0
self.target_map_speed = 0
self.target_map_speed_camera = 0
self.v_set_dis_prev = 180
#self.model_speed_range = [30, 90, 255, 300]
#self.steerMax_range = [SteerLimitParams.STEER_MAX, int(self.params.get('SteerMaxBaseAdj')), int(self.params.get('SteerMaxBaseAdj')), 0]
#self.steerDeltaUp_range = [5, int(self.params.get('SteerDeltaUpAdj')), int(self.params.get('SteerDeltaUpAdj')), 0]
#self.steerDeltaDown_range = [10, int(self.params.get('SteerDeltaDownAdj')), int(self.params.get('SteerDeltaDownAdj')), 0]
self.angle_range = [0, 10, 15, 20, 30, 40, 60] # 호야님 버전 가변 적용
self.SMAX = SteerLimitParams.STEER_MAX # 약 510 이상(SteerMaxBaseAdj의 2배)으로 설정해야 아래 보간 로직이 맞음
self.steerMax_range = [
int(self.params.get('SteerMaxBaseAdj')), self.SMAX * 0.57,
self.SMAX * 0.66, self.SMAX * 0.75, self.SMAX * 0.85,
self.SMAX * 0.93, self.SMAX
]
self.steerDeltaUp_range = [
int(self.params.get('SteerDeltaUpAdj')), 3, 4, 4, 4, 5, 5
]
self.steerDeltaDown_range = [
int(self.params.get('SteerDeltaDownAdj')), 5, 6, 7, 8, 9, 10
]
self.steerMax = int(self.params.get('SteerMaxBaseAdj'))
self.steerDeltaUp = int(self.params.get('SteerDeltaUpAdj'))
self.steerDeltaDown = int(self.params.get('SteerDeltaDownAdj'))
self.variable_steer_max = int(
self.params.get('OpkrVariableSteerMax')) == 1
self.variable_steer_delta = int(
self.params.get('OpkrVariableSteerDelta')) == 1
if CP.lateralTuning.which() == 'pid':
self.str_log2 = 'T={:0.2f}/{:0.3f}/{:0.2f}/{:0.5f}'.format(
CP.lateralTuning.pid.kpV[1], CP.lateralTuning.pid.kiV[1],
CP.lateralTuning.pid.kdV[0], CP.lateralTuning.pid.kf)
elif CP.lateralTuning.which() == 'indi':
self.str_log2 = 'T={:03.1f}/{:03.1f}/{:03.1f}/{:03.1f}'.format(
CP.lateralTuning.indi.innerLoopGain,
CP.lateralTuning.indi.outerLoopGain,
CP.lateralTuning.indi.timeConstant,
CP.lateralTuning.indi.actuatorEffectiveness)
elif CP.lateralTuning.which() == 'lqr':
self.str_log2 = 'T={:04.0f}/{:05.3f}/{:06.4f}'.format(
CP.lateralTuning.lqr.scale, CP.lateralTuning.lqr.ki,
CP.lateralTuning.lqr.dcGain)
self.p = SteerLimitParams
