From 6194244d23867e7d9a5556c526f6b2a723e6d26d Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Tue, 2 Apr 2024 23:25:13 -0700 Subject: [PATCH] Lead departing alert Added toggle to enable an alert when the lead vehicle starts to depart. --- cereal/car.capnp | 1 + common/params.cc | 1 + selfdrive/controls/controlsd.py | 17 +++++++++++++++++ selfdrive/controls/lib/events.py | 8 ++++++++ .../frogpilot/ui/qt/offroad/visual_settings.cc | 1 + .../frogpilot/ui/qt/offroad/visual_settings.h | 2 +- 6 files changed, 29 insertions(+), 1 deletion(-) diff --git a/cereal/car.capnp b/cereal/car.capnp index 3db902e7d..01332eb24 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -122,6 +122,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { goatSteerSaturated @125; greenLight @126; holidayActive @127; + leadDeparting @129; radarCanErrorDEPRECATED @15; communityFeatureDisallowedDEPRECATED @62; diff --git a/common/params.cc b/common/params.cc index 3eb369d92..16b73637b 100644 --- a/common/params.cc +++ b/common/params.cc @@ -292,6 +292,7 @@ std::unordered_map keys = { {"IncreaseThermalLimits", 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 f9eebd067..a63e8d6b6 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -186,6 +186,8 @@ class Controls: self.holiday_theme_alerted = False self.previously_enabled = False + self.previous_lead_distance = 0 + self.green_light_mac = MovingAverageCalculator() self.update_frogpilot_params() @@ -912,6 +914,20 @@ class Controls: self.events.add(EventName.holidayActive) self.holiday_theme_alerted = True + 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 update_frogpilot_variables(self, CS): self.driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown) @@ -950,6 +966,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 89b7f5af9..f97583ff2 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -1010,6 +1010,14 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { EventName.holidayActive: { ET.PERMANENT: holiday_alert, }, + + EventName.leadDeparting: { + ET.PERMANENT: Alert( + "Lead departed", + "", + AlertStatus.frogpilot, AlertSize.small, + Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.), + }, } diff --git a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc index f5c4085f7..85009ba0d 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc @@ -16,6 +16,7 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot {"CustomAlerts", tr("Custom Alerts"), tr("Enable custom alerts for openpilot events."), "../frogpilot/assets/toggle_icons/icon_green_light.png"}, {"GreenLightAlert", tr("Green Light Alert"), tr("Get an alert when a traffic light changes from red to green."), ""}, + {"LeadDepartingAlert", tr("Lead Departing Alert"), tr("Get an alert when the lead vehicle starts departing when at a standstill."), ""}, {"CustomUI", tr("Custom Onroad UI"), tr("Customize the Onroad UI."), "../assets/offroad/icon_road.png"}, {"DeveloperUI", tr("Developer UI"), tr("Get various detailed information of what openpilot is doing behind the scenes."), ""}, diff --git a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h index 3218fbce3..984a252c2 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h @@ -24,7 +24,7 @@ private: void updateToggles(); std::set alertVolumeControlKeys = {"DisengageVolume", "EngageVolume", "PromptDistractedVolume", "PromptVolume", "RefuseVolume", "WarningImmediateVolume", "WarningSoftVolume"}; - std::set customAlertsKeys = {"GreenLightAlert"}; + std::set customAlertsKeys = {"GreenLightAlert", "LeadDepartingAlert"}; std::set customOnroadUIKeys = {"CustomPaths", "DeveloperUI", "FPSCounter", "LeadInfo"}; std::set customThemeKeys = {"CustomColors", "CustomIcons", "CustomSignals", "CustomSounds", "HolidayThemes"}; std::set modelUIKeys = {"DynamicPathWidth", "HideLeadMarker", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength"};