Calculate average desired curvature using planned distance

Added toggle to calculate average desired curvature using planned distance instead of the car's speed.

Credit goes to Pfeiferj!

https: //github.com/pfeiferj
Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev>

cereal/custom.capnp

@@ -22,6 +22,7 @@ struct FrogPilotLateralPlan @0xda96579883444c35 {
 }
 
 struct FrogPilotLongitudinalPlan @0x80ae746ee2596b11 {
+  distances @3 :List(Float32);
 }
 
 struct FrogPilotNavigation @0xa5cd762cd951a455 {

common/params.cc

@@ -217,6 +217,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"AlwaysOnLateral", PERSISTENT},
     {"AlwaysOnLateralMain", PERSISTENT},
     {"ApiCache_DriveStats", PERSISTENT},
+    {"AverageCurvature", PERSISTENT},
     {"FrogPilotTogglesUpdated", PERSISTENT},
     {"GasRegenCmd", PERSISTENT},
     {"LateralTune", PERSISTENT},

selfdrive/controls/controlsd.py

@@ -649,7 +649,9 @@ class Controls:
       self.desired_curvature, self.desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo,
                                                                                        lat_plan.psis,
                                                                                        lat_plan.curvatures,
-                                                                                       lat_plan.curvatureRates)
+                                                                                       lat_plan.curvatureRates,
+                                                                                       frogpilot_long_plan.distances,
+                                                                                       self.average_desired_curvature)
       actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
                                                                              self.steer_limited, self.desired_curvature,
                                                                              self.desired_curvature_rate, self.sm['liveLocationKalman'])
@@ -940,6 +942,8 @@ class Controls:
       if hasattr(obj, 'update_frogpilot_params'):
         obj.update_frogpilot_params(self.params)
 
+    self.average_desired_curvature = self.params.get_bool("AverageCurvature")
+
     longitudinal_tune = self.params.get_bool("LongitudinalTune")
     self.sport_plus = self.params.get_int("AccelerationProfile") == 3 and longitudinal_tune
 

selfdrive/controls/lib/drive_helpers.py

@@ -16,6 +16,7 @@ V_CRUISE_INITIAL = 40
 V_CRUISE_INITIAL_EXPERIMENTAL_MODE = 105
 IMPERIAL_INCREMENT = 1.6  # should be CV.MPH_TO_KPH, but this causes rounding errors
 
+MIN_DIST = 0.001
 MIN_SPEED = 1.0
 CONTROL_N = 17
 CAR_ROTATION_RADIUS = 0.0
@@ -163,11 +164,12 @@ def rate_limit(new_value, last_value, dw_step, up_step):
   return clip(new_value, last_value + dw_step, last_value + up_step)
 
 
-def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates):
+def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates, distances, average_desired_curvature):
-  if len(psis) != CONTROL_N:
+  if len(psis) != CONTROL_N or len(distances) != CONTROL_N:
     psis = [0.0]*CONTROL_N
     curvatures = [0.0]*CONTROL_N
     curvature_rates = [0.0]*CONTROL_N
+    distances = [0.0]*CONTROL_N
   v_ego = max(MIN_SPEED, v_ego)
 
   # TODO this needs more thought, use .2s extra for now to estimate other delays
@@ -178,7 +180,10 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates):
   # psi to calculate a simple linearization of desired curvature
   current_curvature_desired = curvatures[0]
   psi = interp(delay, ModelConstants.T_IDXS[:CONTROL_N], psis)
-  average_curvature_desired = psi / (v_ego * delay)
+  # Pfeiferj's #28118 PR - https://github.com/commaai/openpilot/pull/28118
+  distance = interp(delay, ModelConstants.T_IDXS[:CONTROL_N], distances)
+  distance = max(MIN_DIST, distance)
+  average_curvature_desired = psi / distance if average_desired_curvature else psi / (v_ego * delay)
   desired_curvature = 2 * average_curvature_desired - current_curvature_desired
 
   # This is the "desired rate of the setpoint" not an actual desired rate

selfdrive/controls/lib/lateral_planner.py

@@ -35,6 +35,9 @@ class LateralPlanner:
     if len(md.position.x) == TRAJECTORY_SIZE and len(md.velocity.x) == TRAJECTORY_SIZE and len(md.lateralPlannerSolution.x) == TRAJECTORY_SIZE:
       self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z])
       self.velocity_xyz = np.column_stack([md.velocity.x, md.velocity.y, md.velocity.z])
