Green light alert

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

cereal/custom.capnp

@@ -19,6 +19,7 @@ struct FrogPilotDeviceState @0xaedffd8f31e7b55d {
 
 enum FrogPilotEvents @0xf35cc4560bbf6ec2 {
   frogSteerSaturated @0;
+  greenLight @1;
 }
 
 struct FrogPilotLateralPlan @0xda96579883444c35 {
@@ -30,6 +31,7 @@ struct FrogPilotLongitudinalPlan @0x80ae746ee2596b11 {
   conditionalExperimental @1 :Bool;
   desiredFollowDistance @2 :Int16;
   distances @3 :List(Float32);
+  redLight @4 :Bool;
   safeObstacleDistance @5 :Int16;
   safeObstacleDistanceStock @6 :Int16;
   stoppedEquivalenceFactor @11 :Int16;

common/params.cc

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

selfdrive/controls/controlsd.py

@@ -82,6 +82,8 @@ class Controls:
     fire_the_babysitter = self.params.get_bool("FireTheBabysitter")
     mute_dm = fire_the_babysitter and self.params.get_bool("MuteDM")
 
+    self.stopped_for_light_previously = False
+
     ignore = self.sensor_packets + ['testJoystick']
     if SIMULATION:
       ignore += ['driverCameraState', 'managerState']
@@ -448,6 +450,15 @@ class Controls:
       if self.sm['modelV2'].frameDropPerc > 20:
         self.events.add(EventName.modeldLagging)
 
+    # Green light alert
+    if self.green_light_alert and self.enabled:
+      stopped_for_light = self.sm['frogpilotLongitudinalPlan'].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:
+        self.events.add(FrogPilotEventName.greenLight)
+
   def data_sample(self):
     """Receive data from sockets and update carState"""
 
@@ -964,6 +975,8 @@ class Controls:
     frog_sounds = custom_sounds == 1
     self.goat_scream = self.params.get_bool("GoatScream") and frog_sounds
 
+    self.green_light_alert = self.params.get_bool("GreenLightAlert")
+
     longitudinal_tune = self.params.get_bool("LongitudinalTune")
     self.sport_plus = self.params.get_int("AccelerationProfile") == 3 and longitudinal_tune
 

selfdrive/controls/lib/events.py

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

selfdrive/frogpilot/functions/frogpilot_planner.py

@@ -105,6 +105,7 @@ class FrogPilotPlanner:
 
     frogpilotLongitudinalPlan.conditionalExperimental = self.cem.experimental_mode
     frogpilotLongitudinalPlan.distances = self.x_desired_trajectory.tolist()
+    frogpilotLongitudinalPlan.redLight = bool(self.cem.red_light_detected)
 
     frogpilotLongitudinalPlan.safeObstacleDistance = mpc.safe_obstacle_distance
     frogpilotLongitudinalPlan.stoppedEquivalenceFactor = mpc.stopped_equivalence_factor

selfdrive/frogpilot/ui/visual_settings.cc

@@ -18,6 +18,7 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot
     {"LeadInfo", "Lead Info and Logics", "Get detailed information about the vehicle ahead, including speed and distance, and the logic behind your following distance.", ""},
 
     {"DriverCamera", "Driver Camera On Reverse", "Show the driver's camera feed when you shift to reverse.", "../assets/img_driver_face_static.png"},
+    {"GreenLightAlert", "Green Light Alert", "Get an alert when a traffic light changes from red to green.", "../frogpilot/assets/toggle_icons/icon_green_light.png"},
 
     {"ModelUI", "Model UI", "Personalize how the model's visualizations appear on your screen.", "../assets/offroad/icon_calibration.png"},
     {"AccelerationPath", "Acceleration Path", "Visualize the car's intended acceleration or deceleration with a color-coded path.", ""},