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('pandaStates', timeout=pandaState_timeout) sm = messaging.SubMaster( ["peripheralState", "gpsLocationExternal", "managerState"]) 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 network_type = NetworkType.none network_strength = NetworkStrength.unknown network_info = None modem_version = None registered_count = 0 nvme_temps = None modem_temps = None current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML) temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML) pandaState_prev = None should_start_prev = False in_car = False handle_fan = None is_uno = False ui_running_prev = False params = Params() power_monitor = PowerMonitoring() no_panda_cnt = 0 HARDWARE.initialize_hardware() thermal_config = HARDWARE.get_thermal_config() # TODO: use PI controller for UNO controller = PIController(k_p=0, k_i=2e-3, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML)) # Leave flag for loggerd to indicate device was left onroad if params.get_bool("IsOnroad"): params.put_bool("BootedOnroad", True) while True: pandaStates = messaging.recv_sock(pandaState_sock, wait=True) sm.update(0) peripheralState = sm['peripheralState'] msg = read_thermal(thermal_config) if pandaStates is not None and len(pandaStates.pandaStates) > 0: pandaState = pandaStates.pandaStates[0] # If we lose connection to the panda, wait 5 seconds before going offroad if 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.ignitionLine or pandaState.ignitionCan in_car = pandaState.harnessStatus != log.PandaState.HarnessStatus.notConnected usb_power = peripheralState.usbPowerMode != log.PeripheralState.UsbPowerMode.client # Setup fan handler on first connect to panda if handle_fan is None and peripheralState.pandaType != log.PandaState.PandaType.unknown: is_uno = peripheralState.pandaType == log.PandaState.PandaType.uno if TICI: cloudlog.info("Setting up TICI fan handler") handle_fan = handle_fan_tici elif is_uno or PC: 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.pandaType == log.PandaState.PandaType.unknown and \ pandaState_prev.pandaType != log.PandaState.PandaType.unknown: params.clear_all(ParamKeyType.CLEAR_ON_PANDA_DISCONNECT) pandaState_prev = pandaState # these are expensive calls. 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) network_info = HARDWARE.get_network_info() # pylint: disable=assignment-from-none nvme_temps = HARDWARE.get_nvme_temperatures() modem_temps = HARDWARE.get_modem_temperatures() # Log modem version once if modem_version is None: modem_version = HARDWARE.get_modem_version() # pylint: disable=assignment-from-none if modem_version is not None: cloudlog.warning(f"Modem version: {modem_version}") if TICI and (network_info.get('state', None) == "REGISTERED"): registered_count += 1 else: registered_count = 0 if registered_count > 10: cloudlog.warning( f"Modem stuck in registered state {network_info}. nmcli conn up lte" ) os.system("nmcli conn up lte") registered_count = 0 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(n)) for n in psutil.cpu_percent(percpu=True) ] msg.deviceState.gpuUsagePercent = int( round(HARDWARE.get_gpu_usage_percent())) msg.deviceState.networkType = network_type msg.deviceState.networkStrength = network_strength if network_info is not None: msg.deviceState.networkInfo = network_info if nvme_temps is not None: msg.deviceState.nvmeTempC = nvme_temps if modem_temps is not None: msg.deviceState.modemTempC = modem_temps msg.deviceState.screenBrightnessPercent = HARDWARE.get_screen_brightness( ) msg.deviceState.batteryPercent = HARDWARE.get_battery_capacity() msg.deviceState.batteryCurrent = HARDWARE.get_battery_current() msg.deviceState.usbOnline = HARDWARE.get_usb_present() current_filter.update(msg.deviceState.batteryCurrent / 1e6) max_comp_temp = temp_filter.update( max(max(msg.deviceState.cpuTempC), msg.deviceState.memoryTempC, max(msg.deviceState.gpuTempC))) if handle_fan is not None: fan_speed = handle_fan(controller, max_comp_temp, fan_speed, startup_conditions["ignition"]) msg.deviceState.fanSpeedPercentDesired = fan_speed 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 is_offroad_for_5_min and max_comp_temp > OFFROAD_DANGER_TEMP: # If device is offroad we want to cool down before going onroad # since going onroad increases load and can make temps go over 107 thermal_status = ThermalStatus.danger else: current_band = THERMAL_BANDS[thermal_status] band_idx = list(THERMAL_BANDS.keys()).index(thermal_status) if current_band.min_temp is not None and max_comp_temp < current_band.min_temp: thermal_status = list(THERMAL_BANDS.keys())[band_idx - 1] elif current_band.max_temp is not None and max_comp_temp > current_band.max_temp: thermal_status = list(THERMAL_BANDS.keys())[band_idx + 1] # **** 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 tested_branch: 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_bool( "DisableUpdates") startup_conditions["not_uninstalling"] = not params.get_bool( "DoUninstall") 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 \ params.get_bool("Passive") startup_conditions["not_driver_view"] = not params.get_bool( "IsDriverViewEnabled") startup_conditions["not_taking_snapshot"] = not params.get_bool( "IsTakingSnapshot") # 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"])) if TICI: set_offroad_alert_if_changed("Offroad_StorageMissing", (not Path("/data/media").is_mount())) # Handle offroad/onroad transition should_start = all(startup_conditions.values()) if should_start != should_start_prev or (count == 0): params.put_bool("IsOnroad", should_start) params.put_bool("IsOffroad", not should_start) HARDWARE.set_power_save(not should_start) if should_start: 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) started_ts = None if off_ts is None: off_ts = sec_since_boot() # Offroad power monitoring power_monitor.calculate(peripheralState, startup_conditions["ignition"]) 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( startup_conditions["ignition"], in_car, off_ts) # Check if we need to shut down if power_monitor.should_shutdown(peripheralState, startup_conditions["ignition"], in_car, 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 ui_running = "ui" in (p.name for p in sm["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)) last_ping = params.get("LastAthenaPingTime") if last_ping is not None: msg.deviceState.lastAthenaPingTime = int(last_ping) 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 every 10 minutes if (count % int(600. / DT_TRML)) == 0: if EON and started_ts is None and msg.deviceState.memoryUsagePercent > 40: cloudlog.event("High offroad memory usage", mem=msg.deviceState.memoryUsagePercent) cloudlog.event( "STATUS_PACKET", count=count, pandaStates=(strip_deprecated_keys(pandaStates.to_dict()) if pandaStates else None), peripheralState=strip_deprecated_keys( peripheralState.to_dict()), location=(strip_deprecated_keys( sm["gpsLocationExternal"].to_dict()) if sm.alive["gpsLocationExternal"] else None), deviceState=strip_deprecated_keys(msg.to_dict())) count += 1
def test_athena(): print("ATHENA") start_daemon_process("manage_athenad") params = Params() manage_athenad_pid = params.get("AthenadPid") assert manage_athenad_pid is not None try: os.kill(int(manage_athenad_pid), 0) # process is running except OSError: assert False, "manage_athenad is dead" def expect_athena_starts(timeout=30): now = time.time() athenad_pid = None while athenad_pid is None: try: athenad_pid = subprocess.check_output( ["pgrep", "-P", manage_athenad_pid], encoding="utf-8").strip() return athenad_pid except subprocess.CalledProcessError: if time.time() - now > timeout: assert False, f"Athena did not start within {timeout} seconds" time.sleep(0.5) def athena_post(payload, max_retries=5): tries = 0 while 1: try: return requests.post("https://athena.comma.ai/" + params.get("DongleId", encoding="utf-8"), headers={ "Authorization": "JWT " + os.getenv("COMMA_JWT"), "Content-Type": "application/json" }, data=json.dumps(payload), timeout=30) except Exception as e: print(e) time.sleep(5.0) tries += 1 if tries == max_retries: raise def expect_athena_registers(timeout=60): now = time.time() while 1: resp = athena_post({ "method": "echo", "params": ["hello"], "id": 0, "jsonrpc": "2.0" }) resp_json = resp.json() if resp_json.get('result') == "hello": break elif time.time() - now > timeout: assert False, f"Athena did not become available within {timeout} seconds." else: time.sleep(5.0) try: athenad_pid = expect_athena_starts() # kill athenad and ensure it is restarted (check_output will throw if it is not) os.kill(int(athenad_pid), signal.SIGINT) expect_athena_starts() if not os.getenv('COMMA_JWT'): print( 'WARNING: COMMA_JWT env not set, will not test requests to athena.comma.ai' ) return expect_athena_registers() print("ATHENA: getSimInfo") resp = athena_post({"method": "getSimInfo", "id": 0, "jsonrpc": "2.0"}) resp_json = resp.json() assert resp_json.get('result'), resp_json assert 'sim_id' in resp_json['result'], resp_json['result'] print("ATHENA: takeSnapshot") resp = athena_post({ "method": "takeSnapshot", "id": 0, "jsonrpc": "2.0" }) resp_json = resp.json() assert resp_json.get('result'), resp_json assert resp_json['result']['jpegBack'], resp_json['result'] @with_processes(["thermald"]) def test_athena_thermal(): print("ATHENA: getMessage(thermal)") resp = athena_post({ "method": "getMessage", "params": { "service": "thermal", "timeout": 5000 }, "id": 0, "jsonrpc": "2.0" }) resp_json = resp.json() assert resp_json.get('result'), resp_json assert resp_json['result']['thermal'], resp_json['result'] test_athena_thermal() finally: try: athenad_pid = subprocess.check_output( ["pgrep", "-P", manage_athenad_pid], encoding="utf-8").strip() except subprocess.CalledProcessError: athenad_pid = None try: os.kill(int(manage_athenad_pid), signal.SIGINT) os.kill(int(athenad_pid), signal.SIGINT) except (OSError, TypeError): pass
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"), ] # 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 controlsd_thread(sm=None, pm=None, can_sock=None): gc.disable() # start the loop set_realtime_priority(3) params = Params() is_metric = params.get("IsMetric", encoding='utf8') == "1" is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1" passive = params.get("Passive", encoding='utf8') == "1" openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle", encoding='utf8') == "1" community_feature_toggle = params.get("CommunityFeaturesToggle", encoding='utf8') == "1" passive = passive or not openpilot_enabled_toggle # Pub/Sub Sockets if pm is None: pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', 'carControl', 'carEvents', 'carParams']) if sm is None: sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \ 'model', 'gpsLocation'], ignore_alive=['gpsLocation']) if can_sock is None: can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100 can_sock = messaging.sub_sock('can', timeout=can_timeout) # wait for health and CAN packets hw_type = messaging.recv_one(sm.sock['health']).health.hwType has_relay = hw_type in [HwType.blackPanda, HwType.uno] print("Waiting for CAN messages...") messaging.get_one_can(can_sock) CI, CP = get_car(can_sock, pm.sock['sendcan'], has_relay) 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 community_feature_disallowed = CP.communityFeature and not community_feature_toggle read_only = not car_recognized or not controller_available or CP.dashcamOnly or community_feature_disallowed if read_only: CP.safetyModel = car.CarParams.SafetyModel.noOutput # 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() is_rhd = params.get("IsRHD") if is_rhd is not None: driver_status.is_rhd = bool(int(is_rhd)) state = State.disabled soft_disable_timer = 0 v_cruise_kph = 255 v_cruise_kph_last = 0 mismatch_counter = 0 last_blinker_frame = 0 events_prev = [] sm['liveCalibration'].calStatus = Calibration.INVALID sm['pathPlan'].sensorValid = True sm['pathPlan'].posenetValid = True sm['thermal'].freeSpace = 1. # 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) internet_needed = params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not 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_perc, mismatch_counter = data_sample(CI, CC, sm, can_sock, 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.WARNING])) 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])) if internet_needed: events.append(create_event('internetConnectivityNeeded', [ET.NO_ENTRY, ET.PERMANENT])) if community_feature_disallowed: events.append(create_event('communityFeatureDisallowed', [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, last_blinker_frame = \ 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, read_only, is_metric, cal_perc, last_blinker_frame) prof.checkpoint("State Control") # Publish data CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, last_blinker_frame, is_ldw_enabled) prof.checkpoint("Sent") rk.monitor_time() prof.display()
def main(): # the flippening! os.system( 'LD_LIBRARY_PATH="" content insert --uri content://settings/system --bind name:s:user_rotation --bind value:i:1' ) if os.getenv("NOLOG") is not None: del managed_processes['loggerd'] del managed_processes['tombstoned'] if os.getenv("NOUPLOAD") is not None: del managed_processes['uploader'] if os.getenv("NOVISION") is not None: del managed_processes['visiond'] if os.getenv("LEAN") is not None: del managed_processes['uploader'] del managed_processes['loggerd'] del managed_processes['logmessaged'] del managed_processes['logcatd'] del managed_processes['tombstoned'] del managed_processes['proclogd'] if os.getenv("NOCONTROL") is not None: del managed_processes['controlsd'] del managed_processes['radard'] if os.getenv("DEFAULTD") is not None: managed_processes["controlsd"] = "selfdrive.controls.defaultd" # support additional internal only extensions try: import selfdrive.manager_extensions selfdrive.manager_extensions.register(register_managed_process) except ImportError: pass params = Params() params.manager_start() # set unset params if params.get("IsMetric") is None: params.put("IsMetric", "0") if params.get("RecordFront") is None: params.put("RecordFront", "0") if params.get("IsFcwEnabled") is None: params.put("IsFcwEnabled", "1") if params.get("HasAcceptedTerms") is None: params.put("HasAcceptedTerms", "0") if params.get("IsUploadVideoOverCellularEnabled") is None: params.put("IsUploadVideoOverCellularEnabled", "1") if params.get("IsDriverMonitoringEnabled") is None: params.put("IsDriverMonitoringEnabled", "0") if params.get("IsGeofenceEnabled") is None: params.put("IsGeofenceEnabled", "-1") # 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") # put something on screen while we set things up if os.getenv("PREPAREONLY") is not None: spinner_proc = None else: spinner_text = "chffrplus" if params.get( "Passive") == "1" else "openpilot" spinner_proc = subprocess.Popen( ["./spinner", "loading %s" % spinner_text], cwd=os.path.join(BASEDIR, "selfdrive", "ui", "spinner"), close_fds=True) try: manager_update() manager_init() manager_prepare() finally: if spinner_proc: spinner_proc.terminate() 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") == "1": uninstall()
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) CC = car.CarControl.new_message() CI, CP = get_car(logcan, sendcan) AM = AlertManager() 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 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) else: LaC = LatControlINDI(CP) driver_status = DriverStatus() # Write CarParams for radard and boardd safety mode params.put("CarParams", CP.to_bytes()) params.put("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0") 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 # 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, 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['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])) # 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()
class TestParams(unittest.TestCase): def setUp(self): self.tmpdir = tempfile.mkdtemp() print("using", self.tmpdir) self.params = Params(self.tmpdir) def tearDown(self): shutil.rmtree(self.tmpdir) def test_params_put_and_get(self): self.params.put("DongleId", "cb38263377b873ee") assert self.params.get("DongleId") == b"cb38263377b873ee" def test_persist_params_put_and_get(self): p = Params(persistent_params=True) p.put("DongleId", "cb38263377b873ee") assert p.get("DongleId") == b"cb38263377b873ee" def test_params_non_ascii(self): st = b"\xe1\x90\xff" self.params.put("CarParams", st) assert self.params.get("CarParams") == st def test_params_get_cleared_panda_disconnect(self): self.params.put("CarParams", "test") self.params.put("DongleId", "cb38263377b873ee") assert self.params.get("CarParams") == b"test" self.params.panda_disconnect() assert self.params.get("CarParams") is None assert self.params.get("DongleId") is not None def test_params_get_cleared_manager_start(self): self.params.put("CarParams", "test") self.params.put("DongleId", "cb38263377b873ee") assert self.params.get("CarParams") == b"test" self.params.manager_start() assert self.params.get("CarParams") is None assert self.params.get("DongleId") is not None def test_params_two_things(self): self.params.put("DongleId", "bob") self.params.put("AthenadPid", "123") assert self.params.get("DongleId") == b"bob" assert self.params.get("AthenadPid") == b"123" def test_params_get_block(self): def _delayed_writer(): time.sleep(0.1) self.params.put("CarParams", "test") threading.Thread(target=_delayed_writer).start() assert self.params.get("CarParams") is None assert self.params.get("CarParams", True) == b"test" def test_params_unknown_key_fails(self): with self.assertRaises(UnknownKeyName): self.params.get("swag") def test_params_permissions(self): permissions = stat.S_IRUSR | stat.S_IWUSR | stat.S_IRGRP | stat.S_IWGRP | stat.S_IROTH | stat.S_IWOTH self.params.put("DongleId", "cb38263377b873ee") st_mode = os.stat(f"{self.tmpdir}/d/DongleId").st_mode assert (st_mode & permissions) == permissions def test_delete_not_there(self): assert self.params.get("CarParams") is None self.params.delete("CarParams") assert self.params.get("CarParams") is None def test_put_non_blocking_with_get_block(self): q = Params(self.tmpdir) def _delayed_writer(): time.sleep(0.1) put_nonblocking("CarParams", "test", self.tmpdir) threading.Thread(target=_delayed_writer).start() assert q.get("CarParams") is None assert q.get("CarParams", True) == b"test"
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.safetyModel = car.CarParams.SafetyModel.hyundai params = Params() PidKp = float( Decimal(params.get("PidKp", encoding="utf8")) * Decimal('0.01')) PidKi = float( Decimal(params.get("PidKi", encoding="utf8")) * Decimal('0.001')) PidKd = float( Decimal(params.get("PidKd", encoding="utf8")) * Decimal('0.01')) PidKf = float( Decimal(params.get("PidKf", encoding="utf8")) * Decimal('0.00001')) InnerLoopGain = float( Decimal(params.get("InnerLoopGain", encoding="utf8")) * Decimal('0.1')) OuterLoopGain = float( Decimal(params.get("OuterLoopGain", encoding="utf8")) * Decimal('0.1')) TimeConstant = float( Decimal(params.get("TimeConstant", encoding="utf8")) * Decimal('0.1')) ActuatorEffectiveness = float( Decimal(params.get("ActuatorEffectiveness", encoding="utf8")) * Decimal('0.1')) Scale = float( Decimal(params.get("Scale", encoding="utf8")) * Decimal('1.0')) LqrKi = float( Decimal(params.get("LqrKi", encoding="utf8")) * Decimal('0.001')) DcGain = float( Decimal(params.get("DcGain", encoding="utf8")) * Decimal('0.0001')) SteerMaxV = float( Decimal(params.get("SteerMaxvAdj", encoding="utf8")) * Decimal('0.1')) # Most Hyundai car ports are community features for now ret.communityFeature = False tire_stiffness_factor = float( Decimal(params.get("TireStiffnessFactorAdj", encoding="utf8")) * Decimal('0.01')) ret.steerActuatorDelay = float( Decimal(params.get("SteerActuatorDelayAdj", encoding="utf8")) * Decimal('0.01')) ret.steerRateCost = float( Decimal(params.get("SteerRateCostAdj", encoding="utf8")) * Decimal('0.01')) ret.steerLimitTimer = float( Decimal(params.get("SteerLimitTimerAdj", encoding="utf8")) * Decimal('0.01')) ret.steerRatio = float( Decimal(params.get("SteerRatioAdj", encoding="utf8")) * Decimal('0.01')) lat_control_method = int( params.get("LateralControlMethod", encoding="utf8")) shane_feed_forward = params.get_bool("ShaneFeedForward") if lat_control_method == 0: ret.lateralTuning.pid.kf = PidKf ret.lateralTuning.pid.kpBP = [0., 9.] ret.lateralTuning.pid.kpV = [0.1, PidKp] ret.lateralTuning.pid.kiBP = [0., 9.] ret.lateralTuning.pid.kiV = [0.01, PidKi] ret.lateralTuning.pid.kdBP = [0.] ret.lateralTuning.pid.kdV = [PidKd] ret.lateralTuning.pid.newKfTuned = True if shane_feed_forward else False # Shane's feedforward elif lat_control_method == 1: ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGainBP = [0.] ret.lateralTuning.indi.innerLoopGainV = [ InnerLoopGain ] # third tune. Highest value that still gives smooth control. Effects turning into curves. ret.lateralTuning.indi.outerLoopGainBP = [0.] ret.lateralTuning.indi.outerLoopGainV = [ OuterLoopGain ] # forth tune. Highest value that still gives smooth control. Effects lane centering. ret.lateralTuning.indi.timeConstantBP = [0.] ret.lateralTuning.indi.timeConstantV = [ TimeConstant ] # second tune. Lowest value with smooth actuation. Avoid the noise of actuator gears thrashing. ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] ret.lateralTuning.indi.actuatorEffectivenessV = [ ActuatorEffectiveness ] # first tune. Lowest value without oversteering. May vary with speed. # lateralTuning.indi.actuatorEffectiveness # As effectiveness increases, actuation strength decreases # Too high: weak, sloppy lane centering, slow oscillation, can't follow high curvature, high steering error causes snappy corrections # Too low: overpower, saturation, jerky, fast oscillation # Just right: Highest still able to maintain good lane centering. # lateralTuning.indi.timeConstant # Extend exponential decay of prior output steer # Too high: sloppy lane centering # Too low: noisy actuation, responds to every bump, maybe unable to maintain lane center due to rapid actuation # Just right: above noisy actuation and lane centering instability # lateralTuning.indi.innerLoopGain # Steer rate error gain # Too high: jerky oscillation in high curvature # Too low: sloppy, cannot accomplish desired steer angle # Just right: brief snap on entering high curvature # lateralTuning.indi.outerLoopGain # Steer error gain # Too high: twitchy hyper lane centering, oversteering # Too low: sloppy, all over lane # Just right: crisp lane centering elif lat_control_method == 2: ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = Scale ret.lateralTuning.lqr.ki = LqrKi ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-110., 451.] ret.lateralTuning.lqr.l = [0.33, 0.318] ret.lateralTuning.lqr.dcGain = DcGain ret.steerMaxV = [SteerMaxV] ret.steerMaxBP = [0.] if candidate == CAR.GENESIS: ret.mass = 1900. + STD_CARGO_KG ret.wheelbase = 3.01 elif candidate == CAR.GENESIS_G70: ret.mass = 1640. + STD_CARGO_KG ret.wheelbase = 2.84 elif candidate == CAR.GENESIS_G80: ret.mass = 1855. + STD_CARGO_KG ret.wheelbase = 3.01 elif candidate == CAR.GENESIS_G90: ret.mass = 2200 ret.wheelbase = 3.15 elif candidate == CAR.SANTA_FE: ret.mass = 1694 + STD_CARGO_KG ret.wheelbase = 2.765 elif candidate in [CAR.SONATA, CAR.SONATA_HEV]: ret.mass = 1513. + STD_CARGO_KG ret.wheelbase = 2.84 elif candidate == CAR.SONATA_LF: ret.mass = 4497. * CV.LB_TO_KG ret.wheelbase = 2.804 elif candidate == CAR.SONATA_LF_TURBO: ret.mass = 1470. + STD_CARGO_KG ret.wheelbase = 2.805 elif candidate == CAR.SONATA_LF_HEV: ret.mass = 1595. + STD_CARGO_KG ret.wheelbase = 2.805 elif candidate == CAR.PALISADE: ret.mass = 1999. + STD_CARGO_KG ret.wheelbase = 2.90 elif candidate == CAR.AVANTE: ret.mass = 1245. + STD_CARGO_KG ret.wheelbase = 2.72 elif candidate == CAR.I30: ret.mass = 1380. + STD_CARGO_KG ret.wheelbase = 2.65 elif candidate == CAR.KONA: ret.mass = 1275. + STD_CARGO_KG ret.wheelbase = 2.7 elif candidate in [CAR.KONA_HEV, CAR.KONA_EV]: ret.mass = 1425. + STD_CARGO_KG ret.wheelbase = 2.6 elif candidate in [CAR.IONIQ_HEV, CAR.IONIQ_EV]: ret.mass = 1490. + STD_CARGO_KG #weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx ret.wheelbase = 2.7 elif candidate in [CAR.GRANDEUR_IG, CAR.GRANDEUR_IG_HEV]: ret.mass = 1675. + STD_CARGO_KG ret.wheelbase = 2.845 elif candidate in [CAR.GRANDEUR_IG_FL, CAR.GRANDEUR_IG_FL_HEV]: ret.mass = 1675. + STD_CARGO_KG ret.wheelbase = 2.885 elif candidate == CAR.VELOSTER: ret.mass = 3558. * CV.LB_TO_KG ret.wheelbase = 2.80 elif candidate == CAR.NEXO: ret.mass = 1885. + STD_CARGO_KG ret.wheelbase = 2.79 # kia elif candidate == CAR.SORENTO: ret.mass = 1985. + STD_CARGO_KG ret.wheelbase = 2.78 elif candidate in [CAR.K5, CAR.K5_HEV]: ret.wheelbase = 2.805 ret.mass = 1600. + STD_CARGO_KG elif candidate == CAR.STINGER: ret.mass = 1825.0 + STD_CARGO_KG ret.wheelbase = 2.906 # https://www.kia.com/us/en/stinger/specs elif candidate == CAR.K3: ret.mass = 1260. + STD_CARGO_KG ret.wheelbase = 2.70 elif candidate == CAR.SPORTAGE: ret.mass = 1985. + STD_CARGO_KG ret.wheelbase = 2.78 elif candidate in [CAR.NIRO_HEV, CAR.NIRO_EV]: ret.mass = 1737. + STD_CARGO_KG ret.wheelbase = 2.7 elif candidate in [CAR.K7, CAR.K7_HEV]: ret.mass = 1680. + STD_CARGO_KG ret.wheelbase = 2.855 elif candidate == CAR.SELTOS: ret.mass = 1310. + STD_CARGO_KG ret.wheelbase = 2.6 elif candidate == CAR.SOUL_EV: ret.mass = 1375. + STD_CARGO_KG ret.wheelbase = 2.6 elif candidate == CAR.MOHAVE: ret.mass = 2285. + STD_CARGO_KG ret.wheelbase = 2.895 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) # no rear steering, at least on the listed cars above ret.steerRatioRear = 0. ret.steerControlType = car.CarParams.SteerControlType.torque # TODO: adjust? ret.gasMaxBP = [0., 4., 9., 17., 23., 31.] # m/s ret.gasMaxV = [1.5, 1.35, 0.8, 0.5, 0.4, 0.3] # max gas allowed ret.brakeMaxBP = [0., 8.] # m/s ret.brakeMaxV = [0.7, 3.0] # max brake allowed ret.longitudinalTuning.kpBP = [0., 4., 9., 17., 23., 31.] ret.longitudinalTuning.kpV = [1.2, 0.9, 0.7, 0.6, 0.5, 0.4] ret.longitudinalTuning.kiBP = [0., 4., 9., 17., 23., 31.] ret.longitudinalTuning.kiV = [0.33, 0.22, 0.21, 0.17, 0.15, 0.13] ret.longitudinalTuning.deadzoneBP = [0., 4., 31.] ret.longitudinalTuning.deadzoneV = [0., 0.1, 0.1] ret.longitudinalTuning.kdBP = [0., 4., 9., 17., 23., 31.] ret.longitudinalTuning.kdV = [0.7, 0.65, 0.5, 0.4, 0.3, 0.2] ret.longitudinalTuning.kfBP = [0., 4., 9., 17., 23., 31.] ret.longitudinalTuning.kfV = [1., 1., 1., 1., 1., 1.] ret.enableBsm = 0x58b in fingerprint[0] ret.startAccel = 0.1 ret.standStill = False ret.vCruisekph = 0 # ignore CAN2 address if L-CAN on the same BUS ret.mdpsBus = 1 if 593 in fingerprint[1] and 1296 not in fingerprint[ 1] else 0 ret.sasBus = 1 if 688 in fingerprint[1] and 1296 not in fingerprint[ 1] else 0 ret.sccBus = 0 if 1056 in fingerprint[0] else 1 if 1056 in fingerprint[1] and 1296 not in fingerprint[1] \ else 2 if 1056 in fingerprint[2] else -1 ret.fcaBus = 0 if 909 in fingerprint[ 0] else 2 if 909 in fingerprint[2] else -1 ret.bsmAvailable = True if 1419 in fingerprint[0] else False ret.lfaAvailable = True if 1157 in fingerprint[2] else False ret.lvrAvailable = True if 871 in fingerprint[0] else False ret.evgearAvailable = True if 882 in fingerprint[0] else False ret.emsAvailable = True if 608 and 809 in fingerprint[0] else False ret.radarOffCan = ret.sccBus == -1 ret.openpilotLongitudinalControl = ret.sccBus == 2 # pcmCruise is true ret.pcmCruise = not ret.radarOffCan # set safety_hyundai_community only for non-SCC, MDPS harrness or SCC harrness cars or cars that have unknown issue if ret.radarOffCan or ret.mdpsBus == 1 or ret.openpilotLongitudinalControl or ret.sccBus == 1 or params.get_bool( "MadModeEnabled"): ret.safetyModel = car.CarParams.SafetyModel.hyundaiCommunity # set appropriate safety param for gas signal if candidate in HYBRID_CAR: ret.safetyParam = 2 elif candidate in EV_CAR: ret.safetyParam = 1 return ret
def regen_segment(lr, frs=None, outdir=FAKEDATA, disable_tqdm=False): lr = migrate_carparams(list(lr)) if frs is None: frs = dict() params = Params() os.environ["LOG_ROOT"] = outdir for msg in lr: if msg.which() == 'carParams': setup_env(CP=msg.carParams) elif msg.which() == 'liveCalibration': params.put("CalibrationParams", msg.as_builder().to_bytes()) vs, cam_procs = replay_cameras(lr, frs, disable_tqdm=disable_tqdm) fake_daemons = { 'sensord': [ multiprocessing.Process(target=replay_sensor_events, args=('sensorEvents', lr)), ], 'pandad': [ multiprocessing.Process(target=replay_service, args=('can', lr)), multiprocessing.Process(target=replay_service, args=('ubloxRaw', lr)), multiprocessing.Process(target=replay_panda_states, args=('pandaStates', lr)), ], 'managerState': [ multiprocessing.Process(target=replay_manager_state, args=('managerState', lr)), ], 'thermald': [ multiprocessing.Process(target=replay_device_state, args=('deviceState', lr)), ], 'camerad': [ *cam_procs, ], } try: # TODO: make first run of onnxruntime CUDA provider fast managed_processes["modeld"].start() managed_processes["dmonitoringmodeld"].start() time.sleep(5) # start procs up ignore = list(fake_daemons.keys()) + [ 'ui', 'manage_athenad', 'uploader', 'soundd' ] ensure_running(managed_processes.values(), started=True, params=Params(), CP=car.CarParams(), not_run=ignore) for procs in fake_daemons.values(): for p in procs: p.start() for _ in tqdm(range(60), disable=disable_tqdm): # ensure all procs are running for d, procs in fake_daemons.items(): for p in procs: if not p.is_alive(): raise Exception(f"{d}'s {p.name} died") time.sleep(1) finally: # kill everything for p in managed_processes.values(): p.stop() for procs in fake_daemons.values(): for p in procs: p.terminate() del vs segment = params.get("CurrentRoute", encoding='utf-8') + "--0" seg_path = os.path.join(outdir, segment) # check to make sure openpilot is engaged in the route if not check_enabled(LogReader(os.path.join(seg_path, "rlog"))): raise Exception(f"Route never enabled: {segment}") return seg_path
class Calibrator(object): def __init__(self, param_put=False): self.param_put = param_put self.vp = copy.copy(VP_INIT) self.vps = [] self.cal_status = Calibration.UNCALIBRATED self.write_counter = 0 self.params = Params() calibration_params = self.params.get("CalibrationParams") if calibration_params: try: calibration_params = json.loads(calibration_params) self.vp = np.array(calibration_params["vanishing_point"]) self.vps = np.tile( self.vp, (calibration_params['valid_points'], 1)).tolist() self.update_status() except Exception: cloudlog.exception( "CalibrationParams file found but error encountered") def update_status(self): if len(self.vps) < INPUTS_NEEDED: self.cal_status = Calibration.UNCALIBRATED else: self.cal_status = Calibration.CALIBRATED if is_calibration_valid( self.vp) else Calibration.INVALID def handle_cam_odom(self, log): trans, rot = log.cameraOdometry.trans, log.cameraOdometry.rot if np.linalg.norm(trans) > MIN_SPEED_FILTER and abs( rot[2]) < MAX_YAW_RATE_FILTER: new_vp = eon_intrinsics.dot( view_frame_from_device_frame.dot(trans)) new_vp = new_vp[:2] / new_vp[2] self.vps.append(new_vp) self.vps = self.vps[-INPUTS_WANTED:] self.vp = np.mean(self.vps, axis=0) self.update_status() self.write_counter += 1 if self.param_put and self.write_counter % WRITE_CYCLES == 0: cal_params = { "vanishing_point": list(self.vp), "valid_points": len(self.vps) } self.params.put("CalibrationParams", json.dumps(cal_params)) return new_vp def send_data(self, livecalibration): calib = get_calib_from_vp(self.vp) extrinsic_matrix = get_view_frame_from_road_frame( 0, calib[1], calib[2], model_height) ke = eon_intrinsics.dot(extrinsic_matrix) warp_matrix = get_camera_frame_from_model_frame(ke) warp_matrix_big = get_camera_frame_from_bigmodel_frame(ke) cal_send = messaging.new_message() cal_send.init('liveCalibration') cal_send.liveCalibration.calStatus = self.cal_status cal_send.liveCalibration.calPerc = min( len(self.vps) * 100 / INPUTS_NEEDED, 100) cal_send.liveCalibration.warpMatrix2 = map(float, warp_matrix.flatten()) cal_send.liveCalibration.warpMatrixBig = map(float, warp_matrix_big.flatten()) cal_send.liveCalibration.extrinsicMatrix = map( float, extrinsic_matrix.flatten()) livecalibration.send(cal_send.to_bytes())
def setUpClass(cls): if "DEBUG" in os.environ: segs = filter( lambda x: os.path.exists(os.path.join(x, "rlog.bz2")), Path(ROOT).iterdir()) segs = sorted(segs, key=lambda x: x.stat().st_mtime) cls.lr = list(LogReader(os.path.join(segs[-1], "rlog.bz2"))) return # setup env os.environ['REPLAY'] = "1" os.environ['SKIP_FW_QUERY'] = "1" os.environ['FINGERPRINT'] = "TOYOTA COROLLA TSS2 2019" params = Params() params.clear_all() set_params_enabled() # Make sure athena isn't running os.system("pkill -9 -f athena") # start manager and run openpilot for a minute try: manager_path = os.path.join(BASEDIR, "selfdrive/manager/manager.py") proc = subprocess.Popen(["python", manager_path]) sm = messaging.SubMaster(['carState']) with Timeout(150, "controls didn't start"): while sm.rcv_frame['carState'] < 0: sm.update(1000) # make sure we get at least two full segments route = None cls.segments = [] with Timeout(300, "timed out waiting for logs"): while route is None: route = params.get("CurrentRoute", encoding="utf-8") time.sleep(0.1) while len(cls.segments) < 3: segs = set() if Path(ROOT).exists(): segs = set(Path(ROOT).glob(f"{route}--*")) cls.segments = sorted( segs, key=lambda s: int(str(s).rsplit('--')[-1])) time.sleep(2) # chop off last, incomplete segment cls.segments = cls.segments[:-1] finally: proc.terminate() if proc.wait(60) is None: proc.kill() cls.lrs = [ list(LogReader(os.path.join(str(s), "rlog.bz2"))) for s in cls.segments ] # use the second segment by default as it's the first full segment cls.lr = list(LogReader(os.path.join(str(cls.segments[1]), "rlog.bz2")))
def test_athena(): print("ATHENA") start = sec_since_boot() managed_processes['manage_athenad'].start() params = Params() manage_athenad_pid = params.get("AthenadPid") assert manage_athenad_pid is not None try: os.kill(int(manage_athenad_pid), 0) # process is running except OSError: assert False, "manage_athenad is dead" def expect_athena_starts(timeout=30): now = time.time() athenad_pid = None while athenad_pid is None: try: athenad_pid = subprocess.check_output( ["pgrep", "-P", manage_athenad_pid], encoding="utf-8").strip() return athenad_pid except subprocess.CalledProcessError: if time.time() - now > timeout: assert False, f"Athena did not start within {timeout} seconds" time.sleep(0.5) def athena_post(payload, max_retries=5, wait=5): tries = 0 while 1: try: resp = requests.post("https://athena.comma.ai/" + params.get("DongleId", encoding="utf-8"), headers={ "Authorization": "JWT " + os.getenv("COMMA_JWT"), "Content-Type": "application/json" }, data=json.dumps(payload), timeout=30) resp_json = resp.json() if resp_json.get('error'): raise Exception(resp_json['error']) return resp_json except Exception as e: time.sleep(wait) tries += 1 if tries == max_retries: raise else: print(f'athena_post failed {e}. retrying...') def expect_athena_registers(test_t0): resp = athena_post( { "method": "echo", "params": ["hello"], "id": 0, "jsonrpc": "2.0" }, max_retries=12, wait=5) assert resp.get( 'result') == "hello", f'Athena failed to register ({resp})' last_pingtime = params.get("LastAthenaPingTime", encoding='utf8') assert last_pingtime, last_pingtime assert ((int(last_pingtime) / 1e9) - test_t0) < (sec_since_boot() - test_t0) try: athenad_pid = expect_athena_starts() # kill athenad and ensure it is restarted (check_output will throw if it is not) os.kill(int(athenad_pid), signal.SIGINT) expect_athena_starts() if not os.getenv('COMMA_JWT'): print( 'WARNING: COMMA_JWT env not set, will not test requests to athena.comma.ai' ) return expect_athena_registers(start) print("ATHENA: getSimInfo") resp = athena_post({"method": "getSimInfo", "id": 0, "jsonrpc": "2.0"}) assert resp.get('result'), resp assert 'sim_id' in resp['result'], resp['result'] print("ATHENA: takeSnapshot") resp = athena_post({ "method": "takeSnapshot", "id": 0, "jsonrpc": "2.0" }) assert resp.get('result'), resp assert resp['result']['jpegBack'], resp['result'] @with_processes(["deviceStated"]) def test_athena_deviceState(): print("ATHENA: getMessage(deviceState)") resp = athena_post({ "method": "getMessage", "params": { "service": "deviceState", "timeout": 5000 }, "id": 0, "jsonrpc": "2.0" }) assert resp.get('result'), resp assert resp['result']['deviceState'], resp['result'] test_athena_deviceState() finally: try: athenad_pid = subprocess.check_output( ["pgrep", "-P", manage_athenad_pid], encoding="utf-8").strip() except subprocess.CalledProcessError: athenad_pid = None try: os.kill(int(manage_athenad_pid), signal.SIGINT) os.kill(int(athenad_pid), signal.SIGINT) except (OSError, TypeError): pass
def controlsd_thread(gctx, rate=100): # start the loop set_realtime_priority(2) context = zmq.Context() # pub live100 = messaging.pub_sock(context, service_list['live100'].port) carstate = messaging.pub_sock(context, service_list['carState'].port) carcontrol = messaging.pub_sock(context, service_list['carControl'].port) sendcan = messaging.pub_sock(context, service_list['sendcan'].port) livempc = messaging.pub_sock(context, service_list['liveMpc'].port) # sub thermal = messaging.sub_sock(context, service_list['thermal'].port) health = messaging.sub_sock(context, service_list['health'].port) cal = messaging.sub_sock(context, service_list['liveCalibration'].port) logcan = messaging.sub_sock(context, service_list['can'].port) CC = car.CarControl.new_message() CI, CP = get_car(logcan, sendcan) PL = Planner(CP) LoC = LongControl(CI.compute_gb) VM = VehicleModel(CP) LaC = LatControl(VM) AM = AlertManager() # write CarParams params = Params() params.put("CarParams", CP.to_bytes()) state = State.DISABLED soft_disable_timer = 0 v_cruise_kph = 255 cal_perc = 0 cal_status = Calibration.UNCALIBRATED rear_view_toggle = False rear_view_allowed = params.get("IsRearViewMirror") == "1" # 0.0 - 1.0 awareness_status = 0. rk = Ratekeeper(rate, print_delay_threshold=2./1000) # learned angle offset angle_offset = 0. calibration_params = params.get("CalibrationParams") if calibration_params: try: calibration_params = json.loads(calibration_params) angle_offset = calibration_params["angle_offset"] except (ValueError, KeyError): pass prof = Profiler() while 1: prof.reset() # avoid memory leak # sample data and compute car events CS, events, cal_status, cal_perc = data_sample(CI, CC, thermal, health, cal, cal_status, cal_perc) prof.checkpoint("Sample") # define plan plan, plan_ts = calc_plan(CS, events, PL, LoC) prof.checkpoint("Plan") # update control state state, soft_disable_timer, v_cruise_kph = state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, cal_perc, AM) prof.checkpoint("State transition") # compute actuators actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle = state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_status, PL, LaC, LoC, VM, angle_offset, rear_view_allowed, rear_view_toggle) prof.checkpoint("State Control") # publish data CC = data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, live100, livempc, AM, rear_view_allowed, rear_view_toggle, awareness_status, LaC, LoC, angle_offset) prof.checkpoint("Sent") # *** run loop at fixed rate *** if rk.keep_time(): prof.display()
class Planner(object): def __init__(self, CP, fcw_enabled): context = zmq.Context() self.CP = CP self.poller = zmq.Poller() self.plan = messaging.pub_sock(context, service_list['plan'].port) self.live_longitudinal_mpc = messaging.pub_sock( context, service_list['liveLongitudinalMpc'].port) self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc) self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc) self.v_acc_start = 0.0 self.a_acc_start = 0.0 self.v_acc = 0.0 self.v_acc_future = 0.0 self.a_acc = 0.0 self.v_cruise = 0.0 self.a_cruise = 0.0 self.longitudinalPlanSource = 'cruise' self.fcw_checker = FCWChecker() self.fcw_enabled = fcw_enabled self.params = Params() def choose_solution(self, v_cruise_setpoint, enabled): if enabled: solutions = {'cruise': self.v_cruise} if self.mpc1.prev_lead_status: solutions['mpc1'] = self.mpc1.v_mpc if self.mpc2.prev_lead_status: solutions['mpc2'] = self.mpc2.v_mpc slowest = min(solutions, key=solutions.get) self.longitudinalPlanSource = slowest # Choose lowest of MPC and cruise if slowest == 'mpc1': self.v_acc = self.mpc1.v_mpc self.a_acc = self.mpc1.a_mpc elif slowest == 'mpc2': self.v_acc = self.mpc2.v_mpc self.a_acc = self.mpc2.a_mpc elif slowest == 'cruise': self.v_acc = self.v_cruise self.a_acc = self.a_cruise self.v_acc_future = min([ self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint ]) def update(self, rcv_times, CS, CP, VM, PP, radar_state, controls_state, md, live_map_data): """Gets called when new radarState is available""" cur_time = sec_since_boot() v_ego = CS.carState.vEgo long_control_state = controls_state.controlsState.longControlState v_cruise_kph = controls_state.controlsState.vCruise force_slow_decel = controls_state.controlsState.forceDecel v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS lead_1 = radar_state.radarState.leadOne lead_2 = radar_state.radarState.leadTwo enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0 v_speedlimit = NO_CURVATURE_SPEED v_curvature = NO_CURVATURE_SPEED map_age = cur_time - rcv_times['liveMapData'] map_valid = live_map_data.liveMapData.mapValid and map_age < 10.0 # Speed limit and curvature set_speed_limit_active = self.params.get( "LimitSetSpeed") == "1" and self.params.get( "SpeedLimitOffset") is not None if set_speed_limit_active and map_valid: if live_map_data.liveMapData.speedLimitValid: speed_limit = live_map_data.liveMapData.speedLimit offset = float(self.params.get("SpeedLimitOffset")) v_speedlimit = speed_limit + offset if live_map_data.liveMapData.curvatureValid: curvature = abs(live_map_data.liveMapData.curvature) a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph v_curvature = math.sqrt(a_y_max / max(1e-4, curvature)) v_curvature = min(NO_CURVATURE_SPEED, v_curvature) decel_for_turn = bool( v_curvature < min([v_cruise_setpoint, v_speedlimit, v_ego + 1.])) v_cruise_setpoint = min([v_cruise_setpoint, v_curvature, v_speedlimit]) # Calculate speed for normal cruise control if enabled: accel_limits = [ float(x) for x in calc_cruise_accel_limits(v_ego, following) ] jerk_limits = [ min(-0.1, accel_limits[0]), max(0.1, accel_limits[1]) ] # TODO: make a separate lookup for jerk tuning accel_limits = limit_accel_in_turns(v_ego, CS.carState.steeringAngle, accel_limits, self.CP) if force_slow_decel: # if required so, force a smooth deceleration accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL) accel_limits[0] = min(accel_limits[0], accel_limits[1]) # Change accel limits based on time remaining to turn if decel_for_turn: time_to_turn = max( 1.0, live_map_data.liveMapData.distToTurn / max(self.v_cruise, 1.)) required_decel = min(0, (v_curvature - self.v_cruise) / time_to_turn) accel_limits[0] = max(accel_limits[0], required_decel) self.v_cruise, self.a_cruise = speed_smoother( self.v_acc_start, self.a_acc_start, v_cruise_setpoint, accel_limits[1], accel_limits[0], jerk_limits[1], jerk_limits[0], _DT_MPC) # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) else: starting = long_control_state == LongCtrlState.starting a_ego = min(CS.carState.aEgo, 0.0) reset_speed = MIN_CAN_SPEED if starting else v_ego reset_accel = self.CP.startAccel if starting else a_ego self.v_acc = reset_speed self.a_acc = reset_accel self.v_acc_start = reset_speed self.a_acc_start = reset_accel self.v_cruise = reset_speed self.a_cruise = reset_accel self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc1.update(CS, lead_1, v_cruise_setpoint) self.mpc2.update(CS, lead_2, v_cruise_setpoint) self.choose_solution(v_cruise_setpoint, enabled) # determine fcw if self.mpc1.new_lead: self.fcw_checker.reset_lead(cur_time) blinkers = CS.carState.leftBlinker or CS.carState.rightBlinker fcw = self.fcw_checker.update( self.mpc1.mpc_solution, cur_time, v_ego, CS.carState.aEgo, lead_1.dRel, lead_1.vLead, lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw, blinkers) and not CS.carState.brakePressed if fcw: cloudlog.info("FCW triggered %s", self.fcw_checker.counters) radar_dead = cur_time - rcv_times['radarState'] > 0.5 radar_errors = list(radar_state.radarState.radarErrors) radar_fault = car.RadarData.Error.fault in radar_errors radar_comm_issue = car.RadarData.Error.commIssue in radar_errors # **** send the plan **** plan_send = messaging.new_message() plan_send.init('plan') plan_send.plan.mdMonoTime = md.logMonoTime plan_send.plan.radarStateMonoTime = radar_state.logMonoTime # longitudal plan plan_send.plan.vCruise = self.v_cruise plan_send.plan.aCruise = self.a_cruise plan_send.plan.vStart = self.v_acc_start plan_send.plan.aStart = self.a_acc_start plan_send.plan.vTarget = self.v_acc plan_send.plan.aTarget = self.a_acc plan_send.plan.vTargetFuture = self.v_acc_future plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource plan_send.plan.vCurvature = v_curvature plan_send.plan.decelForTurn = decel_for_turn plan_send.plan.mapValid = map_valid radar_valid = not (radar_dead or radar_fault) plan_send.plan.radarValid = bool(radar_valid) plan_send.plan.radarCommIssue = bool(radar_comm_issue) plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - rcv_times['radarState'] # Send out fcw fcw = fcw and (self.fcw_enabled or long_control_state != LongCtrlState.off) plan_send.plan.fcw = fcw self.plan.send(plan_send.to_bytes()) # Interpolate 0.05 seconds and save as starting point for next iteration dt = 0.05 # s a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc - self.a_acc_start) v_acc_sol = self.v_acc_start + dt * (a_acc_sol + self.a_acc_start) / 2.0 self.v_acc_start = v_acc_sol self.a_acc_start = a_acc_sol
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.manager_start() params.put("OpenpilotEnabledToggle", "1") params.put("Passive", "0") params.put("CommunityFeaturesToggle", "1") os.environ['NO_RADAR_SLEEP'] = "1" os.environ['SKIP_FW_QUERY'] = "1" os.environ['FINGERPRINT'] = "" for msg in lr: if msg.which() == 'carParams': # TODO: get a stock VW route if "Generic Volkswagen" in msg.carParams.carFingerprint: os.environ['FINGERPRINT'] = "VOLKSWAGEN GOLF" else: os.environ['FINGERPRINT'] = msg.carParams.carFingerprint break manager.prepare_managed_process(cfg.proc_name, build=True) 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 __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
class CarController(): def __init__(self, dbc_name): self.fleet_speed_state = 0 self.cc_counter = 0 self.alcaStateData = None self.icLeadsData = None self.params = Params() self.braking = False self.brake_steady = 0. self.brake_last = 0. self.packer = CANPacker(dbc_name) self.epas_disabled = True self.last_angle = 0. self.last_accel = 0. self.blinker = Blinker() self.ALCA = ALCAController(self,True,True) # Enabled and SteerByAngle both True self.ACC = ACCController(self) self.PCC = PCCController(self) self.HSO = HSOController(self) self.GYRO = GYROController() self.AHB = AHBController(self) self.sent_DAS_bootID = False self.speedlimit = None self.trafficevents = messaging.sub_sock('trafficEvents', conflate=True) self.pathPlan = messaging.sub_sock('pathPlan', conflate=True) self.radarState = messaging.sub_sock('radarState', conflate=True) self.icLeads = messaging.sub_sock('uiIcLeads', conflate=True) self.icCarLR = messaging.sub_sock('uiIcCarLR', conflate=True) self.alcaState = messaging.sub_sock('alcaState', conflate=True) self.gpsLocationExternal = None self.opState = 0 # 0-disabled, 1-enabled, 2-disabling, 3-unavailable, 5-warning self.accPitch = 0. self.accRoll = 0. self.accYaw = 0. self.magPitch = 0. self.magRoll = 0. self.magYaw = 0. self.gyroPitch = 0. self.gyroRoll = 0. self.gyroYaw = 0. self.set_speed_limit_active = False self.speed_limit_offset = 0. self.speed_limit_ms = 0. # for warnings self.warningCounter = 0 self.DAS_206_apUnavailable = 0 self.DAS_222_accCameraBlind = 0 #DAS_206 lkas not ebabled self.DAS_219_lcTempUnavailableSpeed = 0 self.DAS_220_lcTempUnavailableRoad = 0 self.DAS_221_lcAborting = 0 self.DAS_211_accNoSeatBelt = 0 self.DAS_207_lkasUnavailable = 0 #use for manual steer? self.DAS_208_rackDetected = 0 #use for low battery? self.DAS_202_noisyEnvironment = 0 #use for planner error? self.DAS_025_steeringOverride = 0 #another one to use for manual steer? self.warningNeeded = 0 # items for IC integration for Lane and Lead Car self.average_over_x_pathplan_values = 1 self.curv0Matrix = MovingAverage(self.average_over_x_pathplan_values) self.curv1Matrix = MovingAverage(self.average_over_x_pathplan_values) self.curv2Matrix = MovingAverage(self.average_over_x_pathplan_values) self.curv3Matrix = MovingAverage(self.average_over_x_pathplan_values) self.leadDxMatrix = MovingAverage(self.average_over_x_pathplan_values) self.leadDyMatrix = MovingAverage(self.average_over_x_pathplan_values) self.leadDx = 0. self.leadDy = 0. self.leadClass = 0 self.leadVx = 0. self.leadId = 0 self.lead2Dx = 0. self.lead2Dy = 0. self.lead2Class = 0 self.lead2Vx = 0. self.lead2Id = 0 self.lLine = 0 self.rLine = 0 self.curv0 = 0. self.curv1 = 0. self.curv2 = 0. self.curv3 = 0. self.visionCurvC0 = 0. self.laneRange = 30 #max is 160m but OP has issues with precision beyond 50 self.laneWidth = 0. self.stopSign_visible = False self.stopSign_distance = 1000. self.stopSign_action = 0 self.stopSign_resume = False self.stopLight_visible = False self.stopLight_distance = 1000. self.stopLight_action = 0 self.stopLight_resume = False self.stopLight_color = 0. #0-unknown, 1-red, 2-yellow, 3-green self.stopSignWarning = 0 self.stopLightWarning = 0 self.stopSignWarning_last = 0 self.stopLightWarning_last = 0 self.roadSignType = 0xFF self.roadSignStopDist = 1000. self.roadSignColor = 0. self.roadSignControlActive = 0 self.roadSignType_last = 0xFF self.roadSignDistanceWarning = 50. self.alca_enabled = False self.ldwStatus = 0 self.prev_ldwStatus = 0 self.radarVin_idx = 0 self.LDW_ENABLE_SPEED = 16 self.should_ldw = False self.ldw_numb_frame_end = 0 self.isMetric = (self.params.get("IsMetric") == "1") self.ahbLead1 = None def reset_traffic_events(self): self.stopSign_visible = False self.stopSign_distance = 1000. self.stopSign_action = 0 self.stopSign_resume = False self.stopLight_visible = False self.stopLight_distance = 1000. self.stopLight_action = 0 self.stopLight_resume = False self.stopLight_color = 0. #0-unknown, 1-red, 2-yellow, 3-green def checkWhichSign(self): self.stopSignWarning = 0 self.stopLightWarning = 0 self.roadSignType_last = self.roadSignType self.roadSignType = 0xFF self.roadSignStopDist = 1000. self.roadSignColor = 0 self.roadSignControlActive = 0 if (self.stopSign_distance < self.stopLight_distance): self.roadSignType = 0x00 self.roadSignStopDist = self.stopSign_distance self.roadSignColor = 0 self.roadSignControlActive = self.stopSign_resume if (self.stopSign_distance < self.roadSignDistanceWarning ): self.stopSignWarning = 1 elif (self.stopLight_distance < self.stopSign_distance ): self.roadSignType = 0x01 self.roadSignStopDist = self.stopLight_distance self.roadSignColor = self.stopLight_color self.roadSignControlActive = self.stopLight_resume if (self.stopLight_distance < self.roadSignDistanceWarning ) and (self.roadSignColor == 1): self.stopLightWarning = 1 def update(self, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \ snd_beep, snd_chime,leftLaneVisible,rightLaneVisible): if (not enabled) and (self.ALCA.laneChange_cancelled): self.ALCA.laneChange_cancelled = False self.ALCA.laneChange_cancelled_counter = 0 self.warningNeeded = 1 if self.warningCounter > 0: self.warningCounter = self.warningCounter - 1 if self.warningCounter == 0: self.warningNeeded = 1 if self.warningCounter == 0 or not enabled: # when zero reset all warnings self.DAS_222_accCameraBlind = 0 #we will see what we can use this for self.DAS_219_lcTempUnavailableSpeed = 0 self.DAS_220_lcTempUnavailableRoad = 0 self.DAS_221_lcAborting = 0 self.DAS_211_accNoSeatBelt = 0 self.DAS_207_lkasUnavailable = 0 #use for manual not in drive? self.DAS_208_rackDetected = 0 #use for low battery? self.DAS_202_noisyEnvironment = 0 #use for planner error? self.DAS_025_steeringOverride = 0 #use for manual steer? self.DAS_206_apUnavailable = 0 #Ap disabled from CID if CS.keepEonOff: if CS.cstm_btns.get_button_status("dsp") != 9: CS.cstm_btns.set_button_status("dsp",9) else: if CS.cstm_btns.get_button_status("dsp") != 1: CS.cstm_btns.set_button_status("dsp",1) # """ Controls thread """ if not CS.useTeslaMapData: if self.speedlimit is None: self.speedlimit = messaging.sub_sock('liveMapData', conflate=True) # *** no output if not enabled *** if not enabled and CS.pcm_acc_status: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated pcm_cancel_cmd = True # vehicle hud display, wait for one update from 10Hz 0x304 msg if hud_show_lanes: hud_lanes = 1 else: hud_lanes = 0 # TODO: factor this out better if enabled: if hud_show_car: hud_car = 2 else: hud_car = 1 else: hud_car = 0 # For lateral control-only, send chimes as a beep since we don't send 0x1fa #if CS.CP.radarOffCan: #print chime, alert_id, hud_alert fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required) if not all(isinstance(x, int) and 0 <= x < 256 for x in hud): print ("INVALID HUD", hud) hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0) # **** process the car messages **** # *** compute control surfaces *** # Prevent steering while stopped MIN_STEERING_VEHICLE_VELOCITY = 0.05 # m/s vehicle_moving = (CS.v_ego >= MIN_STEERING_VEHICLE_VELOCITY) #upodate custom UI buttons and alerts CS.UE.update_custom_ui() if (frame % 100 == 0): CS.cstm_btns.send_button_info() #read speed limit params if CS.hasTeslaIcIntegration: self.set_speed_limit_active = True self.speed_limit_offset = CS.userSpeedLimitOffsetKph else: self.set_speed_limit_active = (self.params.get("SpeedLimitOffset") is not None) and (self.params.get("LimitSetSpeed") == "1") if self.set_speed_limit_active: self.speed_limit_offset = float(self.params.get("SpeedLimitOffset")) if not self.isMetric: self.speed_limit_offset = self.speed_limit_offset * CV.MPH_TO_MS else: self.speed_limit_offset = 0. if CS.useTeslaGPS and (frame % 10 == 0): if self.gpsLocationExternal is None: self.gpsLocationExternal = messaging.pub_sock('gpsLocationExternal') sol = gen_solution(CS) sol.logMonoTime = int(realtime.sec_since_boot() * 1e9) self.gpsLocationExternal.send(sol.to_bytes()) #get pitch/roll/yaw every 0.1 sec if (frame %10 == 0): (self.accPitch, self.accRoll, self.accYaw),(self.magPitch, self.magRoll, self.magYaw),(self.gyroPitch, self.gyroRoll, self.gyroYaw) = self.GYRO.update(CS.v_ego,CS.a_ego,CS.angle_steers) CS.UE.uiGyroInfoEvent(self.accPitch, self.accRoll, self.accYaw,self.magPitch, self.magRoll, self.magYaw,self.gyroPitch, self.gyroRoll, self.gyroYaw) # Update statuses for custom buttons every 0.1 sec. if (frame % 10 == 0): self.ALCA.update_status((CS.cstm_btns.get_button_status("alca") > 0) and ((CS.enableALCA and not CS.hasTeslaIcIntegration) or (CS.hasTeslaIcIntegration and CS.alcaEnabled))) self.blinker.update_state(CS, frame) # update PCC module info pedal_can_sends = self.PCC.update_stat(CS, frame) if self.PCC.pcc_available: self.ACC.enable_adaptive_cruise = False else: # Update ACC module info. self.ACC.update_stat(CS, True) self.PCC.enable_pedal_cruise = False # update CS.v_cruise_pcm based on module selected. speed_uom_kph = 1. if CS.imperial_speed_units: speed_uom_kph = CV.KPH_TO_MPH if self.ACC.enable_adaptive_cruise: CS.v_cruise_pcm = self.ACC.acc_speed_kph * speed_uom_kph elif self.PCC.enable_pedal_cruise: CS.v_cruise_pcm = self.PCC.pedal_speed_kph * speed_uom_kph else: CS.v_cruise_pcm = max(0., CS.v_ego * CV.MS_TO_KPH) * speed_uom_kph self.alca_enabled = self.ALCA.update(enabled, CS, actuators, self.alcaStateData, frame, self.blinker) self.should_ldw = self._should_ldw(CS, frame) apply_angle = -actuators.steerAngle # Tesla is reversed vs OP. # Update HSO module info. human_control = self.HSO.update_stat(self,CS, enabled, actuators, frame) human_lane_changing = CS.turn_signal_stalk_state > 0 and not self.alca_enabled enable_steer_control = (enabled and not human_lane_changing and not human_control and vehicle_moving) angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V) apply_angle = clip(apply_angle, -angle_lim, angle_lim) # Windup slower. if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle): angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V) else: angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU) #BB disable limits to test 0.5.8 # apply_angle = clip(apply_angle , self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) # If human control, send the steering angle as read at steering wheel. if human_control: apply_angle = CS.angle_steers # Send CAN commands. can_sends = [] #if using radar, we need to send the VIN if CS.useTeslaRadar and (frame % 100 == 0): useRadar=0 if CS.useTeslaRadar: useRadar=1 can_sends.append(teslacan.create_radar_VIN_msg(self.radarVin_idx,CS.radarVIN,1,0x108,useRadar,CS.radarPosition,CS.radarEpasType)) self.radarVin_idx += 1 self.radarVin_idx = self.radarVin_idx % 3 #First we emulate DAS. # DAS_longC_enabled (1),DAS_speed_override (1),DAS_apUnavailable (1), DAS_collision_warning (1), DAS_op_status (4) # DAS_speed_kph(8), # DAS_turn_signal_request (2),DAS_forward_collision_warning (2), DAS_hands_on_state (3), # DAS_cc_state (2), DAS_usingPedal(1),DAS_alca_state (5), # DAS_acc_speed_limit (8), # DAS_speed_limit_units(8) #send fake_das data as 0x553 # TODO: forward collision warning if frame % 10 == 0: speedlimitMsg = None if self.speedlimit is not None: speedlimitMsg = messaging.recv_one_or_none(self.speedlimit) icLeadsMsg = self.icLeads.receive(non_blocking=True) radarStateMsg = messaging.recv_one_or_none(self.radarState) alcaStateMsg = self.alcaState.receive(non_blocking=True) pathPlanMsg = messaging.recv_one_or_none(self.pathPlan) icCarLRMsg = self.icCarLR.receive(non_blocking=True) trafficeventsMsgs = None if self.trafficevents is not None: trafficeventsMsgs = messaging.recv_sock(self.trafficevents) if CS.hasTeslaIcIntegration: self.speed_limit_ms = CS.speed_limit_ms if (speedlimitMsg is not None) and not CS.useTeslaMapData: lmd = speedlimitMsg.liveMapData self.speed_limit_ms = lmd.speedLimit if lmd.speedLimitValid else 0 if icLeadsMsg is not None: self.icLeadsData = tesla.ICLeads.from_bytes(icLeadsMsg) if radarStateMsg is not None: #to show lead car on IC if self.icLeadsData is not None: can_messages = self.showLeadCarOnICCanMessage(radarStateMsg = radarStateMsg) can_sends.extend(can_messages) if alcaStateMsg is not None: self.alcaStateData = tesla.ALCAState.from_bytes(alcaStateMsg) if pathPlanMsg is not None: #to show curvature and lanes on IC if self.alcaStateData is not None: self.handlePathPlanSocketForCurvatureOnIC(pathPlanMsg = pathPlanMsg, alcaStateData = self.alcaStateData,CS = CS) if icCarLRMsg is not None: can_messages = self.showLeftAndRightCarsOnICCanMessages(icCarLRMsg = tesla.ICCarsLR.from_bytes(icCarLRMsg)) can_sends.extend(can_messages) if trafficeventsMsgs is not None: can_messages = self.handleTrafficEvents(trafficEventsMsgs = trafficeventsMsgs) can_sends.extend(can_messages) op_status = 0x02 hands_on_state = 0x00 forward_collision_warning = 0 #1 if needed if hud_alert == AH.FCW: forward_collision_warning = hud_alert[1] if forward_collision_warning > 1: forward_collision_warning = 1 #cruise state: 0 unavailable, 1 available, 2 enabled, 3 hold cc_state = 1 alca_state = 0x00 speed_override = 0 collision_warning = 0x00 speed_control_enabled = 0 accel_min = -15 accel_max = 5 acc_speed_kph = 0 send_fake_warning = False send_fake_msg = False if enabled: #self.opState 0-disabled, 1-enabled, 2-disabling, 3-unavailable, 5-warning alca_state = 0x01 if self.opState == 0: op_status = 0x02 if self.opState == 1: op_status = 0x03 if self.opState == 2: op_status = 0x08 if self.opState == 3: op_status = 0x01 if self.opState == 5: op_status = 0x03 if self.blinker.override_direction > 0: alca_state = 0x08 + self.blinker.override_direction elif (self.lLine > 1) and (self.rLine > 1): alca_state = 0x08 elif (self.lLine > 1): alca_state = 0x06 elif (self.rLine > 1): alca_state = 0x07 else: alca_state = 0x01 #canceled by user if self.ALCA.laneChange_cancelled and (self.ALCA.laneChange_cancelled_counter > 0): alca_state = 0x14 #min speed for ALCA if (CS.CL_MIN_V > CS.v_ego): alca_state = 0x05 #max angle for ALCA if (abs(actuators.steerAngle) >= CS.CL_MAX_A): alca_state = 0x15 if not enable_steer_control: #op_status = 0x08 hands_on_state = 0x02 if hud_alert == AH.STEER: if snd_chime == CM.MUTE: hands_on_state = 0x03 else: hands_on_state = 0x05 if self.PCC.pcc_available: acc_speed_kph = self.PCC.pedal_speed_kph if hud_alert == AH.FCW: collision_warning = hud_alert[1] if collision_warning > 1: collision_warning = 1 #use disabling for alerts/errors to make them aware someting is goin going on if (snd_chime == CM.DOUBLE) or (hud_alert == AH.FCW): op_status = 0x08 if self.ACC.enable_adaptive_cruise: acc_speed_kph = self.ACC.new_speed #pcm_speed * CV.MS_TO_KPH if (self.PCC.pcc_available and self.PCC.enable_pedal_cruise) or (self.ACC.enable_adaptive_cruise): speed_control_enabled = 1 cc_state = 2 if not self.ACC.adaptive: cc_state = 3 CS.speed_control_enabled = 1 else: CS.speed_control_enabled = 0 if (CS.pcm_acc_status == 4): #car CC enabled but not OP, display the HOLD message cc_state = 3 else: if (CS.pcm_acc_status == 4): cc_state = 3 if enabled: if frame % 2 == 0: send_fake_msg = True if frame % 25 == 0: send_fake_warning = True else: if frame % 23 == 0: send_fake_msg = True if frame % 60 == 0: send_fake_warning = True if frame % 10 == 0: can_sends.append(teslacan.create_fake_DAS_obj_lane_msg(self.leadDx,self.leadDy,self.leadClass,self.rLine,self.lLine,self.curv0,self.curv1,self.curv2,self.curv3,self.laneRange,self.laneWidth)) speed_override = 0 if (CS.pedal_interceptor_value > 10) and (cc_state > 1): speed_override = 0 #force zero for now if (not enable_steer_control) and op_status == 3: #hands_on_state = 0x03 self.DAS_219_lcTempUnavailableSpeed = 1 self.warningCounter = 100 self.warningNeeded = 1 if enabled and self.ALCA.laneChange_cancelled and (not CS.steer_override) and (CS.turn_signal_stalk_state == 0) and (self.ALCA.laneChange_cancelled_counter > 0): self.DAS_221_lcAborting = 1 self.warningCounter = 300 self.warningNeeded = 1 if CS.hasTeslaIcIntegration: highLowBeamStatus,highLowBeamReason,ahbIsEnabled = self.AHB.update(CS,frame,self.ahbLead1) if frame % 5 == 0: self.cc_counter = (self.cc_counter + 1) % 40 #use this to change status once a second self.fleet_speed_state = 0x00 #fleet speed unavailable if FleetSpeed.is_available(CS): if self.ACC.fleet_speed.is_active(frame) or self.PCC.fleet_speed.is_active(frame): self.fleet_speed_state = 0x02 #fleet speed enabled else: self.fleet_speed_state = 0x01 #fleet speed available can_sends.append(teslacan.create_fake_DAS_msg2(highLowBeamStatus,highLowBeamReason,ahbIsEnabled,self.fleet_speed_state)) if (self.cc_counter < 3) and (self.fleet_speed_state == 0x02): CS.v_cruise_pcm = CS.v_cruise_pcm + 1 send_fake_msg = True if (self.cc_counter == 3): send_fake_msg = True if send_fake_msg: if enable_steer_control and op_status == 3: op_status = 0x5 park_brake_request = int(CS.ahbEnabled) if park_brake_request == 1: print("Park Brake Request received") adaptive_cruise = 1 if (not self.PCC.pcc_available and self.ACC.adaptive) or self.PCC.pcc_available else 0 can_sends.append(teslacan.create_fake_DAS_msg(speed_control_enabled,speed_override,self.DAS_206_apUnavailable, collision_warning, op_status, \ acc_speed_kph, \ self.blinker.override_direction,forward_collision_warning, adaptive_cruise, hands_on_state, \ cc_state, 1 if self.PCC.pcc_available else 0, alca_state, \ CS.v_cruise_pcm, CS.DAS_fusedSpeedLimit, apply_angle, 1 if enable_steer_control else 0, park_brake_request)) if send_fake_warning or (self.opState == 2) or (self.opState == 5) or (self.stopSignWarning != self.stopSignWarning_last) or (self.stopLightWarning != self.stopLightWarning_last) or (self.warningNeeded == 1) or (frame % 100 == 0): #if it's time to send OR we have a warning or emergency disable can_sends.append(teslacan.create_fake_DAS_warning(self.DAS_211_accNoSeatBelt, CS.DAS_canErrors, \ self.DAS_202_noisyEnvironment, CS.DAS_doorOpen, CS.DAS_notInDrive, CS.enableDasEmulation, CS.enableRadarEmulation, \ self.stopSignWarning, self.stopLightWarning, \ self.DAS_222_accCameraBlind, self.DAS_219_lcTempUnavailableSpeed, self.DAS_220_lcTempUnavailableRoad, self.DAS_221_lcAborting, \ self.DAS_207_lkasUnavailable,self.DAS_208_rackDetected, self.DAS_025_steeringOverride,self.ldwStatus,0,CS.useWithoutHarness)) self.stopLightWarning_last = self.stopLightWarning self.stopSignWarning_last = self.stopSignWarning self.warningNeeded = 0 # end of DAS emulation """ if frame % 100 == 0: # and CS.hasTeslaIcIntegration: #IF WE HAVE softPanda RUNNING, send a message every second to say we are still awake can_sends.append(teslacan.create_fake_IC_msg()) # send enabled ethernet every 0.2 sec if frame % 20 == 0: can_sends.append(teslacan.create_enabled_eth_msg(1)) if (not self.PCC.pcc_available) and frame % 5 == 0: # acc processed at 20Hz cruise_btn = self.ACC.update_acc(enabled, CS, frame, actuators, pcm_speed, \ self.speed_limit_ms * CV.MS_TO_KPH, self.set_speed_limit_active, self.speed_limit_offset) if cruise_btn: cruise_msg = teslacan.create_cruise_adjust_msg( spdCtrlLvr_stat=cruise_btn, turnIndLvr_Stat= 0, real_steering_wheel_stalk=CS.steering_wheel_stalk) # Send this CAN msg first because it is racing against the real stalk. can_sends.insert(0, cruise_msg) apply_accel = 0. if self.PCC.pcc_available and frame % 5 == 0: # pedal processed at 20Hz apply_accel, accel_needed, accel_idx = self.PCC.update_pdl(enabled, CS, frame, actuators, pcm_speed, \ self.speed_limit_ms, self.set_speed_limit_active, self.speed_limit_offset * CV.KPH_TO_MS, self.alca_enabled) can_sends.append(teslacan.create_pedal_command_msg(apply_accel, int(accel_needed), accel_idx)) self.last_angle = apply_angle self.last_accel = apply_accel return pedal_can_sends + can_sends #to show lead car on IC def showLeadCarOnICCanMessage(self, radarStateMsg): messages = [] leads = radarStateMsg.radarState if leads is None: self.ahbLead1 = None return messages lead_1 = leads.leadOne lead_2 = leads.leadTwo if (lead_1 is not None) and lead_1.status: self.ahbLead1 = lead_1 self.leadDx = lead_1.dRel self.leadDy = self.curv0-lead_1.yRel self.leadId = self.icLeadsData.lead1trackId self.leadClass = self.icLeadsData.lead1oClass self.leadVx = lead_1.vRel if (self.leadId <= 0) or (self.leadId == 63): self.leadId = 61 else: self.leadDx = 0. self.leadDy = 0. self.leadClass = 0 self.leadId = 0 self.leadVx = 0xF if (lead_2 is not None) and lead_2.status: self.lead2Dx = lead_2.dRel self.lead2Dy = self.curv0-lead_2.yRel self.lead2Id = self.icLeadsData.lead2trackId self.lead2Class = self.icLeadsData.lead2oClass self.lead2Vx = lead_2.vRel if (self.lead2Id <= 0) or (self.lead2Id == 63): self.leadId = 62 else: self.lead2Dx = 0. self.lead2Dy = 0. self.lead2Class = 0 self.lead2Id = 0 self.lead2Vx = 0xF messages.append(teslacan.create_DAS_LR_object_msg(0,self.leadClass, self.leadId, self.leadDx,self.leadDy,self.leadVx,self.lead2Class, self.lead2Id,self.lead2Dx,self.lead2Dy,self.lead2Vx)) return messages def handlePathPlanSocketForCurvatureOnIC(self, pathPlanMsg, alcaStateData, CS): pp = pathPlanMsg.pathPlan if pp.paramsValid: if pp.lProb > 0.75: self.lLine = 3 elif pp.lProb > 0.5: self.lLine = 2 elif pp.lProb > 0.25: self.lLine = 1 else: self.lLine = 0 if pp.rProb > 0.75: self.rLine = 3 elif pp.rProb > 0.5: self.rLine = 2 elif pp.rProb > 0.25: self.rLine = 1 else: self.rLine = 0 #first we clip to the AP limits of the coefficients self.curv0 = -clip(pp.dPoly[3],-3.5,3.5) #self.curv0Matrix.add(-clip(pp.cPoly[3],-3.5,3.5)) self.curv1 = -clip(pp.dPoly[2],-0.2,0.2) #self.curv1Matrix.add(-clip(pp.cPoly[2],-0.2,0.2)) self.curv2 = -clip(pp.dPoly[1],-0.0025,0.0025) #self.curv2Matrix.add(-clip(pp.cPoly[1],-0.0025,0.0025)) self.curv3 = -clip(pp.dPoly[0],-0.00003,0.00003) #self.curv3Matrix.add(-clip(pp.cPoly[0],-0.00003,0.00003)) self.laneWidth = pp.laneWidth self.visionCurvC0 = self.curv0 self.prev_ldwStatus = self.ldwStatus self.ldwStatus = 0 if alcaStateData.alcaEnabled: #exagerate position a little during ALCA to make lane change look smoother on IC self.curv1 = 0.0 #straighten the turn for ALCA self.curv0 = -self.ALCA.laneChange_direction * alcaStateData.alcaLaneWidth * alcaStateData.alcaStep / alcaStateData.alcaTotalSteps #animas late change on IC self.curv0 = clip(self.curv0, -3.5, 3.5) self.lLine = 3 self.rLine = 3 else: if self.should_ldw: if pp.lProb > LDW_LANE_PROBAB: lLaneC0 = -pp.lPoly[3] if abs(lLaneC0) < LDW_WARNING_2: self.ldwStatus = 3 elif abs(lLaneC0) < LDW_WARNING_1: self.ldwStatus = 1 if pp.rProb > LDW_LANE_PROBAB: rLaneC0 = -pp.rPoly[3] if abs(rLaneC0) < LDW_WARNING_2: self.ldwStatus = 4 elif abs(rLaneC0) < LDW_WARNING_1: self.ldwStatus = 2 if not(self.prev_ldwStatus == self.ldwStatus): self.warningNeeded = 1 if self.ldwStatus > 0: self.warningCounter = 50 else: self.lLine = 0 self.rLine = 0 self.curv0 = self.curv0Matrix.add(0.) self.curv1 = self.curv1Matrix.add(0.) self.curv2 = self.curv2Matrix.add(0.) self.curv3 = self.curv3Matrix.add(0.) # Generates IC messages for the Left and Right radar identified cars from radard def showLeftAndRightCarsOnICCanMessages(self, icCarLRMsg): messages = [] icCarLR_msg = icCarLRMsg if icCarLR_msg is not None: #for icCarLR_msg in icCarLR_list: messages.append(teslacan.create_DAS_LR_object_msg(1,icCarLR_msg.v1Type,icCarLR_msg.v1Id, icCarLR_msg.v1Dx,icCarLR_msg.v1Dy,icCarLR_msg.v1Vrel,icCarLR_msg.v2Type, icCarLR_msg.v2Id,icCarLR_msg.v2Dx,icCarLR_msg.v2Dy,icCarLR_msg.v2Vrel)) messages.append(teslacan.create_DAS_LR_object_msg(2,icCarLR_msg.v3Type,icCarLR_msg.v3Id, icCarLR_msg.v3Dx,icCarLR_msg.v3Dy,icCarLR_msg.v3Vrel,icCarLR_msg.v4Type, icCarLR_msg.v4Id,icCarLR_msg.v4Dx,icCarLR_msg.v4Dy,icCarLR_msg.v4Vrel)) return messages def handleTrafficEvents(self, trafficEventsMsgs): messages = [] self.reset_traffic_events() tr_ev_list = trafficEventsMsgs if tr_ev_list is not None: for tr_ev in tr_ev_list.trafficEvents: if tr_ev.type == 0x00: if (tr_ev.distance < self.stopSign_distance): self.stopSign_visible = True self.stopSign_distance = tr_ev.distance self.stopSign_action = tr_ev.action self.stopSign_resume = tr_ev.resuming if tr_ev.type == 0x04: if (tr_ev.distance < self.stopLight_distance): self.stopLight_visible = True self.stopLight_distance = tr_ev.distance self.stopLight_action = tr_ev.action self.stopLight_resume = tr_ev.resuming self.stopLight_color = 1. #0-unknown, 1-red, 2-yellow, 3-green if tr_ev.type == 0x01: if (tr_ev.distance < self.stopLight_distance): self.stopLight_visible = True self.stopLight_distance = tr_ev.distance self.stopLight_action = tr_ev.action self.stopLight_resume = tr_ev.resuming self.stopLight_color = 1. #0-unknown, 1-red, 2-yellow, 3-green if tr_ev.type == 0x02: if (tr_ev.distance < self.stopLight_distance): self.stopLight_visible = True self.stopLight_distance = tr_ev.distance self.stopLight_action = tr_ev.action self.stopLight_resume = tr_ev.resuming self.stopLight_color = 2. #0-unknown, 1-red, 2-yellow, 3-green if tr_ev.type == 0x03: if (tr_ev.distance < self.stopLight_distance): self.stopLight_visible = True self.stopLight_distance = tr_ev.distance self.stopLight_action = tr_ev.action self.stopLight_resume = tr_ev.resuming self.stopLight_color = 3. #0-unknown, 1-red, 2-yellow, 3-green self.checkWhichSign() if not ((self.roadSignType_last == self.roadSignType) and (self.roadSignType == 0xFF)): messages.append(teslacan.create_fake_DAS_sign_msg(self.roadSignType,self.roadSignStopDist,self.roadSignColor,self.roadSignControlActive)) return messages def _should_ldw(self, CS, frame): if not CS.enableLdw: return False if CS.prev_turn_signal_blinking and not CS.turn_signal_blinking: self.ldw_numb_frame_end = frame + int(100 * CS.ldwNumbPeriod) return CS.v_ego >= self.LDW_ENABLE_SPEED and not CS.turn_signal_blinking and frame > self.ldw_numb_frame_end
def thermald_thread(): setup_eon_fan() # prevent LEECO from undervoltage BATT_PERC_OFF = 10 if LEON else 3 # now loop context = zmq.Context() thermal_sock = messaging.pub_sock(context, service_list['thermal'].port) health_sock = messaging.sub_sock(context, service_list['health'].port) location_sock = messaging.sub_sock(context, service_list['gpsLocation'].port) fan_speed = 0 count = 0 off_ts = None started_ts = None ignition_seen = False started_seen = False passive_starter = LocationStarter() thermal_status = ThermalStatus.green health_sock.RCVTIMEO = 1500 current_filter = FirstOrderFilter(0., CURRENT_TAU, 1.) # Make sure charging is enabled charging_disabled = False os.system('echo "1" > /sys/class/power_supply/battery/charging_enabled') params = Params() while 1: health = messaging.recv_sock(health_sock, wait=True) location = messaging.recv_sock(location_sock) location = location.gpsLocation if location else None msg = read_thermal() # loggerd is gated based on free space statvfs = os.statvfs(ROOT) avail = (statvfs.f_bavail * 1.0) / statvfs.f_blocks # thermal message now also includes free space msg.thermal.freeSpace = avail with open("/sys/class/power_supply/battery/capacity") as f: msg.thermal.batteryPercent = int(f.read()) with open("/sys/class/power_supply/battery/status") as f: msg.thermal.batteryStatus = f.read().strip() with open("/sys/class/power_supply/battery/current_now") as f: msg.thermal.batteryCurrent = int(f.read()) with open("/sys/class/power_supply/battery/voltage_now") as f: msg.thermal.batteryVoltage = int(f.read()) with open("/sys/class/power_supply/usb/present") as f: msg.thermal.usbOnline = bool(int(f.read())) current_filter.update(msg.thermal.batteryCurrent / 1e6) # TODO: add car battery voltage check max_cpu_temp = max(msg.thermal.cpu0, msg.thermal.cpu1, msg.thermal.cpu2, msg.thermal.cpu3) / 10.0 max_comp_temp = max(max_cpu_temp, msg.thermal.mem / 10., msg.thermal.gpu / 10.) bat_temp = msg.thermal.bat / 1000. fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed) msg.thermal.fanSpeed = fan_speed # thermal logic with hysterisis if max_cpu_temp > 107. or bat_temp >= 63.: # onroad not allowed thermal_status = ThermalStatus.danger elif max_comp_temp > 95. 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 > 90.0: # hysteresis between engage not allowed and uploader not allowed thermal_status = clip(thermal_status, ThermalStatus.yellow, ThermalStatus.red) elif max_cpu_temp > 85.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: # all good thermal_status = ThermalStatus.green # **** starting logic **** # start constellation of processes when the car starts ignition = health is not None and health.health.started ignition_seen = ignition_seen or ignition # add voltage check for ignition if not ignition_seen and health is not None and health.health.voltage > 13500: ignition = True do_uninstall = params.get("DoUninstall") == "1" accepted_terms = params.get("HasAcceptedTerms") == "1" completed_training = params.get( "CompletedTrainingVersion") == training_version should_start = ignition # have we seen a panda? passive = (params.get("Passive") == "1") # start on gps movement if we haven't seen ignition and are in passive mode should_start = should_start or (not ( ignition_seen and health) # seen ignition and panda is connected and passive and passive_starter.update( started_ts, location)) # with 2% left, we killall, otherwise the phone will take a long time to boot should_start = should_start and msg.thermal.freeSpace > 0.02 # require usb power in passive mode should_start = should_start and (not passive or msg.thermal.usbOnline) # confirm we have completed training and aren't uninstalling should_start = should_start and accepted_terms and ( passive or completed_training) and (not do_uninstall) # 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 if thermal_status >= ThermalStatus.danger: # TODO: Add a better warning when this is happening should_start = False if should_start: off_ts = None if started_ts is None: params.car_start() started_ts = sec_since_boot() started_seen = True os.system( 'echo performance > /sys/class/devfreq/soc:qcom,cpubw/governor' ) else: started_ts = None if off_ts is None: off_ts = sec_since_boot() os.system( 'echo powersave > /sys/class/devfreq/soc:qcom,cpubw/governor' ) # shutdown if the battery gets lower than 3%, it's discharging, we aren't running for # more than a minute but we were running if msg.thermal.batteryPercent < BATT_PERC_OFF and msg.thermal.batteryStatus == "Discharging" and \ started_seen and (sec_since_boot() - off_ts) > 60: os.system('LD_LIBRARY_PATH="" svc power shutdown') #charging_disabled = check_car_battery_voltage(should_start, health, charging_disabled) msg.thermal.chargingDisabled = charging_disabled msg.thermal.chargingError = current_filter.x > 0. # 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 thermal_sock.send(msg.to_bytes()) print(msg) # report to server once per minute if (count % 60) == 0: cloudlog.event("STATUS_PACKET", count=count, health=(health.to_dict() if health else None), location=(location.to_dict() if location else None), thermal=msg.to_dict()) count += 1
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", "1850"), ("LqrKi", "30"), ("DcGain", "30"), ("IgnoreZone", "0"), ("PidKp", "20"), ("PidKi", "40"), ("PidKf", "4"), ("CameraOffsetAdj", "60"), ("SteerRatioAdj", "165"), ("SteerActuatorDelayAdj", "30"), ("SteerRateCostAdj", "50"), ("SteerLimitTimerAdj", "40"), ("TireStiffnessFactorAdj", "100"), ("SteerMaxAdj", "255"), ("SteerDeltaUpAdj", "3"), ("SteerDeltaDownAdj", "7"), ] # 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 get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): # pylint: disable=dangerous-default-value ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) ret.carName = "hyundai" ret.safetyModel = car.CarParams.SafetyModel.hyundai params = Params() if int(params.get('LateralControlPriority')) == 0: ret.radarOffCan = False #False(선행차우선) #True(차선우선) #선행차량 인식 마크 유무. else: ret.radarOffCan = True # Most Hyundai car ports are community features for now ret.communityFeature = False kyd = kyd_conf() tire_stiffness_factor = 1. ret.steerActuatorDelay = float(kyd.conf['SteerActuatorDelay']) # 0.3 ret.steerRateCost = 0.5 ret.steerLimitTimer = float(kyd.conf['SteerLimitTimer']) # 0.4 if int(params.get('LateralControlMethod')) == 0: if candidate == CAR.SANTAFE: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1830. + STD_CARGO_KG ret.wheelbase = 2.765 # Values from optimizer ret.steerRatio = 13.8 # 13.8 is spec end-to-end 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.SORENTO: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1950. + STD_CARGO_KG ret.wheelbase = 2.78 ret.steerRatio = 14.4 * 1.15 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.GENESIS: 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 in [CAR.K5, CAR.SONATA]: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1470. + STD_CARGO_KG ret.wheelbase = 2.80 ret.steerRatio = 12.75 ret.steerRateCost = 0.4 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_TURBO: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1565. + STD_CARGO_KG ret.wheelbase = 2.80 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 in [CAR.K5_HEV, CAR.SONATA_HEV]: ret.lateralTuning.pid.kf = 0.00006 ret.mass = 1595. + STD_CARGO_KG ret.wheelbase = 2.80 ret.steerRatio = 12.75 ret.steerRateCost = 0.4 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.GRANDEUR, CAR.K7]: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1570. + STD_CARGO_KG ret.wheelbase = 2.885 ret.steerRatio = 12.5 ret.steerRateCost = 0.4 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.GRANDEUR_HEV, CAR.K7_HEV]: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1675. + STD_CARGO_KG ret.wheelbase = 2.885 ret.steerRatio = 12.5 ret.steerRateCost = 0.4 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.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.KONA: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1330. + STD_CARGO_KG ret.wheelbase = 2.6 ret.steerRatio = 13.5 #Spec ret.steerRateCost = 0.4 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.KONA_HEV: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1330. + STD_CARGO_KG ret.wheelbase = 2.6 ret.steerRatio = 13.5 #Spec ret.steerRateCost = 0.4 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.KONA_EV: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1330. + STD_CARGO_KG ret.wheelbase = 2.6 ret.steerRatio = 13.5 #Spec ret.steerRateCost = 0.4 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.NIRO_HEV: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1425. + STD_CARGO_KG ret.wheelbase = 2.7 ret.steerRatio = 13.73 #Spec 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.NIRO_EV: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1425. + STD_CARGO_KG ret.wheelbase = 2.7 ret.steerRatio = 13.73 #Spec 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.IONIQ_HEV: ret.lateralTuning.pid.kf = 0.00006 ret.mass = 1275. + STD_CARGO_KG ret.wheelbase = 2.7 ret.steerRatio = 13.73 #Spec 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.IONIQ_EV: ret.lateralTuning.pid.kf = 0.00005 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.25 #Spec ret.steerRateCost = 0.4 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.NEXO: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1885. + STD_CARGO_KG ret.wheelbase = 2.79 ret.steerRatio = 12.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.MOHAVE: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 2250. + STD_CARGO_KG ret.wheelbase = 2.895 ret.steerRatio = 14.1 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.I30: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1380. + STD_CARGO_KG ret.wheelbase = 2.65 ret.steerRatio = 13.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.AVANTE: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1275. + STD_CARGO_KG ret.wheelbase = 2.7 ret.steerRatio = 13.5 # 14 is Stock | Settled Params Learner values are steerRatio: 15.401566348670535 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.SELTOS: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 1470. + STD_CARGO_KG ret.wheelbase = 2.63 ret.steerRatio = 13.0 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 = 1955. + STD_CARGO_KG ret.wheelbase = 2.90 ret.steerRatio = 13.0 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif int(params.get('LateralControlMethod')) == 1: if candidate == CAR.SANTAFE: ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 3.0 ret.lateralTuning.indi.outerLoopGain = 2.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.5 ret.mass = 1830. + STD_CARGO_KG ret.wheelbase = 2.765 ret.steerRatio = 13.8 # 13.8 is spec end-to-end elif candidate == CAR.SORENTO: ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 3.0 ret.lateralTuning.indi.outerLoopGain = 2.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.5 ret.mass = 1950. + STD_CARGO_KG ret.wheelbase = 2.78 ret.steerRatio = 14.4 * 1.15 elif candidate == CAR.GENESIS: ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 3.0 ret.lateralTuning.indi.outerLoopGain = 2.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.5 ret.mass = 2060. + STD_CARGO_KG ret.wheelbase = 3.01 ret.steerRatio = 16.5 elif candidate in [CAR.K5, CAR.SONATA]: ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 3.0 ret.lateralTuning.indi.outerLoopGain = 2.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.5 ret.mass = 1470. + STD_CARGO_KG ret.wheelbase = 2.80 ret.steerRatio = 12.75 ret.steerRateCost = 0.4 elif candidate == CAR.SONATA_TURBO: ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 3.0 ret.lateralTuning.indi.outerLoopGain = 2.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.5 ret.mass = 1565. + STD_CARGO_KG ret.wheelbase = 2.80 ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable elif candidate in [CAR.K5_HEV, CAR.SONATA_HEV]: ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 3.0 ret.lateralTuning.indi.outerLoopGain = 2.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.5 ret.mass = 1595. + STD_CARGO_KG ret.wheelbase = 2.80 ret.steerRatio = 12.75 ret.steerRateCost = 0.4 elif candidate in [CAR.GRANDEUR, CAR.K7]: ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 3.0 ret.lateralTuning.indi.outerLoopGain = 2.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.5 ret.mass = 1570. + STD_CARGO_KG ret.wheelbase = 2.885 ret.steerRatio = 12.5 ret.steerRateCost = 0.4 elif candidate in [CAR.GRANDEUR_HEV, CAR.K7_HEV]: ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 3.0 ret.lateralTuning.indi.outerLoopGain = 2.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.5 ret.mass = 1675. + STD_CARGO_KG ret.wheelbase = 2.885 ret.steerRatio = 12.5 ret.steerRateCost = 0.4 elif candidate == CAR.STINGER: ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 3.0 ret.lateralTuning.indi.outerLoopGain = 2.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.5 ret.mass = 1825. + STD_CARGO_KG ret.wheelbase = 2.78 ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable elif candidate == CAR.KONA: ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 3.0 ret.lateralTuning.indi.outerLoopGain = 2.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.5 ret.mass = 1330. + STD_CARGO_KG ret.wheelbase = 2.6 ret.steerRatio = 13.5 #Spec ret.steerRateCost = 0.4 elif candidate == CAR.KONA_HEV: ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 3.0 ret.lateralTuning.indi.outerLoopGain = 2.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.5 ret.mass = 1330. + STD_CARGO_KG ret.wheelbase = 2.6 ret.steerRatio = 13.5 #Spec ret.steerRateCost = 0.4 elif candidate == CAR.KONA_EV: ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 3.0 ret.lateralTuning.indi.outerLoopGain = 2.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.5 ret.mass = 1330. + STD_CARGO_KG ret.wheelbase = 2.6 ret.steerRatio = 13.5 #Spec ret.steerRateCost = 0.4 elif candidate == CAR.NIRO_HEV: ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 3.0 ret.lateralTuning.indi.outerLoopGain = 2.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.5 ret.mass = 1425. + STD_CARGO_KG ret.wheelbase = 2.7 ret.steerRatio = 13.73 #Spec elif candidate == CAR.NIRO_EV: ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 3.0 ret.lateralTuning.indi.outerLoopGain = 2.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.5 ret.mass = 1425. + STD_CARGO_KG ret.wheelbase = 2.7 ret.steerRatio = 13.73 #Spec elif candidate == CAR.IONIQ_HEV: ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 3.0 ret.lateralTuning.indi.outerLoopGain = 2.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.5 ret.mass = 1275. + STD_CARGO_KG ret.wheelbase = 2.7 ret.steerRatio = 13.73 #Spec elif candidate == CAR.IONIQ_EV: ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 3.0 ret.lateralTuning.indi.outerLoopGain = 2.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.5 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.25 #Spec ret.steerRateCost = 0.4 elif candidate == CAR.NEXO: ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 3.0 ret.lateralTuning.indi.outerLoopGain = 2.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.5 ret.mass = 1885. + STD_CARGO_KG ret.wheelbase = 2.79 ret.steerRatio = 12.5 elif candidate == CAR.MOHAVE: ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 3.0 ret.lateralTuning.indi.outerLoopGain = 2.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.5 ret.mass = 2250. + STD_CARGO_KG ret.wheelbase = 2.895 ret.steerRatio = 14.1 elif candidate == CAR.I30: ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 3.0 ret.lateralTuning.indi.outerLoopGain = 2.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.5 ret.mass = 1380. + STD_CARGO_KG ret.wheelbase = 2.65 ret.steerRatio = 13.5 elif candidate == CAR.AVANTE: ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 3.0 ret.lateralTuning.indi.outerLoopGain = 2.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.5 ret.mass = 1275. + STD_CARGO_KG ret.wheelbase = 2.7 ret.steerRatio = 13.5 # 14 is Stock | Settled Params Learner values are steerRatio: 15.401566348670535 elif candidate == CAR.SELTOS: ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 3.0 ret.lateralTuning.indi.outerLoopGain = 2.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.5 ret.mass = 1470. + STD_CARGO_KG ret.wheelbase = 2.63 ret.steerRatio = 13.0 elif candidate == CAR.PALISADE: ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 3.0 ret.lateralTuning.indi.outerLoopGain = 2.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.5 ret.mass = 1955. + STD_CARGO_KG ret.wheelbase = 2.90 ret.steerRatio = 13.0 elif int(params.get('LateralControlMethod')) == 2: if candidate == CAR.SANTAFE: ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1750.0 ret.lateralTuning.lqr.ki = 0.03 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-100., 450.] ret.lateralTuning.lqr.l = [0.22, 0.318] ret.lateralTuning.lqr.dcGain = 0.003 ret.mass = 1830. + STD_CARGO_KG ret.wheelbase = 2.765 ret.steerRatio = 13.8 # 13.8 is spec end-to-end elif candidate == CAR.SORENTO: ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1750.0 ret.lateralTuning.lqr.ki = 0.03 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-100., 450.] ret.lateralTuning.lqr.l = [0.22, 0.318] ret.lateralTuning.lqr.dcGain = 0.003 ret.mass = 1950. + STD_CARGO_KG ret.wheelbase = 2.78 ret.steerRatio = 14.4 * 1.15 elif candidate == CAR.GENESIS: ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1750.0 ret.lateralTuning.lqr.ki = 0.03 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-100., 450.] ret.lateralTuning.lqr.l = [0.22, 0.318] ret.lateralTuning.lqr.dcGain = 0.003 ret.mass = 2060. + STD_CARGO_KG ret.wheelbase = 3.01 ret.steerRatio = 16.5 elif candidate in [CAR.K5, CAR.SONATA]: ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1750.0 ret.lateralTuning.lqr.ki = 0.03 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-100., 450.] ret.lateralTuning.lqr.l = [0.22, 0.318] ret.lateralTuning.lqr.dcGain = 0.003 ret.mass = 1470. + STD_CARGO_KG ret.wheelbase = 2.80 ret.steerRatio = 12.75 ret.steerRateCost = 0.4 elif candidate == CAR.SONATA_TURBO: ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1750.0 ret.lateralTuning.lqr.ki = 0.03 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-100., 450.] ret.lateralTuning.lqr.l = [0.22, 0.318] ret.lateralTuning.lqr.dcGain = 0.003 ret.mass = 1565. + STD_CARGO_KG ret.wheelbase = 2.80 ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable elif candidate in [CAR.K5_HEV, CAR.SONATA_HEV]: ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1750.0 ret.lateralTuning.lqr.ki = 0.03 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-100., 450.] ret.lateralTuning.lqr.l = [0.22, 0.318] ret.lateralTuning.lqr.dcGain = 0.003 ret.mass = 1595. + STD_CARGO_KG ret.wheelbase = 2.80 ret.steerRatio = 12.75 ret.steerRateCost = 0.4 elif candidate in [CAR.GRANDEUR, CAR.K7]: ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1750.0 ret.lateralTuning.lqr.ki = 0.03 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-100., 450.] ret.lateralTuning.lqr.l = [0.22, 0.318] ret.lateralTuning.lqr.dcGain = 0.003 ret.mass = 1570. + STD_CARGO_KG ret.wheelbase = 2.885 ret.steerRatio = 12.5 ret.steerRateCost = 0.4 elif candidate in [CAR.GRANDEUR_HEV, CAR.K7_HEV]: ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1750.0 ret.lateralTuning.lqr.ki = 0.03 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-100., 450.] ret.lateralTuning.lqr.l = [0.22, 0.318] ret.lateralTuning.lqr.dcGain = 0.003 ret.mass = 1675. + STD_CARGO_KG ret.wheelbase = 2.885 ret.steerRatio = 12.5 ret.steerRateCost = 0.4 elif candidate == CAR.STINGER: ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1750.0 ret.lateralTuning.lqr.ki = 0.03 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-100., 450.] ret.lateralTuning.lqr.l = [0.22, 0.318] ret.lateralTuning.lqr.dcGain = 0.003 ret.mass = 1825. + STD_CARGO_KG ret.wheelbase = 2.78 ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable elif candidate == CAR.KONA: ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1750.0 ret.lateralTuning.lqr.ki = 0.03 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-100., 450.] ret.lateralTuning.lqr.l = [0.22, 0.318] ret.lateralTuning.lqr.dcGain = 0.003 ret.mass = 1330. + STD_CARGO_KG ret.wheelbase = 2.6 ret.steerRatio = 13.5 #Spec ret.steerRateCost = 0.4 elif candidate == CAR.KONA_HEV: ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1750.0 ret.lateralTuning.lqr.ki = 0.03 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-100., 450.] ret.lateralTuning.lqr.l = [0.22, 0.318] ret.lateralTuning.lqr.dcGain = 0.003 ret.mass = 1330. + STD_CARGO_KG ret.wheelbase = 2.6 ret.steerRatio = 13.5 #Spec ret.steerRateCost = 0.4 elif candidate == CAR.KONA_EV: ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1750.0 ret.lateralTuning.lqr.ki = 0.03 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-100., 450.] ret.lateralTuning.lqr.l = [0.22, 0.318] ret.lateralTuning.lqr.dcGain = 0.003 ret.mass = 1330. + STD_CARGO_KG ret.wheelbase = 2.6 ret.steerRatio = 13.5 #Spec ret.steerRateCost = 0.4 elif candidate == CAR.NIRO_HEV: ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1750.0 ret.lateralTuning.lqr.ki = 0.03 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-100., 450.] ret.lateralTuning.lqr.l = [0.22, 0.318] ret.lateralTuning.lqr.dcGain = 0.003 ret.mass = 1425. + STD_CARGO_KG ret.wheelbase = 2.7 ret.steerRatio = 13.73 #Spec elif candidate == CAR.NIRO_EV: ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1750.0 ret.lateralTuning.lqr.ki = 0.03 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-100., 450.] ret.lateralTuning.lqr.l = [0.22, 0.318] ret.lateralTuning.lqr.dcGain = 0.003 ret.mass = 1425. + STD_CARGO_KG ret.wheelbase = 2.7 ret.steerRatio = 13.73 #Spec elif candidate == CAR.IONIQ_HEV: ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1750.0 ret.lateralTuning.lqr.ki = 0.03 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-100., 450.] ret.lateralTuning.lqr.l = [0.22, 0.318] ret.lateralTuning.lqr.dcGain = 0.003 ret.mass = 1275. + STD_CARGO_KG ret.wheelbase = 2.7 ret.steerRatio = 13.73 #Spec elif candidate == CAR.IONIQ_EV: ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1750.0 ret.lateralTuning.lqr.ki = 0.03 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-100., 450.] ret.lateralTuning.lqr.l = [0.22, 0.318] ret.lateralTuning.lqr.dcGain = 0.003 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.25 #Spec ret.steerRateCost = 0.4 elif candidate == CAR.NEXO: ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1750.0 ret.lateralTuning.lqr.ki = 0.03 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-100., 450.] ret.lateralTuning.lqr.l = [0.22, 0.318] ret.lateralTuning.lqr.dcGain = 0.003 ret.mass = 1885. + STD_CARGO_KG ret.wheelbase = 2.79 ret.steerRatio = 12.5 elif candidate == CAR.MOHAVE: ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1750.0 ret.lateralTuning.lqr.ki = 0.03 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-100., 450.] ret.lateralTuning.lqr.l = [0.22, 0.318] ret.lateralTuning.lqr.dcGain = 0.003 ret.mass = 2250. + STD_CARGO_KG ret.wheelbase = 2.895 ret.steerRatio = 14.1 elif candidate == CAR.I30: ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1750.0 ret.lateralTuning.lqr.ki = 0.03 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-100., 450.] ret.lateralTuning.lqr.l = [0.22, 0.318] ret.lateralTuning.lqr.dcGain = 0.003 ret.mass = 1380. + STD_CARGO_KG ret.wheelbase = 2.65 ret.steerRatio = 13.5 elif candidate == CAR.AVANTE: ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1750.0 ret.lateralTuning.lqr.ki = 0.03 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-100., 450.] ret.lateralTuning.lqr.l = [0.22, 0.318] ret.lateralTuning.lqr.dcGain = 0.003 ret.mass = 1275. + STD_CARGO_KG ret.wheelbase = 2.7 ret.steerRatio = 13.5 # 14 is Stock | Settled Params Learner values are steerRatio: 15.401566348670535 elif candidate == CAR.SELTOS: ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1750.0 ret.lateralTuning.lqr.ki = 0.03 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-100., 450.] ret.lateralTuning.lqr.l = [0.22, 0.318] ret.lateralTuning.lqr.dcGain = 0.003 ret.mass = 1470. + STD_CARGO_KG ret.wheelbase = 2.63 ret.steerRatio = 13.0 elif candidate == CAR.PALISADE: ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1750.0 ret.lateralTuning.lqr.ki = 0.03 ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] ret.lateralTuning.lqr.c = [1., 0.] ret.lateralTuning.lqr.k = [-100., 450.] ret.lateralTuning.lqr.l = [0.22, 0.318] ret.lateralTuning.lqr.dcGain = 0.003 ret.mass = 1955. + STD_CARGO_KG ret.wheelbase = 2.90 ret.steerRatio = 13.0 # these cars require a special panda safety mode due to missing counters and checksums in the messages if candidate in [ CAR.GENESIS, CAR.IONIQ_EV, CAR.IONIQ_HEV, CAR.KONA_EV ]: ret.safetyModel = car.CarParams.SafetyModel.hyundaiLegacy 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.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay # ignore CAN2 address if L-CAN on the same BUS ret.mdpsBus = 1 if 593 in fingerprint[1] and 1296 not in fingerprint[ 1] else 0 # MDPS12 ret.sasBus = 1 if 688 in fingerprint[1] and 1296 not in fingerprint[ 1] else 0 # SAS11 ret.sccBus = 0 if 1056 in fingerprint[ 0] else 1 if 1056 in fingerprint[1] and 1296 not in fingerprint[ 1] else 2 if 1056 in fingerprint[2] else -1 # SCC11 return ret
def test_persist_params_put_and_get(self): p = Params(persistent_params=True) p.put("DongleId", "cb38263377b873ee") assert p.get("DongleId") == b"cb38263377b873ee"
def fingerprint(logcan, sendcan, has_relay): params = Params() car_selected = params.get('dp_car_selected', encoding='utf8') car_detected = params.get('dp_car_detected', encoding='utf8') cached_params = params.get("CarParamsCache") if cached_params is None and car_selected == "" and car_detected != "": params.put('dp_car_selected', car_detected) params.put('dp_car_detected', "") fixed_fingerprint = os.environ.get('FINGERPRINT', "") if fixed_fingerprint == "" and cached_params is None and car_selected != "": fixed_fingerprint = car_selected 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 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, 1] } # attempt fingerprint on both bus 0 and 1 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 and 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) put_nonblocking('dp_car_detected', car_fingerprint) return car_fingerprint, finger, vin, car_fw, source
def thermald_thread(): # prevent LEECO from undervoltage BATT_PERC_OFF = 10 if LEON else 3 health_timeout = int(1000 * 2.5 * DT_TRML) # 2.5x the expected health frequency # now loop thermal_sock = messaging.pub_sock('thermal') health_sock = messaging.sub_sock('health', timeout=health_timeout) location_sock = messaging.sub_sock('gpsLocation') ignition = False fan_speed = 0 count = 0 off_ts = None started_ts = None started_seen = False thermal_status = ThermalStatus.green thermal_status_prev = ThermalStatus.green usb_power = True usb_power_prev = True 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 fw_version_match_prev = True current_connectivity_alert = None time_valid_prev = True should_start_prev = False handle_fan = None is_uno = False has_relay = False params = Params() pm = PowerMonitoring() no_panda_cnt = 0 while 1: health = messaging.recv_sock(health_sock, wait=True) location = messaging.recv_sock(location_sock) location = location.gpsLocation if location else None msg = read_thermal() 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 ignition: cloudlog.error("Lost panda connection while onroad") ignition = False else: no_panda_cnt = 0 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 has_relay = health.health.hwType in [ log.HealthData.HwType.blackPanda, log.HealthData.HwType.uno ] if is_uno or not ANDROID: 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 = get_network_type() network_strength = 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 = get_battery_capacity() msg.thermal.batteryStatus = get_battery_status() msg.thermal.batteryCurrent = get_battery_current() msg.thermal.batteryVoltage = get_battery_voltage() msg.thermal.usbOnline = get_usb_present() # Fake battery levels on uno for frame if is_uno: msg.thermal.batteryPercent = 100 msg.thermal.batteryStatus = "Charging" current_filter.update(msg.thermal.batteryCurrent / 1e6) # TODO: add car battery voltage check max_cpu_temp = cpu_temp_filter.update( max(msg.thermal.cpu0, msg.thermal.cpu1, msg.thermal.cpu2, msg.thermal.cpu3) / 10.0) max_comp_temp = max(max_cpu_temp, msg.thermal.mem / 10., msg.thermal.gpu / 10.) bat_temp = msg.thermal.bat / 1000. if handle_fan is not None: fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed, 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 if max_cpu_temp > 107. or bat_temp >= 63. or (has_relay and (started_ts is None) and max_cpu_temp > 75.0): # onroad not allowed thermal_status = ThermalStatus.danger elif max_comp_temp > 100.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 > 97.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: # all good thermal_status = ThermalStatus.green # **** starting logic **** # Check for last update time and display alerts if needed now = datetime.datetime.utcnow() # show invalid date/time alert time_valid = now.year >= 2019 if time_valid and not time_valid_prev: params.delete("Offroad_InvalidTime") if not time_valid and time_valid_prev: put_nonblocking("Offroad_InvalidTime", json.dumps(OFFROAD_ALERTS["Offroad_InvalidTime"])) time_valid_prev = 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) if dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1: if current_connectivity_alert != "expired": current_connectivity_alert = "expired" params.delete("Offroad_ConnectivityNeededPrompt") put_nonblocking( "Offroad_ConnectivityNeeded", json.dumps(OFFROAD_ALERTS["Offroad_ConnectivityNeeded"])) elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT: remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0)) if current_connectivity_alert != "prompt" + remaining_time: current_connectivity_alert = "prompt" + remaining_time alert_connectivity_prompt = copy.copy( OFFROAD_ALERTS["Offroad_ConnectivityNeededPrompt"]) alert_connectivity_prompt["text"] += remaining_time + " days." params.delete("Offroad_ConnectivityNeeded") put_nonblocking("Offroad_ConnectivityNeededPrompt", json.dumps(alert_connectivity_prompt)) elif current_connectivity_alert is not None: current_connectivity_alert = None params.delete("Offroad_ConnectivityNeeded") params.delete("Offroad_ConnectivityNeededPrompt") do_uninstall = params.get("DoUninstall") == b"1" accepted_terms = params.get("HasAcceptedTerms") == terms_version completed_training = params.get( "CompletedTrainingVersion") == training_version panda_signature = params.get("PandaFirmware") fw_version_match = (panda_signature is None) or ( panda_signature == FW_SIGNATURE ) # don't show alert is no panda is connected (None) should_start = ignition # with 2% left, we killall, otherwise the phone will take a long time to boot should_start = should_start and msg.thermal.freeSpace > 0.02 # confirm we have completed training and aren't uninstalling should_start = should_start and accepted_terms and completed_training and ( not do_uninstall) # check for firmware mismatch should_start = should_start and fw_version_match # check if system time is valid should_start = should_start and time_valid # don't start while taking snapshot if not should_start_prev: is_viewing_driver = params.get("IsDriverViewEnabled") == b"1" is_taking_snapshot = params.get("IsTakingSnapshot") == b"1" should_start = should_start and (not is_taking_snapshot) and ( not is_viewing_driver) if fw_version_match and not fw_version_match_prev: params.delete("Offroad_PandaFirmwareMismatch") if not fw_version_match and fw_version_match_prev: put_nonblocking( "Offroad_PandaFirmwareMismatch", json.dumps(OFFROAD_ALERTS["Offroad_PandaFirmwareMismatch"])) # 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 if thermal_status >= ThermalStatus.danger: should_start = False if thermal_status_prev < ThermalStatus.danger: put_nonblocking( "Offroad_TemperatureTooHigh", json.dumps(OFFROAD_ALERTS["Offroad_TemperatureTooHigh"])) else: if thermal_status_prev >= ThermalStatus.danger: params.delete("Offroad_TemperatureTooHigh") 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 os.system( 'echo performance > /sys/class/devfreq/soc:qcom,cpubw/governor' ) else: if should_start_prev or (count == 0): put_nonblocking("IsOffroad", "1") started_ts = None if off_ts is None: off_ts = sec_since_boot() os.system( 'echo powersave > /sys/class/devfreq/soc:qcom,cpubw/governor' ) # shutdown if the battery gets lower than 3%, it's discharging, we aren't running for # more than a minute but we were running if msg.thermal.batteryPercent < BATT_PERC_OFF and msg.thermal.batteryStatus == "Discharging" and \ started_seen and (sec_since_boot() - off_ts) > 60: os.system('LD_LIBRARY_PATH="" svc power shutdown') # Offroad power monitoring pm.calculate(health) msg.thermal.offroadPowerUsage = pm.get_power_used() 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 thermal_sock.send(msg.to_bytes()) if usb_power_prev and not usb_power: put_nonblocking( "Offroad_ChargeDisabled", json.dumps(OFFROAD_ALERTS["Offroad_ChargeDisabled"])) elif usb_power and not usb_power_prev: params.delete("Offroad_ChargeDisabled") thermal_status_prev = thermal_status usb_power_prev = usb_power fw_version_match_prev = fw_version_match should_start_prev = should_start # report to server once per minute if (count % int(60. / DT_TRML)) == 0: cloudlog.event("STATUS_PACKET", count=count, health=(health.to_dict() if health else None), location=(location.to_dict() if location else None), thermal=msg.to_dict()) count += 1
def controlsd_thread(gctx=None, rate=100, default_bias=0.): # start the loop set_realtime_priority(3) context = zmq.Context() params = Params() # pub live100 = messaging.pub_sock(context, service_list['live100'].port) carstate = messaging.pub_sock(context, service_list['carState'].port) carcontrol = messaging.pub_sock(context, service_list['carControl'].port) livempc = messaging.pub_sock(context, service_list['liveMpc'].port) passive = params.get("Passive") != "0" if not passive: sendcan = messaging.pub_sock(context, service_list['sendcan'].port) else: sendcan = None # sub poller = zmq.Poller() thermal = messaging.sub_sock(context, service_list['thermal'].port, conflate=True, poller=poller) health = messaging.sub_sock(context, service_list['health'].port, conflate=True, poller=poller) cal = messaging.sub_sock(context, service_list['liveCalibration'].port, conflate=True, poller=poller) logcan = messaging.sub_sock(context, service_list['can'].port) CC = car.CarControl.new_message() CI, CP = get_car(logcan, sendcan, 1.0 if passive else None) if CI is None: raise Exception("unsupported car") # if stock camera is connected, then force passive behavior if not CP.enableCamera: passive = True sendcan = None if passive: CP.safetyModel = car.CarParams.SafetyModels.honda fcw_enabled = params.get("IsFcwEnabled") == "1" PL = Planner(CP, fcw_enabled) LoC = LongControl(CP, CI.compute_gb) VM = VehicleModel(CP) LaC = LatControl(VM) AM = AlertManager() if not passive: AM.add("startup", False) # write CarParams params.put("CarParams", CP.to_bytes()) state = State.disabled soft_disable_timer = 0 v_cruise_kph = 255 overtemp = False free_space = False cal_status = Calibration.UNCALIBRATED rear_view_toggle = False rear_view_allowed = params.get("IsRearViewMirror") == "1" # 0.0 - 1.0 awareness_status = 1. v_cruise_kph_last = 0 rk = Ratekeeper(rate, print_delay_threshold=2./1000) # learned angle offset angle_offset = default_bias calibration_params = params.get("CalibrationParams") if calibration_params: try: calibration_params = json.loads(calibration_params) angle_offset = calibration_params["angle_offset2"] except (ValueError, KeyError): pass prof = Profiler(False) # off by default while 1: prof.checkpoint("Ratekeeper", ignore=True) # sample data and compute car events CS, events, cal_status, overtemp, free_space = data_sample(CI, CC, thermal, cal, health, poller, cal_status, overtemp, free_space) prof.checkpoint("Sample") # define plan plan, plan_ts = calc_plan(CS, CP, events, PL, LaC, LoC, v_cruise_kph, awareness_status) prof.checkpoint("Plan") if not passive: # update control state state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM) prof.checkpoint("State transition") # compute actuators actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle = state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, awareness_status, PL, LaC, LoC, VM, angle_offset, rear_view_allowed, rear_view_toggle, passive) prof.checkpoint("State Control") # publish data CC = data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, live100, livempc, AM, rear_view_allowed, rear_view_toggle, awareness_status, LaC, LoC, angle_offset, passive) prof.checkpoint("Sent") # *** run loop at fixed rate *** rk.keep_time() prof.display()
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.dRel = 0 self.yRel = 0 self.vRel = 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.dRel, self.yRel, self.vRel = SpdController.get_lead(sm) self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo) # Steering Torque if self.driver_steering_torque_above_timer: new_steer = actuators.steer * SteerLimitParams.STEER_MAX * ( self.driver_steering_torque_above_timer / 100) else: 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 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_log2 = '주행모드={:s} MDPS상태={:s} LKAS버튼={:s} 크루즈갭={:1.0f}'.format( self.steer_mode, self.mdps_status, self.lkas_switch, self.cruise_gap) trace1.printf2('{}'.format(str_log2)) if pcm_cancel_cmd and self.longcontrol: can_sends.append( create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed)) if CS.out.cruiseState.standstill: 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.dRel > 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
class PowerMonitoring: def __init__(self): self.params = Params() self.last_measurement_time = None # Used for integration delta self.last_save_time = 0 # Used for saving current value in a param self.power_used_uWh = 0 # Integrated power usage in uWh since going into offroad self.next_pulsed_measurement_time = None self.car_voltage_mV = 12e3 # Low-passed version of health voltage self.integration_lock = threading.Lock() car_battery_capacity_uWh = self.params.get("CarBatteryCapacity") if car_battery_capacity_uWh is None: car_battery_capacity_uWh = 0 # Reset capacity if it's low self.car_battery_capacity_uWh = max((CAR_BATTERY_CAPACITY_uWh / 10), int(car_battery_capacity_uWh)) # Calculation tick def calculate(self, health): try: now = sec_since_boot() # If health is None, we're probably not in a car, so we don't care if health is None or health.health.hwType == log.HealthData.HwType.unknown: with self.integration_lock: self.last_measurement_time = None self.next_pulsed_measurement_time = None self.power_used_uWh = 0 return # Low-pass battery voltage self.car_voltage_mV = ((health.health.voltage * CAR_VOLTAGE_LOW_PASS_K) + (self.car_voltage_mV * (1 - CAR_VOLTAGE_LOW_PASS_K))) # Cap the car battery power and save it in a param every 10-ish seconds self.car_battery_capacity_uWh = max(self.car_battery_capacity_uWh, 0) self.car_battery_capacity_uWh = min(self.car_battery_capacity_uWh, CAR_BATTERY_CAPACITY_uWh) if now - self.last_save_time >= 10: put_nonblocking("CarBatteryCapacity", str(int(self.car_battery_capacity_uWh))) self.last_save_time = now # First measurement, set integration time with self.integration_lock: if self.last_measurement_time is None: self.last_measurement_time = now return if (health.health.ignitionLine or health.health.ignitionCan): # If there is ignition, we integrate the charging rate of the car with self.integration_lock: self.power_used_uWh = 0 integration_time_h = (now - self.last_measurement_time) / 3600 if integration_time_h < 0: raise ValueError(f"Negative integration time: {integration_time_h}h") self.car_battery_capacity_uWh += (CAR_CHARGING_RATE_W * 1e6 * integration_time_h) self.last_measurement_time = now else: # No ignition, we integrate the offroad power used by the device is_uno = health.health.hwType == log.HealthData.HwType.uno # Get current power draw somehow current_power = 0 if TICI: with open("/sys/class/hwmon/hwmon1/power1_input") as f: current_power = int(f.read()) / 1e6 elif get_battery_status() == 'Discharging': # If the battery is discharging, we can use this measurement # On C2: this is low by about 10-15%, probably mostly due to UNO draw not being factored in current_power = ((get_battery_voltage() / 1000000) * (get_battery_current() / 1000000)) elif (self.next_pulsed_measurement_time is not None) and (self.next_pulsed_measurement_time <= now): # TODO: Figure out why this is off by a factor of 3/4??? FUDGE_FACTOR = 1.33 # Turn off charging for about 10 sec in a thread that does not get killed on SIGINT, and perform measurement here to avoid blocking thermal def perform_pulse_measurement(now): try: set_battery_charging(False) time.sleep(5) # Measure for a few sec to get a good average voltages = [] currents = [] for _ in range(6): voltages.append(get_battery_voltage()) currents.append(get_battery_current()) time.sleep(1) current_power = ((mean(voltages) / 1000000) * (mean(currents) / 1000000)) self._perform_integration(now, current_power * FUDGE_FACTOR) # Enable charging again set_battery_charging(True) except Exception: cloudlog.exception("Pulsed power measurement failed") # Start pulsed measurement and return threading.Thread(target=perform_pulse_measurement, args=(now,)).start() self.next_pulsed_measurement_time = None return elif self.next_pulsed_measurement_time is None and not is_uno: # On a charging EON with black panda, or drawing more than 400mA out of a white/grey one # Only way to get the power draw is to turn off charging for a few sec and check what the discharging rate is # We shouldn't do this very often, so make sure it has been some long-ish random time interval self.next_pulsed_measurement_time = now + random.randint(120, 180) return else: # Do nothing return # Do the integration self._perform_integration(now, current_power) except Exception: cloudlog.exception("Power monitoring calculation failed") def _perform_integration(self, t, current_power): with self.integration_lock: try: if self.last_measurement_time: integration_time_h = (t - self.last_measurement_time) / 3600 power_used = (current_power * 1000000) * integration_time_h if power_used < 0: raise ValueError(f"Negative power used! Integration time: {integration_time_h} h Current Power: {power_used} uWh") self.power_used_uWh += power_used self.car_battery_capacity_uWh -= power_used self.last_measurement_time = t except Exception: cloudlog.exception("Integration failed") # Get the power usage def get_power_used(self): return int(self.power_used_uWh) def get_car_battery_capacity(self): return int(self.car_battery_capacity_uWh) # See if we need to disable charging def should_disable_charging(self, health, offroad_timestamp): if health is None or offroad_timestamp is None: return False now = sec_since_boot() disable_charging = False disable_charging |= (now - offroad_timestamp) > MAX_TIME_OFFROAD_S disable_charging |= (self.car_voltage_mV < (VBATT_PAUSE_CHARGING * 1e3)) disable_charging |= (self.car_battery_capacity_uWh <= 0) disable_charging &= (not health.health.ignitionLine and not health.health.ignitionCan) disable_charging &= (self.params.get("DisablePowerDown") != b"1") return disable_charging # See if we need to shutdown def should_shutdown(self, health, offroad_timestamp, started_seen, LEON): if health is None or offroad_timestamp is None: return False now = sec_since_boot() panda_charging = (health.health.usbPowerMode != log.HealthData.UsbPowerMode.client) BATT_PERC_OFF = 10 if LEON else 3 should_shutdown = False # Wait until we have shut down charging before powering down should_shutdown |= (not panda_charging and self.should_disable_charging(health, offroad_timestamp)) should_shutdown |= ((get_battery_capacity() < BATT_PERC_OFF) and (not get_battery_charging()) and ((now - offroad_timestamp) > 60)) should_shutdown &= started_seen return should_shutdown
def manager_thread(): shutdownd = Process(name="shutdownd", target=launcher, args=("selfdrive.shutdownd", )) shutdownd.start() # 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] 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 regen_segment(lr, frs=None, outdir=FAKEDATA): lr = list(lr) if frs is None: frs = dict() setup_env() params = Params() os.environ["LOG_ROOT"] = outdir os.environ['SKIP_FW_QUERY'] = "" os.environ['FINGERPRINT'] = "" # TODO: remove after getting new route for mazda migration = { "Mazda CX-9 2021": "MAZDA CX-9 2021", } 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 elif msg.which() == 'liveCalibration': params.put("CalibrationParams", msg.as_builder().to_bytes()) vs, cam_procs = replay_cameras(lr, frs) fake_daemons = { 'sensord': [ multiprocessing.Process(target=replay_sensor_events, args=('sensorEvents', lr)), ], 'pandad': [ multiprocessing.Process(target=replay_service, args=('can', lr)), multiprocessing.Process(target=replay_service, args=('ubloxRaw', lr)), multiprocessing.Process(target=replay_panda_states, args=('pandaStates', lr)), ], 'managerState': [ multiprocessing.Process(target=replay_manager_state, args=('managerState', lr)), ], 'thermald': [ multiprocessing.Process(target=replay_device_state, args=('deviceState', lr)), ], 'camerad': [ *cam_procs, ], } try: # start procs up ignore = list(fake_daemons.keys()) + ['ui', 'manage_athenad', 'uploader'] ensure_running(managed_processes.values(), started=True, not_run=ignore) for procs in fake_daemons.values(): for p in procs: p.start() for _ in tqdm(range(60)): # ensure all procs are running for d, procs in fake_daemons.items(): for p in procs: if not p.is_alive(): raise Exception(f"{d}'s {p.name} died") time.sleep(1) finally: # kill everything for p in managed_processes.values(): p.stop() for procs in fake_daemons.values(): for p in procs: p.terminate() del vs segment = params.get("CurrentRoute", encoding='utf-8') + "--0" seg_path = os.path.join(outdir, segment) # check to make sure openpilot is engaged in the route if not check_enabled(LogReader(os.path.join(seg_path, "rlog.bz2"))): raise Exception(f"Route never enabled: {segment}") return seg_path
s for s in tested_socks + extra_socks if s not in recvd_socks ] print("Check if everything is running") running = manager.get_running() for p in tested_procs: if not running[p].is_alive: failures.append(p) manager.kill_managed_process(p) os.killpg(os.getpgid(unlogger.pid), signal.SIGTERM) sockets_ok = len(failures) == 0 params_ok = True try: car_params = car.CarParams.from_bytes(params.get("CarParams")) for k, v in checks.items(): if not v == getattr(car_params, k): params_ok = False failures.append(k) except Exception: params_ok = False if sockets_ok and params_ok: print("Success") results[route] = True, failures else: print("Failure") results[route] = False, failures break
def controlsd_thread(gctx=None, rate=100, default_bias=0.): gc.disable() # start the loop set_realtime_priority(3) context = zmq.Context() params = Params() # Pub Sockets live100 = messaging.pub_sock(context, service_list['live100'].port) carstate = messaging.pub_sock(context, service_list['carState'].port) carcontrol = messaging.pub_sock(context, service_list['carControl'].port) livempc = messaging.pub_sock(context, service_list['liveMpc'].port) is_metric = params.get("IsMetric") == "1" passive = params.get("Passive") != "0" # No sendcan if passive if not passive: sendcan = messaging.pub_sock(context, service_list['sendcan'].port) else: sendcan = None # Sub sockets poller = zmq.Poller() thermal = messaging.sub_sock(context, service_list['thermal'].port, conflate=True, poller=poller) health = messaging.sub_sock(context, service_list['health'].port, conflate=True, poller=poller) cal = messaging.sub_sock(context, service_list['liveCalibration'].port, conflate=True, poller=poller) driver_monitor = messaging.sub_sock(context, service_list['driverMonitoring'].port, conflate=True, poller=poller) gps_location = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, conflate=True, poller=poller) logcan = messaging.sub_sock(context, service_list['can'].port) CC = car.CarControl.new_message() CI, CP = get_car(logcan, sendcan, 1.0 if passive else None) if CI is None: raise Exception("unsupported car") # if stock camera is connected, then force passive behavior if not CP.enableCamera: passive = True sendcan = None if passive: CP.safetyModel = car.CarParams.SafetyModels.noOutput # Get FCW toggle from settings fcw_enabled = params.get("IsFcwEnabled") == "1" geofence = None PL = Planner(CP, fcw_enabled) LoC = LongControl(CP, CI.compute_gb) VM = VehicleModel(CP) LaC = LatControl(CP) AM = AlertManager() driver_status = DriverStatus() if not passive: AM.add("startup", False) # Write CarParams for radard and boardd safety mode params.put("CarParams", CP.to_bytes()) 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 rk = Ratekeeper(rate, print_delay_threshold=2. / 1000) # Read angle offset from previous drive, fallback to default angle_offset = default_bias calibration_params = params.get("CalibrationParams") if calibration_params: try: calibration_params = json.loads(calibration_params) angle_offset = calibration_params["angle_offset2"] except (ValueError, KeyError): pass prof = Profiler(False) # off by default while True: 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, thermal, cal, health, driver_monitor, gps_location, poller, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, geofence, state, mismatch_counter, params) prof.checkpoint("Sample") # Define longitudinal plan (MPC) plan, plan_ts = calc_plan(CS, CP, events, PL, LaC, LoC, v_cruise_kph, driver_status, geofence) prof.checkpoint("Plan") if not passive: # update control state state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \ state_transition(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, angle_offset = state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, driver_status, PL, LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc) prof.checkpoint("State Control") # Publish data CC = data_send(PL.perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, live100, livempc, AM, driver_status, LaC, LoC, angle_offset, passive) prof.checkpoint("Sent") rk.keep_time() # Run at 100Hz prof.display()