From dbe6ba6ae1d734e19bd8cfe57c6510c23ee0bf34 Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Fri, 7 Jun 2024 06:16:18 -0700
Subject: [PATCH] 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>
---
 .../controls/lib/frogpilot_functions.py       | 22 +++++++++++++++++++
 .../controls/lib/frogpilot_variables.py       |  2 ++
 2 files changed, 24 insertions(+)

diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_functions.py b/selfdrive/frogpilot/controls/lib/frogpilot_functions.py
index 631cb1554..080175985 100644
--- a/selfdrive/frogpilot/controls/lib/frogpilot_functions.py
+++ b/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):
diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_variables.py b/selfdrive/frogpilot/controls/lib/frogpilot_variables.py
index f5949267d..f199e5e1a 100644
--- a/selfdrive/frogpilot/controls/lib/frogpilot_variables.py
+++ b/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: