rebase

3 years ago · 4c3b87d518
parent 29d7c8f954 d708a134bd
commit 4c3b87d518
76 changed files with 1147 additions and 871 deletions
--- a/.github/workflows/selfdrive_tests.yaml
+++ b/.github/workflows/selfdrive_tests.yaml
@ -333,7 +333,7 @@ jobs:
    - name: Run replay
      run: |
        ${{ env.RUN }} "scons -j$(nproc) && \
-                        FILEREADER_CACHE=1 CI=1 coverage run selfdrive/test/process_replay/test_processes.py && \
+                        FILEREADER_CACHE=1 CI=1 coverage run selfdrive/test/process_replay/test_processes.py -j$(nproc) && \
                        coverage xml"
    - name: Print diff
      if: always()
@ -348,7 +348,7 @@ jobs:
      if: ${{ failure() && github.event_name == 'pull_request' && github.repository == 'commaai/openpilot' && env.AZURE_TOKEN != '' }}
      run: |
        ${{ env.RUN }} "scons -j$(nproc) && \
-                        CI=1 AZURE_TOKEN='$AZURE_TOKEN' python selfdrive/test/process_replay/test_processes.py --upload-only"
+                        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

--- a/1
+++ b/1
@ -4,6 +4,7 @@ url = "https://pypi.org/simple"
 verify_ssl = true

 [dev-packages]
+av = "*"
 azure-storage-blob = "~=2.1"
 control = "*"
 coverage = "*"
--- a/Pipfile.lock
+++ b/Pipfile.lock
--- a/RELEASES.md
+++ b/RELEASES.md
@ -1,4 +1,7 @@
-Version 0.8.14 (2022-05-30)
+Version 0.8.15 (20XX-XX-XX)
+========================
+
+Version 0.8.14 (2022-06-01)
 ========================
 * New driving model
   * Bigger model, using both of comma three's road-facing cameras
--- a/4
+++ b/4
@ -331,13 +331,13 @@ if GetOption("clazy"):
 Export('env', 'qt_env', 'arch', 'real_arch', 'SHARED', 'USE_WEBCAM')

 SConscript(['common/SConscript'])
-Import('_common', '_gpucommon', '_gpu_libs')
+Import('_common', '_gpucommon')

 if SHARED:
  common, gpucommon = abspath(common), abspath(gpucommon)
 else:
  common = [_common, 'json11']
-  gpucommon = [_gpucommon] + _gpu_libs
+  gpucommon = [_gpucommon]

 Export('common', 'gpucommon')

--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit d61736e301cf27d10b74beac278214f974c55fe4
+Subproject commit f13d16c78a69fdef26b7448520c4398515758861
--- a/common/SConscript
+++ b/common/SConscript
@ -22,13 +22,8 @@ files = [
  'visionimg.cc',
 ]

-if arch == "larch64":
-  _gpu_libs = ["GLESv2"]
-else:
-  _gpu_libs = ["GL"]
-
-_gpucommon = fxn('gpucommon', files, LIBS=_gpu_libs)
-Export('_common', '_gpucommon', '_gpu_libs')
+_gpucommon = fxn('gpucommon', files)
+Export('_common', '_gpucommon')

 if GetOption('test'):
  env.Program('tests/test_util', ['tests/test_util.cc'], LIBS=[_common])
--- a/common/params.cc
+++ b/common/params.cc
@ -144,7 +144,6 @@ std::unordered_map<std::string, uint32_t> keys = {
    {"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
    {"PandaSignatures", CLEAR_ON_MANAGER_START},
    {"Passive", PERSISTENT},
-    {"PrimeRedirected", PERSISTENT},
    {"PrimeType", PERSISTENT},
    {"RecordFront", PERSISTENT},
    {"RecordFrontLock", PERSISTENT},  // for the internal fleet
--- a/common/version.h
+++ b/common/version.h
@ -1 +1 @@
-#define COMMA_VERSION "0.8.14"
+#define COMMA_VERSION "0.8.15"
--- a/docs/CARS.md
+++ b/docs/CARS.md
@ -150,7 +150,7 @@ How We Rate The Cars
 |Volkswagen|Golf SportWagen 2015|Driver Assistance|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
 |Volkswagen|Jetta 2018-21|Driver Assistance|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
 |Volkswagen|Jetta GLI 2021|Driver Assistance|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
-|Volkswagen|Passat 2016-18[<sup>7</sup>](#footnotes)|Driver Assistance|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
+|Volkswagen|Passat 2015-18[<sup>7</sup>](#footnotes)|Driver Assistance|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
 |Volkswagen|Polo 2020|Driver Assistance|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
 |Volkswagen|T-Cross 2021[<sup>8</sup>](#footnotes)|Driver Assistance|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
 |Volkswagen|T-Roc 2021[<sup>8</sup>](#footnotes)|Driver Assistance|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit f5f76d28b4827c3fb706d542729651ceef6c06bd
+Subproject commit 231eafbf659309b85acb5b575b7f898e7a4f196e
--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit 210237fa635eeb76ad855c2031d2cad3bde3a2c0
+Subproject commit 7701277d2666119bc7fcaca9f8cfefd50cd5b071
--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit 36c62afa0c170aa1b7a39bcae3316ffb844499e8
+Subproject commit 6d19b46ef525b0ae224e0bf7db71918e96f9be66
--- a/release/files_common
+++ b/release/files_common
@ -168,7 +168,6 @@ selfdrive/controls/lib/events.py
 selfdrive/controls/lib/lane_planner.py
 selfdrive/controls/lib/latcontrol_angle.py
 selfdrive/controls/lib/latcontrol_indi.py
-selfdrive/controls/lib/latcontrol_lqr.py
 selfdrive/controls/lib/latcontrol_torque.py
 selfdrive/controls/lib/latcontrol_pid.py
 selfdrive/controls/lib/latcontrol.py
@ -491,6 +490,7 @@ opendbc/honda_odyssey_extreme_edition_2018_china_can_generated.dbc
 opendbc/honda_insight_ex_2019_can_generated.dbc
 opendbc/acura_ilx_2016_nidec.dbc

+opendbc/kia_ev6.dbc
 opendbc/hyundai_kia_generic.dbc
 opendbc/hyundai_kia_mando_front_radar.dbc

--- a/selfdrive/athena/athenad.py
+++ b/selfdrive/athena/athenad.py
@ -725,7 +725,6 @@ def main():
                             enable_multithread=True,
                             timeout=30.0)
      cloudlog.event("athenad.main.connected_ws", ws_uri=ws_uri)
-      params.delete("PrimeRedirected")

      conn_retries = 0
      cur_upload_items.clear()
@ -735,22 +734,13 @@ def main():
      break
    except (ConnectionError, TimeoutError, WebSocketException):
      conn_retries += 1
-      params.delete("PrimeRedirected")
      params.delete("LastAthenaPingTime")
    except socket.timeout:
-      try:
-        r = requests.get("http://api.commadotai.com/v1/me", allow_redirects=False,
-                         headers={"User-Agent": f"openpilot-{get_version()}"}, timeout=15.0)
-        if r.status_code == 302 and r.headers['Location'].startswith("http://u.web2go.com"):
-          params.put_bool("PrimeRedirected", True)
-      except Exception:
-        cloudlog.exception("athenad.socket_timeout.exception")
      params.delete("LastAthenaPingTime")
    except Exception:
      cloudlog.exception("athenad.main.exception")

      conn_retries += 1
-      params.delete("PrimeRedirected")
      params.delete("LastAthenaPingTime")

    time.sleep(backoff(conn_retries))
--- a/selfdrive/camerad/cameras/camera_qcom2.cc
+++ b/selfdrive/camerad/cameras/camera_qcom2.cc
@ -74,7 +74,7 @@ const int ANALOG_GAIN_REC_IDX = 0x6; // 0.8x
 const int ANALOG_GAIN_MAX_IDX = 0xD; // 4.0x

 const int EXPOSURE_TIME_MIN = 2; // with HDR, fastest ss
-const int EXPOSURE_TIME_MAX = 1618; // with HDR, slowest ss, 40ms
+const int EXPOSURE_TIME_MAX = 0x0855; // with HDR, slowest ss, 40ms

 // ************** low level camera helpers ****************
 int do_cam_control(int fd, int op_code, void *handle, int size) {
--- a/selfdrive/camerad/cameras/sensor2_i2c.h
+++ b/selfdrive/camerad/cameras/sensor2_i2c.h
@ -49,11 +49,15 @@ struct i2c_random_wr_payload init_array_ar0231[] = {
  {0x301A, 0x0018}, // RESET_REGISTER

  // CLOCK Settings
+  // input clock is 19.2 / 2 * 0x37 = 528 MHz
+  // pixclk is 528 / 6 = 88 MHz
+  // full roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*FRAME_LENGTH_LINES)) = 39.99 ms
+  // img  roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*Y_OUTPUT_CONTROL))   = 22.85 ms
  {0x302A, 0x0006}, // VT_PIX_CLK_DIV
  {0x302C, 0x0001}, // VT_SYS_CLK_DIV
  {0x302E, 0x0002}, // PRE_PLL_CLK_DIV
-  {0x3030, 0x0032}, // PLL_MULTIPLIER
-  {0x3036, 0x000C}, // OP_WORD_CLK_DIV
+  {0x3030, 0x0037}, // PLL_MULTIPLIER
+  {0x3036, 0x000C}, // OP_PIX_CLK_DIV
  {0x3038, 0x0001}, // OP_SYS_CLK_DIV

  // FORMAT
@ -76,8 +80,8 @@ struct i2c_random_wr_payload init_array_ar0231[] = {
  {0x340C, 0x802},  // GPIO_HIDRV_EN | GPIO0_ISEL=2

  // Readout timing
-  {0x300C, 0x07B9}, // LINE_LENGTH_PCK
-  {0x300A, 0x0652}, // FRAME_LENGTH_LINES
+  {0x300C, 0x0672}, // LINE_LENGTH_PCK (valid for 3-exposure HDR)
+  {0x300A, 0x0855}, // FRAME_LENGTH_LINES
  {0x3042, 0x0000}, // EXTRA_DELAY

  // Readout Settings
@ -117,6 +121,8 @@ struct i2c_random_wr_payload init_array_ar0231[] = {
  {0x100C, 0x0589}, // FINE_INTEGRATION_TIME2_MIN
  {0x100E, 0x07B1}, // FINE_INTEGRATION_TIME3_MIN
  {0x1010, 0x0139}, // FINE_INTEGRATION_TIME4_MIN
+
+  // TODO: do these have to be lower than LINE_LENGTH_PCK?
  {0x3014, 0x08CB}, // FINE_INTEGRATION_TIME_
  {0x321E, 0x0894}, // FINE_INTEGRATION_TIME2

--- a/selfdrive/camerad/snapshot/snapshot.py
+++ b/selfdrive/camerad/snapshot/snapshot.py
@ -7,7 +7,7 @@ from PIL import Image
 from typing import List

 import cereal.messaging as messaging
-from cereal.visionipc.visionipc_pyx import VisionIpcClient, VisionStreamType  # pylint: disable=no-name-in-module, import-error
+from cereal.visionipc import VisionIpcClient, VisionStreamType
 from common.params import Params
 from common.realtime import DT_MDL
 from selfdrive.hardware import TICI, PC
--- a/selfdrive/car/chrysler/values.py
+++ b/selfdrive/car/chrysler/values.py
@ -1,6 +1,6 @@
 from dataclasses import dataclass
 from enum import Enum
-from typing import Dict, List, Union
+from typing import Dict, List, Optional, Union

 from selfdrive.car import dbc_dict
 from selfdrive.car.docs_definitions import CarInfo, Harness
@ -31,8 +31,9 @@ class ChryslerCarInfo(CarInfo):
  harness: Enum = Harness.fca


-CAR_INFO: Dict[str, Union[ChryslerCarInfo, List[ChryslerCarInfo]]] = {
+CAR_INFO: Dict[str, Optional[Union[ChryslerCarInfo, List[ChryslerCarInfo]]]] = {
  CAR.PACIFICA_2017_HYBRID: ChryslerCarInfo("Chrysler Pacifica Hybrid 2017-18"),
+  CAR.PACIFICA_2018_HYBRID: None,  # same platforms
  CAR.PACIFICA_2019_HYBRID: ChryslerCarInfo("Chrysler Pacifica Hybrid 2019-21"),
  CAR.PACIFICA_2018: ChryslerCarInfo("Chrysler Pacifica 2017-18"),
  CAR.PACIFICA_2020: ChryslerCarInfo("Chrysler Pacifica 2020"),
--- a/selfdrive/car/docs.py
+++ b/selfdrive/car/docs.py
@ -34,7 +34,7 @@ def get_all_car_info() -> List[CarInfo]:
      fingerprint = {0: {}, 1: {HKG_RADAR_START_ADDR: 8}, 2: {}, 3: {}}
      CP = interfaces[model][0].get_params(model, fingerprint=fingerprint, disable_radar=True)

-      if CP.dashcamOnly:
+      if CP.dashcamOnly or car_info is None:
        continue

      # A platform can include multiple car models
--- a/selfdrive/car/ford/values.py
+++ b/selfdrive/car/ford/values.py
@ -35,6 +35,8 @@ class CAR:


 CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
+  CAR.ESCAPE_MK4: CarInfo("Ford Escape", "NA"),
+  CAR.FOCUS_MK4: CarInfo("Ford Focus", "NA"),
 }


--- a/selfdrive/car/gm/carstate.py
+++ b/selfdrive/car/gm/carstate.py
@ -28,10 +28,15 @@ class CarState(CarStateBase):
    ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
    ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
    ret.standstill = ret.vEgoRaw < 0.01
-
    ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]["PRNDL"], None))
-    ret.brakePressed = pt_cp.vl["ECMEngineStatus"]["Brake_Pressed"] != 0
+
+    # Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed.
    ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] / 0xd0
+    ret.brakePressed = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] >= 10
+
+    # Regen braking is braking
+    if self.car_fingerprint == CAR.VOLT:
+      ret.brakePressed = ret.brakePressed or pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"] != 0

    ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254.
    ret.gasPressed = ret.gas > 1e-5
@ -64,10 +69,6 @@ class CarState(CarStateBase):
    ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1
    ret.accFaulted = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.FAULTED

-    # Regen braking is braking
-    if self.car_fingerprint == CAR.VOLT:
-      ret.brakePressed = ret.brakePressed or pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"] != 0
-
    ret.cruiseState.enabled = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] != AccState.OFF
    ret.cruiseState.standstill = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.STANDSTILL

@ -101,7 +102,6 @@ class CarState(CarStateBase):
      ("TractionControlOn", "ESPStatus"),
      ("EPBClosed", "EPBStatus"),
      ("CruiseMainOn", "ECMEngineStatus"),
-      ("Brake_Pressed", "ECMEngineStatus"),
    ]

    checks = [
--- a/selfdrive/car/honda/carcontroller.py
+++ b/selfdrive/car/honda/carcontroller.py
@ -222,7 +222,7 @@ class CarController:
        if self.CP.carFingerprint in HONDA_BOSCH:
          self.accel = clip(accel, self.params.BOSCH_ACCEL_MIN, self.params.BOSCH_ACCEL_MAX)
          self.gas = interp(accel, self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V)
-          can_sends.extend(hondacan.create_acc_commands(self.packer, CC.enabled, CC.longActive, accel, self.gas,
+          can_sends.extend(hondacan.create_acc_commands(self.packer, CC.enabled, CC.longActive, self.accel, self.gas,
                                                        idx, stopping, self.CP.carFingerprint))
        else:
          apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0)
--- a/selfdrive/car/honda/values.py
+++ b/selfdrive/car/honda/values.py
@ -1,6 +1,6 @@
 from dataclasses import dataclass
 from enum import Enum, IntFlag
-from typing import Dict, List, Union
+from typing import Dict, List, Optional, Union

 from cereal import car
 from common.conversions import Conversions as CV
@ -106,7 +106,7 @@ class HondaCarInfo(CarInfo):
  min_steer_speed: float = 12. * CV.MPH_TO_MS


-CAR_INFO: Dict[str, Union[HondaCarInfo, List[HondaCarInfo]]] = {
+CAR_INFO: Dict[str, Optional[Union[HondaCarInfo, List[HondaCarInfo]]]] = {
  CAR.ACCORD: [
    HondaCarInfo("Honda Accord 2018-21", "All", video_link="https://www.youtube.com/watch?v=mrUwlj3Mi58", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch),
    HondaCarInfo("Honda Inspire 2018", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch),
@ -117,15 +117,17 @@ CAR_INFO: Dict[str, Union[HondaCarInfo, List[HondaCarInfo]]] = {
    HondaCarInfo("Honda Civic 2019-20", "All", video_link="https://www.youtube.com/watch?v=4Iz1Mz5LGF8", footnotes=[Footnote.CIVIC_DIESEL], min_steer_speed=2. * CV.MPH_TO_MS, harness=Harness.bosch),
    HondaCarInfo("Honda Civic Hatchback 2017-21", harness=Harness.bosch),
  ],
+  CAR.CIVIC_BOSCH_DIESEL: None,  # same platform
  CAR.ACURA_ILX: HondaCarInfo("Acura ILX 2016-19", "AcuraWatch Plus", min_steer_speed=25. * CV.MPH_TO_MS, harness=Harness.nidec),
  CAR.CRV: HondaCarInfo("Honda CR-V 2015-16", "Touring", harness=Harness.nidec),
  CAR.CRV_5G: HondaCarInfo("Honda CR-V 2017-21", harness=Harness.bosch),
-  # CAR.CRV_EU: HondaCarInfo("Honda CR-V EU", "Touring"),  # Euro version of CRV Touring
+  CAR.CRV_EU: None,  # HondaCarInfo("Honda CR-V EU", "Touring"),  # Euro version of CRV Touring
  CAR.CRV_HYBRID: HondaCarInfo("Honda CR-V Hybrid 2017-19", harness=Harness.bosch),
  CAR.FIT: HondaCarInfo("Honda Fit 2018-19", harness=Harness.nidec),
  CAR.FREED: HondaCarInfo("Honda Freed 2020", harness=Harness.nidec),
  CAR.HRV: HondaCarInfo("Honda HR-V 2019-20", harness=Harness.nidec),
  CAR.ODYSSEY: HondaCarInfo("Honda Odyssey 2018-20", min_steer_speed=0., harness=Harness.nidec),
+  CAR.ODYSSEY_CHN: None,  # Chinese version of Odyssey
  CAR.ACURA_RDX: HondaCarInfo("Acura RDX 2016-18", "AcuraWatch Plus", harness=Harness.nidec),
  CAR.ACURA_RDX_3G: HondaCarInfo("Acura RDX 2019-21", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch),
  CAR.PILOT: HondaCarInfo("Honda Pilot 2016-21", harness=Harness.nidec),
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@ -3,8 +3,8 @@ from common.realtime import DT_CTRL
 from common.numpy_fast import clip, interp
 from common.conversions import Conversions as CV
 from selfdrive.car import apply_std_steer_torque_limits
-from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11, create_lfahda_mfc, create_acc_commands, create_acc_opt, create_frt_radar_opt
-from selfdrive.car.hyundai.values import Buttons, CarControllerParams, CAR
+from selfdrive.car.hyundai import hda2can, hyundaican
+from selfdrive.car.hyundai.values import Buttons, CarControllerParams, HDA2_CAR, CAR
 from opendbc.can.packer import CANPacker

 VisualAlert = car.CarControl.HUDControl.VisualAlert
@ -44,13 +44,12 @@ class CarController:
    self.apply_steer_last = 0
    self.car_fingerprint = CP.carFingerprint
    self.steer_rate_limited = False
-    self.last_resume_frame = 0
+    self.last_button_frame = 0
    self.accel = 0

  def update(self, CC, CS):
    actuators = CC.actuators
    hud_control = CC.hudControl
-    pcm_cancel_cmd = CC.cruiseControl.cancel

    # Steering Torque
    new_steer = int(round(actuators.steer * self.params.STEER_MAX))
@ -67,58 +66,75 @@ class CarController:

    can_sends = []

-    # tester present - w/ no response (keeps radar disabled)
-    if self.CP.openpilotLongitudinalControl:
-      if self.frame % 100 == 0:
-        can_sends.append([0x7D0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0])
-
-    can_sends.append(create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, CC.latActive,
-                                   CS.lkas11, sys_warning, sys_state, CC.enabled,
-                                   hud_control.leftLaneVisible, hud_control.rightLaneVisible,
-                                   left_lane_warning, right_lane_warning))
-
-    if not self.CP.openpilotLongitudinalControl:
-      if pcm_cancel_cmd:
-        can_sends.append(create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL))
-      elif CS.out.cruiseState.standstill:
-        # send resume at a max freq of 10Hz
-        if (self.frame - self.last_resume_frame) * DT_CTRL > 0.1:
-          # send 25 messages at a time to increases the likelihood of resume being accepted
-          can_sends.extend([create_clu11(self.packer, self.frame, CS.clu11, Buttons.RES_ACCEL)] * 25)
-          self.last_resume_frame = self.frame
-
-    if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
-      accel = actuators.accel
-      jerk = 0
-
-      if CC.longActive:
-        jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7)
-        if accel < 0:
-          accel = interp(accel - CS.out.aEgo, [-1.0, -0.5], [2 * accel, accel])
-
-      accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
-
-      lead_visible = False
-      stopping = actuators.longControlState == LongCtrlState.stopping
-      set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH)
-      can_sends.extend(create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2), lead_visible,
-                                           set_speed_in_units, stopping, CS.out.gasPressed))
-      self.accel = accel
-
-    # 20 Hz LFA MFA message
-    if self.frame % 5 == 0 and self.car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021,
-                                                        CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV,
-                                                        CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022,
-                                                        CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022):
-      can_sends.append(create_lfahda_mfc(self.packer, CC.enabled))
-
-    # 5 Hz ACC options
-    if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:
-      can_sends.extend(create_acc_opt(self.packer))
-
-    # 2 Hz front radar options
-    if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl:
-      can_sends.append(create_frt_radar_opt(self.packer))
+    if self.CP.carFingerprint in HDA2_CAR:
+      # steering control
+      can_sends.append(hda2can.create_lkas(self.packer, CC.enabled, self.frame, CC.latActive, apply_steer))
+
+      # cruise cancel
+      if (self.frame - self.last_button_frame) * DT_CTRL > 0.25:
+        if CC.cruiseControl.cancel:
+          for _ in range(20):
+            can_sends.append(hda2can.create_buttons(self.packer, CS.buttons_counter+1, True, False))
+          self.last_button_frame = self.frame
+
+        # cruise standstill resume
+        elif CC.enabled and CS.out.cruiseState.standstill:
+          can_sends.append(hda2can.create_buttons(self.packer, CS.buttons_counter+1, False, True))
+          self.last_button_frame = self.frame
+    else:
+
+      # tester present - w/ no response (keeps radar disabled)
+      if self.CP.openpilotLongitudinalControl:
+        if self.frame % 100 == 0:
+          can_sends.append([0x7D0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0])
+
+      can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, CC.latActive,
+                                     CS.lkas11, sys_warning, sys_state, CC.enabled,
+                                     hud_control.leftLaneVisible, hud_control.rightLaneVisible,
+                                     left_lane_warning, right_lane_warning))
+
+      if not self.CP.openpilotLongitudinalControl:
+        if CC.cruiseControl.cancel:
+          can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL))
+        elif CS.out.cruiseState.standstill:
+          # send resume at a max freq of 10Hz
+          if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
+            # send 25 messages at a time to increases the likelihood of resume being accepted
+            can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.RES_ACCEL)] * 25)
+            self.last_button_frame = self.frame
+
+      if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
+        accel = actuators.accel
+        jerk = 0
+
+        if CC.longActive:
+          jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7)
+          if accel < 0:
+            accel = interp(accel - CS.out.aEgo, [-1.0, -0.5], [2 * accel, accel])
+
+        accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
+
+        lead_visible = False
+        stopping = actuators.longControlState == LongCtrlState.stopping
+        set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH)
+        can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2), lead_visible,
+                                             set_speed_in_units, stopping, CS.out.gasPressed))
+        self.accel = accel
+
+      # 20 Hz LFA MFA message
+      if self.frame % 5 == 0 and self.car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021,
+                                                          CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV,
+                                                          CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022,
+                                                          CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022):
+        can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled))
+
+      # 5 Hz ACC options
+      if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:
+        can_sends.extend(hyundaican.create_acc_opt(self.packer))
+
+      # 2 Hz front radar options
+      if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl:
+        can_sends.append(hyundaican.create_frt_radar_opt(self.packer))

    new_actuators = actuators.copy()
    new_actuators.steer = apply_steer / self.params.STEER_MAX
