January 1st, 2025 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:
 
-**December 21st, 2024**
+**January 1st, 2025**
 
 Features
 ------

cereal/custom.capnp

@@ -63,26 +63,23 @@ struct FrogPilotPlan @0x80ae746ee2596b11 {
   minAcceleration @12 :Float32;
   mtscSpeed @13 :Float32;
   redLight @14 :Bool;
-  safeObstacleDistance @15 :Int64;
-  safeObstacleDistanceStock @16 :Int64;
-  slcMapSpeedLimit @17 :Float32;
-  slcOverridden @18 :Bool;
-  slcOverriddenSpeed @19 :Float32;
-  slcSpeedLimit @20 :Float32;
-  slcSpeedLimitOffset @21 :Float32;
-  slcSpeedLimitSource @22 :Text;
-  speedJerk @23 :Float32;
-  speedJerkStock @24 :Float32;
-  speedLimitChanged @25 :Bool;
-  stoppedEquivalenceFactor @26 :Int64;
-  tFollow @27 :Float32;
-  themeUpdated @28 :Bool;
-  togglesUpdated @29 :Bool;
-  unconfirmedSlcSpeedLimit @30 :Float32;
-  upcomingSLCSpeedLimit @31 :Float32;
-  vCruise @32 :Float32;
-  vtscControllingCurve @33 :Bool;
-  vtscSpeed @34 :Float32;
+  slcMapSpeedLimit @15 :Float32;
+  slcOverridden @16 :Bool;
+  slcOverriddenSpeed @17 :Float32;
+  slcSpeedLimit @18 :Float32;
+  slcSpeedLimitOffset @19 :Float32;
+  slcSpeedLimitSource @20 :Text;
+  speedJerk @21 :Float32;
+  speedJerkStock @22 :Float32;
+  speedLimitChanged @23 :Bool;
+  tFollow @24 :Float32;
+  themeUpdated @25 :Bool;
+  togglesUpdated @26 :Bool;
+  unconfirmedSlcSpeedLimit @27 :Float32;
+  upcomingSLCSpeedLimit @28 :Float32;
+  vCruise @29 :Float32;
+  vtscControllingCurve @30 :Bool;
+  vtscSpeed @31 :Float32;
 }
 
 struct CustomReserved5 @0xa5cd762cd951a455 {

common/params.cc

@@ -339,8 +339,6 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"GoatScream", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},
     {"GreenLightAlert", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},
     {"HideAlerts", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},
