def update_app(self):
put_nonblocking('dp_is_updating', '1')
if self.has_own_apk:
try:
subprocess.check_output(["pm","install","-r",self.own_apk])
self.is_installed = True
except subprocess.CalledProcessError as e:
self.is_installed = False
else:
apk = self.app + ".apk"
apk_path = "/sdcard/" + apk
try:
os.remove(apk_path)
except (OSError, FileNotFoundError) as e:
pass
self.uninstall_app()
# if local_version is not None:
# try:
# subprocess.check_output(["pm","uninstall", self.app], stderr=subprocess.STDOUT, shell=True)
# except subprocess.CalledProcessError as e:
# pass
try:
url = "https://raw.githubusercontent.com/dragonpilot-community/apps/%s/%s" % (apk, apk)
subprocess.check_output(["curl","-o", apk_path,"-LJO", url])
subprocess.check_output(["pm","install","-r",apk_path])
self.is_installed = True
except subprocess.CalledProcessError as e:
self.is_installed = False
try:
os.remove(apk_path)
except (OSError, FileNotFoundError) as e:
pass
put_nonblocking('dp_is_updating', '0')
def handle_cam_odom(self, trans, rot, trans_std, rot_std):
straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER))
certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < MAX_VEL_ANGLE_STD) or
(self.valid_blocks < INPUTS_NEEDED))
if straight_and_fast and certain_if_calib:
# intrinsics are not eon intrinsics, since this is calibrated frame
intrinsics = intrinsics_from_vp(self.vp)
new_vp = intrinsics.dot(view_frame_from_device_frame.dot(trans))
new_vp = new_vp[:2]/new_vp[2]
new_vp = sanity_clip(new_vp)
self.vps[self.block_idx] = (self.idx*self.vps[self.block_idx] + (BLOCK_SIZE - self.idx) * new_vp) / float(BLOCK_SIZE)
self.idx = (self.idx + 1) % BLOCK_SIZE
if self.idx == 0:
self.block_idx += 1
self.valid_blocks = max(self.block_idx, self.valid_blocks)
self.block_idx = self.block_idx % INPUTS_WANTED
if self.valid_blocks > 0:
self.vp = np.mean(self.vps[:self.valid_blocks], axis=0)
self.update_status()
if self.param_put and ((self.idx == 0 and self.block_idx == 0) or self.just_calibrated):
calib = get_calib_from_vp(self.vp)
cal_params = {"calib_radians": list(calib),
"valid_blocks": self.valid_blocks}
put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8'))
return new_vp
else:
return None
def update_status(self):
if self.valid_blocks > 0:
max_rpy_calib = np.array(np.max(self.rpys[:self.valid_blocks], axis=0))
min_rpy_calib = np.array(np.min(self.rpys[:self.valid_blocks], axis=0))
self.calib_spread = np.abs(max_rpy_calib - min_rpy_calib)
else:
self.calib_spread = np.zeros(3)
if self.valid_blocks < INPUTS_NEEDED:
self.cal_status = Calibration.UNCALIBRATED
elif is_calibration_valid(self.rpy):
self.cal_status = Calibration.CALIBRATED
else:
self.cal_status = Calibration.INVALID
# If spread is too high, assume mounting was changed and reset to last block.
# Make the transition smooth. Abrupt transistion are not good foor feedback loop through supercombo model.
if max(self.calib_spread) > MAX_ALLOWED_SPREAD and self.cal_status == Calibration.CALIBRATED:
self.reset(self.rpys[self.block_idx - 1], valid_blocks=INPUTS_NEEDED, smooth_from=self.rpy)
write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5)
if self.param_put and write_this_cycle:
# TODO: this should use the liveCalibration struct from cereal
cal_params = {"calib_radians": list(self.rpy),
"valid_blocks": int(self.valid_blocks)}
put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8'))
def update_status(self) -> None:
valid_idxs = self.get_valid_idxs()
if valid_idxs:
rpys = self.rpys[valid_idxs]
self.rpy = np.mean(rpys, axis=0)
max_rpy_calib = np.array(np.max(rpys, axis=0))
min_rpy_calib = np.array(np.min(rpys, axis=0))
self.calib_spread = np.abs(max_rpy_calib - min_rpy_calib)
else:
self.calib_spread = np.zeros(3)
if self.valid_blocks < INPUTS_NEEDED:
self.cal_status = Calibration.UNCALIBRATED
elif is_calibration_valid(self.rpy):
self.cal_status = Calibration.CALIBRATED
else:
self.cal_status = Calibration.INVALID
# If spread is too high, assume mounting was changed and reset to last block.
# Make the transition smooth. Abrupt transitions are not good for feedback loop through supercombo model.
if max(self.calib_spread) > MAX_ALLOWED_SPREAD and self.cal_status == Calibration.CALIBRATED:
self.reset(self.rpys[self.block_idx - 1], valid_blocks=INPUTS_NEEDED, smooth_from=self.rpy)
write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5)
if self.param_put and write_this_cycle:
put_nonblocking("CalibrationParams", self.get_msg().to_bytes())
def handle_cam_odom(self, trans, rot, trans_std, rot_std):
straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER))
certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < MAX_VEL_ANGLE_STD) or
(self.valid_blocks < INPUTS_NEEDED))
if straight_and_fast and certain_if_calib:
observed_rpy = np.array([0,
-np.arctan2(trans[2], trans[0]),
np.arctan2(trans[1], trans[0])])
new_rpy = euler_from_rot(rot_from_euler(self.rpy).dot(rot_from_euler(observed_rpy)))
new_rpy = sanity_clip(new_rpy)
self.rpys[self.block_idx] = (self.idx*self.rpys[self.block_idx] + (BLOCK_SIZE - self.idx) * new_rpy) / float(BLOCK_SIZE)
self.idx = (self.idx + 1) % BLOCK_SIZE
if self.idx == 0:
self.block_idx += 1
self.valid_blocks = max(self.block_idx, self.valid_blocks)
self.block_idx = self.block_idx % INPUTS_WANTED
if self.valid_blocks > 0:
self.rpy = np.mean(self.rpys[:self.valid_blocks], axis=0)
self.update_status()
# TODO: this should use the liveCalibration struct from cereal
if self.param_put and ((self.idx == 0 and self.block_idx == 0) or self.just_calibrated):
cal_params = {"calib_radians": list(self.rpy),
"valid_blocks": self.valid_blocks}
put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8'))
return new_rpy
else:
return None
def Task(self, gctx=None):
self.opkr_enable_softkey = int(params.get('OpkrRunSoftkey', encoding='utf8'))
for x in program:
nPos = x[0]
opkr_enable = int(params.get( x[1], encoding='utf8'))
if self.navigation_is_running:
if nPos == 2:
opkr_enable = True
if opkr_enable:
print( '1:{} 2:{}'.format( x[1], x[2], program_runing[nPos] ) )
self.all_kill()
program_runing[nPos] = self.exec_app( opkr_enable, x[2], x[3])
time.sleep(1)
put_nonblocking( x[1], '0')
if nPos == 2:
self.opkr_enable_softkey = True
else:
time.sleep(0.1)
self.navigation_is_running = 0
# allow user to manually start/stop app
if self.opkr_enable_softkey:
put_nonblocking('OpkrRunSoftkey', '0')
if not self.softkey_is_running:
self.softkey_is_running = self.exec_app(self.opkr_enable_softkey, "com.gmd.hidesoftkeys", "com.gmd.hidesoftkeys.MainActivity")
#threading.Timer( 0.3, self.Task ).start()
time.sleep(0.3)
def handle_cam_odom(self, trans, rot, trans_std, rot_std):
if ((trans[0] > MIN_SPEED_FILTER) and (trans_std[0] < MAX_SPEED_STD)
and (abs(rot[2]) < MAX_YAW_RATE_FILTER)):
# intrinsics are not eon intrinsics, since this is calibrated frame
intrinsics = intrinsics_from_vp(self.vp)
new_vp = intrinsics.dot(view_frame_from_device_frame.dot(trans))
new_vp = new_vp[:2] / new_vp[2]
new_vp = sanity_clip(new_vp)
self.vps[self.block_idx] = (
self.idx * self.vps[self.block_idx] +
(BLOCK_SIZE - self.idx) * new_vp) / float(BLOCK_SIZE)
self.idx = (self.idx + 1) % BLOCK_SIZE
if self.idx == 0:
self.block_idx += 1
self.valid_blocks = max(self.block_idx, self.valid_blocks)
self.block_idx = self.block_idx % INPUTS_WANTED
if self.valid_blocks > 0:
self.vp = np.mean(self.vps[:self.valid_blocks], axis=0)
self.update_status()
if self.param_put and ((self.idx == 0 and self.block_idx == 0)
or self.just_calibrated):
cal_params = {
"vanishing_point": list(self.vp),
"valid_blocks": self.valid_blocks
}
put_nonblocking("CalibrationParams",
json.dumps(cal_params).encode('utf8'))
return new_vp
else:
return None
def cache_ephemeris(self, t: GPSTime):
if self.save_ephemeris and (self.last_cached_t is None or t - self.last_cached_t > SECS_IN_MIN):
put_nonblocking(EPHEMERIS_CACHE, json.dumps(
{'version': CACHE_VERSION, 'last_fetch_orbits_t': self.last_fetch_orbits_t, 'orbits': self.astro_dog.orbits, 'nav': self.astro_dog.nav},
cls=CacheSerializer))
cloudlog.debug("Cache saved")
self.last_cached_t = t
def init_vars(self, dragonconf):
self.is_enabled = getattr(dragonconf, self.enable_struct)
if self.is_enabled:
local_version = self.get_local_version()
if local_version is not None:
self.is_installed = True
if self.has_own_apk and not self.is_installed:
self.update_app()
elif is_online:
if local_version is None:
self.update_app()
else:
remote_version = self.get_remote_version() if (auto_update and not self.own_apk) else local_version
if remote_version is not None and local_version != remote_version:
self.update_app()
if self.is_installed:
self.set_package_permissions()
else:
self.uninstall_app()
if self.manual_ctrl_param is not None and getattr(dragonconf, self.manual_ctrl_struct) != self.MANUAL_IDLE:
put_nonblocking(self.manual_ctrl_param, str(self.MANUAL_IDLE))
self.init = True
def all_kill(self, enable = 0 ):
put_nonblocking('OpkrRunSoftkey', '0')
for x in program:
nPos = x[0]
put_nonblocking( x[1], '0')
if program_runing[nPos]:
program_runing[nPos] = 0
self.exec_app( enable, x[2], x[3])
def update(self, c, can_strings):
canMonoTimes = []
buttonEvents = []
# Process the most recent CAN message traffic, and check for validity
# The camera CAN has no signals we use at this time, but we process it
# anyway so we can test connectivity with can_valid
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
ret = self.CS.update(self.cp)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# Update the EON metric configuration to match the car at first startup,
# or if there's been a change.
if self.CS.displayMetricUnits != self.displayMetricUnitsPrev:
put_nonblocking("IsMetric", "1" if self.CS.displayMetricUnits else "0")
# Check for and process state-change events (button press or release) from
# the turn stalk switch or ACC steering wheel/control stalk buttons.
for button in self.CS.buttonStates:
if self.CS.buttonStates[button] != self.buttonStatesPrev[button]:
be = car.CarState.ButtonEvent.new_message()
be.type = button
be.pressed = self.CS.buttonStates[button]
buttonEvents.append(be)
events = self.create_common_events(ret, extra_gears=[GEAR.eco, GEAR.sport])
# Vehicle health and operation safety checks
if self.CS.parkingBrakeSet:
events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE]))
if self.CS.steeringFault:
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
# Engagement and longitudinal control using stock ACC. Make sure OP is
# disengaged if stock ACC is disengaged.
if not ret.cruiseState.enabled:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
# Attempt OP engagement only on rising edge of stock ACC engagement.
elif not self.cruise_enabled_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
ret.events = events
ret.buttonEvents = buttonEvents
ret.canMonoTimes = canMonoTimes
# update previous car states
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled
self.displayMetricUnitsPrev = self.CS.displayMetricUnits
self.buttonStatesPrev = self.CS.buttonStates.copy()
self.CS.out = ret.as_reader()
return self.CS.out
def calculate(self, peripheralState, ignition):
try:
now = sec_since_boot()
# If peripheralState is None, we're probably not in a car, so we don't care
if peripheralState is None or peripheralState.pandaType == log.PandaState.PandaType.unknown:
with self.integration_lock:
self.last_measurement_time = None
self.next_pulsed_measurement_time = None
self.power_used_uWh = 0
return
# Low-pass battery voltage
self.car_voltage_instant_mV = peripheralState.voltage
self.car_voltage_mV = (
(peripheralState.voltage * CAR_VOLTAGE_LOW_PASS_K) +
(self.car_voltage_mV * (1 - CAR_VOLTAGE_LOW_PASS_K)))
statlog.gauge("car_voltage", self.car_voltage_mV / 1e3)
# Cap the car battery power and save it in a param every 10-ish seconds
self.car_battery_capacity_uWh = max(self.car_battery_capacity_uWh,
0)
self.car_battery_capacity_uWh = min(self.car_battery_capacity_uWh,
CAR_BATTERY_CAPACITY_uWh)
if now - self.last_save_time >= 10:
put_nonblocking("CarBatteryCapacity",
str(int(self.car_battery_capacity_uWh)))
self.last_save_time = now
# First measurement, set integration time
with self.integration_lock:
if self.last_measurement_time is None:
self.last_measurement_time = now
return
if ignition:
# If there is ignition, we integrate the charging rate of the car
with self.integration_lock:
self.power_used_uWh = 0
integration_time_h = (now -
self.last_measurement_time) / 3600
if integration_time_h < 0:
raise ValueError(
f"Negative integration time: {integration_time_h}h"
)
self.car_battery_capacity_uWh += (CAR_CHARGING_RATE_W *
1e6 * integration_time_h)
self.last_measurement_time = now
else:
# Get current power draw somehow
current_power = HARDWARE.get_current_power_draw()
# Do the integration
self._perform_integration(now, current_power)
except Exception:
cloudlog.exception("Power monitoring calculation failed")
def update(self, c, can_strings):
canMonoTimes = []
ret_arne182 = arne182.CarStateArne182.new_message()
buttonEvents = []
# Process the most recent CAN message traffic, and check for validity
# The camera CAN has no signals we use at this time, but we process it
# anyway so we can test connectivity with can_valid
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
ret = self.CS.update(self.cp)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# Update the EON metric configuration to match the car at first startup,
# or if there's been a change.
if self.CS.displayMetricUnits != self.displayMetricUnitsPrev:
put_nonblocking("IsMetric",
"1" if self.CS.displayMetricUnits else "0")
# Check for and process state-change events (button press or release) from
# the turn stalk switch or ACC steering wheel/control stalk buttons.
for button in self.CS.buttonStates:
if self.CS.buttonStates[button] != self.buttonStatesPrev[button]:
be = car.CarState.ButtonEvent.new_message()
be.type = button
be.pressed = self.CS.buttonStates[button]
buttonEvents.append(be)
events, events_arne182 = self.create_common_events(
ret, extra_gears=[GEAR.eco, GEAR.sport])
# Vehicle health and operation safety checks
if self.CS.parkingBrakeSet:
events.add(EventName.parkBrake)
if self.CS.steeringFault:
events.add(EventName.steerTempUnavailable)
ret_arne182.events = events_arne182.to_msg()
ret.events = events.to_msg()
ret.buttonEvents = buttonEvents
ret.canMonoTimes = canMonoTimes
# update previous car states
self.displayMetricUnitsPrev = self.CS.displayMetricUnits
self.buttonStatesPrev = self.CS.buttonStates.copy()
self.CS.out = ret.as_reader()
return self.CS.out, ret_arne182.as_reader()
def update_custom_logic(msg):
if msg.dragonConf.dpAssistedLcMinMph > msg.dragonConf.dpAutoLcMinMph:
put_nonblocking('dp_auto_lc_min_mph',
str(msg.dragonConf.dpAssistedLcMinMph))
msg.dragonConf.dpAutoLcMinMph = msg.dragonConf.dpAssistedLcMinMph
if msg.dragonConf.dpAtl:
msg.dragonConf.dpAllowGas = True
msg.dragonConf.dpDynamicFollow = 0
msg.dragonConf.dpAccelProfile = 0
msg.dragonConf.dpGearCheck = False
if msg.dragonConf.dpAppWaze or msg.dragonConf.dpAppHr:
msg.dragonConf.dpDrivingUi = False
if not msg.dragonConf.dpDriverMonitor:
msg.dragonConf.dpUiFace = False
return msg
def kill(self, force = False):
if self.is_installed and (force or self.is_enabled):
# app is manually ctrl, we record that
if self.manual_ctrl_param is not None and self.manual_ctrl_status == self.MANUAL_OFF:
put_nonblocking(self.manual_ctrl_param, '0')
self.manually_ctrled = True
self.is_running = True
# only kill app if it's running
if force or self.is_running:
if self.app_type == self.TYPE_SERVICE:
self.appops_set(self.app, "android:mock_location", "deny")
self.system("pkill %s" % self.app)
self.is_running = False
def __init__(self, app, activity, enable_param, auto_run_param,
manual_ctrl_param, app_type, permissions, opts):
self.app = app
# main activity
self.activity = activity
# read enable param
self.enable_param = enable_param
# read auto run param
self.auto_run_param = auto_run_param
# read manual run param
self.manual_ctrl_param = manual_ctrl_param
# if it's a service app, we do not kill if device is too hot
self.app_type = app_type
# app permissions
self.permissions = permissions
# app options
self.opts = opts
self.is_installed = False
self.is_enabled = True if params.get(self.enable_param,
encoding='utf8') == "1" else False
self.last_is_enabled = False
self.is_auto_runnable = False
self.is_running = False
self.manual_ctrl_status = self.MANUAL_IDLE
self.manually_ctrled = False
if self.is_enabled:
local_version = self.get_local_version()
if local_version is not None:
self.is_installed = True
if not is_offline:
remote_version = local_version
if local_version is not None and auto_update:
remote_version = self.get_remote_version()
if local_version is None or (remote_version is not None and
local_version != remote_version):
self.update_app()
if self.is_installed:
self.set_package_permissions()
else:
self.uninstall_app()
if self.manual_ctrl_param is not None:
put_nonblocking(self.manual_ctrl_param, '0')
self.last_ts = sec_since_boot()
def handle_cam_odom(self, log):
trans, rot = log.trans, log.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)}
put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8'))
return new_vp
else:
return None
def run(self, force = False):
if self.is_installed and (force or self.is_enabled):
# app is manually ctrl, we record that
if self.manual_ctrl_param is not None and self.manual_ctrl_status == self.MANUAL_ON:
put_nonblocking(self.manual_ctrl_param, '0')
put_nonblocking('dp_last_modified', str(floor(time.time())))
self.manually_ctrled = True
self.is_running = False
# only run app if it's not running
if force or not self.is_running:
self.system("pm enable %s" % self.app)
if self.app_type == self.TYPE_SERVICE:
self.appops_set(self.app, "android:mock_location", "allow")
self.system(self.start_cmd)
self.is_running = True
def update_custom_logic(msg):
if msg.dragonConf.dpAtl:
msg.dragonConf.dpAllowGas = True
msg.dragonConf.dpFollowingProfileCtrl = False
msg.dragonConf.dpAccelProfileCtrl = False
msg.dragonConf.dpGearCheck = False
if msg.dragonConf.dpLcMinMph > msg.dragonConf.dpLcAutoMinMph:
put_nonblocking('dp_lc_auto_min_mph', str(msg.dragonConf.dpLcMinMph))
msg.dragonConf.dpLcAutoMinMph = msg.dragonConf.dpLcMinMph
# if msg.dragonConf.dpSrCustom <= 4.99 and msg.dragonConf.dpSrStock > 0:
# put_nonblocking('dp_sr_custom', str(msg.dragonConf.dpSrStock))
# msg.dragonConf.dpSrCustom = msg.dragonConf.dpSrStock
# if msg.dragonConf.dpAppWaze or msg.dragonConf.dpAppHr:
# msg.dragonConf.dpDrivingUi = False
# if not msg.dragonConf.dpDriverMonitor:
# msg.dragonConf.dpUiFace = False
return msg
def data_sample(self):
"""Receive data from sockets and update carState"""
# Update carState from CAN
can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
CS = self.CI.update(self.CC, can_strs)
self.sm.update(0)
all_valid = CS.canValid and self.sm.all_alive_and_valid()
if not self.initialized and (all_valid or self.sm.frame * DT_CTRL > 3.5 or SIMULATION):
if not self.read_only:
self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
self.initialized = True
Params().put_bool("ControlsReady", True)
# Check for CAN timeout
if not can_strs:
self.can_error_counter += 1
self.can_rcv_error = True
else:
self.can_rcv_error = False
# When the panda and controlsd do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through
# another socket other than the CAN messages and one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
if not self.enabled:
self.mismatch_counter = 0
# All pandas not in silent mode must have controlsAllowed when openpilot is enabled
if any(not ps.controlsAllowed and self.enabled for ps in self.sm['pandaStates']
if ps.safetyModel not in IGNORED_SAFETY_MODES):
self.mismatch_counter += 1
self.distance_traveled += CS.vEgo * DT_CTRL
if self.jvePilotState.notifyUi:
self.ui_notify()
elif self.sm.updated['jvePilotUIState']:
self.jvePilotState.carControl.autoFollow = self.sm['jvePilotUIState'].autoFollow
self.jvePilotState.carControl.accEco = self.sm['jvePilotUIState'].accEco
put_nonblocking("jvePilot.carState.accEco", str(self.sm['jvePilotUIState'].accEco))
return CS
def dragonpilot_set_params(params):
# # remove deprecated params
# for old, new in deprecated_conf.items():
# if params.get(old) is not None:
# if new is not None:
# old_val = str(params.get(old))
# new_val = old_val
# # invert the value if true
# if old in deprecated_conf_invert and deprecated_conf_invert[old] is True:
# new_val = "1" if old_val == "0" else "0"
# put_nonblocking(new, new_val)
# params.delete(old)
# set params
for key, val in default_conf.items():
if params.get(key) is None and key not in deprecated_conf:
put_nonblocking(key, str(val))
put_nonblocking('DragonSupportedCars', get_support_car_list())
def run(self, force=False):
if self.is_installed and (force or self.is_enabled):
# app is manually ctrl, we record that
if self.manual_ctrl_param is not None and self.manual_ctrl_status == self.MANUAL_ON:
put_nonblocking(self.manual_ctrl_param, '0')
put_nonblocking('DragonLastModified', str(floor(time.time())))
self.manually_ctrled = True
self.is_running = False
# only run app if it's not running
if force or not self.is_running:
self.system("pm enable %s" % self.app)
if self.app_type == self.TYPE_GPS_SERVICE:
self.appops_set(self.app, "android:mock_location", "allow")
if self.app_type in [self.TYPE_SERVICE, self.TYPE_GPS_SERVICE]:
self.system("am startservice %s/%s" %
(self.app, self.activity))
else:
self.system("am start -n %s/%s" %
(self.app, self.activity))
self.is_running = True
def controlsd_thread(sm=None, pm=None, can_sock=None):
gc.disable()
# start the loop
set_realtime_priority(3)
params = Params()
is_metric = params.get("IsMetric", encoding='utf8') == "1"
is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1"
passive = params.get("Passive", encoding='utf8') == "1"
openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle",
encoding='utf8') == "1"
community_feature_toggle = params.get("CommunityFeaturesToggle",
encoding='utf8') == "1"
passive = passive or not openpilot_enabled_toggle
# Passive if internet needed
internet_needed = params.get("Offroad_ConnectivityNeeded",
encoding='utf8') is not None
passive = passive or internet_needed
# Pub/Sub Sockets
if pm is None:
pm = messaging.PubMaster([
'sendcan', 'controlsState', 'carState', 'carControl', 'carEvents',
'carParams'
])
if sm is None:
sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'dMonitoringState', 'plan', 'pathPlan', \
'model'])
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
can_sock = messaging.sub_sock('can', timeout=can_timeout)
# wait for health and CAN packets
hw_type = messaging.recv_one(sm.sock['health']).health.hwType
has_relay = hw_type in [HwType.blackPanda, HwType.uno]
print("Waiting for CAN messages...")
messaging.get_one_can(can_sock)
CI, CP = get_car(can_sock, pm.sock['sendcan'], has_relay)
car_recognized = CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not chffrplus
controller_available = CP.enableCamera and CI.CC is not None and not passive
community_feature_disallowed = CP.communityFeature and not community_feature_toggle
read_only = not car_recognized or not controller_available or CP.dashcamOnly or community_feature_disallowed
if read_only:
CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard and boardd safety mode
cp_bytes = CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
put_nonblocking("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)
state = State.disabled
soft_disable_timer = 0
v_cruise_kph = 255
v_cruise_kph_last = 0
mismatch_counter = 0
can_error_counter = 0
last_blinker_frame = 0
saturated_count = 0
events_prev = []
sm['liveCalibration'].calStatus = Calibration.INVALID
sm['pathPlan'].sensorValid = True
sm['pathPlan'].posenetValid = True
sm['thermal'].freeSpace = 1.
sm['dMonitoringState'].events = []
sm['dMonitoringState'].awarenessStatus = 1.
sm['dMonitoringState'].faceDetected = False
# 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_perc, mismatch_counter, can_error_counter = data_sample(
CI, CC, sm, can_sock, state, mismatch_counter, can_error_counter,
params)
prof.checkpoint("Sample")
# Create alerts
if not sm.alive['plan'] and sm.alive[
'pathPlan']: # only plan not being received: radar not communicating
events.append(
create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
elif not sm.all_alive_and_valid():
events.append(
create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not sm['pathPlan'].mpcSolutionValid:
events.append(
create_event('plannerError',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not sm['pathPlan'].sensorValid and os.getenv("NOSENSOR") is None:
events.append(
create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT]))
if not sm['pathPlan'].paramsValid:
events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
if not sm['pathPlan'].posenetValid:
events.append(
create_event('posenetInvalid', [ET.NO_ENTRY, ET.WARNING]))
if not sm['plan'].radarValid:
events.append(
create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if sm['plan'].radarCanError:
events.append(
create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not CS.canValid:
events.append(
create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not sounds_available:
events.append(
create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT]))
if internet_needed:
events.append(
create_event('internetConnectivityNeeded',
[ET.NO_ENTRY, ET.PERMANENT]))
if community_feature_disallowed:
events.append(
create_event('communityFeatureDisallowed', [ET.PERMANENT]))
if read_only and not passive:
events.append(create_event('carUnrecognized', [ET.PERMANENT]))
if log.HealthData.FaultType.relayMalfunction in sm['health'].faults:
events.append(
create_event(
'relayMalfunction',
[ET.NO_ENTRY, ET.PERMANENT, ET.IMMEDIATE_DISABLE]))
# Only allow engagement with brake pressed when stopped behind another stopped car
if CS.brakePressed and sm[
'plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
events.append(
create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not read_only:
# update control state
state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
prof.checkpoint("State transition")
# Compute actuators (runs PID loops and lateral MPC)
actuators, v_cruise_kph, v_acc, a_acc, lac_log, last_blinker_frame, saturated_count = \
state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, saturated_count)
prof.checkpoint("State Control")
# Publish data
CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events,
actuators, v_cruise_kph, rk, AM, LaC, LoC,
read_only, start_time, v_acc, a_acc,
lac_log, events_prev, last_blinker_frame,
is_ldw_enabled, can_error_counter)
prof.checkpoint("Sent")
rk.monitor_time()
prof.display()
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')
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
current_branch = get_git_branch()
network_type = NetworkType.none
network_strength = NetworkStrength.unknown
current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML)
cpu_temp_filter = FirstOrderFilter(0., CPU_TEMP_TAU, DT_TRML)
health_prev = None
fw_version_match_prev = True
current_update_alert = None
time_valid_prev = True
should_start_prev = False
handle_fan = None
is_uno = False
has_relay = False
params = Params()
pm = PowerMonitoring()
no_panda_cnt = 0
while 1:
health = messaging.recv_sock(health_sock, wait=True)
location = messaging.recv_sock(location_sock)
location = location.gpsLocation if location else None
msg = read_thermal()
if health is not None:
usb_power = health.health.usbPowerMode != log.HealthData.UsbPowerMode.client
# If we lose connection to the panda, wait 5 seconds before going offroad
if health.health.hwType == log.HealthData.HwType.unknown:
no_panda_cnt += 1
if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML:
if ignition:
cloudlog.error("Lost panda connection while onroad")
ignition = False
else:
no_panda_cnt = 0
ignition = health.health.ignitionLine or health.health.ignitionCan
# Setup fan handler on first connect to panda
if handle_fan is None and health.health.hwType != log.HealthData.HwType.unknown:
is_uno = health.health.hwType == log.HealthData.HwType.uno
has_relay = health.health.hwType in [log.HealthData.HwType.blackPanda, log.HealthData.HwType.uno, log.HealthData.HwType.dos]
if is_uno or not ANDROID:
cloudlog.info("Setting up UNO fan handler")
handle_fan = handle_fan_uno
else:
cloudlog.info("Setting up EON fan handler")
setup_eon_fan()
handle_fan = handle_fan_eon
# Handle disconnect
if health_prev is not None:
if health.health.hwType == log.HealthData.HwType.unknown and \
health_prev.health.hwType != log.HealthData.HwType.unknown:
params.panda_disconnect()
health_prev = health
# get_network_type is an expensive call. update every 10s
if (count % int(10. / DT_TRML)) == 0:
try:
network_type = get_network_type()
network_strength = get_network_strength(network_type)
except Exception:
cloudlog.exception("Error getting network status")
msg.thermal.freeSpace = get_available_percent(default=100.0) / 100.0
msg.thermal.memUsedPercent = int(round(psutil.virtual_memory().percent))
msg.thermal.cpuPerc = int(round(psutil.cpu_percent()))
msg.thermal.networkType = network_type
msg.thermal.networkStrength = network_strength
msg.thermal.batteryPercent = get_battery_capacity()
msg.thermal.batteryStatus = get_battery_status()
msg.thermal.batteryCurrent = get_battery_current()
msg.thermal.batteryVoltage = get_battery_voltage()
msg.thermal.usbOnline = get_usb_present()
# Fake battery levels on uno for frame
if is_uno:
msg.thermal.batteryPercent = 100
msg.thermal.batteryStatus = "Charging"
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.cpu0,
msg.thermal.cpu1,
msg.thermal.cpu2,
msg.thermal.cpu3) / 10.0)
max_comp_temp = max(max_cpu_temp, msg.thermal.mem / 10., msg.thermal.gpu / 10.)
bat_temp = msg.thermal.bat / 1000.
if handle_fan is not None:
fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed, ignition)
msg.thermal.fanSpeed = fan_speed
# If device is offroad we want to cool down before going onroad
# since going onroad increases load and can make temps go over 107
# We only do this if there is a relay that prevents the car from faulting
if max_cpu_temp > 107. or bat_temp >= 63. or (has_relay and (started_ts is None) and max_cpu_temp > 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
time_valid = now.year >= 2019
if time_valid and not time_valid_prev:
set_offroad_alert("Offroad_InvalidTime", False)
if not time_valid and time_valid_prev:
set_offroad_alert("Offroad_InvalidTime", True)
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)
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
if current_update_alert != "update" + extra_text:
current_update_alert = "update" + extra_text
set_offroad_alert("Offroad_ConnectivityNeeded", False)
set_offroad_alert("Offroad_ConnectivityNeededPrompt", False)
set_offroad_alert("Offroad_UpdateFailed", True, extra_text=extra_text)
elif dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1:
if current_update_alert != "expired":
current_update_alert = "expired"
set_offroad_alert("Offroad_UpdateFailed", False)
set_offroad_alert("Offroad_ConnectivityNeededPrompt", False)
set_offroad_alert("Offroad_ConnectivityNeeded", True)
elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT:
remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0))
if current_update_alert != "prompt" + remaining_time:
current_update_alert = "prompt" + remaining_time
set_offroad_alert("Offroad_UpdateFailed", False)
set_offroad_alert("Offroad_ConnectivityNeeded", False)
set_offroad_alert("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining_time} days.")
elif current_update_alert is not None:
current_update_alert = None
set_offroad_alert("Offroad_UpdateFailed", False)
set_offroad_alert("Offroad_ConnectivityNeeded", False)
set_offroad_alert("Offroad_ConnectivityNeededPrompt", False)
do_uninstall = params.get("DoUninstall") == b"1"
accepted_terms = params.get("HasAcceptedTerms") == terms_version
completed_training = params.get("CompletedTrainingVersion") == training_version
panda_signature = params.get("PandaFirmware")
fw_version_match = (panda_signature is None) or (panda_signature == FW_SIGNATURE) # don't show alert is no panda is connected (None)
should_start = ignition
# with 2% left, we killall, otherwise the phone will take a long time to boot
should_start = should_start and msg.thermal.freeSpace > 0.02
# confirm we have completed training and aren't uninstalling
should_start = should_start and accepted_terms and completed_training and (not do_uninstall)
# check for firmware mismatch
should_start = should_start and fw_version_match
# check if system time is valid
should_start = should_start and time_valid
# don't start while taking snapshot
if not should_start_prev:
is_viewing_driver = params.get("IsDriverViewEnabled") == b"1"
is_taking_snapshot = params.get("IsTakingSnapshot") == b"1"
should_start = should_start and (not is_taking_snapshot) and (not is_viewing_driver)
if fw_version_match and not fw_version_match_prev:
set_offroad_alert("Offroad_PandaFirmwareMismatch", False)
if not fw_version_match and fw_version_match_prev:
set_offroad_alert("Offroad_PandaFirmwareMismatch", True)
# 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:
set_offroad_alert("Offroad_TemperatureTooHigh", True)
else:
if thermal_status_prev >= ThermalStatus.danger:
set_offroad_alert("Offroad_TemperatureTooHigh", False)
if should_start:
if not should_start_prev:
params.delete("IsOffroad")
off_ts = None
if started_ts is None:
started_ts = sec_since_boot()
started_seen = True
os.system('echo performance > /sys/class/devfreq/soc:qcom,cpubw/governor')
else:
if should_start_prev or (count == 0):
put_nonblocking("IsOffroad", "1")
started_ts = None
if off_ts is None:
off_ts = sec_since_boot()
os.system('echo powersave > /sys/class/devfreq/soc:qcom,cpubw/governor')
# Offroad power monitoring
pm.calculate(health)
msg.thermal.offroadPowerUsage = pm.get_power_used()
msg.thermal.carBatteryCapacity = 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):
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())
if usb_power_prev and not usb_power:
set_offroad_alert("Offroad_ChargeDisabled", True)
elif usb_power and not usb_power_prev:
set_offroad_alert("Offroad_ChargeDisabled", False)
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 main(sm=None, pm=None):
if sm is None:
sm = messaging.SubMaster(['liveLocationKalman', 'carState'])
if pm is None:
pm = messaging.PubMaster(['liveParameters'])
params_reader = Params()
# wait for stats about the car to come in from controls
cloudlog.info("paramsd is waiting for CarParams")
CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True))
cloudlog.info("paramsd got CarParams")
params = params_reader.get("LiveParameters")
# Check if car model matches
if params is not None:
params = json.loads(params)
if params.get('carFingerprint', None) != CP.carFingerprint:
cloudlog.info("Parameter learner found parameters for wrong car.")
params = None
if params is None:
params = {
'carFingerprint': CP.carFingerprint,
'steerRatio': CP.steerRatio,
'stiffnessFactor': 1.0,
'angleOffsetAverage': 0.0,
}
cloudlog.info("Parameter learner resetting to default values")
learner = ParamsLearner(CP, params['steerRatio'],
params['stiffnessFactor'],
math.radians(params['angleOffsetAverage']))
i = 0
while True:
sm.update()
for which, updated in sm.updated.items():
if not updated:
continue
t = sm.logMonoTime[which] * 1e-9
learner.handle_log(t, which, sm[which])
# TODO: set valid to false when locationd stops sending
# TODO: make sure controlsd knows when there is no gyro
if sm.updated['carState']:
msg = messaging.new_message('liveParameters')
msg.logMonoTime = sm.logMonoTime['carState']
msg.liveParameters.valid = True # TODO: Check if learned values are sane
msg.liveParameters.posenetValid = True
msg.liveParameters.sensorValid = True
x = learner.kf.x
msg.liveParameters.steerRatio = float(x[States.STEER_RATIO])
msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
msg.liveParameters.angleOffsetAverage = math.degrees(
x[States.ANGLE_OFFSET])
msg.liveParameters.angleOffset = msg.liveParameters.angleOffsetAverage + math.degrees(
x[States.ANGLE_OFFSET_FAST])
i += 1
if i % 6000 == 0: # once a minute
params = {
'carFingerprint': CP.carFingerprint,
'steerRatio': msg.liveParameters.steerRatio,
'stiffnessFactor': msg.liveParameters.stiffnessFactor,
'angleOffsetAverage':
msg.liveParameters.angleOffsetAverage,
}
put_nonblocking("LiveParameters", json.dumps(params))
# P = learner.kf.P
# print()
# print("sR", float(x[States.STEER_RATIO]), float(P[States.STEER_RATIO, States.STEER_RATIO])**0.5)
# print("x ", float(x[States.STIFFNESS]), float(P[States.STIFFNESS, States.STIFFNESS])**0.5)
# print("ao avg ", math.degrees(x[States.ANGLE_OFFSET]), math.degrees(P[States.ANGLE_OFFSET, States.ANGLE_OFFSET])**0.5)
# print("ao ", math.degrees(x[States.ANGLE_OFFSET_FAST]), math.degrees(P[States.ANGLE_OFFSET_FAST, States.ANGLE_OFFSET_FAST])**0.5)
pm.send('liveParameters', msg)
def main(sm=None, pm=None):
gc.disable()
if sm is None:
sm = messaging.SubMaster(['liveLocationKalman', 'carState'],
poll=['liveLocationKalman'])
if pm is None:
pm = messaging.PubMaster(['liveParameters'])
params_reader = Params()
# wait for stats about the car to come in from controls
cloudlog.info("paramsd is waiting for CarParams")
CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True))
cloudlog.info("paramsd got CarParams")
min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
params = params_reader.get("LiveParameters")
# Check if car model matches
if params is not None:
params = json.loads(params)
if params.get('carFingerprint', None) != CP.carFingerprint:
cloudlog.info("Parameter learner found parameters for wrong car.")
params = None
# Check if starting values are sane
if params is not None:
try:
angle_offset_sane = abs(params.get('angleOffsetAverageDeg')) < 10.0
steer_ratio_sane = min_sr <= params['steerRatio'] <= max_sr
params_sane = angle_offset_sane and steer_ratio_sane
if not params_sane:
cloudlog.info(f"Invalid starting values found {params}")
params = None
except Exception as e:
cloudlog.info(f"Error reading params {params}: {str(e)}")
params = None
# TODO: cache the params with the capnp struct
if params is None:
params = {
'carFingerprint': CP.carFingerprint,
'steerRatio': CP.steerRatio,
'stiffnessFactor': 1.0,
'angleOffsetAverageDeg': 0.0,
}
cloudlog.info("Parameter learner resetting to default values")
# When driving in wet conditions the stiffness can go down, and then be too low on the next drive
# Without a way to detect this we have to reset the stiffness every drive
params['stiffnessFactor'] = 1.0
learner = ParamsLearner(CP, params['steerRatio'],
params['stiffnessFactor'],
math.radians(params['angleOffsetAverageDeg']))
while True:
sm.update()
for which, updated in sm.updated.items():
if updated:
t = sm.logMonoTime[which] * 1e-9
learner.handle_log(t, which, sm[which])
if sm.updated['liveLocationKalman']:
x = learner.kf.x
if not all(map(math.isfinite, x)):
cloudlog.error(
"NaN in liveParameters estimate. Resetting to default values"
)
learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0)
x = learner.kf.x
msg = messaging.new_message('liveParameters')
msg.logMonoTime = sm.logMonoTime['carState']
msg.liveParameters.posenetValid = True
msg.liveParameters.sensorValid = True
msg.liveParameters.steerRatio = float(x[States.STEER_RATIO])
msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
msg.liveParameters.angleOffsetAverageDeg = math.degrees(
x[States.ANGLE_OFFSET])
msg.liveParameters.angleOffsetDeg = msg.liveParameters.angleOffsetAverageDeg + math.degrees(
x[States.ANGLE_OFFSET_FAST])
msg.liveParameters.valid = all((
abs(msg.liveParameters.angleOffsetAverageDeg) < 30.0,
abs(msg.liveParameters.angleOffsetDeg) < 30.0,
0.2 <= msg.liveParameters.stiffnessFactor <= 5.0,
min_sr <= msg.liveParameters.steerRatio <= max_sr,
))
if sm.frame % 1200 == 0: # once a minute
params = {
'carFingerprint':
CP.carFingerprint,
'steerRatio':
msg.liveParameters.steerRatio,
'stiffnessFactor':
msg.liveParameters.stiffnessFactor,
'angleOffsetAverageDeg':
msg.liveParameters.angleOffsetAverageDeg,
}
put_nonblocking("LiveParameters", json.dumps(params))
pm.send('liveParameters', msg)
def confd_thread():
sm = messaging.SubMaster(['thermal'])
pm = messaging.PubMaster(['dragonConf'])
last_dp_msg = None
last_modified = None
update_params = False
frame = 0
locale = getprop("persist.sys.locale").rstrip('\n')
last_autoshutdown = False
last_sec = None
autoshutdown_frame = 0
last_charging_ctrl = False
last_started = False
dashcam_next_frame = 0
dashcam_folder_exists = False
dashcam_mkdir_retry = 0
# thermal
started = False
free_space = 0.
online = False
battery_percent = 0.
overheat = False
while True:
start_sec = sec_since_boot()
if frame % 6 == 0:
sm.update()
if sm.updated['thermal']:
started = sm['thermal'].started
free_space = sm['thermal'].freeSpace
online = sm['thermal'].usbOnline
battery_percent = sm['thermal'].batteryPercent
overheat = sm['thermal'].thermalStatus >= 2
msg = messaging.new_message('dragonConf')
if last_dp_msg is not None:
msg.dragonConf = last_dp_msg
'''
===================================================
force update when start status changed
===================================================
'''
if last_started != started:
update_params = True
'''
===================================================
we only need to check last_modified every 3 seconds
if val is modified, we then proceed to read params
===================================================
'''
if not update_params and frame % 6 == 0:
try:
modified = params.get('dp_last_modified',
encoding='utf8').rstrip('\x00')
except AttributeError:
modified = str(floor(time.time()))
if last_modified != modified:
update_params = True
last_modified = modified
'''
===================================================
push param vals to message
===================================================
'''
if update_params:
for conf in confs:
conf_type = conf.get('conf_type')
if 'param' in conf_type and 'struct' in conf_type:
update_this_conf = True
update_once = conf.get('update_once')
if update_once is not None and update_once is True and frame > 0:
update_this_conf = False
if frame > 0 and update_this_conf:
dependencies = conf.get('depends')
# if has dependency and the depend param val is not in depend_vals, we dont update that conf val
# this should reduce chance of reading all params
if dependencies is not None:
for dependency in dependencies:
if getattr(msg.dragonConf,
get_struct_name(dependency['name'])
) not in dependency['vals']:
update_this_conf = False
if update_this_conf:
val = params.get(conf['name'], encoding='utf8')
if val is not None:
val = val.rstrip('\x00')
struct_val = to_struct_val(conf['name'], val)
orig_val = struct_val
if conf.get('min') is not None:
struct_val = max(struct_val, conf.get('min'))
if conf.get('max') is not None:
struct_val = min(struct_val, conf.get('max'))
if orig_val != struct_val:
params.put(conf['name'], str(struct_val))
setattr(msg.dragonConf, get_struct_name(conf['name']),
struct_val)
'''
===================================================
push ip addr every 10 secs to message
===================================================
'''
if frame % 20 == 0:
val = 'N/A'
try:
result = subprocess.check_output(["ifconfig", "wlan0"],
encoding='utf8')
val = re.findall(r"inet addr:((\d+\.){3}\d+)", result)[0][0]
except:
pass
msg.dragonConf.dpIpAddr = val
'''
===================================================
push is_updating status every 5 secs to message
===================================================
'''
if frame % 10 == 0:
val = params.get('dp_is_updating', encoding='utf8').rstrip('\x00')
setattr(msg.dragonConf, get_struct_name('dp_is_updating'),
to_struct_val('dp_is_updating', val))
'''
===================================================
push once
===================================================
'''
if frame == 0:
setattr(msg.dragonConf, get_struct_name('dp_locale'), locale)
put_nonblocking('dp_is_updating', '0')
'''
===================================================
we can have some logic here
===================================================
'''
if msg.dragonConf.dpAssistedLcMinMph > msg.dragonConf.dpAutoLcMinMph:
put_nonblocking('dp_auto_lc_min_mph',
str(msg.dragonConf.dpAssistedLcMinMph))
msg.dragonConf.dpAutoLcMinMph = msg.dragonConf.dpAssistedLcMinMph
if msg.dragonConf.dpAtl:
msg.dragonConf.dpAllowGas = True
msg.dragonConf.dpDynamicFollow = 0
msg.dragonConf.dpAccelProfile = 0
msg.dragonConf.dpGearCheck = False
if msg.dragonConf.dpAppWaze:
msg.dragonConf.dpDrivingUi = False
if not msg.dragonConf.dpDriverMonitor:
msg.dragonConf.dpUiFace = False
msg.dragonConf.dpThermalStarted = started
msg.dragonConf.dpThermalOverheat = overheat
'''
===================================================
publish msg
===================================================
'''
last_dp_msg = msg.dragonConf
update_params = False
pm.send('dragonConf', msg)
'''
===================================================
hotspot on boot
we do it after 30 secs just in case
===================================================
'''
if frame == 60 and params.get("dp_hotspot_on_boot") == b'1':
os.system("service call wifi 37 i32 0 i32 1 &")
'''
===================================================
dashcam
===================================================
'''
dashcam = msg.dragonConf.dpDashcam
if frame % 2 == 0 and dashcam:
while not dashcam_folder_exists and dashcam_mkdir_retry <= 5:
try:
if not os.path.exists(DASHCAM_VIDEOS_PATH):
os.makedirs(DASHCAM_VIDEOS_PATH)
else:
dashcam_folder_exists = True
except OSError:
pass
dashcam_mkdir_retry += 1
if dashcam_folder_exists:
if started:
if frame >= dashcam_next_frame - 2:
now = datetime.datetime.now()
file_name = now.strftime("%Y-%m-%d_%H-%M-%S")
os.system(
"screenrecord --bit-rate %s --time-limit %s %s%s.mp4 &"
% (DASHCAM_BIT_RATES, DASHCAM_DURATION,
DASHCAM_VIDEOS_PATH, file_name))
dashcam_next_frame = frame + DASHCAM_DURATION * 2
else:
dashcam_next_frame = 0
if frame % 120 == 0 and ((free_space < DASHCAM_FREESPACE_LIMIT)
or
(get_used_spaces() > DASHCAM_KEPT)):
try:
files = [
f for f in sorted(os.listdir(DASHCAM_VIDEOS_PATH))
if os.path.isfile(DASHCAM_VIDEOS_PATH + f)
]
os.system("rm -fr %s &" %
(DASHCAM_VIDEOS_PATH + files[0]))
except (IndexError, FileNotFoundError, OSError):
pass
'''
===================================================
auto shutdown
===================================================
'''
autoshutdown = msg.dragonConf.dpAutoShutdown
if frame % 20 == 0 and autoshutdown:
sec = msg.dragonConf.dpAutoShutdownIn * 60 * 2
if last_autoshutdown != autoshutdown or last_sec != sec or started or online:
autoshutdown_frame = frame + sec
if not started and not online and sec > 0 and frame >= autoshutdown_frame:
os.system('LD_LIBRARY_PATH="" svc power shutdown')
last_sec = sec
last_autoshutdown = autoshutdown
'''
===================================================
battery ctrl every 30 secs
PowerMonitor in thermald turns back on every mins
so lets turn it off more frequent
===================================================
'''
charging_ctrl = msg.dragonConf.dpChargingCtrl
if last_charging_ctrl != charging_ctrl:
set_battery_charging(True)
if charging_ctrl and frame % 60 == 0:
if battery_percent >= msg.dragonConf.dpDischargingAt and get_battery_charging(
):
set_battery_charging(False)
elif battery_percent <= msg.dragonConf.dpChargingAt and not get_battery_charging(
):
set_battery_charging(True)
last_charging_ctrl = charging_ctrl
'''
===================================================
make it 2 hz
===================================================
'''
last_started = started
frame += 1
sleep = 0.5 - (sec_since_boot() - start_sec)
if sleep > 0:
time.sleep(sleep)
def __init__(self, sm=None, pm=None, can_sock=None):
gc.disable()
set_realtime_priority(3)
self.trace_log = trace1.Loger("controlsd")
# 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:
socks = [
'thermal', 'health', 'model', 'liveCalibration',
'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman'
]
self.sm = messaging.SubMaster(socks,
ignore_alive=['dMonitoringState'])
#self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration', \
# 'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman'])
print(" start_Controls messages...1")
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)
print(" start_Controls messages...2")
# 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]
print("Waiting for CAN messages...")
messaging.get_one_can(self.can_sock)
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'],
has_relay)
# read params
params = Params()
self.is_metric = params.get("IsMetric", encoding='utf8') == "1"
self.is_ldw_enabled = params.get("IsLdwEnabled",
encoding='utf8') == "1"
internet_needed = params.get("Offroad_ConnectivityNeeded",
encoding='utf8') is not None
community_feature_toggle = params.get("CommunityFeaturesToggle",
encoding='utf8') == "1"
openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle",
encoding='utf8') == "1"
passive = params.get("Passive", encoding='utf8') == "1" or \
internet_needed or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = not os.path.isfile('/EON') or (os.path.isfile('/proc/asound/card0/state') \
and open('/proc/asound/card0/state').read().strip() == 'ONLINE')
car_recognized = self.CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not dashcam mode
controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive
community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
self.CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard and boardd safety mode
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
put_nonblocking("LongitudinalControl",
"1" if self.CP.openpilotLongitudinalControl else "0")
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)
print('self.CP.lateralTuning.which()={}'.format(
self.CP.lateralTuning.which()))
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.consecutive_can_error_count = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.events_prev = []
self.current_alert_types = []
self.sm['liveCalibration'].calStatus = Calibration.INVALID
self.sm['thermal'].freeSpace = 1.
self.sm['dMonitoringState'].events = []
self.sm['dMonitoringState'].awarenessStatus = 1.
self.sm['dMonitoringState'].faceDetected = False
self.startup_event = get_startup_event(car_recognized,
controller_available, 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 self.read_only and not passive:
self.events.add(EventName.carUnrecognized, static=True)
# if hw_type == HwType.whitePanda:
# self.events.add(EventName.whitePandaUnsupported, static=True)
uname = subprocess.check_output(["uname", "-v"],
encoding='utf8').strip()
if uname == "#1 SMP PREEMPT Wed Jun 10 12:40:53 PDT 2020":
self.events.add(EventName.neosUpdateRequired, 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
self.hyundai_lkas = self.read_only #read_only
self.init_flag = True
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 __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
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'])
# read params
params = Params()
self.is_metric = params.get_bool("IsMetric")
self.is_ldw_enabled = params.get_bool("IsLdwEnabled")
community_feature_toggle = params.get_bool("CommunityFeaturesToggle")
openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle")
passive = params.get_bool("Passive") or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not dashcam mode
controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive and not self.CP.dashcamOnly
community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
self.CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard and boardd safety mode
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP, self.CI.compute_gb)
self.VM = VehicleModel(self.CP)
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.LaC = LatControlAngle(self.CP)
elif self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.can_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.current_alert_types = [ET.PERMANENT]
self.logged_comm_issue = False
self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED
self.sm['deviceState'].freeSpacePercent = 100
self.sm['driverMonitoringState'].events = []
self.sm['driverMonitoringState'].awarenessStatus = 1.
self.sm['driverMonitoringState'].faceDetected = False
self.sm['liveParameters'].valid = True
self.startup_event = get_startup_event(car_recognized,
controller_available)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if community_feature_disallowed:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
elif self.read_only:
self.events.add(EventName.dashcamMode, static=True)
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
