From e4a3e244bf653490a826a2b5a619f2d7ea4b8d29 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Thu, 16 Apr 2026 22:30:05 -0500 Subject: [PATCH] Dm graphic --- .../onroad/driver_monitoring/dm_center.png | Bin 5875 -> 0 bytes selfdrive/ui/mici/onroad/driver_state.py | 33 +++++++++--------- 2 files changed, 16 insertions(+), 17 deletions(-) delete mode 100644 selfdrive/assets/icons_mici/onroad/driver_monitoring/dm_center.png diff --git a/selfdrive/assets/icons_mici/onroad/driver_monitoring/dm_center.png b/selfdrive/assets/icons_mici/onroad/driver_monitoring/dm_center.png deleted file mode 100644 index bdeb0387c139bf9796167d3b1ce082973b777444..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 5875 zcmVYnvat`KXYgYPdLCX47|@A(Fa#Lj#fIl# zEEYZpAh?uhkwZ>*{bl<5A|k7*honia$laEbRo&BFmERXX84(#3`O96j{@u(8{Z;S@xWfa+A z9O5T3h39gDzfTyIgRF7ffruA4?$d4gJUy20N1RVz$#2rv;p-$H;q_q&0PuVARrNrA zSKpLj^Sy*xbW|W<6L&Jiy`->}Sn=xzX#)QRcm;>g^FcN^Pm{=i;|Y!#&1eL47}uZ)-SDaOeIxboL@qk` zj`62x#Gg(;{t|%p0R1CI&C#?nHl5Vaf*zo2*M_mSV-stcD$F+8eF;8N3o^fe&w0#1 zIn^LwR3fPM$4e1`5(n0B!_`n}JkzBmot?;G`M=opl1} z%Od`mA%2n&UG4D|$vw|O;9x{#*jMa>i~t=$1PR#Uyu)~baRcL1kZ^x`7g+B`0Kne{ z_{Xw8!B4iWwA)TPlCiC&-i}THzS)}ehenhH1y4=DF#&j>XH!j}BS_2ri}XQ4APGtU zzB8s3c$g6--cPb|$xjD6Rvdeha4Hd#Z1#`ek%YGsplW}9aw<=@7gB?O4B$yMpq9LY zUAC2s+esRHJ~l=K!KY>ge)a{BG7Q}up^-3aK{koYr0ziy2#8ZMN7#cH1O_BP05
vF0;oG8o501tPO~ZSzJUaUWp-x&z6`nv=%C$?3vil5?27Hf#~h#?09yn6 zv===m0sa0La{cm=oE`o@66!aD=On*rPvyq$LaJRWm)j&Yz(;BUB#*!iM@(i%y9a2j z2BJP9nK`HyBt2$;CmCGa=?92Cxeq7~tHc13mZeXnKRED>kx={!ooWJ8v?g9BMPF87 zhnKiXogc{_*F4X-!y6rA{bumLr39h&`zY|xc<)%~!aBZ%j8YHvE1gE^0 zN0{1OT<^SpcLL-dCOOci+ZH5rSn5ayF1~|9Ix%%mbcj3i2}%Y?&~w)W2KoUSz)1(j zzMHHjU<72FIFK`bi9k;iP)AUtKbSAjRRG#Ul_!}rB@#)=XE!4*r@3g64}w1=3#ESr zdELT|9*l1%K!tK0Y(E9)2H^8{1J%bwVe?r%KWzRao6RGABlwD1urrd>G${RTHlLu& zAuh`!!Bc&q0SU8J2WWS-MFC>W)PN%S-eU@qK=33&gDEp5(ib(Vtju(1JK2b%7p@pG z;+($pI;JnLBvP*sa96(EoXhhq6B^uE*ZIM0+EsrfP`poDlfG)f3DgoA5Y!xkem^5P zD$8Pq24vNQY!inDglf5~Mt`~#uE0A1si*cNiy~e68c;z6qD!AngVCd&CL4)M#%RPc zr?_m-lrJ4DDNJ-$?8g>0`VnK#-OPoou5e`f2R+qgZdFQBQLNdWi!pvSMZ~0fMt-P5#^@6 zCIi5qw!k>8SA{DYa9%%=mN$j;Q-gRL(cPwHpflO}Pl*D2R)FUCcns{^)T7$CbB=XYXj{vcBA2Vd?0o)wdfAB0$boz&LD97H*R2%i(e=ZfI* z`$`k2mq^$U!Vh=ket)i-b(Mh3N&FKeHPTiIzNs}A4UGZG^yrj+adNKy$&aGoF+pm; z&OQrmP*aFHX_L53KZu4w+1CN{iBc3n9VM^1vGY)w=p4q;^N8n2*U#k^BKY&6_=1%N{4>sbUAuu8F?~elQK6PK(T#3Ivp#A72}w>0 z%^||(^VYzV1i2=2j6mmCJhWU{h;#7ePZpaFR^T2|+39Lm~GE%4Ys z0en!6z{E6H5Y+%xdzG{SyZV}?o@LgG5Q=2;@||FWo7o2g4`<00gE2#%LuacO&}`_t zm_Kj{<~(W@YOfjORJ2?PD0&wx$k;QmuI;>l51!hV>@KxwF^ks7Q8Koo+OU8(bZWQ3 z99Q(%JG%X=?eq3wAt@KFm`%cdpBvM(Vq!F{?53GEX`TloPR?(tZO#<|qcSy?FdX$t zp%D89*#Gwt+cKx3Klth_H2&puQ;1h$k0G1=pgMM1&@r?D^wXJyHF3R;rA8-{>P+_Y@)AN z&axj!ZMIM66Ey-`G(k;)!3}(cRycLJvxyaHmtj&Y%I3~*mTq?7<-2(W-awl+gjq9q zUvUkLjV-a%0$p+`s5&|x8&z$ZJh?NC)icUgcGawld-)~abkRO+v3~TvB&p}6+T)^u zs-VKQq7oMbAp$RR697Q#=>jPA-Uv3 zsXw@|;%3rRT*)9OTAVc@vr7F(Cjc85ll&oC?Y{TYwPjj%R1%oQG5Jt3G}ACiQ2IIn zJ1-N>Im{r+F|DL)@p_p5zLX?tFrc59WNdcVQQ4idy?VoXGdckm}(p7<5eKavGnKoAS}AS zoW6bq(YU!?tgT+)LT2jBi2EKt;6~*&`Y%)+s>rBq^i*@w$g3(EJ(=Nd<>d`DUl zR5Vq;Na+XH%6V?S-@t_*S`c0O2YIHI>7U2=Om2a_6NGPLTmX(_4r5vgGOUOxnDXlV zqN>YebBS4Hi3d!L^L$~A>%*}FEF|b=&+J1y77R{|h?myNmD22WZC<7gCLq)7QV-#x zMFRdy;_u~pz7X4^wbuZzVj0?`fVFrj#Vbcs+f{_O4)IF}n;gIzQ1prAnS^x&c~qU8y>6dfkrd#u7bO1mq|ox~g~U23C+xM``=<0R=(H2X*% zyK5Kn#3VI*p{@TD^q0!N@vV1&+OX0aD_FV~3fK zzE@U}xcZ)H&Js|G3m|wIZO#mt7MINxpJzh0M6gWR5tpnL&vdtdEipvVrsca~PN0f|TtH`^ zPtxLFW&v=Dw#VR`t$E&b?d5{oNpSmny#JhQBYSeTUXxR1up2wEyyP1c*}<1%*|`by zBID}4gI%xxiy4GfJ|HWnwnTl=RbNV4>|u0rLeuO7&DUh_8d376;6+tYVJ#c2Blu22_b`vxp`USW>#cGQkP3pFBk?jV9M-}LNx#PO0v!i^Gsz^WwIS+lEh8w0rO1QK4QAC zPqNT{n6&V!KhS8>r9aIVf%nHcSy(sFcJsjAmLb~~mdL!+mHdD0{N@^77LW0RYe$wS zi@MJ``2E(TTk`s2 zmXHVT$&ZzikhI*Cbg0b_QLC+aH3?zqx_6aR6%9wyj4Q2>RsLV3x-O0sDX*PiSj7}c z=ODUWrhxked|H*%2|kV1XTZNz-gCr}$4Bx^_XvDudrE$wZ8Wk$&4?@Eh#zZaRLV?# zQ)55dIe>?b;Imb4J3~<-yu1#WW{KvRh_#cHD;iKLMdiGl=zX~@-j{i0id5@?r6iX0 zI5`=J0<{ii#E<}XpWT^OqIr<`{XKy9sGkq|#d@?0vBH{|Kh9E53YHTHodM5>VEcCm zyzf^Mo>hHuj=|&V{7U3l2+KzybP1dF>jgZB4s_-|4;C=Eg+80bOakXW>fDFo7cl-f z8s<_IRk3JMaokD|fX&dOrq14+tQFQ81Wc@waNAq=>NtsL8xR(ZB_zIlV^7p zJf2TiD#PaOUtKQq{I@04q{GVq9Z2?}Xj~y7***eykuldqjp!FL0(@3pna8J7aZanU zc&m&Zz!+ORfRLJ?pH(vUliq+kG^`HMV{*$o_%?M(i#dDi{4(il<%Z#IUSvH>GcTTh zT`&o&6nQRzQ?l6ij~Hp6T?17H~-(2bhR4f%2SQEm)t zy8ZQFx(&&Tc3?$-X9rg7v3>%4HJGXy>$u55<|YN52tUW<@+F5>*>Kdf>nL1~vdZjM zCa})`i~k^Nv{y}2Ui!87b|2oK_u%$#=6cqVh@^80Wu5@fBO9tHyneVQdw{<+5No;H zq5;s6$N2|*@g|!Q@k4HrjY@*`;*_dTE>&aZ+m+2Vhcxf<+Olr|a@p%+y--V4dC3cM zDT?@;^~?7LOGk(tg87?+ou>nXmuLiZBqXU2Ce;cfpzdt`>?ET9HdUi4OfUxxh8gj~b_r z3Bc?gXW9yX$SP`5n>PYjBOIeOJF=#hygo21a!Ha(xa@(F?(mjAfwH^HP11P03IxwJ z*4yJn12U!WD*)}DHgEm_?=Q_962M1*PimIlJf&LE>1S-H9J?!#+3Pw)9^%Gdbf?%K zxPbH#J9Ae>34kw%l^(UUq?+!rUbYxKdEykP{u+RI%-;LGSVgW%TDjm4o@3`iKq0UM zJM*$jpyq?}zW4#$cONXAJvd2wIKIQ6KA}CF%Cqje^!+2ncKmvk0IhYj!-f8MqkHnW z8A{3nCS5TERJ*SqNP7(K|>hG;fjG7UTmgh?HTyJSiOzeikpYii zR)hfXaepc=_o#IqzLSTqRs99#+g_PAd)BgBbSuNjLAKZi@31|+S+&Ztg~pmBp%utC zb*H#C?;pI&LstF5rSzVi1lAqZ0cP<6OVs4X!gKQjjO^Ga95uKx$(KIiQJo=fs^DoJ zx3ypRoBdU<|NaCx3Ag1Rx0llIE)?i~*GSy<+8@mKY^2(F*Ld_@TJ0i-P@@|%aT@Dtp)_nv^Ml*S*)~kOdF&-c8xPW=+zAN@c7~KLJm0p4|a#XwOP8B zcX{DL|B$~q+?QXYntClghY#zmT10=xRGKNLI=<1i%Go#2`}TTiZPQ((Xg~68s!}G) zUY)UJ)J(t#sOG|pYJw5~@F-g3ZX-Pi=mCBMzzvq}aJY_@;KIPCcZ2pmFO6fr;DZu0 z1DATcZ6NJl+gQr5r$s8=qMrD$ggU@UgQ*;+xw*0I+>`<=S&C`WqAVYjC{5Ezmqcc# zqjrolq-Cgk=m^{bwjU1`?0l>TCXR@>+k7Uc+oy~&rST_6Y6o9=OHFC0DE43W5tFNb zr8`fHj%o~%Wp0^ISEE8?bq&(mL*HwBuT7wkdJ9e1_fO^Gpexr$ePrf{gnKm{4c3~= z=9xYqp~;hj-;_2Q#M32ZW`(6ZF~(!%_a?rQA(m%`%9CS!Kq{s}fF(AAJ~;-b>>>LF zj_T>(YVRMG03|`fnVi+yfhk>c4NKlj)q>J%q^e)DF=5b0=Q2>R%4S>e4zbpysYgLp#KJ- z{~825udmCE`i1r!)zuE!OJmMACCY4002ov JPDHLkV1iyoLgoMf diff --git a/selfdrive/ui/mici/onroad/driver_state.py b/selfdrive/ui/mici/onroad/driver_state.py index 92ff07c1e..44a248dae 100644 --- a/selfdrive/ui/mici/onroad/driver_state.py +++ b/selfdrive/ui/mici/onroad/driver_state.py @@ -3,6 +3,7 @@ import numpy as np import math from cereal import log from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.selfdrive.monitoring.helpers import face_orientation_from_net from openpilot.system.ui.lib.application import gui_app from openpilot.system.ui.widgets import Widget from openpilot.selfdrive.ui.ui_state import ui_state @@ -11,6 +12,7 @@ AlertSize = log.SelfdriveState.AlertSize DEBUG = False +# TODO: Only left for DM preview, remove LOOKING_CENTER_THRESHOLD_UPPER = math.radians(6) LOOKING_CENTER_THRESHOLD_LOWER = math.radians(3) @@ -59,8 +61,6 @@ class DriverStateRenderer(Widget): self._dm_person = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_person.png", cone_and_person_size, cone_and_person_size) self._dm_cone = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_cone.png", cone_and_person_size, cone_and_person_size) - center_size = round(36 / self.BASE_SIZE * self._rect.width) - self._dm_center = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_center.png", center_size, center_size) self._dm_background = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_background.png", int(self._rect.width), int(self._rect.height)) def set_should_draw(self, should_draw: bool): @@ -113,16 +113,7 @@ class DriverStateRenderer(Widget): dest_rect, rl.Vector2(dest_rect.width / 2, dest_rect.height / 2), self._rotation_filter.x - 90, - rl.Color(255, 255, 255, int(255 * self._fade_filter.x * (1 - self._looking_center_filter.x))), - ) - - rl.draw_texture_ex( - self._dm_center, - (int(self._rect.x + (self._rect.width - self._dm_center.width) / 2), - int(self._rect.y + (self._rect.height - self._dm_center.height) / 2)), - 0, - 1.0, - rl.Color(255, 255, 255, int(255 * self._fade_filter.x * self._looking_center_filter.x)), + rl.Color(255, 255, 255, int(255 * self._fade_filter.x)), ) else: @@ -174,11 +165,22 @@ class DriverStateRenderer(Widget): # Get monitoring state driver_data = self.get_driver_data() driver_orient = driver_data.faceOrientation + driver_position = driver_data.facePosition if len(driver_orient) != 3: return - pitch, yaw, roll = driver_orient + # Calibrate orientation so looking straight ahead at road (instead of at device) is (0, 0, 0) + sm = ui_state.sm + if sm.valid['liveCalibration'] and len(sm['liveCalibration'].rpyCalib) == 3: + cal_rpy = sm['liveCalibration'].rpyCalib + else: + cal_rpy = [0.0, 0.0, 0.0] + + _, pitch, yaw = face_orientation_from_net(driver_orient, driver_position, cal_rpy) + pitch += math.radians(6) # calib or DM pose is not accurate, add a fake upward pitch to bias forward + yaw = -yaw # undo sign flip in face_orientation_from_net to match UI convention + pitch = self._pitch_filter.update(pitch) yaw = self._yaw_filter.update(yaw) @@ -192,7 +194,6 @@ class DriverStateRenderer(Widget): if DEBUG: pitchd = math.degrees(pitch) yawd = math.degrees(yaw) - rolld = math.degrees(roll) rl.draw_line_ex((0, 100), (200, 100), 3, rl.RED) rl.draw_line_ex((0, 120), (200, 120), 3, rl.RED) @@ -200,13 +201,11 @@ class DriverStateRenderer(Widget): pitch_x = 100 + pitchd yaw_x = 100 + yawd - roll_x = 100 + rolld rl.draw_circle(int(pitch_x), 100, 5, rl.GREEN) rl.draw_circle(int(yaw_x), 120, 5, rl.GREEN) - rl.draw_circle(int(roll_x), 140, 5, rl.GREEN) # filter head rotation, handling wrap-around - rotation = math.degrees(math.atan2(pitch, yaw)) + rotation = math.degrees(math.atan2(pitch * 2, yaw)) # reduce yaw sensitivity angle_diff = rotation - self._rotation_filter.x angle_diff = ((angle_diff + 180) % 360) - 180 self._rotation_filter.update(self._rotation_filter.x + angle_diff)