prep for pac

selfdrive/controls/lib/longitudinal_planner.py

@@ -20,6 +20,7 @@ from openpilot.common.params import Params
 from openpilot.dp_ext.selfdrive.controls.lib.dynamic_endtoend_controller import DynamicEndtoEndController
 from openpilot.dp_ext.selfdrive.controls.lib.alt_driving_personality_controller import AlternativeDrivingPersonalityController
 from openpilot.dp_ext.selfdrive.controls.lib.curve_speed_limiter import CurveSpeedLimiter
+from openpilot.dp_ext.selfdrive.controls.lib.personalized_accel_controller import PersonalizedAccelController
 
 LON_MPC_STEP = 0.2  # first step is 0.2s
 A_CRUISE_MIN = -1.2
@@ -90,6 +91,7 @@ class LongitudinalPlanner:
     self._dynamic_endtoend_controller = DynamicEndtoEndController()
     self._adp_controller = AlternativeDrivingPersonalityController()
     self._curve_speed_limiter = CurveSpeedLimiter()
+    self._pac = PersonalizedAccelController()
 
   @staticmethod
   def parse_model(model_msg, model_error):
@@ -110,6 +112,7 @@ class LongitudinalPlanner:
   def update(self, sm):
     if self._frame % 50 == 0:
       self._dynamic_endtoend_controller.set_enabled(self.params.get_bool("dp_long_de2e"))
+      self._pac.set_enabled(self.params.get_bool("dp_long_pac"))
 
     self._frame += 1
 
@@ -139,7 +142,9 @@ class LongitudinalPlanner:
     prev_accel_constraint = not (reset_state or sm['carState'].standstill)
 
     if self.mpc.mode == 'acc':
-      accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
+      self._pac.update(sm['carState'], sm['radarState'].leadOne.status and v_ego > 0. and sm['radarState'].leadOne.dRel / v_ego < 1.)
+      # accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
+      accel_limits = [A_CRUISE_MIN, self._pac.get_max_accel(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)]
       accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
     else:
       accel_limits = [ACCEL_MIN, ACCEL_MAX]