rebase

.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
 

Pipfile

@@ -4,6 +4,7 @@ url = "https://pypi.org/simple"
 verify_ssl = true
 
 [dev-packages]
+av = "*"
 azure-storage-blob = "~=2.1"
 control = "*"
 coverage = "*"

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

SConstruct

@@ -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')
 

cereal

@@ -1 +1 @@
-Subproject commit d61736e301cf27d10b74beac278214f974c55fe4
+Subproject commit f13d16c78a69fdef26b7448520c4398515758861

common/SConscript

@@ -22,13 +22,8 @@ files = [
   'visionimg.cc',
 ]
 
-if arch == "larch64":
+_gpucommon = fxn('gpucommon', files)
-  _gpu_libs = ["GLESv2"]
+Export('_common', '_gpucommon')
-else:
-  _gpu_libs = ["GL"]
-
-_gpucommon = fxn('gpucommon', files, LIBS=_gpu_libs)
-Export('_common', '_gpucommon', '_gpu_libs')
 
 if GetOption('test'):
   env.Program('tests/test_util', ['tests/test_util.cc'], LIBS=[_common])

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

common/version.h

@@ -1 +1 @@
-#define COMMA_VERSION "0.8.14"
+#define COMMA_VERSION "0.8.15"

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>|

laika_repo

@@ -1 +1 @@
-Subproject commit f5f76d28b4827c3fb706d542729651ceef6c06bd
+Subproject commit 231eafbf659309b85acb5b575b7f898e7a4f196e

opendbc

@@ -1 +1 @@
-Subproject commit 210237fa635eeb76ad855c2031d2cad3bde3a2c0
+Subproject commit 7701277d2666119bc7fcaca9f8cfefd50cd5b071

panda

@@ -1 +1 @@
-Subproject commit 36c62afa0c170aa1b7a39bcae3316ffb844499e8
+Subproject commit 6d19b46ef525b0ae224e0bf7db71918e96f9be66

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
 

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))

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) {

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
+  {0x3030, 0x0037}, // PLL_MULTIPLIER
-  {0x3036, 0x000C}, // OP_WORD_CLK_DIV
+  {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
+  {0x300C, 0x0672}, // LINE_LENGTH_PCK (valid for 3-exposure HDR)
-  {0x300A, 0x0652}, // FRAME_LENGTH_LINES
+  {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
 

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

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"),

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

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"),
 }
 
 

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 = [

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)

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),

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

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_AstDrSw", "CGW1"),       # Passenger Door
-      ("CF_Gway_RLDrSw", "CGW2"),        # Rear reft door
+      ("CF_Gway_RLDrSw", "CGW2"),        # Rear left Door
-      ("CF_Gway_RRDrSw", "CGW2"),        # Rear right 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)

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)

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)

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'),
 }
 

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,
+}

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),
 }

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),

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':

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:

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
 

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)
+      set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=1.7, FRICTION=0.06)
-      ret.steerActuatorDelay = 0.3
 
     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

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')
+    set_torque_tune(tune, MAX_LAT_ACCEL, FRICTION)
-    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]
   elif 'PID' in str(name):
     tune.init('pid')
     tune.pid.kiBP = [0.0]

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',
     ],

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',

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
 

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: {

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

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

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])
+      heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.15])
-      self.lat_mpc.set_weights(path_cost, heading_cost, self.steer_rate_cost)
+      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)

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)

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()
+          CP = msg.carParams
-
+          car_fw = CP.carFw
-          car_fw = msg.carParams.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:

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 = {}
 

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.home-only yes
-nmcli connection modify --temporary lte gsm.auto-config yes
+#nmcli connection modify --temporary lte gsm.auto-config yes
-nmcli connection modify --temporary lte connection.autoconnect-retries 20
+#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

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),

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)
+      measurements = process_measurements(new_meas, self.astro_dog)
-
+      pos_fix = calc_pos_fix(measurements, min_measurements=4)
-
-      pos_fix = calc_pos_fix(measurements)
       # To get a position fix a minimum of 5 measurements are needed.
-      # Each report can contain less and some measurement can't be processed.
+      # Each report can contain less and some measurements can't be processed.
-      if len(pos_fix) > 0:
+      corrected_measurements = []
-        measurements = correct_measurements(measurements, pos_fix[0][:3], dog)
+      if len(pos_fix) > 0 and linalg.norm(pos_fix[1]) < 100:
-      meas_msgs = [create_measurement_msg(m) for m in measurements]
+        corrected_measurements = correct_measurements(measurements, pos_fix[0][:3], self.astro_dog)
 
       t = ublox_mono_time * 1e-9
