diff --git a/cereal/car.capnp b/cereal/car.capnp index 40e86a5..e005e4d 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -119,6 +119,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { # FrogPilot events frogSteerSaturated @122; + greenLight @123; radarCanErrorDEPRECATED @15; communityFeatureDisallowedDEPRECATED @62; diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 2f615f2..8857278 100644 --- a/cereal/custom.capnp +++ b/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; diff --git a/common/params.cc b/common/params.cc index 0231a5a..8f8b5fd 100644 --- a/common/params.cc +++ b/common/params.cc @@ -272,6 +272,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 25fc18d..cfb7cfe 100755 --- a/selfdrive/controls/controlsd.py +++ b/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 diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 2c23312..599998b 100755 --- a/selfdrive/controls/lib/events.py +++ b/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.), + }, + } 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 a982f19..286e811 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/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") diff --git a/selfdrive/frogpilot/ui/visual_settings.cc b/selfdrive/frogpilot/ui/visual_settings.cc index b888307..48e5966 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 77fd915..9875863 100644 --- a/selfdrive/frogpilot/ui/visual_settings.h +++ b/selfdrive/frogpilot/ui/visual_settings.h @@ -29,7 +29,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 = {"DynamicPathWidth", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength"};