From 6edc570a2617e9cdac0e97d92d4c1394868a4c5d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mario=20H=C3=BCttel?= Date: Wed, 15 Jul 2020 00:28:23 +0200 Subject: [PATCH] Further development of Inertialsensor demonstration --- CMakeLists.txt | 4 +- include/opengl-playground/imu-serial.hpp | 9 +- include/opengl-playground/openglgraphics.hpp | 2 +- models/bmi160/bmi160.mtl | 13 ++ models/bmi160/bmi160.obj | 40 ++++++ models/bmi160/texture.png | Bin 0 -> 35726 bytes models/bmi160/uv.png | Bin 0 -> 22216 bytes src/imu-serial.cpp | 142 +++++++++++++++---- src/main.cpp | 38 ++++- 9 files changed, 215 insertions(+), 33 deletions(-) create mode 100644 models/bmi160/bmi160.mtl create mode 100644 models/bmi160/bmi160.obj create mode 100644 models/bmi160/texture.png create mode 100644 models/bmi160/uv.png diff --git a/CMakeLists.txt b/CMakeLists.txt index 95e3155..77db17e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -32,7 +32,9 @@ configure_file("shaders/mesh-fragment.glsl" "shaders/mesh-fragment.glsl" COPYONL configure_file("models/coordinate-system/coordsys.obj" "coordinate-system/coordsys.obj" COPYONLY) configure_file("models/coordinate-system/coordsys.mtl" "coordinate-system/coordsys.mtl" COPYONLY) - +configure_file("models/bmi160/bmi160.obj" "bmi160/bmi160.obj" COPYONLY) +configure_file("models/bmi160/bmi160.mtl" "bmi160/bmi160.mtl" COPYONLY) +configure_file("models/bmi160/texture.png" "bmi160/texture.png" COPYONLY) set(IMGUI_SOURCES "${CMAKE_CURRENT_SOURCE_DIR}/imgui-submodule/imgui/imgui.cpp" "${CMAKE_CURRENT_SOURCE_DIR}/imgui-submodule/imgui/imgui_draw.cpp" diff --git a/include/opengl-playground/imu-serial.hpp b/include/opengl-playground/imu-serial.hpp index c574be3..f88a13a 100644 --- a/include/opengl-playground/imu-serial.hpp +++ b/include/opengl-playground/imu-serial.hpp @@ -16,19 +16,26 @@ class ImuSerial void stop(); glm::quat get_quaternion(); + glm::vec3 get_accel(); + glm::vec3 get_gyro(); const glm::mat4 get_rot_matrix(); private: std::string serial_device; unsigned int baud; std::mutex quat_mutex; + std::mutex acc_mutex; + std::mutex gyr_mutex; glm::quat quaternion; + glm::vec3 acc; + glm::vec3 gyr; std::thread worker; int stop_flag; int fd; friend void working_thread(ImuSerial *self); friend void update_quaternion(ImuSerial *self, const glm::quat &quat); - + friend void update_acc_value(ImuSerial *self, const glm::vec3 &acc); + friend void update_gyr_value(ImuSerial *self, const glm::vec3 &gyr); }; diff --git a/include/opengl-playground/openglgraphics.hpp b/include/opengl-playground/openglgraphics.hpp index 9880854..d0bbc30 100644 --- a/include/opengl-playground/openglgraphics.hpp +++ b/include/opengl-playground/openglgraphics.hpp @@ -4,7 +4,7 @@ #include #include #include -//#include +#include #include class OpenGlGraphics diff --git a/models/bmi160/bmi160.mtl b/models/bmi160/bmi160.mtl new file mode 100644 index 0000000..a5a485a --- /dev/null +++ b/models/bmi160/bmi160.mtl @@ -0,0 +1,13 @@ +# Blender MTL File: 'bmi160.blend1' +# Material Count: 1 + +newmtl Material +Ns 323.999994 +Ka 1.000000 1.000000 1.000000 +Kd 0.800000 0.800000 0.800000 +Ks 0.500000 0.500000 0.500000 +Ke 0.000000 0.000000 0.000000 +Ni 1.450000 +d 1.000000 +illum 2 +map_Kd texture.png diff --git a/models/bmi160/bmi160.obj b/models/bmi160/bmi160.obj new file mode 100644 index 0000000..b9e73d0 --- /dev/null +++ b/models/bmi160/bmi160.obj @@ -0,0 +1,40 @@ +# Blender v2.83.2 OBJ File: 'bmi160.blend1' +# www.blender.org +mtllib bmi160.mtl +o Cube +v 1.250000 0.400000 -1.500000 +v 1.250000 -0.400000 -1.500000 +v 1.250000 0.400000 1.500000 +v 1.250000 -0.400000 1.500000 +v -1.250000 0.400000 -1.500000 +v -1.250000 -0.400000 -1.500000 +v -1.250000 0.400000 1.500000 +v -1.250000 -0.400000 1.500000 +vt 0.575745 0.121275 +vt 0.575745 0.500000 +vt 0.121275 0.500000 +vt 0.121275 0.121275 +vt 0.121275 0.999916 +vt 0.000084 0.999917 +vt 0.000083 0.621192 +vt 0.121275 0.621192 +vt 0.575745 0.621192 +vt 0.575745 0.999917 +vt 0.575745 0.000083 +vt 0.121275 0.000083 +vt 0.696937 0.621192 +vt 0.696937 0.999917 +vn 0.0000 1.0000 0.0000 +vn 0.0000 0.0000 1.0000 +vn -1.0000 0.0000 0.0000 +vn 0.0000 -1.0000 0.0000 +vn 1.0000 0.0000 0.0000 +vn 0.0000 0.0000 -1.0000 +usemtl Material +s off +f 1/1/1 5/2/1 7/3/1 3/4/1 +f 4/5/2 3/6/2 7/7/2 8/8/2 +f 8/8/3 7/3/3 5/2/3 6/9/3 +f 6/9/4 2/10/4 4/5/4 8/8/4 +f 2/11/5 1/1/5 3/4/5 4/12/5 +f 6/9/6 5/13/6 1/14/6 2/10/6 diff --git a/models/bmi160/texture.png b/models/bmi160/texture.png new file mode 100644 index 0000000000000000000000000000000000000000..4095444081d67d9ec47940d189f47b4baea58008 GIT binary patch literal 35726 zcmd42bzD?m*FJnGQ5r$Iqy+@&4yBQh21!9mx;qS{K|s0`q`R9zX^D~U?rw&hdC&O0 z@8`Ln-}`$$@4xT(nZr4A_St*wwbs7Ywbnj{y;7CO#iGCh0037}K}G`r(7{u5;Ql@E zPark>M*tx7^U>0C(|F@a=j7sOX=7(W=jQEXL1*D*V+jCWGo`6EE^7^ha)?!83Di1e zx|omrzXXqt@8Z}B1>M!y_sY~OWi;r%^w)o5PDk9{d=5M=uhWxGm>Ab$Kon{Bf5aDW zyQ|(5@49Y*Ad1VYCDZX~NqO?IdQLpPN7b8Cq$fEB9>F8|0>x;rca;vx-!Ej{QdC^V z^U_|x;@A8FM6-5m=|mlA$9!%Aig+Q^le^tkY~@)ancpSv9WQpkSl%%x88j5H`6;o* z!VHahSsf)mW_A0rbse$?Lt8>NUjz<*3c9UmaYTVjNJ(N!wql6-2P|(cZ61r51cc|d zS*b`uzO0=tNj`=j{9HGeyXwK7*0{D1F3@9b`jf3G&&D>ub-sv=34YebpFt| zOK@=L`pub-SlR6&uk&8-{>?Alf2qzzujKZHC|p=f2>p)*ct9^ zQEr&GoO0@UK>v8V?Sxq^XwigLg%a}8?(xTyPmR^>gqyPjnj`fyY-*XilwG7ky(Y(ADqu77dUtSUD|eF9 z$alBZfHtu-=)Kvy!KRd`*p8*=A1m*{@P`J(`b#wPh6HYEgh|$`cj9u3OWW+CGCZf- zJ16En7VCNxmxGA&X@q@dwtI#e#%7+@8#nsQ?oS?E_Rg>uLt5It)Smr*9l)~=tBomf z7O1(9kZ}+DW+Qys2 zM#yi2FK}J&e#FPej+bl?O}ksv5w?` zM}k)HA1~VL&C4t9+k;=bdIOE|@I6eYutF*{E8gng`W%j;-gN;rU*P>ByQObG>+dJ* zYH+?)ic(Va+>w|#oQl(nRAF~7wA=gVF{=KA2y1+V zqF+B9Evt1Yv}-=!N;dGx+TL-s=$51CcMxdG@Q*P3mC!WTmW85qK%F2m4IQlw9_b;g z(kESy-6kch>TLSS6vNG3^_vJ9Z9b>BS!g?0Qh_R)__<5J;CYqDE7XYEE;CG1yUunt zS00HYr$Bg=;iOxAExDo>wX%k#fo$Px$j7`~zKI4$VVbjBTG!iM?kC@cZL|Ko6e;a% zici3~ZZfj3f4bqu>CWjQ$E?^)lhanm{hmQ8MZO8BL| zuZ`lJMi!6r^h4RNFtM)f8I&zw?fhWm7~Ny~d6Igg6l`2ZQ8s>s-C5NWFml$wbr4BvTK*jQqOrxl! zw7Y1|&)Whox2-9bTU$v_XG6q# zikJA#d(6HzB=V8|d0DUa?xh?+qh*qjo}Zz?sWfooRpmxuT7$-9O_6i(WH|P|^D}Cm z=R@<~625K+@V*M?Qz#>-1mfzxe|E2 zJhicrEkp6XFDWjn%dRM5PmY;^_cS*nsxj>ON8)FHugywp?nGwvWCZzTd3&NUa_6W@v4WV|hSB_EAb*pw3h4&**d?>fKUq-7cF-WBLk> zwVPwNFI}%1!Rn~_5x!G+Hfa>(;Tr#%9KQ6EElT>YWnsoyUmWgh3D{3q8xVZN%eDM| zY5Yrlx(0!c%_*h!?Bd5VN<8|`sQM9r^5I3FmxH~l;{uKxe|j!15EAc#U09xG7_!FJ ztu2F#i|%m~DMs~(K^j-}Y1eix*-x_OF8(yCoky#lO{3~G_k%sS-r6q&a3nXkX0g85 zd!YHM^Zv{CvUlpXE}6WEHx{w`kz+nsckTSrH4b_Z<`v#YZ(P1VZjZozM(_)DCGXu2 z%@=}E(N#eD_3yl1VHA~LF1`6g)U8Y1lj*O-7N%U;&ELDBq39*2Dc;*pEDz|axdul> zA;q29ma=wIXFbmwfs$D%) z{6Yys`4^hcv1r2MK1+$I$6Dvrh6)X#_KMOEhu|<0bXCcxMQ?}76MWHYaVkVKVlBoH zDABRW{Jd);rOZTKGWN8sOAm#Y>?_5%b9AhtohOw`m&fIE`q1anH8=8F30LqE&(#hC2f8 zjpJH&z}%_D1gInMUh%`<8F8Q%pB=C9VL1k|0m4}JBR({bJDh!J7-GSD?L8BrB; z`E4j<=&WW+`FxxSF-9$Edf~|uq$Wb_9I1wa5qFplL{joH7I6H*Jm}hFdNw4fV!r%g z6SbO9^FyKmfq{IXaw z_v0D#C9L`PRA>*#Q0#YYKIyYE@{!NsYc~phAYApR=T*{4m8BjRc!6;j$|9nw(X0B6 z+VLkr+6Ona=DMVx`Y5u)m6!!O-jzE)P9Oz{9O?ZSmd=by^Ui=sinCVd+Gu>i50^^A zC$uVD#zxVaQdui$9g2Mg3~Li~BlnNxl;c);y91}UxA;)&bx}*+sOU=ZG4A)5e&tkJ zaC{nipQEgO!TZItKM&go4$gfi-C6K?6ZY5qFt+i#EYgX7*=nrT=AR}}-<1cm!E!Q~8ePHla|4etkGAoqTPRDFZW>!@&^40xJRtJu- zS**p)hPL1NbQDK(Ll-V=v}Cy?F$;bFAu z_ZpF5hlvV^mV^$P0k%T6HR^qZ2m7*D&5EjH+~euAz8lS-F>tt;qDNQ~tyUMWFZOa4 zzBE|B9P4U-fhpZcqy*?#2{~}Zn z=1DkdlbUW(V`XffJhA61Aa6!59cf^tf8IyrJ=0O@GCiE}O3i8KL7A|WPrB9Qfx{?- z2#|kB)h^v%7UmN875$43&5g+cy>)2V0%f5SpZ2p^!3|XtPEndif+lenni1Et=9l@D zIB%jCFz8IVpJl8kyRv78!d>b0r)}v8%94!?`Fuzpc7EkMa@Kr;|1JGB{W8jTfG*N` zg-@f1d@c6y0(iyld5Z)-5D=UbQAKeMoriqz?;#V|)LQN{^92aKlDW)yn zUj7kRqu*(0^zLgByH*ab9BS((?|5HE8${YhXmKdqKHN7kNZQB=g)>fvVSByZ-z)y- zk75fp;o+sWyW*agB=f#lQ5mK55BVJz$HwQ-n4nsNg5Ac#Sy#J#hc&&lj%3)k5qvBu zA7Wz}*OFhCeol7_<7_I3!`3>In?Zc2_3EqMYZa_2pN>PSq zx198sHi=^~15^)!PmpMypy77)H&03NZP21$M4e)KS;^G4IVNvF`3x?t)M1`IwD+eI77BbB4Uy(*{a{{ct|s-IiWPJ7 zQV%`3=4w(B?jAerxFitz?vinFAiuR6hery(r6PVOn5=MV*MpGN*BDvr#GD#(Bj_PhxCg&%_rGAKPL{1)rdyJBzff=PDMYeqaw2n85xSpe44F+Wq3* zS<)-m(~xpvuS+wQTT3g)n4s+s6W%S|=q^jNt%6*(GKU$j``t6@GUxw%PJUjeK~Pi8 z07rdAlh3rXc=^cDrbaRdZ}pMLC)rX$PuB~*)nCh`80CEdhr z{*+cxJ+P&8{NA8pPktUzLKo;t+TQfy@aK>f#_a|I=o#BqH5JOo1B$uQ&Q&(5seCVnYiR~6-{fJDI1#+jMob&T3sG5MYdedRm=KoZ^WNZlrhZM9MGU;GsyCm`Bp!a5M1+Jwe$W~QA!J*K2v7Z~ zp8>=K`ID&fQpzn=W^9wQ&3J2u#%(a)&u5Qr)2idrLc%ddeJF_&>Kh~wG zmf$s9BmH7PE$Z}GXN>Z?*Ixg$T;ZF=JqL_OKb0yezKog^6hhY!H$j=}DWA?TJB!Ph z9DnjZzVP-)zW?~pZIQeEn$O!qTF#>wbuZ2FiBXU6nUG(mzb);voMwWesO-nKp(!Nk zRv9G83CSYN0u;FX0k>*?XsH{&et)2)Tczj02vscJuaC;Qdi~C{R45!)OD`DFOd$Ub z0e5QH(;R5(LQjsj5z2b+^VqTkIL6sm{qRr#{ZJ$-TauX5aBtD{8ZnA8Q&+2)>iQb} zV9ZDii2C-kr`_23g`QY+!(c^My~uF}pHQUFjwVIhRZLos{~wwJZLH}If~$$Ir1Wm1 zWWYesJ#A|otcRLT4zh)l7@Z z^Q9FCn{}!3z0R&s06+=0k(Pd?C@uZ(It*BSN%xNxSLl$U>N3%+rGJX2L*U%|in{Yd zL1>Q7_xq2OO8*pn?8`Oz^W(9ivoocWETr7G%8> zVU3dV3VkU!M`4}S%7Pp#xGUQ#FE?A%s4Qg_A4{;%@NRwRj4@0y3RpFC(pyS-!;q26 zf=3h)ERZ;x{0KL}D_khMfMOk+Jn7MpLHf8$0{>y33E6-Rd%t16@FyXX%1?|ky#?8y zcsOKi#ud0@v+t5a0{#VJ@{9VIPQt_TK25 zdiDOH`K&`a0JmCFvb|z=H{FOod;Kzk3Z-?nz03V&JS?c6+H^@V38ydimM_?k#!E8C z@i(e}y%aQc*1(lEA73i9Nofwi3~5(*Uo|PdFh9RA3$aSQy8{$_ToUC%E7HOG*gdnC z@-hGu@?Y@MOL_2@2Tlt5t^j~dg#4lasp(|kkC<+XDzcbsn7EHfvEWBHI{-ijD9Sw7 z@|xM3_jRN4^cRFfOE{b3bcz!!^sA_ZD?i?U{8Sh1!58M>hu_}2Jox(X0QTZL6)VAS zCIZwaUj@XUeoZ8Kp+pz_;=xfnR}BHD8rGAj<@cLUf(ee8!X2yLmL3$(4jslYM%yda z*}bgeET0q>k-y29P8qlAUPyrr_(2qPb#*B~(b~fk)ltE}1mG6{kY|7t@&MfbdqhDV z>HeM~%Ys1A|9SfV{oLO!Dqd)_gD3LW3k$840oGd;b*)!^w-eXr;b;}gjc8!U9d18t zYVh%)){d0qVbOfrbU_yH^cPU*V($TZV!C+v@;v=v0daZ9naj&wD&{%#C>+xL2Y5gj zAJD$yqZNoqAFZRZpa4*VFx*RID1aa$&o{XbqgGd4RUN5lW3bsD1)~xG91?%qdv~+31;m*ohk=KuyBpvUph1_`e=RPMWn z49`&R10<>7+)R)8WMyW1dm}HP)GpuG4Bs zO(JE*g~FHD#pC1LKE1o4p^Pf4t<~t5poLXKk-a%*NcIQEi-vcR+ao*^<+*5Av$4T` zzW>t?16zW1>iTeB%#h3bb?K0~_ug4~F2XeKojIxh)x&X%MdDC=NY(VEgXPAX6=)pe z_3n;k4MY)h0Gv=f6LLa^n}a-lClmUdC3BlM)ykq!g8#lJ2F*wfZ|dvom>FRVD?Ygr zUObKd{8=G>t)dS6)$3sF>jzX&+VsD3Z+V-z`U9zX|IP>S@4pEvtrCNla-$o;nl+0O zJuXnuAb^iEVBNgVN@{R$dal1Q&DUQA`u9M@-^XwJF1@s9hjmBX;yM$m#WcKs;9FI5 zyLU3Aj3+38HyeI|Lq%u(Z7b%VI2_kxwQcRqIcu2$+mu?00m;Y8nG5?Z;mf&;X81SWKi0vkJZ~WqDcjuI;;x)<`Zb-JU7JQ>zia zGIDZtQ?TjnKmHe7ZAjL*DimFNa9XXua4JM<1kUF4uinHqb8Yxyq6V!IyQ>YeClZ0$ ztOM4zG?Wpi(L#x6Qou9ze_VPN>2=@j1RMsb5O0?EQY2cILXoD4mt(>3#1vQnkT-Nv~E5||NJ_$->yH^J_9H@0i=+( zyvoR_qXr12AIIqG_MG$cW7kDfzI$iZ(sX@W-*(rEuGt%y+(XTt?E@SaQKWiRT2y?0g&AAeW-<6ZKB^=y}i)FNmV6-8<= zOreCZvko6$tFq>{1!Djxv5q<15@OnI$UyQmw#~y!D2`zQ z7lhN|=Og3e32Yz+0ws3jOh|zJ3z>AtGf+P86NEefLi|Mu;JNwq%)EGx{Y$I-Uw=Vb zPZ4XqiXOQZB|&+?PCopdJ_xq?L{i}Oe;~A}{xdA# z?3QQ^4@mqP%@=jCyiC4t&%iJon}-FCP6wDWHMoG%>Z_i`^F_f19^!zVg1?gg2kIgt zM`9Q@dxA9;ZI-Lg>ZO;VULh+nQu3hnJzO+2Y#w~@DX>w6>&sk>6in~}1$a63zv3)7 zTh(!;b`7U14&4ji-CgsRBDNtiDzTEgt6Ys+P21vZl=dezgZeMx1mGi3>dertO6}8R2aZl zW3RIAB37gkHp}b6fpi^xLMzpW+w0p;t8wlGzYcbLS_ywg=*P(*T60jTc2uf4kdgQi z1yCWDq02UCJuA{*3Wr_6IYuoFklTTv{X#cYRb=NMXI#t5{7`(TOP#9;@#N!^No5SF z9amB9cFoTt2F)%7v}>Hj>6gvHCMJ6h7*vn&wWCADtI^T9+)2THK(ENPIc;wT!(_C} z4Z^=oqF*e{QlI4S5USOZ%gq6!IR7$ zZ=~mgQ``15PL(^Inu@%DNJMWU4o`K1e?``9RDy$rHe-Wr2mq(>R(mHWx!G3&l=YU; zfr2gK@nP@h2J+$M$rjXL3mGkTl>Fb)O1UX4Cxe45EiJSHf;~*W7gTN0aCj(DLk_qA zno3Jck-i&b-t6WhLcz?8XZtdqaUltsb;Pqc`-ebHDNX^eaUT-k!jmZ=PHj?Lq~QkW zNhM`TWr>4uK$0%wv+X;M)G{(!W-zj}3@k8)Wq$r_Eo!FsQ#V2BxHgeQ7F>#YHYh=0 z+uhXl3xs24=RN{X?R(r7Af5QqLf51+5+R zKQN4g;Q%llwkM-V9rjp7m8j#Mt+F&=0i0H_)=wg7fyAVQ;RoMjeEOO!-S~Jy&PHen zx&N^n$CQXeL19?EwywO#yB$*lW|dW_ocyZV?yfzETR{mA0flc#WgxEKAvPoa*sMY$ zy1eup4+q?lCIWO+f%Xw?#W=Eob-XrJh7|_v%UguM#K;)&BuF-0I-5Xe0quw-)|RvS zwPbTbnVDw25(Kdz;cYPZ{10IM4}_IMcKsKYfgtZ<9n{38()oGbnL0NQHqd#3K&gJb z;$#Ok8U!{A>Z+@))g}Z$X@L^A|3E76x&jLo$!!6ED&q|@9+*D*g9Oc-(G;A_a{x({fuNkx2Xx@||BJjQ-s=FW*6i)=wUL9N0POSYUD@>DX*y#J zc>F8Qze7d;2MIv$q|U<=YCStLUHA;VjSA?*y=W{!c8l}|lC-~rgHoe@UdI4A@dJ?g zf`-Qc{Rk;e^w6^)@J|KS77|a)c{q?T4FqYoSG0h9km)%XoxvH<0Z5(%v=9F;3Vr$U z4WNHgS69dOS6@L12PbCggy55^aq{31kU}2*rE81lv;c>o%iwNi7N$NP=INB!(<|Ajm=bbWhZT`Nu z-H#c-9ymO?b`GK)&(CD!Q!^9KA@BwB63oV4)QTpwe^Q@Wp=Sh(Gj&fFL?i-!PfQaN znjG#&NqR(L&;g-H^`^6dw6phJpZ4i5dmH;!epOfRUdB=`7upZmPCiZuE`HyH#$%WQ)fVm_E$?tlRlGgT7 z?_~`q)4F?B6Pi7)G+uA~W2=FVE30%uT56hTmi>9kzE)48301Wo+QT_=wb#|mnUf&iV z9u&X({gu%#^1{+6e1|ClUB^X-4uAtCe~&Qe6e`33_rJRa6AjZ@|FARN>udUYsolnG z!)QnQ*^MTy zb(vz-y;)prPY^Ut0?5q8D?l7Z^H|sMB{~ zbVkA6zQuQx*%w?cX01h)+zSmhnm88KA+?e}>IDOu%;m)bY3>*@GV+cy{BKr*PsO?^ z%1Wcy1?7WhDHMp*eo=DdNEgHYl~Vk9(siS26AFLk2Ykg~@g87iqG@e;<#I zo0}ik+RvJGEG@N2^w{rCbTJHx@jAFfVpK3@)FCoV#;3^C3d>{g_hueNg+yWFFtZpi12+P$B40t?k|r2Sfpe&0jU z`xRGKR_0bo63|l~z+cXlzl({v`XMv(W9#_B=7sz2MvMR6PGzOn(80Vqe~iH}m@p#j z?4yzfs!B{2yf=R?(oT(A#OQs%o0qG51f;i6zRaDf3PeP%<|6d!IWNFWfY;+Ny69{w zBoxeZN+}gM)QXtXRscS`frK{rZOI9)YV;fbjx+2=~*VubsT}Pe%#^* zuaFT@9Bpt~Z1d(qLW`4KOrh3FWM!o{H1Jy)=fcb|VZ9|!I3`#{Aj<;tMQE0LA~H{k zy&XJqGOY4+e7oUnWx=GllD5Bp{MpZV859m%4eY4mh_g&rK z+RB&(L5RdA_nWTr0QVy+PB1C#nK(GO+;8>WbUz9F)+XVwwHJB3b5s6^$%X<_ZKZa5 zbkMx|%NwTyCkQZArKP0^k8zy!Fs#p!oy$@PwDX#pKnHXde;TIJ+@=hkfBfjYf3dI5 zniSJmq7>ITNKNew|BUPG&Pf`xSo7q-&OyR16C_fXJ@&IJj#R#2#RCtS1q+FDrlp1> zAtWYa^C_Yg7xy@BFl{vo7Z-E(9{tKlt?jC#phIMzw4CnmA2wnZ(9Fx92lL0QJ;tD! zZrZ4js>q*+0#k?Si~ZQFz0Ow52lRDCfklJ!;@-WIe z`v?dbx$uri0DRYk_7=r7_#WUI3j!)f3#qbdx0=UZ=+he*)E4d!;*jI%hFAaN9z9o@ z7$!xr##(-f>1lG!5OjfJ6;rCBkKu7&=wAQ0KoUUX5lj|7a zMX(q_DrgsqJy{-;MYBopoA&qmN@1FyJeYjw-83yPU(VIpAHN?~r$jZa#RQObrcp;v zb91rNz3Nr!^K+J#$PC4YYIe2}P&be6v&Y~_HWbkLTK2YPPseoEbZK1Fic93elD%2H zO7dLro;yrYjcX{(S^Wh0I;1?>P?pakf{AIx0ecB*WQZZrcgxDpR5wZ7>_Jnd7D&vl z2tECP_ILb?tEJn@%o3Bsgt||Uj`}u#*xYus`GXZb>!EYu1+Y3YP|;k;Z9Fp;j zSbp={MK}{;I~mD-BVX1*`VgoXK?^!~@z}V{!D!Dg$h@S{kXM8PI@2mz6y0zV==q(% zipu3p_c2NPcDz0ot(i!C7&H^a^SOmKI?y!<5Bt|wQqlx_GP}DwW<)=KUg@8$>+~5Fxr)bm_*WyvMOrHu;8IvzsE5*k>_!1Bid|-aj+Sb8@U`?bKSf!r zEQg^DT&xMQcvOI%+~4qTx~!t3-PHs7-^Knb6;nGB6z1i2Ks#Sj||N+j_zx>z-z0V4(r>m)aohZpdZwNopU5(OU92 z|NM-5JiNTz@D@r#iD~*kDAyBE_#sRf?Bd;x55~;HGNWcjRXG0nwco;L6g_*C_HwZ7 zn5~kw!c{#z3YT9@Qb62XpI?9=g~SVF!Q?ORLSCz8y#s3vdTn)&B3{CI@iKpXzmI|> zvLy;?HumC7yTU2z-jNGv|Kg$TzfFm0z z5C5t<mAiLa?ePiz?$kXwGZbO^P!Q3o?NDF9#}29xVK#0%t; zV5>|2(t6m^p_)N6NUV_EmdN1m9YPSn$or;>yvUbTLGB6W4YI#wJEKA*5cohkan1?& ztZAP&$e>!@OqTVksyw*|aD0g;Prw8v{}8K?6>!Lzw7O$>P!sUkQ0DffiHTTmLLew{ z0y(HwH3+G@XOFCk%JIs?1eh-6``wjiC5KKn?0~HQ8JOx8&P#+zWRpl-q z+d1j$yBgzp;C2EwaU#A3Oy77GC@~qkPZ}M6GUJAStL48(;%wwdK^vocF!Kn^k+ATA z2)q9_IUQhHEBs2A)V8}DuT7#^EBp{2w`(!WS-NFr~%zL<~r$4|dT< zCJXslGS_FK2qZn~=s5n->dT-w8X8)Vkr^$y(8!xvW2qump36kCQn4KFB5Yh`tE;8U z7LErBNTnk~3;gw+{Ds}qE3f%H6?G9az93h8s?gbb$CxBF)y$(RsPgEgj+$CacC#-S z4j5`{j}KJR?2xsb1Y)4VZB9e`QOv4(fo@7c`|$5}7Zn#FXBXXSAf4KT=Jdd; zNBP>S9Zg!CGIj4lY;9M=K{v)wQ4s(i3<83JfAQ*uHzlj_v}g`9+iecX@P z*$$&kQaWD1rCce7Wj16b*nsB+kY2x zxt`%UWbF%rWN9<{ydGOu;@u5CGC_x%&o@n(7*Ru~KvWwsKaBfNo&oe200SRI0ij%w z$F6#Vdm!8Mp7M8K`GwJjg7jck@X+F`OQV;Syu9D2Hqte{M=^o?IgnSbzOl-9B&pwg z(Xf!jtmxO=Do!&$y$y0yU`ePEEogLwJ3%R(TR`CO3V&?8%K!{+6OjYpLXiU6tBwRa zI-L6!T4Pgss^JNYVUBh6J(MTj!C;O8mQkn-4GTL%KvuM|+HfNI+JMretXNYF96Nq3 z;GJq%4CsJDLlckN{^EiEH+JD8{9Z0_jjb`GOW&-b?DfIH9DUBWIdF3tT>f%Yfv-jxOS z!9YK%M&DXvD8T>daITj9gox-g6;l8G@d60&l45JW;Zib&O&<0wm0)~owbdO{R-?tC z&a46%%r~&n)B;U?GbLzXQ)6VD)FKxmgtVLveS}C>6tmOQJ8&eJ^~3K@eF8xaX1hDt zV{p*ABbiidLy@(=AElBz;tWA-Lg42=7op-ec9bB_>eU)bSUhhrb>Ru9GVf_ogTvNO zk6Q-QV!Tgi35U}dkJlz9F5g;NDO0<2Nyg_L`!;*_1nx}}fv*l(`a5dpx*UOiYs&A6 z39#(_5d}7E!IDc}bhMYe|J{SqQ!t0v_3_Deb#m%(SYm(J1P|;EnsR}TS zyqs}kCd%5~3N7B*A+bwA#@|yg{z~rsCGU83o;bK&N%C?ee*U29z|>v}wbjj?6zllJ zeb>w?j^ncGP$90NpjEu=3Gs?PNK(gJb>YR}Uf0ig5Nd-zAmfn}R}4_Wn#YsU&{oZx zIWV+zBve?I9c`>blk66#{8@&F=|H~*fQ_T#jg1Ct zFn#Q|yQm3F{CBK9+?|~Lr`|y_0tQ#g)|Yz|HWVmw)}asUFltdElI?h}0|NSn#+4MapY&lj$SKwHv*oUP=46o1Z%=dUn9X}}v> zCl-HT`2SrKVY?3?IV<4g_IF1hC_xQeECAJLA5K5^-zO}7{r{qtg3@sU)=v`A|Bsyq zD>5liTqK?Nr(lGP@!$tk5coks9_WyV|J0^V!aF9@!r7UbB4jI;C)+zaN(soOIs*Tf z3Y99dGW5Sxs95@tl@qY~RI(!q{9gBz^@K$vjakh!93L{IwN7OAJsE|Kw+S76Lu0vx zPtexN9ba{MLnV)<-*0(qL7hv@PbmfE#|O0qzQFA8m=GnysmmITeaci7r z(K_sPHq&dowoI>mnLWQzjSo3WNL3);1sKxbw4a+N+lP$OaJ#SU+WI`#L?k9>I_M1t zXJ)qXT^#s#&#}CDDjCt#RJ<3P?ok`4OZgNr&$C3BNx{pqpC4HeDTT@< zjUG`^;M^jXzRsh(Diovty)GI0^Jh=zpFh{yIvaUY5QIIqxDoH}n4O1EYTBZO6D~vm{Akg(Cx33*sJ5d%@9p!R`^}aLg116tmka@PMXLK8W3bb3jDB< zkBotEd=8%*Wj0umn4SERVQ0<02Xl`ISRCc?~YOwPcd7OJs5mz?^0NRU%6 z79%s=PjPP+HtVWtQh$J{ZgAsyA(NPX?KvqAzxBhZ$AuJeFigx)}&( zcSv;z_rHO&j<{XKr+)stz)D&`BOEM)+u zI!l|?-zwSEv;eX0rzN{Ijf{F^m66z|+|%SfDHpd_OK~xDlF`3ej|<1hL7Jq#t&ZCyDmYkSmi(dsJ8sDm0MU}(D!F? zkNN%#OuqALt79gT*Zi{4fh{yy`}k?%*ri{!Bi_5>%dApTlz z$;kCZ4>8tH#noAFb;rj2AM>QllB{>o2p#2a?cPKY`5cm4`3daol)XOt5FH)Bs+3#3 zncfy~w;A2M_;LNnlPj-3A`{;PYr@Y>1t-Ai*~q{E9*VE=6cHudLYnbrLt?sEtnJ|q zXCnj}Q7ieHl0WML=c>vz62)Ldnc~jqZg_E=b{twBy@DXlS)RpJzv+A3$O3Nr-3La_ z(pi&Ch^3`BdzMT$Te5bcmLb`Cw~Awvf#OBQkeZzcVez}Osp`#b@ZB7nt3yzcrsd|| zaI53>9X;5Yd*$J;6l&dQ{+O;K?T+Eg5Z4%B1Tg_mF3X4x4NVuzb4flZ z@*tk9>#1l$m__Nae-!soIhs1>K#L<}Gdr^&P|07*-`vv5i71ed5OYY}2}Hc4S^BFK zgN~$}wP?nj1#MDyb<(Gh`G@97QC;nc@5zjKea?u|yN{<}1~>Ep?DU8YKVk?M@-2j5xT3X!#qMHBO z&K6dw3QURGVBwPw)I~(vCRkF3~D}EW;G-XmiW1-JSAlT4lYF*AH5{i#yL~s2+8Q{>vB2m@ky;M zvXL70gSMM)kopE@Hh%xkGEp9A$*5RvmhlZ)RF15}^l*PNV8eIUxjl6pORZj*>Ftt4 zJtVhjkr8XBW_GPOTDe+&m{qiARbVUL%ydze*M$7D?|tn~EPi(`RDYm%bLQQa_p zwgtAhxmcNb(cSF*F#F0Y{xtJDNO{aP+If%PqD+U(p6_1URKWSg)ZNEL#Bpcvw@SF_4zl1uO&^cjiC4e~?ny$U$0F&}wO(m**R zD7Kb?`(!Ovfm1R2k5Op%ECmBB#gNf+D#^4hdO~ejyoWt2l<_L%1xiUbu=L8@+}6x) zPm%EW2P<(?&Z|o(2pBoM&7lQaG?c!uHq)VP6&2C!@5}ZV&veSr2ummHyN4vFh0Qfm z70sdX7gT=pepaq;5%k5DGBPrQxctLz9j$j5EoNe$xl&C6QP3zc%}iIN-g5TKoN}e* zEc@cdwsKN(f6IH8)Yqp=RhyMvS4S&jzrXl|xScK5#^x%k_ieQJ*Tp4Mt2uv|tNCml zmn?`PW`4JWgq5#*j?1Fk+F+e)ZyhcQ$Jh0mFLs{E(6bxu_t@sFBdA+7Ap=^>spaPi zgwh88&;0dylqZ$#7!~t@-n$i5B>2EBAShTb@wh8Ma_;hKfZfVtuR_P z)eOUDP_crT=Y4+@$R}X@{S{EWt?V(FY9;4-Dy+^4dG`4;CrH_bgdw0muq6};IW5g0 zzXgTfihP2I#e#AKte~**%{a0B+qIACZ1$QEFl$g4A}x&=UGuN)AQuT24}kqB*nGR?c_O zS|$DbQ#Cza0U?W}#2v=iSKF16kn~(Vr%;!x@U4S@uJln+kv0-sTt{P`EO7G;X&^_? z_?)^;o-#${GTP&V0?h5Gsl%93%%`orn-J0~zs|9-Q!}@-!r?{4pX6j~q3Z|jTd8E! zt&H@X!apEEYVvV&m1f}ydA8;VxMa%dnq(4X{QRI#4>jzd9&*Tuj^Q5YKwBK@m! zATy77N{plNz3UE<${Q>Kob4|# zfe@7vxF0cAR?LgXq(nsJk;}`A(Hh&&LB{Pm7L14I(GB|BTOMe%*>O@VG*GZ=de^KG zHs+?r)wFAwk;rIMQ|s?1i$TNQH|5*CazHxg|Kia8>M&97bdSH-$Ga}UK2S-`xK1nk zw#5ZU!sprN3H&R3oM=sx#RU27d~30ZGJg|}q=4|xpGC~9w^CvmA*5X^*LhQso(+X1M4(2NGZnrGx^6C)T2DHebEE_X0 z1QO!yP45c!w7kAKl#U012cy};%Cx?0|k4-`X^v_AZ_z3@Qyp#DJ$#uJ72Eun zw2wOM#jiILbF*s0dm$dvdm{(=tcnMqjGptp@3-sC?e?n3$fh3VI#j;|INSiIsSibt;V+i+Sc!|w%~l?9^PGHQO`xID)SS*~ zb2K$I2Mr#XnCP1H_V8-`L^P-TP!Jgj;0Rgw^Q3)6KwiD^+e33Gb;M73{O+TsWp7CN z_xi_Jl58?}kO}#XX7b=q!Z>&VUPH~?@r+Ry7lC>Lcf{J-{wya~hcXhh+`?DC$f*5Y z&R~*^%q+Xn*KNzob!0RFuNnJthsSEfPp)R^lbR*s-sV%IFSQ+hVhBMoRIr<wUdg zT3+svEp+qvu|0?B`1C1)8VsktHq;e5oRJx#k^)wt>KqOc6BGGU5hN*ZfTXsw7QO)Y z(Q?b0O{K;yKev3;(uq4WO^S8WB=gG;%Vbl01U5X7^0b-Q8CG}=87^XiU>sps`o#$c zT%qC32R{AjF>H0U^Ay3q*b}czrinojVwRU9a45p)U-fp+_3DB@9gMt zQqVISOAvOD;>&M8&rL2%S#-VAj@MSdmE}HJ9F*U8uO(zqdT<&G`gogvxv-L?toZPh zR~OdhOHBH-yW(C;1lvT3Fw5_C(w?QID{djLvoGF0P0ZKV0ldiXJ6jXN!9mHW>+7_f zrt4D=8gTzOnI|Pn#{W(&yGX6{XGIdrw`)*y)1?97{QCMp0iG1v zsgJ7f`y*s!gP0t4yfwK`#$s8;Ylw+(iB5gg;+ThPTA!TMuQR~54H;*}C-HlVA&6Ag zq|KdtYmwIm2%fkfp0am?urt!5p@QNFy=D*i9fqg|oES4RM=3%W%D(sg;Z}JdkE-&2+g^ z0}CV;m?n;Mm-VrO@Y>dgdL(S5+Gc0MIoy}MKgsk(5ic*UGXEjrkTKZ}0qA*07_EHE zD%l-z>22mQ{vp?rEVqbHU&}Q%n0Qfwb%tohMnK%zF_Q~*|g zWGgQVnKZxf*@OH^5PD^&&nZaIwIaGF++OrGGhXg0c%4inkoBG*-q1}WeD9S&xa1SL zyUCrLYi4lRz9KEoNO4~{NNc)QF08G+ZiyIz+xFa*@!FH&P_&X{UOtW z5U%9o9aeY?8SbJ(D9D@TlnZG&#+D5n51Y;3u?hBsISIdhT^2rm6XQTvHEjzP%>+Q_ z5d@uQs2CEYlq+(5O+)4Hj~|_yn$(@jFB$z}*}_8*bQuj5<(7*Ds}9S_i3>RLiy~+K6E-6JkQi3W^dXC!t*m zG#SZA79{7KXxt_iE+4tPqHA2B<%amm^ShY`~2;!L~C2Y?nwun^eC~D~z-Uju8?0$!^eu3F8@9byqRc zdXO5%Hq;sCb+qwy9V|{BhvlheIje$L##m-osm&li%XH%I)sPTMBgqSXRH;U6j&mlW z>xXk+1A8Kq#WSW*=fbMa3f8@oqy8a~kl?AbvqPVkma%eq`L`oc7)}<2A4I>q`lIjk z_eHdTh0=Ne7U{d9vLs)2_WRxAFyqkF=2Xar76XARWkIaE!1hmflj&GU;4f|<9Timu zyH>!pbdAg59x{?7z+mv}FDQ;fG4R)F1On4Hm7e<`HYP5E;6bS^J#`sak(G;+CKEs5a+F~72y(QuQ6E_4^} zok_|5ilv?3a&Ddi2*f$#TK=|5lD2z7So#MdrAqdDEiLye-W3up#IF^zdx04k|IV0! z-L1DF3)>P)E3XaqT*MC0M^{%P!T3Vr7Xtub<0bjt^2H@D z7Y%mJQstsN4r%q?Q5x5jn3$pn2Js2^pKcZlIV|x$q`SnLZopwdyaKKunrQ!e&xJ6& z`WU#qp}_40idxB?8H&U=Zru~vPZurnW|O0}vqRS0bgx%Z>zDVYJ% zI{x2xYQcOp!F+AUE}>A``PJ0|LhGRqr-DKFfEy@~QC0F`K!+ZZgg5oQ4`2C^o7-+_ zN7xRzrgNq2^V~_`fI0yUAPOANau@PRg4T0mO+4ExJC&&n!VSrEt#A`f&lC=BD0Cna}zzsQVv$L5d*6eu~ zjt(gTg2?B*UjXtjj%0D@KM$bSzdgeE<-FsUpa186(0+miK(GJ7kqJe>MbIt)YW&}{ z4mhD!a2!_r0fnlwkkK<3*9560^zm==@;EfXJ*?{W+U+FLbt5k_IFWt>uBM0t zwYE*tQ&S5HRv^In$e&;pz?mb(kIU8DT}CoeQf^q!4;4sW5W*DWN;CC6L~SSWBol1F z;VK|lW9UiP9&pB=2naWDM9*5}U|qgu<*hsyqJ`yOH~>(4-xY1tF0R+#0OtaE*T@9Z zdcV2pyE=x;zILrr^7{v+KVSjU=>QR(o8I1xK&K10Kw)$@EDepDK}t#*xajM#*I|2> zOy+Q_XV2_b$GqG=RKO)^X(ujHtB1c&)nD(9DDD|2L<2uv0hn*>v@m9X z{akiEk-=#`(ILFGRif>61u(HQYViCZAWjQT?ZNzu#{j=ejEo|AMn)SL;87qyF3mCw=Mw!X}D%@QFdcX`DeEt4Td%LZM3fS z@Uh`F#=okLJHBzMETnJ=MPGk$?b@HboaMvVXMCK=H%c#@Jl0HUQTZ4B)wo$l-j63K zZycxk$oukTLeoW;(n~t`jU#Ht$Jg`v3chE_-@-us6tZQ~UhBlO}P^}_O5 zRKY6P6XZ#Ev9?FvxopU=kh0hQ*?a+AwGqffv;SSh^QS|GA+m6u0urw}4r1c)YoJL3 zBHeM)X)h&p;-a_*5w41^e-Z662MyWcqvU;in`1ovHVylkkhvrFtvON$It zfqOn&7&@3Qh^psWGx@0cRt+99HPw-kvp^_um~23K6BZpt`>na%H~b&jj_iE6cFog= zwOxYtH<$p)KK%r&xZKU7L9pD`%Vu^PkFMEBf20z2igDkUlbh&33thT|8Q00>ki@zq z_U4?rL?=bp+fHb)`<2)XZnS;+r@FVPr z>irsL{fCThJk5GmD%_mODRS6g}nW^)Yo!&Y_Q_y*sZH5zJh zN8Iyw@Ur@w3z+(B+DkMl^j;Fqc-gn40TX0#boA^5AeCkW%1`Y5@E6{_f~U{)LngRA9X*~)X2MXjO>wfALY>T4p(*-Sw0q*`Uo)3HOOKM5rSmQhpE8b0&zxpN;+ zuFY4mzMjp{Yji$#V8f6YJSMBET&9WmNv652Qv&C~#%`49#jY1FPWJnMB2Be1MYafY zaofE9j?KDr4lZKHY{kmE)1Dp@MzlX(nps-1Fy~`}r>SV0Er?O|^0`Huc@Fgi5j~p< z3@2&8Ucem(MSe~PUnSC2 zyWDv%Y%b_tC)y?9$I>5y*~APHUW_}Dgg4L_9;KFPw{Ys~1$sQrReP67~2>a6ZYS_Q^~M=&HT14BPt{65o}w$Wsep79GQjNQZ{(!bg-2Od$wTOD6h2 z>T{E(cpfiqC8lJXOWbsj%Dv@lj(Ns`o~DxB@pN+-7!}F|(5G@Zz-+Y6cgBN&=^z^K z^c>P(Ljm5Tcf!v6Rpq1S4V~{3%>108Vgg&OLzdNpjkfyytv%KrY-Hik z;it|LY;}RtdPNs%y{XNp?P*3#n6;zSg*kc3b);l|g3nHXL z4=I)IzuaMs!#m=W`}>vcuOD#Mn%!_WsK{F@|Kt~Rt=_g0GjPqiIaP13`LNMfN@{0q zJv>SQA>bVNq}uxK7oNLqeK9#E7#lAp>I8x4#mOEfocLB0ep{baJm?{n$3|Vl{VnYN zzG?U3-gnE0=G9gFOyhG42DdbRg2XXMo(Lq=y76VVjusK&?oB$gMEoO_-@p3#a_`nT zP6m=*_Qecsm2*W^wfyz-TprwHDk&(zXF~rnlU{_kw-<6wg(oZXXZXWe(hVktwB=%v zS(ak~TzVr#m1&*+)|w4DEiIEkCQe>EbEXbx3b21H0!OmSDoL4p2#H>AmqF3N>!wB5 z!k-~7meKT26VdlSOn&x2WNeH`p1ZFgz-=ovhmB3}YE}Fi(aDLgj8sFMJ$%Mo?XVL0 zHNI=2cyWrNvFCMY^194SD6b%Pa{6038#x6J_WInsq!#t7bhIU zi%mF{c6sqNF5bA&biyKQWZQ<96oyNf5Mr47xb+^rlDHaLFOJW^^VG0lQ%Y?_lgeme zhk5dIRHl>l+R|kscVUo;QJj8GX0^&q<&|*R#MfI`)-K$xXvu~&)rsQ^e$%y!hmUjW zsTGR!!nVRyeCq=0$NhSI5Dv4F@!_+nC??hdER6YPhG7G1TwLl^YU-2}rroAmZ9qgI{M?`Y8uQggYNjCh#E(@-C4 zPG)Xc+bBTRbwF<}amRbL_Nj%2c%EHF8otwog_K<@Q({}DV8e@S@x7d;6@mMRn+b-+ z+S+<;T}4sCbZ$tNp0VV>W@d6yh=SV(KWvn)upl@lPL zDy(6~hQd+6K-jIUAxcjXK|jExJk@Ezeg%WmcpI;Hg94_*l?pR9NJb9&-v2prN_lxm z2|ju`Q2T6Z|4fxuA;CJ=ArCRvADd9+dE2}@1xqy)dPw3Y6~^gm5Bdn4gqW&WNI-_J z#1Z@OtAr1Cm|l2~ESJM@BGTaa%Tp3R8*TKgXJ!ER0fpCi-QVJg?~KVS!^dG5Hbv=z z%<}vuK-J6ZTv9}F+~VO*D*;NSqJY6xbg^O9uXuP6n;9mYJKUTr32&pMCfDm13H}bV z_Ftr(RWhQ;$yNhN2mC9%q+A??Ac$mf;VV8(ELl5z=$7udNlmc@E#k~o zp4BrWC8_r&@Evg@U?&TL>lruvDJg&Kd*8fT^dzEcp;LO(spXwnyl@e`?p5R`p3_kc z5=;!ju2Qc)TqMsrN!`y)P(D63WB-*2=PoBVc`l?#;^)aI^cbp2);(d63KFo!ofsoE z^cF`4@AikpJLkAaD!aHq+#+7p$dw?i{dWCc5!2Cly_Ln(v3cyg_=w>IQ3vzk?E`TO z@ywsyL5oChk7xDETqlm&31RCoB0Fy%b@Q|9dOjL;ONvzcFi~JYBx_QvPzwh~oE}*G`KJt-@h^xG5#DoyIV6@(& zsIx?ZQ;{?k>>PidMp1ym9Bg3rD3?+ySpIPiF)&`^lSUAW0tvHcJdZ*D1ug)L?9}`h zFVHyl*$1GPB9@ktGqhH`bh4-^DIL(PRTk3Sy_Wjd)YUh_LuO^UQc}Z-#KD^1AR5Dz zD`Z$0g3l9l)eG`gau=7q7s66=)VvJn#dh9ikNo_JHULmN5&%>SS(Wa|vOaT*w36us zD`9i+f7Ou60!c%UgnS;&I`=A-B_?hIQSz*%OEpgdo}JTelRM|!0)hd6;R8TJ9j>5H z21-SADAq4?+J!^=+uKDV>!K78#T=8|JSM-ZB`11~grVRPbp@TGq~KOcaxbN&9lM^< znj(xA`#I^dgJpF_AFHL@JMpKIt?NMkdrH0148;R5e13U-J=2Fi`K%6QuQ6xbExRu4 zqjMnpAB=u`*4Ze66(igq){-M7)qOW&gh^dp#U505kZ`U_7NmzL+IZcIUwQmW&?i5o zvX+)q9sSz*w{7}5mAB*Ks*|k2=O}qX=?gMSBI;W>q%EeiQ)1(7n~jI9BuN5w@(7Td zyZ>!qgJkQL>{nU}aEnZp(4x*{>iF8KbHzLf>FR91_ub#_Frv@iC(SmTTdQg`>kgav$*=&E>}s# z=mPd&>rsI28jhEre?Vhp^))IoQpM&T5UH?$vUJ*@huiD~l*~myaNXVCz}pgYcoKgS3SY25Sop;(e1v1Tev`_vZ|_1)dEAOG1K@s zLz=lQV|A`H_=Uu;+W*n8cW^H=PPfVz=Y-~nH>%AqA@79Zldf(AX;!&Rj25TiW$Hvz z`u_i*JWF+TeSSKbF$QLnU6vz(Y1N~lr?ZG^X%S|`CIki9m|9`-L@zE*Qe*lUK#_9D zeJc2}pyx2AkVQl9Myje(hywzM8La)lOa1j$*_`+!Yp|{%GNvEQCS_(eRXHWgF>%T~ z(%gFsw7--4b16hKfTXDmj8>jo^QTY!TzD;ZZRxsH``jz18$wuzF;PbtRr?Zm<3`5D zO7*Py`MWJ$R^gV+5TpR?o6MOPi;H&z0WTG(7CDG-RcqA~>8itN zv$Zp14Bg(;;5njLQ4kOIB_w$}t8>ZGf+#~g?MX5T4CyQ7oEUwWY%iE#}F9kAuXh5 zb>QImvoIuKfD!g1;cwLhSg&>dcL1EcS;(T6tTVv}zy;s|Yzrf={C|;#U>k!af?ofF ze*bz1!1drEi`fU{pJaFsU~ho^@Xr8$X>A+?#BZ16|1h>d0c$+HNk*4EegKO3IunzF zCFSe!t4Ir8cj81>`Kb5aZmTHIeYT8h=LH-6p%4S3u`A&c;ndWsl|>ht;?LCp=#I2r z$6z6m@U;+NsPwsyt}fcXD&i>us(OOgWp*B}snqf#_>=*F1RCe1|NQvy+0~JvGv2^| zRYh9+Nq4FHDGA=@P8Hemp52X-B^9&~$%M}Df}iwwYcfm6vgR=2(x2czx`Rn$y~OJ^ z{!AeHlm(+3!+KaMe6`2-Xkg6$z~u&ocY%=3m2dvW_Tr}1hlaapmF71yNV6{r;bn}c zxZ#eo*z6i%>9#iU9eQY08xvpF-KBfvalUL^i= zB1ZghyVS66t1Mwihj?Hxas()^igjplg1nGG)%uin9w z&t@_`60Ak&X8MfS6R#2UO3A&Dj zud(Xiv|dR{-fQcN&olCXC~UrFt5l=iros$BNh3emUVUZ%wAEQ{^t9P1Oa0VrN>p}AtZ zImsilEQk60_1R6Y2frPMIm-AK(J?UzKfNT99aO>DT)5&6802V22=~)ILJhgBq$U8D zJR}4-JG{8K25vwQy_4x!G3hq zPZa0t+3%Fm5GpI2(7v88rJ|zpj5ry5+c#cP1DNTO`*IX=5}`eNDDUIGEcMS11sJM| z<}-cf?2uW6j`u9Gb}1-Mj-*ke#epz9;ua5v{~Ba&thaJBAIU z!($9|!|`x$WFLP46K>%Df%BQqNH*rW-c_NyTI%ChZEEaheOP)t+>8{7ozD~{=07$N zJ-QAR?)~81C{a$o*F%UOk2;8CLy*~jnJxk%;~fX< zxaSI|`F{n_z+5vQ!; zI`Qo~ZviNB1>yi7#1CVeoEGJRBfUz?=alqr64pMLJ87WWgfpD`LyKr$rgXRdFbM4N zC@Z@~#@9*{=`G+1JH=XfSamhp<*lmaBz^AypRL(oQzKw3swM{q)||o1_$9SO1YV&=4pPotRv0jZ9xJ z(J-)K(mkF&X=4Qp?!G6zvFF3$q0Sxca}jkM(g4K+eQ8h7IPK21uPyw%6<+qgtbgpf z?6`e_E;zG^jzFV@KTwFMqS%C{wT%dx3KH61TZURpX_(Z@L^6hm{B-d|Nqx1TssM=ipHe>?G&m%`TyK0%vb;0 zDKr)J?U)kT`Rh@RKd*geHvBhc&@WaA zm?U`q(VY2r{Q}56ei_1e?TaOa&Bx9b6NR-e5B~s)35a@I*3sLyb;F^71Fo2YIM=^7 z%zhc}0o3-t!KiqdPq$I`UQCA-7&rig^6f{55^jU$uVu2KUIEbSZ_{q&PeGW@C426x zde+)kPt#ID)|oNOun=#LF4Agd630#5++g6YuYQ#2+yx|9s5)x3ZF?Wh(JU3+=L+ZX`D7&sOl4-(W+sx=O1+mXJ6;3$Zq>dMke;yuFp6G9cq7|SxUdcM*EqKAPvCr0<6&r|A&phw3*LdD^wjZ zCx5yZ=1`&~ErakE(*iPG+3Fz;vw7ZnMYk2Ko#g>I!_JW`1FYZnUCp=xYR2 zjts0_S#3@phhe>&&Fb2kU2dAVI9-M9BqLG>lmFR^+g4zDmw=xsqq={=3JKk42luuQ z*5#r-Cqx&TS=Yf0N4oo5!7ScZT!ZtYKXX5bd;XT7oIsrLBIkRlS zAXLqzKySuaa#DjSIWcq;%&;Q$M=wkTGdKgT5w>vbt%mOK9sA2!h;C!}eM=B|K90H8 zn7$~|2u_auO1J3V*#5|(WgpY7z=Lcly5Z`w4 zBKX0*-VMLL%xR9quTdjB8+Ci12hj{n9yegHfOFlKV6Ro-@>$bQGBY<(F5PzCw9I;W zEgPW7LWQ!j45?|CB#XJ6HeT=dM3{tP4Moyl56wwM`uBE%g=`%Dd|zuFdaLl zwB&Gwy$$p%tK^k8QLIk*&$9Y^A!vFsGyMkB3FI#5pAPidmGXc3bhu5?&8|AxAcnQxtxI$hd4D?d1O<6%Wh5#L(Nyc>bb)A{Q$H+k z`*=_9b&0b1|0n{k89-nl7@nMhT3`}!eCR@AT3mMC25eV+cKx-;jG;ze3pi(qqS z`Wp-xhE~&m65XMud#+5k)}lLgQE#4|g!lHDfk#F(kIP-8rsm1hni&TZB~qU`(dFja zj758E_zIf31FjAL>WdcU>+UU<%FA0)+i;Mhwiyo=tgEoFd=w6sU;tp1{3?myHjqxD!DyWXuo&qDG!IGE1Tt6;>=!*5QP z5eS=wXShRfw!5-iRsz#cPMvQ~sc~;{7_hcOE6}1?NPwQ8C`gd6zqi;#gDIke1;3`D zj@cg@(z%a(#=4G=M-b6-XFy>WvyQm^rEibb>pY&`kbG~7@O5?{faKzFm~P|J&a$;2 z(Q2V9u5AZxu8l0Qax-@zD2vl>=ad z?v7!FlO-(PaX4*~3L4dNM%W=TrDH#j)HC+7v$dC}lG$HUWJrTYgMD)oNG($r7jB?R z9kt$Th*^w9KD<%LNE=p{<2XZdj1AfMNET{AF0Y|TKN~4~HhGXZ65w@rbNOeHflZFfBp;`KZK99iHS`Q>s7S+1wB;vbM!ad2ABbt@P*H*lhaSY z_cT}c)PZe9M6($vDAK;&6>j|n4RW5CLCL<7m+j3YKO)Wv_wBJ_(X53dE5RFAOl%3wDm?oU73{*h_=MIosiLy;ju;e1oKr4bwer5>JKhOVP zf;ihFEb4KTkxwGhzja}ShM@I)4Edjls|IrvS zHUJih&8J^^>;E+qc-u4o3k*gx0p$YxUzo$c(D0X;7TaFu`r!%{-tT#m$KW*{5<)rz z+SWszBUys1W;TW|wKrMEx{9Upi!I*-g!&C8GOmV_+0kTIDic;xY4bRBQfueFLsDua z5%Johq-GT2)%`&{KNz>Vt`kL^32(UqSE=E3Fq(Qv<~kT+^VImbs!DXWu05JoqLg;7m(0nV9ld${=beCcsE$dTYEU z5~;8zoRbaCfoRIE&rCf0qX}RJY6JhCX6vF}U!@V2jyS{;r6PoAc{R8zQuCCC0Bit6 z^C#9-?8`7XtO5FDJnOq08W-&15_SLPgF8UG6`&LBLAwCZZl_o1%r>F*t|FzUcd{vR zc@qdB7zNB?0FOV=wI;;M->)--PnD$V6(Kf0Jyal2+I0a8pq{QC(ajp#+6sg{HQGx|gG2^;0tp~^I1?8Z?a?+ju{!*>rZu9m!9 zQ0Z{s@vda)%F)H(n_30REk+vl~&}W0|W7bmmN# zG779bSha!G#SI6Hfux|@*bKK_Y4@&>DVQ1<;@tlB!}Kio^)da~NT;usnbeQwoD~SA zcCCOl0chHvbBed;ONX;{zgsrr<+Cyww+pMPWEnU2NxnjKMqSSYz^()Bblp6X8^+-H zYrU}YvSIg%25^i3xKd}7__nAhOXwb)NyWF>2*+|WRl5k`kSu)+-e(TP1Pl}ky8*01 z6R|ZdQB!>#G zn?D|?s3_Vnk%^C;(X(TmLw;>K7EmH&bIiAQh6K8?HJ^It(rS>_(Gm+HMk`pBGTVZy z4!ZggQ@UK=(fb+!y6jgr>`FbEk3dwW`1zG!sm~Ay1AQ8<1J+#liH%HX9OZ<&Kx7-; zxvXoQSy~FQZl8mFDbG=_=iTO|B`>Y~nQ!%5N^OO-k!V0wox92XfGYs^tn_!sZ^jeS z@GI~mMAtG21djKR!vZE%ypgTOJ38#uw^o{xvN`^W>B&*23G}Ixk=1L6wjrR*17HDe zvl2L8f#|nc6SC3!nZAaBpcxQ^$9-a0&+qJf=-XS^E~!w0AU1BX&dki5vjaZ^P@Rwp z0xSYOK)`}e&>R(b&FyO*wWfkh^RB8p1f!Q9<`hE*BE+p&`dXgdlK-bv-C&Q-J%O!|$L(WJ< zrc*koCNtT z^;Oq2Tp1rX(i^BJuZhk~Zof7HBhaU@vMfeFys8wXjFj;9*%P$mh<%Zv>xXJRLQ&<* zT!$_=s7^uDudm>rcI>Jx-6Cg%!i))@Tl)Icb#o02kl1gGRXozseaWD*rb=iC;~JDr z2rFjfZ9Qv0zexwo z-1el6%j&kk1&Dtdbq6h9Gqp_5Bu(zCgX;m3gYO+I7A$V27~1Au2}g&^%p3#JP6$+- zCc8t3F4w3A$7TQPa$DPRelgD+&YnH&&eko6LxBT#a4HPZk}Emc$6SFcjnt*Ox3Pr7 zn*nnOqPZYQEH+nE5r)^Nb#@`Zc$5wS{`Y<9#!bAkB7PD|$8L;S`NhnROZe z)vJXAe`i5_C!et64%gTu3~Njm=2Nr&PJ4LeO~?S3ewEa@jt-5Ly}w3A(q&|3LZC=W zeJd*cj^ue;Jj`+ieBSA>azuIcYh{;%KB*)D%Cob_P7eIt>eJV+EnH>m>kg?~mB8{T z6CHH06d|v1zgC{X)zZ>TbR`a={erUBs^YvtPcQ$lMskXTuyt7nuqz<0vu2Mc87LGq zklHwe{Ea44?7DEyC~|&DaZ*^zfHV#yG`u&N@$ev-vC7Rl))lGvPWP~6| zg%Hu_{GZ7Bu^k+^3-*KLY#rU|R-jiQW~Bv0FYANO-pL6zn4O0}XAeY0T!l3r#Ip7e zeOR)f$`b&$U#(iYW5C53Av^DfQmjsgG zotA!>*E0DeBBi0Cbgg`%uFk3JOn2zJ!Z92zjgZM8jYo?~w~3V0<#mqcpJq+m$MZ9q zf=RhqK!gUh!WvJSo`b!SYfE!)&Ze;od(`0m&I~%T-Z7Y@8ApDrGg(OXYWo_MOwtg% z63E2pN^Km=Ahu&;cMRr75r6_Q$~V$efkB@+KXjj>()VXxRw*d@?3G)DX>D$~1Wp#m zBrh&Clnnt@3?(1pf~O)iB79BX5|n%jX?q=#C9yHjZr(0Rj*!{#|*tU;5&+04$TW2G>CgxgI7U9o; zGFz@}f21`o5G$pqkN=iS!cpQD{VghP7pccSytUh4jI#dxg%)Bb(k+$0ZXhkW<5}K}ZURq1tq~Vd10_#8 zI+#2&)=MI-ja%8$&r!5)TI{}-sI8g6mfetiSd$)fQvSJP?8Ed|$0V4KZ$5YY>+*9c zYDBYY*q0l!zh`J@YP`Ae_OiT!Jf-THx3XEcq+V)W)?mFLcUFENa+8|L#z$p?mvG_y z^CG2(^hE@cUzo+@rgEx*S1sCNV(|SF+K_Ya2wU!JzE(LW2hLf}=2Ex+;R*x)ZduRzJFvT8&3aFjV9WK^2 zxprUM9y0x<;p4t$p%6%`z`)~rM?EEq^~4BeE<7`p*&^XEy7s$8mZ0Noz*g#3+;0M$ zk3KT|fveazleGU5;v)SUBv#&lg17}c_22TI4-J))gbVFnd8Ogn2it?Amk*T}Qkg_< zJ>se;|6KB%?vq9Sn^!eAb7rmNZq2i4ypOM750G%swzQEYB5`xNGbYZmP^94jAc5GO zRMoyNO>s@<2%_VHa&^DwADIyhFTyOYmTeEFMdnTF7d5_zQFL7#cpCRU-dxc2)5{mM z!Cxl2W1TSvYt~-E-g7#LCjwXa62)cgwmsKx0(<`s7+StiK6g96iy?(QKmhdwmzpj}`3QpXLuH8a&NLn|&dX2MoDvL!D*M^hZmd7pYui z+8;M{oLj!X-TUVoE4P?2s>=3rUyP!)4{ChiMi1UdyvgO#8Ip+tngW{8mk52q zjQy?sw==nVpuU^^x#v@*f2!F(QEpe?v#*Egeav27>)*25YOfpMI(P9F011RRWg*h9 z;o$0y(*Dzexsj{GM&*~Twwn3%tGTyNbkqF!Dssi2Bfn8?J`Oa01H|)#*hr4M^+JVz z*4&$)>50$SGFoA~^Qyp8Mm16T`mIR0JD0i?4Uaubu@-$F^y7T7?e4BzTf)4=aj=X4 zS|X>PkuF2Hm<;lsf_T5PcX*$Q%#iThSDJedXrAW2w4kmnMOTzRcYTAOR;J7`U@C{Fo;iEAM0p5J{bFO(MaWv zb}WYtQxo51OR9IvlbU>K0%o@S=6;dm<7{^meYM=&sJ=|;bkhMx1MEK7`TFmnx6HV| zT3SxMjCN~(zqgG&xa`T#yy0DBTQv0BmWvujw`FGUUHa5BoUgC~`vm~)8chf6*pR~j zb=(AUouE3}`4^@&2z6XQZ<1!4(~PH+-OEHRaQmd$^|=&#{k!oG9fD_l+S%ID%so^L zy1x4`ojEy^i>cBR8l4YxkKm*Pmoj%+uYiT%iBzGjGLdTUjh6Slm7Y;ri$7TNs(GmQ zXscm5-ljl7o_cPAmflv29s4q)C&w&l?;@~+-+)Q@Zl0#O@=8oRt6BAT)V|D@k+$Nq zCp?bNO-{$-{<>>=Y7eV2`|^>{yV7kudu%J*ZQ}{|Pd@3JlaJl7c6{ zx6{-fHCb2b=lu@+RfwXyk!}fDtI_!{RIUv??tMJGhph=7KTY6a-Pkr1O`Yj0En3}e zxJ&D&)YQb$KThC!GO(|$H0w-JR>q}mFy7X2V)r65d6UClSs1qjfGrp6@n^f}Wp5tV zNw}YLffMf^1e$CkO-=1D0`Zu<6>GE7k4^~Pxb*dP^;hzCkL}iY<{a-Me2=hs{6fyY zS6Kc9kgc=Gah|=6F+R!JdLm`ZlCPr6LVd23NszY>I?2(N?@5lUcMGE9#02}uk${2? zv1z7go<3w_-nLRNuc$0S=3bM@%gy6?)#d^UcXDeYrJ1+qC&oVZ2A0VRL!%} zI^}>P!1qL5zG*BLysV|Geedw$(&vx9Twi0Ytw-|f>(e-*L|i|Wgf`#LQbPmIx|=vJ z{|Oi+fSCh&R`1qWlk_+y;l9p=ZWlibn3UEPj*M)yulPMMFj!Em)HjxN$gT5Bw6>1a zyyKdDHZv9&s0k+`bU4kaDFd9b4vX58Q+{}?#JufmmPt3sPseZfj8qe1kZmU09TP>5 zMO11$c(nPRz?XD-=`|iTH93n7DlhSZEO1O43Iu+?-aFrRQaTf^=0`BU9>dw6(0TF- zdNN~b3UMn4T4HFoJ$`a<dzx)hsCxEXuqq%(VhqtXJbf~^_8=;%Zu$8 zmvTHJd_b7^=BQ9!;W~Y1;&!cV`Sf3=+g|9AkwT4|CCYJv=>Hrhi#csWMo^ffC~4IF zHWM6#>oPE`yy`ZR0Tu?t$N};*j`#B{ckcHOHV7>_KTlMY-Q2gMW%QR)d-AXz zkrs_E*d}Xedz8X??cMRljnmJwoa=6z>2;b|(#NTN||Hgvr^FobbXJ>eSuS=#H zhSm^>3-$!+@D@%JAaxI{?8XfTDs^m$6!5DZxr5)n$zyk3yFNyPklw1eJ)~)4_#*62 z1jgr8;k2D=VvYm;1lkFf(5_z?+&|=3c7}s19w&^U641rn@>%Myx|!JIoNJg^FC0w|d~f{N42=LDi0se2WE&u=k literal 0 HcmV?d00001 diff --git a/models/bmi160/uv.png b/models/bmi160/uv.png new file mode 100644 index 0000000000000000000000000000000000000000..4b4b9c117e7df5bec72c7905348fcc6efef14d19 GIT binary patch literal 22216 zcmeHPZA@EL7(Vx|%oav$fnhbGUC>d)kj}=8$)tq=g)#Y>!%T5BQ-t^fD_hV7Mq8LV zl&o81sgibOS=@pyY)m5{y58taXNkB7i4ZicAq1!37H77irS+WN-rjrr$LyCMd4G_S z!})yszR!8y4^B^Yr6D=-ociAHga>M)SwU!sI)uwVgChhti43g*6*9%vXHzz7l z|7R6Xum_6{-$bp^pEbQK<7Okff=%)w0!i}920AG)TQD3RngD_T%;Cm~fdk-VOcWFi zI0-ljq&G&QfKSFmfdD4~C&8uVy?*e!!S9Ax5Fd$xfE6Fp1Dph$gfI()Ss=_3WR&}rKeDD*wS=_#&q#xzot^95m-`rx$MfA_cSZ%9OG%BONg^`{iwgj?U8+$Jg^Rh`c?qM57S_rx3XKWs#KQwPfU^bI*h z8!l_je|D^o`<~gqIuh2QrpNA#JHrtRWjvE(kK5RXnA-1H+#o)~j6#E!_Ak_mH)cxL zAjj3M5Aac9vRpo{jdc#yZx?TD61ReILAb;uL36Pw11u1X0$9M{X$4pSScqPi01E&M zQKAR1K%&aP76|DESO8c6SP;0-01E&Mkw^+S5p3ZBumxZXTp;e>#1^K<>V8EieJ^wE(a{Xg>VKE!;L+@J~z* z=B}30KJ&Nd=It9IenJT@lX&RkTrEc0PKDKHK6dZkV6v4|1e@pgxuR78I!S>}=eDw< zpP)lhrnS8#i}N$sW*biJ8`LfZ_x(iNa%&?lWq`g2&`Kp*~d`rvmj z&kV~LP1?4?Ps3L>&FV+nO{<#^?|f4g^4>{dFW{#szdx}Q%>fbb2w$_tG!5#lo@6yC+Gf$?+QjOXsy4xv za0d;3h_X~op41dv(*w8?OFfd6&kZHpm&%p6S}?OuD;{5sYyJA>-gdHGxH)?9Sry*9 z#pBhu;L*GMRi(H9uoUwF=|Zn~d>>wF;pX#kgyIw~Ogz+DDjpAF1+kjEKQ@|E0w?wy mC}x!-NQayqcMzpMx_;C#ypj5)+o8oz)1vZ{%Hp9SW7l67$=9d= literal 0 HcmV?d00001 diff --git a/src/imu-serial.cpp b/src/imu-serial.cpp index 5202f45..4ec6e06 100644 --- a/src/imu-serial.cpp +++ b/src/imu-serial.cpp @@ -17,6 +17,8 @@ ImuSerial::ImuSerial(const std::string &serial_device, unsigned int baud) this->baud = baud; fd = -1; stop_flag = 0; + + this->acc = this->gyr = glm::vec3(0.0f, 0.0f, 0.0f); } ImuSerial::~ImuSerial() @@ -25,6 +27,51 @@ ImuSerial::~ImuSerial() this->stop(); } + +static speed_t baudrate(int baud) +{ + switch (baud) { + case 9600: + return B9600; + case 19200: + return B19200; + case 38400: + return B38400; + case 57600: + return B57600; + case 115200: + return B115200; + case 230400: + return B230400; + case 460800: + return B460800; + case 500000: + return B500000; + case 576000: + return B576000; + case 921600: + return B921600; + case 1000000: + return B1000000; + case 1152000: + return B1152000; + case 1500000: + return B1500000; + case 2000000: + return B2000000; + case 2500000: + return B2500000; + case 3000000: + return B3000000; + case 3500000: + return B3500000; + case 4000000: + return B4000000; + default: + return -1; + } +} + static int open_serial(const char *serdev, unsigned int baud) { int fd; @@ -61,7 +108,7 @@ static int open_serial(const char *serdev, unsigned int baud) tty.c_cc[VTIME] = 0; // Wait for up to 1s (10 deciseconds), returning as soon as any data is received. tty.c_cc[VMIN] = 0; - cfsetispeed(&tty, baud); + cfsetispeed(&tty, baudrate(baud)); if (tcsetattr(fd, TCSANOW, &tty) != 0) { std::cout << "Error setting serial config" << std::endl; } @@ -80,60 +127,91 @@ void update_quaternion(ImuSerial *self, const glm::quat &quat) self->quaternion = quat; } +void update_acc_value(ImuSerial *self, const glm::vec3 &acc) +{ + std::lock_guard(self->acc_mutex); + self->acc = acc; +} + +void update_gyr_value(ImuSerial *self, const glm::vec3 &gyr) +{ + std::lock_guard(self->gyr_mutex); + self->gyr = gyr; +} + void working_thread(ImuSerial *self) { using namespace std::literals::chrono_literals; - enum rcv_state {RECV_W, RECV_I, RECV_J, RECV_K, RECV_IDLE}; - enum rcv_state state = RECV_IDLE; - int data_ptr = 0; + char state = 0; std::string buffer; char data; float value; glm::quat quat; + glm::vec3 acc; + glm::vec3 gyr; quat = self->get_quaternion(); while (!self->stop_flag) { if (read(self->fd, &data, 1) == 1) { - if (data == 'W') { - state = RECV_W; - data_ptr = 0; - buffer = ""; - } else if (data == 'I') { - state = RECV_I; - data_ptr = 0; - buffer = ""; - } else if (data == 'J') { - state = RECV_J; - data_ptr = 0; - buffer = ""; - } else if (data == 'K') { - state = RECV_K; - data_ptr = 0; - buffer = ""; - } else if (state != RECV_IDLE ){ + if (state == 0) { + switch (data) { + case 'W': + case 'I': + case 'K': + case 'J': + case 'x': + case 'y': + case 'z': + case 'X': + case 'Y': + case 'Z': + state = data; + buffer = ""; + } + } else { if (data == '\n') { try { value = parse_buffer(buffer); switch (state) { - case RECV_W: + case 'W': quat.w = value; break; - case RECV_I: + case 'I': quat.x = value; break; - case RECV_J: + case 'J': quat.y = value; break; - case RECV_K: + case 'K': quat.z = value; update_quaternion(self, quat); break; + case 'x': + acc.x = value; + break; + case 'y': + acc.y = value; + break; + case 'z': + acc.z = value; + update_acc_value(self, acc); + break; + case 'X': + gyr.x = value; + break; + case 'Y': + gyr.y = value; + break; + case 'Z': + gyr.z = value; + update_gyr_value(self, gyr); + break; } } catch(...) { std::cerr << "Invalid data received: " << buffer << std::endl; } - state = RECV_IDLE; + state = 0; } else { buffer = buffer.append(&data, 1); } @@ -174,6 +252,18 @@ glm::quat ImuSerial::get_quaternion() return ret_val; } +glm::vec3 ImuSerial::get_accel() +{ + std::lock_guard(this->acc_mutex); + return this->acc; +} + +glm::vec3 ImuSerial::get_gyro() +{ + std::lock_guard(this->gyr_mutex); + return this->gyr; +} + const glm::mat4 ImuSerial::get_rot_matrix() { return glm::mat4_cast(this->get_quaternion()); diff --git a/src/main.cpp b/src/main.cpp index 839d06f..293ddb1 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -13,8 +13,8 @@ #include #include -#define WINDOW_WIDTH 1200 -#define WINDOW_HEIGHT 800 +#define WINDOW_WIDTH 1500 +#define WINDOW_HEIGHT 1000 int main(int argc, char **argv) { @@ -63,7 +63,10 @@ int main(int argc, char **argv) coords.realize(); - Model imu = coords; + Model imu("bmi160/bmi160.obj", glm::vec4(1.0f, 0.0f, 0.0f, 1.0f), shader); + + imu.realize(); + auto imu_base_mat = glm::translate(glm::mat4(1.0f), glm::vec3(2.0,0.0,0.0)); imu.setModelMatrix(imu_base_mat); @@ -155,7 +158,7 @@ int main(int argc, char **argv) imu.render(); { - ImGui::Begin("IMU Rotation"); + ImGui::Begin("IMU Attitude & Heading"); auto quat = imu_serial.get_quaternion(); auto euler = glm::eulerAngles(quat) * 180.0f / (float)M_PI; ImGui::Text("Euler XYZ: %.0f | %.0f | %.0f", euler.x, euler.y, euler.z); @@ -163,6 +166,33 @@ int main(int argc, char **argv) ImGui::End(); } + { + auto accel = imu_serial.get_accel(); + auto gyro = imu_serial.get_gyro(); + auto current_rot_matrix = imu_serial.get_rot_matrix(); + auto global_accel = current_rot_matrix * glm::vec4(accel, 0.0f); + auto global_gyr = current_rot_matrix * glm::vec4(gyro, 0.0f); + + ImGui::Begin("IMU Data"); + ImGui::Text("GYR X: %8.2f\t\t °/s", glm::degrees(gyro.x)); + ImGui::Text("GYR Y: %8.2f\t\t °/s", glm::degrees(gyro.y)); + ImGui::Text("GYR Z: %8.2f\t\t °/s", glm::degrees(gyro.z)); + ImGui::Spacing(); + ImGui::Text("GYR right: %8.2f\t\t °/s", glm::degrees(global_gyr.x)); + ImGui::Text("GYR front: %8.2f\t\t °/s", glm::degrees(global_gyr.y)); + ImGui::Text("GYR up: %8.2f\t\t °/s", glm::degrees(global_gyr.z)); + ImGui::Separator(); + ImGui::Text("ACC X: %8.3f\t\t g", accel.x); + ImGui::Text("ACC Y: %8.3f\t\t g", accel.y); + ImGui::Text("ACC Z: %8.3f\t\t g", accel.z); + ImGui::Spacing(); + ImGui::Text("ACC right\t: %8.3f\t g", global_accel.x); + ImGui::Text("ACC front\t: %8.3f\t g", global_accel.y); + ImGui::Text("ACC up \t: %8.3f\t g", global_accel.z); + + ImGui::End(); + } + ImGui::Render(); ImGui_ImplOpenGL3_RenderDrawData(ImGui::GetDrawData());