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 @122;
+    greenLight @123;
 
     radarCanErrorDEPRECATED @15;
     communityFeatureDisallowedDEPRECATED @62;

cereal/custom.capnp

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

common/params.cc

@@ -272,6 +272,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

@@ -178,6 +178,8 @@ class Controls:
     self.frogpilot_variables = SimpleNamespace()
 
     self.driving_gear = False
+    self.previously_enabled = False
+    self.stopped_for_light_previously = False
 
     ignore = self.sensor_packets + ['testJoystick']
     if SIMULATION:
@@ -530,6 +532,22 @@ class Controls:
       if self.sm['modelV2'].frameDropPerc > 20:
         self.events.add(EventName.modeldLagging)
 
+    # Green light alert
+    if self.green_light_alert:
+      stopped_for_light = frogpilot_plan.redLight and CS.standstill
+      green_light = not stopped_for_light and self.stopped_for_light_previously
+      self.stopped_for_light_previously = stopped_for_light
+
+      self.previously_enabled |= (self.enabled or self.FPCC.alwaysOnLateral) and CS.vEgo > CRUISING_SPEED
+      self.previously_enabled &= self.driving_gear
+
+      green_light &= self.previously_enabled
+      green_light &= not CS.gasPressed
+      green_light &= not self.sm['longitudinalPlan'].hasLead
+
+      if green_light:
+        self.events.add(EventName.greenLight)
+
   def data_sample(self):
     """Receive data from sockets and update carState"""
 
@@ -1017,6 +1035,7 @@ class Controls:
     self.frogpilot_variables.conditional_experimental_mode = self.params.get_bool("ConditionalExperimental")
 
     custom_alerts = self.params.get_bool("CustomAlerts")
+    self.green_light_alert = custom_alerts and self.params.get_bool("GreenLightAlert")
 
     custom_theme = self.params.get_bool("CustomTheme")
     custom_sounds = self.params.get_int("CustomSounds") if custom_theme else 0

selfdrive/controls/lib/events.py

@@ -966,6 +966,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

@@ -53,7 +53,7 @@ class FrogPilotPlanner:
     self.accel_limits = [min_accel, max_accel]
 
     # Update Conditional Experimental Mode
-    if self.conditional_experimental_mode and self.CP.openpilotLongitudinalControl:
+    if self.conditional_experimental_mode and self.CP.openpilotLongitudinalControl or self.green_light_alert and carState.standstill:
       self.cem.update(carState, enabled, sm['frogpilotNavigation'], modelData, sm['radarState'], self.road_curvature, self.stop_distance, mpc.t_follow, v_ego)
 
     # Update the current lane widths
@@ -102,6 +102,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):
@@ -112,6 +114,11 @@ class FrogPilotPlanner:
       self.cem.update_frogpilot_params(self.is_metric, params)
       params.put_bool("ExperimentalMode", True)
 
+    custom_alerts = params.get_bool("CustomAlerts")
+    self.green_light_alert = custom_alerts and params.get_bool("GreenLightAlert")
+    if self.green_light_alert and not self.conditional_experimental_mode:
+      self.cem.update_frogpilot_params(self.is_metric, params)
+
     self.custom_personalities = params.get_bool("CustomPersonalities")
     self.aggressive_follow = params.get_float("AggressiveFollow")
     self.standard_follow = params.get_float("StandardFollow")

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

@@ -29,7 +29,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 = {"DynamicPathWidth", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength"};