Acceleration/Deceleration Profiles

2025-12-03 10:54:45 -07:00
parent ab8c4dea61
commit cc612ebd03
5 changed files with 77 additions and 4 deletions
@@ -3,6 +3,36 @@ import numpy as np

 from openpilot.selfdrive.controls.lib.longitudinal_planner import ACCEL_MIN, get_max_accel

+A_CRUISE_MIN_ECO =   ACCEL_MIN / 2
+A_CRUISE_MIN_SPORT = ACCEL_MIN * 2
+
+                  # MPH = [0.0,  11,  22,  34,  45,  56,  89]
+A_CRUISE_MAX_BP_CUSTOM =  [0.0,  5., 10., 15., 20., 25., 40.]
+A_CRUISE_MAX_VALS_ECO =   [2.0, 1.5, 1.0, 0.8, 0.6, 0.4, 0.2]
+A_CRUISE_MAX_VALS_SPORT = [3.0, 2.5, 2.0, 1.5, 1.0, 0.8, 0.6]
+
+ACCELERATION_PROFILES = {
+  "STANDARD": 0,
+  "ECO": 1,
+  "SPORT": 2,
+  "SPORT_PLUS": 3
+}
+
+DECELERATION_PROFILES = {
+  "STANDARD": 0,
+  "ECO": 1,
+  "SPORT": 2
+}
+
+def get_max_accel_eco(v_ego):
+  return np.interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO)
+
+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_allowed_accel(v_ego):
+  return np.interp(v_ego, [0., 5., 20.], [4.0, 4.0, 2.0])  # ISO 15622:2018
+
 class FrogPilotAcceleration:
  def __init__(self, FrogPilotPlanner):
    self.frogpilot_planner = FrogPilotPlanner
@@ -11,9 +41,38 @@ class FrogPilotAcceleration:
    self.min_accel = 0

  def update(self, v_ego, sm, frogpilot_toggles):
-    self.max_accel = get_max_accel(v_ego)
+    eco_gear = sm["frogpilotCarState"].ecoGear
+    sport_gear = sm["frogpilotCarState"].sportGear
+
+    if (eco_gear or sport_gear) and frogpilot_toggles.map_acceleration:
+      if eco_gear:
+        self.max_accel = get_max_accel_eco(v_ego)
+      else:
+        if frogpilot_toggles.acceleration_profile == ACCELERATION_PROFILES["SPORT"]:
+          self.max_accel = get_max_accel_sport(v_ego)
+        else:
+          self.max_accel = get_max_allowed_accel(v_ego)
+    else:
+      if frogpilot_toggles.acceleration_profile == ACCELERATION_PROFILES["ECO"]:
+        self.max_accel = get_max_accel_eco(v_ego)
+      elif frogpilot_toggles.acceleration_profile == ACCELERATION_PROFILES["SPORT"]:
+        self.max_accel = get_max_accel_sport(v_ego)
+      elif frogpilot_toggles.acceleration_profile == ACCELERATION_PROFILES["SPORT_PLUS"]:
+        self.max_accel = get_max_allowed_accel(v_ego)
+      else:
+        self.max_accel = get_max_accel(v_ego)

    if self.frogpilot_planner.tracking_lead:
      self.min_accel = ACCEL_MIN
+    elif (eco_gear or sport_gear) and frogpilot_toggles.map_deceleration:
+      if eco_gear:
+        self.min_accel = A_CRUISE_MIN_ECO
+      else:
+        self.min_accel = A_CRUISE_MIN_SPORT
    else:
-      self.min_accel = ACCEL_MIN
+      if frogpilot_toggles.deceleration_profile == DECELERATION_PROFILES["ECO"]:
+        self.min_accel = A_CRUISE_MIN_ECO
+      elif frogpilot_toggles.deceleration_profile == DECELERATION_PROFILES["SPORT"]:
+        self.min_accel = A_CRUISE_MIN_SPORT
+      else:
+        self.min_accel = ACCEL_MIN
@@ -314,6 +314,8 @@ class CarInterfaceBase(ABC):
    self.CS.out = ret

    # FrogPilot variables
+    fp_ret.ecoGear |= ret.gearShifter == GearShifter.eco
+    fp_ret.sportGear |= ret.gearShifter == GearShifter.sport

    return ret, fp_ret

@@ -204,6 +204,10 @@ class CarState(CarStateBase):
    # FrogPilot variables
    fp_ret = custom.FrogPilotCarState.new_message()

+    if not self.CP.flags & ToyotaFlags.SECOC.value:
+      fp_ret.ecoGear = cp.vl["GEAR_PACKET"]["ECON_ON"] == 1
+      fp_ret.sportGear = cp.vl["GEAR_PACKET"]["SPORT_ON_2" if self.CP.flags & ToyotaFlags.NO_DSU else "SPORT_ON"] == 1
+
    ret.buttonEvents = buttonEvents

    return ret, fp_ret
@@ -217,6 +217,7 @@ BO_ 956 GEAR_PACKET: 8 XXX
 SG_ ECON_ON : 40|1@0+ (1,0) [0|1] "" XXX
 SG_ B_GEAR_ENGAGED : 41|1@0+ (1,0) [0|1] "" XXX
 SG_ DRIVE_ENGAGED : 47|1@0+ (1,0) [0|1] "" XXX
+ SG_ SPORT_ON_2 : 55|1@0+ (1,0) [0|1] "" XXX

 BO_ 1005 REVERSE_CAMERA_STATE: 2 BGM
 SG_ REVERSE_CAMERA_GUIDELINES : 9|2@0+ (1,0) [1|3] "" XXX
@@ -522,6 +523,7 @@ VAL_ 898 AIR_CONDITIONER_STATE 20 "CLIMATE OFF";
 VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
 VAL_ 921 DISTANCE_LINES 0 "not displayed" 1 "close" 2 "medium" 3 "far";
 VAL_ 956 SPORT_ON 0 "off" 1 "on";
+VAL_ 956 SPORT_ON_2 0 "off" 1 "on";
 VAL_ 956 GEAR 0 "D" 1 "S" 8 "N" 16 "R" 32 "P";
 VAL_ 956 SPORT_GEAR_ON 0 "off" 1 "on";
 VAL_ 956 SPORT_GEAR 1 "S1" 2 "S2" 3 "S3" 4 "S4" 5 "S5" 6 "S6";
@@ -15,6 +15,8 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_accel_
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
 from openpilot.common.swaglog import cloudlog

+from openpilot.frogpilot.common.frogpilot_variables import MINIMUM_LATERAL_ACCELERATION
+
 LON_MPC_STEP = 0.2  # first step is 0.2s
 A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
 A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
@@ -43,7 +45,11 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
  # The lookup table for turns should also be updated if we do this
  a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
  a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
-  a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.))
+
+  if abs(a_y) > MINIMUM_LATERAL_ACCELERATION:
+    a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.))
+  else:
+    a_x_allowed = a_target[1]

  return [a_target[0], min(a_target[1], a_x_allowed)]

@@ -116,7 +122,7 @@ class LongitudinalPlanner:
    prev_accel_constraint = not (reset_state or sm['carState'].standstill)

    if mode == 'acc':
-      accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
+      accel_clip = [sm['frogpilotPlan'].minAcceleration, sm['frogpilotPlan'].maxAcceleration]
      steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
      accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
    else: