Map Turn Speed Control

Added toggle for "Map Turn Speed Control".

Credit goes to Pfeiferj!

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

selfdrive/frogpilot/functions/frogpilot_planner.py

@@ -1,6 +1,8 @@
 import cereal.messaging as messaging
+import numpy as np
 
 from openpilot.common.conversions import Conversions as CV
+from openpilot.common.numpy_fast import clip
 from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX
 from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE
@@ -9,6 +11,8 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN,
 from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED, FrogPilotFunctions
 
 from openpilot.selfdrive.frogpilot.functions.conditional_experimental_mode import ConditionalExperimentalMode
+from openpilot.selfdrive.frogpilot.functions.map_turn_speed_controller import MapTurnSpeedController
+
 
 class FrogPilotPlanner:
   def __init__(self, CP, params, params_memory):
@@ -18,7 +22,9 @@ class FrogPilotPlanner:
     self.fpf = FrogPilotFunctions()
 
     self.cem = ConditionalExperimentalMode(self.params_memory)
+    self.mtsc = MapTurnSpeedController()
 
+    self.mtsc_target = 0
     self.road_curvature = 0
     self.stop_distance = 0
     self.v_cruise = 0
@@ -30,8 +36,13 @@ class FrogPilotPlanner:
   def update(self, carState, controlsState, modelData, mpc, sm, v_cruise, v_ego):
     enabled = controlsState.enabled
 
+    # Use the stock deceleration profile to handle MTSC more precisely
+    v_cruise_changed = self.mtsc_target < v_cruise
+
     # Configure the deceleration profile
-    if self.deceleration_profile == 1:
+    if v_cruise_changed:
+      min_accel = A_CRUISE_MIN
+    elif self.deceleration_profile == 1:
       min_accel = self.fpf.get_min_accel_eco(v_ego)
     elif self.deceleration_profile == 2:
       min_accel = self.fpf.get_min_accel_sport(v_ego)
@@ -82,7 +93,20 @@ class FrogPilotPlanner:
     v_cruise_cluster = max(controlsState.vCruiseCluster, controlsState.vCruise) * CV.KPH_TO_MS
     v_cruise_diff = v_cruise_cluster - v_cruise
 
-    targets = []
+    # Pfeiferj's Map Turn Speed Controller
+    if self.map_turn_speed_controller and v_ego > CRUISING_SPEED and enabled:
+      mtsc_active = self.mtsc_target < v_cruise
+      self.mtsc_target = np.clip(self.mtsc.target_speed(v_ego, carState.aEgo), CRUISING_SPEED, v_cruise)
+
+      # MTSC failsafes
+      if self.mtsc_curvature_check and self.road_curvature < 1.0 and not mtsc_active:
+        self.mtsc_target = v_cruise
+      if v_ego - self.mtsc_limit >= self.mtsc_target:
+        self.mtsc_target = v_cruise
+    else:
+      self.mtsc_target = v_cruise
+
+    targets = [self.mtsc_target]
     filtered_targets = [target for target in targets if target > CRUISING_SPEED]
 
     return min(filtered_targets) if filtered_targets else v_cruise
@@ -92,6 +116,7 @@ class FrogPilotPlanner:
     frogpilot_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
     frogpilotPlan = frogpilot_plan_send.frogpilotPlan
 
+    frogpilotPlan.adjustedCruise = float(self.mtsc_target * (CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH))
     frogpilotPlan.conditionalExperimental = self.cem.experimental_mode
 
     frogpilotPlan.desiredFollowDistance = mpc.safe_obstacle_distance - mpc.stopped_equivalence_factor
@@ -136,3 +161,9 @@ class FrogPilotPlanner:
     self.deceleration_profile = params.get_int("DecelerationProfile") if longitudinal_tune else 0
     self.aggressive_acceleration = longitudinal_tune and params.get_bool("AggressiveAcceleration")
     self.increased_stopping_distance = params.get_int("StoppingDistance") * (1 if self.is_metric else CV.FOOT_TO_METER) if longitudinal_tune else 0
+
+    self.map_turn_speed_controller = params.get_bool("MTSCEnabled")
+    if self.map_turn_speed_controller:
+      self.mtsc_curvature_check = params.get_bool("MTSCCurvatureCheck")
+      self.mtsc_limit = params.get_float("MTSCLimit") * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS)
+      self.params_memory.put_float("MapTargetLatA", 2 * (params.get_int("MTSCAggressiveness") / 100))

selfdrive/frogpilot/functions/map_turn_speed_controller.py

@@ -0,0 +1,152 @@
+import json
+import math
+
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.numpy_fast import interp
+from openpilot.common.params import Params
+
+params_memory = Params("/dev/shm/params")
+
+R = 6373000.0 # approximate radius of earth in meters
+TO_RADIANS = math.pi / 180
+TO_DEGREES = 180 / math.pi
+TARGET_JERK = -0.6   # m/s^3 There's some jounce limits that are not consistent so we're fudging this some
+TARGET_ACCEL = -1.2  # m/s^2 should match up with the long planner limit
+TARGET_OFFSET = 1.0  # seconds - This controls how soon before the curve you reach the target velocity. It also helps
+                     # reach the target velocity when innacuracies in the distance modeling logic would cause overshoot.
+                     # The value is multiplied against the target velocity to determine the additional distance. This is
+                     # done to keep the distance calculations consistent but results in the offset actually being less
+                     # time than specified depending on how much of a speed diffrential there is between v_ego and the
+                     # target velocity.
+
+def calculate_accel(t, target_jerk, a_ego):
+  return a_ego  + target_jerk * t
+
+def calculate_distance(t, target_jerk, a_ego, v_ego):
+  return t * v_ego + a_ego/2 * (t ** 2) + target_jerk/6 * (t ** 3)
+
+def calculate_velocity(t, target_jerk, a_ego, v_ego):
+  return v_ego + a_ego * t + target_jerk/2 * (t ** 2)
+
+
+# points should be in radians
+# output is meters
+def distance_to_point(ax, ay, bx, by):
+  a = math.sin((bx-ax)/2)*math.sin((bx-ax)/2) + math.cos(ax) * math.cos(bx)*math.sin((by-ay)/2)*math.sin((by-ay)/2)
+  c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
+
+  return R * c  # in meters
+
+class MapTurnSpeedController:
+  def __init__(self):
+    self.target_lat = 0.0
+    self.target_lon = 0.0
+    self.target_v = 0.0
+
+  def target_speed(self, v_ego, a_ego) -> float:
+    lat = 0.0
+    lon = 0.0
+    try:
+      position = json.loads(params_memory.get("LastGPSPosition"))
+      lat = position["latitude"]
+      lon = position["longitude"]
+    except: return 0.0
+
+    try:
+      target_velocities = json.loads(params_memory.get("MapTargetVelocities"))
+    except: return 0.0
+
+    min_dist = 1000
+    min_idx = 0
+    distances = []
+
+    # find our location in the path
+    for i in range(len(target_velocities)):
+      target_velocity = target_velocities[i]
+      tlat = target_velocity["latitude"]
+      tlon = target_velocity["longitude"]
+      d = distance_to_point(lat * TO_RADIANS, lon * TO_RADIANS, tlat * TO_RADIANS, tlon * TO_RADIANS)
+      distances.append(d)
+      if d < min_dist:
+        min_dist = d
+        min_idx = i
+
+    # only look at values from our current position forward
+    forward_points = target_velocities[min_idx:]
+    forward_distances = distances[min_idx:]
+
+    # find velocities that we are within the distance we need to adjust for
+    valid_velocities = []
+    for i in range(len(forward_points)):
+      target_velocity = forward_points[i]
+      tlat = target_velocity["latitude"]
+      tlon = target_velocity["longitude"]
+      tv = target_velocity["velocity"]
+      if tv > v_ego:
+        continue
+
+      d = forward_distances[i]
+
+      a_diff = (a_ego - TARGET_ACCEL)
+      accel_t = abs(a_diff / TARGET_JERK)
+      min_accel_v = calculate_velocity(accel_t, TARGET_JERK, a_ego, v_ego)
+
+      max_d = 0
+      if tv > min_accel_v:
+        # calculate time needed based on target jerk
+        a = 0.5 * TARGET_JERK
+        b = a_ego
+        c = v_ego - tv
+        t_a = -1 * ((b**2 - 4 * a * c) ** 0.5 + b) / 2 * a
+        t_b = ((b**2 - 4 * a * c) ** 0.5 - b) / 2 * a
+        if not isinstance(t_a, complex) and t_a > 0:
+          t = t_a
+        else:
+          t = t_b
+        if isinstance(t, complex):
+          continue
+
+        max_d = max_d + calculate_distance(t, TARGET_JERK, a_ego, v_ego)
+
+      else:
+        t = accel_t
+        max_d = calculate_distance(t, TARGET_JERK, a_ego, v_ego)
+
+        # calculate additional time needed based on target accel
+        t = abs((min_accel_v - tv) / TARGET_ACCEL)
+        max_d += calculate_distance(t, 0, TARGET_ACCEL, min_accel_v)
+
+      if d < max_d + tv * TARGET_OFFSET:
+        valid_velocities.append((float(tv), tlat, tlon))
+
+    # Find the smallest velocity we need to adjust for
+    min_v = 100.0
+    target_lat = 0.0
+    target_lon = 0.0
+    for tv, lat, lon in valid_velocities:
+      if tv < min_v:
+        min_v = tv
+        target_lat = lat
+        target_lon = lon
+
+    if self.target_v < min_v and not (self.target_lat == 0 and self.target_lon == 0):
+      for i in range(len(forward_points)):
+        target_velocity = forward_points[i]
+        tlat = target_velocity["latitude"]
+        tlon = target_velocity["longitude"]
+        tv = target_velocity["velocity"]
+        if tv > v_ego:
+          continue
+
+        if tlat == self.target_lat and tlon == self.target_lon and tv == self.target_v:
+          return float(self.target_v)
+      # not found so lets reset
+      self.target_v = 0.0
+      self.target_lat = 0.0
+      self.target_lon = 0.0
+
+    self.target_v = min_v
+    self.target_lat = target_lat
+    self.target_lon = target_lon
+
+    return min_v

selfdrive/frogpilot/ui/control_settings.cc

@@ -33,6 +33,12 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
     {"AggressiveAcceleration", "Aggressive Acceleration With Lead", "Increase acceleration aggressiveness when following a lead vehicle from a stop.", ""},
     {"StoppingDistance", "Increase Stop Distance Behind Lead", "Increase the stopping distance for a more comfortable stop from lead vehicles.", ""},
 
+    {"MTSCEnabled", "Map Turn Speed Control", "Slow down for anticipated curves detected by your downloaded maps.", "../frogpilot/assets/toggle_icons/icon_speed_map.png"},
+    {"DisableMTSCSmoothing", "Disable MTSC UI Smoothing", "Disables the smoothing for the requested speed in the onroad UI.", ""},
+    {"MTSCCurvatureCheck", "Model Curvature Detection Failsafe", "Only trigger MTSC when the model detects a curve in the road. Purely used as a failsafe to prevent false positives. Leave this off if you never experience false positives.", ""},
+    {"MTSCLimit", "Speed Change Hard Cap", "Set a hard cap for MTSC. If MTSC requests a speed decrease greater than this value, it ignores the requested speed from MTSC. Purely used as a failsafe to prevent false positives. Leave this off if you never experience false positives.", ""},
+    {"MTSCAggressiveness", "Turn Speed Aggressiveness", "Set turn speed aggressiveness. Higher values result in faster turns, lower values yield gentler turns.\n\nA change of +- 1% results in the velocity being raised or lowered by about 1 mph.", ""},
+
     {"QOLControls", "Quality of Life", "Miscellaneous quality of life changes to improve your overall openpilot experience.", "../frogpilot/assets/toggle_icons/quality_of_life.png"},
     {"DisableOnroadUploads", "Disable Onroad Uploads", "Prevent large data uploads when onroad.", ""},
     {"HigherBitrate", "Higher Bitrate Recording", "Increases the quality of the footage uploaded to comma connect.", ""},
