From 1d91b7e552580afd3e73a4f80f86932dc51e6797 Mon Sep 17 00:00:00 2001 From: CT921 Date: Mon, 1 May 2023 10:20:37 +0800 Subject: [PATCH] update --- opendbc | 2 +- selfdrive/controls/controlsd.py | 10 +- .../controls/lib/longitudinal_planner.py | 2 +- selfdrive/ui/qt/onroad.cc | 359 +++++++++++++++++- selfdrive/ui/qt/onroad.h | 3 +- 5 files changed, 356 insertions(+), 20 deletions(-) diff --git a/opendbc b/opendbc index 4ed040fe1d10a4..95a590db891e73 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 4ed040fe1d10a4a127ae626c2ca60ba53cbe461a +Subproject commit 95a590db891e7394202e5488080e693df0a4b329 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 8ff8f0c79f2ee2..442caa08be4e1c 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -295,12 +295,12 @@ def update_events(self, CS): # Alert if fan isn't spinning for 5 seconds if self.sm['peripheralState'].pandaType != log.PandaState.PandaType.unknown: - if self.sm['peripheralState'].fanSpeedRpm < 500 and self.sm['deviceState'].fanSpeedPercentDesired > 50: + # if self.sm['peripheralState'].fanSpeedRpm < 500 and self.sm['deviceState'].fanSpeedPercentDesired > 50: # allow enough time for the fan controller in the panda to recover from stalls - if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 15.0: - self.events.add(EventName.fanMalfunction) - else: - self.last_functional_fan_frame = self.sm.frame + # if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 15.0: + # self.events.add(EventName.fanMalfunction) + # else: + self.last_functional_fan_frame = self.sm.frame # Handle calibration status cal_status = self.sm['liveCalibration'].calStatus diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 6bb1540c910d95..a94801c401154d 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -22,7 +22,7 @@ A_CRUISE_MIN_BP_TOYOTA = [0., 8.3, 14, 20., 30., 55.] A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6] A_CRUISE_MAX_BP = [0., 10.0, 25., 40.] -A_CRUISE_MAX_VALS_TOYOTA = [2.0, 1.7, 1.3, 1.0, 0.89, 0.81, 0.63, 0.4, 0.31, 0.11] # Sets the limits of the planner accel, PID may exceed +A_CRUISE_MAX_VALS_TOYOTA = [2.2, 1.8, 1.4, 1.0, 0.89, 0.81, 0.63, 0.4, 0.31, 0.11] # Sets the limits of the planner accel, PID may exceed A_CRUISE_MAX_BP_TOYOTA = [0., 3., 6., 8., 11., 15., 20., 25., 30., 55.] # Lookup table for turns diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index dfda6a049bc10e..434c64e1f54814 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -15,6 +15,7 @@ #include "selfdrive/ui/qt/maps/map_helpers.h" #endif +#define FONT_OPEN_SANS "Inter" //"Open Sans" OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) { QVBoxLayout *main_layout = new QVBoxLayout(this); main_layout->setMargin(bdr_s); @@ -681,18 +682,25 @@ void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s) painter.restore(); } #endif -void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd) { - painter.save(); +static float global_a_rel; +static float global_a_rel_col; +void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd , int num /*不使用, size_t leads_num*/) { + painter.save(); const float speedBuff = 10.; const float leadBuff = 40.; +// const float d_rel = lead_data.getX()[0]; +// const float v_rel = lead_data.getV()[0]; const float d_rel = lead_data.getDRel(); - const float v_rel = lead_data.getVRel(); + const float v_rel = lead_data.getVRel(); //這就像相對速度。 +// const float t_rel = lead_data.getT()[0]; +// const float y_rel = lead_data.getY()[0]; +// const float a_rel = lead_data.getA()[0]; float fillAlpha = 0; if (d_rel < leadBuff) { fillAlpha = 255 * (1.0 - (d_rel / leadBuff)); - if (v_rel < 0) { + if (v_rel < 0) { //速度？如果它是負數，它會增加紅色三角形的密度。 fillAlpha += 255 * (-1 * (v_rel / speedBuff)); } fillAlpha = (int)(fmin(fillAlpha, 255)); @@ -705,18 +713,337 @@ void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState float g_xo = sz / 5; float g_yo = sz / 10; - QPointF glow[] = {{x + (sz * 1.35) + g_xo, y + sz + g_yo}, {x, y - g_yo}, {x - (sz * 1.35) - g_xo, y + sz + g_yo}}; - painter.setBrush(QColor(218, 202, 37, 255)); + //QPointF glow[] = {{x + (sz * 1.35) + g_xo, y + sz + g_yo}, {x, y - g_yo}, {x - (sz * 1.35) - g_xo, y + sz + g_yo}}; + float homebase_h = 12; + QPointF glow[] = {{x + (sz * 1.35) + g_xo, y + sz + g_yo + homebase_h},{x + (sz * 1.35) + g_xo, y + sz + g_yo}, {x, y - g_yo}, {x - (sz * 1.35) - g_xo, y + sz + g_yo},{x - (sz * 1.35) - g_xo, y + sz + g_yo + homebase_h}, {x, y + sz + homebase_h + g_yo + 10}}; + painter.setBrush(QColor(218, 202, 37, 210)); painter.drawPolygon(glow, std::size(glow)); // chevron - QPointF chevron[] = {{x + (sz * 1.25), y + sz}, {x, y}, {x - (sz * 1.25), y + sz}}; + //QPointF chevron[] = {{x + (sz * 1.25), y + sz}, {x, y}, {x - (sz * 1.25), y + sz}}; + QPointF chevron[] = {{x + (sz * 1.25), y + sz + homebase_h},{x + (sz * 1.25), y + sz}, {x, y}, {x - (sz * 1.25), y + sz},{x - (sz * 1.25), y + sz + homebase_h}, {x, y + sz + homebase_h - 7}}; painter.setBrush(redColor(fillAlpha)); painter.drawPolygon(chevron, std::size(chevron)); - painter.restore(); + if(num == 0){ //顯示到第 0 輛前車的距離 + //float dist = d_rel; //lead_data.getT()[0]; + QString dist = QString::number(d_rel,'f',0) + "m"; + int str_w = 200; + QString kmph = QString::number((v_rel + vc_speed)*3.6,'f',0) + "k"; + int str_w2 = 200; +// dist += "<" + QString::number(rect().height()) + ">"; str_w += 500;c2 和 c3 的屏幕高度均為 1020。 +// dist += "<" + QString::number(leads_num) + ">"; +// int str_w = 600; //200; +// dist += QString::number(v_rel,'f',1) + "v"; +// dist += QString::number(t_rel,'f',1) + "t"; +// dist += QString::number(y_rel,'f',1) + "y"; +// dist += QString::number(a_rel,'f',1) + "a"; + configFont(painter, FONT_OPEN_SANS, 44, "SemiBold"); + painter.setPen(QColor(0x0, 0x0, 0x0 , 200)); //影 + float lock_indicator_dx = 2; //避免向下的十字準星。 + painter.drawText(QRect(x+2+lock_indicator_dx, y-50+2, str_w, 50), Qt::AlignBottom | Qt::AlignLeft, dist); + painter.drawText(QRect(x+2-lock_indicator_dx-str_w2-2, y-50+2, str_w2, 50), Qt::AlignBottom | Qt::AlignRight, kmph); + painter.setPen(QColor(0xff, 0xff, 0xff)); + painter.drawText(QRect(x+lock_indicator_dx, y-50, str_w, 50), Qt::AlignBottom | Qt::AlignLeft, dist); + if(global_a_rel >= global_a_rel_col){ + global_a_rel_col = -0.1; //減少混亂的緩衝區。 + painter.setPen(QColor(0.09*255, 0.945*255, 0.26*255, 255)); + } else { + global_a_rel_col = 0; + painter.setPen(QColor(245, 0, 0, 255)); + } + painter.drawText(QRect(x-lock_indicator_dx-str_w2-2, y-50, str_w2, 50), Qt::AlignBottom | Qt::AlignRight, kmph); + painter.setPen(Qt::NoPen); + } + + painter.restore(); } +// Ichirio Stuff +struct LeadcarLockon { + float x,y,d,a,lxt,lxf,lockOK; +}; +#define LeadcarLockon_MAX 5 +LeadcarLockon leadcar_lockon[LeadcarLockon_MAX]; //這個數組編號 0 稱為推理框架 1。 + +void AnnotatedCameraWidget::drawLockon(QPainter &painter, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const QPointF &vd , int num /*不使用 , size_t leads_num , const cereal::RadarState::LeadData::Reader &lead0, const cereal::RadarState::LeadData::Reader &lead1 */) { + //const float speedBuff = 10.; + //const float leadBuff = 40.; + const float d_rel = lead_data.getX()[0]; + //const float d_rel = lead_data.getDRel(); + //const float v_rel = lead_data.getV()[0]; + //const float t_rel = lead_data.getT()[0]; + //const float y_rel = lead_data.getY()[0]; + float a_rel = lead_data.getA()[0]; + //float a_rel = lead_data.getARel(); //是？ + global_a_rel = a_rel; + + float sz = std::clamp((25 * 30) / (d_rel / 3 + 30), 15.0f, 30.0f) * 2.35; + float x = std::clamp((float)vd.x(), 0.f, width() - sz / 2); + //float y = std::fmin(height() /*- sz * .6*/, (float)vd.y()); + float y = (float)vd.y(); + + //float g_xo = sz / 5; + //float g_yo = sz / 10; + + //QPointF glow[] = {{x + (sz * 1.35) + g_xo, y + sz + g_yo}, {x, y - g_yo}, {x - (sz * 1.35) - g_xo, y + sz + g_yo}}; + + painter.setCompositionMode(QPainter::CompositionMode_Plus); + //p.setPen(QColor(0, 255, 0, 255)); + + float prob_alpha = lead_data.getProb(); //getModelProb(); + if(prob_alpha < 0){ + prob_alpha = 0; + } else if(prob_alpha > 1.0){ + prob_alpha = 1.0; + } + prob_alpha *= 245; + + painter.setPen(QPen(QColor(0.09*255, 0.945*255, 0.26*255, prob_alpha), 2)); + painter.setBrush(QColor(0, 0, 0, 0)); + float ww = 300 , hh = 300; + if(Hardware::TICI()){ + ww *= 1.25; hh *= 1.25; + } + float d = d_rel; //距離反映在鎖定目標的大小上。 + if(d < 1){ + d = 1; + } + + //緩衝運動。 + leadcar_lockon[num].x = leadcar_lockon[num].x + (x - leadcar_lockon[num].x) / 6; + leadcar_lockon[num].y = leadcar_lockon[num].y + (y - leadcar_lockon[num].y) / 6; + leadcar_lockon[num].d = leadcar_lockon[num].d + (d - leadcar_lockon[num].d) / 6; + x = leadcar_lockon[num].x; + y = leadcar_lockon[num].y; + d = leadcar_lockon[num].d; + if(d < 1){ + d = 1; + } + + leadcar_lockon[num].a = leadcar_lockon[num].a + (a_rel - leadcar_lockon[num].a) / 10; + a_rel = leadcar_lockon[num].a; + + float dh = 50; + if(uiState()->scene.wide_cam == false) { //反映 dh 中的深度值。 + float dd = d; + dd -= 25; //dd=0〜75 + dd /= (75.0/2); //dd=0〜2 + dd += 1; //dd=1〜3 + if(dd < 1)dd = 1; + dh /= dd; + } else { //使用廣角鏡頭改變邏輯。實時變化。 + ww *= 0.5; hh *= 0.5; + dh = 100; + float dd = d; + dd -= 5; //dd=0〜95 + dd /= (95.0/10); //dd=0〜10 + dd += 1; //dd=1〜11 + if(dd < 1)dd = 1; + dh /= dd*dd; + } + + ww = ww * 2 * 5 / d; + hh = hh * 2 * 5 / d; + y = std::fmin(height() /*- sz * .6*/, y - dh) + dh; + QRect r = QRect(x - ww/2, y /*- g_yo*/ - hh - dh, ww, hh); + +#if 0 + float y0 = lead0.getY()[0]; + float y1 = lead1.getY()[0]; +#else + //是嗎？它是水平的。 getY 似乎不起作用。 + float y0 = leadcar_lockon[0].x * leadcar_lockon[0].d; //如果發生這種情況，請從屏幕坐標倒數。 + float y1 = leadcar_lockon[1].x * leadcar_lockon[1].d; +#endif + + configFont(painter, FONT_OPEN_SANS, 38, "SemiBold"); + if(num == 0 /* && uiState()->scene.mLockOnButton */){ + //推理第一 + painter.setPen(QPen(QColor(0.09*255, 0.945*255, 0.26*255, prob_alpha), 2)); + painter.drawRect(r); + + //painter.setPen(QPen(QColor(0.09*255, 0.945*255, 0.26*255, prob_alpha), 2)); + if(leadcar_lockon[0].x > leadcar_lockon[1].x - 20){ + leadcar_lockon[num].lxt = leadcar_lockon[num].lxt + (r.right() - leadcar_lockon[num].lxt) / 20; + leadcar_lockon[num].lxf = leadcar_lockon[num].lxf + (width() - leadcar_lockon[num].lxf) / 20; + //painter.drawLine(r.right(),r.top() , width() , 0); + } else { + leadcar_lockon[num].lxt = leadcar_lockon[num].lxt + (r.left() - leadcar_lockon[num].lxt) / 20; + leadcar_lockon[num].lxf = leadcar_lockon[num].lxf + (0 - leadcar_lockon[num].lxf) / 20; + //painter.drawLine(r.left(),r.top() , 0 , 0); + } + painter.drawText(r, Qt::AlignTop | Qt::AlignLeft, " " + QString::number(num+1)); + + //painter.setPen(QPen(QColor(245, 245, 0, prob_alpha), 2)); + float lxt = leadcar_lockon[num].lxt; + if(lxt < r.left()){ + lxt = r.left(); + } else if(lxt > r.right()){ + lxt = r.right(); + } + painter.drawLine(lxt,r.top() , leadcar_lockon[num].lxf , 0); + if(ww >= 40){ + //painter.drawText(r, Qt::AlignTop | Qt::AlignRight, QString::number((int)(lead_data.getProb()*100)) + "％"); + + //將num==0 的lock-on 右端約20 個點設置為a_rel 數值表。 + painter.setPen(Qt::NoPen); + float wwa = ww * 0.15; + if(wwa > 40){ + wwa = 40; + } else if(wwa < 10){ + wwa = 10; + } + if(wwa > ww){ + wwa = ww; + } + + float hha = 0; + if(a_rel > 0){ + hha = 1 - 0.1 / a_rel; + painter.setBrush(QColor(0.09*255, 0.945*255, 0.26*255, prob_alpha*0.9)); + + if(hha < 0){ + hha = 0; + } + hha = hha * hh; +#if 0 + QRect ra = QRect(x - ww/2 + (ww - wwa), y /*- g_yo*/ - hh - dh + (hh-hha), wwa, hha); + painter.drawRect(ra); +#else //對角切米 + QPointF meter[] = {{(float)x + ww/2 - wwa/2 - wwa/2 * hha / hh , (float)y /*- g_yo*/ - hh - dh + (hh-hha)},{(float)x + ww/2 , (float)y /*- g_yo*/ - hh - dh + (hh-hha)}, {(float)x + ww/2 , (float)y /*- g_yo*/ - hh - dh + hh}, {(float)x + ww/2 - wwa/2 , (float)y /*- g_yo*/ - hh - dh + hh}}; + painter.drawPolygon(meter, std::size(meter)); +#endif + } + if(a_rel < 0){ + hha = 1 + 0.1 / a_rel; + painter.setBrush(QColor(245, 0, 0, prob_alpha)); + //減速度從上到下變化。 + if(hha < 0){ + hha = 0; + } + hha = hha * hh; +#if 0 + QRect ra = QRect(x - ww/2 + (ww - wwa), y /*- g_yo*/ - hh - dh , wwa, hha); + painter.drawRect(ra); +#else //對角切米 + QPointF meter[] = {{(float)x + ww/2 - wwa/2 , (float)y /*- g_yo*/ - hh - dh},{(float)x + ww/2 , (float)y /*- g_yo*/ - hh - dh}, {(float)x + ww/2 , (float)y /*- g_yo*/ - hh - dh + hha}, {(float)x + ww/2 - wwa/2 - wwa/2 * hha / hh, (float)y /*- g_yo*/ - hh - dh + hha}}; + painter.drawPolygon(meter, std::size(meter)); +#endif + } + } + + if(//lead0.getX()[0] > lead1.getX()[0] //lead1 在 lead0 後面 + //y0 > y1 //lead1 在 lead0 的左邊 + std::abs(y0 - y1) <= 300 //向側面大移動 → 反向 + // ||另外v_rel和a_rel在前面顯示突然減速（我試過在num==0顯示） + //&& lead1.getX()[0] < 10 //前方10m以內的lead1 + ){ + leadcar_lockon[num].lockOK = leadcar_lockon[num].lockOK + (40 - leadcar_lockon[num].lockOK) / 5; + //float td = 40; + } else { + leadcar_lockon[num].lockOK = leadcar_lockon[num].lockOK + (0 - leadcar_lockon[num].lockOK) / 5; + } + float td = leadcar_lockon[num].lockOK; + //d: 從 10 到 100->1 到 3 + if(td >= 3){ + float dd = leadcar_lockon[num].d; + if(dd < 10){ + dd = 10; + } + dd -= 10; //dd=0〜90 + dd /= (90.0/2); //dd=0〜2 + dd += 1; //dd=1〜3 + td /= dd; + + float tlw = 8; + float tlw_2 = tlw / 2; + painter.setPen(QPen(QColor(0.09*255, 0.945*255, 0.26*255, prob_alpha), tlw)); + painter.drawLine(r.center().x() , r.top()-tlw_2 , r.center().x() , r.top() - td); + painter.drawLine(r.left()-tlw_2 , r.center().y() , r.left() - td , r.center().y()); + painter.drawLine(r.right()+tlw_2 , r.center().y() , r.right() + td , r.center().y()); + painter.drawLine(r.center().x() , r.bottom()+tlw_2 , r.center().x() , r.bottom() + td); + } + + } else if(/* uiState()->scene.mLockOnButton */ true){ + if(num == 1){ + //推理二 + //我想認出一輛擋路的右前方行駛的摩托車。 + if(//lead0.getX()[0] > lead1.getX()[0] //lead1 在 lead0 後面 + //y0 > y1 //lead1 在 lead0 的左邊 + std::abs(y0 - y1) > 300 //側身移動 + // ||另外v_rel和a_rel在前面顯示突然減速（我試過在num==0顯示） + //&& lead1.getX()[0] < 10 //前方10m以內lead1 + ){ + //painter.setPen(QPen(QColor(245, 0, 0, prob_alpha), 4)); + //painter.drawEllipse(r); //畫一條邊 + //painter.setPen(QPen(QColor(0.09*255, 0.945*255, 0.26*255, prob_alpha), 1)); //稍後寫信重置顏色。 -> 我不介意文字是紅色的 + + //不要畫圓（因為沒有意義），把整個框都畫成紅色。很容易認識到推理 1 包含推理和其他內容。 + painter.setPen(QPen(QColor(245, 0, 0, prob_alpha), 2)); + } else { + painter.setPen(QPen(QColor(0.09*255, 0.945*255, 0.26*255, prob_alpha), 2)); + } + + if(leadcar_lockon[0].x > leadcar_lockon[1].x - 20){ //允許輕微倒置 + leadcar_lockon[num].lxt = leadcar_lockon[num].lxt + (r.left() - leadcar_lockon[num].lxt) / 20; + leadcar_lockon[num].lxf = leadcar_lockon[num].lxf + (0 - leadcar_lockon[num].lxf) / 20; + //painter.drawLine(r.left(),r.top() , 0 , 0); + } else { + leadcar_lockon[num].lxt = leadcar_lockon[num].lxt + (r.right() - leadcar_lockon[num].lxt) / 20; + leadcar_lockon[num].lxf = leadcar_lockon[num].lxf + (width() - leadcar_lockon[num].lxf) / 20; + //painter.drawLine(r.right(),r.top() , width() , 0); + } + float lxt = leadcar_lockon[num].lxt; + if(lxt < r.left()){ + lxt = r.left(); + } else if(lxt > r.right()){ + lxt = r.right(); + } + painter.drawLine(lxt,r.top() , leadcar_lockon[num].lxf , 0); + + if(ww >= 80){ + //float dy = y0 - y1; + //painter.drawText(r, Qt::AlignBottom | Qt::AlignLeft, " " + QString::number(dy,'f',1) + "m"); + //painter.drawText(r, Qt::AlignBottom | Qt::AlignLeft, " " + QString::number(dy,'f',1)); + } + } else if(num == 2){ + //推理數 3 + //幾乎沒有。就像在 0,0 處不動一樣？ + painter.setPen(QPen(QColor(0.09*255, 0.945*255, 0.26*255, prob_alpha), 2)); + //painter.drawLine(r.right(),r.center().y() , width() , height()); + } else { + //推論 4 號起。 + //不存在。 + painter.setPen(QPen(QColor(0.09*255, 0.945*255, 0.26*255, prob_alpha), 2)); + //painter.drawLine(r.left(),r.center().y() , 0 , height()); + } + + painter.drawRect(r); + + //painter.setPen(QPen(QColor(0.09*255, 0.945*255, 0.26*255, prob_alpha), 2)); + + if(ww >= 80){ + //此處不能引用 y0 和 y1。 + float d_lim = 12; + if(wide_cam_requested == false){ + d_lim = 32; //對於長相機來說，鏡框有點大。實測 + } + if(num == 0 || (num==1 && (d_rel < d_lim || std::abs(y0 - y1) > 300))){ //當num==1時，'2'的顯示與前車的速度重疊，所以刪除這個。 + painter.drawText(r, Qt::AlignBottom | Qt::AlignLeft, " " + QString::number(num+1)); + } + } + if(ww >= 160 /*80*/){ + //painter.drawText(r, Qt::AlignBottom | Qt::AlignRight, QString::number((int)(lead_data.getProb()*100)) + "％"); + //painter.drawText(r, Qt::AlignBottom | Qt::AlignRight, QString::number(a_rel,'f',1) + "a"); + } + } + painter.setPen(Qt::NoPen); + painter.setCompositionMode(QPainter::CompositionMode_SourceOver); +} + + + #ifndef QCOM void AnnotatedCameraWidget::paintGL() { UIState *s = uiState(); @@ -783,13 +1110,21 @@ void AnnotatedCameraWidget::paintGL() { drawLaneLines(painter, s); if (s->scene.longitudinal_control) { + const auto leads = model.getLeadsV3(); + size_t leads_num = leads.size(); + for(size_t i=0; i .2){ //顯示信用評級為 20% 或更高。調整中 + drawLockon(painter, leads[i], s->scene.lead_vertices[i] , i /*, leads_num , leads[0] , leads[1]*/); + } + } + auto lead_one = radar_state.getLeadOne(); auto lead_two = radar_state.getLeadTwo(); if (lead_one.getStatus()) { - drawLead(painter, lead_one, s->scene.lead_vertices[0]); + drawLead(painter, lead_one, s->scene.lead_vertices[0] , 0); } if (lead_two.getStatus() && (std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0)) { - drawLead(painter, lead_two, s->scene.lead_vertices[1]); + drawLead(painter, lead_two, s->scene.lead_vertices[1] , 1); } } } @@ -851,10 +1186,10 @@ void AnnotatedCameraWidget::paintGL() { auto lead_one = radar_state.getLeadOne(); auto lead_two = radar_state.getLeadTwo(); if (lead_one.getStatus()) { - drawLead(painter, lead_one, s->scene.lead_vertices[0]); + drawLead(painter, lead_one, s->scene.lead_vertices[0] , 0); } if (lead_two.getStatus() && (std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0)) { - drawLead(painter, lead_two, s->scene.lead_vertices[1]); + drawLead(painter, lead_two, s->scene.lead_vertices[1] , 1); } } } diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 65c24ae0c51bf0..8d9a8d76e2e3ac 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -126,8 +126,9 @@ class AnnotatedCameraWidget : public CameraWidget { void showEvent(QShowEvent *event) override; void updateFrameMat() override; void drawLaneLines(QPainter &painter, const UIState *s); - void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd); + void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd , int num); void drawHud(QPainter &p); + void drawLockon(QPainter &painter, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const QPointF &vd , int num /*不使用, size_t leads_num , const cereal::RadarState::LeadData::Reader &lead0, const cereal::RadarState::LeadData::Reader &lead1 */); #ifndef QCOM void drawDriverState(QPainter &painter, const UIState *s); #endif