radar tracks to staging :)

cereal/car.capnp

@@ -117,6 +117,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     paramsdTemporaryError @50;
     paramsdPermanentError @119;
     actuatorsApiUnavailable @120;
+    hyundaiRadarTracksAvailable @150;  # Use ID 150 instead of 142
 
     # FrogPilot Events
     accel30 @121;
@@ -488,6 +489,7 @@ struct CarParams {
   enableDsu @5 :Bool;        # driving support unit
   enableBsm @56 :Bool;       # blind spot monitoring
   flags @64 :UInt32;         # flags for car specific quirks
+  fpFlags @76 :UInt32;       # flags for car specific quirks that are fingerprint specific
   experimentalLongitudinalAvailable @71 :Bool;
 
   minEnableSpeed @7 :Float32;

common/params.cc

@@ -136,6 +136,9 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"GsmRoaming", PERSISTENT},
     {"HardwareSerial", PERSISTENT},
     {"HasAcceptedTerms", PERSISTENT},
+    {"HyundaiRadarTracksAvailable", PERSISTENT},
+    {"HyundaiRadarTracksAvailableCache", PERSISTENT},
+    {"HyundaiRadarTracksAvailablePersistent", PERSISTENT},
     {"IMEI", PERSISTENT},
     {"InstallDate", PERSISTENT},
     {"IsDriverViewEnabled", CLEAR_ON_MANAGER_START},

selfdrive/car/__init__.py

@@ -11,6 +11,7 @@
 from openpilot.common.utils import Freezable
 from openpilot.selfdrive.car.docs_definitions import CarDocs
 
+DT_CTRL = 0.01  # car state and control loop timestep (s)
 
 # kg of standard extra cargo to count for drive, gas, etc...
 STD_CARGO_KG = 136.
@@ -272,6 +273,8 @@ class PlatformConfig(Freezable):
 
   flags: int = 0
 
+  fpFlags: int = 0
+
   platform_str: str | None = None
 
   def __hash__(self) -> int:
@@ -315,3 +318,11 @@ def create_dbc_map(cls) -> dict[str, DbcDict]:
   @classmethod
   def with_flags(cls, flags: IntFlag) -> set['Platforms']:
     return {p for p in cls if p.config.flags & flags}
+
+  @classmethod
+  def with_fp_flags(cls, fpFlags: IntFlag) -> set['Platforms']:
+    return {p for p in cls if p.config.fpFlags & fpFlags}
+
+  @classmethod
+  def without_fp_flags(cls, fpFlags: IntFlag) -> set['Platforms']:
+    return {p for p in cls if not (p.config.fpFlags & fpFlags)}

selfdrive/car/card.py

@@ -1,6 +1,8 @@
 #!/usr/bin/env python3
 import os
 import time
+import threading
+from types import SimpleNamespace
 
 import cereal.messaging as messaging
 
@@ -15,6 +17,7 @@
 from openpilot.selfdrive.pandad import can_list_to_can_capnp
 from openpilot.selfdrive.car.car_helpers import get_car, get_one_can
 from openpilot.selfdrive.car.interfaces import CarInterfaceBase
+from openpilot.selfdrive.car.param_manager import ParamManager
 from openpilot.selfdrive.controls.lib.events import Events
 
 from openpilot.selfdrive.frogpilot.frogpilot_variables import get_frogpilot_toggles, update_frogpilot_toggles
@@ -40,6 +43,7 @@ def __init__(self, CI=None):
 
     self.last_actuators_output = car.CarControl.Actuators.new_message()
 
+    self.frogpilot_toggles = get_frogpilot_toggles()
     self.params = Params()
 
     if CI is None:
@@ -108,14 +112,22 @@ def __init__(self, CI=None):
     self.params.put_nonblocking("CarParamsCache", cp_bytes)
     self.params.put_nonblocking("CarParamsPersistent", cp_bytes)
 
