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>

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 = {};