Merge remote-tracking branch 'upstream/master' into enable-planner

.github/workflows/selfdrive_tests.yaml

@@ -299,17 +299,20 @@ jobs:
                         $UNIT_TEST selfdrive/loggerd && \
                         $UNIT_TEST selfdrive/car && \
                         $UNIT_TEST selfdrive/locationd && \
+                        selfdrive/locationd/test/_test_locationd_lib.py && \
                         $UNIT_TEST selfdrive/athena && \
                         $UNIT_TEST selfdrive/thermald && \
                         $UNIT_TEST selfdrive/hardware/tici && \
                         $UNIT_TEST selfdrive/modeld && \
                         $UNIT_TEST tools/lib/tests && \
+                        ./selfdrive/ui/tests/create_test_translations.sh && \
+                        QT_QPA_PLATFORM=offscreen ./selfdrive/ui/tests/test_translations && \
                         ./common/tests/test_util && \
                         ./common/tests/test_swaglog && \
                         ./selfdrive/boardd/tests/test_boardd_usbprotocol && \
                         ./selfdrive/loggerd/tests/test_logger &&\
                         ./system/proclogd/tests/test_proclog && \
-                        ./selfdrive/ui/replay/tests/test_replay && \
+                        ./tools/replay/tests/test_replay && \
                         ./system/camerad/test/ae_gray_test && \
                         coverage xml"
     - name: "Upload coverage to Codecov"
@@ -364,7 +367,7 @@ jobs:
                         CI=1 AZURE_TOKEN='$AZURE_TOKEN' python selfdrive/test/process_replay/test_processes.py -j$(nproc) --upload-only"
     - name: "Upload coverage to Codecov"
       uses: codecov/codecov-action@v2
-  
+
   model_replay_onnx:
     name: model replay onnx
     runs-on: ubuntu-20.04

README.md

@@ -121,6 +121,7 @@ Directory Structure
         ├── modeld          # Driving and monitoring model runners
         ├── proclogd        # Logs information from proc
         ├── sensord         # IMU interface code
+        ├── navd            # Turn-by-turn navigation
         ├── test            # Unit tests, system tests, and a car simulator
         └── ui              # The UI
 

SConstruct

@@ -356,22 +356,27 @@ Export('cereal', 'messaging', 'visionipc')
 
 # Build rednose library and ekf models
 
+rednose_deps = [
+  "#selfdrive/locationd/models/constants.py",
+  "#selfdrive/locationd/models/gnss_helpers.py",
+]
+
 rednose_config = {
   'generated_folder': '#selfdrive/locationd/models/generated',
   'to_build': {
-    'gnss': ('#selfdrive/locationd/models/gnss_kf.py', True, []),
-    'live': ('#selfdrive/locationd/models/live_kf.py', True, ['live_kf_constants.h']),
-    'car': ('#selfdrive/locationd/models/car_kf.py', True, []),
+    'gnss': ('#selfdrive/locationd/models/gnss_kf.py', True, [], rednose_deps),
+    'live': ('#selfdrive/locationd/models/live_kf.py', True, ['live_kf_constants.h'], rednose_deps),
+    'car': ('#selfdrive/locationd/models/car_kf.py', True, [], rednose_deps),
   },
 }
 
 if arch != "larch64":
   rednose_config['to_build'].update({
-    'loc_4': ('#selfdrive/locationd/models/loc_kf.py', True, []),
-    'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', False, []),
-    'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', False, []),
-    'feature_handler_5': ('#rednose/helpers/feature_handler.py', False, []),
-    'lane': ('#xx/pipeline/lib/ekf/lane_kf.py', True, []),
+    'loc_4': ('#selfdrive/locationd/models/loc_kf.py', True, [], rednose_deps),
+    'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', False, [], []),
+    'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', False, [], []),
+    'feature_handler_5': ('#rednose/helpers/feature_handler.py', False, [], []),
+    'lane': ('#xx/pipeline/lib/ekf/lane_kf.py', True, [], rednose_deps),
   })
 
 Export('rednose_config')