+    self.param_manager: ParamManager = ParamManager()
+    self.param_manager.update(self.params)
+    self._params_list: SimpleNamespace = self.param_manager.get_params()
+
     update_frogpilot_toggles()
 
   def state_update(self) -> car.CarState:
     """carState update loop, driven by can"""
 
     # Update carState from CAN
     can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
-    CS, FPCS = self.CI.update(self.CC_prev, can_strs, self.frogpilot_toggles)
+
+    if self.sm['frogpilotPlan'].togglesUpdated:
+        self.frogpilot_toggles = get_frogpilot_toggles()
+
+    CS, FPCS = self.CI.update(self.CC_prev, can_strs, self._params_list, self.frogpilot_toggles)
 
     self.sm.update(0)
 
@@ -206,14 +218,33 @@ def step(self):
     self.initialized_prev = initialized
     self.CS_prev = CS.as_reader()
 
-  def card_thread(self):
-    while True:
-      self.step()
-      self.rk.monitor_time()
+  def fp_params_thread(self, event: threading.Event) -> None:
+    while not event.is_set():
+        self.param_manager.update(self.params)
+        self._params_list = self.param_manager.get_params()
+        time.sleep(0.1)
 
-      # Update FrogPilot parameters
-      if self.sm['frogpilotPlan'].togglesUpdated:
-        self.frogpilot_toggles = get_frogpilot_toggles()
+  def frogpilot_params_thread(self, event: threading.Event) -> None:
+    while not event.is_set():
+        if self.sm['frogpilotPlan'].togglesUpdated:
+            self.frogpilot_toggles = get_frogpilot_toggles()
+        time.sleep(0.1)
+
+  def card_thread(self):
+    event = threading.Event()
+    fp_thread = threading.Thread(target=self.fp_params_thread, args=(event,))
+    frog_thread = threading.Thread(target=self.frogpilot_params_thread, args=(event,))
+
+    try:
+        fp_thread.start()
+        frog_thread.start()
+        while True:
+            self.step()
+            self.rk.monitor_time()
+    finally:
+        event.set()
+        fp_thread.join()
+        frog_thread.join()
 
 def main():
   config_realtime_process(4, Priority.CTRL_HIGH)

selfdrive/car/fingerprinting.py

@@ -0,0 +1,37 @@
+from collections.abc import Callable
+
+import cereal.messaging as messaging
+from openpilot.selfdrive.car import gen_empty_fingerprint
+
+FRAME_FINGERPRINT = 25  # 0.25s
+
+
+def get_one_can(logcan):
+  while True:
+    can = messaging.recv_one_retry(logcan)
+    if len(can.can) > 0:
+      return can
+
+
+def can_fingerprint(next_can: Callable) -> tuple[str | None, dict[int, dict]]:
+  finger = gen_empty_fingerprint()
+  frame = 0
+  done = False
+
+  while not done:
+    a = next_can()
+
+    for can in a.can:
+      # The fingerprint dict is generated for all buses, this way the car interface
+      # can use it to detect a (valid) multipanda setup and initialize accordingly
+      if can.src < 128:
+        if can.src not in finger:
+          finger[can.src] = {}
+        finger[can.src][can.address] = len(can.dat)
+
+    # bail if we've been waiting for more than 2s
+    done = frame > 100
+
+    frame += 1
+
+  return finger

selfdrive/car/hyundai/carcontroller.py

@@ -1,6 +1,8 @@
 from cereal import car
+import cereal.messaging as messaging
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.numpy_fast import clip
+from openpilot.common.params import Params
 from openpilot.common.realtime import DT_CTRL
 from opendbc.can.packer import CANPacker
 from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fault_avoidance
@@ -59,6 +61,15 @@ def __init__(self, dbc_name, CP, VM):
     self.car_fingerprint = CP.carFingerprint
     self.last_button_frame = 0
 
