diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a97f6b25af..f1d112a2c8 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -921,6 +921,9 @@ class Controls: else: self.stopped_for_light = False + if frogpilotPlan.leadDeparting: + self.events.add(EventName.leadDeparting) + if not self.openpilot_crashed_triggered and os.path.isfile(os.path.join(sentry.CRASHES_DIR, 'error.txt')): self.events.add(EventName.openpilotCrashed) self.openpilot_crashed_triggered = True diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 2437f7d1ba..0b5d9a6379 100755 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -1002,6 +1002,14 @@ EVENTS: dict[int, dict[str, 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.), + }, + EventName.noLaneAvailable: { ET.PERMANENT: no_lane_available_alert, }, diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index de4ef92b71..ef233dc407 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -51,6 +51,7 @@ class FrogPilotPlanner: self.mtsc = MapTurnSpeedController() self.forcing_stop = False + self.lead_departing = False self.model_stopped = False self.override_force_stop = False self.override_slc = False @@ -66,6 +67,7 @@ class FrogPilotPlanner: self.slc_target = 0 self.speed_jerk = 0 self.tracked_model_length = 0 + self.tracking_lead_distance = 0 self.v_cruise = 0 self.vtsc_target = 0 @@ -104,6 +106,16 @@ class FrogPilotPlanner: self.lane_width_left = 0 self.lane_width_right = 0 + if frogpilot_toggles.lead_departing_alert and self.tracking_lead and driving_gear and carState.standstill: + if self.tracking_lead_distance == 0: + self.tracking_lead_distance = lead_distance + + self.lead_departing = lead_distance - self.tracking_lead_distance > 1 + self.lead_departing &= v_lead > 1 + else: + self.lead_departing = False + self.tracking_lead_distance = 0 + self.model_length = modelData.position.x[MODEL_LENGTH - 1] self.model_stopped = self.model_length < CRUISING_SPEED * PLANNER_TIME self.model_stopped |= self.forcing_stop @@ -323,6 +335,8 @@ class FrogPilotPlanner: frogpilotPlan.laneWidthLeft = self.lane_width_left frogpilotPlan.laneWidthRight = self.lane_width_right + frogpilotPlan.leadDeparting = self.lead_departing + frogpilotPlan.maxAcceleration = float(self.max_accel) frogpilotPlan.minAcceleration = float(self.min_accel)