From 45a1d3fa8e65d2b2e3cb586109358b6d9c1551a7 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Fri, 15 May 2026 18:34:50 -0500 Subject: [PATCH] volt and old ioniq and new ioniq and honda --- selfdrive/controls/lib/latcontrol_torque.py | 44 +++++++-------- .../controls/lib/longitudinal_planner.py | 15 +++++- selfdrive/controls/tests/test_latcontrol.py | 10 ++-- .../tests/test_longitudinal_planner.py | 53 +++++++++++++++++++ .../controls/tests/test_starpilot_vcruise.py | 18 +++++++ 5 files changed, 112 insertions(+), 28 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index f430fd305..86414054a 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -66,16 +66,16 @@ CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_LEFT = 0.02 CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_RIGHT = 0.00 CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_LEFT = 0.26 CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_RIGHT = 0.40 -CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_LEFT = -0.06 -CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_RIGHT = -0.02 -CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_LEFT = -0.02 -CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_RIGHT = 0.00 -CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_LEFT = 0.16 -CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_RIGHT = 0.10 -CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = -0.01 -CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.00 -CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.14 -CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.08 +CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_LEFT = -0.10 +CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_RIGHT = 0.02 +CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_LEFT = -0.05 +CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_RIGHT = 0.02 +CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_LEFT = 0.24 +CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_RIGHT = 0.06 +CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = -0.02 +CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.01 +CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.22 +CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.05 CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_MAX = 0.12 CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT = 0.24 CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT_WIDTH = 0.05 @@ -308,23 +308,23 @@ IONIQ_5_TURN_IN_FRICTION_BOOST_RIGHT = 0.00 IONIQ_5_UNWIND_FRICTION_REDUCTION_LEFT = 0.15 IONIQ_5_UNWIND_FRICTION_REDUCTION_RIGHT = 0.26 -IONIQ_EV_OLD_BASE_LAT_ACCEL_FACTOR_MULT = 1.08 -IONIQ_EV_OLD_FF_REDUCTION_LEFT = 0.08 -IONIQ_EV_OLD_FF_REDUCTION_RIGHT = 0.16 +IONIQ_EV_OLD_BASE_LAT_ACCEL_FACTOR_MULT = 1.12 +IONIQ_EV_OLD_FF_REDUCTION_LEFT = 0.12 +IONIQ_EV_OLD_FF_REDUCTION_RIGHT = 0.24 IONIQ_EV_OLD_FF_ONSET = 0.14 IONIQ_EV_OLD_FF_ONSET_WIDTH = 0.05 IONIQ_EV_OLD_FF_CUTOFF = 1.10 IONIQ_EV_OLD_FF_CUTOFF_WIDTH = 0.30 IONIQ_EV_OLD_TRANSITION_SPEED = 10.0 IONIQ_EV_OLD_PHASE_SCALE = 0.10 -IONIQ_EV_OLD_TURN_IN_BOOST_LEFT = 0.06 +IONIQ_EV_OLD_TURN_IN_BOOST_LEFT = 0.03 IONIQ_EV_OLD_TURN_IN_BOOST_RIGHT = 0.00 -IONIQ_EV_OLD_UNWIND_TAPER_LEFT = 0.18 -IONIQ_EV_OLD_UNWIND_TAPER_RIGHT = 0.08 -IONIQ_EV_OLD_CENTER_TAPER_MAX = 0.10 -IONIQ_EV_OLD_CENTER_TAPER_LAT = 0.14 +IONIQ_EV_OLD_UNWIND_TAPER_LEFT = 0.26 +IONIQ_EV_OLD_UNWIND_TAPER_RIGHT = 0.06 +IONIQ_EV_OLD_CENTER_TAPER_MAX = 0.14 +IONIQ_EV_OLD_CENTER_TAPER_LAT = 0.12 IONIQ_EV_OLD_CENTER_TAPER_LAT_WIDTH = 0.03 -IONIQ_EV_OLD_CENTER_TAPER_SPEED = 24.0 +IONIQ_EV_OLD_CENTER_TAPER_SPEED = 22.0 IONIQ_EV_OLD_CENTER_TAPER_SPEED_WIDTH = 2.2 IONIQ_6_FF_GAIN_LEFT = 0.045 @@ -357,10 +357,10 @@ IONIQ_6_CENTER_TAPER_LAT = 0.24 IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.025 IONIQ_6_CENTER_TAPER_SPEED = 18.0 IONIQ_6_CENTER_TAPER_SPEED_WIDTH = 2.5 -IONIQ_6_HIGHWAY_CENTER_TAPER_MAX = 0.030 -IONIQ_6_HIGHWAY_CENTER_TAPER_LAT = 0.10 +IONIQ_6_HIGHWAY_CENTER_TAPER_MAX = 0.034 +IONIQ_6_HIGHWAY_CENTER_TAPER_LAT = 0.09 IONIQ_6_HIGHWAY_CENTER_TAPER_LAT_WIDTH = 0.03 -IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED = 26.0 +IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED = 24.5 IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED_WIDTH = 1.8 IONIQ_6_LOW_MID_CENTER_TAPER_MAX = 0.088 IONIQ_6_LOW_MID_CENTER_TAPER_LAT = 0.28 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 881000470..3617d8013 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -139,6 +139,7 @@ VISION_CLOSE_STOP_HOLD_MAX_BRAKE = 0.28 MANUAL_STOP_RESUME_OVERRIDE_TIME = 3.0 MANUAL_STOP_RESUME_OVERRIDE_MAX_SPEED = 2.0 MANUAL_STOP_RESUME_OVERRIDE_MIN_ACCEL = 0.2 +FORCE_STOP_HANDOFF_MAX_VCRUISE = 0.5 LEAD_CATCHUP_ACCEL_MIN_EGO = 8.0 LEAD_CATCHUP_ACCEL_MIN_LEAD_DELTA = -0.5 LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_MIN = 4.0 @@ -1687,6 +1688,18 @@ class LongitudinalPlanner: output_accel_max = no_throttle_output_max if not self.allow_throttle else accel_limits_turns[1] output_a_target = float(np.clip(output_a_target, output_accel_min, output_accel_max)) + force_stop_handoff = bool( + getattr(sm['starpilotPlan'], 'forcingStop', False) and + not lead_control_active and + ( + float(getattr(sm['starpilotPlan'], 'forcingStopLength', float('inf'))) < 1.0 or + float(getattr(sm['starpilotPlan'], 'vCruise', float('inf'))) <= FORCE_STOP_HANDOFF_MAX_VCRUISE + ) + ) + + if force_stop_handoff: + output_should_stop = True + manual_stop_resume_override = self._update_manual_stop_resume_override(sm) if manual_stop_resume_override: output_a_target = max(output_a_target, MANUAL_STOP_RESUME_OVERRIDE_MIN_ACCEL) @@ -1717,7 +1730,7 @@ class LongitudinalPlanner: force_stop_handoff = bool( sm['starpilotPlan'].forcingStop and ( sm['starpilotPlan'].forcingStopLength < 1.0 or - sm['starpilotPlan'].vCruise <= 0.0 + sm['starpilotPlan'].vCruise <= FORCE_STOP_HANDOFF_MAX_VCRUISE ) ) longitudinalPlan.shouldStop = bool(self.output_should_stop) or force_stop_handoff diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 59410c735..ed06cb969 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -604,14 +604,14 @@ class TestLatControl: a_variant_unwind_right_friction = get_civic_bosch_modified_b_friction_scale(12.0, -0.5, 0.8) assert a_variant_steady_left < base_steady_left - assert a_variant_steady_right < base_steady_right + assert a_variant_steady_right > base_steady_right assert a_variant_turn_in_left < base_turn_in_left - assert a_variant_turn_in_right < base_turn_in_right + assert a_variant_turn_in_right > base_turn_in_right assert a_variant_unwind_left < base_unwind_left - assert a_variant_unwind_right < base_unwind_right - assert a_variant_turn_in_right_friction <= base_turn_in_right_friction + assert a_variant_unwind_right > (base_unwind_right * 0.95) + assert a_variant_turn_in_right_friction > base_turn_in_right_friction assert a_variant_unwind_left_friction < base_unwind_left_friction - assert a_variant_unwind_right_friction < base_unwind_right_friction + assert a_variant_unwind_right_friction >= 0.82 def test_modified_civic_a_variant_center_taper_curve(self): assert get_civic_bosch_modified_a_center_taper_scale(0.0, 25.0) < get_civic_bosch_modified_a_center_taper_scale(0.0, 10.0) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 3b3cb7c62..8cadadc57 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -1207,6 +1207,59 @@ def test_publish_force_stop_handoff_sets_should_stop_when_vcruise_zero(): assert pm.sent["longitudinalPlan"].longitudinalPlan.shouldStop +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) +def test_force_stop_handoff_sets_output_should_stop_before_zero_vcruise(model_version): + v_ego = 1.25 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + sm = make_sm(v_ego, desired_accel=-0.35, min_accel=-1.0, experimental_mode=False) + sm["starpilotPlan"].forcingStop = True + sm["starpilotPlan"].forcingStopLength = 6.5 + sm["starpilotPlan"].vCruise = 0.4 + sm["modelV2"].action.shouldStop = False + + planner.update(sm, make_toggles(model_version)) + + assert planner.output_should_stop + + +def test_publish_force_stop_handoff_sets_should_stop_when_vcruise_low(): + class FakePM: + def __init__(self): + self.sent = {} + + def send(self, name, msg): + self.sent[name] = msg + + class FakeSM(dict): + def all_checks(self, service_list=None): + return True + + logMonoTime = {"modelV2": int(1e9)} + + v_ego = 1.25 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + planner.output_a_target = -0.35 + planner.output_should_stop = False + planner.v_desired_trajectory = np.zeros(CONTROL_N) + planner.a_desired_trajectory = np.zeros(CONTROL_N) + planner.j_desired_trajectory = np.zeros(CONTROL_N) + planner.fcw = False + planner.mpc.source = "cruise" + planner.mpc.solve_time = 0.0 + pm = FakePM() + + sm = FakeSM(make_sm(v_ego, desired_accel=0.0, min_accel=-1.0, experimental_mode=False)) + sm["starpilotPlan"].forcingStop = True + sm["starpilotPlan"].forcingStopLength = 6.5 + sm["starpilotPlan"].vCruise = 0.4 + + planner.publish(sm, pm) + + assert pm.sent["longitudinalPlan"].longitudinalPlan.shouldStop + + def test_allow_throttle_hysteresis_filters_gas_prob_chatter(): v_ego = 10.0 diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index 14338960b..e7f613f2f 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -115,3 +115,21 @@ def test_force_stop_stays_committed_while_model_still_sees_stop(): assert result == pytest.approx(0.0) assert vcruise.force_stop_timer >= 0.5 assert vcruise.forcing_stop + + +def test_force_stop_stays_committed_while_moving_even_if_scene_opens(): + planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True) + + result = vcruise.update( + controls_enabled=True, + now=0.0, + time_validated=True, + v_cruise=20.0, + v_ego=1.5, + sm=make_sm(standstill=False), + starpilot_toggles=make_toggles(), + ) + + assert result == pytest.approx(0.0) + assert vcruise.force_stop_timer >= 0.5 + assert vcruise.forcing_stop