+      if frogpilot_planner.average_desired_curvature:
+        car_speed = np.array(md.velocity.x) - get_speed_error(md, v_ego_car)
+      else:
         car_speed = np.linalg.norm(self.velocity_xyz, axis=1) - get_speed_error(md, v_ego_car)
       self.v_plan = np.clip(car_speed, MIN_SPEED, np.inf)
       self.v_ego = self.v_plan[0]

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -230,6 +230,7 @@ class LongitudinalMpc:
     # self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.solver.reset()
     # self.solver.options_set('print_level', 2)
+    self.x_solution = np.zeros(N+1)
     self.v_solution = np.zeros(N+1)
     self.a_solution = np.zeros(N+1)
     self.prev_a = np.array(self.a_solution)
@@ -442,6 +443,7 @@ class LongitudinalMpc:
     for i in range(N):
       self.u_sol[i] = self.solver.get(i, 'u')
 
+    self.x_solution = self.x_sol[:,0]
     self.v_solution = self.x_sol[:,1]
     self.a_solution = self.x_sol[:,2]
     self.j_solution = self.u_sol[:,0]

selfdrive/frogpilot/functions/frogpilot_planner.py

@@ -1,9 +1,12 @@
 import cereal.messaging as messaging
+import numpy as np
 
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.numpy_fast import interp
-from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, V_CRUISE_MAX
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
 from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, A_CRUISE_MAX_VALS, A_CRUISE_MAX_BP, get_max_accel
+from openpilot.selfdrive.modeld.constants import ModelConstants
 
 # Acceleration profiles - Credit goes to the DragonPilot team!
                  # MPH = [0.,  35,   35,  40,    40,  45,    45,  67,    67,   67, 123]
@@ -34,6 +37,8 @@ class FrogPilotPlanner:
   def __init__(self, params):
     self.v_cruise = 0
 
+    self.x_desired_trajectory = np.zeros(CONTROL_N)
+
     self.update_frogpilot_params(params)
 
   def update(self, sm, mpc):
@@ -55,6 +60,9 @@ class FrogPilotPlanner:
 
     self.v_cruise = self.update_v_cruise(carState, controlsState, modelData, enabled, v_cruise, v_ego)
 
+    self.x_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, mpc.x_solution)
+    self.x_desired_trajectory = self.x_desired_trajectory_full[:CONTROL_N]
+
   def update_v_cruise(self, carState, controlsState, modelData, enabled, v_cruise, v_ego):
     v_ego_diff = max(carState.vEgoRaw - carState.vEgoCluster, 0)
     return v_cruise - v_ego_diff
@@ -71,12 +79,15 @@ class FrogPilotPlanner:
     frogpilot_longitudinal_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
     frogpilotLongitudinalPlan = frogpilot_longitudinal_plan_send.frogpilotLongitudinalPlan
 
+    frogpilotLongitudinalPlan.distances = self.x_desired_trajectory.tolist()
+
     pm.send('frogpilotLongitudinalPlan', frogpilot_longitudinal_plan_send)
 
   def update_frogpilot_params(self, params):
     self.is_metric = params.get_bool("IsMetric")
 
     lateral_tune = params.get_bool("LateralTune")
+    self.average_desired_curvature = params.get_bool("AverageCurvature") and lateral_tune
 
     longitudinal_tune = params.get_bool("LongitudinalTune")
     self.acceleration_profile = params.get_int("AccelerationProfile") if longitudinal_tune else 0

selfdrive/frogpilot/ui/control_settings.cc

@@ -6,6 +6,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
     {"AlwaysOnLateral", "Always on Lateral", "Maintain openpilot lateral control when the brake or gas pedals are used.\n\nDeactivation occurs only through the 'Cruise Control' button.", "../frogpilot/assets/toggle_icons/icon_always_on_lateral.png"},
 
     {"LateralTune", "Lateral Tuning", "Modify openpilot's steering behavior.", "../frogpilot/assets/toggle_icons/icon_lateral_tune.png"},
+    {"AverageCurvature", "Average Desired Curvature", "Use Pfeiferj's distance-based curvature adjustment for improved curve handling.", ""},
 
     {"LongitudinalTune", "Longitudinal Tuning", "Modify openpilot's acceleration and braking behavior.", "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"},
     {"AccelerationProfile", "Acceleration Profile", "Change the acceleration rate to be either sporty or eco-friendly.", ""},
@@ -97,7 +98,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
   conditionalExperimentalKeys = {};
   fireTheBabysitterKeys = {};
   laneChangeKeys = {};
-  lateralTuneKeys = {};
+  lateralTuneKeys = {"AverageCurvature"};
   longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration"};
   speedLimitControllerKeys = {};
   visionTurnControlKeys = {};