diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 578fc8b..b3e6f71 100644 --- a/cereal/custom.capnp +++ b/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; diff --git a/common/params.cc b/common/params.cc index b1f020f..87959b0 100644 --- a/common/params.cc +++ b/common/params.cc @@ -253,6 +253,7 @@ std::unordered_map keys = { {"FrogPilotTogglesUpdated", 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 fea72a4..3300c09 100644 --- a/selfdrive/controls/controlsd.py +++ b/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 diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 5cf1458..f3dd4cf 100755 --- a/selfdrive/controls/lib/events.py +++ b/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.), + }, + } 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 4aa9cb0..14666dd 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/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 diff --git a/selfdrive/frogpilot/ui/visual_settings.cc b/selfdrive/frogpilot/ui/visual_settings.cc index 8cbe770..a3f8e64 100644 --- a/selfdrive/frogpilot/ui/visual_settings.cc +++ b/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.", ""},