+    sub_services = ['longitudinalPlan']
+    if CP.openpilotLongitudinalControl:
+      sub_services.append('radarState')
+
+    if sub_services:
+      self.sm = messaging.SubMaster(sub_services)
+
+    self.param_s = Params()
+
   def update(self, CC, CS, now_nanos, frogpilot_toggles):
     actuators = CC.actuators
     hud_control = CC.hudControl
@@ -108,6 +119,7 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles):
       if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
         can_sends.append([0x7b1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", self.CAN.ECAN])
 
+
     # CAN-FD platforms
     if self.CP.carFingerprint in CANFD_CAR:
       hda2 = self.CP.flags & HyundaiFlags.CANFD_HDA2
@@ -133,8 +145,7 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles):
         if hda2:
           can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame))
         if self.frame % 2 == 0:
-          can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
-                                                           set_speed_in_units, hud_control))
+          can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CS, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override, set_speed_in_units, hud_control))
           self.accel_last = accel
       else:
         # button presses
@@ -148,21 +159,22 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles):
       if not self.CP.openpilotLongitudinalControl:
         can_sends.extend(self.create_button_messages(CC, CS, use_clu11=True))
 
+
       if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
         # TODO: unclear if this is needed
         jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
         use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
         can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2),
                                                         hud_control, set_speed_in_units, stopping,
-                                                        CC.cruiseControl.override, use_fca, CS.out.cruiseState.available))
+                                                        CC.cruiseControl.override, use_fca, CS, self.CP, CS.out.cruiseState.available))
 
       # 20 Hz LFA MFA message
       if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value:
         can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled, CC.latActive))
 
       # 5 Hz ACC options
       if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:
-        can_sends.extend(hyundaican.create_acc_opt(self.packer))
+        can_sends.extend(hyundaican.create_acc_opt(self.packer, CS, self.CP))
 
       # 2 Hz front radar options
       if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl:
@@ -211,3 +223,25 @@ def create_button_messages(self, CC: car.CarControl, CS: car.CarState, use_clu11
             self.last_button_frame = self.frame
 
     return can_sends
+
+  def get_target_speed(self, v_cruise_kph_prev):
+    v_cruise_kph = v_cruise_kph_prev
+    if self.slc_state > 1:
+      v_cruise_kph = (self.speed_limit + self.speed_limit_offset) * CV.MS_TO_KPH
+      if not self.slc_active_stock:
+        v_cruise_kph = v_cruise_kph_prev
+    return v_cruise_kph
+
+    # jerk calculations thanks to apilot!
+  def cal_jerk(self, accel, actuators):
+    self.accel_raw = accel
+    if actuators.longControlState == LongCtrlState.off:
+      accel_diff = 0.0
+    elif actuators.longControlState == LongCtrlState.stopping:# or hud_control.softHold > 0:
+      accel_diff = 0.0
+    else:
+      accel_diff = self.accel_raw - self.accel_last_jerk
+
+    accel_diff /= DT_CTRL
+    self.jerk = self.jerk * 0.9 + accel_diff * 0.1
+    return self.jerk

selfdrive/car/hyundai/carstate.py

@@ -136,6 +136,7 @@ def update(self, cp, cp_cam, frogpilot_toggles):
     ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] == 2  # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
     ret.parkingBrake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1
     ret.espDisabled = cp.vl["TCS11"]["TCS_PAS"] == 1
+    ret.brakeLightsDEPRECATED = bool(cp.vl["TCS13"]["BrakeLight"])
     ret.accFaulted = cp.vl["TCS13"]["ACCEnable"] != 0  # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
 
     if self.CP.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV):

selfdrive/car/hyundai/enable_radar_tracks.py

