January 28th, 2025 Update

common/params.cc

@@ -285,6 +285,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"DeviceShutdown", PERSISTENT | FROGPILOT_CONTROLS},
     {"DisableOnroadUploads", PERSISTENT | FROGPILOT_CONTROLS},
     {"DisableOpenpilotLongitudinal", PERSISTENT | FROGPILOT_VEHICLES},
+    {"DiscordUsername", PERSISTENT},
     {"DissolvedOxygenDrives", PERSISTENT | FROGPILOT_CONTROLS},
     {"DissolvedOxygenScore", PERSISTENT | FROGPILOT_CONTROLS},
     {"DistanceIconToDownload", CLEAR_ON_MANAGER_START},
@@ -348,6 +349,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"IconToDownload", CLEAR_ON_MANAGER_START},
     {"IncreasedStoppedDistance", PERSISTENT | FROGPILOT_CONTROLS},
     {"IncreaseThermalLimits", PERSISTENT | FROGPILOT_CONTROLS},
+    {"IssueReported", CLEAR_ON_MANAGER_START},
     {"JerkInfo", PERSISTENT | FROGPILOT_VISUALS},
     {"LaneChangeCustomizations", PERSISTENT | FROGPILOT_CONTROLS},
     {"LaneChangeTime", PERSISTENT | FROGPILOT_CONTROLS},
@@ -488,6 +490,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"SLCPriority1", PERSISTENT | FROGPILOT_CONTROLS},
     {"SLCPriority2", PERSISTENT | FROGPILOT_CONTROLS},
     {"SLCPriority3", PERSISTENT | FROGPILOT_CONTROLS},
+    {"SmartTurnControl", PERSISTENT | FROGPILOT_CONTROLS},
     {"SNGHack", PERSISTENT | FROGPILOT_VEHICLES},
     {"SoundToDownload", CLEAR_ON_MANAGER_START},
     {"SpeedLimitAccepted", CLEAR_ON_MANAGER_START},

selfdrive/car/hyundai/interface.py

@@ -191,6 +191,8 @@ def _update(self, c, frogpilot_toggles):
         *create_button_events(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT),
         *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
       ]
+    else:
+      ret.buttonEvents = create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas})
 
     # On some newer model years, the CANCEL button acts as a pause/resume button based on the PCM state
     # To avoid re-engaging when openpilot cancels, check user engagement intention via buttons

selfdrive/car/toyota/carcontroller.py

@@ -249,8 +249,9 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles):
           pcm_accel_cmd = rate_limit(pcm_accel_cmd, self.prev_accel, ACCEL_WINDDOWN_LIMIT, ACCEL_WINDUP_LIMIT)
         self.prev_accel = pcm_accel_cmd
 
-        # calculate amount of acceleration PCM should apply to reach target, given pitch
-        accel_due_to_pitch = math.sin(self.pitch.x) * ACCELERATION_DUE_TO_GRAVITY
+        # calculate amount of acceleration PCM should apply to reach target, given pitch.
+        # clipped to only include downhill angles, avoids erroneously unsetting PERMIT_BRAKING when stopping on uphills
+        accel_due_to_pitch = math.sin(min(self.pitch.x, 0.0)) * ACCELERATION_DUE_TO_GRAVITY
         # TODO: on uphills this sometimes sets PERMIT_BRAKING low not considering the creep force
         net_acceleration_request = pcm_accel_cmd + accel_due_to_pitch
 

selfdrive/classic_modeld/fill_model_msg.py

@@ -99,14 +99,14 @@ 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.5:
+      if lane_probs[0] > 0:
         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.empty((0,)), np.empty((0,)), np.empty((0,)))
     elif i == 5:
-      if lane_probs[3] > 0.5:
+      if lane_probs[3] > 0:
         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)

selfdrive/frogpilot/controls/lib/frogpilot_vcruise.py

@@ -32,6 +32,7 @@ def __init__(self, FrogPilotPlanner):
     self.slc_offset = 0
     self.slc_target = 0
     self.speed_limit_timer = 0
+    self.stsc_target = 0
     self.tracked_model_length = 0
     self.vtsc_target = 0
 
