mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-10 11:52:10 +08:00
Code Issues Packages Projects Releases Wiki Activity

Far/radar only lead detection

Browse Source
This commit is contained in:
James
2025-12-01 12:00:00 -07:00
parent 4b57104d3e
commit 99e013e233
2 changed files with 28 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/radard.py
+23 -4
View File
@@ -13,7 +13,7 @@ from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process
from openpilot.common.swaglog import cloudlog
from openpilot.common.simple_kalman import KF1D
from openpilot.frogpilot.common.frogpilot_variables import get_frogpilot_toggles
from openpilot.frogpilot.common.frogpilot_variables import THRESHOLD, get_frogpilot_toggles
# Default lead acceleration decay set to 50% at 1s
@@ -67,6 +67,8 @@ class Track:
self.leadTrackID = 0
self.radarfulFilter = FirstOrderFilter(0, 1, self.K_A[0][1])
def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float, measured: float):
# relative values, copy
self.dRel = d_rel # LONG_DIST
@@ -136,6 +138,17 @@ class Track:
else:
return self.leadRight
def potential_far_lead(self, lead_msg: capnp._DynamicStructReader, model_data: capnp._DynamicStructReader):
left_lane = np.interp(self.dRel, model_data.laneLines[1].x, model_data.laneLines[1].y)
right_lane = np.interp(self.dRel, model_data.laneLines[2].x, model_data.laneLines[2].y)
if left_lane < -self.yRel < right_lane and self.dRel < model_data.position.x[-1] and self.vLeadK > 1:
self.radarfulFilter.update(1)
return True
else:
self.radarfulFilter.update(0)
return False
def laplacian_pdf(x: float, mu: float, b: float):
b = max(b, 1e-4)
@@ -186,7 +199,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_v_ego: float, model_data: capnp._DynamicStructReader,
frogpilot_toggles: SimpleNamespace,
low_speed_override: bool = True) -> dict[str, Any]:
# Determine leads, this is where the essential logic happens
@@ -210,6 +223,12 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn
if (not lead_dict['status']) or (closest_track.dRel < lead_dict['dRel']):
lead_dict = closest_track.get_RadarState()
if low_speed_override and not lead_dict['status'] and len(tracks) > 0:
far_lead_tracks = [c for c in tracks.values() if c.potential_far_lead(lead_msg, model_data) and c.radarfulFilter.x >= THRESHOLD]
if len(far_lead_tracks) > 0:
closest_track = min(far_lead_tracks, key=lambda c: c.dRel)
lead_dict = closest_track.get_RadarState()
# FrogPilot variables
for track in tracks.values():
track.leadTrackID = lead_dict.get('radarTrackId', -1)
@@ -291,8 +310,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, 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, 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'], 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)
# FrogPilot variables
if self.ready and (self.frogpilot_toggles.adjacent_lead_tracking):
selfdrive/ui/qt/onroad/model.cc
+5 -1
View File
@@ -39,7 +39,11 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) {
update_leads(radar_state, model.getPosition());
const auto &lead_two = radar_state.getLeadTwo();
if (lead_one.getStatus()) {
drawLead(painter, lead_one, lead_vertices[0], surface_rect, QColor(frogpilot_toggles.value("lead_marker_color").toString()));
if (lead_one.getModelProb() >= frogpilot_toggles.value("lead_detection_probability").toDouble()) {
drawLead(painter, lead_one, lead_vertices[0], surface_rect, QColor(frogpilot_toggles.value("lead_marker_color").toString()));
} else {
drawLead(painter, lead_one, lead_vertices[0], surface_rect, frogpilot_nvg->whiteColor());
}
} else {
// FrogPilot variables
if (lead_two.getStatus() && (std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0)) {