From f930a2a3a5fbcb1cc89ebbcf16713b2ba30ba759 Mon Sep 17 00:00:00 2001
From: James <91348155+FrogAi@users.noreply.github.com>
Date: Mon, 1 Dec 2025 12:00:00 -0700
Subject: [PATCH] Human-Like Acceleration

---
 frogpilot/controls/lib/frogpilot_acceleration.py | 12 ++++++++++++
 selfdrive/controls/lib/longcontrol.py            |  5 ++++-
 2 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/frogpilot/controls/lib/frogpilot_acceleration.py b/frogpilot/controls/lib/frogpilot_acceleration.py
index e5400256..6833b517 100644
--- a/frogpilot/controls/lib/frogpilot_acceleration.py
+++ b/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:
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 8de7c268..c8edb4ed 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/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