January 27th, 2025 Update

common/params.cc

@@ -378,20 +378,19 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"MapSpeedLimit", CLEAR_ON_MANAGER_START},
     {"MapStyle", PERSISTENT | FROGPILOT_VISUALS},
     {"MapTargetVelocities", CLEAR_ON_MANAGER_START},
+    {"MapTurnControl", PERSISTENT | FROGPILOT_CONTROLS},
     {"MaxDesiredAcceleration", PERSISTENT | FROGPILOT_CONTROLS},
     {"MinimumBackupSize", PERSISTENT},
     {"MinimumLaneChangeSpeed", PERSISTENT | FROGPILOT_CONTROLS},
     {"Model", PERSISTENT | FROGPILOT_CONTROLS},
     {"ModelDrivesAndScores", PERSISTENT | FROGPILOT_CONTROLS},
     {"ModelDownloadProgress", CLEAR_ON_MANAGER_START},
-    {"ModelName", PERSISTENT | FROGPILOT_CONTROLS},
     {"ModelRandomizer", PERSISTENT | FROGPILOT_CONTROLS},
     {"ModelToDownload", CLEAR_ON_MANAGER_START},
     {"ModelUI", PERSISTENT | FROGPILOT_VISUALS},
     {"ModelVersion", PERSISTENT | FROGPILOT_CONTROLS},
     {"ModelVersions", PERSISTENT | FROGPILOT_CONTROLS},
     {"MTSCCurvatureCheck", PERSISTENT | FROGPILOT_CONTROLS},
-    {"MTSCEnabled", PERSISTENT | FROGPILOT_CONTROLS},
     {"NavigationUI", PERSISTENT | FROGPILOT_VISUALS},
     {"NextMapSpeedLimit", CLEAR_ON_MANAGER_START},
     {"NewLongAPI", PERSISTENT | FROGPILOT_VEHICLES},
@@ -536,6 +535,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"UnlockDoors", PERSISTENT | FROGPILOT_VEHICLES},
     {"Updated", PERSISTENT},
     {"UpdateWheelImage", CLEAR_ON_MANAGER_START},
+    {"UserCurvature", PERSISTENT},
     {"UseSI", PERSISTENT | FROGPILOT_VISUALS},
     {"UseStockColors", CLEAR_ON_MANAGER_START},
     {"UseVienna", PERSISTENT | FROGPILOT_CONTROLS},

selfdrive/car/car_helpers.py

@@ -1,5 +1,4 @@
 import os
-import threading
 import time
 from collections.abc import Callable
 
@@ -12,7 +11,6 @@
 from openpilot.selfdrive.car.mock.values import CAR as MOCK
 from openpilot.common.swaglog import cloudlog
 import cereal.messaging as messaging
-import openpilot.system.sentry as sentry
 from openpilot.selfdrive.car import gen_empty_fingerprint
 
 FRAME_FINGERPRINT = 100  # 1s
@@ -205,10 +203,6 @@ def get_car(logcan, sendcan, disable_openpilot_long, experimental_long_allowed,
 
   if frogpilot_toggles.block_user:
     candidate = "MOCK"
-    sentry.capture_fingerprint(candidate, params, blocked_user=True)
-  elif candidate != "MOCK" and not params.get_bool("FingerprintLogged"):
-    sentry.capture_fingerprint(candidate, params)
-    params.put_bool("FingerprintLogged", True)
 
   CarInterface, _, _ = interfaces[candidate]
   CP = CarInterface.get_params(candidate, fingerprints, car_fw, disable_openpilot_long, experimental_long_allowed, params, docs=False)

selfdrive/controls/lib/latcontrol_pid.py

@@ -10,7 +10,7 @@ def __init__(self, CP, CI):
     super().__init__(CP, CI)
     self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
                              (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
-                             k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max, lateral_pid=True)
+                             k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max)
     self.get_steer_feedforward = CI.get_steer_feedforward_function()
 
   def reset(self):
@@ -36,9 +36,8 @@ def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk,
       # offset does not contribute to resistive torque
       steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
 
-      #self.pid._k_p = frogpilot_toggles.steer_kp
       output_steer = self.pid.update(error, override=CS.steeringPressed,
-                                     feedforward=steer_feedforward, speed=CS.vEgo, frogpilot_toggles=frogpilot_toggles)
+                                     feedforward=steer_feedforward, speed=CS.vEgo)
       pid_log.active = True
       pid_log.p = self.pid.p
       pid_log.i = self.pid.i

