diff --git a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 0a0722fbc6..885f39bcc3 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -57,6 +57,12 @@ COMFORT_BRAKE = 2.5 STOP_DISTANCE = 6.0 MIN_X_LEAD_FACTOR = 0.5 +# aggressive matches stock Hyundai SCC closest setting: 2.4 m stopped, 14.8 m @ 14, 20.2 m @ 17.8, 25.2 m @ 21.3 m/s +# the MPC settles ~1.1 m short of its target at standstill, so 3.5 m stops at ~2.4 m +AGGRESSIVE_STOP_DISTANCE = 3.5 +AGGRESSIVE_T_FOLLOW_BP = [0., 14., 16., 18., 20., 21.3, 30.] +AGGRESSIVE_T_FOLLOW_V = [0.82, 0.82, 0.87, 0.94, 0.97, 1.02, 1.08] + def get_jerk_factor(personality=log.LongitudinalPersonality.standard): if personality==log.LongitudinalPersonality.relaxed: return 1.0 @@ -309,6 +315,11 @@ class LongitudinalMpc: def update(self, radarstate, personality=log.LongitudinalPersonality.standard): t_follow = get_T_FOLLOW(personality) + stop_distance_offset = 0. + if personality == log.LongitudinalPersonality.aggressive: + t_follow = float(np.interp(self.x0[1], AGGRESSIVE_T_FOLLOW_BP, AGGRESSIVE_T_FOLLOW_V)) + # STOP_DISTANCE is compiled into the solver, so move the lead forward instead + stop_distance_offset = STOP_DISTANCE - AGGRESSIVE_STOP_DISTANCE lead_xv_0 = self.process_lead(radarstate.leadOne) lead_xv_1 = self.process_lead(radarstate.leadTwo) @@ -316,8 +327,8 @@ class LongitudinalMpc: # To estimate a safe distance from a moving lead, we calculate how much stopping # distance that lead needs as a minimum. We can add that to the current distance # and then treat that as a stopped car/obstacle at this new distance. - lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1]) - lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) + lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1]) + stop_distance_offset + lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) + stop_distance_offset x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle]) self.source = MPC_SOURCES[np.argmin(x_obstacles[0])] diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index 87f62fbe95..36ea482a6b 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -2,6 +2,7 @@ import numpy as np import openpilot.cereal.messaging as messaging +from openpilot.cereal import log from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX from openpilot.common.constants import CV from openpilot.common.filter_simple import FirstOrderFilter @@ -20,6 +21,7 @@ from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import Lon J_CRUISE_VALS = [1.6, 1.2, 0.8, 0.6] J_CRUISE_BP = [0., 10.0, 25., 40.] A_CRUISE_MIN = -0.5 +AGGRESSIVE_LEAD_ONLY_DIST = 40. # m CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] @@ -114,6 +116,10 @@ class LongitudinalPlanner(LongitudinalPlannerSP): candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc), (self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop), (output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e)] + # aggressive: with a close radar lead, follow it like stock SCC instead of the model's larger stop gap + lead = sm['radarState'].leadOne + if sm['selfdriveState'].personality == log.LongitudinalPersonality.aggressive and lead.radar and lead.dRel < AGGRESSIVE_LEAD_ONLY_DIST: + candidates.pop() output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0]) self.output_should_stop = any(should_stop for _, _, should_stop in candidates) diff --git a/openpilot/selfdrive/controls/tests/test_following_distance.py b/openpilot/selfdrive/controls/tests/test_following_distance.py index ccae879692..142dce8c6c 100644 --- a/openpilot/selfdrive/controls/tests/test_following_distance.py +++ b/openpilot/selfdrive/controls/tests/test_following_distance.py @@ -4,7 +4,9 @@ from openpilot.common.parameterized import parameterized_class from openpilot.cereal import log -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW +import numpy as np +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW, \ + STOP_DISTANCE, AGGRESSIVE_STOP_DISTANCE, AGGRESSIVE_T_FOLLOW_BP, AGGRESSIVE_T_FOLLOW_V from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver @@ -38,7 +40,11 @@ class TestFollowingDistance(OpenpilotTestCase): def test_following_distance(self): v_lead = float(self.speed) simulation_steady_state = run_following_distance_simulation(v_lead, personality=self.personality) - correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality)) + if self.personality == log.LongitudinalPersonality.aggressive: + t_follow = float(np.interp(v_lead, AGGRESSIVE_T_FOLLOW_BP, AGGRESSIVE_T_FOLLOW_V)) + correct_steady_state = desired_follow_distance(v_lead, v_lead, t_follow) - (STOP_DISTANCE - AGGRESSIVE_STOP_DISTANCE) + else: + correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality)) abs_err_margin = 0.5 if v_lead > 0.0 else 1.15 self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=0.2 * correct_steady_state + abs_err_margin)