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

@@ -328,6 +328,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 +361,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
@@ -467,6 +467,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 +499,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()

selfdrive/frogpilot/fleetmanager/templates/tools.html

@@ -75,7 +75,7 @@
 
   <div class="separator"></div>
 
-  <h1>Toggle Values</h1>
+  <h1>Backup/Restore Toggle Values</h1>
   <div class="textarea-container">
     <textarea id="toggleValuesBox" placeholder="Retrieve or restore toggle values."></textarea>
     <button type="button" id="retrieveButton" class="toggle-values-btn">
@@ -85,15 +85,54 @@ <h1>Toggle Values</h1>
       Submit Toggle Values
     </button>
   </div>
+
+  <div class="separator"></div>
+
+  <h1>Reset Toggle Values</h1>
+  <div class="textarea-container">
+    <button type="button" id="resetButton" class="toggle-values-btn">
+      Reset Toggle Values
+    </button>
+  </div>
+
+  <div class="separator"></div>
+
+  <h1>Reboot Device</h1>
+  <div class="textarea-container">
+    <button type="button" id="rebootButton" class="toggle-values-btn">
+      Reboot Device
+    </button>
+  </div>
 </div>
 
 <script>
   const toggleValuesBox = document.getElementById('toggleValuesBox');
+  const rebootButton = document.getElementById('rebootButton');
   const retrieveButton = document.getElementById('retrieveButton');
+  const resetButton = document.getElementById('resetButton');
   const restoreButton = document.getElementById('restoreButton');
   const lockButton = document.getElementById('lockButton');
   const unlockButton = document.getElementById('unlockButton');
 
+  rebootButton.addEventListener('click', () => {
+    fetch('/reboot_device', {
+      method: 'POST'
+    })
+      .then(response =>
+        response.json().then(data => ({ status: response.status, body: data }))
+      )
+      .then(result => {
+        if (result.status === 200) {
+          alert(result.body.message);
+        } else {
+          alert('Error rebooting device: ' + result.body.error);
+        }
+      })
+      .catch(error =>
+        console.error('Error rebooting device:', error)
+      );
+  });
+
   retrieveButton.addEventListener('click', () => {
     fetch('/get_toggle_values')
       .then(response => response.text())
@@ -105,6 +144,25 @@ <h1>Toggle Values</h1>
       );
   });
 
+  resetButton.addEventListener('click', () => {
+    fetch('/reset_toggle_values', {
+      method: 'POST'
+    })
+      .then(response =>
+        response.json().then(data => ({ status: response.status, body: data }))
+      )
+      .then(result => {
+        if (result.status === 200) {
+          alert(result.body.message);
+        } else {
+          alert('Error resetting toggle values: ' + result.body.error);
+        }
+      })
+      .catch(error =>
+        console.error('Error resetting toggle values:', error)
+      );
+  });
+
   restoreButton.addEventListener('click', () => {
     const inputData = toggleValuesBox.value;
     fetch('/store_toggle_values', {

selfdrive/frogpilot/frogpilot_process.py

@@ -1,5 +1,6 @@
 #!/usr/bin/env python3
 import datetime
+import json
 import time
 
 import openpilot.system.sentry as sentry
@@ -17,20 +18,25 @@
 from openpilot.selfdrive.frogpilot.controls.frogpilot_planner import FrogPilotPlanner
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_tracking import FrogPilotTracking
 from openpilot.selfdrive.frogpilot.frogpilot_functions import backup_toggles
-from openpilot.selfdrive.frogpilot.frogpilot_utilities import flash_panda, is_url_pingable, lock_doors, run_thread_with_lock, update_maps, update_openpilot
+from openpilot.selfdrive.frogpilot.frogpilot_utilities import flash_panda, is_url_pingable, lock_doors, run_thread_with_lock, send_sentry_reports, update_maps, update_openpilot
 from openpilot.selfdrive.frogpilot.frogpilot_variables import FrogPilotVariables, get_frogpilot_toggles, params, params_memory
 
 def assets_checks(model_manager, theme_manager):
-  if params_memory.get_bool("FlashPanda"):
-    run_thread_with_lock("flash_panda", flash_panda)
-
   if params_memory.get_bool("DownloadAllModels"):
     run_thread_with_lock("download_all_models", model_manager.download_all_models)
 
+  if params_memory.get_bool("FlashPanda"):
+    run_thread_with_lock("flash_panda", flash_panda)
+
   model_to_download = params_memory.get(MODEL_DOWNLOAD_PARAM, encoding='utf-8')
   if model_to_download is not None:
     run_thread_with_lock("download_model", model_manager.download_model, (model_to_download,))
 
+  report_data = json.loads(params_memory.get("IssueReported", encoding="utf-8") or "{}")
+  if report_data:
+    sentry.capture_report(report_data["DiscordUser"], report_data["Issue"], vars(get_frogpilot_toggles()))
+    params_memory.remove("IssueReported")
+
   assets = [
     ("ColorToDownload", "colors"),
     ("DistanceIconToDownload", "distance_icons"),
@@ -119,9 +125,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.model_name)
-      sentry.capture_user(frogpilot_variables.short_branch)
+      run_thread_with_lock("send_sentry_reports", send_sentry_reports, (frogpilot_toggles, frogpilot_variables, params, frogpilot_tracking.params_tracking))
 
       radarless_model = frogpilot_toggles.radarless_model
 
@@ -152,7 +156,7 @@ def frogpilot_thread():
     manually_updated = params_memory.get_bool("ManualUpdateInitiated")
 
     run_update_checks |= manually_updated
-    run_update_checks |= now.second == 0 and (now.minute % 60 == 0 or now.minute % 5 == 0 and frogpilot_toggles.frogs_go_moo)
+    run_update_checks |= now.second == 0 and (now.minute % 60 == 0 or (now.minute % 5 == 0 and frogpilot_toggles.frogs_go_moo))
     run_update_checks &= time_validated
 
     if run_update_checks: