diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 699b6524a9..b079f5b307 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -536,13 +536,13 @@ IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_WIDTH = 1.5 IONIQ_6_DIRECTIONAL_TAPER_LAT_START = 0.19 IONIQ_6_DIRECTIONAL_TAPER_LAT_END = 0.90 IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH = 0.06 -IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT = 0.13 +IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT = 0.11 IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT = 0.45 -IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 1.82 +IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 2.15 IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 4.25 IONIQ_6_DIRECTIONAL_TAPER_FLOOR_LEFT = 0.48 IONIQ_6_DIRECTIONAL_TAPER_FLOOR_RIGHT = 0.52 -IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT = 0.10 +IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT = 0.16 IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT = 0.04 IONIQ_6_DIRECTIONAL_TAPER_JERK_ONSET = 0.60 IONIQ_6_DIRECTIONAL_TAPER_JERK_WIDTH = 0.14 @@ -551,21 +551,21 @@ IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED = 11.2 IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED_WIDTH = 1.5 IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT = 0.10 IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT_WIDTH = 0.06 -IONIQ_6_CRAWL_TURN_IN_FF_BOOST_LEFT = 0.12 -IONIQ_6_CRAWL_TURN_IN_FF_BOOST_RIGHT = 0.16 -IONIQ_6_CRAWL_TURN_IN_FF_SPEED = 4.5 -IONIQ_6_CRAWL_TURN_IN_FF_SPEED_WIDTH = 0.8 -IONIQ_6_CRAWL_TURN_IN_FF_LAT = 0.10 -IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH = 0.05 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_MAX_TORQUE = 0.38 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED = 4.0 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED_WIDTH = 0.55 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR = 3.0 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR_WIDTH = 1.5 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE = 8.0 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE_WIDTH = 3.0 -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_BP = [0.0, 0.4, 0.7, 1.0] -IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_V = [1.0, 1.0, 0.75, 0.0] +IONIQ_6_CRAWL_TURN_IN_FF_BOOST_LEFT = 0.14 +IONIQ_6_CRAWL_TURN_IN_FF_BOOST_RIGHT = 0.19 +IONIQ_6_CRAWL_TURN_IN_FF_SPEED = 5.0 +IONIQ_6_CRAWL_TURN_IN_FF_SPEED_WIDTH = 0.9 +IONIQ_6_CRAWL_TURN_IN_FF_LAT = 0.08 +IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH = 0.045 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_MAX_TORQUE = 0.42 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED = 4.5 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_SPEED_WIDTH = 0.65 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR = 2.4 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR_WIDTH = 1.35 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE = 6.5 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_DESIRED_ANGLE_WIDTH = 2.6 +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_BP = [0.0, 0.35, 0.65, 1.0] +IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ADD_V = [1.0, 1.0, 0.82, 0.0] IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_BOOST = 0.10 IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED = 18.0 IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED_WIDTH = 2.5 @@ -575,9 +575,9 @@ IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH = 0.035 IONIQ_6_LOW_SPEED_PID_RESET_SPEED = 0.1 * CV.MPH_TO_MS IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START = 0.82 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH = 0.12 -IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT = 0.10 +IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT = 0.06 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_RIGHT = 0.17 -IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.62 +IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.84 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT = 1.18 IONIQ_6_OUTPUT_TAPER_SPEED = 8.5 IONIQ_6_OUTPUT_TAPER_SPEED_WIDTH = 2.5 @@ -614,6 +614,11 @@ KIA_EV6_CENTER_TAPER_LAT = 0.16 KIA_EV6_CENTER_TAPER_LAT_WIDTH = 0.04 KIA_EV6_CENTER_TAPER_SPEED = 17.0 KIA_EV6_CENTER_TAPER_SPEED_WIDTH = 2.8 +KIA_EV6_LOW_SPEED_CENTER_TAPER_MAX = 0.24 +KIA_EV6_LOW_SPEED_CENTER_TAPER_LAT = 0.08 +KIA_EV6_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.02 +KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 8.5 +KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.4 VOLT_PLEXY_LATERAL_TESTING_GROUND_ID = testing_ground.id_7 VOLT_PLEXY_FF_GAIN_LEFT = 0.12 @@ -1979,6 +1984,13 @@ def get_kia_ev6_center_taper_scale(desired_lateral_accel: float, v_ego: float) - return 1.0 - reduction +def get_kia_ev6_low_speed_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _kia_ev6_sigmoid((KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_MAX - v_ego) / KIA_EV6_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH) + center_weight = _kia_ev6_sigmoid((KIA_EV6_LOW_SPEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / KIA_EV6_LOW_SPEED_CENTER_TAPER_LAT_WIDTH) + reduction = KIA_EV6_LOW_SPEED_CENTER_TAPER_MAX * speed_weight * center_weight + return 1.0 - reduction + + def volt_plexy_lateral_testing_ground_active() -> bool: return testing_ground.use(VOLT_PLEXY_LATERAL_TESTING_GROUND_ID) @@ -2239,6 +2251,7 @@ class LatControlTorque(LatControl): kia_niro_phev_2022_center_taper = get_kia_niro_phev_2022_center_taper_scale(setpoint, CS.vEgo) if kia_niro_phev_2022_active else 1.0 kia_forte_center_taper = get_kia_forte_center_taper_scale(setpoint, CS.vEgo) if kia_forte_active else 1.0 kia_ev6_center_taper = get_kia_ev6_center_taper_scale(setpoint, CS.vEgo) if kia_ev6_test_active else 1.0 + kia_ev6_low_speed_center_taper = get_kia_ev6_low_speed_center_taper_scale(setpoint, CS.vEgo) if kia_ev6_test_active else 1.0 silverado_center_taper = get_silverado_center_taper_scale(setpoint, CS.vEgo) if self.is_silverado else 1.0 civic_bosch_modified_a_center_taper = get_civic_bosch_modified_a_center_taper_scale(setpoint, CS.vEgo) if ( self.is_civic_bosch_modified and civic_bosch_modified_a_lateral_testing_ground_active() @@ -2338,6 +2351,8 @@ class LatControlTorque(LatControl): output_torque, CS.vEgo) elif volt_standard_test_active: output_torque *= volt_standard_center_taper + elif kia_ev6_test_active: + output_torque *= kia_ev6_low_speed_center_taper elif self.is_silverado: output_torque *= silverado_center_taper elif kia_niro_phev_2022_active: diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index b8d6ef488a..72fd6abcee 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -1446,7 +1446,7 @@ class LongitudinalPlanner: if gap_error > gap_buffer: return None - if current_source == "cruise" and tracking_lead_active and self.is_comfortable_accelerating_away_follow(lead, v_ego, t_follow): + if tracking_lead_active and self.is_comfortable_accelerating_away_follow(lead, v_ego, t_follow): return None if current_source == "cruise" and self.spacious_follow_cap_bypass_active(lead, v_ego, t_follow, tracking_lead_active): return None diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 057644c67e..38f440f7e2 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -2447,6 +2447,27 @@ def test_route_8bc6_catchup_cap_skips_slightly_negative_delta_when_lead_accelera assert cap is None +def test_route_8bc6_catchup_cap_skips_comfortable_accelerating_lead_when_source_flips_to_lead0(): + v_ego = 24.361867904663086 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead( + status=True, d_rel=38.5, v_lead=24.46365737915039, + a_lead=0.4625629186630249, radar=True, model_prob=1.0, y_rel=0.0, + ) + + cap = planner.get_lead_catchup_accel_cap( + lead, + v_ego, + 1.3549551963806152, + current_source="lead0", + tracking_lead_active=True, + ) + + assert planner.is_comfortable_accelerating_away_follow(lead, v_ego, 1.3549551963806152) + assert cap is None + + def test_route_8bc6_radar_matched_follow_catchup_cap_skips_buffer_edge_square_wave(): v_ego = 22.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)