@@ -411,6 +416,8 @@ SConscript(['selfdrive/locationd/SConscript'])
 SConscript(['selfdrive/sensord/SConscript'])
 SConscript(['selfdrive/ui/SConscript'])
 
+SConscript(['tools/replay/SConscript'])
+
 if GetOption('test'):
   SConscript('panda/tests/safety/SConscript')
 

common/params.cc

@@ -128,7 +128,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"IsTakingSnapshot", CLEAR_ON_MANAGER_START},
     {"IsUpdateAvailable", CLEAR_ON_MANAGER_START},
     {"JoystickDebugMode", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
-    {"LaikadEphemeris", PERSISTENT},
+    {"LaikadEphemeris", PERSISTENT | DONT_LOG},
     {"LastAthenaPingTime", CLEAR_ON_MANAGER_START},
     {"LastGPSPosition", PERSISTENT},
     {"LastManagerExitReason", CLEAR_ON_MANAGER_START},

docs/c_docs.rst

@@ -54,7 +54,7 @@ soundd
 replay
 """"""
 .. autodoxygenindex::
-   :project: selfdrive_ui_replay
+   :project: tools_replay
 
 qt
 ""

release/files_common

@@ -296,8 +296,8 @@ selfdrive/ui/qt/offroad/*.qml
 selfdrive/ui/qt/widgets/*.cc
 selfdrive/ui/qt/widgets/*.h
 
-selfdrive/ui/replay/*.cc
-selfdrive/ui/replay/*.h
+tools/replay/*.cc
+tools/replay/*.h
 
 selfdrive/ui/qt/maps/*.cc
 selfdrive/ui/qt/maps/*.h

selfdrive/car/chrysler/carstate.py

@@ -47,8 +47,6 @@ def update(self, cp, cp_cam):
 
     ret.leftBlinker = cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 1
     ret.rightBlinker = cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 2
-    ret.steeringAngleDeg = cp.vl["STEERING"]["STEER_ANGLE"]
-    ret.steeringRateDeg = cp.vl["STEERING"]["STEERING_RATE"]
     ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl["GEAR"]["PRNDL"], None))
 
     ret.cruiseState.available = cp.vl["DAS_3"]["ACC_AVAILABLE"] == 1  # ACC is white
@@ -58,6 +56,8 @@ def update(self, cp, cp_cam):
     ret.cruiseState.nonAdaptive = cp.vl["DASHBOARD"]["CRUISE_STATE"] in (1, 2)
     ret.accFaulted = cp.vl["DAS_3"]["ACC_FAULTED"] != 0
 
+    ret.steeringAngleDeg = cp.vl["STEERING"]["STEER_ANGLE"]
+    ret.steeringRateDeg = cp.vl["STEERING"]["STEERING_RATE"]
     ret.steeringTorque = cp.vl["EPS_STATUS"]["TORQUE_DRIVER"]
     ret.steeringTorqueEps = cp.vl["EPS_STATUS"]["TORQUE_MOTOR"]
     ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD

selfdrive/car/hyundai/carstate.py

@@ -5,7 +5,7 @@
 from common.conversions import Conversions as CV
 from opendbc.can.parser import CANParser
 from opendbc.can.can_define import CANDefine
-from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD, FEATURES, HDA2_CAR, EV_CAR, HYBRID_CAR, Buttons
+from selfdrive.car.hyundai.values import DBC, FEATURES, HDA2_CAR, EV_CAR, HYBRID_CAR, Buttons, CarControllerParams
 from selfdrive.car.interfaces import CarStateBase
 
 PREV_BUTTON_SAMPLES = 4
@@ -32,6 +32,8 @@ def __init__(self, CP):
     self.park_brake = False
     self.buttons_counter = 0
 
+    self.params = CarControllerParams(CP)
+
   def update(self, cp, cp_cam):
     if self.CP.carFingerprint in HDA2_CAR:
       return self.update_hda2(cp, cp_cam)
@@ -61,7 +63,7 @@ def update(self, cp, cp_cam):
       50, cp.vl["CGW1"]["CF_Gway_TurnSigLh"], cp.vl["CGW1"]["CF_Gway_TurnSigRh"])
     ret.steeringTorque = cp.vl["MDPS12"]["CR_Mdps_StrColTq"]
     ret.steeringTorqueEps = cp.vl["MDPS12"]["CR_Mdps_OutTq"]
-    ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
+    ret.steeringPressed = abs(ret.steeringTorque) > self.params.STEER_THRESHOLD
     ret.steerFaultTemporary = cp.vl["MDPS12"]["CF_Mdps_ToiUnavail"] != 0 or cp.vl["MDPS12"]["CF_Mdps_ToiFlt"] != 0
 
     # cruise state
@@ -157,7 +159,7 @@ def update_hda2(self, cp, cp_cam):
     ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] * -1
     ret.steeringTorque = cp.vl["MDPS"]["STEERING_COL_TORQUE"]
     ret.steeringTorqueEps = cp.vl["MDPS"]["STEERING_OUT_TORQUE"]
-    ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
+    ret.steeringPressed = abs(ret.steeringTorque) > self.params.STEER_THRESHOLD
 
     ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"]["LEFT_LAMP"],
                                                                       cp.vl["BLINKERS"]["RIGHT_LAMP"])

selfdrive/car/hyundai/interface.py

@@ -7,7 +7,6 @@
 from selfdrive.car import STD_CARGO_KG, create_button_enable_events, create_button_event, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
 from selfdrive.car.disable_ecu import disable_ecu
-from selfdrive.controls.lib.latcontrol_torque import set_torque_tune
 
 ButtonType = car.CarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName
@@ -51,7 +50,6 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
     ret.stopAccel = 0.0
 
     ret.longitudinalActuatorDelayUpperBound = 1.0  # s
-    torque_params = CarInterfaceBase.get_torque_params(candidate)
     if candidate in (CAR.SANTA_FE, CAR.SANTA_FE_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022):
       ret.lateralTuning.pid.kf = 0.00005
       ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG
@@ -66,7 +64,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
       ret.wheelbase = 2.84
       ret.steerRatio = 13.27 * 1.15   # 15% higher at the center seems reasonable
       tire_stiffness_factor = 0.65
-      set_torque_tune(ret.lateralTuning, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION'])
+      CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
     elif candidate == CAR.SONATA_LF:
       ret.lateralTuning.pid.kf = 0.00005
       ret.mass = 4497. * CV.LB_TO_KG
@@ -96,13 +94,13 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
       ret.wheelbase = 2.72
       ret.steerRatio = 12.9
       tire_stiffness_factor = 0.65
-      set_torque_tune(ret.lateralTuning, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION'])
+      CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
     elif candidate == CAR.ELANTRA_HEV_2021:
       ret.mass = (3017. * CV.LB_TO_KG) + STD_CARGO_KG
       ret.wheelbase = 2.72
       ret.steerRatio = 12.9
       tire_stiffness_factor = 0.65
-      set_torque_tune(ret.lateralTuning, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION'])
+      CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
     elif candidate == CAR.HYUNDAI_GENESIS:
       ret.lateralTuning.pid.kf = 0.00005
       ret.mass = 2060. + STD_CARGO_KG
@@ -204,7 +202,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
       ret.wheelbase = 2.80
       ret.steerRatio = 13.75
       tire_stiffness_factor = 0.5
-      set_torque_tune(ret.lateralTuning, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION'])
+      CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
     elif candidate == CAR.KIA_STINGER:
       ret.lateralTuning.pid.kf = 0.00005
       ret.mass = 1825. + STD_CARGO_KG
@@ -244,7 +242,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
       ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.noOutput),
                            get_safety_config(car.CarParams.SafetyModel.hyundaiHDA2)]
       tire_stiffness_factor = 0.65
-      set_torque_tune(ret.lateralTuning, torque_params['LAT_ACCEL_FACTOR'], torque_params['FRICTION'])
+      CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
 
     # Genesis
     elif candidate == CAR.GENESIS_G70:

selfdrive/car/hyundai/values.py

@@ -13,21 +13,27 @@ class CarControllerParams:
   ACCEL_MAX = 2.0 # m/s
 
   def __init__(self, CP):
+    self.STEER_DELTA_UP = 3
+    self.STEER_DELTA_DOWN = 7
+    self.STEER_DRIVER_ALLOWANCE = 50
+    self.STEER_DRIVER_MULTIPLIER = 2
+    self.STEER_DRIVER_FACTOR = 1
+    self.STEER_THRESHOLD = 150
+
+    if CP.carFingerprint in HDA2_CAR:
+      self.STEER_MAX = 270
+      self.STEER_DRIVER_ALLOWANCE = 250
+      self.STEER_DRIVER_MULTIPLIER = 2
+      self.STEER_THRESHOLD = 250
+
     # To determine the limit for your car, find the maximum value that the stock LKAS will request.
     # If the max stock LKAS request is <384, add your car to this list.
-    if CP.carFingerprint in HDA2_CAR:
-      self.STEER_MAX = 150
     elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.HYUNDAI_GENESIS, CAR.ELANTRA_GT_I30, CAR.IONIQ,
-                             CAR.IONIQ_EV_LTD, CAR.SANTA_FE_PHEV_2022, CAR.SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_HEV,
-                             CAR.KIA_OPTIMA_H, CAR.KIA_SORENTO, CAR.KIA_STINGER):
+                               CAR.IONIQ_EV_LTD, CAR.SANTA_FE_PHEV_2022, CAR.SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_HEV,
+                               CAR.KIA_OPTIMA_H, CAR.KIA_SORENTO, CAR.KIA_STINGER):
       self.STEER_MAX = 255
     else:
       self.STEER_MAX = 384
-    self.STEER_DELTA_UP = 3
-    self.STEER_DELTA_DOWN = 7
-    self.STEER_DRIVER_ALLOWANCE = 50
-    self.STEER_DRIVER_MULTIPLIER = 2
-    self.STEER_DRIVER_FACTOR = 1
 
 
 class CAR:
@@ -1140,9 +1146,6 @@ class Buttons:
       b'\xf1\000DNhe SCC F-CUP      1.00 1.02 99110-L5000         ',
       b'\xf1\x8799110L5000\xf1\000DNhe SCC F-CUP      1.00 1.02 99110-L5000         ',
     ],
-    (Ecu.esp, 0x7b0, None): [
-      b'\xf1\x87\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81\x00\x00\x00\x00\x00\x00\x00\x00',
-    ],
     (Ecu.eps, 0x7d4, None): [
       b'\xf1\x8756310-L5500\xf1\x00DN8 MDPS C 1.00 1.02 56310-L5500 4DNHC102',
       b'\xf1\x8756310-L5450\xf1\x00DN8 MDPS C 1.00 1.02 56310-L5450 4DNHC102',
@@ -1268,5 +1271,3 @@ class Buttons:
   CAR.KIA_EV6: dbc_dict('kia_ev6', None),
   CAR.SONATA_HYBRID: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'),
 }
-
-STEER_THRESHOLD = 150

selfdrive/car/interfaces.py

@@ -20,11 +20,36 @@
 MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS
 ACCEL_MAX = 2.0
 ACCEL_MIN = -3.5
+
 TORQUE_PARAMS_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/params.yaml')
 TORQUE_OVERRIDE_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/override.yaml')
 TORQUE_SUBSTITUTE_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/substitute.yaml')
 
 
+def get_torque_params(candidate):
+  with open(TORQUE_SUBSTITUTE_PATH) as f:
+    sub = yaml.load(f, Loader=yaml.CSafeLoader)
+  if candidate in sub:
+    candidate = sub[candidate]
+
+  with open(TORQUE_PARAMS_PATH) as f:
+    params = yaml.load(f, Loader=yaml.CSafeLoader)
+  with open(TORQUE_OVERRIDE_PATH) as f:
+    override = yaml.load(f, Loader=yaml.CSafeLoader)
+
+  # Ensure no overlap
+  if sum([candidate in x for x in [sub, params, override]]) > 1:
+    raise RuntimeError(f'{candidate} is defined twice in torque config')
+
+  if candidate in override:
+    out = override[candidate]
+  elif candidate in params:
+    out = params[candidate]
+  else:
+    raise NotImplementedError(f"Did not find torque params for {candidate}")
+  return {key: out[i] for i, key in enumerate(params['legend'])}
+
+
 # generic car and radar interfaces
 
 class CarInterfaceBase(ABC):
@@ -85,7 +110,7 @@ def get_std_params(candidate, fingerprint):
     ret.steerControlType = car.CarParams.SteerControlType.torque
     ret.minSteerSpeed = 0.
     ret.wheelSpeedFactor = 1.0
-    ret.maxLateralAccel = CarInterfaceBase.get_torque_params(candidate)['MAX_LAT_ACCEL_MEASURED']
+    ret.maxLateralAccel = get_torque_params(candidate)['MAX_LAT_ACCEL_MEASURED']
 
     ret.pcmCruise = True     # openpilot's state is tied to the PCM's cruise state on most cars
     ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
@@ -110,28 +135,16 @@ def get_std_params(candidate, fingerprint):
     return ret
 
   @staticmethod
-  def get_torque_params(candidate, default=float('NaN')):
-    with open(TORQUE_SUBSTITUTE_PATH) as f:
-      sub = yaml.load(f, Loader=yaml.FullLoader)
-    if candidate in sub:
-      candidate = sub[candidate]
-
-    with open(TORQUE_PARAMS_PATH) as f:
-      params = yaml.load(f, Loader=yaml.FullLoader)
-    with open(TORQUE_OVERRIDE_PATH) as f:
-      override = yaml.load(f, Loader=yaml.FullLoader)
-
-    # Ensure no overlap
-    if sum([candidate in x for x in [sub, params, override]]) > 1:
-      raise RuntimeError(f'{candidate} is defined twice in torque config')
-
-    if candidate in override:
-      out = override[candidate]
-    elif candidate in params:
-      out = params[candidate]
-    else:
-      raise NotImplementedError(f"Did not find torque params for {candidate}")
-    return {key:out[i] for i, key in enumerate(params['legend'])}
+  def configure_torque_tune(candidate, tune, steering_angle_deadzone_deg=0.0):
+    params = get_torque_params(candidate)
+
+    tune.init('torque')
+    tune.torque.useSteeringAngle = True
+    tune.torque.kp = 1.0 / params['LAT_ACCEL_FACTOR']
+    tune.torque.kf = 1.0 / params['LAT_ACCEL_FACTOR']
+    tune.torque.ki = 0.1 / params['LAT_ACCEL_FACTOR']
+    tune.torque.friction = params['FRICTION']
+    tune.torque.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg
 
   @abstractmethod
   def _update(self, c: car.CarControl) -> car.CarState:

selfdrive/car/torque_data/override.yaml

@@ -1,7 +1,7 @@
 legend: [LAT_ACCEL_FACTOR, MAX_LAT_ACCEL_MEASURED, FRICTION]
 ### angle control
 # Nissan appears to have torque
-NISSAN X-TRAIL 2017: [.nan, 1.5, .nan] 
+NISSAN X-TRAIL 2017: [.nan, 1.5, .nan]
 NISSAN ALTIMA 2020: [.nan, 1.5, .nan]
 NISSAN LEAF 2018 Instrument Cluster: [.nan, 1.5, .nan]
 NISSAN LEAF 2018: [.nan, 1.5, .nan]
@@ -20,7 +20,7 @@ FORD FOCUS 4TH GEN: [.nan, 1.5, .nan]
 COMMA BODY: [.nan, 1000, .nan]
 
 # Totally new car
-KIA EV6 2022: [3.0, 2.5, 0.05]
+KIA EV6 2022: [3.0, 2.5, 0.0]
 
 # Dashcam or fallback configured as ideal car
 mock: [10.0, 10, 0.0]