-
+      self.update_localizer(pos_fix, t, corrected_measurements)
-      self.update_localizer(pos_fix, t, 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)
 

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;

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)
+    msg = laikad.process_ublox_msg(m.ubloxGnss, m.logMonoTime)
-    if msg is not None and len(msg.gnssMeasurements.correctedMeasurements) > 0:
+    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(use_internet=True)
-    laikad = Laikad()
+    msgs = process_msgs(self.logs, laikad)
-    correct_msgs = verify_messages(self.logs, dog, 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()

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
 

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

selfdrive/loggerd/video_writer.cc

@@ -1,3 +1,4 @@
+#pragma clang diagnostic ignored "-Wdeprecated-declarations"
 #include <cassert>
 #include <cstdlib>
 

selfdrive/modeld/models/supercombo.dlc

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:ba3fe3e61853cc1434e3e220f40c8e9d1f1b9bab8458196ba3bea6a10b82c6ed
+oid sha256:027cbb1fabae369878271cb0e3505071a8bdaa07473fad9a0b2e8d695c5dc1ff
-size 72718099
+size 76725611

selfdrive/modeld/models/supercombo.onnx

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:bda57c1a66944f5a633ecd739a24d62702c717a234f2fdcc499dfa1d61c3c19e
+oid sha256:484976ea5bd4ddcabc82e95faf30d7311a27802c1e337472558699fa2395a499
-size 73147489
+size 77472267

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>

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

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

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, \

selfdrive/test/process_replay/model_replay_ref_commit

@@ -1 +1 @@
-5fb5cd71a6e878cf45593ecea22c93e932f70c31
+f74ab97371be93fdc28333e5ea12bbb78c3a32d0

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()
+  with OpenpilotPrefix():
-  try:
     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()

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-b8c35486e8354713221d4237e97e5abced6f5228
+0956446adfa91506f0a3d88f893e041bfb2890c1

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
+  # TODO: remove after getting new route for Mazda
-  migration = {
+  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)
+      car_fingerprint = fp_migration.get(msg.carParams.carFingerprint, msg.carParams.carFingerprint)
-      if len(msg.carParams.carFw) and (car_fingerprint in FW_VERSIONS):
+      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"

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_path, args.ignore_fields, args.ignore_msgs)
-    res, log_msgs = test_process(cfg, lr, ref_log_fn, 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("\n\nTo push the new reference logs for this commit run:")
+      print("./test_processes.py --upload-only")
-        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}")

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]);
       }

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};
   }

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) {
+void CameraViewWidget::vipcFrameReceived(VisionBuf *buf, uint32_t frame_id) {
-  latest_frame = buf;
+  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)) {
+    if (VisionBuf *buf = vipc_client->recv(&meta_main, 1000)) {
-      emit vipcThreadFrameReceived(buf);
+      emit vipcThreadFrameReceived(buf, meta_main.frame_id);
     }
   }
 }

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);
 };

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");

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 = argparse.ArgumentParser(description="Decode video streams and broacast on VisionIPC")
-  parser.add_argument("addr", help="Address of comma 3")
+  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("--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()
 

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.
+Timestamps are visualized as diamonds
-![plot-1](https://user-images.githubusercontent.com/42323981/162108694-fbfe907b-a1ee-4cc7-bc8b-162a7d9305d4.png)
 
+|  | 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
+    roadCameraState start of frame                       0.049583
-    wideRoadCameraState published                        47.804745
+    wideRoadCameraState published                        35.01206
-    WideRoadCamera: Image set                            47.839849
+    WideRoadCamera: Image set                            35.020028
-    roadCameraState published                            48.319166
+    roadCameraState published                            38.508261
-    RoadCamera: Image set                                48.354478
+    RoadCamera: Image set                                38.520344
-    RoadCamera: Transformed                              48.430258
+    RoadCamera: Transformed                              38.616176
-    wideRoadCameraState.processingTime                   16.733376309275627
+    wideRoadCameraState.processingTime                   3.152403049170971
-    roadCameraState.processingTime                       16.218071803450584
+    roadCameraState.processingTime                       6.453451234847307
   modeld
-    Image added                                          51.346522
+    Image added                                          40.909841
-    Extra image added                                    53.179467
+    Extra image added                                    42.515027
-    Execution finished                                   71.584437
+    Execution finished                                   63.002552
-    modelV2 published                                    71.76881
+    modelV2 published                                    63.148747
-    modelV2.modelExecutionTime                           22.54236489534378
+    modelV2.modelExecutionTime                           23.62649142742157
     modelV2.gpuExecutionTime                             0.0
   plannerd
-    lateralPlan published                                77.381862
+    lateralPlan published                                66.915049
-    longitudinalPlan published                           84.207972
+    longitudinalPlan published                           69.715999
-    lateralPlan.solverExecutionTime                      1.3547739945352077
+    lateralPlan.solverExecutionTime                      0.8170719956979156
-    longitudinalPlan.solverExecutionTime                 2.0179999992251396
+    longitudinalPlan.solverExecutionTime                 0.5619999719783664
   controlsd
-    Data sampled                                         78.909759
+    Data sampled                                         70.217763
-    Events updated                                       79.711884
+    Events updated                                       71.037178
-    sendcan published                                    80.721038
+    sendcan published                                    72.278775
-    controlsState published                              81.081398
+    controlsState published                              72.825226
-    Data sampled                                         88.663748
+    Data sampled                                         80.008354
-    Events updated                                       89.535403
+    Events updated                                       80.787666
-    sendcan published                                    90.587889
+    sendcan published                                    81.849682
-    controlsState published                              91.019707
+    controlsState published                              82.238323
-    Data sampled                                         98.667003
+    Data sampled                                         90.521123
-    Events updated                                       99.661261
+    Events updated                                       91.626003
-    sendcan published                                    100.776507
+    sendcan published                                    93.413218
-    controlsState published                              101.198794
+    controlsState published                              94.143989
-    Data sampled                                         108.967078
+    Data sampled                                         100.991497
-    Events updated                                       109.95842
+    Events updated                                       101.973774
-    sendcan published                                    111.263142
+    sendcan published                                    103.565575
-    controlsState published                              111.678085
+    controlsState published                              104.146088
-    Data sampled                                         118.574923
+    Data sampled                                         110.284387
-    Events updated                                       119.608555
+    Events updated                                       111.183541
-    sendcan published                                    120.73427
+    sendcan published                                    112.981692
-    controlsState published                              121.111036
+    controlsState published                              113.731994
-    Data sampled                                         128.596408
-    Events updated                                       129.382283
-    sendcan published                                    130.330083
-    controlsState published                              130.676485
   boardd
-    sending sendcan to panda: 250027001751393037323631   90.7257
+    sending sendcan to panda: 250027001751393037323631   81.928119
-    sendcan sent to panda: 250027001751393037323631      91.078143
+    sendcan sent to panda: 250027001751393037323631      82.164834
-    sending sendcan to panda: 250027001751393037323631   100.941766
+    sending sendcan to panda: 250027001751393037323631   93.569986
-    sendcan sent to panda: 250027001751393037323631      101.306865
+    sendcan sent to panda: 250027001751393037323631      93.92795
-    sending sendcan to panda: 250027001751393037323631   111.411786
+    sending sendcan to panda: 250027001751393037323631   103.689167
-    sendcan sent to panda: 250027001751393037323631      111.754074
+    sendcan sent to panda: 250027001751393037323631      104.012235
-    sending sendcan to panda: 250027001751393037323631   120.875987
+    sending sendcan to panda: 250027001751393037323631   113.109555
-    sendcan sent to panda: 250027001751393037323631      121.188535
+    sendcan sent to panda: 250027001751393037323631      113.525487
-    sending sendcan to panda: 250027001751393037323631   130.454248
+    sending sendcan to panda: 250027001751393037323631   122.508434
-    sendcan sent to panda: 250027001751393037323631      130.757994
+    sendcan sent to panda: 250027001751393037323631      122.834314
-    sending sendcan to panda: 250027001751393037323631   140.353234
 ```

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):
+  left = bisect_left(KeyifyList(list(start_times.items()),
-    if start_times[frame_id][service] and end_times[frame_id][service]:
+                     lambda x: x[1][service] if x[1][service] else -1), time) - 1
-      if start_times[frame_id][service] <= time <= end_times[frame_id][service]:
+  right = bisect_right(KeyifyList(list(end_times.items()),
-        yield frame_id
+                      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 = find_frame_id(time, service, start_times, end_times)
-          frame_id = next(frame_id_gen, False)
           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_xlim(0, 130 if relative else 750)
-  ax.set_ylim(0, 15)
+  ax.set_ylim(0, 17)
-  ax.set_xlabel('milliseconds')
+  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))

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 = np.frombuffer(yuv_img_raw, dtype=np.uint8).reshape((vipc_client.height * 3 // 2, vipc_client.width))
-    imgff_shape = (vipc_client.height, vipc_client.width, 3)
+    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]
 

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