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