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