--- a/selfdrive/car/hyundai/carstate.py
+++ b/selfdrive/car/hyundai/carstate.py
@ -5,7 +5,7 @@ from cereal import car
 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, EV_CAR, HYBRID_CAR, Buttons
+from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD, FEATURES, HDA2_CAR, EV_CAR, HYBRID_CAR, Buttons
 from selfdrive.car.interfaces import CarStateBase

 PREV_BUTTON_SAMPLES = 4
@ -19,14 +19,23 @@ class CarState(CarStateBase):
    self.cruise_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)
    self.main_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)

-    if self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
+    if CP.carFingerprint in HDA2_CAR:
+      self.shifter_values = can_define.dv["ACCELERATOR"]["GEAR"]
+    elif self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
      self.shifter_values = can_define.dv["CLU15"]["CF_Clu_Gear"]
    elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
      self.shifter_values = can_define.dv["TCU12"]["CUR_GR"]
    else:  # preferred and elect gear methods use same definition
      self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"]

+    self.brake_error = False
+    self.park_brake = False
+    self.buttons_counter = 0
+
  def update(self, cp, cp_cam):
+    if self.CP.carFingerprint in HDA2_CAR:
+      return self.update_hda2(cp, cp_cam)
+
    ret = car.CarState.new_message()

    ret.doorOpen = any([cp.vl["CGW1"]["CF_Gway_DrvDrSw"], cp.vl["CGW1"]["CF_Gway_AstDrSw"],
@ -120,10 +129,57 @@ class CarState(CarStateBase):

    return ret

+  def update_hda2(self, cp, cp_cam):
+    ret = car.CarState.new_message()
+
+    ret.gas = cp.vl["ACCELERATOR"]["ACCELERATOR_PEDAL"] / 255.
+    ret.gasPressed = ret.gas > 1e-3
+    ret.brakePressed = cp.vl["BRAKE"]["BRAKE_PRESSED"] == 1
+
+    ret.doorOpen = cp.vl["DOORS_SEATBELTS"]["DRIVER_DOOR_OPEN"] == 1
+    ret.seatbeltUnlatched = cp.vl["DOORS_SEATBELTS"]["DRIVER_SEATBELT_LATCHED"] == 0
+
+    gear = cp.vl["ACCELERATOR"]["GEAR"]
+    ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear))
+
+    # TODO: figure out positions
+    ret.wheelSpeeds = self.get_wheel_speeds(
+      cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_1"],
+      cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_2"],
+      cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_3"],
+      cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_4"],
+    )
+    ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
+    ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
+    ret.standstill = ret.vEgoRaw < 0.1
+
+    ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEERING_RATE"]
+    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.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"]["LEFT_LAMP"],
+                                                                      cp.vl["BLINKERS"]["RIGHT_LAMP"])
+
+    ret.cruiseState.available = True
+    ret.cruiseState.enabled = cp.vl["SCC1"]["CRUISE_ACTIVE"] == 1
+    ret.cruiseState.standstill = cp.vl["CRUISE_INFO"]["CRUISE_STANDSTILL"] == 1
+
+    speed_factor = CV.MPH_TO_MS if cp.vl["CLUSTER_INFO"]["DISTANCE_UNIT"] == 1 else CV.KPH_TO_MS
+    ret.cruiseState.speed = cp.vl["CRUISE_INFO"]["SET_SPEED"] * speed_factor
+
+    self.buttons_counter = cp.vl["CRUISE_BUTTONS"]["_COUNTER"]
+
+    return ret
+
  @staticmethod
  def get_can_parser(CP):
+    if CP.carFingerprint in HDA2_CAR:
+      return CarState.get_can_parser_hda2(CP)
+
    signals = [
-      # sig_name, sig_address
+      # signal_name, signal_address
      ("WHL_SPD_FL", "WHL_SPD11"),
      ("WHL_SPD_FR", "WHL_SPD11"),
      ("WHL_SPD_RL", "WHL_SPD11"),
@ -135,9 +191,9 @@ class CarState(CarStateBase):

      ("CF_Gway_DrvSeatBeltSw", "CGW1"),
      ("CF_Gway_DrvDrSw", "CGW1"),       # Driver Door
-      ("CF_Gway_AstDrSw", "CGW1"),       # Passenger door
-      ("CF_Gway_RLDrSw", "CGW2"),        # Rear reft door
-      ("CF_Gway_RRDrSw", "CGW2"),        # Rear right door
+      ("CF_Gway_AstDrSw", "CGW1"),       # Passenger Door
+      ("CF_Gway_RLDrSw", "CGW2"),        # Rear left Door
+      ("CF_Gway_RRDrSw", "CGW2"),        # Rear right Door
      ("CF_Gway_TurnSigLh", "CGW1"),
      ("CF_Gway_TurnSigRh", "CGW1"),
      ("CF_Gway_ParkBrakeSw", "CGW1"),
@ -175,7 +231,6 @@ class CarState(CarStateBase):
      ("SAS_Angle", "SAS11"),
      ("SAS_Speed", "SAS11"),
    ]
-
    checks = [
      # address, frequency
      ("MDPS12", 50),
@ -198,7 +253,6 @@ class CarState(CarStateBase):
        ("ACC_ObjDist", "SCC11"),
        ("ACCMode", "SCC12"),
      ]
