Calculate average desired curvature using planned distance

Added toggle to calculate average desired curvature using planned distance instead of vego.

Credit goes to Pfeiferj!

https://github.com/pfeiferj
commaai#28118

cereal/car.capnp

@@ -501,6 +501,8 @@ struct CarParams {
   wheelSpeedFactor @63 :Float32; # Multiplier on wheels speeds to computer actual speeds
 
   # FrogPilot CarParams
+  lateralTune @78 :Bool;
+  pfeiferjDesiredCurvatures @80 :Bool;
 
   struct SafetyConfig {
     safetyModel @0 :SafetyModel;

cereal/log.capnp

@@ -996,6 +996,7 @@ struct LongitudinalPlan @0xe00b5b3eba12876c {
   accels @32 :List(Float32);
   speeds @33 :List(Float32);
   jerks @34 :List(Float32);
+  distances @37 :List(Float32);
 
   solverExecutionTime @35 :Float32;
   personality @36 :LongitudinalPersonality;

common/params.cc

@@ -91,6 +91,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"AssistNowToken", PERSISTENT},
     {"AthenadPid", PERSISTENT},
     {"AthenadUploadQueue", PERSISTENT},
+    {"AverageDesiredCurvature", PERSISTENT},
     {"BlindSpotPath", PERSISTENT},
     {"CalibrationParams", PERSISTENT},
     {"CameraDebugExpGain", CLEAR_ON_MANAGER_START},
@@ -171,6 +172,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"LastSystemShutdown", CLEAR_ON_MANAGER_START},
     {"LastUpdateException", CLEAR_ON_MANAGER_START},
     {"LastUpdateTime", PERSISTENT},
+    {"LateralTuning", PERSISTENT},
     {"LiveParameters", PERSISTENT},
     {"LiveTorqueCarParams", PERSISTENT},
     {"LiveTorqueParameters", PERSISTENT | DONT_LOG},

selfdrive/car/interfaces.py

@@ -183,6 +183,8 @@ def get_std_params(candidate):
 
     # FrogPilot variables
     params = Params()
+    ret.lateralTune = params.get_bool("LateralTuning")
+    ret.pfeiferjDesiredCurvatures = ret.lateralTune and params.get_bool("AverageDesiredCurvature")
     return ret
 
   @staticmethod

selfdrive/controls/controlsd.py

@@ -118,6 +118,7 @@ def __init__(self, sm=None, pm=None, can_sock=None, CI=None):
     # FrogPilot variables
     fire_the_babysitter = self.params.get_bool("FireTheBabysitter")
     frog_theme = self.params.get_bool("FrogTheme")
+    self.average_desired_curvature = self.CP.pfeiferjDesiredCurvatures
     self.frog_sounds = frog_theme and self.params.get_bool("FrogSounds")
     self.mute_dm = fire_the_babysitter and self.params.get_bool("MuteDM")
     self.mute_overheat = fire_the_babysitter and self.params.get_bool("MuteSystemOverheat")
@@ -630,7 +631,9 @@ def state_control(self, CS):
       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,
+                                                                                       long_plan.distances,
+                                                                                       self.average_desired_curvature)
       actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
                                                                              self.last_actuators, self.steer_limited, self.desired_curvature,
                                                                              self.desired_curvature_rate, self.sm['liveLocationKalman'])

selfdrive/controls/lib/drive_helpers.py

@@ -16,6 +16,7 @@
 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):
-  if len(psis) != CONTROL_N:
+def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates, distances, average_desired_curvature):
+  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, 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, 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

@@ -51,6 +51,7 @@ def __init__(self, CP, debug=False):
     self.reset_mpc(np.zeros(4))
 
     # FrogPilot variables
+    self.average_desired_curvature = CP.pfeiferjDesiredCurvatures
 
   def reset_mpc(self, x0=None):
     if x0 is None:
@@ -71,7 +72,10 @@ def update(self, sm):
       self.plan_yaw = np.array(md.orientation.z)
       self.plan_yaw_rate = np.array(md.orientationRate.z)
       self.velocity_xyz = np.column_stack([md.velocity.x, md.velocity.y, md.velocity.z])
-      car_speed = np.linalg.norm(self.velocity_xyz, axis=1) - get_speed_error(md, v_ego_car)
+      if self.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

@@ -234,6 +234,7 @@ def reset(self):
     # 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)
@@ -440,6 +441,7 @@ def run(self):
     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/controls/lib/longitudinal_planner.py

@@ -56,6 +56,7 @@ def __init__(self, CP, init_v=0.0, init_a=0.0):
     self.v_desired_filter = FirstOrderFilter(init_v, 2.0, DT_MDL)
     self.v_model_error = 0.0
 
+    self.x_desired_trajectory = np.zeros(CONTROL_N)
     self.v_desired_trajectory = np.zeros(CONTROL_N)
     self.a_desired_trajectory = np.zeros(CONTROL_N)
     self.j_desired_trajectory = np.zeros(CONTROL_N)
@@ -146,8 +147,10 @@ def update(self, sm):
     x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
     self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=self.personality)
 
+    self.x_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.x_solution)
     self.v_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.a_solution)
+    self.x_desired_trajectory = self.x_desired_trajectory_full[:CONTROL_N]
     self.v_desired_trajectory = self.v_desired_trajectory_full[:CONTROL_N]
     self.a_desired_trajectory = self.a_desired_trajectory_full[:CONTROL_N]
     self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution)
@@ -171,6 +174,7 @@ def publish(self, sm, pm):
     longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
     longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2']
 
+    longitudinalPlan.distances = self.x_desired_trajectory.tolist()
     longitudinalPlan.speeds = self.v_desired_trajectory.tolist()
     longitudinalPlan.accels = self.a_desired_trajectory.tolist()
     longitudinalPlan.jerks = self.j_desired_trajectory.tolist()

selfdrive/ui/qt/offroad/frogpilot_settings.cc

@@ -19,6 +19,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(QWidget *parent) : FrogPilotPanel
   static const std::vector<std::tuple<QString, QString, QString, QString>> toggles = {
     {"DeviceShutdownTimer", "Device Shutdown Timer", "Set the timer for when the device turns off after being offroad to reduce energy waste and prevent battery drain.", "../assets/offroad/icon_time.png"},
     {"FireTheBabysitter", "Fire the Babysitter", "Disable some of openpilot's 'Babysitter Protocols'.", "../assets/offroad/icon_babysitter.png"},
+    {"LateralTuning", "Lateral Tuning", "Change the way openpilot steers.", "../assets/offroad/icon_lateral_tune.png"},
     {"NudgelessLaneChange", "Nudgeless Lane Change", "Switch lanes without having to nudge the steering wheel.", "../assets/offroad/icon_lane.png"}
   };
 
@@ -37,6 +38,10 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(QWidget *parent) : FrogPilotPanel
         {"MuteSeatbelt", "Mute Seatbelt"},
         {"MuteSystemOverheat", "Mute Overheat"}
       }, mainLayout);
+    } else if (key == "LateralTuning") {
+      createSubControl(key, label, desc, icon, {}, {
+        {"AverageDesiredCurvature", "Average Desired Curvature", "Use Pfeiferj's distance based curvature adjustment for smoother handling of curves."}
+      });
     } else if (key == "NudgelessLaneChange") {
       createSubControl(key, label, desc, icon, {
         new LaneChangeTimer(),