diff --git a/cereal/car.capnp b/cereal/car.capnp index 1403a82..20cc323 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -119,6 +119,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { # FrogPilot events frogSteerSaturated @121; + greenLight @122; radarCanErrorDEPRECATED @15; communityFeatureDisallowedDEPRECATED @62; diff --git a/cereal/custom.capnp b/cereal/custom.capnp index b33be7c..5897d39 100644 --- a/cereal/custom.capnp +++ b/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; diff --git a/common/params.cc b/common/params.cc index 8511c2e..b8de58b 100644 --- a/common/params.cc +++ b/common/params.cc @@ -267,6 +267,7 @@ std::unordered_map keys = { {"FullMap", PERSISTENT}, {"GasRegenCmd", PERSISTENT}, {"GoatScream", PERSISTENT}, + {"GreenLightAlert", PERSISTENT}, {"LaneLinesWidth", PERSISTENT}, {"LateralTune", PERSISTENT}, {"LeadInfo", PERSISTENT}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 18291eb..1d5c12f 100755 --- a/selfdrive/controls/controlsd.py +++ b/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 diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index bef97fe..1d92b0f 100755 --- a/selfdrive/controls/lib/events.py +++ b/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.), + }, + } diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_green_light.png b/selfdrive/frogpilot/assets/toggle_icons/icon_green_light.png new file mode 100644 index 0000000..f43b2ed Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/icon_green_light.png differ diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index 0f639c6..a38b310 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/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): diff --git a/selfdrive/frogpilot/ui/visual_settings.cc b/selfdrive/frogpilot/ui/visual_settings.cc index 300a8ef..b74280e 100644 --- a/selfdrive/frogpilot/ui/visual_settings.cc +++ b/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.", ""}, diff --git a/selfdrive/frogpilot/ui/visual_settings.h b/selfdrive/frogpilot/ui/visual_settings.h index 4c5de0d..5115edb 100644 --- a/selfdrive/frogpilot/ui/visual_settings.h +++ b/selfdrive/frogpilot/ui/visual_settings.h @@ -25,7 +25,7 @@ private: void updateToggles(); std::set alertVolumeControlKeys = {"EngageVolume", "DisengageVolume", "RefuseVolume", "PromptVolume", "PromptDistractedVolume", "WarningSoftVolume", "WarningImmediateVolume"}; - std::set customAlertsKeys = {}; + std::set customAlertsKeys = {"GreenLightAlert"}; std::set customOnroadUIKeys = {"AccelerationPath", "AdjacentPath", "BlindSpotPath", "FPSCounter", "LeadInfo"}; std::set customThemeKeys = {"CustomColors", "CustomIcons", "CustomSignals", "CustomSounds"}; std::set modelUIKeys = {"LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength"};