@@ -62,6 +63,10 @@ def update(self, carControl, carState, controlsState, frogpilotCarControl, frogp
 
     # FrogsGoMoo's Smart Turn Speed Controller
     self.stsc.update(carControl, v_cruise, round(v_ego, 2))
+    if frogpilot_toggles.smart_turn_speed_controller and v_ego > CRUISING_SPEED and carControl.longActive and self.frogpilot_planner.road_curvature_detected:
+      self.stsc_target = self.stsc.stsc_target
+    else:
+      self.stsc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0
 
     # Pfeiferj's Map Turn Speed Controller
     if frogpilot_toggles.map_turn_speed_controller and v_ego > CRUISING_SPEED and carControl.longActive:
@@ -149,12 +154,13 @@ def update(self, carControl, carState, controlsState, frogpilotCarControl, frogp
       self.tracked_model_length = self.frogpilot_planner.model_length
 
       if frogpilot_toggles.speed_limit_controller:
-        targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target + self.slc_offset) - v_ego_diff, self.vtsc_target]
+        targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target + self.slc_offset) - v_ego_diff, self.stsc_target, self.vtsc_target]
       else:
-        targets = [self.mtsc_target, self.vtsc_target]
+        targets = [self.mtsc_target, self.stsc_target, self.vtsc_target]
       v_cruise = float(min([target if target > CRUISING_SPEED else v_cruise for target in targets]))
 
     self.mtsc_target = clip(self.mtsc_target, self.mtsc_target + v_cruise_diff, v_cruise)
