def __init__(self, mpc_id): self.mpc_id = mpc_id self.op_params = opParams() self.df_profiles = dfProfiles() self.df_manager = dfManager(self.op_params) if not travis and mpc_id == 1: self.pm = messaging.PubMaster(['dynamicFollowData']) else: self.pm = None # Model variables mpc_rate = 1 / 20. self.model_scales = { 'v_ego': [-0.06112159043550491, 37.96522521972656], 'a_lead': [-3.109330892562866, 3.3612186908721924], 'v_lead': [0.0, 35.27671432495117], 'x_lead': [2.4600000381469727, 141.44000244140625] } self.predict_rate = 1 / 4. self.skip_every = round(0.25 / mpc_rate) self.model_input_len = round(45 / mpc_rate) # Dynamic follow variables self.default_TR = 1.8 self.TR = 1.8 self.v_ego_retention = 2.5 self.v_rel_retention = 1.75 self.sng_TR = 1.8 # reacceleration stop and go TR self.sng_speed = 18.0 * CV.MPH_TO_MS self._setup_collector() self._setup_changing_variables()
def __init__(self, mpc_id): self.mpc_id = mpc_id self.op_params = opParams() self.df_profiles = dfProfiles() self.df_manager = dfManager() self.dmc_v_rel = DistanceModController(k_i=0.042, k_d=0.08, x_clip=[-1, 0, 0.66], mods=[1.15, 1., 0.95]) self.dmc_a_rel = DistanceModController( k_i=0.042 * 1.05, k_d=0.08, x_clip=[-1, 0, 0.33], mods=[1.15, 1., 0.98]) # a_lead loop is 5% faster if not travis and mpc_id == 1: self.pm = messaging.PubMaster(['dynamicFollowData']) else: self.pm = None # Model variables mpc_rate = 1 / 20. self.model_scales = { 'v_ego': [-0.06112159043550491, 37.96522521972656], 'a_lead': [-3.109330892562866, 3.3612186908721924], 'v_lead': [0.0, 35.27671432495117], 'x_lead': [2.4600000381469727, 141.44000244140625] } self.predict_rate = 1 / 4. self.skip_every = round(0.25 / mpc_rate) self.model_input_len = round(45 / mpc_rate) # Dynamic follow variables self.default_TR = 1.8 self.TR = 1.8 self.v_ego_retention = 2.5 self.v_rel_retention = 1.75 #self.sng_TR = 1.8 # reacceleration stop and go TR #Orig #self.sng_TR = 2.3 # Last value #self.sng_TR = 1.5 # Try to decrease stop and start TR so would start move faster from stop self.sng_TR = 1. # Try to decrease stop and start TR so would start move faster from stop self.sng_TR = 2.5 # Maybe I thought this wrong self.sng_speed = 18.0 * CV.MPH_TO_MS #Orig #self.sng_speed = 12.0 * CV.MPH_TO_MS self._setup_collector() self._setup_changing_variables()
update_v_cruise, \ initialize_v_cruise from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEED from selfdrive.controls.lib.latcontrol_pid import LatControlPID from selfdrive.controls.lib.latcontrol_indi import LatControlINDI from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR from selfdrive.controls.lib.alertmanager import AlertManager from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.controls.lib.planner import LON_MPC_STEP from selfdrive.locationd.calibration_helpers import Calibration, Filter #from common.travis_checker import travis from common.op_params import opParams from selfdrive.controls.lib.dynamic_follow.df_manager import dfManager op_params = opParams() df_manager = dfManager(op_params) hide_auto_df_alerts = op_params.get('hide_auto_df_alerts', False) traffic_light_alerts = op_params.get('traffic_light_alerts', True) #LANE_DEPARTURE_THRESHOLD = 0.1 STEER_ANGLE_SATURATION_TIMEOUT = 1.0 / DT_CTRL STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees ThermalStatus = log.ThermalData.ThermalStatus State = log.ControlsState.OpenpilotState HwType = log.HealthData.HwType LaneChangeState = log.PathPlan.LaneChangeState LaneChangeDirection = log.PathPlan.LaneChangeDirection
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: self.sm = messaging.SubMaster([ 'thermal', 'health', 'model', 'liveCalibration', 'frontFrame', 'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman' ]) # golden patched self.sm_smiskol = messaging.SubMaster([ 'radarState', 'dynamicFollowData', 'liveTracks', 'dynamicFollowButton', 'laneSpeed', 'dynamicCameraOffset', 'modelLongButton', 'liveMapData' ], ignore_alive=['liveMapData']) self.op_params = opParams() self.df_manager = dfManager(self.op_params) self.hide_auto_df_alerts = self.op_params.get('hide_auto_df_alerts') self.support_white_panda = self.op_params.get('support_white_panda') self.last_model_long = False # golden patched self.git_check_time = sec_since_boot() self.git_alert_times = 6 self.git_alerted = False self.nav_alerted = 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 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, 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" 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, 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.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) if hw_type == HwType.whitePanda and not self.support_white_panda: self.events.add(EventName.whitePandaUnsupportedDEPRECATED, 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, arne_sm=None): gc.disable() set_realtime_priority(53) set_core_affinity(3) # 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', 'frame', 'model', 'liveCalibration', 'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman', 'radarState']) self.arne_sm = arne_sm if self.arne_sm is None: self.arne_sm = messaging_arne.SubMaster(['arne182Status', 'dynamicFollowButton', 'trafficModelEvent', 'modelLongButton' ]) self.op_params = opParams() self.df_manager = dfManager(self.op_params) self.hide_auto_df_alerts = self.op_params.get('hide_auto_df_alerts') self.traffic_light_alerts = self.op_params.get('traffic_light_alerts') 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 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...") messaging.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 = not ANDROID or 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 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) put_nonblocking("LongitudinalControl", "1" if self.CP.openpilotLongitudinalControl else "0") if self.CP.openpilotLongitudinalControl and self.CP.safetyModel in [car.CarParams.SafetyModel.hondaBoschGiraffe, car.CarParams.SafetyModel.hondaBoschHarness]: disable_radar(can_sock, pm.sock['sendcan'], 1 if has_relay else 0, timeout=1, retry=10) self.CC = car.CarControl.new_message() self.AM = AlertManager() self.events = Events() self.eventsArne182 = Events_arne182() 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.last_ldw_frame = 0 self.saturated_count = 0 self.distance_traveled_now = 0 if not travis: self.distance_traveled = float(params.get("DistanceTraveled", encoding='utf8')) self.distance_traveled_engaged = float(params.get("DistanceTraveledEngaged", encoding='utf8')) self.distance_traveled_override = float(params.get("DistanceTraveledOverride", encoding='utf8')) else: self.distance_traveled = 0 self.distance_traveled_engaged = 0 self.distance_traveled_override = 0 self.distance_traveled_frame = 0 self.events_prev = [] self.current_alert_types = [ET.PERMANENT] self.sm['liveCalibration'].calStatus = Calibration.INVALID 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 self.read_only and not passive: 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