October 21st, 2024 Update

README.md

@@ -21,7 +21,7 @@ FrogPilot is a fully open-sourced fork of openpilot, featuring clear and concise
 ------
 FrogPilot was last updated on:
 
-**October 21st, 2024**
+**November 1st, 2024**
 
 Features
 ------

cereal/car.capnp

@@ -137,14 +137,15 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     openpilotCrashedRandomEvent @137;
     pedalInterceptorNoBrake @138;
     speedLimitChanged @139;
-    torqueNNLoad @140;
-    trafficModeActive @141;
-    trafficModeInactive @142;
-    turningLeft @143;
-    turningRight @144;
-    vCruise69 @145;
-    yourFrogTriedToKillMe @146;
-    youveGotMail @147;
+    thisIsFineSteerSaturated @140;
+    torqueNNLoad @141;
+    trafficModeActive @142;
+    trafficModeInactive @143;
+    turningLeft @144;
+    turningRight @145;
+    vCruise69 @146;
+    yourFrogTriedToKillMe @147;
+    youveGotMail @148;
 
     radarCanErrorDEPRECATED @15;
     communityFeatureDisallowedDEPRECATED @62;
@@ -453,7 +454,8 @@ struct CarControl {
       mail @16;
       nessie @17;
       noice @18;
-      uwu @19;
+      thisIsFine @19;
+      uwu @20;
     }
   }
 

cereal/log.capnp

@@ -618,6 +618,10 @@ struct RadarState @0x9a185389d6fdd05f {
 
   leadOne @3 :LeadData;
   leadTwo @4 :LeadData;
+  leadLeft @13 :LeadData;
+  leadRight @14 :LeadData;
+  leadLeftFar @15 :LeadData;
+  leadRightFar @16 :LeadData;
   cumLagMs @5 :Float32;
 
   struct LeadData {

common/params.cc

@@ -211,6 +211,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     // FrogPilot parameters
     {"AccelerationPath", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},
     {"AccelerationProfile", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
+    {"AdjacentLeadsUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
     {"AdjacentPath", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},
     {"AdjacentPathMetrics", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},
     {"AdvancedCustomUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},

selfdrive/controls/lib/events.py

@@ -1091,6 +1091,14 @@ def no_lane_available_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S
       Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 3.),
   },
 
+  EventName.thisIsFineSteerSaturated: {
+    ET.WARNING: Alert(
+      "This is fine",
+      "☕",
+      AlertStatus.userPrompt, AlertSize.mid,
+      Priority.LOW, VisualAlert.steerRequired, AudibleAlert.thisIsFine, 2.),
+  },
+
   EventName.torqueNNLoad: {
     ET.PERMANENT: torque_nn_load_alert,
   },

selfdrive/controls/radard.py

@@ -108,6 +108,19 @@ def get_RadarState(self, model_prob: float = 0.0):
       "radarTrackId": self.identifier,
     }
 
+  def potential_adjacent_lead(self, far: bool, lane_width: float, left: bool, model_data: capnp._DynamicStructReader):
+    adjacent_lane_max = float('inf') if far else lane_width * 1.5
+    adjacent_lane_min = max(lane_width * 1.5, 4.5) if far else max(lane_width * 0.5, 1.5)
+
+    y_delta = self.yRel + interp(self.dRel, model_data.position.x, model_data.position.y)
+
+    if left and adjacent_lane_min < y_delta < adjacent_lane_max:
+      return True
+    elif not left and adjacent_lane_min < -y_delta < adjacent_lane_max:
+      return True
+    else:
+      return False
+
   def potential_low_speed_lead(self, v_ego: float):
     # stop for stuff in front of you and low speed, even without model confirmation
     # Radar points closer than 0.75, are almost always glitches on toyota radars
@@ -193,6 +206,17 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn
   return lead_dict
 
 
+def get_lead_adjacent(tracks: dict[int, Track], model_data: capnp._DynamicStructReader, lane_width: float, left: bool = True, far: bool = False) -> dict[str, Any]:
+  lead_dict = {'status': False}
+
+  adjacent_tracks = [c for c in tracks.values() if c.vLead > 1 and c.potential_adjacent_lead(far, lane_width, left, model_data)]
+  if len(adjacent_tracks) > 0:
+    closest_track = min(adjacent_tracks, key=lambda c: c.dRel)
+    lead_dict = closest_track.get_RadarState()
+
+  return lead_dict
+
+
 class RadarD:
   def __init__(self, radar_ts: float, delay: int = 0):
     self.points: dict[int, tuple[float, float, float]] = {}
@@ -274,6 +298,12 @@ def update(self, sm: messaging.SubMaster, rr):
       self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.frogpilot_toggles.lead_detection_threshold, low_speed_override=True)
       self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.frogpilot_toggles.lead_detection_threshold, low_speed_override=False)
 
