Merge remote-tracking branch 'upstream/master' into draw-map-with-ui

.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

common/version.h

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

opendbc

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

release/files_common

@@ -491,6 +491,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/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/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/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,7 +2,7 @@
 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
@@ -34,7 +34,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
@@ -243,6 +243,21 @@ 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.
+      ret.lateralTuning.init('torque')
+      ret.lateralTuning.torque.useSteeringAngle = True
+      ret.lateralTuning.torque.kp = 1.0 / max_lat_accel
+      ret.lateralTuning.torque.kf = 1.0 / max_lat_accel
+      ret.lateralTuning.torque.ki = 0.1 / max_lat_accel
+      ret.lateralTuning.torque.friction = 0.01
 
     # Genesis
     elif candidate == CAR.GENESIS_G70:
@@ -321,7 +336,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),
@@ -1164,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 = {
@@ -1181,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}
 
@@ -1228,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_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):
@@ -168,7 +170,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/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]),
 
@@ -1767,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/controls/controlsd.py

@@ -294,6 +294,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}

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

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/widgets/cameraview.cc

@@ -163,7 +163,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(); });
@@ -214,14 +214,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]);
@@ -244,7 +250,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;
 
@@ -263,19 +269,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) {
@@ -286,8 +296,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);
 };

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
@@ -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/latency_logger.py

@@ -1,9 +1,11 @@
+#!/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
@@ -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,12 +175,16 @@ 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, title=""):
+  # mpld3 doesn't convert properly to D3 font sizes
+  plt.rcParams.update({'font.size': 18})
+
   t0 = find_t0(start_times)
   fig, ax = plt.subplots()
   ax.set_xlim(0, 150 if relative else 750)
   ax.set_ylim(0, 15)
-  ax.set_xlabel('milliseconds')
+  ax.set_xlabel('Time (milliseconds)')
+  ax.set_ylabel('Frame ID')
   colors = ['blue', 'green', 'red', 'yellow', 'purple']
   assert len(colors) == len(SERVICES), 'Each service needs a color'
 
@@ -189,9 +206,12 @@ def graph_timestamps(timestamps, start_times, end_times, relative):
 
   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)
+  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
 
 def get_timestamps(lr):
@@ -219,4 +239,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, r))