Green light alert

Added toggle to alert the user when a red light turns to green.

cereal/car.capnp

@@ -119,6 +119,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
 
     # FrogPilot events
     frogSteerSaturated @121;
+    greenLight @122;
 
     radarCanErrorDEPRECATED @15;
     communityFeatureDisallowedDEPRECATED @62;

cereal/custom.capnp

@@ -29,6 +29,7 @@ struct FrogPilotPlan @0x80ae746ee2596b11 {
   desiredFollowDistance @2 :Int16;
   laneWidthLeft @3 :Float32;
   laneWidthRight @4 :Float32;
+  redLight @5 :Bool;
   safeObstacleDistance @6 :Int16;
   safeObstacleDistanceStock @7 :Int16;
   stoppedEquivalenceFactor @12 :Int16;

common/params.cc

@@ -267,6 +267,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"FullMap", PERSISTENT},
     {"GasRegenCmd", PERSISTENT},
     {"GoatScream", PERSISTENT},
+    {"GreenLightAlert", PERSISTENT},
     {"LaneLinesWidth", PERSISTENT},
     {"LateralTune", PERSISTENT},
     {"LeadInfo", PERSISTENT},

selfdrive/controls/controlsd.py

@@ -84,6 +84,7 @@ class Controls:
     self.frogpilot_variables = SimpleNamespace()
 
     self.driving_gear = False
+    self.stopped_for_light_previously = False
 
     ignore = self.sensor_packets + ['testJoystick']
     if SIMULATION:
@@ -443,6 +444,15 @@ class Controls:
       if self.sm['modelV2'].frameDropPerc > 20:
         self.events.add(EventName.modeldLagging)
 
+    # Green light alert
+    if self.green_light_alert:
+      stopped_for_light = self.sm['frogpilotPlan'].redLight and CS.standstill
+      green_light = not stopped_for_light and self.stopped_for_light_previously
+      self.stopped_for_light_previously = stopped_for_light
+
+      if green_light and not CS.gasPressed and not self.sm['longitudinalPlan'].hasLead:
+        self.events.add(EventName.greenLight)
+
   def data_sample(self):
     """Receive data from sockets and update carState"""
 
@@ -945,6 +955,7 @@ class Controls:
     self.conditional_experimental_mode = self.params.get_bool("ConditionalExperimental")
 
     custom_alerts = self.params.get_bool("CustomAlerts")
+    self.green_light_alert = self.params.get_bool("GreenLightAlert") and custom_alerts
 
     custom_theme = self.params.get_bool("CustomTheme")
     custom_sounds = self.params.get_int("CustomSounds") if custom_theme else 0

selfdrive/controls/lib/events.py

@@ -967,6 +967,14 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
       Priority.LOW, VisualAlert.steerRequired, AudibleAlert.warningSoft, 2.),
   },
 
+  EventName.greenLight: {
+    ET.PERMANENT: Alert(
+      "Light turned green",
+      "",
+      AlertStatus.frogpilot, AlertSize.small,
+      Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
+  },
+
 }
 
 

selfdrive/frogpilot/functions/frogpilot_planner.py

@@ -69,6 +69,8 @@ class FrogPilotPlanner:
     frogpilotPlan.laneWidthLeft = self.lane_width_left
     frogpilotPlan.laneWidthRight = self.lane_width_right
 
+    frogpilotPlan.redLight = self.cem.red_light_detected
+
     pm.send('frogpilotPlan', frogpilot_plan_send)
 
   def update_frogpilot_params(self, params, params_memory):

selfdrive/frogpilot/ui/visual_settings.cc

@@ -20,6 +20,7 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot
     {"CameraView", "Camera View", "Choose your preferred camera view for the onroad UI. This is a visual change only and doesn't impact openpilot.", "../frogpilot/assets/toggle_icons/icon_camera.png"},
 
     {"CustomAlerts", "Custom Alerts", "Enable custom alerts for various logic or situational changes.", "../frogpilot/assets/toggle_icons/icon_green_light.png"},
+    {"GreenLightAlert", "Green Light Alert", "Get an alert when a traffic light changes from red to green.", ""},
 
     {"CustomUI", "Custom Onroad UI", "Customize the Onroad UI with some additional visual functions.", "../assets/offroad/icon_road.png"},
     {"AccelerationPath", "Acceleration Path", "Visualize the car's intended acceleration or deceleration with a color-coded path.", ""},

selfdrive/frogpilot/ui/visual_settings.h

@@ -25,7 +25,7 @@ private:
   void updateToggles();
 
   std::set<QString> alertVolumeControlKeys = {"EngageVolume", "DisengageVolume", "RefuseVolume", "PromptVolume", "PromptDistractedVolume", "WarningSoftVolume", "WarningImmediateVolume"};
-  std::set<QString> customAlertsKeys = {};
+  std::set<QString> customAlertsKeys = {"GreenLightAlert"};
   std::set<QString> customOnroadUIKeys = {"AccelerationPath", "AdjacentPath", "BlindSpotPath", "FPSCounter", "LeadInfo"};
   std::set<QString> customThemeKeys = {"CustomColors", "CustomIcons", "CustomSignals", "CustomSounds"};
   std::set<QString> modelUIKeys = {"LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength"};