selfdrive/controls/lib/latcontrol_torque.py

@@ -68,7 +68,7 @@ def __init__(self, CP, CI):
     super().__init__(CP, CI)
     self.torque_params = CP.lateralTuning.torque
     self.pid = PIDController(self.torque_params.kp, self.torque_params.ki,
-                             k_f=self.torque_params.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max, lateral_pid=True)
+                             k_f=self.torque_params.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max)
     self.torque_from_lateral_accel = CI.torque_from_lateral_accel()
     self.use_steering_angle = self.torque_params.useSteeringAngle
     self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
@@ -247,7 +247,7 @@ def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk,
       output_torque = self.pid.update(pid_log.error,
                                       feedforward=ff,
                                       speed=CS.vEgo,
-                                      freeze_integrator=freeze_integrator, frogpilot_toggles=frogpilot_toggles)
+                                      freeze_integrator=freeze_integrator)
 
       pid_log.active = True
       pid_log.p = self.pid.p

selfdrive/controls/lib/longcontrol.py

@@ -125,7 +125,7 @@ def update(self, active, CS, a_target, should_stop, accel_limits, frogpilot_togg
     else:  # LongCtrlState.pid
       error = a_target - CS.aEgo
       output_accel = self.pid.update(error, speed=CS.vEgo,
-                                     feedforward=a_target, frogpilot_toggles=frogpilot_toggles)
+                                     feedforward=a_target)
 
     self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
     return self.last_output_accel

selfdrive/controls/lib/longitudinal_planner.py

@@ -227,12 +227,12 @@ def update(self, radarless_model, sm, frogpilot_toggles):
     # No change cost when user is controlling the speed, or when standstill
     prev_accel_constraint = not (reset_state or sm['carState'].standstill)
 
-    accel_limits = [sm['frogpilotPlan'].minAcceleration, sm['frogpilotPlan'].maxAcceleration]
     if self.mpc.mode == 'acc':
       accel_limits = [sm['frogpilotPlan'].minAcceleration, sm['frogpilotPlan'].maxAcceleration]
       steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
       accel_limits_turns = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_limits, self.CP)
     else:
+      accel_limits = [ACCEL_MIN, ACCEL_MAX]
       accel_limits_turns = [ACCEL_MIN, ACCEL_MAX]
 
     if reset_state:

selfdrive/controls/lib/pid.py

@@ -5,7 +5,7 @@
 
 
 class PIDController:
-  def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100, lateral_pid=False, longitudinal_pid=False):
+  def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100):
     self._k_p = k_p
     self._k_i = k_i
     self._k_d = k_d
@@ -26,10 +26,6 @@ def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308,
 
     self.reset()
 
-    # FrogPilot variables
-    self.lateral_pid = lateral_pid
-    self.longitudinal_pid = longitudinal_pid
-
   @property
   def k_p(self):
     return interp(self.speed, self._k_p[0], self._k_p[1])
@@ -53,13 +49,10 @@ def reset(self):
     self.f = 0.0
     self.control = 0
 
-  def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False, frogpilot_toggles=None):
+  def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False):
     self.speed = speed
 
-    if self.lateral_pid:
-      self.p = float(error) * (frogpilot_toggles.steer_kp if frogpilot_toggles.use_custom_kp else self.k_p)
-    else:
-      self.p = float(error) * self.k_p
+    self.p = float(error) * self.k_p
     self.f = feedforward * self.k_f
     self.d = error_rate * self.k_d
 

selfdrive/frogpilot/controls/lib/frogpilot_vcruise.py

@@ -7,6 +7,7 @@
 from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_UNSET
 
 from openpilot.selfdrive.frogpilot.controls.lib.map_turn_speed_controller import MapTurnSpeedController
+from openpilot.selfdrive.frogpilot.controls.lib.smart_turn_speed_controller import SmartTurnSpeedController
 from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
 from openpilot.selfdrive.frogpilot.frogpilot_variables import CRUISING_SPEED, PLANNER_TIME, params_memory
 
@@ -18,6 +19,7 @@ def __init__(self, FrogPilotPlanner):
 
     self.mtsc = MapTurnSpeedController()
     self.slc = SpeedLimitController()
+    self.stsc = SmartTurnSpeedController(self)
 
     self.forcing_stop = False
     self.override_force_stop = False
