mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-19 00:02:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

fixes

Browse Source
This commit is contained in:
firestar5683
2026-07-17 22:49:09 -05:00
parent fcf1bf2f3c
commit 4c07ea96e7
4 changed files with 166 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/safety/modes/hyundai_canfd.h
+19
View File
@@ -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) {
opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py
+19
View File
@@ -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):
selfdrive/controls/lib/longitudinal_planner.py
+32 -2
View File
@@ -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)
selfdrive/controls/tests/test_longitudinal_planner.py
+96
View File
@@ -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)