diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 6804bb2ab..be72f0fbe 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -25,6 +25,7 @@ class FrogPilotPlanner: self.cem = ConditionalExperimentalMode(self) + self.slower_lead = False self.tracking_lead = False self.acceleration_jerk = 0 @@ -95,6 +96,11 @@ class FrogPilotPlanner: self.tracking_lead = self.tracking_lead_mac.get_moving_average() >= PROBABILITY def update_follow_values(self, lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles): + # Offset by FrogAi for FrogPilot for a more natural approach to a slower lead + if frogpilot_toggles.conditional_slower_lead and v_lead < v_ego: + distance_factor = max(lead_distance - (v_lead * self.t_follow), 1) + braking_offset = clip((v_ego - v_lead) - COMFORT_BRAKE, 1, distance_factor) + self.slower_lead = braking_offset > 1 def update_v_cruise(self, carState, controlsState, frogpilotCarState, frogpilotNavigation, modelData, v_cruise, v_ego, frogpilot_toggles): v_cruise_cluster = max(controlsState.vCruiseCluster, v_cruise) * CV.KPH_TO_MS diff --git a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py index 436c7c679..0cdf7ec49 100644 --- a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py +++ b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py @@ -10,6 +10,7 @@ class ConditionalExperimentalMode: self.frogpilot_planner = FrogPilotPlanner self.curvature_mac = MovingAverageCalculator() + self.slow_lead_mac = MovingAverageCalculator() self.curve_detected = False self.experimental_mode = False @@ -38,10 +39,15 @@ class ConditionalExperimentalMode: self.status_value = 12 return True + if frogpilot_toggles.conditional_lead and self.slow_lead_detected: + self.status_value = 13 if v_lead < 1 else 14 + return True + return False def update_conditions(self, tracking_lead, v_ego, v_lead, frogpilot_toggles): self.curve_detection(v_ego, frogpilot_toggles) + self.slow_lead(tracking_lead, v_lead, frogpilot_toggles) def curve_detection(self, v_ego, frogpilot_toggles): curve_detected = (1 / self.frogpilot_planner.road_curvature)**0.5 < v_ego @@ -49,3 +55,14 @@ class ConditionalExperimentalMode: self.curvature_mac.add_data(curve_detected or curve_active) self.curve_detected = self.curvature_mac.get_moving_average() >= PROBABILITY + + def slow_lead(self, tracking_lead, v_lead, frogpilot_toggles): + if tracking_lead: + slower_lead = self.frogpilot_planner.slower_lead and frogpilot_toggles.conditional_slower_lead + stopped_lead = frogpilot_toggles.conditional_stopped_lead and v_lead < 1 + + self.slow_lead_mac.add_data(slower_lead or stopped_lead) + self.slow_lead_detected = self.slow_lead_mac.get_moving_average() >= PROBABILITY + else: + self.slow_lead_mac.reset_data() + self.slow_lead_detected = False