From 445bd6b2ff649fc2c2c4339889b50fe839405f3a Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Sat, 29 Jun 2024 14:34:18 -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 dd1463568..913141f69 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
@@ -197,6 +197,7 @@ class Controls:
     self.onroad_distance_pressed = False
     self.openpilot_crashed_triggered = False
     self.previous_traffic_mode = False
+    self.previously_enabled = False
     self.speed_check = False
     self.speed_limit_changed = False
     self.update_toggles = False
@@ -909,6 +910,10 @@ class Controls:
       t.join()
 
   def update_frogpilot_events(self, CS):
+    if self.frogpilot_toggles.green_light_alert and self.previously_enabled and CS.standstill:
+      if self.sm['frogpilotPlan'].greenLight and not self.sm['longitudinalPlan'].hasLead:
+        self.events.add(EventName.greenLight)
+
     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
@@ -971,6 +976,9 @@ class Controls:
         else:
           self.params.put_bool_nonblocking("ExperimentalMode", not self.experimental_mode)
 
+    self.previously_enabled |= (self.enabled or self.always_on_lateral_active) and CS.vEgo > CRUISING_SPEED
+    self.previously_enabled &= driving_gear
+
     self.speed_check = CS.vEgo >= self.frogpilot_toggles.pause_lateral_below_speed
     self.speed_check |= self.frogpilot_toggles.pause_lateral_below_signal and not (CS.leftBlinker or CS.rightBlinker)
     self.speed_check |= CS.standstill
diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py
index 08ba1631e..8a5b95157 100755
--- a/selfdrive/controls/lib/events.py
+++ b/selfdrive/controls/lib/events.py
@@ -986,6 +986,14 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
     ET.NO_ENTRY: NoEntryAlert("Please don't use the 'Development' branch!"),
   },
 
+  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 6e38b2a81..d9c7a3e6f 100644
--- a/selfdrive/frogpilot/controls/frogpilot_planner.py
+++ b/selfdrive/frogpilot/controls/frogpilot_planner.py
@@ -293,6 +293,8 @@ class FrogPilotPlanner:
 
     frogpilotPlan.conditionalExperimentalActive = bool(self.cem.experimental_mode)
 
+    frogpilotPlan.greenLight = self.model_length > TRAJECTORY_SIZE
+
     frogpilotPlan.laneWidthLeft = self.lane_width_left
     frogpilotPlan.laneWidthRight = self.lane_width_right