From 2fbd5714306b092ec017caf43ddd1eb708313a89 Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Sat, 11 May 2024 12:39:20 -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                 | 17 +++++++++++++++++
 selfdrive/controls/lib/events.py                |  8 ++++++++
 .../frogpilot/controls/frogpilot_planner.py     |  3 ++-
 3 files changed, 27 insertions(+), 1 deletion(-)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index a9ab215..81945e4 100644
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -75,6 +75,7 @@ class Controls:
     self.drive_added = False
     self.onroad_distance_pressed = False
     self.openpilot_crashed_triggered = False
+    self.previously_enabled = False
     self.speed_check = False
 
     self.display_timer = 0
@@ -83,6 +84,8 @@ class Controls:
     self.previous_speed_limit = 0
     self.speed_limit_timer = 0
 
+    self.green_light_mac = MovingAverageCalculator()
+
     self.card = CarD(CI)
 
     self.params = Params()
@@ -932,6 +935,17 @@ class Controls:
       self.events.add(EventName.openpilotCrashed)
       self.openpilot_crashed_triggered = True
 
+    if self.frogpilot_toggles.green_light_alert:
+      green_light = not self.sm['frogpilotPlan'].redLight
+      green_light &= not CS.gasPressed
+      green_light &= not self.sm['longitudinalPlan'].hasLead
+      green_light &= self.previously_enabled
+      green_light &= CS.standstill
+
+      self.green_light_mac.add_data(green_light)
+      if self.green_light_mac.get_moving_average() >= PROBABILITY:
+        self.events.add(EventName.greenLight)
+
     if self.frogpilot_toggles.speed_limit_alert or self.frogpilot_toggles.speed_limit_confirmation:
       current_speed_limit = self.sm['frogpilotPlan'].slcSpeedLimit
       desired_speed_limit = self.sm['frogpilotPlan'].unconfirmedSlcSpeedLimit
@@ -1030,6 +1044,9 @@ class Controls:
       else:
         self.params.put_bool_nonblocking("ExperimentalMode", not self.experimental_mode)
 
+    self.previously_enabled |= (self.enabled or self.FPCC.alwaysOnLateral) and CS.vEgo > CRUISING_SPEED
+    self.previously_enabled &= self.driving_gear
+
     signal_check = CS.vEgo >= self.frogpilot_toggles.pause_lateral_below_speed or not (CS.leftBlinker or CS.rightBlinker) or CS.standstill
     self.speed_check = CS.vEgo >= self.frogpilot_toggles.pause_lateral_below_speed or CS.standstill or signal_check and self.frogpilot_toggles.pause_lateral_below_signal
 
diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py
index da3cc7f..f7cf0ee 100755
--- a/selfdrive/controls/lib/events.py
+++ b/selfdrive/controls/lib/events.py
@@ -990,6 +990,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 2a9df74..29eb94b 100644
--- a/selfdrive/frogpilot/controls/frogpilot_planner.py
+++ b/selfdrive/frogpilot/controls/frogpilot_planner.py
@@ -157,7 +157,7 @@ class FrogPilotPlanner:
     self.road_curvature = calculate_road_curvature(modelData, v_ego)
     self.v_cruise = self.update_v_cruise(carState, controlsState, frogpilotCarState, frogpilotNavigation, liveLocationKalman, modelData, v_cruise, v_ego, frogpilot_toggles)
 
-    if frogpilot_toggles.conditional_experimental_mode:
+    if frogpilot_toggles.conditional_experimental_mode or frogpilot_toggles.green_light_alert:
       self.cem.update(carState, controlsState.enabled, frogpilotNavigation, lead_distance, self.lead_one, modelData, self.road_curvature, self.slower_lead, v_ego, v_lead, frogpilot_toggles)
 
     self.frame += 1
@@ -265,6 +265,7 @@ class FrogPilotPlanner:
     frogpilotPlan.adjustedCruise = float(min(self.mtsc_target, self.vtsc_target) * (CV.MS_TO_KPH if frogpilot_toggles.is_metric else CV.MS_TO_MPH))
 
     frogpilotPlan.conditionalExperimental = self.cem.experimental_mode
+    frogpilotPlan.redLight = self.cem.red_light_detected
 
     frogpilotPlan.maxAcceleration = self.max_accel
     frogpilotPlan.minAcceleration = self.min_accel