def controlsd_thread(gctx=None): gc.disable() # start the loop set_realtime_priority(3) params = Params() # Pub Sockets sendcan = messaging.pub_sock(service_list['sendcan'].port) controlsstate = messaging.pub_sock(service_list['controlsState'].port) carstate = messaging.pub_sock(service_list['carState'].port) carcontrol = messaging.pub_sock(service_list['carControl'].port) carevents = messaging.pub_sock(service_list['carEvents'].port) carparams = messaging.pub_sock(service_list['carParams'].port) is_metric = params.get("IsMetric") == "1" passive = params.get("Passive") != "0" sm = messaging.SubMaster([ 'thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan' ]) logcan = messaging.sub_sock(service_list['can'].port) # wait for health and CAN packets hw_type = messaging.recv_one(sm.sock['health']).health.hwType is_panda_black = hw_type == log.HealthData.HwType.blackPanda wait_for_can(logcan) CI, CP = get_car(logcan, sendcan, is_panda_black) logcan.close() # TODO: Use the logcan socket from above, but that will currenly break the tests can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100 can_sock = messaging.sub_sock(service_list['can'].port, timeout=can_timeout) car_recognized = CP.carName != 'mock' # If stock camera is disconnected, we loaded car controls and it's not chffrplus controller_available = CP.enableCamera and CI.CC is not None and not passive read_only = not car_recognized or not controller_available if read_only: CP.safetyModel = car.CarParams.SafetyModel.elm327 # diagnostic only # Write CarParams for radard and boardd safety mode params.put("CarParams", CP.to_bytes()) params.put("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0") CC = car.CarControl.new_message() AM = AlertManager() startup_alert = get_startup_alert(car_recognized, controller_available) AM.add(sm.frame, startup_alert, False) LoC = LongControl(CP, CI.compute_gb) VM = VehicleModel(CP) if CP.lateralTuning.which() == 'pid': LaC = LatControlPID(CP) elif CP.lateralTuning.which() == 'indi': LaC = LatControlINDI(CP) elif CP.lateralTuning.which() == 'lqr': LaC = LatControlLQR(CP) driver_status = DriverStatus() state = State.disabled soft_disable_timer = 0 v_cruise_kph = 255 v_cruise_kph_last = 0 overtemp = False free_space = False cal_status = Calibration.INVALID cal_perc = 0 mismatch_counter = 0 low_battery = False events_prev = [] sm['pathPlan'].sensorValid = True sm['pathPlan'].posenetValid = True # detect sound card presence sounds_available = not os.path.isfile('/EON') or ( os.path.isdir('/proc/asound/card0') and open('/proc/asound/card0/state').read().strip() == 'ONLINE') # controlsd is driven by can recv, expected at 100Hz rk = Ratekeeper(100, print_delay_threshold=None) prof = Profiler(False) # off by default while True: start_time = sec_since_boot() prof.checkpoint("Ratekeeper", ignore=True) # Sample data and compute car events CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter =\ data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, state, mismatch_counter, params) prof.checkpoint("Sample") # Create alerts if not sm.all_alive_and_valid(): events.append( create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not sm['pathPlan'].mpcSolutionValid: events.append( create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not sm['pathPlan'].sensorValid: events.append( create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT])) if not sm['pathPlan'].paramsValid: events.append(create_event('vehicleModelInvalid', [ET.WARNING])) if not sm['pathPlan'].posenetValid: events.append( create_event('posenetInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not sm['plan'].radarValid: events.append( create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if sm['plan'].radarCanError: events.append( create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not CS.canValid: events.append( create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not sounds_available: events.append( create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT])) # Only allow engagement with brake pressed when stopped behind another stopped car if CS.brakePressed and sm[ 'plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3: events.append( create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not read_only: # update control state state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \ state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM) prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log = \ state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, driver_status, LaC, LoC, VM, read_only, is_metric, cal_perc) prof.checkpoint("State Control") # Publish data CC, events_prev = data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, carevents, carparams, controlsstate, sendcan, AM, driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev) prof.checkpoint("Sent") rk.monitor_time() prof.display()
def get_can_parser(CP): signals = [ # sig_name, sig_address, default ("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0), ("GEAR", "GEAR_PACKET", 0), ("BRAKE_PRESSED", "BRAKE_MODULE", 0), ("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0), ("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0), ("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0), ("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0), ("DOOR_OPEN_FL", "SEATS_DOORS", 1), ("DOOR_OPEN_FR", "SEATS_DOORS", 1), ("DOOR_OPEN_RL", "SEATS_DOORS", 1), ("DOOR_OPEN_RR", "SEATS_DOORS", 1), ("SEATBELT_DRIVER_UNLATCHED", "SEATS_DOORS", 1), ("TC_DISABLED", "ESP_CONTROL", 1), ("STEER_FRACTION", "STEER_ANGLE_SENSOR", 0), ("STEER_RATE", "STEER_ANGLE_SENSOR", 0), ("CRUISE_ACTIVE", "PCM_CRUISE", 0), ("CRUISE_STATE", "PCM_CRUISE", 0), ("GAS_RELEASED", "PCM_CRUISE", 1), ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0), ("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0), ("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0), ("TURN_SIGNALS", "STEERING_LEVERS", 3), # 3 is no blinkers ("LKA_STATE", "EPS_STATUS", 0), ("AUTO_HIGH_BEAM", "LIGHT_STALK", 0), #dp ("SPORT_ON", "GEAR_PACKET", 0), ("ECON_ON", "GEAR_PACKET", 1), ("DISTANCE_LINES", "PCM_CRUISE_SM", 0), ("RPM", "ENGINE_RPM", 0), ("BRAKE_LIGHTS_ACC", "ESP_CONTROL", 0), ] checks = [ ("GEAR_PACKET", 1), ("LIGHT_STALK", 1), ("STEERING_LEVERS", 0.15), ("SEATS_DOORS", 3), ("ESP_CONTROL", 3), ("EPS_STATUS", 25), ("BRAKE_MODULE", 40), ("WHEEL_SPEEDS", 80), ("STEER_ANGLE_SENSOR", 80), ("PCM_CRUISE", 33), ("STEER_TORQUE_SENSOR", 50), #dp ("ENGINE_RPM", 100), ] #dp acceleration if CP.carFingerprint == CAR.RAV4_TSS2: signals.append(("SPORT_ON_2", "GEAR_PACKET", 0)) if CP.carFingerprint in [ CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.CHRH, CAR.PRIUS_TSS2, CAR.HIGHLANDERH_TSS2 ]: signals.append(("SPORT_ON", "GEAR_PACKET2", 0)) signals.append(("ECON_ON", "GEAR_PACKET2", 0)) if CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_ISH]: signals.append(("MAIN_ON", "DSU_CRUISE", 0)) signals.append(("SET_SPEED", "DSU_CRUISE", 0)) checks.append(("DSU_CRUISE", 5)) else: signals.append(("MAIN_ON", "PCM_CRUISE_2", 0)) signals.append(("SET_SPEED", "PCM_CRUISE_2", 0)) signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2", 0)) checks.append(("PCM_CRUISE_2", 33)) if CP.carFingerprint == CAR.LEXUS_ISH: signals.append(("GAS_PEDAL", "GAS_PEDAL_ALT", 0)) checks.append(("GAS_PEDAL_ALT", 33)) else: signals.append(("GAS_PEDAL", "GAS_PEDAL", 0)) checks.append(("GAS_PEDAL", 33)) # add gas interceptor reading if we are using it if CP.enableGasInterceptor: signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0)) signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR", 0)) checks.append(("GAS_SENSOR", 50)) if CP.enableBsm: signals += [ ("L_ADJACENT", "BSM", 0), ("L_APPROACHING", "BSM", 0), ("R_ADJACENT", "BSM", 0), ("R_APPROACHING", "BSM", 0), ] checks += [("BSM", 1)] if Params().get('dp_toyota_zss') == b'1': signals += [("ZORRO_STEER", "SECONDARY_STEER_ANGLE", 0)] return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
def __init__(self, dbc_name, CP, VM): self.car_fingerprint = CP.carFingerprint self.packer = CANPacker(dbc_name) self.accel_steady = 0 self.apply_steer_last = 0 self.steer_rate_limited = False self.lkas11_cnt = 0 self.scc12_cnt = 0 self.resume_cnt = 0 self.last_lead_distance = 0 self.resume_wait_timer = 0 self.last_resume_frame = 0 self.lanechange_manual_timer = 0 self.emergency_manual_timer = 0 self.driver_steering_torque_above_timer = 0 self.mode_change_timer = 0 self.params = Params() self.mode_change_switch = int(self.params.get('CruiseStatemodeSelInit')) self.opkr_variablecruise = int(self.params.get('OpkrVariableCruise')) self.opkr_autoresume = int(self.params.get('OpkrAutoResume')) self.opkr_autoresumeoption = int(self.params.get('OpkrAutoResumeOption')) self.opkr_maxanglelimit = int(self.params.get('OpkrMaxAngleLimit')) self.steer_mode = "" self.mdps_status = "" self.lkas_switch = "" self.timer1 = tm.CTime1000("time") self.SC = Spdctrl() self.model_speed = 0 self.model_sum = 0 self.dRele = 0 self.yRele = 0 self.vRele = 0 self.cruise_gap = 0.0 self.cruise_gap_prev = 0 self.cruise_gap_set_init = 0 self.cruise_gap_switch_timer = 0 self.lkas_button_on = True self.longcontrol = CP.openpilotLongitudinalControl self.scc_live = not CP.radarOffCan if CP.lateralTuning.which() == 'pid': self.str_log2 = 'TUNE={:0.2f}/{:0.3f}/{:0.5f}'.format(CP.lateralTuning.pid.kpV[1], CP.lateralTuning.pid.kiV[1], CP.lateralTuning.pid.kf) elif CP.lateralTuning.which() == 'indi': self.str_log2 = 'TUNE={:03.1f}/{:03.1f}/{:03.1f}/{:03.1f}'.format(CP.lateralTuning.indi.innerLoopGain, CP.lateralTuning.indi.outerLoopGain, CP.lateralTuning.indi.timeConstant, CP.lateralTuning.indi.actuatorEffectiveness) elif CP.lateralTuning.which() == 'lqr': self.str_log2 = 'TUNE={:04.0f}/{:05.3f}/{:06.4f}'.format(CP.lateralTuning.lqr.scale, CP.lateralTuning.lqr.ki, CP.lateralTuning.lqr.dcGain) if CP.spasEnabled: self.en_cnt = 0 self.apply_steer_ang = 0.0 self.en_spas = 3 self.mdps11_stat_last = 0 self.spas_always = False
def fingerprint(logcan, sendcan, has_relay): fixed_fingerprint = os.environ.get('FINGERPRINT', "") skip_fw_query = os.environ.get('SKIP_FW_QUERY', False) if has_relay and not fixed_fingerprint and not skip_fw_query: # Vin query only reliably works thorugh OBDII bus = 1 cached_params = Params().get("CarParamsCache") if cached_params is not None: cached_params = car.CarParams.from_bytes(cached_params) if cached_params.carName == "mock": cached_params = None if cached_params is not None and len( cached_params.carFw ) > 0 and cached_params.carVin is not VIN_UNKNOWN: cloudlog.warning("Using cached CarParams") vin = cached_params.carVin car_fw = list(cached_params.carFw) else: cloudlog.warning("Getting VIN & FW versions") _, vin = get_vin(logcan, sendcan, bus) car_fw = get_fw_versions(logcan, sendcan, bus) fw_candidates = match_fw_to_car(car_fw) else: vin = VIN_UNKNOWN fw_candidates, car_fw = set(), [] cloudlog.warning("VIN %s", vin) Params().put("CarVin", vin) finger = gen_empty_fingerprint() candidate_cars = {i: all_known_cars() for i in [0]} # attempt fingerprint on bus 0 only frame = 0 frame_fingerprint = 10 # 0.1s car_fingerprint = None done = False while not done: a = get_one_can(logcan) for can in a.can: # need to independently try to fingerprint both bus 0 and 1 to work # for the combo black_panda and honda_bosch. Ignore extended messages # and VIN query response. # Include bus 2 for toyotas to disambiguate cars using camera messages # (ideally should be done for all cars but we can't for Honda Bosch) if can.src in range(0, 4): finger[can.src][can.address] = len(can.dat) for b in candidate_cars: if (can.src == b or (only_toyota_left(candidate_cars[b]) and can.src == 2)) and \ can.address < 0x800 and can.address not in [0x7df, 0x7e0, 0x7e8]: candidate_cars[b] = eliminate_incompatible_cars( can, candidate_cars[b]) # if we only have one car choice and the time since we got our first # message has elapsed, exit for b in candidate_cars: # Toyota needs higher time to fingerprint, since DSU does not broadcast immediately if only_toyota_left(candidate_cars[b]): frame_fingerprint = 100 # 1s if len(candidate_cars[b]) == 1: if frame > frame_fingerprint: # fingerprint done car_fingerprint = candidate_cars[b][0] # bail if no cars left or we've been waiting for more than 2s failed = all(len(cc) == 0 for cc in candidate_cars.values()) or frame > 200 succeeded = car_fingerprint is not None done = failed or succeeded frame += 1 source = car.CarParams.FingerprintSource.can # If FW query returns exactly 1 candidate, use it if len(fw_candidates) == 1: car_fingerprint = list(fw_candidates)[0] source = car.CarParams.FingerprintSource.fw if fixed_fingerprint: car_fingerprint = fixed_fingerprint source = car.CarParams.FingerprintSource.fixed cloudlog.warning("fingerprinted %s", car_fingerprint) return car_fingerprint, finger, vin, car_fw, source
if car_model not in tested_cars: print("***** WARNING: %s not tested *****" % car_model) print("Preparing processes") for p in tested_procs: manager.prepare_managed_process(p) results = {} for route, checks in routes.items(): if route not in non_public_routes: get_route_logs(route) elif "UNLOGGER_PATH" not in os.environ: continue shutil.rmtree('/data/params') params = Params() params.manager_start() params.put("OpenpilotEnabledToggle", "1") params.put("CommunityFeaturesToggle", "1") params.put("Passive", "1" if route in passive_routes else "0") print("testing ", route, " ", checks['carFingerprint']) print("Starting processes") for p in tested_procs: manager.start_managed_process(p) # Start unlogger print("Start unlogger") if route in non_public_routes: unlogger_cmd = [os.path.join(BASEDIR, os.environ['UNLOGGER_PATH']), route] else:
class CarController(): def __init__(self, dbc_name, CP, VM): self.CP = CP self.apply_steer_last = 0 self.car_fingerprint = CP.carFingerprint self.packer = CANPacker(dbc_name) self.steer_rate_limited = False self.resume_cnt = 0 self.last_resume_frame = 0 self.last_lead_distance = 0 self.lanechange_manual_timer = 0 self.emergency_manual_timer = 0 self.driver_steering_torque_above_timer = 0 self.mode_change_timer = 0 self.steer_mode = "" self.mdps_status = "" self.lkas_switch = "" self.lkas11_cnt = 0 self.nBlinker = 0 self.dRel = 0 self.yRel = 0 self.vRel = 0 self.timer1 = tm.CTime1000("time") self.model_speed = 0 self.model_sum = 0 # hud self.hud_timer_left = 0 self.hud_timer_right = 0 self.command_cnt = 0 self.command_load = 0 self.params = Params() # param self.param_preOpkrAccelProfile = -1 self.param_OpkrAccelProfile = 0 self.param_OpkrAutoResume = 0 self.SC = None self.traceCC = trace1.Loger("CarController") self.res_cnt = 7 self.res_delay = 0 self.params = Params() self.mode_change_switch = int( self.params.get('CruiseStatemodeSelInit')) def process_hud_alert(self, enabled, CC): visual_alert = CC.hudControl.visualAlert left_lane = CC.hudControl.leftLaneVisible right_lane = CC.hudControl.rightLaneVisible sys_warning = (visual_alert == VisualAlert.steerRequired) if left_lane: self.hud_timer_left = 100 if right_lane: self.hud_timer_right = 100 if self.hud_timer_left: self.hud_timer_left -= 1 if self.hud_timer_right: self.hud_timer_right -= 1 # initialize to no line visible sys_state = 1 if self.hud_timer_left and self.hud_timer_right or sys_warning: # HUD alert only display when LKAS status is active if enabled or sys_warning: sys_state = 3 else: sys_state = 4 elif self.hud_timer_left: sys_state = 5 elif self.hud_timer_right: sys_state = 6 return sys_warning, sys_state def param_load(self): self.command_cnt += 1 if self.command_cnt > 100: self.command_cnt = 0 if self.command_cnt % 10: return self.command_load += 1 if self.command_load == 1: self.param_OpkrAccelProfile = int( self.params.get('OpkrAccelProfile')) elif self.command_load == 2: self.param_OpkrAutoResume = int(self.params.get('OpkrAutoResume')) else: self.command_load = 0 # speed controller if self.param_preOpkrAccelProfile != self.param_OpkrAccelProfile: self.param_preOpkrAccelProfile = self.param_OpkrAccelProfile if self.param_OpkrAccelProfile == 1: self.SC = SpdctrlSlow() elif self.param_OpkrAccelProfile == 2: self.SC = SpdctrlNormal() else: self.SC = SpdctrlFast() 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) > 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 get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # pylint: disable=dangerous-default-value ret = CarInterfaceBase.get_std_params(candidate, fingerprint) ret.carName = "hyundai" ret.safetyConfigs = [ get_safety_config(car.CarParams.SafetyModel.hyundai, 0) ] ret.radarOffCan = RADAR_START_ADDR not in fingerprint[1] # WARNING: disabling radar also disables AEB (and we show the same warning on the instrument cluster as if you manually disabled AEB) ret.openpilotLongitudinalControl = Params().get_bool( "DisableRadar") and candidate in [ CAR.SONATA, CAR.SONATA_HYBRID, CAR.PALISADE, CAR.SANTA_FE ] ret.pcmCruise = not ret.openpilotLongitudinalControl ret.steerActuatorDelay = 0.1 # Default delay ret.steerRateCost = 0.5 ret.steerLimitTimer = 0.4 tire_stiffness_factor = 1. ret.stoppingControl = True ret.vEgoStopping = 1.0 ret.longitudinalTuning.kpV = [0.1] ret.longitudinalTuning.kiV = [0.0] ret.stopAccel = 0.0 ret.startAccel = 0.0 ret.longitudinalActuatorDelayUpperBound = 1.0 # s if candidate in [CAR.SANTA_FE, CAR.SANTA_FE_2022]: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.766 # Values from optimizer ret.steerRatio = 16.55 # 13.8 is spec end-to-end tire_stiffness_factor = 0.82 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[ 9., 22. ], [9., 22.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[ 0.2, 0.35 ], [0.05, 0.09]] elif candidate in [CAR.SONATA, CAR.SONATA_HYBRID]: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1513. + STD_CARGO_KG ret.wheelbase = 2.84 ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable tire_stiffness_factor = 0.65 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.SONATA_LF: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 4497. * CV.LB_TO_KG ret.wheelbase = 2.804 ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.PALISADE: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1999. + STD_CARGO_KG ret.wheelbase = 2.90 ret.steerRatio = 15.6 * 1.15 tire_stiffness_factor = 0.63 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]] elif candidate in [CAR.ELANTRA, CAR.ELANTRA_GT_I30]: ret.lateralTuning.pid.kf = 0.00006 ret.mass = 1275. + STD_CARGO_KG ret.wheelbase = 2.7 ret.steerRatio = 15.4 # 14 is Stock | Settled Params Learner values are steerRatio: 15.401566348670535 tire_stiffness_factor = 0.385 # stiffnessFactor settled on 1.0081302973865127 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] ret.minSteerSpeed = 32 * CV.MPH_TO_MS elif candidate == CAR.ELANTRA_2021: ret.lateralTuning.pid.kf = 0.00005 ret.mass = (2800. * CV.LB_TO_KG) + STD_CARGO_KG ret.wheelbase = 2.72 ret.steerRatio = 12.9 tire_stiffness_factor = 0.65 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.ELANTRA_HEV_2021: ret.lateralTuning.pid.kf = 0.00005 ret.mass = (3017. * CV.LB_TO_KG) + STD_CARGO_KG ret.wheelbase = 2.72 ret.steerRatio = 12.9 tire_stiffness_factor = 0.65 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.HYUNDAI_GENESIS: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 2060. + STD_CARGO_KG ret.wheelbase = 3.01 ret.steerRatio = 16.5 ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGainBP = [0.] ret.lateralTuning.indi.innerLoopGainV = [3.5] ret.lateralTuning.indi.outerLoopGainBP = [0.] ret.lateralTuning.indi.outerLoopGainV = [2.0] ret.lateralTuning.indi.timeConstantBP = [0.] ret.lateralTuning.indi.timeConstantV = [1.4] ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] ret.lateralTuning.indi.actuatorEffectivenessV = [2.3] ret.minSteerSpeed = 60 * CV.KPH_TO_MS elif candidate in [CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV]: ret.lateralTuning.pid.kf = 0.00005 ret.mass = { CAR.KONA_EV: 1685., CAR.KONA_HEV: 1425. }.get(candidate, 1275.) + STD_CARGO_KG ret.wheelbase = 2.7 ret.steerRatio = 13.73 * 1.15 # Spec tire_stiffness_factor = 0.385 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate in [ CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV ]: ret.lateralTuning.pid.kf = 0.00006 ret.mass = 1490. + STD_CARGO_KG # weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx ret.wheelbase = 2.7 ret.steerRatio = 13.73 # Spec tire_stiffness_factor = 0.385 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] if candidate not in [CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV]: ret.minSteerSpeed = 32 * CV.MPH_TO_MS elif candidate == CAR.VELOSTER: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 3558. * CV.LB_TO_KG ret.wheelbase = 2.80 ret.steerRatio = 13.75 * 1.15 tire_stiffness_factor = 0.5 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] # Kia elif candidate == CAR.KIA_SORENTO: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1985. + STD_CARGO_KG ret.wheelbase = 2.78 ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate in [ CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021 ]: ret.lateralTuning.pid.kf = 0.00006 ret.mass = 1737. + STD_CARGO_KG ret.wheelbase = 2.7 ret.steerRatio = 13.9 if CAR.KIA_NIRO_HEV_2021 else 13.73 # Spec tire_stiffness_factor = 0.385 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] if candidate == CAR.KIA_NIRO_HEV: ret.minSteerSpeed = 32 * CV.MPH_TO_MS elif candidate == CAR.KIA_SELTOS: ret.mass = 1337. + STD_CARGO_KG ret.wheelbase = 2.63 ret.steerRatio = 14.56 tire_stiffness_factor = 1 ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGainBP = [0.] ret.lateralTuning.indi.innerLoopGainV = [4.] ret.lateralTuning.indi.outerLoopGainBP = [0.] ret.lateralTuning.indi.outerLoopGainV = [3.] ret.lateralTuning.indi.timeConstantBP = [0.] ret.lateralTuning.indi.timeConstantV = [1.4] ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] ret.lateralTuning.indi.actuatorEffectivenessV = [1.8] elif candidate in [CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_H]: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 3558. * CV.LB_TO_KG ret.wheelbase = 2.80 ret.steerRatio = 13.75 tire_stiffness_factor = 0.5 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.KIA_STINGER: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1825. + STD_CARGO_KG ret.wheelbase = 2.78 ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.KIA_FORTE: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 3558. * CV.LB_TO_KG ret.wheelbase = 2.80 ret.steerRatio = 13.75 tire_stiffness_factor = 0.5 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.KIA_CEED: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1450. + STD_CARGO_KG ret.wheelbase = 2.65 ret.steerRatio = 13.75 tire_stiffness_factor = 0.5 ret.lateralTuning.pid.kf = 0.00005 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.KIA_K5_2021: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 3228. * CV.LB_TO_KG ret.wheelbase = 2.85 ret.steerRatio = 13.27 # 2021 Kia K5 Steering Ratio (all trims) tire_stiffness_factor = 0.5 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] # Genesis elif candidate == CAR.GENESIS_G70: ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGainBP = [0.] ret.lateralTuning.indi.innerLoopGainV = [2.5] ret.lateralTuning.indi.outerLoopGainBP = [0.] ret.lateralTuning.indi.outerLoopGainV = [3.5] ret.lateralTuning.indi.timeConstantBP = [0.] ret.lateralTuning.indi.timeConstantV = [1.4] ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] ret.lateralTuning.indi.actuatorEffectivenessV = [1.8] ret.steerActuatorDelay = 0.1 ret.mass = 1640.0 + STD_CARGO_KG ret.wheelbase = 2.84 ret.steerRatio = 13.56 elif candidate == CAR.GENESIS_G80: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 2060. + STD_CARGO_KG ret.wheelbase = 3.01 ret.steerRatio = 16.5 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]] elif candidate == CAR.GENESIS_G90: ret.mass = 2200 ret.wheelbase = 3.15 ret.steerRatio = 12.069 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]] # these cars require a special panda safety mode due to missing counters and checksums in the messages if candidate in [ CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA ]: ret.safetyConfigs = [ get_safety_config(car.CarParams.SafetyModel.hyundaiLegacy) ] # set appropriate safety param for gas signal if candidate in HYBRID_CAR: ret.safetyConfigs[0].safetyParam = 2 elif candidate in EV_CAR: ret.safetyConfigs[0].safetyParam = 1 ret.centerToFront = ret.wheelbase * 0.4 # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness( ret.mass, ret.wheelbase, ret.centerToFront, tire_stiffness_factor=tire_stiffness_factor) ret.enableBsm = 0x58b in fingerprint[0] if ret.openpilotLongitudinalControl: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_LONG return ret
def main(): os.environ['PARAMS_PATH'] = PARAMS if ANDROID: # the flippening! os.system( 'LD_LIBRARY_PATH="" content insert --uri content://settings/system --bind name:s:user_rotation --bind value:i:1' ) # disable bluetooth os.system('service call bluetooth_manager 8') params = Params() params.manager_start() default_params = [ ("CommunityFeaturesToggle", "0"), ("CompletedTrainingVersion", "0"), ("IsRHD", "0"), ("IsMetric", "0"), ("RecordFront", "0"), ("HasAcceptedTerms", "0"), ("HasCompletedSetup", "0"), ("IsUploadRawEnabled", "1"), ("IsLdwEnabled", "1"), ("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')), ("OpenpilotEnabledToggle", "1"), ("LaneChangeEnabled", "1"), ("IsDriverViewEnabled", "0"), ] # set unset params for k, v in default_params: if params.get(k) is None: params.put(k, v) # is this chffrplus? if os.getenv("PASSIVE") is not None: params.put("Passive", str(int(os.getenv("PASSIVE")))) if params.get("Passive") is None: raise Exception("Passive must be set to continue") if ANDROID: update_apks() manager_init() manager_prepare(spinner) spinner.close() if os.getenv("PREPAREONLY") is not None: return # SystemExit on sigterm signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(1)) try: manager_thread() except Exception: traceback.print_exc() crash.capture_exception() finally: cleanup_all_processes(None, None) if params.get("DoUninstall", encoding='utf8') == "1": uninstall()
def getSshAuthorizedKeys(): return Params().get("GithubSshKeys", encoding='utf8') or ''
def main(sm=None, pm=None): gc.disable() set_realtime_priority(5) if sm is None: sm = messaging.SubMaster(['liveLocationKalman', 'carState'], poll=['liveLocationKalman']) if pm is None: pm = messaging.PubMaster(['liveParameters']) params_reader = Params() # wait for stats about the car to come in from controls cloudlog.info("paramsd is waiting for CarParams") CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True)) cloudlog.info("paramsd got CarParams") min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio params = params_reader.get("LiveParameters") if not params_reader.get_bool( 'dp_reset_live_param_on_start') else None # Check if car model matches if params is not None: params = json.loads(params) if params.get('carFingerprint', None) != CP.carFingerprint: cloudlog.info("Parameter learner found parameters for wrong car.") params = None # Check if starting values are sane if params is not None: try: angle_offset_sane = abs(params.get('angleOffsetAverageDeg')) < 10.0 steer_ratio_sane = min_sr <= params['steerRatio'] <= max_sr params_sane = angle_offset_sane and steer_ratio_sane if not params_sane: cloudlog.info(f"Invalid starting values found {params}") params = None except Exception as e: cloudlog.info(f"Error reading params {params}: {str(e)}") params = None # TODO: cache the params with the capnp struct if params is None: params = { 'carFingerprint': CP.carFingerprint, 'steerRatio': CP.steerRatio, 'stiffnessFactor': 1.0, 'angleOffsetAverageDeg': 0.0, } cloudlog.info("Parameter learner resetting to default values") # When driving in wet conditions the stiffness can go down, and then be too low on the next drive # Without a way to detect this we have to reset the stiffness every drive params['stiffnessFactor'] = 1.0 learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverageDeg'])) angle_offset_average = params['angleOffsetAverageDeg'] angle_offset = angle_offset_average while True: sm.update() for which in sorted(sm.updated.keys(), key=lambda x: sm.logMonoTime[x]): if sm.updated[which]: t = sm.logMonoTime[which] * 1e-9 learner.handle_log(t, which, sm[which]) if sm.updated['liveLocationKalman']: x = learner.kf.x P = np.sqrt(learner.kf.P.diagonal()) if not all(map(math.isfinite, x)): cloudlog.error( "NaN in liveParameters estimate. Resetting to default values" ) learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0) x = learner.kf.x angle_offset_average = clip( math.degrees(x[States.ANGLE_OFFSET]), angle_offset_average - MAX_ANGLE_OFFSET_DELTA, angle_offset_average + MAX_ANGLE_OFFSET_DELTA) angle_offset = clip( math.degrees(x[States.ANGLE_OFFSET] + x[States.ANGLE_OFFSET_FAST]), angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA) msg = messaging.new_message('liveParameters') msg.logMonoTime = sm.logMonoTime['carState'] msg.liveParameters.posenetValid = True msg.liveParameters.sensorValid = True msg.liveParameters.steerRatio = float(x[States.STEER_RATIO]) msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS]) msg.liveParameters.angleOffsetAverageDeg = angle_offset_average msg.liveParameters.angleOffsetDeg = angle_offset msg.liveParameters.valid = all(( abs(msg.liveParameters.angleOffsetAverageDeg) < 10.0, abs(msg.liveParameters.angleOffsetDeg) < 10.0, 0.2 <= msg.liveParameters.stiffnessFactor <= 5.0, min_sr <= msg.liveParameters.steerRatio <= max_sr, )) msg.liveParameters.steerRatioStd = float(P[States.STEER_RATIO]) msg.liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS]) msg.liveParameters.angleOffsetAverageStd = float( P[States.ANGLE_OFFSET]) msg.liveParameters.angleOffsetFastStd = float( P[States.ANGLE_OFFSET_FAST]) if sm.frame % 1200 == 0: # once a minute params = { 'carFingerprint': CP.carFingerprint, 'steerRatio': msg.liveParameters.steerRatio, 'stiffnessFactor': msg.liveParameters.stiffnessFactor, 'angleOffsetAverageDeg': msg.liveParameters.angleOffsetAverageDeg, } put_nonblocking("LiveParameters", json.dumps(params)) pm.send('liveParameters', msg)
def manager_thread(): # now loop thermal_sock = messaging.sub_sock('thermal') cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) # save boot log subprocess.call(["./loggerd", "--bootlog"], cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) params = Params() # start daemon processes for p in daemon_processes: start_daemon_process(p) # start persistent processes for p in persistent_processes: start_managed_process(p) # start offroad if ANDROID: pm_apply_packages('enable') start_offroad() if os.getenv("NOBOARD") is None: start_managed_process("pandad") if os.getenv("BLOCK") is not None: for k in os.getenv("BLOCK").split(","): del managed_processes[k] started_prev = False logger_dead = False while 1: msg = messaging.recv_sock(thermal_sock, wait=True) # heavyweight batch processes are gated on favorable thermal conditions if msg.thermal.thermalStatus >= ThermalStatus.yellow: for p in green_temp_processes: if p in persistent_processes: kill_managed_process(p) else: for p in green_temp_processes: if p in persistent_processes: start_managed_process(p) if msg.thermal.freeSpace < 0.05: logger_dead = True if msg.thermal.started: for p in car_started_processes: if p == "loggerd" and logger_dead: kill_managed_process(p) else: start_managed_process(p) else: logger_dead = False driver_view = params.get("IsDriverViewEnabled") == b"1" # TODO: refactor how manager manages processes for p in reversed(car_started_processes): if p not in driver_view_processes or not driver_view: kill_managed_process(p) for p in driver_view_processes: if driver_view: start_managed_process(p) else: kill_managed_process(p) # trigger an update after going offroad if started_prev: send_managed_process_signal("updated", signal.SIGHUP) started_prev = msg.thermal.started # check the status of all processes, did any of them die? running_list = [ "%s%s\u001b[0m" % ("\u001b[32m" if running[p].is_alive() else "\u001b[31m", p) for p in running ] cloudlog.debug(' '.join(running_list)) # Exit main loop when uninstall is needed if params.get("DoUninstall", encoding='utf8') == "1": break
def dmonitoringd_thread(sm=None, pm=None): if pm is None: pm = messaging.PubMaster(['dMonitoringState']) if sm is None: sm = messaging.SubMaster([ 'driverState', 'liveCalibration', 'carState', 'controlsState', 'model' ], poll=['driverState']) driver_status = DriverStatus() driver_status.is_rhd_region = Params().get("IsRHD") == b"1" offroad = Params().get("IsOffroad") == b"1" sm['liveCalibration'].calStatus = Calibration.INVALID sm['liveCalibration'].rpyCalib = [0, 0, 0] sm['carState'].vEgo = 0. sm['carState'].cruiseState.speed = 0. sm['carState'].buttonEvents = [] sm['carState'].steeringPressed = False sm['carState'].gasPressed = False sm['carState'].standstill = True v_cruise_last = 0 driver_engaged = False # 10Hz <- dmonitoringmodeld while True: sm.update() if not sm.updated['driverState']: continue butpressed = False # Get interaction if sm.updated['carState']: v_cruise = sm['carState'].cruiseState.speed for b in sm['carState'].buttonEvents: butpressed = b.pressed driver_engaged = butpressed or \ v_cruise != v_cruise_last or \ sm['carState'].steeringPressed or \ sm['carState'].gasPressed if driver_engaged: driver_status.update(Events(), True, sm['controlsState'].enabled, sm['carState'].standstill) v_cruise_last = v_cruise if sm.updated['model']: driver_status.set_policy(sm['model']) # Get data from dmonitoringmodeld events = Events() driver_status.get_pose(sm['driverState'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled) # Block engaging after max number of distrations if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION: events.add(car.CarEvent.EventName.tooDistracted) # Update events from driver state driver_status.update(events, driver_engaged, sm['controlsState'].enabled, sm['carState'].standstill) # build dMonitoringState packet dat = messaging.new_message('dMonitoringState') dat.dMonitoringState = { "events": events.to_msg(), "faceDetected": driver_status.face_detected, "isDistracted": driver_status.driver_distracted, "awarenessStatus": driver_status.awareness, "isRHD": driver_status.is_rhd_region, "posePitchOffset": driver_status.pose.pitch_offseter.filtered_stat.mean(), "posePitchValidCount": driver_status.pose.pitch_offseter.filtered_stat.n, "poseYawOffset": driver_status.pose.yaw_offseter.filtered_stat.mean(), "poseYawValidCount": driver_status.pose.yaw_offseter.filtered_stat.n, "stepChange": driver_status.step_change, "awarenessActive": driver_status.awareness_active, "awarenessPassive": driver_status.awareness_passive, "isLowStd": driver_status.pose.low_std, "hiStdCount": driver_status.hi_stds, "isPreview": offroad, } pm.send('dMonitoringState', dat)
def manager_init(): # update system time from panda set_time(cloudlog) params = Params() params.clear_all(ParamKeyType.CLEAR_ON_MANAGER_START) default_params = [ ("CompletedTrainingVersion", "0"), ("HasAcceptedTerms", "0"), ("OpenpilotEnabledToggle", "1"), ("IsMetric", "1"), ("CommunityFeaturesToggle", "1"), ("EndToEndToggle", "1"), ("IsOpenpilotViewEnabled", "0"), ("OpkrAutoShutdown", "2"), ("OpkrForceShutdown", "5"), ("OpkrAutoScreenOff", "0"), ("OpkrUIBrightness", "0"), ("OpkrUIBrightness", "0"), ("OpkrUIVolumeBoost", "0"), ("OpkrEnableDriverMonitoring", "1"), ("OpkrEnableLogger", "0"), ("OpkrEnableUploader", "0"), ("OpkrEnableGetoffAlert", "1"), ("OpkrAutoResume", "1"), ("OpkrVariableCruise", "1"), ("OpkrLaneChangeSpeed", "45"), ("OpkrAutoLaneChangeDelay", "0"), ("OpkrSteerAngleCorrection", "0"), ("PutPrebuiltOn", "1"), ("LdwsCarFix", "0"), ("LateralControlMethod", "0"), ("CruiseStatemodeSelInit", "1"), ("InnerLoopGain", "35"), ("OuterLoopGain", "20"), ("TimeConstant", "14"), ("ActuatorEffectiveness", "20"), ("Scale", "1500"), ("LqrKi", "15"), ("DcGain", "27"), ("IgnoreZone", "0"), ("PidKp", "20"), ("PidKi", "40"), ("PidKd", "150"), ("PidKf", "7"), ("CameraOffsetAdj", "60"), ("SteerRatioAdj", "155"), ("SteerRatioMaxAdj", "175"), ("SteerActuatorDelayAdj", "20"), ("SteerRateCostAdj", "35"), ("SteerLimitTimerAdj", "80"), ("TireStiffnessFactorAdj", "100"), ("SteerMaxBaseAdj", "384"), ("SteerMaxAdj", "384"), ("SteerDeltaUpBaseAdj", "3"), ("SteerDeltaUpAdj", "3"), ("SteerDeltaDownBaseAdj", "7"), ("SteerDeltaDownAdj", "7"), ("SteerMaxvAdj", "10"), ("OpkrBatteryChargingControl", "1"), ("OpkrBatteryChargingMin", "70"), ("OpkrBatteryChargingMax", "80"), ("LeftCurvOffsetAdj", "0"), ("RightCurvOffsetAdj", "0"), ("DebugUi1", "0"), ("DebugUi2", "0"), ("LongLogDisplay", "0"), ("OpkrBlindSpotDetect", "1"), ("OpkrMaxAngleLimit", "90"), ("OpkrSpeedLimitOffset", "0"), ("OpkrLiveSteerRatio", "1"), ("OpkrVariableSteerMax", "1"), ("OpkrVariableSteerDelta", "0"), ("FingerprintTwoSet", "0"), ("OpkrVariableCruiseProfile", "1"), ("OpkrLiveTune", "0"), ("OpkrDrivingRecord", "0"), ("OpkrTurnSteeringDisable", "0"), ("CarModel", ""), ("CarModelAbb", ""), ("OpkrHotspotOnBoot", "0"), ("OpkrSSHLegacy", "1"), ("ShaneFeedForward", "0"), ("CruiseOverMaxSpeed", "0"), ("JustDoGearD", "0"), ("LanelessMode", "0"), ("ComIssueGone", "0"), ("MaxSteer", "384"), ("MaxRTDelta", "112"), ("MaxRateUp", "3"), ("MaxRateDown", "7"), ("SteerThreshold", "150"), ("RecordingCount", "100"), ("RecordingQuality", "1"), ("CruiseGapAdjust", "0"), ("AutoEnable", "1"), ("CruiseAutoRes", "0"), ("AutoResOption", "0"), ("SteerWindDown", "0"), ("OpkrMonitoringMode", "0"), ("OpkrMonitorEyesThreshold", "75"), ("OpkrMonitorNormalEyesThreshold", "50"), ("OpkrMonitorBlinkThreshold", "50"), ("MadModeEnabled", "1"), ("OpkrFanSpeedGain", "0"), ("WhitePandaSupport", "0"), ("SteerWarningFix", "0"), ("OpkrRunNaviOnBoot", "0"), ("OpkrApksEnable", "0"), ("CruiseGap1", "10"), ("CruiseGap2", "12"), ("CruiseGap3", "15"), ("CruiseGap4", "20"), ("DynamicTR", "2"), ("OpkrBattLess", "0"), ("LCTimingFactorUD", "0"), ("LCTimingFactor30", "10"), ("LCTimingFactor60", "20"), ("LCTimingFactor80", "70"), ("LCTimingFactor110", "110"), ] if not PC: default_params.append(("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8'))) if params.get_bool("RecordFrontLock"): params.put_bool("RecordFront", True) # set unset params for k, v in default_params: if params.get(k) is None: params.put(k, v) # is this dashcam? if os.getenv("PASSIVE") is not None: params.put_bool("Passive", bool(int(os.getenv("PASSIVE")))) if params.get("Passive") is None: raise Exception("Passive must be set to continue") if EON and params.get_bool("OpkrApksEnable"): update_apks(show_spinner=True) os.umask(0) # Make sure we can create files with 777 permissions # Create folders needed for msgq try: os.mkdir("/dev/shm") except FileExistsError: pass except PermissionError: print("WARNING: failed to make /dev/shm") # set version params params.put("Version", version) params.put("TermsVersion", terms_version) params.put("TrainingVersion", training_version) params.put("GitCommit", get_git_commit(default="")) params.put("GitBranch", get_git_branch(default="")) params.put("GitRemote", get_git_remote(default="")) # set dongle id reg_res = register(show_spinner=True) if reg_res: dongle_id = reg_res elif not reg_res: dongle_id = "maintenance" else: serial = params.get("HardwareSerial") raise Exception(f"Registration failed for device {serial}") os.environ['DONGLE_ID'] = dongle_id # Needed for swaglog if not dirty: os.environ['CLEAN'] = '1' cloudlog.bind_global(dongle_id=dongle_id, version=version, dirty=dirty, device=HARDWARE.get_device_type()) if comma_remote and not (os.getenv("NOLOG") or os.getenv("NOCRASH") or PC): crash.init() # ensure shared libraries are readable by apks if EON and params.get_bool("OpkrApksEnable"): os.chmod(BASEDIR, 0o755) os.chmod("/dev/shm", 0o777) os.chmod(os.path.join(BASEDIR, "cereal"), 0o755) crash.bind_user(id=dongle_id) crash.bind_extra(dirty=dirty, origin=origin, branch=branch, commit=commit, device=HARDWARE.get_device_type()) os.system("/data/openpilot/gitcommit.sh")
def replay_process(cfg, lr): sub_sockets = [s for _, sub in cfg.pub_sub.items() for s in sub] pub_sockets = [s for s in cfg.pub_sub.keys() if s != 'can'] fsm = FakeSubMaster(pub_sockets) fpm = FakePubMaster(sub_sockets) args = (fsm, fpm) if 'can' in list(cfg.pub_sub.keys()): can_sock = FakeSocket() args = (fsm, fpm, can_sock) all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime) pub_msgs = [ msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys()) ] params = Params() params.clear_all() params.manager_start() params.put("OpenpilotEnabledToggle", "1") params.put("Passive", "0") params.put("CommunityFeaturesToggle", "1") os.environ['NO_RADAR_SLEEP'] = "1" manager.prepare_managed_process(cfg.proc_name) mod = importlib.import_module(manager.managed_processes[cfg.proc_name]) thread = threading.Thread(target=mod.main, args=args) thread.daemon = True thread.start() if cfg.init_callback is not None: if 'can' not in list(cfg.pub_sub.keys()): can_sock = None cfg.init_callback(all_msgs, fsm, can_sock) CP = car.CarParams.from_bytes(params.get("CarParams", block=True)) # wait for started process to be ready if 'can' in list(cfg.pub_sub.keys()): can_sock.wait_for_recv() else: fsm.wait_for_update() log_msgs, msg_queue = [], [] for msg in tqdm(pub_msgs): if cfg.should_recv_callback is not None: recv_socks, should_recv = cfg.should_recv_callback( msg, CP, cfg, fsm) else: recv_socks = [ s for s in cfg.pub_sub[msg.which()] if (fsm.frame + 1) % int(service_list[msg.which()].frequency / service_list[s].frequency) == 0 ] should_recv = bool(len(recv_socks)) if msg.which() == 'can': can_sock.send(msg.as_builder().to_bytes()) else: msg_queue.append(msg.as_builder()) if should_recv: fsm.update_msgs(0, msg_queue) msg_queue = [] recv_cnt = len(recv_socks) while recv_cnt > 0: m = fpm.wait_for_msg() log_msgs.append(m) recv_cnt -= m.which() in recv_socks return log_msgs
def main(): os.environ['PARAMS_PATH'] = PARAMS if ANDROID: # the flippening! os.system( 'LD_LIBRARY_PATH="" content insert --uri content://settings/system --bind name:s:user_rotation --bind value:i:1' ) # disable bluetooth os.system('service call bluetooth_manager 8') params = Params() params.manager_start() default_params = [ ("CommunityFeaturesToggle", "0"), ("CompletedTrainingVersion", "0"), ("IsRHD", "0"), ("IsMetric", "0"), ("RecordFront", "0"), ("HasAcceptedTerms", "0"), ("HasCompletedSetup", "0"), ("IsUploadRawEnabled", "1"), ("IsLdwEnabled", "1"), ("IsGeofenceEnabled", "-1"), ("SpeedLimitOffset", "0"), ("LongitudinalControl", "0"), ("LimitSetSpeed", "0"), ("LimitSetSpeedNeural", "0"), ("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')), ("OpenpilotEnabledToggle", "1"), ("LaneChangeEnabled", "1"), ("IsDriverViewEnabled", "0"), ("IsOpenpilotViewEnabled", "0"), ("OpkrAutoShutdown", "0"), ("OpkrAutoScreenOff", "0"), ("OpkrUIBrightness", "0"), ("OpkrEnableDriverMonitoring", "1"), ("OpkrEnableLogger", "0"), ("OpkrEnableGetoffAlert", "1"), ("OpkrAutoResume", "1"), ("OpkrAccelProfile", "0"), ("OpkrAutoLanechangedelay", "0"), ("PutPrebuiltOn", "0"), ("FingerprintIssuedFix", "0"), ("LdwsCarFix", "0"), ("LateralControlMethod", "0"), ("CruiseStatemodeSelInit", "0"), ("LateralControlPriority", "0"), ("OuterLoopGain", "20"), ("InnerLoopGain", "30"), ("TimeConstant", "10"), ("ActuatorEffectiveness", "15"), ("Scale", "1630"), ("LqrKi", "10"), ("DcGain", "28"), ("IgnoreZone", "0"), ("PidKp", "20"), ("PidKi", "40"), ("PidKf", "4"), ("CameraOffsetAdj", "60"), ("SteerRatioAdj", "130"), ("SteerActuatorDelayAdj", "20"), ("SteerRateCostAdj", "55"), ("SteerLimitTimerAdj", "240"), ("TireStiffnessFactorAdj", "100"), ("SteerMaxAdj", "430"), ("SteerDeltaUpAdj", "3"), ("SteerDeltaDownAdj", "6"), ] # set unset params for k, v in default_params: if params.get(k) is None: params.put(k, v) # is this chffrplus? if os.getenv("PASSIVE") is not None: params.put("Passive", str(int(os.getenv("PASSIVE")))) if params.get("Passive") is None: raise Exception("Passive must be set to continue") if ANDROID: update_apks() manager_init() manager_prepare(spinner) spinner.close() if os.getenv("PREPAREONLY") is not None: return # SystemExit on sigterm signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(1)) try: manager_thread() except Exception: traceback.print_exc() crash.capture_exception() finally: cleanup_all_processes(None, None) if params.get("DoUninstall", encoding='utf8') == "1": uninstall()
def thermald_thread(): pm = messaging.PubMaster(['thermal']) health_timeout = int(1000 * 2.5 * DT_TRML) # 2.5x the expected health frequency health_sock = messaging.sub_sock('health', timeout=health_timeout) location_sock = messaging.sub_sock('gpsLocation') fan_speed = 0 count = 0 startup_conditions = { "ignition": False, } startup_conditions_prev = startup_conditions.copy() off_ts = None started_ts = None started_seen = False thermal_status = ThermalStatus.green usb_power = True current_branch = get_git_branch() network_type = NetworkType.none network_strength = NetworkStrength.unknown current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML) cpu_temp_filter = FirstOrderFilter(0., CPU_TEMP_TAU, DT_TRML) health_prev = None should_start_prev = False handle_fan = None is_uno = False params = Params() power_monitor = PowerMonitoring() no_panda_cnt = 0 thermal_config = get_thermal_config() while 1: health = messaging.recv_sock(health_sock, wait=True) msg = read_thermal(thermal_config) if health is not None: usb_power = health.health.usbPowerMode != log.HealthData.UsbPowerMode.client # If we lose connection to the panda, wait 5 seconds before going offroad if health.health.hwType == log.HealthData.HwType.unknown: no_panda_cnt += 1 if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML: if startup_conditions["ignition"]: cloudlog.error("Lost panda connection while onroad") startup_conditions["ignition"] = False else: no_panda_cnt = 0 startup_conditions[ "ignition"] = health.health.ignitionLine or health.health.ignitionCan # Setup fan handler on first connect to panda if handle_fan is None and health.health.hwType != log.HealthData.HwType.unknown: is_uno = health.health.hwType == log.HealthData.HwType.uno if (not EON) or is_uno: cloudlog.info("Setting up UNO fan handler") handle_fan = handle_fan_uno else: cloudlog.info("Setting up EON fan handler") setup_eon_fan() handle_fan = handle_fan_eon # Handle disconnect if health_prev is not None: if health.health.hwType == log.HealthData.HwType.unknown and \ health_prev.health.hwType != log.HealthData.HwType.unknown: params.panda_disconnect() health_prev = health # get_network_type is an expensive call. update every 10s if (count % int(10. / DT_TRML)) == 0: try: network_type = HARDWARE.get_network_type() network_strength = HARDWARE.get_network_strength(network_type) except Exception: cloudlog.exception("Error getting network status") msg.thermal.freeSpace = get_available_percent(default=100.0) / 100.0 msg.thermal.memUsedPercent = int(round( psutil.virtual_memory().percent)) msg.thermal.cpuPerc = int(round(psutil.cpu_percent())) msg.thermal.networkType = network_type msg.thermal.networkStrength = network_strength msg.thermal.batteryPercent = HARDWARE.get_battery_capacity() msg.thermal.batteryStatus = HARDWARE.get_battery_status() msg.thermal.batteryCurrent = HARDWARE.get_battery_current() msg.thermal.batteryVoltage = HARDWARE.get_battery_voltage() msg.thermal.usbOnline = HARDWARE.get_usb_present() # Fake battery levels on uno for frame if (not EON) or is_uno: msg.thermal.batteryPercent = 100 msg.thermal.batteryStatus = "Charging" msg.thermal.bat = 0 current_filter.update(msg.thermal.batteryCurrent / 1e6) # TODO: add car battery voltage check max_cpu_temp = cpu_temp_filter.update(max(msg.thermal.cpu)) max_comp_temp = max(max_cpu_temp, msg.thermal.mem, max(msg.thermal.gpu)) bat_temp = msg.thermal.bat if handle_fan is not None: fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed, startup_conditions["ignition"]) msg.thermal.fanSpeed = fan_speed # If device is offroad we want to cool down before going onroad # since going onroad increases load and can make temps go over 107 # We only do this if there is a relay that prevents the car from faulting is_offroad_for_5_min = (started_ts is None) and ( (not started_seen) or (off_ts is None) or (sec_since_boot() - off_ts > 60 * 5)) if max_cpu_temp > 107. or bat_temp >= 63. or (is_offroad_for_5_min and max_cpu_temp > 70.0): # onroad not allowed thermal_status = ThermalStatus.danger elif max_comp_temp > 96.0 or bat_temp > 60.: # hysteresis between onroad not allowed and engage not allowed thermal_status = clip(thermal_status, ThermalStatus.red, ThermalStatus.danger) elif max_cpu_temp > 94.0: # hysteresis between engage not allowed and uploader not allowed thermal_status = clip(thermal_status, ThermalStatus.yellow, ThermalStatus.red) elif max_cpu_temp > 80.0: # uploader not allowed thermal_status = ThermalStatus.yellow elif max_cpu_temp > 75.0: # hysteresis between uploader not allowed and all good thermal_status = clip(thermal_status, ThermalStatus.green, ThermalStatus.yellow) else: thermal_status = ThermalStatus.green # default to good condition # **** starting logic **** # Check for last update time and display alerts if needed now = datetime.datetime.utcnow() # show invalid date/time alert startup_conditions["time_valid"] = (now.year > 2020) or ( now.year == 2020 and now.month >= 10) set_offroad_alert_if_changed("Offroad_InvalidTime", (not startup_conditions["time_valid"])) # Show update prompt try: last_update = datetime.datetime.fromisoformat( params.get("LastUpdateTime", encoding='utf8')) except (TypeError, ValueError): last_update = now dt = now - last_update update_failed_count = params.get("UpdateFailedCount") update_failed_count = 0 if update_failed_count is None else int( update_failed_count) last_update_exception = params.get("LastUpdateException", encoding='utf8') if update_failed_count > 15 and last_update_exception is not None: if current_branch in ["release2", "dashcam"]: extra_text = "Ensure the software is correctly installed" else: extra_text = last_update_exception set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) set_offroad_alert_if_changed("Offroad_UpdateFailed", True, extra_text=extra_text) elif dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1: set_offroad_alert_if_changed("Offroad_UpdateFailed", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", True) elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT: remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0)) set_offroad_alert_if_changed("Offroad_UpdateFailed", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining_time} days.") else: set_offroad_alert_if_changed("Offroad_UpdateFailed", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) startup_conditions["up_to_date"] = params.get( "Offroad_ConnectivityNeeded") is None or params.get( "DisableUpdates") == b"1" startup_conditions["not_uninstalling"] = not params.get( "DoUninstall") == b"1" startup_conditions["accepted_terms"] = params.get( "HasAcceptedTerms") == terms_version panda_signature = params.get("PandaFirmware") startup_conditions["fw_version_match"] = (panda_signature is None) or ( panda_signature == FW_SIGNATURE ) # don't show alert is no panda is connected (None) set_offroad_alert_if_changed( "Offroad_PandaFirmwareMismatch", (not startup_conditions["fw_version_match"])) # with 2% left, we killall, otherwise the phone will take a long time to boot startup_conditions["free_space"] = msg.thermal.freeSpace > 0.02 startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version or \ (current_branch in ['dashcam', 'dashcam-staging']) startup_conditions["not_driver_view"] = not params.get( "IsDriverViewEnabled") == b"1" startup_conditions["not_taking_snapshot"] = not params.get( "IsTakingSnapshot") == b"1" # if any CPU gets above 107 or the battery gets above 63, kill all processes # controls will warn with CPU above 95 or battery above 60 startup_conditions[ "device_temp_good"] = thermal_status < ThermalStatus.danger set_offroad_alert_if_changed( "Offroad_TemperatureTooHigh", (not startup_conditions["device_temp_good"])) startup_conditions[ "hardware_supported"] = health is not None and health.health.hwType not in [ log.HealthData.HwType.whitePanda, log.HealthData.HwType.greyPanda ] set_offroad_alert_if_changed( "Offroad_HardwareUnsupported", health is not None and not startup_conditions["hardware_supported"]) # Handle offroad/onroad transition should_start = all(startup_conditions.values()) if should_start: if not should_start_prev: params.delete("IsOffroad") off_ts = None if started_ts is None: started_ts = sec_since_boot() started_seen = True else: if startup_conditions["ignition"] and (startup_conditions != startup_conditions_prev): cloudlog.event("Startup blocked", startup_conditions=startup_conditions) if should_start_prev or (count == 0): params.put("IsOffroad", "1") started_ts = None if off_ts is None: off_ts = sec_since_boot() # Offroad power monitoring power_monitor.calculate(health) msg.thermal.offroadPowerUsage = power_monitor.get_power_used() msg.thermal.carBatteryCapacity = max( 0, power_monitor.get_car_battery_capacity()) # Check if we need to disable charging (handled by boardd) msg.thermal.chargingDisabled = power_monitor.should_disable_charging( health, off_ts) # Check if we need to shut down if power_monitor.should_shutdown(health, off_ts, started_seen, LEON): cloudlog.info(f"shutting device down, offroad since {off_ts}") # TODO: add function for blocking cloudlog instead of sleep time.sleep(10) os.system('LD_LIBRARY_PATH="" svc power shutdown') msg.thermal.chargingError = current_filter.x > 0. and msg.thermal.batteryPercent < 90 # if current is positive, then battery is being discharged msg.thermal.started = started_ts is not None msg.thermal.startedTs = int(1e9 * (started_ts or 0)) msg.thermal.thermalStatus = thermal_status pm.send("thermal", msg) set_offroad_alert_if_changed("Offroad_ChargeDisabled", (not usb_power)) should_start_prev = should_start startup_conditions_prev = startup_conditions.copy() # report to server once per minute if (count % int(60. / DT_TRML)) == 0: location = messaging.recv_sock(location_sock) cloudlog.event("STATUS_PACKET", count=count, health=(health.to_dict() if health else None), location=(location.gpsLocation.to_dict() if location else None), thermal=msg.to_dict()) count += 1
# # courtesy of pjlao307 (https://github.com/pjlao307/) # this is just his original implementation but # in openpilot service form so it's always on # # with the highest bit rates, the video is approx. 0.5MB per second # the default value is set to 2.56Mbps = 0.32MB per second # import os import time import datetime import cereal.messaging as messaging import subprocess from selfdrive.swaglog import cloudlog from common.params import Params params = Params() dashcam_videos = '/sdcard/dashcam/' duration = 60 # max is 180 bit_rates = 2560000 # max is 4000000 max_size_per_file = bit_rates / 8 * duration # 2.56Mbps / 8 * 60 = 19.2MB per 60 seconds max_storage = max_size_per_file / duration * 60 * 60 * 6 # 6 hours worth of footage (around 7gb) freespace_limit = 0.15 # we start cleaning up footage when freespace is below 15% def main(gctx=None): if not os.path.exists(dashcam_videos): os.makedirs(dashcam_videos) thermal_sock = messaging.sub_sock('thermal') while 1:
def manager_thread(): cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) # save boot log subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) # start daemon processes for p in daemon_processes: start_daemon_process(p) # start persistent processes for p in persistent_processes: start_managed_process(p) # start offroad if EON: pm_apply_packages('enable') start_offroad() if os.getenv("NOBOARD") is not None: del managed_processes["pandad"] if os.getenv("BLOCK") is not None: for k in os.getenv("BLOCK").split(","): del managed_processes[k] started_prev = False logger_dead = False params = Params() thermal_sock = messaging.sub_sock('thermal') pm = messaging.PubMaster(['managerState']) while 1: msg = messaging.recv_sock(thermal_sock, wait=True) if msg.thermal.freeSpacePercent < 0.05: logger_dead = True if msg.thermal.started: for p in car_started_processes: if p == "loggerd" and logger_dead: kill_managed_process(p) else: start_managed_process(p) else: logger_dead = False driver_view = params.get("IsDriverViewEnabled") == b"1" # TODO: refactor how manager manages processes for p in reversed(car_started_processes): if p not in driver_view_processes or not driver_view: kill_managed_process(p) for p in driver_view_processes: if driver_view: start_managed_process(p) else: kill_managed_process(p) # trigger an update after going offroad if started_prev: os.sync() send_managed_process_signal("updated", signal.SIGHUP) started_prev = msg.thermal.started # check the status of all processes, did any of them die? running_list = [ "%s%s\u001b[0m" % ("\u001b[32m" if running[p].is_alive() else "\u001b[31m", p) for p in running ] cloudlog.debug(' '.join(running_list)) # send managerState states = [] for p in managed_processes: state = log.ManagerState.ProcessState.new_message() state.name = p if p in running: state.running = running[p].is_alive() state.pid = running[p].pid state.exitCode = running[p].exitcode or 0 states.append(state) msg = messaging.new_message('managerState') msg.managerState.processes = states pm.send('managerState', msg) # Exit main loop when uninstall is needed if params.get("DoUninstall", encoding='utf8') == "1": break
def register(): params = Params() params.put("Version", version) params.put("TermsVersion", terms_version) params.put("TrainingVersion", training_version) params.put("GitCommit", get_git_commit()) params.put("GitBranch", get_git_branch()) params.put("GitRemote", get_git_remote()) params.put("SubscriberInfo", get_subscriber_info()) # create a key for auth # your private key is kept on your device persist partition and never sent to our servers # do not erase your persist partition if not os.path.isfile("/persist/comma/id_rsa.pub"): cloudlog.warning("generating your personal RSA key") mkdirs_exists_ok("/persist/comma") assert os.system( "openssl genrsa -out /persist/comma/id_rsa.tmp 2048") == 0 assert os.system( "openssl rsa -in /persist/comma/id_rsa.tmp -pubout -out /persist/comma/id_rsa.tmp.pub" ) == 0 os.rename("/persist/comma/id_rsa.tmp", "/persist/comma/id_rsa") os.rename("/persist/comma/id_rsa.tmp.pub", "/persist/comma/id_rsa.pub") # make key readable by app users (ai.comma.plus.offroad) os.chmod('/persist/comma/', 0o755) os.chmod('/persist/comma/id_rsa', 0o744) dongle_id, access_token = params.get("DongleId"), params.get("AccessToken") public_key = open("/persist/comma/id_rsa.pub").read() # create registration token # in the future, this key will make JWTs directly private_key = open("/persist/comma/id_rsa").read() # late import import jwt register_token = jwt.encode( { 'register': True, 'exp': datetime.utcnow() + timedelta(hours=1) }, private_key, algorithm='RS256') try: cloudlog.info("getting pilotauth") resp = api_get("v2/pilotauth/", method='POST', timeout=15, imei=get_imei(), serial=get_serial(), public_key=public_key, register_token=register_token) dongleauth = json.loads(resp.text) dongle_id, access_token = dongleauth["dongle_id"].encode( 'ascii'), dongleauth["access_token"].encode('ascii') params.put("DongleId", dongle_id) params.put("AccessToken", access_token) return dongle_id, access_token except Exception: cloudlog.exception("failed to authenticate") if dongle_id is not None and access_token is not None: return dongle_id, access_token else: return None
def main(): params = Params() params.manager_start() default_params = [ ("CommunityFeaturesToggle", "0"), ("CompletedTrainingVersion", "0"), ("IsRHD", "0"), ("IsMetric", "0"), ("RecordFront", "0"), ("HasAcceptedTerms", "0"), ("HasCompletedSetup", "0"), ("IsUploadRawEnabled", "1"), ("IsLdwEnabled", "1"), ("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')), ("OpenpilotEnabledToggle", "1"), ("VisionRadarToggle", "0"), ("LaneChangeEnabled", "1"), ("IsDriverViewEnabled", "0"), ] # set unset params for k, v in default_params: if params.get(k) is None: params.put(k, v) # is this dashcam? if os.getenv("PASSIVE") is not None: params.put("Passive", str(int(os.getenv("PASSIVE")))) if params.get("Passive") is None: raise Exception("Passive must be set to continue") if EON: update_apks() manager_init() manager_prepare() spinner.close() if os.getenv("PREPAREONLY") is not None: return # SystemExit on sigterm signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(1)) try: manager_thread() except Exception: traceback.print_exc() crash.capture_exception() finally: cleanup_all_processes(None, None) if params.get("DoUninstall", encoding='utf8') == "1": cloudlog.warning("uninstalling") HARDWARE.uninstall()
def __init__(self, sm=None, pm=None, can_sock=None): config_realtime_process(3, Priority.CTRL_HIGH) # Setup sockets self.pm = pm if self.pm is None: self.pm = messaging.PubMaster([ 'sendcan', 'controlsState', 'carState', 'carControl', 'carEvents', 'carParams' ]) self.sm = sm if self.sm is None: self.sm = messaging.SubMaster([ 'thermal', 'health', 'model', 'liveCalibration', 'frontFrame', 'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman' ]) self.can_sock = can_sock if can_sock is None: can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100 self.can_sock = messaging.sub_sock('can', timeout=can_timeout) # wait for one health and one CAN packet print("Waiting for CAN messages...") get_one_can(self.can_sock) self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan']) # read params params = Params() self.is_metric = params.get("IsMetric", encoding='utf8') == "1" self.is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1" internet_needed = (params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not None) and ( params.get("DisableUpdates") != b"1") community_feature_toggle = params.get("CommunityFeaturesToggle", encoding='utf8') == "1" openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle", encoding='utf8') == "1" passive = params.get("Passive", encoding='utf8') == "1" or \ internet_needed or not openpilot_enabled_toggle # detect sound card presence and ensure successful init sounds_available = HARDWARE.get_sound_card_online() car_recognized = self.CP.carName != 'mock' # If stock camera is disconnected, we loaded car controls and it's not dashcam mode controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive and not self.CP.dashcamOnly community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle self.read_only = not car_recognized or not controller_available or \ self.CP.dashcamOnly or community_feature_disallowed if self.read_only: self.CP.safetyModel = car.CarParams.SafetyModel.noOutput # Write CarParams for radard and boardd safety mode cp_bytes = self.CP.to_bytes() params.put("CarParams", cp_bytes) put_nonblocking("CarParamsCache", cp_bytes) self.CC = car.CarControl.new_message() self.AM = AlertManager() self.events = Events() self.LoC = LongControl(self.CP, self.CI.compute_gb) self.VM = VehicleModel(self.CP) if self.CP.lateralTuning.which() == 'pid': self.LaC = LatControlPID(self.CP) elif self.CP.lateralTuning.which() == 'indi': self.LaC = LatControlINDI(self.CP) elif self.CP.lateralTuning.which() == 'lqr': self.LaC = LatControlLQR(self.CP) self.state = State.disabled self.enabled = False self.active = False self.can_rcv_error = False self.soft_disable_timer = 0 self.v_cruise_kph = 255 self.v_cruise_kph_last = 0 self.mismatch_counter = 0 self.can_error_counter = 0 self.last_blinker_frame = 0 self.saturated_count = 0 self.distance_traveled = 0 self.last_functional_fan_frame = 0 self.events_prev = [] self.current_alert_types = [ET.PERMANENT] self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED self.sm['thermal'].freeSpace = 1. self.sm['dMonitoringState'].events = [] self.sm['dMonitoringState'].awarenessStatus = 1. self.sm['dMonitoringState'].faceDetected = False self.startup_event = get_startup_event(car_recognized, controller_available) if not sounds_available: self.events.add(EventName.soundsUnavailable, static=True) if internet_needed: self.events.add(EventName.internetConnectivityNeeded, static=True) if community_feature_disallowed: self.events.add(EventName.communityFeatureDisallowed, static=True) if not car_recognized: self.events.add(EventName.carUnrecognized, static=True) # controlsd is driven by can recv, expected at 100Hz self.rk = Ratekeeper(100, print_delay_threshold=None) self.prof = Profiler(False) # off by default
def lead_control(self, CS, sm, CC): dRel = CC.dRel yRel = CC.yRel vRel = CC.vRel active_time = 10 btn_type = Buttons.NONE #lead_1 = sm['radarState'].leadOne long_wait_cmd = 500 set_speed = int(round(self.cruise_set_speed_kph)) if self.long_curv_timer < 600: self.long_curv_timer += 1 # 선행 차량 거리유지 lead_wait_cmd, lead_set_speed = self.update_lead( sm, CS, dRel, yRel, vRel) # 커브 감속. model_speed = CC.model_speed #calc_va( CS.out.vEgo ) 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: 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 >= int(round(self.cruise_set_speed_kph)): set_speed = int(round(self.cruise_set_speed_kph)) elif set_speed <= 30: set_speed = 30 # control process target_set_speed = set_speed delta = int(round(set_speed)) - int(CS.VSetDis) dec_step_cmd = 1 camspeed = Params().get("LimitSetSpeedCamera", encoding="utf8") if camspeed is not None: self.map_spd_camera = int(float(camspeed.rstrip('\n'))) self.map_spd_enable = self.map_spd_camera > 29 else: self.map_spd_enable = False self.map_spd_camera = 0 self.map_spd_limit_offset = int(Params().get("OpkrSpeedLimitOffset", encoding='utf8')) if self.long_curv_timer < long_wait_cmd: pass elif delta > 0: if ((self.map_spd_camera + round(self.map_spd_camera * 0.01 * self.map_spd_limit_offset)) == int(CS.VSetDis)) and self.map_spd_enable: set_speed = int(CS.VSetDis) + 0 btn_type = Buttons.NONE self.long_curv_timer = 0 else: set_speed = int(CS.VSetDis) + dec_step_cmd btn_type = Buttons.RES_ACCEL self.long_curv_timer = 0 elif delta < 0: set_speed = int(CS.VSetDis) - dec_step_cmd btn_type = Buttons.SET_DECEL self.long_curv_timer = 0 if self.cruise_set_mode == 0: btn_type = Buttons.NONE self.update_log(CS, set_speed, target_set_speed, long_wait_cmd) return btn_type, set_speed, active_time
def python_replay_process(cfg, lr): sub_sockets = [s for _, sub in cfg.pub_sub.items() for s in sub] pub_sockets = [s for s in cfg.pub_sub.keys() if s != 'can'] fsm = FakeSubMaster(pub_sockets) fpm = FakePubMaster(sub_sockets) args = (fsm, fpm) if 'can' in list(cfg.pub_sub.keys()): can_sock = FakeSocket() args = (fsm, fpm, can_sock) all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime) pub_msgs = [ msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys()) ] params = Params() params.clear_all() params.put_bool("OpenpilotEnabledToggle", True) params.put_bool("Passive", False) params.put_bool("CommunityFeaturesToggle", True) os.environ['NO_RADAR_SLEEP'] = "1" os.environ['SKIP_FW_QUERY'] = "" os.environ['FINGERPRINT'] = "" # TODO: remove after getting new route for civic & accord migration = { "HONDA CIVIC 2016 TOURING": "HONDA CIVIC 2016", "HONDA ACCORD 2018 SPORT 2T": "HONDA ACCORD 2018 2T", } for msg in lr: if msg.which() == 'carParams': car_fingerprint = migration.get(msg.carParams.carFingerprint, msg.carParams.carFingerprint) if len(msg.carParams.carFw) and (car_fingerprint in FW_VERSIONS): params.put("CarParamsCache", msg.carParams.as_builder().to_bytes()) else: os.environ['SKIP_FW_QUERY'] = "1" os.environ['FINGERPRINT'] = car_fingerprint assert (type(managed_processes[cfg.proc_name]) is PythonProcess) managed_processes[cfg.proc_name].prepare() mod = importlib.import_module(managed_processes[cfg.proc_name].module) thread = threading.Thread(target=mod.main, args=args) thread.daemon = True thread.start() if cfg.init_callback is not None: if 'can' not in list(cfg.pub_sub.keys()): can_sock = None cfg.init_callback(all_msgs, fsm, can_sock) CP = car.CarParams.from_bytes(params.get("CarParams", block=True)) # wait for started process to be ready if 'can' in list(cfg.pub_sub.keys()): can_sock.wait_for_recv() else: fsm.wait_for_update() log_msgs, msg_queue = [], [] for msg in tqdm(pub_msgs, disable=CI): if cfg.should_recv_callback is not None: recv_socks, should_recv = cfg.should_recv_callback( msg, CP, cfg, fsm) else: recv_socks = [ s for s in cfg.pub_sub[msg.which()] if (fsm.frame + 1) % int(service_list[msg.which()].frequency / service_list[s].frequency) == 0 ] should_recv = bool(len(recv_socks)) if msg.which() == 'can': can_sock.send(msg.as_builder().to_bytes()) else: msg_queue.append(msg.as_builder()) if should_recv: fsm.update_msgs(0, msg_queue) msg_queue = [] recv_cnt = len(recv_socks) while recv_cnt > 0: m = fpm.wait_for_msg() log_msgs.append(m) recv_cnt -= m.which() in recv_socks return log_msgs
class SpdController(): def __init__(self, CP=None): self.long_control_state = 0 # initialized to off self.seq_step_debug = "" self.long_curv_timer = 0 self.path_x = np.arange(192) self.traceSC = trace1.Loger("SPD_CTRL") self.wheelbase = 2.8 self.steerRatio = 13.5 # 13.5 self.v_model = 0 self.a_model = 0 self.v_cruise = 0 self.a_cruise = 0 self.l_poly = [] self.r_poly = [] self.movAvg = moveavg1.MoveAvg() self.Timer1 = tm.CTime1000("SPD") self.time_no_lean = 0 self.wait_timer2 = 0 self.cruise_set_speed_kph = 0 self.curise_set_first = 0 self.curise_sw_check = 0 self.prev_clu_CruiseSwState = 0 self.prev_VSetDis = 0 self.sc_clu_speed = 0 self.btn_type = Buttons.NONE self.active_time = 0 self.old_model_speed = 0 self.old_model_init = 0 self.params = Params() self.cruise_set_mode = int(self.params.get('CruiseStatemodeSelInit')) self.map_spd_enable = False self.map_spd_camera = 0 def reset(self): self.v_model = 0 self.a_model = 0 self.v_cruise = 0 self.a_cruise = 0 def calc_laneProb(self, prob, v_ego): if len(prob) >= 32: path = list(prob) # TODO: compute max speed without using a list of points and without numpy y_p = 3 * path[0] * self.path_x**2 + \ 2 * path[15] * self.path_x + path[30] y_pp = 6 * path[0] * self.path_x + 2 * path[15] curv = y_pp / (1. + y_p**2)**1.5 #print( 'curv = {}'.format( curv) ) a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None)) model_speed = np.min(v_curvature) # Don't slow down below 20mph # model_speed = max(30.0 * CV.KPH_TO_MS, model_speed) # print( 'v_curvature = {}'.format( v_curvature) ) #print( 'model_speed = {} '.format( model_speed) ) # model_speed = model_speed * CV.MS_TO_KPH #if model_speed > MAX_SPEED: # model_speed = MAX_SPEED else: model_speed = MAX_SPEED return model_speed def calc_va(self, sm, v_ego): md = sm['modelV2'] #print('{}'.format( md ) ) if len(md.laneLines) > 1: self.prob = md.laneLines[1].y #self.prob = md.curv model_speed = self.calc_laneProb(self.prob, v_ego) delta_model = model_speed - self.old_model_speed if self.old_model_init < 10: self.old_model_init += 1 self.old_model_speed = model_speed elif self.old_model_speed == model_speed: pass elif delta_model < -1: self.old_model_speed -= 1 #model_speed elif delta_model > 0: self.old_model_speed += 0.1 else: self.old_model_speed = model_speed return self.old_model_speed def update_cruiseSW(self, CS): set_speed_kph = int(round(self.cruise_set_speed_kph)) delta_vsetdis = 0 if CS.acc_active: delta_vsetdis = abs(int(CS.VSetDis) - self.prev_VSetDis) if self.prev_clu_CruiseSwState != CS.cruise_buttons: if CS.cruise_buttons == Buttons.RES_ACCEL or CS.cruise_buttons == Buttons.SET_DECEL: self.prev_VSetDis = int(CS.VSetDis) elif CS.driverOverride: set_speed_kph = int(CS.VSetDis) elif self.prev_clu_CruiseSwState == Buttons.RES_ACCEL: # up if self.curise_set_first: self.curise_set_first = 0 set_speed_kph = int(CS.VSetDis) elif delta_vsetdis > 0: set_speed_kph = int(CS.VSetDis) elif not self.curise_sw_check: set_speed_kph += 1 elif self.prev_clu_CruiseSwState == Buttons.SET_DECEL: # dn if self.curise_set_first: self.curise_set_first = 0 set_speed_kph = int(CS.VSetDis) elif delta_vsetdis > 0: set_speed_kph = int(CS.VSetDis) elif not self.curise_sw_check: set_speed_kph -= 1 self.prev_clu_CruiseSwState = CS.cruise_buttons elif (CS.cruise_buttons == Buttons.RES_ACCEL or CS.cruise_buttons == Buttons.SET_DECEL) and delta_vsetdis > 0: self.curise_sw_check = True set_speed_kph = int(CS.VSetDis) else: self.curise_sw_check = False self.curise_set_first = 1 self.prev_VSetDis = int(CS.VSetDis) set_speed_kph = int(CS.VSetDis) if self.prev_clu_CruiseSwState != CS.cruise_buttons: # MODE 전환. if CS.cruise_buttons == Buttons.GAP_DIST and not CS.acc_active and CS.out.cruiseState.available: self.cruise_set_mode += 1 if self.cruise_set_mode > 3: self.cruise_set_mode = 0 self.prev_clu_CruiseSwState = CS.cruise_buttons if set_speed_kph <= 30: set_speed_kph = 30 self.cruise_set_speed_kph = set_speed_kph return self.cruise_set_mode, set_speed_kph @staticmethod def get_lead(sm): plan = sm['longitudinalPlan'] if 0 < plan.dRel1 < 149: dRel = int(plan.dRel1) #EON Lead yRel = int(plan.yRel1) #EON Lead vRel = int(plan.vRel1 * 3.6 + 0.5) #EON Lead else: dRel = 150 yRel = 0 vRel = 0 return dRel, yRel, vRel def get_tm_speed(self, CS, set_time, add_val, safety_dis=5): time = int(set_time) delta_speed = int(CS.VSetDis) - int(round(CS.clu_Vanz)) set_speed = int(CS.VSetDis) + add_val if add_val > 0: # 증가 if delta_speed > safety_dis: time = int(set_time) else: if delta_speed < -safety_dis: time = int(set_time) return time, set_speed # returns a def update_lead(self, c, can_strings): raise NotImplementedError def update_curv(self, CS, sm, model_speed): raise NotImplementedError 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 lead_control(self, CS, sm, CC): dRel = CC.dRel yRel = CC.yRel vRel = CC.vRel active_time = 10 btn_type = Buttons.NONE #lead_1 = sm['radarState'].leadOne long_wait_cmd = 500 set_speed = int(round(self.cruise_set_speed_kph)) if self.long_curv_timer < 600: self.long_curv_timer += 1 # 선행 차량 거리유지 lead_wait_cmd, lead_set_speed = self.update_lead( sm, CS, dRel, yRel, vRel) # 커브 감속. model_speed = CC.model_speed #calc_va( CS.out.vEgo ) 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: 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 >= int(round(self.cruise_set_speed_kph)): set_speed = int(round(self.cruise_set_speed_kph)) elif set_speed <= 30: set_speed = 30 # control process target_set_speed = set_speed delta = int(round(set_speed)) - int(CS.VSetDis) dec_step_cmd = 1 camspeed = Params().get("LimitSetSpeedCamera", encoding="utf8") if camspeed is not None: self.map_spd_camera = int(float(camspeed.rstrip('\n'))) self.map_spd_enable = self.map_spd_camera > 29 else: self.map_spd_enable = False self.map_spd_camera = 0 self.map_spd_limit_offset = int(Params().get("OpkrSpeedLimitOffset", encoding='utf8')) if self.long_curv_timer < long_wait_cmd: pass elif delta > 0: if ((self.map_spd_camera + round(self.map_spd_camera * 0.01 * self.map_spd_limit_offset)) == int(CS.VSetDis)) and self.map_spd_enable: set_speed = int(CS.VSetDis) + 0 btn_type = Buttons.NONE self.long_curv_timer = 0 else: set_speed = int(CS.VSetDis) + dec_step_cmd btn_type = Buttons.RES_ACCEL self.long_curv_timer = 0 elif delta < 0: set_speed = int(CS.VSetDis) - dec_step_cmd btn_type = Buttons.SET_DECEL self.long_curv_timer = 0 if self.cruise_set_mode == 0: btn_type = Buttons.NONE self.update_log(CS, set_speed, target_set_speed, long_wait_cmd) return btn_type, set_speed, active_time def update(self, CS, sm, CC): self.cruise_set_mode = CS.out.cruiseState.modeSel #self.cruise_set_speed_kph = int(round(CS.out.cruiseState.speed * CV.MS_TO_KPH)) self.cruise_set_speed_kph = int(round(CC.vCruiseSet)) if CS.driverOverride == 2 or not CS.acc_active or CS.cruise_buttons == Buttons.RES_ACCEL or CS.cruise_buttons == Buttons.SET_DECEL: self.resume_cnt = 0 self.btn_type = Buttons.NONE self.wait_timer2 = 10 self.active_timer2 = 0 elif self.wait_timer2: self.wait_timer2 -= 1 else: btn_type, clu_speed, active_time = self.lead_control( CS, sm, CC) # speed controller spdcontroller.py if (0 <= int(CS.clu_Vanz) <= 1 or 7 < int(CS.clu_Vanz) < 15) and CC.vRel <= 0: self.btn_type = Buttons.NONE elif self.btn_type != Buttons.NONE: pass elif btn_type != Buttons.NONE: self.resume_cnt = 0 self.active_timer2 = 0 self.btn_type = btn_type self.sc_clu_speed = clu_speed self.active_time = max(5, active_time) if self.btn_type != Buttons.NONE: self.active_timer2 += 1 if self.active_timer2 > self.active_time: self.wait_timer2 = 5 self.resume_cnt = 0 self.active_timer2 = 0 self.btn_type = Buttons.NONE else: return 1 return 0
def thermald_thread(): pm = messaging.PubMaster(['deviceState']) pandaState_timeout = int(1000 * 2.5 * DT_TRML) # 2.5x the expected pandaState frequency pandaState_sock = messaging.sub_sock('pandaState', timeout=pandaState_timeout) location_sock = messaging.sub_sock('gpsLocationExternal') managerState_sock = messaging.sub_sock('managerState', conflate=True) fan_speed = 0 count = 0 startup_conditions = { "ignition": False, } startup_conditions_prev = startup_conditions.copy() off_ts = None started_ts = None started_seen = False thermal_status = ThermalStatus.green usb_power = True current_branch = get_git_branch() network_type = NetworkType.none network_strength = NetworkStrength.unknown current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML) cpu_temp_filter = FirstOrderFilter(0., CPU_TEMP_TAU, DT_TRML) charging_disabled = False #CHO: Add battery management pandaState_prev = None should_start_prev = False handle_fan = None is_uno = False #ui_running_prev = False ui_running_prev = True #CHO: Add battery management params = Params() power_monitor = PowerMonitoring() no_panda_cnt = 0 thermal_config = HARDWARE.get_thermal_config() # CPR3 logging if EON: base_path = "/sys/kernel/debug/cpr3-regulator/" cpr_files = [p for p in Path(base_path).glob("**/*") if p.is_file()] cpr_data = {} for cf in cpr_files: with open(cf, "r") as f: try: cpr_data[str(cf)] = f.read().strip() except Exception: pass cloudlog.event("CPR", data=cpr_data) while 1: pandaState = messaging.recv_sock(pandaState_sock, wait=True) msg = read_thermal(thermal_config) if pandaState is not None: usb_power = pandaState.pandaState.usbPowerMode != log.PandaState.UsbPowerMode.client # If we lose connection to the panda, wait 5 seconds before going offroad if pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown: no_panda_cnt += 1 if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML: if startup_conditions["ignition"]: cloudlog.error("Lost panda connection while onroad") startup_conditions["ignition"] = False else: no_panda_cnt = 0 startup_conditions[ "ignition"] = pandaState.pandaState.ignitionLine or pandaState.pandaState.ignitionCan startup_conditions["hardware_supported"] = True set_offroad_alert_if_changed( "Offroad_HardwareUnsupported", not startup_conditions["hardware_supported"]) # Setup fan handler on first connect to panda if handle_fan is None and pandaState.pandaState.pandaType != log.PandaState.PandaType.unknown: is_uno = pandaState.pandaState.pandaType == log.PandaState.PandaType.uno if (not EON) or is_uno: cloudlog.info("Setting up UNO fan handler") handle_fan = handle_fan_uno else: cloudlog.info("Setting up EON fan handler") setup_eon_fan() handle_fan = handle_fan_eon # Handle disconnect if pandaState_prev is not None: if pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown and \ pandaState_prev.pandaState.pandaType != log.PandaState.PandaType.unknown: params.panda_disconnect() pandaState_prev = pandaState # get_network_type is an expensive call. update every 10s if (count % int(10. / DT_TRML)) == 0: try: network_type = HARDWARE.get_network_type() network_strength = HARDWARE.get_network_strength(network_type) except Exception: cloudlog.exception("Error getting network status") msg.deviceState.freeSpacePercent = get_available_percent(default=100.0) msg.deviceState.memoryUsagePercent = int( round(psutil.virtual_memory().percent)) msg.deviceState.cpuUsagePercent = int(round(psutil.cpu_percent())) msg.deviceState.networkType = network_type msg.deviceState.networkStrength = network_strength msg.deviceState.batteryPercent = HARDWARE.get_battery_capacity() msg.deviceState.batteryStatus = HARDWARE.get_battery_status() msg.deviceState.batteryCurrent = HARDWARE.get_battery_current() msg.deviceState.batteryVoltage = HARDWARE.get_battery_voltage() msg.deviceState.usbOnline = HARDWARE.get_usb_present() # Fake battery levels on uno for frame if (not EON) or is_uno: msg.deviceState.batteryPercent = 100 msg.deviceState.batteryStatus = "Charging" msg.deviceState.batteryTempC = 0 current_filter.update(msg.deviceState.batteryCurrent / 1e6) # TODO: add car battery voltage check max_cpu_temp = cpu_temp_filter.update(max(msg.deviceState.cpuTempC)) max_comp_temp = max(max_cpu_temp, msg.deviceState.memoryTempC, max(msg.deviceState.gpuTempC)) bat_temp = msg.deviceState.batteryTempC if handle_fan is not None: fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed, startup_conditions["ignition"]) msg.deviceState.fanSpeedPercentDesired = fan_speed # If device is offroad we want to cool down before going onroad # since going onroad increases load and can make temps go over 107 # We only do this if there is a relay that prevents the car from faulting is_offroad_for_5_min = (started_ts is None) and ( (not started_seen) or (off_ts is None) or (sec_since_boot() - off_ts > 60 * 5)) if max_cpu_temp > 107. or bat_temp >= 63. or (is_offroad_for_5_min and max_cpu_temp > 70.0): # onroad not allowed thermal_status = ThermalStatus.danger elif max_comp_temp > 96.0 or bat_temp > 60.: # hysteresis between onroad not allowed and engage not allowed thermal_status = clip(thermal_status, ThermalStatus.red, ThermalStatus.danger) elif max_cpu_temp > 94.0: # hysteresis between engage not allowed and uploader not allowed thermal_status = clip(thermal_status, ThermalStatus.yellow, ThermalStatus.red) elif max_cpu_temp > 80.0: # uploader not allowed thermal_status = ThermalStatus.yellow elif max_cpu_temp > 75.0: # hysteresis between uploader not allowed and all good thermal_status = clip(thermal_status, ThermalStatus.green, ThermalStatus.yellow) else: thermal_status = ThermalStatus.green # default to good condition # **** starting logic **** # Check for last update time and display alerts if needed now = datetime.datetime.utcnow() # show invalid date/time alert startup_conditions["time_valid"] = (now.year > 2020) or ( now.year == 2020 and now.month >= 10) set_offroad_alert_if_changed("Offroad_InvalidTime", (not startup_conditions["time_valid"])) # Show update prompt try: last_update = datetime.datetime.fromisoformat( params.get("LastUpdateTime", encoding='utf8')) except (TypeError, ValueError): last_update = now dt = now - last_update update_failed_count = params.get("UpdateFailedCount") update_failed_count = 0 if update_failed_count is None else int( update_failed_count) last_update_exception = params.get("LastUpdateException", encoding='utf8') if update_failed_count > 15 and last_update_exception is not None: if current_branch in ["release2", "dashcam"]: extra_text = "Ensure the software is correctly installed" else: extra_text = last_update_exception set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) set_offroad_alert_if_changed("Offroad_UpdateFailed", True, extra_text=extra_text) elif dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1: set_offroad_alert_if_changed("Offroad_UpdateFailed", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", True) elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT: remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0)) set_offroad_alert_if_changed("Offroad_UpdateFailed", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining_time} days.") else: set_offroad_alert_if_changed("Offroad_UpdateFailed", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) startup_conditions["up_to_date"] = params.get( "Offroad_ConnectivityNeeded") is None or params.get( "DisableUpdates") == b"1" startup_conditions["not_uninstalling"] = not params.get( "DoUninstall") == b"1" startup_conditions["accepted_terms"] = params.get( "HasAcceptedTerms") == terms_version panda_signature = params.get("PandaFirmware") startup_conditions["fw_version_match"] = (panda_signature is None) or ( panda_signature == FW_SIGNATURE ) # don't show alert is no panda is connected (None) set_offroad_alert_if_changed( "Offroad_PandaFirmwareMismatch", (not startup_conditions["fw_version_match"])) # with 2% left, we killall, otherwise the phone will take a long time to boot startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2 startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version or \ (current_branch in ['dashcam', 'dashcam-staging']) startup_conditions["not_driver_view"] = not params.get( "IsDriverViewEnabled") == b"1" startup_conditions["not_taking_snapshot"] = not params.get( "IsTakingSnapshot") == b"1" # if any CPU gets above 107 or the battery gets above 63, kill all processes # controls will warn with CPU above 95 or battery above 60 startup_conditions[ "device_temp_good"] = thermal_status < ThermalStatus.danger set_offroad_alert_if_changed( "Offroad_TemperatureTooHigh", (not startup_conditions["device_temp_good"])) # Handle offroad/onroad transition should_start = all(startup_conditions.values()) if should_start: if not should_start_prev: params.delete("IsOffroad") if TICI and DISABLE_LTE_ONROAD: os.system("sudo systemctl stop --no-block lte") off_ts = None if started_ts is None: started_ts = sec_since_boot() started_seen = True else: if startup_conditions["ignition"] and (startup_conditions != startup_conditions_prev): cloudlog.event("Startup blocked", startup_conditions=startup_conditions) if should_start_prev or (count == 0): params.put("IsOffroad", "1") if TICI and DISABLE_LTE_ONROAD: os.system("sudo systemctl start --no-block lte") started_ts = None if off_ts is None: off_ts = sec_since_boot() #CHO: Added battery management charging_disabled = check_car_battery_voltage(should_start, pandaState, charging_disabled, msg) if msg.deviceState.batteryCurrent > 0: msg.deviceState.batteryStatus = "Discharging" else: msg.deviceState.batteryStatus = "Charging" msg.deviceState.chargingDisabled = charging_disabled #CHO: END # Offroad power monitoring power_monitor.calculate(pandaState) msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used() msg.deviceState.carBatteryCapacityUwh = max( 0, power_monitor.get_car_battery_capacity()) # Check if we need to disable charging (handled by boardd) msg.deviceState.chargingDisabled = power_monitor.should_disable_charging( pandaState, off_ts) # Check if we need to shut down if power_monitor.should_shutdown(pandaState, off_ts, started_seen): cloudlog.info(f"shutting device down, offroad since {off_ts}") # TODO: add function for blocking cloudlog instead of sleep time.sleep(10) HARDWARE.shutdown() # If UI has crashed, set the brightness to reasonable non-zero value manager_state = messaging.recv_one_or_none(managerState_sock) if manager_state is not None: ui_running = "ui" in (p.name for p in manager_state.managerState.processes if p.running) if ui_running_prev and not ui_running: HARDWARE.set_screen_brightness(20) ui_running_prev = ui_running msg.deviceState.chargingError = current_filter.x > 0. and msg.deviceState.batteryPercent < 90 # if current is positive, then battery is being discharged msg.deviceState.started = started_ts is not None msg.deviceState.startedMonoTime = int(1e9 * (started_ts or 0)) msg.deviceState.thermalStatus = thermal_status pm.send("deviceState", msg) if EON and not is_uno: set_offroad_alert_if_changed("Offroad_ChargeDisabled", (not usb_power)) should_start_prev = should_start startup_conditions_prev = startup_conditions.copy() # report to server once per minute if (count % int(60. / DT_TRML)) == 0: location = messaging.recv_sock(location_sock) cloudlog.event("STATUS_PACKET", count=count, pandaState=(strip_deprecated_keys( pandaState.to_dict()) if pandaState else None), location=(strip_deprecated_keys( location.gpsLocationExternal.to_dict()) if location else None), deviceState=strip_deprecated_keys(msg.to_dict())) count += 1
def __init__(self, sm=None, pm=None, can_sock=None): config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH) # Setup sockets self.pm = pm if self.pm is None: self.pm = messaging.PubMaster([ 'sendcan', 'controlsState', 'carState', 'carControl', 'carEvents', 'carParams' ]) self.camera_packets = ["roadCameraState", "driverCameraState"] if TICI: self.camera_packets.append("wideRoadCameraState") params = Params() self.joystick_mode = params.get_bool("JoystickDebugMode") joystick_packet = ['testJoystick'] if self.joystick_mode else [] self.sm = sm if self.sm is None: ignore = ['driverCameraState', 'managerState' ] if SIMULATION else None self.sm = messaging.SubMaster( [ 'deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman', 'managerState', 'liveParameters', 'radarState' ] + self.camera_packets + joystick_packet, ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan']) self.can_sock = can_sock if can_sock is None: can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100 self.can_sock = messaging.sub_sock('can', timeout=can_timeout) if TICI: self.log_sock = messaging.sub_sock('androidLog') # wait for one pandaState and one CAN packet print("Waiting for CAN messages...") get_one_can(self.can_sock) self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan']) # read params self.is_metric = params.get_bool("IsMetric") self.is_ldw_enabled = params.get_bool("IsLdwEnabled") community_feature_toggle = params.get_bool("CommunityFeaturesToggle") openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle") passive = params.get_bool("Passive") or not openpilot_enabled_toggle # detect sound card presence and ensure successful init sounds_available = HARDWARE.get_sound_card_online() car_recognized = self.CP.carName != 'mock' controller_available = self.CI.CC is not None and not passive and not self.CP.dashcamOnly community_feature = self.CP.communityFeature or \ self.CP.fingerprintSource == car.CarParams.FingerprintSource.can community_feature_disallowed = community_feature and ( not community_feature_toggle) self.read_only = not car_recognized or not controller_available or \ self.CP.dashcamOnly or community_feature_disallowed if self.read_only: safety_config = car.CarParams.SafetyConfig.new_message() safety_config.safetyModel = car.CarParams.SafetyModel.noOutput self.CP.safetyConfigs = [safety_config] # Write CarParams for radard cp_bytes = self.CP.to_bytes() params.put("CarParams", cp_bytes) put_nonblocking("CarParamsCache", cp_bytes) self.CC = car.CarControl.new_message() self.AM = AlertManager() self.events = Events() self.LoC = LongControl(self.CP) self.VM = VehicleModel(self.CP) if self.CP.steerControlType == car.CarParams.SteerControlType.angle: self.LaC = LatControlAngle(self.CP) elif self.CP.lateralTuning.which() == 'pid': self.LaC = LatControlPID(self.CP, self.CI) elif self.CP.lateralTuning.which() == 'indi': self.LaC = LatControlINDI(self.CP) elif self.CP.lateralTuning.which() == 'lqr': self.LaC = LatControlLQR(self.CP) self.initialized = False self.state = State.disabled self.enabled = False self.active = False self.can_rcv_error = False self.soft_disable_timer = 0 self.v_cruise_kph = 255 self.v_cruise_kph_last = 0 self.mismatch_counter = 0 self.cruise_mismatch_counter = 0 self.can_error_counter = 0 self.last_blinker_frame = 0 self.saturated_count = 0 self.distance_traveled = 0 self.last_functional_fan_frame = 0 self.events_prev = [] self.current_alert_types = [ET.PERMANENT] self.logged_comm_issue = False self.button_timers = { ButtonEvent.Type.decelCruise: 0, ButtonEvent.Type.accelCruise: 0 } # scc smoother self.is_cruise_enabled = False self.applyMaxSpeed = 0 self.apply_accel = 0. self.fused_accel = 0. self.lead_drel = 0. self.aReqValue = 0. self.aReqValueMin = 0. self.aReqValueMax = 0. self.sccStockCamStatus = 0 self.sccStockCamAct = 0 self.left_lane_visible = False self.right_lane_visible = False self.wide_camera = TICI and params.get_bool('EnableWideCamera') self.disable_op_fcw = params.get_bool('DisableOpFcw') # TODO: no longer necessary, aside from process replay self.sm['liveParameters'].valid = True self.startup_event = get_startup_event(car_recognized, controller_available, len(self.CP.carFw) > 0) if not sounds_available: self.events.add(EventName.soundsUnavailable, static=True) if community_feature_disallowed and car_recognized and not self.CP.dashcamOnly: self.events.add(EventName.communityFeatureDisallowed, static=True) if not car_recognized: self.events.add(EventName.carUnrecognized, static=True) if len(self.CP.carFw) > 0: set_offroad_alert("Offroad_CarUnrecognized", True) else: set_offroad_alert("Offroad_NoFirmware", True) elif self.read_only: self.events.add(EventName.dashcamMode, static=True) elif self.joystick_mode: self.events.add(EventName.joystickDebug, static=True) self.startup_event = None # controlsd is driven by can recv, expected at 100Hz self.rk = Ratekeeper(100, print_delay_threshold=None) self.prof = Profiler(False) # off by default
class CarController(): def __init__(self, dbc_name, CP, VM): self.car_fingerprint = CP.carFingerprint self.packer = CANPacker(dbc_name) self.accel_steady = 0 self.apply_steer_last = 0 self.steer_rate_limited = False self.lkas11_cnt = 0 self.scc12_cnt = 0 self.resume_cnt = 0 self.last_lead_distance = 0 self.resume_wait_timer = 0 self.last_resume_frame = 0 self.lanechange_manual_timer = 0 self.emergency_manual_timer = 0 self.driver_steering_torque_above_timer = 0 self.mode_change_timer = 0 self.params = Params() self.mode_change_switch = int(self.params.get('CruiseStatemodeSelInit')) self.opkr_variablecruise = int(self.params.get('OpkrVariableCruise')) self.opkr_autoresume = int(self.params.get('OpkrAutoResume')) self.opkr_autoresumeoption = int(self.params.get('OpkrAutoResumeOption')) self.opkr_maxanglelimit = int(self.params.get('OpkrMaxAngleLimit')) self.steer_mode = "" self.mdps_status = "" self.lkas_switch = "" self.timer1 = tm.CTime1000("time") self.SC = Spdctrl() self.model_speed = 0 self.model_sum = 0 self.dRele = 0 self.yRele = 0 self.vRele = 0 self.cruise_gap = 0.0 self.cruise_gap_prev = 0 self.cruise_gap_set_init = 0 self.cruise_gap_switch_timer = 0 self.lkas_button_on = True self.longcontrol = CP.openpilotLongitudinalControl self.scc_live = not CP.radarOffCan if CP.lateralTuning.which() == 'pid': self.str_log2 = 'TUNE={:0.2f}/{:0.3f}/{:0.5f}'.format(CP.lateralTuning.pid.kpV[1], CP.lateralTuning.pid.kiV[1], CP.lateralTuning.pid.kf) elif CP.lateralTuning.which() == 'indi': self.str_log2 = 'TUNE={:03.1f}/{:03.1f}/{:03.1f}/{:03.1f}'.format(CP.lateralTuning.indi.innerLoopGain, CP.lateralTuning.indi.outerLoopGain, CP.lateralTuning.indi.timeConstant, CP.lateralTuning.indi.actuatorEffectiveness) elif CP.lateralTuning.which() == 'lqr': self.str_log2 = 'TUNE={:04.0f}/{:05.3f}/{:06.4f}'.format(CP.lateralTuning.lqr.scale, CP.lateralTuning.lqr.ki, CP.lateralTuning.lqr.dcGain) if CP.spasEnabled: self.en_cnt = 0 self.apply_steer_ang = 0.0 self.en_spas = 3 self.mdps11_stat_last = 0 self.spas_always = False def 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.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo) plan = sm['plan'] self.dRele = plan.ddRel #EON Lead self.yRele = plan.yyRel #EON Lead self.vRele = plan.vvRel #EON Lead # 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 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 = 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.opkr_autoresumeoption == 1: 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 else: # run only first time when the car stopped if self.last_lead_distance == 0 or not self.opkr_autoresume: # 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 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.dRele > 18 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 manager_thread(): # now loop thermal_sock = messaging.sub_sock('thermal') cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) params = Params() EnableLogger = int(params.get('OpkrEnableLogger')) #EnableLogger = (params.get("RecordFront") != b"0") if not EnableLogger: car_started_processes.remove('loggerd') persistent_processes.remove('logmessaged') persistent_processes.remove('uploader') persistent_processes.remove('logcatd') persistent_processes.remove('updated') persistent_processes.remove('deleter') persistent_processes.remove('tombstoned') else: # save boot log subprocess.call(["./loggerd", "--bootlog"], cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) # start daemon processes for p in daemon_processes: start_daemon_process(p) # start persistent processes for p in persistent_processes: start_managed_process(p) # start offroad if ANDROID: pm_apply_packages('enable') start_offroad() if os.getenv("NOBOARD") is None: start_managed_process("pandad") if os.getenv("BLOCK") is not None: for k in os.getenv("BLOCK").split(","): del managed_processes[k] logger_dead = False while 1: msg = messaging.recv_sock(thermal_sock, wait=True) # heavyweight batch processes are gated on favorable thermal conditions if msg.thermal.thermalStatus >= ThermalStatus.yellow: for p in green_temp_processes: if p in persistent_processes: kill_managed_process(p) else: for p in green_temp_processes: if p in persistent_processes: start_managed_process(p) if msg.thermal.freeSpace < 0.05: logger_dead = True if msg.thermal.started and "driverview" not in running: for p in car_started_processes: if p == "loggerd" and logger_dead: kill_managed_process(p) else: start_managed_process(p) else: logger_dead = False for p in reversed(car_started_processes): kill_managed_process(p) # this is ugly if "driverview" not in running and params.get( "IsDriverViewEnabled") == b"1": start_managed_process("driverview") elif "driverview" in running and params.get( "IsDriverViewEnabled") == b"0": kill_managed_process("driverview") # check the status of all processes, did any of them die? running_list = [ "%s%s\u001b[0m" % ("\u001b[32m" if running[p].is_alive() else "\u001b[31m", p) for p in running ] cloudlog.debug(' '.join(running_list)) # Exit main loop when uninstall is needed if params.get("DoUninstall", encoding='utf8') == "1": break
def radard_thread(sm=None, pm=None, can_sock=None): config_realtime_process(5 if TICI else 1 if JETSON else 2, Priority.CTRL_LOW) # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) cloudlog.info("radard got CarParams") # import the radar from the fingerprint cloudlog.info("radard is importing %s", CP.carName) RadarInterface = importlib.import_module( 'selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface # *** setup messaging if can_sock is None: can_sock = messaging.sub_sock('can') if sm is None: sm = messaging.SubMaster( ['modelV2', 'carState'], ignore_avg_freq=[ 'modelV2', 'carState' ]) # Can't check average frequency, since radar determines timing if pm is None: pm = messaging.PubMaster(['radarState', 'liveTracks']) RI = RadarInterface(CP) rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None) RD = RadarD(CP.radarTimeStep, RI.delay) # TODO: always log leads once we can hide them conditionally enable_lead = True #CP.openpilotLongitudinalControl or not CP.radarOffCan while 1: can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) rr = RI.update(can_strings) if rr is None: continue sm.update(0) dat = RD.update(sm, rr, enable_lead) dat.radarState.cumLagMs = -rk.remaining * 1000. pm.send('radarState', dat) # *** publish tracks for UI debugging (keep last) *** tracks = RD.tracks dat = messaging.new_message('liveTracks', len(tracks)) for cnt, ids in enumerate(sorted(tracks.keys())): dat.liveTracks[cnt] = { "trackId": ids, "dRel": float(tracks[ids].dRel), "yRel": float(tracks[ids].yRel), "vRel": float(tracks[ids].vRel), } pm.send('liveTracks', dat) rk.monitor_time()
def __init__(self, sm=None, pm=None, can_sock=None): config_realtime_process(3, Priority.CTRL_HIGH) self.op_params = opParams() # Setup sockets self.pm = pm if self.pm is None: self.pm = messaging.PubMaster([ 'sendcan', 'controlsState', 'carState', 'carControl', 'carEvents', 'carParams' ]) self.sm = sm if self.sm is None: ignore = ['driverCameraState', 'managerState' ] if SIMULATION else None self.sm = messaging.SubMaster([ 'deviceState', 'pandaState', 'modelV2', 'liveCalibration', 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman', 'roadCameraState', 'driverCameraState', 'managerState', 'liveParameters', 'radarState' ], ignore_alive=ignore) self.sm_smiskol = messaging.SubMaster([ 'radarState', 'dynamicFollowData', 'liveTracks', 'dynamicFollowButton', 'laneSpeed', 'dynamicCameraOffset', 'modelLongButton' ]) self.op_params = opParams() self.df_manager = dfManager() self.support_white_panda = self.op_params.get('support_white_panda') self.last_model_long = False self.can_sock = can_sock if can_sock is None: can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100 self.can_sock = messaging.sub_sock('can', timeout=can_timeout) # wait for one pandaState and one CAN packet panda_type = messaging.recv_one( self.sm.sock['pandaState']).pandaState.pandaType has_relay = panda_type in [ PandaType.blackPanda, PandaType.uno, PandaType.dos ] print("Waiting for CAN messages...") get_one_can(self.can_sock) self.CI, self.CP, candidate = get_car(self.can_sock, self.pm.sock['sendcan'], has_relay) threading.Thread(target=log_fingerprint, args=[candidate]).start() # read params params = Params() self.is_metric = params.get("IsMetric", encoding='utf8') == "1" self.is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1" community_feature_toggle = params.get("CommunityFeaturesToggle", encoding='utf8') == "1" openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle", encoding='utf8') == "1" passive = params.get( "Passive", encoding='utf8') == "1" or not openpilot_enabled_toggle # detect sound card presence and ensure successful init sounds_available = HARDWARE.get_sound_card_online() car_recognized = self.CP.carName != 'mock' # If stock camera is disconnected, we loaded car controls and it's not dashcam mode controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive and not self.CP.dashcamOnly community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle self.read_only = not car_recognized or not controller_available or \ self.CP.dashcamOnly or community_feature_disallowed if self.read_only: self.CP.safetyModel = car.CarParams.SafetyModel.noOutput # Write CarParams for radard and boardd safety mode cp_bytes = self.CP.to_bytes() params.put("CarParams", cp_bytes) put_nonblocking("CarParamsCache", cp_bytes) self.CC = car.CarControl.new_message() self.AM = AlertManager() self.events = Events() self.LoC = LongControl(self.CP, self.CI.compute_gb, candidate) self.VM = VehicleModel(self.CP) if self.CP.lateralTuning.which() == 'pid': self.LaC = LatControlPID(self.CP) elif self.CP.lateralTuning.which() == 'indi': self.LaC = LatControlINDI(self.CP) elif self.CP.lateralTuning.which() == 'lqr': self.LaC = LatControlLQR(self.CP) self.state = State.disabled self.enabled = False self.active = False self.can_rcv_error = False self.soft_disable_timer = 0 self.v_cruise_kph = 255 self.v_cruise_kph_last = 0 self.mismatch_counter = 0 self.can_error_counter = 0 self.last_blinker_frame = 0 self.saturated_count = 0 self.distance_traveled = 0 self.last_functional_fan_frame = 0 self.events_prev = [] self.current_alert_types = [ET.PERMANENT] self.logged_comm_issue = False self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED self.sm['deviceState'].freeSpacePercent = 100 self.sm['driverMonitoringState'].events = [] self.sm['driverMonitoringState'].awarenessStatus = 1. self.sm['driverMonitoringState'].faceDetected = False self.startup_event = get_startup_event(car_recognized, controller_available) if not sounds_available: self.events.add(EventName.soundsUnavailable, static=True) if community_feature_disallowed: self.events.add(EventName.communityFeatureDisallowed, static=True) if not car_recognized: self.events.add(EventName.carUnrecognized, static=True) # controlsd is driven by can recv, expected at 100Hz self.rk = Ratekeeper(100, print_delay_threshold=None) self.prof = Profiler(False) # off by default self.lead_rel_speed = 255 self.lead_long_dist = 255