FrogPilot community - Moving Average Calculator

Co-Authored-By: Efini <19368997+efini@users.noreply.github.com>
Co-Authored-By: Kumar <36933347+rav4kumar@users.noreply.github.com>

selfdrive/frogpilot/controls/lib/frogpilot_functions.py

@@ -11,6 +11,8 @@ from openpilot.common.params_pyx import Params, UnknownKeyName
 from openpilot.system.hardware import HARDWARE
 from openpilot.system.version import get_build_metadata
 
+from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import THRESHOLD
+
 def calculate_lane_width(lane, current_lane, road_edge):
   current_x, current_y = np.array(current_lane.x), np.array(current_lane.y)
   edge_x, edge_y = np.array(road_edge.x), np.array(road_edge.y)
@@ -40,6 +42,26 @@ def run_cmd(cmd, success_msg, fail_msg):
   except Exception as e:
     print(f"Unexpected error occurred: {e}")
 
+class MovingAverageCalculator:
+  def __init__(self):
+    self.data = []
+    self.total = 0
+
+  def add_data(self, value):
+    if len(self.data) == THRESHOLD:
+      self.total -= self.data.pop(0)
+    self.data.append(value)
+    self.total += value
+
+  def get_moving_average(self):
+    if len(self.data) == 0:
+      return None
+    return self.total / len(self.data)
+
+  def reset_data(self):
+    self.data = []
+    self.total = 0
+
 class FrogPilotFunctions:
   @classmethod
   def backup_frogpilot(cls):

selfdrive/frogpilot/controls/lib/frogpilot_variables.py

@@ -11,6 +11,8 @@ from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_M
 
 CITY_SPEED_LIMIT = 25                   # 55mph is typically the minimum speed for highways
 CRUISING_SPEED = 5                      # Roughly the speed cars go when not touching the gas while in drive
+PROBABILITY = 0.6                       # 60% chance of condition being true
+THRESHOLD = 5                           # Time threshold (0.25s)
 TRAJECTORY_SIZE = ModelConstants.IDX_N  # Minimum path length
 
 class FrogPilotVariables: