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

@@ -11,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
@@ -204,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/classic_modeld/constants.py

@@ -34,7 +34,6 @@ class ModelConstants:
   DISENGAGE_WIDTH = 5
   POSE_WIDTH = 6
   WIDE_FROM_DEVICE_WIDTH = 3
-  SIM_POSE_WIDTH = 6
   LEAD_WIDTH = 4
   LANE_LINES_WIDTH = 2
   ROAD_EDGES_WIDTH = 2

selfdrive/classic_modeld/fill_model_msg.py

@@ -99,19 +99,19 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str,
     if i < 4:
       fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1])
     elif i == 4:
-      if lane_probs[0] > 0:
+      if lane_probs[0] > 0.5:
         leftLane_x = 0.5 * (net_output_data['lane_lines'][0,0,:,0] + net_output_data['lane_lines'][0,1,:,0])
         leftLane_y = 0.5 * (net_output_data['lane_lines'][0,0,:,1] + net_output_data['lane_lines'][0,1,:,1])
         fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), leftLane_x, leftLane_y)
       else:
-        fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), np.empty(ModelConstants.IDX_N), np.empty(ModelConstants.IDX_N))
+        fill_xyzt(lane_line, PLAN_T_IDXS, np.empty((0,)), np.empty((0,)), np.empty((0,)))
     elif i == 5:
-      if lane_probs[3] > 0:
+      if lane_probs[3] > 0.5:
         rightLane_x = 0.5 * (net_output_data['lane_lines'][0,2,:,0] + net_output_data['lane_lines'][0,3,:,0])
         rightLane_y = 0.5 * (net_output_data['lane_lines'][0,2,:,1] + net_output_data['lane_lines'][0,3,:,1])
         fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), rightLane_x, rightLane_y)
       else:
-        fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), np.empty(ModelConstants.IDX_N), np.empty(ModelConstants.IDX_N))
+        fill_xyzt(lane_line, PLAN_T_IDXS, np.empty((0,)), np.empty((0,)), np.empty((0,)))
   modelV2.laneLineStds = net_output_data['lane_lines_stds'][0,:,0,0].tolist()
   modelV2.laneLineProbs = lane_probs
 
@@ -155,10 +155,10 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str,
 
   # temporal pose
   temporal_pose = modelV2.temporalPose
-  temporal_pose.trans = net_output_data['sim_pose'][0,:3].tolist()
-  temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:3].tolist()
-  temporal_pose.rot = net_output_data['sim_pose'][0,3:].tolist()
-  temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,3:].tolist()
+  temporal_pose.trans = net_output_data['sim_pose'][0,:ModelConstants.POSE_WIDTH//2].tolist()
+  temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:ModelConstants.POSE_WIDTH//2].tolist()
+  temporal_pose.rot = net_output_data['sim_pose'][0,ModelConstants.POSE_WIDTH//2:].tolist()
+  temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,ModelConstants.POSE_WIDTH//2:].tolist()
 
   # confidence
   if vipc_frame_id % (2*ModelConstants.MODEL_FREQ) == 0:

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,7 +119,8 @@ 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_model(frogpilot_toggles)
+      sentry.capture_fingerprint(frogpilot_toggles, params, frogpilot_tracking.params_tracking)
+      sentry.capture_model(frogpilot_toggles.model_name)
       sentry.capture_user(frogpilot_variables.short_branch)
 
       radarless_model = frogpilot_toggles.radarless_model

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)