diff --git a/frogpilot/controls/frogpilot_planner.py b/frogpilot/controls/frogpilot_planner.py index 3392b9bc4..cafb04458 100644 --- a/frogpilot/controls/frogpilot_planner.py +++ b/frogpilot/controls/frogpilot_planner.py @@ -130,6 +130,8 @@ class FrogPilotPlanner: frogpilotPlan.frogpilotToggles = json.dumps(vars(frogpilot_toggles)) + frogpilotPlan.increasedStoppedDistance = frogpilot_toggles.increase_stopped_distance + frogpilotPlan.lateralCheck = self.lateral_check frogpilotPlan.maxAcceleration = float(self.frogpilot_acceleration.max_accel) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 736f7644f..66497c76b 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -213,7 +213,7 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader, model_v_ego: float, model_data: capnp._DynamicStructReader, - frogpilot_toggles: SimpleNamespace, + frogpilot_plan: capnp._DynamicStructReader, frogpilot_toggles: SimpleNamespace, low_speed_override: bool = True) -> dict[str, Any]: # Determine leads, this is where the essential logic happens if len(tracks) > 0 and ready and lead_msg.prob > .5: @@ -246,6 +246,9 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn for track in tracks.values(): track.leadTrackID = lead_dict.get('radarTrackId', -1) + if 'dRel' in lead_dict: + lead_dict['dRel'] -= frogpilot_plan.increasedStoppedDistance + return lead_dict @@ -323,8 +326,8 @@ class RadarD: model_v_ego = self.v_ego leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: - self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, sm['modelV2'], self.frogpilot_toggles, low_speed_override=True) - self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, sm['modelV2'], self.frogpilot_toggles, low_speed_override=False) + self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, sm['modelV2'], sm['frogpilotPlan'], self.frogpilot_toggles, low_speed_override=True) + self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, sm['modelV2'], sm['frogpilotPlan'], self.frogpilot_toggles, low_speed_override=False) # FrogPilot variables if self.ready and (self.frogpilot_toggles.adjacent_lead_tracking or self.frogpilot_toggles.human_lane_changes):