@@ -0,0 +1,47 @@
+from openpilot.selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
+from openpilot.common.swaglog import cloudlog
+
+EXT_DIAG_REQUEST = b'\x10\x07'
+EXT_DIAG_RESPONSE = b'\x50\x07'
+
+WRITE_DATA_REQUEST = b'\x2e'
+WRITE_DATA_RESPONSE = b'\x68'
+
+RADAR_TRACKS_CONFIG = b"\x00\x00\x00\x01\x00\x01"
+
+
+def enable_radar_tracks(logcan, sendcan, bus=0, addr=0x7d0, config_data_id=b'\x01\x42', timeout=0.1, retry=10, debug=False):
+  cloudlog.warning("radar_tracks: enabling ...")
+
+  for i in range(retry):
+    try:
+      query = IsoTpParallelQuery(sendcan, logcan, bus, [addr], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], debug=debug)
+
+      for _, _ in query.get_data(timeout).items():
+        cloudlog.warning("radar_tracks: reconfigure radar to output radar points ...")
+
+        query = IsoTpParallelQuery(sendcan, logcan, bus, [addr],
+                                   [WRITE_DATA_REQUEST + config_data_id + RADAR_TRACKS_CONFIG],
+                                   [WRITE_DATA_RESPONSE], debug=debug)
+        query.get_data(0)
+
+        cloudlog.warning("radar_tracks: successfully enabled")
+        return True
+
+    except Exception as e:
+      cloudlog.exception(f"radar_tracks exception: {e}")
+
+    cloudlog.error(f"radar_tracks retry ({i + 1}) ...")
+  cloudlog.error(f"radar_tracks: failed")
+  return False
+
+
+if __name__ == "__main__":
+  import time
+  import cereal.messaging as messaging
+  sendcan = messaging.pub_sock('sendcan')
+  logcan = messaging.sub_sock('can')
+  time.sleep(1)
+
+  enabled = enable_radar_tracks(logcan, sendcan, bus=0, addr=0x7d0, config_data_id=b'\x01\x42', timeout=0.1, debug=False)
+  print(f"enabled: {enabled}")

selfdrive/car/hyundai/hyundaican.py

@@ -126,7 +126,7 @@ def create_lfahda_mfc(packer, enabled, lat_active, hda_set_speed=0):
   }
   return packer.make_can_msg("LFAHDA_MFC", 0, values)
 
-def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, cruise_available):
+def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, CS, CP, cruise_available):
   commands = []
 
   scc11_values = {
@@ -187,7 +187,7 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, se
 
   return commands
 
-def create_acc_opt(packer):
+def create_acc_opt(packer, CS, CP):
   commands = []
 
   scc13_values = {
@@ -211,3 +211,4 @@ def create_frt_radar_opt(packer):
     "CF_FCA_Equip_Front_Radar": 1,
   }
   return packer.make_can_msg("FRT_RADAR11", 0, frt_radar11_values)
+

selfdrive/car/hyundai/hyundaicanfd.py

@@ -121,7 +121,7 @@ def create_lfahda_cluster(packer, CAN, enabled, lat_active):
   return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)
 
 
-def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control):
+def create_acc_control(packer, CAN, CS, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control):
   jerk = 5
   jn = jerk / 50
   if not enabled or gas_override:
@@ -171,6 +171,18 @@ def create_spas_messages(packer, CAN, frame, left_blink, right_blink):
 
   return ret
 
+def create_fca_warning_light(packer, CAN, frame):
+  ret = []
+  if frame % 2 == 0:
+    values = {
+      'AEB_SETTING': 0x1,  # show AEB disabled icon
+      'SET_ME_2': 0x2,
+      'SET_ME_FF': 0xff,
+      'SET_ME_FC': 0xfc,
+      'SET_ME_9': 0x9,
+    }
+    ret.append(packer.make_can_msg("ADRV_0x160", CAN.ECAN, values))
+  return ret
 
 def create_adrv_messages(packer, CAN, frame):
   # messages needed to car happy after disabling
@@ -182,6 +194,8 @@ def create_adrv_messages(packer, CAN, frame):
   }
   ret.append(packer.make_can_msg("ADRV_0x51", CAN.ACAN, values))
 
+  ret.extend(create_fca_warning_light(packer, CAN, frame))
+
   if frame % 2 == 0:
     values = {
       'AEB_SETTING': 0x1,  # show AEB disabled icon