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: