def update_log(self, CS, set_speed, target_set_speed, long_wait_cmd): if CS.out.cruiseState.modeSel == 0: self.steer_mode = "오파모드" elif CS.out.cruiseState.modeSel == 1: self.steer_mode = "차간+커브" elif CS.out.cruiseState.modeSel == 2: self.steer_mode = "차간ONLY" elif CS.out.cruiseState.modeSel == 3: self.steer_mode = "편도1차선" elif CS.out.cruiseState.modeSel == 4: self.steer_mode = "맵감속ONLY" if self.cruise_gap != CS.cruiseGapSet: self.cruise_gap = CS.cruiseGapSet str3 = 'MODE={:s} BS={:1.0f}/{:1.0f} VL={:03.0f}/{:03.0f} TM={:03.0f}/{:03.0f} TS={:03.0f}'.format( self.steer_mode, CS.CP.mdpsBus, CS.CP.sccBus, set_speed, CS.VSetDis, long_wait_cmd, self.long_curv_timer, int(round(self.target_speed))) str4 = ' RD=D:{:03.0f}/V:{:03.0f} CG={:1.0f} DG={:s}'.format( CS.lead_distance, CS.lead_objspd, self.cruise_gap, self.seq_step_debug) str5 = str3 + str4 trace1.printf2(str5)
def update_log(self, CS, set_speed, target_set_speed, long_wait_cmd ): str3 = 'SS={:03.0f}/{:03.0f} DAt={:03.0f}/{:03.0f}/{:03.0f} DG={:02.0f}'.format( set_speed, CS.VSetDis, CS.driverAcc_time, long_wait_cmd, self.long_curv_timer, self.seq_step_debug ) str4 = ' LD/LS={:03.0f}/{:03.0f} '.format( CS.lead_distance, CS.lead_objspd ) str5 = str3 + str4 trace1.printf2( str5 )
def update_log(self, CS, set_speed, target_set_speed, long_wait_cmd): str3 = 'M={:3.0f} DST={:3.0f} VSD={:.0f} DA={:.0f}/{:.0f}/{:.0f} DG={:s} DO={:.0f}'.format( CS.out.cruiseState.modeSel, target_set_speed, CS.VSetDis, CS.driverAcc_time, long_wait_cmd, self.long_curv_timer, self.seq_step_debug, CS.driverOverride) str4 = ' CS={:.1f}/{:.1f} '.format(CS.lead_distance, CS.lead_objspd) str5 = str3 + str4 trace1.printf2(str5)
def update_log(self, CS, set_speed, target_set_speed, long_wait_cmd ): if CS.out.cruiseState.modeSel == 0: self.steer_mode = "오파모드" elif CS.out.cruiseState.modeSel == 1: self.steer_mode = "차간+커브" elif CS.out.cruiseState.modeSel == 2: self.steer_mode = "차간ONLY" elif CS.out.cruiseState.modeSel == 3: self.steer_mode = "자동RES" elif CS.out.cruiseState.modeSel == 4: self.steer_mode = "순정모드" str3 = '주행모드={:s} 설정속도={:03.0f}/{:03.0f} 타이머={:03.0f}/{:03.0f}/{:03.0f}'.format( self.steer_mode, set_speed, CS.VSetDis, CS.driverAcc_time, long_wait_cmd, self.long_curv_timer ) str4 = ' 거리차/속도차={:03.0f}/{:03.0f} 구분={:s}'.format( CS.lead_distance, CS.lead_objspd, self.seq_step_debug ) str5 = str3 + str4 trace1.printf2( str5 )
def update_log(self, CS, set_speed, target_set_speed, long_wait_cmd ): if CS.out.cruiseState.modeSel == 0: self.steer_mode = "오파모드" elif CS.out.cruiseState.modeSel == 1: self.steer_mode = "차간+커브" elif CS.out.cruiseState.modeSel == 2: self.steer_mode = "차간ONLY" elif CS.out.cruiseState.modeSel == 3: self.steer_mode = "편도1차선" if self.cruise_gap != CS.cruiseGapSet: self.cruise_gap = CS.cruiseGapSet str3 = '주행모드={:s} 설정속도={:03.0f}/{:03.0f} 타이머={:03.0f}/{:03.0f}'.format( self.steer_mode, set_speed, CS.VSetDis, long_wait_cmd, self.long_curv_timer ) str4 = ' 레이더=D:{:04.1f}/V:{:05.1f} CG={:1.0f} 구분={:s}'.format( CS.lead_distance, CS.lead_objspd, self.cruise_gap, self.seq_step_debug ) str5 = str3 + str4 trace1.printf2( str5 )
def update_log(self, CS, set_speed, target_set_speed, long_wait_cmd): if CS.out.cruiseState.modeSel == 0: self.steer_mode = "OpenPilot" elif CS.out.cruiseState.modeSel == 1: self.steer_mode = "Dist+Curv" elif CS.out.cruiseState.modeSel == 2: self.steer_mode = "DistOnly" elif CS.out.cruiseState.modeSel == 3: self.steer_mode = "OneWay" if self.cruise_gap != CS.cruiseGapSet: self.cruise_gap = CS.cruiseGapSet str3 = 'MODE={:s} VL={:03.0f}/{:03.0f} TM={:03.0f}/{:03.0f} TS={:03.0f}'.format( self.steer_mode, set_speed, CS.VSetDis, long_wait_cmd, self.long_curv_timer, int(round(self.target_speed))) str4 = ' RD=D:{:03.0f}/V:{:03.0f} CG={:1.0f} DG={:s}'.format( CS.lead_distance, CS.lead_objspd, self.cruise_gap, self.seq_step_debug) str5 = str3 + str4 trace1.printf2(str5)
def update(self, enabled, CS, frame, CC, actuators, pcm_cancel_cmd, visual_alert, left_lane, right_lane, left_lane_depart, right_lane_depart, set_speed, lead_visible, sm): # *** compute control surfaces *** # gas and brake apply_accel = actuators.gas - actuators.brake apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady) apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) self.dRel, self.yRel, self.vRel = SpdController.get_lead(sm) self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo) # Steering Torque steerAngleAbs = abs(actuators.steerAngle) limitParams = copy.copy(SteerLimitParams) limitParams.STEER_MAX = int(interp(steerAngleAbs, [5., 10., 20., 30., 70.], [255, SteerLimitParams.STEER_MAX, 500, 600, 800])) limitParams.STEER_DELTA_UP = int(interp(steerAngleAbs, [5., 20., 50.], [2, 3, 3])) limitParams.STEER_DELTA_DOWN = int(interp(steerAngleAbs, [5., 20., 50.], [4, 5, 5])) if self.driver_steering_torque_above_timer: new_steer = actuators.steer * limitParams.STEER_MAX * (self.driver_steering_torque_above_timer / 100) else: new_steer = actuators.steer * limitParams.STEER_MAX apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, limitParams) self.steer_rate_limited = new_steer != apply_steer CC.applyAccel = apply_accel CC.applySteer = apply_steer # SPAS limit angle extremes for safety if CS.spas_enabled: apply_steer_ang_req = clip(actuators.steerAngle, -1*(STEER_ANG_MAX), STEER_ANG_MAX) # SPAS limit angle rate for safety if abs(self.apply_steer_ang - apply_steer_ang_req) > STEER_ANG_MAX_RATE: if apply_steer_ang_req > self.apply_steer_ang: self.apply_steer_ang += STEER_ANG_MAX_RATE else: self.apply_steer_ang -= STEER_ANG_MAX_RATE else: self.apply_steer_ang = apply_steer_ang_req spas_active = CS.spas_enabled and enabled and (self.spas_always or CS.out.vEgo < 7.0) # 25km/h # disable if steer angle reach 90 deg, otherwise mdps fault in some models # temporarily disable steering when LKAS button off #lkas_active = enabled and abs(CS.out.steeringAngle) < 90. and not spas_active lkas_active = enabled and not spas_active #and abs(CS.out.steeringAngle) < 90. if (( CS.out.leftBlinker and not CS.out.rightBlinker) or ( CS.out.rightBlinker and not CS.out.leftBlinker)) and CS.out.vEgo < LANE_CHANGE_SPEED_MIN: self.lanechange_manual_timer = 10 if CS.out.leftBlinker and CS.out.rightBlinker: self.emergency_manual_timer = 10 if abs(CS.out.steeringTorque) > 200: self.driver_steering_torque_above_timer = 100 if self.lanechange_manual_timer: lkas_active = 0 if self.lanechange_manual_timer > 0: self.lanechange_manual_timer -= 1 if self.emergency_manual_timer > 0: self.emergency_manual_timer -= 1 if self.driver_steering_torque_above_timer > 0: self.driver_steering_torque_above_timer -= 1 if not lkas_active: apply_steer = 0 self.apply_accel_last = apply_accel self.apply_steer_last = apply_steer sys_warning, sys_state, left_lane_warning, right_lane_warning =\ process_hud_alert(lkas_active, self.car_fingerprint, visual_alert, left_lane, right_lane, left_lane_depart, right_lane_depart, self.lkas_button_on) clu11_speed = CS.clu11["CF_Clu_Vanz"] enabled_speed = 38 if CS.is_set_speed_in_mph else 55 if clu11_speed > enabled_speed or not lkas_active: enabled_speed = clu11_speed if not(min_set_speed < set_speed < 255 * CV.KPH_TO_MS): set_speed = min_set_speed set_speed *= CV.MS_TO_MPH if CS.is_set_speed_in_mph else CV.MS_TO_KPH if frame == 0: # initialize counts from last received count signals self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"] self.scc12_cnt = CS.scc12["CR_VSM_Alive"] + 1 if not CS.no_radar else 0 #TODO: fix this # self.prev_scc_cnt = CS.scc11["AliveCounterACC"] # self.scc_update_frame = frame # check if SCC is alive # if frame % 7 == 0: # if CS.scc11["AliveCounterACC"] == self.prev_scc_cnt: # if frame - self.scc_update_frame > 20 and self.scc_live: # self.scc_live = False # else: # self.scc_live = True # self.prev_scc_cnt = CS.scc11["AliveCounterACC"] # self.scc_update_frame = frame self.prev_scc_cnt = CS.scc11["AliveCounterACC"] self.lkas11_cnt = (self.lkas11_cnt + 1) % 0x10 self.scc12_cnt %= 0xF can_sends = [] can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active, CS.lkas11, sys_warning, sys_state, enabled, left_lane, right_lane, left_lane_warning, right_lane_warning, 0)) if CS.mdps_bus or CS.scc_bus == 1: # send lkas11 bus 1 if mdps or scc is on bus 1 can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active, CS.lkas11, sys_warning, sys_state, enabled, left_lane, right_lane, left_lane_warning, right_lane_warning, 1)) if frame % 2 and CS.mdps_bus: # send clu11 to mdps if it is not on bus 0 can_sends.append(create_clu11(self.packer, frame, CS.mdps_bus, CS.clu11, Buttons.NONE, enabled_speed)) str_log1 = '곡률={:03.0f} 토크={:03.0f} 프레임률={:03.0f} ST={:03.0f}/{:01.0f}/{:01.0f}'.format(abs(self.model_speed), abs(new_steer), self.timer1.sampleTime(), SteerLimitParams.STEER_MAX, SteerLimitParams.STEER_DELTA_UP, SteerLimitParams.STEER_DELTA_DOWN) trace1.printf1('{} {}'.format(str_log1, self.str_log2)) if CS.out.cruiseState.modeSel == 0 and self.mode_change_switch == 3: self.mode_change_timer = 50 self.mode_change_switch = 0 elif CS.out.cruiseState.modeSel == 1 and self.mode_change_switch == 0: self.mode_change_timer = 50 self.mode_change_switch = 1 elif CS.out.cruiseState.modeSel == 2 and self.mode_change_switch == 1: self.mode_change_timer = 50 self.mode_change_switch = 2 elif CS.out.cruiseState.modeSel == 3 and self.mode_change_switch == 2: self.mode_change_timer = 50 self.mode_change_switch = 3 if self.mode_change_timer > 0: self.mode_change_timer -= 1 run_speed_ctrl = self.opkr_variablecruise and CS.acc_active and (CS.out.cruiseState.modeSel == 1 or CS.out.cruiseState.modeSel == 2 or CS.out.cruiseState.modeSel == 3) if not run_speed_ctrl: if CS.out.cruiseState.modeSel == 0: self.steer_mode = "오파모드" elif CS.out.cruiseState.modeSel == 1: self.steer_mode = "차간+커브" elif CS.out.cruiseState.modeSel == 2: self.steer_mode = "차간ONLY" elif CS.out.cruiseState.modeSel == 3: self.steer_mode = "편도1차선" if CS.out.steerWarning == 0: self.mdps_status = "정상" elif CS.out.steerWarning == 1: self.mdps_status = "오류" if CS.lkas_button_on == 0: self.lkas_switch = "OFF" elif CS.lkas_button_on == 1: self.lkas_switch = "ON" else: self.lkas_switch = "-" if self.cruise_gap != CS.cruiseGapSet: self.cruise_gap = CS.cruiseGapSet str_log3 = '주행모드={:s} MDPS상태={:s} LKAS버튼={:s} 크루즈갭={:1.0f}'.format( self.steer_mode, self.mdps_status, self.lkas_switch, self.cruise_gap) trace1.printf2( '{}'.format( str_log3 ) ) if pcm_cancel_cmd and self.longcontrol: can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed)) if CS.out.cruiseState.standstill: if self.last_lead_distance == 0 or not self.opkr_autoresume: self.last_lead_distance = CS.lead_distance self.resume_cnt = 0 self.resume_wait_timer = 0 elif self.resume_wait_timer > 0: self.resume_wait_timer -= 1 elif CS.lead_distance != self.last_lead_distance: can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed)) self.resume_cnt += 1 if self.resume_cnt > 5: self.resume_cnt = 0 self.resume_wait_timer = int(0.25 / DT_CTRL) elif self.cruise_gap_prev == 0 and run_speed_ctrl: self.cruise_gap_prev = CS.cruiseGapSet self.cruise_gap_set_init = 1 elif CS.cruiseGapSet != 1.0 and run_speed_ctrl: self.cruise_gap_switch_timer += 1 if self.cruise_gap_switch_timer > 100: can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.GAP_DIST, clu11_speed)) self.cruise_gap_switch_timer = 0 # reset lead distnce after the car starts moving elif self.last_lead_distance != 0: self.last_lead_distance = 0 elif run_speed_ctrl: is_sc_run = self.SC.update(CS, sm, self) if is_sc_run: can_sends.append(create_clu11(self.packer, self.resume_cnt, CS.scc_bus, CS.clu11, self.SC.btn_type, self.SC.sc_clu_speed)) self.resume_cnt += 1 else: self.resume_cnt = 0 if self.dRel > 17 and self.cruise_gap_prev != CS.cruiseGapSet and self.cruise_gap_set_init == 1: self.cruise_gap_switch_timer += 1 if self.cruise_gap_switch_timer > 50: can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.GAP_DIST, clu11_speed)) self.cruise_gap_switch_timer = 0 elif self.cruise_gap_prev == CS.cruiseGapSet: self.cruise_gap_set_init = 0 self.cruise_gap_prev = 0 if CS.mdps_bus: # send mdps12 to LKAS to prevent LKAS error can_sends.append(create_mdps12(self.packer, frame, CS.mdps12)) # send scc to car if longcontrol enabled and SCC not on bus 0 or ont live if self.longcontrol and (CS.scc_bus or not self.scc_live) and frame % 2 == 0: can_sends.append(create_scc12(self.packer, apply_accel, enabled, self.scc12_cnt, self.scc_live, CS.scc12)) can_sends.append(create_scc11(self.packer, frame, enabled, set_speed, lead_visible, self.scc_live, CS.scc11)) if CS.has_scc13 and frame % 20 == 0: can_sends.append(create_scc13(self.packer, CS.scc13)) if CS.has_scc14: can_sends.append(create_scc14(self.packer, enabled, CS.scc14)) self.scc12_cnt += 1 # 20 Hz LFA MFA message if frame % 5 == 0 and self.car_fingerprint in FEATURES["send_lfa_mfa"]: can_sends.append(create_lfa_mfa(self.packer, frame, lkas_active)) if CS.spas_enabled: if CS.mdps_bus: can_sends.append(create_ems11(self.packer, CS.ems11, spas_active)) # SPAS11 50hz if (frame % 2) == 0: if CS.mdps11_stat == 7 and not self.mdps11_stat_last == 7: self.en_spas = 7 self.en_cnt = 0 if self.en_spas == 7 and self.en_cnt >= 8: self.en_spas = 3 self.en_cnt = 0 if self.en_cnt < 8 and spas_active: self.en_spas = 4 elif self.en_cnt >= 8 and spas_active: self.en_spas = 5 if not spas_active: self.apply_steer_ang = CS.mdps11_strang self.en_spas = 3 self.en_cnt = 0 self.mdps11_stat_last = CS.mdps11_stat self.en_cnt += 1 can_sends.append(create_spas11(self.packer, self.car_fingerprint, (frame // 2), self.en_spas, self.apply_steer_ang, CS.mdps_bus)) # SPAS12 20Hz if (frame % 5) == 0: can_sends.append(create_spas12(CS.mdps_bus)) return can_sends
def update(self, enabled, CS, frame, CC, actuators, pcm_cancel_cmd, visual_alert, left_lane, right_lane, left_lane_depart, right_lane_depart, set_speed, lead_visible, sm): # *** compute control surfaces *** # gas and brake apply_accel = actuators.gas - actuators.brake apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady) apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) param = self.p #self.model_speed = 255 - self.SC.calc_va(sm, CS.out.vEgo) #atom model_speed #self.model_speed = self.SC.cal_model_speed(sm, CS.out.vEgo) #self.curve_speed = self.SC.cal_curve_speed(sm, CS.out.vEgo, frame) plan = sm['longitudinalPlan'] self.dRel = int(plan.dRel1) #EON Lead self.yRel = int(plan.yRel1) #EON Lead self.vRel = int(plan.vRel1 * 3.6 + 0.5) #EON Lead self.dRel2 = int(plan.dRel2) #EON Lead self.yRel2 = int(plan.yRel2) #EON Lead self.vRel2 = int(plan.vRel2 * 3.6 + 0.5) #EON Lead self.lead2_status = plan.status2 self.target_map_speed_camera = plan.targetSpeedCamera self.accActive = CS.acc_active lateral_plan = sm['lateralPlan'] self.outScale = lateral_plan.outputScale self.vCruiseSet = lateral_plan.vCruiseSet #self.model_speed = interp(abs(lateral_plan.vCurvature), [0.0002, 0.01], [255, 30]) #Hoya self.model_speed = interp(abs(lateral_plan.vCurvature), [0.0, 0.0002, 0.00074, 0.0025, 0.008, 0.02], [255, 255, 130, 90, 60, 20]) if CS.out.vEgo > 8: if self.variable_steer_max: self.steerMax = interp(int(abs(self.model_speed)), self.model_speed_range, self.steerMax_range) else: self.steerMax = int(self.params.get("SteerMaxBaseAdj", encoding='utf8')) if self.variable_steer_delta: self.steerDeltaUp = interp(int(abs(self.model_speed)), self.model_speed_range, self.steerDeltaUp_range) self.steerDeltaDown = interp(int(abs(self.model_speed)), self.model_speed_range, self.steerDeltaDown_range) else: self.steerDeltaUp = int(self.params.get("SteerDeltaUpBaseAdj", encoding='utf8')) self.steerDeltaDown = int(self.params.get("SteerDeltaDownBaseAdj", encoding='utf8')) else: self.steerMax = int(self.params.get("SteerMaxBaseAdj", encoding='utf8')) self.steerDeltaUp = int(self.params.get("SteerDeltaUpBaseAdj", encoding='utf8')) self.steerDeltaDown = int(self.params.get("SteerDeltaDownBaseAdj", encoding='utf8')) param.STEER_MAX = min(CarControllerParams.STEER_MAX, self.steerMax) # variable steermax param.STEER_DELTA_UP = min(CarControllerParams.STEER_DELTA_UP, self.steerDeltaUp) # variable deltaUp param.STEER_DELTA_DOWN = min(CarControllerParams.STEER_DELTA_DOWN, self.steerDeltaDown) # variable deltaDown #param.STEER_DELTA_UP = CarControllerParams.STEER_DELTA_UP # fixed deltaUp #param.STEER_DELTA_DOWN = CarControllerParams.STEER_DELTA_DOWN # fixed deltaDown # Steering Torque if 0 <= self.driver_steering_torque_above_timer < 100: new_steer = int(round(actuators.steer * self.steerMax * (self.driver_steering_torque_above_timer / 100))) else: new_steer = int(round(actuators.steer * self.steerMax)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, param) self.steer_rate_limited = new_steer != apply_steer # SPAS limit angle extremes for safety if CS.spas_enabled: apply_steer_ang_req = clip(actuators.steeringAngleDeg, -1*(STEER_ANG_MAX), STEER_ANG_MAX) # SPAS limit angle rate for safety if abs(self.apply_steer_ang - apply_steer_ang_req) > STEER_ANG_MAX_RATE: if apply_steer_ang_req > self.apply_steer_ang: self.apply_steer_ang += STEER_ANG_MAX_RATE else: self.apply_steer_ang -= STEER_ANG_MAX_RATE else: self.apply_steer_ang = apply_steer_ang_req spas_active = CS.spas_enabled and enabled and (self.spas_always or CS.out.vEgo < 7.0) # 25km/h # disable if steer angle reach 90 deg, otherwise mdps fault in some models if self.opkr_maxanglelimit >= 90: lkas_active = enabled and abs(CS.out.steeringAngleDeg) < self.opkr_maxanglelimit and not spas_active else: lkas_active = enabled and not spas_active if (( CS.out.leftBlinker and not CS.out.rightBlinker) or ( CS.out.rightBlinker and not CS.out.leftBlinker)) and CS.out.vEgo < LANE_CHANGE_SPEED_MIN and self.opkr_turnsteeringdisable: self.lanechange_manual_timer = 50 if CS.out.leftBlinker and CS.out.rightBlinker: self.emergency_manual_timer = 50 if self.lanechange_manual_timer: lkas_active = 0 if self.lanechange_manual_timer > 0: self.lanechange_manual_timer -= 1 if self.emergency_manual_timer > 0: self.emergency_manual_timer -= 1 if abs(CS.out.steeringTorque) > 180 and CS.out.vEgo < LANE_CHANGE_SPEED_MIN: self.driver_steering_torque_above = True else: self.driver_steering_torque_above = False if self.driver_steering_torque_above == True: self.driver_steering_torque_above_timer -= 1 if self.driver_steering_torque_above_timer <= 0: self.driver_steering_torque_above_timer = 0 elif self.driver_steering_torque_above == False: self.driver_steering_torque_above_timer += 5 if self.driver_steering_torque_above_timer >= 100: self.driver_steering_torque_above_timer = 100 if not lkas_active: apply_steer = 0 self.apply_accel_last = apply_accel self.apply_steer_last = apply_steer if CS.acc_active and CS.lead_distance > 149 and self.dRel < ((CS.out.vEgo * CV.MS_TO_KPH)+5) < 100 and self.vRel < -5 and CS.out.vEgo > 7 and abs(lateral_plan.steerAngleDesireDeg) < 15: self.need_brake_timer += 1 if self.need_brake_timer > 50: self.need_brake = True else: self.need_brake = False self.need_brake_timer = 0 sys_warning, sys_state, left_lane_warning, right_lane_warning =\ process_hud_alert(lkas_active, self.car_fingerprint, visual_alert, left_lane, right_lane, left_lane_depart, right_lane_depart, self.lkas_button_on) clu11_speed = CS.clu11["CF_Clu_Vanz"] enabled_speed = 38 if CS.is_set_speed_in_mph else 55 if clu11_speed > enabled_speed or not lkas_active: enabled_speed = clu11_speed if not(min_set_speed < set_speed < 255 * CV.KPH_TO_MS): set_speed = min_set_speed set_speed *= CV.MS_TO_MPH if CS.is_set_speed_in_mph else CV.MS_TO_KPH if frame == 0: # initialize counts from last received count signals self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"] self.scc12_cnt = CS.scc12["CR_VSM_Alive"] + 1 if not CS.no_radar else 0 #TODO: fix this # self.prev_scc_cnt = CS.scc11["AliveCounterACC"] # self.scc_update_frame = frame # check if SCC is alive # if frame % 7 == 0: # if CS.scc11["AliveCounterACC"] == self.prev_scc_cnt: # if frame - self.scc_update_frame > 20 and self.scc_live: # self.scc_live = False # else: # self.scc_live = True # self.prev_scc_cnt = CS.scc11["AliveCounterACC"] # self.scc_update_frame = frame self.prev_scc_cnt = CS.scc11["AliveCounterACC"] self.lkas11_cnt = (self.lkas11_cnt + 1) % 0x10 self.scc12_cnt %= 0xF can_sends = [] can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active, CS.lkas11, sys_warning, sys_state, enabled, left_lane, right_lane, left_lane_warning, right_lane_warning, 0)) if CS.mdps_bus or CS.scc_bus == 1: # send lkas11 bus 1 if mdps or scc is on bus 1 can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active, CS.lkas11, sys_warning, sys_state, enabled, left_lane, right_lane, left_lane_warning, right_lane_warning, 1)) if frame % 2 and CS.mdps_bus: # send clu11 to mdps if it is not on bus 0 can_sends.append(create_clu11(self.packer, frame, CS.mdps_bus, CS.clu11, Buttons.NONE, enabled_speed)) str_log1 = 'CV={:03.0f} TQ={:03.0f} R={:03.0f} ST={:03.0f}/{:01.0f}/{:01.0f}'.format(abs(self.model_speed), abs(new_steer), self.timer1.sampleTime(), self.steerMax, self.steerDeltaUp, self.steerDeltaDown) if self.params.get("OpkrLiveTune", encoding='utf8') == "1": if int(self.params.get("LateralControlMethod", encoding='utf8')) == 0: self.str_log2 = 'T={:0.2f}/{:0.3f}/{:0.2f}/{:0.5f}'.format(float(int(self.params.get("PidKp", encoding='utf8')) * 0.01), float(int(self.params.get("PidKi", encoding='utf8')) * 0.001), float(int(self.params.get("PidKd", encoding='utf8')) * 0.01), float(int(self.params.get("PidKf", encoding='utf8')) * 0.00001)) elif int(self.params.get("LateralControlMethod", encoding='utf8')) == 1: self.str_log2 = 'T={:03.1f}/{:03.1f}/{:03.1f}/{:03.1f}'.format(float(int(self.params.get("InnerLoopGain", encoding='utf8')) * 0.1), float(int(self.params.get("OuterLoopGain", encoding='utf8')) * 0.1), float(int(self.params.get("TimeConstant", encoding='utf8')) * 0.1), float(int(self.params.get("ActuatorEffectiveness", encoding='utf8')) * 0.1)) elif int(self.params.get("LateralControlMethod", encoding='utf8')) == 2: self.str_log2 = 'T={:04.0f}/{:05.3f}/{:06.4f}'.format(float(int(self.params.get("Scale", encoding='utf8')) * 1.0), float(int(self.params.get("LqrKi", encoding='utf8')) * 0.001), float(int(self.params.get("DcGain", encoding='utf8')) * 0.0001)) trace1.printf1('{} {}'.format(str_log1, self.str_log2)) if CS.out.cruiseState.modeSel == 0 and self.mode_change_switch == 3: self.mode_change_timer = 50 self.mode_change_switch = 0 elif CS.out.cruiseState.modeSel == 1 and self.mode_change_switch == 0: self.mode_change_timer = 50 self.mode_change_switch = 1 elif CS.out.cruiseState.modeSel == 2 and self.mode_change_switch == 1: self.mode_change_timer = 50 self.mode_change_switch = 2 elif CS.out.cruiseState.modeSel == 3 and self.mode_change_switch == 2: self.mode_change_timer = 50 self.mode_change_switch = 3 if self.mode_change_timer > 0: self.mode_change_timer -= 1 run_speed_ctrl = self.opkr_variablecruise and CS.acc_active and (CS.out.cruiseState.modeSel == 1 or CS.out.cruiseState.modeSel == 2 or CS.out.cruiseState.modeSel == 3) if not run_speed_ctrl: if CS.out.cruiseState.modeSel == 0: self.steer_mode = "OpenPilot" elif CS.out.cruiseState.modeSel == 1: self.steer_mode = "Dist+Curv" elif CS.out.cruiseState.modeSel == 2: self.steer_mode = "DistOnly" elif CS.out.cruiseState.modeSel == 3: self.steer_mode = "OneWay" if CS.out.steerWarning == 0: self.mdps_status = "OK" elif CS.out.steerWarning == 1: self.mdps_status = "ERR" if CS.lkas_button_on == 0: self.lkas_switch = "OFF" elif CS.lkas_button_on == 1: self.lkas_switch = "ON" else: self.lkas_switch = "-" if self.cruise_gap != CS.cruiseGapSet: self.cruise_gap = CS.cruiseGapSet if CS.lead_distance < 149: self.leadcar_status = "O" else: self.leadcar_status = "-" str_log2 = 'MODE={:s} MDPS={:s} LKAS={:s} CSG={:1.0f} LEAD={:s}'.format(self.steer_mode, self.mdps_status, self.lkas_switch, self.cruise_gap, self.leadcar_status) trace1.printf2( '{}'.format( str_log2 ) ) if pcm_cancel_cmd and self.longcontrol: can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed)) if CS.out.cruiseState.standstill: self.standstill_status = 1 if self.opkr_autoresume: # run only first time when the car stopped if self.last_lead_distance == 0: # get the lead distance from the Radar self.last_lead_distance = CS.lead_distance self.resume_cnt = 0 self.res_switch_timer = 0 self.standstill_fault_reduce_timer += 1 elif self.res_switch_timer > 0: self.res_switch_timer -= 1 self.standstill_fault_reduce_timer += 1 # at least 1 sec delay after entering the standstill elif 100 < self.standstill_fault_reduce_timer and CS.lead_distance != self.last_lead_distance: can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed)) self.resume_cnt += 1 if self.resume_cnt > 5: self.resume_cnt = 0 self.res_switch_timer = randint(10, 15) self.standstill_fault_reduce_timer += 1 # gap save elif 160 < self.standstill_fault_reduce_timer and self.cruise_gap_prev == 0 and self.opkr_autoresume: self.cruise_gap_prev = CS.cruiseGapSet self.cruise_gap_set_init = 1 # gap adjust to 1 for fast start elif 160 < self.standstill_fault_reduce_timer and CS.cruiseGapSet != 1.0 and self.opkr_autoresume: self.cruise_gap_switch_timer += 1 if self.cruise_gap_switch_timer > 100: can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.GAP_DIST, clu11_speed)) self.cruise_gap_switch_timer = 0 elif self.opkr_autoresume: self.standstill_fault_reduce_timer += 1 # reset lead distnce after the car starts moving elif self.last_lead_distance != 0: self.last_lead_distance = 0 elif run_speed_ctrl: is_sc_run = self.SC.update(CS, sm, self) if is_sc_run: can_sends.append(create_clu11(self.packer, self.resume_cnt, CS.scc_bus, CS.clu11, self.SC.btn_type, self.SC.sc_clu_speed)) self.resume_cnt += 1 else: self.resume_cnt = 0 # gap restore if self.dRel > 17 and self.vRel < 5 and self.cruise_gap_prev != CS.cruiseGapSet and self.cruise_gap_set_init == 1 and self.opkr_autoresume: self.cruise_gap_switch_timer += 1 if self.cruise_gap_switch_timer > 50: can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.GAP_DIST, clu11_speed)) self.cruise_gap_switch_timer = 0 elif self.cruise_gap_prev == CS.cruiseGapSet and self.opkr_autoresume: self.cruise_gap_set_init = 0 self.cruise_gap_prev = 0 if CS.out.brakeLights and CS.out.vEgo == 0 and not CS.acc_active: self.standstill_status_timer += 1 if self.standstill_status_timer > 200: self.standstill_status = 1 self.standstill_status_timer = 0 if self.standstill_status == 1 and CS.out.vEgo > 1: self.standstill_status = 0 self.standstill_fault_reduce_timer = 0 self.v_set_dis_prev = 180 self.last_resume_frame = frame self.res_switch_timer = 0 self.resume_cnt = 0 if CS.mdps_bus: # send mdps12 to LKAS to prevent LKAS error can_sends.append(create_mdps12(self.packer, frame, CS.mdps12)) # send scc to car if longcontrol enabled and SCC not on bus 0 or ont live if self.longcontrol and (CS.scc_bus or not self.scc_live) and frame % 2 == 0: can_sends.append(create_scc12(self.packer, apply_accel, enabled, self.scc12_cnt, self.scc_live, CS.scc12)) can_sends.append(create_scc11(self.packer, frame, enabled, set_speed, lead_visible, self.scc_live, CS.scc11)) if CS.has_scc13 and frame % 20 == 0: can_sends.append(create_scc13(self.packer, CS.scc13)) if CS.has_scc14: can_sends.append(create_scc14(self.packer, enabled, CS.scc14)) self.scc12_cnt += 1 # 20 Hz LFA MFA message if frame % 5 == 0 and self.car_fingerprint in FEATURES["send_lfahda_mfa"]: can_sends.append(create_lfahda_mfc(self.packer, frame, lkas_active)) if CS.spas_enabled: if CS.mdps_bus: can_sends.append(create_ems11(self.packer, CS.ems11, spas_active)) # SPAS11 50hz if (frame % 2) == 0: if CS.mdps11_stat == 7 and not self.mdps11_stat_last == 7: self.en_spas = 7 self.en_cnt = 0 if self.en_spas == 7 and self.en_cnt >= 8: self.en_spas = 3 self.en_cnt = 0 if self.en_cnt < 8 and spas_active: self.en_spas = 4 elif self.en_cnt >= 8 and spas_active: self.en_spas = 5 if not spas_active: self.apply_steer_ang = CS.mdps11_strang self.en_spas = 3 self.en_cnt = 0 self.mdps11_stat_last = CS.mdps11_stat self.en_cnt += 1 can_sends.append(create_spas11(self.packer, self.car_fingerprint, (frame // 2), self.en_spas, self.apply_steer_ang, CS.mdps_bus)) # SPAS12 20Hz if (frame % 5) == 0: can_sends.append(create_spas12(CS.mdps_bus)) return can_sends
def update(self, active, CS, v_target, v_target_future, a_target, CP, hasLead, radarState, decelForTurn, longitudinalPlanSource, extras): """Update longitudinal control. This updates the state machine and runs a PID loop""" # Actuation limits gas_max = interp(CS.vEgo, CP.gasMaxBP, CP.gasMaxV) brake_max = interp(CS.vEgo, CP.brakeMaxBP, CP.brakeMaxV) multiplier = 0 multiplier2 = 0 multiplier3 = 0 vRel = 0 if self.enable_dg: gas_max = self.dynamic_gas.update(CS, extras) # Update state machine output_gb = self.last_output_gb if radarState is None: dRel = 200 vRel = 0 else: dRel = radarState.leadOne.dRel vRel = radarState.leadOne.vRel # 앞차와 거리가 3.5m이하일때 상태를 강제로 STOP으로 만듬 if hasLead: stop = True if (dRel < 3.5 and radarState.leadOne.status) else False else: stop = False self.long_control_state = long_control_state_trans( active, self.long_control_state, CS.vEgo, v_target_future, self.v_pid, output_gb, CS.brakePressed, CS.cruiseState.standstill, stop, CS.gasPressed, CP.minSpeedCan) v_ego_pid = max( CS.vEgo, CP.minSpeedCan ) # Without this we get jumps, CAN bus reports 0 when speed < 0.3 if self.long_control_state == LongCtrlState.off or (CS.brakePressed or CS.gasPressed): self.v_pid = v_ego_pid self.pid.reset() output_gb = 0. # tracking objects and driving elif self.long_control_state == LongCtrlState.pid: self.v_pid = v_target self.pid.pos_limit = gas_max self.pid.neg_limit = -brake_max # Toyota starts braking more when it thinks you want to stop # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7 deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV) if longitudinalPlanSource == 'cruise': if decelForTurn and not self.lastdecelForTurn: self.lastdecelForTurn = True self.pid._k_p = (CP.longitudinalTuning.kpBP, [ x * 0 for x in CP.longitudinalTuning.kpV ]) self.pid._k_i = (CP.longitudinalTuning.kiBP, [ x * 0 for x in CP.longitudinalTuning.kiV ]) self.pid.i = 0.0 self.pid.k_f = 1.0 self.v_pid = CS.vEgo self.pid.reset() if self.lastdecelForTurn and not decelForTurn: self.lastdecelForTurn = False self.pid._k_p = (CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV) self.pid._k_i = (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV) self.pid.k_f = 1.0 self.v_pid = CS.vEgo self.pid.reset() else: if self.lastdecelForTurn: self.v_pid = CS.vEgo self.pid.reset() self.lastdecelForTurn = False self.pid._k_p = (CP.longitudinalTuning.kpBP, [x * 1 for x in CP.longitudinalTuning.kpV]) self.pid._k_i = (CP.longitudinalTuning.kiBP, [x * 1 for x in CP.longitudinalTuning.kiV]) self.pid.k_f = 1.0 output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot) ## 감속 보충을 위해 out_gb -값일 때 임의의 값을 더 곱해줌 #if hasLead and radarState.leadOne.status and 4 < dRel <= 55 and output_gb < 0 and vRel < 0: # multiplier = max((self.v_pid/(max(v_target_future, 1))), 1) # multiplier = clip(multiplier, 1.2, 3.5) # output_gb *= multiplier # #20m 간격 이하에서 거리보다 속도가 2배 이상인경우 조금더 감속 보충 # if dRel*2 < CS.vEgo*3.6 and dRel <= 20: # multiplier2 = interp(dRel, [4, 20], [2, 1]) # elif dRel*1.5 < CS.vEgo*3.6 and dRel <= 20: # multiplier2 = interp(dRel, [4, 20], [1.5, 1]) # output_gb *= multiplier3 # output_gb = clip(output_gb, -brake_max, gas_max) ## 앞차 감속시 가속하는것을 완화해줌 #elif hasLead and radarState.leadOne.status and 4 < dRel <= 55 and output_gb > 0 and vRel < 0: # multiplier3 = interp(abs(vRel*3.6), [0, 1, 2], [1.0, 0.5, 0.0]) # output_gb *= multiplier3 # output_gb = clip(output_gb, -brake_max, gas_max) if prevent_overshoot: output_gb = min(output_gb, 0.0) # Intention is to stop, switch to a different brake control until we stop elif self.long_control_state == LongCtrlState.stopping: # Keep applying brakes until the car is stopped factor = 1 if hasLead: factor = interp(dRel, [2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0], [6, 3.5, 1.5, 0.8, 0.5, 0.3, 0.2]) if not CS.standstill or output_gb > -BRAKE_STOPPING_TARGET: output_gb -= CP.stoppingBrakeRate / RATE * factor output_gb = clip(output_gb, -brake_max, gas_max) self.reset(CS.vEgo) # Intention is to move again, release brake fast before handing control to PID elif self.long_control_state == LongCtrlState.starting: factor = 1 if hasLead: factor = interp(dRel, [0.0, 2.0, 3.0, 4.0, 5.0], [0.0, 0.5, 0.75, 1.0, 1000.0]) if output_gb < -0.2: output_gb += CP.startingBrakeRate / RATE * factor self.reset(CS.vEgo) self.last_output_gb = output_gb final_gas = clip(output_gb, 0., gas_max) final_brake = -clip(output_gb, -brake_max, 0.) if self.long_control_state == LongCtrlState.stopping: self.long_stat = "STP" elif self.long_control_state == LongCtrlState.starting: self.long_stat = "STR" elif self.long_control_state == LongCtrlState.pid: self.long_stat = "PID" elif self.long_control_state == LongCtrlState.off: self.long_stat = "OFF" else: self.long_stat = "---" str_log3 = 'LS={:s} GS={:01.2f}/{:01.2f} BK={:01.2f}/{:01.2f} GB={:+04.2f} TG=P:{:05.2f}/F:{:05.2f}/m:{:.1f}/m2:{:.1f}/m3:{:.1f}/vr:{:+04.1f}'.format( self.long_stat, final_gas, gas_max, abs(final_brake), abs(brake_max), output_gb, abs(self.v_pid), abs(v_target_future), multiplier, multiplier2, multiplier3, vRel) trace1.printf2('{}'.format(str_log3)) return final_gas, final_brake
def update(self, CC, CS, frame, sm, CP): if self.CP != CP: self.CP = CP self.param_load() enabled = CC.enabled actuators = CC.actuators pcm_cancel_cmd = CC.cruiseControl.cancel path_plan = sm['pathPlan'] abs_angle_steers = abs(actuators.steerAngle) self.dRel, self.yRel, self.vRel = SpdController.get_lead(sm) if self.SC is not None: self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo) else: self.model_speed = self.model_sum = 0 # Steering Torque param = self.steerParams_torque(CS, abs_angle_steers, path_plan, CC) new_steer = actuators.steer * param.STEER_MAX apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, param) self.steer_rate_limited = new_steer != apply_steer apply_steer_limit = param.STEER_MAX if self.steer_torque_ratio < 1: apply_steer_limit = int(self.steer_torque_ratio * param.STEER_MAX) apply_steer = self.limit_ctrl(apply_steer, apply_steer_limit, 0) # disable if steer angle reach 90 deg, otherwise mdps fault in some models lkas_active = enabled and abs( CS.out.steeringAngle) < 180. #and self.lkas_button # fix for Genesis hard fault at low speed #if CS.out.vEgo < 16.666667 and self.car_fingerprint == CAR.GENESIS: # lkas_active = 0 if not lkas_active: apply_steer = 0 steer_req = 1 if apply_steer else 0 self.apply_steer_last = apply_steer sys_warning, sys_state = self.process_hud_alert(lkas_active, CC) clu11_speed = CS.clu11["CF_Clu_Vanz"] enabled_speed = 38 if CS.is_set_speed_in_mph else 60 if clu11_speed > enabled_speed or not lkas_active: enabled_speed = clu11_speed can_sends = [] if frame == 0: # initialize counts from last received count signals self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"] + 1 self.lkas11_cnt %= 0x10 can_sends.append( create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint, apply_steer, steer_req, CS.lkas11, sys_warning, sys_state, CC, enabled, 0)) if CS.mdps_bus or CS.scc_bus == 1: # send lkas11 bus 1 if mdps is on bus 1 can_sends.append( create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint, apply_steer, steer_req, CS.lkas11, sys_warning, sys_state, CC, enabled, 1)) if CS.mdps_bus: # send clu11 to mdps if it is not on bus 0 can_sends.append( create_clu11(self.packer, frame, CS.mdps_bus, CS.clu11, Buttons.NONE, enabled_speed)) #if pcm_cancel_cmd and self.longcontrol: # can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed)) #else: # send mdps12 to LKAS to prevent LKAS error if no cancel cmd can_sends.append(create_mdps12(self.packer, frame, CS.mdps12)) #str_log1 = 'CV={:5.1f}/{:5.3f} torg:{:5.0f}'.format( self.model_speed, self.model_sum, apply_steer ) str_log1 = 'CV={:5.1f} torg:{:6.1f}'.format(self.model_speed, apply_steer) #str_log2 = 'limit={:.0f} tm={:.1f} '.format( apply_steer_limit, self.timer1.sampleTime() ) str_log2 = ' limit={:6.1f}/tm={:3.1f} MAX={:5.1f} UP/DN={:3.1f}/{:3.1f} '.format( apply_steer_limit, self.timer1.sampleTime(), self.MAX, self.UP, self.DN) trace1.printf('{} {}'.format(str_log1, str_log2)) run_speed_ctrl = self.param_OpkrAccelProfile and CS.acc_active and self.SC != None if not run_speed_ctrl: str_log2 = 'U={:.0f} LK={:.0f} dir={} steer={:5.0f} '.format( CS.Mdps_ToiUnavail, CS.lkas_button_on, self.steer_torque_ratio_dir, CS.out.steeringTorque) trace1.printf2('{}'.format(str_log2)) if pcm_cancel_cmd and self.CP.longcontrolEnabled: can_sends.append( create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL)) elif CS.out.cruiseState.standstill: # run only first time when the car stopped if self.last_lead_distance == 0 or not self.param_OpkrAutoResume: # get the lead distance from the Radar self.last_lead_distance = CS.lead_distance self.resume_cnt = 0 # when lead car starts moving, create 6 RES msgs elif CS.lead_distance != self.last_lead_distance < CS.lead_distance > 4.8 and ( frame - self.last_resume_frame) > 5: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed)) self.resume_cnt += 1 # interval after 10 msgs if self.resume_cnt > 10: self.last_resume_frame = frame self.resume_cnt = 0 self.clu11_cnt = 0 # reset lead distnce after the car starts moving elif self.last_lead_distance != 0: self.last_lead_distance = 0 elif run_speed_ctrl and self.SC != None: is_sc_run = self.SC.update(CS, sm, self) if is_sc_run: can_sends.append( create_clu11(self.packer, self.resume_cnt, CS.scc_bus, CS.clu11, self.SC.btn_type, self.SC.sc_clu_speed)) self.resume_cnt += 1 else: self.resume_cnt = 0 str1 = 'run={} cruise_set_mode={} kph={:.1f}/{:.1f} DO={:.0f}/{:.0f} '.format( is_sc_run, self.SC.cruise_set_mode, self.SC.cruise_set_speed_kph, CS.VSetDis, CS.driverOverride, CS.cruise_buttons) str2 = 'btn_type={:.0f} speed={:.1f} cnt={:.0f}'.format( self.SC.btn_type, self.SC.sc_clu_speed, self.resume_cnt) str_log = str1 + str2 self.traceCC.add(str_log) # 20 Hz LFA MFA message if frame % 5 == 0 and self.car_fingerprint in [ CAR.PALISADE, CAR.SELTOS ]: can_sends.append(create_lfa_mfa(self.packer, frame, enabled)) # counter inc self.lkas11_cnt += 1 return can_sends
def update(self, c, CS, frame, sm, CP): if self.CP != CP: self.CP = CP self.param_load() enabled = c.enabled actuators = c.actuators pcm_cancel_cmd = c.cruiseControl.cancel path_plan = sm['pathPlan'] self.dRel, self.yRel, self.vRel = SpdController.get_lead(sm) if self.SC is not None: self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo) else: self.model_speed = self.model_sum = 0 # Steering Torque param, dst_steer = self.steerParams_torque(CS, c.actuators, path_plan) new_steer = actuators.steer * param.STEER_MAX apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, param) self.steer_rate_limited = new_steer != apply_steer apply_steer_limit = param.STEER_MAX if self.steer_torque_ratio < 1: apply_steer_limit = int(self.steer_torque_ratio * param.STEER_MAX) apply_steer = self.limit_ctrl(apply_steer, apply_steer_limit, 0) # disable if steer angle reach 90 deg, otherwise mdps fault in some models lkas_active = enabled and CS.main_on and CS.out.cruiseState.enabled and abs( CS.out.steeringAngle) < 180. #and self.lkas_button if not lkas_active: apply_steer = 0 steer_req = 1 if apply_steer else 0 self.apply_steer_last = apply_steer sys_warning, self.hud_sys_state = self.process_hud_alert( lkas_active, c) can_sends = [] if frame == 0: # initialize counts from last received count signals self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"] + 1 self.lkas11_cnt %= 0x10 can_sends.append( create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint, apply_steer, steer_req, CS.lkas11, sys_warning, self.hud_sys_state, c)) # send mdps12 to LKAS to prevent LKAS error if no cancel cmd can_sends.append(create_mdps12(self.packer, frame, CS.mdps12)) str_log1 = 'torg:{:5.0f}/{:5.0f}/{:5.0f} CV={:5.1f}'.format( apply_steer, new_steer, dst_steer, self.model_speed) str_log2 = 'limit={:.0f} tm={:.1f} '.format(apply_steer_limit, self.timer1.sampleTime()) trace1.printf('{} {}'.format(str_log1, str_log2)) run_speed_ctrl = self.param_OpkrAccelProfile and CS.acc_active and self.SC != None if not run_speed_ctrl: str_log2 = 'LKAS={:.0f} steer={:5.0f} '.format( CS.lkas_button_on, CS.out.steeringTorque) trace1.printf2('{}'.format(str_log2)) if pcm_cancel_cmd and self.CP.longcontrolEnabled: can_sends.append( create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL)) elif CS.out.cruiseState.standstill: # run only first time when the car stopped if self.last_lead_distance == 0 or not self.param_OpkrAutoResume: # get the lead distance from the Radar self.last_lead_distance = CS.lead_distance self.resume_cnt = 0 # when lead car starts moving, create 6 RES msgs elif CS.lead_distance != self.last_lead_distance and ( frame - self.last_resume_frame) > 5: can_sends.append( create_clu11(self.packer, self.resume_cnt, CS.clu11, Buttons.RES_ACCEL)) self.resume_cnt += 1 # interval after 6 msgs if self.resume_cnt > 5: self.last_resume_frame = frame self.resume_cnt = 0 # reset lead distnce after the car starts moving elif self.last_lead_distance != 0: self.last_lead_distance = 0 elif run_speed_ctrl and self.SC != None: is_sc_run = self.SC.update(CS, sm, self) if is_sc_run: can_sends.append( create_clu11(self.packer, self.resume_cnt, CS.clu11, self.SC.btn_type, self.SC.sc_clu_speed)) self.resume_cnt += 1 else: self.resume_cnt = 0 str1 = 'run={} cruise_set_mode={} kph={:.1f}/{:.1f} DO={:.0f}/{:.0f} '.format( is_sc_run, self.SC.cruise_set_mode, self.SC.cruise_set_speed_kph, CS.VSetDis, CS.driverOverride, CS.cruise_buttons) str2 = 'btn_type={:.0f} speed={:.1f} cnt={:.0f}'.format( self.SC.btn_type, self.SC.sc_clu_speed, self.resume_cnt) str_log = str1 + str2 self.traceCC.add(str_log) # 20 Hz LFA MFA message if frame % 5 == 0 and self.car_fingerprint in [ CAR.SONATA, CAR.PALISADE ]: can_sends.append(create_lfa_mfa(self.packer, frame, enabled)) # counter inc self.lkas11_cnt += 1 return can_sends
def update(self, v_ego_kph, CS, sm, actuators, dRel, yRel, vRel, model_speed): btn_type = Buttons.NONE #lead_1 = sm['radarState'].leadOne long_wait_cmd = 500 set_speed = CS.cruise_set_speed_kph dec_step_cmd = 0 if self.long_curv_timer < 600: self.long_curv_timer += 1 # 선행 차량 거리유지 lead_wait_cmd, lead_set_speed = self.update_lead(CS, dRel, yRel, vRel) # 커브 감속. curv_wait_cmd, curv_set_speed = self.update_curv(CS, sm, model_speed) if curv_wait_cmd != 0: if lead_set_speed > curv_set_speed: dec_step_cmd = 1 set_speed = curv_set_speed long_wait_cmd = curv_wait_cmd else: set_speed = lead_set_speed long_wait_cmd = lead_wait_cmd else: set_speed = lead_set_speed long_wait_cmd = lead_wait_cmd if set_speed > CS.cruise_set_speed_kph: set_speed = CS.cruise_set_speed_kph elif set_speed < 30: set_speed = 30 # control process target_set_speed = set_speed delta = int(set_speed) - int(CS.VSetDis) if dec_step_cmd == 0 and delta < -1: if delta < -3: dec_step_cmd = 4 elif delta < -2: dec_step_cmd = 3 else: dec_step_cmd = 2 else: dec_step_cmd = 1 if self.long_curv_timer < long_wait_cmd: pass elif CS.driverOverride == 1: # 가속패달에 의한 속도 설정. if CS.cruise_set_speed_kph > CS.clu_Vanz: delta = int(CS.clu_Vanz) - int(CS.VSetDis) if delta > 1: set_speed = CS.clu_Vanz self.seq_step_debug = 97 btn_type = Buttons.SET_DECEL elif delta <= -1: set_speed = CS.VSetDis - dec_step_cmd self.seq_step_debug = 98 btn_type = Buttons.SET_DECEL self.long_curv_timer = 0 elif delta >= 1 and (model_speed > 200 or CS.clu_Vanz < 70): set_speed = CS.VSetDis + dec_step_cmd self.seq_step_debug = 99 btn_type = Buttons.RES_ACCEL self.long_curv_timer = 0 if set_speed > CS.cruise_set_speed_kph: set_speed = CS.cruise_set_speed_kph if CS.cruise_set_mode == 0: btn_type = Buttons.NONE str3 = 'SS={:03.0f} TSS={:03.0f} VSD={:03.0f} DAt={:03.0f}/{:03.0f}/{:03.0f} DG/dec={:02.0f}/{:02.0f}'.format( set_speed, target_set_speed, CS.VSetDis, CS.driverAcc_time, long_wait_cmd, self.long_curv_timer, self.seq_step_debug, dec_step_cmd) str4 = ' LD/LS={:03.0f}/{:03.0f} '.format(CS.lead_distance, CS.lead_objspd) str5 = str3 + str4 trace1.printf2(str5) return btn_type, set_speed
def update(self, active, CS, CP, long_plan, radarState): """Update longitudinal control. This updates the state machine and runs a PID loop""" # Interp control trajectory # TODO estimate car specific lag, use .5s for now if len(long_plan.speeds) == CONTROL_N: v_target = interp(DEFAULT_LONG_LAG, T_IDXS[:CONTROL_N], long_plan.speeds) v_target_future = long_plan.speeds[-1] a_target = interp(DEFAULT_LONG_LAG, T_IDXS[:CONTROL_N], long_plan.accels) else: v_target = 0.0 v_target_future = 0.0 a_target = 0.0 # Actuation limits gas_max = interp(CS.vEgo, CP.gasMaxBP, CP.gasMaxV) brake_max = interp(CS.vEgo, CP.brakeMaxBP, CP.brakeMaxV) # Update state machine output_gb = self.last_output_gb if radarState is None: dRel = 200 vRel = 0 else: dRel = radarState.leadOne.dRel vRel = radarState.leadOne.vRel if long_plan.hasLead: stop = True if (dRel < 4.0 and radarState.leadOne.status) else False else: stop = False self.long_control_state = long_control_state_trans(active, self.long_control_state, CS.vEgo, v_target_future, self.v_pid, output_gb, CS.brakePressed, CS.cruiseState.standstill, stop, CS.gasPressed, CP.minSpeedCan) v_ego_pid = max(CS.vEgo, CP.minSpeedCan) # Without this we get jumps, CAN bus reports 0 when speed < 0.3 if (self.long_control_state == LongCtrlState.off or (CS.brakePressed or CS.gasPressed)) and self.candidate not in [CAR.NIRO_EV]: self.v_pid = v_ego_pid self.pid.reset() output_gb = 0. elif self.long_control_state == LongCtrlState.off or CS.gasPressed: self.reset(v_ego_pid) output_gb = 0. # tracking objects and driving elif self.long_control_state == LongCtrlState.pid: self.v_pid = v_target self.pid.pos_limit = gas_max self.pid.neg_limit = - brake_max # Toyota starts braking more when it thinks you want to stop # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7 deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV) # opkr if self.vRel_prev != vRel and vRel <= 0 and CS.vEgo > 13. and self.damping_timer <= 0: # decel mitigation for a while if (vRel - self.vRel_prev)*3.6 < -4: self.damping_timer = 45 self.decel_damping2 = interp(abs((vRel - self.vRel_prev)*3.6), [0, 10], [1, 0.1]) self.vRel_prev = vRel elif self.damping_timer > 0: self.damping_timer -= 1 self.decel_damping = interp(self.damping_timer, [0, 45], [1, self.decel_damping2]) output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot) output_gb *= self.decel_damping if prevent_overshoot or CS.brakeHold: output_gb = min(output_gb, 0.0) # Intention is to stop, switch to a different brake control until we stop elif self.long_control_state == LongCtrlState.stopping: # Keep applying brakes until the car is stopped factor = 1 if long_plan.hasLead: factor = interp(dRel,[2.0,4.0,5.0,6.0,7.0,8.0], [2.0,1.0,0.7,0.5,0.3,0.0]) if not CS.standstill or output_gb > -BRAKE_STOPPING_TARGET: output_gb -= CP.stoppingBrakeRate / RATE * factor elif CS.cruiseState.standstill and output_gb < -BRAKE_STOPPING_TARGET: output_gb += CP.stoppingBrakeRate / RATE output_gb = clip(output_gb, -brake_max, gas_max) self.reset(CS.vEgo) # Intention is to move again, release brake fast before handing control to PID elif self.long_control_state == LongCtrlState.starting: factor = 1 if long_plan.hasLead: factor = interp(dRel,[0.0,2.0,3.0,4.0,5.0], [0.0,0.5,1,250.0,500.0]) if output_gb < -0.2: output_gb += CP.startingBrakeRate / RATE * factor self.reset(CS.vEgo) self.last_output_gb = output_gb final_gas = clip(output_gb, 0., gas_max) final_brake = -clip(output_gb, -brake_max, 0.) if self.long_control_state == LongCtrlState.stopping: self.long_stat = "STP" elif self.long_control_state == LongCtrlState.starting: self.long_stat = "STR" elif self.long_control_state == LongCtrlState.pid: self.long_stat = "PID" elif self.long_control_state == LongCtrlState.off: self.long_stat = "OFF" else: self.long_stat = "---" if long_plan.longitudinalPlanSource == LongitudinalPlanSource.cruise: self.long_plan_source = "cruise" elif long_plan.longitudinalPlanSource == LongitudinalPlanSource.lead0: self.long_plan_source = "lead0" elif long_plan.longitudinalPlanSource == LongitudinalPlanSource.lead1: self.long_plan_source = "lead1" elif long_plan.longitudinalPlanSource == LongitudinalPlanSource.lead2: self.long_plan_source = "lead2" elif long_plan.longitudinalPlanSource == LongitudinalPlanSource.e2e: self.long_plan_source = "e2e" else: self.long_plan_source = "---" if CP.sccBus != 0 and self.long_log: str_log3 = 'BUS={:1.0f}/{:1.0f} LS={:s} LP={:s} GS={:01.2f}/{:01.2f} BK={:01.2f}/{:01.2f} GB={:+04.2f} G={:1.0f} GS={} TG={:04.2f}/{:+04.2f}'.format(CP.mdpsBus, CP.sccBus, self.long_stat, self.long_plan_source, final_gas, gas_max, abs(final_brake), abs(brake_max), output_gb, CS.cruiseGapSet, int(CS.gasPressed), v_target, a_target) trace1.printf2('{}'.format(str_log3)) return final_gas, final_brake, v_target, a_target
def update(self, c, CS, frame, sm): enabled = c.enabled actuators = c.actuators pcm_cancel_cmd = c.cruiseControl.cancel abs_angle_steers = abs(actuators.steerAngle) self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo) # Steering Torque path_plan = sm['pathPlan'] param = self.steerParams_torque(CS, abs_angle_steers, path_plan) new_steer = actuators.steer * param.STEER_MAX apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, param) self.steer_rate_limited = new_steer != apply_steer apply_steer_limit = param.STEER_MAX if self.steer_torque_ratio < 1: apply_steer_limit = int(self.steer_torque_ratio * param.STEER_MAX) apply_steer = self.limit_ctrl(apply_steer, apply_steer_limit, 0) # disable if steer angle reach 90 deg, otherwise mdps fault in some models lkas_active = enabled and abs(CS.out.steeringAngle) < 120. # fix for Genesis hard fault at low speed if CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS: lkas_active = False if not lkas_active: apply_steer = 0 steer_req = 1 if apply_steer else 0 self.apply_steer_last = apply_steer sys_warning, sys_state = self.process_hud_alert(lkas_active, c) if frame == 0: # initialize counts from last received count signals self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"] self.lkas11_cnt = (self.lkas11_cnt + 1) % 0x10 can_sends = [] can_sends.append( create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint, apply_steer, steer_req, CS.lkas11, sys_warning, sys_state, c)) can_sends.append(create_mdps12(self.packer, frame, CS.mdps12)) str_log1 = 'torg:{:>4.0f}/{:>4.0f} CV:{:>5.0f}'.format( apply_steer, new_steer, self.model_speed) str_log2 = 'max={:>4.0f} tm={:>5.1f} '.format(apply_steer_limit, self.timer1.sampleTime()) trace1.printf('{} {}'.format(str_log1, str_log2)) lfa_usm = CS.lfahda["LFA_USM"] lfa_warn = CS.lfahda["LFA_SysWarning"] lfa_active = CS.lfahda["ACTIVE2"] hda_usm = CS.lfahda["HDA_USM"] hda_active = CS.lfahda["ACTIVE"] str_log1 = 'hda={:.0f},{:.0f}'.format(hda_usm, hda_active) str_log2 = 'lfa={:.0f},{:.0f},{:.0f}'.format(lfa_usm, lfa_warn, lfa_active) trace1.printf2('{} {}'.format(str_log1, str_log2)) if pcm_cancel_cmd and self.CP.openpilotLongitudinalControl: can_sends.append( create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL)) elif CS.out.cruiseState.standstill: # run only first time when the car stopped if self.last_lead_distance == 0: # get the lead distance from the Radar self.last_lead_distance = CS.lead_distance self.resume_cnt = 0 # when lead car starts moving, create 6 RES msgs elif CS.lead_distance != self.last_lead_distance and ( frame - self.last_resume_frame) > 5: can_sends.append( create_clu11(self.packer, self.resume_cnt, CS.clu11, Buttons.RES_ACCEL)) self.resume_cnt += 1 # interval after 6 msgs if self.resume_cnt > 5: self.last_resume_frame = frame # reset lead distnce after the car starts moving elif self.last_lead_distance != 0: self.last_lead_distance = 0 # 20 Hz LFA MFA message if frame % 5 == 0 and self.car_fingerprint in [ CAR.SONATA, CAR.PALISADE, CAR.IONIQ ]: can_sends.append(create_lfa_mfa(self.packer, frame, enabled)) return can_sends
def update(self, active, CS, v_target, v_target_future, a_target_raw, a_target, CP, hasLead, radarState, longitudinalPlanSource, extras): """Update longitudinal control. This updates the state machine and runs a PID loop""" # Actuation limits gas_max = interp(CS.vEgo, CP.gasMaxBP, CP.gasMaxV) brake_max = interp(CS.vEgo, CP.brakeMaxBP, CP.brakeMaxV) # Update state machine output_gb = self.last_output_gb if radarState is None: dRel = 200 vRel = 0 else: dRel = radarState.leadOne.dRel vRel = radarState.leadOne.vRel if hasLead: stop = True if (dRel < 4.0 and radarState.leadOne.status) else False else: stop = False self.long_control_state = long_control_state_trans( active, self.long_control_state, CS.vEgo, v_target_future, self.v_pid, output_gb, CS.brakePressed, CS.cruiseState.standstill, stop, CS.gasPressed, CP.minSpeedCan) v_ego_pid = max( CS.vEgo, CP.minSpeedCan ) # Without this we get jumps, CAN bus reports 0 when speed < 0.3 if self.long_control_state == LongCtrlState.off or (CS.brakePressed or CS.gasPressed): self.v_pid = v_ego_pid self.pid.reset() output_gb = 0. # tracking objects and driving elif self.long_control_state == LongCtrlState.pid: self.v_pid = v_target self.pid.pos_limit = gas_max self.pid.neg_limit = -brake_max afactor = 1 vfactor = 1 dfactor = 1 dvfactor = 1 # Toyota starts braking more when it thinks you want to stop # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7 deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV) output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot) # added by opkr afactor = interp(CS.vEgo, [0, 1, 2, 3, 4, 8, 12, 16, 20], [4.5, 4.2, 3.65, 3.375, 3.1, 2.3, 2.1, 2, 2]) vfactor = interp(dRel, [1, 30, 50], [15, 7, 4]) dfactor = interp(dRel, [4, 10], [1.6, 1]) dvfactor = interp(((CS.vEgo * 3.6) / (max(3, dRel))), [1, 2, 3], [1, 3, 5]) # if abs(output_gb) < abs(a_target_raw)/afactor and a_target_raw < 0 and dRel >= 4.2: # output_gb = (-abs(a_target_raw)/afactor)*dfactor if output_gb > 0 and a_target_raw < 0 and dRel >= 4.2: output_gb = output_gb / vfactor elif output_gb > 0 and a_target_raw > 0 and dRel >= 4.2 and ( CS.vEgo * 3.6) < 65: output_gb = output_gb / dvfactor if prevent_overshoot or CS.brakeHold: output_gb = min(output_gb, 0.0) # Intention is to stop, switch to a different brake control until we stop elif self.long_control_state == LongCtrlState.stopping: # Keep applying brakes until the car is stopped factor = 1 if hasLead: factor = interp(dRel, [2.0, 4.2, 5.0, 6.0, 7.0, 8.0], [2.5, 1, 0.7, 0.5, 0.3, 0.0]) if not CS.standstill or output_gb > -BRAKE_STOPPING_TARGET: output_gb -= CP.stoppingBrakeRate / RATE * factor elif CS.cruiseState.standstill and output_gb < -BRAKE_STOPPING_TARGET: output_gb += CP.stoppingBrakeRate / RATE output_gb = clip(output_gb, -brake_max, gas_max) self.reset(CS.vEgo) # Intention is to move again, release brake fast before handing control to PID elif self.long_control_state == LongCtrlState.starting: factor = 1 if hasLead: factor = interp(dRel, [0.0, 2.0, 3.0, 4.2, 5.0], [0.0, 0.5, 1, 500.0, 1500.0]) if output_gb < -0.2: output_gb += CP.startingBrakeRate / RATE * factor self.reset(CS.vEgo) self.last_output_gb = output_gb final_gas = clip(output_gb, 0., gas_max) final_brake = -clip(output_gb, -brake_max, 0.) if self.long_control_state == LongCtrlState.stopping: self.long_stat = "STP" elif self.long_control_state == LongCtrlState.starting: self.long_stat = "STR" elif self.long_control_state == LongCtrlState.pid: self.long_stat = "PID" elif self.long_control_state == LongCtrlState.off: self.long_stat = "OFF" else: self.long_stat = "---" if Params().get_bool("OpenpilotLongitudinalControl") and Params( ).get_bool("LongLogDisplay"): str_log3 = 'MDPS={:1.0f} SCC={:1.0f} LS={:s} GS={:01.2f}/{:01.2f} BK={:01.2f}/{:01.2f} GB={:+04.2f} TG={:+04.2f} G={:1.0f} GS={}'.format( CP.mdpsBus, CP.sccBus, self.long_stat, final_gas, gas_max, abs(final_brake), abs(brake_max), output_gb, a_target_raw, CS.cruiseGapSet, CS.gasPressed) trace1.printf2('{}'.format(str_log3)) return final_gas, final_brake
def update(self, CC, CS, frame, sm, CP): if self.CP != CP: self.CP = CP enabled = CC.enabled actuators = CC.actuators pcm_cancel_cmd = CC.cruiseControl.cancel self.dRel, self.yRel, self.vRel = SpdController.get_lead(sm) self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo) # Steering Torque new_steer = actuators.steer * SteerLimitParams.STEER_MAX apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, SteerLimitParams) self.steer_rate_limited = new_steer != apply_steer # disable if steer angle reach 90 deg, otherwise mdps fault in some models lkas_active = enabled and abs(CS.out.steeringAngle) < 90. if ((CS.out.leftBlinker and not CS.out.rightBlinker) or (CS.out.rightBlinker and not CS.out.leftBlinker)) and CS.out.vEgo <= 59 * CV.KPH_TO_MS: self.lanechange_manual_timer = 10 if CS.out.leftBlinker and CS.out.rightBlinker: self.emergency_manual_timer = 10 if abs(CS.out.steeringTorque) > 200: self.driver_steering_torque_above_timer = 15 if self.lanechange_manual_timer or self.driver_steering_torque_above_timer: lkas_active = 0 if self.lanechange_manual_timer > 0: self.lanechange_manual_timer -= 1 if self.emergency_manual_timer > 0: self.emergency_manual_timer -= 1 if self.driver_steering_torque_above_timer > 0: self.driver_steering_torque_above_timer -= 1 if not lkas_active: apply_steer = 0 steer_req = 1 if apply_steer else 0 self.apply_steer_last = apply_steer sys_warning, sys_state = self.process_hud_alert(lkas_active, CC) clu11_speed = CS.clu11["CF_Clu_Vanz"] enabled_speed = 38 if CS.is_set_speed_in_mph else 55 if clu11_speed > enabled_speed: enabled_speed = clu11_speed can_sends = [] if frame == 0: # initialize counts from last received count signals self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"] + 1 self.lkas11_cnt %= 0x10 can_sends.append( create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint, apply_steer, steer_req, CS.lkas11, sys_warning, sys_state, CC, enabled, 0)) if CS.mdps_bus or CS.scc_bus == 1: # send lkas11 bus 1 if mdps is on bus 1 can_sends.append( create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint, apply_steer, steer_req, CS.lkas11, sys_warning, sys_state, CC, enabled, 1)) if CS.mdps_bus: # send clu11 to mdps if it is not on bus 0 #if frame % 2 and CS.mdps_bus == 1: # send clu11 to mdps if it is not on bus 0 can_sends.append( create_clu11(self.packer, frame, CS.mdps_bus, CS.clu11, Buttons.NONE, enabled_speed)) #if CS.mdps_bus: can_sends.append(create_mdps12(self.packer, frame, CS.mdps12)) str_log1 = '곡률={:05.1f}'.format(self.model_speed) str_log2 = '프레임율={:03.0f} TPMS=FL:{:04.1f}/FR:{:04.1f}/RL:{:04.1f}/RR:{:04.1f}'.format( self.timer1.sampleTime(), CS.tpmsPressureFl, CS.tpmsPressureFr, CS.tpmsPressureRl, CS.tpmsPressureRr) trace1.printf('{} {}'.format(str_log1, str_log2)) if CS.out.cruiseState.modeSel == 0 and self.mode_change_switch == 4: self.mode_change_timer = 50 self.mode_change_switch = 0 elif CS.out.cruiseState.modeSel == 1 and self.mode_change_switch == 0: self.mode_change_timer = 50 self.mode_change_switch = 1 elif CS.out.cruiseState.modeSel == 2 and self.mode_change_switch == 1: self.mode_change_timer = 50 self.mode_change_switch = 2 elif CS.out.cruiseState.modeSel == 3 and self.mode_change_switch == 2: self.mode_change_timer = 50 self.mode_change_switch = 3 elif CS.out.cruiseState.modeSel == 4 and self.mode_change_switch == 3: self.mode_change_timer = 50 self.mode_change_switch = 4 if self.mode_change_timer > 0: self.mode_change_timer -= 1 run_speed_ctrl = self.param_OpkrAccelProfile and CS.acc_active and self.SC != None and ( CS.out.cruiseState.modeSel == 1 or CS.out.cruiseState.modeSel == 2 or CS.out.cruiseState.modeSel == 3) if not run_speed_ctrl: if CS.out.cruiseState.modeSel == 0: self.steer_mode = "오파모드" elif CS.out.cruiseState.modeSel == 1: self.steer_mode = "차간+커브" elif CS.out.cruiseState.modeSel == 2: self.steer_mode = "차간ONLY" elif CS.out.cruiseState.modeSel == 3: self.steer_mode = "정체구간" elif CS.out.cruiseState.modeSel == 4: self.steer_mode = "순정모드" if CS.out.steerWarning == 0: self.mdps_status = "정상" elif CS.out.steerWarning == 1: self.mdps_status = "오류" if CS.lkas_button_on == 0: self.lkas_switch = "OFF" elif CS.lkas_button_on == 1: self.lkas_switch = "ON" else: self.lkas_switch = "-" if CS.out.cruiseState.modeSel == 3: str_log2 = '주행모드={:s} MDPS상태={:s} LKAS버튼={:s} CG:{:1.0f}'.format( self.steer_mode, self.mdps_status, self.lkas_switch, CS.cruiseGapSet) else: str_log2 = '주행모드={:s} MDPS상태={:s} LKAS버튼={:s}'.format( self.steer_mode, self.mdps_status, self.lkas_switch) trace1.printf2('{}'.format(str_log2)) #print( 'st={} cmd={} long={} steer={} req={}'.format(CS.out.cruiseState.standstill, pcm_cancel_cmd, self.CP.openpilotLongitudinalControl, apply_steer, steer_req ) ) if pcm_cancel_cmd and self.CP.openpilotLongitudinalControl: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed)) elif CS.out.cruiseState.standstill: # run only first time when the car stopped if self.last_lead_distance == 0 or not self.param_OpkrAutoResume: # get the lead distance from the Radar self.last_lead_distance = CS.lead_distance self.resume_cnt = 0 # when lead car starts moving, create 6 RES msgs elif CS.lead_distance != self.last_lead_distance and ( frame - self.last_resume_frame) > 5: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed)) self.resume_cnt += 1 # interval after 6 msgs if self.resume_cnt > 5: self.last_resume_frame = frame self.resume_cnt = 0 elif self.cruise_gap_prev == 0: self.cruise_gap_prev = CS.cruiseGapSet self.cruise_gap_set_init = 1 elif CS.cruiseGapSet != 1.0: self.cruise_gap_switch_timer += 1 if self.cruise_gap_switch_timer > 100: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.GAP_DIST, clu11_speed)) self.cruise_gap_switch_timer = 0 # reset lead distnce after the car starts moving elif self.last_lead_distance != 0: self.last_lead_distance = 0 self.cruise_gap = 0 elif run_speed_ctrl and self.SC != None: is_sc_run = self.SC.update(CS, sm, self) if is_sc_run: can_sends.append( create_clu11(self.packer, self.resume_cnt, CS.scc_bus, CS.clu11, self.SC.btn_type, self.SC.sc_clu_speed)) self.resume_cnt += 1 else: self.resume_cnt = 0 if self.dRel > 17 and self.cruise_gap_prev != CS.cruiseGapSet and self.cruise_gap_set_init == 1 and CS.out.cruiseState.modeSel != 3: self.cruise_gap_switch_timer += 1 if self.cruise_gap_switch_timer > 50: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.GAP_DIST, clu11_speed)) self.cruise_gap_switch_timer = 0 elif self.cruise_gap_prev == CS.cruiseGapSet: self.cruise_gap_set_init = 0 self.cruise_gap_prev = 0 if CS.out.cruiseState.modeSel == 3 and CS.acc_active: if 20 > self.dRel > 18 and self.vRel < 0 and CS.cruiseGapSet != 4.0: self.cruise_gap_switch_timer += 1 if self.cruise_gap_switch_timer > 30: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.GAP_DIST, clu11_speed)) self.cruise_gap_switch_timer = 0 elif 16 > self.dRel > 14 and self.vRel < 0 and CS.cruiseGapSet != 3.0: self.cruise_gap_switch_timer += 1 if self.cruise_gap_switch_timer > 30: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.GAP_DIST, clu11_speed)) self.cruise_gap_switch_timer = 0 elif 12 > self.dRel > 10 and self.vRel < 0 and CS.cruiseGapSet != 2.0: self.cruise_gap_switch_timer += 1 if self.cruise_gap_switch_timer > 30: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.GAP_DIST, clu11_speed)) self.cruise_gap_switch_timer = 0 elif 9 > self.dRel > 7 and self.vRel < 0 and CS.cruiseGapSet != 1.0: self.cruise_gap_switch_timer += 1 if self.cruise_gap_switch_timer > 30: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.GAP_DIST, clu11_speed)) self.cruise_gap_switch_timer = 0 elif 15 > self.dRel > 4 and self.vRel > 0 and CS.cruiseGapSet != 1.0: self.cruise_gap_switch_timer += 1 if self.cruise_gap_switch_timer > 30: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.GAP_DIST, clu11_speed)) self.cruise_gap_switch_timer = 0 elif 25 > self.dRel > 18 and self.vRel >= 0 and CS.cruiseGapSet != 2.0: self.cruise_gap_switch_timer += 1 if self.cruise_gap_switch_timer > 30: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.GAP_DIST, clu11_speed)) self.cruise_gap_switch_timer = 0 # 20 Hz LFA MFA message #if frame % 5 == 0 and self.car_fingerprint in [CAR.IONIQ]: # can_sends.append(create_lfa_mfa(self.packer, frame, enabled)) self.lkas11_cnt += 1 return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_lane, right_lane, left_lane_depart, right_lane_depart, set_speed, lead_visible, lead_dist, lead_vrel, lead_yrel, sm): # *** compute control surfaces *** # gas and brake self.accel_lim_prev = self.accel_lim apply_accel = actuators.gas - actuators.brake apply_accel, self.accel_steady = accel_hysteresis( apply_accel, self.accel_steady) apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) self.accel_lim = apply_accel apply_accel = accel_rate_limit(self.accel_lim, self.accel_lim_prev) param = self.p #self.model_speed = 255 - self.SC.calc_va(sm, CS.out.vEgo) #atom model_speed #self.model_speed = self.SC.cal_model_speed(sm, CS.out.vEgo) if frame % 10 == 0: self.curve_speed = self.SC.cal_curve_speed(sm, CS.out.vEgo) plan = sm['longitudinalPlan'] self.dRel = int(plan.dRel1) #EON Lead self.yRel = int(plan.yRel1) #EON Lead self.vRel = int(plan.vRel1 * 3.6 + 0.5) #EON Lead self.dRel2 = int(plan.dRel2) #EON Lead self.yRel2 = int(plan.yRel2) #EON Lead self.vRel2 = int(plan.vRel2 * 3.6 + 0.5) #EON Lead self.lead2_status = plan.status2 lateral_plan = sm['lateralPlan'] self.outScale = lateral_plan.outputScale self.vCruiseSet = lateral_plan.vCruiseSet #self.model_speed = interp(abs(lateral_plan.vCurvature), [0.0002, 0.01], [255, 30]) #Hoya self.model_speed = interp(abs(lateral_plan.vCurvature), [0.0, 0.0002, 0.00074, 0.0025, 0.008, 0.02], [255, 255, 130, 90, 60, 20]) if CS.out.vEgo > 8: if self.variable_steer_max: self.steerMax = interp(int(abs(self.model_speed)), self.model_speed_range, self.steerMax_range) else: self.steerMax = self.steerMax_base if self.variable_steer_delta: self.steerDeltaUp = interp(int(abs(self.model_speed)), self.model_speed_range, self.steerDeltaUp_range) self.steerDeltaDown = interp(int(abs(self.model_speed)), self.model_speed_range, self.steerDeltaDown_range) else: self.steerDeltaUp = self.steerDeltaUp_base self.steerDeltaDown = self.steerDeltaDown_base else: self.steerMax = self.steerMax_base self.steerDeltaUp = self.steerDeltaUp_base self.steerDeltaDown = self.steerDeltaDown_base param.STEER_MAX = min(CarControllerParams.STEER_MAX, self.steerMax) # variable steermax param.STEER_DELTA_UP = min(CarControllerParams.STEER_DELTA_UP, self.steerDeltaUp) # variable deltaUp param.STEER_DELTA_DOWN = min(CarControllerParams.STEER_DELTA_DOWN, self.steerDeltaDown) # variable deltaDown # Steering Torque if 0 <= self.driver_steering_torque_above_timer < 100: new_steer = int( round(actuators.steer * self.steerMax * (self.driver_steering_torque_above_timer / 100))) else: new_steer = int(round(actuators.steer * self.steerMax)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, param) self.steer_rate_limited = new_steer != apply_steer # disable if steer angle reach 90 deg, otherwise mdps fault in some models if self.opkr_maxanglelimit >= 90 and not self.steer_wind_down_enabled: lkas_active = enabled and abs( CS.out.steeringAngleDeg ) < self.opkr_maxanglelimit and CS.out.gearShifter == GearShifter.drive else: lkas_active = enabled and not CS.out.steerWarning and CS.out.gearShifter == GearShifter.drive if ( (CS.out.leftBlinker and not CS.out.rightBlinker) or (CS.out.rightBlinker and not CS.out.leftBlinker) ) and CS.out.vEgo < LANE_CHANGE_SPEED_MIN and self.opkr_turnsteeringdisable: self.lanechange_manual_timer = 50 if CS.out.leftBlinker and CS.out.rightBlinker: self.emergency_manual_timer = 50 if self.lanechange_manual_timer: lkas_active = 0 if self.lanechange_manual_timer > 0: self.lanechange_manual_timer -= 1 if self.emergency_manual_timer > 0: self.emergency_manual_timer -= 1 if abs(CS.out.steeringTorque ) > 180 and CS.out.vEgo < LANE_CHANGE_SPEED_MIN: self.driver_steering_torque_above = True else: self.driver_steering_torque_above = False if self.driver_steering_torque_above == True: self.driver_steering_torque_above_timer -= 1 if self.driver_steering_torque_above_timer <= 0: self.driver_steering_torque_above_timer = 0 elif self.driver_steering_torque_above == False: self.driver_steering_torque_above_timer += 5 if self.driver_steering_torque_above_timer >= 100: self.driver_steering_torque_above_timer = 100 if not lkas_active: apply_steer = 0 if self.apply_steer_last != 0: self.steer_wind_down = 1 if lkas_active or CS.out.steeringPressed: self.steer_wind_down = 0 self.apply_accel_last = apply_accel self.apply_steer_last = apply_steer if CS.acc_active and CS.lead_distance > 149 and self.dRel < ((CS.out.vEgo * CV.MS_TO_KPH)+5) < 100 and \ self.vRel < -(CS.out.vEgo * CV.MS_TO_KPH * 0.16) and CS.out.vEgo > 7 and abs(CS.out.steeringAngleDeg) < 10 and not self.longcontrol: self.need_brake_timer += 1 if self.need_brake_timer > 50: self.need_brake = True else: self.need_brake = False self.need_brake_timer = 0 sys_warning, sys_state, left_lane_warning, right_lane_warning =\ process_hud_alert(lkas_active, self.car_fingerprint, visual_alert, left_lane, right_lane, left_lane_depart, right_lane_depart) clu11_speed = CS.clu11["CF_Clu_Vanz"] enabled_speed = 38 if CS.is_set_speed_in_mph else 60 if clu11_speed > enabled_speed or not lkas_active or CS.out.gearShifter != GearShifter.drive: enabled_speed = clu11_speed can_sends = [] can_sends.append( create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active, self.steer_wind_down, CS.lkas11, sys_warning, sys_state, enabled, left_lane, right_lane, left_lane_warning, right_lane_warning, 0, self.ldws_fix, self.steer_wind_down_enabled)) if CS.CP.mdpsBus: # send lkas11 bus 1 if mdps is bus 1 can_sends.append( create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active, self.steer_wind_down, CS.lkas11, sys_warning, sys_state, enabled, left_lane, right_lane, left_lane_warning, right_lane_warning, 1, self.ldws_fix, self.steer_wind_down_enabled)) if frame % 2: # send clu11 to mdps if it is not on bus 0 can_sends.append( create_clu11(self.packer, frame, CS.clu11, Buttons.NONE, enabled_speed, CS.CP.mdpsBus)) if CS.out.cruiseState.modeSel == 0 and self.mode_change_switch == 4: self.mode_change_timer = 50 self.mode_change_switch = 0 elif CS.out.cruiseState.modeSel == 1 and self.mode_change_switch == 0: self.mode_change_timer = 50 self.mode_change_switch = 1 elif CS.out.cruiseState.modeSel == 2 and self.mode_change_switch == 1: self.mode_change_timer = 50 self.mode_change_switch = 2 elif CS.out.cruiseState.modeSel == 3 and self.mode_change_switch == 2: self.mode_change_timer = 50 self.mode_change_switch = 3 elif CS.out.cruiseState.modeSel == 4 and self.mode_change_switch == 3: self.mode_change_timer = 50 self.mode_change_switch = 4 if self.mode_change_timer > 0: self.mode_change_timer -= 1 run_speed_ctrl = self.opkr_variablecruise and CS.acc_active and ( CS.out.cruiseState.modeSel > 0) if not run_speed_ctrl: if CS.out.cruiseState.modeSel == 0: self.steer_mode = "오파모드" elif CS.out.cruiseState.modeSel == 1: self.steer_mode = "차간+커브" elif CS.out.cruiseState.modeSel == 2: self.steer_mode = "차간ONLY" elif CS.out.cruiseState.modeSel == 3: self.steer_mode = "편도1차선" elif CS.out.cruiseState.modeSel == 4: self.steer_mode = "맵감속ONLY" if CS.out.steerWarning == 0: self.mdps_status = "정상" elif CS.out.steerWarning == 1: self.mdps_status = "오류" if CS.lkas_button_on == 0: self.lkas_switch = "OFF" elif CS.lkas_button_on == 1: self.lkas_switch = "ON" else: self.lkas_switch = "-" if self.cruise_gap != CS.cruiseGapSet: self.cruise_gap = CS.cruiseGapSet if CS.lead_distance < 149: self.leadcar_status = "O" else: self.leadcar_status = "-" str_log2 = 'MODE={:s} MDPS={:s} LKAS={:s} CSG={:1.0f} LEAD={:s} FR={:03.0f}'.format(self.steer_mode, \ self.mdps_status, self.lkas_switch, self.cruise_gap, self.leadcar_status, self.timer1.sampleTime()) trace1.printf2('{}'.format(str_log2)) if pcm_cancel_cmd and self.longcontrol: can_sends.append( create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL, clu11_speed, CS.CP.sccBus)) if CS.out.cruiseState.standstill: self.standstill_status = 1 if self.opkr_autoresume: # run only first time when the car stopped if self.last_lead_distance == 0: # get the lead distance from the Radar self.last_lead_distance = CS.lead_distance self.resume_cnt = 0 self.res_switch_timer = 0 self.standstill_fault_reduce_timer += 1 elif self.res_switch_timer > 0: self.res_switch_timer -= 1 self.standstill_fault_reduce_timer += 1 # at least 1 sec delay after entering the standstill elif 100 < self.standstill_fault_reduce_timer and CS.lead_distance != self.last_lead_distance: self.acc_standstill_timer = 0 self.acc_standstill = False can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)) if not self.longcontrol \ else can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL, clu11_speed, CS.CP.sccBus)) self.resume_cnt += 1 if self.resume_cnt > 5: self.resume_cnt = 0 self.res_switch_timer = randint(10, 15) self.standstill_fault_reduce_timer += 1 # gap save elif 160 < self.standstill_fault_reduce_timer and self.cruise_gap_prev == 0 and self.opkr_autoresume and self.opkr_cruisegap_auto_adj: self.cruise_gap_prev = CS.cruiseGapSet self.cruise_gap_set_init = 1 # gap adjust to 1 for fast start elif 160 < self.standstill_fault_reduce_timer and CS.cruiseGapSet != 1.0 and self.opkr_autoresume and self.opkr_cruisegap_auto_adj: self.cruise_gap_switch_timer += 1 if self.cruise_gap_switch_timer > 100: can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.GAP_DIST)) if not self.longcontrol \ else can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.GAP_DIST, clu11_speed, CS.CP.sccBus)) self.cruise_gap_switch_timer = 0 elif self.opkr_autoresume: self.standstill_fault_reduce_timer += 1 # reset lead distnce after the car starts moving elif self.last_lead_distance != 0: self.last_lead_distance = 0 elif run_speed_ctrl: is_sc_run = self.SC.update(CS, sm, self) if is_sc_run: can_sends.append(create_clu11(self.packer, self.resume_cnt, CS.clu11, self.SC.btn_type, self.SC.sc_clu_speed)) if not self.longcontrol \ else can_sends.append(create_clu11(self.packer, self.resume_cnt, CS.clu11, self.SC.btn_type, self.SC.sc_clu_speed, CS.CP.sccBus)) self.resume_cnt += 1 else: self.resume_cnt = 0 if self.opkr_cruisegap_auto_adj: # gap restore if self.dRel > 17 and self.vRel < 5 and self.cruise_gap_prev != CS.cruiseGapSet and self.cruise_gap_set_init == 1 and self.opkr_autoresume: self.cruise_gap_switch_timer += 1 if self.cruise_gap_switch_timer > 50: can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.GAP_DIST)) if not self.longcontrol \ else can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.GAP_DIST, clu11_speed, CS.CP.sccBus)) self.cruise_gap_switch_timer = 0 elif self.cruise_gap_prev == CS.cruiseGapSet and self.opkr_autoresume: self.cruise_gap_set_init = 0 self.cruise_gap_prev = 0 if CS.cruise_buttons == 4: self.cancel_counter += 1 elif CS.acc_active: self.cancel_counter = 0 if self.res_speed_timer > 0: self.res_speed_timer -= 1 else: self.v_cruise_kph_auto_res = 0 self.res_speed = 0 if self.model_speed > 95 and self.cancel_counter == 0 and not CS.acc_active and not CS.out.brakeLights and int(CS.VSetDis) > 30 and \ (CS.lead_distance < 149 or int(CS.clu_Vanz) > 30) and int(CS.clu_Vanz) >= 3 and self.auto_res_timer <= 0 and self.opkr_cruise_auto_res: if self.opkr_cruise_auto_res_option == 0: can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)) if not self.longcontrol \ else can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL, clu11_speed, CS.CP.sccBus)) # auto res self.res_speed = int(CS.clu_Vanz * 1.1) self.res_speed_timer = 350 elif self.opkr_cruise_auto_res_option == 1: can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.SET_DECEL)) if not self.longcontrol \ else can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.SET_DECEL, clu11_speed, CS.CP.sccBus)) # auto res but set_decel to set current speed self.v_cruise_kph_auto_res = int(CS.clu_Vanz) self.res_speed_timer = 50 if self.auto_res_timer <= 0: self.auto_res_timer = randint(10, 15) elif self.auto_res_timer > 0 and self.opkr_cruise_auto_res: self.auto_res_timer -= 1 if CS.out.brakeLights and CS.out.vEgo == 0 and not CS.acc_active: self.standstill_status_timer += 1 if self.standstill_status_timer > 200: self.standstill_status = 1 self.standstill_status_timer = 0 if self.standstill_status == 1 and CS.out.vEgo > 1: self.standstill_status = 0 self.standstill_fault_reduce_timer = 0 self.last_resume_frame = frame self.res_switch_timer = 0 self.resume_cnt = 0 if CS.out.vEgo <= 1: self.sm.update(0) long_control_state = self.sm['controlsState'].longControlState if long_control_state == LongCtrlState.stopping and CS.out.vEgo < 0.1 and not CS.out.gasPressed: self.acc_standstill_timer += 1 if self.acc_standstill_timer >= 200: self.acc_standstill_timer = 200 self.acc_standstill = True else: self.acc_standstill_timer = 0 self.acc_standstill = False elif CS.out.gasPressed or CS.out.vEgo > 1: self.acc_standstill = False self.acc_standstill_timer = 0 else: self.acc_standstill = False self.acc_standstill_timer = 0 if CS.CP.mdpsBus: # send mdps12 to LKAS to prevent LKAS error can_sends.append(create_mdps12(self.packer, frame, CS.mdps12)) if CS.CP.sccBus != 0 and self.counter_init and self.longcontrol: if frame % 2 == 0: self.scc12cnt += 1 self.scc12cnt %= 0xF self.scc11cnt += 1 self.scc11cnt %= 0x10 self.fca11supcnt += 1 self.fca11supcnt %= 0xF if self.fca11alivecnt == 1: self.fca11inc = 0 if self.fca11cnt13 == 3: self.fca11maxcnt = 0x9 self.fca11cnt13 = 0 else: self.fca11maxcnt = 0xD self.fca11cnt13 += 1 else: self.fca11inc += 4 self.fca11alivecnt = self.fca11maxcnt - self.fca11inc lead_objspd = CS.lead_objspd # vRel (km/h) aReqValue = CS.scc12["aReqValue"] if 0 < CS.out.radarDistance <= 149: if aReqValue > 0.: stock_weight = interp(CS.out.radarDistance, [3., 25.], [0.8, 0.]) elif aReqValue < 0.: stock_weight = interp(CS.out.radarDistance, [3., 25.], [1., 0.]) if lead_objspd < 0: vRel_weight = interp(abs(lead_objspd), [0, 25], [1, 2]) stock_weight = interp( CS.out.radarDistance, [3.**vRel_weight, 25. * vRel_weight], [1., 0.]) else: stock_weight = 0. apply_accel = apply_accel * ( 1. - stock_weight) + aReqValue * stock_weight else: stock_weight = 0. can_sends.append( create_scc11(self.packer, frame, set_speed, lead_visible, self.scc_live, lead_dist, lead_vrel, lead_yrel, self.car_fingerprint, CS.out.vEgo * CV.MS_TO_KPH, self.acc_standstill, CS.scc11)) if (CS.brake_check or CS.cancel_check) and self.car_fingerprint not in [ CAR.NIRO_EV ]: can_sends.append( create_scc12(self.packer, apply_accel, enabled, self.scc_live, CS.out.gasPressed, 1, CS.out.stockAeb, self.car_fingerprint, CS.out.vEgo * CV.MS_TO_KPH, CS.scc12)) else: can_sends.append( create_scc12(self.packer, apply_accel, enabled, self.scc_live, CS.out.gasPressed, CS.out.brakePressed, CS.out.stockAeb, self.car_fingerprint, CS.out.vEgo * CV.MS_TO_KPH, CS.scc12)) can_sends.append( create_scc14(self.packer, enabled, CS.scc14, CS.out.stockAeb, lead_visible, lead_dist, CS.out.vEgo, self.acc_standstill, self.car_fingerprint)) if CS.CP.fcaBus == -1: can_sends.append( create_fca11(self.packer, CS.fca11, self.fca11alivecnt, self.fca11supcnt)) if frame % 20 == 0: can_sends.append(create_scc13(self.packer, CS.scc13)) if CS.CP.fcaBus == -1: can_sends.append(create_fca12(self.packer)) if frame % 50 == 0: can_sends.append(create_scc42a(self.packer)) elif CS.CP.sccBus == 2 and self.longcontrol: self.counter_init = True self.scc12cnt = CS.scc12init["CR_VSM_Alive"] self.scc11cnt = CS.scc11init["AliveCounterACC"] self.fca11alivecnt = CS.fca11init["CR_FCA_Alive"] self.fca11supcnt = CS.fca11init["Supplemental_Counter"] aq_value = CS.scc12["aReqValue"] if CS.CP.sccBus == 0 else apply_accel if self.apks_enabled: str_log1 = 'M/C={:03.0f}/{:03.0f} TQ={:03.0f} ST={:03.0f}/{:01.0f}/{:01.0f} AQ={:+04.2f}'.format(abs(self.model_speed), self.curve_speed, \ abs(new_steer), max(self.steerMax, abs(new_steer)), self.steerDeltaUp, self.steerDeltaDown, aq_value) else: str_log1 = 'M/C={:03.0f}/{:03.0f} TQ={:03.0f} ST={:03.0f}/{:01.0f}/{:01.0f} AQ={:+04.2f} S={:.0f}/{:.0f}'.format(abs(self.model_speed), self.curve_speed, \ abs(new_steer), max(self.steerMax, abs(new_steer)), self.steerDeltaUp, self.steerDeltaDown, aq_value, int(CS.is_highway), CS.safety_sign_check) trace1.printf1('{} {}'.format(str_log1, self.str_log2)) # 20 Hz LFA MFA message if frame % 5 == 0 and self.car_fingerprint in FEATURES[ "send_lfahda_mfa"]: can_sends.append(create_lfahda_mfc(self.packer, frame, lkas_active)) return can_sends
def update(self, enabled, CS, frame, CC, actuators, pcm_cancel_cmd, visual_alert, left_lane, right_lane, left_lane_depart, right_lane_depart, set_speed, lead_visible, sm): # *** compute control surfaces *** # gas and brake apply_accel = actuators.gas - actuators.brake apply_accel, self.accel_steady = accel_hysteresis( apply_accel, self.accel_steady) apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) param = self.p self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo) plan = sm['plan'] self.dRel = int(plan.dRel1) #EON Lead self.yRel = int(plan.yRel1) #EON Lead self.vRel = int(plan.vRel1 * 3.6 + 0.5) #EON Lead self.dRel2 = int(plan.dRel2) #EON Lead self.yRel2 = int(plan.yRel2) #EON Lead self.vRel2 = int(plan.vRel2 * 3.6 + 0.5) #EON Lead self.lead2_status = plan.status2 self.target_map_speed = plan.targetSpeed self.target_map_speed_camera = plan.targetSpeedCamera self.accActive = CS.acc_active path_plan = sm['pathPlan'] self.outScale = path_plan.outputScale self.vCruiseSet = path_plan.vCruiseSet self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset self.angle_steers = CS.out.steeringAngle self.angle_diff = abs(self.angle_steers_des) - abs(self.angle_steers) if abs(self.outScale) >= 1 and CS.out.vEgo > 8: self.steerMax_prev = interp(self.angle_diff, self.angle_differ_range, self.steerMax_range) if self.steerMax_prev > self.steerMax: self.steerMax = self.steerMax_prev self.steerDeltaUp_prev = interp(self.angle_diff, self.angle_differ_range, self.steerDeltaUp_range) if self.steerDeltaUp_prev > self.steerDeltaUp: self.steerDeltaUp = self.steerDeltaUp_prev self.steerDeltaDown_prev = interp(self.angle_diff, self.angle_differ_range, self.steerDeltaDown_range) if self.steerDeltaDown_prev > self.steerDeltaDown: self.steerDeltaDown = self.steerDeltaDown_prev #if abs(self.outScale) >= 0.9 and CS.out.vEgo > 8: # self.steerMax_timer += 1 # self.steerDeltaUp_timer += 1 # self.steerDeltaDown_timer += 1 # if self.steerMax_timer > 5: # self.steerMax += int(CS.out.vEgo//2) # self.steerMax_timer = 0 # if self.steerMax >= int(self.params.get('SteerMaxAdj')): # self.steerMax = int(self.params.get('SteerMaxAdj')) # if self.steerDeltaUp_timer > 50: # self.steerDeltaUp += 1 # self.steerDeltaUp_timer = 0 # if self.steerDeltaUp >= 7: # self.steerDeltaUp = 7 # if self.steerDeltaDown_timer > 25: # self.steerDeltaDown += 1 # self.steerDeltaDown_timer = 0 # if self.steerDeltaDown >= 15: # self.steerDeltaDown = 15 else: self.steerMax_timer += 1 self.steerDeltaUp_timer += 1 self.steerDeltaDown_timer += 1 if self.steerMax_timer > 20: self.steerMax -= 5 self.steerMax_timer = 0 if self.steerMax < int(self.params.get('SteerMaxBaseAdj')): self.steerMax = int(self.params.get('SteerMaxBaseAdj')) if self.steerDeltaUp_timer > 100: self.steerDeltaUp -= 1 self.steerDeltaUp_timer = 0 if self.steerDeltaUp <= int( self.params.get('SteerDeltaUpAdj')): self.steerDeltaUp = int(self.params.get('SteerDeltaUpAdj')) if self.steerDeltaDown_timer > 50: self.steerDeltaDown -= 1 self.steerDeltaDown_timer = 0 if self.steerDeltaDown <= int( self.params.get('SteerDeltaDownAdj')): self.steerDeltaDown = int( self.params.get('SteerDeltaDownAdj')) param.STEER_MAX = min(SteerLimitParams.STEER_MAX, self.steerMax) param.STEER_DELTA_UP = max(int(self.params.get('SteerDeltaUpAdj')), self.steerDeltaUp) param.STEER_DELTA_DOWN = max(int(self.params.get('SteerDeltaDownAdj')), self.steerDeltaDown) # Steering Torque if 0 <= self.driver_steering_torque_above_timer < 100: new_steer = actuators.steer * self.steerMax * ( self.driver_steering_torque_above_timer / 100) else: new_steer = actuators.steer * self.steerMax apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, param) self.steer_rate_limited = new_steer != apply_steer CC.applyAccel = apply_accel CC.applySteer = apply_steer # SPAS limit angle extremes for safety if CS.spas_enabled: apply_steer_ang_req = clip(actuators.steerAngle, -1 * (STEER_ANG_MAX), STEER_ANG_MAX) # SPAS limit angle rate for safety if abs(self.apply_steer_ang - apply_steer_ang_req) > STEER_ANG_MAX_RATE: if apply_steer_ang_req > self.apply_steer_ang: self.apply_steer_ang += STEER_ANG_MAX_RATE else: self.apply_steer_ang -= STEER_ANG_MAX_RATE else: self.apply_steer_ang = apply_steer_ang_req spas_active = CS.spas_enabled and enabled and ( self.spas_always or CS.out.vEgo < 7.0) # 25km/h # disable if steer angle reach 90 deg, otherwise mdps fault in some models if self.opkr_maxanglelimit >= 90: lkas_active = enabled and abs( CS.out.steeringAngle ) < self.opkr_maxanglelimit and not spas_active else: lkas_active = enabled and not spas_active if ((CS.out.leftBlinker and not CS.out.rightBlinker) or (CS.out.rightBlinker and not CS.out.leftBlinker)) and CS.out.vEgo < LANE_CHANGE_SPEED_MIN: self.lanechange_manual_timer = 50 if CS.out.leftBlinker and CS.out.rightBlinker: self.emergency_manual_timer = 50 if self.lanechange_manual_timer: lkas_active = 0 if self.lanechange_manual_timer > 0: self.lanechange_manual_timer -= 1 if self.emergency_manual_timer > 0: self.emergency_manual_timer -= 1 if abs(CS.out.steeringTorque ) > 200 and CS.out.vEgo < LANE_CHANGE_SPEED_MIN: self.driver_steering_torque_above = True else: self.driver_steering_torque_above = False if self.driver_steering_torque_above == True: self.driver_steering_torque_above_timer -= 1 if self.driver_steering_torque_above_timer <= 0: self.driver_steering_torque_above_timer = 0 elif self.driver_steering_torque_above == False: self.driver_steering_torque_above_timer += 5 if self.driver_steering_torque_above_timer >= 100: self.driver_steering_torque_above_timer = 100 if not lkas_active: apply_steer = 0 self.apply_accel_last = apply_accel self.apply_steer_last = apply_steer if CS.acc_active and CS.lead_distance > 149 and self.dRel < ( (CS.out.vEgo * CV.MS_TO_KPH) + 3) and self.vRel < -5 and CS.out.vEgo > 7: self.need_brake_timer += 1 if self.need_brake_timer > 50: self.need_brake = True else: self.need_brake = False self.need_brake_timer = 0 sys_warning, sys_state, left_lane_warning, right_lane_warning =\ process_hud_alert(lkas_active, self.car_fingerprint, visual_alert, left_lane, right_lane, left_lane_depart, right_lane_depart, self.lkas_button_on) clu11_speed = CS.clu11["CF_Clu_Vanz"] enabled_speed = 38 if CS.is_set_speed_in_mph else 55 if clu11_speed > enabled_speed or not lkas_active: enabled_speed = clu11_speed if not (min_set_speed < set_speed < 255 * CV.KPH_TO_MS): set_speed = min_set_speed set_speed *= CV.MS_TO_MPH if CS.is_set_speed_in_mph else CV.MS_TO_KPH if frame == 0: # initialize counts from last received count signals self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"] self.scc12_cnt = CS.scc12[ "CR_VSM_Alive"] + 1 if not CS.no_radar else 0 #TODO: fix this # self.prev_scc_cnt = CS.scc11["AliveCounterACC"] # self.scc_update_frame = frame # check if SCC is alive # if frame % 7 == 0: # if CS.scc11["AliveCounterACC"] == self.prev_scc_cnt: # if frame - self.scc_update_frame > 20 and self.scc_live: # self.scc_live = False # else: # self.scc_live = True # self.prev_scc_cnt = CS.scc11["AliveCounterACC"] # self.scc_update_frame = frame self.prev_scc_cnt = CS.scc11["AliveCounterACC"] self.lkas11_cnt = (self.lkas11_cnt + 1) % 0x10 self.scc12_cnt %= 0xF can_sends = [] can_sends.append( create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active, CS.lkas11, sys_warning, sys_state, enabled, left_lane, right_lane, left_lane_warning, right_lane_warning, 0)) if CS.mdps_bus or CS.scc_bus == 1: # send lkas11 bus 1 if mdps or scc is on bus 1 can_sends.append( create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active, CS.lkas11, sys_warning, sys_state, enabled, left_lane, right_lane, left_lane_warning, right_lane_warning, 1)) if frame % 2 and CS.mdps_bus: # send clu11 to mdps if it is not on bus 0 can_sends.append( create_clu11(self.packer, frame, CS.mdps_bus, CS.clu11, Buttons.NONE, enabled_speed)) str_log1 = 'CV={:03.0f} TQ={:03.0f} R={:03.0f} ST={:03.0f}/{:01.0f}/{:01.0f}'.format( abs(self.model_speed), abs(new_steer), self.timer1.sampleTime(), self.steerMax, self.steerDeltaUp, self.steerDeltaDown) trace1.printf1('{} {}'.format(str_log1, self.str_log2)) if CS.out.cruiseState.modeSel == 0 and self.mode_change_switch == 3: self.mode_change_timer = 50 self.mode_change_switch = 0 elif CS.out.cruiseState.modeSel == 1 and self.mode_change_switch == 0: self.mode_change_timer = 50 self.mode_change_switch = 1 elif CS.out.cruiseState.modeSel == 2 and self.mode_change_switch == 1: self.mode_change_timer = 50 self.mode_change_switch = 2 elif CS.out.cruiseState.modeSel == 3 and self.mode_change_switch == 2: self.mode_change_timer = 50 self.mode_change_switch = 3 if self.mode_change_timer > 0: self.mode_change_timer -= 1 run_speed_ctrl = self.opkr_variablecruise and CS.acc_active and ( CS.out.cruiseState.modeSel == 1 or CS.out.cruiseState.modeSel == 2 or CS.out.cruiseState.modeSel == 3) if not run_speed_ctrl: if CS.out.cruiseState.modeSel == 0: self.steer_mode = "오파모드" elif CS.out.cruiseState.modeSel == 1: self.steer_mode = "차간+커브" elif CS.out.cruiseState.modeSel == 2: self.steer_mode = "차간ONLY" elif CS.out.cruiseState.modeSel == 3: self.steer_mode = "편도1차선" if CS.out.steerWarning == 0: self.mdps_status = "정상" elif CS.out.steerWarning == 1: self.mdps_status = "오류" if CS.lkas_button_on == 0: self.lkas_switch = "OFF" elif CS.lkas_button_on == 1: self.lkas_switch = "ON" else: self.lkas_switch = "-" if self.cruise_gap != CS.cruiseGapSet: self.cruise_gap = CS.cruiseGapSet if CS.lead_distance < 149: self.leadcar_status = "O" else: self.leadcar_status = "-" str_log2 = 'MODE={:s} MDPS={:s} LKAS={:s} CSG={:1.0f} LEAD={:s}'.format( self.steer_mode, self.mdps_status, self.lkas_switch, self.cruise_gap, self.leadcar_status) trace1.printf2('{}'.format(str_log2)) if pcm_cancel_cmd and self.longcontrol: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed)) if CS.out.cruiseState.standstill: self.standstill_status = 1 if self.opkr_autoresumeoption == 1 and self.opkr_autoresume: # run only first time when the car stopped if self.last_lead_distance == 0: # get the lead distance from the Radar self.last_lead_distance = CS.lead_distance self.resume_cnt = 0 self.res_switch_timer = 0 self.standstill_fault_reduce_timer += 1 elif self.res_switch_timer > 0: self.res_switch_timer -= 1 self.standstill_fault_reduce_timer += 1 # standstill 진입하자마자 바로 누르지 말고 최소 1초의 딜레이를 주기 위함 elif 100 < self.standstill_fault_reduce_timer and CS.lead_distance != self.last_lead_distance: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed)) self.resume_cnt += 1 if self.resume_cnt > 5: self.resume_cnt = 0 self.res_switch_timer = randint(10, 15) self.standstill_fault_reduce_timer += 1 # gap save elif 160 < self.standstill_fault_reduce_timer and self.cruise_gap_prev == 0 and self.opkr_autoresume: self.cruise_gap_prev = CS.cruiseGapSet self.cruise_gap_set_init = 1 # gap adjust to 1 for fast start elif 160 < self.standstill_fault_reduce_timer and CS.cruiseGapSet != 1.0 and self.opkr_autoresume: self.cruise_gap_switch_timer += 1 if self.cruise_gap_switch_timer > 100: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.GAP_DIST, clu11_speed)) self.cruise_gap_switch_timer = 0 elif self.opkr_autoresume: self.standstill_fault_reduce_timer += 1 elif self.opkr_autoresumeoption == 0 and self.opkr_autoresume: # run only first time when the car stopped if self.last_lead_distance == 0: # get the lead distance from the Radar self.last_lead_distance = CS.lead_distance self.resume_cnt = 0 # when lead car starts moving, create 6 RES msgs elif CS.lead_distance != self.last_lead_distance and ( frame - self.last_resume_frame) > 5 and self.opkr_autoresume: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed)) self.resume_cnt += 1 # interval after 6 msgs if self.resume_cnt > 5: self.last_resume_frame = frame self.resume_cnt = 0 elif self.cruise_gap_prev == 0 and self.opkr_autoresume: self.cruise_gap_prev = CS.cruiseGapSet self.cruise_gap_set_init = 1 elif CS.cruiseGapSet != 1.0 and self.opkr_autoresume: self.cruise_gap_switch_timer += 1 if self.cruise_gap_switch_timer > 100: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.GAP_DIST, clu11_speed)) self.cruise_gap_switch_timer = 0 # reset lead distnce after the car starts moving elif self.last_lead_distance != 0: self.last_lead_distance = 0 elif run_speed_ctrl: is_sc_run = self.SC.update(CS, sm, self) if is_sc_run: can_sends.append( create_clu11(self.packer, self.resume_cnt, CS.scc_bus, CS.clu11, self.SC.btn_type, self.SC.sc_clu_speed)) self.resume_cnt += 1 else: self.resume_cnt = 0 # gap restore if self.dRel > 17 and self.vRel < 5 and self.cruise_gap_prev != CS.cruiseGapSet and self.cruise_gap_set_init == 1 and self.opkr_autoresume: self.cruise_gap_switch_timer += 1 if self.cruise_gap_switch_timer > 50: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.GAP_DIST, clu11_speed)) self.cruise_gap_switch_timer = 0 elif self.cruise_gap_prev == CS.cruiseGapSet and self.opkr_autoresume: self.cruise_gap_set_init = 0 self.cruise_gap_prev = 0 #if CS.out.vEgo > 8 and self.lead2_status and self.dRel - self.dRel2 > 3 and self.cut_in_detection == 0 and self.cruise_gap_prev2 == 0: # self.cut_in_detection = 1 # self.cruise_gap_prev2 = CS.cruiseGapSet #elif CS.out.vEgo > 8 and self.cut_in_detection == 1 and CS.cruiseGapSet != 1.0: # self.cruise_gap_switch_timer2 += 1 # if self.cruise_gap_switch_timer2 > 150: # can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.GAP_DIST, clu11_speed)) # self.cruise_gap_switch_timer2 = 0 #elif CS.out.vEgo > 8 and self.cut_in_detection == 1 and CS.cruiseGapSet == 1.0: # self.cruise_gap_switch_timer2 += 1 # if self.cruise_gap_switch_timer2 > 600: # if self.cruise_gap_prev2 != CS.cruiseGapSet: # self.cruise_gap_switch_timer3 += 1 # if self.cruise_gap_switch_timer3 > 50: # can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.GAP_DIST, clu11_speed)) # self.cruise_gap_switch_timer3 = 0 #elif self.cruise_gap_prev2 == CS.cruiseGapSet: # self.cut_in_detection == 0 # self.cruise_gap_prev2 = 0 # self.cruise_gap_switch_timer2 = 0 # self.cruise_gap_switch_timer3 = 0 if CS.out.brakeLights and CS.out.vEgo == 0 and not CS.acc_active: self.standstill_status_timer += 1 if self.standstill_status_timer > 200: self.standstill_status = 1 self.standstill_status_timer = 0 if self.standstill_status == 1 and CS.out.vEgo > 1: self.standstill_status = 0 self.standstill_fault_reduce_timer = 0 self.v_set_dis_prev = 180 self.last_resume_frame = frame self.res_switch_timer = 0 self.resume_cnt = 0 if CS.mdps_bus: # send mdps12 to LKAS to prevent LKAS error can_sends.append(create_mdps12(self.packer, frame, CS.mdps12)) # send scc to car if longcontrol enabled and SCC not on bus 0 or ont live if self.longcontrol and (CS.scc_bus or not self.scc_live) and frame % 2 == 0: can_sends.append( create_scc12(self.packer, apply_accel, enabled, self.scc12_cnt, self.scc_live, CS.scc12)) can_sends.append( create_scc11(self.packer, frame, enabled, set_speed, lead_visible, self.scc_live, CS.scc11)) if CS.has_scc13 and frame % 20 == 0: can_sends.append(create_scc13(self.packer, CS.scc13)) if CS.has_scc14: can_sends.append(create_scc14(self.packer, enabled, CS.scc14)) self.scc12_cnt += 1 # 20 Hz LFA MFA message if frame % 5 == 0 and self.car_fingerprint in FEATURES["send_lfa_mfa"]: can_sends.append(create_lfa_mfa(self.packer, frame, lkas_active)) if CS.spas_enabled: if CS.mdps_bus: can_sends.append( create_ems11(self.packer, CS.ems11, spas_active)) # SPAS11 50hz if (frame % 2) == 0: if CS.mdps11_stat == 7 and not self.mdps11_stat_last == 7: self.en_spas = 7 self.en_cnt = 0 if self.en_spas == 7 and self.en_cnt >= 8: self.en_spas = 3 self.en_cnt = 0 if self.en_cnt < 8 and spas_active: self.en_spas = 4 elif self.en_cnt >= 8 and spas_active: self.en_spas = 5 if not spas_active: self.apply_steer_ang = CS.mdps11_strang self.en_spas = 3 self.en_cnt = 0 self.mdps11_stat_last = CS.mdps11_stat self.en_cnt += 1 can_sends.append( create_spas11(self.packer, self.car_fingerprint, (frame // 2), self.en_spas, self.apply_steer_ang, CS.mdps_bus)) # SPAS12 20Hz if (frame % 5) == 0: can_sends.append(create_spas12(CS.mdps_bus)) return can_sends
def update(self, active, CS, CP, path_plan): lqr_log = log.ControlsState.LateralLQRState.new_message() steers_max = get_steer_max(CP, CS.vEgo) torque_scale = (0.45 + CS.vEgo / 60.0)**2 # Scale actuator model with speed #neokii torque_scale = min(torque_scale, 0.65) steering_angle = CS.steeringAngle steeringTQ = CS.steeringTorqueEps v_ego_kph = CS.vEgo * CV.MS_TO_KPH self.ki, self.scale = self.atom_tune(v_ego_kph, CS.steeringAngle, CP) # ### 설정값 최적화 분석을 위한 랜덤화 임시 코드 #now = datetime.datetime.now() # current date and time #micro_S = int(now.microsecond) #if micro_S < 10000 : #1초에 한번만 랜덤변환 # self.ki = random.uniform(0.015, 0.025) #self.ki - (self.ki*0.5), self.ki + (self.ki*0.5) ) # self.scale = random.uniform(1750, 1950) #int(self.scale) - int(self.scale*0.055), int(self.scale) + int(self.scale*0.055) ) ) # self.dc_gain = random.uniform(0.0028, 0.0032) #self.dc_gain - (self.dc_gain*0.1), self.dc_gain + (self.dc_gain*0.1) ) # steers_max = random.uniform(1.0, 1.2) # ########################### log_ki = self.ki log_scale = self.scale log_dc_gain = self.dc_gain # Subtract offset. Zero angle should correspond to zero torque self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset steering_angle -= path_plan.angleOffset # Update Kalman filter angle_steers_k = float(self.C.dot(self.x_hat)) e = steering_angle - angle_steers_k self.x_hat = self.A.dot(self.x_hat) + self.B.dot( CS.steeringTorqueEps / torque_scale) + self.L.dot(e) error = self.angle_steers_des - angle_steers_k u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat)) if CS.vEgo < 0.3 or not active: lqr_log.active = False lqr_output = 0. self.reset() else: lqr_log.active = True # LQR #u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat)) lqr_output = torque_scale * u_lqr / self.scale # Integrator if CS.steeringPressed: self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr)) else: #error = self.angle_steers_des - angle_steers_k i = self.i_lqr + self.ki * self.i_rate * error control = lqr_output + i if (error >= 0 and (control <= steers_max or i < 0.0)) or \ (error <= 0 and (control >= -steers_max or i > 0.0)): self.i_lqr = i self.output_steer = lqr_output + self.i_lqr self.output_steer = clip(self.output_steer, -steers_max, steers_max) check_saturation = ( CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed saturated = self._check_saturation(self.output_steer, check_saturation, steers_max) if not CS.steeringPressed: str2 = '/{} /{} /{} /{} /{} /{} /{} /{} /{} /{}'.format( v_ego_kph, steering_angle, self.angle_steers_des, angle_steers_k, steeringTQ, torque_scale, log_scale, log_ki, log_dc_gain, self.output_steer) self.trLQR.add(str2) str5 = 'LQR_Set:dc_gain={:06.4f}/scale={:06.1f}/ki={:05.3f}/OutputSteer={:5.3f}/Angle={:5.1f}|{:5.1f}'.format( self.scale, self.dc_gain, self.ki, self.output_steer, steering_angle, angle_steers_k) trace1.printf2(str5) lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset lqr_log.i = self.i_lqr lqr_log.output = self.output_steer lqr_log.lqrOutput = lqr_output lqr_log.saturated = saturated return self.output_steer, float(self.angle_steers_des), lqr_log
def update(self, CC, CS, frame, sm, CP): if self.CP != CP: self.CP = CP self.param_load() enabled = CC.enabled actuators = CC.actuators pcm_cancel_cmd = CC.cruiseControl.cancel self.dRel, self.yRel, self.vRel = SpdController.get_lead(sm) if self.SC is not None: self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo) else: self.model_speed = self.model_sum = 0 # Steering Torque new_steer = actuators.steer * SteerLimitParams.STEER_MAX apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, SteerLimitParams) self.steer_rate_limited = new_steer != apply_steer # disable if steer angle reach 90 deg, otherwise mdps fault in some models lkas_active = enabled and abs(CS.out.steeringAngle) < 180. #90 if ((CS.out.leftBlinker and not CS.out.rightBlinker) or (CS.out.rightBlinker and not CS.out.leftBlinker)) and CS.out.vEgo < 60 * CV.KPH_TO_MS: self.lanechange_manual_timer = 10 if CS.out.leftBlinker and CS.out.rightBlinker: self.emergency_manual_timer = 10 if abs(CS.out.steeringTorque) > 409: #360: #180 self.driver_steering_torque_above_timer = 100 if self.lanechange_manual_timer or self.driver_steering_torque_above_timer: lkas_active = 0 if self.lanechange_manual_timer > 0: self.lanechange_manual_timer -= 1 if self.emergency_manual_timer > 0: self.emergency_manual_timer -= 1 if self.driver_steering_torque_above_timer > 0: self.driver_steering_torque_above_timer -= 1 if not lkas_active: apply_steer = 0 steer_req = 1 if apply_steer else 0 self.apply_steer_last = apply_steer sys_warning, sys_state = self.process_hud_alert(lkas_active, CC) clu11_speed = CS.clu11["CF_Clu_Vanz"] enabled_speed = 38 if CS.is_set_speed_in_mph else 55 if clu11_speed > enabled_speed: enabled_speed = clu11_speed can_sends = [] if frame == 0: # initialize counts from last received count signals self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"] + 1 self.lkas11_cnt %= 0x10 can_sends.append( create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint, apply_steer, steer_req, CS.lkas11, sys_warning, sys_state, CC, enabled, 0)) if CS.mdps_bus or CS.scc_bus == 1: # send lkas11 bus 1 if mdps is on bus 1 can_sends.append( create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint, apply_steer, steer_req, CS.lkas11, sys_warning, sys_state, CC, enabled, 1)) if CS.mdps_bus: # send clu11 to mdps if it is not on bus 0 #if frame % 2 and CS.mdps_bus == 1: # send clu11 to mdps if it is not on bus 0 can_sends.append( create_clu11(self.packer, frame, CS.mdps_bus, CS.clu11, Buttons.NONE, enabled_speed)) #if CS.mdps_bus: can_sends.append(create_mdps12(self.packer, frame, CS.mdps12)) str_log1 = '곡률={:04.1f}/{:=+06.3f} 토크={:=+04.0f}/{:=+04.0f}'.format( self.model_speed, self.model_sum, new_steer, CS.out.steeringTorque) str_log2 = '프레임율={:03.0f}'.format(self.timer1.sampleTime()) trace1.printf('{} {}'.format(str_log1, str_log2)) if CS.out.cruiseState.modeSel == 0 and self.mode_change_switch == 4: self.mode_change_timer = 50 self.mode_change_switch = 0 elif CS.out.cruiseState.modeSel == 1 and self.mode_change_switch == 0: self.mode_change_timer = 50 self.mode_change_switch = 1 elif CS.out.cruiseState.modeSel == 2 and self.mode_change_switch == 1: self.mode_change_timer = 50 self.mode_change_switch = 2 elif CS.out.cruiseState.modeSel == 3 and self.mode_change_switch == 2: self.mode_change_timer = 50 self.mode_change_switch = 3 elif CS.out.cruiseState.modeSel == 4 and self.mode_change_switch == 3: self.mode_change_timer = 50 self.mode_change_switch = 4 if self.mode_change_timer > 0: self.mode_change_timer -= 1 run_speed_ctrl = self.param_OpkrAccelProfile and CS.acc_active and self.SC != None if not run_speed_ctrl: if CS.out.cruiseState.modeSel == 0: self.steer_mode = "오파모드" elif CS.out.cruiseState.modeSel == 1: self.steer_mode = "차간+커브" elif CS.out.cruiseState.modeSel == 2: self.steer_mode = "차간ONLY" elif CS.out.cruiseState.modeSel == 3: self.steer_mode = "자동RES" elif CS.out.cruiseState.modeSel == 4: self.steer_mode = "순정모드" if CS.out.steerWarning == 0: self.mdps_status = "정상" elif CS.out.steerWarning == 1: self.mdps_status = "오류" if CS.lkas_button_on == 0: self.lkas_switch = "OFF" elif CS.lkas_button_on == 1: self.lkas_switch = "ON" else: self.lkas_switch = "-" str_log2 = '주행모드={:s} MDPS상태={:s} LKAS버튼={:s}'.format( self.steer_mode, self.mdps_status, self.lkas_switch) trace1.printf2('{}'.format(str_log2)) #print( 'st={} cmd={} long={} steer={} req={}'.format(CS.out.cruiseState.standstill, pcm_cancel_cmd, self.CP.openpilotLongitudinalControl, apply_steer, steer_req ) ) if pcm_cancel_cmd and self.CP.openpilotLongitudinalControl: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed)) elif CS.out.cruiseState.standstill and not self.car_fingerprint == CAR.NIRO_EV: # run only first time when the car stopped if self.last_lead_distance == 0 or not self.param_OpkrAutoResume: # get the lead distance from the Radar self.last_lead_distance = CS.lead_distance self.resume_cnt = 0 # when lead car starts moving, create 6 RES msgs elif CS.lead_distance != self.last_lead_distance and ( frame - self.last_resume_frame) > 5: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed)) self.resume_cnt += 1 # interval after 6 msgs if self.resume_cnt > 5: self.last_resume_frame = frame self.resume_cnt = 0 elif CS.out.cruiseState.standstill and self.car_fingerprint == CAR.NIRO_EV: if CS.lead_distance > 3.7 and ( frame - self.last_resume_frame ) * DT_CTRL > 0.2 and self.param_OpkrAutoResume: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed)) self.last_resume_frame = frame # reset lead distnce after the car starts moving elif self.last_lead_distance != 0: self.last_lead_distance = 0 elif run_speed_ctrl and self.SC != None: is_sc_run = self.SC.update(CS, sm, self) if is_sc_run: can_sends.append( create_clu11(self.packer, self.resume_cnt, CS.scc_bus, CS.clu11, self.SC.btn_type, self.SC.sc_clu_speed)) self.resume_cnt += 1 else: self.resume_cnt = 0 if CS.out.cruiseState.modeSel == 3: if CS.out.brakeLights and CS.VSetDis > 30: self.res_cnt = 0 self.res_delay = 50 elif self.res_delay: self.res_delay -= 1 elif not self.res_delay and self.res_cnt < 0 and CS.VSetDis > 30 and CS.out.vEgo > 30 * CV.KPH_TO_MS: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed)) can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed)) self.res_cnt += 1 else: self.res_cnt = 7 self.res_delay = 0 # 20 Hz LFA MFA message if frame % 5 == 0 and self.car_fingerprint in FEATURES["send_lfa_mfa"]: can_sends.append(create_lfa_mfa(self.packer, frame, enabled)) self.lkas11_cnt += 1 return can_sends
def update(self, CC, CS, frame, sm, CP ): if self.CP != CP: self.CP = CP self.param_load() enabled = CC.enabled actuators = CC.actuators pcm_cancel_cmd = CC.cruiseControl.cancel self.dRel, self.yRel, self.vRel = SpdController.get_lead( sm ) if self.SC is not None: self.model_speed, self.model_sum = self.SC.calc_va( sm, CS.out.vEgo ) else: self.model_speed = self.model_sum = 0 # Steering Torque if self.param_OpkrEnableLearner: new_steer = actuators.steer * SteerLimitParams.STEER_MAX apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, SteerLimitParams) self.steer_rate_limited = new_steer != apply_steer else: path_plan = sm['pathPlan'] self.cV_tune( CS.out.vEgo, self.model_speed ) param = SteerLimitParams() param.STEER_MAX = min( param.STEER_MAX, self.MAX ) param.STEER_DELTA_UP = min( param.STEER_DELTA_UP, self.UP ) param.STEER_DELTA_DOWN = min( param.STEER_DELTA_DOWN, self.DN ) new_steer = actuators.steer * param.STEER_MAX apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, param) self.steer_rate_limited = new_steer != apply_steer # disable if steer angle reach 90 deg, otherwise mdps fault in some models lkas_active = enabled and abs(CS.out.steeringAngle) < 90. #TenesiDel -> and self.lkas_button_on # fix for Genesis hard fault at low speed 테네시 추가 if CS.out.vEgo < 60 * CV.KPH_TO_MS and self.car_fingerprint == CAR.GENESIS and not CS.mdps_bus: lkas_active = False # if (( CS.out.leftBlinker and not CS.out.rightBlinker) or ( CS.out.rightBlinker and not CS.out.leftBlinker)) and CS.out.vEgo > 60 * CV.KPH_TO_MS: # 깜빡이 작동시에도 상히조향 유지 수정해보기 # self.lanechange_manual_timer = 10 # if CS.out.leftBlinker and CS.out.rightBlinker: # self.emergency_manual_timer = 10 # if abs(CS.out.steeringTorque) > self.driver_steering_torque_above and CS.out.vEgo > 60: # 깜빡이 작동시에도 상히조향 유지 수정해보기 # self.driver_steering_torque_above_timer = 30 # if self.lanechange_manual_timer or self.driver_steering_torque_above_timer: # lkas_active = 0 # if self.lanechange_manual_timer > 0: # self.lanechange_manual_timer -= 1 # if self.emergency_manual_timer > 0: # self.emergency_manual_timer -= 1 # if self.driver_steering_torque_above_timer > 0: # self.driver_steering_torque_above_timer -= 1 # Disable steering while turning blinker on and speed below 60 kph #201011 상시조향 작업 작동성공 if CS.out.leftBlinker or CS.out.rightBlinker: if self.car_fingerprint not in [CAR.K5, CAR.K5_HEV]: # 테네시 추가 OPTIMA -> K5 self.turning_signal_timer = 100 # Disable for 1.0 Seconds after blinker turned off elif CS.left_blinker_flash or CS.right_blinker_flash: # Optima has blinker flash signal only self.turning_signal_timer = 100 if self.turning_signal_timer and CS.out.vEgo > 70 * CV.KPH_TO_MS: # TenesiADD Blinker tune 시속60미만에서는 상시조향 lkas_active = 0 if self.turning_signal_timer: # TenesiADD self.turning_signal_timer -= 1 #201011 상시조향 작업 작동성공 if not lkas_active: apply_steer = 0 steer_req = 1 if apply_steer else 0 self.apply_steer_last = apply_steer sys_warning, sys_state = self.process_hud_alert( lkas_active, CC ) clu11_speed = CS.clu11["CF_Clu_Vanz"] enabled_speed = 38 if CS.is_set_speed_in_mph else 55 if clu11_speed > enabled_speed: enabled_speed = clu11_speed can_sends = [] if frame == 0: # initialize counts from last received count signals self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"] + 1 self.lkas11_cnt %= 0x10 can_sends.append(create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint, apply_steer, steer_req, CS.lkas11, sys_warning, sys_state, CC, enabled, 0 )) if CS.mdps_bus or CS.scc_bus == 1: # send lkas11 bus 1 if mdps is on bus 1 can_sends.append(create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint, apply_steer, steer_req, CS.lkas11, sys_warning, sys_state, CC, enabled, 1 )) if CS.mdps_bus: # send clu11 to mdps if it is not on bus 0 #if frame % 2 and CS.mdps_bus == 1: # send clu11 to mdps if it is not on bus 0 can_sends.append(create_clu11(self.packer, frame, CS.mdps_bus, CS.clu11, Buttons.NONE, enabled_speed)) #if CS.mdps_bus: can_sends.append(create_mdps12(self.packer, frame, CS.mdps12)) str_log1 = '곡률={:05.1f}/{:=+06.3f} 토크={:=+04.0f}/{:=+04.0f}'.format( self.model_speed, self.model_sum, new_steer, CS.out.steeringTorque ) if self.param_OpkrEnableLearner: str_log2 = '프레임율={:03.0f} STMAX={:03.0f}'.format( self.timer1.sampleTime(), SteerLimitParams.STEER_MAX, ) else: str_log2 = '프레임율={:03.0f} ST={:03.0f}/{:01.0f}/{:01.0f} SR={:05.2f}'.format( self.timer1.sampleTime(), self.MAX, self.UP, self.DN, path_plan.steerRatio ) trace1.printf( '{} {}'.format( str_log1, str_log2 ) ) if CS.out.cruiseState.modeSel == 0 and self.mode_change_switch == 4: self.mode_change_timer = 50 self.mode_change_switch = 0 elif CS.out.cruiseState.modeSel == 1 and self.mode_change_switch == 0: self.mode_change_timer = 50 self.mode_change_switch = 1 elif CS.out.cruiseState.modeSel == 2 and self.mode_change_switch == 1: self.mode_change_timer = 50 self.mode_change_switch = 2 elif CS.out.cruiseState.modeSel == 3 and self.mode_change_switch == 2: self.mode_change_timer = 50 self.mode_change_switch = 3 elif CS.out.cruiseState.modeSel == 4 and self.mode_change_switch == 3: self.mode_change_timer = 50 self.mode_change_switch = 4 if self.mode_change_timer > 0: self.mode_change_timer -= 1 run_speed_ctrl = self.param_OpkrAccelProfile and CS.acc_active and self.SC != None and (CS.out.cruiseState.modeSel == 1 or CS.out.cruiseState.modeSel == 2 or CS.out.cruiseState.modeSel == 3) if not run_speed_ctrl: if CS.out.cruiseState.modeSel == 0: self.steer_mode = "오파모드" elif CS.out.cruiseState.modeSel == 1: self.steer_mode = "차간+커브" elif CS.out.cruiseState.modeSel == 2: self.steer_mode = "차간ONLY" elif CS.out.cruiseState.modeSel == 3: self.steer_mode = "자동RES" elif CS.out.cruiseState.modeSel == 4: self.steer_mode = "순정모드" if CS.out.steerWarning == 0: self.mdps_status = "정상" elif CS.out.steerWarning == 1: self.mdps_status = "오류" if CS.lkas_button_on == 0: self.lkas_switch = "OFF" elif CS.lkas_button_on == 1: self.lkas_switch = "ON" else: self.lkas_switch = "-" if CS.out.cruiseState.modeSel == 3: str_log2 = '주행모드={:s} MDPS상태={:s} LKAS버튼={:s} AUTORES=(VS:{:03.0f}/CN:{:01.0f}/RD:{:03.0f}/BK:{})'.format( self.steer_mode, self.mdps_status, self.lkas_switch, CS.VSetDis, self.res_cnt, self.res_delay, CS.out.brakeLights ) else: str_log2 = '주행모드={:s} MDPS상태={:s} LKAS버튼={:s}'.format( self.steer_mode, self.mdps_status, self.lkas_switch ) trace1.printf2( '{}'.format( str_log2 ) ) #print( 'st={} cmd={} long={} steer={} req={}'.format(CS.out.cruiseState.standstill, pcm_cancel_cmd, self.CP.openpilotLongitudinalControl, apply_steer, steer_req ) ) if pcm_cancel_cmd and self.CP.openpilotLongitudinalControl: can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed)) elif CS.out.cruiseState.standstill and not self.car_fingerprint == CAR.NIRO_EV: # run only first time when the car stopped if self.last_lead_distance == 0 or not self.param_OpkrAutoResume: # get the lead distance from the Radar self.last_lead_distance = CS.lead_distance self.resume_cnt = 0 # when lead car starts moving, create 6 RES msgs elif CS.lead_distance != self.last_lead_distance and (frame - self.last_resume_frame) > 5: can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed)) self.resume_cnt += 1 # interval after 6 msgs if self.resume_cnt > 5: self.last_resume_frame = frame self.resume_cnt = 0 elif CS.out.cruiseState.standstill and self.car_fingerprint == CAR.NIRO_EV: if CS.lead_distance > 3.7 and (frame - self.last_resume_frame)*DT_CTRL > 0.2 and self.param_OpkrAutoResume: can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed)) self.last_resume_frame = frame # reset lead distnce after the car starts moving elif self.last_lead_distance != 0: self.last_lead_distance = 0 elif run_speed_ctrl and self.SC != None: is_sc_run = self.SC.update( CS, sm, self ) if is_sc_run: can_sends.append(create_clu11(self.packer, self.resume_cnt, CS.scc_bus, CS.clu11, self.SC.btn_type, self.SC.sc_clu_speed )) self.resume_cnt += 1 else: self.resume_cnt = 0 if CS.out.cruiseState.modeSel == 3: if CS.out.brakeLights and CS.VSetDis > 30: self.res_cnt = 0 self.res_delay = 50 elif self.res_delay: self.res_delay -= 1 elif not self.res_delay and self.res_cnt < 6 and CS.VSetDis > 30 and CS.out.vEgo > 30 * CV.KPH_TO_MS: if self.res_cnt < 1: can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed)) can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed)) self.res_cnt += 1 else: self.res_cnt = 7 self.res_delay = 0 # 20 Hz LFA MFA message if frame % 5 == 0 and self.car_fingerprint in FEATURES["send_lfa_mfa"]: can_sends.append(create_lfa_mfa(self.packer, frame, enabled)) self.lkas11_cnt += 1 return can_sends