diff --git a/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h b/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h index 416e46bba..bdeeffae8 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h @@ -26,6 +26,7 @@ #define HYUNDAI_CANFD_MRR35_RADAR_TRACK_START 0x3A5 #define HYUNDAI_CANFD_MRR35_RADAR_TRACK_END 0x3C4 +#define HYUNDAI_CANFD_INACTIVE_ACCEL_TX_THRESHOLD 10U #define HYUNDAI_CANFD_BLINDSPOT_DASH_TX_MSGS(e_can) \ {0x1BA, e_can, 24, .check_relay = false}, /* BLINDSPOTS_REAR_CORNERS */ \ @@ -61,6 +62,7 @@ static bool hyundai_canfd_lka_steering_alt = false; static bool hyundai_canfd_angle_steering = false; static bool hyundai_ccnc = false; static bool hyundai_canfd_lka_alt_drive_gear = false; +static uint8_t hyundai_canfd_inactive_accel_tx_count = 0U; static unsigned int hyundai_canfd_get_lka_addr(void) { return hyundai_canfd_lka_steering_alt ? 0x110U : 0x50U; @@ -130,6 +132,7 @@ static void hyundai_canfd_rx_hook(const CANPacket_t *msg) { // cruise buttons const unsigned int button_addr = hyundai_canfd_alt_buttons ? 0x1aaU : 0x1cfU; if (msg->addr == button_addr) { + const bool controls_allowed_prev = controls_allowed; bool main_button = false; int cruise_button = 0; if (msg->addr == 0x1cfU) { @@ -144,6 +147,9 @@ static void hyundai_canfd_rx_hook(const CANPacket_t *msg) { hyundai_lkas_button_check(GET_BIT(msg, 39U)); } hyundai_common_cruise_buttons_check(cruise_button, main_button); + if (!controls_allowed_prev && controls_allowed) { + hyundai_canfd_inactive_accel_tx_count = 0U; + } } // gas press, different for EV, hybrid, and ICE models @@ -303,6 +309,18 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *msg) { bool violation = false; if (hyundai_longitudinal) { + const int acc_mode = (msg->data[8] >> 4) & 0x7U; + const bool inactive_accel = (acc_mode == 0) && (desired_accel_raw == 0) && (desired_accel_val == 0); + if (inactive_accel) { + hyundai_canfd_inactive_accel_tx_count = SAFETY_MIN(hyundai_canfd_inactive_accel_tx_count + 1U, + HYUNDAI_CANFD_INACTIVE_ACCEL_TX_THRESHOLD); + if (hyundai_canfd_inactive_accel_tx_count >= HYUNDAI_CANFD_INACTIVE_ACCEL_TX_THRESHOLD) { + controls_allowed = false; + } + } else { + hyundai_canfd_inactive_accel_tx_count = 0U; + } + violation |= longitudinal_accel_checks(desired_accel_raw, HYUNDAI_LONG_LIMITS); violation |= longitudinal_accel_checks(desired_accel_val, HYUNDAI_LONG_LIMITS); } else { @@ -422,6 +440,7 @@ static safety_config hyundai_canfd_init(uint16_t param) { hyundai_canfd_angle_steering = GET_FLAG(param, HYUNDAI_PARAM_CANFD_ANGLE_STEERING); hyundai_ccnc = GET_FLAG(param, HYUNDAI_PARAM_CCNC); hyundai_canfd_lka_alt_drive_gear = false; + hyundai_canfd_inactive_accel_tx_count = 0U; safety_config ret; if (hyundai_longitudinal) { diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py index 80728e003..aa8f09bb9 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py @@ -610,6 +610,25 @@ class TestHyundaiCanfdLKASteeringLongEV(HyundaiLongitudinalBase, TestHyundaiCanf values = {"MainMode_ACC": int(main_on), "ACCMode": 0} return self.packer.make_can_msg_safety("SCC_CONTROL", 1, values) + def test_inactive_accel_resets_controls_before_reengagement(self): + self.safety.set_controls_allowed(True) + + for _ in range(9): + self.assertTrue(self._tx(self._accel_msg(0))) + self.assertTrue(self.safety.get_controls_allowed()) + + self.assertTrue(self._tx(self._accel_msg(0))) + self.assertFalse(self.safety.get_controls_allowed()) + + self._rx(self._button_msg(Buttons.RESUME)) + self._rx(self._button_msg(Buttons.NONE)) + self.assertTrue(self.safety.get_controls_allowed()) + + # One inactive frame can race the state transition after button release. + self.assertTrue(self._tx(self._accel_msg(0))) + self.assertTrue(self.safety.get_controls_allowed()) + self.assertTrue(self._tx(self._accel_msg(-0.1))) + class TestHyundaiCanfdLKASteeringAltAngleLongEV(HyundaiLongitudinalBase, TestHyundaiCanfdAngleSteering): diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index e7aff6005..e01c9268b 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -359,6 +359,10 @@ MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP = 0.10 LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED = 10.0 LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_SPEED = MATCHED_FOLLOW_TRANSITION_MIN_SPEED LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN = 0.45 +LOW_SPEED_MATCHED_FOLLOW_TRANSITION_OPENING_RADAR_MIN_HEADWAY_MARGIN = 0.20 +LOW_SPEED_MATCHED_FOLLOW_TRANSITION_OPENING_RADAR_MIN_LEAD_DELTA = 0.25 +MATCHED_FOLLOW_TRANSITION_STABLE_RADAR_MAX_CLOSING_SPEED = 0.25 +MATCHED_FOLLOW_TRANSITION_STABLE_RADAR_MAX_STEP = 0.05 LOW_SPEED_MATCHED_FOLLOW_TRANSITION_FULL_HEADWAY_MARGIN = 1.00 LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_MODEL_PROB = 0.98 LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE = 0.08 @@ -2020,18 +2024,27 @@ class LongitudinalPlanner: if float(v_ego) < MATCHED_FOLLOW_TRANSITION_MIN_SPEED and not low_speed_extension_active: return None + lead_radar = bool(getattr(lead, "radar", False)) lead_prob = float(getattr(lead, "modelProb", 0.0)) min_model_prob = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_MODEL_PROB if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MIN_MODEL_PROB if lead_prob < min_model_prob: return None - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) + lead_accel = float(getattr(lead, "aLeadK", 0.0)) + lead_brake = max(0.0, -lead_accel) max_lead_brake = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE if lead_brake > max_lead_brake: return None relative_speed = float(v_ego) - float(lead.vLead) - if not (STEADY_FOLLOW_BRAKE_CAP_MIN_REL_SPEED <= relative_speed <= STEADY_FOLLOW_SMOOTHING_MAX_CLOSING_SPEED): + opening_radar_transition = bool( + lead_radar and + -relative_speed >= LOW_SPEED_MATCHED_FOLLOW_TRANSITION_OPENING_RADAR_MIN_LEAD_DELTA and + -relative_speed <= CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_PULLAWAY_SPEED and + lead_accel >= 0.0 + ) + relative_speed_in_range = STEADY_FOLLOW_BRAKE_CAP_MIN_REL_SPEED <= relative_speed <= STEADY_FOLLOW_SMOOTHING_MAX_CLOSING_SPEED + if not relative_speed_in_range and not opening_radar_transition: return None closing_speed = max(0.0, relative_speed) @@ -2047,6 +2060,13 @@ class LongitudinalPlanner: actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) headway_margin = actual_headway - float(base_t_follow) min_headway_margin = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN + opening_radar_lead = bool( + low_speed_extension_active and + lead_radar and + opening_radar_transition + ) + if opening_radar_lead: + min_headway_margin = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_OPENING_RADAR_MIN_HEADWAY_MARGIN full_headway_margin = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_FULL_HEADWAY_MARGIN if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_FULL_HEADWAY_MARGIN if headway_margin < min_headway_margin: return None @@ -2090,6 +2110,16 @@ class LongitudinalPlanner: positive_step = float(np.interp(headway_factor, [0.0, 1.0], [positive_step, min_positive_step])) negative_step = float(np.interp(headway_factor, [0.0, 1.0], [negative_step, min_negative_step])) + stable_radar_catchup = bool( + lead_radar and + relative_speed <= MATCHED_FOLLOW_TRANSITION_STABLE_RADAR_MAX_CLOSING_SPEED and + lead_accel >= 0.0 and + headway_margin >= LOW_SPEED_MATCHED_FOLLOW_TRANSITION_OPENING_RADAR_MIN_HEADWAY_MARGIN + ) + if stable_radar_catchup: + positive_step = min(positive_step, MATCHED_FOLLOW_TRANSITION_STABLE_RADAR_MAX_STEP) + negative_step = min(negative_step, MATCHED_FOLLOW_TRANSITION_STABLE_RADAR_MAX_STEP) + if float(prev_output_a_target) * float(output_a_target) < 0.0: sign_cross_step = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP positive_step = min(positive_step, sign_cross_step) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 82ab76ad5..cdc333414 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -2737,6 +2737,102 @@ def test_matched_follow_transition_target_damps_low_speed_tracking_cruise_thrott assert smoothed == pytest.approx(0.14, abs=1e-6) +def test_route_8bc6_opening_radar_catchup_cap_does_not_stab_out_throttle(): + v_ego = 15.60 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=24.3, v_lead=16.61, a_lead=0.82, radar=True, model_prob=1.0) + + smoothed = planner.get_matched_follow_transition_target( + lead, + v_ego, + 1.25, + prev_output_a_target=1.31, + output_a_target=0.32, + current_source="cruise", + tracking_lead_active=True, + ) + + assert smoothed is not None + assert 1.20 < smoothed < 1.31 + + +def test_opening_radar_catchup_smoothing_yields_immediately_if_lead_brakes(): + v_ego = 15.60 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=24.3, v_lead=16.61, a_lead=-0.50, radar=True, model_prob=1.0) + + smoothed = planner.get_matched_follow_transition_target( + lead, + v_ego, + 1.25, + prev_output_a_target=1.31, + output_a_target=0.32, + current_source="cruise", + tracking_lead_active=True, + ) + + assert smoothed is None + + +def test_opening_radar_catchup_smoothing_has_no_pullaway_boundary_jump(): + v_ego = 16.30 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=25.5, v_lead=17.54, a_lead=0.68, radar=True, model_prob=1.0) + + smoothed = planner.get_matched_follow_transition_target( + lead, + v_ego, + 1.25, + prev_output_a_target=0.51, + output_a_target=0.87, + current_source="cruise", + tracking_lead_active=True, + ) + + assert smoothed == pytest.approx(0.56, abs=1e-6) + + +def test_route_8bc6_radar_lead_source_handoff_keeps_catchup_transition_smooth(): + v_ego = 24.44 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=37.1, v_lead=24.46, a_lead=0.33, radar=True, model_prob=1.0) + + smoothed = planner.get_matched_follow_transition_target( + lead, + v_ego, + 1.15, + prev_output_a_target=0.42, + output_a_target=0.26, + current_source="lead0", + tracking_lead_active=True, + ) + + assert smoothed == pytest.approx(0.37, abs=1e-6) + + +def test_opening_radar_catchup_smoothing_stays_off_inside_requested_headway(): + v_ego = 15.60 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=21.0, v_lead=16.61, a_lead=0.82, radar=True, model_prob=1.0) + + smoothed = planner.get_matched_follow_transition_target( + lead, + v_ego, + 1.25, + prev_output_a_target=1.31, + output_a_target=0.32, + current_source="cruise", + tracking_lead_active=True, + ) + + assert smoothed is None + + def test_matched_follow_transition_target_skips_low_speed_without_tracking(): v_ego = 14.5 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)