diff --git a/opendbc_repo/opendbc/car/hyundai/interface.py b/opendbc_repo/opendbc/car/hyundai/interface.py index 6ab72ebb5..65c5b72a1 100644 --- a/opendbc_repo/opendbc/car/hyundai/interface.py +++ b/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] diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index 4e7986882..e9d507e99 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/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): diff --git a/opendbc_repo/opendbc/safety/modes/hyundai.h b/opendbc_repo/opendbc/safety/modes/hyundai.h index dc8c95b22..cf304924c 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai.h +++ b/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); + } } } } diff --git a/opendbc_repo/opendbc/safety/modes/hyundai_common.h b/opendbc_repo/opendbc/safety/modes/hyundai_common.h index 9886ecde6..10678df85 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai_common.h +++ b/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; diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai.py b/opendbc_repo/opendbc/safety/tests/test_hyundai.py index 957d4e921..80d7d30ae 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai.py +++ b/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") diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 69870016c..ff84d39eb 100644 Binary files a/panda/board/obj/body_h7.bin.signed and b/panda/board/obj/body_h7.bin.signed differ diff --git a/panda/board/obj/body_h7/bootstub.elf b/panda/board/obj/body_h7/bootstub.elf index c9e5d6f56..d6cc8ee2b 100755 Binary files a/panda/board/obj/body_h7/bootstub.elf and b/panda/board/obj/body_h7/bootstub.elf differ diff --git a/panda/board/obj/body_h7/main.bin b/panda/board/obj/body_h7/main.bin index c1f7a27b9..bb433822c 100755 Binary files a/panda/board/obj/body_h7/main.bin and b/panda/board/obj/body_h7/main.bin differ diff --git a/panda/board/obj/body_h7/main.elf b/panda/board/obj/body_h7/main.elf index c476af913..816eb624f 100755 Binary files a/panda/board/obj/body_h7/main.elf and b/panda/board/obj/body_h7/main.elf differ diff --git a/panda/board/obj/bootstub.body_h7.bin b/panda/board/obj/bootstub.body_h7.bin index 13a5d6baf..1154c010f 100755 Binary files a/panda/board/obj/bootstub.body_h7.bin and b/panda/board/obj/bootstub.body_h7.bin differ diff --git a/panda/board/obj/bootstub.panda.bin b/panda/board/obj/bootstub.panda.bin index 383083532..29a544740 100755 Binary files a/panda/board/obj/bootstub.panda.bin and b/panda/board/obj/bootstub.panda.bin differ diff --git a/panda/board/obj/bootstub.panda_h7.bin b/panda/board/obj/bootstub.panda_h7.bin index 764e7708f..9888e351c 100755 Binary files a/panda/board/obj/bootstub.panda_h7.bin and b/panda/board/obj/bootstub.panda_h7.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_remote.bin b/panda/board/obj/bootstub.panda_h7_remote.bin index 764e7708f..9888e351c 100755 Binary files a/panda/board/obj/bootstub.panda_h7_remote.bin and b/panda/board/obj/bootstub.panda_h7_remote.bin differ diff --git a/panda/board/obj/bootstub.panda_jungle_h7.bin b/panda/board/obj/bootstub.panda_jungle_h7.bin index 0743b3f1b..3bbfec881 100755 Binary files a/panda/board/obj/bootstub.panda_jungle_h7.bin and b/panda/board/obj/bootstub.panda_jungle_h7.bin differ diff --git a/panda/board/obj/bootstub.panda_remote.bin b/panda/board/obj/bootstub.panda_remote.bin index 383083532..29a544740 100755 Binary files a/panda/board/obj/bootstub.panda_remote.bin and b/panda/board/obj/bootstub.panda_remote.bin differ diff --git a/panda/board/obj/gitversion.h b/panda/board/obj/gitversion.h index e0fda4924..0ea3b6f47 100644 --- a/panda/board/obj/gitversion.h +++ b/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"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 35b4e6039..570ed179b 100644 Binary files a/panda/board/obj/panda.bin.signed and b/panda/board/obj/panda.bin.signed differ diff --git a/panda/board/obj/panda/bootstub.elf b/panda/board/obj/panda/bootstub.elf index feab3ae44..e13775a5a 100755 Binary files a/panda/board/obj/panda/bootstub.elf and b/panda/board/obj/panda/bootstub.elf differ diff --git a/panda/board/obj/panda/main.bin b/panda/board/obj/panda/main.bin index d3ce83434..bd7813246 100755 Binary files a/panda/board/obj/panda/main.bin and b/panda/board/obj/panda/main.bin differ diff --git a/panda/board/obj/panda/main.elf b/panda/board/obj/panda/main.elf index 0df197695..374dc4025 100755 Binary files a/panda/board/obj/panda/main.elf and b/panda/board/obj/panda/main.elf differ diff --git a/panda/board/obj/panda_h7.bin.signed b/panda/board/obj/panda_h7.bin.signed index ed7282e38..aa7574d8b 100644 Binary files a/panda/board/obj/panda_h7.bin.signed and b/panda/board/obj/panda_h7.bin.signed differ diff --git a/panda/board/obj/panda_h7/bootstub.elf b/panda/board/obj/panda_h7/bootstub.elf index e10ec3e93..ae9287106 100755 Binary files a/panda/board/obj/panda_h7/bootstub.elf and b/panda/board/obj/panda_h7/bootstub.elf differ diff --git a/panda/board/obj/panda_h7/main.bin b/panda/board/obj/panda_h7/main.bin index b8cb02424..7c4b56df8 100755 Binary files a/panda/board/obj/panda_h7/main.bin and b/panda/board/obj/panda_h7/main.bin differ diff --git a/panda/board/obj/panda_h7/main.elf b/panda/board/obj/panda_h7/main.elf index c37a3654b..91c602c54 100755 Binary files a/panda/board/obj/panda_h7/main.elf and b/panda/board/obj/panda_h7/main.elf differ diff --git a/panda/board/obj/panda_h7_remote.bin.signed b/panda/board/obj/panda_h7_remote.bin.signed index 130ff2b08..5ecd78842 100644 Binary files a/panda/board/obj/panda_h7_remote.bin.signed and b/panda/board/obj/panda_h7_remote.bin.signed differ diff --git a/panda/board/obj/panda_h7_remote/bootstub.elf b/panda/board/obj/panda_h7_remote/bootstub.elf index fe7d090f1..e24a0962a 100755 Binary files a/panda/board/obj/panda_h7_remote/bootstub.elf and b/panda/board/obj/panda_h7_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_remote/main.bin b/panda/board/obj/panda_h7_remote/main.bin index e94d1fb82..b2e4ee4a7 100755 Binary files a/panda/board/obj/panda_h7_remote/main.bin and b/panda/board/obj/panda_h7_remote/main.bin differ diff --git a/panda/board/obj/panda_h7_remote/main.elf b/panda/board/obj/panda_h7_remote/main.elf index 306ca3503..5fdf57206 100755 Binary files a/panda/board/obj/panda_h7_remote/main.elf and b/panda/board/obj/panda_h7_remote/main.elf differ diff --git a/panda/board/obj/panda_jungle_h7.bin.signed b/panda/board/obj/panda_jungle_h7.bin.signed index 29e328054..07098d24a 100644 Binary files a/panda/board/obj/panda_jungle_h7.bin.signed and b/panda/board/obj/panda_jungle_h7.bin.signed differ diff --git a/panda/board/obj/panda_jungle_h7/bootstub.elf b/panda/board/obj/panda_jungle_h7/bootstub.elf index 0efe21ae4..dff0c15c2 100755 Binary files a/panda/board/obj/panda_jungle_h7/bootstub.elf and b/panda/board/obj/panda_jungle_h7/bootstub.elf differ diff --git a/panda/board/obj/panda_jungle_h7/main.bin b/panda/board/obj/panda_jungle_h7/main.bin index 11672f77e..f7f90e62b 100755 Binary files a/panda/board/obj/panda_jungle_h7/main.bin and b/panda/board/obj/panda_jungle_h7/main.bin differ diff --git a/panda/board/obj/panda_jungle_h7/main.elf b/panda/board/obj/panda_jungle_h7/main.elf index d4d9ae80b..72cf9746b 100755 Binary files a/panda/board/obj/panda_jungle_h7/main.elf and b/panda/board/obj/panda_jungle_h7/main.elf differ diff --git a/panda/board/obj/panda_remote.bin.signed b/panda/board/obj/panda_remote.bin.signed index fc321ab51..18d7828d7 100644 Binary files a/panda/board/obj/panda_remote.bin.signed and b/panda/board/obj/panda_remote.bin.signed differ diff --git a/panda/board/obj/panda_remote/bootstub.elf b/panda/board/obj/panda_remote/bootstub.elf index e7367dd7b..93f27747d 100755 Binary files a/panda/board/obj/panda_remote/bootstub.elf and b/panda/board/obj/panda_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_remote/main.bin b/panda/board/obj/panda_remote/main.bin index aa77976d2..66ab94a29 100755 Binary files a/panda/board/obj/panda_remote/main.bin and b/panda/board/obj/panda_remote/main.bin differ diff --git a/panda/board/obj/panda_remote/main.elf b/panda/board/obj/panda_remote/main.elf index 1290d9efc..ddfe24445 100755 Binary files a/panda/board/obj/panda_remote/main.elf and b/panda/board/obj/panda_remote/main.elf differ diff --git a/panda/board/obj/version b/panda/board/obj/version index f13ac0b86..8225be37a 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-9d2a8050-DEBUG \ No newline at end of file +DEV-031ad898-DEBUG \ No newline at end of file diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index e5e3dab56..539431832 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/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])) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 6151e92e3..5cbcd2dc4 100644 --- a/selfdrive/controls/radard.py +++ b/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) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index c8c141ea1..e945c3ed8 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/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