-
      checks += [
        ("SCC11", 50),
        ("SCC12", 50),
@ -256,8 +310,11 @@ class CarState(CarStateBase):

  @staticmethod
  def get_cam_can_parser(CP):
+    if CP.carFingerprint in HDA2_CAR:
+      return None
+
    signals = [
-      # sig_name, sig_address
+      # signal_name, signal_address
      ("CF_Lkas_LdwsActivemode", "LKAS11"),
      ("CF_Lkas_LdwsSysState", "LKAS11"),
      ("CF_Lkas_SysWarning", "LKAS11"),
@ -274,9 +331,55 @@ class CarState(CarStateBase):
      ("CF_Lkas_FcwOpt_USM", "LKAS11"),
      ("CF_Lkas_LdwsOpt_USM", "LKAS11"),
    ]
-
    checks = [
      ("LKAS11", 100)
    ]

    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
+
+  @staticmethod
+  def get_can_parser_hda2(CP):
+    signals = [
+      ("WHEEL_SPEED_1", "WHEEL_SPEEDS"),
+      ("WHEEL_SPEED_2", "WHEEL_SPEEDS"),
+      ("WHEEL_SPEED_3", "WHEEL_SPEEDS"),
+      ("WHEEL_SPEED_4", "WHEEL_SPEEDS"),
+
+      ("ACCELERATOR_PEDAL", "ACCELERATOR"),
+      ("GEAR", "ACCELERATOR"),
+      ("BRAKE_PRESSED", "BRAKE"),
+
+      ("STEERING_RATE", "STEERING_SENSORS"),
+      ("STEERING_ANGLE", "STEERING_SENSORS"),
+      ("STEERING_COL_TORQUE", "MDPS"),
+      ("STEERING_OUT_TORQUE", "MDPS"),
+
+      ("CRUISE_ACTIVE", "SCC1"),
+      ("SET_SPEED", "CRUISE_INFO"),
+      ("CRUISE_STANDSTILL", "CRUISE_INFO"),
+      ("_COUNTER", "CRUISE_BUTTONS"),
+
+      ("DISTANCE_UNIT", "CLUSTER_INFO"),
+
+      ("LEFT_LAMP", "BLINKERS"),
+      ("RIGHT_LAMP", "BLINKERS"),
+
+      ("DRIVER_DOOR_OPEN", "DOORS_SEATBELTS"),
+      ("DRIVER_SEATBELT_LATCHED", "DOORS_SEATBELTS"),
+    ]
+
+    checks = [
+      ("WHEEL_SPEEDS", 100),
+      ("ACCELERATOR", 100),
+      ("BRAKE", 100),
+      ("STEERING_SENSORS", 100),
+      ("MDPS", 100),
+      ("SCC1", 50),
+      ("CRUISE_INFO", 50),
+      ("CRUISE_BUTTONS", 50),
+      ("CLUSTER_INFO", 4),
+      ("BLINKERS", 4),
+      ("DOORS_SEATBELTS", 4),
+    ]
+
+    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 5)
--- a/selfdrive/car/hyundai/hda2can.py
+++ b/selfdrive/car/hyundai/hda2can.py
@ -0,0 +1,23 @@
+def create_lkas(packer, enabled, frame, lat_active, apply_steer):
+  values = {
+    "LKA_MODE": 2,
+    "LKA_ICON": 2 if enabled else 1,
+    "TORQUE_REQUEST": apply_steer,
+    "LKA_ASSIST": 0,
+    "STEER_REQ": 1 if lat_active else 0,
+    "STEER_MODE": 0,
+    "SET_ME_1": 0,
+    "NEW_SIGNAL_1": 0,
+    "NEW_SIGNAL_2": 0,
+  }
+  return packer.make_can_msg("LKAS", 4, values, frame % 255)
+
+
+def create_buttons(packer, cnt, cancel, resume):
+  values = {
+    "_COUNTER": cnt % 0xf,
+    "SET_ME_1": 1,
+    "DISTANCE_BTN": 1 if resume else 0,
+    "PAUSE_RESUME_BTN": 1 if cancel else 0,
+  }
+  return packer.make_can_msg("CRUISE_BUTTONS", 5, values)
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@ -2,11 +2,12 @@
 from cereal import car
 from panda import Panda
 from common.conversions import Conversions as CV
-from selfdrive.car.hyundai.values import CAR, DBC, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, Buttons, CarControllerParams
+from selfdrive.car.hyundai.values import CAR, DBC, HDA2_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, Buttons, CarControllerParams
 from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR
 from selfdrive.car import STD_CARGO_KG, 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
@ -34,7 +35,7 @@ class CarInterface(CarInterfaceBase):
    # These cars have been put into dashcam only due to both a lack of users and test coverage.
    # These cars likely still work fine. Once a user confirms each car works and a test route is
    # added to selfdrive/car/tests/routes.py, we can remove it from this list.
-    ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, CAR.ELANTRA_GT_I30}
+    ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, CAR.ELANTRA_GT_I30} or candidate in HDA2_CAR

    ret.steerActuatorDelay = 0.1  # Default delay
    ret.steerRateCost = 0.5
@ -48,7 +49,7 @@ class CarInterface(CarInterfaceBase):
    ret.longitudinalTuning.kiV = [0.0]
    ret.stopAccel = 0.0

-    ret.longitudinalActuatorDelayUpperBound = 1.0 # s
+    ret.longitudinalActuatorDelayUpperBound = 1.0  # s

    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
@ -243,6 +244,16 @@ class CarInterface(CarInterfaceBase):
      tire_stiffness_factor = 0.5
      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
+    elif candidate == CAR.KIA_EV6:
+      ret.mass = 2055 + STD_CARGO_KG
+      ret.wheelbase = 2.9
+      ret.steerRatio = 16.
+      ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.noOutput),
+                           get_safety_config(car.CarParams.SafetyModel.hyundaiHDA2)]
+      tire_stiffness_factor = 0.65
+
+      max_lat_accel = 2.
+      set_torque_tune(ret.lateralTuning, max_lat_accel, 0.01)

    # Genesis
    elif candidate == CAR.GENESIS_G70:
@ -321,7 +332,8 @@ class CarInterface(CarInterfaceBase):
    # To avoid re-engaging when openpilot cancels, check user engagement intention via buttons
    # Main button also can trigger an engagement on these cars
    allow_enable = any(btn in ENABLE_BUTTONS for btn in self.CS.cruise_buttons) or any(self.CS.main_buttons)
-    events = self.create_common_events(ret, pcm_enable=self.CS.CP.pcmCruise, allow_enable=allow_enable)
+    allow_enable = allow_enable or self.CP.carFingerprint in HDA2_CAR
+    events = self.create_common_events(ret, pcm_enable=self.CS.CP.pcmCruise, allow_enable=allow_enable or True)

    if self.CS.brake_error:
      events.add(EventName.brakeUnavailable)
--- a/selfdrive/car/hyundai/values.py
+++ b/selfdrive/car/hyundai/values.py
@ -1,5 +1,5 @@
 from dataclasses import dataclass
-from typing import Dict, List, Union
+from typing import Dict, List, Optional, Union

 from cereal import car
 from common.conversions import Conversions as CV
@ -7,7 +7,7 @@ from selfdrive.car import dbc_dict
 from selfdrive.car.docs_definitions import CarInfo, Harness
 Ecu = car.CarParams.Ecu

-# Steer torque limits
+
 class CarControllerParams:
  ACCEL_MIN = -3.5 # m/s
  ACCEL_MAX = 2.0 # m/s
@ -15,7 +15,9 @@ class CarControllerParams:
  def __init__(self, CP):
    # 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 (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.HYUNDAI_GENESIS, CAR.ELANTRA_GT_I30, CAR.IONIQ,
+    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):
      self.STEER_MAX = 255
@ -67,6 +69,7 @@ class CAR:
  KIA_SORENTO = "KIA SORENTO GT LINE 2018"
  KIA_STINGER = "KIA STINGER GT2 2018"
  KIA_CEED = "KIA CEED INTRO ED 2019"
+  KIA_EV6 = "KIA EV6 2022"

  # Genesis
  GENESIS_G70 = "GENESIS G70 2018"
@ -81,10 +84,11 @@ class HyundaiCarInfo(CarInfo):
  good_torque: bool = True


-CAR_INFO: Dict[str, Union[HyundaiCarInfo, List[HyundaiCarInfo]]] = {
+CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
  CAR.ELANTRA: HyundaiCarInfo("Hyundai Elantra 2017-19", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_b),
  CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-22", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k),
  CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021-22", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k),
+  CAR.ELANTRA_GT_I30: None,  # dashcamOnly and same platform as CAR.ELANTRA
  CAR.HYUNDAI_GENESIS: HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_j),
  CAR.IONIQ: HyundaiCarInfo("Hyundai Ioniq Hybrid 2017-19", harness=Harness.hyundai_c),
  CAR.IONIQ_HEV_2022: HyundaiCarInfo("Hyundai Ioniq Hybrid 2020-22", "SCC + LFA", harness=Harness.hyundai_h),
@ -129,6 +133,7 @@ CAR_INFO: Dict[str, Union[HyundaiCarInfo, List[HyundaiCarInfo]]] = {
    HyundaiCarInfo("Kia Optima 2017", min_steer_speed=32. * CV.MPH_TO_MS, harness=Harness.hyundai_b),
    HyundaiCarInfo("Kia Optima 2019", harness=Harness.hyundai_g),
  ],
+  CAR.KIA_OPTIMA_H: HyundaiCarInfo("Kia Optima 2017, 2019"),  # TODO: info may be incorrect
  CAR.KIA_SELTOS: HyundaiCarInfo("Kia Seltos 2021", harness=Harness.hyundai_a),
  CAR.KIA_SORENTO: [
    HyundaiCarInfo("Kia Sorento 2018", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", harness=Harness.hyundai_c),
@ -136,6 +141,7 @@ CAR_INFO: Dict[str, Union[HyundaiCarInfo, List[HyundaiCarInfo]]] = {
  ],
  CAR.KIA_STINGER: HyundaiCarInfo("Kia Stinger 2018", video_link="https://www.youtube.com/watch?v=MJ94qoofYw0", harness=Harness.hyundai_c),
  CAR.KIA_CEED: HyundaiCarInfo("Kia Ceed 2019", harness=Harness.hyundai_e),
+  CAR.KIA_EV6: HyundaiCarInfo("Kia EV6 2022", "All", harness=Harness.none),

  # Genesis
  CAR.GENESIS_G70: HyundaiCarInfo("Genesis G70 2018", "All", harness=Harness.hyundai_f),
@ -805,21 +811,26 @@ FW_VERSIONS = {
    (Ecu.eps, 0x7d4, None): [
      b'\xf1\x00IK  MDPS R 1.00 1.07 57700-G9220 4I2VL107',
      b'\xf1\x00IK  MDPS R 1.00 1.07 57700-G9420 4I4VL107',
+      b'\xf1\x00IK  MDPS R 1.00 1.08 57700-G9420 4I4VL108',
    ],
    (Ecu.transmission, 0x7e1, None): [
      b'\xf1\x87VCJLP18407832DN3\x88vXfvUVT\x97eFU\x87d7v\x88eVeveFU\x89\x98\x7f\xff\xb2\xb0\xf1\x81E25\x00\x00\x00',
      b'\x00\x00\x00\x00\xf1\x00bcsh8p54  E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB4\xecE\xefL',
      b'\xf1\x87VDKLT18912362DN4wfVfwefeveVUwfvw\x88vWfvUFU\x89\xa9\x8f\xff\x87w\xf1\x81E25\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54  E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB4\xecE\xefL',
+      b'\xf1\x87VDJLC18480772DK9\x88eHfwfff\x87eFUeDEU\x98eFe\x86T5DVyo\xff\x87s\xf1\x81E25\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54  E25\x00\x00\x00\x00\x00\x00\x00SIK0T33KB5\x9f\xa5&\x81',
    ],
    (Ecu.fwdRadar, 0x7d0, None): [
      b'\xf1\x00IK__ SCC F-CUP      1.00 1.02 96400-G9100         ',
      b'\xf1\x00IK__ SCC F-CUP      1.00 1.02 96400-G9100         \xf1\xa01.02',
+      b'\xf1\x00IK__ SCC FHCUP      1.00 1.02 96400-G9000         ',
    ],
    (Ecu.fwdCamera, 0x7c4, None): [
      b'\xf1\x00IK  MFC  AT USA LHD 1.00 1.01 95740-G9000 170920',
+      b'\xf1\x00IK  MFC  AT KOR LHD 1.00 1.01 95740-G9000 170920',
    ],
    (Ecu.engine, 0x7e0, None): [
      b'\xf1\x81640J0051\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'\xf1\x81640H0051\x00\x00\x00\x00\x00\x00\x00\x00',
    ],
  },
  CAR.KONA: {
@ -1159,6 +1170,21 @@ FW_VERSIONS = {
      b'\xf1\x81640F0051\x00\x00\x00\x00\x00\x00\x00\x00'
    ],
  },
+  CAR.KIA_EV6: {
+    (Ecu.esp, 0x7d1, None): [
+      b'\xf1\x8758520CV100\xf1\x00CV  IEB \x02 101!\x10\x18 58520-CV100',
+    ],
+    (Ecu.eps, 0x7d4, None): [
+      b'\xf1\x00CV1 MDPS R 1.00 1.04 57700-CV000 1B30',
+    ],
+    (Ecu.fwdRadar, 0x7d0, None): [
+      b'\xf1\x00CV1_ RDR -----      1.00 1.01 99110-CV000         ',
+      b'\xf1\x8799110CV000\xf1\x00CV1_ RDR -----      1.00 1.01 99110-CV000         ',
+    ],
+    (Ecu.fwdCamera, 0x7c4, None): [
+      b'\xf1\x00CV1 MFC  AT USA LHD 1.00 1.05 99210-CV000 211027',
+    ],
+  },
 }

 CHECKSUM = {
@ -1176,6 +1202,8 @@ FEATURES = {
  "use_fca": {CAR.SONATA, CAR.SONATA_HYBRID, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.GENESIS_G70_2020, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.TUCSON_DIESEL_2019},
 }

+HDA2_CAR = {CAR.KIA_EV6, }
+
 HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019}  # these cars use a different gas signal
 EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV}

@ -1223,6 +1251,7 @@ DBC = {
  CAR.PALISADE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'),
  CAR.VELOSTER: dbc_dict('hyundai_kia_generic', None),
  CAR.KIA_CEED: dbc_dict('hyundai_kia_generic', None),
+  CAR.KIA_EV6: dbc_dict('kia_ev6', None),
  CAR.SONATA_HYBRID: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'),
 }

--- a/selfdrive/car/mock/values.py
+++ b/selfdrive/car/mock/values.py
@ -1,4 +1,4 @@
-from typing import Dict, List, Union
+from typing import Dict, List, Optional, Union

 from selfdrive.car.docs_definitions import CarInfo

@ -7,4 +7,6 @@ class CAR:
  MOCK = 'mock'


-CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {}
+CAR_INFO: Dict[str, Optional[Union[CarInfo, List[CarInfo]]]] = {
+  CAR.MOCK: None,
+}
--- a/selfdrive/car/nissan/values.py
+++ b/selfdrive/car/nissan/values.py
@ -1,5 +1,5 @@
 from dataclasses import dataclass
-from typing import Dict, List, Union
+from typing import Dict, List, Optional, Union
 from enum import Enum

 from selfdrive.car import dbc_dict
@ -32,9 +32,10 @@ class NissanCarInfo(CarInfo):
  harness: Enum = Harness.nissan_a


-CAR_INFO: Dict[str, Union[NissanCarInfo, List[NissanCarInfo]]] = {
+CAR_INFO: Dict[str, Optional[Union[NissanCarInfo, List[NissanCarInfo]]]] = {
  CAR.XTRAIL: NissanCarInfo("Nissan X-Trail 2017"),
  CAR.LEAF: NissanCarInfo("Nissan Leaf 2018-22"),
+  CAR.LEAF_IC: None,  # same platforms
  CAR.ROGUE: NissanCarInfo("Nissan Rogue 2018-20"),
  CAR.ALTIMA: NissanCarInfo("Nissan Altima 2019-20", harness=Harness.nissan_b),
 }
--- a/selfdrive/car/tests/routes.py
+++ b/selfdrive/car/tests/routes.py
@ -98,6 +98,7 @@ routes = [
  TestRoute("49f3c13141b6bc87|2021-07-28--08-05-13", HYUNDAI.KONA_HEV),
  TestRoute("5dddcbca6eb66c62|2020-07-26--13-24-19", HYUNDAI.KIA_STINGER),
  TestRoute("d624b3d19adce635|2020-08-01--14-59-12", HYUNDAI.VELOSTER),
+  TestRoute("d824e27e8c60172c|2022-05-19--16-15-28", HYUNDAI.KIA_EV6),
  TestRoute("007d5e4ad9f86d13|2021-09-30--15-09-23", HYUNDAI.KIA_K5_2021),
  TestRoute("50c6c9b85fd1ff03|2020-10-26--17-56-06", HYUNDAI.KIA_NIRO_EV),
  TestRoute("173219cf50acdd7b|2021-07-05--10-27-41", HYUNDAI.KIA_NIRO_HEV),
--- a/selfdrive/car/tests/test_car_interfaces.py
+++ b/selfdrive/car/tests/test_car_interfaces.py
@ -38,8 +38,6 @@ class TestCarInterfaces(unittest.TestCase):
      tuning = car_params.lateralTuning.which()
      if tuning == 'pid':
        self.assertTrue(len(car_params.lateralTuning.pid.kpV))
-      elif tuning == 'lqr':
-        self.assertTrue(len(car_params.lateralTuning.lqr.a))
      elif tuning == 'torque':
        self.assertTrue(car_params.lateralTuning.torque.kf > 0)
      elif tuning == 'indi':
--- a/selfdrive/car/tests/test_docs.py
+++ b/selfdrive/car/tests/test_docs.py
@ -1,6 +1,7 @@
 #!/usr/bin/env python3
 import unittest

+from selfdrive.car.car_helpers import interfaces, get_interface_attr
 from selfdrive.car.docs import CARS_MD_OUT, CARS_MD_TEMPLATE, generate_cars_md, get_all_car_info


@ -16,6 +17,12 @@ class TestCarDocs(unittest.TestCase):
    self.assertEqual(generated_cars_md, current_cars_md,
                     "Run selfdrive/car/docs.py to generate new supported cars documentation")

+  def test_missing_car_info(self):
+    all_car_info_platforms = [p for i in get_interface_attr("CAR_INFO").values() for p in i]
+    for platform in sorted(interfaces.keys()):
+      if platform not in all_car_info_platforms:
+        self.fail("Platform: {} doesn't exist in CarInfo".format(platform))
+
  def test_naming_conventions(self):
    # Asserts market-standard car naming conventions by make
    for car in self.all_cars:
--- a/selfdrive/car/tests/test_models.py
+++ b/selfdrive/car/tests/test_models.py
@ -69,7 +69,7 @@ class TestCarModel(unittest.TestCase):
        continue

      can_msgs = []
-      fingerprint = {i: dict() for i in range(3)}
+      fingerprint = defaultdict(dict)
      for msg in lr:
        if msg.which() == "can":
          for m in msg.can:
@ -98,8 +98,10 @@ class TestCarModel(unittest.TestCase):

    # TODO: check safetyModel is in release panda build
    self.safety = libpandasafety_py.libpandasafety
-    set_status = self.safety.set_safety_hooks(self.CP.safetyConfigs[0].safetyModel.raw, self.CP.safetyConfigs[0].safetyParam)
-    self.assertEqual(0, set_status, f"failed to set safetyModel {self.CP.safetyConfigs}")
+
+    cfg = self.CP.safetyConfigs[-1]
+    set_status = self.safety.set_safety_hooks(cfg.safetyModel.raw, cfg.safetyParam)
+    self.assertEqual(0, set_status, f"failed to set safetyModel {cfg}")
    self.safety.init_tests()

  def test_car_params(self):
@ -116,8 +118,6 @@ class TestCarModel(unittest.TestCase):
        self.assertTrue(len(self.CP.lateralTuning.pid.kpV))
      elif tuning == 'torque':
        self.assertTrue(self.CP.lateralTuning.torque.kf > 0)
-      elif tuning == 'lqr':
-        self.assertTrue(len(self.CP.lateralTuning.lqr.a))
      elif tuning == 'indi':
        self.assertTrue(len(self.CP.lateralTuning.indi.outerLoopGainV))
      else:
@ -168,7 +168,7 @@ class TestCarModel(unittest.TestCase):
        if msg.src >= 64:
          continue

-        to_send = package_can_msg([msg.address, 0, msg.dat, msg.src])
+        to_send = package_can_msg([msg.address, 0, msg.dat, msg.src % 4])
        if self.safety.safety_rx_hook(to_send) != 1:
          failed_addrs[hex(msg.address)] += 1

--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@ -38,8 +38,7 @@ class CarInterface(CarInterfaceBase):
      ret.steerRatio = 15.74   # unknown end-to-end spec
      tire_stiffness_factor = 0.6371   # hand-tune
      ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG
-      set_lat_tune(ret.lateralTuning, LatTunes.INDI_PRIUS)
-      ret.steerActuatorDelay = 0.3
+      set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=1.7, FRICTION=0.06)

    elif candidate == CAR.PRIUS_V:
      stop_and_go = True
@ -47,7 +46,7 @@ class CarInterface(CarInterfaceBase):
      ret.steerRatio = 17.4
      tire_stiffness_factor = 0.5533
      ret.mass = 3340. * CV.LB_TO_KG + STD_CARGO_KG
-      set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4)
+      set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=1.8, FRICTION=0.06)

    elif candidate in (CAR.RAV4, CAR.RAV4H):
      stop_and_go = True if (candidate in CAR.RAV4H) else False
@ -55,7 +54,7 @@ class CarInterface(CarInterfaceBase):
      ret.steerRatio = 16.88   # 14.5 is spec end-to-end
      tire_stiffness_factor = 0.5533
      ret.mass = 3650. * CV.LB_TO_KG + STD_CARGO_KG  # mean between normal and hybrid
-      set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4)
+      set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=1.8, FRICTION=0.06)

    elif candidate == CAR.COROLLA:
      ret.wheelbase = 2.70
@ -87,7 +86,10 @@ class CarInterface(CarInterfaceBase):
      ret.steerRatio = 13.7
      tire_stiffness_factor = 0.7933
      ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG  # mean between normal and hybrid
-      set_lat_tune(ret.lateralTuning, LatTunes.PID_C)
+      if candidate in (CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2):
+        set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.4, FRICTION=0.05)
+      else:
+        set_lat_tune(ret.lateralTuning, LatTunes.PID_C)

    elif candidate in (CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2):
      stop_and_go = True
@ -136,7 +138,7 @@ class CarInterface(CarInterfaceBase):
      ret.steerRatio = 13.9
      tire_stiffness_factor = 0.444  # not optimized yet
      ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG
-      set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
+      set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.0, FRICTION=0.07)

    elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ESH):
      stop_and_go = True
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 from enum import Enum
-
+from selfdrive.controls.lib.latcontrol_torque import set_torque_tune

 class LongTunes(Enum):
  PEDAL = 0
@ -50,34 +50,9 @@ def set_long_tune(tune, name):


 ###### LAT ######
-def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=.1):
+def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=.1, use_steering_angle=True):
  if name == LatTunes.TORQUE:
-    tune.init('torque')
-    tune.torque.useSteeringAngle = True
-    tune.torque.kp = 1.0 / MAX_LAT_ACCEL
-    tune.torque.kf = 1.0 / MAX_LAT_ACCEL
-    tune.torque.ki = 0.1 / MAX_LAT_ACCEL
-    tune.torque.friction = FRICTION
-  elif name == LatTunes.LQR_RAV4:
-    tune.init('lqr')
-    tune.lqr.scale = 1500.0
-    tune.lqr.ki = 0.05
-    tune.lqr.a = [0., 1., -0.22619643, 1.21822268]
-    tune.lqr.b = [-1.92006585e-04, 3.95603032e-05]
-    tune.lqr.c = [1., 0.]
-    tune.lqr.k = [-110.73572306, 451.22718255]
-    tune.lqr.l = [0.3233671, 0.3185757]
-    tune.lqr.dcGain = 0.002237852961363602
-  elif name == LatTunes.INDI_PRIUS:
-    tune.init('indi')
-    tune.indi.innerLoopGainBP = [0.]
-    tune.indi.innerLoopGainV = [4.0]
-    tune.indi.outerLoopGainBP = [0.]
-    tune.indi.outerLoopGainV = [3.0]
-    tune.indi.timeConstantBP = [0.]
-    tune.indi.timeConstantV = [1.0]
-    tune.indi.actuatorEffectivenessBP = [0.]
-    tune.indi.actuatorEffectivenessV = [1.0]
+    set_torque_tune(tune, MAX_LAT_ACCEL, FRICTION)
  elif 'PID' in str(name):
    tune.init('pid')
    tune.pid.kiBP = [0.0]
--- a/selfdrive/car/toyota/values.py
+++ b/selfdrive/car/toyota/values.py
@ -146,7 +146,7 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
  CAR.RAV4_TSS2: ToyotaCarInfo("Toyota RAV4 2019-21", video_link="https://www.youtube.com/watch?v=wJxjDd42gGA"),
  CAR.RAV4_TSS2_2022: ToyotaCarInfo("Toyota RAV4 2022"),
  CAR.RAV4H_TSS2: ToyotaCarInfo("Toyota RAV4 Hybrid 2019-21"),
-  CAR.RAV4H_TSS2_2022: ToyotaCarInfo("Toyota RAV4 Hybrid 2022"),
+  CAR.RAV4H_TSS2_2022: ToyotaCarInfo("Toyota RAV4 Hybrid 2022", video_link="https://youtu.be/U0nH9cnrFB0"),
  CAR.MIRAI: ToyotaCarInfo("Toyota Mirai 2021"),
  CAR.SIENNA: ToyotaCarInfo("Toyota Sienna 2018-20", video_link="https://www.youtube.com/watch?v=q1UPOo4Sh68", footnotes=[Footnote.DSU]),

@ -741,6 +741,7 @@ FW_VERSIONS = {
      b'\x01F152612C00\x00\x00\x00\x00\x00\x00',
      b'F152602191\x00\x00\x00\x00\x00\x00',
      b'\x01F152612862\x00\x00\x00\x00\x00\x00',
+      b'\x01F152612B91\x00\x00\x00\x00\x00\x00',
    ],
    (Ecu.fwdRadar, 0x750, 0xf): [
      b'\x018821F3301100\x00\x00\x00\x00',
@ -1766,11 +1767,13 @@ FW_VERSIONS = {
  CAR.LEXUS_RXH_TSS2: {
    (Ecu.engine, 0x7e0, None): [
      b'\x02348X8000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'\x02348Y3000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
      b'\x0234D14000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
      b'\x0234D16000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
    ],
    (Ecu.esp, 0x7b0, None): [
      b'F152648831\x00\x00\x00\x00\x00\x00',
+      b'F152648891\x00\x00\x00\x00\x00\x00',
      b'F152648D00\x00\x00\x00\x00\x00\x00',
      b'F152648D60\x00\x00\x00\x00\x00\x00',
    ],
--- a/selfdrive/car/volkswagen/values.py
+++ b/selfdrive/car/volkswagen/values.py
@ -135,7 +135,7 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = {
    VWCarInfo("Volkswagen Jetta 2018-21"),
    VWCarInfo("Volkswagen Jetta GLI 2021"),
  ],
-  CAR.PASSAT_MK8: VWCarInfo("Volkswagen Passat 2016-18", footnotes=[Footnote.PASSAT]),
+  CAR.PASSAT_MK8: VWCarInfo("Volkswagen Passat 2015-18", footnotes=[Footnote.PASSAT]),
  CAR.POLO_MK6: VWCarInfo("Volkswagen Polo 2020"),
  CAR.TAOS_MK1: VWCarInfo("Volkswagen Taos 2022", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
  CAR.TCROSS_MK1: VWCarInfo("Volkswagen T-Cross 2021", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
@ -439,6 +439,7 @@ FW_VERSIONS = {
  CAR.PASSAT_MK8: {
    (Ecu.engine, 0x7e0, None): [
      b'\xf1\x8704E906023AH\xf1\x893379',
+      b'\xf1\x8704L906026ET\xf1\x891990',
      b'\xf1\x8704L906026GA\xf1\x892013',
      b'\xf1\x8704L906026KD\xf1\x894798',
      b'\xf1\x873G0906264  \xf1\x890004',
@ -446,22 +447,26 @@ FW_VERSIONS = {
    (Ecu.transmission, 0x7e1, None): [
      b'\xf1\x870CW300048R \xf1\x890610',
      b'\xf1\x870D9300014L \xf1\x895002',
+      b'\xf1\x870D9300041A \xf1\x894801',
      b'\xf1\x870DD300045T \xf1\x891601',
      b'\xf1\x870GC300042H \xf1\x891404',
    ],
    (Ecu.srs, 0x715, None): [
+      b'\xf1\x873Q0959655AE\xf1\x890195\xf1\x82\r56140056130012416612124111',
      b'\xf1\x873Q0959655AN\xf1\x890306\xf1\x82\r58160058140013036914110311',
      b'\xf1\x873Q0959655BB\xf1\x890195\xf1\x82\r56140056130012026612120211',
      b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\0165915005914001344701311442900',
      b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\02315120011111200631145171716121691132111',
    ],
    (Ecu.eps, 0x712, None): [
+      b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820522B0060803',
      b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820522B0080803',
      b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521B00606A1',
      b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\00516B00501A1',
      b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\00521B00703A1',
    ],
    (Ecu.fwdRadar, 0x757, None): [
+      b'\xf1\x873Q0907572A \xf1\x890130',
      b'\xf1\x873Q0907572B \xf1\x890192',
      b'\xf1\x873Q0907572C \xf1\x890195',
      b'\xf1\x875Q0907572R \xf1\x890771',
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@ -23,7 +23,6 @@ from selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
 from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
 from selfdrive.controls.lib.latcontrol_torque import LatControlTorque
-from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
 from selfdrive.controls.lib.events import Events, ET
 from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
 from selfdrive.controls.lib.vehicle_model import VehicleModel
@ -146,8 +145,6 @@ class Controls:
      self.LaC = LatControlPID(self.CP, self.CI)
    elif self.CP.lateralTuning.which() == 'indi':
      self.LaC = LatControlINDI(self.CP, self.CI)
-    elif self.CP.lateralTuning.which() == 'lqr':
-      self.LaC = LatControlLQR(self.CP, self.CI)
    elif self.CP.lateralTuning.which() == 'torque':
      self.LaC = LatControlTorque(self.CP, self.CI)

@ -294,6 +291,7 @@ class Controls:

    # Handle HW and system malfunctions
    # Order is very intentional here. Be careful when modifying this.
+    # All events here should at least have NO_ENTRY and SOFT_DISABLE.
    num_events = len(self.events)

    not_running = {p.name for p in self.sm['managerState'].processes if not p.running and p.shouldBeRunning}
@ -751,8 +749,6 @@ class Controls:
      controlsState.lateralControlState.pidState = lac_log
    elif lat_tuning == 'torque':
      controlsState.lateralControlState.torqueState = lac_log
-    elif lat_tuning == 'lqr':
-      controlsState.lateralControlState.lqrState = lac_log
    elif lat_tuning == 'indi':
      controlsState.lateralControlState.indiState = lac_log

--- a/selfdrive/controls/lib/events.py
+++ b/selfdrive/controls/lib/events.py
@ -558,10 +558,14 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
  # Camera is not outputting frames
  EventName.cameraMalfunction: {
    ET.PERMANENT: camera_malfunction_alert,
+    ET.SOFT_DISABLE: soft_disable_alert("Camera Malfunction"),
+    ET.NO_ENTRY: NoEntryAlert("Camera Malfunction: Reboot Your Device"),
  },
  # Camera framerate too low
  EventName.cameraFrameRate: {
    ET.PERMANENT: NormalPermanentAlert("Camera Frame Rate Low", "Reboot your Device"),
+    ET.SOFT_DISABLE: soft_disable_alert("Camera Frame Rate Low"),
+    ET.NO_ENTRY: NoEntryAlert("Camera Frame Rate Low: Reboot Your Device"),
  },

  # Unused
@ -741,6 +745,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
  # Thrown when manager detects a service exited unexpectedly while driving
  EventName.processNotRunning: {
    ET.NO_ENTRY: process_not_running_alert,
+    ET.SOFT_DISABLE: soft_disable_alert("Process Not Running"),
  },

  EventName.radarFault: {
--- a/selfdrive/controls/lib/latcontrol_lqr.py
+++ b/selfdrive/controls/lib/latcontrol_lqr.py
@ -1,84 +0,0 @@
-import math
-import numpy as np
-
-from common.numpy_fast import clip
-from common.realtime import DT_CTRL
-from cereal import log
-from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
-
-
-class LatControlLQR(LatControl):
-  def __init__(self, CP, CI):
-    super().__init__(CP, CI)
-    self.scale = CP.lateralTuning.lqr.scale
-    self.ki = CP.lateralTuning.lqr.ki
-
-    self.A = np.array(CP.lateralTuning.lqr.a).reshape((2, 2))
-    self.B = np.array(CP.lateralTuning.lqr.b).reshape((2, 1))
-    self.C = np.array(CP.lateralTuning.lqr.c).reshape((1, 2))
-    self.K = np.array(CP.lateralTuning.lqr.k).reshape((1, 2))
-    self.L = np.array(CP.lateralTuning.lqr.l).reshape((2, 1))
-    self.dc_gain = CP.lateralTuning.lqr.dcGain
-
-    self.x_hat = np.array([[0], [0]])
-    self.i_unwind_rate = 0.3 * DT_CTRL
-    self.i_rate = 1.0 * DT_CTRL
-
-    self.reset()
-
-  def reset(self):
-    super().reset()
-    self.i_lqr = 0.0
-
-  def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
-    lqr_log = log.ControlsState.LateralLQRState.new_message()
-
-    torque_scale = (0.45 + CS.vEgo / 60.0)**2  # Scale actuator model with speed
-
-    # Subtract offset. Zero angle should correspond to zero torque
-    steering_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetAverageDeg
-
-    desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
-
-    instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg
-    desired_angle += instant_offset  # Only add offset that originates from vehicle model errors
-    lqr_log.steeringAngleDesiredDeg = desired_angle
-
-    # Update Kalman filter
-    angle_steers_k = float(self.C.dot(self.x_hat))
-    e = steering_angle_no_offset - angle_steers_k
-    self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e)
-
-    if CS.vEgo < MIN_STEER_SPEED or not active:
-      lqr_log.active = False
-      lqr_output = 0.
-      output_steer = 0.
-      self.reset()
-    else:
-      lqr_log.active = True
-
-      # LQR
-      u_lqr = float(desired_angle / self.dc_gain - self.K.dot(self.x_hat))
-      lqr_output = torque_scale * u_lqr / self.scale
-
-      # Integrator
-      if CS.steeringPressed:
-        self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
-      else:
-        error = desired_angle - angle_steers_k
-        i = self.i_lqr + self.ki * self.i_rate * error
-        control = lqr_output + i
-
-        if (error >= 0 and (control <= self.steer_max or i < 0.0)) or \
-           (error <= 0 and (control >= -self.steer_max or i > 0.0)):
-          self.i_lqr = i
-
-      output_steer = lqr_output + self.i_lqr
-      output_steer = clip(output_steer, -self.steer_max, self.steer_max)
-
-    lqr_log.steeringAngleDeg = angle_steers_k
-    lqr_log.i = self.i_lqr
-    lqr_log.output = output_steer
-    lqr_log.lqrOutput = lqr_output
-    lqr_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS)
-    return output_steer, desired_angle, lqr_log
--- a/selfdrive/controls/lib/latcontrol_torque.py
+++ b/selfdrive/controls/lib/latcontrol_torque.py
@ -22,6 +22,15 @@ LOW_SPEED_FACTOR = 200
 JERK_THRESHOLD = 0.2


+def set_torque_tune(tune, MAX_LAT_ACCEL=2.5, FRICTION=.1):
+  tune.init('torque')
+  tune.torque.useSteeringAngle = True
+  tune.torque.kp = 1.0 / MAX_LAT_ACCEL
+  tune.torque.kf = 1.0 / MAX_LAT_ACCEL
+  tune.torque.ki = 0.1 / MAX_LAT_ACCEL
+  tune.torque.friction = FRICTION
+
+
 class LatControlTorque(LatControl):
  def __init__(self, CP, CI):
    super().__init__(CP, CI)
@ -46,7 +55,9 @@ class LatControlTorque(LatControl):
      if self.use_steering_angle:
        actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
      else:
-        actual_curvature = llk.angularVelocityCalibrated.value[2] / CS.vEgo
+        actual_curvature_vm = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
+        actual_curvature_llk = llk.angularVelocityCalibrated.value[2] / CS.vEgo
+        actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk])
      desired_lateral_accel = desired_curvature * CS.vEgo ** 2
      desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2
      actual_lateral_accel = actual_curvature * CS.vEgo ** 2
--- a/selfdrive/controls/lib/lateral_planner.py
+++ b/selfdrive/controls/lib/lateral_planner.py
@ -62,10 +62,9 @@ class LateralPlanner:
      self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, self.steer_rate_cost)
    else:
      d_path_xyz = self.path_xyz
-      path_cost = np.clip(abs(self.path_xyz[0, 1] / self.path_xyz_stds[0, 1]), 0.5, 1.5) * MPC_COST_LAT.PATH
      # Heading cost is useful at low speed, otherwise end of plan can be off-heading
-      heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.0])
-      self.lat_mpc.set_weights(path_cost, heading_cost, self.steer_rate_cost)
+      heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.15])
+      self.lat_mpc.set_weights(MPC_COST_LAT.PATH, heading_cost, self.steer_rate_cost)

    y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1])
    heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
