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()
