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()
def manager_init(spinner=None):
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"),
("LastUpdateTime",
datetime.datetime.utcnow().isoformat().encode('utf8')),
("OpenpilotEnabledToggle", "1"),
("VisionRadarToggle", "0"),
("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 dashcam?
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 EON:
update_apks()
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(spinner)
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())
# ensure shared libraries are readable by apks
if EON:
os.chmod(BASEDIR, 0o755)
os.chmod("/dev/shm", 0o777)
os.chmod(os.path.join(BASEDIR, "cereal"), 0o755)
os.chmod(os.path.join(BASEDIR, "cereal", "libmessaging_shared.so"),
0o755)
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_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 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)
health_prev = None
fw_version_match_prev = True
current_connectivity_alert = None
time_valid_prev = True
should_start_prev = False
is_uno = (read_tz(29, clip=False) < -1000)
if is_uno or not ANDROID:
handle_fan = handle_fan_uno
else:
setup_eon_fan()
handle_fan = handle_fan_eon
params = Params()
pm = PowerMonitoring(is_uno)
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()
# clear car params when panda gets disconnected
if health is None and health_prev is not None:
params.panda_disconnect()
health_prev = health
if health is not None:
usb_power = health.health.usbPowerMode != log.HealthData.UsbPowerMode.client
# 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 = 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, ignition)
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 > 92.5 or bat_temp > 60.: # CPU throttling starts around ~90C
# hysteresis between onroad not allowed and engage not allowed
thermal_status = clip(thermal_status, ThermalStatus.red, ThermalStatus.danger)
elif max_cpu_temp > 87.5:
# 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:
params.put("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")
params.put("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")
params.put("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")
# start constellation of processes when the car starts
ignition = health is not None and (health.health.ignitionLine or health.health.ignitionCan)
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_taking_snapshot = params.get("IsTakingSnapshot") == b"1"
should_start = should_start and (not is_taking_snapshot)
if fw_version_match and not fw_version_match_prev:
params.delete("Offroad_PandaFirmwareMismatch")
if not fw_version_match and fw_version_match_prev:
params.put("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:
params.put("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):
params.put("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:
params.put("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 __init__(self, sm=None, pm=None, can_sock=None):
config_realtime_process(3, Priority.CTRL_HIGH)
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster([
'sendcan', 'controlsState', 'carState', 'carControl',
'carEvents', 'carParams'
])
self.sm = sm
if self.sm is None:
self.sm = messaging.SubMaster([
'thermal', 'health', 'model', 'liveCalibration',
'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman'
])
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT',
False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
# wait for one health and one CAN packet
hw_type = messaging.recv_one(self.sm.sock['health']).health.hwType
has_relay = hw_type in [HwType.blackPanda, HwType.uno, HwType.dos]
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'],
has_relay)
# read params
params = Params()
self.is_metric = params.get("IsMetric", encoding='utf8') == "1"
self.is_ldw_enabled = params.get("IsLdwEnabled",
encoding='utf8') == "1"
internet_needed = (params.get("Offroad_ConnectivityNeeded",
encoding='utf8') is not None) and (
params.get("DisableUpdates") != b"1")
community_feature_toggle = params.get("CommunityFeaturesToggle",
encoding='utf8') == "1"
openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle",
encoding='utf8') == "1"
passive = params.get("Passive", encoding='utf8') == "1" or \
internet_needed or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not dashcam mode
controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive and not self.CP.dashcamOnly
community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
self.CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard and boardd safety mode
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP, self.CI.compute_gb)
self.VM = VehicleModel(self.CP)
if self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.can_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.current_alert_types = [ET.PERMANENT]
self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED
self.sm['thermal'].freeSpace = 1.
self.sm['dMonitoringState'].events = []
self.sm['dMonitoringState'].awarenessStatus = 1.
self.sm['dMonitoringState'].faceDetected = False
self.startup_event = get_startup_event(car_recognized,
controller_available, hw_type)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if internet_needed:
self.events.add(EventName.internetConnectivityNeeded, static=True)
if community_feature_disallowed:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
if hw_type == HwType.whitePanda:
self.events.add(EventName.whitePandaUnsupported, static=True)
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
def controlsd_thread(gctx=None, rate=100, default_bias=0.):
gc.disable()
# 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:
while 1:
try:
sendcan = messaging.pub_sock(context,
service_list['sendcan'].port)
break
except zmq.error.ZMQError:
kill_defaultd()
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)
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
fcw_enabled = params.get("IsFcwEnabled") == "1"
geofence = None
try:
from selfdrive.controls.lib.geofence import Geofence
geofence = Geofence(params.get("IsGeofenceEnabled") == "1")
except ImportError:
# geofence not available
params.put("IsGeofenceEnabled", "-1")
PL = Planner(CP, fcw_enabled)
LoC = LongControl(CP, CI.compute_gb)
VM = VehicleModel(CP)
LaC = LatControl(VM)
AM = AlertManager()
driver_status = DriverStatus()
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
v_cruise_kph_last = 0
overtemp = False
free_space = False
cal_status = Calibration.UNCALIBRATED
mismatch_counter = 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, mismatch_counter = data_sample(
CI, CC, thermal, cal, health, driver_monitor, gps_location, poller,
cal_status, overtemp, free_space, driver_status, geofence, state,
mismatch_counter, params)
prof.checkpoint("Sample")
# define plan
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
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)
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")
# *** run loop at fixed rate ***
rk.keep_time()
prof.display()
def controlsd_thread(gctx=None):
gc.disable()
# start the loop
set_realtime_priority(3)
params = Params()
tinklaClient = TinklaClient()
# 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', 'liveParameters'
])
logcan = messaging.sub_sock(service_list['can'].port)
# wait for health and CAN packets
hw_type = messaging.recv_one(sm.sock['health']).health.hwType
is_panda_black = hw_type == log.HealthData.HwType.blackPanda
wait_for_can(logcan)
CI, CP = get_car(logcan, sendcan, is_panda_black)
logcan.close()
# TODO: Use the logcan socket from above, but that will currenly break the tests
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
can_sock = messaging.sub_sock(service_list['can'].port,
timeout=can_timeout)
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
# 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()
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
sm['pathPlan'].posenetValid = True
# 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)
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, can_sock, 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]))
logAllAliveAndValidInfoToTinklad(sm=sm, tinklaClient=tinklaClient)
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.SOFT_DISABLE]))
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]))
tinklaClient.logCANErrorEvent(source="carcontroller",
canMessage=0,
additionalInformation="Invalid CAN")
if not sounds_available:
events.append(
create_event('soundsUnavailable', [ET.NO_ENTRY, 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 = \
state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], sm['liveParameters'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
driver_status, LaC, LoC, VM, read_only, is_metric, cal_perc)
rk.keep_time(1. / 10000) # Run at 100Hz
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()
def __init__(self, gctx, rate=100):
self.rate = rate
# *** log ***
context = zmq.Context()
# pub
self.live100 = messaging.pub_sock(context,
service_list['live100'].port)
self.carstate = messaging.pub_sock(context,
service_list['carState'].port)
self.carcontrol = messaging.pub_sock(context,
service_list['carControl'].port)
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
# sub
self.thermal = messaging.sub_sock(context,
service_list['thermal'].port)
self.health = messaging.sub_sock(context, service_list['health'].port)
logcan = messaging.sub_sock(context, service_list['can'].port)
self.cal = messaging.sub_sock(context,
service_list['liveCalibration'].port)
self.CC = car.CarControl.new_message()
self.CI, self.CP = get_car(logcan, sendcan)
self.PL = Planner(self.CP)
self.AM = AlertManager()
self.LoC = LongControl()
self.LaC = LatControl()
self.VM = VehicleModel(self.CP)
# write CarParams
params = Params()
params.put("CarParams", self.CP.to_bytes())
# fake plan
self.plan_ts = 0
self.plan = log.Plan.new_message()
self.plan.lateralValid = False
self.plan.longitudinalValid = False
# controls enabled state
self.enabled = False
self.last_enable_request = 0
# learned angle offset
self.angle_offset = 0
calibration_params = params.get("CalibrationParams")
if calibration_params:
try:
calibration_params = json.loads(calibration_params)
self.angle_offset = calibration_params["angle_offset"]
except (ValueError, KeyError):
pass
# rear view camera state
self.rear_view_toggle = False
self.rear_view_allowed = (params.get("IsRearViewMirror") == "1")
self.v_cruise_kph = 255
# 0.0 - 1.0
self.awareness_status = 1.0
self.soft_disable_timer = None
self.overtemp = False
self.free_space = 1.0
self.cal_status = Calibration.UNCALIBRATED
self.cal_perc = 0
self.rk = Ratekeeper(self.rate, print_delay_threshold=2. / 1000)
def __init__(self, sm=None, pm=None, can_sock=None):
config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH)
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster([
'sendcan', 'controlsState', 'carState', 'carControl',
'carEvents', 'carParams'
])
self.sm = sm
if self.sm is None:
ignore = ['driverCameraState', 'managerState'
] if SIMULATION else None
self.sm = messaging.SubMaster([
'deviceState', 'pandaState', 'modelV2', 'liveCalibration',
'driverMonitoringState', 'longitudinalPlan', 'lateralPlan',
'liveLocationKalman', 'roadCameraState', 'driverCameraState',
'managerState', 'liveParameters', 'radarState'
],
ignore_alive=ignore)
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT',
False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
# wait for one pandaState and one CAN packet
hw_type = messaging.recv_one(
self.sm.sock['pandaState']).pandaState.pandaType
has_relay = hw_type in [
PandaType.blackPanda, PandaType.uno, PandaType.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_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
# scc smoother
self.is_cruise_enabled = False
self.cruiseVirtualMaxSpeed = 0
self.clu_speed_ms = 0.
self.apply_accel = 0.
self.fused_accel = 0.
self.lead_drel = 0.
self.aReqValue = 0.
self.aReqValueMin = 0.
self.aReqValueMax = 0.
self.angle_steers_des = 0.
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, hw_type)
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 regen_segment(lr, frs=None, outdir=FAKEDATA):
lr = list(lr)
if frs is None:
frs = dict()
# setup env
params = Params()
params.clear_all()
params.put_bool("Passive", False)
params.put_bool("OpenpilotEnabledToggle", True)
params.put_bool("CommunityFeaturesToggle", True)
params.put_bool("CommunityFeaturesToggle", True)
cal = messaging.new_message('liveCalibration')
cal.liveCalibration.validBlocks = 20
cal.liveCalibration.rpyCalib = [0.0, 0.0, 0.0]
params.put("CalibrationParams", cal.to_bytes())
os.environ["LOG_ROOT"] = outdir
os.environ["SIMULATION"] = "1"
os.environ['SKIP_FW_QUERY'] = ""
os.environ['FINGERPRINT'] = ""
for msg in lr:
if msg.which() == 'carParams':
car_fingerprint = 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
#TODO: init car, make sure starts engaged when segment is engaged
fake_daemons = {
'sensord': [
multiprocessing.Process(target=replay_service,
args=('sensorEvents', lr)),
],
'pandad': [
multiprocessing.Process(target=replay_service, args=('can', lr)),
multiprocessing.Process(target=replay_service,
args=('pandaStates', lr)),
],
#'managerState': [
# multiprocessing.Process(target=replay_service, args=('managerState', lr)),
#],
'thermald': [
multiprocessing.Process(target=replay_service,
args=('deviceState', lr)),
],
'camerad': [
*replay_cameras(lr, frs),
],
# TODO: fix these and run them
'paramsd': [
multiprocessing.Process(target=replay_service,
args=('liveParameters', lr)),
],
'locationd': [
multiprocessing.Process(target=replay_service,
args=('liveLocationKalman', lr)),
],
}
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()
r = params.get("CurrentRoute", encoding='utf-8')
return os.path.join(outdir, r + "--0")
def register():
params = Params()
params.put("Version", version)
params.put("TrainingVersion", training_version)
params.put("GitCommit", get_git_commit())
params.put("GitBranch", get_git_branch())
params.put("GitRemote", get_git_remote())
dongle_id, access_token = params.get("DongleId"), params.get("AccessToken")
try:
if dongle_id is None or access_token is None:
cloudlog.info("getting pilotauth")
resp = api_get("v1/pilotauth/",
method='POST',
timeout=15,
imei=get_imei(),
serial=get_serial())
dongleauth = json.loads(resp.text)
dongle_id, access_token = dongleauth["dongle_id"].encode(
'ascii'), dongleauth["access_token"].encode('ascii')
params.put("DongleId", dongle_id)
params.put("AccessToken", access_token)
return dongle_id, access_token
except Exception:
cloudlog.exception("failed to authenticate")
return None
def manager_init():
params = Params()
params.manager_start()
default_params = [
("CommunityFeaturesToggle", "0"),
("EndToEndToggle", "0"),
("CompletedTrainingVersion", "0"),
("IsRHD", "0"),
("IsMetric", "1"),
("RecordFront", "0"),
("HasAcceptedTerms", "0"),
("HasCompletedSetup", "0"),
("IsUploadRawEnabled", "1"),
("IsLdwEnabled", "0"),
("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')),
("OpenpilotEnabledToggle", "1"),
("VisionRadarToggle", "0"),
("IsDriverViewEnabled", "0"),
("IsOpenpilotViewEnabled", "0"),
("OpkrAutoShutdown", "2"),
("OpkrAutoScreenDimming", "0"),
("OpkrUIBrightness", "0"),
("OpkrUIBrightness", "0"),
("OpkrUIVolumeBoost", "0"),
("OpkrEnableDriverMonitoring", "1"),
("OpkrEnableLogger", "0"),
("OpkrEnableGetoffAlert", "1"),
("OpkrAutoResume", "1"),
("OpkrVariableCruise", "1"),
("OpkrLaneChangeSpeed", "45"),
("OpkrAutoLaneChangeDelay", "0"),
("OpkrSteerAngleCorrection", "0"),
("PutPrebuiltOn", "0"),
("FingerprintIssuedFix", "0"),
("LdwsCarFix", "0"),
("LateralControlMethod", "0"),
("CruiseStatemodeSelInit", "1"),
("InnerLoopGain", "35"),
("OuterLoopGain", "20"),
("TimeConstant", "14"),
("ActuatorEffectiveness", "20"),
("Scale", "1750"),
("LqrKi", "10"),
("DcGain", "30"),
("IgnoreZone", "1"),
("PidKp", "20"),
("PidKi", "40"),
("PidKd", "150"),
("PidKf", "5"),
("CameraOffsetAdj", "60"),
("SteerRatioAdj", "150"),
("SteerRatioMaxAdj", "180"),
("SteerActuatorDelayAdj", "0"),
("SteerRateCostAdj", "45"),
("SteerLimitTimerAdj", "40"),
("TireStiffnessFactorAdj", "85"),
("SteerMaxBaseAdj", "300"),
("SteerMaxAdj", "384"),
("SteerDeltaUpBaseAdj", "3"),
("SteerDeltaUpAdj", "3"),
("SteerDeltaDownBaseAdj", "7"),
("SteerDeltaDownAdj", "7"),
("SteerMaxvAdj", "10"),
("OpkrBatteryChargingControl", "1"),
("OpkrBatteryChargingMin", "70"),
("OpkrBatteryChargingMax", "80"),
("LeftCurvOffsetAdj", "0"),
("RightCurvOffsetAdj", "0"),
("DebugUi1", "0"),
("DebugUi2", "0"),
("OpkrBlindSpotDetect", "1"),
("OpkrMaxAngleLimit", "90"),
("OpkrSpeedLimitOffset", "0"),
("LimitSetSpeedCamera", "0"),
("LimitSetSpeedCameraDist", "0"),
("OpkrLiveSteerRatio", "1"),
("OpkrVariableSteerMax", "1"),
("OpkrVariableSteerDelta", "0"),
("FingerprintTwoSet", "1"),
("OpkrVariableCruiseProfile", "0"),
("OpkrLiveTune", "0"),
("OpkrDrivingRecord", "0"),
("OpkrTurnSteeringDisable", "0"),
("CarModel", ""),
("OpkrHotspotOnBoot", "0"),
("OpkrSSHLegacy", "1"),
("ShaneFeedForward", "0"),
("CruiseOverMaxSpeed", "0"),
("OpkrMapDecelOnly", "0"),
("JustDoGearD", "0"),
("LanelessMode", "0"),
("ComIssueGone", "0"),
("MaxSteer", "384"),
("MaxRTDelta", "112"),
("MaxRateUp", "3"),
("MaxRateDown", "7"),
("SteerThreshold", "150"),
]
if params.get("RecordFrontLock", encoding='utf-8') == "1":
params.put("RecordFront", "1")
# 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("Passive", str(int(os.getenv("PASSIVE"))))
if params.get("Passive") is None:
raise Exception("Passive must be set to continue")
if EON:
update_apks()
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())
# ensure shared libraries are readable by apks
if EON:
os.chmod(BASEDIR, 0o755)
os.chmod("/dev/shm", 0o777)
os.chmod(os.path.join(BASEDIR, "cereal"), 0o755)
os.chmod(os.path.join(BASEDIR, "cereal", "libmessaging_shared.so"), 0o755)
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 TICI:
default_params.append(("IsUploadRawEnabled", "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 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())
class LatControlPID(object):
def __init__(self, CP):
kegman_conf(CP)
self.frame = 0
self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
k_f=CP.lateralTuning.pid.kf, pos_limit=1.0)
self.angle_steers_des = 0.
self.total_poly_projection = max(0.0, CP.lateralTuning.pid.polyReactTime + CP.lateralTuning.pid.polyDampTime)
self.poly_smoothing = max(1.0, CP.lateralTuning.pid.polyDampTime * 100.)
self.poly_scale = CP.lateralTuning.pid.polyScale
self.poly_factor = CP.lateralTuning.pid.polyFactor
self.poly_scale = CP.lateralTuning.pid.polyScale
self.path_error = 0.0
self.cur_poly_scale = 0.0
self.p_poly = [0., 0., 0., 0.]
self.s_poly = [0., 0., 0., 0.]
self.p_prob = 0.
self.damp_angle_steers = 0.
self.damp_time = 0.1
self.react_mpc = 0.0
self.damp_mpc = 0.25
self.angle_ff_ratio = 0.0
self.gernbySteer = True
self.standard_ff_ratio = 0.0
self.angle_ff_gain = 1.0
self.rate_ff_gain = CP.lateralTuning.pid.rateFFGain
self.angle_ff_bp = [[0.5, 5.0],[0.0, 1.0]]
self.steer_p_scale = CP.lateralTuning.pid.steerPscale
self.calculate_rate = True
self.prev_angle_steers = 0.0
self.rough_steers_rate = 0.0
self.steer_counter = 1
self.lane_change_adjustment = 0.0
self.lane_changing = 0.0
self.starting_angle = 0.0
self.half_lane_width = 0.0
self.steer_counter_prev = 1
self.params = Params()
self.prev_override = False
self.driver_assist_offset = 0.0
self.driver_assist_hold = False
self.angle_bias = 0.
try:
lateral_params = self.params.get("LateralParams")
lateral_params = json.loads(lateral_params)
self.angle_ff_gain = max(1.0, float(lateral_params['angle_ff_gain']))
except:
self.angle_ff_gain = 1.0
def live_tune(self, CP):
self.frame += 1
if self.frame % 3600 == 0:
self.params.put("LateralParams", json.dumps({'angle_ff_gain': self.angle_ff_gain}))
if self.frame % 300 == 0:
# live tuning through /data/openpilot/tune.py overrides interface.py settings
kegman = kegman_conf()
self.pid._k_i = ([0.], [float(kegman.conf['Ki'])])
self.pid._k_p = ([0.], [float(kegman.conf['Kp'])])
self.pid.k_f = (float(kegman.conf['Kf']))
self.damp_time = (float(kegman.conf['dampTime']))
self.react_mpc = (float(kegman.conf['reactMPC']))
self.damp_mpc = (float(kegman.conf['dampMPC']))
self.total_poly_projection = max(0.0, float(kegman.conf['polyReact']) + float(kegman.conf['polyDamp']))
self.poly_smoothing = max(1.0, float(kegman.conf['polyDamp']) * 100.)
self.poly_factor = float(kegman.conf['polyFactor'])
def get_projected_path_error(self, v_ego, angle_feedforward, angle_steers, live_params, path_plan, VM):
curv_factor = interp(abs(angle_feedforward), [1.0, 5.0], [0.0, 1.0])
self.p_poly[3] += (path_plan.pPoly[3] - self.p_poly[3]) / self.poly_smoothing
self.p_poly[2] += curv_factor * (path_plan.pPoly[2] - self.p_poly[2]) / (self.poly_smoothing * 1.5)
self.p_poly[1] += curv_factor * (path_plan.pPoly[1] - self.p_poly[1]) / (self.poly_smoothing * 3.0)
self.p_poly[0] += curv_factor * (path_plan.pPoly[0] - self.p_poly[0]) / (self.poly_smoothing * 4.5)
self.p_prob += (path_plan.pProb - self. p_prob) / (self.poly_smoothing)
self.s_poly[1] = float(np.tan(VM.calc_curvature(np.radians(angle_steers - live_params.angleOffset), float(v_ego))))
x = int(float(v_ego) * self.total_poly_projection * interp(abs(angle_feedforward), [0., 5.], [0.25, 1.0]))
self.p_pts = np.polyval(self.p_poly, np.arange(0, x))
self.s_pts = np.polyval(self.s_poly, np.arange(0, x))
return self.p_prob * (np.sum(self.p_pts) - np.sum(self.s_pts))
def reset(self):
self.pid.reset()
def adjust_angle_gain(self):
if (self.pid.f > 0) == (self.pid.i > 0) and abs(self.pid.i) >= abs(self.previous_integral):
if not abs(self.pid.f + self.pid.i + self.pid.p) > 1: self.angle_ff_gain *= 1.0001
elif self.angle_ff_gain > 1.0:
self.angle_ff_gain *= 0.9999
self.previous_integral = self.pid.i
def update_lane_state(self, angle_steers, driver_opposing_lane, blinkers_on, path_plan):
if self.lane_changing > 0.0:
if self.lane_changing > 2.75 or (not blinkers_on and self.lane_changing < 1.0 and abs(path_plan.cPoly[3]) < 0.5 and min(abs(self.starting_angle - angle_steers), abs(self.angle_steers_des - angle_steers)) < 1.5):
self.lane_changing = 0.0
elif 2.25 <= self.lane_changing < 2.5 and abs(path_plan.lPoly[3] + path_plan.rPoly[3]) < abs(path_plan.cPoly[3]):
self.lane_changing = 2.5
elif 2.0 <= self.lane_changing < 2.25 and (path_plan.lPoly[3] + path_plan.rPoly[3]) * path_plan.cPoly[3] < 0:
self.lane_changing = 2.25
elif self.lane_changing < 2.0 and self.half_lane_width < 1.05 * abs(path_plan.lPoly[3] + path_plan.rPoly[3]):
self.lane_changing = 2.0
else:
self.lane_changing = max(self.lane_changing + 0.01, abs(path_plan.lPoly[3] + path_plan.rPoly[3]))
if blinkers_on:
self.lane_change_adjustment = 0.0
else:
self.lane_change_adjustment = interp(self.lane_changing, [0.0, 1.0, 2.0, 2.25, 2.5, 2.75], [1.0, 0.0, 0.0, 0.1, .2, 1.0])
#print("%0.2f lane_changing %0.2f adjustment %0.2f p_poly %0.2f avg_poly" % (self.lane_changing, self.lane_change_adjustment, path_plan.cPoly[3], path_plan.lPoly[3] + path_plan.rPoly[3]))
elif driver_opposing_lane and (blinkers_on or abs(path_plan.cPoly[3]) > 0.5 or min(abs(self.starting_angle - angle_steers), abs(self.angle_steers_des - angle_steers)) > 1.5):
self.lane_changing = 0.01
else:
self.half_lane_width = (path_plan.lPoly[3] - path_plan.rPoly[3]) / 2.
self.starting_angle = angle_steers
self.lane_change_adjustment = 1.0
def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, blinkers_on, CP, VM, path_plan, live_params):
if angle_steers_rate == 0.0 and self.calculate_rate:
if angle_steers != self.prev_angle_steers:
self.steer_counter_prev = self.steer_counter
self.rough_steers_rate = (self.rough_steers_rate + 100.0 * (angle_steers - self.prev_angle_steers) / self.steer_counter_prev) / 2.0
self.steer_counter = 0.0
elif self.steer_counter >= self.steer_counter_prev:
self.rough_steers_rate = (self.steer_counter * self.rough_steers_rate) / (self.steer_counter + 1.0)
self.steer_counter += 1.0
angle_steers_rate = self.rough_steers_rate
else:
# If non-zero angle_rate is provided, stop calculating angle rate
self.calculate_rate = False
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steerAngle = float(angle_steers)
pid_log.steerRate = float(angle_steers_rate)
max_bias_change = 0.0002 / (abs(self.angle_bias) + 0.0001)
self.angle_bias = float(clip(live_params.angleOffset - live_params.angleOffsetAverage, self.angle_bias - max_bias_change, self.angle_bias + max_bias_change))
self.live_tune(CP)
if v_ego < 0.3 or not active:
output_steer = 0.0
self.lane_changing = 0.0
self.previous_integral = 0.0
self.damp_angle_steers= 0.0
self.damp_rate_steers_des = 0.0
self.damp_angle_steers_des = 0.0
pid_log.active = False
self.pid.reset()
else:
self.angle_steers_des = path_plan.angleSteers
if not self.driver_assist_hold:
self.damp_angle_steers_des += (interp(sec_since_boot() + self.damp_mpc + self.react_mpc, path_plan.mpcTimes, path_plan.mpcAngles) - self.damp_angle_steers_des) / max(1.0, self.damp_mpc * 100.)
self.damp_rate_steers_des += (interp(sec_since_boot() + self.damp_mpc + self.react_mpc, path_plan.mpcTimes, path_plan.mpcRates) - self.damp_rate_steers_des) / max(1.0, self.damp_mpc * 100.)
self.damp_angle_steers += (angle_steers - self.angle_bias + self.damp_time * angle_steers_rate - self.damp_angle_steers) / max(1.0, self.damp_time * 100.)
else:
self.damp_angle_steers = angle_steers
self.damp_angle_steers_des = self.damp_angle_steers + self.driver_assist_offset
if steer_override and abs(self.damp_angle_steers) > abs(self.damp_angle_steers_des) and self.pid.saturated:
self.damp_angle_steers_des = self.damp_angle_steers
steers_max = get_steer_max(CP, v_ego)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
angle_feedforward = float(self.damp_angle_steers_des - path_plan.angleOffset)
self.angle_ff_ratio = interp(abs(angle_feedforward), self.angle_ff_bp[0], self.angle_ff_bp[1])
rate_feedforward = (1.0 - self.angle_ff_ratio) * self.rate_ff_gain * self.damp_rate_steers_des
steer_feedforward = float(v_ego)**2 * (rate_feedforward + angle_feedforward * self.angle_ff_ratio * self.angle_ff_gain)
if len(self.poly_scale) > 0:
if abs(self.damp_angle_steers_des) > abs(self.damp_angle_steers):
self.cur_poly_scale += 0.05 * (interp(abs(self.damp_rate_steers_des), self.poly_scale[0], self.poly_scale[1]) - self.cur_poly_scale)
else:
self.cur_poly_scale += 0.05 * (interp(abs(self.damp_rate_steers_des), self.poly_scale[0], self.poly_scale[2]) - self.cur_poly_scale)
else:
self.cur_poly_scale = 1.0
if len(self.steer_p_scale) > 0:
if abs(self.damp_angle_steers_des) > abs(self.damp_angle_steers):
p_scale = interp(abs(angle_feedforward), self.steer_p_scale[0], self.steer_p_scale[1])
else:
p_scale = interp(abs(angle_feedforward), self.steer_p_scale[0], self.steer_p_scale[2])
else:
p_scale = 1.0
if CP.carName == "honda" and steer_override and not self.prev_override and not self.driver_assist_hold and self.pid.saturated and abs(angle_steers) < abs(self.damp_angle_steers_des) and not blinkers_on:
self.driver_assist_hold = True
self.driver_assist_offset = self.damp_angle_steers_des - self.damp_angle_steers
else:
self.driver_assist_hold = steer_override and self.driver_assist_hold
self.path_error = float(v_ego) * float(self.get_projected_path_error(v_ego, angle_feedforward, angle_steers, live_params, path_plan, VM)) * self.poly_factor * self.cur_poly_scale * self.angle_ff_gain
if self.driver_assist_hold and not steer_override and abs(angle_steers) > abs(self.damp_angle_steers_des):
#self.angle_bias = 0.0
driver_opposing_i = False
elif (steer_override and self.pid.saturated) or self.driver_assist_hold or self.lane_changing > 0.0 or blinkers_on:
#self.angle_bias = 0.0
self.path_error = 0.0
if self.gernbySteer and not steer_override and v_ego > 10.0:
if abs(angle_steers) > (self.angle_ff_bp[0][1] / 2.0):
self.adjust_angle_gain()
else:
self.previous_integral = self.pid.i
driver_opposing_i = steer_override and self.pid.i * self.pid.p > 0 and not self.pid.saturated and not self.driver_assist_hold
deadzone = 0.0
output_steer = self.pid.update(self.damp_angle_steers_des, self.damp_angle_steers, check_saturation=(v_ego > 10), override=driver_opposing_i,
add_error=float(self.path_error), feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone, p_scale=p_scale)
driver_opposing_op = steer_override and (angle_steers - self.prev_angle_steers) * output_steer < 0
self.update_lane_state(angle_steers, driver_opposing_op, blinkers_on, path_plan)
output_steer *= self.lane_change_adjustment
pid_log.active = True
pid_log.p = float(self.pid.p)
pid_log.i = float(self.pid.i)
pid_log.f = float(self.pid.f)
pid_log.p2 = float(self.pid.p2)
pid_log.output = float(output_steer)
pid_log.saturated = bool(self.pid.saturated)
pid_log.angleFFRatio = self.angle_ff_ratio
pid_log.angleBias = self.angle_bias
self.prev_angle_steers = angle_steers
self.prev_override = steer_override
self.sat_flag = self.pid.saturated
return output_steer, float(self.angle_steers_des), pid_log
def register():
params = 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=""))
params.put("SubscriberInfo", get_subscriber_info())
# create a key for auth
# your private key is kept on your device persist partition and never sent to our servers
# do not erase your persist partition
if not os.path.isfile(PERSIST + "/comma/id_rsa.pub"):
cloudlog.warning("generating your personal RSA key")
mkdirs_exists_ok(PERSIST + "/comma")
assert os.system("openssl genrsa -out " + PERSIST +
"/comma/id_rsa.tmp 2048") == 0
assert os.system("openssl rsa -in " + PERSIST +
"/comma/id_rsa.tmp -pubout -out " + PERSIST +
"/comma/id_rsa.tmp.pub") == 0
os.rename(PERSIST + "/comma/id_rsa.tmp", PERSIST + "/comma/id_rsa")
os.rename(PERSIST + "/comma/id_rsa.tmp.pub",
PERSIST + "/comma/id_rsa.pub")
# make key readable by app users (ai.comma.plus.offroad)
os.chmod(PERSIST + '/comma/', 0o755)
os.chmod(PERSIST + '/comma/id_rsa', 0o744)
dongle_id, access_token = params.get(
"DongleId", encoding='utf8'), params.get("AccessToken",
encoding='utf8')
public_key = open(PERSIST + "/comma/id_rsa.pub").read()
# create registration token
# in the future, this key will make JWTs directly
private_key = open(PERSIST + "/comma/id_rsa").read()
# late import
import jwt
register_token = jwt.encode(
{
'register': True,
'exp': datetime.utcnow() + timedelta(hours=1)
},
private_key,
algorithm='RS256')
try:
cloudlog.info("getting pilotauth")
resp = api_get("v2/pilotauth/",
method='POST',
timeout=15,
imei=get_imei(0),
imei2=get_imei(1),
serial=get_serial(),
public_key=public_key,
register_token=register_token)
dongleauth = json.loads(resp.text)
dongle_id, access_token = dongleauth["dongle_id"], dongleauth[
"access_token"]
params.put("DongleId", dongle_id)
params.put("AccessToken", access_token)
return dongle_id, access_token
except Exception:
cloudlog.exception("failed to authenticate")
if dongle_id is not None and access_token is not None:
return dongle_id, access_token
else:
return None
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':
os.environ['FINGERPRINT'] = msg.carParams.carFingerprint
manager.prepare_managed_process(cfg.proc_name)
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
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.just_calibrated = False
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):
start_status = self.cal_status
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
end_status = self.cal_status
self.just_calibrated = False
if start_status == Calibration.UNCALIBRATED and end_status == Calibration.CALIBRATED:
self.just_calibrated = True
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
or self.just_calibrated):
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 main():
update_failed_count = 0
overlay_init_done = False
wait_helper = WaitTimeHelper()
params = Params()
carSettings = CarSettings()
doAutoUpdate = carSettings.doAutoUpdate
if not os.geteuid() == 0:
raise RuntimeError("updated must be launched as root!")
# Set low io priority
p = psutil.Process()
if psutil.LINUX:
p.ionice(psutil.IOPRIO_CLASS_BE, value=7)
ov_lock_fd = open('/tmp/safe_staging_overlay.lock', 'w')
try:
fcntl.flock(ov_lock_fd, fcntl.LOCK_EX | fcntl.LOCK_NB)
except IOError:
raise RuntimeError("couldn't get overlay lock; is another updated running?")
while True:
update_failed_count += 1
time_wrong = datetime.datetime.now().year < 2019
ping_failed = subprocess.call(["ping", "-W", "4", "-c", "1", "8.8.8.8"])
if doAutoUpdate:
# Wait until we have a valid datetime to initialize the overlay
if not (ping_failed or time_wrong):
try:
# If the git directory has modifcations after we created the overlay
# we need to recreate the overlay
if overlay_init_done:
overlay_init_fn = os.path.join(BASEDIR, ".overlay_init")
git_dir_path = os.path.join(BASEDIR, ".git")
new_files = run(["find", git_dir_path, "-newer", overlay_init_fn])
if len(new_files.splitlines()):
cloudlog.info(".git directory changed, recreating overlay")
overlay_init_done = False
if not overlay_init_done:
init_ovfs()
overlay_init_done = True
if params.get("IsOffroad") == b"1":
attempt_update()
update_failed_count = 0
else:
cloudlog.info("not running updater, openpilot running")
except subprocess.CalledProcessError as e:
cloudlog.event(
"update process failed",
cmd=e.cmd,
output=e.output,
returncode=e.returncode
)
overlay_init_done = False
except Exception:
cloudlog.exception("uncaught updated exception, shouldn't happen")
params.put("UpdateFailedCount", str(update_failed_count))
wait_between_updates(wait_helper.ready_event)
if wait_helper.shutdown:
break
# We've been signaled to shut down
dismount_ovfs()
print("frame: ", "engaged:", is_openpilot_engaged, "; throttle: ",
round(vc.throttle, 3), "; steer(c/deg): ",
round(vc.steer, 3), round(steer_out, 3), "; brake: ",
round(vc.brake, 3))
rk.keep_time()
if __name__ == "__main__":
# make sure params are in a good state
params = Params()
params.clear_all()
set_params_enabled()
params.delete("Offroad_ConnectivityNeeded")
params.put("CalibrationParams",
'{"calib_radians": [0,0,0], "valid_blocks": 20}')
from multiprocessing import Process, Queue
q = Queue()
p = Process(target=go, args=(q, ))
p.daemon = True
p.start()
if args.joystick:
# start input poll for joystick
from lib.manual_ctrl import wheel_poll_thread
wheel_poll_thread(q)
else:
# start input poll for keyboard
from lib.keyboard_ctrl import keyboard_poll_thread
keyboard_poll_thread(q)
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")
init_params_vals(params, put_nonblocking)
update_params_vals(params)
if ANDROID:
update_apks()
manager_init(params.get('dp_reg') == b'1')
manager_prepare(spinner)
spinner.close()
if os.getenv("PREPAREONLY") is not None:
return
# dp
del managed_processes['tombstoned']
if params.get("dp_logger") == b'0':
del managed_processes['loggerd']
del managed_processes['logmessaged']
del managed_processes['proclogd']
del managed_processes['logcatd']
del managed_processes['deleter']
if params.get("dp_uploader") == b'0' or \
params.get("dp_atl") == b'1' or \
params.get("dp_steering_monitor") == b'0':
del managed_processes['uploader']
if params.get("dp_updated") == b'0':
del managed_processes['updated']
# 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()
if car_model not in non_tested_cars:
print("TEST FAILED: Missing route for car '%s'" % car_model)
sys.exit(1)
print("Preparing processes")
for p in tested_procs:
manager.prepare_managed_process(p)
results = {}
for route, checks in routes.items():
get_route_log(route)
params = Params()
params.clear_all()
params.manager_start()
params.put("OpenpilotEnabledToggle", "1")
params.put("CommunityFeaturesToggle", "1")
params.put("Passive", "1" if route in passive_routes else "0")
os.environ['SKIP_FW_QUERY'] = "1"
if checks.get('fingerprintSource', None) == 'fixed':
os.environ['FINGERPRINT'] = checks['carFingerprint']
else:
os.environ['FINGERPRINT'] = ""
print("testing ", route, " ", checks['carFingerprint'])
print("Starting processes")
for p in tested_procs:
manager.start_managed_process(p)
# Start unlogger
def main():
# 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')
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['plannerd']
del managed_processes['radard']
# support additional internal only extensions
try:
import selfdrive.manager_extensions
selfdrive.manager_extensions.register(register_managed_process) # pylint: disable=no-member
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("HasAcceptedTerms") is None:
params.put("HasAcceptedTerms", "0")
if params.get("IsUploadRawEnabled") is None:
params.put("IsUploadRawEnabled", "1")
if params.get("IsUploadVideoOverCellularEnabled") is None:
params.put("IsUploadVideoOverCellularEnabled", "1")
if params.get("IsGeofenceEnabled") is None:
params.put("IsGeofenceEnabled", "-1")
if params.get("SpeedLimitOffset") is None:
params.put("SpeedLimitOffset", "0")
if params.get("LongitudinalControl") is None:
params.put("LongitudinalControl", "0")
if params.get("LimitSetSpeed") is None:
params.put("LimitSetSpeed", "0")
if params.get("LimitSetSpeedNeural") is None:
params.put("LimitSetSpeedNeural", "0")
# 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 register(spinner=None):
params = Params()
params.put("Version", "kegman-ultimate-0.001")
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=""))
params.put("SubscriberInfo", HARDWARE.get_subscriber_info())
IMEI = params.get("IMEI", encoding='utf8')
HardwareSerial = params.get("HardwareSerial", encoding='utf8')
needs_registration = (None in [IMEI, HardwareSerial])
# create a key for auth
# your private key is kept on your device persist partition and never sent to our servers
# do not erase your persist partition
if not os.path.isfile(PERSIST + "/comma/id_rsa.pub"):
needs_registration = True
cloudlog.warning("generating your personal RSA key")
mkdirs_exists_ok(PERSIST + "/comma")
assert os.system("openssl genrsa -out " + PERSIST +
"/comma/id_rsa.tmp 2048") == 0
assert os.system("openssl rsa -in " + PERSIST +
"/comma/id_rsa.tmp -pubout -out " + PERSIST +
"/comma/id_rsa.tmp.pub") == 0
os.rename(PERSIST + "/comma/id_rsa.tmp", PERSIST + "/comma/id_rsa")
os.rename(PERSIST + "/comma/id_rsa.tmp.pub",
PERSIST + "/comma/id_rsa.pub")
# make key readable by app users (ai.comma.plus.offroad)
os.chmod(PERSIST + '/comma/', 0o755)
os.chmod(PERSIST + '/comma/id_rsa', 0o744)
dongle_id = params.get("DongleId", encoding='utf8')
needs_registration = needs_registration or dongle_id is None
if needs_registration:
if spinner is not None:
spinner.update("registering device")
# Create registration token, in the future, this key will make JWTs directly
private_key = open(PERSIST + "/comma/id_rsa").read()
public_key = open(PERSIST + "/comma/id_rsa.pub").read()
register_token = jwt.encode(
{
'register': True,
'exp': datetime.utcnow() + timedelta(hours=1)
},
private_key,
algorithm='RS256')
# Block until we get the imei
imei1, imei2 = None, None
while imei1 is None and imei2 is None:
try:
imei1, imei2 = HARDWARE.get_imei(0), HARDWARE.get_imei(1)
except Exception:
cloudlog.exception("Error getting imei, trying again...")
time.sleep(1)
serial = HARDWARE.get_serial()
params.put("IMEI", imei1)
params.put("HardwareSerial", serial)
while True:
try:
cloudlog.info("getting pilotauth")
resp = api_get("v2/pilotauth/",
method='POST',
timeout=15,
imei=imei1,
imei2=imei2,
serial=serial,
public_key=public_key,
register_token=register_token)
dongleauth = json.loads(resp.text)
dongle_id = dongleauth["dongle_id"]
params.put("DongleId", dongle_id)
break
except Exception:
cloudlog.exception("failed to authenticate")
time.sleep(1)
return dongle_id
def set_params(new_version: bool, failed_count: int,
exception: Optional[str]) -> None:
params = Params()
params.put("UpdateFailedCount", str(failed_count))
last_update = datetime.datetime.utcnow()
if failed_count == 0:
t = last_update.isoformat()
params.put("LastUpdateTime", t.encode('utf8'))
else:
try:
t = params.get("LastUpdateTime", encoding='utf8')
last_update = datetime.datetime.fromisoformat(t)
except (TypeError, ValueError):
pass
if exception is None:
params.delete("LastUpdateException")
else:
params.put("LastUpdateException", exception)
# Write out release notes for new versions
if new_version:
try:
with open(os.path.join(FINALIZED, "RELEASES.md"), "rb") as f:
r = f.read().split(b'\n\n', 1)[0] # Slice latest release notes
try:
params.put("ReleaseNotes", parse_markdown(r.decode("utf-8")))
except Exception:
params.put("ReleaseNotes", r + b"\n")
except Exception:
params.put("ReleaseNotes", "")
params.put_bool("UpdateAvailable", True)
# Handle user prompt
for alert in ("Offroad_UpdateFailed", "Offroad_ConnectivityNeeded",
"Offroad_ConnectivityNeededPrompt"):
set_offroad_alert(alert, False)
now = datetime.datetime.utcnow()
dt = now - last_update
if failed_count > 15 and exception is not None:
if get_tested_branch():
extra_text = "Ensure the software is correctly installed"
else:
extra_text = exception
set_offroad_alert("Offroad_UpdateFailed", True, extra_text=extra_text)
elif dt.days > DAYS_NO_CONNECTIVITY_MAX and failed_count > 1:
set_offroad_alert("Offroad_ConnectivityNeeded", True)
elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT:
remaining = max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 1)
set_offroad_alert(
"Offroad_ConnectivityNeededPrompt",
True,
extra_text=f"{remaining} day{'' if remaining == 1 else 's'}.")
def main():
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", "1"),
("CompletedTrainingVersion", "0"),
("IsRHD", "0"),
("IsMetric", "1"),
("RecordFront", "0"),
("HasAcceptedTerms", "0"),
("HasCompletedSetup", "0"),
("IsUploadRawEnabled", "1"),
("IsLdwEnabled", "1"),
("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 init_overlay() -> None:
overlay_init_file = Path(os.path.join(BASEDIR, ".overlay_init"))
# Re-create the overlay if BASEDIR/.git has changed since we created the overlay
if overlay_init_file.is_file():
git_dir_path = os.path.join(BASEDIR, ".git")
new_files = run(
["find", git_dir_path, "-newer",
str(overlay_init_file)])
if not len(new_files.splitlines()):
# A valid overlay already exists
return
else:
cloudlog.info(".git directory changed, recreating overlay")
cloudlog.info("preparing new safe staging area")
params = Params()
params.put_bool("UpdateAvailable", False)
set_consistent_flag(False)
dismount_overlay()
if TICI:
run(["sudo", "rm", "-rf", STAGING_ROOT])
if os.path.isdir(STAGING_ROOT):
shutil.rmtree(STAGING_ROOT)
for dirname in [
STAGING_ROOT, OVERLAY_UPPER, OVERLAY_METADATA, OVERLAY_MERGED
]:
os.mkdir(dirname, 0o755)
if os.lstat(BASEDIR).st_dev != os.lstat(OVERLAY_MERGED).st_dev:
raise RuntimeError(
"base and overlay merge directories are on different filesystems; not valid for overlay FS!"
)
# Leave a timestamped canary in BASEDIR to check at startup. The device clock
# should be correct by the time we get here. If the init file disappears, or
# critical mtimes in BASEDIR are newer than .overlay_init, continue.sh can
# assume that BASEDIR has used for local development or otherwise modified,
# and skips the update activation attempt.
consistent_file = Path(os.path.join(BASEDIR, ".overlay_consistent"))
if consistent_file.is_file():
consistent_file.unlink()
overlay_init_file.touch()
os.sync()
overlay_opts = f"lowerdir={BASEDIR},upperdir={OVERLAY_UPPER},workdir={OVERLAY_METADATA}"
mount_cmd = [
"mount", "-t", "overlay", "-o", overlay_opts, "none", OVERLAY_MERGED
]
if TICI:
run(["sudo"] + mount_cmd)
run(["sudo", "chmod", "755", os.path.join(OVERLAY_METADATA, "work")])
else:
run(mount_cmd)
git_diff = run(["git", "diff"], OVERLAY_MERGED, low_priority=True)
params.put("GitDiff", git_diff)
cloudlog.info(f"git diff output:\n{git_diff}")
def thermald_thread():
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')
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
current_branch = get_git_branch()
network_type = NetworkType.none
network_strength = NetworkStrength.unknown
wifiIpAddress = 'N/A'
current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML)
cpu_temp_filter = FirstOrderFilter(0., CPU_TEMP_TAU, DT_TRML)
health_prev = None
should_start_prev = False
handle_fan = None
is_uno = False
params = Params()
pm = PowerMonitoring()
no_panda_cnt = 0
thermal_config = get_thermal_config()
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(thermal_config)
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 startup_conditions["ignition"]:
cloudlog.error("Lost panda connection while onroad")
startup_conditions["ignition"] = False
else:
no_panda_cnt = 0
startup_conditions[
"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
if (not EON) or is_uno:
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 = HARDWARE.get_network_type()
network_strength = HARDWARE.get_network_strength(network_type)
wifiIpAddress = HARDWARE.get_ip_address()
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.wifiIpAddress = wifiIpAddress
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 (not EON) or is_uno:
msg.thermal.batteryPercent = 100
msg.thermal.batteryStatus = "Charging"
msg.thermal.bat = 0
current_filter.update(msg.thermal.batteryCurrent / 1e6)
# TODO: add car battery voltage check
max_cpu_temp = cpu_temp_filter.update(max(msg.thermal.cpu))
max_comp_temp = max(max_cpu_temp, msg.thermal.mem,
max(msg.thermal.gpu))
bat_temp = msg.thermal.bat
if handle_fan is not None:
fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed,
startup_conditions["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
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 max_cpu_temp > 107. or bat_temp >= 63. or (is_offroad_for_5_min
and max_cpu_temp > 70.0):
# onroad not allowed
thermal_status = ThermalStatus.danger
elif max_comp_temp > 96.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 > 94.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
startup_conditions["time_valid"] = now.year >= True
set_offroad_alert_if_changed("Offroad_InvalidTime",
(not startup_conditions["time_valid"]))
# Show update prompt
try:
last_update = now #datetime.datetime.fromisoformat(params.get("LastUpdateTime", encoding='utf8'))
except (TypeError, ValueError):
last_update = now
dt = now - last_update
update_failed_count = 0 #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 current_branch in ["release2", "dashcam"]:
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["not_uninstalling"] = not params.get(
"DoUninstall") == b"1"
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.thermal.freeSpace > 0.02
startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version or \
(current_branch in ['dashcam', 'dashcam-staging'])
startup_conditions["not_driver_view"] = not params.get(
"IsDriverViewEnabled") == b"1"
startup_conditions["not_taking_snapshot"] = not params.get(
"IsTakingSnapshot") == b"1"
# 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"]))
should_start = all(startup_conditions.values())
startup_conditions[
"hardware_supported"] = health is not None and health.health.hwType not in [
log.HealthData.HwType.whitePanda,
log.HealthData.HwType.greyPanda
]
set_offroad_alert_if_changed(
"Offroad_HardwareUnsupported", health is not None
and not startup_conditions["hardware_supported"])
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 startup_conditions["ignition"] and (startup_conditions !=
startup_conditions_prev):
cloudlog.event("Startup blocked",
startup_conditions=startup_conditions)
if should_start_prev or (count == 0):
params.put("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'
)
# Offroad power monitoring
pm.calculate(health)
msg.thermal.offroadPowerUsage = pm.get_power_used()
msg.thermal.carBatteryCapacity = max(0, pm.get_car_battery_capacity())
# Check if we need to disable charging (handled by boardd)
msg.thermal.chargingDisabled = pm.should_disable_charging(
health, off_ts)
# Check if we need to shut down
if pm.should_shutdown(health, off_ts, started_seen, LEON):
cloudlog.info(f"shutting device down, offroad since {off_ts}")
# TODO: add function for blocking cloudlog instead of sleep
time.sleep(10)
os.system('LD_LIBRARY_PATH="" svc power shutdown')
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())
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 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 main():
params = Params()
if params.get_bool("DisableUpdates"):
cloudlog.warning("updates are disabled by the DisableUpdates param")
exit(0)
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 instance running?") from e
# Set low io priority
proc = psutil.Process()
if psutil.LINUX:
proc.ionice(psutil.IOPRIO_CLASS_BE, value=7)
# Check if we just performed an update
if Path(os.path.join(STAGING_ROOT, "old_openpilot")).is_dir():
cloudlog.event("update installed")
if not params.get("InstallDate"):
t = datetime.datetime.utcnow().isoformat()
params.put("InstallDate", t.encode('utf8'))
overlay_init = Path(os.path.join(BASEDIR, ".overlay_init"))
overlay_init.unlink(missing_ok=True)
first_run = True
last_fetch_time = 0
update_failed_count = 0
# Set initial params for offroad alerts
set_params(False, 0, None)
# Wait for IsOffroad to be set before our first update attempt
wait_helper = WaitTimeHelper(proc)
wait_helper.sleep(30)
# 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}"
overlay_init.unlink(missing_ok=True)
except Exception as e:
cloudlog.exception("uncaught updated exception, shouldn't happen")
exception = str(e)
overlay_init.unlink(missing_ok=True)
try:
set_params(new_version, update_failed_count, exception)
except Exception:
cloudlog.exception(
"uncaught updated exception while setting params, shouldn't happen"
)
wait_helper.sleep(60)
dismount_overlay()
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', 'pandaState', '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
hw_type = messaging.recv_one(
self.sm.sock['pandaState']).pandaState.pandaType
has_relay = hw_type in [
PandaType.blackPanda, PandaType.uno, PandaType.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
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:
self.CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP)
self.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.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.v_target = 0.0
self.a_target = 0.0
# scc smoother
self.is_cruise_enabled = False
self.applyMaxSpeed = 0
self.clu_speed_ms = 0.
self.apply_accel = 0.
self.fused_accel = 0.
self.lead_drel = 0.
self.aReqValue = 0.
self.aReqValueMin = 0.
self.aReqValueMax = 0.
self.sccStockCamStatus = 0
self.sccStockCamAct = 0
self.left_lane_visible = False
self.right_lane_visible = False
self.wide_camera = TICI and params.get_bool('EnableWideCamera')
# TODO: no longer necessary, aside from process replay
self.sm['liveParameters'].valid = True
self.startup_event = get_startup_event(car_recognized,
controller_available,
self.CP.fuzzyFingerprint,
len(self.CP.carFw) > 0)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if community_feature_disallowed and car_recognized and not self.CP.dashcamOnly:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
elif self.read_only:
self.events.add(EventName.dashcamMode, static=True)
elif self.joystick_mode:
self.events.add(EventName.joystickDebug, static=True)
self.startup_event = None
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
def set_params_enabled():
from common.params import Params
params = Params()
params.put("HasAcceptedTerms", terms_version)
params.put("HasCompletedSetup", "1")
params.put("OpenpilotEnabledToggle", "1")
params.put("CommunityFeaturesToggle", "1")
params.put("Passive", "0")
params.put("CompletedTrainingVersion", training_version)
