From 5d32ed66c35edbcbcfefd482f76a166a49a34ed7 Mon Sep 17 00:00:00 2001 From: whowechina Date: Mon, 2 Dec 2024 14:52:13 +0800 Subject: [PATCH] Fix i2c write timing --- Production/Firmware/chu_pico.uf2 | Bin 157184 -> 157184 bytes firmware/src/vl53l0x.c | 1548 +++++++++++++++--------------- 2 files changed, 776 insertions(+), 772 deletions(-) diff --git a/Production/Firmware/chu_pico.uf2 b/Production/Firmware/chu_pico.uf2 index f89dad485876bc8c3b519bde47905fc091a48dee..af03b0f296754be73b60c2d1199dd952afe6b400 100644 GIT binary patch delta 21253 zcmcJ133yY*+VITD)~1b38=z^^ISx`5rlLP64|J+w_(iwLwx%4S%^uqZ`~ErJRt zpTGf6Y0-p{IXZPVm`Cnrr$?|1+0`Jcbf^D>i} zdDq$Ad1nqyXQP|WMz{GWMcYuUy?nfns=-N@A$uAjdU`6N*X3a?LUNkGI&QUmWb~Ll zNV`LUw4=DEkhO zZdVSExzdX)$kW)V5CrZKWevXL_;<<`INs7HPfKv4X1otENtH_BVQxVbqZ!|eX-X*T z!?Z$pm3urYW7=~-ejU$eKfQ`s`Mv;Fb5VAtm0i{)}*Yw!j8i&gvB7wxS8Evz>M z)5vHoi58Xswex;rG3Rlwdc6#%a0_nA81qGd7>@Q&qh)5Y$VfrMp}`?u z+N3;4khor&Sx^n`$7}+U;nng_J*jCgC^Pq%V9%6l^aZo%Vb`bg7e z;OcAtn4QcvwVSKv(Tp>8_`JrXM!E1wkBl)G=Z#Q}+{c=nGiImd<)BzbE~vQ``lL}G z`qrj2vy9Wqimi|K7>qWScGiq2o}HFck`z0pHtT|?mY!847rfk2y#c?-eWXvEu(?-e z)WS$U18F>@@Ae`S4wZZr%8|0qdgswH!4in7qFptRV*(CWEu^7I#`}|~1RcyTjF1K) zln?3b;G>t*Ib-rIjpuu#7_)NGLOK0xokBR*t7R1QqJ<>?F$@WkixJxW>BeMWvk@4g zP;Pgxn7-5-BfJ2$2B=*HWd<6V6vF0EP9f~;Rkckxxs%<;KEobng;Q>G)9_jL+bZ_4 zdK4qX62&S2ab{SvsS(Wn< zpo=eeVrH}T{IO>CrKCqn&{v;!=Ay!6Fg*88S_YoW{gjr9E04#d=VF}8O{7z?iL0Tr za4h!-EZZ$?n62WUp#(=j-l}F+znF_!C=ZT9?8uxw@+jde?wicS z%#XX>|trqA;mY-9_Y%2nss3@HJnQHdy6CV%O} z33o{mkc(stl73U>3 z{uNkE>YfNJjA0hZAr|uou^1g@5glfc4lJs=-#KQWM#)oc)ZUCKc?cU}#vfJ-vjUWf zaMb}<3|tq2YS=_UE#lOIx37t*p(xI1SI`PBt->zbBj@BoJ&57=89=!zmUhea|Oqdcmg@qC;Ys<&xelVB+Hgaueg!*%*haO@K|KoGUI! z$D5DW6}*d!qWZ|5>^s$D%0M$=j3|ZAd~wE8oCs{KbDuD?mPff1KacYFu zgUFKTR)a26xVo{a>1TtF{HEPXYF-Qe)2k_03F`wNdYg=~XT;!e{n`oQ^Ln#&p zAn|yFbc9^+n`E@@GVo!dgw*4ojrHK9G+&cx#*|zTgY;w~?2MVtsX%Z!_h#X!^fP@; zrb$!FDsrG+A^aZ9a^6~%?_gkB#lf^1eLQBI5o->DeHmAlGv6T>61j2X|C;?0@Up@L zs0IEGfUWSmJPQ;}hv9CoLe2174KAPo2!+j7l!*^;BPVRYM(*he6XV^mZfCg??8#05 zhfJ8j{W2jHk2$KQA!AA^s%?^@{IYf&0PofgIo2*u>=cZYQ zs(8O1)Y-$Wu%<3~$gj-)#e>XoP}1m+o0{d!L58HM`APM-KqcG@wg2*{X_e60>pB%x z@rl|Y4rNq!U#(xr)4>h#T@t;U38Ef2XL2{5L!~X&)2dt^RKA^i5 zcr^nY0C2S%q5XgLCL8G~ba|E&ndi<{G24~9<6^Aa;#ES7-(bvg8p@$mQ23LfZGCSN z{CiL6Uw^NDY8uoN-RCZxO#!ptdTCI72sHV*H+@2+GE{e^*BCa7x;M)iTTYx8mGBjq z=rsEr2ty2X1k%3)Tn0!%SUU@#7GN>J%Hz?taTup@w$d?p54WLoGWcS^!Xue$`E@t(jR)W z=p&gWK9UO>7IMSOv+&d0jB;HD-*d9$U!HQd0I7ssJy8>A6zKY>mny(8e^kQOo)b_@ zI_+g{NBQtoZ+Az7Ur!Xd(G3R-w+aNj(0$z-=z6*LsyCa$+6e#!0Hpv`0Iv2#GtE5K zwz8CQ$%OI#fqe;qcObeME+ft#20k|*R0|*aF(Um|)Pw2p_^%-2zj8=*Y@hNawP5I5 z!6JJ8qiR78IsF4_;cY;P1r$pU8l`q4vwEuXupuc2C2b(HAhqX6$sgy53MJjKZLU*f zdvXxL02;V^DvJ{rciUKlGr4@;14$341aCLhNoLqw?xV`#qut$st^Y09ye54nT8bs)pa3&`mox zZ!Fn}H=f>zb}E4r2OJfUD!QpQGUG_!M#->N3m|aYsw$>_?#p-9%uiNnJaMMj3eKkS zs4HUtV-{eNnf9KKjIcOU6x7{tD=P&r85@L7Ezp<%uo%D!PzWHAa%z^g(T6R$p6vRH zawBxB(#Mrgy(4<1&vz;ZewH48W$GJPa~ZG*wFXEJa`#N1B)bya&W)ONo8lheUVJ=$ zh5aBq4k-imB-V$V-%flv4^98%^?G1JXdY3}Yj^M3~$@yt*vMK}A zTLda(cZmzSYq83{G5ronq&UxjY+GtgD zoKpBXNOjJE3J3S*%%MfM1xsMvy_F#>)_EpZ5aczUvdS`O;R#;xPJ+}H9B03^+~6c* z0Jg*6SKdM>Ej%7qt;5l;!})C#{LJLCX4T|A95gu-%c~pW8w^y8ZC69IJjUi}h*l=q zl(6eQ=G8;X?*nQf(2dbB$gkpFo0VEcI^=aZ)}{auQ(7Qh4A9@v{ewl?Ve6p20e~#m zJ0X1z;M(90oUK!kE`=FAdlY_xtDeoQI1~tWS$nd9b0bK>ffSjLmk}F*m5K6WJtHed zLRoi!l#Pg(?MC3A-o;L)f<|zrhb`RpzK11SDgykef1xv&Jg0cCH27LP#wDNh6T96w)eyXjrgTLb?v% zE-3GWm2C^;=MG0&H{|y~K0JEoS{lzm$#YOL--NU$A^kf59*MLaP<{dOKN*qscSsS) zcaa8X6w(v`@RPL#kWK(N1^HS?7X!Qt++C2a1BlLo#UJQ^cVU;6D?d~UC=l8W!NL7I z)C=vOk7ikR8k=lC-JS%Chf)yw0$u7Lg((ePk5e&UxmI!XI$K zc}Y>1fxtBY$waJ8oalp+C>5xY4-4!lMDNUm8cd;l_?ZKvG7bTnxRI*z_0g%o1WY=}?gH^;~TH{8vQfPxP zR_FOB@cxc73@tGwgck*(Lrp zUP`D6l)~~-<7`UaSBx@b9=pA{1a&leF@sEuYm&XZJlRohCnI5sKhSk&9~mKJ>0$k) z_OhX&AyVuwg@O~kg(0~kK4hJ_aoELTJ2JPhW9(>nGdpS8q^U*D5_VroHJdQkFgFJ& z1&hB1bj4%j1y7a;=SSHKLljf}rCz2|=Oh*w?!`s<?KT4*4B%uA#oBy;uxl-{5Nqfg5~MW3jGwZNf97G&xZ&CL)F+RYWMOu|FC+g94~0`BRRsktWu zlOgD0XASi&P+!qYD2N^v#mr!>EZQ3-914*7;Z3APA@|G5p+oisYS<1m*1pU1POZtc z$VKWSc4)jsl=&8k%mmf=ABjI0fRiU05aFi(*y`>;kQr#>@A*$+&Id}Q%_vcq@Bd>X zJ2Ywyv)czkeZd`FH54K?|5#;LzR>e3!c0Vuv4x@@g%UjoU!7_E6k^`xr4=mVEU1MA z69>Tit8WE@U}9Iu#IBns-qlkbiiGJ*V*9zF_Lzwh&i0Y^#40sCc1c`~7+rCQbQ+un zT*jnZp?0i(8bsZSA;JbH`6pz&;!u?E6xZuYjY!IA*65u?Anm4eZ>}4faMZt> zkstaL)HBqR(uv*N_Ir~PTOnYVP`?ujGP}h|LA2`+rQKtWE{2g!)O>|A-kXZGgqC(S zAWgC-)H6*4G2r7)uNxA-z(0ePAM&j0r}qQ5?OsDnMMz-6u!DQ$-l3!B`)9IDJw#B! zF%qW(se9W;cvB4FjrN*QF-ur7+>LwFqh9pED1_RG-8fCXE?x|IiY=-9C_x2R1WbHv z6xQki$kG;6B4p;~*Jt3B+#~hI=zsLu*)-^#+qt9l#>CHiv+8kv4LW~}X=0fo=s8kX z#9gm94A~cMnc*Ag^$7P76UAPeDCxCoHZy#-%0)Qe3Zft4vhFjce&p*hx@`Z-*@l8S zGB`HO2r=q(q}5}gmz&XGh@Kl_g@xW-{2I@n|4=!WD(Q$*E8O{>GIV&M@&DfS9X8`Ah5vGEWU6%zjsj%z5^ zMuQ;Ei1?I`yWCI^VasVA&{e=t9NEflW#v1ukQx-1)oOO-l>VdU+|aVx08Gf7$3jdH zG2$$p%i?@B%wIB3qlCw~x(8m4UK#8$K3KX<+$rGNrg6mNr9q6Q!vI+rL^l_CHXlP- zw|c;}FshEMHa!cSW^$$1*SWCpUx9(HLgFxjnIy{}2cB7!@B>%Z^jbu6L$PSEVu`^v zfpEq^k)5L-)Q@Z=C)ALG0Je^A8a`4~8OQtQl zs1a~Y_*gEYIeJSr-o{PYVo-Q}SlGbbwI!)>CB(YOSneM#WaEeuKJxY3r?yWV>*DBc zVNEcc=7!pQ<74V;R5y9BP}wJMQM8KgPrfGw+@G8dOk-iI|BN>_Jn>VZOUL$!YifUA zWFSfx>f9%M)ht;ZNF#G!#4MRM(JVHJS*pWpstIKvD@*hyb8QQE^TYW?d)wrPlkYug z?-|j`rocE*!Wj77-@moK7Fzk$hl~(4C0$VM1DB>g_DBmD`V#l#!%2yYeQ;F(D*-l< zm6O!&gSs=^M-L|&GJJosH?xRoVQp;9)|$UCP5t$4T=3yUO6v=Y{fK!11-|HRJ9wjI zmp!{+iPPOn+BO5NCm;$)tgSv2T329aATfk4xStX0ANGs-kyV7~MkFPBj@s8)$<*&= zXRyka2e|cHQ>i6>?!N8G&Ub=@S!d~6z{&xeMOuVar6ke|64TQ6dmO2jwm=N2nPLmUz%Dh{fdebA@PY|7N5rE9lR zV?+I_VABCDqH!ZDYRpPBepbR++(X4}l1y%ufPz!Yu6pE$pW2A%wP{ZkuPuokp)#de z<%erF?qQo*#ZJVSJ9CJ`3$dsl!Oi>~fJ{*zq^?9@1UrVJWoMbrt*0C12cz|0X}8zb zu}M~SF*e4R$lQu%ob&cE#crIRlcjSXFS+*BLKf4>)}2-g>1p{ZUuq}m8*6bsZyS%~ zuN3M_X60o4;7Nh|5Y0<6!QVrSC_lWPHPT9xqf~uRCir?pbBzUa?W!ck3a?s2xm={m z7l*^1z=5#EBIe;V|D|i(2KLMEw_nJ7$^T0Wtq%|jZ3OpKWGWlfdY~tX-=q>80U~PR zh%L<0(tT_`^gijJ>phVLwh=t%{yQ$Q3nqh?OFI9Hfkwq*N10G=XaT^CN|+PS!sxDG z$;My98eO5`G6>_}MKJ`#1x=*>Qn)SEq>tF6v0NN3<&xnt7kK|WB}G8ls)v~v>~i>3 zt<|MzXpW;eWDug<_CZpJiv=85A!14l;_X}Qyv4q+WJU)ST3*TW(K7;;r;%7_ybD}m zvc`zrM^+NV?Sn~A<*S4b`>b%e@Hao1OSiZ!A$H=*Gc)YV_KPX{tSaGH9|DZ$Ll_o! zQW%4*nd2hy4tn2)s~v@q@B7)i->8P% zXy0}3`$iSyGJKc4WD4ZL6i~|%!q^%z+5Jw$(wt_GPDR{GEF9pD?itEu z@43v;{7;Ns>;b!^2OjsYNMT%Y%p%yCQ@g9-r1r$X3}>oDgenPw)DGpaTyk{t00O6J z(P_zXO*^HM6*~v+8R?e(7OiG&|3jBY+f*DGy&S#2zZS<2nISQ2B))<qsJ#YVDF8HZqh2~6HGCG*8UZBzcu1$E?EBd|@U*bQHQ7pf znm#1abG9hC*dTMgiQq4R(0w6U#lRnE~zZbVGS+%6*AEkW*-APx4n-&-1WAR5jmN;BGI`Lq87W%w4Ze|CYM18 zK^P-PA3FeOZylR9fHqG=n-Hk6!b&-84l)F8)P|l! zRZ`_hcnIDL?c|F8^3MoGPIFSHl?i8iPQ+ksJp_4w`Ayrz(I*GgVHSuy7f5(!Ikv)m zFQ%;}iy%h#qno4uHUa;goBy}t5yFf^L~}^cTAisg-u!LB;{=e20^j&_KcBbES_}?x zvTYAr$Bwq+mR2W6zujMp=f4jiaGLjYS|NF{!aTIJ z6eV?Y}2M{>TCps;uLm4W!V;K@I z_1IE4`V+}STP~7ZE+L6LyF2aEs_ZKB?WHYccXV*{xqgzLnYEv~ne=8>p8bkPW^VGL z437T1zm^RBj>OnujJ6#k{Z=a9u$7}b_Q*P8F!xQzQ0{C;tU?YC^xo$#b}(9!^Y@ac zm&!_Ih2%L8bC)R7T@sm;l`6P)*P<9_v#rvi3qDC2v__NOfg2$iS}UE zH~uDuqc5z4=-ZA9o5LgYB3TZHth#O0?RO8mJJ@w4IAkeDe=Q;E5{bGbL~%Ykg)$fq z%F=sKW-JW*&H)5Y^Q}&MUVkR}v%%IQrXx7R(xv1`uEwH8+to8_nlw`xGK-z5n5iwO5i5MBc! zYR%cW>NJDmO!7IJDXxkIGqO;%RvpgZC@2 zS!=C~Yu&>xDn%xPBG_g3@w5{E$}_X6uZXL}Y!NALkx)cPZw<;28f8oQ;oGsvlXyBx zA_K2cgJLs->YXvW0$ts~2iZW^t3jSt4{8$&Z5G*o@fIPtbb$Anfv(m7Psc_i(|}}f zGBM>a3ro|pl^x7JT)@*e;dSw@o4|oWos;Tg!S4as%0GV-uF-CREV+c63NN9_JLrfN zd5so${i!VlSz0+T^gk>AY2U{3v=(^v-zYHKL@l>Tv^=fPug@t=MmghEC_3}EFL^p) zkOo75Dn18~lKs)G@(&Z_39-9@>JUjA=24OAqY|q7*(Ieo3w7-L_85ByybI>(VYo2< zUys1ODO{551SQS^(EPGt_{ES4ygw04k%{<{-#pK<6XAK#Y*y3C(|ReR9U`M05=Io$ zY%8;156vXmRHkUaNX7$$kJqGt_{fuVNd*u23gKl+o$teAJe?w?`nX8-aS2tNAHzkr z79d0YY<55E0P)lC7W^gud>URMu{aIHDGsjuxM7DgVU;qX8fe--wt2r1n^g&W6VVj5 zg>NztW7`J!{?5`Be92{I|8TU9sGt7_-4G3KR5!{#B2rMN``QewmpjQ z2)!TYH5L$m2*O#)4sz3I1Xtk*ObZJxSCXwU*3Yl9;AbK-F?&Us_DWZXRYcfxdmUJJl$-%m4U=(7Y<&D``$7ko@%L6p`i8S{~XbPvy z%YO9g?Ws~c6{N@hAjy-`QNOE>&p53@UhsYIS1c_bO4a53;aD}acg8|@|r^E427^f z2)EUzU?CUEx{-ya3nko`Cq-^gO1NcRByG2Mzi6P)>m}$SPmjm1<4*p|@p#}>9j}=H z#BJP=dAby@!#VukQhfhFFFq?0e^x>~#ySUm5Isu&vOW{x5jlB04Pk|F?@b1kEcHlr zIxg7tMqsMfS+QgaPnS!WGtY|@pO;WvxPh*J)`Nt_8NVoD zENjVH);w9UGs+_SQr7Zd2~STQw2n;&s%Qkt1gcjk)1nQ{5Od<`=@E5jU~|U8QtgJI ztE-PXhy+#l_A5L+ z3ztU|lv2<dJ5%-hUV9Qo{bQ;Zs*7dzQh*YFm$< zr&kT4;Q|^DvQHJxM8U#6>?W2Pu7sKC!Y%k){;w`PJ2LR{-Vg|XoTRkZJ;*r^t};X*MqJvHeG3AHX#dk%itp{dKpjG zO9nM_N~Cy7LQ!mcEnp$*`0z#Bc=|qx47{cRB}K!$z*sP_AHyrv1Lo_3N4wb)ouA`_nMBeFJysq+3!pbPmb zsOGMhCL_xTH#M>mmY4?zNwx)ORF}A|i>h&l1}R!JM?U6Ja$5~}cmdw46{ zPOd;7&YK&9JUqP}Fo?dq|54E8LVmJ?LLGTS9rM^^M>TMLOiJ+|BE^45C~Di2=R?v!y4ECuWvJN zLG)Svjh(nZf(FNiGa{oi5=L)NMOlid-W z+aVxk?&aw{QqeyYDSjxSh)sLgy!y`4qWYa>H_jKqq>PUmjB2cCr%BMql2DBRj`Lprx&IvA(ssM^Kxe`7XU;l*?71@ad7geDjKTX~1eL5MU0xrUirkNE z0iAjJB`L+vM2eqDD9T?M-_%+!GmS&9r~cJNHnu}>5^Ca~KQ!nhbOgl3*-;n2OFbWT z9{ra5Bzp;tSx2PAzYvLkAtA18lkPNk!0P*F2?noe1FF8RXT+;49J*2jx3vvl7(#?c zC>Y&!E*K;VT_^}Y#^e1*K~JH5R@yVg4s-uwRh~XJi0}y@OrvF`JzaNn)AgrVTgjYa zo<1RI!<-kzJueZr9kx#FiO*KKiK}r67M)im7`*0npc?0gZKvmDxRpE6D^HsZJ@9(C z4ex&gBy*eh!CjkKxzD=LsX}U4g#R*##z~;@MyDD=1zlZV1TyQ_I`>$4`XsdJA9&0+ zqGaDlBx@;CTx8&&Lb;nKlP3l|{kK6>PXkrT2w%AsDf5C|R|6~9w5dFOTGEEOBvQR3 zp~|fe!r)&4@0AKt*9=J7Nu8731 zNQh&)t7}ECoFR(J4h;u!KEQkMe!l$!TuZDMUg*yP54g$!OW|A5=-)ld*;G4EpAE~w z`#%EBeyyk0E@wYojq_Dtuf?!Ne>_M|cgUW>uIoKaeQFlvSyZnAN75~6!(0~yye<)t zSpnn6{oRVw$#As_k572|6A1>dIR{jAwr4lJ2(DbPD;OkSU-R@iT%L3UPK}!HfgrGuwG$2sv!fY^*b zkoG#jMu1a%-RJm~@CN$@Hpj#A6qKh~9KU*EnS96hUWiR};6w!xcs0J3jLkCJ}G=t3igi0EQ^c zxr_>uq9#1!l#Sd|ax0wBFG$)jzlc0mj!73|`X-RG);0 z)h}Ois*z_;r`$pgW}V?Sy#Eq(eAL)?9WObo>NZ%KuR$Bp$JMpqn|wcr#*aW_zd@GW-61o}i&~>_ek<_# zQPPI#5+&=BNS1Gf74QEIG?T+8v%#*%x)HNz zaj;9@&C|b2q~BghP694%7ioWU}xYA8552E*Ig|y7m$YuD~Bp`(TaB46hbUpQuis zL>-ik({KS;$%swZj38XwGe}%N#GkaicnELyL$m4pTt9v-#FY2eH5e@f@&U+yO3*-$Ysk%M$jxoY&1=ZbZ`cbTZuD!Bz~4ks3EDMl z*NvyyWsuOz)-7362jTL8Ht|EjpCbe*3DG6v`7bFdlb;}`&h-ce9(v~<11iLV3f0cL*V8MOY@pZFt*6&40Tp7Ybrb#Y1&uri!AJl- zfEmC7kOOcU>fQ$+-}R6$dB|5gPgrbGRGhzz7P2gJ@dzR1`e% zsvLnsL~A8R;57+gNa$%~bKXNwU$*FO+IIIU2(43lhMt3m3>N@80h-nDxB;MwKa)TW z1d$jhBBH7#qRJ3bMjPWX!r+Uk6q_LiA7MB&!6YKYND$OXI3s6U)V^DGQ4cBhj=G1Z zheZ%DbhREN%mE4WM(2(u=_2Pn3n1Q3r8JWNuSSVxPM>9Z=1Dz-=}G?K6d4s8&i^$y z;Zj|Z_cOpAh_yZd@HUja8cr8NIyIaxB>5M_ z`piS{tu}xbfV3yjb5Jbn|4mXde&LU|%%(o${OR!D@PurrmDo_)oaDiMqA8|vVjI$y zLHKYNK+-XI#sq;s5t zGeA{YDT)T$0V;I>4#HqZ{vDc{LDt=l(48loiAmqXniAP-2qSfI#gww~^wRYU?p?yu zCRjAgp|}eJ%U-zdu4PN>)+~W#ny1ZFePhc9u=)ZtK=6X3|FO_rn5{>$0sRjB=fo@t@#stXsocX_#^+#y`)&ByguAkX|O8)|g1pZUq z|7qD8{^AHKUouH7FiFOO31&^r&zn+RX3NRRuZAl;YF1JKoRB8~lmb)%%mSzd$mVxg zsDAf6qkrQVDzO-bKW*r*B=pA{GbtUPpGob=HN#4u126%g9AFlJ17IbOe#GhFEdQ+pJ5ViG3QZ699$Tv9R??PNd;Gglz4&5`$xf+yL&Wc;`2sR zUk^~xi&XRyDzeqG5H>O;J2t_~FrLnVaSPEs`2W;$`;|Q^OA z!jwj7HY9xIKL9Zcc*|;pi##U=)jyebqo-@(sxKl2oa&4sMWck`x3f};I`v4{(EY&`Wcn5YpCwc|94VHSP%jR2 zUf5GHNf4`&irL1ba>JpLX!peW;ou&YMp%`Bd&q)&_@Mr)2UF@^hi`w6);>K8ldC@J zT9g&Wn9b=Lcm=_jMY+rp^2pdUJk3e&iCzlm>~~?f1*G<12j5;!)r92Y{S~0_W*9ce zBKIiTVnf#-aP=3F0u&x05*{HTjE}^^T`CRt<{iGQdIsdKqE^NafjK~K+_nOQE(-Z4 ztEjaD1{f(48Yv+JchIAC?tB!LU&sF997itys5yrN7naX!hg~>N+HoOY+PlJw_4jloDP5)ox^dVXp zH`A%Y==Z_7?gY5Z*G;Dk=9BPf*AM=!98x_14Nw433NQ=63BVcQjC5i;Rdv(Li(uO1 zBUA-2E3~?OO{POthG?k_iKS*jlPT2zHiW9-3tAh%j(te`Pz%z23F$saL*fC>uKjQl z1UL$}^e^)jGpYQ>+<(Av0v5p(fKJ#fd;sv41s+^NsJLG(F}(o^wxO*j#(fU^@fRZ5 zgAgIbUn1?euV9~s2w^(}crRWM>)!en{zJv@pzboXhiKZitH2tMuUTeMf5$}M*EF-K+m-O;m6l&zONoy`c>jDDnr?_4bcCT3Qn<5} z4o$!y`*@DTPVQvD2ty~NaDy4asDqwd2tDaGK$|dhLJD`5kv1^k;SfNm=1Hlh07e)( RA$^TrOr6E@oV$;z{|D6driTCk delta 21349 zcmch933OA{`tRN+L&wn5258!JI2kDE0Hy;_Xo0k84{cLsM4&}eCc_|>NsFQ_f?5Rs z0tY-Dz>7FB=(Pe>^opXQR}?i7)GDAwfjE0muT7gYdEd@S)8zi&|GjnBTkEmb*L`yK z_x-+M&%>eVTvXG!s16q;J1`8>7moK+H8|-4Jf22~nURW^C3%=eNXige!mW{xj2f{A z(T~ayoyt8ay*tw5MzqXAck-IK)FsY?<2i-=CN+7BI=6JJWU= zW@N(OxyK?ir|DP zh&FkvtVJ&T+8Zkj>5UQI?~4*ty;#`Oud8N|Oej1a5v9g4pL$lXj81vCOc=(EicT+C zti#u{#rIr=RA}-&Yk97^m3_|A4%Egv zQ!s-xrqU>3A)sCKkce52JEQNG;Gx`YLo&yF;w6D&oRuuCoxw&3`}=6!EEZ`fU{v^q zIvK6}FcI4BW^^l989Ki~CVa%58lqE<^d2fhqmTnT%UNRs9vdq(9RFiTCB|=Y6X~L1 z9sP*z06XbUsv=6b--Q_ZR1^0oJu-b}pVp$X&bQuh#$@(5vUI+D#APdL58WgLoVN|(N z;jI6;mNCNfJ1z#%5dTYK5L4eNMd=2*D^?>HHugqZOz7POu^I-+g|)}s!^h$ARW8EX z0$N?2AG1@~rcQnJ0!C+x9kHM>sZlC?&?nKUH48E#Mz*qA+swIXc}5hglL{2KQk697 z9oM>)7FK67u_E{3KDEZoGPatGlDTQd(xliiwb_>(walC%sqhW=l1h#L#(ktpEWXz* z(a=zn--0wA(o=4v#r`*+`k%zhW*vD{qQ z%BoO|ASI4ff?!p^GdAd1P0+KsZoPbzL+0&_T{JUk%%ow6O$n)K1ARtkWVBXowNs>%)e8$1Il8DeIrj*ELCnnLe|h zQ;P^E;!GM!M>t>VQ&~uU$^Vf5WM8~LKUWoC5LHn@RlANWQq@?N;zhKS8Co5-Z%WUhwE z#!~KKc>Wjn3^M|M%YDcs;m?j=V`3;gox7n;A`-fCjMKT9`p#slrwXblW|XG4tB^)% zl?u0bq}tP#WNS!xGsfW(t~^77joj@SBb814PfyKmK-p6J0^JhLJhM!=n|m@NGk#J3 z%CNT!xQiK?>hk`hVQ(k)za95S;zOmgOSB=a-H;n^=>I8RErGUH?Vvp0sSl5pD$ znd!fJR>E6kZ|;64?sq@f$~E&WH6Wj0mkQ?N4#RC2E4kdP^td~HMHcl!oc*cf;MMMY`RnNj1O3!sZ2w6YETDv@p#l6}mY-D;3vqomrZwT2H-GT@fjK%6*fS znDt(7XQ|5}-xskznu!#Ca1~qcv#xWl8jl!N9`tXgr6v^=BeF|PArZ_OnM4Y=A_p<1 zp@?aQE=3BzBY|jcYK|F?Pv+12FUXSx@|ur-GWt$DYzAb?{h1c>WF|ux%gr{L)kD2kB3v}^OUa3}RQ<`$q|HRLppM9ek6dw@l&M-vmtr3FSmpm=G(r8l% z9X@18v`2$4bAsrzr{3Z`{rv&COk;1U!pN)0kq~We6?aZmUUZ4R-gv>U6BorrbBhNRHJ729Su`JH2?*|R+NQXxRJ#haT51r@x=J|q2JD~Pp~B0 zfUFXsfcvdD6^}Tsn3#@nGdF4yk00jVoisi6Hkc#&dgl7b!0dPh{ zSj1x<6}Yp5TWLyNdY?z0^P29OZc(x{qk!Hh4~?l#bOj8;}&N(RiLE??H-ny8yQV zbTtF*1gh`U0RKboWDPTwsmQh={rtHRx<}-@<6=zP;v)o!N3F@WsVm^E(CO&~0kJKGjpK_)K|!lw|T z)9fBFh6HFIq$hwb0L3CqmjP7+%>k-C9%UYn@o>&uHU@9yHkM6+dE8rN;|!-EQ-CR# z0zKG)4SG9eklKsDgcti{&}3q53{t?nys$D5uktK>f*Z-E8N$EjB;rpM5_$qbqxzTQR1u0udwE%Q3wko@uCkKSSD-DVA)oJXCsd zj-(HnBUypbz>TQL#;x4U3T5VFeXp1P%Td7=pa@}GUt}?ZygeVesR9h`CqiiII{~?* z(4OHQtr)TTwcaS0*Aqv6=!FG_Jpv58(EEea+w+3^x-*5sbPkXnr~s%KsJ=5%&**k^ ztSZyhON2bn;JAcfb|6kO>NPljILx^p^F<5qcrYU6mfwe&K>e?z5*kHEMua#LElB(2 zhoS{#|4J4y3%5rL9?wcvb$_(*8bHJX1k;B`Mca`+dYb&GI?0HVHj-8`tnc~Kzb+D& zCiKR3)Sn{5)eJDy3bYDH=zNbQWr#W{19J%(a_u85E!?A3nTfT%W>#%Wu2^t?()|&_ zH@#FhX^AtrkE%wDe!th-bHF1_Q(RU|#l_vnyggIA(J-G|;n{qQ1oC7r%K6Jc|Z_cFFE_m$pl`T+9?N3+d-oY(ta1!L0TrM z11{i45=?h<+o~(4eeBA&)htYoP&neWv6Y-z;fSt^0f}mmBrWm{7paMH+DOQ|VV744 zPEv;mPBmcU0L=k10vUipteloj-|fPNTt`krWd$re1h0>)n09N_5|`_g5o$y6@fWAP ziWL_?ilD86w3XX1W3uG3?-6d)oSS7EK>PUP@iW!9G23-#gT7wXprS`9%Rhp-kde8XsWLi`sYbH zqGG*cN+l6q>nN`(2adD8YtG4#7Wu|oCRM0yqz=Ha>HErA2yczY<7O*yR0pi{M!`=l zmp!K@_kN$&mRK=+Q~V}16=UADDM}h+c5I50Cz<6iI`43*fb&~#wD4CiM#JHG8TZPZ z)M`>7hoqQ}1tOtTK{^L$pr8jTOK5>DA-q7OTW^AN4^UTV0jjda_LCjben<&JM=*C3 z{x5F!T;0lpUSE%?F9!^{8;sfxMrFaX+dCM@v5t|IBjMc*|GSJ}6o2r3;#^|WRWeX7 zo`RWx9;OydWvs9tp5Js#*2d|ES8Hs|wGtCzw$F(Y?md2Gt{G<%PtOp`!OQ=h2gE>9 zB@Nuvd7Q}uobwGZJjpPv0!qq2bOEHrKvB@o)k3-i=ni;qhYoQ)JkKA2=+p4L6`li? zdA_0X47}L`Zx(71{XC>6fbd8}zYp)v!SgQ~L|=te0J_Y;92V@11%l}{t%o!R=oCCx zLplfOUC?fYbO}%t1o{Htz@)??sgQpt7hL_Msji24@d?NmIzJ!Hvg~v=*>buw3HlPb z@O{6xC)!7$7?cRVuJk8b5-XBya^X@xsT9*W$AVG#ZO*qKDe?jUbOD95%i6?=-@_Xh z(3yPbmq#IHXBJ|LVU7|T0X<1)1Yiv9ms`}f7F&$q?q34?NFR794T7~a@~ODG+7Z~! zJzQHJ^MWTAQPAnf3YU(5QM(1>Wn9gz;~QIt5~~MyZXWHB3#q>C7G#KUsOlAAtKsB^ zoh-_Vc3^FCJtds+ma-4{_f0lilIwqPHdT{(me!6Kxo`+}zq&6+8shDZhB$k);a0oC zP-}0f*4g^AFLd2qWuGG#uDW5}rpVV>&pFptDQzhG{mVhm8;xRxHilGaT(q`AX_dp; z{%t2E?D4LE#$01-N^UJdnUrIdrKQyVCDtLWu6~(ic}22yxrK!JagVp>WIqW%>0=)A zEVC^0hyM}JQuy8PUJCnCYEVA>@?s0px3TbdL@V34X!_!5MYbxoHDxxNFke02h~&Z= zPYtl*G17wlrNYHpORXPhnP;yPX)7&N)k(H!X3?BvF!3QbgXTgDk$NNz5eQ|Q>`2$j z&YDU+pNXL41ecq!B7@vc*7{e@6yngaj#3{QA zq2L1Hsna)ZagT?>gv}b5vLS=af!a~F#bIXg9>n1S>6nA%lzp@fjsD(&+i`x39k*Ao zZD_28I(&X!%M_zU1(AvK=$&~GgUxQOh4H&0hgcc>u}XvYK6jcW&Huj7oeID2a1-}Y z_BWSP#`{C1_J>M6UH#u8^{t<8dVF48L!!O~wu@ky ztRvj3%EWvv!uchBo_KEIDhsaR4zEh3PkN`oMvaA4H?)CFxtptBJ#^BuUh-l@6M0qO zCLolKfN0T7*2JQ{k-}bY4ci6PW0&@wT5bK}`T>Ac;0`*%DOM}>8%xAV9igd%l9XE`f&uR(*A<3)qVhZxKm)*5gGH*>98xwQXK zxzn*FLwfY<^%gd@ff59+ajh<@s$cx|6?YW4YWrcq-jGqRG?VfoT<>sSu2tu2`jL*G z+QQz1qQ|XlrpDB~Mnawj`Gk@KKU-9Wg;bHA)tGd%3|CBPU& z2^s&UO_T}-cxaIr;G&QKALTOdO0~rPZ`Iuaz>_Tr4Z0=*-scLff&(VicxJNFBaZb0 zCOElWcd4hA`$IrXyx^CDO(}oN8jzarnZ@cFVE-28nqs>myl;ePKzp@_gey`==X&o- zk9*1$T0aN37*mQxpNd0#B63uIq#)sP8+7p==%OMc#je#SM`$Rwv>_AQxW^haQSZ3L zIxw4iwLz2ksXMy?=hvW%$8=4st_bQ6sUPFG8x87|y#dYQsp%eW2sj1u8(*GxZhw>7N+3q9PLd(=@k`?0ZbjoW)q zI$p|k-g8sx<}j&yT=jk_rjAIxNTgyF0jd4IK7Tm;hj7YCVmKy+goA6^G&EJTC@tvW z;-)nL53#VtZ^ad8#RPo~ci*N|{5#jasf3n+6}Euw6j}ZJys01(>h{QXwt1kJzfUid z?!-b8H|qYRm<9kL9SRnp(cYhkw{y$ye<8u<>(e|?whd-#l(5K$(To9Ea%$K#4ByIS zH>Kl8I7?G1c5=&`UWwcc2&vv8OE9!!OGL*?LL3tZiZ)j4d$DdlG?N(+_j}2T)o(;J zXL&%4cW}2q@M>fRoEwtqacwRKp`S=T@i=5a4>O|o8e=^APxhB&am_DkDj?c(Gt z-;n~7Gh-KKu&~v0))^b<08*h0^7?0ook(ON%3q^)yJoYblOQ|{@QA%`&?8RqkTOn- zGLeZTeK2VQP29ML@^5~$LwYoM!yA^qjCM8!icby&CU^!0+E=F%FQblVU}u#KuhU&H zrD=$Mz6}cDvPW!pVkG==03Hm;R_@J*hN<86h-W8(qmO(o5{Kv!t|z(Q9!k_c?-4y2 zJQ@-6V_g)?^7dG|%L3LvtiQ!ve$}xHIymJ9qQ3>S?}q(D((6PWAr_MHh3qQ)0Vg|R z{sVwcVnF%|;xVxw>mmiYYjc&ZVK+OImA5r1se}A*O{7oOx+FkY*XtKgck`~m& zhHTM@y;aoVXov^V*b3*Z2ZK6ux&CG~ecYE29A|2v-Gr3EBT5MtlDX_h)I|-xG8o$R zMNice9t<mr0qu%2+e3ezQh72=yz^GoD`K3UXW?2CrE zIMTJSsncF6Th}Z-Qu7+8c%X(&2eBvo;6WD~S+U`;Aq_`7bQu^wVHyte04TMw5kjAL zTKP4H^ym|t5VJ1riIR1tu^AECG?Vmb&89spl=4WVlkU`XpJoSU374S9z6wNI>-Uh> zCn6+3Ds9_Y9%rv(8^59t)+U*vOA<>Zc4-SXzI{x(8|NFd753w$KYz7|EkwztohAx> zNn2}i{!X)Xju93T(j!gzV%6%)Xo2w?3njt1rWi07o}dCn2v5SqPpn|Z!2D7yDpDQf z`G#S@veSD2VkLcC#-ouBS05r|d6QW59VVK+B2|y7bp6G?C29zfR z3%2RjdCOpubFW2ugahsr;P&oL{q1G!U9g%*kZb=y&Ne3!f0!=nVlB`@A~z73*Z!N#pdl(RoR1&& ziO4+Z9@|DBmc2PRXG=^F>7D6hO-g<4o&Pn{>CU3z$SSqhqGoopc}Bv20pLBazS zAzbUPvyHX1md@;=pbhx)NwE4dNY93al`tJ9eFpN^Z^UUJw5`yJsr(4xY`>`y&KXX7 zND~@o9~lzo88CAB#r(&`#K@>4BxGE$7m*DtSaH0mQmiSJAvGn}^{cZ*69pGoa$G++&uHZEG}3j|sf8&_ zv=kxyz0a3E4N@4+^xVST@6cr4h=l|GDI6*8U`13`iSSh+@SJgvKtJYLnWC$Y(=D!t zQMPw>=xF@7k~2OoaP;1OWwj_>9VXo(#F4cbJ+;a|G(va-CEKd!jBNrvaMfjdM+8SI zXV4f<;fnex9gvcZan#w; zfea;f+28XBqyHw0a<-^^c9`;Ke;&FAgc^Vo&9PaBe;*F;|qB z8z%Amjks*|)xAzLyNrd(vj^`D2)qDZ|6QP2Hc~VH*2uAz6rFtvyCWpd^F+9LVQ?Mq z!wsU|j*+y%BGK++?K*9}S`HgQ{t6*ZM(2xi^TXth9RNAUvFY@8*2T@T7M#!2)kx(n zHL*eSImHPmr4s@@v@70@1g19+6`_RW?Il4DWsQhj6Ndb93*N<@efbu8ov;2BOsgtQ zr7ow>=W7hey#juoyrka+@rN?izL?)SuvYM@=np~+RTc1=FRe^*D;cihAH}kbmXo+yH@97v!+h8;Iw?F8n zLE~=~A#V+Vfk4A}CoJ_Kjj!jB`(39=K8UwW#9J1I zcTS*}lr6SxX8%@-zP#O#O+}cA48v@ZO7lK5^%EBV0jSDpC6Rq6n2P% zU7$%G%8amEmm|2}Yga(vJUDehpwHWN z=`e@LdC?)!H#w32mR#T}<>#RAbAfJ|LZ2G0qdPHDHBG?Go5_X%&`n`!8*Q?s4eqn^)g*=kPmyJ=c6gmj#e7 zgNI*3UCc+PP^Qj-vQ-Y06+2j?mGz?D`Y^rS0)0_+Hu>YoW#+N5FPCx0)|m$_T_Z}Y z36uC%prwv>^mtspc{ifQT#BI_K?zQA6)e3f&@Yt?i%pANY6ev4O|g?gFxQEg>%uUv zmnkK8AzF1J5zY@hEaoClp>Gu2!`D;syWBI^$zIrJ*DZ9^1&c}Y+4fT8c%amGsDe{N#57uhUkxA(~sw{gX{lM$Gz@~ihy&VxD%4sgJvs1H^KK^XoL=xx-$ob@nK&Vid#*iy41$f4XMVr~k< zblIpoHgVn`wGcTK)WFYN7Md#^`l%Adw4G$zlPzV{1H)D?c-be=F`uUAEb5AFyE)CM z#obI!Pywg#fC(OfX7rCZvvsj;%A0Y)6HYuS$e|1zzc+@N;Jq=#{=t>m>|1OU*Q7JF zT_pVeFzi)ew5K+e@o8>4Dt+B+T z3KoeQI=n}M^}JCE`KGG4_C4(4GNe_@d_5Ld&`4e($4cHP$G;CznS2t2MwwH7`gUyc zqC4em#i+tGjPNI`~n4&wSr)hCv10|klWc?C^W(V*(ID!;*4n2d}QB2ZM;A72K| zQEn5x+!p3#0*K%Xa5>o%)h_)oL7EV|I}~;(fgS4iU}+i7MqN9rMCn@0 zU5_xS~?$`f#8Xldz-^`%{RI`7vBn zdjV26%w_kn))4z26Hy-vLrue5@manz4X>mr&H_E$@S|CH(GRe6S

NydW3qh6a@%7vZmx<`tBVU(vsHVKdmt^3P_$_{U22RKK@!K-+ zP52J})eKyd(f|`K3BpBmXCxYmaJIaQoPBCw_V9dYn=ueW47fs021<{czu$ll(j8Du z$!tFsMcH7VSp2eq_hgb#WP#Ce4s-OI)MyOp*$q1+P z%YSyNEU7lwwi%@zuRZ)jdAN-p3ga5ej<(Oxaf@fJR22p4Xg+bY z0HAk3N0V&*!kGg@6Ke9j87v)Dy9nJL0^I=Ducq>2#$vPL417NFJ`j;R$3Hk0x6?9V zg^xk?r=WL-d!@(_gyI#2#FIi$mwlP!KGOS~nnEW_(cp2na=)l2ey>yIe9{M0^l9KWkYa%@Z;<4$%d++cI7i7=9GHVEqKHHpNJsIT=Rnh~*VcD#Z-_<7r5NsS)Sn7!R~{9$9}UsY z8|Q*Qy0hl9aR&5<6=~1nk@?>vvXiLq^aAHV1gmeB$#0EX0xT zSA0EhKx?5KXoSDX`0E?5sKrD1+*?$HuR`%WC?OO=+i=tsKh8cD%Xq{}p zT}Eh$2EcyK*E@I_k5^cUWme!RM14KEec!^#^L3RGo`cpaUlL(o3WHq)BK_X%hHkjQ zV1+o>0{KOF&tl@t63Bny*mfx7`h2ivkmr(N$tnlVS@Aq?7Ix2zA2)<8HRd4pq{poE4Nrv zSt^0MbwN8DiJeV=2p{#-+;M0MGGy4Pk&PjJ&RZhlTVaR~0OIWOh4!1FT&Om+ZPr3G zB4`A!*h=i&3OsG>K_><0F>UM^dJ=6qlAXN&hPzA(er(+E3I zK@ncjOkkUV2QGP!Xot%cE76Aw=Eop$vC4b45oaEO{7d=CRtk0H4YMv_ldT~;v%F#l z!Po&jbY~LN+w-T_u4~d>(WvD?J<5NIru{R_wErSempbfBkqsBOOzA#WWD830ik$>? zC*Gkr1YIAw2%I$zF|YGycj8;R+Iy|Uec zwQKH?=RX3QVS2Y;g!gd7zdewTwR_mShVHVWhMna%E*6Ef+Yd#^55piI2N6|;r{@*# z^UlIz#OTL{?4|OG7OcFv0$w-3W&T2#i|fJmrpH@QR~lk2fj@=hl0Ur91TiT*za9_w z72z66H4yQW_&W!lYk|%H|LZ_Gz>~s1*aDN6NNr!wH$D?P9z|-SbK{MJ{(mGI`%##& zZP0hCEK$aLt+cLaZ67p&LLw39yZ4bm@5A~^*kPgtP+|xA6BKqsYJhi#AgzTo5>gu& zR17I`2q`hM1D;m_k#*E4cvnk)^ZWPV^rR(lQ&D1&bWxTjbb4O>6kOqP&VhF+{Ac^H zh5ps?K7)&0J1w|a(Z2{o|2v4ddJgvaPcSe!cL>Z=dC!Z);0}CAPn`5HnAf3v?=^U%M#6UJQeM8ARedFy{2V0B2Yy+|snUgC)c(P7>IYz+>09 z!ilL~-fCK8i$Kw0FU)&hB_6&8`LA|I!v;c6&lldThIO_*rl2>x;&p=YI`FiW%Py1k z4rOndf^=;mrhOxt_Dz^+Z-5A;fsf#d)w@Y+O!bZ(**W3@NjEb#E4H1J!O1#2OFzmpiTM1Rj{@u#c$#BF9 zS51PxD1Q(Q{2|Q1PobAlnxESI98BeWJw6{yZ;3>pm$^VtFW`!#eXwxsCjd;U3gs5Ou+EpYOhqm zimI%aScHa=74QBoyoHukMq05Z#ys2lnX@n{WNcFYB3kuJm{ngv^a_nvQ9)8<@j;tp zA1k_^pSTf1JM`_V02l{}3_%2*bP%X2g{C+Gnr}uhZ~<3A}%Oo7i=e z%FQdjC!pU0Pb1U9ZolT$-mlq>x~{JiXDhtt2ja?)VAnpiB&WAaqL&u6N8*%O|B0q z9)X|>u=~);{~*BbgT{V;$_aL2Lkyz2;h-Z#Nw27+H%!UzV3!m=wDtAu=ta84OME@5 zUOzhT`GdIjCl+|qUsxLZg9P?NPEZwFSv+_uz$^a3iG0Cd_(a(cs}Ma5&TZ;eBf1r~ z$z>Z59R=w&c$W(_6KE+AhI7t4ApHoAZ?}O=-ED}5YwWZGwpEDYZZYmI;+6|~y&Rl| z3m_62ti}4!^XGoio&GR)`oLVq0}Ij-bEz&7$wQDlxDcE8xF1+V#aUEHEzny-ISZRDK~#pFxE;0;~`WnLis@Ce>vD|qy&2Hy7dzn zb~z->^7TvC)(uD`@Ixpn0k+bCXXylH`C4ZE+U2+3zH}{9JXnHCnC%fnDg+gF*C`=x zbl#XT#op&=Umu_U%*4@plT{_@f4EPpHe2b;l zPyE@7=s$rxK#@K~4+T;IF+d&g?j#WT^oM-xLq6;wp9GN)b1ng0<*&q2lj#xbqTyZ} zLM3StQm8(2lq$9 zWUJk4LqM0Hk4k1=VhyxBcNLKsZT~k6=p=)+pphJ2L8TcIQgMg{ z1m$C}h@}5+-T$P4yng^>20W+^gf*0(Jd^4GM;4RGUw&T-wMlgjtcZk)mH=-^x*K?I zo?XfW&Eyqnq_U<_D#|m2uS%nCZEQOT9R=(nPkI_U0Z6X{odKEv^dsbb4YU*6p zgLlsc(pivB3OpOg^Rr@p@)7td90&msenLMU7T*p04oRbeOaUX)6O>{o>5x#ul#8Ks zjiWjcJq9)zHv>f-g9}lxI~adrAQA|Tcp1^-fhGdYB=idKlASd8w*#K-C!zBLIsim| z|2r*!_jy1=fvU^PP!z-vM5&#yq=tC%=NW2dY;;NG)ba_;vb%4)YiTgBN?2->@`qk1 zI2Q3nE$M1R_B)m@t6RGidThT4@6nOYPfs;8KJo!{!a%EFYlEc!DV2fT5}-7Yy@%lZ zXBbg*H3-iIp`b5~fdWv5_%bA59R-X(p8e0PyafCo{0A~KLql)`1PO({`%j-2@xNqH z`Se&Q$~n{W^QO)&Hye%lvx5#R$-)|(9U9^=9Q;vplJt<115^N13{(bG4V1#~H&9z> z4F72Vzf%9d0AeN###t~J=Vn1;GiOnc<_?8kp8?7NDg-J6ngLV`vyc)E4xM=b4FpG1j8*w+~T(s-R4LkB>QB1dAg7=If zwQV%iM#&n99|>ubt()N**&w1yL{x<#lG#)_Tyk!6t&L-5NA?h&Lu^?i(;?{jv zOTmnWxv|B6-mL)G6bS+gxObtVv0Ei4qGb794(;{7 zX?LS1YXeqJCRUn2=eKiGin>(@{?D-wt|9HWA={-I(M(O4(o(7dr|=s~spshGq!P9< zsX~3EG|E1)VZ`8Ukyn(Fu$BS(1FEkcNU1vsAJx88`{W!vaP&|<;E^s2oTa{38R1aR zY^(1?nx+wHHgcwP$g8y6fd|8<7wyuAiB?d1@C^Rl3aW?*kn_1_Apa>N|3Wpj4$jumXr(Lkpk?LZg!H8Uu+ z{&~0({4-2({{)JJXBtQkQ~*>4G!KZ=zkMPG<7x*G}YPu z3pW1RS)3_GCo?2E<^qK7Yen>iHbkF=bQ`45Kl&F_oA<#I5$H)c$luS;pGD<2YW@jJ z3K$n+q4Tpt`aKX0&!-^;)X;xqivXKZ^NGyQVI=-Sgl&aA)f-)i&io2SXxKx1#sMqN zOJd$j-y(X{caV1#_+htg<8@FCyI6HjFav&1{2s~Z^6$kF%ENnRQ#r2`&Y{j=&IrMP zU4d5=bE#Y8{Q6qzdD;&93|#^0hBQza7m~_&D^x~%GUNxS8`8mQ4q#*uFopyP&O3>S x8bkur4QW864vKLRDaJ*FCk1!{)D7t%EUXC#Y+eZLE5;?%IluK%|1Wne#Tx(s diff --git a/firmware/src/vl53l0x.c b/firmware/src/vl53l0x.c index 5d028af..2635b82 100644 --- a/firmware/src/vl53l0x.c +++ b/firmware/src/vl53l0x.c @@ -1,773 +1,777 @@ -/* - * VL53L0X Distance measurement sensor - * WHowe - * - * Most of this VL53L0X code is from https://github.com/pololu/vl53l0x-arduino - */ - -#include -#include -#include - -#include "hardware/i2c.h" -#include "board_defs.h" - -#include "vl53l0x.h" - -#define VL53L0X_DEF_ADDR 0x29 - -#define IO_TIMEOUT_US 1000 -#define TOF_WAIT_US 200000 - -// Decode VCSEL (vertical cavity surface emitting laser) pulse period in PCLKs -#define decodeVcselPeriod(reg_val) (((reg_val) + 1) << 1) - -// Encode VCSEL pulse period register value from period in PCLKs -#define encodeVcselPeriod(period_pclks) (((period_pclks) >> 1) - 1) - -// Calculate macro period in *nanoseconds* from VCSEL period in PCLKs -// PLL_period_ps = 1655; macro_period_vclks = 2304 -#define calcMacroPeriod(vcsel_period_pclks) ((((uint32_t)2304 * (vcsel_period_pclks) * 1655) + 500) / 1000) - -static i2c_inst_t *port = i2c0; -static uint8_t addr = VL53L0X_DEF_ADDR; - -static struct { - uint8_t stop_variable; // read by init and used when starting measurement - uint16_t range; - uint32_t timing_budget_us; -} instances[16]; - -#define INSTANCE_NUM (sizeof(instances) / sizeof(instances[0])) - -// Write an 8-bit register -void write_reg(uint8_t reg, uint8_t value) -{ - uint8_t data[2] = { reg, value }; - i2c_write_blocking_until(I2C_PORT, addr, data, 2, false, time_us_64() + IO_TIMEOUT_US); -} - -// Write a 16-bit register -void write_reg16(uint8_t reg, uint16_t value) -{ - uint8_t data[3] = { reg, value >> 8, value & 0xff }; - i2c_write_blocking_until(I2C_PORT, addr, data, 3, false, time_us_64() + IO_TIMEOUT_US); -} - -static void write_reg_list(const uint16_t *list) -{ - const uint16_t *regs = list + 1; - for (int i = 0; i < *list; i++) { - write_reg(regs[i] >> 8, regs[i] & 0xff); - } -} - -// Read an 8-bit register -uint8_t read_reg(uint8_t reg) -{ - uint8_t value; - i2c_write_blocking_until(I2C_PORT, addr, ®, 1, true, time_us_64() + IO_TIMEOUT_US); - i2c_read_blocking_until(I2C_PORT, addr, &value, 1, false, time_us_64() + IO_TIMEOUT_US); - return value; -} - -// Read a 16-bit register -uint16_t read_reg16(uint8_t reg) -{ - uint8_t value[2]; - i2c_write_blocking_until(I2C_PORT, addr, ®, 1, true, time_us_64() + IO_TIMEOUT_US); - i2c_read_blocking_until(I2C_PORT, addr, value, 2, false, time_us_64() + IO_TIMEOUT_US); - return (value[0] << 8) | value[1]; -} - -// Write an arbitrary number of bytes from the given array to the sensor, -// starting at the given register -void write_many(uint8_t reg, const uint8_t *src, uint8_t len) -{ - i2c_write_blocking_until(I2C_PORT, addr, ®, 1, true, time_us_64() + IO_TIMEOUT_US); - i2c_write_blocking_until(I2C_PORT, addr, src, len, false, time_us_64() + IO_TIMEOUT_US); -} - -// Read an arbitrary number of bytes from the sensor, starting at the given -// register, into the given array -void read_many(uint8_t reg, uint8_t *dst, uint8_t len) -{ - i2c_write_blocking_until(I2C_PORT, addr, ®, 1, true, time_us_64() + IO_TIMEOUT_US); - i2c_read_blocking_until(I2C_PORT, addr, dst, len, false, time_us_64() + IO_TIMEOUT_US * len); -} - - -const uint16_t reg_mode1[] = { 4, 0x8800, 0x8001, 0xff01, 0x0000 }; -const uint16_t reg_mode2[] = { 3, 0x0001, 0xff00, 0x8000 }; -const uint16_t reg_spad0[] = { 4, 0x8001, 0xff01, 0x0000, 0xff06 }; -const uint16_t reg_spad1[] = { 5, 0xff07, 0x8101, 0x8001, 0x946b, 0x8300 }; -const uint16_t reg_spad2[] = { 4, 0xff01, 0x0001, 0xff00, 0x8000 }; -const uint16_t reg_spad[] = { 5, 0xff01, 0x4f00, 0x4e2c, 0xff00, 0xb6b4 }; -const uint16_t reg_tuning[] = { 80, - 0xff01, 0x0000, 0xff00, 0x0900, 0x1000, 0x1100, 0x2401, 0x25ff, - 0x7500, 0xff01, 0x4e2c, 0x4800, 0x3020, 0xff00, 0x3009, 0x5400, - 0x3104, 0x3203, 0x4083, 0x4625, 0x6000, 0x2700, 0x5006, 0x5100, - 0x5296, 0x5608, 0x5730, 0x6100, 0x6200, 0x6400, 0x6500, 0x66a0, - 0xff01, 0x2232, 0x4714, 0x49ff, 0x4a00, 0xff00, 0x7a0a, 0x7b00, - 0x7821, 0xff01, 0x2334, 0x4200, 0x44ff, 0x4526, 0x4605, 0x4040, - 0x0e06, 0x201a, 0x4340, 0xff00, 0x3403, 0x3544, 0xff01, 0x3104, - 0x4b09, 0x4c05, 0x4d04, 0xff00, 0x4400, 0x4520, 0x4708, 0x4828, - 0x6700, 0x7004, 0x7101, 0x72fe, 0x7600, 0x7700, 0xff01, 0x0d01, - 0xff00, 0x8001, 0x01f8, 0xff01, 0x8e01, 0x0001, 0xff00, 0x8000, - }; - -void vl53l0x_init(i2c_inst_t *i2c_port, uint8_t i2c_addr) -{ - port = i2c_port; - if (i2c_addr != 0) { - addr = i2c_addr; - } -} - -bool vl53l0x_is_present() -{ - return read_reg(IDENTIFICATION_MODEL_ID) == 0xEE; -} - -bool vl53l0x_init_tof(int index) -{ - write_reg(VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, - read_reg(VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV) | 0x01); - - write_reg_list(reg_mode1); - instances[index].stop_variable = read_reg(0x91); - write_reg_list(reg_mode2); - - // disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks - write_reg(MSRC_CONFIG_CONTROL, read_reg(MSRC_CONFIG_CONTROL) | 0x12); - - // Q9.7 fixed point format (9 integer bits, 7 fractional bits) - write_reg16(FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, 32); - - write_reg(SYSTEM_SEQUENCE_CONFIG, 0xFF); - - uint8_t spad_count; - bool is_aperture; - if (!getSpadInfo(&spad_count, &is_aperture)) { - printf("%d\n", __LINE__); - return false; - } - - // The SPAD map (RefGoodSpadMap) is read by VL53L0X_get_info_from_device() in - // the API, but the same data seems to be more easily readable from - // GLOBAL_CONFIG_SPAD_ENABLES_REF_0 through _6, so read it from there - uint8_t ref_spad_map[6]; - read_many(GLOBAL_CONFIG_SPAD_ENABLES_REF_0, ref_spad_map, 6); - write_reg_list(reg_spad); - - uint8_t first_spad = is_aperture ? 12 : 0; // 12 is the first aperture spad - uint8_t spads_enabled = 0; - - for (int i = 0; i < 48; i++) { - if (i < first_spad || spads_enabled == spad_count) { - // This bit is lower than the first one that should be enabled, or - // (reference_spad_count) bits have already been enabled, so zero this bit - ref_spad_map[i / 8] &= ~(1 << (i % 8)); - } else if (ref_spad_map[i / 8] & (1 << i % 8)) { - spads_enabled++; - } - } - - write_many(GLOBAL_CONFIG_SPAD_ENABLES_REF_0, ref_spad_map, 6); - - write_reg_list(reg_tuning); - - write_reg(SYSTEM_INTERRUPT_CONFIG_GPIO, 0x04); - write_reg(GPIO_HV_MUX_ACTIVE_HIGH, read_reg(GPIO_HV_MUX_ACTIVE_HIGH) & ~0x10); // active low - write_reg(SYSTEM_INTERRUPT_CLEAR, 0x01); - - instances[index].timing_budget_us = getMeasurementTimingBudget(index); - write_reg(SYSTEM_SEQUENCE_CONFIG, 0xE8); - setMeasurementTimingBudget(index, instances[index].timing_budget_us); - - write_reg(SYSTEM_SEQUENCE_CONFIG, 0x01); - if (!performSingleRefCalibration(0x40)) { - printf("%d\n", __LINE__); - return false; - } - - write_reg(SYSTEM_SEQUENCE_CONFIG, 0x02); - if (!performSingleRefCalibration(0x00)) { - printf("%d\n", __LINE__); - return false; - } - - write_reg(SYSTEM_SEQUENCE_CONFIG, 0xE8); - - return true; -} - -// Get the return signal rate limit check value in MCPS -float getSignalRateLimit() -{ - return (float)read_reg16(FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT) / (1 << 7); -} - -// Decode sequence step timeout in MCLKs from register value -// based on VL53L0X_decode_timeout() -// Note: the original function returned a uint32_t, but the return value is -// always stored in a uint16_t. -uint16_t decodeTimeout(uint16_t reg_val) -{ - // format: "(LSByte * 2^MSByte) + 1" - return (uint16_t)((reg_val & 0x00FF) << - (uint16_t)((reg_val & 0xFF00) >> 8)) + 1; -} - -// Encode sequence step timeout register value from timeout in MCLKs -// based on VL53L0X_encode_timeout() -static uint16_t encodeTimeout(uint16_t timeout_mclks) -{ - // format: "(LSByte * 2^MSByte) + 1" - uint32_t ls_byte = 0; - uint16_t ms_byte = 0; - - if (timeout_mclks > 0) { - ls_byte = timeout_mclks - 1; - - while ((ls_byte & 0xFFFFFF00) > 0) { - ls_byte >>= 1; - ms_byte++; - } - - return (ms_byte << 8) | (ls_byte & 0xFF); - } - - return 0; -} - -// Convert sequence step timeout from MCLKs to microseconds with given VCSEL period in PCLKs -// based on VL53L0X_calc_timeout_us() -uint32_t timeoutMclksToMicroseconds(uint16_t timeout_period_mclks, uint8_t vcsel_period_pclks) -{ - uint32_t macro_period_ns = calcMacroPeriod(vcsel_period_pclks); - - return ((timeout_period_mclks * macro_period_ns) + 500) / 1000; -} - -// Convert sequence step timeout from microseconds to MCLKs with given VCSEL period in PCLKs -// based on VL53L0X_calc_timeout_mclks() -uint32_t timeoutMicrosecondsToMclks(uint32_t timeout_period_us, uint8_t vcsel_period_pclks) -{ - uint32_t macro_period_ns = calcMacroPeriod(vcsel_period_pclks); - - return (((timeout_period_us * 1000) + (macro_period_ns / 2)) / macro_period_ns); -} - -// Set the measurement timing budget in microseconds, which is the time allowed -// for one measurement; the ST API and this library take care of splitting the -// timing budget among the sub-steps in the ranging sequence. A longer timing -// budget allows for more accurate measurements. Increasing the budget by a -// factor of N decreases the range measurement standard deviation by a factor of -// sqrt(N). Defaults to about 33 milliseconds; the minimum is 20 ms. -// based on VL53L0X_set_measurement_timing_budget_micro_seconds() -bool setMeasurementTimingBudget(int index, uint32_t budget_us) -{ - SequenceStepEnables enables; - SequenceStepTimeouts timeouts; - - uint16_t const StartOverhead = 1320; // different than the value in get_ - uint16_t const EndOverhead = 960; - uint16_t const MsrcOverhead = 660; - uint16_t const TccOverhead = 590; - uint16_t const DssOverhead = 690; - uint16_t const PreRangeOverhead = 660; - uint16_t const FinalRangeOverhead = 550; - - uint32_t const MinTimingBudget = 20000; - - if (budget_us < MinTimingBudget) { - return false; - } - - uint32_t used_budget_us = StartOverhead + EndOverhead; - - getSequenceStepEnables(&enables); - getSequenceStepTimeouts(&enables, &timeouts); - - if (enables.tcc) { - used_budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead); - } - - if (enables.dss) { - used_budget_us += 2 * (timeouts.msrc_dss_tcc_us + DssOverhead); - } else if (enables.msrc) { - used_budget_us += (timeouts.msrc_dss_tcc_us + MsrcOverhead); - } - - if (enables.pre_range) { - used_budget_us += (timeouts.pre_range_us + PreRangeOverhead); - } - - if (enables.final_range) { - used_budget_us += FinalRangeOverhead; - - // "Note that the final range timeout is determined by the timing - // budget and the sum of all other timeouts within the sequence. - // If there is no room for the final range timeout, then an error - // will be set. Otherwise the remaining time will be applied to - // the final range." - - if (used_budget_us > budget_us) - { - // "Requested timeout too big." - return false; - } - - uint32_t final_range_timeout_us = budget_us - used_budget_us; - - // set_sequence_step_timeout() begin - // (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE) - - // "For the final range timeout, the pre-range timeout - // must be added. To do this both final and pre-range - // timeouts must be expressed in macro periods MClks - // because they have different vcsel periods." - - uint32_t final_range_timeout_mclks = - timeoutMicrosecondsToMclks(final_range_timeout_us, - timeouts.final_range_vcsel_period_pclks); - - if (enables.pre_range) { - final_range_timeout_mclks += timeouts.pre_range_mclks; - } - - write_reg16(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, - encodeTimeout(final_range_timeout_mclks)); - - // set_sequence_step_timeout() end - - instances[index].timing_budget_us = budget_us; // store for internal reuse - } - return true; -} - -// Get the measurement timing budget in microseconds -// based on VL53L0X_get_measurement_timing_budget_micro_seconds() -// in us -uint32_t getMeasurementTimingBudget(int index) -{ - SequenceStepEnables enables; - SequenceStepTimeouts timeouts; - - uint16_t const StartOverhead = 1910; - uint16_t const EndOverhead = 960; - uint16_t const MsrcOverhead = 660; - uint16_t const TccOverhead = 590; - uint16_t const DssOverhead = 690; - uint16_t const PreRangeOverhead = 660; - uint16_t const FinalRangeOverhead = 550; - - // "Start and end overhead times always present" - uint32_t budget_us = StartOverhead + EndOverhead; - - getSequenceStepEnables(&enables); - getSequenceStepTimeouts(&enables, &timeouts); - - if (enables.tcc) { - budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead); - } - - if (enables.dss) { - budget_us += 2 * (timeouts.msrc_dss_tcc_us + DssOverhead); - } else if (enables.msrc) { - budget_us += (timeouts.msrc_dss_tcc_us + MsrcOverhead); - } - - if (enables.pre_range) { - budget_us += (timeouts.pre_range_us + PreRangeOverhead); - } - - if (enables.final_range) { - budget_us += (timeouts.final_range_us + FinalRangeOverhead); - } - - instances[index].timing_budget_us = budget_us; // cache for reuse - return budget_us; -} - -// Set the VCSEL (vertical cavity surface emitting laser) pulse period for the -// given period type (pre-range or final range) to the given value in PCLKs. -// Longer periods seem to increase the potential range of the sensor. -// Valid values are (even numbers only): -// pre: 12 to 18 (initialized default: 14) -// final: 8 to 14 (initialized default: 10) -// based on VL53L0X_set_vcsel_pulse_period() -bool setVcselPulsePeriod(int index, vcselPeriodType type, uint8_t period_pclks) -{ - uint8_t vcsel_period_reg = encodeVcselPeriod(period_pclks); - - SequenceStepEnables enables; - SequenceStepTimeouts timeouts; - - getSequenceStepEnables(&enables); - getSequenceStepTimeouts(&enables, &timeouts); - - // "Apply specific settings for the requested clock period" - // "Re-calculate and apply timeouts, in macro periods" - - // "When the VCSEL period for the pre or final range is changed, - // the corresponding timeout must be read from the device using - // the current VCSEL period, then the new VCSEL period can be - // applied. The timeout then must be written back to the device - // using the new VCSEL period. - // - // For the MSRC timeout, the same applies - this timeout being - // dependant on the pre-range vcsel period." - - - if (type == VcselPeriodPreRange) { - // "Set phase check limits" - switch (period_pclks) { - case 12: - write_reg(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x18); - break; - - case 14: - write_reg(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x30); - break; - - case 16: - write_reg(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x40); - break; - - case 18: - write_reg(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x50); - break; - - default: - // invalid period - return false; - } - write_reg(PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); - - // apply new VCSEL period - write_reg(PRE_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg); - - // update timeouts - - // set_sequence_step_timeout() begin - // (SequenceStepId == VL53L0X_SEQUENCESTEP_PRE_RANGE) - - uint16_t new_pre_range_timeout_mclks = - timeoutMicrosecondsToMclks(timeouts.pre_range_us, period_pclks); - - write_reg16(PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, - encodeTimeout(new_pre_range_timeout_mclks)); - - // set_sequence_step_timeout() end - - // set_sequence_step_timeout() begin - // (SequenceStepId == VL53L0X_SEQUENCESTEP_MSRC) - - uint16_t new_msrc_timeout_mclks = - timeoutMicrosecondsToMclks(timeouts.msrc_dss_tcc_us, period_pclks); - - write_reg(MSRC_CONFIG_TIMEOUT_MACROP, - (new_msrc_timeout_mclks > 256) ? 255 : (new_msrc_timeout_mclks - 1)); - - // set_sequence_step_timeout() end - } else if (type == VcselPeriodFinalRange) { - switch (period_pclks) { - case 8: - write_reg(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x10); - write_reg(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); - write_reg(GLOBAL_CONFIG_VCSEL_WIDTH, 0x02); - write_reg(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C); - write_reg(0xFF, 0x01); - write_reg(ALGO_PHASECAL_LIM, 0x30); - write_reg(0xFF, 0x00); - break; - - case 10: - write_reg(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x28); - write_reg(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); - write_reg(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03); - write_reg(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09); - write_reg(0xFF, 0x01); - write_reg(ALGO_PHASECAL_LIM, 0x20); - write_reg(0xFF, 0x00); - break; - - case 12: - write_reg(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x38); - write_reg(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); - write_reg(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03); - write_reg(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08); - write_reg(0xFF, 0x01); - write_reg(ALGO_PHASECAL_LIM, 0x20); - write_reg(0xFF, 0x00); - break; - - case 14: - write_reg(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x48); - write_reg(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); - write_reg(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03); - write_reg(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07); - write_reg(0xFF, 0x01); - write_reg(ALGO_PHASECAL_LIM, 0x20); - write_reg(0xFF, 0x00); - break; - - default: - // invalid period - return false; - } - - // apply new VCSEL period - write_reg(FINAL_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg); - - // update timeouts - - // set_sequence_step_timeout() begin - // (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE) - - // "For the final range timeout, the pre-range timeout - // must be added. To do this both final and pre-range - // timeouts must be expressed in macro periods MClks - // because they have different vcsel periods." - - uint16_t new_final_range_timeout_mclks = - timeoutMicrosecondsToMclks(timeouts.final_range_us, period_pclks); - - if (enables.pre_range) { - new_final_range_timeout_mclks += timeouts.pre_range_mclks; - } - - write_reg16(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, - encodeTimeout(new_final_range_timeout_mclks)); - - // set_sequence_step_timeout end - } - else { - // invalid type - return false; - } - - // "Finally, the timing budget must be re-applied" - - setMeasurementTimingBudget(index, instances[index].timing_budget_us); - - // "Perform the phase calibration. This is needed after changing on vcsel period." - // VL53L0X_perform_phase_calibration() begin - - uint8_t sequence_config = read_reg(SYSTEM_SEQUENCE_CONFIG); - write_reg(SYSTEM_SEQUENCE_CONFIG, 0x02); - performSingleRefCalibration(0x0); - write_reg(SYSTEM_SEQUENCE_CONFIG, sequence_config); - - // VL53L0X_perform_phase_calibration() end - - return true; -} - -// Get the VCSEL pulse period in PCLKs for the given period type. -// based on VL53L0X_get_vcsel_pulse_period() -uint8_t getVcselPulsePeriod(vcselPeriodType type) -{ - if (type == VcselPeriodPreRange) - { - return decodeVcselPeriod(read_reg(PRE_RANGE_CONFIG_VCSEL_PERIOD)); - } - else if (type == VcselPeriodFinalRange) - { - return decodeVcselPeriod(read_reg(FINAL_RANGE_CONFIG_VCSEL_PERIOD)); - } - else { return 255; } -} - -// Start continuous ranging measurements. -// based on VL53L0X_StartMeasurement() -void vl53l0x_start_continuous(int index) -{ - if (index >= INSTANCE_NUM) { - return; - } - - write_reg(0x80, 0x01); - write_reg(0xFF, 0x01); - write_reg(0x00, 0x00); - write_reg(0x91, instances[index].stop_variable); - write_reg(0x00, 0x01); - write_reg(0xFF, 0x00); - write_reg(0x80, 0x00); - - write_reg(SYSRANGE_START, 0x02); // VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK -} - -// Stop continuous measurements -// based on VL53L0X_StopMeasurement() -void vl53l0x_stop_continuous(int index) -{ - write_reg(SYSRANGE_START, 0x01); // VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT - - write_reg(0xFF, 0x01); - write_reg(0x00, 0x00); - write_reg(0x91, 0x00); - write_reg(0x00, 0x01); - write_reg(0xFF, 0x00); -} - -// Returns a range reading in millimeters when continuous mode is active -// (readRangeSingleMillimeters() also calls this function after starting a -// single-shot range measurement) -uint16_t readRangeContinuousMillimeters(int index) -{ - if (index >= INSTANCE_NUM) { - return 65535; - } - - if ((read_reg(RESULT_INTERRUPT_STATUS) & 0x07) == 0) { - return instances[index].range; // use last result - } - - // assumptions: Linearity Corrective Gain is 1000 (default); - // fractional ranging is not enabled - instances[index].range = read_reg16(RESULT_RANGE_STATUS + 10); - - write_reg(SYSTEM_INTERRUPT_CLEAR, 0x01); - - return instances[index].range; -} - -#if 0 -// Performs a single-shot range measurement and returns the reading in -// millimeters -// based on VL53L0X_PerformSingleRangingMeasurement() -uint16_t readRangeSingleMillimeters(int index) -{ - static uint16_t range = 65535; - static bool reading = false; - static uint64_t start_time = 0; - - uint64_t now = time_us_64(); - if (now - start_time > TOF_WAIT_US) { - reading = false; - } - - if (reading) { - if ((read_reg(SYSRANGE_START) & 0x01) == 0) { - range = readRangeContinuousMillimeters(index); - reading = false; - } - } else { - write_reg(0x80, 0x01); - write_reg(0xFF, 0x01); - write_reg(0x00, 0x00); - write_reg(0x91, instances[index].stop_variable); - write_reg(0x00, 0x01); - write_reg(0xFF, 0x00); - write_reg(0x80, 0x00); - - write_reg(SYSRANGE_START, 0x01); - start_time = now; - reading = true; - } - return range; -} -#endif - -// Private Methods ///////////////////////////////////////////////////////////// - -// Get reference SPAD (single photon avalanche diode) count and type -// based on VL53L0X_get_info_from_device(), -// but only gets reference SPAD count and type -bool getSpadInfo(uint8_t *count, bool *type_is_aperture) -{ - write_reg_list(reg_spad0); - write_reg(0x83, read_reg(0x83) | 0x04); - write_reg_list(reg_spad1); - - uint64_t start = time_us_64(); - while (read_reg(0x83) == 0x00) { - if (time_us_64() - start > TOF_WAIT_US) { - return false; - } - sleep_ms(1); - } - - write_reg(0x83, 0x01); - - uint8_t tmp = read_reg(0x92); - *count = tmp & 0x7f; - *type_is_aperture = (tmp & 0x80); - - write_reg(0x81, 0x00); - write_reg(0xFF, 0x06); - - write_reg(0x83, read_reg(0x83) & ~0x04); - - write_reg_list(reg_spad2); - - return true; -} - -// Get sequence step enables -// based on VL53L0X_GetSequenceStepEnables() -void getSequenceStepEnables(SequenceStepEnables * enables) -{ - uint8_t seq_cfg = read_reg(SYSTEM_SEQUENCE_CONFIG); - - enables->tcc = (seq_cfg >> 4) & 0x1; - enables->dss = (seq_cfg >> 3) & 0x1; - enables->msrc = (seq_cfg >> 2) & 0x1; - enables->pre_range = (seq_cfg >> 6) & 0x1; - enables->final_range = (seq_cfg >> 7) & 0x1; -} - -// Get sequence step timeouts -// based on get_sequence_step_timeout(), -// but gets all timeouts instead of just the requested one, and also stores -// intermediate values -void getSequenceStepTimeouts(SequenceStepEnables const * enables, SequenceStepTimeouts * timeouts) -{ - timeouts->pre_range_vcsel_period_pclks = getVcselPulsePeriod(VcselPeriodPreRange); - - timeouts->msrc_dss_tcc_mclks = read_reg(MSRC_CONFIG_TIMEOUT_MACROP) + 1; - timeouts->msrc_dss_tcc_us = - timeoutMclksToMicroseconds(timeouts->msrc_dss_tcc_mclks, - timeouts->pre_range_vcsel_period_pclks); - - timeouts->pre_range_mclks = - decodeTimeout(read_reg16(PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI)); - timeouts->pre_range_us = - timeoutMclksToMicroseconds(timeouts->pre_range_mclks, - timeouts->pre_range_vcsel_period_pclks); - - timeouts->final_range_vcsel_period_pclks = getVcselPulsePeriod(VcselPeriodFinalRange); - - timeouts->final_range_mclks = - decodeTimeout(read_reg16(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI)); - - if (enables->pre_range) { - timeouts->final_range_mclks -= timeouts->pre_range_mclks; - } - - timeouts->final_range_us = - timeoutMclksToMicroseconds(timeouts->final_range_mclks, - timeouts->final_range_vcsel_period_pclks); -} - -// based on VL53L0X_perform_single_ref_calibration() -bool performSingleRefCalibration(uint8_t vhv_init_byte) -{ - write_reg(SYSRANGE_START, 0x01 | vhv_init_byte); // VL53L0X_REG_SYSRANGE_MODE_START_STOP - - uint64_t start = time_us_64(); - while ((read_reg(RESULT_INTERRUPT_STATUS) & 0x07) == 0) { - if (time_us_64() - start > TOF_WAIT_US) { - return false; - } - sleep_ms(1); - } - - write_reg(SYSTEM_INTERRUPT_CLEAR, 0x01); - write_reg(SYSRANGE_START, 0x00); - - return true; +/* + * VL53L0X Distance measurement sensor + * WHowe + * + * Most of this VL53L0X code is from https://github.com/pololu/vl53l0x-arduino + */ + +#include +#include +#include + +#include "hardware/i2c.h" +#include "board_defs.h" + +#include "vl53l0x.h" + +#define VL53L0X_DEF_ADDR 0x29 + +#define IO_TIMEOUT_US 1000 +#define TOF_WAIT_US 200000 + +// Decode VCSEL (vertical cavity surface emitting laser) pulse period in PCLKs +#define decodeVcselPeriod(reg_val) (((reg_val) + 1) << 1) + +// Encode VCSEL pulse period register value from period in PCLKs +#define encodeVcselPeriod(period_pclks) (((period_pclks) >> 1) - 1) + +// Calculate macro period in *nanoseconds* from VCSEL period in PCLKs +// PLL_period_ps = 1655; macro_period_vclks = 2304 +#define calcMacroPeriod(vcsel_period_pclks) ((((uint32_t)2304 * (vcsel_period_pclks) * 1655) + 500) / 1000) + +static i2c_inst_t *port = i2c0; +static uint8_t addr = VL53L0X_DEF_ADDR; + +static struct { + uint8_t stop_variable; // read by init and used when starting measurement + uint16_t range; + uint32_t timing_budget_us; +} instances[16]; + +#define INSTANCE_NUM (sizeof(instances) / sizeof(instances[0])) + +// Write an 8-bit register +void write_reg(uint8_t reg, uint8_t value) +{ + uint8_t data[2] = { reg, value }; + i2c_write_blocking_until(I2C_PORT, addr, data, 2, false, time_us_64() + IO_TIMEOUT_US); +} + +// Write a 16-bit register +void write_reg16(uint8_t reg, uint16_t value) +{ + uint8_t data[3] = { reg, value >> 8, value & 0xff }; + i2c_write_blocking_until(I2C_PORT, addr, data, 3, false, time_us_64() + IO_TIMEOUT_US); +} + +static void write_reg_list(const uint16_t *list) +{ + const uint16_t *regs = list + 1; + for (int i = 0; i < *list; i++) { + write_reg(regs[i] >> 8, regs[i] & 0xff); + } +} + +// Read an 8-bit register +uint8_t read_reg(uint8_t reg) +{ + uint8_t value; + i2c_write_blocking_until(I2C_PORT, addr, ®, 1, true, time_us_64() + IO_TIMEOUT_US); + i2c_read_blocking_until(I2C_PORT, addr, &value, 1, false, time_us_64() + IO_TIMEOUT_US); + return value; +} + +// Read a 16-bit register +uint16_t read_reg16(uint8_t reg) +{ + uint8_t value[2]; + i2c_write_blocking_until(I2C_PORT, addr, ®, 1, true, time_us_64() + IO_TIMEOUT_US); + i2c_read_blocking_until(I2C_PORT, addr, value, 2, false, time_us_64() + IO_TIMEOUT_US); + return (value[0] << 8) | value[1]; +} + +// Write an arbitrary number of bytes from the given array to the sensor, +// starting at the given register +void write_many(uint8_t reg, const uint8_t *src, uint8_t len) +{ + uint8_t cache[32] = { reg, 0 }; + if (len > sizeof(cache) - 1) { + return; + } + memcpy(cache + 1, src, len); + i2c_write_blocking_until(I2C_PORT, addr, cache, len + 1, false, time_us_64() + IO_TIMEOUT_US * len); +} + +// Read an arbitrary number of bytes from the sensor, starting at the given +// register, into the given array +void read_many(uint8_t reg, uint8_t *dst, uint8_t len) +{ + i2c_write_blocking_until(I2C_PORT, addr, ®, 1, true, time_us_64() + IO_TIMEOUT_US); + i2c_read_blocking_until(I2C_PORT, addr, dst, len, false, time_us_64() + IO_TIMEOUT_US * len); +} + + +const uint16_t reg_mode1[] = { 4, 0x8800, 0x8001, 0xff01, 0x0000 }; +const uint16_t reg_mode2[] = { 3, 0x0001, 0xff00, 0x8000 }; +const uint16_t reg_spad0[] = { 4, 0x8001, 0xff01, 0x0000, 0xff06 }; +const uint16_t reg_spad1[] = { 5, 0xff07, 0x8101, 0x8001, 0x946b, 0x8300 }; +const uint16_t reg_spad2[] = { 4, 0xff01, 0x0001, 0xff00, 0x8000 }; +const uint16_t reg_spad[] = { 5, 0xff01, 0x4f00, 0x4e2c, 0xff00, 0xb6b4 }; +const uint16_t reg_tuning[] = { 80, + 0xff01, 0x0000, 0xff00, 0x0900, 0x1000, 0x1100, 0x2401, 0x25ff, + 0x7500, 0xff01, 0x4e2c, 0x4800, 0x3020, 0xff00, 0x3009, 0x5400, + 0x3104, 0x3203, 0x4083, 0x4625, 0x6000, 0x2700, 0x5006, 0x5100, + 0x5296, 0x5608, 0x5730, 0x6100, 0x6200, 0x6400, 0x6500, 0x66a0, + 0xff01, 0x2232, 0x4714, 0x49ff, 0x4a00, 0xff00, 0x7a0a, 0x7b00, + 0x7821, 0xff01, 0x2334, 0x4200, 0x44ff, 0x4526, 0x4605, 0x4040, + 0x0e06, 0x201a, 0x4340, 0xff00, 0x3403, 0x3544, 0xff01, 0x3104, + 0x4b09, 0x4c05, 0x4d04, 0xff00, 0x4400, 0x4520, 0x4708, 0x4828, + 0x6700, 0x7004, 0x7101, 0x72fe, 0x7600, 0x7700, 0xff01, 0x0d01, + 0xff00, 0x8001, 0x01f8, 0xff01, 0x8e01, 0x0001, 0xff00, 0x8000, + }; + +void vl53l0x_init(i2c_inst_t *i2c_port, uint8_t i2c_addr) +{ + port = i2c_port; + if (i2c_addr != 0) { + addr = i2c_addr; + } +} + +bool vl53l0x_is_present() +{ + return read_reg(IDENTIFICATION_MODEL_ID) == 0xEE; +} + +bool vl53l0x_init_tof(int index) +{ + write_reg(VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, + read_reg(VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV) | 0x01); + + write_reg_list(reg_mode1); + instances[index].stop_variable = read_reg(0x91); + write_reg_list(reg_mode2); + + // disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks + write_reg(MSRC_CONFIG_CONTROL, read_reg(MSRC_CONFIG_CONTROL) | 0x12); + + // Q9.7 fixed point format (9 integer bits, 7 fractional bits) + write_reg16(FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, 32); + + write_reg(SYSTEM_SEQUENCE_CONFIG, 0xFF); + + uint8_t spad_count; + bool is_aperture; + if (!getSpadInfo(&spad_count, &is_aperture)) { + printf("%d\n", __LINE__); + return false; + } + + // The SPAD map (RefGoodSpadMap) is read by VL53L0X_get_info_from_device() in + // the API, but the same data seems to be more easily readable from + // GLOBAL_CONFIG_SPAD_ENABLES_REF_0 through _6, so read it from there + uint8_t ref_spad_map[6]; + read_many(GLOBAL_CONFIG_SPAD_ENABLES_REF_0, ref_spad_map, 6); + write_reg_list(reg_spad); + + uint8_t first_spad = is_aperture ? 12 : 0; // 12 is the first aperture spad + uint8_t spads_enabled = 0; + + for (int i = 0; i < 48; i++) { + if (i < first_spad || spads_enabled == spad_count) { + // This bit is lower than the first one that should be enabled, or + // (reference_spad_count) bits have already been enabled, so zero this bit + ref_spad_map[i / 8] &= ~(1 << (i % 8)); + } else if (ref_spad_map[i / 8] & (1 << i % 8)) { + spads_enabled++; + } + } + + write_many(GLOBAL_CONFIG_SPAD_ENABLES_REF_0, ref_spad_map, 6); + + write_reg_list(reg_tuning); + + write_reg(SYSTEM_INTERRUPT_CONFIG_GPIO, 0x04); + write_reg(GPIO_HV_MUX_ACTIVE_HIGH, read_reg(GPIO_HV_MUX_ACTIVE_HIGH) & ~0x10); // active low + write_reg(SYSTEM_INTERRUPT_CLEAR, 0x01); + + instances[index].timing_budget_us = getMeasurementTimingBudget(index); + write_reg(SYSTEM_SEQUENCE_CONFIG, 0xE8); + setMeasurementTimingBudget(index, instances[index].timing_budget_us); + + write_reg(SYSTEM_SEQUENCE_CONFIG, 0x01); + if (!performSingleRefCalibration(0x40)) { + printf("%d\n", __LINE__); + return false; + } + + write_reg(SYSTEM_SEQUENCE_CONFIG, 0x02); + if (!performSingleRefCalibration(0x00)) { + printf("%d\n", __LINE__); + return false; + } + + write_reg(SYSTEM_SEQUENCE_CONFIG, 0xE8); + + return true; +} + +// Get the return signal rate limit check value in MCPS +float getSignalRateLimit() +{ + return (float)read_reg16(FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT) / (1 << 7); +} + +// Decode sequence step timeout in MCLKs from register value +// based on VL53L0X_decode_timeout() +// Note: the original function returned a uint32_t, but the return value is +// always stored in a uint16_t. +uint16_t decodeTimeout(uint16_t reg_val) +{ + // format: "(LSByte * 2^MSByte) + 1" + return (uint16_t)((reg_val & 0x00FF) << + (uint16_t)((reg_val & 0xFF00) >> 8)) + 1; +} + +// Encode sequence step timeout register value from timeout in MCLKs +// based on VL53L0X_encode_timeout() +static uint16_t encodeTimeout(uint16_t timeout_mclks) +{ + // format: "(LSByte * 2^MSByte) + 1" + uint32_t ls_byte = 0; + uint16_t ms_byte = 0; + + if (timeout_mclks > 0) { + ls_byte = timeout_mclks - 1; + + while ((ls_byte & 0xFFFFFF00) > 0) { + ls_byte >>= 1; + ms_byte++; + } + + return (ms_byte << 8) | (ls_byte & 0xFF); + } + + return 0; +} + +// Convert sequence step timeout from MCLKs to microseconds with given VCSEL period in PCLKs +// based on VL53L0X_calc_timeout_us() +uint32_t timeoutMclksToMicroseconds(uint16_t timeout_period_mclks, uint8_t vcsel_period_pclks) +{ + uint32_t macro_period_ns = calcMacroPeriod(vcsel_period_pclks); + + return ((timeout_period_mclks * macro_period_ns) + 500) / 1000; +} + +// Convert sequence step timeout from microseconds to MCLKs with given VCSEL period in PCLKs +// based on VL53L0X_calc_timeout_mclks() +uint32_t timeoutMicrosecondsToMclks(uint32_t timeout_period_us, uint8_t vcsel_period_pclks) +{ + uint32_t macro_period_ns = calcMacroPeriod(vcsel_period_pclks); + + return (((timeout_period_us * 1000) + (macro_period_ns / 2)) / macro_period_ns); +} + +// Set the measurement timing budget in microseconds, which is the time allowed +// for one measurement; the ST API and this library take care of splitting the +// timing budget among the sub-steps in the ranging sequence. A longer timing +// budget allows for more accurate measurements. Increasing the budget by a +// factor of N decreases the range measurement standard deviation by a factor of +// sqrt(N). Defaults to about 33 milliseconds; the minimum is 20 ms. +// based on VL53L0X_set_measurement_timing_budget_micro_seconds() +bool setMeasurementTimingBudget(int index, uint32_t budget_us) +{ + SequenceStepEnables enables; + SequenceStepTimeouts timeouts; + + uint16_t const StartOverhead = 1320; // different than the value in get_ + uint16_t const EndOverhead = 960; + uint16_t const MsrcOverhead = 660; + uint16_t const TccOverhead = 590; + uint16_t const DssOverhead = 690; + uint16_t const PreRangeOverhead = 660; + uint16_t const FinalRangeOverhead = 550; + + uint32_t const MinTimingBudget = 20000; + + if (budget_us < MinTimingBudget) { + return false; + } + + uint32_t used_budget_us = StartOverhead + EndOverhead; + + getSequenceStepEnables(&enables); + getSequenceStepTimeouts(&enables, &timeouts); + + if (enables.tcc) { + used_budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead); + } + + if (enables.dss) { + used_budget_us += 2 * (timeouts.msrc_dss_tcc_us + DssOverhead); + } else if (enables.msrc) { + used_budget_us += (timeouts.msrc_dss_tcc_us + MsrcOverhead); + } + + if (enables.pre_range) { + used_budget_us += (timeouts.pre_range_us + PreRangeOverhead); + } + + if (enables.final_range) { + used_budget_us += FinalRangeOverhead; + + // "Note that the final range timeout is determined by the timing + // budget and the sum of all other timeouts within the sequence. + // If there is no room for the final range timeout, then an error + // will be set. Otherwise the remaining time will be applied to + // the final range." + + if (used_budget_us > budget_us) + { + // "Requested timeout too big." + return false; + } + + uint32_t final_range_timeout_us = budget_us - used_budget_us; + + // set_sequence_step_timeout() begin + // (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE) + + // "For the final range timeout, the pre-range timeout + // must be added. To do this both final and pre-range + // timeouts must be expressed in macro periods MClks + // because they have different vcsel periods." + + uint32_t final_range_timeout_mclks = + timeoutMicrosecondsToMclks(final_range_timeout_us, + timeouts.final_range_vcsel_period_pclks); + + if (enables.pre_range) { + final_range_timeout_mclks += timeouts.pre_range_mclks; + } + + write_reg16(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, + encodeTimeout(final_range_timeout_mclks)); + + // set_sequence_step_timeout() end + + instances[index].timing_budget_us = budget_us; // store for internal reuse + } + return true; +} + +// Get the measurement timing budget in microseconds +// based on VL53L0X_get_measurement_timing_budget_micro_seconds() +// in us +uint32_t getMeasurementTimingBudget(int index) +{ + SequenceStepEnables enables; + SequenceStepTimeouts timeouts; + + uint16_t const StartOverhead = 1910; + uint16_t const EndOverhead = 960; + uint16_t const MsrcOverhead = 660; + uint16_t const TccOverhead = 590; + uint16_t const DssOverhead = 690; + uint16_t const PreRangeOverhead = 660; + uint16_t const FinalRangeOverhead = 550; + + // "Start and end overhead times always present" + uint32_t budget_us = StartOverhead + EndOverhead; + + getSequenceStepEnables(&enables); + getSequenceStepTimeouts(&enables, &timeouts); + + if (enables.tcc) { + budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead); + } + + if (enables.dss) { + budget_us += 2 * (timeouts.msrc_dss_tcc_us + DssOverhead); + } else if (enables.msrc) { + budget_us += (timeouts.msrc_dss_tcc_us + MsrcOverhead); + } + + if (enables.pre_range) { + budget_us += (timeouts.pre_range_us + PreRangeOverhead); + } + + if (enables.final_range) { + budget_us += (timeouts.final_range_us + FinalRangeOverhead); + } + + instances[index].timing_budget_us = budget_us; // cache for reuse + return budget_us; +} + +// Set the VCSEL (vertical cavity surface emitting laser) pulse period for the +// given period type (pre-range or final range) to the given value in PCLKs. +// Longer periods seem to increase the potential range of the sensor. +// Valid values are (even numbers only): +// pre: 12 to 18 (initialized default: 14) +// final: 8 to 14 (initialized default: 10) +// based on VL53L0X_set_vcsel_pulse_period() +bool setVcselPulsePeriod(int index, vcselPeriodType type, uint8_t period_pclks) +{ + uint8_t vcsel_period_reg = encodeVcselPeriod(period_pclks); + + SequenceStepEnables enables; + SequenceStepTimeouts timeouts; + + getSequenceStepEnables(&enables); + getSequenceStepTimeouts(&enables, &timeouts); + + // "Apply specific settings for the requested clock period" + // "Re-calculate and apply timeouts, in macro periods" + + // "When the VCSEL period for the pre or final range is changed, + // the corresponding timeout must be read from the device using + // the current VCSEL period, then the new VCSEL period can be + // applied. The timeout then must be written back to the device + // using the new VCSEL period. + // + // For the MSRC timeout, the same applies - this timeout being + // dependant on the pre-range vcsel period." + + + if (type == VcselPeriodPreRange) { + // "Set phase check limits" + switch (period_pclks) { + case 12: + write_reg(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x18); + break; + + case 14: + write_reg(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x30); + break; + + case 16: + write_reg(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x40); + break; + + case 18: + write_reg(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x50); + break; + + default: + // invalid period + return false; + } + write_reg(PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); + + // apply new VCSEL period + write_reg(PRE_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg); + + // update timeouts + + // set_sequence_step_timeout() begin + // (SequenceStepId == VL53L0X_SEQUENCESTEP_PRE_RANGE) + + uint16_t new_pre_range_timeout_mclks = + timeoutMicrosecondsToMclks(timeouts.pre_range_us, period_pclks); + + write_reg16(PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, + encodeTimeout(new_pre_range_timeout_mclks)); + + // set_sequence_step_timeout() end + + // set_sequence_step_timeout() begin + // (SequenceStepId == VL53L0X_SEQUENCESTEP_MSRC) + + uint16_t new_msrc_timeout_mclks = + timeoutMicrosecondsToMclks(timeouts.msrc_dss_tcc_us, period_pclks); + + write_reg(MSRC_CONFIG_TIMEOUT_MACROP, + (new_msrc_timeout_mclks > 256) ? 255 : (new_msrc_timeout_mclks - 1)); + + // set_sequence_step_timeout() end + } else if (type == VcselPeriodFinalRange) { + switch (period_pclks) { + case 8: + write_reg(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x10); + write_reg(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); + write_reg(GLOBAL_CONFIG_VCSEL_WIDTH, 0x02); + write_reg(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C); + write_reg(0xFF, 0x01); + write_reg(ALGO_PHASECAL_LIM, 0x30); + write_reg(0xFF, 0x00); + break; + + case 10: + write_reg(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x28); + write_reg(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); + write_reg(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03); + write_reg(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09); + write_reg(0xFF, 0x01); + write_reg(ALGO_PHASECAL_LIM, 0x20); + write_reg(0xFF, 0x00); + break; + + case 12: + write_reg(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x38); + write_reg(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); + write_reg(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03); + write_reg(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08); + write_reg(0xFF, 0x01); + write_reg(ALGO_PHASECAL_LIM, 0x20); + write_reg(0xFF, 0x00); + break; + + case 14: + write_reg(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x48); + write_reg(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); + write_reg(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03); + write_reg(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07); + write_reg(0xFF, 0x01); + write_reg(ALGO_PHASECAL_LIM, 0x20); + write_reg(0xFF, 0x00); + break; + + default: + // invalid period + return false; + } + + // apply new VCSEL period + write_reg(FINAL_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg); + + // update timeouts + + // set_sequence_step_timeout() begin + // (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE) + + // "For the final range timeout, the pre-range timeout + // must be added. To do this both final and pre-range + // timeouts must be expressed in macro periods MClks + // because they have different vcsel periods." + + uint16_t new_final_range_timeout_mclks = + timeoutMicrosecondsToMclks(timeouts.final_range_us, period_pclks); + + if (enables.pre_range) { + new_final_range_timeout_mclks += timeouts.pre_range_mclks; + } + + write_reg16(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, + encodeTimeout(new_final_range_timeout_mclks)); + + // set_sequence_step_timeout end + } + else { + // invalid type + return false; + } + + // "Finally, the timing budget must be re-applied" + + setMeasurementTimingBudget(index, instances[index].timing_budget_us); + + // "Perform the phase calibration. This is needed after changing on vcsel period." + // VL53L0X_perform_phase_calibration() begin + + uint8_t sequence_config = read_reg(SYSTEM_SEQUENCE_CONFIG); + write_reg(SYSTEM_SEQUENCE_CONFIG, 0x02); + performSingleRefCalibration(0x0); + write_reg(SYSTEM_SEQUENCE_CONFIG, sequence_config); + + // VL53L0X_perform_phase_calibration() end + + return true; +} + +// Get the VCSEL pulse period in PCLKs for the given period type. +// based on VL53L0X_get_vcsel_pulse_period() +uint8_t getVcselPulsePeriod(vcselPeriodType type) +{ + if (type == VcselPeriodPreRange) + { + return decodeVcselPeriod(read_reg(PRE_RANGE_CONFIG_VCSEL_PERIOD)); + } + else if (type == VcselPeriodFinalRange) + { + return decodeVcselPeriod(read_reg(FINAL_RANGE_CONFIG_VCSEL_PERIOD)); + } + else { return 255; } +} + +// Start continuous ranging measurements. +// based on VL53L0X_StartMeasurement() +void vl53l0x_start_continuous(int index) +{ + if (index >= INSTANCE_NUM) { + return; + } + + write_reg(0x80, 0x01); + write_reg(0xFF, 0x01); + write_reg(0x00, 0x00); + write_reg(0x91, instances[index].stop_variable); + write_reg(0x00, 0x01); + write_reg(0xFF, 0x00); + write_reg(0x80, 0x00); + + write_reg(SYSRANGE_START, 0x02); // VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK +} + +// Stop continuous measurements +// based on VL53L0X_StopMeasurement() +void vl53l0x_stop_continuous(int index) +{ + write_reg(SYSRANGE_START, 0x01); // VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT + + write_reg(0xFF, 0x01); + write_reg(0x00, 0x00); + write_reg(0x91, 0x00); + write_reg(0x00, 0x01); + write_reg(0xFF, 0x00); +} + +// Returns a range reading in millimeters when continuous mode is active +// (readRangeSingleMillimeters() also calls this function after starting a +// single-shot range measurement) +uint16_t readRangeContinuousMillimeters(int index) +{ + if (index >= INSTANCE_NUM) { + return 65535; + } + + if ((read_reg(RESULT_INTERRUPT_STATUS) & 0x07) == 0) { + return instances[index].range; // use last result + } + + // assumptions: Linearity Corrective Gain is 1000 (default); + // fractional ranging is not enabled + instances[index].range = read_reg16(RESULT_RANGE_STATUS + 10); + + write_reg(SYSTEM_INTERRUPT_CLEAR, 0x01); + + return instances[index].range; +} + +#if 0 +// Performs a single-shot range measurement and returns the reading in +// millimeters +// based on VL53L0X_PerformSingleRangingMeasurement() +uint16_t readRangeSingleMillimeters(int index) +{ + static uint16_t range = 65535; + static bool reading = false; + static uint64_t start_time = 0; + + uint64_t now = time_us_64(); + if (now - start_time > TOF_WAIT_US) { + reading = false; + } + + if (reading) { + if ((read_reg(SYSRANGE_START) & 0x01) == 0) { + range = readRangeContinuousMillimeters(index); + reading = false; + } + } else { + write_reg(0x80, 0x01); + write_reg(0xFF, 0x01); + write_reg(0x00, 0x00); + write_reg(0x91, instances[index].stop_variable); + write_reg(0x00, 0x01); + write_reg(0xFF, 0x00); + write_reg(0x80, 0x00); + + write_reg(SYSRANGE_START, 0x01); + start_time = now; + reading = true; + } + return range; +} +#endif + +// Private Methods ///////////////////////////////////////////////////////////// + +// Get reference SPAD (single photon avalanche diode) count and type +// based on VL53L0X_get_info_from_device(), +// but only gets reference SPAD count and type +bool getSpadInfo(uint8_t *count, bool *type_is_aperture) +{ + write_reg_list(reg_spad0); + write_reg(0x83, read_reg(0x83) | 0x04); + write_reg_list(reg_spad1); + + uint64_t start = time_us_64(); + while (read_reg(0x83) == 0x00) { + if (time_us_64() - start > TOF_WAIT_US) { + return false; + } + sleep_ms(1); + } + + write_reg(0x83, 0x01); + + uint8_t tmp = read_reg(0x92); + *count = tmp & 0x7f; + *type_is_aperture = (tmp & 0x80); + + write_reg(0x81, 0x00); + write_reg(0xFF, 0x06); + + write_reg(0x83, read_reg(0x83) & ~0x04); + + write_reg_list(reg_spad2); + + return true; +} + +// Get sequence step enables +// based on VL53L0X_GetSequenceStepEnables() +void getSequenceStepEnables(SequenceStepEnables * enables) +{ + uint8_t seq_cfg = read_reg(SYSTEM_SEQUENCE_CONFIG); + + enables->tcc = (seq_cfg >> 4) & 0x1; + enables->dss = (seq_cfg >> 3) & 0x1; + enables->msrc = (seq_cfg >> 2) & 0x1; + enables->pre_range = (seq_cfg >> 6) & 0x1; + enables->final_range = (seq_cfg >> 7) & 0x1; +} + +// Get sequence step timeouts +// based on get_sequence_step_timeout(), +// but gets all timeouts instead of just the requested one, and also stores +// intermediate values +void getSequenceStepTimeouts(SequenceStepEnables const * enables, SequenceStepTimeouts * timeouts) +{ + timeouts->pre_range_vcsel_period_pclks = getVcselPulsePeriod(VcselPeriodPreRange); + + timeouts->msrc_dss_tcc_mclks = read_reg(MSRC_CONFIG_TIMEOUT_MACROP) + 1; + timeouts->msrc_dss_tcc_us = + timeoutMclksToMicroseconds(timeouts->msrc_dss_tcc_mclks, + timeouts->pre_range_vcsel_period_pclks); + + timeouts->pre_range_mclks = + decodeTimeout(read_reg16(PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI)); + timeouts->pre_range_us = + timeoutMclksToMicroseconds(timeouts->pre_range_mclks, + timeouts->pre_range_vcsel_period_pclks); + + timeouts->final_range_vcsel_period_pclks = getVcselPulsePeriod(VcselPeriodFinalRange); + + timeouts->final_range_mclks = + decodeTimeout(read_reg16(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI)); + + if (enables->pre_range) { + timeouts->final_range_mclks -= timeouts->pre_range_mclks; + } + + timeouts->final_range_us = + timeoutMclksToMicroseconds(timeouts->final_range_mclks, + timeouts->final_range_vcsel_period_pclks); +} + +// based on VL53L0X_perform_single_ref_calibration() +bool performSingleRefCalibration(uint8_t vhv_init_byte) +{ + write_reg(SYSRANGE_START, 0x01 | vhv_init_byte); // VL53L0X_REG_SYSRANGE_MODE_START_STOP + + uint64_t start = time_us_64(); + while ((read_reg(RESULT_INTERRUPT_STATUS) & 0x07) == 0) { + if (time_us_64() - start > TOF_WAIT_US) { + return false; + } + sleep_ms(1); + } + + write_reg(SYSTEM_INTERRUPT_CLEAR, 0x01); + write_reg(SYSRANGE_START, 0x00); + + return true; } \ No newline at end of file