+    if self.frogpilot_toggles.adjacent_lead_tracking_ui:
+      self.radar_state.leadLeft = get_lead_adjacent(self.tracks, sm['modelV2'], sm['frogpilotPlan'].laneWidthLeft, left=True)
+      self.radar_state.leadLeftFar = get_lead_adjacent(self.tracks, sm['modelV2'], sm['frogpilotPlan'].laneWidthLeft, left=True, far=True)
+      self.radar_state.leadRight = get_lead_adjacent(self.tracks, sm['modelV2'], sm['frogpilotPlan'].laneWidthRight, left=False)
+      self.radar_state.leadRightFar = get_lead_adjacent(self.tracks, sm['modelV2'], sm['frogpilotPlan'].laneWidthRight, left=False, far=True)
+
     # Update FrogPilot parameters
     if FrogPilotVariables.toggles_updated:
       self.update_toggles = True
@@ -355,7 +385,7 @@ def main():
   FrogPilotVariables.update_frogpilot_params()
 
   if not frogpilot_toggles.radarless_model:
-    sm = messaging.SubMaster(['modelV2', 'carState'], frequency=int(1./DT_CTRL))
+    sm = messaging.SubMaster(['modelV2', 'carState', 'frogpilotPlan'], frequency=int(1./DT_CTRL))
     pm = messaging.PubMaster(['radarState', 'liveTracks'])
     while 1:
       can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)

selfdrive/frogpilot/assets/download_functions.py

@@ -1,7 +1,7 @@
 import os
 import requests
 
-from openpilot.selfdrive.frogpilot.frogpilot_functions import delete_file, is_url_pingable
+from openpilot.selfdrive.frogpilot.frogpilot_utilities import delete_file, is_url_pingable
 
 GITHUB_URL = "https://raw.githubusercontent.com/FrogAi/FrogPilot-Resources/"
 GITLAB_URL = "https://gitlab.com/FrogAi/FrogPilot-Resources/-/raw/"

selfdrive/frogpilot/assets/model_manager.py

@@ -9,7 +9,8 @@
 from openpilot.common.params import Params, UnknownKeyName
 
 from openpilot.selfdrive.frogpilot.assets.download_functions import GITHUB_URL, GITLAB_URL, download_file, get_remote_file_size, get_repository_url, handle_error, handle_request_error, verify_download
-from openpilot.selfdrive.frogpilot.frogpilot_functions import MODELS_PATH, delete_file
+from openpilot.selfdrive.frogpilot.frogpilot_functions import MODELS_PATH
+from openpilot.selfdrive.frogpilot.frogpilot_utilities import delete_file
 
 VERSION = "v9"
 
@@ -176,17 +177,13 @@ def copy_default_model(self):
     if os.path.isfile(source_path) and not os.path.isfile(default_model_path):
       shutil.copyfile(source_path, default_model_path)
       print(f"Copied default model from {source_path} to {default_model_path}")
-    else:
-      print(f"Source default model not found at {source_path}. Exiting...")
 
     sgo_model_path = os.path.join(MODELS_PATH, "secret-good-openpilot.thneed")
     source_path = os.path.join(BASEDIR, "selfdrive", "modeld", "models", "supercombo.thneed")
 
     if os.path.isfile(source_path) and not os.path.isfile(sgo_model_path):
       shutil.copyfile(source_path, sgo_model_path)
       print(f"Copied 'secret-good-openpilot' model from {source_path} to {sgo_model_path}")
