diff --git a/opendbc_repo b/opendbc_repo index 8f8b4114c7..5e53e1d8a8 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 8f8b4114c73a831aac0c4ed0510c9cacc997cd6d +Subproject commit 5e53e1d8a8ab9d01a8ed98543416ea13fe112f68 diff --git a/openpilot/cereal/custom.capnp b/openpilot/cereal/custom.capnp index 5518bca72e..430aa140d9 100644 --- a/openpilot/cereal/custom.capnp +++ b/openpilot/cereal/custom.capnp @@ -389,44 +389,6 @@ struct CarControlSP @0xa5cd762cd951a455 { leadOne @2 :LeadData; leadTwo @3 :LeadData; intelligentCruiseButtonManagement @4 :IntelligentCruiseButtonManagement; - smartCruiseControl @5 :SmartCruiseControl; - speedLimit @6 :SpeedLimit; - laneChange @7 :LaneChange; - driverMonitoring @8 :DriverMonitoring; - leadFollowing @9 :Bool; - - struct SmartCruiseControl { - visionState @0 :LongitudinalPlanSP.SmartCruiseControl.VisionState; - mapState @1 :LongitudinalPlanSP.SmartCruiseControl.MapState; - } - - struct SpeedLimit { - assistState @0 :LongitudinalPlanSP.SpeedLimit.AssistState; - speedLimitFinalLast @1 :Float32; - } - - struct DriverMonitoring { - alertLevel @0 :UInt8; - visionPolicy @1 :Bool; - } - - struct LaneChange { - state @0 :State; - direction @1 :Direction; - - enum State { - off @0; - preLaneChange @1; - laneChangeStarting @2; - laneChangeFinishing @3; - } - - enum Direction { - none @0; - left @1; - right @2; - } - } struct Param { key @0 :Text; diff --git a/openpilot/selfdrive/car/helpers.py b/openpilot/selfdrive/car/helpers.py index eae8d8c03b..ec9e3efeeb 100644 --- a/openpilot/selfdrive/car/helpers.py +++ b/openpilot/selfdrive/car/helpers.py @@ -63,9 +63,5 @@ def convert_carControlSP(struct: capnp.lib.capnp._DynamicStructReader) -> struct struct_dataclass.intelligentCruiseButtonManagement = structs.IntelligentCruiseButtonManagement( **remove_deprecated(struct_dict.get('intelligentCruiseButtonManagement', {})) ) - struct_dataclass.smartCruiseControl = structs.CarControlSP.SmartCruiseControl(**struct_dict.get('smartCruiseControl', {})) - struct_dataclass.speedLimit = structs.CarControlSP.SpeedLimit(**struct_dict.get('speedLimit', {})) - struct_dataclass.laneChange = structs.CarControlSP.LaneChange(**struct_dict.get('laneChange', {})) - struct_dataclass.driverMonitoring = structs.CarControlSP.DriverMonitoring(**struct_dict.get('driverMonitoring', {})) return struct_dataclass 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 a5d74a8f50..0a0722fbc6 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -57,14 +57,6 @@ COMFORT_BRAKE = 2.5 STOP_DISTANCE = 6.0 MIN_X_LEAD_FACTOR = 0.5 -# aggressive follows close to stock Hyundai SCC closest setting: 2.4 m stopped (stock 2.4), 16.3 m @ 31 mph (stock 14.8), 22 m @ 40 (20.2), 27.5 m @ 48 (25.2) -# the MPC settles ~0.8 m short of its target at standstill, so a 3.2 m target stops at ~2.4 m -# the short stop gap fades out by 10 m/s and moving gaps are ~0.1 s over stock: exact stock gaps crash in sim when the lead brakes 2 m/s^2 to a stop -AGGRESSIVE_STOP_DISTANCE_BP = [0., 3., 10.] -AGGRESSIVE_STOP_DISTANCE_V = [3.2, 3.2, STOP_DISTANCE] -AGGRESSIVE_T_FOLLOW_BP = [0., 10., 14., 16., 18., 20., 21.3, 30.] -AGGRESSIVE_T_FOLLOW_V = [0.74, 0.74, 0.74, 0.81, 0.90, 0.945, 1.0, 1.1] - def get_jerk_factor(personality=log.LongitudinalPersonality.standard): if personality==log.LongitudinalPersonality.relaxed: return 1.0 @@ -317,11 +309,6 @@ 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 - float(np.interp(self.x0[1], AGGRESSIVE_STOP_DISTANCE_BP, AGGRESSIVE_STOP_DISTANCE_V)) lead_xv_0 = self.process_lead(radarstate.leadOne) lead_xv_1 = self.process_lead(radarstate.leadTwo) @@ -329,8 +316,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]) + stop_distance_offset - lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) + stop_distance_offset + 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]) 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 36ea482a6b..87f62fbe95 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -2,7 +2,6 @@ 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 @@ -21,7 +20,6 @@ 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] @@ -116,10 +114,6 @@ 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 c6bae0c856..ccae879692 100644 --- a/openpilot/selfdrive/controls/tests/test_following_distance.py +++ b/openpilot/selfdrive/controls/tests/test_following_distance.py @@ -4,9 +4,7 @@ from openpilot.common.parameterized import parameterized_class from openpilot.cereal import log -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_BP, AGGRESSIVE_STOP_DISTANCE_V, AGGRESSIVE_T_FOLLOW_BP, AGGRESSIVE_T_FOLLOW_V +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver @@ -40,12 +38,7 @@ 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) - if self.personality == log.LongitudinalPersonality.aggressive: - t_follow = float(np.interp(v_lead, AGGRESSIVE_T_FOLLOW_BP, AGGRESSIVE_T_FOLLOW_V)) - stop_distance = float(np.interp(v_lead, AGGRESSIVE_STOP_DISTANCE_BP, AGGRESSIVE_STOP_DISTANCE_V)) - correct_steady_state = desired_follow_distance(v_lead, v_lead, t_follow) - (STOP_DISTANCE - stop_distance) - else: - correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality)) + 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) diff --git a/openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py b/openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py index 4990ea8980..344ae52a93 100644 --- a/openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py +++ b/openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py @@ -31,7 +31,7 @@ class ControlsExt(ModelStateBase): self.CP_SP = messaging.log_from_bytes(params.get("CarParamsSP", block=True), custom.CarParamsSP) cloudlog.info("controlsd_ext got CarParamsSP") - self.sm_services_ext = ['radarState', 'selfdriveStateSP', 'longitudinalPlanSP'] + self.sm_services_ext = ['radarState', 'selfdriveStateSP'] self.pm_services_ext = ['carControlSP'] def initialize_lateral_control(self, lac, CI, dt): @@ -104,31 +104,6 @@ class ControlsExt(ModelStateBase): CC_SP.intelligentCruiseButtonManagement.sendButton = icbm_src.sendButton CC_SP.intelligentCruiseButtonManagement.vTarget = icbm_src.vTarget - # Smart Cruise Control state - scc_src = sm['longitudinalPlanSP'].smartCruiseControl - CC_SP.smartCruiseControl.visionState = scc_src.vision.state - CC_SP.smartCruiseControl.mapState = scc_src.map.state - - # Speed Limit Assist state - sl_src = sm['longitudinalPlanSP'].speedLimit - CC_SP.speedLimit.assistState = sl_src.assist.state - CC_SP.speedLimit.speedLimitFinalLast = sl_src.resolver.speedLimitFinalLast - - # Lane change state - meta = sm['modelV2'].meta - CC_SP.laneChange.state = str(meta.laneChangeState) - CC_SP.laneChange.direction = str(meta.laneChangeDirection) - - # Driver monitoring alert - dm = sm['driverMonitoringState'] - CC_SP.driverMonitoring.alertLevel = dm.alertLevel.raw - CC_SP.driverMonitoring.visionPolicy = dm.activePolicy == 'vision' - - # Planner following a lead, the model path (e2e) wins often while following so count it when there is a lead - long_plan = sm['longitudinalPlan'] - CC_SP.leadFollowing = str(long_plan.longitudinalPlanSource) in ('lead0', 'lead1', 'lead2') or \ - (str(long_plan.longitudinalPlanSource) == 'e2e' and long_plan.hasLead) - return CC_SP @staticmethod