Human-Like Acceleration

frogpilot/controls/lib/frogpilot_acceleration.py

@@ -3,6 +3,8 @@ import numpy as np
 
 from openpilot.selfdrive.controls.lib.longitudinal_planner import ACCEL_MIN, get_max_accel
 
+from openpilot.frogpilot.common.frogpilot_variables import CITY_SPEED_LIMIT
+
 A_CRUISE_MIN_ECO =   ACCEL_MIN / 2
 A_CRUISE_MIN_SPORT = ACCEL_MIN * 2
 
@@ -30,6 +32,12 @@ def get_max_accel_eco(v_ego):
 def get_max_accel_sport(v_ego):
   return np.interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT)
 
+def get_max_accel_low_speeds(max_accel, v_cruise):
+  return np.interp(v_cruise, [0., CITY_SPEED_LIMIT / 2, CITY_SPEED_LIMIT], [max_accel / 4, max_accel / 2, max_accel])
+
+def get_max_accel_ramp_off(max_accel, v_cruise, v_ego):
+  return np.interp(v_cruise - v_ego, [0., 1., 5.], [0., 0.5, max_accel])
+
 def get_max_allowed_accel(v_ego):
   return np.interp(v_ego, [0., 5., 20.], [4.0, 4.0, 2.0])  # ISO 15622:2018
 
@@ -62,6 +70,10 @@ class FrogPilotAcceleration:
       else:
         self.max_accel = get_max_accel(v_ego)
 
+    if frogpilot_toggles.human_acceleration:
+      self.max_accel = get_max_accel_low_speeds(self.max_accel, self.frogpilot_planner.v_cruise)
+      self.max_accel = min(get_max_accel_ramp_off(self.max_accel, self.frogpilot_planner.v_cruise, v_ego), self.max_accel)
+
     if self.frogpilot_planner.tracking_lead:
       self.min_accel = ACCEL_MIN
     elif sm["frogpilotCarState"].forceCoast:

selfdrive/controls/lib/longcontrol.py

@@ -76,7 +76,10 @@ class LongControl:
       self.reset()
 
     elif self.long_control_state == LongCtrlState.starting:
-      output_accel = frogpilot_toggles.startAccel
+      if frogpilot_toggles.human_acceleration:
+        output_accel = max(a_target, frogpilot_toggles.startAccel)
+      else:
+        output_accel = frogpilot_toggles.startAccel
       self.reset()
 
     else:  # LongCtrlState.pid