-    else:
-      print(f"Source 'secret-good-openpilot' model not found at {source_path}. Exiting...")
 
   def update_models(self, boot_run=False):
     if boot_run:

selfdrive/frogpilot/assets/theme_manager.py

@@ -12,7 +12,8 @@
 from openpilot.common.params import Params
 
 from openpilot.selfdrive.frogpilot.assets.download_functions import GITHUB_URL, GITLAB_URL, download_file, get_repository_url, handle_error, handle_request_error, link_valid, verify_download
-from openpilot.selfdrive.frogpilot.frogpilot_functions import ACTIVE_THEME_PATH, THEME_SAVE_PATH, update_frogpilot_toggles
+from openpilot.selfdrive.frogpilot.frogpilot_functions import ACTIVE_THEME_PATH, THEME_SAVE_PATH
+from openpilot.selfdrive.frogpilot.frogpilot_utilities import update_frogpilot_toggles
 
 CANCEL_DOWNLOAD_PARAM = "CancelThemeDownload"
 DOWNLOAD_PROGRESS_PARAM = "ThemeDownloadProgress"

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -12,7 +12,7 @@
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_events import FrogPilotEvents
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_following import FrogPilotFollowing
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_vcruise import FrogPilotVCruise
-from openpilot.selfdrive.frogpilot.frogpilot_functions import MovingAverageCalculator, calculate_lane_width, calculate_road_curvature, update_frogpilot_toggles
+from openpilot.selfdrive.frogpilot.frogpilot_utilities import MovingAverageCalculator, calculate_lane_width, calculate_road_curvature, update_frogpilot_toggles
 from openpilot.selfdrive.frogpilot.frogpilot_variables import CRUISING_SPEED, MODEL_LENGTH, NON_DRIVING_GEARS, PLANNER_TIME, THRESHOLD
 
 class FrogPilotPlanner:
@@ -73,7 +73,7 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState
     self.frogpilot_following.update(carState.aEgo, controlsState, frogpilotCarState, lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles)
 
     check_lane_width = frogpilot_toggles.adjacent_lanes or frogpilot_toggles.adjacent_path_metrics or frogpilot_toggles.blind_spot_path or frogpilot_toggles.lane_detection
-    if check_lane_width and v_ego >= frogpilot_toggles.minimum_lane_change_speed:
+    if check_lane_width and v_ego >= frogpilot_toggles.minimum_lane_change_speed or frogpilot_toggles.adjacent_lead_tracking_ui:
       self.lane_width_left = calculate_lane_width(modelData.laneLines[0], modelData.laneLines[1], modelData.roadEdges[0])
       self.lane_width_right = calculate_lane_width(modelData.laneLines[3], modelData.laneLines[2], modelData.roadEdges[1])
     else:

selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py

@@ -1,6 +1,6 @@
 from openpilot.common.params import Params
 
-from openpilot.selfdrive.frogpilot.frogpilot_functions import MovingAverageCalculator
+from openpilot.selfdrive.frogpilot.frogpilot_utilities import MovingAverageCalculator
 from openpilot.selfdrive.frogpilot.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, THRESHOLD
 
 class ConditionalExperimentalMode:

selfdrive/frogpilot/controls/lib/frogpilot_acceleration.py

@@ -5,11 +5,11 @@
 
 from openpilot.selfdrive.frogpilot.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED
 
-A_CRUISE_MIN_ECO = A_CRUISE_MIN / 4
-A_CRUISE_MIN_SPORT = A_CRUISE_MIN / 2
+A_CRUISE_MIN_ECO =   A_CRUISE_MIN / 2
+A_CRUISE_MIN_SPORT = A_CRUISE_MIN * 2
 
