Voacc refinement and non-scc cruise check

opendbc_repo/opendbc/car/hyundai/interface.py

@@ -141,6 +141,9 @@ class CarInterface(CarInterfaceBase):
       # see https://github.com/commaai/opendbc/pull/1137/
       ret.dashcamOnly = True
 
+    if ret.flags & HyundaiFlags.NON_SCC:
+      ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.NON_SCC.value
+
     # Common longitudinal control setup
 
     ret.radarUnavailable = RADAR_START_ADDR not in fingerprint[1] or Bus.radar not in DBC[ret.carFingerprint]

opendbc_repo/opendbc/car/hyundai/values.py

@@ -95,6 +95,7 @@ class HyundaiSafetyFlags(IntFlag):
   FCEV_GAS = 256
   ALT_LIMITS_2 = 512
   CANFD_ANGLE_STEERING = 1024
+  NON_SCC = 4096
 
 
 class HyundaiStarPilotSafetyFlags(IntFlag):

opendbc_repo/opendbc/safety/modes/hyundai.h

@@ -55,6 +55,9 @@ const LongitudinalLimits HYUNDAI_LONG_LIMITS = {
 #define HYUNDAI_LDA_BUTTON_ADDR_CHECK \
   {.msg = {{0x391, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
 
+#define HYUNDAI_NON_SCC_CRUISE_ADDR_CHECK \
+  {.msg = {{0x367, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
+
 static const CanMsg HYUNDAI_TX_MSGS[] = {
   HYUNDAI_COMMON_TX_MSGS(0)
 };
@@ -140,6 +143,11 @@ static void hyundai_rx_hook(const CANPacket_t *msg) {
     }
   }
 
+  if ((msg->addr == 0x367U) && (msg->bus == 0U) && hyundai_non_scc) {
+    uint8_t cruise_set_speed = msg->data[0];
+    hyundai_common_cruise_state_check((cruise_set_speed > 0U) && (cruise_set_speed < 255U));
+  }
+
   if (msg->bus == 0U) {
     if (msg->addr == 0x251U) {
       int torque_driver_new = (GET_BYTES(msg, 0, 2) & 0x7ffU) - 1024U;
@@ -351,6 +359,17 @@ static safety_config hyundai_init(uint16_t param) {
        HYUNDAI_LDA_BUTTON_ADDR_CHECK
     };
 
+    static RxCheck hyundai_non_scc_rx_checks[] = {
+      HYUNDAI_COMMON_RX_CHECKS(false)
+      HYUNDAI_NON_SCC_CRUISE_ADDR_CHECK
+    };
+
+    static RxCheck hyundai_non_scc_rx_checks_lda[] = {
+      HYUNDAI_COMMON_RX_CHECKS(false)
+      HYUNDAI_NON_SCC_CRUISE_ADDR_CHECK
+      HYUNDAI_LDA_BUTTON_ADDR_CHECK
+    };
+
     static RxCheck hyundai_fcev_rx_checks_lda[] = {
       HYUNDAI_COMMON_RX_CHECKS(false)
       HYUNDAI_SCC12_ADDR_CHECK(0)
@@ -366,10 +385,18 @@ static safety_config hyundai_init(uint16_t param) {
         SET_RX_CHECKS(hyundai_fcev_rx_checks, ret);
       }
     } else {
-      if (hyundai_has_lda_button) {
-        SET_RX_CHECKS(hyundai_rx_checks_lda, ret);
+      if (hyundai_non_scc) {
+        if (hyundai_has_lda_button) {
+          SET_RX_CHECKS(hyundai_non_scc_rx_checks_lda, ret);
+        } else {
+          SET_RX_CHECKS(hyundai_non_scc_rx_checks, ret);
+        }
       } else {
-        SET_RX_CHECKS(hyundai_rx_checks, ret);
+        if (hyundai_has_lda_button) {
+          SET_RX_CHECKS(hyundai_rx_checks_lda, ret);
+        } else {
+          SET_RX_CHECKS(hyundai_rx_checks, ret);
+        }
       }
     }
   }

opendbc_repo/opendbc/safety/modes/hyundai_common.h

@@ -48,6 +48,9 @@ bool hyundai_has_lda_button = false;
 extern bool hyundai_aol_lkas_on_engage;
 bool hyundai_aol_lkas_on_engage = false;
 
+extern bool hyundai_non_scc;
+bool hyundai_non_scc = false;
+
 static uint8_t hyundai_last_button_interaction;  // button messages since the user pressed an enable button
 
 void hyundai_common_init(uint16_t param) {
@@ -61,6 +64,7 @@ void hyundai_common_init(uint16_t param) {
 
   const int HYUNDAI_PARAM_HAS_LDA_BUTTON = 1024;
   const uint16_t HYUNDAI_PARAM_AOL_LKAS_ON_ENGAGE = 2048;
+  const uint16_t HYUNDAI_PARAM_NON_SCC = 4096;
 
   hyundai_ev_gas_signal = GET_FLAG(param, HYUNDAI_PARAM_EV_GAS);
   hyundai_hybrid_gas_signal = !hyundai_ev_gas_signal && GET_FLAG(param, HYUNDAI_PARAM_HYBRID_GAS);
@@ -72,6 +76,7 @@ void hyundai_common_init(uint16_t param) {
 
   hyundai_has_lda_button = GET_FLAG(param, HYUNDAI_PARAM_HAS_LDA_BUTTON);
   hyundai_aol_lkas_on_engage = GET_FLAG(param, HYUNDAI_PARAM_AOL_LKAS_ON_ENGAGE);
+  hyundai_non_scc = GET_FLAG(param, HYUNDAI_PARAM_NON_SCC);
 
   hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES;
 

opendbc_repo/opendbc/safety/tests/test_hyundai.py

@@ -7,7 +7,7 @@ from opendbc.car.structs import CarParams
 from opendbc.safety.tests.libsafety import libsafety_py
 import opendbc.safety.tests.common as common
 from opendbc.safety.tests.common import CANPackerSafety
-from opendbc.safety.tests.hyundai_common import HyundaiAolLkasOnEngageBase, HyundaiAolLkasOnEngageStockBase, HyundaiButtonBase, HyundaiLongitudinalBase
+from opendbc.safety.tests.hyundai_common import Buttons, HyundaiAolLkasOnEngageBase, HyundaiAolLkasOnEngageStockBase, HyundaiButtonBase, HyundaiLongitudinalBase
 
 
 # 4 bit checkusm used in some hyundai messages
@@ -147,6 +147,27 @@ class TestHyundaiSafetyCameraSCC(TestHyundaiSafety):
     self.safety.init_tests()
 
 
+class TestHyundaiSafetyNonScc(TestHyundaiSafety):
+  def setUp(self):
+    self.packer = CANPackerSafety("hyundai_kia_generic")
+    self.safety = libsafety_py.libsafety
+    self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiSafetyFlags.NON_SCC)
+    self.safety.init_tests()
+
+  def _pcm_status_msg(self, enable):
+    values = {"CF_Lvr_CruiseSet": 30 if enable else 0}
+    return self.packer.make_can_msg_safety("LVR12", 0, values)
+
+  def test_non_scc_uses_lvr12_rx_checks(self):
+    self._rx(self._user_gas_msg(False))
+    self._rx(self._torque_driver_msg(0))
+    self._rx(self._speed_msg(0))
+    self._rx(self._user_brake_msg(False))
+    self._rx(self._button_msg(Buttons.NONE))
+    self._rx(self._pcm_status_msg(False))
+    self.assertTrue(self.safety.safety_config_valid())
+
+
 class TestHyundaiSafetyFCEV(TestHyundaiSafety):
   def setUp(self):
     self.packer = CANPackerSafety("hyundai_kia_generic")

panda/board/obj/gitversion.h

@@ -1,2 +1,2 @@
 extern const uint8_t gitversion[19];
-const uint8_t gitversion[19] = "DEV-9d2a8050-DEBUG";
+const uint8_t gitversion[19] = "DEV-031ad898-DEBUG";

panda/board/obj/version

@@ -1 +1 @@
-DEV-9d2a8050-DEBUG
+DEV-031ad898-DEBUG

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -494,7 +494,7 @@ class LongitudinalMpc:
     model_v = getattr(model_lead, "v", ())
 
     if tracking_lead and radar_lead is not None and radar_lead.status:
-      if model_prob > 0.5 and len(model_x) and len(model_v):
+      if bool(getattr(radar_lead, "radar", False)) and model_prob > 0.5 and len(model_x) and len(model_v):
         x_lead_traj = float(radar_lead.dRel) + (np.asarray(model_x, dtype=np.float64) - float(model_x[0]))
         v_lead_traj = float(radar_lead.vLead) + (np.asarray(model_v, dtype=np.float64) - float(model_v[0]))
 

selfdrive/controls/radard.py

@@ -22,6 +22,9 @@ _LEAD_ACCEL_TAU = 0.6
 # Compare model v against dRel motion over a short window to correct sustained over-prediction.
 _BIAS_FD_WINDOW_S = 2.0
 _BIAS_EMA_TAU = 0.5
+_BIAS_MAX_ABS = 8.0
+_BIAS_MIN_PROB = 0.5
+_BIAS_RESET_JUMP = 10.0
 
 # radar tracks
 SPEED, ACCEL = 0, 1     # Kalman filter states enum
@@ -241,7 +244,7 @@ class RadarD:
 
     self._bias_fd_k = int(round(_BIAS_FD_WINDOW_S / DT_MDL))
     self.lead_drel_hists = [deque(maxlen=self._bias_fd_k + 1) for _ in range(2)]
-    self.vego_hist_bias = deque(maxlen=self._bias_fd_k + 1)
+    self.vego_hists_bias = [deque(maxlen=self._bias_fd_k + 1) for _ in range(2)]
     self.bias_ema_filters = [FirstOrderFilter(0.0, _BIAS_EMA_TAU, DT_MDL) for _ in range(2)]
 
     self.v_ego = 0.0
@@ -256,6 +259,35 @@ class RadarD:
     self.starpilot_radar_state = custom.StarPilotRadarState.new_message()
     self.starpilot_toggles = get_starpilot_toggles()
 
+  def _reset_lead_bias(self, idx: int) -> None:
+    self.lead_drel_hists[idx].clear()
+    self.vego_hists_bias[idx].clear()
+    self.bias_ema_filters[idx].x = 0.0
+
+  def _update_lead_bias(self, idx: int, lead_msg: capnp._DynamicStructReader) -> float:
+    # A newly-acquired or hopped lead does not have enough history for finite-difference biasing.
+    if len(lead_msg.x) == 0 or len(lead_msg.v) == 0 or float(lead_msg.prob) <= _BIAS_MIN_PROB:
+      self._reset_lead_bias(idx)
+      return 0.0
+
+    lead_x = float(lead_msg.x[0])
+    if len(self.lead_drel_hists[idx]) and abs(lead_x - self.lead_drel_hists[idx][-1]) > _BIAS_RESET_JUMP:
+      self._reset_lead_bias(idx)
+
+    self.lead_drel_hists[idx].append(lead_x)
+    self.vego_hists_bias[idx].append(self.v_ego)
+
+    if len(self.lead_drel_hists[idx]) == self._bias_fd_k + 1 and len(self.vego_hists_bias[idx]) == self._bias_fd_k + 1:
+      dt_win = _BIAS_FD_WINDOW_S
+      v_ego_hist = self.vego_hists_bias[idx]
+      v_ego_avg = sum(v_ego_hist) / len(v_ego_hist)
+      a_ego_win = (v_ego_hist[-1] - v_ego_hist[0]) / dt_win
+      fd_vlead = v_ego_avg + (self.lead_drel_hists[idx][-1] - self.lead_drel_hists[idx][0]) / dt_win + a_ego_win * dt_win / 2
+      bias_raw = np.clip(float(lead_msg.v[0]) - fd_vlead, -_BIAS_MAX_ABS, _BIAS_MAX_ABS)
+      self.bias_ema_filters[idx].update(bias_raw)
+
+    return float(np.clip(self.bias_ema_filters[idx].x, -_BIAS_MAX_ABS, _BIAS_MAX_ABS))
+
   def update(self, sm: messaging.SubMaster, rr: car.RadarData):
     self.ready = sm.seen['modelV2']
     self.current_time = 1e-9 * max(sm.logMonoTime.values())
@@ -299,20 +331,7 @@ class RadarD:
     leads_v3 = sm['modelV2'].leadsV3
     if len(leads_v3) > 1:
       lead_probs = [self.lead_prob_filters[i].update(leads_v3[i].prob) for i in range(2)]
-
-      self.vego_hist_bias.append(self.v_ego)
-      lead_biases = [0.0, 0.0]
-      for i in range(2):
-        lead_x = float(leads_v3[i].x[0]) if len(leads_v3[i].x) else 0.0
-        self.lead_drel_hists[i].append(lead_x)
-        if len(self.lead_drel_hists[i]) == self._bias_fd_k + 1 and len(self.vego_hist_bias) == self._bias_fd_k + 1:
-          dt_win = _BIAS_FD_WINDOW_S
-          v_ego_avg = sum(self.vego_hist_bias) / len(self.vego_hist_bias)
-          a_ego_win = (self.vego_hist_bias[-1] - self.vego_hist_bias[0]) / dt_win
-          fd_vlead = v_ego_avg + (self.lead_drel_hists[i][-1] - self.lead_drel_hists[i][0]) / dt_win + a_ego_win * dt_win / 2
-          bias_raw = float(leads_v3[i].v[0]) - fd_vlead
-          self.bias_ema_filters[i].update(bias_raw)
-        lead_biases[i] = self.bias_ema_filters[i].x
+      lead_biases = [self._update_lead_bias(i, leads_v3[i]) for i in range(2)]
 
       self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, lead_probs[0],
                                           sm['modelV2'], sm['carState'].standstill, sm['starpilotPlan'],
@@ -320,6 +339,9 @@ class RadarD:
       self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, lead_probs[1],
                                           sm['modelV2'], sm['carState'].standstill, sm['starpilotPlan'],
                                           self.starpilot_toggles, lead_bias=lead_biases[1], low_speed_override=False)
+    else:
+      for i in range(2):
+        self._reset_lead_bias(i)
 
     if self.ready and (self.starpilot_toggles.adjacent_lead_tracking or self.starpilot_toggles.human_lane_changes):
       self.starpilot_radar_state.leadLeft = get_adjacent_lead(self.tracks, sm['carState'].standstill, sm['modelV2'], left=True)

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -8,21 +8,27 @@ import pytest
 from cereal import log
 from opendbc.car.honda.interface import CarInterface
 from opendbc.car.honda.values import CAR
+import openpilot.selfdrive.controls.radard as radard_mod
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
 from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_vehicle_min_accel
+from openpilot.selfdrive.controls.radard import RadarD
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
 
 
-def make_lead(*, status: bool, d_rel: float = 200.0, v_lead: float = 0.0, a_lead: float = 0.0):
+def make_lead(*, status: bool, d_rel: float = 200.0, v_lead: float = 0.0, a_lead: float = 0.0,
+              radar: bool = True, model_prob: float = 0.0, a_lead_tau: float = 0.3):
   lead = log.RadarState.LeadData.new_message()
   lead.status = status
   lead.dRel = d_rel
   lead.vLead = v_lead
   lead.vLeadK = v_lead
   lead.aLeadK = a_lead
+  lead.aLeadTau = a_lead_tau
   lead.vRel = 0.0
   lead.aRel = 0.0
-  lead.modelProb = 0.0
+  lead.modelProb = model_prob
+  lead.radar = radar
   return lead
 
 
@@ -40,6 +46,13 @@ def fill_model_lead(lead_msg, lead, prob: float):
   lead_msg.aStd = [0.5] * len(ModelConstants.LEAD_T_IDXS)
 
 
+def make_model_lead_msg(*, d_rel: float, v_lead: float, prob: float):
+  model = log.ModelDataV2.new_message()
+  model.init('leadsV3', 1)
+  fill_model_lead(model.leadsV3[0], make_lead(status=True, d_rel=d_rel, v_lead=v_lead), prob)
+  return model.leadsV3[0]
+
+
 def make_model(v_ego: float, desired_accel: float, gas_press_prob: float = 1.0, lead_one=None, lead_two=None):
   model = log.ModelDataV2.new_message()
   t_idxs = ModelConstants.T_IDXS
@@ -246,3 +259,38 @@ def test_allow_throttle_hysteresis_filters_gas_prob_chatter():
   planner.update(sm, toggles)
   assert planner.model_allow_throttle
   assert planner.allow_throttle
+
+
+def test_radard_bias_resets_across_low_prob_lead_acquisition(monkeypatch):
+  monkeypatch.setattr(radard_mod, "get_starpilot_toggles", lambda sm=None: SimpleNamespace())
+  rd = RadarD()
+  rd.v_ego = 33.0
+
+  for _ in range(rd._bias_fd_k + 5):
+    bias = rd._update_lead_bias(0, make_model_lead_msg(d_rel=120.0, v_lead=33.0, prob=0.01))
+    assert bias == 0.0
+
+  bias = rd._update_lead_bias(0, make_model_lead_msg(d_rel=56.0, v_lead=33.0, prob=0.99))
+  assert bias == 0.0
+  assert len(rd.lead_drel_hists[0]) == 1
+  assert len(rd.vego_hists_bias[0]) == 1
+
+
+def test_model_trajectory_requires_radar_anchor():
+  radar_lead = make_lead(status=True, d_rel=60.0, v_lead=20.0, radar=False, model_prob=0.99)
+  model = make_model(30.0, desired_accel=0.0)
+  fill_model_lead(model.leadsV3[0], make_lead(status=True, d_rel=60.0, v_lead=32.0), 0.99)
+  model.leadsV3[0].v = [32.0 + 2.0 * i for i in range(len(ModelConstants.LEAD_T_IDXS))]
+
+  vision_mpc = LongitudinalMpc()
+  vision_mpc.set_cur_state(30.0, 0.0)
+  vision_mpc.lead_v_filter.x = radar_lead.vLead
+  vision_traj = vision_mpc.process_lead(model.leadsV3[0], radar_lead, tracking_lead=True)
+
+  radar_mpc = LongitudinalMpc()
+  radar_mpc.set_cur_state(30.0, 0.0)
+  radar_lead.radar = True
+  radar_traj = radar_mpc.process_lead(model.leadsV3[0], radar_lead, tracking_lead=True)
+
+  assert np.allclose(vision_traj[:, 1], 20.0, atol=1e-3)
+  assert radar_traj[5, 1] > vision_traj[5, 1] + 1.0