@@ -58,6 +60,9 @@ def update(self, carControl, carState, controlsState, frogpilotCarControl, frogp
     v_ego_cluster = max(carState.vEgoCluster, v_ego)
     v_ego_diff = v_ego_cluster - v_ego
 
+    # FrogsGoMoo's Smart Turn Speed Controller
+    self.stsc.update(carControl, v_cruise, round(v_ego, 2))
+
     # Pfeiferj's Map Turn Speed Controller
     if frogpilot_toggles.map_turn_speed_controller and v_ego > CRUISING_SPEED and carControl.longActive:
       mtsc_active = self.mtsc_target < v_cruise
@@ -120,7 +125,7 @@ def update(self, carControl, carState, controlsState, frogpilotCarControl, frogp
       self.slc_target = 0
 
     # Pfeiferj's Vision Turn Controller
-    if frogpilot_toggles.vision_turn_controller and v_ego > CRUISING_SPEED and carControl.longActive:
+    if frogpilot_toggles.vision_turn_speed_controller and v_ego > CRUISING_SPEED and carControl.longActive and self.frogpilot_planner.road_curvature_detected:
       self.vtsc_target = ((TARGET_LAT_A * frogpilot_toggles.turn_aggressiveness) / (self.frogpilot_planner.road_curvature * frogpilot_toggles.curve_sensitivity))**0.5
       self.vtsc_target = clip(self.vtsc_target, CRUISING_SPEED, v_cruise)
     else:
@@ -149,7 +154,7 @@ def update(self, carControl, carState, controlsState, frogpilotCarControl, frogp
         targets = [self.mtsc_target, self.vtsc_target]
       v_cruise = float(min([target if target > CRUISING_SPEED else v_cruise for target in targets]))
 
-    self.mtsc_target += v_cruise_diff
-    self.vtsc_target += v_cruise_diff
+    self.mtsc_target = clip(self.mtsc_target, self.mtsc_target + v_cruise_diff, v_cruise)
+    self.vtsc_target = clip(self.vtsc_target, self.mtsc_target + v_cruise_diff, v_cruise)
 
     return v_cruise

selfdrive/frogpilot/controls/lib/smart_turn_speed_controller.py

@@ -0,0 +1,54 @@
+#!/usr/bin/env python3
+import json
+import numpy as np
+
+from sortedcontainers import SortedDict
+
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.numpy_fast import clip
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
+
+from openpilot.selfdrive.frogpilot.frogpilot_variables import CRUISING_SPEED, PLANNER_TIME, params
+
+CURVATURE_THRESHOLD = 1e-10
+ROUNDING_PRECISION = 10
+
+class SmartTurnSpeedController:
+  def __init__(self, FrogPilotVCruise):
+    self.frogpilot_vcruise = FrogPilotVCruise
+
+    self.data = SortedDict({entry["speed"]: [np.array([curve["curvature"], curve["lateral_accel"]]) for curve in entry.get("curvatures", [])] for entry in json.loads(params.get("UserCurvature") or "[]")})
+
+    self.manual_long_timer = 0
+
+  def update_curvature_data(self, v_ego):
+    road_curvature = round(self.frogpilot_vcruise.frogpilot_planner.road_curvature, ROUNDING_PRECISION)
+    lateral_accel = round(v_ego**2 * road_curvature, ROUNDING_PRECISION)
+
+    if abs(road_curvature) < CURVATURE_THRESHOLD or abs(lateral_accel) < 1:
+      return
+
+    entries = self.data.setdefault(v_ego, [])
+    for i, entry in enumerate(entries):
+      if abs(entry[0] - road_curvature) < CURVATURE_THRESHOLD:
+        entries[i][1] = (entry[1] + lateral_accel) / 2
+        return
+
+    entries.append(np.array([road_curvature, lateral_accel]))
+
+  def update(self, carControl, v_cruise, v_ego):
+    if not carControl.longActive and V_CRUISE_MAX * CV.KPH_TO_MS >= v_ego > CRUISING_SPEED and not self.frogpilot_vcruise.frogpilot_planner.tracking_lead:
+      if self.manual_long_timer >= PLANNER_TIME:
+        self.update_curvature_data(v_ego)
+      self.manual_long_timer += DT_MDL
+
+    elif self.manual_long_timer >= PLANNER_TIME:
+      params.put_nonblocking("UserCurvature", json.dumps([
+        {"speed": speed, "curvatures": [{"curvature": entry[0], "lateral_accel": entry[1]} for entry in entries]}
+        for speed, entries in self.data.items()
+      ]))
+      self.manual_long_timer = 0
+
+    else:
+      self.manual_long_timer = 0

selfdrive/frogpilot/frogpilot_process.py

@@ -119,6 +119,7 @@ def frogpilot_thread():
       if frogpilot_toggles.lock_doors_timer != 0:
         run_thread_with_lock("lock_doors", lock_doors, (frogpilot_toggles.lock_doors_timer, sm))
     elif started and not started_previously:
+      sentry.capture_fingerprint(frogpilot_toggles, params, frogpilot_tracking.params_tracking)
       sentry.capture_model(frogpilot_toggles)
       sentry.capture_user(frogpilot_variables.short_branch)
 

selfdrive/frogpilot/frogpilot_utilities.py

@@ -104,7 +104,7 @@ def extract_zip(zip_file, extract_path):
   print(f"Extracting {zip_file} to {extract_path}")
 
   try:
-    with zipfile.ZipFile(zip_file, 'r') as zip_ref:
+    with zipfile.ZipFile(zip_file, "r") as zip_ref:
       zip_ref.extractall(extract_path)
     zip_file.unlink()
     print(f"Extraction completed: {zip_file} has been removed")
@@ -121,9 +121,9 @@ def is_url_pingable(url, timeout=5):
     request = urllib.request.Request(
       url,
       headers={
-        'User-Agent': 'Mozilla/5.0 (compatible; Python urllib)',
-        'Accept': '*/*',
-        'Connection': 'keep-alive'
+        "User-Agent": "Mozilla/5.0 (compatible; Python urllib)",
+        "Accept": "*/*",
+        "Connection": "keep-alive"
       }
     )
     urllib.request.urlopen(request, timeout=timeout)
@@ -143,13 +143,13 @@ def is_url_pingable(url, timeout=5):
     return False
 
 def lock_doors(lock_doors_timer, sm):
-  while any(proc.name == "dmonitoringd" and proc.running for proc in sm['managerState'].processes):
+  while any(proc.name == "dmonitoringd" and proc.running for proc in sm["managerState"].processes):
     time.sleep(DT_HW)
     sm.update()
 
   params.put_bool("IsDriverViewEnabled", True)
 
-  while not any(proc.name == "dmonitoringd" and proc.running for proc in sm['managerState'].processes):
+  while not any(proc.name == "dmonitoringd" and proc.running for proc in sm["managerState"].processes):
     time.sleep(DT_HW)
     sm.update()
 
@@ -159,10 +159,10 @@ def lock_doors(lock_doors_timer, sm):
     if elapsed_time >= lock_doors_timer:
       break
 
-    if any(ps.ignitionLine or ps.ignitionCan for ps in sm['pandaStates'] if ps.pandaType != log.PandaState.PandaType.unknown):
+    if any(ps.ignitionLine or ps.ignitionCan for ps in sm["pandaStates"] if ps.pandaType != log.PandaState.PandaType.unknown):
       break
 
-    if sm['driverMonitoringState'].faceDetected or not sm.alive['driverMonitoringState']:
+    if sm["driverMonitoringState"].faceDetected or not sm.alive["driverMonitoringState"]:
       start_time = time.monotonic()
 
     time.sleep(DT_DMON)
@@ -189,7 +189,7 @@ def update_maps(now):
   while not MAPD_PATH.exists():
     time.sleep(60)
 
-  maps_selected = json.loads(params.get("MapsSelected", encoding='utf8') or "{}")
+  maps_selected = json.loads(params.get("MapsSelected", encoding="utf8") or "{}")
   if not maps_selected.get("nations") and not maps_selected.get("states"):
     return
 
@@ -205,13 +205,13 @@ def update_maps(now):
   suffix = "th" if 4 <= day <= 20 or 24 <= day <= 30 else ["st", "nd", "rd"][day % 10 - 1]
   todays_date = now.strftime(f"%B {day}{suffix}, %Y")
 
-  if maps_downloaded and params.get("LastMapsUpdate", encoding='utf-8') == todays_date:
+  if maps_downloaded and params.get("LastMapsUpdate", encoding="utf-8") == todays_date:
     return
 
-  if params.get("OSMDownloadProgress", encoding='utf-8') is None:
+  if params.get("OSMDownloadProgress", encoding="utf-8") is None:
     params_memory.put("OSMDownloadLocations", json.dumps(maps_selected))
 
-  while params.get("OSMDownloadProgress", encoding='utf-8') is not None:
+  while params.get("OSMDownloadProgress", encoding="utf-8") is not None:
     time.sleep(60)
 
   params.put("LastMapsUpdate", todays_date)