--- a/selfdrive/controls/lib/tests/test_latcontrol.py
+++ b/selfdrive/controls/lib/tests/test_latcontrol.py
@ -9,7 +9,7 @@ from selfdrive.car.honda.values import CAR as HONDA
 from selfdrive.car.toyota.values import CAR as TOYOTA
 from selfdrive.car.nissan.values import CAR as NISSAN
 from selfdrive.controls.lib.latcontrol_pid import LatControlPID
-from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
+from selfdrive.controls.lib.latcontrol_torque import LatControlTorque
 from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
 from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
 from selfdrive.controls.lib.vehicle_model import VehicleModel
@ -17,7 +17,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel

 class TestLatControl(unittest.TestCase):

-  @parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlLQR), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)])
+  @parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlTorque), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)])
  def test_saturation(self, car_name, controller):
    CarInterface, CarController, CarState = interfaces[car_name]
    CP = CarInterface.get_params(car_name)
--- a/selfdrive/debug/test_fw_query_on_routes.py
+++ b/selfdrive/debug/test_fw_query_on_routes.py
@ -73,6 +73,7 @@ if __name__ == "__main__":
      lr = LogReader(qlog_path)
      dongles.append(dongle_id)

+      CP = None
      for msg in lr:
        if msg.which() == "pandaStates":
          if msg.pandaStates[0].pandaType not in ('uno', 'blackPanda', 'dos'):
@ -80,14 +81,13 @@ if __name__ == "__main__":
            break

        elif msg.which() == "carParams":
-          bts = msg.carParams.as_builder().to_bytes()
-
-          car_fw = msg.carParams.carFw
+          CP = msg.carParams
+          car_fw = CP.carFw
          if len(car_fw) == 0:
            print("no fw")
            break

-          live_fingerprint = msg.carParams.carFingerprint
+          live_fingerprint = CP.carFingerprint
          live_fingerprint = migration.get(live_fingerprint, live_fingerprint)

          if args.car is not None:
@ -116,7 +116,7 @@ if __name__ == "__main__":
            break

          print(f"{dongle_id}|{time}")
-          print("Old style:", live_fingerprint, "Vin", msg.carParams.carVin)
+          print("Old style:", live_fingerprint, "Vin", CP.carVin)
          print("New style (exact):", exact_matches)
          print("New style (fuzzy):", fuzzy_matches)

@ -164,6 +164,9 @@ if __name__ == "__main__":
              print("Fuzzy match wrong! Fuzzy:", fuzzy_matches, "Live:", live_fingerprint)

          break
+
+      if CP is None:
+        print("no CarParams in logs")
    except Exception:
      traceback.print_exc()
    except KeyboardInterrupt:
--- a/selfdrive/hardware/tici/hardware.py
+++ b/selfdrive/hardware/tici/hardware.py
@ -457,22 +457,23 @@ class Tici(HardwareBase):
  def configure_modem(self):
    sim_id = self.get_sim_info().get('sim_id', '')

+    # configure modem as data-centric
+    cmds = [
+      'AT+QNVW=5280,0,"0102000000000000"',
+      'AT+QNVFW="/nv/item_files/ims/IMS_enable",00',
+      'AT+QNVFW="/nv/item_files/modem/mmode/ue_usage_setting",01',
+    ]
+    modem = self.get_modem()
+    for cmd in cmds:
+      try:
+        modem.Command(cmd, math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)
+      except Exception:
+        pass
+
    # blue prime config
    if sim_id.startswith('8901410'):
-      cmds = [
-        'AT+QNVW=5280,0,"0102000000000000"',
-        'AT+QNVFW="/nv/item_files/ims/IMS_enable",00',
-        'AT+QNVFW="/nv/item_files/modem/mmode/ue_usage_setting",01',
-      ]
-      modem = self.get_modem()
-      for cmd in cmds:
-        try:
-          modem.Command(cmd, math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)
-        except Exception:
-          pass
      os.system('mmcli -m 0 --3gpp-set-initial-eps-bearer-settings="apn=Broadband"')

-
  def get_networks(self):
    r = {}

--- a/selfdrive/hardware/tici/restart_modem.sh
+++ b/selfdrive/hardware/tici/restart_modem.sh
@ -1,18 +1,18 @@
 #!/usr/bin/bash

-nmcli connection modify --temporary lte gsm.home-only yes
-nmcli connection modify --temporary lte gsm.auto-config yes
-nmcli connection modify --temporary lte connection.autoconnect-retries 20
+#nmcli connection modify --temporary lte gsm.home-only yes
+#nmcli connection modify --temporary lte gsm.auto-config yes
+#nmcli connection modify --temporary lte connection.autoconnect-retries 20
+sudo nmcli connection reload

 sudo systemctl stop ModemManager
-
-# full restart
-#/usr/comma/lte/lte.sh stop_blocking
-#sudo systemctl restart lte
-
-# quick shutdown
-/usr/comma/lte/lte.sh stop
 nmcli con down lte
+nmcli con down magenta-prime
+
+# power cycle modem
+/usr/comma/lte/lte.sh stop_blocking
+/usr/comma/lte/lte.sh start

+sudo systemctl restart NetworkManager
 #sudo systemctl restart ModemManager
 sudo ModemManager --debug
--- a/selfdrive/hardware/tici/test_power_draw.py
+++ b/selfdrive/hardware/tici/test_power_draw.py
@ -19,7 +19,7 @@ class Proc:
  warmup: float = 3.

 PROCS = [
-  Proc('camerad', 2.02),
+  Proc('camerad', 2.25),
  Proc('modeld', 0.95),
  Proc('dmonitoringmodeld', 0.25),
  Proc('encoderd', 0.42),
--- a/selfdrive/locationd/laikad.py
+++ b/selfdrive/locationd/laikad.py
@ -4,8 +4,11 @@ from typing import List
 import numpy as np
 from collections import defaultdict

+from numpy.linalg import linalg
+
 from cereal import log, messaging
 from laika import AstroDog
+from laika.ephemeris import convert_ublox_ephem
 from laika.helpers import ConstellationId
 from laika.raw_gnss import GNSSMeasurement, calc_pos_fix, correct_measurements, process_measurements, read_raw_ublox
 from selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind
@ -19,28 +22,25 @@ MAX_TIME_GAP = 10

 class Laikad:

-  def __init__(self):
+  def __init__(self, use_internet):
+    self.astro_dog = AstroDog(use_internet=use_internet)
    self.gnss_kf = GNSSKalman(GENERATED_DIR)

-  def process_ublox_msg(self, ublox_msg, dog: AstroDog, ublox_mono_time: int):
+  def process_ublox_msg(self, ublox_msg, ublox_mono_time: int):
    if ublox_msg.which == 'measurementReport':
      report = ublox_msg.measurementReport
      new_meas = read_raw_ublox(report)
-      measurements = process_measurements(new_meas, dog)
-
-
-      pos_fix = calc_pos_fix(measurements)
+      measurements = process_measurements(new_meas, self.astro_dog)
+      pos_fix = calc_pos_fix(measurements, min_measurements=4)
      # To get a position fix a minimum of 5 measurements are needed.
-      # Each report can contain less and some measurement can't be processed.
-      if len(pos_fix) > 0:
-        measurements = correct_measurements(measurements, pos_fix[0][:3], dog)
-      meas_msgs = [create_measurement_msg(m) for m in measurements]
+      # Each report can contain less and some measurements can't be processed.
+      corrected_measurements = []
+      if len(pos_fix) > 0 and linalg.norm(pos_fix[1]) < 100:
+        corrected_measurements = correct_measurements(measurements, pos_fix[0][:3], self.astro_dog)

      t = ublox_mono_time * 1e-9
-
-      self.update_localizer(pos_fix, t, measurements)
+      self.update_localizer(pos_fix, t, corrected_measurements)
      localizer_valid = self.localizer_valid(t)
-
      ecef_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist()
      ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist()

@ -49,6 +49,8 @@ class Laikad:

      bearing_deg, bearing_std = get_bearing_from_gnss(ecef_pos, ecef_vel, vel_std)

+      meas_msgs = [create_measurement_msg(m) for m in corrected_measurements]
+
      dat = messaging.new_message("gnssMeasurements")
      measurement_msg = log.GnssMeasurements.Measurement.new_message
      dat.gnssMeasurements = {
@ -59,6 +61,11 @@ class Laikad:
        "correctedMeasurements": meas_msgs
      }
      return dat
+    elif ublox_msg.which == 'ephemeris':
+      ephem = convert_ublox_ephem(ublox_msg.ephemeris)
+      self.astro_dog.add_ephem(ephem, self.astro_dog.orbits)
+    # elif ublox_msg.which == 'ionoData':
+    # todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them.

  def update_localizer(self, pos_fix, t: float, measurements: List[GNSSMeasurement]):
    # Check time and outputs are valid
@ -67,9 +74,10 @@ class Laikad:
      if len(pos_fix) == 0:
        return
      post_est = pos_fix[0][:3].tolist()
-      if self.gnss_kf.filter.filter_time is None:
+      filter_time = self.gnss_kf.filter.filter_time
+      if filter_time is None:
        cloudlog.info("Init gnss kalman filter")
-      elif (self.gnss_kf.filter.filter_time - t) > MAX_TIME_GAP:
+      elif (t - filter_time) > MAX_TIME_GAP:
        cloudlog.error("Time gap of over 10s detected, gnss kalman reset")
      else:
        cloudlog.error("Gnss kalman filter state is nan")
@ -82,7 +90,7 @@ class Laikad:

  def localizer_valid(self, t: float):
    filter_time = self.gnss_kf.filter.filter_time
-    return filter_time is not None and (filter_time - t) < MAX_TIME_GAP and \
+    return filter_time is not None and (t - filter_time) < MAX_TIME_GAP and \
           all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS]))

  def init_gnss_localizer(self, est_pos):
@ -97,11 +105,10 @@ def create_measurement_msg(meas: GNSSMeasurement):
  c = log.GnssMeasurements.CorrectedMeasurement.new_message()
  c.constellationId = meas.constellation_id.value
  c.svId = meas.sv_id
-  observables = meas.observables_final
  c.glonassFrequency = meas.glonass_freq if meas.constellation_id == ConstellationId.GLONASS else 0
-  c.pseudorange = float(observables['C1C'])
+  c.pseudorange = float(meas.observables_final['C1C'])
  c.pseudorangeStd = float(meas.observables_std['C1C'])
-  c.pseudorangeRate = float(observables['D1C'])
+  c.pseudorangeRate = float(meas.observables_final['D1C'])
  c.pseudorangeRateStd = float(meas.observables_std['D1C'])
  c.satPos = meas.sat_pos_final.tolist()
  c.satVel = meas.sat_vel.tolist()