-                       # MPH = [ 0.,  11,  22,  34,  45,  56,  89]
-A_CRUISE_MAX_BP_CUSTOM =       [ 0.,  5., 10., 15., 20., 25., 40.]
+                       # MPH = [0.0,  11,  22,  34,  45,  56,  89]
+A_CRUISE_MAX_BP_CUSTOM =       [0.0,  5., 10., 15., 20., 25., 40.]
 A_CRUISE_MAX_VALS_ECO =        [2.0, 1.5, 1.0, 0.8, 0.6, 0.4, 0.2]
 A_CRUISE_MAX_VALS_SPORT =      [3.0, 2.5, 2.0, 1.5, 1.0, 0.8, 0.6]
 A_CRUISE_MAX_VALS_SPORT_PLUS = [4.0, 3.5, 3.0, 2.0, 1.0, 0.8, 0.6]
@@ -24,10 +24,7 @@ def get_max_accel_sport_plus(v_ego):
   return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT_PLUS)
 
 def get_max_accel_ramp_off(max_accel, v_cruise, v_ego):
-  return interp(v_ego, [0., v_cruise * 0.5, v_cruise * 0.75, v_cruise], [max_accel, max_accel, max_accel / 2, max_accel / 4])
-
-def get_max_accel_ramp_off_highway(max_accel, v_cruise, v_ego):
-  return interp(v_ego, [0., v_cruise * 0.75, v_cruise], [max_accel, max_accel, max_accel / 2])
+  return interp(v_cruise - v_ego, [0., 1., 5., 10.], [0., 0.1, 1.0, max_accel])
 
 def get_max_allowed_accel(v_ego):
   return interp(v_ego, [0., 5., 20.], [4.0, 4.0, 2.0])  # ISO 15622:2018
@@ -77,10 +74,7 @@ def update(self, controlsState, frogpilotCarState, v_cruise, v_ego, frogpilot_to
     if frogpilot_toggles.human_acceleration:
       if self.frogpilot_planner.frogpilot_following.following_lead and not frogpilotCarState.trafficModeActive:
         self.max_accel = clip(self.frogpilot_planner.lead_one.aLeadK, get_max_accel_sport_plus(v_ego), get_max_allowed_accel(v_ego))
-      if self.frogpilot_planner.v_cruise < CITY_SPEED_LIMIT:
-        self.max_accel = get_max_accel_ramp_off(self.max_accel, self.frogpilot_planner.v_cruise, v_ego)
-      else:
-        self.max_accel = get_max_accel_ramp_off_highway(self.max_accel, self.frogpilot_planner.v_cruise, v_ego)
+      self.max_accel = min(get_max_accel_ramp_off(self.max_accel, self.frogpilot_planner.v_cruise, v_ego), self.max_accel)
 
     self.max_accel = min(self.max_accel, frogpilot_toggles.max_desired_accel)
 

selfdrive/frogpilot/controls/lib/frogpilot_events.py

@@ -33,6 +33,7 @@ def __init__(self, FrogPilotPlanner):
     self.previous_traffic_mode = False
     self.random_event_played = False
     self.stopped_for_light = False
+    self.this_is_fine_played = False
     self.vCruise69_played = False
     self.youveGotMail_played = False
 
@@ -125,6 +126,8 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState
           event_choices.append("firefoxSteerSaturated")
         if not self.goat_played:
           event_choices.append("goatSteerSaturated")
+        if not self.this_is_fine_played:
+          event_choices.append("thisIsFineSteerSaturated")
 
         if self.frame % 100 == 0 and event_choices:
           event_choice = random.choice(event_choices)
@@ -138,6 +141,11 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState
             update_wheel_image("goat")
             self.params_memory.put_bool("UpdateWheelImage", True)
             self.goat_played = True
+          elif event_choice == "thisIsFineSteerSaturated":
+            self.events.add(EventName.thisIsFineSteerSaturated)
+            update_wheel_image("this_is_fine")
+            self.params_memory.put_bool("UpdateWheelImage", True)
+            self.this_is_fine_played = True
           self.random_event_played = True
 
       if not self.vCruise69_played and 70 > v_cruise * (1 if frogpilot_toggles.is_metric else CV.KPH_TO_MPH) >= 69: