diff --git a/cereal/car.capnp b/cereal/car.capnp index 08d24baf..42652875 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -120,6 +120,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { # FrogPilot events frogSteerSaturated @121; greenLight @122; + leadDeparting @124; radarCanErrorDEPRECATED @15; communityFeatureDisallowedDEPRECATED @62; diff --git a/common/params.cc b/common/params.cc index 21aa6c7c..f5e03ec2 100644 --- a/common/params.cc +++ b/common/params.cc @@ -276,6 +276,7 @@ std::unordered_map keys = { {"HigherBitrate", PERSISTENT}, {"LaneLinesWidth", PERSISTENT}, {"LateralTune", PERSISTENT}, + {"LeadDepartingAlert", PERSISTENT}, {"LeadInfo", PERSISTENT}, {"LongitudinalTune", PERSISTENT}, {"LongPitch", PERSISTENT}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 37755ae3..7a607b49 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -181,6 +181,8 @@ class Controls: self.previously_enabled = False self.stopped_for_light_previously = False + self.previous_lead_distance = 0 + ignore = self.sensor_packets + ['testJoystick'] if SIMULATION: ignore += ['driverCameraState', 'managerState'] @@ -548,6 +550,20 @@ class Controls: if green_light: self.events.add(EventName.greenLight) + # Lead departing alert + if self.lead_departing_alert and self.sm.frame % 50 == 0: + lead = self.sm['radarState'].leadOne + lead_distance = lead.dRel + lead_departing = lead_distance - self.previous_lead_distance > 0.5 and self.previous_lead_distance != 0 and CS.standstill + self.previous_lead_distance = lead_distance + + lead_departing &= not CS.gasPressed + lead_departing &= lead.vLead > 1 + lead_departing &= self.driving_gear + + if lead_departing: + self.events.add(EventName.leadDeparting) + def data_sample(self): """Receive data from sockets and update carState""" @@ -1036,6 +1052,7 @@ class Controls: custom_alerts = self.params.get_bool("CustomAlerts") self.green_light_alert = custom_alerts and self.params.get_bool("GreenLightAlert") + self.lead_departing_alert = custom_alerts and self.params.get_bool("LeadDepartingAlert") 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 599998b4..466d401e 100755 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -974,6 +974,13 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.), }, + EventName.leadDeparting: { + ET.PERMANENT: Alert( + "Lead departed", + "", + AlertStatus.frogpilot, AlertSize.small, + Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.), + }, } diff --git a/selfdrive/frogpilot/ui/visual_settings.cc b/selfdrive/frogpilot/ui/visual_settings.cc index d5febe7e..416ff2bb 100644 --- a/selfdrive/frogpilot/ui/visual_settings.cc +++ b/selfdrive/frogpilot/ui/visual_settings.cc @@ -21,6 +21,7 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot {"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.", ""}, + {"LeadDepartingAlert", "Lead Departing Alert", "Get an alert when your lead vehicle starts departing when you're at a standstill.", ""}, {"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 d56ed63a..727ccf6a 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 = {"GreenLightAlert"}; + std::set customAlertsKeys = {"GreenLightAlert", "LeadDepartingAlert"}; std::set customOnroadUIKeys = {"AccelerationPath", "AdjacentPath", "BlindSpotPath", "FPSCounter", "LeadInfo"}; std::set customThemeKeys = {"CustomColors", "CustomIcons", "CustomSignals", "CustomSounds"}; std::set modelUIKeys = {"DynamicPathWidth", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength"};