@ -134,19 +141,17 @@ def get_bearing_from_gnss(ecef_pos, ecef_vel, vel_std):


 def main():
-  dog = AstroDog(use_internet=True)
  sm = messaging.SubMaster(['ubloxGnss'])
  pm = messaging.PubMaster(['gnssMeasurements'])

-  laikad = Laikad()
+  laikad = Laikad(use_internet=True)

  while True:
    sm.update()

-    # Todo if no internet available use latest ephemeris
    if sm.updated['ubloxGnss']:
      ublox_msg = sm['ubloxGnss']
-      msg = laikad.process_ublox_msg(ublox_msg, dog, sm.logMonoTime['ubloxGnss'])
+      msg = laikad.process_ublox_msg(ublox_msg, sm.logMonoTime['ubloxGnss'])
      if msg is not None:
        pm.send('gnssMeasurements', msg)

--- a/selfdrive/locationd/locationd.cc
+++ b/selfdrive/locationd/locationd.cc
@ -250,12 +250,12 @@ void Localizer::input_fake_gps_observations(double current_time) {
  // Steps : first predict -> observe current obs with reasonable STD
  this->kf->predict(current_time);

-  VectorXd current_x = this->kf->get_x();  
+  VectorXd current_x = this->kf->get_x();
  VectorXd ecef_pos = current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
  VectorXd ecef_vel = current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
  MatrixXdr ecef_pos_R = this->kf->get_fake_gps_pos_cov();
  MatrixXdr ecef_vel_R = this->kf->get_fake_gps_vel_cov();
-  
+
  this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
  this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
 }
@ -283,7 +283,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
  VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos;
  MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(10.0 * log.getAccuracy(),2) + std::pow(10.0 * log.getVerticalAccuracy(),2)).asDiagonal();
  MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(log.getSpeedAccuracy() * 10.0, 2)).asDiagonal();
-  
+
  this->unix_timestamp_millis = log.getTimestamp();
  double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();

@ -419,7 +419,7 @@ void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd
  init_P.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START).diagonal() = reset_orientation_P.diagonal();
  init_P.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START).diagonal() = init_vel_R.diagonal();
  init_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal() = current_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal();
-  
+
  this->reset_kalman(current_time, current_x, init_P);
 }

@ -494,7 +494,7 @@ int Localizer::locationd_thread() {
  PubMaster pm({"liveLocationKalman"});

  // TODO: remove carParams once we're always sending at 100Hz
-  SubMaster sm(service_list, nullptr, {"gpsLocationExternal", "carParams"});
+  SubMaster sm(service_list, {}, nullptr, {"gpsLocationExternal", "carParams"});

  uint64_t cnt = 0;
  bool filterInitialized = false;
--- a/selfdrive/locationd/test/test_laikad.py
+++ b/selfdrive/locationd/test/test_laikad.py
@ -2,7 +2,6 @@
 import unittest
 from datetime import datetime

-from laika import AstroDog
 from laika.gps_time import GPSTime
 from laika.helpers import ConstellationId
 from laika.raw_gnss import GNSSMeasurement
@ -18,11 +17,11 @@ def get_log(segs=range(0)):
  return [m for m in logs if m.which() == 'ubloxGnss']


-def verify_messages(lr, dog, laikad):
+def process_msgs(lr, laikad: Laikad):
  good_msgs = []
  for m in lr:
-    msg = laikad.process_ublox_msg(m.ubloxGnss, dog, m.logMonoTime)
-    if msg is not None and len(msg.gnssMeasurements.correctedMeasurements) > 0:
+    msg = laikad.process_ublox_msg(m.ubloxGnss, m.logMonoTime)
+    if msg is not None:
      good_msgs.append(msg)
  return good_msgs

@ -44,14 +43,31 @@ class TestLaikad(unittest.TestCase):

  def test_laika_online(self):
    # Set to offline forces to use ephemeris messages
-    dog = AstroDog(use_internet=True)
-    laikad = Laikad()
-    correct_msgs = verify_messages(self.logs, dog, laikad)
-
+    laikad = Laikad(use_internet=True)
+    msgs = process_msgs(self.logs, laikad)
+    correct_msgs = [m for m in msgs if len(m.gnssMeasurements.correctedMeasurements) > 0]
    correct_msgs_expected = 560
    self.assertEqual(correct_msgs_expected, len(correct_msgs))
    self.assertEqual(correct_msgs_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))

+  def test_laika_offline(self):
+    # Set to offline forces to use ephemeris messages
+    laikad = Laikad(use_internet=False)
+    msgs = process_msgs(self.logs, laikad)
+    correct_msgs = [m for m in msgs if len(m.gnssMeasurements.correctedMeasurements) > 0]
+
+    self.assertEqual(256, len(correct_msgs))
+    self.assertEqual(256, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))
+
+  def test_laika_offline_ephem_at_start(self):
+    # Test offline but process ephemeris msgs of segment first
+    laikad = Laikad(use_internet=False)
+    ephemeris_logs = [m for m in self.logs if m.ubloxGnss.which() == 'ephemeris']
+    msgs = process_msgs(ephemeris_logs+self.logs, laikad)
+    correct_msgs = [m for m in msgs if len(m.gnssMeasurements.correctedMeasurements) > 0]
+    self.assertEqual(554, len(correct_msgs))
+    self.assertGreaterEqual(554, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))
+

 if __name__ == "__main__":
  unittest.main()
--- a/selfdrive/loggerd/tests/test_loggerd.py
+++ b/selfdrive/loggerd/tests/test_loggerd.py
@ -20,7 +20,7 @@ from selfdrive.loggerd.config import ROOT
 from selfdrive.manager.process_config import managed_processes
 from selfdrive.version import get_version
 from tools.lib.logreader import LogReader
-from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType  # pylint: disable=no-name-in-module, import-error
+from cereal.visionipc import VisionIpcServer, VisionStreamType
 from common.transformations.camera import eon_f_frame_size, tici_f_frame_size, \
                                          eon_d_frame_size, tici_d_frame_size, tici_e_frame_size

--- a/selfdrive/loggerd/uploader.py
+++ b/selfdrive/loggerd/uploader.py
@ -24,7 +24,7 @@ NetworkType = log.DeviceState.NetworkType
 UPLOAD_ATTR_NAME = 'user.upload'
 UPLOAD_ATTR_VALUE = b'1'

-UPLOAD_QLOG_QCAM_MAX_SIZE = 1e7  # 10 MB
+UPLOAD_QLOG_QCAM_MAX_SIZE = 100 * 1e6  # MB

 allow_sleep = bool(os.getenv("UPLOADER_SLEEP", "1"))
 force_wifi = os.getenv("FORCEWIFI") is not None
--- a/selfdrive/loggerd/video_writer.cc
+++ b/selfdrive/loggerd/video_writer.cc
@ -1,3 +1,4 @@
+#pragma clang diagnostic ignored "-Wdeprecated-declarations"
 #include <cassert>
 #include <cstdlib>

--- a/selfdrive/modeld/models/supercombo.dlc
+++ b/selfdrive/modeld/models/supercombo.dlc
@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:ba3fe3e61853cc1434e3e220f40c8e9d1f1b9bab8458196ba3bea6a10b82c6ed
-size 72718099
+oid sha256:027cbb1fabae369878271cb0e3505071a8bdaa07473fad9a0b2e8d695c5dc1ff
+size 76725611
--- a/selfdrive/modeld/models/supercombo.onnx
+++ b/selfdrive/modeld/models/supercombo.onnx
@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:bda57c1a66944f5a633ecd739a24d62702c717a234f2fdcc499dfa1d61c3c19e
-size 73147489
+oid sha256:484976ea5bd4ddcabc82e95faf30d7311a27802c1e337472558699fa2395a499
+size 77472267
--- a/selfdrive/modeld/runners/snpemodel.h
+++ b/selfdrive/modeld/runners/snpemodel.h
@ -1,4 +1,5 @@
 #pragma once
+#pragma clang diagnostic ignored "-Wdeprecated-declarations"

 #include <DlContainer/IDlContainer.hpp>
 #include <DlSystem/DlError.hpp>
--- a/selfdrive/modeld/test/test_modeld.py
+++ b/selfdrive/modeld/test/test_modeld.py
@ -5,7 +5,7 @@ import numpy as np
 import random

 import cereal.messaging as messaging
-from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType  # pylint: disable=no-name-in-module, import-error
+from cereal.visionipc import VisionIpcServer, VisionStreamType
 from common.transformations.camera import tici_f_frame_size
 from common.realtime import DT_MDL
 from selfdrive.manager.process_config import managed_processes
--- a/selfdrive/test/process_replay/helpers.py
+++ b/selfdrive/test/process_replay/helpers.py
@ -0,0 +1,26 @@
+import os
+import shutil
+import uuid
+
+from common.params import Params
+
+class OpenpilotPrefix(object):
+  def __init__(self, prefix: str = None) -> None:
+    self.prefix = prefix if prefix else str(uuid.uuid4())
+    self.msgq_path = os.path.join('/dev/shm', self.prefix)
+
+  def __enter__(self):
+    os.environ['OPENPILOT_PREFIX'] = self.prefix
+    try:
+      os.mkdir(self.msgq_path)
+    except FileExistsError:
+      pass
+
+  def __exit__(self, exc_type, exc_obj, exc_tb):
+    symlink_path = Params().get_param_path()
+    if os.path.exists(symlink_path):
+      shutil.rmtree(os.path.realpath(symlink_path), ignore_errors=True)
+      os.remove(symlink_path)
+    shutil.rmtree(self.msgq_path, ignore_errors=True)
+    del os.environ['OPENPILOT_PREFIX']
+    return True
--- a/selfdrive/test/process_replay/model_replay.py
+++ b/selfdrive/test/process_replay/model_replay.py
@ -7,7 +7,7 @@ from typing import Any
 from itertools import zip_longest

 import cereal.messaging as messaging
-from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType  # pylint: disable=no-name-in-module, import-error
+from cereal.visionipc import VisionIpcServer, VisionStreamType
 from common.spinner import Spinner
 from common.timeout import Timeout
 from common.transformations.camera import get_view_frame_from_road_frame, eon_f_frame_size, tici_f_frame_size, \
--- a/selfdrive/test/process_replay/model_replay_ref_commit
+++ b/selfdrive/test/process_replay/model_replay_ref_commit
@ -1 +1 @@
-5fb5cd71a6e878cf45593ecea22c93e932f70c31
+f74ab97371be93fdc28333e5ea12bbb78c3a32d0
--- a/selfdrive/test/process_replay/process_replay.py
+++ b/selfdrive/test/process_replay/process_replay.py
@ -4,9 +4,7 @@ import os
 import sys
 import threading
 import time
-import shutil
 import signal
-import uuid
 from collections import namedtuple

 import capnp
@ -18,6 +16,7 @@ from common.params import Params
 from common.timeout import Timeout
 from selfdrive.car.fingerprints import FW_VERSIONS
 from selfdrive.car.car_helpers import get_car, interfaces
+from selfdrive.test.process_replay.helpers import OpenpilotPrefix
 from selfdrive.manager.process import PythonProcess
 from selfdrive.manager.process_config import managed_processes

@ -337,35 +336,13 @@ CONFIGS = [
  ),
 ]

-def setup_prefix():
-  os.environ['OPENPILOT_PREFIX'] = str(uuid.uuid4())
-  msgq_path = os.path.join('/dev/shm', os.environ['OPENPILOT_PREFIX'])
-  try:
-    os.mkdir(msgq_path)
-  except FileExistsError:
-    pass
-
-
-def teardown_prefix():
-  if not os.environ.get("OPENPILOT_PREFIX", 0):
-    return
-  symlink_path = Params().get_param_path()
-  if os.path.exists(symlink_path):
-    shutil.rmtree(os.path.realpath(symlink_path), ignore_errors=True)
-    os.remove(symlink_path)
-  msgq_path = os.path.join('/dev/shm', os.environ['OPENPILOT_PREFIX'])
-  shutil.rmtree(msgq_path, ignore_errors=True)
-

 def replay_process(cfg, lr, fingerprint=None):
-  setup_prefix()
-  try:
+  with OpenpilotPrefix():
    if cfg.fake_pubsubmaster:
      return python_replay_process(cfg, lr, fingerprint)
    else:
      return cpp_replay_process(cfg, lr, fingerprint)
-  finally:
-    teardown_prefix()

 def setup_env(simulation=False):
  params = Params()
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@ -1 +1 @@
-b8c35486e8354713221d4237e97e5abced6f5228
+0956446adfa91506f0a3d88f893e041bfb2890c1
--- a/selfdrive/test/process_replay/regen.py
+++ b/selfdrive/test/process_replay/regen.py
@ -11,7 +11,7 @@ os.environ["USE_WEBCAM"] = "1"
 import cereal.messaging as messaging
 from cereal import car
 from cereal.services import service_list
-from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType  # pylint: disable=no-name-in-module, import-error
+from cereal.visionipc import VisionIpcServer, VisionStreamType
 from common.params import Params
 from common.realtime import Ratekeeper, DT_MDL, DT_DMON, sec_since_boot
 from common.transformations.camera import eon_f_frame_size, eon_d_frame_size, tici_f_frame_size, tici_d_frame_size
@ -30,9 +30,17 @@ def replay_panda_states(s, msgs):
  rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None)
  smsgs = [m for m in msgs if m.which() in ['pandaStates', 'pandaStateDEPRECATED']]

+  # TODO: new safety params from flags, remove after getting new routes for Toyota
+  safety_param_migration = {
+    "TOYOTA PRIUS 2017": 578,
+    "TOYOTA RAV4 2017": 329
+  }
+
  # Migrate safety param base on carState
  cp = [m for m in msgs if m.which() == 'carParams'][0].carParams
-  if len(cp.safetyConfigs):
+  if cp.carFingerprint in safety_param_migration:
+    safety_param = safety_param_migration[cp.carFingerprint]
+  elif len(cp.safetyConfigs):
    safety_param = cp.safetyConfigs[0].safetyParam
    if cp.safetyConfigs[0].safetyParamDEPRECATED != 0:
      safety_param = cp.safetyConfigs[0].safetyParamDEPRECATED
@ -181,15 +189,17 @@ def regen_segment(lr, frs=None, outdir=FAKEDATA):
  os.environ['SKIP_FW_QUERY'] = ""
  os.environ['FINGERPRINT'] = ""