@@ -213,6 +219,20 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
     } else if (param == "StoppingDistance") {
       toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 10, std::map<int, QString>(), this, false, " feet");
 
+    } else if (param == "MTSCEnabled") {
+      FrogPilotParamManageControl *mtscToggle = new FrogPilotParamManageControl(param, title, desc, icon, this);
+      QObject::connect(mtscToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() {
+        parentToggleClicked();
+        for (auto &[key, toggle] : toggles) {
+          toggle->setVisible(mtscKeys.find(key.c_str()) != mtscKeys.end());
+        }
+      });
+      toggle = mtscToggle;
+    } else if (param == "MTSCAggressiveness") {
+      toggle = new FrogPilotParamValueControl(param, title, desc, icon, 1, 200, std::map<int, QString>(), this, false, "%");
+    } else if (param == "MTSCLimit") {
+      toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 99, std::map<int, QString>(), this, false, " mph");
+
     } else if (param == "QOLControls") {
       FrogPilotParamManageControl *qolToggle = new FrogPilotParamManageControl(param, title, desc, icon, this);
       QObject::connect(qolToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() {
@@ -309,17 +329,24 @@ void FrogPilotControlsPanel::updateMetric() {
     double speedConversion = isMetric ? MILE_TO_KM : KM_TO_MILE;
     params.putIntNonBlocking("CESpeed", std::nearbyint(params.getInt("CESpeed") * speedConversion));
     params.putIntNonBlocking("CESpeedLead", std::nearbyint(params.getInt("CESpeedLead") * speedConversion));
+    params.putIntNonBlocking("MTSCLimit", std::nearbyint(params.getInt("MTSCLimit") * speedConversion));
     params.putIntNonBlocking("StoppingDistance", std::nearbyint(params.getInt("StoppingDistance") * distanceConversion));
   }
 
+  FrogPilotParamValueControl *mtscLimitToggle = static_cast<FrogPilotParamValueControl*>(toggles["MTSCLimit"]);
   FrogPilotParamValueControl *stoppingDistanceToggle = static_cast<FrogPilotParamValueControl*>(toggles["StoppingDistance"]);
 
   if (isMetric) {
+    mtscLimitToggle->updateControl(0, 99, " kph");
+
     stoppingDistanceToggle->updateControl(0, 5, " meters");
   } else {
+    mtscLimitToggle->updateControl(0, 99, " mph");
+
     stoppingDistanceToggle->updateControl(0, 10, " feet");
   }
 
+  mtscLimitToggle->refresh();
   stoppingDistanceToggle->refresh();
 
   previousIsMetric = isMetric;

selfdrive/frogpilot/ui/control_settings.h

@@ -41,7 +41,7 @@ private:
   std::set<QString> laneChangeKeys = {};
   std::set<QString> lateralTuneKeys = {"ForceAutoTune"};
   std::set<QString> longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "AggressiveAcceleration", "StoppingDistance"};
-  std::set<QString> mtscKeys = {};
+  std::set<QString> mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck", "MTSCLimit"};
   std::set<QString> qolKeys = {"DisableOnroadUploads", "HigherBitrate", "ReverseCruise"};
   std::set<QString> speedLimitControllerKeys = {};
   std::set<QString> speedLimitControllerControlsKeys = {};