dragonpilot mod for 0.8.5-4

common/basedir.py

@@ -1,11 +1,11 @@
 import os
 from pathlib import Path
 
-from selfdrive.hardware import PC
+from selfdrive.hardware import PC, JETSON
 
 BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../"))
 
-if PC:
+if PC or JETSON:
   PERSIST = os.path.join(str(Path.home()), ".comma", "persist")
 else:
   PERSIST = "/persist"

common/dp_common.py

@@ -0,0 +1,80 @@
+#!/usr/bin/env python3.7
+import subprocess
+from cereal import car
+from common.params import Params
+from common.realtime import sec_since_boot
+import os
+params = Params()
+PARAM_PATH = params.get_params_path() + '/d/'
+LAST_MODIFIED = PARAM_PATH + "dp_last_modified"
+
+def is_online():
+  try:
+    return not subprocess.call(["ping", "-W", "4", "-c", "1", "117.28.245.92"])
+  except ProcessLookupError:
+    return False
+
+def common_controller_ctrl(enabled, dragonconf, blinker_on, steer_req, v_ego):
+  if enabled:
+    if dragonconf.dpLateralMode == 0 and blinker_on:
+      steer_req = 0 if isinstance(steer_req, int) else False
+  return steer_req
+
+def common_interface_atl(ret, atl):
+  # dp
+  enable_acc = ret.cruiseState.enabled
+  if atl and ret.cruiseState.available:
+    enable_acc = True
+    if ret.gearShifter in [car.CarState.GearShifter.reverse, car.CarState.GearShifter.park]:
+      enable_acc = False
+    if ret.seatbeltUnlatched or ret.doorOpen:
+      enable_acc = False
+  return enable_acc
+
+def common_interface_get_params_lqr(ret):
+  if params.get_bool('dp_lqr'):
+    ret.lateralTuning.init('lqr')
+    ret.lateralTuning.lqr.scale = 1500.0
+    ret.lateralTuning.lqr.ki = 0.05
+
+    ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
+    ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
+    ret.lateralTuning.lqr.c = [1., 0.]
+    ret.lateralTuning.lqr.k = [-110.73572306, 451.22718255]
+    ret.lateralTuning.lqr.l = [0.3233671, 0.3185757]
+    ret.lateralTuning.lqr.dcGain = 0.002237852961363602
+  return ret
+
+
+def get_last_modified(delay, old_check, old_modified):
+  new_check = sec_since_boot()
+  if old_check is None or new_check - old_check >= delay:
+    return new_check, os.stat(LAST_MODIFIED).st_mtime
+  else:
+    return old_check, old_modified
+
+def param_get_if_updated(param, type, old_val, old_modified):
+  try:
+    modified = os.stat(PARAM_PATH + param).st_mtime
+  except OSError:
+    return old_val, old_modified
+  if old_modified != modified:
+    new_val = param_get(param, type, old_val)
+    new_modified = modified
+  else:
+    new_val = old_val
+    new_modified = old_modified
+  return new_val, new_modified
+
+def param_get(param_name, type, default):
+  try:
+    val = params.get(param_name, encoding='utf8').rstrip('\x00')
+    if type == 'bool':
+      val = val == '1'
+    elif type == 'int':
+      val = int(val)
+    elif type == 'float':
+      val = float(val)
+  except (TypeError, ValueError):
+    val = default
+  return val

common/dp_conf.py

