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, controls): # *** 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) # 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 # 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 lkas_active = enabled and abs(CS.out.steeringAngle) < 90. and not spas_active # 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 # Disable steering while turning blinker on and speed below 60 kph if CS.out.leftBlinker or CS.out.rightBlinker: self.turning_signal_timer = 0.5 / DT_CTRL # Disable for 0.5 Seconds after blinker turned off if self.turning_indicator_alert and CS.out.vEgo < 2 * CV.KPH_TO_MS: # set and clear by interface lkas_active = 0 if self.turning_signal_timer > 0: self.turning_signal_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) 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 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 CS.mdps_bus: # send clu11 to mdps if it is not on bus 0 can_sends.append(create_clu11(self.packer, frame % 0x10, CS.mdps_bus, CS.clu11, Buttons.NONE, enabled_speed)) if pcm_cancel_cmd and self.longcontrol: can_sends.append(create_clu11(self.packer, frame % 0x10, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed)) # fix auto resume - by neokii if CS.out.cruiseState.standstill: if self.last_lead_distance == 0: self.last_lead_distance = CS.lead_distance self.resume_cnt = 0 self.resume_wait_timer = 0 # scc smoother elif self.scc_smoother.is_active(frame): pass 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, self.resume_cnt, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed)) self.resume_cnt += 1 if self.resume_cnt >= 8: self.resume_cnt = 0 self.resume_wait_timer = SccSmoother.get_wait_count() # reset lead distnce after the car starts moving elif self.last_lead_distance != 0: self.last_lead_distance = 0 # scc smoother if not self.longcontrol: self.scc_smoother.update(enabled, can_sends, self.packer, CC, CS, frame, apply_accel, controls) 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, sendcan, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): if not self.enable_camera: return if CS.camcan > 0: if self.checksum == "NONE": self.checksum = learn_checksum(self.packer, CS.lkas11) print("Discovered Checksum", self.checksum) if self.checksum == "NONE": return elif CS.steer_error == 1: if self.checksum_learn_cnt > 200: self.checksum_learn_cnt = 0 if self.checksum == "NONE": print("Testing 6B Checksum") self.checksum == "6B" elif self.checksum == "6B": print("Testing 7B Checksum") self.checksum == "7B" elif self.checksum == "7B": print("Testing CRC8 Checksum") self.checksum == "crc8" else: self.checksum == "NONE" else: self.checksum_learn_cnt += 1 force_enable = False # I don't care about your opinion, deal with it! if (CS.cstm_btns.get_button_status("alwon") > 0) and CS.acc_active: enabled = True force_enable = True if (self.car_fingerprint in FEATURES["soft_disable"] and CS.v_wheel < 16.8): enabled = False force_enable = False if (CS.left_blinker_on == 1 or CS.right_blinker_on == 1): self.turning_signal_timer = 100 # Disable for 1.0 Seconds after blinker turned off #update custom UI buttons and alerts CS.UE.update_custom_ui() if (self.cnt % 100 == 0): CS.cstm_btns.send_button_info() CS.UE.uiSetCarEvent(CS.cstm_btns.car_folder, CS.cstm_btns.car_name) # Get the angle from ALCA. alca_enabled = False alca_steer = 0. alca_angle = 0. turn_signal_needed = 0 # Update ALCA status and custom button every 0.1 sec. if self.ALCA.pid == None: self.ALCA.set_pid(CS) self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0) alca_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update( enabled, CS, self.cnt, actuators) if force_enable and not CS.acc_active: apply_steer = int( round(actuators.steer * SteerLimitParams.STEER_MAX)) else: apply_steer = int(round(alca_steer * SteerLimitParams.STEER_MAX)) # SPAS limit angle extremes for safety apply_steer_ang_req = np.clip(actuators.steerAngle, -1 * (SteerLimitParams.STEER_ANG_MAX), SteerLimitParams.STEER_ANG_MAX) # SPAS limit angle rate for safety if abs(self.apply_steer_ang - apply_steer_ang_req) > 0.6: if apply_steer_ang_req > self.apply_steer_ang: self.apply_steer_ang += 0.5 else: self.apply_steer_ang -= 0.5 else: self.apply_steer_ang = apply_steer_ang_req # Limit steer rate for safety apply_steer = limit_steer_rate(apply_steer, self.apply_steer_last, SteerLimitParams, CS.steer_torque_driver) if alca_enabled: self.turning_signal_timer = 0 if self.turning_signal_timer > 0: self.turning_signal_timer = self.turning_signal_timer - 1 turning_signal = 1 else: turning_signal = 0 # Use LKAS or SPAS if CS.mdps11_stat == 7 or CS.v_wheel > 2.7: self.lkas = True elif CS.v_wheel < 0.1: self.lkas = False if self.spas_present: self.lkas = True # If ALCA is disabled, and turning indicators are turned on, we do not want OP to steer, if not enabled or (turning_signal and not alca_enabled): if self.lkas: apply_steer = 0 else: self.apply_steer_ang = 0.0 self.en_cnt = 0 steer_req = 1 if enabled and self.lkas else 0 self.apply_steer_last = apply_steer can_sends = [] self.lkas11_cnt = self.cnt % 0x10 self.clu11_cnt = self.cnt % 0x10 self.mdps12_cnt = self.cnt % 0x100 self.spas_cnt = self.cnt % 0x200 can_sends.append(create_lkas11(self.packer, self.car_fingerprint, apply_steer, steer_req, self.lkas11_cnt, \ enabled if self.lkas else False, False if CS.camcan == 0 else CS.lkas11, hud_alert, (CS.cstm_btns.get_button_status("cam") > 0), \ (False if CS.camcan == 0 else True), self.checksum)) if CS.camcan > 0: can_sends.append(create_mdps12(self.packer, self.car_fingerprint, self.mdps12_cnt, CS.mdps12, CS.lkas11, \ CS.camcan, self.checksum)) # SPAS11 50hz if (self.cnt % 2) == 0 and not self.spas_present: 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 enabled and not self.lkas: self.en_spas = 4 elif self.en_cnt >= 8 and enabled and not self.lkas: self.en_spas = 5 if self.lkas or not enabled: 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.spas_cnt / 2), self.en_spas, self.apply_steer_ang, self.checksum)) # SPAS12 20Hz if (self.cnt % 5) == 0 and not self.spas_present: can_sends.append(create_spas12(self.packer)) # Force Disable if pcm_cancel_cmd and (not force_enable): can_sends.append( create_clu11(self.packer, CS.clu11, Buttons.CANCEL, 0)) elif CS.stopped and (self.cnt - self.last_resume_cnt) > 5: self.last_resume_cnt = self.cnt can_sends.append( create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL, 0)) # Speed Limit Related Stuff Lot's of comments for others to understand! # Run this twice a second if (self.cnt % 50) == 0: if self.params.get("LimitSetSpeed") == "1" and self.params.get( "SpeedLimitOffset") is not None: # If Not Enabled, or cruise not set, allow auto speed adjustment again if not (enabled and CS.acc_active_real): self.speed_adjusted = False # Attempt to read the speed limit from zmq map_data = messaging.recv_one_or_none(self.map_data_sock) # If we got a message if map_data != None: # See if we use Metric or dead kings extremeties for measurements, and set a variable to the conversion value if bool(self.params.get("IsMetric")): self.speed_conv = CV.MS_TO_KPH else: self.speed_conv = CV.MS_TO_MPH # If the speed limit is valid if map_data.liveMapData.speedLimitValid: last_speed = self.map_speed # Get the speed limit, and add the offset to it, v_speed = (map_data.liveMapData.speedLimit + float(self.params.get("SpeedLimitOffset"))) ## Stolen curvature code from planner.py, and updated it for us v_curvature = 45.0 if map_data.liveMapData.curvatureValid: v_curvature = math.sqrt( 1.85 / max(1e-4, abs(map_data.liveMapData.curvature))) # Use the minimum between Speed Limit and Curve Limit, and convert it as needed #self.map_speed = min(v_speed, v_curvature) * self.speed_conv self.map_speed = v_speed * self.speed_conv # Compare it to the last time the speed was read. If it is different, set the flag to allow it to auto set our speed if last_speed != self.map_speed: self.speed_adjusted = False else: # If it is not valid, set the flag so the cruise speed won't be changed. self.map_speed = 0 self.speed_adjusted = True else: self.speed_adjusted = True # Ensure we have cruise IN CONTROL, so we don't do anything dangerous, like turn cruise on # Ensure the speed limit is within range of the stock cruise control capabilities # Do the spamming 10 times a second, we might get from 0 to 10 successful # Only do this if we have not yet set the cruise speed if CS.acc_active_real and not self.speed_adjusted and self.map_speed > ( 8.5 * self.speed_conv) and (self.cnt % 9 == 0 or self.cnt % 9 == 1): # Use some tolerance because of Floats being what they are... if (CS.cruise_set_speed * self.speed_conv) > (self.map_speed * 1.005): can_sends.append( create_clu11(self.packer, CS.clu11, Buttons.SET_DECEL, (1 if self.cnt % 9 == 1 else 0))) elif (CS.cruise_set_speed * self.speed_conv) < (self.map_speed / 1.005): can_sends.append( create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL, (1 if self.cnt % 9 == 1 else 0))) # If nothing needed adjusting, then the speed has been set, which will lock out this control else: self.speed_adjusted = True ### If Driver Overrides using accelerator (or gas for the antiquated), cancel auto speed adjustment if CS.pedal_gas: self.speed_adjusted = True ### Send messages to canbus sendcan.send( can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes()) self.cnt += 1
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, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): # *** 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) ### Steering Torque new_steer = actuators.steer * SteerLimitParams.STEER_MAX apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steer_torque_driver, SteerLimitParams) self.steer_rate_limited = new_steer != apply_steer # SPAS limit angle extremes for safety apply_steer_ang_req = clip(actuators.steer, -1 * (STEER_ANG_MAX), STEER_ANG_MAX) # SPAS limit angle rate for safety if abs(self.apply_steer_ang - apply_steer_ang_req) > 1.5: if apply_steer_ang_req > self.apply_steer_ang: self.apply_steer_ang += 0.5 else: self.apply_steer_ang -= 0.5 else: self.apply_steer_ang = apply_steer # LKAS button to temporarily disable steering if not CS.lkas_error: if CS.lkas_button_on != self.lkas_button_last: self.lkas_button = not self.lkas_button self.lkas_button_last = CS.lkas_button_on # disable if steer angle reach 90 deg, otherwise mdps fault in some models lkas_active = enabled and abs( CS.angle_steers) < 90. and self.lkas_button # fix for Genesis hard fault at low speed if CS.v_ego < 16.7 and self.car_fingerprint == CAR.GENESIS and not CS.mdps_bus: lkas_active = 0 # Disable steering while turning blinker on and speed below 60 kph if CS.left_blinker_on or CS.right_blinker_on: if self.car_fingerprint not in [CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_H]: 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.v_ego < 16.7: lkas_active = 0 if self.turning_signal_timer: self.turning_signal_timer -= 1 if not lkas_active: apply_steer = 0 steer_req = 1 if apply_steer else 0 self.apply_accel_last = apply_accel self.apply_steer_last = apply_steer hud_alert, lane_visible, left_lane_warning, right_lane_warning =\ process_hud_alert(lkas_active, self.lkas_button, self.car_fingerprint, visual_alert, left_line, right_line,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: 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.scc12_cnt = CS.scc12[ "CR_VSM_Alive"] + 1 if not CS.no_radar else 0 self.lkas11_cnt %= 0x10 self.scc12_cnt %= 0xF self.clu11_cnt = frame % 0x10 self.mdps12_cnt = frame % 0x100 self.spas_cnt = frame % 0x200 # 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 enabled and not self.lkas: self.en_spas = 4 elif self.en_cnt >= 8 and enabled and not self.lkas: self.en_spas = 5 if self.lkas or not enabled: 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, (frame // 2), self.en_spas, self.apply_steer_ang, self.checksum)) #can_sends.append(create_spas11(self.packer, (self.spas_cnt / 2), self.en_spas, apply_steer, 'crc8')) # SPAS12 20Hz if (frame % 5) == 0: can_sends.append(create_spas12(self.packer)) #can_sends.append(create_ems11(self.packer, CS.ems11, enabled)) #can_sends.append(create_vsm11(self.packer, CS.vsm11, enabled, 1, steer_req, 1, self.lkas11_cnt)) #can_sends.append(create_790()) #can_sends.append([790, 0, b'\x00\x00\xff\xff\x00\xff\xff\xff', 0]) #print('send car data',CS.vsm11, enabled, 1, steer_req, self.lkas11_cnt) #can_sends.append(create_vsm11(self.packer, CS.vsm11, 1, 2, steer_req,0, self.clu11_cnt)) #can_sends.append(create_vsm11(self.packer, CS.vsm11, 1, 2, steer_req,1, self.clu11_cnt)) #can_sends.append(create_clu11(self.packer, CS.scc_bus, CS.clu11, Buttons.NONE, clu11_speed, self.clu11_cnt)) #can_sends.append(create_clu11(self.packer, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed, self.clu11_cnt)) #can_sends.append(create_vsm2(self.packer, CS.vsm2, 1, apply_steer,0, self.lkas11_cnt)) #can_sends.append(create_vsm2(self.packer, CS.vsm2, 1, apply_steer,1, self.lkas11_cnt)) can_sends.append( create_lkas11(self.packer, self.car_fingerprint, 0, apply_steer, steer_req, self.lkas11_cnt, lkas_active, CS.lkas11, hud_alert, lane_visible, left_lane_depart, right_lane_depart, keep_stock=True)) if CS.mdps_bus or CS.scc_bus == 1: # send lkas12 bus 1 if mdps or scc is on bus 1 can_sends.append( create_lkas11(self.packer, self.car_fingerprint, 1, apply_steer, steer_req, self.lkas11_cnt, lkas_active, CS.lkas11, hud_alert, lane_visible, left_lane_depart, right_lane_depart, keep_stock=True)) if CS.mdps_bus: # send clu11 to mdps if it is not on bus 0 can_sends.append( create_clu11(self.packer, CS.mdps_bus, CS.clu11, Buttons.NONE, enabled_speed, self.clu11_cnt)) if pcm_cancel_cmd and self.longcontrol: can_sends.append( create_clu11(self.packer, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed, self.clu11_cnt)) #else: send mdps12 to LKAS to prevent LKAS error if no cancel cmd #can_sends.append(create_mdps12(self.packer, self.car_fingerprint, self.mdps12_cnt, CS.mdps12)) if CS.scc_bus and self.longcontrol and frame % 2: # send scc12 to car if SCC not on bus 0 and longcontrol enabled can_sends.append( create_scc12(self.packer, apply_accel, enabled, self.scc12_cnt, CS.scc12)) self.scc12_cnt += 1 if CS.stopped: # run only first time when the car stopped if self.last_lead_distance == 0: # get the lead distance from the Radar self.last_lead_distance = CS.lead_distance self.resume_cnt = 0 # when lead car starts moving, create 6 RES msgs elif CS.lead_distance > self.last_lead_distance and ( frame - self.last_resume_frame) > 5: can_sends.append( create_clu11(self.packer, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed, self.clu11_cnt)) self.resume_cnt += 1 # interval after 6 msgs if self.resume_cnt > 5: self.last_resume_frame = frame self.clu11_cnt = 0 # reset lead distnce after the car starts moving elif self.last_lead_distance != 0: self.last_lead_distance = 0 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): # *** 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) # 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 # 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 self.lkas_button_on and not spas_active # fix for Genesis hard fault at low speed if CS.out.vEgo < 60 * CV.KPH_TO_MS and self.car_fingerprint == CAR.HYUNDAI_GENESIS and not CS.mdps_bus: lkas_active = False # Disable steering while turning blinker on and speed below 60 kph if CS.out.leftBlinker or CS.out.rightBlinker: if self.car_fingerprint not in [CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_H]: 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_indicator_alert: # set and clear by interface lkas_active = 0 if self.turning_signal_timer > 0: self.turning_signal_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 60 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)) if pcm_cancel_cmd and self.longcontrol: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed)) elif CS.out.cruiseState.standstill: # SCC won't resume anyway when the lead distace is less than 3.7m # send resume at a max freq of 5Hz if CS.lead_distance > 3.7 and ( frame - self.last_resume_frame) * DT_CTRL > 0.2: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed)) self.last_resume_frame = frame 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)) # DIY cruise speed code sm.update(0) lead_data = sm['radarState'].leadOne lead_one = sm['radarState'].leadOne lead_two = sm['radarState'].leadTwo if lead_one.status == True: lead_data = lead_one if lead_two.status == True and ((lead_one.dRel - lead_two.dRel) > 3.0): lead_data = lead_two lead_rel_dist = lead_data.dRel lead_rel_vel = lead_data.vRel lead_vel = lead_data.vLead cruise_curr_set_speed = CS.out.cruiseState.speed #currently set cruise speed in m/s d_cru_vego = 1.11 #4km/h diff between cruise/realspeed #cruise_curr_set_speed - CS.out.vEgo min_dist = 10. #min dist to lead car to engage in meters max_dist = 300. #max dist to lead car to disable acceleration and allow only deceleration max_cru_speed = 36.11 #limit set max cruise speed to 130km/h press_button_speed = 40 #press buttons at 400 milliseconds interval if ( cruise_curr_set_speed - lead_vel ) > 8.33: #>30km/h faster button presses on deceleration and big cruise speed gap press_button_speed = 5 elif (cruise_curr_set_speed - lead_vel) > 5.55: #>20km/h press_button_speed = 10 elif (cruise_curr_set_speed - lead_vel) > 2.77: #>10km/h press_button_speed = 20 if lead_vel > ( cruise_curr_set_speed - d_cru_vego ) and lead_rel_dist > 0. and cruise_curr_set_speed < max_cru_speed and ( frame % press_button_speed == 0 or frame % press_button_speed == 1): if not CS.out.cruiseState.available: #prevent pressing up and resume prev speed can_sends.append( create_clu11(self.packer, frame, 0, CS.clu11, Buttons.SET_DECEL, clu11_speed)) #button down elif lead_rel_dist > min_dist and lead_rel_dist < max_dist: can_sends.append( create_clu11(self.packer, frame, 0, CS.clu11, Buttons.RES_ACCEL, clu11_speed)) #button up elif lead_vel < (cruise_curr_set_speed - d_cru_vego) and lead_rel_dist > 0. and ( frame % press_button_speed == 0 or frame % press_button_speed == 1): can_sends.append( create_clu11(self.packer, frame, 0, CS.clu11, Buttons.SET_DECEL, clu11_speed)) #button down 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, 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