-  # TODO: remove after getting new route for mazda
-  migration = {
+  # TODO: remove after getting new route for Mazda
+  fp_migration = {
    "Mazda CX-9 2021": "MAZDA CX-9 2021",
  }
+  # TODO: remove after getting new route for Subaru
+  fingerprint_problem = ["SUBARU IMPREZA LIMITED 2019"]

  for msg in lr:
    if msg.which() == 'carParams':
-      car_fingerprint = migration.get(msg.carParams.carFingerprint, msg.carParams.carFingerprint)
-      if len(msg.carParams.carFw) and (car_fingerprint in FW_VERSIONS):
+      car_fingerprint = fp_migration.get(msg.carParams.carFingerprint, msg.carParams.carFingerprint)
+      if len(msg.carParams.carFw) and (car_fingerprint in FW_VERSIONS) and (car_fingerprint not in fingerprint_problem):
        params.put("CarParamsCache", msg.carParams.as_builder().to_bytes())
      else:
        os.environ['SKIP_FW_QUERY'] = "1"
--- a/selfdrive/test/process_replay/test_processes.py
+++ b/selfdrive/test/process_replay/test_processes.py
@ -15,7 +15,7 @@ from selfdrive.version import get_commit
 from tools.lib.logreader import LogReader

 original_segments = [
-  ("BODY", "bd6a637565e91581|2022-04-04--22-05-08--0"),        # COMMA.BODY
+  ("BODY", "937ccb7243511b65|2022-05-24--16-03-09--1"),        # COMMA.BODY
  ("HYUNDAI", "02c45f73a2e5c6e9|2021-01-01--19-08-22--1"),     # HYUNDAI.SONATA
  ("TOYOTA", "0982d79ebb0de295|2021-01-04--17-13-21--13"),     # TOYOTA.PRIUS (INDI)
  ("TOYOTA2", "0982d79ebb0de295|2021-01-03--20-03-36--6"),     # TOYOTA.RAV4  (LQR)
@ -57,13 +57,13 @@ REF_COMMIT_FN = os.path.join(PROC_REPLAY_DIR, "ref_commit")


 def run_test_process(data):
-  segment, cfg, args, cur_log_fn, lr, ref_commit = data
+  segment, cfg, args, cur_log_fn, ref_log_path, lr = data
  res = None
  if not args.upload_only:
-    ref_log_fn = os.path.join(PROC_REPLAY_DIR, f"{segment}_{cfg.proc_name}_{ref_commit}.bz2")
-    res, log_msgs = test_process(cfg, lr, ref_log_fn, args.ignore_fields, args.ignore_msgs)
+    res, log_msgs = test_process(cfg, lr, ref_log_path, args.ignore_fields, args.ignore_msgs)
    # save logs so we can upload when updating refs
    save_log(cur_log_fn, log_msgs)
+
  if args.update_refs or args.upload_only:
    print(f'Uploading: {os.path.basename(cur_log_fn)}')
    assert os.path.exists(cur_log_fn), f"Cannot find log to upload: {cur_log_fn}"
@ -78,13 +78,12 @@ def get_logreader(segment):
  return (segment, lr)


-def test_process(cfg, lr, ref_log_fn, ignore_fields=None, ignore_msgs=None):
+def test_process(cfg, lr, ref_log_path, ignore_fields=None, ignore_msgs=None):
  if ignore_fields is None:
    ignore_fields = []
  if ignore_msgs is None:
    ignore_msgs = []

-  ref_log_path = ref_log_fn if os.path.exists(ref_log_fn) else BASE_URL + os.path.basename(ref_log_fn)
  ref_log_msgs = list(LogReader(ref_log_path))

  log_msgs = replay_process(cfg, lr)
@ -92,7 +91,7 @@ def test_process(cfg, lr, ref_log_fn, ignore_fields=None, ignore_msgs=None):
  # check to make sure openpilot is engaged in the route
  if cfg.proc_name == "controlsd":
    if not check_enabled(log_msgs):
-      segment = ref_log_fn.split("/")[-1].split("_")[0]
+      segment = os.path.basename(ref_log_path).split("/")[-1].split("_")[0]
      raise Exception(f"Route never enabled: {segment}")

  try:
@ -191,13 +190,21 @@ if __name__ == "__main__":
      if (len(args.whitelist_cars) and car_brand.upper() not in args.whitelist_cars) or \
         (not len(args.whitelist_cars) and car_brand.upper() in args.blacklist_cars):
        continue
+
      for cfg in CONFIGS:
        if (len(args.whitelist_procs) and cfg.proc_name not in args.whitelist_procs) or \
           (not len(args.whitelist_procs) and cfg.proc_name in args.blacklist_procs):
          continue
+
        cur_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{cur_commit}.bz2")
+        if args.update_refs:  # reference logs will not exist if routes were just regenerated
+          ref_log_path = get_url(*segment.rsplit("--", 1))
+        else:
+          ref_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{ref_commit}.bz2")
+          ref_log_path = ref_log_fn if os.path.exists(ref_log_fn) else BASE_URL + os.path.basename(ref_log_fn)
+
        lr = None if args.upload_only else lreaders[segment]
-        pool_args.append((segment, cfg, args, cur_log_fn, lr, ref_commit))
+        pool_args.append((segment, cfg, args, cur_log_fn, ref_log_path, lr))

    results: Any = defaultdict(dict)
    p2 = pool.map(run_test_process, pool_args)
@ -206,20 +213,19 @@ if __name__ == "__main__":
        results[segment][proc] = result

  diff1, diff2, failed = format_diff(results, ref_commit)
-  if not args.upload_only:
+  if not upload:
    with open(os.path.join(PROC_REPLAY_DIR, "diff.txt"), "w") as f:
      f.write(diff2)
    print(diff1)

    if failed:
      print("TEST FAILED")
-      if not args.update_refs:
-        print("\n\nTo push the new reference logs for this commit run:")
-        print("./test_processes.py --upload-only")
+      print("\n\nTo push the new reference logs for this commit run:")
+      print("./test_processes.py --upload-only")
    else:
      print("TEST SUCCEEDED")

-  if upload:
+  else:
    with open(REF_COMMIT_FN, "w") as f:
      f.write(cur_commit)
    print(f"\n\nUpdated reference logs for commit: {cur_commit}")
--- a/selfdrive/ui/qt/onroad.cc
+++ b/selfdrive/ui/qt/onroad.cc
@ -372,19 +372,21 @@ void NvgWindow::drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV
 }

 void NvgWindow::paintGL() {
+  UIState *s = uiState();
+  const cereal::ModelDataV2::Reader &model = (*s->sm)["modelV2"].getModelV2();
+  CameraViewWidget::setFrameId(model.getFrameId());
  CameraViewWidget::paintGL();

  QPainter painter(this);
  painter.setRenderHint(QPainter::Antialiasing);
  painter.setPen(Qt::NoPen);

-  UIState *s = uiState();
  if (s->worldObjectsVisible()) {

    drawLaneLines(painter, s);

    if (s->scene.longitudinal_control) {
-      auto leads = (*s->sm)["modelV2"].getModelV2().getLeadsV3();
+      const auto leads = model.getLeadsV3();
      if (leads[0].getProb() > .5) {
        drawLead(painter, leads[0], s->scene.lead_vertices[0]);
      }
--- a/selfdrive/ui/qt/sidebar.cc
+++ b/selfdrive/ui/qt/sidebar.cc
@ -57,7 +57,7 @@ void Sidebar::updateState(const UIState &s) {
  ItemStatus connectStatus;
  auto last_ping = deviceState.getLastAthenaPingTime();
  if (last_ping == 0) {
-    connectStatus = params.getBool("PrimeRedirected") ? ItemStatus{"NO\nPRIME", danger_color} : ItemStatus{"CONNECT\nOFFLINE", warning_color};
+    connectStatus = ItemStatus{"CONNECT\nOFFLINE", warning_color};
  } else {
    connectStatus = nanos_since_boot() - last_ping < 80e9 ? ItemStatus{"CONNECT\nONLINE", good_color} : ItemStatus{"CONNECT\nERROR", danger_color};
  }
--- a/selfdrive/ui/qt/widgets/cameraview.cc
+++ b/selfdrive/ui/qt/widgets/cameraview.cc
@ -153,7 +153,7 @@ void CameraViewWidget::initializeGL() {
 }

 void CameraViewWidget::showEvent(QShowEvent *event) {
-  latest_frame = nullptr;
+  frames.clear();
  if (!vipc_thread) {
    vipc_thread = new QThread();
    connect(vipc_thread, &QThread::started, [=]() { vipcThread(); });
@ -204,14 +204,20 @@ void CameraViewWidget::paintGL() {
  glClearColor(bg.redF(), bg.greenF(), bg.blueF(), bg.alphaF());
  glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);

-  if (latest_frame == nullptr) return;
+  if (frames.empty()) return;
+
+  int frame_idx;
+  for (frame_idx = 0; frame_idx < frames.size() - 1; frame_idx++) {
+    if (frames[frame_idx].first == draw_frame_id) break;
+  }
+  VisionBuf *frame = frames[frame_idx].second;

  glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
  glViewport(0, 0, width(), height());
  glBindVertexArray(frame_vao);

  glUseProgram(program->programId());
-  uint8_t *address[3] = {latest_frame->y, latest_frame->u, latest_frame->v};
+  uint8_t *address[3] = {frame->y, frame->u, frame->v};
  for (int i = 0; i < 3; ++i) {
    glActiveTexture(GL_TEXTURE0 + i);
    glBindTexture(GL_TEXTURE_2D, textures[i]);
@ -234,7 +240,7 @@ void CameraViewWidget::paintGL() {

 void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) {
  makeCurrent();
-  latest_frame = nullptr;
+  frames.clear();
  stream_width = vipc_client->buffers[0].width;
  stream_height = vipc_client->buffers[0].height;

@ -253,19 +259,23 @@ void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) {
  updateFrameMat(width(), height());
 }

-void CameraViewWidget::vipcFrameReceived(VisionBuf *buf) {
-  latest_frame = buf;
+void CameraViewWidget::vipcFrameReceived(VisionBuf *buf, uint32_t frame_id) {
+  frames.push_back(std::make_pair(frame_id, buf));
+  while (frames.size() > FRAME_BUFFER_SIZE) {
+    frames.pop_front();
+  }
  update();
 }

 void CameraViewWidget::vipcThread() {
  VisionStreamType cur_stream_type = stream_type;
  std::unique_ptr<VisionIpcClient> vipc_client;
+  VisionIpcBufExtra meta_main = {0};

  while (!QThread::currentThread()->isInterruptionRequested()) {
    if (!vipc_client || cur_stream_type != stream_type) {
      cur_stream_type = stream_type;
-      vipc_client.reset(new VisionIpcClient(stream_name, cur_stream_type, true));
+      vipc_client.reset(new VisionIpcClient(stream_name, cur_stream_type, false));
    }

    if (!vipc_client->connected) {
@ -276,8 +286,8 @@ void CameraViewWidget::vipcThread() {
      emit vipcThreadConnected(vipc_client.get());
    }

-    if (VisionBuf *buf = vipc_client->recv(nullptr, 1000)) {
-      emit vipcThreadFrameReceived(buf);
+    if (VisionBuf *buf = vipc_client->recv(&meta_main, 1000)) {
+      emit vipcThreadFrameReceived(buf, meta_main.frame_id);
    }
  }
 }
--- a/selfdrive/ui/qt/widgets/cameraview.h
+++ b/selfdrive/ui/qt/widgets/cameraview.h
@ -10,6 +10,9 @@
 #include "selfdrive/camerad/cameras/camera_common.h"
 #include "selfdrive/ui/ui.h"

+const int FRAME_BUFFER_SIZE = 5;
+static_assert(FRAME_BUFFER_SIZE <= YUV_BUFFER_COUNT);
+
 class CameraViewWidget : public QOpenGLWidget, protected QOpenGLFunctions {
  Q_OBJECT

@ -19,11 +22,12 @@ public:
  ~CameraViewWidget();
  void setStreamType(VisionStreamType type) { stream_type = type; }
  void setBackgroundColor(const QColor &color) { bg = color; }
+  void setFrameId(int frame_id) { draw_frame_id = frame_id; }

 signals:
  void clicked();
  void vipcThreadConnected(VisionIpcClient *);
-  void vipcThreadFrameReceived(VisionBuf *);
+  void vipcThreadFrameReceived(VisionBuf *, quint32);

 protected:
  void paintGL() override;
@ -36,8 +40,8 @@ protected:
  void vipcThread();

  bool zoomed_view;
-  VisionBuf *latest_frame = nullptr;
  GLuint frame_vao, frame_vbo, frame_ibo;
+  GLuint textures[3];
  mat4 frame_mat;
  std::unique_ptr<QOpenGLShaderProgram> program;
  QColor bg = QColor("#000000");
@ -48,9 +52,10 @@ protected:
  std::atomic<VisionStreamType> stream_type;
  QThread *vipc_thread = nullptr;

-  GLuint textures[3];
+  std::deque<std::pair<uint32_t, VisionBuf*>> frames;
+  uint32_t draw_frame_id = 0;

 protected slots:
  void vipcConnected(VisionIpcClient *vipc_client);
-  void vipcFrameReceived(VisionBuf *vipc_client);
+  void vipcFrameReceived(VisionBuf *vipc_client, uint32_t frame_id);
 };
--- a/selfdrive/ui/replay/replay.cc
+++ b/selfdrive/ui/replay/replay.cc
@ -275,7 +275,10 @@ void Replay::startStream(const Segment *cur_segment) {
  it = std::find_if(events.begin(), events.end(), [](auto e) { return e->which == cereal::Event::Which::CAR_PARAMS; });
  if (it != events.end()) {
    car_fingerprint_ = (*it)->event.getCarParams().getCarFingerprint();
-    auto bytes = (*it)->bytes();
+    capnp::MallocMessageBuilder builder;
+    builder.setRoot((*it)->event.getCarParams());
+    auto words = capnp::messageToFlatArray(builder);
+    auto bytes = words.asBytes();
    Params().put("CarParams", (const char *)bytes.begin(), bytes.size());
  } else {
    rWarning("failed to read CarParams from current segment");
--- a/tools/camerastream/compressed_vipc.py
+++ b/tools/camerastream/compressed_vipc.py
@ -1,6 +1,6 @@
 #!/usr/bin/env python3
+import av
 import os
-os.environ["NV_LOW_LATENCY"] = "3"    # both bLowLatency and CUVID_PKT_ENDOFPICTURE
 import sys
 import argparse
 import numpy as np
@ -8,7 +8,7 @@ import multiprocessing
 import time

 import cereal.messaging as messaging
-from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType  # pylint: disable=no-name-in-module, import-error
+from cereal.visionipc import VisionIpcServer, VisionStreamType

 W, H = 1928, 1208
 V4L2_BUF_FLAG_KEYFRAME = 8
@ -16,6 +16,7 @@ V4L2_BUF_FLAG_KEYFRAME = 8
 def decoder(addr, sock_name, vipc_server, vst, nvidia):
  print("start decoder for %s" % sock_name)
  if nvidia:
+    os.environ["NV_LOW_LATENCY"] = "3"    # both bLowLatency and CUVID_PKT_ENDOFPICTURE
    sys.path += os.environ["LD_LIBRARY_PATH"].split(":")
    import PyNvCodec as nvc # pylint: disable=import-error

@ -25,7 +26,6 @@ def decoder(addr, sock_name, vipc_server, vst, nvidia):
    nvDwn_yuv = nvc.PySurfaceDownloader(W, H, nvc.PixelFormat.YUV420, 0)
    img_yuv = np.ndarray((H*W//2*3), dtype=np.uint8)
  else:
-    import av # pylint: disable=import-error
    codec = av.CodecContext.create("hevc", "r")

  os.environ["ZMQ"] = "1"
@ -82,9 +82,9 @@ def decoder(addr, sock_name, vipc_server, vst, nvidia):
      print("%2d %4d %.3f %.3f roll %6.2f ms latency %6.2f ms + %6.2f ms + %6.2f ms = %6.2f ms" % (len(msgs), evta.idx.encodeId, evt.logMonoTime/1e9, evta.idx.timestampEof/1e6, frame_latency, process_latency, network_latency, pc_latency, process_latency+network_latency+pc_latency ), len(evta.data), sock_name)

 if __name__ == "__main__":
-  parser = argparse.ArgumentParser(description='Decode video streams and broacast on VisionIPC')
-  parser.add_argument("addr", help="Address of comma 3")
-  parser.add_argument('--nvidia', action='store_true', help='Use nvidia instead of ffmpeg')
+  parser = argparse.ArgumentParser(description="Decode video streams and broacast on VisionIPC")
+  parser.add_argument("addr", help="Address of comma three")
+  parser.add_argument("--nvidia", action="store_true", help="Use nvidia instead of ffmpeg")
  parser.add_argument("--cams", default="0,1,2", help="Cameras to decode")
  args = parser.parse_args()

--- a/tools/latencylogger/README.md
+++ b/tools/latencylogger/README.md
@ -19,77 +19,74 @@ optional arguments:
  --relative            Make timestamps relative to the start of each frame (default: False)
  --demo                Use the demo route instead of providing one (default: False)
  --plot                If a plot should be generated (default: False)
+  --offset              Offset service to better visualize overlap (default: False)
 ```
 To timestamp an event, use `LOGT("msg")` in c++ code or `cloudlog.timestamp("msg")` in python code. If the print is warning for frameId assignment ambiguity, use `LOGT(frameId ,"msg")`.

 ## Examples
-Plotting with relative starts each process at time=0 and gives a nice overview. Timestamps are visualized as diamonds. The opacity allows for visualization of overlapping services.
-![relplot-1](https://user-images.githubusercontent.com/42323981/162108651-e0beee14-56e4-466d-8af1-cb37129fd94a.png)

-Plotting without relative provides info about the frames relative time.
-![plot-1](https://user-images.githubusercontent.com/42323981/162108694-fbfe907b-a1ee-4cc7-bc8b-162a7d9305d4.png)
+Timestamps are visualized as diamonds

+|  | Relative  | Absolute |
+| ------------- | ------------- | ------------- |
+| Inline | ![inrel](https://user-images.githubusercontent.com/42323981/170559939-465df3b1-bf87-46d5-b5ee-5cc87dc49470.png) | ![inabs](https://user-images.githubusercontent.com/42323981/170559985-a82f87e7-82c4-4e48-a348-4221568dd589.png) |
+| Offset | ![offrel](https://user-images.githubusercontent.com/42323981/170559854-93fba90f-acc4-4d08-b317-d3f8fc649ea8.png) | ![offabs](https://user-images.githubusercontent.com/42323981/170559782-06ed5599-d4e3-4701-ad78-5c1eec6cb61e.png) |

 Printed timestamps of a frame with internal durations.
 ```
-Frame ID: 371
+Frame ID: 1202
  camerad
    wideRoadCameraState start of frame                   0.0
-    roadCameraState start of frame                       0.072395
-    wideRoadCameraState published                        47.804745
-    WideRoadCamera: Image set                            47.839849
-    roadCameraState published                            48.319166
-    RoadCamera: Image set                                48.354478
-    RoadCamera: Transformed                              48.430258
-    wideRoadCameraState.processingTime                   16.733376309275627
-    roadCameraState.processingTime                       16.218071803450584
+    roadCameraState start of frame                       0.049583
+    wideRoadCameraState published                        35.01206
+    WideRoadCamera: Image set                            35.020028
+    roadCameraState published                            38.508261
+    RoadCamera: Image set                                38.520344
+    RoadCamera: Transformed                              38.616176
+    wideRoadCameraState.processingTime                   3.152403049170971
+    roadCameraState.processingTime                       6.453451234847307
  modeld
-    Image added                                          51.346522
-    Extra image added                                    53.179467
-    Execution finished                                   71.584437
-    modelV2 published                                    71.76881
-    modelV2.modelExecutionTime                           22.54236489534378
+    Image added                                          40.909841
+    Extra image added                                    42.515027
+    Execution finished                                   63.002552
+    modelV2 published                                    63.148747
+    modelV2.modelExecutionTime                           23.62649142742157
    modelV2.gpuExecutionTime                             0.0
  plannerd
-    lateralPlan published                                77.381862
-    longitudinalPlan published                           84.207972
-    lateralPlan.solverExecutionTime                      1.3547739945352077
-    longitudinalPlan.solverExecutionTime                 2.0179999992251396
+    lateralPlan published                                66.915049
+    longitudinalPlan published                           69.715999
+    lateralPlan.solverExecutionTime                      0.8170719956979156
+    longitudinalPlan.solverExecutionTime                 0.5619999719783664
  controlsd
-    Data sampled                                         78.909759
-    Events updated                                       79.711884
-    sendcan published                                    80.721038
-    controlsState published                              81.081398
-    Data sampled                                         88.663748
-    Events updated                                       89.535403
-    sendcan published                                    90.587889
-    controlsState published                              91.019707
-    Data sampled                                         98.667003
-    Events updated                                       99.661261
-    sendcan published                                    100.776507
-    controlsState published                              101.198794
-    Data sampled                                         108.967078
-    Events updated                                       109.95842
-    sendcan published                                    111.263142
-    controlsState published                              111.678085
-    Data sampled                                         118.574923
-    Events updated                                       119.608555
-    sendcan published                                    120.73427
-    controlsState published                              121.111036
-    Data sampled                                         128.596408
-    Events updated                                       129.382283
-    sendcan published                                    130.330083
-    controlsState published                              130.676485
+    Data sampled                                         70.217763
+    Events updated                                       71.037178
+    sendcan published                                    72.278775
+    controlsState published                              72.825226
+    Data sampled                                         80.008354
+    Events updated                                       80.787666
+    sendcan published                                    81.849682
+    controlsState published                              82.238323
+    Data sampled                                         90.521123
+    Events updated                                       91.626003
+    sendcan published                                    93.413218
+    controlsState published                              94.143989
+    Data sampled                                         100.991497
+    Events updated                                       101.973774
+    sendcan published                                    103.565575
+    controlsState published                              104.146088
+    Data sampled                                         110.284387
+    Events updated                                       111.183541
+    sendcan published                                    112.981692
+    controlsState published                              113.731994
  boardd
-    sending sendcan to panda: 250027001751393037323631   90.7257
-    sendcan sent to panda: 250027001751393037323631      91.078143
-    sending sendcan to panda: 250027001751393037323631   100.941766
-    sendcan sent to panda: 250027001751393037323631      101.306865
-    sending sendcan to panda: 250027001751393037323631   111.411786
-    sendcan sent to panda: 250027001751393037323631      111.754074
-    sending sendcan to panda: 250027001751393037323631   120.875987
-    sendcan sent to panda: 250027001751393037323631      121.188535
-    sending sendcan to panda: 250027001751393037323631   130.454248
-    sendcan sent to panda: 250027001751393037323631      130.757994
-    sending sendcan to panda: 250027001751393037323631   140.353234
+    sending sendcan to panda: 250027001751393037323631   81.928119
+    sendcan sent to panda: 250027001751393037323631      82.164834
+    sending sendcan to panda: 250027001751393037323631   93.569986
+    sendcan sent to panda: 250027001751393037323631      93.92795
+    sending sendcan to panda: 250027001751393037323631   103.689167
+    sendcan sent to panda: 250027001751393037323631      104.012235
+    sending sendcan to panda: 250027001751393037323631   113.109555
+    sendcan sent to panda: 250027001751393037323631      113.525487
+    sending sendcan to panda: 250027001751393037323631   122.508434
+    sendcan sent to panda: 250027001751393037323631      122.834314
 ```
--- a/tools/latencylogger/latency_logger.py
+++ b/tools/latencylogger/latency_logger.py
@ -1,14 +1,16 @@
+#!/usr/bin/env python3
 import argparse
 import json
 import matplotlib.patches as mpatches
 import matplotlib.pyplot as plt
 import mpld3
 import sys
+from bisect import bisect_left, bisect_right
 from collections import defaultdict

 from tools.lib.logreader import logreader_from_route_or_segment

-DEMO_ROUTE = "9f583b1d93915c31|2022-04-26--18-49-49"
+DEMO_ROUTE = "9f583b1d93915c31|2022-05-18--10-49-51--0"

 SERVICES = ['camerad', 'modeld', 'plannerd', 'controlsd', 'boardd']
 # Retrive controlsd frameId from lateralPlan, mismatch with longitudinalPlan will be ignored
@ -51,7 +53,7 @@ def read_logs(lr):
        for key in MONOTIME_KEYS:
          if hasattr(msg_obj, key):
            if getattr(msg_obj, key) == 0:
-              # Filter out controlsd messages which arrive before the camera loop 
+              # Filter out controlsd messages which arrive before the camera loop
              continue_outer = True
            elif getattr(msg_obj, key) in mono_to_frame:
              frame_id = mono_to_frame[getattr(msg_obj, key)]
@ -89,11 +91,22 @@ def read_logs(lr):
    print("Warning, many frame mismatches", len(frame_mismatches))
  return (data, frame_mismatches)

+# This is not needed in 3.10 as a "key" parameter is added to bisect
+class KeyifyList(object):
+  def __init__(self, inner, key):
+    self.inner = inner
+    self.key = key
+  def __len__(self):
+    return len(self.inner)
+  def __getitem__(self, k):
+    return self.key(self.inner[k])
+
 def find_frame_id(time, service, start_times, end_times):
-  for frame_id in reversed(start_times):
-    if start_times[frame_id][service] and end_times[frame_id][service]:
-      if start_times[frame_id][service] <= time <= end_times[frame_id][service]:
-        yield frame_id
+  left = bisect_left(KeyifyList(list(start_times.items()),
+                     lambda x: x[1][service] if x[1][service] else -1), time) - 1
+  right = bisect_right(KeyifyList(list(end_times.items()),
+                      lambda x: x[1][service] if x[1][service] else float("inf")), time)
+  return left, right

 def find_t0(start_times, frame_id=-1):
  frame_id = frame_id if frame_id > -1 else min(start_times.keys())
@ -130,13 +143,13 @@ def insert_cloudlogs(lr, timestamps, start_times, end_times):
          timestamps[latest_controls_frameid][service].append((event, time))
          end_times[latest_controls_frameid][service] = time
        else:
-          frame_id_gen = find_frame_id(time, service, start_times, end_times)
-          frame_id = next(frame_id_gen, False)
+          frame_id = find_frame_id(time, service, start_times, end_times)
          if frame_id:
+            if frame_id[0] != frame_id[1]:
+              event += " (warning: ambiguity)"
+            frame_id = frame_id[0]
            if service == 'controlsd':
              latest_controls_frameid = frame_id
-            if next(frame_id_gen, False):
-              event += " (warning: ambiguity)"
            timestamps[frame_id][service].append((event, time))
          else:
            failed_inserts += 1
@ -162,13 +175,18 @@ def print_timestamps(timestamps, durations, start_times, relative):
      for event, time in durations[frame_id][service]:
        print("    "+'%-53s%-53s' %(event, str(time*1000)))

-def graph_timestamps(timestamps, start_times, end_times, relative):
+def graph_timestamps(timestamps, start_times, end_times, relative, offset_services=False, title=""):
+  # mpld3 doesn't convert properly to D3 font sizes
+  plt.rcParams.update({'font.size': 18})
+
  t0 = find_t0(start_times)
  fig, ax = plt.subplots()
-  ax.set_xlim(0, 150 if relative else 750)
-  ax.set_ylim(0, 15)
-  ax.set_xlabel('milliseconds')
+  ax.set_xlim(0, 130 if relative else 750)
+  ax.set_ylim(0, 17)
+  ax.set_xlabel('Time (milliseconds)')
  colors = ['blue', 'green', 'red', 'yellow', 'purple']
+  offsets = [[0, -5*j] for j in range(len(SERVICES))] if offset_services else None
+  height = 0.3 if offset_services else 0.9
  assert len(colors) == len(SERVICES), 'Each service needs a color'

  points = {"x": [], "y": [], "labels": []}
@ -185,12 +203,15 @@ def graph_timestamps(timestamps, start_times, end_times, relative):
          points['x'].append((event[1]-t0)/1e6)
          points['y'].append(i)
          points['labels'].append(event[0])
-    ax.broken_barh(service_bars, (i-0.45, 0.9), facecolors=(colors), alpha=0.5)
+    ax.broken_barh(service_bars, (i-height/2, height), facecolors=(colors), alpha=0.5, offsets=offsets)

  scatter = ax.scatter(points['x'], points['y'], marker='d', edgecolor='black')
  tooltip = mpld3.plugins.PointLabelTooltip(scatter, labels=points['labels'])
-
  mpld3.plugins.connect(fig, tooltip)
+
+  plt.title(title)
+  # Set size relative window size is not trivial: https://github.com/mpld3/mpld3/issues/65
+  fig.set_size_inches(18, 9)
  plt.legend(handles=[mpatches.Patch(color=colors[i], label=SERVICES[i]) for i in range(len(SERVICES))])
  return fig

@ -206,6 +227,7 @@ if __name__ == "__main__":
  parser.add_argument("--relative", action="store_true", help="Make timestamps relative to the start of each frame")
  parser.add_argument("--demo", action="store_true", help="Use the demo route instead of providing one")
  parser.add_argument("--plot", action="store_true", help="If a plot should be generated")
+  parser.add_argument("--offset", action="store_true", help="Vertically offset service to better visualize overlap")
  parser.add_argument("route_or_segment_name", nargs='?', help="The route to print")

  if len(sys.argv) == 1:
@ -219,4 +241,4 @@ if __name__ == "__main__":
  data, _ = get_timestamps(lr)
  print_timestamps(data['timestamp'], data['duration'], data['start'], args.relative)
  if args.plot:
-    mpld3.show(graph_timestamps(data['timestamp'], data['start'], data['end'], args.relative))
+    mpld3.show(graph_timestamps(data['timestamp'], data['start'], data['end'], args.relative, offset_services=args.offset, title=r))
--- a/tools/replay/ui.py
+++ b/tools/replay/ui.py
@ -17,7 +17,7 @@ from tools.replay.lib.ui_helpers import (_BB_TO_FULL_FRAME, UP,
                                         maybe_update_radar_points, plot_lead,
                                         plot_model,
                                         pygame_modules_have_loaded)
-from cereal.visionipc.visionipc_pyx import VisionIpcClient, VisionStreamType # pylint: disable=no-name-in-module, import-error
+from cereal.visionipc import VisionIpcClient, VisionStreamType

 os.environ['BASEDIR'] = BASEDIR

@ -99,7 +99,7 @@ def ui_thread(addr):

  draw_plots = init_plots(plot_arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_colors, plot_styles)

-  vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_RGB_ROAD, True)
+  vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
  while 1:
    list(pygame.event.get())

@ -111,21 +111,17 @@ def ui_thread(addr):
    if not vipc_client.is_connected():
      vipc_client.connect(True)

-    rgb_img_raw = vipc_client.recv()
+    yuv_img_raw = vipc_client.recv()

-    if rgb_img_raw is None or not rgb_img_raw.any():
+    if yuv_img_raw is None or not yuv_img_raw.any():
      continue

-    num_px = len(rgb_img_raw) // 3
-    imgff_shape = (vipc_client.height, vipc_client.width, 3)
+    imgff = np.frombuffer(yuv_img_raw, dtype=np.uint8).reshape((vipc_client.height * 3 // 2, vipc_client.width))
+    num_px = vipc_client.width * vipc_client.height
+    bgr = cv2.cvtColor(imgff, cv2.COLOR_YUV2RGB_I420)

-    if imgff is None or imgff.shape != imgff_shape:
-      imgff = np.zeros(imgff_shape, dtype=np.uint8)
-
-    imgff = np.frombuffer(rgb_img_raw, dtype=np.uint8).reshape((vipc_client.height, vipc_client.width, 3))
-    imgff = imgff[:, :, ::-1]  # Convert BGR to RGB
    zoom_matrix = _BB_TO_FULL_FRAME[num_px]
-    cv2.warpAffine(imgff, zoom_matrix[:2], (img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP)
+    cv2.warpAffine(bgr, zoom_matrix[:2], (img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP)

    intrinsic_matrix = _INTRINSICS[num_px]

--- a/tools/sim/bridge.py
+++ b/tools/sim/bridge.py
@ -15,7 +15,7 @@ import pyopencl.array as cl_array

 import cereal.messaging as messaging
 from cereal import log
-from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType  # pylint: disable=no-name-in-module, import-error
+from cereal.visionipc import VisionIpcServer, VisionStreamType
 from common.basedir import BASEDIR
 from common.numpy_fast import clip
 from common.params import Params