def plannerd_thread(sm=None, pm=None): config_realtime_process(5, Priority.CTRL_LOW) cloudlog.info("plannerd is waiting for CarParams") params = Params() CP = car.CarParams.from_bytes(params.get("CarParams", block=True)) cloudlog.info("plannerd got CarParams: %s", CP.carName) use_lanelines = False wide_camera = params.get_bool('WideCameraOnly') cloudlog.event("e2e mode", on=use_lanelines) longitudinal_planner = Planner(CP) lateral_planner = LateralPlanner(use_lanelines=use_lanelines, wide_camera=wide_camera) if sm is None: sm = messaging.SubMaster( ['carState', 'controlsState', 'radarState', 'modelV2'], poll=['radarState', 'modelV2'], ignore_avg_freq=['radarState']) if pm is None: pm = messaging.PubMaster(['longitudinalPlan', 'lateralPlan']) while True: sm.update() if sm.updated['modelV2']: lateral_planner.update(sm) lateral_planner.publish(sm, pm) longitudinal_planner.update(sm) longitudinal_planner.publish(sm, pm)
def radard_thread(sm=None, pm=None, can_sock=None): config_realtime_process(5, 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( f'selfdrive.car.{CP.carName}.radar_interface').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 = 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 plannerd_thread(sm=None, pm=None): config_realtime_process(2, Priority.CTRL_LOW) cloudlog.info("plannerd is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) cloudlog.info("plannerd got CarParams: %s", CP.carName) PL = Planner(CP) PP = PathPlanner(CP) VM = VehicleModel(CP) if sm is None: sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters', 'modelLongButton'], poll=['radarState', 'model']) if pm is None: pm = messaging.PubMaster(['plan', 'liveLongitudinalMpc', 'pathPlan', 'liveMpc']) sm['liveParameters'].valid = True sm['liveParameters'].sensorValid = True sm['liveParameters'].steerRatio = CP.steerRatio sm['liveParameters'].stiffnessFactor = 1.0 while True: sm.update() if sm.updated['model']: PP.update(sm, pm, CP, VM) if sm.updated['radarState']: PL.update(sm, pm, CP, VM, PP)
def plannerd_thread(sm=None, pm=None): config_realtime_process(5 if TICI else 2, Priority.CTRL_LOW) cloudlog.info("plannerd is waiting for CarParams") params = Params() CP = car.CarParams.from_bytes(params.get("CarParams", block=True)) cloudlog.info("plannerd got CarParams: %s", CP.carName) use_lanelines = not params.get_bool('EndToEndToggle') wide_camera = params.get_bool('EnableWideCamera') if TICI else False longitudinal_planner = Planner(CP) lateral_planner = LateralPlanner(CP, use_lanelines=use_lanelines, wide_camera=wide_camera) if sm is None: sm = messaging.SubMaster( ['carState', 'controlsState', 'radarState', 'modelV2'], poll=['radarState', 'modelV2']) if pm is None: pm = messaging.PubMaster([ 'longitudinalPlan', 'liveLongitudinalMpc', 'lateralPlan', 'liveMpc' ]) while True: sm.update() if sm.updated['modelV2']: lateral_planner.update(sm, CP) lateral_planner.publish(sm, pm) if sm.updated['radarState']: longitudinal_planner.update(sm, CP) longitudinal_planner.publish(sm, pm)
def send_thread(sender, core): config_realtime_process(core, 55) if "Jungle" in str(type(sender)): for i in [0, 1, 2, 3, 0xFFFF]: sender.can_clear(i) sender.set_ignition(False) time.sleep(5) sender.set_ignition(True) sender.set_panda_power(True) else: sender.set_safety_mode(Panda.SAFETY_ALLOUTPUT) sender.set_can_loopback(False) log_idx = 0 rk = Ratekeeper(100) while True: snd = CAN_MSGS[log_idx] log_idx = (log_idx + 1) % len(CAN_MSGS) snd = list(filter(lambda x: x[-1] <= 2, snd)) sender.can_send_many(snd) # Drain panda message buffer sender.can_recv() rk.keep_time()
def connect(): config_realtime_process(3, 55) serials = {} flashing_lock = threading.Lock() while True: # look for new devices for p in [Panda, PandaJungle]: if p is None: continue for s in p.list(): if s not in serials: print("starting send thread for", s) serials[s] = threading.Thread(target=send_thread, args=(p(s), flashing_lock)) serials[s].start() # try to join all send threads cur_serials = serials.copy() for s, t in cur_serials.items(): t.join(0.01) if not t.is_alive(): del serials[s] time.sleep(1)
def send_thread(sender, core): config_realtime_process(core, 55) if "Jungle" in str(type(sender)): for i in [0, 1, 2, 3, 0xFFFF]: sender.can_clear(i) sender.set_ignition(False) time.sleep(5) sender.set_ignition(True) sender.set_panda_power(True) else: sender.set_safety_mode(Panda.SAFETY_ALLOUTPUT) sender.set_can_loopback(False) ignition = None if IGN_ON > 0 and IGN_OFF > 0: ignition = True print(f"Cycling ignition: on for {IGN_ON}s, off for {IGN_OFF}s") log_idx = 0 rk = Ratekeeper(1 / DT_CTRL, print_delay_threshold=None) while True: # handle ignition cycling if ignition is not None: ign = (rk.frame * DT_CTRL) % (IGN_ON + IGN_OFF) < IGN_ON if ign != ignition: ignition = ign sender.set_ignition(ignition) snd = CAN_MSGS[log_idx] log_idx = (log_idx + 1) % len(CAN_MSGS) snd = list(filter(lambda x: x[-1] <= 2, snd)) sender.can_send_many(snd) # Drain panda message buffer sender.can_recv() rk.keep_time()
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', '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 hw_type = messaging.recv_one(self.sm.sock['health']).health.hwType has_relay = hw_type in [HwType.blackPanda, HwType.uno, HwType.dos] print("Waiting for CAN messages...") get_one_can(self.can_sock) self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], has_relay) # 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, hw_type) 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) if hw_type == HwType.whitePanda: self.events.add(EventName.whitePandaUnsupported, 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 __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.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.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 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_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' # 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.steerControlType == car.CarParams.SteerControlType.angle: self.LaC = LatControlAngle(self.CP) elif 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.sm['liveParameters'].valid = True 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) elif self.read_only: self.events.add(EventName.dashcamMode, 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 __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.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 } # 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) 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
def main(sm=None, pm=None): config_realtime_process([0, 1, 2, 3], 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") # 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() if sm.all_checks(): 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') liveParameters = msg.liveParameters liveParameters.posenetValid = True liveParameters.sensorValid = True liveParameters.steerRatio = float(x[States.STEER_RATIO]) liveParameters.stiffnessFactor = float(x[States.STIFFNESS]) liveParameters.roll = float(x[States.ROAD_ROLL]) liveParameters.angleOffsetAverageDeg = angle_offset_average liveParameters.angleOffsetDeg = angle_offset liveParameters.valid = all(( abs(liveParameters.angleOffsetAverageDeg) < 10.0, abs(liveParameters.angleOffsetDeg) < 10.0, 0.2 <= liveParameters.stiffnessFactor <= 5.0, min_sr <= liveParameters.steerRatio <= max_sr, )) liveParameters.steerRatioStd = float(P[States.STEER_RATIO]) liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS]) liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET]) liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST]) msg.valid = sm.all_checks() if sm.frame % 1200 == 0: # once a minute params = { 'carFingerprint': CP.carFingerprint, 'steerRatio': liveParameters.steerRatio, 'stiffnessFactor': liveParameters.stiffnessFactor, 'angleOffsetAverageDeg': liveParameters.angleOffsetAverageDeg, } put_nonblocking("LiveParameters", json.dumps(params)) pm.send('liveParameters', msg)
def __init__(self, sm=None, pm=None, can_sock=None, CI=None): config_realtime_process(4, 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") self.can_sock = can_sock if can_sock is None: can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 20 self.can_sock = messaging.sub_sock('can', timeout=can_timeout) if TICI: self.log_sock = messaging.sub_sock('androidLog') if CI is None: # 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']) else: self.CI, self.CP = CI, CI.CP params = Params() self.joystick_mode = params.get_bool("JoystickDebugMode") or (self.CP.notCar and sm is None) 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']) # set alternative experiences from parameters self.disengage_on_accelerator = params.get_bool("DisengageOnAccelerator") self.CP.alternativeExperience = 0 if not self.disengage_on_accelerator: self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS # read params self.is_metric = params.get_bool("IsMetric") self.is_ldw_enabled = params.get_bool("IsLdwEnabled") 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 self.read_only = not car_recognized or not controller_available or self.CP.dashcamOnly 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.CS_prev = car.CarState.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, self.CI) elif self.CP.lateralTuning.which() == 'pid': self.LaC = LatControlPID(self.CP, self.CI) elif self.CP.lateralTuning.which() == 'indi': self.LaC = LatControlINDI(self.CP, self.CI) elif self.CP.lateralTuning.which() == 'lqr': self.LaC = LatControlLQR(self.CP, self.CI) elif self.CP.lateralTuning.which() == 'torque': self.LaC = LatControlTorque(self.CP, self.CI) 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_rcv_error_counter = 0 self.last_blinker_frame = 0 self.distance_traveled = 0 self.last_functional_fan_frame = 0 self.events_prev = [] self.current_alert_types = [ET.PERMANENT] self.logged_comm_issue = None self.button_timers = {ButtonEvent.Type.decelCruise: 0, ButtonEvent.Type.accelCruise: 0} self.last_actuators = car.CarControl.Actuators.new_message() # 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') self.mad_mode_enabled = Params().get_bool('MadModeEnabled') # 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 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
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.sm = sm if self.sm is None: ignore = ['driverCameraState', 'managerState'] if SIMULATION else ['ubloxd'] self.sm = messaging.SubMaster(['deviceState', 'pandaState', 'modelV2', 'liveCalibration', 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman', 'roadCameraState', 'driverCameraState', 'managerState', 'liveParameters', 'radarState'], 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) # wait for one pandaState and one CAN packet 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']) # read params params = Params() self.is_metric = params.get_bool("IsMetric") self.is_ldw_enabled = params.get_bool("IsLdwEnabled") self.enable_lte_onroad = params.get_bool("EnableLteOnroad") community_feature_toggle = params.get_bool("CommunityFeaturesToggle") openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle") passive = params.get_bool("Passive") or not openpilot_enabled_toggle self.commIssue_ignored = params.get_bool("ComIssueGone") self.auto_enabled = params.get_bool("AutoEnable") and params.get_bool("MadModeEnabled") # detect sound card presence and ensure successful init sounds_available = HARDWARE.get_sound_card_online() car_recognized = self.CP.carName != 'mock' fuzzy_fingerprint = self.CP.fuzzyFingerprint # 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 = self.CP.communityFeature or fuzzy_fingerprint 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: self.CP.safetyModel = car.CarParams.SafetyModel.noOutput # 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.CI.compute_gb, candidate) self.VM = VehicleModel(self.CP) self.lateral_control_method = 0 if self.CP.steerControlType == car.CarParams.SteerControlType.angle: self.LaC = LatControlAngle(self.CP) self.lateral_control_method = 3 elif self.CP.lateralTuning.which() == 'pid': self.LaC = LatControlPID(self.CP) self.lateral_control_method = 0 elif self.CP.lateralTuning.which() == 'indi': self.LaC = LatControlINDI(self.CP) self.lateral_control_method = 1 elif self.CP.lateralTuning.which() == 'lqr': self.LaC = LatControlLQR(self.CP) self.lateral_control_method = 2 self.long_plan_source = 0 self.controlsAllowed = False 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.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 # TODO: no longer necessary, aside from process replay self.sm['liveParameters'].valid = True self.startup_event = get_startup_event(car_recognized, controller_available, fuzzy_fingerprint) 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) #elif self.read_only: # self.events.add(EventName.dashcamMode, 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.hyundai_lkas = self.read_only #read_only self.mpc_frame = 0 self.steerRatio_Max = float(int(params.get("SteerRatioMaxAdj", encoding="utf8")) * 0.1) self.angle_differ_range = [0, 15] self.steerRatio_range = [self.CP.steerRatio, self.steerRatio_Max] self.new_steerRatio = self.CP.steerRatio self.new_steerRatio_prev = self.CP.steerRatio self.steerRatio_to_send = 0 self.model_long_alert_prev = True self.delayed_comm_issue_timer = 0