-    {"HideAOLStatusBar", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
-    {"HideCEMStatusBar", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
     {"HideCSCUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
     {"HideLeadMarker", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},
     {"HideMapIcon", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},
@@ -467,6 +465,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"SecretGoodOpenpilotScore", PERSISTENT | FROGPILOT_CONTROLS},
     {"SetSpeedLimit", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
     {"SetSpeedOffset", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
+    {"ShowCEMStatus", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},
     {"ShowCPU", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},
     {"ShowGPU", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},
     {"ShowIP", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},
@@ -485,7 +484,6 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"SLCConfirmation", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
     {"SLCConfirmationHigher", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
     {"SLCConfirmationLower", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
-    {"SLCConfirmed", PERSISTENT},
     {"SLCLookaheadHigher", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
     {"SLCLookaheadLower", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
     {"SLCFallback", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
@@ -495,6 +493,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"SLCPriority3", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
     {"SNGHack", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES},
     {"SoundToDownload", PERSISTENT},
+    {"SpeedLimitAccepted", PERSISTENT},
     {"SpeedLimitChangedAlert", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
     {"SpeedLimitController", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
     {"SpeedLimitSources", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},

selfdrive/car/toyota/carcontroller.py

@@ -100,7 +100,6 @@ def __init__(self, dbc_name, CP, VM):
     self.secoc_lka_message_counter = 0
     self.secoc_lta_message_counter = 0
     self.secoc_prev_reset_counter = 0
-    self.secoc_mismatch_counter = 0
 
     # FrogPilot variables
     self.stock_max_accel = self.params.ACCEL_MAX
@@ -148,9 +147,8 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles):
         self.secoc_prev_reset_counter = CS.secoc_synchronization['RESET_CNT']
 
         expected_mac = build_sync_mac(self.secoc_key, int(CS.secoc_synchronization['TRIP_CNT']), int(CS.secoc_synchronization['RESET_CNT']))
-        if int(CS.secoc_synchronization['AUTHENTICATOR']) != expected_mac and self.secoc_mismatch_counter < 100:
+        if int(CS.secoc_synchronization['AUTHENTICATOR']) != expected_mac:
           carlog.error("SecOC synchronization MAC mismatch, wrong key?")
-          self.secoc_mismatch_counter += 1
 
     # *** steer torque ***
     new_steer = int(round(actuators.steer * self.params.STEER_MAX))

selfdrive/controls/lib/longitudinal_planner.py

@@ -52,6 +52,27 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
   return [a_target[0], min(a_target[1], a_x_allowed)]
 
 
+def get_accel_from_plan_classic(CP, speeds, accels):
+  if len(speeds) == CONTROL_N:
+    v_target_now = interp(DT_MDL, CONTROL_N_T_IDX, speeds)
+    a_target_now = interp(DT_MDL, CONTROL_N_T_IDX, accels)
+
+    v_target = interp(CP.longitudinalActuatorDelay + DT_MDL, CONTROL_N_T_IDX, speeds)
+    if v_target != v_target_now:
+      a_target = 2 * (v_target - v_target_now) / CP.longitudinalActuatorDelay - a_target_now
+    else:
+      a_target = a_target_now
+
+    v_target_1sec = interp(CP.longitudinalActuatorDelay + DT_MDL + 1.0, CONTROL_N_T_IDX, speeds)
+  else:
+    v_target = 0.0
+    v_target_1sec = 0.0
+    a_target = 0.0
+  should_stop = (v_target < CP.vEgoStopping and
+                 v_target_1sec < CP.vEgoStopping)
+  return a_target, should_stop
+
+
 def get_accel_from_plan(speeds, accels, action_t=DT_MDL, vEgoStopping=0.05):
   if len(speeds) == CONTROL_N:
     v_now = speeds[0]
@@ -273,7 +294,7 @@ def update(self, classic_model, radarless_model, sm, frogpilot_toggles):
     self.a_desired = float(interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory))
     self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0
 
-  def publish(self, sm, pm):
+  def publish(self, classic_model, sm, pm):
     plan_send = messaging.new_message('longitudinalPlan')
 
     plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
@@ -291,9 +312,12 @@ def publish(self, sm, pm):
     longitudinalPlan.longitudinalPlanSource = self.mpc.source
     longitudinalPlan.fcw = self.fcw
 
-    action_t =  self.CP.longitudinalActuatorDelay + DT_MDL
-    a_target, should_stop = get_accel_from_plan(longitudinalPlan.speeds, longitudinalPlan.accels,
-                                                action_t=action_t, vEgoStopping=self.CP.vEgoStopping)
+    if classic_model:
+      a_target, should_stop = get_accel_from_plan_classic(self.CP, longitudinalPlan.speeds, longitudinalPlan.accels)
+    else:
+      action_t = self.CP.longitudinalActuatorDelay + DT_MDL
+      a_target, should_stop = get_accel_from_plan(longitudinalPlan.speeds, longitudinalPlan.accels,
+                                                  action_t=action_t, vEgoStopping=self.CP.vEgoStopping)
     longitudinalPlan.aTarget = a_target
     longitudinalPlan.shouldStop = should_stop
     longitudinalPlan.allowBrake = True

selfdrive/controls/plannerd.py

@@ -44,7 +44,7 @@ def plannerd_thread():
     sm.update()
     if sm.updated['modelV2']:
       longitudinal_planner.update(classic_model, radarless_model, sm, frogpilot_toggles)
-      longitudinal_planner.publish(sm, pm)
+      longitudinal_planner.publish(classic_model, sm, pm)
       publish_ui_plan(sm, pm, longitudinal_planner)
 
     # Update FrogPilot parameters

selfdrive/frogpilot/assets/theme_manager.py

@@ -349,23 +349,23 @@ def validate_themes(self, downloadable_colors, downloadable_distance_icons, down
       if theme_name in {"none", "stock"}:
         continue
 
+      if theme_name.replace('_', ' ').split('.')[0].title() not in downloadable_list:
+        print(f"  {theme_name} for {theme_component} is outdated. Deleting...")
+        delete_file(theme_path)
+        continue
+
       if theme_component == "steering_wheels":
         matching_files = list(THEME_SAVE_PATH.joinpath(theme_component).glob(f"{theme_name}.*"))
         if not matching_files:
-          continue
-        theme_path = matching_files[0]
+          print(f"  {theme_name} for {theme_component} not found. Downloading...")
+          self.download_theme(theme_component, theme_name, theme_param)
+          update_frogpilot_toggles()
       else:
         theme_path = THEME_SAVE_PATH / "theme_packs" / theme_name / theme_component
-
-      if theme_name.replace('_', ' ').split('.')[0].title() not in downloadable_list:
-        print(f"{theme_name} for {theme_component} is outdated. Deleting...")
-        delete_file(theme_path)
-        continue
-
-      if not theme_path.exists():
-        print(f"{theme_name} for {theme_component} not found. Downloading...")
-        self.download_theme(theme_component, theme_name, theme_param)
-        update_frogpilot_toggles()
+        if not theme_path.exists():
+          print(f"  {theme_name} for {theme_component} not found. Downloading...")
+          self.download_theme(theme_component, theme_name, theme_param)
+          update_frogpilot_toggles()
 
     for dir_path in THEME_SAVE_PATH.glob('**/*'):
       if dir_path.is_dir() and not any(dir_path.iterdir()):
@@ -375,6 +375,8 @@ def validate_themes(self, downloadable_colors, downloadable_distance_icons, down
         print(f"Deleting temp file: {dir_path}")
         dir_path.unlink()
 
+    print("Theme validation complete.")
+
   def update_themes(self, frogpilot_toggles, boot_run=False):
     if self.downloading_theme:
       return

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -81,15 +81,13 @@ def update(self, carControl, carState, controlsState, frogpilotCarControl, frogp
 
     self.road_curvature = calculate_road_curvature(modelData, v_ego) if not carState.standstill else 1
 
-    self.tracking_lead = self.set_lead_status(frogpilotCarState, v_ego, frogpilot_toggles)
+    self.tracking_lead = self.set_lead_status(carState, v_lead)
     self.v_cruise = self.frogpilot_vcruise.update(carControl, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, v_cruise, v_ego, frogpilot_toggles)
 
-  def set_lead_status(self, frogpilotCarState, v_ego, frogpilot_toggles):
-    distance_offset = frogpilot_toggles.increased_stopped_distance if not frogpilotCarState.trafficModeActive else 0
-
+  def set_lead_status(self, carState, v_lead):
     following_lead = self.lead_one.status
-    following_lead &= 1 < self.lead_one.dRel < self.model_length + STOP_DISTANCE + distance_offset
-    following_lead &= v_ego > CRUISING_SPEED or self.tracking_lead
+    following_lead &= self.lead_one.dRel < self.model_length + STOP_DISTANCE
+    following_lead &= not carState.standstill or self.tracking_lead
 
     self.tracking_lead_mac.add_data(following_lead)
     return self.tracking_lead_mac.get_moving_average() >= THRESHOLD
@@ -110,10 +108,7 @@ def publish(self, sm, pm, theme_updated, toggles_updated, frogpilot_toggles):
     frogpilotPlan.vtscControllingCurve = bool(self.frogpilot_vcruise.mtsc_target > self.frogpilot_vcruise.vtsc_target)
     frogpilotPlan.vtscSpeed = float(self.frogpilot_vcruise.vtsc_target)
 
-    frogpilotPlan.desiredFollowDistance = self.frogpilot_following.safe_obstacle_distance - self.frogpilot_following.stopped_equivalence_factor
-    frogpilotPlan.safeObstacleDistance = self.frogpilot_following.safe_obstacle_distance
-    frogpilotPlan.safeObstacleDistanceStock = self.frogpilot_following.safe_obstacle_distance_stock
-    frogpilotPlan.stoppedEquivalenceFactor = self.frogpilot_following.stopped_equivalence_factor
+    frogpilotPlan.desiredFollowDistance = self.frogpilot_following.desired_follow_distance
 
     frogpilotPlan.experimentalMode = self.cem.experimental_mode or self.frogpilot_vcruise.slc.experimental_mode
 
@@ -136,7 +131,7 @@ def publish(self, sm, pm, theme_updated, toggles_updated, frogpilot_toggles):
     frogpilotPlan.slcOverridden = bool(self.frogpilot_vcruise.override_slc)
     frogpilotPlan.slcOverriddenSpeed = float(self.frogpilot_vcruise.overridden_speed)
     frogpilotPlan.slcSpeedLimit = self.frogpilot_vcruise.slc_target
-    frogpilotPlan.slcSpeedLimitOffset = self.frogpilot_vcruise.slc.offset
+    frogpilotPlan.slcSpeedLimitOffset = self.frogpilot_vcruise.slc_offset
     frogpilotPlan.slcSpeedLimitSource = self.frogpilot_vcruise.slc.source
     frogpilotPlan.speedLimitChanged = self.frogpilot_vcruise.speed_limit_changed
     frogpilotPlan.unconfirmedSlcSpeedLimit = self.frogpilot_vcruise.slc.desired_speed_limit

selfdrive/frogpilot/controls/lib/frogpilot_events.py

@@ -61,7 +61,7 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState
       self.stopped_for_light = False
 
     if not self.holiday_theme_played and frogpilot_toggles.current_holiday_theme != "stock" and self.frame >= 10:
-      #self.events.add(EventName.holidayActive)
+      self.events.add(EventName.holidayActive)
       self.holiday_theme_played = True
 
     if frogpilot_toggles.lead_departing_alert and self.frogpilot_planner.tracking_lead and carState.standstill:
@@ -177,7 +177,7 @@ def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState
           self.random_event_played = True
       self.always_on_lateral_active_previously = frogpilotCarControl.alwaysOnLateralActive
 
-    if frogpilot_toggles.speed_limit_changed_alert and self.frogpilot_planner.frogpilot_vcruise.speed_limit_changed:
+    if frogpilot_toggles.speed_limit_changed_alert and self.frogpilot_planner.frogpilot_vcruise.speed_limit_changed and self.frogpilot_planner.frogpilot_vcruise.speed_limit_timer < 1:
       self.events.add(EventName.speedLimitChanged)
 
     if 5 > self.frame > 4 and params.get("NNFFModelName", encoding='utf-8') is not None:

selfdrive/frogpilot/controls/lib/frogpilot_following.py

@@ -1,5 +1,5 @@
 from openpilot.common.numpy_fast import clip, interp
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, STOP_DISTANCE, get_jerk_factor, get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, STOP_DISTANCE, desired_follow_distance, get_jerk_factor, get_T_FOLLOW
 
 from openpilot.selfdrive.frogpilot.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED
 
@@ -16,10 +16,8 @@ def __init__(self, FrogPilotPlanner):
     self.base_acceleration_jerk = 0
     self.base_speed_jerk = 0
     self.danger_jerk = 0
-    self.safe_obstacle_distance = 0
-    self.safe_obstacle_distance_stock = 0
+    self.desired_follow_distance = 0
     self.speed_jerk = 0
-    self.stopped_equivalence_factor = 0
     self.t_follow = 0
 
   def update(self, aEgo, controlsState, frogpilotCarState, lead_distance, v_ego, v_lead, frogpilot_toggles):
@@ -63,14 +61,10 @@ def update(self, aEgo, controlsState, frogpilotCarState, lead_distance, v_ego, v
     self.following_lead = self.frogpilot_planner.tracking_lead and lead_distance < (self.t_follow + 1) * v_ego
 
     if self.frogpilot_planner.tracking_lead:
-      self.safe_obstacle_distance = int(get_safe_obstacle_distance(v_ego, self.t_follow))
-      self.safe_obstacle_distance_stock = self.safe_obstacle_distance
-      self.stopped_equivalence_factor = int(get_stopped_equivalence_factor(v_lead))
       self.update_follow_values(lead_distance, v_ego, v_lead, frogpilot_toggles)
+      self.desired_follow_distance = int(desired_follow_distance(v_ego, v_lead, self.t_follow))
     else:
-      self.safe_obstacle_distance = 0
-      self.safe_obstacle_distance_stock = 0
-      self.stopped_equivalence_factor = 0
+      self.desired_follow_distance = 0
 
   def update_follow_values(self, lead_distance, v_ego, v_lead, frogpilot_toggles):
     # Offset by FrogAi for FrogPilot for a more natural approach to a faster lead