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, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, sm, LaC): path_plan = sm['pathPlan'] # *** compute control surfaces *** v_ego_kph = CS.v_ego * CV.MS_TO_KPH # gas and brake apply_accel = actuators.gas - actuators.brake apply_accel, self.accel_steady = self.accel_hysteresis( apply_accel, self.accel_steady) apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) abs_angle_steers = abs( actuators.steerAngle) # abs(CS.angle_steers) # param = SteerLimitParams() if path_plan.laneChangeState != LaneChangeState.off: #param.STEER_MAX *= 0.99 param.STEER_DELTA_UP = 1 param.STEER_DELTA_DOWN = 5 #v_curvature elif LaC.v_curvature < 200: self.timer_curvature = 300 elif abs_angle_steers < 1 and self.timer_curvature <= 0: xp = [0.5, 1] fp = [240, 255] param.STEER_MAX = interp(abs_angle_steers, xp, fp) if abs_angle_steers < 1 or v_ego_kph < 5: param.STEER_DELTA_UP = 2 param.STEER_DELTA_DOWN = 4 if self.timer_curvature: self.timer_curvature -= 1 ### Steering Torque new_steer = actuators.steer * param.STEER_MAX apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steer_torque_driver, param) self.steer_rate_limited = new_steer != apply_steer if abs(CS.steer_torque_driver) > self.steer_torque_over_max: #200: self.steer_torque_over_timer += 1 if self.steer_torque_over_timer > 5: self.steer_torque_over = True self.steer_torque_over_timer = 100 elif self.steer_torque_over_timer: self.steer_torque_over_timer -= 1 else: self.steer_torque_over = False ### LKAS button to temporarily disable steering #if not CS.lkas_error: # if self.lkas_button != CS.lkas_button_on: # self.lkas_button = CS.lkas_button_on # disable if steer angle reach 90 deg, otherwise mdps fault in some models if self.car_fingerprint == CAR.GENESIS: lkas_active = enabled and abs(CS.angle_steers) < 90. else: lkas_active = enabled and abs(CS.angle_steers) < 100. # fix for Genesis hard fault at low speed if v_ego_kph < 60 and self.car_fingerprint == CAR.GENESIS and not CS.mdps_bus: lkas_active = 0 low_speed = self.low_speed_car #if not self.lkas_button: # low_speed = False #elif not CS.cruiseState.enabled: # low_speed = False if CS.stopped: low_speed = False elif CS.v_ego > (CS.CP.minSteerSpeed + 0.7): low_speed = False elif CS.v_ego < (CS.CP.minSteerSpeed + 0.2): low_speed = True if self.low_speed_car != low_speed: self.low_speed_car = low_speed # streer over check if enabled and abs(CS.angle_steers) > 100. or CS.steer_error: self.streer_angle_over = True self.steer_timer = 250 elif abs(CS.angle_steers) < 7.5 or not self.steer_timer: self.streer_angle_over = False elif self.steer_timer: self.steer_timer -= 1 # Disable steering while turning blinker on and speed below 60 kph if CS.left_blinker_on or CS.right_blinker_on: self.steer_torque_over = False self.turning_signal_timer = 100 # Disable for 1.0 Seconds after blinker turned off elif CS.left_blinker_flash or CS.right_blinker_flash: self.steer_torque_over = False self.turning_signal_timer = 100 # turning indicator alert logic if self.lane_change_enabled: self.turning_indicator = self.turning_signal_timer and CS.v_ego < LaneChangeParms.LANE_CHANGE_SPEED_MIN else: self.turning_indicator = self.turning_signal_timer if self.turning_signal_timer: self.turning_signal_timer -= 1 if left_line: self.hud_timer_left = 100 elif self.hud_timer_left: self.hud_timer_left -= 1 if right_line: self.hud_timer_right = 100 elif self.hud_timer_right: self.hud_timer_right -= 1 if path_plan.laneChangeState != LaneChangeState.off: if LaC.v_curvature > 200: self.lkas_active_timer1 = 300 apply_steer = self.limit_ctrl(apply_steer, 100, 0) elif not self.hud_timer_left and not self.hud_timer_right: self.lkas_active_timer1 = 200 # apply_steer = 70 elif path_plan.laneChangeState != LaneChangeState.off: self.steer_torque_over = False # disable lkas if not CS.main_on: lkas_active = 0 self.turning_signal_timer = 0 self.turning_indicator = False self.steer_torque_over = False elif CS.stopped: lkas_active = 0 elif self.steer_torque_over: lkas_active = 0 if self.streer_angle_over: lkas_active = 0 elif self.turning_indicator: lkas_active = 0 if not lkas_active: apply_steer = 0 steer_req = 1 if apply_steer else 0 steer_limit = param.STEER_MAX #param.STEER_MAX if not lkas_active: self.lkas_active_timer1 = 200 elif self.lkas_active_timer1 < 400: self.lkas_active_timer1 += 1 ratio_steer = self.lkas_active_timer1 / 400 if ratio_steer < 1: steer_limit = ratio_steer * param.STEER_MAX apply_steer = self.limit_ctrl(apply_steer, steer_limit, 0) dRel, yRel, vRel = self.SC.get_lead(sm, CS) vRel = int(vRel * 3.6 + 0.5) lead_objspd = CS.lead_objspd str_log1 = 'CV={:03.0f}/{:06.3f} TQ=V:{:04.0f}/S:{:04.0f}'.format( LaC.v_curvature, LaC.model_sum, apply_steer, CS.steer_torque_driver) str_log2 = 'D={:05.1f} V={:03.0f} S_LIM={:03.0f} S_MAX={:03.0f}'.format( dRel, vRel, steer_limit, param.STEER_MAX) trace1.printf('{} {}'.format(str_log1, str_log2)) self.apply_accel_last = apply_accel self.apply_steer_last = apply_steer hud_alert, lane_visible = self.process_hud_alert( lkas_active, self.lkas_button, visual_alert, self.hud_timer_left, self.hud_timer_right, CS) 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 self.clu11_cnt = frame % 0x10 self.mdps12_cnt = frame % 0x100 can_sends.append( create_lkas11(self.packer, self.car_fingerprint, 0, apply_steer, steer_req, self.lkas11_cnt, lkas_active, CS.lkas11, hud_alert, lane_visible, keep_stock=True)) if CS.mdps_bus or CS.scc_bus == 1: # send lkas12 bus 1 if mdps or scc is on bus 1 can_sends.append( create_lkas11(self.packer, self.car_fingerprint, 1, apply_steer, steer_req, self.lkas11_cnt, lkas_active, CS.lkas11, hud_alert, lane_visible, keep_stock=True)) if CS.mdps_bus: # send clu11 to mdps if it is not on bus 0 can_sends.append( create_clu11(self.packer, CS.mdps_bus, CS.clu11, Buttons.NONE, enabled_speed, self.clu11_cnt)) if pcm_cancel_cmd and self.longcontrol: can_sends.append( create_clu11(self.packer, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed, self.clu11_cnt)) else: # send mdps12 to LKAS to prevent LKAS error if no cancel cmd can_sends.append( create_mdps12(self.packer, self.car_fingerprint, self.mdps12_cnt, CS.mdps12)) # AVM #if CS.mdps_bus: #if not CS.cp_AVM.can_valid: #can_sends.append(create_AVM(self.packer, self.car_fingerprint, CS.avm, CS )) if CS.stopped: #self.model_speed = 300 # 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, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed, self.resume_cnt)) 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 CS.driverOverride == 2 or not CS.pcm_acc_status or CS.clu_CruiseSwState == 1 or CS.clu_CruiseSwState == 2: #self.model_speed = 300 self.resume_cnt = 0 self.sc_btn_type = Buttons.NONE self.sc_wait_timer2 = 10 self.sc_active_timer2 = 0 elif self.sc_wait_timer2: self.sc_wait_timer2 -= 1 elif self.speed_control_enabled: #acc_mode, clu_speed = self.long_speed_cntrl( v_ego_kph, CS, actuators ) btn_type, clu_speed = self.SC.update( v_ego_kph, CS, sm, actuators, dRel, yRel, vRel, LaC.v_curvature) # speed controller spdcontroller.py if CS.clu_Vanz < 5: self.sc_btn_type = Buttons.NONE elif self.sc_btn_type != Buttons.NONE: pass elif btn_type != Buttons.NONE: self.resume_cnt = 0 self.sc_active_timer2 = 0 self.sc_btn_type = btn_type self.sc_clu_speed = clu_speed if self.sc_btn_type != Buttons.NONE: self.sc_active_timer2 += 1 if self.sc_active_timer2 > 10: self.sc_wait_timer2 = 5 self.resume_cnt = 0 self.sc_active_timer2 = 0 self.sc_btn_type = Buttons.NONE else: #self.traceCC.add( 'sc_btn_type={} clu_speed={} set={:.0f} vanz={:.0f}'.format( self.sc_btn_type, self.sc_clu_speed, CS.VSetDis, clu11_speed ) ) can_sends.append( create_clu11(self.packer, CS.scc_bus, CS.clu11, self.sc_btn_type, self.sc_clu_speed, self.resume_cnt)) self.resume_cnt += 1 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 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 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): # 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. # 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, left_lane_warning, right_lane_warning =\ process_hud_alert(enabled, self.car_fingerprint, visual_alert, left_lane, right_lane, left_lane_depart, right_lane_depart) clu11_speed = CS.clu11["CF_Clu_Vanz"] enabled_speed = 38 if CS.is_set_speed_in_mph else 60 if clu11_speed > enabled_speed: enabled_speed = clu11_speed 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, 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, self.lkas11_cnt, self.car_fingerprint, apply_steer, steer_req, 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 == 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 = 'torg:{:3.0f}'.format(apply_steer) str_log2 = 'new_steer={:.0f} tm={:.1f} '.format( new_steer, self.timer1.sampleTime()) trace1.printf('{} {}'.format(str_log1, 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.clu11, Buttons.CANCEL)) elif CS.out.cruiseState.standstill: if self.last_lead_distance == 0: # get the lead distance from the Radar self.last_lead_distance = CS.lead_distance # SCC won't resume anyway when the lead distace is less than 3.7m # send resume at a max freq of 5Hz #if CS.lead_distance > 3.7 and (frame - self.last_resume_frame)*DT_CTRL > 0.2: if CS.lead_distance != self.last_lead_distance and ( frame - self.last_resume_frame) * DT_CTRL > 0.2: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed)) self.last_resume_frame = frame # 20 Hz LFA MFA message if frame % 5 == 0 and self.car_fingerprint in [ CAR.SONATA, CAR.PALISADE, CAR.IONIQ ]: can_sends.append(create_lfa_mfa(self.packer, frame, enabled)) 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
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): 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