@@ -0,0 +1,214 @@
+#!/usr/bin/env python3.7
+import os
+import sys
+import json
+import time
+from math import floor
+
+'''
+* type: Bool, Int8, UInt8, UInt16, Float32
+* conf_type: param, struct
+* dependencies needs to use struct and loaded prior so we don't have to read the param multiple times.
+* update_once: True, False (the param will only load up once, need both param and struct defined)
+'''
+confs = [
+  # thermald data
+  {'name': 'dp_thermal_started', 'default': False, 'type': 'Bool', 'conf_type': ['struct']},
+  {'name': 'dp_thermal_overheat', 'default': False, 'type': 'Bool', 'conf_type': ['struct']},
+
+  {'name': 'dp_atl', 'default': False, 'type': 'Bool', 'conf_type': ['param', 'struct'], 'update_once': True},
+  # dashcam related
+  {'name': 'dp_dashcamd', 'default': False, 'type': 'Bool', 'conf_type': ['param', 'struct']},
+  # auto shutdown
+  {'name': 'dp_auto_shutdown', 'default': False, 'type': 'Bool', 'conf_type': ['param', 'struct']},
+  {'name': 'dp_auto_shutdown_in', 'default': 90, 'type': 'UInt16', 'min': 0, 'max': 600, 'depends': [{'name': 'dp_auto_shutdown', 'vals': [True]}], 'conf_type': ['param']},
+  # service
+  {'name': 'dp_updated', 'default': True, 'type': 'Bool', 'conf_type': ['param']},
+  {'name': 'dp_logger', 'default': True, 'type': 'Bool', 'conf_type': ['param']},
+  {'name': 'dp_athenad', 'default': True, 'type': 'Bool', 'depends': [{'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param', 'struct']},
+  {'name': 'dp_uploader', 'default': True, 'type': 'Bool', 'depends': [{'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param', 'struct']},
+  # {'name': 'dp_gpxd', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
+  {'name': 'dp_hotspot_on_boot', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
+  # lat ctrl
+  {'name': 'dp_lateral_mode', 'default': 1, 'type': 'UInt8', 'min': 0, 'max': 2, 'conf_type': ['param', 'struct']},
+  {'name': 'dp_signal_off_delay', 'default': 3., 'type': 'Float32', 'min': 0., 'max': 10., 'depends': [{'name': 'dp_lateral_mode', 'vals': [0]}], 'conf_type': ['param', 'struct']},
+  {'name': 'dp_lc_min_mph', 'default': 45, 'type': 'UInt8', 'min': 0, 'max': 255, 'depends': [{'name': 'dp_lateral_mode', 'vals': [1,2]}], 'conf_type': ['param', 'struct']},
+  {'name': 'dp_lc_auto_cont', 'default': False, 'type': 'Bool', 'depends': [{'name': 'dp_lateral_mode', 'vals': [2]}], 'conf_type': ['param', 'struct']},
+  {'name': 'dp_lc_auto_min_mph', 'default': 60, 'type': 'UInt8', 'min': 0, 'max': 255, 'depends': [{'name': 'dp_lateral_mode', 'vals': [2]}], 'conf_type': ['param', 'struct']},
+  {'name': 'dp_lc_auto_delay', 'default': 3., 'type': 'Float32', 'min': 0., 'max': 10., 'depends': [{'name': 'dp_lateral_mode', 'vals': [2]}], 'conf_type': ['param', 'struct']},
+  # long ctrl
+  {'name': 'dp_allow_gas', 'default': False, 'type': 'Bool', 'depends': [{'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param', 'struct']},
+  {'name': 'dp_following_profile_ctrl', 'default': False, 'type': 'Bool', 'depends': [{'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param', 'struct']},
+  {'name': 'dp_following_profile', 'default': 0, 'type': 'UInt8', 'min': 0, 'max': 2, 'depends': [{'name': 'dp_atl', 'vals': [False]}, {'name': 'dp_following_profile_ctrl', 'vals': [True]}], 'conf_type': ['param', 'struct']},
+  {'name': 'dp_accel_profile_ctrl', 'default': False, 'type': 'Bool', 'depends': [{'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param', 'struct']},
+  {'name': 'dp_accel_profile', 'default': 0, 'type': 'UInt8', 'min': 0, 'max': 2, 'depends': [{'name': 'dp_atl', 'vals': [False]}, {'name': 'dp_accel_profile_ctrl', 'vals': [True]}], 'conf_type': ['param', 'struct']},
+  # safety
+  {'name': 'dp_gear_check', 'default': True, 'type': 'Bool', 'depends': [{'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param', 'struct']},
+  {'name': 'dp_speed_check', 'default': True, 'type': 'Bool', 'conf_type': ['param', 'struct']},
+  {'name': 'dp_temp_monitor', 'default': True, 'type': 'Bool', 'conf_type': ['param']},
+  # UIs
+  {'name': 'dp_ui_display_mode', 'default': 0, 'type': 'UInt8', 'min': 0, 'max': 2, 'conf_type': ['param', 'struct']},
+  {'name': 'dp_ui_speed', 'default': True, 'type': 'Bool', 'conf_type': ['param', 'struct']},
+  {'name': 'dp_ui_event', 'default': True, 'type': 'Bool', 'conf_type': ['param', 'struct']},
+  {'name': 'dp_ui_max_speed', 'default': True, 'type': 'Bool', 'conf_type': ['param', 'struct']},
+  {'name': 'dp_ui_face', 'default': True, 'type': 'Bool', 'conf_type': ['param', 'struct']},
+  {'name': 'dp_ui_lane', 'default': True, 'type': 'Bool', 'conf_type': ['param', 'struct']},
+  {'name': 'dp_ui_lead', 'default': True, 'type': 'Bool', 'conf_type': ['param', 'struct']},
+  {'name': 'dp_ui_dev', 'default': False, 'type': 'Bool', 'conf_type': ['param', 'struct']},
+  {'name': 'dp_ui_dev_mini', 'default': False, 'type': 'Bool', 'conf_type': ['param', 'struct']},
+  {'name': 'dp_ui_blinker', 'default': False, 'type': 'Bool', 'conf_type': ['param', 'struct']},
+  {'name': 'dp_ui_brightness', 'default': 0, 'type': 'UInt8', 'min': 0, 'max': 100, 'conf_type': ['param', 'struct']},
+  {'name': 'dp_ui_volume', 'default': -5, 'type': 'Int8', 'min': -5, 'max': 100, 'conf_type': ['param', 'struct']},
+  # toyota
+  {'name': 'dp_toyota_ldw', 'default': True, 'type': 'Bool', 'conf_type': ['param', 'struct']},
+  {'name': 'dp_toyota_sng', 'default': False, 'type': 'Bool', 'depends': [{'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param', 'struct']},
+  {'name': 'dp_toyota_zss', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
+  {'name': 'dp_toyota_disable_relay', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
+  # hyundai
+  {'name': 'dp_hkg_smart_mdps', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
+  # honda
+  {'name': 'dp_honda_eps_mod', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
+  # volkswagen
+  {'name': 'dp_vw_panda', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
+  {'name': 'dp_vw_timebomb_assist', 'default': False, 'type': 'Bool', 'conf_type': ['param', 'struct']},
+  #misc
+  {'name': 'dp_ip_addr', 'default': '', 'type': 'Text', 'conf_type': ['struct']},
+  {'name': 'dp_fan_mode', 'default': 0, 'type': 'UInt8', 'min': 0, 'max': 2, 'conf_type': ['param']},
+  {'name': 'dp_last_modified', 'default': str(floor(time.time())), 'type': 'Text', 'conf_type': ['param']},
+  {'name': 'dp_camera_offset', 'default': 6, 'type': 'Int8', 'min': -100, 'max': 100, 'conf_type': ['param', 'struct']},
+  {'name': 'dp_path_offset', 'default': 0, 'type': 'Int8', 'min': -100, 'max': 100, 'conf_type': ['param', 'struct']},
+
+  {'name': 'dp_locale', 'default': 'en-US', 'type': 'Text', 'conf_type': ['param', 'struct'], 'update_once': True},
+  {'name': 'dp_reg', 'default': True, 'type': 'Bool', 'conf_type': ['param']},
+  # sr learner related
+  {'name': 'dp_sr_learner', 'default': True, 'type': 'Bool', 'conf_type': ['param', 'struct']},
+  {'name': 'dp_sr_custom', 'default': 9.99, 'min': 9.99, 'max': 30., 'type': 'Float32', 'depends': [{'name': 'dp_sr_learner', 'vals': [False]}], 'conf_type': ['param', 'struct']},
+  {'name': 'dp_sr_stock', 'default': 9.99, 'min': 9.99, 'max': 100., 'type': 'Float32', 'conf_type': ['param']},
+
+  {'name': 'dp_lqr', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
+  {'name': 'dp_reset_live_param_on_start', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
+
+  {'name': 'dp_appd', 'default': False, 'type': 'Bool', 'conf_type': ['param', 'struct']},
+  {'name': 'dp_jetson', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
+  {'name': 'dp_car_assigned', 'default': '', 'type': 'Text', 'conf_type': ['param']},
+  {'name': 'dp_car_list', 'default': '', 'type': 'Text', 'conf_type': ['param']},
+  {'name': 'dp_no_batt', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
+  {'name': 'dp_panda_fake_black', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
+  {'name': 'dp_panda_no_gps', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
+  {'name': 'dp_last_candidate', 'default': '', 'type': 'Text', 'conf_type': ['param']},
+  {'name': 'dp_debug', 'default': False, 'type': 'Bool', 'conf_type': ['param', 'struct']},
+]
+
+def get_definition(name):
+  for conf in confs:
+    if conf['name'] == name:
+      return conf
+  return None
+
+def to_param_val(name, val):
+  conf = get_definition(name)
+  if conf is not None:
+    type = conf['type'].lower()
+    try:
+      if 'bool' in type:
+        val = '1' if val else '0'
+      elif 'int' in type:
+        val = int(val)
+      elif 'float' in type:
+        val = float(val)
+      return str(val)
+    except (ValueError, TypeError):
+      return ''
+  return ''
+
+def to_struct_val(name, val):
+  conf = get_definition(name)
+  if conf is not None:
+    try:
+      type = conf['type'].lower()
+      if 'bool' in type:
+        val = True if val == '1' else False
+      elif 'int' in type:
+        val = int(val)
+      elif 'float' in type:
+        val = float(val)
+      return val
+    except (ValueError, TypeError):
+      return None
+  return None
+
+'''
+function to convert param name into struct name.
+'''
+def get_struct_name(snake_str):
+  components = snake_str.split('_')
+  # We capitalize the first letter of each component except the first one
+  # with the 'title' method and join them together.
+  return components[0] + ''.join(x.title() for x in components[1:])
+
+'''
+function to generate struct for log.capnp
+'''
+def gen_log_struct():
+  count = 0
+  str = "# dp\n"
+  str += "struct DragonConf {\n"
+  for conf in confs:
+    name = get_struct_name(conf['name'])
+    if 'struct' in conf['conf_type']:
+      str += f"  {name} @{count} :{conf['type']};\n"
+      count += 1
+  str += "}"
+  print(str)
+
+'''
+function to generate support car list
+'''
+def get_support_car_list():
+  attrs = ['FINGERPRINTS', 'FW_VERSIONS']
+  cars = dict({"cars": []})
+  for car_folder in [x[0] for x in os.walk('/data/openpilot/selfdrive/car')]:
+    try:
+      car_name = car_folder.split('/')[-1]
+      if car_name != "mock":
+        names = []
+        for attr in attrs:
+          values = __import__('selfdrive.car.%s.values' % car_name, fromlist=[attr])
+          if hasattr(values, attr):
+            attr_values = getattr(values, attr)
+          else:
+            continue
+          if isinstance(attr_values, dict):
+            for f, v in attr_values.items():
+              if f not in names:
+                names.append(f)
+          names.sort()
+        brand_models = {"brand": car_name.upper(), "models": names }
+        cars["cars"].append(brand_models)
+    except (ImportError, IOError, ValueError):
+      pass
+  return json.dumps(cars)
+
+'''
+function to init param value.
+should add this into manager.py
+'''
+def init_params_vals(params):
+  for conf in confs:
+    if 'param' in conf['conf_type']:
+      if conf['name'] == 'dp_car_list':
+        params.put(conf['name'], get_support_car_list())
+      elif params.get(conf['name']) is None:
+        params.put(conf['name'], to_param_val(conf['name'], conf['default']))
+
+def gen_params_cc_keys():
+  for conf in confs:
+    if 'param' in conf['conf_type']:
+      print("    {\"%s\", PERSISTENT}," % conf['name'])
+
+
+if __name__ == "__main__":
+  if (len(sys.argv) > 1) and sys.argv[1] == 'cc':
+    gen_params_cc_keys()
+  else:
+    gen_log_struct()

common/dp_time.py

@@ -0,0 +1,7 @@
+#!/usr/bin/env python3.7
+
+# delay of reading last modified
+LAST_MODIFIED_THERMALD = 10.
+LAST_MODIFIED_SYSTEMD = 1.
+LAST_MODIFIED_LANE_PLANNER = 3.
+LAST_MODIFIED_UPLOADER = 10.

common/i18n.py

@@ -0,0 +1,14 @@
+import gettext
+from selfdrive.hardware import EON
+from selfdrive.hardware.eon.hardware import getprop
+
+locale_dir = '/data/openpilot/selfdrive/assets/locales'
+supported_language = ['en-US', 'zh-TW', 'zh-CN', 'ja-JP', 'ko-KR']
+
+def get_locale():
+  return getprop("persist.sys.locale") if EON else 'en-US'
+
+def events():
+  i18n = gettext.translation('events', localedir=locale_dir, fallback=True, languages=[get_locale()])
+  i18n.install()
+  return i18n.gettext

common/params_pxd.pxd

@@ -26,3 +26,5 @@ cdef extern from "selfdrive/common/params.h":
     int putBool(string, bool)
     bool checkKey(string)
     void clearAll(ParamKeyType)
+
+    string get_params_path()

common/params_pyx.pyx

@@ -97,6 +97,8 @@ cdef class Params:
     cdef string k = self.check_key(key)
     self.p.remove(k)
 
+  def get_params_path(self):
+    return self.p.get_params_path().decode("utf-8")
 
 def put_nonblocking(key, val, d=None):
   def f(key, val):

common/realtime.py

@@ -6,7 +6,7 @@ import multiprocessing
 from typing import Optional
 
 from common.clock import sec_since_boot  # pylint: disable=no-name-in-module, import-error
-from selfdrive.hardware import PC, TICI
+from selfdrive.hardware import PC, TICI, JETSON
 
 
 # time step for each process
@@ -33,12 +33,12 @@ class Priority:
 
 
 def set_realtime_priority(level: int) -> None:
-  if not PC:
+  if JETSON or not PC:
     os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(level))  # type: ignore[attr-defined]
 
 
 def set_core_affinity(core: int) -> None:
-  if not PC:
+  if JETSON or not PC:
     os.sched_setaffinity(0, [core,])
 
 

common/text_window.py

@@ -33,6 +33,7 @@ class TextWindow:
     if self.text_proc is not None:
       while True:
         if self.get_status() == 1:
+          os.system('/data/openpilot/scripts/reset_update.sh')
           return
         time.sleep(0.1)
 

common/transformations/camera.py

@@ -5,12 +5,14 @@ from selfdrive.hardware import TICI
 
 ## -- hardcoded hardware params --
 eon_f_focal_length = 910.0
-eon_d_focal_length = 650.0
+eon_d_focal_length = 860.0
+leon_d_focal_length = 650.0
 tici_f_focal_length = 2648.0
 tici_e_focal_length = tici_d_focal_length = 567.0 # probably wrong? magnification is not consistent across frame
 
 eon_f_frame_size = (1164, 874)
-eon_d_frame_size = (816, 612)
+eon_d_frame_size = (1152, 864)
+leon_d_frame_size = (816, 612)
 tici_f_frame_size = tici_e_frame_size = tici_d_frame_size = (1928, 1208)
 
 # aka 'K' aka camera_frame_from_view_frame
@@ -20,6 +22,11 @@ eon_fcam_intrinsics = np.array([
   [0.0,  0.0,                                          1.0]])
 eon_intrinsics = eon_fcam_intrinsics # xx
 
+leon_dcam_intrinsics = np.array([
+  [leon_d_focal_length,  0.0,  float(leon_d_frame_size[0])/2],
+  [0.0,  leon_d_focal_length,  float(leon_d_frame_size[1])/2],
+  [0.0,  0.0,                                            1.0]])
+
 eon_dcam_intrinsics = np.array([
   [eon_d_focal_length,  0.0,  float(eon_d_frame_size[0])/2],
   [0.0,  eon_d_focal_length,  float(eon_d_frame_size[1])/2],