+    self.stsc_target = clip(self.stsc_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

@@ -11,6 +11,7 @@
 
 from openpilot.selfdrive.frogpilot.frogpilot_variables import CRUISING_SPEED, PLANNER_TIME, params
 
+CACHE_WINDOW = 1
 CURVATURE_THRESHOLD = 1e-10
 ROUNDING_PRECISION = 10
 
@@ -20,7 +21,40 @@ def __init__(self, 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.last_cached_speed = 0
     self.manual_long_timer = 0
+    self.stsc_target = 0
+
+    self.cached_entries = None
+    self.cached_speeds = None
+
+  def update_cache(self, v_ego):
+    if abs(self.last_cached_speed - v_ego) <= CACHE_WINDOW / 2:
+      return
+
+    speeds_in_range = list(self.data.irange(v_ego - CACHE_WINDOW, v_ego + CACHE_WINDOW))
+    if speeds_in_range:
+      self.cached_entries = np.vstack([self.data[speed] for speed in speeds_in_range])
+      self.cached_speeds = np.array(speeds_in_range)
+    else:
+      self.cached_entries = None
+      self.cached_speeds = None
+
+    self.last_cached_speed = v_ego
+
+  def set_stsc_target(self, carControl, v_cruise, v_ego):
+    if not self.data or v_ego < CRUISING_SPEED or not carControl.longActive:
+      self.stsc_target = v_cruise
+      return
+
+    self.update_cache(v_ego)
+    if self.cached_speeds is None:
+      self.stsc_target = v_cruise
+      return
+
+    road_curvature = round(self.frogpilot_vcruise.frogpilot_planner.road_curvature, ROUNDING_PRECISION)
+    closest_entry = min(self.cached_entries, key=lambda x: abs(x[0] - road_curvature))
+    self.stsc_target = clip((closest_entry[1] / closest_entry[0])**0.5, CRUISING_SPEED, v_cruise)
 
   def update_curvature_data(self, v_ego):
     road_curvature = round(self.frogpilot_vcruise.frogpilot_planner.road_curvature, ROUNDING_PRECISION)
@@ -51,4 +85,5 @@ def update(self, carControl, v_cruise, v_ego):
       self.manual_long_timer = 0
 
     else:
+      self.set_stsc_target(carControl, v_cruise, v_ego)
       self.manual_long_timer = 0

selfdrive/frogpilot/fleetmanager/fleet_manager.py

@@ -171,56 +171,53 @@ def open_error_log(file_name):
 @app.route("/addr_input", methods=['GET', 'POST'])
 def addr_input():
   preload = fleet.preload_favs()
-  SearchInput = fleet.get_SearchInput()
+  search_input = fleet.get_search_input()
   token = fleet.get_public_token()
-  s_token = fleet.get_app_token()
-  gmap_key = fleet.get_gmap_key()
-  lon = float(0.0)
-  lat = float(0.0)
+
+  lon = 0.0
+  lat = 0.0
+
   if request.method == 'POST':
-    valid_addr = False
     postvars = request.form.to_dict()
+    valid_addr = False
     addr, lon, lat, valid_addr, token = fleet.parse_addr(postvars, lon, lat, valid_addr, token)
+
     if not valid_addr:
-      # If address is not found, try searching
-      postvars = request.form.to_dict()
       addr = request.form.get('addr_val')
       addr, lon, lat, valid_addr, token = fleet.search_addr(postvars, lon, lat, valid_addr, token)
+
     if valid_addr:
-      # If a valid address is found, redirect to nav_confirmation
       return redirect(url_for('nav_confirmation', addr=addr, lon=lon, lat=lat))
-    else:
-      return render_template("error.html")
-  elif has_prime():
+    return render_template("error.html")
+
+  if has_prime():
     return render_template("prime.html")
-  # amap stuff
-  elif SearchInput == 1:
-    amap_key, amap_key_2 = fleet.get_amap_key()
-    if amap_key == "" or amap_key is None or amap_key_2 == "" or amap_key_2 is None:
-      return redirect(url_for('amap_key_input'))
-    elif token == "" or token is None:
+
+  if search_input == 0:
+    if fleet.get_public_token() is None:
       return redirect(url_for('public_token_input'))
-    elif s_token == "" or s_token is None:
+
+    if fleet.get_secret_token() is None:
       return redirect(url_for('app_token_input'))
-    else:
-      return redirect(url_for('amap_addr_input'))
-  elif fleet.get_nav_active():
-    if SearchInput == 2:
-      return render_template("nonprime.html", gmap_key=gmap_key, lon=lon, lat=lat, home=preload[0], work=preload[1], fav1=preload[2], fav2=preload[3], fav3=preload[4])
-    else:
-      return render_template("nonprime.html", gmap_key=None, lon=None, lat=None, home=preload[0], work=preload[1], fav1=preload[2], fav2=preload[3], fav3=preload[4])
-  elif token == "" or token is None:
-    return redirect(url_for('public_token_input'))
-  elif s_token == "" or s_token is None:
-    return redirect(url_for('app_token_input'))
-  elif SearchInput == 2:
+
+  if search_input == 1:
+    amap_key, amap_key_2 = fleet.get_amap_key()
+    if not amap_key or not amap_key_2:
+      return redirect(url_for('amap_key_input'))
+    return redirect(url_for('amap_addr_input'))
+
+  if search_input == 2:
+    gmap_key = fleet.get_gmap_key()
     lon, lat = fleet.get_last_lon_lat()
-    if gmap_key == "" or gmap_key is None:
+
+    if not gmap_key:
       return redirect(url_for('gmap_key_input'))
-    else:
-      return render_template("addr.html", gmap_key=gmap_key, lon=lon, lat=lat, home=preload[0], work=preload[1], fav1=preload[2], fav2=preload[3], fav3=preload[4])
-  else:
-      return render_template("addr.html", gmap_key=None, lon=None, lat=None, home=preload[0], work=preload[1], fav1=preload[2], fav2=preload[3], fav3=preload[4])
+    return render_template("addr.html", gmap_key=gmap_key, lon=lon, lat=lat, home=preload[0], work=preload[1], fav1=preload[2], fav2=preload[3], fav3=preload[4])
+
+  if fleet.get_nav_active():
+    return render_template("nonprime.html", gmap_key=None, lon=None, lat=None, home=preload[0], work=preload[1], fav1=preload[2], fav2=preload[3], fav3=preload[4])
+
+  return render_template("addr.html", gmap_key=None, lon=None, lat=None, home=preload[0], work=preload[1], fav1=preload[2], fav2=preload[3], fav3=preload[4])
 
 @app.route("/nav_confirmation", methods=['GET', 'POST'])
 def nav_confirmation():
@@ -328,6 +325,14 @@ def get_toggle_values_route():
   toggle_values = fleet.get_all_toggle_values()
   return jsonify(toggle_values)
 
+@app.route("/reset_toggle_values", methods=['POST'])
+def reset_toggle_values_route():
+  try:
+    fleet.reset_toggle_values()
+    return jsonify({"message": "Toggles reset successfully! Rebooting..."}), 200
+  except Exception as error:
+    return jsonify({"error": "Failed to reset toggles...", "details": str(error)}), 400
+
 @app.route("/store_toggle_values", methods=['POST'])
 def store_toggle_values_route():
   try:
@@ -353,6 +358,14 @@ def unlock_doors_route():
   except Exception as error:
     return jsonify({"error": "Failed to unlock doors...", "details": str(error)}), 400
 
+@app.route("/reboot_device", methods=['POST'])
+def reboot_device_route():
+  try:
+    fleet.reboot_device()
+    return jsonify({"message": "Successfully rebooted!"}), 200
+  except Exception as error:
+    return jsonify({"error": "Failed to reboot...", "details": str(error)}), 400
+
 def main():
   try:
     set_core_affinity([0, 1, 2, 3])

selfdrive/frogpilot/fleetmanager/helpers.py

@@ -35,7 +35,7 @@
 
 from openpilot.common.params import ParamKeyType
 from openpilot.selfdrive.car.toyota.carcontroller import LOCK_CMD, UNLOCK_CMD
-from openpilot.system.hardware import PC
+from openpilot.system.hardware import HARDWARE, PC
 from openpilot.system.hardware.hw import Paths
 from openpilot.system.loggerd.uploader import listdir_by_creation
 from openpilot.system.loggerd.xattr_cache import getxattr
@@ -227,23 +227,24 @@ def get_nav_active():
     return False
 
 def get_amap_key():
-  return (
-    token.strip() if (token := params.get("AMapKey1", encoding='utf8')) != "0" else None,
-    token2.strip() if (token2 := params.get("AMapKey2", encoding='utf8')) != "0" else None
-  )
+  token = params.get("AMapKey1", encoding='utf8')
+  token2 = params.get("AMapKey2", encoding='utf8')
+  return (token.strip() if token else None, token2.strip() if token2 else None)
 
 def get_gmap_key():
-  return token.strip() if (token := params.get("GMapKey", encoding='utf8')) != "0" else None
+  token = params.get("GMapKey", encoding='utf8')
+  return token.strip() if token else None
 
 def get_public_token():
-  return token.strip() if (token := params.get("MapboxPublicKey", encoding='utf8')).startswith("pk") else None
+  token = params.get("MapboxPublicKey", encoding='utf8')
+  return token.strip() if token and token.startswith("pk") else None
 
-def get_app_token():
-  return token.strip() if (token := params.get("MapboxSecretKey", encoding='utf8')).startswith("sk") else None
+def get_secret_token():
+  token = params.get("MapboxSecretKey", encoding='utf8')
+  return token.strip() if token and token.startswith("sk") else None
 
-def get_SearchInput():
-  SearchInput = params.get_int("SearchInput")
-  return SearchInput
+def get_search_input():
+  return params.get_int("SearchInput")
 
 def get_last_lon_lat():
   last_pos = params.get("LastGPSPosition")
@@ -467,6 +468,10 @@ def get_all_toggle_values():
 
   return encode_parameters(toggle_values)
 
+def reset_toggle_values():
+  params.put_bool("DoToggleReset", True)
+  HARDWARE.reboot()
+
 def store_toggle_values(request_data):
   excluded_keys = [
     "ApiCache_NavDestinations", "CalibrationParams", "CarParamsPersistent",
@@ -495,3 +500,6 @@ def unlock_doors():
   panda.can_send(0x750, UNLOCK_CMD, 0)
   panda.set_safety_mode(panda.SAFETY_TOYOTA)
   panda.send_heartbeat()
+
+def reboot_device():
+  HARDWARE.reboot()