From 9e03767e1bf51909784cd19e29c6e1ca2c59334f Mon Sep 17 00:00:00 2001 From: Sean Smith Date: Mon, 6 Jul 2026 17:31:02 -0600 Subject: [PATCH] Split sensor types into files --- code.py | 78 ++++++++ lib/adafruit_bno055.mpy | Bin 0 -> 8446 bytes lib/adafruit_bus_device/__init__.py | 0 lib/adafruit_bus_device/i2c_device.mpy | Bin 0 -> 1180 bytes lib/adafruit_bus_device/spi_device.mpy | Bin 0 -> 851 bytes lib/adafruit_gps.mpy | Bin 0 -> 5972 bytes lib/adafruit_register/__init__.py | 0 lib/adafruit_register/i2c_bcd_alarm.mpy | Bin 0 -> 1764 bytes lib/adafruit_register/i2c_bcd_datetime.mpy | Bin 0 -> 1140 bytes lib/adafruit_register/i2c_bit.mpy | Bin 0 -> 801 bytes lib/adafruit_register/i2c_bits.mpy | Bin 0 -> 1053 bytes lib/adafruit_register/i2c_struct.mpy | Bin 0 -> 1040 bytes lib/adafruit_register/i2c_struct_array.mpy | Bin 0 -> 1008 bytes lib/adafruit_register/register_accessor.mpy | Bin 0 -> 1370 bytes lib/adafruit_register/register_bit.mpy | Bin 0 -> 659 bytes lib/adafruit_register/register_bits.mpy | Bin 0 -> 882 bytes lib/car_os/__init__.py | 0 lib/car_os/enums.py | 4 + lib/car_os/gps.py | 135 +++++++++++++ lib/car_os/imu.py | 211 ++++++++++++++++++++ lib/car_os/lidar_2d.py | 110 ++++++++++ lib/car_os/network_handler.py | 125 ++++++++++++ lib/car_os/pps.py | 49 +++++ lib/car_os/timer.py | 27 +++ lidar_code.py | 179 +++++++++++++++++ 25 files changed, 918 insertions(+) create mode 100644 code.py create mode 100644 lib/adafruit_bno055.mpy create mode 100644 lib/adafruit_bus_device/__init__.py create mode 100644 lib/adafruit_bus_device/i2c_device.mpy create mode 100644 lib/adafruit_bus_device/spi_device.mpy create mode 100644 lib/adafruit_gps.mpy create mode 100644 lib/adafruit_register/__init__.py create mode 100644 lib/adafruit_register/i2c_bcd_alarm.mpy create mode 100644 lib/adafruit_register/i2c_bcd_datetime.mpy create mode 100644 lib/adafruit_register/i2c_bit.mpy create mode 100644 lib/adafruit_register/i2c_bits.mpy create mode 100644 lib/adafruit_register/i2c_struct.mpy create mode 100644 lib/adafruit_register/i2c_struct_array.mpy create mode 100644 lib/adafruit_register/register_accessor.mpy create mode 100644 lib/adafruit_register/register_bit.mpy create mode 100644 lib/adafruit_register/register_bits.mpy create mode 100644 lib/car_os/__init__.py create mode 100644 lib/car_os/enums.py create mode 100644 lib/car_os/gps.py create mode 100644 lib/car_os/imu.py create mode 100644 lib/car_os/lidar_2d.py create mode 100644 lib/car_os/network_handler.py create mode 100644 lib/car_os/pps.py create mode 100644 lib/car_os/timer.py create mode 100644 lidar_code.py diff --git a/code.py b/code.py new file mode 100644 index 0000000..5be4f55 --- /dev/null +++ b/code.py @@ -0,0 +1,78 @@ +import time +import board +import busio +import adafruit_bno055 +import adafruit_gps +import digitalio +from car_os.network_handler import NetworkHandler +import math +from car_os.pps import PPS +from car_os.gps import GPS +from car_os.imu import IMU +from car_os.timer import Timer +from car_os.enums import SensorClass + +def main(): + heartbeat_timer = Timer(15) + + network = NetworkHandler(SensorClass.VEHICLE, 256, 0.001, 5000, 0.1, 5001) + network.initialize() + network_timer = Timer(0.5) + + pps = PPS(board.GP22) + imu = IMU(board.GP21, board.GP20, 0x0C, (44, 66, -109), (-1, 3, -1), (-7, -16, -39)) + gps = GPS(board.GP27, board.GP26, 0x42) + + while True: + pps.update() + gps.update(pps.utc) + imu.update(pps.utc) + + if not gps.has_fix: + print(time.time()) + print("waiting for gps fix") + print() + continue + + if gps.time_updated: + pps.update_from_gps(gps) + + if network_timer.check_timer(pps.utc): + network.update(pps.utc, network_timer) + + #if heartbeat_timer.check_timer(pps.utc): + # message = bytearray(b'\xd4\x53\x6e\x4e\x00\x00\x00') + # message[4] = pps.utc[0] + # message[5] = pps.utc[1] + # message[6] = pps.utc[2] + # message += b'\x00\x00' + # network.send(message) + + if not gps.ready: + continue + try: + message = gps.get_position_message() + if message is not None: + network.send(message) + except: + pass + try: + message = gps.get_sat_message() + if message is not None: + network.send(message) + except: + pass + try: + message = imu.get_accel_message() + if message is not None: + network.send(message) + except: + pass + try: + message = imu.get_mag_message() + if message is not None: + network.send(message) + except: + pass + +main() diff --git a/lib/adafruit_bno055.mpy b/lib/adafruit_bno055.mpy new file mode 100644 index 0000000000000000000000000000000000000000..d889c163d1d62f4bf7d51505bf20090267cc4399 GIT binary patch literal 8446 zcmbVQeNbChc7MWvv5?{EgKHVE{rn_^B))_Y0{jspB!O|vH`vBb_93JP$N`}Uk-+Bb zU4d<|y@@B2b~D>-m)YHRyq)QeBw#nm&g|6SWE1zVwtu8)-EL+&{iB`P(%tQ})Be?S z?|VYRW}R(~_4GTxd+xdCoO|EB_sCnwoWSQx&4Ey0A~qk63*(VlM{{$1bdf35%*A8# z!8lVC56_5=yYw={EC$h+}ebIP$HWHX-)Wh@9X^|-!UW|&2dL#lYqlqs@!;wj*5S(F(Mm&SV zjCy>2E3n6bvdF?gkw8C4W5 z3cIJb?#!eZ5f`Gd=a?QO2(O4Scnc%2-dO?Si_QW<-oD<>?k=IH&*x{19xsR&M-{2Z z)0HE2jSgld1e(yC?M7O&-Lewv>B(qy_l)#k92ruyynQ|Wo*_kIdt8bZ-|Oq^6gqpn zLSJX6tg53Fb&!Q&{TBsi7o#B8E^6&!baHE>yNgBwiY-EGN21 zG)$8*(l5s-(@%xSQ|&!byD8=IUGnsL{XS}B$aBdrn=!!Z?DY=|Wb6zk?q_f-V5I0Y z(%H=jbP;5F=eEczLTRCSu_{7lLgk??3OnfU8oB5hlu5Tj`g_k)clR`A?7;Mn40^oE z8Zawea+Ag@+|Wf>1AB+tlN-CD*V#Ad^^;de)-`&rbPoxG{vJ=ia0S+IRvO(SU1cme zZT)>i-NW6N{Cmh=f0u`n4Ry_IG)A^!*xQQ}dM_yb-LQzu4Z>Z<#-^(1*gJM zE*#>lubnb2yuqD3%TZusit?dTW6=ReeG7%o0z=RIT$~#hx$q3!qh`cNTnyDS>H{j3 zN}Uf6Rl)HdxSDV?L=BD@q8i{P$ZaaZ(Q}R%ayA#}$nA$CXTsbZC&ps4v3jP!KuLP2 z$=oUKwKo_teq({2wvkgVr|AdzwV&px#yF|M(pl@g$e-|E^O{4PDd(Rz2Lr6Cs`8u6 zyZ@@H2=wjB6j*K=$#>{Z=}YZ*j6WK>Q!5Qr7!_A)kf!o5I$T+TN-9fH zDfG~?I!R^8(qJHUb>5>B{YLkB!Om^P*g%MFQeEBn!AhQ_t5-%=#}@;s~?~R2`#Rn zZ}L(*dX1OH(Ca*A&>KAF(3=u^3nR=V5LPWAoVSed0SV#!6@&}!Azb)A!Uxw7F1nBK zA>M(onr}t87&J7{>oD{xfnKH1iv>;#oDMiWa0cMYZXk?qB7Ec)!bWI63RcU(Y6V!W z1o_w+!pDhI_yq9^bHJSh8$8%B0X74!8g?>lnYPe%0F@0pRV>mT1c-*6dF&C&9vOBX zV2!RkWE^(pvq!Z?Xg)gZEMUtiEFTCKCSEX>mfiV-3GVtBqs114tJyMytx1kiA#T8~ z8cLp2sSxkXL#AZ#K*IPSznsH%T!U+I-AU;W>vXujib<7m>2-JB=tjzTav+!p{YAl1 zo+>q9hx9?ciYis9%6OvnP=^~+V1DBpE|p6aRN<+u_L3Js-7bB1C^a8EZ%!M{N=j(3pKMUV zwFyDH@_fBI+`Q>h;V*r@O=%>!8C)*)%eCZMmpgjbJG|?5)hCK= zQ+ku68*C2*e{hClMmt5ZHs@5KYAv=o^IR%q)|&Md&I1$yl@Fkvv=sno zAfOO{L+b#ggMbR4 zLRU};=t=-ytyfu;T}36JEdV;To%)IuLRV7>Xe)p&Z5J4q#z=#W$WA-5IcrcAiL4fg^V;)} z{5pc1^~maMK-~jD$1S#z=Wvbm%j#-}#bUL=CT}w*4ONm`^&ka1;A3CE4!ccx1`Cng zFx6txQ0-<$lLjk&+HOg(43h>Me~tj0G}@)4vF1k7SgSi=s52dKt48l8;f=@$1HmO@ zv!NQs{5Q_b4|ze`e7=|(SOWtWiZAPMi?Y?zPWfN~_5kD25;JuAZ|OR?$Dehp-q{H* zwc8Ppxb@zo%~oY2M$Qs1XuksJmy#a)1}UA&(8M9|f$c{1xaQt0XSG-?Bx1gqgnaYY zOFLVsWq59<67YPOTH5&}y}q$&&krWXJS7d#XOXid_4SVY2V2_@Q|ZgszU*Yz$y2>S zp7~2(c63}F4JH;Ekpr~f;>>2N)tuCuACOSN86{&?{5iL3D+OE{&YLfTOHrq>5rT2v zTZJBtq{r6Y9+2ad&kxES9uj4GZ(7F=X`C?LyIfM=6kO1%n?U$B?0_bC^+X)Cu`VF~ zXsnJHmJ&-h>+=8MZ+yO_zBy<*Nc5Yb`G>pu3yrKBi2GypR39(hSg9+JJG3YrTA)3Z z>5yRB*j6Am#~S1giRIPXb%k<|Hl;@!DEvIr14kR#Q$TFVF0HKIzH=9bATFI&^iMOTj>S5NDoQ|zCE=6}i6ZwKO6PwRUWeGfGMXGTBa zu<76wy!St2)@m!6Rt$3j3uy(`QD&BxQaiyL?T=JEIWF7Q^7(3*+G>m4Y8luf)9~1L zK6je47Pfi(`ufg8u(Ps}`V}=SubcG1Kfjk&0S};R-5hn z`m{H@Ti;9XY?I8a@5zjuj~W=Z{fS|3=gi10l8-3l0~BFKE@bji1#`0R*w4xb7~bYv z<#gPB#yqv!bUyIw>NEV}TZv;+;23DaM1?Yrng73z-THdJVEy>PulN&w-FYHkbdo^P zy!PhrH%VY%OOhw)&VG|r3xlpaG3e@(gGL{z%0UsIUVE2}YWdow0T#A+X+a)$b-!^t zVcZX%G4A_hMNYwr1V!`OpZ;Hlo%~(H!mH2)!zQ0GEF9aJR{>PbYajo9Stju+ufkK& zVxviCgXEpT&b95#j)jZLOR5OD_GyM%9((!td2sKMq_bHpKJp=Fq2z(tpvT}AJqEYo z7=+CT8amvnPc{?3#yg*GVtD>y6YqSsv7Y|PPd~I525(Iz*jg}$&#wKebhz8<W9 z1^P%i3gNi?4kuy>zyMZi~$pj_8 z^}4m?=+6veoV~ubrnaHJx0ZvycJjI29@;H~j>1h>i+C4X2?bP>wrqtxhIuLUUr<9e zYs=mS=^nu?+Oqf9V&=Y{cQhp+Oi*KC6D$9FO9Ir zB+$D=@HH7&uEuXSux2bxD%z4`U}#fG(7R8r2hG?;_Y?A zMUcq#>4g{J2E|Pn=weE?SuAc@o9LBor42ox2mje#%=TgTGEBn@FA_r|!FOEjwep{} zk8u<2%ZFF=VGatEJ%887!tJ@AOvP5dTd|QK=?xtMC6`-LbkOR76dkl^AVmkQ6-d!R zO9WDL(8_?+9JC-HH3zK&NX6EdbzqQPy^!>(x zJ@asz3tHrP=ty3uv7M>r-F0@WLx){y9rn8Nyxs7Xu7SE4ZV>&3iy%Q6D#1mpIqg!_ zKoZiwfjNRJ@SvO7?q~-k{ZQfyUysbJ|JJ1b!V7z8zhRg*xm8e&H*dI9paYkH78B$_ z+>l0i9C_^R+Q1|Dv9FI@4m1V}NNyNd90=ZJ|4HSf(~o`0%e<+%ns2RZ;_Y^;t)Z$~ z+U|HP<#>lvY2w(fTMret(uv>fI=1m!)8Q|?)?CM+)Fq{I;_rRWL#fQ;(_i{JIxei^ zf8JhS`ojCiZz;_=e#>f#!zBjGrs8lK|E;fUGciz~Hxkrg|7KzYes)+mO76(Xm&lIf za;t&~vquA$;N+}BmuG;N5%`sx?mTzi11P|qOe*tZMgK?bYWT4t868T(PZesYlVD4L zUn*Si5`hW07ANCb6L0TlqFpgDPfZj(NZlg_+_C|rU`4^k!- z`LU!~wgC6$eW(2V+*1rD? zYuBEzHVW46=UNM7tw9@D`}1d53*=fGYtvi>YwNPL6J$wClW^9#^WZn8#6pJ-T5(tM p!|Gc37?5K<0kOK3P$!=-cMZ(_Jl9+}*BrEhx$WO=E}UcTe*yK9-o^j` literal 0 HcmV?d00001 diff --git a/lib/adafruit_bus_device/__init__.py b/lib/adafruit_bus_device/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/lib/adafruit_bus_device/i2c_device.mpy b/lib/adafruit_bus_device/i2c_device.mpy new file mode 100644 index 0000000000000000000000000000000000000000..561f277e74f24524bfd5be551e499b07c56446d1 GIT binary patch literal 1180 zcmZuu-EZ1f9R1k@LJ5P5ac@X)62{k3pwJLPwP>1Hf+7-WZD=w@Xo?(cF2zcMkuhna zsnR8jR^GZj_hGMnBuU%%X;uG-Nn8JwT^rIxQ(x?J&$;LKaqhYc79Kj*>WzBS=xL^` z^ty6`oMOm+}O>q2Td{6D|xU-n!;1HEze#c@v*#K z+v4`o7dROM{MBwAkgvdFUsdIP`6d+i4Z?J8F!?g@GVqLM8l{Unfkk`|`EcqLJUj0f3rRZEb_aGnyMS4haBAM7oy!q7Y{ksvo zDsUgHw)0kvD-^g!!W$HPi^O-ufrEALw`RNxY5H4qX{{pgVuTjCnsxunB9wDOwZHg! z0!7nH`DOZQ(c)ul&29C?_TT(`C{2GqC`FgzCy{9Khn>&(H!BstWwI8CaYeqiRooK_ sAIo~JA_^=00{4fd+KE^qw#COap%~-EIP(;_0!Q}QvsZ6g?^o4-0moNTdH?_b literal 0 HcmV?d00001 diff --git a/lib/adafruit_bus_device/spi_device.mpy b/lib/adafruit_bus_device/spi_device.mpy new file mode 100644 index 0000000000000000000000000000000000000000..34e296fb0e4fd6f09282897a51633f409b79f843 GIT binary patch literal 851 zcmXv~-A)rh82z?Dt+hYX-F94P1uSj(5!+a$60i7K378Pth(e8K*-i(#v357RvtX~> zB0;?HT~-Bs0r3UAkVF%&d|- zG&M_SZ3vnT&88sJm(0>s(lp3(8^-+HHek}wP1UANN*IjVdpaWxl^LqxFq4b#-yaOx zO-fBb%xbIMmmm_OwXN?s7KP;gO2}#T*j22myVT`REgBx*MR zh;$Km3ZWiVgN`sT*I>-F?-dICuNzJ+PmC9ZiPi+xUcG>0py?ZB`>80@IcCs{hSH# z+sml{4saag_)02>gVhJ&jo8R9WvN=h!3V-K6vCrpfUmC?0fq1QaJWK**G!IwkCN_< zkxt?u5KH4R6vmm#lrRlt6se^T-;JVZ%|(c>7++%THejhJV7chSsS4>k*!=OWX`|rdryuZdvf%|lhG$nj(zrI@q;HPK6-M*lM>1rFL8{ndQ#+DX&PbezkkN#fC; q({BCi{#2Bc&hOI`cVTtoYghT&G56B>z4_$(V!qllHC%#fnfwDJwe~;& literal 0 HcmV?d00001 diff --git a/lib/adafruit_gps.mpy b/lib/adafruit_gps.mpy new file mode 100644 index 0000000000000000000000000000000000000000..1cb52139842d5c9c67c1579579016838fdf978ed GIT binary patch literal 5972 zcmZu!dvH@%dOwn}EkC)sQZ51<15p-3o}o>#KcOe*6_T_RfzStTAN)JSIxN0m&L zaAT>gl1zl=Nd58o)LfJ_W-q0b#0;q!a(js(cxd7{sSnR*lq6}Hh+@0&TvVKo#iD7_ zAuDMad#5gCXOju3vab24l8vUZpRLT0q8GE#1lA`qq;aon&mPx)QZqO{NdzEiFq=%V zgIc6iD4mH)k?2f18qJVJyWT|_FCXlC&9gvgr@1n%B(lU(p<{E&P?oICWU^_pLrO!# z^*yN6vd)m=3eK;;yx_XL0J-vE+0fiM*deT)9my^rW)MDHhhh3JQf zK11}Aq&XCcq@x*Z6^VxDXGk-1XL3GE?Df)oD4iuc7$cJn#Z%IJRwmqBD63@WBT-^q zqt^9%&Vh3CSy;`_B@;7caoak%F28}bwm^s}7o~URLvu>@5^32mkoHE1ITOl8=P-qx zcRi=2R62T|w9O{d%Dc$~d>}=Xxp|fZ#2lI{Yb(WxWj3nJ%p!53NhLyrOez|UNaqsC zEYz$F;K=B`S3*B34xst;vk1ySu@PoJ5&xpg`nm)Gf;nb31ObAAK;oPRz$ zLoC=mBFV{kJd}u##UYlu?u!egc{Y@h5Eo)*#NLQTb8rXLL>=x(#G|3-Ceh1gB_Ul1 zDOpy5^hzavpKHFSfwnuP`4rng+MH?4*|-#sgf5|gLBMjxxD*W`zfl|Wa#oWk1}zly zKusb#L-ZGjK12+c7f=*D)+DpZgd!7qjJ`vg%!*q^?wwcQ>$deif>yA`Y+oq0tI)=oGO^lZQu-A0G7{mc|cF96B~hcr6@d51pj1*Oyz&VlU5Ftid5-Yr!ZF z`6zM}?G7f#lf6CrxKuisil$MGizb8C5G+Bs6sdkVokot*3$(gPZbD^;NaHzViR?ba%&e z(*rq+|ETShyZeOK5gllK{;$nHZN0WV&qb!$FK@^Tfbl=A$pv=m1$~aIa_V+&X{i#b zrjGb>Ts2q2)jF$o8d|D_>XsUzrlnS>oyu`_T)k7jv!SIcMHmx~7&!p%HX5 z=&dcyLi5yc!$~70|LXHj4eQQuoIrJUB2ax!pjA|$TNVXceN~_}d4bkm6R6?3KIi%|_ZJ(5-fzKsh%NOm3ZEaqETc?kZu2dyBBsT`j!qt`T;- zYX$3Zw}CTy^n&rYyN=_HMo*Q%2ju!29f2JtYRtXe8@6`1d%o4H2EOtR^ORRTx~o6+ zi2+$Po4J?zbq{%JD&&G5oh=vnt?jS!17APd!1?c$5=NY0-d3Q&K;ESf_UoAFKXKpu zY6UYTZH4W%Z#=q{V(FPo$BoSv96Q%xV+VRwFHV9SPSxhNV=ldU&b&6a19KNQ&%MOv z7LV-Zcg7RR+?CFrHnk9BM}D)S7U;8}xuu^JSAG7HUH{L&UMb1fJLHy^ndt3~v6;a+ zS5GVV5?1`eHwIj`w@<(FSRBMKcY5V<>HPpq`ji!j!(6YX|aYkQ~PVRQQJoet6Iclqr@t_`@y-wDppyKrE$dh)HG`D=K(^=G1Z zdaCSIP7NYfe9@x=E_ifbvaoiMl_MoRPc2|bBY&*#(?9;Scs~fc_$3&$6!_|J)q~vX z(`D|JRbSu%)fSb^wxxdEl=xqCnX|9@&V1q<0xQ5+qc4d33q)F1eWRG?P1upi{55!v zRp03i%SJZNe-HC*l{MQdYi@(r{yWyJ&HsCDnd_*mu~gRl1ia2w-_(oetj%9o=A5gV z>Y6pP%Usv0FStQ^aLv$TJl$^JV&1mQ(m5*rWC5ki(;cjI1M-=YeEU7a3FDowr>!@G zdCyHw8Msxe3@8H;{20z#^TwNe-jZ+0yYgN6b|c*>&=$@p(3i)Zy2-sn=ta$rO)y$7 zoHo*3?kbN?IKzl7Al_y~H3*3jH6W%@%OSTxc#ARWzzDnR1=-yoMBI(Scz|uxB+R;- zg(>$|;kcUIb^p`d+%5T6=Q;<7>5U`sJEhbG4f|Y`(&n zp58En-dRvBeF4?72jB+m1$Y4a0R4bhbE;)O-~i}@fNud_1H2A+1Mntb0B{H(0=xhp z;4r`s7z7*v3;~7#BY>lTV}Q}3YUv5ccRS{fjM3fqMsu~cal6CjXm>bmJ-6!TMO#Ok zv#k?9Cw@am461p&pFGr}_ev#tg}_%naa>kSJ-$Jn+E52mg+Bcwv@2FB*U_;v zUh3C95&to_WN${kspgLhYhAN_#p;IWoof;A!yom?@tC)Q=d5m#WdzyV7A};bB)Qgl~Z+% zVJs_QS)VB^Q(|nCF@B&)$o)jVJss?hfb%!)V%t8O?WA|I$JXOx!9_20nNUkwf%bteU4=X zcDni-Dvq6ejLWX0a6jnL{c?r=>eT6(m=|3!(XIQ>GDqdODxU>lFN~ut!&nnl!ZOb1RowM*&Cw1QYkM8bn zNBdA)+mY_>ai(96T^+bm8NJAEU61ZDqx07_`qLs(Jc5|uGi2Y^SM}txr{V<0Cglmk zXFp^3+*5HDi;9)aa08f%mCZi0-DT$#EBifr%InQ@pFCJD-CxS(t~l%Zb*%j5{Iqk| z6x|*FzOGm7bcFHj^s^YSX9Rq93w}W*S$W!2fGe3!7#DLvlzuS}{ATew@Idh<@UaQ` z78!0m&UV}0Yr}3`7rHJen>M_$zG2md4f&nL9Ov{@p&9WnbMXR~$Z<(9u9H4JniF~x zBo?_8lh{|0ScF8XBGHeIrJ84<5i6Z*(8Z_e1&!{`4(|^#jrQ%e9dNXU=i0j4{Sl^E zEavi`(~KQ08~eeBJ$lv+n8Dq+UblP@=4s=HK+F}vHGTc8H=_68yj&25_v@#MR6CMt zj}`8nRo^kxTyE9pqYxE+Y=LILCKH(fFPNC$Y-0We6E&OOEZ)Fjhl;m?2-9HkHa9dG3HeSc(|-zu(-r^zM|*9~5oj(V zkK7JN`s~@WR-~bq#SAU|JHNLcv+{V=H^S1jrigSEhZtGQNS3)Qa0l4EQ0^oTwKVo? zu{*%iZl3l8ii_{D>cg{erHkM?D@h}!$ExYD*~X>uKE{I)o)EjB>qnZdgP`te)Dckk zG-_x<{;uH%T8Q`2GGtdwWmoz|q5p73+Ar!!a^E);?oxx4Sw!9bN6t zBYtKWik2C6&(Aa{lYjG!sj?2hWhwC5lgJk)f+VOU~-2_szCX zV12o1Hcd^bCJ|0O%={$Ymqr?&f>9O`-KuFsH4U%&{xgSDz(<5l2UmToIo0IFqQPz~ z@`~?PH0dIL3%)tqPoxo{Pycs+zx0>+;BUV7g>UdjX4_vdk9YhC2cYkH&GUH9O|b(+ zX{6yFehk)+bHNkhUUvBTegdry;<+**=d;$`{Ga{rwDsKdoratE<4~CJ1=Krk2f@ig*+hEbj&imdiZtqaIzs@?IAy zSQeHFmiGgA3;dhTrroeAH(+mzb+&ce+xxo6qe4zRe{w}Ny*t?T_`dmyYI+Yw1LuOD zF5PDD3)AHfzw~(r#&pHEuOgT2yQ&6N2nIY5JH+b!`R;>eVY`eNg#q#?C z!U1_)ySCN#fqU`|?m#Q9Y_)r!OLW=rCJDMa9kz0ORMY#<9*Q%nX~Bo6E?fyh1yKyC p<`8A~pH~za+iXKULCr#W(HGvWYyq?`FF;!NfvyzRLJ8OG{{b=fZ&3gM literal 0 HcmV?d00001 diff --git a/lib/adafruit_register/__init__.py b/lib/adafruit_register/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/lib/adafruit_register/i2c_bcd_alarm.mpy b/lib/adafruit_register/i2c_bcd_alarm.mpy new file mode 100644 index 0000000000000000000000000000000000000000..cd17a7c1aac56bfc8fd9c3e88eb5c2151e0209dc GIT binary patch literal 1764 zcmZ8fOLNm!6uwGgV_Xw%WFri5K}e1UQ{E6Jpp zPK#txO4)SLO?UkXHAzStb{!HRY`XwvXuG1j4%39`blEE_cBh@u=$`YP@4QdK4i2B` zVz>fVRBEEeDndzAH9?7pQJ&571(uuRluEd|0QR~D!UF^0LEzBDiU8nbRTE{2n*+{F ztvV+FM`oca0GGB<6{Qk5bSau(gj*snFp79fP{1fJDm= z$3&DH#iT4dD}15}l8W-E0IC8nONF@w04t(Y(*z{B%5qIXud~33raCVOAECFqB1>8s z3Bh=R88I8kppvI{S_9B*ESeW3Qxc^p(g01AIhD0isKAx46^p27pBb>gEwE~tQ-lII z*w!^p(LguLmIRGufybPd)e8vBE2s%pD+>~uCRY$8O$Oa^O=AVA0H7$bYSWoeHKoRD zY;&3@lG==995aSpTE(mc&9cIxRzWv<%|@HTJSwLtR7|0MiC|eYUUNjK4#=XERT)Xd z=u|R2lblH8*bEpyIWjgfHO?j`$5WFN$%zchT$&z_ z2&xJU%r56|q8{=$(OLvxT#;?(%9>VH&qN|6Q7hN-VP398T1ONaX2w}aueGbBbWG9l} z^EnA;uE$AZGsN-B&MQ49eq$23%h>fOAly-m@K6RI`k@Q%f!%N~gs|thod{>59T1Oo z5O|vJG{6y;JE9-DK{oC-7XN&=&6|iPq6eDD{lDIA@%3vE$L~dT-+lbv*`AZc0K|#t z)RYnGoE`+kzH|ti9>j=)(GG${5f>dleSuN?24jTxfIdUfe%kL32K--aEHlhQg!L^VTKM< z{qzJCp;Ht!8Kcv*8lzKzV4z6F$p`M<3;P}~dweJo`r!KpJY)O zO)w>7GG@?B-s-QJBy_FE({Rr^?zUk4_S+ctq`vVl_;7}@K>aDAAkFlA#iU`k<U2KMZDn!IzUqRn^~!vA3n z_knE`%$vxY-gnv&r`z5p*37%Pmj3l;=Hgm?`^&$d$5W3?ZS+xnYctN=eQs>xou0$g z*POBT;}5Hk)^3M(4`C~z4*r{qrL*MVP(=Uk*oAP^A3Qo53>7bjCi*7&Qo+92q2N>i zKfM&g>aUp#_%PiO!vy>^swIXQ+j@Nb%457AY0Jh|)_8Mu_C+F`%Pko*>vX5*$jT75 zv4V$xL1o~>X#WBp_D~3XvTD3wK16Fu?BM3c67~3q@px5#$xNH-1nybVXWTTp8nnsm zs`0V~`J2#kcD4SJNm;Bu3qm!Z)DXU6YUaFWEn@(Ua4UvU`uRW1LhD=Ec;-JA6TO4b gS<|`a6*Fa7k6J-Z{(1B0_Tft{(}5xy$(S^+Upo*6FqobC|Af! zLn9U2cu8z@q#3pb**iVjvP?V)%j-M@>paFmq3;c-3(1m`drW?%#B`v5gEEiGl?oU+ zgR&?8IYc7!SEw5%Lxx!ZhElHp=xPsJjfO)gTAy-b#ishC&!O z3B?%2Slbh0D069pBTQSwgt4f)VNwf)tS-gG)RAbIc#=zYsNK~l<}-mzP!2K$X&aEn z|84LX0^YK^HV2pBjDt1ihi#|D-LUPKjZp_JZ-{o9! zUR=ofHWuUa@_c+jUZ}a^QdZdT$Culcpt{n?c3rG$Y1bOAG z;1iciK6$yS%?;4QU~tQgKTCLHZ}j)aTa76zTW*>wzyI^`MjM{_qs!MhH}ac!y%x#H zpNY%z_Q@dYJ1rtPRLu&fMM0JdK6woiUVZ6}g!Rx)_Vb8a-p-_s(v@iLx+Hk3;WwTX zb}!ymj$Ch5d{i00`OaI7q~I=E)&AJN6VlSoZr(l39HbLPrKl)MQh6iAzP~6Wuy3J*Dsb9}||r;zK0b6$XJ4>;$BIp5@b%y9zxgW9a+3pSaR8l16* Lzz+mAk*fADPn%km literal 0 HcmV?d00001 diff --git a/lib/adafruit_register/i2c_bit.mpy b/lib/adafruit_register/i2c_bit.mpy new file mode 100644 index 0000000000000000000000000000000000000000..be27c0f3e137e912bb6ff083b497e96be7530c34 GIT binary patch literal 801 zcmZvY-EI;=6vt|&eHx1q%wuYChwK;sM4)Wmq{0M>Y6GRZk}a(?Ih&p&Mklef8rl2oc_4MMRdSBXw# zO&|#omkEv6o51Uyi_gu)lK{R{^D=E{3V^G1N>rt^4;+PNT?TI2tP`aQZmcBI8TpWi zaz-PEvId@sL=)NIdXv^v1rOTdQojXD{T>~-@>@%Uvas3<5ywUPjf-q*% zbf%xpf|y}-m1Qr4yZ2jxsVfI?Ns=^K*MXhM0sPd|g~cnk(*twX2Il1dcpqmBIC(oe!M&a` zoHLo}?G5o%fEI)z09{*-8qf-dpe<$%+j%Rp@)p=S*3VD<@i71&80+9yi>l-nP>>jwYZJh0T1-vYrHJBp2h> zlN|CUEGU>0t;gWwF*h;7U%xRqWz61*MI%}By#4EYC*=0uG`p86t`izP>jXxP%@3LE zdHbh%*$H`;mb)zd!LpqAe&K?HUVb`aJc!Pp1>p0sPu-x85$r*~2?OT*v89-Mwd7cXLD)wpa17?JDc6$7^}>4 iy)8M>oCPjdd)aXHZ+D?^0`yMhYGHSO-MT-T0`hPELhJqj literal 0 HcmV?d00001 diff --git a/lib/adafruit_register/i2c_bits.mpy b/lib/adafruit_register/i2c_bits.mpy new file mode 100644 index 0000000000000000000000000000000000000000..6b156ab5d02ec2a4b2b2ea7f29caf6685606ee78 GIT binary patch literal 1053 zcmZvZ-%}b_6vyv^Xdnw*7P2Lg7>cqOqEK*~hrF~3jYgYBjL|xi%P;F@X@Y~bizb!*Yrnf)qE#1nkQC9Mz)so@>V;l}6i= z`6GyJ>ATn@0~Wwmo12@oCbwl)(WYvc1VB@*X|6MbPz@7=Kx}}1DgcCCLnRoII@V}w zlA>xv2O+&l5Y`j`RSlU$G6)ZLWmE>p7c2hc?-gsf1;fF{(<~-33~YWIN&cY2p06hA?V9`M~4$$-kTWWhY~*C zw_(ANEHio`F~W~*EC$viSKgFM8;i`N)gpj?(Zl;!iwq1z{Gyi+RAf*4A>{m>%U1Ze zXeSmK2EXT>sf))2LJwjz_5EB!}$Prxs3f0OZ1%2 z@$#=;_t~F4mlQ2UUUXCT6Ur7kUh8)L>CH_a#y+F3o6Mlgifm7TK%k>oth=57f3*k( Q%V6rGtPA}UcS8jD1B{oUMv+1>088hW)$ zO^R=W(l@a0(MM>~o8ERxu=n8;)8j)2+ya3)}L%YDhQ*3G&u%xNU zN)=3P^SL~}lqH` zLUBc%0Mt-4QTi-YbhX}dGHj=y9kXj%+kUZJ*0Es#BnXtopyad}6sWJJ(%Wu&@l`97 z80=CQ$S3>D6Dq1+7YVQ!w|aI?XE-~j3&0>;$8}Y2Uds3iv^LgV?`ni-#%elUl}W8} zmXg$Zdhk){^}&zj`kVaN-{k(o|D|XR_(m3{nNJCGWG25k9p5;VZsIr7(1D&N0h%%k zQ73Y4@g^LCF6f5C&@=Cfj>Lx0P|Sr~F*kC@hS6}$gFGb@dgqzwXv~Yer5*2aApSF- zEA23+MIJyl<3MbYXP}Q>d_~>?{Q+ObiTnl0*;?=f*>=?Qme<-NfdqO6Sv1W9G**~r z4ptZx;vHzBr(1gi$C5M8O>Tz2>qKCXvx3fWpc8^w=68naM1r@SP|!RoNMCEe7#K>< zzA)2V_BIrLKYrg0kJBVQ5LxhvmA!V93MZ$uK0@n*b2O1*tU}6hm~}`k?HEoZXXnk# z5=Rx2R6+CBUHkP=spmFI-J&#-WqKY3sR!U|uTp?Sd(;N-Jqln&l71UtW2QI%Z6^{d zNY`6`n6N$T?h?oG|3Te%Lb0xS)ax4f@jX1=?LIbt*UCE0L+RRc9Jc2!E^&+8<4zAe oUF$B}hZ&)^(e41nS^!>u`^5Cx7=9ar?w+YdS=+f9e3=3GFC0!G8vpv08_GbFCYn;98X6wmx@@a%-D{^cw69CT_*vSjRfgJ(&iue1y9`k}kIUH>V zb%UB9WA-W53>84#V5t?G0@ODwg}lH$+dKw$`B^>F4j->%6jik~#{qDL2J0%w;F&iD z2xFGiVN@^%5Hk~X^f8J35^(SWx)EO^c6 zH;0s3&b?;yK&OZO{RT0Qn-?c*-oLnACxH7jfcyRVB^&Gwh-S`@Ia~Pm*Wk~}%_6r~ zaubz}&844yAb+)B2$jkJU329^7=<5IT^NB;7^{a1Ulb!KQjDT#F@|DY7sl&cAyJH@ zcy}wglUw?4qto5ucDE$}lkE^nZc7|Yu_7gt(C`S%Cr=A*;v{{NQuoxoTp3kh5?z-7 zN=qEdNg*UiVRS``prTBoLokY(66UW| zS8lrN%Z*y_@{IN8S%2b%wyo zsQ`pr=bo){XmJXx=10VIb((*dI<`Spm{02IEUG)R?%}nI?t~wa1-;uc6b73X%X#Fffv^%S=0# zsGGQx$9zJ6K!W~2olnR^`qJrBA3JF?{eW)7rEb#cllIwXt-aS;I+-bSa~0ny38jkM zmQ+p_tCFIMa&la7C7u@*r70&`9pv|}CDzsw>&Si3keUtTRXZ)IQAOldDw7pYBwj=x zPDYj63MW|2gp}e(B_j9oTmJ`DTGANG?vBLV8%aPhAivp!}2!IB`zAe=ju1ht03 z<72s9sfaQn>q?ocNV1}$6xS;854kZwYN$sqL!??08vri} zgW*WOL37-RC@YXZ$034)t5y@LPm`;6x&a-V9vMNN?j3TV>7-O|i>T=xdD4&UAJtT~ zrM#a^RwcF8E+_cradQ0IlJ~|>AtT8=Jc)cqt--2s!U^CYJ2M%~9=8>hEsN~iY*S{} z(4%^s#lB=UcQiY{6F=a;4?p>U!0ERlh`#wrOe1V7Sm;{VM%#)SVTXDuY^Uu|JD_%k z9kipk?LL}ef6Zo!+t`Es6e3({oObP}Fv30KO514nehMc%R_Q75wr@6efsVSzPFBOZwjM{0*~vhA2f{cMty!Q7F{w8#z!ugY0O}zdj{h#@Za6v zq=;xtyZ!M}C$VG%{MnKb@M)X7N0(;r;;%pF^|O~oV7hmx|91cL7n$W^akFYtGk_sT zBCyfLo>}mnqsxBp;0iEVEEf3g*UX&z;vLQNkUe*u`;n#7O-C$|#w~c6bR15$)yEk&U*VlZs5h9$xsFUTeL#VMwhkRa&_C(^i}+q=h+;& z62a1#2#fD(y%_u@GTuvLugJI&2&J)~vw3o1(i}X9&}85=APR7ozz=&+LNx(G%YcP*!^A?w MjmZr>h2ieM0eS79XaE2J literal 0 HcmV?d00001 diff --git a/lib/adafruit_register/register_bit.mpy b/lib/adafruit_register/register_bit.mpy new file mode 100644 index 0000000000000000000000000000000000000000..2981ccfec23a72f7745e6250250d3ef82580d35b GIT binary patch literal 659 zcmY*V%Wl&^6dl`nH8qK2r-NxiTS!(F#LWYc*g(@DMQrkDD4;H;eoPVrjU#(P$pXoV zvPl2NO}gPX_yWWafK&;wWE`3lHgnINd+xpGo`VqB-eJnBp|XlnsBPK~0iw?` zdj|dlBHEB(5JRYg2}0ohYLTkzwA-@5rj1o&rNpN?3u^kzHVM8dK!iSG+cB*kog*Mx z=vx#i4tqMj!C)$ zEvH+j%F-ZZf9ceLY3tOn`jB*~fabMtIobop69Vk(-Jxe=bbJ2?-_z4sMK^yX;{Rz+hvSIV>`eFR(D)(7DOpmkF~K zgtz58Ywo>jrJBuVs-xNT_fJm_U3hX>+?s?553mC&et_Ma=gIT-|DAur2}Xmqq$rCSt)w79gIPsK zPzzrtLRvw3s8|O5?no#S3C#ib%6c5vaD3U00Sb=@c~Rd416%42)KO~(ctly&ppNI# zrA!8DfZo#6LPpVa1Snm}7A~g*Aq$Zp0DnhNlq9^SYG6=55vwcPIfjiYm^+vkxi^RVd}#UjY1#z!lhVr?R8SCOeVxaK}CE{+=hH!)gX^e(|ME=3msBkS3W>cNpckN-f#uwPpg!T^#%Y-uSEex z#%wIP79}XVmyD6DJtf&H?$Ypqn0`Jzkqj;d&Hbi%UTa#M(_)62cvSG;9~&Rr+L&0}XZi*_ z7SnApJr8YfjRrkR^Ei(Y=K1b!j6Aj&SK=9c7hgR-6qfo8@ time_2: + time_1 -= 30000 + return time_2 - time_1 + +class Lidar2d: + def __init__(self, tx_pin, rx_pin, baudrate=230400): + self._tx_pin = tx_pin + self._rx_pin = rx_pin + self._baudrate = baudrate + self._uart = busio.UART(self._tx_pin, self._rx_pin, baudrate=baudrate) + self._prev_time = 0 + self._timestamp = 0 + self.has_update = False + self._update_time = [0, 0, 0, 0] + self._latest_data = [] + + def update(self, utc): + data = self._uart.read(2) + header = data[0] + verlen = data[1] + if header != 84 and verlen != 44: + return + data = self._uart.read(45) + speed = int.from_bytes(data[0:2], "little") # deg / s + start_angle = int.from_bytes(data[2:4], "little") # deg + points = [ + (int.from_bytes(data[4:6], "little"), data[6]), + (int.from_bytes(data[7:9], "little"), data[9]), + (int.from_bytes(data[10:12], "little"), data[12]), + (int.from_bytes(data[13:15], "little"), data[15]), + (int.from_bytes(data[16:18], "little"), data[18]), + (int.from_bytes(data[19:21], "little"), data[21]), + (int.from_bytes(data[22:24], "little"), data[24]), + (int.from_bytes(data[25:27], "little"), data[27]), + (int.from_bytes(data[28:30], "little"), data[30]), + (int.from_bytes(data[31:33], "little"), data[33]), + (int.from_bytes(data[34:36], "little"), data[36]), + (int.from_bytes(data[37:39], "little"), data[39]), + ] + end_angle = int.from_bytes(data[40:42], "little") + self._prev_time = self._timestamp + self._timestamp = int.from_bytes(data[42:44], "little") + crc_check = data[44] + if calc_crc_from_buffer(data, 44) != crc_check: + return + self.has_update = True + self._latest_data = [] + self._update_time = utc + step = angle_step(start_angle, end_angle, 11) + for idx, distance in enumerate(points): + angle = int(angle_from_step(start_angle, step, idx) * 100) + self._latest_data.append((angle, distance[0], distance[1])) + + def get_lidar_message(self, utc): + if not self.has_update: + return None + self.has_update = False + message = bytearray(b'\xd4\x53\x2a\x6f') + message += self._update_time[0].to_bytes(1, 'little') + message += self._update_time[1].to_bytes(1, 'little') + message += self._update_time[2].to_bytes(1, 'little') + message += self._update_time[3].to_bytes(4, 'little') + message += (len(self._latest_data)).to_bytes(1, 'little') + for idx in range(len(self._latest_data)): + angle, distance, intensity = self._latest_data[idx] + message += angle.to_bytes(4, 'little') + message += distance.to_bytes(2, 'little') + message += intensity.to_bytes(1, 'little') + message += b'\x00\x00' + return message \ No newline at end of file diff --git a/lib/car_os/network_handler.py b/lib/car_os/network_handler.py new file mode 100644 index 0000000..6938e49 --- /dev/null +++ b/lib/car_os/network_handler.py @@ -0,0 +1,125 @@ +from os import getenv +import ipaddress +import wifi +import socketpool +import time +import random + +random.seed(time.time()) +random.random() + + +class NetworkHandler: + def __init__(self, sensor_class, listen_max_buffer, listen_timeout, listen_port, send_timeout, send_port): + self._sensor_class = sensor_class + print(self._sensor_class) + self._wifi_connected = False + self._listen_socket = None + self._listen_max_buffer = listen_max_buffer + self._listen_timeout = listen_timeout + self._listen_port = listen_port + self._listen_buffer = bytearray(listen_max_buffer) + self._pool = None + self._host_addr = '' + self._listen_port = listen_port + self._send_addr = '' + self._send_port = send_port + self._send_socket = None + self._send_timeout = send_timeout + self._ssid = '' + + def initialize(self): + self._connect_wifi() + while not self._wifi_connected: + time.sleep(15) + self._connect_wifi() + print('WiFi connected -', self._ssid) + self._pool = socketpool.SocketPool(wifi.radio) + self._host_addr = str(wifi.radio.ipv4_address) + print(self._host_addr) + self._setup_listen_socket() + print(f'Send addr: "{self._send_addr}"') + while len(self._send_addr) == 0: + print('Trying to connect to CarOS host', time.time()) + if self._receive_udp(): + break + time.sleep(random.randint(1, 10) / 10) + + def update(self, utc, timer): + if self._receive_udp(): + timer.reset(utc) + timer.set_interval(10) + if not timer.check_timer(utc): + return + if self._send_socket: + self._send_socket.close() + self._send_socket = None + self._send_addr = None + + def send(self, message): + if self._send_addr is None or self._send_socket is None: + return + buffer = bytearray(message) + + ck_a, ck_b = self._calculate_checksum(buffer) + buffer[-2] = ck_a + buffer[-1] = ck_b + try: + self._send_socket.sendto(buffer, (self._send_addr, self._send_port)) + except Exception as e: + print("Error while sending data to: '", self._send_addr, "'", self._send_port) + print(e) + + def _send_peer_request(self): + print("Send peer request") + message = bytearray(b'\xd4\x53\x6E\x77\x00') + message[-1] = self._sensor_class + message += '\x00' + '\x00' + self.send(message) + + def _calculate_checksum(self, packet): + ck_a = 0 + ck_b = 0 + for i in range(2, len(packet) - 2): + ck_a += packet[i] + ck_b += ck_a + return [ck_a % 0x100, ck_b % 0x100] + + def _connect_wifi(self): + self._ssid = getenv("CIRCUITPY_WIFI_SSID") + print(f'Trying to connect to "{self._ssid}"') + password = getenv("CIRCUITPY_WIFI_PASSWORD") + try: + wifi.radio.connect(self._ssid, password) + self._wifi_connected = True + except Exception as e: + print("Could not find WiFi info. Check your settings.toml file!") + self._wifi_connected = False + + def _setup_listen_socket(self): + self._listen_socket = self._pool.socket(self._pool.AF_INET, self._pool.SOCK_DGRAM) + self._listen_socket.settimeout(self._listen_timeout) + self._listen_socket.bind((self._host_addr, self._listen_port)) + + def _setup_send_socket(self): + self._send_socket = self._pool.socket(self._pool.AF_INET, self._pool.SOCK_DGRAM) + self._send_socket.settimeout(self._send_timeout) + + def _receive_udp(self): + send_addr = None + try: + size, addr = self._listen_socket.recvfrom_into(self._listen_buffer) + data = self._listen_buffer[:size].decode('utf-8') + send_addr, time_str = data.split('|') + print('Data received:', data) + except OSError: + return False + if send_addr is None: + return False + if send_addr != self._send_addr: + self._send_addr = send_addr + if self._send_socket: + self._send_socket.close() + self._setup_send_socket() + self._send_peer_request() + return True diff --git a/lib/car_os/pps.py b/lib/car_os/pps.py new file mode 100644 index 0000000..a4f5ed2 --- /dev/null +++ b/lib/car_os/pps.py @@ -0,0 +1,49 @@ +import digitalio +import time + + +class PPS: + def __init__(self, pin): + self.pin = pin + self.gpio = digitalio.DigitalInOut(self.pin) + self.gpio.direction = digitalio.Direction.INPUT + self.previous_value = self.gpio.value + self.current_value = self.gpio.value + self._update_time = time.monotonic_ns() + self.gps_set = False + self._pps_offset = [0, 0, 0, 0] + self._utc = [0, 0, 0] + + def update(self): + current_time = time.monotonic_ns() + self.previous_value = self.current_value + self.current_value = self.gpio.value + if not self.current_value and self.previous_value: + self._update_time = current_time + self._pps_offset = [0, 0, 0, 0] + self._utc[2] = self._utc[2] + 1 + + def update_from_gps(self, gps): + self._utc[0] = gps.time[0] + self._utc[1] = gps.time[1] + self._utc[2] = gps.time[2] + gps.time_updated = False + + def on_gps_fixed(self): + self.gps_set = True + + def update_pps_offset(self): + ns = time.monotonic_ns() - self._update_time + s, ns = divmod(ns, 1_000_000_000) + m, s = divmod(self._pps_offset[2] + s, 60) + h, m = divmod(self._pps_offset[1] + m, 60) + _, h = divmod(self._pps_offset[0] + h, 24) + self._pps_offset = [h, m, s, ns] + + @property + def utc(self): + self.update_pps_offset() + m, s = divmod(self._pps_offset[2] + self._utc[2], 60) + h, m = divmod(self._pps_offset[1] + self._utc[1] + m, 60) + _, h = divmod(self._pps_offset[0] + self._utc[0] + h, 24) + return (h, m, s, self._pps_offset[3]) \ No newline at end of file diff --git a/lib/car_os/timer.py b/lib/car_os/timer.py new file mode 100644 index 0000000..bed7600 --- /dev/null +++ b/lib/car_os/timer.py @@ -0,0 +1,27 @@ + + +class Timer: + def __init__(self, interval_s): + self._interval_s = interval_s + self._last_time = 0 + self._last_utc = (0,0,0) + + def set_interval(self, new_interval_s): + self._interval_s = new_interval_s + + def check_timer(self, utc): + ns = utc[3] * 0.000000001 + timestamp = utc[0] * 3600 + utc[1] * 60 + utc[2] + ns + shifted_timestamp = timestamp + if self._last_utc[0] == 23 and utc[0] == 0: + shifted_timestamp += 24*3600 + if (shifted_timestamp - self._last_time) >= self._interval_s: + self._last_time = timestamp + self._last_utc = (utc[0], utc[1], utc[2]) + return True + return False + + def reset(self, utc): + ns = utc[3] * 0.000000001 + self._last_time = utc[0] * 3600 + utc[1] * 60 + utc[2] + ns + self._last_utc = (utc[0], utc[1], utc[2]) \ No newline at end of file diff --git a/lidar_code.py b/lidar_code.py new file mode 100644 index 0000000..8eabf5a --- /dev/null +++ b/lidar_code.py @@ -0,0 +1,179 @@ +import board +import busio +import digitalio +import time +from data_broadcast import DataBroadcast + + + +class SensorClass: + VEHICLE = 0xf1 + LIDAR_2D = 0x23 + +class PPS: + def __init__(self, pin): + self.pin = pin + self.gpio = digitalio.DigitalInOut(self.pin) + self.gpio.direction = digitalio.Direction.INPUT + self.previous_value = self.gpio.value + self.current_value = self.gpio.value + self._update_time = time.monotonic_ns() + self.gps_set = False + self._utc = [0, 0, 0, 0] + + def update(self): + ns = self.get_ns() + s, ns = divmod(ns + self._utc[3], 1_000_000_000) + m, s = divmod(self._utc[2] + s, 60) + h, m = divmod(self._utc[1] + m, 60) + _, h = divmod(self._utc[0] + h, 24) + self._utc = [h, m, s, ns] + self._update_time = time.monotonic_ns() + + def update_from_gps(self, gps): + self._utc[0] = gps.utc[0] + self._utc[1] = gps.utc[1] + self._utc[2] = gps.utc[2] + gps.time_updated = False + + + def on_gps_fixed(self): + self.gps_set = True + self._update_time = time.monotonic_ns() + + def get_ns(self): + return (time.monotonic_ns() - self._update_time) + + @property + def utc(self): + return self._utc + +class Lidar2d: + CRC_TABLE = [ + 0x00, 0x4d, 0x9a, 0xd7, 0x79, 0x34, 0xe3, + 0xae, 0xf2, 0xbf, 0x68, 0x25, 0x8b, 0xc6, 0x11, 0x5c, 0xa9, 0xe4, 0x33, + 0x7e, 0xd0, 0x9d, 0x4a, 0x07, 0x5b, 0x16, 0xc1, 0x8c, 0x22, 0x6f, 0xb8, + 0xf5, 0x1f, 0x52, 0x85, 0xc8, 0x66, 0x2b, 0xfc, 0xb1, 0xed, 0xa0, 0x77, + 0x3a, 0x94, 0xd9, 0x0e, 0x43, 0xb6, 0xfb, 0x2c, 0x61, 0xcf, 0x82, 0x55, + 0x18, 0x44, 0x09, 0xde, 0x93, 0x3d, 0x70, 0xa7, 0xea, 0x3e, 0x73, 0xa4, + 0xe9, 0x47, 0x0a, 0xdd, 0x90, 0xcc, 0x81, 0x56, 0x1b, 0xb5, 0xf8, 0x2f, + 0x62, 0x97, 0xda, 0x0d, 0x40, 0xee, 0xa3, 0x74, 0x39, 0x65, 0x28, 0xff, + 0xb2, 0x1c, 0x51, 0x86, 0xcb, 0x21, 0x6c, 0xbb, 0xf6, 0x58, 0x15, 0xc2, + 0x8f, 0xd3, 0x9e, 0x49, 0x04, 0xaa, 0xe7, 0x30, 0x7d, 0x88, 0xc5, 0x12, + 0x5f, 0xf1, 0xbc, 0x6b, 0x26, 0x7a, 0x37, 0xe0, 0xad, 0x03, 0x4e, 0x99, + 0xd4, 0x7c, 0x31, 0xe6, 0xab, 0x05, 0x48, 0x9f, 0xd2, 0x8e, 0xc3, 0x14, + 0x59, 0xf7, 0xba, 0x6d, 0x20, 0xd5, 0x98, 0x4f, 0x02, 0xac, 0xe1, 0x36, + 0x7b, 0x27, 0x6a, 0xbd, 0xf0, 0x5e, 0x13, 0xc4, 0x89, 0x63, 0x2e, 0xf9, + 0xb4, 0x1a, 0x57, 0x80, 0xcd, 0x91, 0xdc, 0x0b, 0x46, 0xe8, 0xa5, 0x72, + 0x3f, 0xca, 0x87, 0x50, 0x1d, 0xb3, 0xfe, 0x29, 0x64, 0x38, 0x75, 0xa2, + 0xef, 0x41, 0x0c, 0xdb, 0x96, 0x42, 0x0f, 0xd8, 0x95, 0x3b, 0x76, 0xa1, + 0xec, 0xb0, 0xfd, 0x2a, 0x67, 0xc9, 0x84, 0x53, 0x1e, 0xeb, 0xa6, 0x71, + 0x3c, 0x92, 0xdf, 0x08, 0x45, 0x19, 0x54, 0x83, 0xce, 0x60, 0x2d, 0xfa, + 0xb7, 0x5d, 0x10, 0xc7, 0x8a, 0x24, 0x69, 0xbe, 0xf3, 0xaf, 0xe2, 0x35, + 0x78, 0xd6, 0x9b, 0x4c, 0x01, 0xf4, 0xb9, 0x6e, 0x23, 0x8d, 0xc0, 0x17, + 0x5a, 0x06, 0x4b, 0x9c, 0xd1, 0x7f, 0x32, 0xe5, 0xa8 + ] + + def __init__(self, tx_pin, rx_pin, baudrate=230400): + self._tx_pin = tx_pin + self._rx_pin = rx_pin + self._baudrate = baudrate + self._uart = busio.UART(self._tx_pin, self._rx_pin, baudrate=baudrate) + self._prev_time = 0 + self._timestamp = 0 + self.has_update = False + self._latest_data = [] + + def _angle_step(self, start_angle, end_angle, length): + if start_angle <= end_angle: + return (end_angle - start_angle) / length + return (36000 + end_angle - start_angle) / length + + def _angle_from_step(self, start_angle, step, index): + return (start_angle + (step * index)) % 36000 + + def _calc_crc_from_buffer(self, buffer, buffer_size): + crc = 0xD8 + for i in range(0, buffer_size): + crc = self.CRC_TABLE[(crc ^ buffer[i]) & 0xff] + return crc + + def _time_diff(self, time_1, time_2): + if time_1 > time_2: + time_1 -= 30000 + return time_2 - time_1 + + def update(self, utc): + data = self._uart.read(2) + header = data[0] + verlen = data[1] + if header != 84 and verlen != 44: + return + data = self._uart.read(45) + speed = int.from_bytes(data[0:2], "little") # deg / s + start_angle = int.from_bytes(data[2:4], "little") # deg + points = [ + (int.from_bytes(data[4:6], "little"), data[6]), + (int.from_bytes(data[7:9], "little"), data[9]), + (int.from_bytes(data[10:12], "little"), data[12]), + (int.from_bytes(data[13:15], "little"), data[15]), + (int.from_bytes(data[16:18], "little"), data[18]), + (int.from_bytes(data[19:21], "little"), data[21]), + (int.from_bytes(data[22:24], "little"), data[24]), + (int.from_bytes(data[25:27], "little"), data[27]), + (int.from_bytes(data[28:30], "little"), data[30]), + (int.from_bytes(data[31:33], "little"), data[33]), + (int.from_bytes(data[34:36], "little"), data[36]), + (int.from_bytes(data[37:39], "little"), data[39]), + ] + end_angle = int.from_bytes(data[40:42], "little") + self._prev_time = self._timestamp + self._timestamp = int.from_bytes(data[42:44], "little") + crc_check = data[44] + if self._calc_crc_from_buffer(data, 44) != crc_check: + return + self.has_update = True + self._latest_data = [utc[:]] + step = self._angle_step(start_angle, end_angle, 11) + for idx, distance in enumerate(points): + angle = int(self._angle_from_step(start_angle, step, idx) * 100) + self._latest_data.append((angle, distance[0], distance[1])) + + def get_lidar_message(self, utc): + self.has_update = False + message = bytearray(b'\xd4\x53\x2a\x6f\x00\x00\x00') + message[4] = utc[0] + message[5] = utc[1] + message[6] = utc[2] + message += utc[3].to_bytes(4, 'little') + message += self._latest_data[0][0].to_bytes(1, 'little') + message += self._latest_data[0][1].to_bytes(1, 'little') + message += self._latest_data[0][2].to_bytes(1, 'little') + message += self._latest_data[0][3].to_bytes(4, 'little') + message += (len(self._latest_data)-1).to_bytes(1, 'little') + for idx in range(len(self._latest_data)-1): + angle, distance, intensity = self._latest_data[idx+1] + message += angle.to_bytes(4, 'little') + message += distance.to_bytes(2, 'little') + message += intensity.to_bytes(1, 'little') + message += b'\x00\x00' + return message + + +if __name__ == "__main__": + + network = DataBroadcast(SensorClass.LIDAR_2D, 256, 0.001, 5000, 0.1, 5001) + network.initialize() + network_timer = Timer(0.5) + + pps = PPS(board.GP22) + lidar_2d = Lidar2d(board.GP0, board.GP1, baudrate=230400) + while True: + pps.update() + + if network_timer.check_timer(pps.utc): + network.update(pps.utc, network_timer) + + lidar_2d.update(pps.utc) + if lidar_2d.has_update: + print(lidar_2d.get_lidar_message(pps.utc)) \ No newline at end of file