From acff272b4b0cbccb1a71c6db5674e5f0339aa4fe Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Tue, 23 Jul 2024 00:29:11 -0700
Subject: [PATCH] Visuals - Custom Alerts - Green light alert

Get an alert when a traffic light changes from red to green.
---
 selfdrive/controls/controlsd.py                   | 10 +++++++++-
 selfdrive/controls/lib/events.py                  |  8 ++++++++
 selfdrive/frogpilot/controls/frogpilot_planner.py |  2 ++
 3 files changed, 19 insertions(+), 1 deletion(-)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 45df49e74..a97f6b25a 100644
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -32,7 +32,7 @@ from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
 
 from openpilot.system.hardware import HARDWARE
 
-from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables
+from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CRUISING_SPEED, FrogPilotVariables
 from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
 
 SOFT_DISABLE_TIME = 3  # seconds
@@ -201,6 +201,7 @@ class Controls:
     self.resume_previously_pressed = False
     self.speed_check = False
     self.speed_limit_changed = False
+    self.stopped_for_light = False
     self.update_toggles = False
 
     self.display_timer = 0
@@ -913,6 +914,13 @@ class Controls:
     if frogpilotPlan.forcingStop:
       self.events.add(EventName.forcingStop)
 
+    if self.frogpilot_toggles.green_light_alert and not self.sm['longitudinalPlan'].hasLead and CS.standstill:
+      if frogpilotPlan.greenLight and self.stopped_for_light:
+        self.events.add(EventName.greenLight)
+      self.stopped_for_light = frogpilotPlan.redLight
+    else:
+      self.stopped_for_light = False
+
     if not self.openpilot_crashed_triggered and os.path.isfile(os.path.join(sentry.CRASHES_DIR, 'error.txt')):
       self.events.add(EventName.openpilotCrashed)
       self.openpilot_crashed_triggered = True
diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py
index ffba83623..2437f7d1b 100755
--- a/selfdrive/controls/lib/events.py
+++ b/selfdrive/controls/lib/events.py
@@ -994,6 +994,14 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
       Priority.MID, VisualAlert.none, AudibleAlert.prompt, 1.),
   },
 
+  EventName.greenLight: {
+    ET.PERMANENT: Alert(
+      "Light turned green",
+      "",
+      AlertStatus.frogpilot, AlertSize.small,
+      Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
+  },
+
   EventName.noLaneAvailable: {
     ET.PERMANENT: no_lane_available_alert,
   },
diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py
index 2cb0d6c3c..de4ef92b7 100644
--- a/selfdrive/frogpilot/controls/frogpilot_planner.py
+++ b/selfdrive/frogpilot/controls/frogpilot_planner.py
@@ -318,6 +318,8 @@ class FrogPilotPlanner:
 
     frogpilotPlan.forcingStop = self.forcing_stop
 
+    frogpilotPlan.greenLight = not self.model_stopped
+
     frogpilotPlan.laneWidthLeft = self.lane_width_left
     frogpilotPlan.laneWidthRight = self.lane_width_right