def __init__(self, dbc_name, CP, VM):
self.car_fingerprint = CP.carFingerprint
self.packer = CANPacker(dbc_name)
self.apply_steer_last = 0
self.accel = 0
self.lkas11_cnt = 0
self.scc12_cnt = -1
self.resume_cnt = 0
self.last_lead_distance = 0
self.resume_wait_timer = 0
self.turning_signal_timer = 0
self.longcontrol = CP.openpilotLongitudinalControl
self.scc_live = not CP.radarOffCan
self.turning_indicator_alert = False
param = Params()
self.mad_mode_enabled = param.get_bool('MadModeEnabled')
self.ldws_opt = param.get_bool('IsLdwsCar')
self.stock_navi_decel_enabled = param.get_bool('StockNaviDecelEnabled')
self.keep_steering_turn_signals = param.get_bool(
'KeepSteeringTurnSignals')
self.haptic_feedback_speed_camera = param.get_bool(
'HapticFeedbackWhenSpeedCamera')
self.scc_smoother = SccSmoother()
self.last_blinker_frame = 0
self.prev_active_cam = False
self.active_cam_timer = 0
def plannerd_thread(sm=None, pm=None):
config_realtime_process(5 if TICI else 2, Priority.CTRL_LOW)
cloudlog.info("plannerd is waiting for CarParams")
params = Params()
CP = car.CarParams.from_bytes(params.get("CarParams", block=True))
cloudlog.info("plannerd got CarParams: %s", CP.carName)
use_lanelines = not params.get_bool('EndToEndToggle')
wide_camera = params.get_bool('EnableWideCamera') if TICI else False
cloudlog.event("e2e mode", on=use_lanelines)
longitudinal_planner = Planner(CP)
lateral_planner = LateralPlanner(CP,
use_lanelines=use_lanelines,
wide_camera=wide_camera)
if sm is None:
sm = messaging.SubMaster(
['carState', 'controlsState', 'radarState', 'modelV2'],
poll=['radarState', 'modelV2'],
ignore_avg_freq=['radarState'])
if pm is None:
pm = messaging.PubMaster(['longitudinalPlan', 'lateralPlan'])
while True:
sm.update()
if sm.updated['modelV2']:
lateral_planner.update(sm, CP)
lateral_planner.publish(sm, pm)
longitudinal_planner.update(sm, CP)
longitudinal_planner.publish(sm, pm)
def manager_thread():
cloudlog.info("manager start")
cloudlog.info({"environ": os.environ})
# save boot log
#subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd"))
ignore = []
if os.getenv("NOBOARD") is not None:
ignore.append("pandad")
if os.getenv("BLOCK") is not None:
ignore += os.getenv("BLOCK").split(",")
if EON:
pm_apply_packages('enable')
ensure_running(managed_processes.values(), started=False, not_run=ignore)
started_prev = False
params = Params()
sm = messaging.SubMaster(['deviceState'])
pm = messaging.PubMaster(['managerState'])
while True:
sm.update()
not_run = ignore[:]
if sm['deviceState'].freeSpacePercent < 5:
not_run.append("loggerd")
started = sm['deviceState'].started
driverview = params.get_bool("IsDriverViewEnabled")
ensure_running(managed_processes.values(), started, driverview,
not_run)
# trigger an update after going offroad
if started_prev and not started and 'updated' in managed_processes:
os.sync()
managed_processes['updated'].signal(signal.SIGHUP)
started_prev = started
running_list = [
"%s%s\u001b[0m" %
("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name)
for p in managed_processes.values() if p.proc
]
cloudlog.debug(' '.join(running_list))
# send managerState
msg = messaging.new_message('managerState')
msg.managerState.processes = [
p.get_process_state_msg() for p in managed_processes.values()
]
pm.send('managerState', msg)
# TODO: let UI handle this
# Exit main loop when uninstall is needed
if params.get_bool("DoUninstall"):
break
def __init__(self, dbc_name, CP, VM):
self.car_fingerprint = CP.carFingerprint
self.packer = CANPacker(dbc_name)
self.apply_steer_last = 0
self.steer_rate_limited = False
self.lkas11_cnt = 0
self.scc12_cnt = -1
self.resume_cnt = 0
self.last_lead_distance = 0
self.resume_wait_timer = 0
self.turning_signal_timer = 0
self.longcontrol = CP.openpilotLongitudinalControl
self.scc_live = not CP.radarOffCan
self.turning_indicator_alert = False
param = Params()
self.mad_mode_enabled = param.get_bool('MadModeEnabled')
self.ldws_opt = param.get_bool('IsLdwsCar')
self.stock_navi_decel_enabled = param.get_bool('StockNaviDecelEnabled')
self.keep_steering_turn_signals = param.get_bool(
'KeepSteeringTurnSignals')
self.warning_over_speed_limit = param.get_bool('WarningOverSpeedLimit')
# gas_factor, brake_factor
# Adjust it in the range of 0.7 to 1.3
self.scc_smoother = SccSmoother()
self.last_blinker_frame = 0
def main():
prepare_only = os.getenv("PREPAREONLY") is not None
manager_init()
# Start UI early so prepare can happen in the background
if not prepare_only:
managed_processes['ui'].start()
manager_prepare()
if prepare_only:
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:
manager_cleanup()
params = Params()
if params.get_bool("DoUninstall"):
cloudlog.warning("uninstalling")
HARDWARE.uninstall()
elif params.get_bool("DoReboot"):
cloudlog.warning("reboot")
HARDWARE.reboot()
elif params.get_bool("DoShutdown"):
cloudlog.warning("shutdown")
HARDWARE.shutdown()
def manager_thread():
cloudlog.info("manager start")
cloudlog.info({"environ": os.environ})
params = Params()
ignore = []
if params.get("DongleId", encoding='utf8') == UNREGISTERED_DONGLE_ID:
ignore += ["manage_athenad", "uploader"]
if os.getenv("NOBOARD") is not None:
ignore.append("pandad")
if os.getenv("BLOCK") is not None:
ignore += os.getenv("BLOCK").split(",")
ensure_running(managed_processes.values(), started=False, not_run=ignore)
started_prev = False
sm = messaging.SubMaster(['deviceState'])
pm = messaging.PubMaster(['managerState'])
while True:
sm.update()
not_run = ignore[:]
if sm['deviceState'].freeSpacePercent < 5:
not_run.append("loggerd")
started = sm['deviceState'].started
driverview = params.get_bool("IsDriverViewEnabled")
ensure_running(managed_processes.values(), started, driverview, not_run)
# trigger an update after going offroad
if started_prev and not started and 'updated' in managed_processes:
os.sync()
managed_processes['updated'].signal(signal.SIGHUP)
started_prev = started
running = ' '.join(["%s%s\u001b[0m" % ("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name)
for p in managed_processes.values() if p.proc])
print(running)
cloudlog.debug(running)
# send managerState
msg = messaging.new_message('managerState')
msg.managerState.processes = [p.get_process_state_msg() for p in managed_processes.values()]
pm.send('managerState', msg)
# Exit main loop when uninstall/shutdown/reboot is needed
shutdown = False
for param in ("DoUninstall", "DoShutdown", "DoReboot"):
if params.get_bool(param):
cloudlog.warning(f"Shutting down manager - {param} set")
shutdown = True
if shutdown:
break
def uploader_fn(exit_event):
params = Params()
dongle_id = params.get("DongleId", encoding='utf8')
if dongle_id is None:
cloudlog.info("uploader missing dongle_id")
raise Exception("uploader can't start without dongle id")
if TICI and not Path("/data/media").is_mount():
cloudlog.warning("NVME not mounted")
sm = messaging.SubMaster(['deviceState'])
pm = messaging.PubMaster(['uploaderState'])
uploader = Uploader(dongle_id, ROOT)
backoff = 0.1
while not exit_event.is_set():
sm.update(0)
offroad = params.get_bool("IsOffroad")
network_type = sm[
'deviceState'].networkType if not force_wifi else NetworkType.wifi
if network_type == NetworkType.none:
if allow_sleep:
time.sleep(60 if offroad else 5)
continue
good_internet = network_type in [
NetworkType.wifi, NetworkType.ethernet
]
allow_raw_upload = params.get_bool("UploadRaw")
d = uploader.next_file_to_upload(
with_raw=allow_raw_upload and good_internet and offroad)
if d is None: # Nothing to upload
if allow_sleep:
time.sleep(60 if offroad else 5)
continue
key, fn = d
cloudlog.debug("upload %r over %s", d, network_type)
success = uploader.upload(key, fn)
if success:
backoff = 0.1
elif allow_sleep:
cloudlog.info("upload backoff %r", backoff)
time.sleep(backoff + random.uniform(0, backoff))
backoff = min(backoff * 2, 120)
pm.send("uploaderState", uploader.get_msg())
cloudlog.info("upload done, success=%r", success)
def manager_thread() -> None:
Process(name="road_speed_limiter", target=launcher, args=("selfdrive.road_speed_limiter", "road_speed_limiter")).start()
cloudlog.bind(daemon="manager")
cloudlog.info("manager start")
cloudlog.info({"environ": os.environ})
params = Params()
ignore: List[str] = []
if params.get("DongleId", encoding='utf8') in (None, UNREGISTERED_DONGLE_ID):
ignore += ["manage_athenad", "uploader"]
if os.getenv("NOBOARD") is not None:
ignore.append("pandad")
ignore += [x for x in os.getenv("BLOCK", "").split(",") if len(x) > 0]
ensure_running(managed_processes.values(), started=False, not_run=ignore)
sm = messaging.SubMaster(['deviceState', 'carParams'], poll=['deviceState'])
pm = messaging.PubMaster(['managerState'])
while True:
sm.update()
started = sm['deviceState'].started
driverview = params.get_bool("IsDriverViewEnabled")
ensure_running(managed_processes.values(), started=started, driverview=driverview, notcar=sm['carParams'].notCar, not_run=ignore)
running = ' '.join("%s%s\u001b[0m" % ("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name)
for p in managed_processes.values() if p.proc)
print(running)
cloudlog.debug(running)
# send managerState
msg = messaging.new_message('managerState')
msg.managerState.processes = [p.get_process_state_msg() for p in managed_processes.values()]
pm.send('managerState', msg)
# Exit main loop when uninstall/shutdown/reboot is needed
shutdown = False
for param in ("DoUninstall", "DoShutdown", "DoReboot"):
if params.get_bool(param):
shutdown = True
params.put("LastManagerExitReason", param)
cloudlog.warning(f"Shutting down manager - {param} set")
if shutdown:
break
def snapshot():
params = Params()
front_camera_allowed = params.get_bool("RecordFront")
if (not params.get_bool("IsOffroad")) or params.get_bool("IsTakingSnapshot"):
print("Already taking snapshot")
return None, None
params.put_bool("IsTakingSnapshot", True)
set_offroad_alert("Offroad_IsTakingSnapshot", True)
time.sleep(2.0) # Give thermald time to read the param, or if just started give camerad time to start
# Check if camerad is already started
try:
subprocess.check_call(["pgrep", "camerad"])
print("Camerad already running")
params.put_bool("IsTakingSnapshot", False)
params.delete("Offroad_IsTakingSnapshot")
return None, None
except subprocess.CalledProcessError:
pass
env = os.environ.copy()
env["SEND_ROAD"] = "1"
env["SEND_WIDE_ROAD"] = "1"
if front_camera_allowed:
env["SEND_DRIVER"] = "1"
proc = subprocess.Popen(os.path.join(BASEDIR, "selfdrive/camerad/camerad"),
cwd=os.path.join(BASEDIR, "selfdrive/camerad"), env=env)
time.sleep(3.0)
frame = "wideRoadCameraState" if TICI else "roadCameraState"
front_frame = "driverCameraState" if front_camera_allowed else None
rear, front = get_snapshots(frame, front_frame)
proc.send_signal(signal.SIGINT)
proc.communicate()
params.put_bool("IsTakingSnapshot", False)
set_offroad_alert("Offroad_IsTakingSnapshot", False)
if not front_camera_allowed:
front = None
return rear, front
def __init__(self, param_put=False):
self.param_put = param_put
# Read saved calibration
params = Params()
calibration_params = params.get("CalibrationParams")
self.wide_camera = TICI and params.get_bool('EnableWideCamera')
rpy_init = RPY_INIT
valid_blocks = 0
cached_params = params.get("CarParamsCache")
if cached_params is not None:
CP = car.CarParams.from_bytes(params.get("CarParams", block=True))
cached_params = car.CarParams.from_bytes(cached_params)
if cached_params.carFingerprint != CP.carFingerprint:
calibration_params = None
if param_put and calibration_params:
try:
msg = log.Event.from_bytes(calibration_params)
rpy_init = list(msg.liveCalibration.rpyCalib)
valid_blocks = msg.liveCalibration.validBlocks
except (ValueError, capnp.lib.capnp.KjException):
# TODO: remove this after next release
calibration_params = json.loads(calibration_params)
rpy_init = calibration_params["calib_radians"]
valid_blocks = calibration_params['valid_blocks']
except Exception:
cloudlog.exception(
"CalibrationParams file found but error encountered")
self.reset(rpy_init, valid_blocks)
self.update_status()
def manager_init():
# update system time from panda
set_time(cloudlog)
params = Params()
params.manager_start()
default_params = [
("CompletedTrainingVersion", "0"),
("HasAcceptedTerms", "0"),
("LastUpdateTime",
datetime.datetime.utcnow().isoformat().encode('utf8')),
("OpenpilotEnabledToggle", "1"),
]
if params.get_bool("RecordFrontLock"):
params.put_bool("RecordFront", True)
# set unset params
for k, v in default_params:
if params.get(k) is None:
params.put(k, v)
# is this dashcam?
if os.getenv("PASSIVE") is not None:
params.put_bool("Passive", bool(int(os.getenv("PASSIVE"))))
if params.get("Passive") is None:
raise Exception("Passive must be set to continue")
os.umask(0) # Make sure we can create files with 777 permissions
# Create folders needed for msgq
try:
os.mkdir("/dev/shm")
except FileExistsError:
pass
except PermissionError:
print("WARNING: failed to make /dev/shm")
# set dongle id
reg_res = register(show_spinner=True)
if reg_res:
dongle_id = reg_res
else:
raise Exception("server registration failed")
os.environ['DONGLE_ID'] = dongle_id # Needed for swaglog and loggerd
if not dirty:
os.environ['CLEAN'] = '1'
cloudlog.bind_global(dongle_id=dongle_id,
version=version,
dirty=dirty,
device=HARDWARE.get_device_type())
crash.bind_user(id=dongle_id)
crash.bind_extra(version=version,
dirty=dirty,
device=HARDWARE.get_device_type())
def snapshot():
params = Params()
front_camera_allowed = params.get_bool("RecordFront")
if (not params.get_bool("IsOffroad")
) or params.get_bool("IsTakingSnapshot"):
print("Already taking snapshot")
return None, None
params.put_bool("IsTakingSnapshot", True)
set_offroad_alert("Offroad_IsTakingSnapshot", True)
time.sleep(
2.0
) # Give thermald time to read the param, or if just started give camerad time to start
# Check if camerad is already started
try:
subprocess.check_call(["pgrep", "camerad"])
print("Camerad already running")
params.put_bool("IsTakingSnapshot", False)
params.delete("Offroad_IsTakingSnapshot")
return None, None
except subprocess.CalledProcessError:
pass
os.environ["SEND_ROAD"] = "1"
os.environ["SEND_WIDE_ROAD"] = "1"
if front_camera_allowed:
os.environ["SEND_DRIVER"] = "1"
try:
managed_processes['camerad'].start()
frame = "wideRoadCameraState" if TICI else "roadCameraState"
front_frame = "driverCameraState" if front_camera_allowed else None
focus_perc_threshold = 0. if TICI else 10 / 12.
rear, front = get_snapshots(frame, front_frame, focus_perc_threshold)
finally:
managed_processes['camerad'].stop()
params.put_bool("IsTakingSnapshot", False)
set_offroad_alert("Offroad_IsTakingSnapshot", False)
if not front_camera_allowed:
front = None
return rear, front
def main() -> NoReturn:
params = Params()
tf = TimezoneFinder()
# Get allowed timezones
valid_timezones = subprocess.check_output(
'timedatectl list-timezones', shell=True,
encoding='utf8').strip().split('\n')
while True:
time.sleep(60)
is_onroad = not params.get_bool("IsOffroad")
if is_onroad:
continue
# Set based on param
timezone = params.get("Timezone", encoding='utf8')
if timezone is not None:
cloudlog.debug("Setting timezone based on param")
set_timezone(valid_timezones, timezone)
continue
location = params.get("LastGPSPosition", encoding='utf8')
# Find timezone based on IP geolocation if no gps location is available
if location is None:
cloudlog.debug("Setting timezone based on IP lookup")
try:
r = requests.get("https://ipapi.co/timezone", timeout=10)
if r.status_code == 200:
set_timezone(valid_timezones, r.text)
else:
cloudlog.error(
f"Unexpected status code from api {r.status_code}")
time.sleep(3600) # Don't make too many API requests
except requests.exceptions.RequestException:
cloudlog.exception("Error getting timezone based on IP")
continue
# Find timezone by reverse geocoding the last known gps location
else:
cloudlog.debug("Setting timezone based on GPS location")
try:
location = json.loads(location)
except Exception:
cloudlog.exception("Error parsing location")
continue
timezone = tf.timezone_at(lng=location['longitude'],
lat=location['latitude'])
if timezone is None:
cloudlog.error(
f"No timezone found based on location, {location}")
continue
set_timezone(valid_timezones, timezone)
def uploader_fn(exit_event):
try:
set_core_affinity([0, 1, 2, 3])
except Exception:
cloudlog.exception("failed to set core affinity")
clear_locks(ROOT)
params = Params()
dongle_id = params.get("DongleId", encoding='utf8')
if dongle_id is None:
cloudlog.info("uploader missing dongle_id")
raise Exception("uploader can't start without dongle id")
if TICI and not Path("/data/media").is_mount():
cloudlog.warning("NVME not mounted")
sm = messaging.SubMaster(['deviceState'])
pm = messaging.PubMaster(['uploaderState'])
uploader = Uploader(dongle_id, ROOT)
backoff = 0.1
while not exit_event.is_set():
sm.update(0)
offroad = params.get_bool("IsOffroad")
network_type = sm[
'deviceState'].networkType if not force_wifi else NetworkType.wifi
if network_type == NetworkType.none:
if allow_sleep:
time.sleep(60 if offroad else 5)
continue
d = uploader.next_file_to_upload()
if d is None: # Nothing to upload
if allow_sleep:
time.sleep(60 if offroad else 5)
continue
name, key, fn = d
# qlogs and bootlogs need to be compressed before uploading
if key.endswith(('qlog', 'rlog')) or (key.startswith('boot/')
and not key.endswith('.bz2')):
key += ".bz2"
success = uploader.upload(name, key, fn,
sm['deviceState'].networkType.raw,
sm['deviceState'].networkMetered)
if success:
backoff = 0.1
elif allow_sleep:
cloudlog.info("upload backoff %r", backoff)
time.sleep(backoff + random.uniform(0, backoff))
backoff = min(backoff * 2, 120)
pm.send("uploaderState", uploader.get_msg())
def snapshot():
params = Params()
if (not params.get_bool("IsOffroad")
) or params.get_bool("IsTakingSnapshot"):
print("Already taking snapshot")
return None, None
front_camera_allowed = params.get_bool("RecordFront")
params.put_bool("IsTakingSnapshot", True)
set_offroad_alert("Offroad_IsTakingSnapshot", True)
time.sleep(
2.0
) # Give thermald time to read the param, or if just started give camerad time to start
# Check if camerad is already started
try:
subprocess.check_call(["pgrep", "camerad"])
print("Camerad already running")
params.put_bool("IsTakingSnapshot", False)
params.delete("Offroad_IsTakingSnapshot")
return None, None
except subprocess.CalledProcessError:
pass
try:
# Allow testing on replay on PC
if not PC:
managed_processes['camerad'].start()
frame = "wideRoadCameraState"
front_frame = "driverCameraState" if front_camera_allowed else None
rear, front = get_snapshots(frame, front_frame)
finally:
managed_processes['camerad'].stop()
params.put_bool("IsTakingSnapshot", False)
set_offroad_alert("Offroad_IsTakingSnapshot", False)
if not front_camera_allowed:
front = None
return rear, front
def __init__(self, param_put=False):
self.param_put = param_put
# Read saved calibration
params = Params()
calibration_params = params.get("CalibrationParams")
self.wide_camera = TICI and params.get_bool('EnableWideCamera')
rpy_init = RPY_INIT
valid_blocks = 0
if param_put and calibration_params:
try:
msg = log.Event.from_bytes(calibration_params)
rpy_init = list(msg.liveCalibration.rpyCalib)
valid_blocks = msg.liveCalibration.validBlocks
except Exception:
cloudlog.exception("Error reading cached CalibrationParams")
self.reset(rpy_init, valid_blocks)
self.update_status()
def __init__(self, CP, CarController, CarState):
self.CP = CP
self.VM = VehicleModel(CP)
self.frame = 0
self.steering_unpressed = 0
self.low_speed_alert = False
self.silent_steer_warning = True
if CarState is not None:
self.CS = CarState(CP)
self.cp = self.CS.get_can_parser(CP)
self.cp_cam = self.CS.get_cam_can_parser(CP)
self.cp_body = self.CS.get_body_can_parser(CP)
self.cp_loopback = self.CS.get_loopback_can_parser(CP)
self.CC = None
if CarController is not None:
self.CC = CarController(self.cp.dbc_name, CP, self.VM)
# KRKeegan Option to Enable Gas on Cruise
params = Params()
self.enable_gas_on_cruise = params.get_bool("EnableGasOnCruise")
def __init__(self, param_put: bool = False):
self.param_put = param_put
self.CP = car.CarParams.from_bytes(Params().get("CarParams",
block=True))
# Read saved calibration
params = Params()
calibration_params = params.get("CalibrationParams")
self.wide_camera = params.get_bool('WideCameraOnly')
rpy_init = RPY_INIT
valid_blocks = 0
if param_put and calibration_params:
try:
msg = log.Event.from_bytes(calibration_params)
rpy_init = np.array(msg.liveCalibration.rpyCalib)
valid_blocks = msg.liveCalibration.validBlocks
except Exception:
cloudlog.exception("Error reading cached CalibrationParams")
self.reset(rpy_init, valid_blocks)
self.update_status()
def get_std_params(candidate, fingerprint):
ret = car.CarParams.new_message()
ret.carFingerprint = candidate
ret.unsafeMode = 0 # see panda/board/safety_declarations.h for allowed values
params = Params()
if params.get_bool("EnableGasOnCruise"):
ret.unsafeMode = ret.unsafeMode | 1
# standard ALC params
ret.steerControlType = car.CarParams.SteerControlType.torque
ret.steerMaxBP = [0.]
ret.steerMaxV = [1.]
ret.minSteerSpeed = 0.
ret.wheelSpeedFactor = 1.0
ret.pcmCruise = True # openpilot's state is tied to the PCM's cruise state on most cars
ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
ret.steerRatioRear = 0. # no rear steering, at least on the listed cars aboveA
ret.openpilotLongitudinalControl = False
ret.stopAccel = -2.0
ret.stoppingDecelRate = 0.8 # brake_travel/s while trying to stop
ret.vEgoStopping = 0.5
ret.vEgoStarting = 0.5 # needs to be >= vEgoStopping to avoid state transition oscillation
ret.stoppingControl = True
ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.]
ret.longitudinalTuning.kf = 1.
ret.longitudinalTuning.kpBP = [0.]
ret.longitudinalTuning.kpV = [1.]
ret.longitudinalTuning.kiBP = [0.]
ret.longitudinalTuning.kiV = [1.]
# TODO estimate car specific lag, use .15s for now
ret.longitudinalActuatorDelayLowerBound = 0.15
ret.longitudinalActuatorDelayUpperBound = 0.15
ret.steerLimitTimer = 1.0
return ret
def __init__(self, CP):
params = Params()
wide_camera = (params.get('EnableWideCamera') == b'1') if TICI else False
self.LP = LanePlanner(wide_camera)
self.last_cloudlog_t = 0
self.steer_rate_cost = CP.steerRateCost
self.setup_mpc()
self.solution_invalid_cnt = 0
self.use_lanelines = not params.get_bool('EndToEndToggle')
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
self.lane_change_timer = 0.0
self.lane_change_ll_prob = 1.0
self.prev_one_blinker = False
self.desire = log.LateralPlan.Desire.none
self.path_xyz = np.zeros((TRAJECTORY_SIZE,3))
self.path_xyz_stds = np.ones((TRAJECTORY_SIZE,3))
self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
self.t_idxs = np.arange(TRAJECTORY_SIZE)
self.y_pts = np.zeros(TRAJECTORY_SIZE)
def thermald_thread(end_event, hw_queue):
pm = messaging.PubMaster(['deviceState'])
sm = messaging.SubMaster([
"peripheralState", "gpsLocationExternal", "controlsState",
"pandaStates"
],
poll=["pandaStates"])
count = 0
onroad_conditions: Dict[str, bool] = {
"ignition": False,
}
startup_conditions: Dict[str, bool] = {}
startup_conditions_prev: Dict[str, bool] = {}
off_ts = None
started_ts = None
started_seen = False
thermal_status = ThermalStatus.green
last_hw_state = HardwareState(
network_type=NetworkType.none,
network_metered=False,
network_strength=NetworkStrength.unknown,
network_info=None,
nvme_temps=[],
modem_temps=[],
wifi_address='N/A',
)
current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML)
temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML)
should_start_prev = False
in_car = False
engaged_prev = False
params = Params()
power_monitor = PowerMonitoring()
HARDWARE.initialize_hardware()
thermal_config = HARDWARE.get_thermal_config()
fan_controller = None
restart_triggered_ts = 0.
panda_state_ts = 0.
while not end_event.is_set():
sm.update(PANDA_STATES_TIMEOUT)
pandaStates = sm['pandaStates']
peripheralState = sm['peripheralState']
msg = read_thermal(thermal_config)
# neokii
if sec_since_boot() - restart_triggered_ts < 5.:
onroad_conditions["not_restart_triggered"] = False
else:
onroad_conditions["not_restart_triggered"] = True
if params.get_bool("SoftRestartTriggered"):
params.put_bool("SoftRestartTriggered", False)
restart_triggered_ts = sec_since_boot()
if sm.updated['pandaStates'] and len(pandaStates) > 0:
# Set ignition based on any panda connected
onroad_conditions["ignition"] = any(
ps.ignitionLine or ps.ignitionCan for ps in pandaStates
if ps.pandaType != log.PandaState.PandaType.unknown)
pandaState = pandaStates[0]
if pandaState.pandaType != log.PandaState.PandaType.unknown:
panda_state_ts = sec_since_boot()
in_car = pandaState.harnessStatus != log.PandaState.HarnessStatus.notConnected
# Setup fan handler on first connect to panda
if fan_controller is None and peripheralState.pandaType != log.PandaState.PandaType.unknown:
if TICI:
fan_controller = TiciFanController()
try:
last_hw_state = hw_queue.get_nowait()
except queue.Empty:
pass
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 = last_hw_state.network_type
msg.deviceState.networkMetered = last_hw_state.network_metered
msg.deviceState.networkStrength = last_hw_state.network_strength
if last_hw_state.network_info is not None:
msg.deviceState.networkInfo = last_hw_state.network_info
msg.deviceState.nvmeTempC = last_hw_state.nvme_temps
msg.deviceState.modemTempC = last_hw_state.modem_temps
msg.deviceState.wifiIpAddress = last_hw_state.wifi_address
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 fan_controller is not None:
msg.deviceState.fanSpeedPercentDesired = fan_controller.update(
max_comp_temp, onroad_conditions["ignition"])
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 ****
# Ensure date/time are valid
now = datetime.datetime.utcnow()
startup_conditions[
"time_valid"] = True #(now.year > 2020) or (now.year == 2020 and now.month >= 10)
set_offroad_alert_if_changed("Offroad_InvalidTime",
(not startup_conditions["time_valid"]))
startup_conditions[
"up_to_date"] = True #params.get("Offroad_ConnectivityNeeded") is None or params.get_bool("DisableUpdates") or params.get_bool("SnoozeUpdate")
startup_conditions["not_uninstalling"] = not params.get_bool(
"DoUninstall")
startup_conditions["accepted_terms"] = params.get(
"HasAcceptedTerms") == terms_version
# 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
onroad_conditions[
"device_temp_good"] = thermal_status < ThermalStatus.danger
set_offroad_alert_if_changed(
"Offroad_TemperatureTooHigh",
(not onroad_conditions["device_temp_good"]))
# TODO: this should move to TICI.initialize_hardware, but we currently can't import params there
if TICI:
if not os.path.isfile("/persist/comma/living-in-the-moment"):
if not Path("/data/media").is_mount():
set_offroad_alert_if_changed("Offroad_StorageMissing",
True)
else:
# check for bad NVMe
try:
with open("/sys/block/nvme0n1/device/model") as f:
model = f.read().strip()
if not model.startswith(
"Samsung SSD 980") and params.get(
"Offroad_BadNvme") is None:
set_offroad_alert_if_changed(
"Offroad_BadNvme", True)
cloudlog.event("Unsupported NVMe",
model=model,
error=True)
except Exception:
pass
# Handle offroad/onroad transition
should_start = all(onroad_conditions.values())
if started_ts is None:
should_start = should_start and 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)
params.put_bool("IsEngaged", False)
engaged_prev = False
HARDWARE.set_power_save(not should_start)
if sm.updated['controlsState']:
engaged = sm['controlsState'].enabled
if engaged != engaged_prev:
params.put_bool("IsEngaged", engaged)
engaged_prev = engaged
try:
with open('/dev/kmsg', 'w') as kmsg:
kmsg.write(f"<3>[thermald] engaged: {engaged}\n")
except Exception:
pass
if should_start:
off_ts = None
if started_ts is None:
started_ts = sec_since_boot()
started_seen = True
else:
if onroad_conditions["ignition"] and (startup_conditions !=
startup_conditions_prev):
cloudlog.event("Startup blocked",
startup_conditions=startup_conditions,
onroad_conditions=onroad_conditions)
started_ts = None
if off_ts is None:
off_ts = sec_since_boot()
# Offroad power monitoring
power_monitor.calculate(peripheralState, onroad_conditions["ignition"])
msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used()
msg.deviceState.carBatteryCapacityUwh = max(
0, power_monitor.get_car_battery_capacity())
current_power_draw = HARDWARE.get_current_power_draw() # pylint: disable=assignment-from-none
if current_power_draw is not None:
statlog.sample("power_draw", current_power_draw)
msg.deviceState.powerDrawW = current_power_draw
else:
msg.deviceState.powerDrawW = 0
# Check if we need to disable charging (handled by boardd)
msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(
onroad_conditions["ignition"], in_car, off_ts)
# Check if we need to shut down
if power_monitor.should_shutdown(peripheralState,
onroad_conditions["ignition"], in_car,
off_ts, started_seen):
cloudlog.warning(f"shutting device down, offroad since {off_ts}")
params.put_bool("DoShutdown", True)
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)
should_start_prev = should_start
startup_conditions_prev = startup_conditions.copy()
# Log to statsd
statlog.gauge("free_space_percent", msg.deviceState.freeSpacePercent)
statlog.gauge("gpu_usage_percent", msg.deviceState.gpuUsagePercent)
statlog.gauge("memory_usage_percent",
msg.deviceState.memoryUsagePercent)
for i, usage in enumerate(msg.deviceState.cpuUsagePercent):
statlog.gauge(f"cpu{i}_usage_percent", usage)
for i, temp in enumerate(msg.deviceState.cpuTempC):
statlog.gauge(f"cpu{i}_temperature", temp)
for i, temp in enumerate(msg.deviceState.gpuTempC):
statlog.gauge(f"gpu{i}_temperature", temp)
statlog.gauge("memory_temperature", msg.deviceState.memoryTempC)
statlog.gauge("ambient_temperature", msg.deviceState.ambientTempC)
for i, temp in enumerate(msg.deviceState.pmicTempC):
statlog.gauge(f"pmic{i}_temperature", temp)
for i, temp in enumerate(last_hw_state.nvme_temps):
statlog.gauge(f"nvme_temperature{i}", temp)
for i, temp in enumerate(last_hw_state.modem_temps):
statlog.gauge(f"modem_temperature{i}", temp)
statlog.gauge("fan_speed_percent_desired",
msg.deviceState.fanSpeedPercentDesired)
statlog.gauge("screen_brightness_percent",
msg.deviceState.screenBrightnessPercent)
# report to server once every 10 minutes
if (count % int(600. / DT_TRML)) == 0:
cloudlog.event(
"STATUS_PACKET",
count=count,
pandaStates=[
strip_deprecated_keys(p.to_dict()) for p in pandaStates
],
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 manager_init():
# update system time from panda
set_time(cloudlog)
params = Params()
params.clear_all(ParamKeyType.CLEAR_ON_MANAGER_START)
default_params = [
("OpenpilotEnabledToggle", "1"),
("CommunityFeaturesToggle", "1"),
("IsMetric", "1"),
# HKG
("UseClusterSpeed", "1"),
("LongControlEnabled", "0"),
("MadModeEnabled", "1"),
("IsLdwsCar", "0"),
("LaneChangeEnabled", "0"),
("AutoLaneChangeEnabled", "0"),
("SccSmootherSlowOnCurves", "0"),
("SccSmootherSyncGasPressed", "0"),
("StockNaviDecelEnabled", "0"),
("ShowDebugUI", "0"),
("CustomLeadMark", "0")
]
if not PC:
default_params.append(
("LastUpdateTime",
datetime.datetime.utcnow().isoformat().encode('utf8')))
if params.get_bool("RecordFrontLock"):
params.put_bool("RecordFront", True)
# set unset params
for k, v in default_params:
if params.get(k) is None:
params.put(k, v)
# is this dashcam?
if os.getenv("PASSIVE") is not None:
params.put_bool("Passive", bool(int(os.getenv("PASSIVE"))))
if params.get("Passive") is None:
raise Exception("Passive must be set to continue")
# Create folders needed for msgq
try:
os.mkdir("/dev/shm")
except FileExistsError:
pass
except PermissionError:
print("WARNING: failed to make /dev/shm")
# set version params
params.put("Version", version)
params.put("TermsVersion", terms_version)
params.put("TrainingVersion", training_version)
params.put("GitCommit", get_git_commit(default=""))
params.put("GitBranch", get_git_branch(default=""))
params.put("GitRemote", get_git_remote(default=""))
# set dongle id
reg_res = register(show_spinner=True)
if reg_res:
dongle_id = reg_res
else:
serial = params.get("HardwareSerial")
raise Exception(f"Registration failed for device {serial}")
os.environ['DONGLE_ID'] = dongle_id # Needed for swaglog
if not dirty:
os.environ['CLEAN'] = '1'
cloudlog.bind_global(dongle_id=dongle_id,
version=version,
dirty=dirty,
device=HARDWARE.get_device_type())
if comma_remote and not (os.getenv("NOLOG") or os.getenv("NOCRASH") or PC):
crash.init()
crash.bind_user(id=dongle_id)
crash.bind_extra(dirty=dirty,
origin=origin,
branch=branch,
commit=commit,
device=HARDWARE.get_device_type())
def thermald_thread() -> NoReturn:
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
onroad_conditions: Dict[str, bool] = {
"ignition": False,
}
startup_conditions: Dict[str, bool] = {}
startup_conditions_prev: Dict[str, bool] = {}
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 onroad_conditions["ignition"]:
cloudlog.error("Lost panda connection while onroad")
onroad_conditions["ignition"] = False
else:
no_panda_cnt = 0
onroad_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, onroad_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 ****
# Ensure date/time are valid
now = datetime.datetime.utcnow()
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"]))
startup_conditions["up_to_date"] = params.get("Offroad_ConnectivityNeeded") is None or params.get_bool("DisableUpdates") or params.get_bool("SnoozeUpdate")
startup_conditions["not_uninstalling"] = not params.get_bool("DoUninstall")
startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version
# 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
onroad_conditions["device_temp_good"] = thermal_status < ThermalStatus.danger
set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", (not onroad_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(onroad_conditions.values())
if started_ts is None:
should_start = should_start and 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 onroad_conditions["ignition"] and (startup_conditions != startup_conditions_prev):
cloudlog.event("Startup blocked", startup_conditions=startup_conditions, onroad_conditions=onroad_conditions)
started_ts = None
if off_ts is None:
off_ts = sec_since_boot()
# Offroad power monitoring
power_monitor.calculate(peripheralState, onroad_conditions["ignition"])
msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used()
msg.deviceState.carBatteryCapacityUwh = max(0, power_monitor.get_car_battery_capacity())
current_power_draw = HARDWARE.get_current_power_draw() # pylint: disable=assignment-from-none
msg.deviceState.powerDrawW = current_power_draw if current_power_draw is not None else 0
# Check if we need to disable charging (handled by boardd)
msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(onroad_conditions["ignition"], in_car, off_ts)
# Check if we need to shut down
if power_monitor.should_shutdown(peripheralState, onroad_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 main():
params = Params()
if params.get_bool("DisableUpdates"):
raise RuntimeError("updates are disabled by the DisableUpdates param")
if EON and os.geteuid() != 0:
raise RuntimeError("updated must be launched as root!")
# Set low io priority
proc = psutil.Process()
if psutil.LINUX:
proc.ionice(psutil.IOPRIO_CLASS_BE, value=7)
ov_lock_fd = open(LOCK_FILE, 'w')
try:
fcntl.flock(ov_lock_fd, fcntl.LOCK_EX | fcntl.LOCK_NB)
except IOError as e:
raise RuntimeError(
"couldn't get overlay lock; is another updated running?") from e
# Wait for IsOffroad to be set before our first update attempt
wait_helper = WaitTimeHelper(proc)
wait_helper.sleep(30)
overlay_init = Path(os.path.join(BASEDIR, ".overlay_init"))
if overlay_init.exists():
overlay_init.unlink()
first_run = True
last_fetch_time = 0
update_failed_count = 0
# Run the update loop
# * every 1m, do a lightweight internet/update check
# * every 10m, do a full git fetch
while not wait_helper.shutdown:
update_now = wait_helper.ready_event.is_set()
wait_helper.ready_event.clear()
# Don't run updater while onroad or if the time's wrong
time_wrong = datetime.datetime.utcnow().year < 2019
is_onroad = not params.get_bool("IsOffroad")
if is_onroad or time_wrong:
wait_helper.sleep(30)
cloudlog.info("not running updater, not offroad")
continue
# Attempt an update
exception = None
new_version = False
update_failed_count += 1
try:
init_overlay()
internet_ok, update_available = check_for_update()
if internet_ok and not update_available:
update_failed_count = 0
# Fetch updates at most every 10 minutes
if internet_ok and (update_now or
time.monotonic() - last_fetch_time > 60 * 10):
new_version = fetch_update(wait_helper)
update_failed_count = 0
last_fetch_time = time.monotonic()
if first_run and not new_version and os.path.isdir(
NEOSUPDATE_DIR):
shutil.rmtree(NEOSUPDATE_DIR)
first_run = False
except subprocess.CalledProcessError as e:
cloudlog.event("update process failed",
cmd=e.cmd,
output=e.output,
returncode=e.returncode)
exception = f"command failed: {e.cmd}\n{e.output}"
except Exception as e:
cloudlog.exception("uncaught updated exception, shouldn't happen")
exception = str(e)
set_params(new_version, update_failed_count, exception)
wait_helper.sleep(60)
dismount_overlay()
def manager_thread():
if EON:
Process(name="shutdownd",
target=launcher,
args=("selfdrive.shutdownd", )).start()
system("am startservice com.neokii.optool/.MainService")
Process(name="road_speed_limiter",
target=launcher,
args=("selfdrive.road_speed_limiter", )).start()
cloudlog.info("manager start")
cloudlog.info({"environ": os.environ})
# save boot log
#subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd"))
params = Params()
ignore = []
if params.get("DongleId", encoding='utf8') == UNREGISTERED_DONGLE_ID:
ignore += ["manage_athenad", "uploader"]
if os.getenv("NOBOARD") is not None:
ignore.append("pandad")
if os.getenv("BLOCK") is not None:
ignore += os.getenv("BLOCK").split(",")
ensure_running(managed_processes.values(), started=False, not_run=ignore)
started_prev = False
sm = messaging.SubMaster(['deviceState'])
pm = messaging.PubMaster(['managerState'])
while True:
sm.update()
not_run = ignore[:]
if sm['deviceState'].freeSpacePercent < 5:
not_run.append("loggerd")
started = sm['deviceState'].started
driverview = params.get_bool("IsDriverViewEnabled")
ensure_running(managed_processes.values(), started, driverview,
not_run)
# trigger an update after going offroad
if started_prev and not started and 'updated' in managed_processes:
os.sync()
managed_processes['updated'].signal(signal.SIGHUP)
started_prev = started
running_list = [
"%s%s\u001b[0m" %
("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name)
for p in managed_processes.values() if p.proc
]
cloudlog.debug(' '.join(running_list))
# send managerState
msg = messaging.new_message('managerState')
msg.managerState.processes = [
p.get_process_state_msg() for p in managed_processes.values()
]
pm.send('managerState', msg)
# TODO: let UI handle this
# Exit main loop when uninstall is needed
if params.get_bool("DoUninstall"):
break
class Planner():
def __init__(self, CP):
self.CP = CP
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.mpc_model = LongitudinalMpcModel()
self.model_mpc_helper = ModelMpcHelper()
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc_next = 0.0
self.a_acc_next = 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.path_x = np.arange(192)
self.fcw = False
self.params = Params()
self.first_loop = True
self.target_speed_map = 0.0
self.target_speed_map_counter = 0
self.target_speed_map_counter_check = False
self.target_speed_map_counter1 = 0
self.target_speed_map_counter2 = 0
self.target_speed_map_counter3 = 0
self.target_speed_map_dist = 0
self.target_speed_map_block = False
self.target_speed_map_sign = False
self.tartget_speed_offset = int(self.params.get("OpkrSpeedLimitOffset", encoding="utf8"))
self.vego = 0
def choose_solution(self, v_cruise_setpoint, enabled, model_enabled):
possible_futures = [self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint]
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
if self.mpc_model.valid and model_enabled:
solutions['model'] = self.mpc_model.v_mpc
possible_futures.append(self.mpc_model.v_mpc_future) # only used when using model
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
elif slowest == 'model':
self.v_acc = self.mpc_model.v_mpc
self.a_acc = self.mpc_model.a_mpc
self.v_acc_future = min(possible_futures)
def update(self, sm, CP):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
v_ego = sm['carState'].vEgo
self.vego = v_ego
long_control_state = sm['controlsState'].longControlState
if self.params.get_bool("OpenpilotLongitudinalControl"):
v_cruise_kph = sm['carState'].vSetDis
else:
v_cruise_kph = sm['controlsState'].vCruise
force_slow_decel = sm['controlsState'].forceDecel
v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX)
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
lead_1 = sm['radarState'].leadOne
lead_2 = sm['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
self.v_acc_start = self.v_acc_next
self.a_acc_start = self.a_acc_next
if self.params.get_bool("OpkrMapEnable"):
self.target_speed_map_counter += 1
if self.target_speed_map_counter >= (50+self.target_speed_map_counter1) and self.target_speed_map_counter_check == False:
self.target_speed_map_counter_check = True
os.system("logcat -d -s opkrspdlimit,opkrspd2limit | grep opkrspd | tail -n 1 | awk \'{print $7}\' > /data/params/d/LimitSetSpeedCamera &")
os.system("logcat -d -s opkrspddist | grep opkrspd | tail -n 1 | awk \'{print $7}\' > /data/params/d/LimitSetSpeedCameraDist &")
self.target_speed_map_counter3 += 1
if self.target_speed_map_counter3 > 2:
self.target_speed_map_counter3 = 0
os.system("logcat -c &")
elif self.target_speed_map_counter >= (75+self.target_speed_map_counter1):
self.target_speed_map_counter1 = 0
self.target_speed_map_counter = 0
self.target_speed_map_counter_check = False
mapspeed = self.params.get("LimitSetSpeedCamera", encoding="utf8")
mapspeeddist = self.params.get("LimitSetSpeedCameraDist", encoding="utf8")
if mapspeed is not None and mapspeeddist is not None:
mapspeed = int(float(mapspeed.rstrip('\n')))
mapspeeddist = int(float(mapspeeddist.rstrip('\n')))
if mapspeed > 29:
self.target_speed_map = mapspeed
self.target_speed_map_dist = mapspeeddist
if self.target_speed_map_dist > 1001:
self.target_speed_map_block = True
self.target_speed_map_counter1 = 80
os.system("logcat -c &")
else:
self.target_speed_map = 0
self.target_speed_map_dist = 0
self.target_speed_map_block = False
elif mapspeed is None and mapspeeddist is None and self.target_speed_map_counter2 < 2:
self.target_speed_map_counter2 += 1
self.target_speed_map_counter = 51
self.target_speed_map = 0
self.target_speed_map_dist = 0
self.target_speed_map_counter_check = True
self.target_speed_map_block = False
self.target_speed_map_sign = False
else:
self.target_speed_map_counter = 49
self.target_speed_map_counter2 = 0
self.target_speed_map = 0
self.target_speed_map_dist = 0
self.target_speed_map_counter_check = False
self.target_speed_map_block = False
self.target_speed_map_sign = False
# Calculate speed for normal cruise control
if enabled and not self.first_loop and not sm['carState'].brakePressed and not sm['carState'].gasPressed:
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_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
if force_slow_decel and False: # awareness decel is disabled for now
# if required so, force a smooth deceleration
accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)
accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1])
self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start,
v_cruise_setpoint,
accel_limits_turns[1], accel_limits_turns[0],
jerk_limits[1], jerk_limits[0],
LON_MPC_STEP)
# 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(sm['carState'].aEgo, 0.0)
reset_speed = self.CP.minSpeedCan 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.mpc_model.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(sm['carState'], lead_1)
self.mpc2.update(sm['carState'], lead_2)
distances, speeds, accelerations = self.model_mpc_helper.convert_data(sm)
self.mpc_model.update(sm['carState'].vEgo, sm['carState'].aEgo,
distances,
speeds,
accelerations)
self.choose_solution(v_cruise_setpoint, enabled, self.params.get_bool("ModelLongEnabled"))
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time,
sm['controlsState'].active,
v_ego, sm['carState'].aEgo,
lead_1.dRel, lead_1.vLead, lead_1.aLeadK,
lead_1.yRel, lead_1.vLat,
lead_1.fcw, blinkers) and not sm['carState'].brakePressed
if self.fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
# Interpolate 0.05 seconds and save as starting point for next iteration
a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (self.a_acc - self.a_acc_start)
v_acc_sol = self.v_acc_start + CP.radarTimeStep * (a_acc_sol + self.a_acc_start) / 2.0
self.v_acc_next = v_acc_sol
self.a_acc_next = a_acc_sol
self.first_loop = False
def publish(self, sm, pm):
self.mpc1.publish(pm)
self.mpc2.publish(pm)
plan_send = messaging.new_message('longitudinalPlan')
plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'radarState'])
longitudinalPlan = plan_send.longitudinalPlan
longitudinalPlan.mdMonoTime = sm.logMonoTime['modelV2']
longitudinalPlan.radarStateMonoTime = sm.logMonoTime['radarState']
longitudinalPlan.vCruise = float(self.v_cruise)
longitudinalPlan.aCruise = float(self.a_cruise)
longitudinalPlan.vStart = float(self.v_acc_start)
longitudinalPlan.aStart = float(self.a_acc_start)
longitudinalPlan.vTarget = float(self.v_acc)
longitudinalPlan.aTarget = float(self.a_acc)
longitudinalPlan.vTargetFuture = float(self.v_acc_future)
longitudinalPlan.hasLead = self.mpc1.prev_lead_status
longitudinalPlan.longitudinalPlanSource = self.longitudinalPlanSource
longitudinalPlan.fcw = self.fcw
longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState']
# Send radarstate(dRel, vRel, yRel)
lead_1 = sm['radarState'].leadOne
lead_2 = sm['radarState'].leadTwo
longitudinalPlan.dRel1 = float(lead_1.dRel)
longitudinalPlan.yRel1 = float(lead_1.yRel)
longitudinalPlan.vRel1 = float(lead_1.vRel)
longitudinalPlan.dRel2 = float(lead_2.dRel)
longitudinalPlan.yRel2 = float(lead_2.yRel)
longitudinalPlan.vRel2 = float(lead_2.vRel)
longitudinalPlan.status2 = bool(lead_2.status)
cam_distance_calc = 0
cam_distance_calc = interp(self.vego*CV.MS_TO_KPH, [30,60,100,160], [3.75,5.5,6,7])
consider_speed = interp((self.vego*CV.MS_TO_KPH - self.target_speed_map), [10, 30], [1, 1.3])
if self.target_speed_map > 29 and self.target_speed_map_dist < cam_distance_calc*consider_speed*self.vego*CV.MS_TO_KPH:
longitudinalPlan.targetSpeedCamera = float(self.target_speed_map)
longitudinalPlan.targetSpeedCameraDist = float(self.target_speed_map_dist)
self.target_speed_map_sign = True
elif self.target_speed_map > 29 and self.target_speed_map_dist >= cam_distance_calc*consider_speed*self.vego*CV.MS_TO_KPH and self.target_speed_map_block:
longitudinalPlan.targetSpeedCamera = float(self.target_speed_map)
longitudinalPlan.targetSpeedCameraDist = float(self.target_speed_map_dist)
self.target_speed_map_sign = True
elif self.target_speed_map > 29 and self.target_speed_map_sign:
longitudinalPlan.targetSpeedCamera = float(self.target_speed_map)
longitudinalPlan.targetSpeedCameraDist = float(self.target_speed_map_dist)
else:
longitudinalPlan.targetSpeedCamera = 0
longitudinalPlan.targetSpeedCameraDist = 0
pm.send('longitudinalPlan', plan_send)
def uploader_fn(exit_event):
params = Params()
dongle_id = params.get("DongleId", encoding='utf8')
transition_to_offroad_last = 0.
disable_onroad_upload_offroad_transition_timeout = 900. # wait until offroad for 15 minutes before starting uploads
offroad_last = params.get_bool("IsOffroad")
if dongle_id is None:
cloudlog.info("uploader missing dongle_id")
raise Exception("uploader can't start without dongle id")
if TICI and not Path("/data/media").is_mount():
cloudlog.warning("NVME not mounted")
sm = messaging.SubMaster(['deviceState'])
pm = messaging.PubMaster(['uploaderState'])
uploader = Uploader(dongle_id, ROOT)
backoff = 0.1
while not exit_event.is_set():
sm.update(0)
offroad = params.get_bool("IsOffroad")
t = sec_since_boot()
if offroad and not offroad_last and t > 300.:
transition_to_offroad_last = sec_since_boot()
offroad_last = offroad
network_type = sm[
'deviceState'].networkType if not force_wifi else NetworkType.wifi
if network_type == NetworkType.none:
if allow_sleep:
time.sleep(60 if offroad else 5)
continue
on_wifi = network_type == NetworkType.wifi
allow_raw_upload = params.get_bool("UploadRaw")
if Params().get_bool("DisableOnroadUploads"):
if not offroad or (
transition_to_offroad_last > 0.
and t - transition_to_offroad_last <
disable_onroad_upload_offroad_transition_timeout):
if not offroad:
cloudlog.info("not uploading: onroad uploads disabled")
else:
wait_minutes = int(
disable_onroad_upload_offroad_transition_timeout / 60)
time_left = disable_onroad_upload_offroad_transition_timeout - (
t - transition_to_offroad_last)
if (time_left / 60. > 2.):
time_left_str = f"{int(time_left / 60)} minute(s)"
else:
time_left_str = f"{int(time_left)} seconds(s)"
cloudlog.info(
f"not uploading: waiting until offroad for {wait_minutes} minutes; {time_left_str} left"
)
if allow_sleep:
time.sleep(60)
continue
d = uploader.next_file_to_upload(
with_raw=allow_raw_upload and on_wifi and offroad)
if d is None: # Nothing to upload
if allow_sleep:
time.sleep(60 if offroad else 5)
continue
key, fn = d
cloudlog.debug("upload %r over %s", d, network_type)
success = uploader.upload(key, fn)
if success:
backoff = 0.1
elif allow_sleep:
cloudlog.info("upload backoff %r", backoff)
time.sleep(backoff + random.uniform(0, backoff))
backoff = min(backoff * 2, 120)
pm.send("uploaderState", uploader.get_msg())
cloudlog.info("upload done, success=%r", success)
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:
ignore = ['driverCameraState', 'managerState'
] if SIMULATION else None
self.sm = messaging.SubMaster([
'deviceState', 'pandaState', 'modelV2', 'liveCalibration',
'driverMonitoringState', 'longitudinalPlan', 'lateralPlan',
'liveLocationKalman', 'roadCameraState', 'driverCameraState',
'managerState', 'liveParameters', 'radarState'
],
ignore_alive=ignore)
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT',
False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
# wait for one pandaState and one CAN packet
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'])
# read params
params = Params()
self.is_metric = params.get_bool("IsMetric")
self.is_ldw_enabled = params.get_bool("IsLdwEnabled")
community_feature_toggle = params.get_bool("CommunityFeaturesToggle")
openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle")
passive = params.get_bool("Passive") or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not dashcam mode
controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive and not self.CP.dashcamOnly
community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
self.CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard and boardd safety mode
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP, self.CI.compute_gb)
self.VM = VehicleModel(self.CP)
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.LaC = LatControlAngle(self.CP)
elif self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.can_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.current_alert_types = [ET.PERMANENT]
self.logged_comm_issue = False
self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED
self.sm['deviceState'].freeSpacePercent = 100
self.sm['driverMonitoringState'].events = []
self.sm['driverMonitoringState'].awarenessStatus = 1.
self.sm['driverMonitoringState'].faceDetected = False
self.sm['liveParameters'].valid = True
self.startup_event = get_startup_event(car_recognized,
controller_available)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if community_feature_disallowed:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
elif self.read_only:
self.events.add(EventName.dashcamMode, static=True)
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
def manager_init():
# update system time from panda
set_time(cloudlog)
# save boot log
subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd"))
params = Params()
params.clear_all(ParamKeyType.CLEAR_ON_MANAGER_START)
default_params = [
("CompletedTrainingVersion", "0"),
("HasAcceptedTerms", "0"),
("OpenpilotEnabledToggle", "1"),
]
if not PC:
default_params.append(("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')))
if params.get_bool("RecordFrontLock"):
params.put_bool("RecordFront", True)
if not params.get_bool("DisableRadar_Allow"):
params.delete("DisableRadar")
# set unset params
for k, v in default_params:
if params.get(k) is None:
params.put(k, v)
# is this dashcam?
if os.getenv("PASSIVE") is not None:
params.put_bool("Passive", bool(int(os.getenv("PASSIVE"))))
if params.get("Passive") is None:
raise Exception("Passive must be set to continue")
# Create folders needed for msgq
try:
os.mkdir("/dev/shm")
except FileExistsError:
pass
except PermissionError:
print("WARNING: failed to make /dev/shm")
# set version params
params.put("Version", version)
params.put("TermsVersion", terms_version)
params.put("TrainingVersion", training_version)
params.put("GitCommit", get_git_commit(default=""))
params.put("GitBranch", get_git_branch(default=""))
params.put("GitRemote", get_git_remote(default=""))
# set dongle id
reg_res = register(show_spinner=True)
if reg_res:
dongle_id = reg_res
else:
serial = params.get("HardwareSerial")
raise Exception(f"Registration failed for device {serial}")
os.environ['DONGLE_ID'] = dongle_id # Needed for swaglog
if not dirty:
os.environ['CLEAN'] = '1'
cloudlog.bind_global(dongle_id=dongle_id, version=version, dirty=dirty,
device=HARDWARE.get_device_type())
if comma_remote and not (os.getenv("NOLOG") or os.getenv("NOCRASH") or PC):
crash.init()
crash.bind_user(id=dongle_id)
crash.bind_extra(dirty=dirty, origin=origin, branch=branch, commit=commit,
device=HARDWARE.get_device_type())
def __init__(self, sm=None, pm=None, can_sock=None):
config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH)
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster([
'sendcan', 'controlsState', 'carState', 'carControl',
'carEvents', 'carParams'
])
self.camera_packets = ["roadCameraState", "driverCameraState"]
if TICI:
self.camera_packets.append("wideRoadCameraState")
params = Params()
self.joystick_mode = params.get_bool("JoystickDebugMode")
joystick_packet = ['testJoystick'] if self.joystick_mode else []
self.sm = sm
if self.sm is None:
ignore = ['driverCameraState', 'managerState'
] if SIMULATION else None
self.sm = messaging.SubMaster(
[
'deviceState', 'pandaStates', 'peripheralState', 'modelV2',
'liveCalibration', 'driverMonitoringState',
'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
'managerState', 'liveParameters', 'radarState'
] + self.camera_packets + joystick_packet,
ignore_alive=ignore,
ignore_avg_freq=['radarState', 'longitudinalPlan'])
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT',
False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
if TICI:
self.log_sock = messaging.sub_sock('androidLog')
# wait for one pandaState and one CAN packet
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'])
# read params
self.is_metric = params.get_bool("IsMetric")
self.is_ldw_enabled = params.get_bool("IsLdwEnabled")
community_feature_toggle = params.get_bool("CommunityFeaturesToggle")
openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle")
passive = params.get_bool("Passive") or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
controller_available = self.CI.CC is not None and not passive and not self.CP.dashcamOnly
community_feature = self.CP.communityFeature or \
self.CP.fingerprintSource == car.CarParams.FingerprintSource.can
community_feature_disallowed = community_feature and (
not community_feature_toggle)
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
safety_config = car.CarParams.SafetyConfig.new_message()
safety_config.safetyModel = car.CarParams.SafetyModel.noOutput
self.CP.safetyConfigs = [safety_config]
# Write CarParams for radard
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP)
self.VM = VehicleModel(self.CP)
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.LaC = LatControlAngle(self.CP)
elif self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.initialized = False
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.can_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.current_alert_types = [ET.PERMANENT]
self.logged_comm_issue = False
self.button_timers = {
ButtonEvent.Type.decelCruise: 0,
ButtonEvent.Type.accelCruise: 0
}
# TODO: no longer necessary, aside from process replay
self.sm['liveParameters'].valid = True
self.startup_event = get_startup_event(car_recognized,
controller_available,
len(self.CP.carFw) > 0)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if community_feature_disallowed and car_recognized and not self.CP.dashcamOnly:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
elif self.read_only:
self.events.add(EventName.dashcamMode, static=True)
elif self.joystick_mode:
self.events.add(EventName.joystickDebug, static=True)
self.startup_event = None
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
