From bbed209a3ad72ffe0202b362ebbbb2ea03f4f77f Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Fri, 8 Sep 2023 16:47:03 +0200 Subject: [PATCH 01/33] first commit sensors --- Telemetrix4RpiPico.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Telemetrix4RpiPico.c b/Telemetrix4RpiPico.c index f9772f1..9e32956 100644 --- a/Telemetrix4RpiPico.c +++ b/Telemetrix4RpiPico.c @@ -1359,7 +1359,7 @@ int main() { watchdog_update(); get_next_command(); - + if (!stop_reports) { if (time_us_32() - last_scan >= (scan_delay*1000)) From 2fbaf10bc3d3ed5df53418f3d4590fb967ad7449 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Fri, 8 Sep 2023 16:53:12 +0200 Subject: [PATCH 02/33] convert to cpp --- CMakeLists.txt | 2 +- Telemetrix4RpiPico.c => Telemetrix4RpiPico.cpp | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) rename Telemetrix4RpiPico.c => Telemetrix4RpiPico.cpp (99%) diff --git a/CMakeLists.txt b/CMakeLists.txt index c4709e4..472ea30 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,7 +6,7 @@ include(pico_sdk_import.cmake) project(Telemetrix4RpiPico) pico_sdk_init() add_executable(Telemetrix4RpiPico - Telemetrix4RpiPico.c + Telemetrix4RpiPico.cpp ) pico_generate_pio_header(Telemetrix4RpiPico ${CMAKE_CURRENT_LIST_DIR}/Telemetrix4RpiPico.pio) diff --git a/Telemetrix4RpiPico.c b/Telemetrix4RpiPico.cpp similarity index 99% rename from Telemetrix4RpiPico.c rename to Telemetrix4RpiPico.cpp index 9e32956..6c4cb39 100644 --- a/Telemetrix4RpiPico.c +++ b/Telemetrix4RpiPico.cpp @@ -263,7 +263,7 @@ void set_pin_mode() gpio_init(pin); gpio_set_dir(pin, GPIO_OUT); break; - case PWM_OUTPUT: + case PWM_OUTPUT: { /* Here we will set the operating frequency to be 50 hz to simplify support PWM as well as servo support. */ @@ -287,7 +287,7 @@ void set_pin_mode() pwm_set_enabled(slice_num, true); // let's go! gpio_set_function(pin, GPIO_FUNC_PWM); break; - + } case ANALOG_INPUT: // if the temp sensor was selected, then turn it on if (pin == ADC_TEMPERATURE_REGISTER) @@ -766,7 +766,7 @@ bool create_encoder_timer() void encoder_new() { - ENCODER_TYPES type = command_buffer[ENCODER_TYPE]; + ENCODER_TYPES type = (ENCODER_TYPES) command_buffer[ENCODER_TYPE]; uint pin_a = command_buffer[ENCODER_PIN_A]; uint pin_b = command_buffer[ENCODER_PIN_B]; // both cases will have a pin B if (encoders.next_encoder_index == 0) @@ -1006,8 +1006,8 @@ void set_format_spi() { spi_inst_t *spi_port; uint data_bits = command_buffer[SPI_NUMBER_OF_BITS]; - spi_cpol_t cpol = command_buffer[SPI_CLOCK_PHASE]; - spi_cpha_t cpha = command_buffer[SPI_CLOCK_POLARITY]; + spi_cpol_t cpol = (spi_cpol_t) command_buffer[SPI_CLOCK_PHASE]; + spi_cpha_t cpha = (spi_cpha_t) command_buffer[SPI_CLOCK_POLARITY]; if (command_buffer[SPI_PORT] == 0) { @@ -1017,7 +1017,7 @@ void set_format_spi() { spi_port = spi1; } - spi_set_format(spi_port, data_bits, cpol, cpha, 1); + spi_set_format(spi_port, data_bits, cpol, cpha, (spi_order_t)1); } /******************* FOR FUTURE RELEASES **********************/ From 75c075ebcb809e02db847a227c08d4be99de746a Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Fri, 8 Sep 2023 17:06:24 +0200 Subject: [PATCH 03/33] cpp cleanup --- CMakeLists.txt | 2 +- Telemetrix4RpiPico.cpp | 9 ++------- cmake-build-release/Telemetrix4RpiPico.uf2 | Bin 71168 -> 0 bytes include/Telemetrix4RpiPico.h | 2 -- 4 files changed, 3 insertions(+), 10 deletions(-) delete mode 100644 cmake-build-release/Telemetrix4RpiPico.uf2 diff --git a/CMakeLists.txt b/CMakeLists.txt index 472ea30..74da990 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,7 +8,7 @@ pico_sdk_init() add_executable(Telemetrix4RpiPico Telemetrix4RpiPico.cpp ) - +# target_compile_options(Telemetrix4RpiPico PRIVATE -Wall -Wextra -Wpedantic -Werror) pico_generate_pio_header(Telemetrix4RpiPico ${CMAKE_CURRENT_LIST_DIR}/Telemetrix4RpiPico.pio) pico_enable_stdio_usb(Telemetrix4RpiPico 1) pico_enable_stdio_uart(Telemetrix4RpiPico 1) diff --git a/Telemetrix4RpiPico.cpp b/Telemetrix4RpiPico.cpp index 6c4cb39..98677b4 100644 --- a/Telemetrix4RpiPico.cpp +++ b/Telemetrix4RpiPico.cpp @@ -26,9 +26,6 @@ * *************************************************************************/ -#pragma clang diagnostic push -#pragma ide diagnostic ignored "EndlessLoop" - #include "include/Telemetrix4RpiPico.h" // #include "pico/stdio_uart.h" @@ -522,7 +519,7 @@ void i2c_read() } // copy the data returned from i2c device into the report message buffer - for (uint i = 0; i < i2c_sdk_call_return_value; i++) + for (int i = 0; i < i2c_sdk_call_return_value; i++) { i2c_report_message[i + I2C_READ_START_OF_DATA] = data_from_device[i]; } @@ -589,7 +586,7 @@ void init_neo_pixels() actual_number_of_pixels = command_buffer[NP_NUMBER_OF_PIXELS]; // set the pixels to the fill color - for (int i = 0; i < actual_number_of_pixels; i++) + for (uint i = 0; i < actual_number_of_pixels; i++) { pixel_buffer[i][RED] = command_buffer[NP_RED_FILL]; pixel_buffer[i][GREEN] = command_buffer[NP_GREEN_FILL]; @@ -1374,5 +1371,3 @@ int main() } } } - -#pragma clang diagnostic pop diff --git a/cmake-build-release/Telemetrix4RpiPico.uf2 b/cmake-build-release/Telemetrix4RpiPico.uf2 deleted file mode 100644 index 7e8cffd40090885ee7eba31d9b77ba6f58238e28..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 71168 zcmd?Sd3Y36)<1r#dd=QhAk_<@x;sFU4iFMRG$@s>P)T&7qelOqs_*u(*%5?6s%KYO{VV1n2hYZ==W= z%d9ov?_B&%{#2NAb{}Qd+WUQ^EF+cx=1R%nFqTr`vcATd zfCfNuGng#^-{Wr!3s2&A>$Km1AFq#XKWkrl_t=jKc#?PfE3wx{S z1wZxHKKqa_-LbP|bca7&3Q)(Xw_ELlC*384u3Z4ED;f9QSSBc0vWL%gcuscQ+$Axk zilFuqlmLv>J(=87Ivv<=t^wV=$)G!12)Y|@08qn<@)uu~Rx0lT9XWr%q@dGA&R1EU zek=>mRha(iE#JGybNV{E1ZAm8)OBpPD`cezHjrCNbe-XM!e@}@t4lSG7{eaj@ucI7 zuEU}Gf8`^2C*n2^e?{_O!tYrmlE@`&o_lBaMS`*|}uSYgs|1 z@_I?vsdS_$zYmc6<3YuRLgcBTP=c7(VRV6QlI&&U%GSU)SlUcIN1Hu$o*3Gw)bocu zdOn6;=~5tHE9NHGJYx^~@;cwhh3l@K;9gSK`Nx;@=gf0b72T=*>KS8YARn;JKl!fM)4SVfMIX9D8W2mF@nCq$R>*TrNybHq^m%CawL~>Sa z+=e+h%vn`Wa?;t^a_LB^?vYYYZ;a$^xs~Kyg?U?VA$b+qsq*TPymy52E_WGj^WhuB za;I+Fjd@+He)Z|K)ExaPigkC&EU1fJk8R*w|KRGu!hN#A&nbaoSx(2V(Av3*jXFcI ziO$XG=BBO2_hFfe>t72*?po8pxh5j+qu(?Re^ms3RTTaasvT zIzqJv0=d&JP47xyz2+z%$|EU;x}OKQe@m;Xe{h7lXEu&vV!HLGrMKJ~DV^n&>nm!=(0XVyH=hvtwxr`I;1%qc}aM=lmA*5Jk#ObD2OjTI|BqqLfc$8q?p zBlxSM@Ne~W3FMEm5^1d-k=Bv4QJLL4BC~0+%0r70`Vkqd)e$+KAhvqCJ^H3KmjA1{+TNz9D>k`mxmeC?c2IG=0p)n44Zs$IBUtoxlsF&ttn#R4 zrgblNya?}CJRvA^X&-IoMZHu9c85v7vgruam7nyaG#Tl5eH(LF>g&8#G*i{g8tH_G zg;t3QZWyGjvgI&`?M$s==4x3v^B+FS0%jJd;5P#o0@SoQqJ?V1J4ni^LD~8YHxY3k z{ibpFYa;k-FvF<)H>%gvo(kB)bfGOOwn|b21ASQzMUWm;1m79fm=r;M=uhogpa{M- zOqmwo?}p*+=Hn_w&^xRM#)W25UF|BKVyA2FOnMaz&Nh7;JkIDH2In18tdw7)cj_Gn z1R7=Y3VSSpYM3UDu%uKH3K; z1tQl@4WMo_fwyt^Ya{q;@ta5SkI)6MmH?{=CiYY2SUkRq6zdY)9Xe6xc+B&Ory8}x zNxsBVb@1#Ua)Ox(&KzLF$LxVDAZSs+g#)9gT{YnDRAS!u2fqyKYEM2u!2;CDIo#bH za;9oYrrOpJ%huI^nZ|l(YoOS}HSW%q6kvlL7SXbFjpvZ3z;*x8D$@+P%9JlDbEq?_ zqrpyU`c+_ZC<=_!`&f$)Nnm}MLsP-DXSoz=HVH%nubH0kdGE%eeCP_i%=qXJ2IBKJ)4?%dqvR{F-Cnz zc(sYM8Y&A9xC_zPp zo^eo!DTK@x;g#Y=YEhW7hMJp&GGVx%7hdGH)NZM1y!){IyHDcZ*ee_q`l8wq?87)p zJ-a>REmR_BKO}e+Qn6x=I(XqybbDjM)~ed0w8eEB9c|dsTv7NVhCl(tM&bX2U=UNN zR%WFuWTRxTHcRdjFt3DGW|FuceC54?G!ZO%I6Ega2V3rfgZ4pRqG*s#ORY6kwT8Mt zfC}f13Fr0))MKTcmMJXl?<4ZN4Ecq~Z6d!P!lS5RKAt8TT#%F6lPMl-*lI77AtymT zxM8P#flTFS<%1h{+RZYJJ;>Hu73OVrlk6^u(+qvggzxI>Nyoc(+B5O4@4|Pb;a%T^ z?=sL*3~BWzjlPs9gnpfoQl(RXhm&VwTJZ_mK4FC4YN&(;HF{rZbk4m zZ2i9-9<5t_Nkq19JL(dY!Q%rPg!r=1wCP~*WX*qCs`7j;wR$Wyr#-bbhNh=$s+vBp zp+3ms6v49LV`v{%1m7E`kGeZE@wx7fv?V)^=DO(I)KBV6sdBPN&CQjPm)zvBQeQKV zOLVZkzs_XDb4IF^@=!Je`;>Su72G_m6=O!Ex)!AxO84Mxpc!)J~yRD+XE*Jj|rl%#$$mE_BBNr;;krw!R!)#qqwD55$qchg$Ci2 zCkw2g4&FLc&(guxzDY~ZnF^!?YojCz)t)bfSEcVI$kodE09IXok#Kx9RY1Re*xr9K zWSuspMSx6Rp#DP5Wsq}TzVwW}_Y*~M=5VJl+8PjyNuOc|9)lf!6P5{r(A9CRxb5hA zp+zt@eI=Zpazc9ob{puI9~JDv3eRIxt|7h!MLuPPy?Kb%{{HQz>oMJq6fAGD3;R6wjJ-t`h+O?AUY#vNmPVc?S|F22_E7os zJ5SpOe3qs5DJRjk7^1=mqJO;fvCt`) z!Y%yMLa;M=>Cu4FnuzkmDya(UBp!L?F#d1zoRiY5Ey9z<5m_?*(vypcEIESre;U?Z zh|a0S`W{5xpXi>!juPQ7sIL@=bC*_W-0V+y z0p+PJ2S)?`93nmG7U9)!UmDE&$Q26&ouLH)!QX~>!704Xfw{SMK{)-o5N$aOM(p{0 z&D>P`L@irjaNAr|flX2!jnvLyCj{Fh73S zw(V#g_FWQusio7J&UtRj=qP&1Q#%D7RR^E#+ip_GS5*B^ir`Or&ZGT5((Wdd1_Me< zf*6`+kd*mVQmrQg?I}-tOmJFCn;)rNcMU~a>K}&AkJYVkox-{m9O(yaEPL?iMhELs z2lM+kh?y>|J6^PzK658QhBR{q?oYV zIQ>r-!5;_g#_;d(l!#L#n7LLiXj<$%Bs4bldRl~L&061vj2K(psY1F&Faa-Ul_p-V zHquI-fjS|cXZWj?TE0lgz_dI*QK^Tk`8oW5XG1`x z5axyMMyDC78jo;cn}*SO~3*bbFvtx@FPZ~tdE#Ztlb!wiEhn~cKcTWj{>DBKq#{X_DM$aoxEJ`>0DOfJYRkaHb-G$p>e47IKJ z)YIB>@G{tUv3Nnt7^Etx75QpA;Vae(+j-WWE|aI)SFJH=8M}L%`x$%U2^VmB`zpF1 zl5)tdI#mJhFjcGGXR6$-?n&Ymg?~x}|CA{FiDi!|U2^L*-AB(`cc`0N=mIj&O{IvHjw=kWRJtNCO;pU&a;3VVbiVTLkC zoy4c}Ikbh}Cpd)P30EnT_^sOAf&+F5zG8>43y&7{HT+aKSX?ADNtMDi((k4CtClV9 zMR`NRZk|HESPr|G4_4i1s$`b9A9g>8e9;8Q^&hOFv8yq0w{i79HG)6N!ZGtJx)ix=vGA$TL$wMP^4x2&vq}MgKp@|Fj7HX;Jv=ovD)2qDPC~P=0|c)jlL| za@DzFl_~Kj4c+NRhUcDiwb(!TL=<+l$HYD+osvSI#Q`q7<$rW%f z+P{?y@#$`;ij_2V`M`*V6tYfpQ#okhzq;y4{`Fw;b# zuPD@*rK4CmF;!95`><}1YiD$I9xI3J7o?hC0*R<+_|uhobn z4Yw2<99ek1FhmE3L+N;jC8P|OtHQkNLzIcdUn!wvypoN1wuKTMUSR>IS@75SP@Ln6 z!hdoE|H)DK)4`Z9#qJ2Hkz(Z|6zd;WBE@t_qxGQ&0xw}b24IT--n9b^*e};%f5-ht z6~X)Z!TcWf#eH}@j>ixCvHtg~f=h_qJ2drb;XE zcWO;@&B>aTwavAubt~&2xNA_J=C#gLw<$N&4wz{+)O*|TZ^ai;!u(eJZCNY+KPY6N z%-{q%_+hA?RRl}>c~i2ySeV72*YGDV`eSF1)N3&nuMtr`59epbVsx1X=OH{LNezU#%%-z`Mz;Rrgvd0s~7vx?3 zyTbht2FL-q+wT$?9G^Wt;0R>7sd*iCkXHtH!RV=Tw!z7YmXaGsD1CLHA`qHZ zE#6dG-SC!V@ zDI)hAmMMsi(zXqwW%55M^OcZ2s>~Nc*F}|iI8+!_<{>Q8)K`FI-XDNz^PG9&0bz=p z{HB_XR9$h)mF54ZkKnJ5!avme+NIgzY$sBrAXS>-#G19g*$$tq3FM$%s0xwD6sMQ= zY#c>`{Rt8qLsO%WxFuwYLgJU32?f@WTgJc|!`0Wu}FY!Ig#oyhylX1P!G*hX`@wNn(nA@_st&-mH zUKAHHp|q&?Ovd*Z<-!SE{m0vX3=#YdQTWrrSoDUuTniP5)ArEt1v>a;e~dF$q=Ucj zN8PHw-mKV*`T-tQplG3kV*d*l2<-_LkG%au*6K$-UnHYWW^@HRT4AxU7klr!oSzCS zDsB{-g{q9^qCCq|q1tZX?#CY5Az_E8o!%;JKU!N1`{#l7+OM?OD?O+!XoJ-ikZZ*j z2wM#FAV@-wkgKt|pJ7jxMPY@&p&v*}g~1;0(^#~S!|jK?91UM>GuV?5fyHPmuXL`cG}yIB^Y^vM+%DM3UGF6HZYj6MzJhnWKkkmQ zad$XI?}*!kceIb)akbAHKL=BuA4@6nSrh)10k$W{Qdan^iLYSFuf|f;BX5=;e$Glc zGih@MzRT*oqUygX zg1;#We}d~DqHvu$g3_@O3K_!meJv_S`bdtK0vhWS?lEmg~w)8<_=Sk`!n` zl!}YUY>+OB1CHWiMK2r@lvb%2vMD*CKvI9ow(aL{clPHf8zA82X`|6vo7OMQJ%HlZsIKBB&6ayS`BM}1)LYQatre_ zO6!aFLmMQQf5E`aWEscgC=)MR%@Uzr`m!TXSP8ad8D?e3*DZ&clYQuu_mez{BYD2@ z!K`m(uq3c?4$K)`)(${V!ExV)OKB!WG3skwh%)Ok0*rf{ZWk7P)G z^Wwxs|3BFOG9vhAMB#t)Bkq!13Xl>vKSDgC4U+ewZf7K|uLG&|;O5s7mlljr>*hz^ z*Zz^wOgkd{aW!R4;9leyRMD*1GSmuIKGjW1$&(($68buI=$rM&VN!O)XXqjHyuk8y zZhp>cr$_R3 zRp1p&b9W>Fa@GTzUrQRR*Ec_s5YDv&jFoX{X;8?I*(sR{rVeXR_n)$yd~ujNCNKkx zlmBc4e>NKbaGigE)cMX#QXi0>WKMUmBQkRLDY4R{p00v)LgU?=Ut1GJV_>U9=7u%_ zvNrZq3B^#cG?O_31^O1Dtw}d8)qTDA0<>X|vaRBf&}N)%%j%{qHNrmr{ifIL+U^Gb zN}T!G1{aw#O`Z1Tr}Ut<>_!X3#nW+9+Jus&ys%nO78;F4)#9@11S`MCyK_^KuzQme z#}gm+#z}wOv|BRzaYS*k-DejTiS*o5?{(sQy|VCe<9yWu@y>({zEHfu+w2Wl|Guet z6Y*NYc|umZVBil3HNs4hYMsFFkMIAPBlw%6@NaV+EFY^2mgy)VDWcbJ9Z;9ZKzMEtEpRV8u__*?V=U(Aad%paPotIZG zeZe?8Cl_n#Hg3PLTqsAIP%=mDTP`d`J3iqRjI*$%4bno~7oY?_b6Emg3NUTgIIf!nUjyM0r8pz_M<|63#YTchxY>?B`k&c00s$38FP zi1oB?(mP4bt>RM>V%<*#CYPQIOm?O_P8hV;l+#=S#UJ#hIaX+6`P1J0h{p08&~B6c z+tXFZ4`2Bf3!h*+9o>$E>+49HPuhP{hezuusl&TTeGcNRy}nQ=i29*wE0w9_BdxhP zsj95}8YIREcWT;>f--JieD&vl2)2+$CPz;0qk_#~nH} zBl;p(R8|%0i$4zGT(h33R>nZ|#)nXE{1VHgvHw<6ybCDQ7&(BsIA0S&y$df<$ReR$ zWD=`2j3AkF61g^O@6kwe=hh4>CiBx_h|={iZ7q>mVR2FZ>3;{@Ku z;Xf^c|1|vO(f&W7$#z#{K@`qqk1Gt~13I=W4b0 zclQR+KQIaBi$L7^cxWksEE-1#Yh1cczl0-r>_8w^)Vr_-V?R3<=XXW=c}7>H?nU~S z+?xogne;VYLfrdM*IbN#VxOX}c~FRdQSV83(dbdZUZGXcU`&b>-ygQXti|%rpwM+h zuUKf*S9#{Z|7j#vo`FWNw4xW9!L9HoLK)OEiiNp2W9^XeqN}F(5c;LWP&LJIIKxDg zV%zpeac%EQg)XB$a?h466Bz#S?f>)${?nuIN8R0)i!*L2h2OchxLQiCK`*^@w7ype z)lglWrdJd`>Gm-vSCjZUb?#~*(Y>Yd)x~LY9$fbC=r>VsGIAB> zj&tAJXvcX$S@%HDKcvi1%PaAD3f~;aazSufC=oS-Q?kkZ_b^Xg*4e=7@7Qxx+q_oX z1|L;`wM~fG3&D9qwJtRzk$%Yin_nBX;N1L()pbay-hy6VHkbUgt4iD}h{Z;#rI;5C zIEt+U^jtIkz6i4G&cw6;`jLYbaBc%w)%!KgR}}s;BKXgU!XK=yLS49SRtW~_ery#9 zLRueW{n;1ylTj0GTam!fqQLNS$$!QHW-se0fI^;@t?R)i`K+$fTOdb+-j}->z3UfZGT(jKWim8+j`(lkw;dG-YoOMx!@2#ft-&<8JSZjXzHroR~K?}W*T7_o=Xy?T@ z{1rx)lLQEUhW>#BX@V!t3v#t$t?zNQ2OYTEW2 zUoS$gp1|8U`JWZRA7|{3$^S_`m4b2~WmfES2=u-pf#~@xz`juLvEknaAh=~P9||Fb z5s_B^bDFX!#1;1Qs1xi&xlP&u-0#zMq?Og7g-|KSnp3cKH{xtTDjMO!fL%EGHxgp*WIB(OTHw1BApJjpuR!c1{Jnhzd*Ug zVLL^h38n55a*fz0h|lLzY%}gZioPJ&V!#Xq&ieFXs{#9Yp^CIY*yp<}CwO6s>!mG_ zyG&k-qGG;Yaa|i33ZJmsIQ+9C_-Er6kGB6+o)^OH=vOGGpK#yop6cH3a-x?69Hd&$ zF~mngkDm64t*ug0>uJg6YSTw@ZP-Fu4nek{bbZdX0b7n7Y&qtPwH&|1mgDGH%h8B! zLA-lIGwyqHX9pxfw>1-vBw$A8r+6}f)^6yu9Uu%CES#r7!LT7_zq0Cx{$GE0i!0QD%${Qfj$ zRhCuTaZcWWZv()#hudu;1Kb^UmvUUo%(~ud=eIQ5W95)Fj=KpheQMV+p{`g_*oK~X zsqzI^$a+kO7%p$v~ul2FI^-kL!8x_4sRqo^m z?YGjLntX}SX`*i9@V7398LZAVY*gTi&tNMtCE z*pN645j}ohSc0yW>u|Q`>5Kz{&8661SA1EDV5=V1#@k&L+I zhVr4I|FV4e0p)`U<-_X&kq|cr2!4t}MaxO%kUN=4Sq(X9z9jd?#=H?3{kQ(@D5DR` zX2?ScJ!R+sYI{^BOrx;>mJ_IQC%L~2i0=E*uRuK_b@Bo?2v~aNe$+vT{&6+x2Sok^ z(N>TE>gjjdd(;1nHs#xkpkv3;N9<|cI656<`omg`VGn9ClRg34bux>r4neN}io$

)_$!YpagI;0^8x$Ur(inFfFK=3tN%FzR=SyLo+RQ#sphoOLbSuYeF?HYLtow& z;SC|#nY?UE!=c5>?%y{&Z2#!gp~WYB$s>LLxUn(g)NpMp67=D*W%kYU0m%7->y#t?#U^@Zh8Hr*5IZ4O~<50-Lp_CC_qU^}Bm+>Z7QSP0%=%>yv+$B|~m z;N^Ro#@&N<#qBDL5w_zf*pg(O2yA7;Lv}@XTf-HF|Evi9v!d{)@?Nx4CrJdAdP7ad zLq-1fzCg@Uf_aQcBj$M!hnSaMqB72%Vc*Qd^Vs1}vF?TaGf@vG7>4b(+amEcbkNnI zTM%hA%w=Q-R;1ma(C2O|m$76jtgm}Pn6e~WnzCe;=Otkt{E3^*+3b_hGN4~_bHjF6 zQ~9!;IhpQ!m~X476()y$3czBCjPcvl80A;#C(?UHIK?y;Cn6p#`-iZHnd(Xn+xx(3 z4d?hKkVVEdk-rwYRon&aL;Mc+=OzOyFH&(=6#la#_|J~QU)Mna?NpQw_wEeG@qG?$ zv3{hFLac0vg_efOhTUPWfXnqW#ksOU55H_!*{}z%0Y;Q=JYAo%beW+XGQ;M{|s$n1I zZ*AB{@|RqGCh`-G?i-@U17M~RW%zJ!e`vB}E9#fzd@w|r$?;8$UU8U9Mw@i9cm&Re z$2Vcj>hJE16Q72U%PGs#TPct9qRysnx=P?dHq2Cjs^Wi$P z80EzRNyU-6Lh4PhW0~-`a7-NybVd+`*M;#1ivwQK4y+BEk%r*md3NR{d^N{wMeV#JJ$Djvgr zO2qc897%aGK(YTAkzq)R3A>Hc|K>#SpMzgKs{a$&I4Tp#_?(XTXzf7Dz2kEGO?-7U zPHkywE$wOL=?HzTancU#8<6pqQTa;Bad)U75pafSb|yw<%apxg+dwwj1`@-4hRvWT z+^>J1QC#!5dndCytPv{Rd64LDgf@LGUQG#KJv4Ha*pbz47Tld$7($JZYV~# zT60~{EoC)E3X=xU&W zoc%1Mgp&a~&Iwqff7#v2RENhCHn`K=|ASaY+wu}%15mDN{bw0Ucy|qYneBg14_W0h zi8-Mte4n|=b4-9)1|Rh}%(BZDa9l_!6V5@7DqQ0}yoE$Fc&71uBU#-6tSj9=^<>!$ z!~(sGaeDR&U~#(lU0LJ*1rhuUqVTVBQ;?5l)ljbmqdK9*488BF!R+GEMuRajJdiUP zeXEE~IMkaQGP|fY#KZPBTHpUVh6%3T;(j+kygXj>{V_nIbSM_YFr}XDLa9)SJ^Z5Z z-QNvY3f}P9q7X*o2|x48VXyGlK(K?vh#;{d{2js;CE#=)*KhQSYZ3i@MQiMO$X+26 z-x5XPJIu|VHw2h%@==e$%wqZc7MQ7&H$b)i9lUze2>#DDUTB2uIf#EAtaR5j{*(vq z4)cJQv3t;m#d1Z}|G5$T=SJag$2w4j^7R_T_o?A(L}-))s>e7W-B*P*mvF$|VXBS7 zML!S|TbB>AKODmaqr|%Z9Oi*zzPF=zVA}`}REF<&53dxk$3MbjE61Lo_EqBC9`b~n zhqnv=effT2nTK#tg#VTgUkzKsxgH(NZJ*J|GWtP^P=$o8teFO`;R`mr^=1C7t3}x zkK7RI?HI1v5~1+qVX2W~!H;cwC+{h6e~r;o^_b&GIQ@I1eIL?Z&0^~te6Js}7Rxp< z*Kv-ivac-iT23eq;HsHTtvy@|*4+8Oq2}>6NzFp=(f+H!i}_w2l&qgOtmJMRndebJ z;>(NHIvsj_Clns8@9Rt?-oK~7ny@Uv*&VpgqT{GMPWuF9fJMv|=KnW8g8%#|{Qrvh zfp_RlBszx}a}We>2ad8*NSfggKv!rHuCRi-L`OWk{U`)q?H}&_c#vpw2gC!;NED{i zlA)%mrnTmDjiGk0v#M5cG_UiKvM1L;Fdw6DKa5&;-A2lW5rixc5e?kgyn+0}8QkVw z&RwO@tSo?FLBGoSpcAcz<_Al06|w#Y#41-PhwjF;U!Y3zT0CXPp6_CSHrzydST{P{~*t zDf4#GT64RIq?oYVIQ{Rc2>w^$7mw|sRaX4$aLVp*0p%EqTqcy75)Tmeq$T#YD>zMt3P5X}uKln_*O6g)HM|LO?-SK}9t*8d_3S4g8O4aYHB*DeYUz@qkiTdz4K{ix!qYrR&RMoAZvI`a^UJ6(j*7j)8QAcm$5bPGFztXqLO*D zWxDjF6f2Q?hfbf9v}h;j>s(|pyJER^d$;dtSBfenVVnJ=?_2ZduH$qp*J}UJ_YK+t zFTFY1{*boFYo&3fu7W>Vi*`V-ieu0k&7hZJca4`#7rRu9lwOuzs>HS+R`#1Gi^r9E z336h5UQ4Q&qDCE{({GNX{eZI;%PFtHi~SOys+AaR#T*~XK33&KOgW~j7T?ZgR=}c z16%K8c)fTU*LbmWhT6*7mRdr8a($9xGR_w8TTUo3Z>}Z^mjto9=IemhT1%xE_d|+4 z1u`%e zl8if?4$o!dXbz5IHJ4~ESDfi|cNnBx=S*>?19=4YxR4YRb{mI3uC)l*;uYc-kJkSd zth=_dT=9@S<^)wZ#rPX7HSaeV3sU<*OUX;P)p zW?j;{u4Q<-VWQ?4!xdYc9Bz4VMU+ zE5&n?Mtsxp)56!tI*?@5N?k_?#|65Cq+`&bEhQT4O~XpGRA^+?^C%AXVg3objl+Lo z1b?LD82&gm-lsr~d*v|2hI%iAKEUyxPjJU^A@tN;Ya z3P^L|XevhQr!25fUAn@qc>c)}id7yZ`ErMW?GLz7W=z9pQl&~WB{C&lI9~WD{!X)! zQ72}nvcR~Ltl2qvq z7sfq{enD9f8N;Tg^#@X%&9}E{C!N^4*0+}Q6G8#qg0U_17|Mvabm=m=Eb1Vtt(?qVue`dCWF5Y?hDYZhV_uG)xdHJoG zV+x#K7Rj&u>`xr%c4B{)d<&Ybvs5_4`K|O)jx!9%$2F-}mi_;_2>#ba;jc!@x0J-c zQEYme)2UM86pk2Uj7pDDcy*o6Vf~$oW;I<$8Pq0tttXfsIAN$(-J7F3^Y}HXr$0!K zQ8=`Eeudq~)dMwmQa+obIxTZrUc$RkU+B~drAz}JEnN3nUiGQnHeZu z6rmYq6lxR*PVk#SdZt>=#MI!hRb(W0CtGH5K3)GqU=~a%W1WY!)%FZ-ihZ_R02Ya5 zUx6CNYmAkg(r)QJDYnL0v%BWKn%G)r?e5z5YGdo1b-U}{tBb97*6*%=uRit`=PkQ$ zdGD6kwa&G>*S@zlcAaw_r`2UCd`OB3yN#>=ShGm`zX-p0wEf?`?!9$S*S{Uez-T_7 zmp@(Ki5^(QCbY5`fYh5Z94sJwFca_9bVvfVGd7ma`z&X`cf5U@R(X=%$?XKCj>neQ zMLEmM6?-+E_*GT|8Lo?d3(w zi)w`>30YSb{wNCx{#+FP2?-h;%b6-?>bW9E5p_*MLcd%O>XTF+K?kE~`8X{H)HQJ* z9WZ?ntMsL)l8bvZ`r=^cDGe*4udV--20a5hA^VIEa(jHZMqmD)HJtWCO&0Ia!mP9M zsldWn*{9j-xG*LlQ<>j!X}kem=S=*`^p^rJ3G8g`X?DI6j&TXl!S!)h7iIC!1QyPY zKg}-U!s+C$K2FJN;T0~Pe=Gf~0GZVTndmPFLG=BLPl#~Vh1hykeF81tV)`ali7gXl zj!_X_IwF`7)@3YG^%w&`&0(3al^;KBT>USK;9nGlKV>zBDRdWCU;L|MJKBHQ{xe<^ ze=k2N{&wEm?&9Y&^ht^Z+m-F%QwfxlhpYsp5-^=gUW3K7* zl}GfGmFe;GnRP|f8(T5ga$bqKjPa|ntO`D5q^wLvr%a9Kn7X2w%uPkWmK3Sq*xBBU zIpX8%_>}ln{4Tt+n&dXeDC-%$(hwiR)E3>!+z`&qywOi@PDtj1bVGuU|AvlDQ21_* z@NZ$t(l+Cr@G3@lzZEoui>GfYS}V-sBYbRB9cRAKn2~$qPE!SHA6}op+c^ADcOv!Q z9)-WsN6lH5K*%R7v+H3f9j>cRs9ny+lrrz6JMGB^S3+^a-K1uDXhxM^vs;XldKIX)1ztVD`ID94cIaM2`O+QImkxV6- zOOgQB^Yuoa|HANS`vdKFAUgN*iV<{P8$%~wSrETMzolq}{ZJ9)(jOsTU}w6epe3eLUf~in0`Os*j2|>f^0;)EjZ9cn3qtsHHKQ zPC(nu$02VKbUF;U!kkZwD;2(#bX)@;qwa{al)o1g$Kt~M7xf!kxyKi9cJCa`i8!@J zc7P+cOQqwnCsIP?^#vTUyXYSR+04WUUzPtwf0=id7PILDR;DQ zqQa-5e^}yug;l1co+=XtA+ww?ppVCy$!5xO+(27*b9*=)Q*CFs@KbO-BA64bJDll?iND$v~cylW|7Oz*Rdj-aq8pa2BU= zO+q2@Dd}m?SreO+DpPZxwg2mr2keS7a}0&9ET*O8H+t$?nRT2>uAg82*L#{5_yRG_nwn^VsjNxQoo^7+trC%&JTW z6ISyC^b7xC5Ulym$XxL1o;ldP+l{L*EDce6H<%PS6MznO4P%AM&GOmA7QrTxHM}Ot zFx%?OUPk*f*F$ciOlE}i%Z2!#k`LRzM_}#G=hI=6#iuFWGl*rdJN>b5%*1a9+^)US=)X4(G7wMKAn>1 z9`3yZ@i&a%pWCBH{C6Tv|1z8|YJgxpS)NSr2_voh54fH4**k1^c_vk|Z9yRhT z$*;!I9wkOD+*gNDY;lBx@FlrU^1X}4DPpk0;KFgn3A~NNUyR@{M&aKgn8ZYgpl(=} zR$xM@3LS+wvjb%#M)sa8UMpotk?~cq^~!tez|6v>-h=@bbdF2C;@~#{783Dn;Q$rB z0t382uo{n4cND*bu|OPr$dkH`crm`r=!q9;Gx-m!Rl3ys^FG>4v6p&1eP_+A%RvwK zzCT2nN%Xijj4yMn33LR@-{BEB1?NXSGwknSp+o~lD*ha=;i{x;$QI|@(?lW~z28LH zR6cEv;?!;rEym=LT! z0?mFztLzfUGp%N&KEesAD=kah?>~!0+*3eN`Ix_dP~+D*sE%3Bc87WL+nj zk@|HAuoP{$401ola+hfI?jgAS5qakW+;tt8>bcZ=W52HR96l#mL=PYIJu}ESk8SJb z*L29qk-3LB1GKbKqTP{5bOvcF&R)Zw#suER;lCt;|B@*DVVWjPzpK#ip3Jfvz~~D{ z;QN3VY54{2gr?!S6>K64TEe-8 z2OulXM`63H$V7_)#(=B0lN{d^5xCsi1T4*G3wCXNNQBKbYeWqe||%Gknka0g~qdq;Xmp`2xiCtZ%rty003e z_mk*sVBR4lm6G1$6Lz}$r0>9D-zm~3yKNBX<62@+<9{-Mc$igK%PS>vrDpK)a36r+ z?n(iceM$oae_cmr8TuZXS>&X3_#BfD{nN0{Paaq2sZ1lO^B$w?M7uIFZtH9sd- z4*M!0Ehg$V4*z8l{FmVekKsRpeCiOPX4c^MflIv~4k@vP9}W#zz=|H4;n;66J(QW= zhik6x(>hp;4|l2e4?~}h)Y5-p{UjKT$_yX2GXy6U>SAQoXQiyfh@5&C6|Cwbdf2G! zmeDR~RPQ)ya$V|e9=g=~=%pvI#Q~P!G96c#qk>NlO)I_ByJdjr0jmcv{%e`l!MYjl zQm?9y^-$)a-s2$^JJjpP^+7GZOTF&!n?LoFH@A$v`J|CIuNl%}^biQgs+*|TlkXab ze|ZG|a=bQ*zikk5|J0LQN|{5ww+~TlR{$)D4nou6&`|Ha=wKOx2-FXJGg3#14)+G? zTm)~@Hgup&A$Sw)`*gs*HG(~9=T~{~d3Xdok~U;=sRE;Zu`=YUeQ#x^m8M~I2Hvam zx;s_5rtTCmsT89%RBSbYRq3Y+V!ia7R9k}l)#^{$A?HV5mzA1Z4{c!L&j*sx4xQ0= zI`CfR5%?6P#+kqe`22;~&x=7|UHClI3(&s&RhZwN#A|4Ev``r1;P(T%PVzgiL(HJE z;mY#=SRTQDc@+Lf{TrOgo`mFF$ea5_Hk^9VuXN+8sJ{R9Cz;bcW-cDnW3u7QQ~qK% zjp?uZmq+D3=-=U1VCqrKy(%jABmO_Sm6(3Szb`8HgMQ^E6{a4<+y|m^-{UXdq{j4L z_by?$J~k5xXTyHvgasczY#jb8Blxe3!XIsSNI4=aS7Q&VOW2Dt)QfAvte{WVhx^BD za(TobTa6=gnf>ur(~^+o#dRvy=osqVILu)jFnt+^We`j3j{YKyt4z%RiA#vaGr}Pujr=?1QQWNe;`ZBxjv;R(iS?W46?ut)+v5eYm!mYIgnC^?R)` z&OG?4JkmefC&i8LwgYtKK(>~FIaz%ER?Dk;qAFY?9 zUqAZZA9!|pVw=RK`^-l)6lbLW6qCv?gQt_yAb;uI=tMSgw{iHdir~KrKY6tNub|qQ zy>5ziapi7ct5wwrvO9JAM|XOK_kfyNAm@~#zZX?oyaU&7k%Vm$C+Kj_Re?Osc^u=j z*9t|&%cVt@MFq_~jjJzK16z8xo^p{=eNu)KqjE^gxs{yPvQt<=HQTegdEtoK#Cxsx z>F=YA>L2wczR_iplKp8veNJuUIWZW+;a2KSeWR2hUX*ZlK{!^O_VTl9y8^?#&kvpq zjPiyO`ND(kJK&M<2;mjf1%e`K0=ka-*mz($4!(#<2GARD(Txj1{Ad_2Z$!BKxk?cgYmNuf|q1aC(=(pH7@cucjSdF{7h zx}(*l{PKO_nIcvWj>i-+%2Eg32^NLp9XB0Z-uJI?{!lNj)niEzS15Hj8w+iJBXhCH zT%_2tLnE@}ramv8t)Mh$PpasPvDU34@(KLFjm`rib)4Yr?$~}Wi3UhUkM+ngW;nNv zDtLC=;X9|*JZUn{p@RRw{((~Vn)h0@&Rw*x97m#pUtyX`zM}BIA%g!6QTS6<>s@H$ z3l4@pM>{SR{AZ{a_37ktoEPXJb8N`!YsDDjl^ z^W34{tl=!YF9Ge7*q34d5O4}9t~y*#)Tr-)x?-?>FB@Iw0ucOm7)xZYDEx1X;D2Kj z{$Tr!?{jP~34R3Ep-C9Y1&<8=K7_|JC`)g}KAkBQ*i-dC^$qyNQ7ZU%^acW7(%BEi zzbrW@{6&m;sZE&W+PpqueNDxYn27cDm6|+s9>O){`!}zDb$vJkt9PHeDv|3Ce8 z9*mbnzn%5KgSVN!ZQ5z3T1NN_HFn-72c!yVbk#_}yyq%Zl}+ z71vqNT#_u-CIctsB*+|6{7rw4x+jtJ7Oy%>`i39*$++q}e%1qMO{g07npyPux?%Xv zz|N%wo*oT+v?;JlBzbNn^E%&b2y7ckza_9EoL&>i^8iX2a~`^6KtFC{W24=uuP4eKl+bI>&0`^In{ zuy~%DH{AQ$Fj@QK*6?#?hM%7ksK2t>|H=sdl~MTDMp1aGTpyZO7i8yB2-Xc*n_hMQa3SF*F}PQtOm0M(+%_VUUy-X_ zzr@wvewz9zroJ8^W&RCU_#kCI=Gzm9)ME&)8hY&2%Ttyu@}RB%#8dt*V^=$&=&!<*EIrFEquJV~y}~x0Li+M& zv28{QD+ATVkL&a1U-W2SXw&p0(fO%fwBcK0pNHW6gLho0!ZGG}XAh*fPBKIS+!@GN z-*KT!@T0Acv@@jbA?*xlZ)lC+=kW_KIID|$P&%mO z=fhI}rjw&G|Md&cls=0b_|hQZjEk<=7lSAPZ+{^b`d7fBe+~-AdjSqn8DLKk(N1M&v)_ zJ$Jc4g?aD42Y!3rzf%$0)lT?j&V@QhE9|cLl@R+Ju0J(cer2`)t0VZYj>7+`rcm!a zgS4fIEU3=bz_C^L%x_TuAj0KKt3gkqhQs zq27P=)Bm@#hj0vcal|Xjk9$6@RWyT3K7`rKN46KV=Pqo8D;N@5!VP?Ca&Xs-W==UzNjjh=JcfNBH!!w5QGLp*`*8e%i|@H2@`GiT6N% zwr-ftjK=*4{_kE9|9eC5U(L7N1J3MhQjgpd_p7m2-{ZAR^f6KVq`M^Vaj5rE!#MqSfU+f;H?`3O68cu2ZRM7I! zwx{1As}h@>eqS+s31a;B7;Is6=num39!B$ic}7az_`>mH^ig{FjbSJUKfxAGoCcbU z#rMEY^to8W2g=CVvbykRWR6oDX??XLy#8SuQ#%Hc=LT#{U5?GKeAtHd31M9D2>v8R=2%tG9_MgM>+Ukj>*8!qu0(i?Kf&GEN(5EUa^s4n3*etyTEBN; zM4GfsTE7!^*fkw;$1vypeRCMbvER3N;Dn7YVCpc+Q=qqxIXOn)m!9B~TUl40k&d}g zZau7nj^1?xB40PoMS1sC{E)9}popj3dn>s2uLmgaJ{wYlR-9$(>#db;rEace8=`lM zU-CtkX3&Fw_DaZ4?sVfMi&D4E;8)TS|4#QQ%ZrHjG>51Oo{eV}3`C@n*5lnAk2h$3 zwL1|~2I!lQ;%x-}_lfx57lJ>No(5aasE$*%7G=A$D7Vc9e}KXZ-Av{;)@CQ;sOK)Y z?0goHAU4lLtP+%5mOB>a#@SAw7cBKucO#YtXmw72?%(3hAq&yg)YG%vSc_<(E?1a? zsrwZ2h2gPZ0bY*sZ^L}uUEupD1T+RHjR2a*M9@^$eoV>ulZ1Mo#r*304Op zTwOMJ3K(~agFPDGU{iCg0#=OQq0b(z+X(zkBK{`4@UZ-U58^z%$RCA{)R>Sq>uG18 z-8U=~B3eX;JmP1GEN_P3r6GqNPX7G|V)mIKdAz>~`F4)tAq{ur_Y>-S21d!(LZ zWx>;Pfo#!u3zb{P57K(6yo!E5{C2sFYsy(0cO*sYO=Rrie@3DyM(s8Nf3t|c8E-s{ zf0A;IDkHlG5}xzm0w4)a>9IvFnq?ri9>`MNn1e657;P73`z~vjO`1vfKVUr5_NM*A z+870CZ-h1@po>@2Pc-H`iSCTy%(ygi(L}Uom=h+y??Xi7Y2v?wY*CaE@8Kseu{T!m z^u8F3^&#IzXbqo$NB@fzKb9XYPeS}IUHm&7Y0HT};5<=5rcQtzQ6g*vNP4D_6R)3< z*m}U3W!sTHsg-DMD#!`YfI5F<2ho%TpHH-l_%nhIehuhb@m}&ct6oKn7UxmIgI(-#R)`b3nO1fo{6mCVlnc`G$Qn# zo`u;%iV;s^1l0HH5S!J2h>}uG24s5-`)|*V^sv^0pf5KIQ%HI~Y)SeWEa$54Ru!tc zs%o9*Dh`?IxQW92X%pK1Tzyy_9&dN*)26rTZ0|XDq)$c9IKpKM3FsZPewqKnK=fBR zo*)T4fqF5%uJA7r@h=I%|LS-p7>jhgc!!+8{|B9YKTkeeG>PlqqW__i)iEh{F>aCRCFfSYl-)Tfs;0QVuT6^WNUC9=u(Vjw*yX;JA1XhVz^1x_@e5QePAnWtFcH1J8bF9&}XjO?V3D!Ij@CrY!;|P2b zQ2zeBvcz`GnUww?K@RVuGSosPm@@WB)n>^xdoQ{?{iSMb0@fCGm*&S%*&pa1E zHT^#0V{8ywRapeNb@O)=>vk_oUD ziq#XAO`qVJv81FtwLBB@njAwt7v(CkY131M@K!ecCP4!nnaQ^a45*Ke#tN9M7o?MY zE*za1l21qDy-V$UKfd!4Nex%( zs&~?v_p(x<+sB{GoeXZBct7o5iRY?>hs*uxHwt9>MB#|47O`EW9n7qic`-OY1rq;W z7cIL7^Up#`q+kS8;OcG13M*i(R zP36hdzJS&--+~t&{{EXCppyk?MURV2?-5Rfb-9w}9kS`c zJsAd*i7c^QAUbpk{uR*A;=!|?Q?}h)8+>#K(7d}el)9LteeGgFYtv9itB_2RZY3-& zn*?eg2>XD$M>z+404=k(?-iRgF-?F45$H$`O1$cN*K<3sz&+pNo+zAQTt{sOJQU9a z;2GWa2W>6U!@Br;TLCn9HG@ZOd+_8t1|A3ZK<|;^%##65!auP0O2539+PYxNMZbiO zD?8*n4sG8i_%0qooFmv5+N_Q3;V>2$#oGw{%S8Ok@Xo{dk0oD^Bk4D}q_(1P<}CG? zz-Tf^KJ2=SmuYo2COOuivr0Nh@^>DR4D#t63l5UTUy31&Mus~n zUzmh@i^9Lby|Q+CJb0P%ZL~gFpq4t+@w11dcs+)4Nja6+7s*%0d=QY(Niedd>F^uE zQkzrjLdoQKpdC^PqIR{bc^)tmQ-+E*y zSDd>H9zUC$)OLmXv_oJUe~r@G_zV}`Jc{8T$^Wer@n08$|BjT!Hu+SU5N(T8)o{J7 z*3;SKY1>rz^OdR2N0zJ<&;Qu^Gulqn14pcSM2btjUYTxx3vzR{%uqoMug zFPGV}t49Z*xuG*{lNCjumnIm9uESDN@v^?nAW>4W7ZJ26CVRPowsTb)OQEbVdbuHu zoKar(J$~6-%_JM+9nOPQ`?#@)zlD%3D9e_W1d;!FkWw3xTvF|D>PaPcq5a<;EX=5D zl(o)}Jdw)Xc)~wDH0CUisI#EdW!3y1zNzu*ue>m*sV=F0xvIwbf^#32-*RW%8%{Fa zD7?wZo8RVEHaDxDbKXWuD-8Vcyv415a=wP&dp_oWw}S3mwh(@+n1|$ zs~paU$Ug3dHpex%J~5admQ|& z0>Cp0kcW#ogStL;arf z2mFTX3ja+a{+mMa=U?GZ!RPxvjK^!^qz<5E_j``S^;AtF8)X}#R~r7tAI?1h?~8XE zc7r}FKm>=&zVU@oc4))oDM?71|$LhC1&ENIB!k>iSfG0g4 zcrd49d|w^ie;f$q7OLQ3jB*IW+<_@@m5%MA0Vd#Eiq2I{me9Fs(=|2xeqe{DtNc8p z(sx)U8#=d$lwsbLdS;^@E4~8|hh7tbXq49#{+mVoH;3R)+aRJK0x|!n^-VZpHX+Uz ziM5PD1i*m(Uq^_&O_NM7`re>l`IVpya@bQ1`aXk%))b*3cGaNIGi1PwV$TgHa*duJ z^nE*&qps&!V+C);_5ECSYa;9hKOf4AFQWdxbX{w;7FpJEeONGlrw*|Im-Ag_Bd;rl zz6iTCUD~!RP}AE(2X$J~vCOXP*!)Olo_?B)>i5m*GhV z@FeO7Y(D&l*q`4^^tMMC1)@OhR4?aNF9^TBj^GNAPy#@ zlKaY4!l!S!mWG)(G8(4Vt}R(#OJccxfvMMRB9q+$tW@C#?!UsB&#$ZTzeU7G3SCQq+kH-@;Tsnn_o znYDz;dhU(`+|R8gd;xzq9}B%uX7BQ4{k}GQ$6q@R$rMGqikSFk^b<)n&v4Uv>GT^9*=${#+Hxx0#$r| z?is`cn@zIGSF@Qtw^%;OuH;!uQ_@cSiceTQhGkt>)<*{Yi$@bS=vE4v6|1ndGPSGRv!m+lkDy zLD>yCQVRL#X3FzVvV4_2+jO7hLDKixPn z35_cq&Qku3MwVWt|4b{uP(u?rUSP7uaD6V7Wn6(`agDOZpykH7WLYY@ z6f$bJ5##@U5&!$~#>4XeF$H34L)l+T+My-G36-={tlI;#_>C0MDZh1SU6G8 zJWYF%cyrl?(lzY%&8&G1c4Zq&w?d7aC^)2LNE4AHNESR(=7+PjV%>N$k$Y*$W_Fvk zY}1C#CF{4b=B=fy`2o~qF5Up86kNN1Tj^FdzwnL`*A|(#u6Zy}euH`IMmBj3Yu>Q6 z)V!vgE!kkk)2U8V)RZYx*kucr%X{u#*6oIYzh-CN5_sAzz!Ks#+L-M+QVmR)EqTNXHb zySbdr%3w1yGP6h+DO`5SKktob)JOB3JF9kF0?stw;=P~y5&0N2{>lY~E7)z@*OWov zG@Rdj|Mn~SvQ6v^dYzu5_bz|^(xL6BGXnWq{EmOZU$G16Yoti%8OVdW1}m3yVzpOq zzjFWUxWCZ6&01W#b!$1hj4pK&@eL67YBTK&hFk;A6_#!*rSsru-bUaLTU(014KF>6 zf23pukut>oXv7#Ikw^%k4)hpgWO78|VB|>*VPr`Xi99KcFyZnFX#}v4F$80qkZ`#~ zqNlr^bQdYZ?S8_@;VDM)lZ4UNOh+Mxk)Zy-_BpWWMxws@e#E{PY2_x1dD9w7N-*l# z3|q1-ij-RE_)gx&K4=CV2oCcQI$BSN{dj2`VXPD+|&lBY%Ng?+j)cT zTeoih{Z{iv%njxes-8$*l@P^2q-T+`A4Cja?Dg0$rg@}ikv>9_5@>f=V2QG8>l+gI zfEB%s$h7r$k||@_$>ed+9f&fCKA(wJZQ2EoBZ||4c0$+$3&X0ZC}h-bBk+Gn#Q!0@ z@i6`uz}Nj0kn?e-glw(`-4kM>#p4T#&`>jG(l_AmhYJ2Zqr&) zw}g6yVIX}H5Th&3qT8Yqp9=J-&u)gL@?23DA%=ijc@GWsYs1JmLUhnI(O~BA+K0q= z6I0v8ItXHjj|u*&8vg&-iuHEz)a{)4yk37_msvNWadaTy|Ypet1EVJl^uj zGFes~*YBeK;i-<+x0oN^x-sxusUKG(lO@J)z&Tb9PtWiaNS5oHpcfXamtg&Zu2V$Y z8GZB~MlZ$nCR~R~Y|JK683;n+L{A6mJ(4^6lh*SxiFTa)SUYDR( z!do1f8Y#ra#lRv9v6yVkyfJGT-G06^7fUSDqFu2DlK!2`?|aSPgj$~UUzPWgDHnt> zo0z#-)>y*atb;EBGOa}*Q(VGD^sRh+W#+9y%reTf>Ea6%jdi9LQ(OVj(7n1qy-3{C z|Ehwk|CNEf`2VIrUi^P!ATR#E0r^o3|H$$GsEGfgA^5kLXe+dsCI2|V+C3PFuD;OcS|5Is6 zL=kz>;7}GZ%X5`jT_oN&V0RMjVzS>fbUX1iJHzb?zsA212Z?`kjr5oUR{sRvUokJ# z5wHAmxNf}_t3~zcTj*TDXpUT8_&+A%|5ynA>$ec^n*(~ZY0>~)?+N%+%oxAS3at0) z&S0hV>?*r%Dse~@De%;;PvFXh|7rZk;`(*}fSteEl%wmqhkEPyrQ_jXj_EuV%#H0F z2xmP>?9Lzn}@lr7NO(&Ti;W`AL^3X_i*)<0X_GD2%kj!7NtMR`>#D7N!{(Fqk zu++&Q`!^8p{ew-qHF5`86SLivL9*YbEMZ+{YwvGcn`0V;_e- zB*2`^^(y=?z1Ar^4xQt_ioYV>K)jiQ2T>b+I%E>w@B8iA?Zyd=y42$dEj4KnwOuLo zuYQGgy-{5Xi?Wn>LyQ-~e|rf2ufu=F--fcu9?$PER;VwBHJ+cqKb^aWdi7e-xm+AULYe0-@w#wpXNl*Q zEcZRsJCn>K$qJNo`Q1dzl5;EA4C0;H-|MzvwA0#p-H&Jw-2**g_}c&3O>``2Sb@&v zoz(w>`>kM^AKdR;Dbo`ya}Z@>`oD4a2FrZo9=uYfHCUztWd{0O?m1bMw$P`!+<(#* zlHuol>}J6;XfhgB_)*f`_rAOQO3C-#?*>YaR_y6}jljP`#J>X9hQI$rWKIQI${cJd zSKkTu$tx{&!u?H1o4y(-c_>)&oxYdg!!0mp=;eAkXIPMOAP0IF=@ilcQjQeINT-kn zkiG$bPcX+Q2rhz0cpK;Xk@~JYbBB8<GdqLN+}US!-yZC%R`(ylu?*u~hJMko zq6)pY1J=Wv`poX!U^%n594t4({a#49v_QGHg5?tXST`Fi z$GSfYDW?jQ`zTm0s&BmereL`^w;yLkio=59oLdo}L$U7d#XVP!F7!sDJoOnh*4^ON zAc9Gm9zL%7=c2nkg!DGu(>{m&Xx>KPUn$~W8G`@Z;Mk+%(r$xHwo$UXvPK4J%8da_ zi^O<)@*7S^+&P=Wd7d=vglCW`5@ApBF=m#5(G=*QVc5^;Vv!26OgO_l~t z>@Le&%zi$Tm~u5`uodsul|jo~w>B5EzahokP}2>5v(iE}Iujrb!d=m@<&__cA?cB> zu9_c|T|0HVkHTUDqk1Q`73i{5qrNGGEi>m5Z&-h@Oj(giXZ{{_QQgu?#F>h-_HO8m-@B+s=c+}2??mYXUwmx8ir8wO&==akY ztCao|UF8D?vozov1ncp@mJ#AD8aoS-l!Ld(D*&oUIFPsEy7Ff@MOo6ucsz&rmWG}Msv9|bhUsNF`4|HnoA zAIBRH(|<-oM}^FPP$w^FmuKqb@TBJs;t#prYe?T(8y|;%eKIXp@cVA}({^KAbUe~K zH~Qs?u>8`IWZi?8JoJpy?}g9qVf3Uy^d|K+@vjH5^EMh_2mbhR8X@TZ4!)IsKUQ`~ z(^tBdEm>ATlCnTih^w|^$x_$?XLmdIa}!$|xW2YImSXZ0KS9+@igT&dsIZJo`L~c} zE3@o@7x*lAf#1*lcN=F^+X}-Mo|S3THs)}RL2V@(d|B!hy2mtZE*86K8U~>3$hzr!)7pON1^(uc=$JeNrVaFpPRfS=^b-ZmKEC$D0 zWt!#ISlezxtTp;q<^O^5M9XA0!y0Iytpfcv)L%XIQAK@@Wnf-VBt{7_q(&*xbXgG1 zSV*@qvl>iCxa$i4Cq?|948h;$>arAt%YaCOg|^IqOCh$-a9)o5W#khgu-1$G*JFv|8RVbAxwFt8f`>T;gY$Bn%Ns`&|3Dqn#uLSJ zNcT=4id#|N86*S=Q!GGzm*u#Q{rQ+cJV}2Te$8(gY|`Hv%Fff{(A6D!EJtF3(En-?|7yJLF#Q)_oQ293p|Mr{6!$UW$t(XOk)v(@f7=vS3eZ*_-a=bCv`0ZH z3>(w9$imG015XxO`N&(C{Hle_>8EvwFR7r@2x2*c*BeBfXPT#5H03XY8-3m~5SGPA zE07A2ZeNaMAY^$qAqFI-0Pl?LcI^9tdD;mYZx_!8s~VNN5%|}L_}AcVhw)EFI~-da zYzwr@F6dOOL8?lI-;CcFZ2{!;GJT-Y?7@Ao1+iYID4j #include #include From 573fc54213c202f92948591d5160ce43903ba779 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Tue, 12 Sep 2023 11:16:45 +0200 Subject: [PATCH 04/33] Add first sensor --- Telemetrix4RpiPico.cpp | 2029 ++++++++--------- ...etrix4RpiPico.h => Telemetrix4RpiPico.hpp} | 180 +- 2 files changed, 1108 insertions(+), 1101 deletions(-) rename include/{Telemetrix4RpiPico.h => Telemetrix4RpiPico.hpp} (77%) diff --git a/Telemetrix4RpiPico.cpp b/Telemetrix4RpiPico.cpp index 98677b4..ba8ecf5 100644 --- a/Telemetrix4RpiPico.cpp +++ b/Telemetrix4RpiPico.cpp @@ -20,13 +20,15 @@ * This file contains modifications of the work of others to support some * of the project's features. * - * Neopixel support: https://github.com/raspberrypi/pico-examples/tree/master/pio/ws2812 + * Neopixel support: + *https://github.com/raspberrypi/pico-examples/tree/master/pio/ws2812 * - * DHT sensor support: https://github.com/raspberrypi/pico-examples/tree/master/gpio/dht_sensor + * DHT sensor support: + *https://github.com/raspberrypi/pico-examples/tree/master/gpio/dht_sensor * *************************************************************************/ -#include "include/Telemetrix4RpiPico.h" +#include "include/Telemetrix4RpiPico.hpp" // #include "pico/stdio_uart.h" /******************************************************************* @@ -73,16 +75,12 @@ uint actual_number_of_pixels; // scan delay uint8_t scan_delay = 10; -static inline void put_pixel(uint32_t pixel_grb) -{ - pio_sm_put_blocking(pio0, 0, pixel_grb << 8u); +static inline void put_pixel(uint32_t pixel_grb) { + pio_sm_put_blocking(pio0, 0, pixel_grb << 8u); } -static inline uint32_t urgb_u32(uint8_t r, uint8_t g, uint8_t b) -{ - return ((uint32_t)(r) << 8) | - ((uint32_t)(g) << 16) | - (uint32_t)(b); +static inline uint32_t urgb_u32(uint8_t r, uint8_t g, uint8_t b) { + return ((uint32_t)(r) << 8) | ((uint32_t)(g) << 16) | (uint32_t)(b); } // PWM values @@ -104,7 +102,8 @@ int loop_back_report_message[] = {2, (int)SERIAL_LOOP_BACK, 0}; uint debug_info_report_message[] = {4, DEBUG_PRINT, 0, 0, 0}; // buffer to hold firmware version info -int firmware_report_message[] = {3, FIRMWARE_REPORT, FIRMWARE_MAJOR, FIRMWARE_MINOR}; +int firmware_report_message[] = {3, FIRMWARE_REPORT, FIRMWARE_MAJOR, + FIRMWARE_MINOR}; // buffer to hold i2c report data int i2c_report_message[64]; @@ -113,8 +112,8 @@ int i2c_report_message[64]; int spi_report_message[64]; // get_pico_unique_id report buffer -int unique_id_report_report_message[] = {9, REPORT_PICO_UNIQUE_ID, - 0, 0, 0, 0, 0, 0, 0, 0}; +int unique_id_report_report_message[] = { + 9, REPORT_PICO_UNIQUE_ID, 0, 0, 0, 0, 0, 0, 0, 0}; // digital input report buffer int digital_input_report_message[] = {3, DIGITAL_REPORT, 0, 0}; @@ -122,17 +121,12 @@ int digital_input_report_message[] = {3, DIGITAL_REPORT, 0, 0}; int analog_input_report_message[] = {4, ANALOG_REPORT, 0, 0, 0}; // sonar report message -int sonar_report_message[] = {4, SONAR_DISTANCE, SONAR_TRIG_PIN, M_WHOLE_VALUE, CM_WHOLE_VALUE}; +int sonar_report_message[] = {4, SONAR_DISTANCE, SONAR_TRIG_PIN, M_WHOLE_VALUE, + CM_WHOLE_VALUE}; // dht report message int dht_report_message[] = { - 6, - DHT_REPORT, - 0, - 0, - 0, - 0, - 0, + 6, DHT_REPORT, 0, 0, 0, 0, 0, }; /***************************************************************** @@ -144,39 +138,38 @@ int dht_report_message[] = { The command_func is a pointer the command's function. ****************************************************************/ // An array of pointers to the command functions -command_descriptor command_table[] = - { - {&serial_loopback}, - {&set_pin_mode}, - {&digital_write}, - {&pwm_write}, - {&modify_reporting}, - {&get_firmware_version}, - {&get_pico_unique_id}, - {&servo_attach}, - {&servo_write}, - {&servo_detach}, - {&i2c_begin}, - {&i2c_read}, - {&i2c_write}, - {&sonar_new}, - {&dht_new}, - {&stop_all_reports}, - {&enable_all_reports}, - {&reset_data}, - {&reset_board}, - {&init_neo_pixels}, - {&show_neo_pixels}, - {&set_neo_pixel}, - {&clear_all_neo_pixels}, - {&fill_neo_pixels}, - {&init_spi}, - {&write_blocking_spi}, - {&read_blocking_spi}, - {&set_format_spi}, - {&spi_cs_control}, - {&set_scan_delay}, - {&encoder_new}}; +command_descriptor command_table[] = {{&serial_loopback}, + {&set_pin_mode}, + {&digital_write}, + {&pwm_write}, + {&modify_reporting}, + {&get_firmware_version}, + {&get_pico_unique_id}, + {&servo_attach}, + {&servo_write}, + {&servo_detach}, + {&i2c_begin}, + {&i2c_read}, + {&i2c_write}, + {&sonar_new}, + {&dht_new}, + {&stop_all_reports}, + {&enable_all_reports}, + {&reset_data}, + {&reset_board}, + {&init_neo_pixels}, + {&show_neo_pixels}, + {&set_neo_pixel}, + {&clear_all_neo_pixels}, + {&fill_neo_pixels}, + {&init_spi}, + {&write_blocking_spi}, + {&read_blocking_spi}, + {&set_format_spi}, + {&spi_cs_control}, + {&set_scan_delay}, + {&encoder_new}, + {&sensor_new}}; /*************************************************************************** * DEBUGGING FUNCTIONS @@ -185,11 +178,10 @@ command_descriptor command_table[] = /************************************************************ * Loop back the received character */ -void serial_loopback() -{ - loop_back_report_message[LOOP_BACK_DATA] = command_buffer[DATA_TO_LOOP_BACK]; - serial_write(loop_back_report_message, - sizeof(loop_back_report_message) / sizeof(int)); +void serial_loopback() { + loop_back_report_message[LOOP_BACK_DATA] = command_buffer[DATA_TO_LOOP_BACK]; + serial_write(loop_back_report_message, + sizeof(loop_back_report_message) / sizeof(int)); } /****************************************************************** @@ -198,13 +190,12 @@ void serial_loopback() * @param value: 16 bit value */ // A method to send debug data across the serial link -void send_debug_info(uint id, uint value) -{ - debug_info_report_message[DEBUG_ID] = id; - debug_info_report_message[DEBUG_VALUE_HIGH_BYTE] = (value & 0xff00) >> 8; - debug_info_report_message[DEBUG_VALUE_LOW_BYTE] = value & 0x00ff; - serial_write((int *)debug_info_report_message, - sizeof(debug_info_report_message) / sizeof(int)); +void send_debug_info(uint id, uint value) { + debug_info_report_message[DEBUG_ID] = id; + debug_info_report_message[DEBUG_VALUE_HIGH_BYTE] = (value & 0xff00) >> 8; + debug_info_report_message[DEBUG_VALUE_LOW_BYTE] = value & 0x00ff; + serial_write((int *)debug_info_report_message, + sizeof(debug_info_report_message) / sizeof(int)); } /************************************************************ @@ -212,15 +203,13 @@ void send_debug_info(uint id, uint value) * @param blinks - number of blinks * @param delay - delay in milliseconds */ -void led_debug(int blinks, uint delay) -{ - for (int i = 0; i < blinks; i++) - { - gpio_put(LED_PIN, 1); - sleep_ms(delay); - gpio_put(LED_PIN, 0); - sleep_ms(delay); - } +void led_debug(int blinks, uint delay) { + for (int i = 0; i < blinks; i++) { + gpio_put(LED_PIN, 1); + sleep_ms(delay); + gpio_put(LED_PIN, 0); + sleep_ms(delay); + } } /******************************************************************************* @@ -230,791 +219,697 @@ void led_debug(int blinks, uint delay) /************************************************************************ * Set a Pins mode */ -void set_pin_mode() -{ - uint pin; - uint mode; - pin = command_buffer[SET_PIN_MODE_GPIO_PIN]; - mode = command_buffer[SET_PIN_MODE_MODE_TYPE]; - - switch (mode) - { - case DIGITAL_INPUT: - case DIGITAL_INPUT_PULL_UP: - case DIGITAL_INPUT_PULL_DOWN: - the_digital_pins[pin].pin_mode = mode; - the_digital_pins[pin].reporting_enabled = command_buffer[SET_PIN_MODE_DIGITAL_IN_REPORTING_STATE]; - gpio_init(pin); - gpio_set_dir(pin, GPIO_IN); - if (mode == DIGITAL_INPUT_PULL_UP) - { - gpio_pull_up(pin); - } - if (mode == DIGITAL_INPUT_PULL_DOWN) - { - gpio_pull_down(pin); - } - break; - case DIGITAL_OUTPUT: - the_digital_pins[pin].pin_mode = mode; - gpio_init(pin); - gpio_set_dir(pin, GPIO_OUT); - break; - case PWM_OUTPUT: { - /* Here we will set the operating frequency to be 50 hz to - simplify support PWM as well as servo support. - */ - the_digital_pins[pin].pin_mode = mode; - - const uint32_t f_hz = 50; // frequency in hz. - - uint slice_num = pwm_gpio_to_slice_num(pin); // get PWM slice for the pin - - // set frequency - // determine top given Hz using the free-running clock - uint32_t f_sys = clock_get_hz(clk_sys); - float divider = (float)(f_sys / 1000000UL); // run the pwm clock at 1MHz - pwm_set_clkdiv(slice_num, divider); // pwm clock should now be running at 1MHz - top = 1000000UL / f_hz - 1; // calculate the TOP value - pwm_set_wrap(slice_num, (uint16_t)top); - - // set the current level to 0 - pwm_set_gpio_level(pin, 0); - - pwm_set_enabled(slice_num, true); // let's go! - gpio_set_function(pin, GPIO_FUNC_PWM); - break; +void set_pin_mode() { + uint pin; + uint mode; + pin = command_buffer[SET_PIN_MODE_GPIO_PIN]; + mode = command_buffer[SET_PIN_MODE_MODE_TYPE]; + + switch (mode) { + case DIGITAL_INPUT: + case DIGITAL_INPUT_PULL_UP: + case DIGITAL_INPUT_PULL_DOWN: + the_digital_pins[pin].pin_mode = mode; + the_digital_pins[pin].reporting_enabled = + command_buffer[SET_PIN_MODE_DIGITAL_IN_REPORTING_STATE]; + gpio_init(pin); + gpio_set_dir(pin, GPIO_IN); + if (mode == DIGITAL_INPUT_PULL_UP) { + gpio_pull_up(pin); } - case ANALOG_INPUT: - // if the temp sensor was selected, then turn it on - if (pin == ADC_TEMPERATURE_REGISTER) - { - adc_set_temp_sensor_enabled(true); - } - the_analog_pins[pin].reporting_enabled = command_buffer[SET_PIN_MODE_ANALOG_IN_REPORTING_STATE]; - // save the differential value - the_analog_pins[pin].differential = - (int)((command_buffer[SET_PIN_MODE_ANALOG_DIFF_HIGH] << 8) + - command_buffer[SET_PIN_MODE_ANALOG_DIFF_LOW]); - break; - default: - break; + if (mode == DIGITAL_INPUT_PULL_DOWN) { + gpio_pull_down(pin); + } + break; + case DIGITAL_OUTPUT: + the_digital_pins[pin].pin_mode = mode; + gpio_init(pin); + gpio_set_dir(pin, GPIO_OUT); + break; + case PWM_OUTPUT: { + /* Here we will set the operating frequency to be 50 hz to + simplify support PWM as well as servo support. + */ + the_digital_pins[pin].pin_mode = mode; + + const uint32_t f_hz = 50; // frequency in hz. + + uint slice_num = pwm_gpio_to_slice_num(pin); // get PWM slice for the pin + + // set frequency + // determine top given Hz using the free-running clock + uint32_t f_sys = clock_get_hz(clk_sys); + float divider = (float)(f_sys / 1000000UL); // run the pwm clock at 1MHz + pwm_set_clkdiv(slice_num, + divider); // pwm clock should now be running at 1MHz + top = 1000000UL / f_hz - 1; // calculate the TOP value + pwm_set_wrap(slice_num, (uint16_t)top); + + // set the current level to 0 + pwm_set_gpio_level(pin, 0); + + pwm_set_enabled(slice_num, true); // let's go! + gpio_set_function(pin, GPIO_FUNC_PWM); + break; + } + case ANALOG_INPUT: + // if the temp sensor was selected, then turn it on + if (pin == ADC_TEMPERATURE_REGISTER) { + adc_set_temp_sensor_enabled(true); } + the_analog_pins[pin].reporting_enabled = + command_buffer[SET_PIN_MODE_ANALOG_IN_REPORTING_STATE]; + // save the differential value + the_analog_pins[pin].differential = + (int)((command_buffer[SET_PIN_MODE_ANALOG_DIFF_HIGH] << 8) + + command_buffer[SET_PIN_MODE_ANALOG_DIFF_LOW]); + break; + default: + break; + } } /********************************************************** * Set a digital output pin's value */ -void digital_write() -{ - uint pin; - uint value; - pin = command_buffer[DIGITAL_WRITE_GPIO_PIN]; - value = command_buffer[DIGITAL_WRITE_VALUE]; - gpio_put(pin, (bool)value); +void digital_write() { + uint pin; + uint value; + pin = command_buffer[DIGITAL_WRITE_GPIO_PIN]; + value = command_buffer[DIGITAL_WRITE_VALUE]; + gpio_put(pin, (bool)value); } /********************************************** * Set A PWM Pin's value */ -void pwm_write() -{ - uint pin; - uint16_t value; +void pwm_write() { + uint pin; + uint16_t value; - pin = command_buffer[PWM_WRITE_GPIO_PIN]; + pin = command_buffer[PWM_WRITE_GPIO_PIN]; - value = (command_buffer[SET_PIN_MODE_PWM_HIGH_VALUE] << 8) + - command_buffer[SET_PIN_MODE_PWM_LOW_VALUE]; - pwm_set_gpio_level(pin, value); + value = (command_buffer[SET_PIN_MODE_PWM_HIGH_VALUE] << 8) + + command_buffer[SET_PIN_MODE_PWM_LOW_VALUE]; + pwm_set_gpio_level(pin, value); } /*************************************************** * Control reporting */ -void modify_reporting() -{ - int pin = command_buffer[MODIFY_REPORTING_PIN]; +void modify_reporting() { + int pin = command_buffer[MODIFY_REPORTING_PIN]; - switch (command_buffer[MODIFY_REPORTING_TYPE]) - { - case REPORTING_DISABLE_ALL: - for (int i = 0; i < MAX_DIGITAL_PINS_SUPPORTED; i++) - { - the_digital_pins[i].reporting_enabled = false; - } - for (int i = 0; i < MAX_ANALOG_PINS_SUPPORTED; i++) - { - the_analog_pins[i].reporting_enabled = false; - } - break; - case REPORTING_ANALOG_ENABLE: - the_analog_pins[pin].reporting_enabled = true; - break; - case REPORTING_ANALOG_DISABLE: - the_analog_pins[pin].reporting_enabled = false; - break; - case REPORTING_DIGITAL_ENABLE: - if (the_digital_pins[pin].pin_mode != PIN_MODE_NOT_SET) - { - the_digital_pins[pin].reporting_enabled = true; - } - break; - case REPORTING_DIGITAL_DISABLE: - if (the_digital_pins[pin].pin_mode != PIN_MODE_NOT_SET) - { - the_digital_pins[pin].reporting_enabled = false; - } - break; - default: - break; + switch (command_buffer[MODIFY_REPORTING_TYPE]) { + case REPORTING_DISABLE_ALL: + for (int i = 0; i < MAX_DIGITAL_PINS_SUPPORTED; i++) { + the_digital_pins[i].reporting_enabled = false; + } + for (int i = 0; i < MAX_ANALOG_PINS_SUPPORTED; i++) { + the_analog_pins[i].reporting_enabled = false; + } + break; + case REPORTING_ANALOG_ENABLE: + the_analog_pins[pin].reporting_enabled = true; + break; + case REPORTING_ANALOG_DISABLE: + the_analog_pins[pin].reporting_enabled = false; + break; + case REPORTING_DIGITAL_ENABLE: + if (the_digital_pins[pin].pin_mode != PIN_MODE_NOT_SET) { + the_digital_pins[pin].reporting_enabled = true; + } + break; + case REPORTING_DIGITAL_DISABLE: + if (the_digital_pins[pin].pin_mode != PIN_MODE_NOT_SET) { + the_digital_pins[pin].reporting_enabled = false; } + break; + default: + break; + } } /*********************************************************************** * Retrieve the current firmware version */ -void get_firmware_version() -{ - serial_write(firmware_report_message, - sizeof(firmware_report_message) / sizeof(int)); +void get_firmware_version() { + serial_write(firmware_report_message, + sizeof(firmware_report_message) / sizeof(int)); } /************************************************************** * Retrieve the Pico's Unique ID */ -void get_pico_unique_id() -{ - // get the unique id - pico_unique_board_id_t board_id; - pico_get_unique_board_id(&board_id); - - unique_id_report_report_message[2] = (board_id.id[0]); - unique_id_report_report_message[3] = (board_id.id[1]); - unique_id_report_report_message[4] = (board_id.id[2]); - unique_id_report_report_message[5] = (board_id.id[3]); - unique_id_report_report_message[6] = (board_id.id[4]); - unique_id_report_report_message[7] = (board_id.id[5]); - - serial_write(unique_id_report_report_message, - sizeof(unique_id_report_report_message) / sizeof(int)); +void get_pico_unique_id() { + // get the unique id + pico_unique_board_id_t board_id; + pico_get_unique_board_id(&board_id); + + unique_id_report_report_message[2] = (board_id.id[0]); + unique_id_report_report_message[3] = (board_id.id[1]); + unique_id_report_report_message[4] = (board_id.id[2]); + unique_id_report_report_message[5] = (board_id.id[3]); + unique_id_report_report_message[6] = (board_id.id[4]); + unique_id_report_report_message[7] = (board_id.id[5]); + + serial_write(unique_id_report_report_message, + sizeof(unique_id_report_report_message) / sizeof(int)); } /******************************************** * Stop reporting for all input pins */ -void stop_all_reports() -{ - stop_reports = true; - sleep_ms(20); - stdio_flush(); +void stop_all_reports() { + stop_reports = true; + sleep_ms(20); + stdio_flush(); } /********************************************** * Enable reporting for all input pins */ -void enable_all_reports() -{ - stdio_flush(); - stop_reports = false; - sleep_ms(20); +void enable_all_reports() { + stdio_flush(); + stop_reports = false; + sleep_ms(20); } /****************************************** * Use the watchdog time to reset the board. */ -void reset_board() -{ - watchdog_reboot(0, 0, 0); - watchdog_enable(10, 1); +void reset_board() { + watchdog_reboot(0, 0, 0); + watchdog_enable(10, 1); } -void i2c_begin() -{ - // get the GPIO pins associated with this i2c instance - uint sda_gpio = command_buffer[I2C_SDA_GPIO_PIN]; - uint scl_gpio = command_buffer[I2C_SCL_GPIO_PIN]; - - // set the i2c instance - 0 or 1 - if (command_buffer[I2C_PORT] == 0) - { - i2c_init(i2c0, 100 * 1000); - } - else - { - i2c_init(i2c1, 100 * 1000); - } - gpio_set_function(sda_gpio, GPIO_FUNC_I2C); - gpio_set_function(scl_gpio, GPIO_FUNC_I2C); - gpio_pull_up(sda_gpio); - gpio_pull_up(scl_gpio); +void i2c_begin() { + // get the GPIO pins associated with this i2c instance + uint sda_gpio = command_buffer[I2C_SDA_GPIO_PIN]; + uint scl_gpio = command_buffer[I2C_SCL_GPIO_PIN]; + + // set the i2c instance - 0 or 1 + if (command_buffer[I2C_PORT] == 0) { + i2c_init(i2c0, 100 * 1000); + } else { + i2c_init(i2c1, 100 * 1000); + } + gpio_set_function(sda_gpio, GPIO_FUNC_I2C); + gpio_set_function(scl_gpio, GPIO_FUNC_I2C); + gpio_pull_up(sda_gpio); + gpio_pull_up(scl_gpio); } -void i2c_read() -{ - - // The report_message offsets: - // 0 = packet length - this must be calculated - // 1 = I2C_READ_REPORT - // 2 = The i2c port - 0 or 1 - // 3 = i2c device address - // 4 = message_id - // 5 = i2c read register - // 6 = number of bytes read - // 7... = bytes read - - // length of i2c report packet - int num_of_bytes_to_send = I2C_READ_START_OF_DATA + command_buffer[I2C_READ_LENGTH]; - - // We have a separate buffer ot store the data read from the device - // and combine that data back into the i2c report buffer. - // This gets around casting. - uint8_t data_from_device[command_buffer[I2C_READ_LENGTH]]; - - uint8_t message_id = command_buffer[I2C_READ_MESSAGE_ID]; - // return value from write and read i2c sdk commands - int i2c_sdk_call_return_value; - - // selector for i2c0 or i2c1 - i2c_inst_t *i2c; - - // Determine the i2c port to use. - if (command_buffer[I2C_PORT]) - { - i2c = i2c1; - } - else - { - i2c = i2c0; - } - - // If there is an i2c register specified, set the register pointer - if (command_buffer[I2C_READ_NO_STOP_FLAG] != I2C_NO_REGISTER_SPECIFIED) - { - i2c_sdk_call_return_value = i2c_write_blocking(i2c, - (uint8_t)command_buffer[I2C_DEVICE_ADDRESS], - (const uint8_t *)&command_buffer[I2C_READ_REGISTER], 1, - (bool)command_buffer[I2C_READ_NO_STOP_FLAG]); - if (i2c_sdk_call_return_value == PICO_ERROR_GENERIC) - { - return; - } +void i2c_read() { + + // The report_message offsets: + // 0 = packet length - this must be calculated + // 1 = I2C_READ_REPORT + // 2 = The i2c port - 0 or 1 + // 3 = i2c device address + // 4 = message_id + // 5 = i2c read register + // 6 = number of bytes read + // 7... = bytes read + + // length of i2c report packet + int num_of_bytes_to_send = + I2C_READ_START_OF_DATA + command_buffer[I2C_READ_LENGTH]; + + // We have a separate buffer ot store the data read from the device + // and combine that data back into the i2c report buffer. + // This gets around casting. + uint8_t data_from_device[command_buffer[I2C_READ_LENGTH]]; + + uint8_t message_id = command_buffer[I2C_READ_MESSAGE_ID]; + // return value from write and read i2c sdk commands + int i2c_sdk_call_return_value; + + // selector for i2c0 or i2c1 + i2c_inst_t *i2c; + + // Determine the i2c port to use. + if (command_buffer[I2C_PORT]) { + i2c = i2c1; + } else { + i2c = i2c0; + } + + // If there is an i2c register specified, set the register pointer + if (command_buffer[I2C_READ_NO_STOP_FLAG] != I2C_NO_REGISTER_SPECIFIED) { + i2c_sdk_call_return_value = + i2c_write_blocking(i2c, (uint8_t)command_buffer[I2C_DEVICE_ADDRESS], + (const uint8_t *)&command_buffer[I2C_READ_REGISTER], + 1, (bool)command_buffer[I2C_READ_NO_STOP_FLAG]); + if (i2c_sdk_call_return_value == PICO_ERROR_GENERIC) { + return; } - - // now do the read request - i2c_sdk_call_return_value = i2c_read_blocking(i2c, - (uint8_t)command_buffer[I2C_DEVICE_ADDRESS], - data_from_device, - (size_t)(command_buffer[I2C_READ_LENGTH]), - (bool)command_buffer[I2C_READ_NO_STOP_FLAG]); - if (i2c_sdk_call_return_value == PICO_ERROR_GENERIC) - { - i2c_report_message[I2C_PACKET_LENGTH] = I2C_ERROR_REPORT_LENGTH; // length of the packet - i2c_report_message[I2C_REPORT_ID] = I2C_READ_FAILED; // report ID - i2c_report_message[I2C_REPORT_PORT] = command_buffer[I2C_PORT]; - i2c_report_message[I2C_REPORT_DEVICE_ADDRESS] = command_buffer[I2C_DEVICE_ADDRESS]; - - serial_write(i2c_report_message, I2C_ERROR_REPORT_NUM_OF_BYTE_TO_SEND); - return; - } - - // copy the data returned from i2c device into the report message buffer - for (int i = 0; i < i2c_sdk_call_return_value; i++) - { - i2c_report_message[i + I2C_READ_START_OF_DATA] = data_from_device[i]; - } - // length of the packet - i2c_report_message[I2C_PACKET_LENGTH] = (uint8_t)(i2c_sdk_call_return_value + - I2C_READ_DATA_BASE_BYTES); - - i2c_report_message[I2C_REPORT_ID] = I2C_READ_REPORT; - - // i2c_port + } + + // now do the read request + i2c_sdk_call_return_value = i2c_read_blocking( + i2c, (uint8_t)command_buffer[I2C_DEVICE_ADDRESS], data_from_device, + (size_t)(command_buffer[I2C_READ_LENGTH]), + (bool)command_buffer[I2C_READ_NO_STOP_FLAG]); + if (i2c_sdk_call_return_value == PICO_ERROR_GENERIC) { + i2c_report_message[I2C_PACKET_LENGTH] = + I2C_ERROR_REPORT_LENGTH; // length of the packet + i2c_report_message[I2C_REPORT_ID] = I2C_READ_FAILED; // report ID i2c_report_message[I2C_REPORT_PORT] = command_buffer[I2C_PORT]; + i2c_report_message[I2C_REPORT_DEVICE_ADDRESS] = + command_buffer[I2C_DEVICE_ADDRESS]; - // i2c_address - i2c_report_message[I2C_REPORT_DEVICE_ADDRESS] = command_buffer[I2C_DEVICE_ADDRESS]; - - // i2c register - i2c_report_message[I2C_REPORT_READ_REGISTER] = command_buffer[I2C_READ_REGISTER]; - - // number of bytes read from i2c device - i2c_report_message[I2C_REPORT_READ_NUMBER_DATA_BYTES] = (uint8_t)i2c_sdk_call_return_value; + serial_write(i2c_report_message, I2C_ERROR_REPORT_NUM_OF_BYTE_TO_SEND); + return; + } - i2c_report_message[I2C_READ_MESSAGE_ID] = message_id; + // copy the data returned from i2c device into the report message buffer + for (int i = 0; i < i2c_sdk_call_return_value; i++) { + i2c_report_message[i + I2C_READ_START_OF_DATA] = data_from_device[i]; + } + // length of the packet + i2c_report_message[I2C_PACKET_LENGTH] = + (uint8_t)(i2c_sdk_call_return_value + I2C_READ_DATA_BASE_BYTES); - serial_write((int *)i2c_report_message, num_of_bytes_to_send); -} + i2c_report_message[I2C_REPORT_ID] = I2C_READ_REPORT; -void i2c_write() -{ - // i2c instance pointer - i2c_inst_t *i2c; + // i2c_port + i2c_report_message[I2C_REPORT_PORT] = command_buffer[I2C_PORT]; - // Determine the i2c port to use. - if (command_buffer[I2C_PORT]) - { - i2c = i2c1; - } - else - { - i2c = i2c0; - } + // i2c_address + i2c_report_message[I2C_REPORT_DEVICE_ADDRESS] = + command_buffer[I2C_DEVICE_ADDRESS]; - int i2c_sdk_call_return_value = i2c_write_blocking_until(i2c, (uint8_t)command_buffer[I2C_DEVICE_ADDRESS], - &(command_buffer[I2C_WRITE_BYTES_TO_WRITE]), - command_buffer[I2C_WRITE_NUMBER_OF_BYTES], - (bool)command_buffer[I2C_WRITE_NO_STOP_FLAG], make_timeout_time_ms(50)); + // i2c register + i2c_report_message[I2C_REPORT_READ_REGISTER] = + command_buffer[I2C_READ_REGISTER]; - i2c_report_message[I2C_PACKET_LENGTH] = 4; // length of the packet - i2c_report_message[I2C_REPORT_ID] = I2C_WRITE_REPORT; // bytes written or error - i2c_report_message[I2C_REPORT_PORT] = command_buffer[I2C_PORT]; - i2c_report_message[I2C_WRITE_MESSAGE_ID] = command_buffer[I2C_WRITE_MESSAGE_ID]; + // number of bytes read from i2c device + i2c_report_message[I2C_REPORT_READ_NUMBER_DATA_BYTES] = + (uint8_t)i2c_sdk_call_return_value; - i2c_report_message[4] = i2c_sdk_call_return_value; + i2c_report_message[I2C_READ_MESSAGE_ID] = message_id; - serial_write(i2c_report_message, I2C_ERROR_REPORT_NUM_OF_BYTE_TO_SEND); + serial_write((int *)i2c_report_message, num_of_bytes_to_send); } -void init_neo_pixels() -{ - // initialize the pico support a NeoPixel string - uint offset = pio_add_program(np_pio, &ws2812_program); - ws2812_init(np_pio, np_sm, offset, command_buffer[NP_PIN_NUMBER], 800000, - false); +void i2c_write() { + // i2c instance pointer + i2c_inst_t *i2c; + + // Determine the i2c port to use. + if (command_buffer[I2C_PORT]) { + i2c = i2c1; + } else { + i2c = i2c0; + } + + int i2c_sdk_call_return_value = i2c_write_blocking_until( + i2c, (uint8_t)command_buffer[I2C_DEVICE_ADDRESS], + &(command_buffer[I2C_WRITE_BYTES_TO_WRITE]), + command_buffer[I2C_WRITE_NUMBER_OF_BYTES], + (bool)command_buffer[I2C_WRITE_NO_STOP_FLAG], make_timeout_time_ms(50)); + + i2c_report_message[I2C_PACKET_LENGTH] = 4; // length of the packet + i2c_report_message[I2C_REPORT_ID] = + I2C_WRITE_REPORT; // bytes written or error + i2c_report_message[I2C_REPORT_PORT] = command_buffer[I2C_PORT]; + i2c_report_message[I2C_WRITE_MESSAGE_ID] = + command_buffer[I2C_WRITE_MESSAGE_ID]; + + i2c_report_message[4] = i2c_sdk_call_return_value; + + serial_write(i2c_report_message, I2C_ERROR_REPORT_NUM_OF_BYTE_TO_SEND); +} - actual_number_of_pixels = command_buffer[NP_NUMBER_OF_PIXELS]; +void init_neo_pixels() { + // initialize the pico support a NeoPixel string + uint offset = pio_add_program(np_pio, &ws2812_program); + ws2812_init(np_pio, np_sm, offset, command_buffer[NP_PIN_NUMBER], 800000, + false); + + actual_number_of_pixels = command_buffer[NP_NUMBER_OF_PIXELS]; + + // set the pixels to the fill color + for (uint i = 0; i < actual_number_of_pixels; i++) { + pixel_buffer[i][RED] = command_buffer[NP_RED_FILL]; + pixel_buffer[i][GREEN] = command_buffer[NP_GREEN_FILL]; + pixel_buffer[i][BLUE] = command_buffer[NP_BLUE_FILL]; + } + show_neo_pixels(); + sleep_ms(1); +} - // set the pixels to the fill color - for (uint i = 0; i < actual_number_of_pixels; i++) - { - pixel_buffer[i][RED] = command_buffer[NP_RED_FILL]; - pixel_buffer[i][GREEN] = command_buffer[NP_GREEN_FILL]; - pixel_buffer[i][BLUE] = command_buffer[NP_BLUE_FILL]; - } +void set_neo_pixel() { + // set a single neopixel in the pixel buffer + pixel_buffer[command_buffer[NP_PIXEL_NUMBER]][RED] = + command_buffer[NP_SET_RED]; + pixel_buffer[command_buffer[NP_PIXEL_NUMBER]][GREEN] = + command_buffer[NP_SET_GREEN]; + pixel_buffer[command_buffer[NP_PIXEL_NUMBER]][BLUE] = + command_buffer[NP_SET_BLUE]; + if (command_buffer[NP_SET_AUTO_SHOW]) { show_neo_pixels(); - sleep_ms(1); + } } -void set_neo_pixel() -{ - // set a single neopixel in the pixel buffer - pixel_buffer[command_buffer[NP_PIXEL_NUMBER]][RED] = command_buffer[NP_SET_RED]; - pixel_buffer[command_buffer[NP_PIXEL_NUMBER]][GREEN] = command_buffer[NP_SET_GREEN]; - pixel_buffer[command_buffer[NP_PIXEL_NUMBER]][BLUE] = command_buffer[NP_SET_BLUE]; - if (command_buffer[NP_SET_AUTO_SHOW]) - { - show_neo_pixels(); - } +void show_neo_pixels() { + // show the neopixels in the buffer + for (int i = 0; i < actual_number_of_pixels; i++) { + put_pixel(urgb_u32(pixel_buffer[i][RED], pixel_buffer[i][GREEN], + pixel_buffer[i][BLUE])); + } } -void show_neo_pixels() -{ - // show the neopixels in the buffer - for (int i = 0; i < actual_number_of_pixels; i++) - { - put_pixel(urgb_u32(pixel_buffer[i][RED], - pixel_buffer[i][GREEN], - pixel_buffer[i][BLUE])); - } +void clear_all_neo_pixels() { + // set all the neopixels in the buffer to all zeroes + for (int i = 0; i < actual_number_of_pixels; i++) { + pixel_buffer[i][RED] = 0; + pixel_buffer[i][GREEN] = 0; + pixel_buffer[i][BLUE] = 0; + } + if (command_buffer[NP_CLEAR_AUTO_SHOW]) { + show_neo_pixels(); + } } -void clear_all_neo_pixels() -{ - // set all the neopixels in the buffer to all zeroes - for (int i = 0; i < actual_number_of_pixels; i++) - { - pixel_buffer[i][RED] = 0; - pixel_buffer[i][GREEN] = 0; - pixel_buffer[i][BLUE] = 0; - } - if (command_buffer[NP_CLEAR_AUTO_SHOW]) - { - show_neo_pixels(); - } +void fill_neo_pixels() { + // fill all the neopixels in the buffer with the + // specified rgb values. + for (int i = 0; i < actual_number_of_pixels; i++) { + pixel_buffer[i][RED] = command_buffer[NP_FILL_RED]; + pixel_buffer[i][GREEN] = command_buffer[NP_FILL_GREEN]; + pixel_buffer[i][BLUE] = command_buffer[NP_FILL_BLUE]; + } + if (command_buffer[NP_FILL_AUTO_SHOW]) { + show_neo_pixels(); + } } -void fill_neo_pixels() -{ - // fill all the neopixels in the buffer with the - // specified rgb values. - for (int i = 0; i < actual_number_of_pixels; i++) - { - pixel_buffer[i][RED] = command_buffer[NP_FILL_RED]; - pixel_buffer[i][GREEN] = command_buffer[NP_FILL_GREEN]; - pixel_buffer[i][BLUE] = command_buffer[NP_FILL_BLUE]; +void sonar_callback(uint gpio, uint32_t events) { + if (events & GPIO_IRQ_EDGE_FALL) { + // stop time + for (int i = 0; i <= sonar_count; i++) { + hc_sr04_descriptor *sonar = &the_hc_sr04s.sonars[i]; + if (gpio == sonar->echo_pin) { + sonar->last_time_diff = time_us_32() - sonar->start_time; + return; + } } - if (command_buffer[NP_FILL_AUTO_SHOW]) - { - show_neo_pixels(); + } else if (events & GPIO_IRQ_EDGE_RISE) { + // start time + for (int i = 0; i <= sonar_count; i++) { + hc_sr04_descriptor *sonar = &the_hc_sr04s.sonars[i]; + if (gpio == sonar->echo_pin) { + sonar->start_time = time_us_32(); + return; + } } + } } -void sonar_callback(uint gpio, uint32_t events) -{ - if (events & GPIO_IRQ_EDGE_FALL) - { - // stop time - for (int i = 0; i <= sonar_count; i++) - { - hc_sr04_descriptor *sonar = &the_hc_sr04s.sonars[i]; - if (gpio == sonar->echo_pin) - { - sonar->last_time_diff = time_us_32() - sonar->start_time; - return; - } - } - } - else if (events & GPIO_IRQ_EDGE_RISE) - { - // start time - for (int i = 0; i <= sonar_count; i++) - { - hc_sr04_descriptor *sonar = &the_hc_sr04s.sonars[i]; - if (gpio == sonar->echo_pin) - { - sonar->start_time = time_us_32(); - return; - } - } - } +void init_quadrature_encoder(int A, int B, encoder_t *enc) { + gpio_init(A); + gpio_set_dir(A, GPIO_IN); + gpio_set_pulls(A, false, false); + gpio_init(B); + gpio_set_dir(B, GPIO_IN); + gpio_set_pulls(B, false, false); + + enc->A = A; + enc->B = B; + enc->type = QUADRATURE; } +void init_single_encoder(int A, encoder_t *enc) { + gpio_init(A); + gpio_set_dir(A, GPIO_IN); + gpio_set_pulls(A, false, false); -void init_quadrature_encoder(int A, int B, encoder_t *enc) -{ - gpio_init(A); - gpio_set_dir(A, GPIO_IN); - gpio_set_pulls(A, false, false); - gpio_init(B); - gpio_set_dir(B, GPIO_IN); - gpio_set_pulls(B, false, false); - - enc->A = A; - enc->B = B; - enc->type = QUADRATURE; -} -void init_single_encoder(int A, encoder_t *enc) -{ - gpio_init(A); - gpio_set_dir(A, GPIO_IN); - gpio_set_pulls(A, false, false); - - enc->A = A; - enc->type = SINGLE; + enc->A = A; + enc->type = SINGLE; } -bool encoder_callback(repeating_timer_t *timer) -{ - (void)timer; - if (encoders.next_encoder_index == 0) - { - return true; - } - for (int i = 0; i < encoders.next_encoder_index; i++) - { - encoder_t *enc = &encoders.encoders[i]; - if (enc->type == QUADRATURE) - { - bool a = gpio_get(enc->A); - - bool b = gpio_get(enc->B); - - int new_gray_code_step = a << 1 | b; - int new_bin_step = new_gray_code_step >> 1 ^ new_gray_code_step; - int diff = new_bin_step - enc->last_state; - if (diff > -2 && diff < 2) - { - enc->step += diff; - } - else - { - enc->step += (diff < 0) ? 1 : -1; - } - enc->last_state = new_bin_step; - } - else - { - bool a = gpio_get(enc->A); - if (a != enc->last_state) - { - enc->step++; - enc->last_state = a; - } - } - } +bool encoder_callback(repeating_timer_t *timer) { + (void)timer; + if (encoders.next_encoder_index == 0) { return true; + } + for (int i = 0; i < encoders.next_encoder_index; i++) { + encoder_t *enc = &encoders.encoders[i]; + if (enc->type == QUADRATURE) { + bool a = gpio_get(enc->A); + + bool b = gpio_get(enc->B); + + int new_gray_code_step = a << 1 | b; + int new_bin_step = new_gray_code_step >> 1 ^ new_gray_code_step; + int diff = new_bin_step - enc->last_state; + if (diff > -2 && diff < 2) { + enc->step += diff; + } else { + enc->step += (diff < 0) ? 1 : -1; + } + enc->last_state = new_bin_step; + } else { + bool a = gpio_get(enc->A); + if (a != enc->last_state) { + enc->step++; + enc->last_state = a; + } + } + } + return true; } -bool create_encoder_timer() -{ - int hz = 2000; - /* blue encoder motor: - - 110 rpm = ~2 rot/s - - 540 steps/rot - - >1000 steps/s - - requires at least 1 scan per step +bool create_encoder_timer() { + int hz = 2000; + /* blue encoder motor: + - 110 rpm = ~2 rot/s + - 540 steps/rot + - >1000 steps/s + - requires at least 1 scan per step */ - if (!add_repeating_timer_us(-1000000 / hz, encoder_callback, NULL, &encoders.trigger_timer)) - { - printf("Failed to add timer\n"); - return false; - } - return true; + if (!add_repeating_timer_us(-1000000 / hz, encoder_callback, NULL, + &encoders.trigger_timer)) { + printf("Failed to add timer\n"); + return false; + } + return true; } -void encoder_new() -{ - ENCODER_TYPES type = (ENCODER_TYPES) command_buffer[ENCODER_TYPE]; - uint pin_a = command_buffer[ENCODER_PIN_A]; - uint pin_b = command_buffer[ENCODER_PIN_B]; // both cases will have a pin B - if (encoders.next_encoder_index == 0) - { - bool timer = create_encoder_timer(); - if (!timer) - { - return; - } - } - else if (encoders.next_encoder_index > MAX_ENCODERS) - { - return; - } - encoder_t *new_encoder = &encoders.encoders[encoders.next_encoder_index]; - if (type == SINGLE) - { - init_single_encoder(pin_a, new_encoder); - } - else - { - init_quadrature_encoder(pin_a, pin_b, new_encoder); +void encoder_new() { + ENCODER_TYPES type = (ENCODER_TYPES)command_buffer[ENCODER_TYPE]; + uint pin_a = command_buffer[ENCODER_PIN_A]; + uint pin_b = command_buffer[ENCODER_PIN_B]; // both cases will have a pin B + if (encoders.next_encoder_index == 0) { + bool timer = create_encoder_timer(); + if (!timer) { + return; } - encoders.next_encoder_index++; + } else if (encoders.next_encoder_index > MAX_ENCODERS) { + return; + } + encoder_t *new_encoder = &encoders.encoders[encoders.next_encoder_index]; + if (type == SINGLE) { + init_single_encoder(pin_a, new_encoder); + } else { + init_quadrature_encoder(pin_a, pin_b, new_encoder); + } + encoders.next_encoder_index++; } int encoder_report_message[] = {3, ENCODER_REPORT, 0, 0}; -void scan_encoders() -{ - for (int i = 0; i < encoders.next_encoder_index; i++) - { - encoder_t *enc = &encoders.encoders[i]; - if (enc->step != 0) - { - encoder_report_message[ENCODER_REPORT_PIN_A] = (uint8_t)enc->A; - encoder_report_message[ENCODER_REPORT_STEP] = enc->step; - enc->step = 0; - serial_write(encoder_report_message, 4); - } +void scan_encoders() { + for (int i = 0; i < encoders.next_encoder_index; i++) { + encoder_t *enc = &encoders.encoders[i]; + if (enc->step != 0) { + encoder_report_message[ENCODER_REPORT_PIN_A] = (uint8_t)enc->A; + encoder_report_message[ENCODER_REPORT_STEP] = enc->step; + enc->step = 0; + serial_write(encoder_report_message, 4); } + } } -bool sonar_timer_callback(repeating_timer_t *rt) -{ // periodically trigger all sonars at the same time - gpio_set_mask(the_hc_sr04s.trigger_mask); - busy_wait_us(10); - gpio_clr_mask(the_hc_sr04s.trigger_mask); - return true; +bool sonar_timer_callback( + repeating_timer_t *rt) { // periodically trigger all sonars at the same time + gpio_set_mask(the_hc_sr04s.trigger_mask); + busy_wait_us(10); + gpio_clr_mask(the_hc_sr04s.trigger_mask); + return true; } -void sonar_new() -{ - // add the sonar to the sonar struct to be processed within - // the main loop - uint trig_pin = command_buffer[SONAR_TRIGGER_PIN]; - uint echo_pin = command_buffer[SONAR_ECHO_PIN]; - - // for the first HC-SR04, add the program. - if (sonar_count == -1) - { - // Init timer - int hz = 10; - // negative timeout means exact delay (rather than delay between callbacks) - if (!add_repeating_timer_us(-1000000 / hz, sonar_timer_callback, NULL, &the_hc_sr04s.trigger_timer)) - { - printf("Failed to add timer\n"); - return; - } +void sonar_new() { + // add the sonar to the sonar struct to be processed within + // the main loop + uint trig_pin = command_buffer[SONAR_TRIGGER_PIN]; + uint echo_pin = command_buffer[SONAR_ECHO_PIN]; + + // for the first HC-SR04, add the program. + if (sonar_count == -1) { + // Init timer + int hz = 10; + // negative timeout means exact delay (rather than delay between callbacks) + if (!add_repeating_timer_us(-1000000 / hz, sonar_timer_callback, NULL, + &the_hc_sr04s.trigger_timer)) { + printf("Failed to add timer\n"); + return; } - sonar_count++; - if (sonar_count > MAX_SONARS) - { - return; - } - the_hc_sr04s.sonars[sonar_count].trig_pin = trig_pin; - the_hc_sr04s.sonars[sonar_count].echo_pin = echo_pin; - the_hc_sr04s.sonars[sonar_count].last_time_diff = 0; - the_hc_sr04s.trigger_mask |= 1ul << trig_pin; - gpio_init(trig_pin); - gpio_set_dir(trig_pin, GPIO_OUT); - gpio_init(echo_pin); - gpio_set_dir(echo_pin, GPIO_IN); - gpio_set_irq_enabled_with_callback(echo_pin, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true, &sonar_callback); -} - -bool repeating_timer_callback(struct repeating_timer *t) -{ - (void)t; - // printf("Repeat at %lld\n", time_us_64()); - timer_fired = true; - return true; + } + sonar_count++; + if (sonar_count > MAX_SONARS) { + return; + } + the_hc_sr04s.sonars[sonar_count].trig_pin = trig_pin; + the_hc_sr04s.sonars[sonar_count].echo_pin = echo_pin; + the_hc_sr04s.sonars[sonar_count].last_time_diff = 0; + the_hc_sr04s.trigger_mask |= 1ul << trig_pin; + gpio_init(trig_pin); + gpio_set_dir(trig_pin, GPIO_OUT); + gpio_init(echo_pin); + gpio_set_dir(echo_pin, GPIO_IN); + gpio_set_irq_enabled_with_callback( + echo_pin, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true, &sonar_callback); } -void dht_new() -{ - if (dht_count > MAX_DHTS) - { - return; - } - if (dht_count == -1) - { - // first time through start repeating timer - add_repeating_timer_ms(2000, repeating_timer_callback, NULL, &timer); - } - dht_count++; - - uint dht_pin = command_buffer[DHT_DATA_PIN]; - the_dhts.dhts[dht_count].data_pin = dht_pin; - the_dhts.dhts[dht_count].previous_time = get_absolute_time(); - gpio_init(dht_pin); +bool repeating_timer_callback(struct repeating_timer *t) { + (void)t; + // printf("Repeat at %lld\n", time_us_64()); + timer_fired = true; + return true; } -void init_spi() -{ - spi_inst_t *spi_port; - uint spi_baud_rate; - uint cs_pin; - - // initialize the spi port - if (command_buffer[SPI_PORT] == 0) - { - spi_port = spi0; - } - else - { - spi_port = spi1; - } - - spi_baud_rate = ((command_buffer[SPI_FREQ_MSB] << 24) + - (command_buffer[SPI_FREQ_3] << 16) + - (command_buffer[SPI_FREQ_2] << 8) + - (command_buffer[SPI_FREQ_1])); - - spi_init(spi_port, spi_baud_rate); - - // set gpio pins for miso, mosi and clock - gpio_set_function(command_buffer[SPI_MISO], GPIO_FUNC_SPI); - gpio_set_function(command_buffer[SPI_MOSI], GPIO_FUNC_SPI); - gpio_set_function(command_buffer[SPI_CLK_PIN], GPIO_FUNC_SPI); - - // initialize chip select GPIO pins - for (int i = 0; i < command_buffer[SPI_CS_LIST_LENGTH]; i++) - { - cs_pin = command_buffer[SPI_CS_LIST + i]; - // Chip select is active-low, so we'll initialise it to a driven-high state - gpio_init(cs_pin); - gpio_set_dir(cs_pin, GPIO_OUT); - gpio_put(cs_pin, 1); - } +void dht_new() { + if (dht_count > MAX_DHTS) { + return; + } + if (dht_count == -1) { + // first time through start repeating timer + add_repeating_timer_ms(2000, repeating_timer_callback, NULL, &timer); + } + dht_count++; + + uint dht_pin = command_buffer[DHT_DATA_PIN]; + the_dhts.dhts[dht_count].data_pin = dht_pin; + the_dhts.dhts[dht_count].previous_time = get_absolute_time(); + gpio_init(dht_pin); } -void spi_cs_control() -{ - uint8_t cs_pin; - uint8_t cs_state; - - cs_pin = command_buffer[SPI_SELECT_PIN]; - cs_state = command_buffer[SPI_SELECT_STATE]; - asm volatile("nop \n nop \n nop"); - gpio_put(cs_pin, cs_state); - asm volatile("nop \n nop \n nop"); +void init_spi() { + spi_inst_t *spi_port; + uint spi_baud_rate; + uint cs_pin; + + // initialize the spi port + if (command_buffer[SPI_PORT] == 0) { + spi_port = spi0; + } else { + spi_port = spi1; + } + + spi_baud_rate = + ((command_buffer[SPI_FREQ_MSB] << 24) + + (command_buffer[SPI_FREQ_3] << 16) + (command_buffer[SPI_FREQ_2] << 8) + + (command_buffer[SPI_FREQ_1])); + + spi_init(spi_port, spi_baud_rate); + + // set gpio pins for miso, mosi and clock + gpio_set_function(command_buffer[SPI_MISO], GPIO_FUNC_SPI); + gpio_set_function(command_buffer[SPI_MOSI], GPIO_FUNC_SPI); + gpio_set_function(command_buffer[SPI_CLK_PIN], GPIO_FUNC_SPI); + + // initialize chip select GPIO pins + for (int i = 0; i < command_buffer[SPI_CS_LIST_LENGTH]; i++) { + cs_pin = command_buffer[SPI_CS_LIST + i]; + // Chip select is active-low, so we'll initialise it to a driven-high state + gpio_init(cs_pin); + gpio_set_dir(cs_pin, GPIO_OUT); + gpio_put(cs_pin, 1); + } } -void set_scan_delay() -{ +void spi_cs_control() { + uint8_t cs_pin; + uint8_t cs_state; - scan_delay = command_buffer[SCAN_DELAY]; + cs_pin = command_buffer[SPI_SELECT_PIN]; + cs_state = command_buffer[SPI_SELECT_STATE]; + asm volatile("nop \n nop \n nop"); + gpio_put(cs_pin, cs_state); + asm volatile("nop \n nop \n nop"); } -void read_blocking_spi() -{ - // The report_message offsets: - // 0 = packet length - this must be calculated - // 1 = SPI_READ_REPORT - // 2 = The i2c port - 0 or 1 - // 3 = number of bytes read - // 4... = bytes read - - spi_inst_t *spi_port; - size_t data_length; - uint8_t repeated_transmit_byte; - uint8_t data[command_buffer[SPI_READ_LEN]]; - - if (command_buffer[SPI_PORT] == 0) - { - spi_port = spi0; - } - else - { - spi_port = spi1; - } - - data_length = command_buffer[SPI_READ_LEN]; - // memset(data, 0, data_length); - memset(data, 0, sizeof(data)); - - repeated_transmit_byte = command_buffer[SPI_REPEATED_DATA]; - - // read data - spi_read_blocking(spi_port, repeated_transmit_byte, data, data_length); - sleep_ms(100); - - // build a report from the data returned - spi_report_message[SPI_PACKET_LENGTH] = SPI_REPORT_NUMBER_OF_DATA_BYTES + data_length; - spi_report_message[SPI_REPORT_ID] = SPI_REPORT; - spi_report_message[SPI_REPORT_PORT] = command_buffer[SPI_PORT]; - spi_report_message[SPI_REPORT_NUMBER_OF_DATA_BYTES] = data_length; - for (int i = 0; i < data_length; i++) - { - spi_report_message[SPI_DATA + i] = data[i]; - } - serial_write((int *)spi_report_message, - SPI_DATA + data_length); +void set_scan_delay() { scan_delay = command_buffer[SCAN_DELAY]; } + +void read_blocking_spi() { + // The report_message offsets: + // 0 = packet length - this must be calculated + // 1 = SPI_READ_REPORT + // 2 = The i2c port - 0 or 1 + // 3 = number of bytes read + // 4... = bytes read + + spi_inst_t *spi_port; + size_t data_length; + uint8_t repeated_transmit_byte; + uint8_t data[command_buffer[SPI_READ_LEN]]; + + if (command_buffer[SPI_PORT] == 0) { + spi_port = spi0; + } else { + spi_port = spi1; + } + + data_length = command_buffer[SPI_READ_LEN]; + // memset(data, 0, data_length); + memset(data, 0, sizeof(data)); + + repeated_transmit_byte = command_buffer[SPI_REPEATED_DATA]; + + // read data + spi_read_blocking(spi_port, repeated_transmit_byte, data, data_length); + sleep_ms(100); + + // build a report from the data returned + spi_report_message[SPI_PACKET_LENGTH] = + SPI_REPORT_NUMBER_OF_DATA_BYTES + data_length; + spi_report_message[SPI_REPORT_ID] = SPI_REPORT; + spi_report_message[SPI_REPORT_PORT] = command_buffer[SPI_PORT]; + spi_report_message[SPI_REPORT_NUMBER_OF_DATA_BYTES] = data_length; + for (int i = 0; i < data_length; i++) { + spi_report_message[SPI_DATA + i] = data[i]; + } + serial_write((int *)spi_report_message, SPI_DATA + data_length); } -void write_blocking_spi() -{ - spi_inst_t *spi_port; - uint cs_pin; - size_t data_length; +void write_blocking_spi() { + spi_inst_t *spi_port; + uint cs_pin; + size_t data_length; - if (command_buffer[SPI_PORT] == 0) - { - spi_port = spi0; - } - else - { - spi_port = spi1; - } + if (command_buffer[SPI_PORT] == 0) { + spi_port = spi0; + } else { + spi_port = spi1; + } - data_length = command_buffer[SPI_WRITE_LEN]; - // write data - spi_write_blocking(spi_port, &command_buffer[SPI_WRITE_DATA], data_length); + data_length = command_buffer[SPI_WRITE_LEN]; + // write data + spi_write_blocking(spi_port, &command_buffer[SPI_WRITE_DATA], data_length); } -void set_format_spi() -{ - spi_inst_t *spi_port; - uint data_bits = command_buffer[SPI_NUMBER_OF_BITS]; - spi_cpol_t cpol = (spi_cpol_t) command_buffer[SPI_CLOCK_PHASE]; - spi_cpha_t cpha = (spi_cpha_t) command_buffer[SPI_CLOCK_POLARITY]; - - if (command_buffer[SPI_PORT] == 0) - { - spi_port = spi0; - } - else - { - spi_port = spi1; - } - spi_set_format(spi_port, data_bits, cpol, cpha, (spi_order_t)1); +void set_format_spi() { + spi_inst_t *spi_port; + uint data_bits = command_buffer[SPI_NUMBER_OF_BITS]; + spi_cpol_t cpol = (spi_cpol_t)command_buffer[SPI_CLOCK_PHASE]; + spi_cpha_t cpha = (spi_cpha_t)command_buffer[SPI_CLOCK_POLARITY]; + + if (command_buffer[SPI_PORT] == 0) { + spi_port = spi0; + } else { + spi_port = spi1; + } + spi_set_format(spi_port, data_bits, cpol, cpha, (spi_order_t)1); } /******************* FOR FUTURE RELEASES **********************/ @@ -1035,256 +930,332 @@ void servo_detach() {} /*************************************************** * Retrieve the next command and process it */ -void get_next_command() -{ - int packet_size; - int packet_data; - command_descriptor command_entry; - - // clear the command buffer for the new incoming command - memset(command_buffer, 0, sizeof(command_buffer)); - - // Get the number of bytes of the command packet. - // The first byte is the command ID and the following bytes - // are the associated data bytes - packet_size = getchar_timeout_us(100); - if (packet_size == PICO_ERROR_TIMEOUT) - { - // no data, let the main loop continue to run to handle inputs - return; - } - else - { - // get the rest of the packet - for (int i = 0; i < packet_size; i++) - { - for (int retries = 10; retries > 0; retries--) - { - packet_data = getchar_timeout_us(100); - - if (packet_data != PICO_ERROR_TIMEOUT) - { - break; - } - sleep_ms(1); - } - if (packet_data == PICO_ERROR_TIMEOUT) - { - return; // failed message - } - command_buffer[i] = (uint8_t)packet_data; +void get_next_command() { + int packet_size; + int packet_data; + command_descriptor command_entry; + + // clear the command buffer for the new incoming command + memset(command_buffer, 0, sizeof(command_buffer)); + + // Get the number of bytes of the command packet. + // The first byte is the command ID and the following bytes + // are the associated data bytes + packet_size = getchar_timeout_us(100); + if (packet_size == PICO_ERROR_TIMEOUT) { + // no data, let the main loop continue to run to handle inputs + return; + } else { + // get the rest of the packet + for (int i = 0; i < packet_size; i++) { + for (int retries = 10; retries > 0; retries--) { + packet_data = getchar_timeout_us(100); + + if (packet_data != PICO_ERROR_TIMEOUT) { + break; } + sleep_ms(1); + } + if (packet_data == PICO_ERROR_TIMEOUT) { + return; // failed message + } + command_buffer[i] = (uint8_t)packet_data; + } - // the first byte is the command ID. - // look up the function and execute it. - // data for the command starts at index 1 in the command_buffer - command_entry = command_table[command_buffer[0]]; + // the first byte is the command ID. + // look up the function and execute it. + // data for the command starts at index 1 in the command_buffer + command_entry = command_table[command_buffer[0]]; - // uncomment to see the command and first byte of data - // send_debug_info(command_buffer[0], command_buffer[1]); + // uncomment to see the command and first byte of data + // send_debug_info(command_buffer[0], command_buffer[1]); - // call the command + // call the command - command_entry.command_func(); - } + command_entry.command_func(); + } } /************************************** * Scan all pins set as digital inputs * and generate a report. */ -void scan_digital_inputs() -{ - int value; +void scan_digital_inputs() { + int value; + + // report message + + // index 0 = packet length + // index 1 = report type + // index 2 = pin number + // index 3 = value + + for (int i = 0; i < MAX_DIGITAL_PINS_SUPPORTED; i++) { + if (the_digital_pins[i].pin_mode == DIGITAL_INPUT || + the_digital_pins[i].pin_mode == DIGITAL_INPUT_PULL_UP || + the_digital_pins[i].pin_mode == DIGITAL_INPUT_PULL_DOWN) { + if (the_digital_pins[i].reporting_enabled) { + // if the value changed since last read + value = gpio_get(the_digital_pins[i].pin_number); + if (value != the_digital_pins[i].last_value) { + the_digital_pins[i].last_value = value; + digital_input_report_message[DIGITAL_INPUT_GPIO_PIN] = i; + digital_input_report_message[DIGITAL_INPUT_GPIO_VALUE] = value; + serial_write(digital_input_report_message, 4); + } + } + } + } +} - // report message +void scan_analog_inputs() { + uint16_t value; + + // report message + + // byte 0 = packet length + // byte 1 = report type + // byte 2 = pin number + // byte 3 = high order byte of value + // byte 4 = low order byte of value + + int differential; + + for (uint8_t i = 0; i < MAX_ANALOG_PINS_SUPPORTED; i++) { + if (the_analog_pins[i].reporting_enabled) { + adc_select_input(i); + value = adc_read(); + differential = abs(value - the_analog_pins[i].last_value); + if (differential >= the_analog_pins[i].differential) { + // trigger value achieved, send out the report + the_analog_pins[i].last_value = value; + // input_message[1] = the_analog_pins[i].pin_number; + analog_input_report_message[ANALOG_INPUT_GPIO_PIN] = (uint8_t)i; + analog_input_report_message[ANALOG_VALUE_HIGH_BYTE] = value >> 8; + analog_input_report_message[ANALOG_VALUE_LOW_BYTE] = value & 0x00ff; + serial_write(analog_input_report_message, 5); + } + } + } +} - // index 0 = packet length - // index 1 = report type - // index 2 = pin number - // index 3 = value +void scan_sonars() { + uint32_t current_time = time_us_32(); + for (int i = 0; i <= sonar_count; i++) { + hc_sr04_descriptor *sonar = &the_hc_sr04s.sonars[i]; - for (int i = 0; i < MAX_DIGITAL_PINS_SUPPORTED; i++) + if ((current_time - sonar->start_time) > + 1000000) // if too long since last trigger, send 0 { - if (the_digital_pins[i].pin_mode == DIGITAL_INPUT || - the_digital_pins[i].pin_mode == DIGITAL_INPUT_PULL_UP || - the_digital_pins[i].pin_mode == DIGITAL_INPUT_PULL_DOWN) - { - if (the_digital_pins[i].reporting_enabled) - { - // if the value changed since last read - value = gpio_get(the_digital_pins[i].pin_number); - if (value != the_digital_pins[i].last_value) - { - the_digital_pins[i].last_value = value; - digital_input_report_message[DIGITAL_INPUT_GPIO_PIN] = i; - digital_input_report_message[DIGITAL_INPUT_GPIO_VALUE] = value; - serial_write(digital_input_report_message, 4); - } - } - } + sonar->last_time_diff = 0; + } + if (sonar->last_time_diff > 30000) { + sonar->last_time_diff = 0; // HC-SR04 has max range of 4 / 5m, with a + // timeout pulse longer than 35ms } + // 0.1mm increments + int distance = (sonar->last_time_diff) / (58.0 / 100); + + sonar_report_message[SONAR_TRIG_PIN] = (uint8_t)sonar->trig_pin; + sonar_report_message[M_WHOLE_VALUE] = distance / 10000; + sonar_report_message[CM_WHOLE_VALUE] = (distance / 100) % 100; + serial_write(sonar_report_message, 5); + } } -void scan_analog_inputs() -{ - uint16_t value; +// SENSORSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSS + +void sensor_new() { + const SENSOR_TYPES type = (SENSOR_TYPES)command_buffer[0]; + const uint8_t sensor_num = command_buffer[1]; + uint8_t sensor_data[SENSORS_MAX_SETTINGS_A]; + std::copy(command_buffer + 2, command_buffer + 8, sensor_data); + + if (type >= SENSOR_TYPES::MAX_SENSORS) { + return; + } + Sensor *sensor = nullptr; + if (type == GPS) { + sensor = new GPS_Sensor(sensor_num, sensor_data); + } else if (type == SENSOR_TYPES::ADXL345) { + sensor = new ADXL345_Sensor(sensor_num, sensor_data); + } + sensors.push_back(sensor); +} - // report message +GPS_Sensor::GPS_Sensor(int num, uint8_t settings[SENSORS_MAX_SETTINGS_A]) { + // TODO: read uart, forward to serial +} +void GPS_Sensor::readSensor() { + // TODO: read uart, forward to serial +} - // byte 0 = packet length - // byte 1 = report type - // byte 2 = pin number - // byte 3 = high order byte of value - // byte 4 = low order byte of value +int write_i2c(int i2c_port, int addr, const std::vector &bytes) { + i2c_inst_t *i2c; + + if (i2c_port) { + i2c = i2c1; + } else { + i2c = i2c0; + } + int i2c_sdk_call_return_value = + i2c_write_blocking_until(i2c, (uint8_t)addr, bytes.data(), bytes.size(), + false, make_timeout_time_ms(50)); + return i2c_sdk_call_return_value == bytes.size(); +} - int differential; +int read_i2c(int i2c_port, int addr, const std::vector &write_bytes, + int bytes_to_read, std::vector &read_bytes) { + + // write i2c + if (!write_bytes.empty()) { + write_i2c(i2c_port, addr, write_bytes); + } + i2c_inst_t *i2c; + + if (i2c_port) { + i2c = i2c1; + } else { + i2c = i2c0; + } + read_bytes.resize(bytes_to_read, 0); + auto i2c_sdk_return = + i2c_read_blocking_until(i2c, addr, read_bytes.data(), bytes_to_read, + false, make_timeout_time_ms(50)); + + return i2c_sdk_return == bytes_to_read; +} - for (uint8_t i = 0; i < MAX_ANALOG_PINS_SUPPORTED; i++) - { - if (the_analog_pins[i].reporting_enabled) - { - adc_select_input(i); - value = adc_read(); - differential = abs(value - the_analog_pins[i].last_value); - if (differential >= the_analog_pins[i].differential) - { - // trigger value achieved, send out the report - the_analog_pins[i].last_value = value; - // input_message[1] = the_analog_pins[i].pin_number; - analog_input_report_message[ANALOG_INPUT_GPIO_PIN] = (uint8_t)i; - analog_input_report_message[ANALOG_VALUE_HIGH_BYTE] = value >> 8; - analog_input_report_message[ANALOG_VALUE_LOW_BYTE] = value & 0x00ff; - serial_write(analog_input_report_message, 5); - } - } - } +ADXL345_Sensor::ADXL345_Sensor(int num, + uint8_t settings[SENSORS_MAX_SETTINGS_A]) { + // assume i2c is done + this->i2c_port = settings[0]; + this->init_sequence(); +} +void ADXL345_Sensor::init_sequence() { + // write 83, 43, 0 + bool ok = write_i2c(this->i2c_port, this->i2c_addr, {43, 0}); + // write 83, 45, 8 + ok |= write_i2c(this->i2c_port, this->i2c_addr, {45, 8}); + // write 83, 49, 8 + ok |= write_i2c(this->i2c_port, this->i2c_addr, {49, 8}); + if (!ok) { + this->stop = true; + } } +void ADXL345_Sensor::readSensor() { + // read 83, 50, 6bytes + std::vector out(6); + read_i2c(this->i2c_port, this->i2c_addr, {50}, 6, out); -void scan_sonars() -{ - uint32_t current_time = time_us_32(); - for (int i = 0; i <= sonar_count; i++) - { - hc_sr04_descriptor *sonar = &the_hc_sr04s.sonars[i]; + this->writeSensorData(out); +} - if ((current_time - sonar->start_time) > 1000000) // if too long since last trigger, send 0 - { - sonar->last_time_diff = 0; - } - if (sonar->last_time_diff > 30000) - { - sonar->last_time_diff = 0; // HC-SR04 has max range of 4 / 5m, with a timeout pulse longer than 35ms - } - // 0.1mm increments - int distance = (sonar->last_time_diff) / (58.0 / 100); +void Sensor::writeSensorData(std::vector data) { + std::vector out = { + 0, // write len + SENSOR_REPORT, // write type + (uint8_t)this->num, // write num + this->type, // write sensor type + }; + out.insert(out.end(), data.begin(), data.end()); + out[0] = out.size(); + serial_write(out); +} - sonar_report_message[SONAR_TRIG_PIN] = (uint8_t)sonar->trig_pin; - sonar_report_message[M_WHOLE_VALUE] = distance / 10000; - sonar_report_message[CM_WHOLE_VALUE] = (distance / 100) % 100; - serial_write(sonar_report_message, 5); - } +void serial_write(std::vector data) { + for (auto i : data) { + putchar(i); + } + stdio_flush(); } -void scan_dhts() -{ - // read the next dht device - // one device is read each cycle - if (dht_count >= 0) - { - if (timer_fired) - { - timer_fired = false; - int the_index = the_dhts.next_dht_index; - read_dht(the_dhts.dhts[the_index].data_pin); - the_dhts.next_dht_index++; - if (the_dhts.next_dht_index > dht_count) - { - the_dhts.next_dht_index = 0; - } - } +// DHTS sensor? +void scan_dhts() { + // read the next dht device + // one device is read each cycle + if (dht_count >= 0) { + if (timer_fired) { + timer_fired = false; + int the_index = the_dhts.next_dht_index; + read_dht(the_dhts.dhts[the_index].data_pin); + the_dhts.next_dht_index++; + if (the_dhts.next_dht_index > dht_count) { + the_dhts.next_dht_index = 0; + } } + } } -void read_dht(uint dht_pin) -{ - int data[5] = {0, 0, 0, 0, 0}; - uint last = 1; - uint j = 0; - float temp_celsius; - float humidity; - float nearest; - int temp_int_part; - int temp_dec_part; - int humidity_int_part; - int humidity_dec_part; - - gpio_set_dir(dht_pin, GPIO_OUT); - gpio_put(dht_pin, 0); - sleep_ms(20); - gpio_set_dir(dht_pin, GPIO_IN); - - sleep_us(1); - for (uint i = 0; i < DHT_MAX_TIMINGS; i++) - { - uint count = 0; - while (gpio_get(dht_pin) == last) - { - count++; - sleep_us(1); - if (count == 255) - break; - } - last = gpio_get(dht_pin); - if (count == 255) - break; - - if ((i >= 4) && (i % 2 == 0)) - { - data[j / 8] <<= 1; - if (count > 46) - data[j / 8] |= 1; - j++; - } +void read_dht(uint dht_pin) { + int data[5] = {0, 0, 0, 0, 0}; + uint last = 1; + uint j = 0; + float temp_celsius; + float humidity; + float nearest; + int temp_int_part; + int temp_dec_part; + int humidity_int_part; + int humidity_dec_part; + + gpio_set_dir(dht_pin, GPIO_OUT); + gpio_put(dht_pin, 0); + sleep_ms(20); + gpio_set_dir(dht_pin, GPIO_IN); + + sleep_us(1); + for (uint i = 0; i < DHT_MAX_TIMINGS; i++) { + uint count = 0; + while (gpio_get(dht_pin) == last) { + count++; + sleep_us(1); + if (count == 255) + break; } - if ((j >= 40) && (data[4] == ((data[0] + data[1] + data[2] + data[3]) & 0xFF))) - { - humidity = (float)((data[0] << 8) + data[1]) / 10; - if (humidity > 100) - { - humidity = data[0]; - } - temp_celsius = (float)(((data[2] & 0x7F) << 8) + data[3]) / 10; - if (temp_celsius > 125) - { - temp_celsius = data[2]; - } - if (data[2] & 0x80) - { - temp_celsius = -temp_celsius; - } - - nearest = roundf(temp_celsius * 100) / 100; - temp_int_part = (int)nearest; - temp_dec_part = (int)((nearest - temp_int_part) * 100); - - nearest = roundf(humidity * 100) / 100; - humidity_int_part = (int)nearest; - humidity_dec_part = (int)((nearest - humidity_int_part) * 100); + last = gpio_get(dht_pin); + if (count == 255) + break; + + if ((i >= 4) && (i % 2 == 0)) { + data[j / 8] <<= 1; + if (count > 46) + data[j / 8] |= 1; + j++; } - else - { - // bad data return zeros - temp_int_part = temp_dec_part = - humidity_int_part = humidity_dec_part = 0; + } + if ((j >= 40) && + (data[4] == ((data[0] + data[1] + data[2] + data[3]) & 0xFF))) { + humidity = (float)((data[0] << 8) + data[1]) / 10; + if (humidity > 100) { + humidity = data[0]; + } + temp_celsius = (float)(((data[2] & 0x7F) << 8) + data[3]) / 10; + if (temp_celsius > 125) { + temp_celsius = data[2]; + } + if (data[2] & 0x80) { + temp_celsius = -temp_celsius; } - dht_report_message[DHT_REPORT_PIN] = (int)dht_pin; - dht_report_message[DHT_HUMIDITY_WHOLE_VALUE] = humidity_int_part; - dht_report_message[DHT_HUMIDITY_FRAC_VALUE] = humidity_dec_part; - dht_report_message[DHT_TEMPERATURE_WHOLE_VALUE] = temp_int_part; - dht_report_message[DHT_TEMPERATURE_FRAC_VALUE] = temp_dec_part; - serial_write(dht_report_message, 7); + nearest = roundf(temp_celsius * 100) / 100; + temp_int_part = (int)nearest; + temp_dec_part = (int)((nearest - temp_int_part) * 100); + + nearest = roundf(humidity * 100) / 100; + humidity_int_part = (int)nearest; + humidity_dec_part = (int)((nearest - humidity_int_part) * 100); + } else { + // bad data return zeros + temp_int_part = temp_dec_part = humidity_int_part = humidity_dec_part = 0; + } + dht_report_message[DHT_REPORT_PIN] = (int)dht_pin; + dht_report_message[DHT_HUMIDITY_WHOLE_VALUE] = humidity_int_part; + dht_report_message[DHT_HUMIDITY_FRAC_VALUE] = humidity_dec_part; + + dht_report_message[DHT_TEMPERATURE_WHOLE_VALUE] = temp_int_part; + dht_report_message[DHT_TEMPERATURE_FRAC_VALUE] = temp_dec_part; + serial_write(dht_report_message, 7); } /************************************************* @@ -1292,82 +1263,74 @@ void read_dht(uint dht_pin) * @param buffer * @param num_of_bytes_to_send */ -void serial_write(const int *buffer, int num_of_bytes_to_send) -{ - for (int i = 0; i < num_of_bytes_to_send; i++) - { - putchar((buffer[i]) & 0x00ff); - } - stdio_flush(); +void serial_write(const int *buffer, int num_of_bytes_to_send) { + for (int i = 0; i < num_of_bytes_to_send; i++) { + putchar((buffer[i]) & 0x00ff); + } + stdio_flush(); } /*************************************************************** * MAIN FUNCTION ****************************************************************/ -int main() -{ - stdio_init_all(); - stdio_set_translate_crlf(&stdio_usb, false); - stdio_set_translate_crlf(&stdio_uart, false); - stdio_flush(); - // uint offset = pio_add_program(pio, &Telemetrix4RpiPico_program); - // ws2812_init(pio, sm, offset, 28, 800000, - // false); - - // stdio_set_translate_crlf(&stdio_usb, false); - adc_init(); - // create an array of pin_descriptors for 100 pins - // establish the digital pin array - for (uint8_t i = 0; i < MAX_DIGITAL_PINS_SUPPORTED; i++) - { - the_digital_pins[i].pin_number = i; - the_digital_pins[i].pin_mode = PIN_MODE_NOT_SET; - the_digital_pins[i].reporting_enabled = false; - the_digital_pins[i].last_value = -1; - } - - // establish the analog pin array - for (uint8_t i = 0; i < MAX_ANALOG_PINS_SUPPORTED; i++) - { - the_analog_pins[i].reporting_enabled = false; - the_analog_pins[i].last_value = -1; - } - - // initialize the sonar structures - sonar_data the_hc_sr04s = {.next_sonar_index = 0, .trigger_mask = 0}; - for (int i = 0; i < MAX_SONARS; i++) - { - the_hc_sr04s.sonars[i].trig_pin = the_hc_sr04s.sonars[i].echo_pin = (uint)-1; - } - - gpio_init(LED_PIN); - gpio_set_dir(LED_PIN, GPIO_OUT); - - // blink the board LED twice to show that the board is - // starting afresh - led_debug(2, 250); - - watchdog_enable(1000, 1); // Add watchdog requiring trigger every 1s - - // infinite loop - uint32_t last_scan = 0; - while (true) - { - watchdog_update(); - get_next_command(); - - if (!stop_reports) - { - if (time_us_32() - last_scan >= (scan_delay*1000)) - { - last_scan = time_us_32(); - scan_digital_inputs(); - scan_analog_inputs(); - scan_sonars(); - scan_dhts(); - scan_encoders(); - } - } +int main() { + stdio_init_all(); + stdio_set_translate_crlf(&stdio_usb, false); + stdio_set_translate_crlf(&stdio_uart, false); + stdio_flush(); + // uint offset = pio_add_program(pio, &Telemetrix4RpiPico_program); + // ws2812_init(pio, sm, offset, 28, 800000, + // false); + + // stdio_set_translate_crlf(&stdio_usb, false); + adc_init(); + // create an array of pin_descriptors for 100 pins + // establish the digital pin array + for (uint8_t i = 0; i < MAX_DIGITAL_PINS_SUPPORTED; i++) { + the_digital_pins[i].pin_number = i; + the_digital_pins[i].pin_mode = PIN_MODE_NOT_SET; + the_digital_pins[i].reporting_enabled = false; + the_digital_pins[i].last_value = -1; + } + + // establish the analog pin array + for (uint8_t i = 0; i < MAX_ANALOG_PINS_SUPPORTED; i++) { + the_analog_pins[i].reporting_enabled = false; + the_analog_pins[i].last_value = -1; + } + + // initialize the sonar structures + sonar_data the_hc_sr04s = {.next_sonar_index = 0, .trigger_mask = 0}; + for (int i = 0; i < MAX_SONARS; i++) { + the_hc_sr04s.sonars[i].trig_pin = the_hc_sr04s.sonars[i].echo_pin = + (uint)-1; + } + + gpio_init(LED_PIN); + gpio_set_dir(LED_PIN, GPIO_OUT); + + // blink the board LED twice to show that the board is + // starting afresh + led_debug(2, 250); + + watchdog_enable(1000, 1); // Add watchdog requiring trigger every 1s + + // infinite loop + uint32_t last_scan = 0; + while (true) { + watchdog_update(); + get_next_command(); + + if (!stop_reports) { + if (time_us_32() - last_scan >= (scan_delay * 1000)) { + last_scan = time_us_32(); + scan_digital_inputs(); + scan_analog_inputs(); + scan_sonars(); + scan_dhts(); + scan_encoders(); + } } + } } diff --git a/include/Telemetrix4RpiPico.h b/include/Telemetrix4RpiPico.hpp similarity index 77% rename from include/Telemetrix4RpiPico.h rename to include/Telemetrix4RpiPico.hpp index ae992f4..bd0d332 100644 --- a/include/Telemetrix4RpiPico.h +++ b/include/Telemetrix4RpiPico.hpp @@ -5,21 +5,22 @@ #ifndef TELEMETRIX4RPIPICO_TELEMETRIX4RPIPICO_H #define TELEMETRIX4RPIPICO_TELEMETRIX4RPIPICO_H -#include -#include -#include -#include "pico/stdlib.h" -#include "hardware/pwm.h" -#include "pico/unique_id.h" -#include "hardware/watchdog.h" +#include "Telemetrix4RpiPico.pio.h" #include "hardware/adc.h" +#include "hardware/clocks.h" #include "hardware/i2c.h" #include "hardware/pio.h" -#include "hardware/clocks.h" +#include "hardware/pwm.h" #include "hardware/spi.h" -#include "Telemetrix4RpiPico.pio.h" +#include "hardware/watchdog.h" #include "math.h" - +#include "pico/stdlib.h" +#include "pico/unique_id.h" +#include +#include +#include +#include +#include /************************** FORWARD REFERENCES *********************** We define all functions here as extern to provide allow forward referencing. @@ -112,7 +113,8 @@ void encoder_new(); #define SET_PIN_MODE 1 #define DIGITAL_WRITE 2 #define PWM_WRITE 3 -#define MODIFY_REPORTING 4 // mode(all, analog, or digital), pin, enable or disable +#define MODIFY_REPORTING \ + 4 // mode(all, analog, or digital), pin, enable or disable #define GET_FIRMWARE_VERSION 5 #define GET_PICO_UNIQUE_ID 6 #define SERVO_ATTACH 7 // unused @@ -139,6 +141,7 @@ void encoder_new(); #define SPI_CS_CONTROL 28 #define SET_SCAN_DELAY 29 #define ENCODER_NEW 30 +const int SENSOR_NEW = 31; /***************************************************** * MESSAGE OFFSETS ***************************************************/ @@ -324,11 +327,7 @@ const uint DHT_MAX_TIMINGS = 85; // Max encoder devices #define MAX_ENCODERS 4 -typedef enum -{ - SINGLE = 1, - QUADRATURE = 2 -} ENCODER_TYPES; +typedef enum { SINGLE = 1, QUADRATURE = 2 } ENCODER_TYPES; // encoder reports are identified by pin A #define ENCODER_REPORT_PIN_A 2 @@ -395,7 +394,7 @@ typedef enum #define SPI_REPORT 13 #define ENCODER_REPORT 14 #define DEBUG_PRINT 99 - +const int SENSOR_REPORT = 20; /*************************************************************** * INPUT PIN REPORTING CONTROL SUB COMMANDS ***************************************************************/ @@ -420,80 +419,125 @@ typedef enum #define I2C_NO_REGISTER_SPECIFIED 254 // a descriptor for digital pins -typedef struct -{ - uint pin_number; - uint pin_mode; - uint reporting_enabled; // If true, then send reports if an input pin - int last_value; // Last value read for input mode +typedef struct { + uint pin_number; + uint pin_mode; + uint reporting_enabled; // If true, then send reports if an input pin + int last_value; // Last value read for input mode } pin_descriptor; // a descriptor for analog pins -typedef struct analog_pin_descriptor -{ - uint reporting_enabled; // If true, then send reports if an input pin - int last_value; // Last value read for input mode - int differential; // difference between current and last value needed +typedef struct analog_pin_descriptor { + uint reporting_enabled; // If true, then send reports if an input pin + int last_value; // Last value read for input mode + int differential; // difference between current and last value needed } analog_pin_descriptor; // This structure describes an HC-SR04 type device -typedef struct hc_sr04_descriptor -{ - uint trig_pin; // trigger pin - uint echo_pin; // echo pin - uint32_t start_time; - uint32_t last_time_diff; +typedef struct hc_sr04_descriptor { + uint trig_pin; // trigger pin + uint echo_pin; // echo pin + uint32_t start_time; + uint32_t last_time_diff; } hc_sr04_descriptor; // this structure holds an index into the sonars array // and the sonars array -typedef struct sonar_data -{ - int next_sonar_index; - repeating_timer_t trigger_timer; - uint32_t trigger_mask; - hc_sr04_descriptor sonars[MAX_SONARS]; +typedef struct sonar_data { + int next_sonar_index; + repeating_timer_t trigger_timer; + uint32_t trigger_mask; + hc_sr04_descriptor sonars[MAX_SONARS]; } sonar_data; // this structure describes a DHT type device -typedef struct dht_descriptor -{ - uint data_pin; // data pin - absolute_time_t previous_time; - /* for possible future use - int last_val_whole; // value on right side of decimal - int last_val_frac; // value on left side of decimal - */ +typedef struct dht_descriptor { + uint data_pin; // data pin + absolute_time_t previous_time; + /* for possible future use + int last_val_whole; // value on right side of decimal + int last_val_frac; // value on left side of decimal + */ } dht_descriptor; // this structure holds an index into the dht array // and the dhts array -typedef struct dht_data -{ - int next_dht_index; - dht_descriptor dhts[MAX_DHTS]; +typedef struct dht_data { + int next_dht_index; + dht_descriptor dhts[MAX_DHTS]; } dht_data; // encoder type -typedef struct -{ - ENCODER_TYPES type; - int A; - int B; - int8_t step; - int last_state; +typedef struct { + ENCODER_TYPES type; + int A; + int B; + int8_t step; + int last_state; } encoder_t; -typedef struct -{ - int next_encoder_index; - repeating_timer_t trigger_timer; - encoder_t encoders[MAX_ENCODERS]; +typedef struct { + int next_encoder_index; + repeating_timer_t trigger_timer; + encoder_t encoders[MAX_ENCODERS]; } encoder_data; encoder_data encoders; -typedef struct -{ - // a pointer to the command processing function - void (*command_func)(void); +typedef struct { + // a pointer to the command processing function + void (*command_func)(void); } command_descriptor; + +/*****************************************************************************/ +/****SENSORS*/ +/*****************************************************************************/ + +enum SENSOR_TYPES : uint8_t { // Max 255 sensors, but will always fit in a + // single byte! + GPS = 0, + LOAD_CELL = 1, + MPU_9250 = 2, + TOF_VL53 = 3, + VEML6040 = 4, // Color sensor + ADXL345 = 5, // 3 axis accel + MAX_SENSORS +}; +class Sensor { +public: + virtual void readSensor(); + bool stop = false; + void writeSensorData(std::vector data); + int num; + SENSOR_TYPES type = SENSOR_TYPES::MAX_SENSORS; +}; +const int SENSORS_MAX_SETTINGS_A = 6; +class GPS_Sensor : public Sensor { +public: + GPS_Sensor(int num, uint8_t settings[SENSORS_MAX_SETTINGS_A]); + void readSensor(); + +private: +}; +class ADXL345_Sensor : public Sensor { +public: + ADXL345_Sensor(int num, uint8_t settings[SENSORS_MAX_SETTINGS_A]); + void readSensor(); + +private: + void init_sequence(); + int num; + int i2c_port; + int i2c_addr = 83; +}; +void sensor_new(); +void addSensor(uint8_t data[], size_t len); +void serial_write(std::vector data); +struct Sensor_new_command { + SENSOR_TYPES type; + uint8_t sensor_number; + uint8_t settings[SENSORS_MAX_SETTINGS_A]; // max length of sensor settings +}; +static_assert(sizeof(Sensor_new_command) == SENSORS_MAX_SETTINGS_A + 2); +std::vector sensors; +void reportBytes(std::vector); + #endif // TELEMETRIX4RPIPICO_TELEMETRIX4RPIPICO_H From d452db92600eda5bf2eb4d57db0767163f147ec4 Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Tue, 12 Sep 2023 12:22:56 +0200 Subject: [PATCH 05/33] Fix sensors with py code --- Telemetrix4RpiPico.cpp | 45 ++++++++++++++++++++++------------ flashLoop.sh | 10 ++++++++ include/Telemetrix4RpiPico.hpp | 11 +++------ 3 files changed, 43 insertions(+), 23 deletions(-) create mode 100755 flashLoop.sh diff --git a/Telemetrix4RpiPico.cpp b/Telemetrix4RpiPico.cpp index ba8ecf5..e3e28c4 100644 --- a/Telemetrix4RpiPico.cpp +++ b/Telemetrix4RpiPico.cpp @@ -1066,24 +1066,34 @@ void scan_sonars() { // SENSORSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSS void sensor_new() { - const SENSOR_TYPES type = (SENSOR_TYPES)command_buffer[0]; + const SENSOR_TYPES type = (SENSOR_TYPES)command_buffer[2]; const uint8_t sensor_num = command_buffer[1]; uint8_t sensor_data[SENSORS_MAX_SETTINGS_A]; - std::copy(command_buffer + 2, command_buffer + 8, sensor_data); - + std::copy(command_buffer + 3, command_buffer + 3 + SENSORS_MAX_SETTINGS_A, sensor_data); if (type >= SENSOR_TYPES::MAX_SENSORS) { return; } Sensor *sensor = nullptr; + if (type == GPS) { - sensor = new GPS_Sensor(sensor_num, sensor_data); + sensor = new GPS_Sensor(sensor_data); } else if (type == SENSOR_TYPES::ADXL345) { - sensor = new ADXL345_Sensor(sensor_num, sensor_data); + send_debug_info(5,92); + sensor = new ADXL345_Sensor(sensor_data); } + sensor->type = type; + sensor->num = sensor_num; + sensors.push_back(sensor); } -GPS_Sensor::GPS_Sensor(int num, uint8_t settings[SENSORS_MAX_SETTINGS_A]) { +void readSensors() { + for(auto& sensor: sensors) { + sensor->readSensor(); + } +} + +GPS_Sensor::GPS_Sensor( uint8_t settings[SENSORS_MAX_SETTINGS_A]) { // TODO: read uart, forward to serial } void GPS_Sensor::readSensor() { @@ -1126,27 +1136,30 @@ int read_i2c(int i2c_port, int addr, const std::vector &write_bytes, return i2c_sdk_return == bytes_to_read; } -ADXL345_Sensor::ADXL345_Sensor(int num, +ADXL345_Sensor::ADXL345_Sensor( uint8_t settings[SENSORS_MAX_SETTINGS_A]) { - // assume i2c is done + // assume i2c is done this->i2c_port = settings[0]; - this->init_sequence(); + this->init_sequence(); } void ADXL345_Sensor::init_sequence() { // write 83, 43, 0 - bool ok = write_i2c(this->i2c_port, this->i2c_addr, {43, 0}); + bool ok = write_i2c(this->i2c_port, this->i2c_addr, {43, 0}); // write 83, 45, 8 - ok |= write_i2c(this->i2c_port, this->i2c_addr, {45, 8}); + ok &= write_i2c(this->i2c_port, this->i2c_addr, {45, 8}); // write 83, 49, 8 - ok |= write_i2c(this->i2c_port, this->i2c_addr, {49, 8}); - if (!ok) { + ok &= write_i2c(this->i2c_port, this->i2c_addr, {49, 8}); + if (!ok) { this->stop = true; } } void ADXL345_Sensor::readSensor() { + if(this.stop) { + return; + } // read 83, 50, 6bytes std::vector out(6); - read_i2c(this->i2c_port, this->i2c_addr, {50}, 6, out); + this.stop = !read_i2c(this->i2c_port, this->i2c_addr, {50}, 6, out); this->writeSensorData(out); } @@ -1159,7 +1172,8 @@ void Sensor::writeSensorData(std::vector data) { this->type, // write sensor type }; out.insert(out.end(), data.begin(), data.end()); - out[0] = out.size(); + out[0] = out.size() -1 ; // dont count the packet length + serial_write(out); } @@ -1330,6 +1344,7 @@ int main() { scan_sonars(); scan_dhts(); scan_encoders(); + readSensors(); } } } diff --git a/flashLoop.sh b/flashLoop.sh new file mode 100755 index 0000000..857ccbc --- /dev/null +++ b/flashLoop.sh @@ -0,0 +1,10 @@ + +while true +do +echo "keep plugged" +sudo picotool load -F build/Telemetrix4RpiPico.uf2 +sudo picotool reboot +echo "unplug" +sleep 2 + +done \ No newline at end of file diff --git a/include/Telemetrix4RpiPico.hpp b/include/Telemetrix4RpiPico.hpp index bd0d332..4569cbe 100644 --- a/include/Telemetrix4RpiPico.hpp +++ b/include/Telemetrix4RpiPico.hpp @@ -512,14 +512,14 @@ class Sensor { const int SENSORS_MAX_SETTINGS_A = 6; class GPS_Sensor : public Sensor { public: - GPS_Sensor(int num, uint8_t settings[SENSORS_MAX_SETTINGS_A]); + GPS_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]); void readSensor(); private: }; class ADXL345_Sensor : public Sensor { public: - ADXL345_Sensor(int num, uint8_t settings[SENSORS_MAX_SETTINGS_A]); + ADXL345_Sensor( uint8_t settings[SENSORS_MAX_SETTINGS_A]); void readSensor(); private: @@ -530,13 +530,8 @@ class ADXL345_Sensor : public Sensor { }; void sensor_new(); void addSensor(uint8_t data[], size_t len); +void readSensors(); void serial_write(std::vector data); -struct Sensor_new_command { - SENSOR_TYPES type; - uint8_t sensor_number; - uint8_t settings[SENSORS_MAX_SETTINGS_A]; // max length of sensor settings -}; -static_assert(sizeof(Sensor_new_command) == SENSORS_MAX_SETTINGS_A + 2); std::vector sensors; void reportBytes(std::vector); From 62f8c63c4d9f6bce2979e9c5968732cac25384fd Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Tue, 12 Sep 2023 12:23:51 +0200 Subject: [PATCH 06/33] fix pointersss --- Telemetrix4RpiPico.cpp | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/Telemetrix4RpiPico.cpp b/Telemetrix4RpiPico.cpp index e3e28c4..d0956f3 100644 --- a/Telemetrix4RpiPico.cpp +++ b/Telemetrix4RpiPico.cpp @@ -1069,7 +1069,8 @@ void sensor_new() { const SENSOR_TYPES type = (SENSOR_TYPES)command_buffer[2]; const uint8_t sensor_num = command_buffer[1]; uint8_t sensor_data[SENSORS_MAX_SETTINGS_A]; - std::copy(command_buffer + 3, command_buffer + 3 + SENSORS_MAX_SETTINGS_A, sensor_data); + std::copy(command_buffer + 3, command_buffer + 3 + SENSORS_MAX_SETTINGS_A, + sensor_data); if (type >= SENSOR_TYPES::MAX_SENSORS) { return; } @@ -1078,7 +1079,7 @@ void sensor_new() { if (type == GPS) { sensor = new GPS_Sensor(sensor_data); } else if (type == SENSOR_TYPES::ADXL345) { - send_debug_info(5,92); + send_debug_info(5, 92); sensor = new ADXL345_Sensor(sensor_data); } sensor->type = type; @@ -1088,12 +1089,12 @@ void sensor_new() { } void readSensors() { - for(auto& sensor: sensors) { + for (auto &sensor : sensors) { sensor->readSensor(); } } -GPS_Sensor::GPS_Sensor( uint8_t settings[SENSORS_MAX_SETTINGS_A]) { +GPS_Sensor::GPS_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]) { // TODO: read uart, forward to serial } void GPS_Sensor::readSensor() { @@ -1136,30 +1137,29 @@ int read_i2c(int i2c_port, int addr, const std::vector &write_bytes, return i2c_sdk_return == bytes_to_read; } -ADXL345_Sensor::ADXL345_Sensor( - uint8_t settings[SENSORS_MAX_SETTINGS_A]) { - // assume i2c is done +ADXL345_Sensor::ADXL345_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]) { + // assume i2c is done this->i2c_port = settings[0]; - this->init_sequence(); + this->init_sequence(); } void ADXL345_Sensor::init_sequence() { // write 83, 43, 0 - bool ok = write_i2c(this->i2c_port, this->i2c_addr, {43, 0}); + bool ok = write_i2c(this->i2c_port, this->i2c_addr, {43, 0}); // write 83, 45, 8 ok &= write_i2c(this->i2c_port, this->i2c_addr, {45, 8}); // write 83, 49, 8 ok &= write_i2c(this->i2c_port, this->i2c_addr, {49, 8}); - if (!ok) { + if (!ok) { this->stop = true; } } void ADXL345_Sensor::readSensor() { - if(this.stop) { + if (this->stop) { return; } // read 83, 50, 6bytes std::vector out(6); - this.stop = !read_i2c(this->i2c_port, this->i2c_addr, {50}, 6, out); + this->stop = !read_i2c(this->i2c_port, this->i2c_addr, {50}, 6, out); this->writeSensorData(out); } @@ -1172,7 +1172,7 @@ void Sensor::writeSensorData(std::vector data) { this->type, // write sensor type }; out.insert(out.end(), data.begin(), data.end()); - out[0] = out.size() -1 ; // dont count the packet length + out[0] = out.size() - 1; // dont count the packet length serial_write(out); } From 5cb7f5edc34d576ff821677ab11e00837d8fbf19 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Tue, 12 Sep 2023 13:58:41 +0200 Subject: [PATCH 07/33] move to src folder --- CMakeLists.txt | 5 +++-- Telemetrix4RpiPico.cpp => src/Telemetrix4RpiPico.cpp | 2 +- Telemetrix4RpiPico.pio => src/Telemetrix4RpiPico.pio | 0 3 files changed, 4 insertions(+), 3 deletions(-) rename Telemetrix4RpiPico.cpp => src/Telemetrix4RpiPico.cpp (99%) rename Telemetrix4RpiPico.pio => src/Telemetrix4RpiPico.pio (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 74da990..9d46765 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,10 +6,11 @@ include(pico_sdk_import.cmake) project(Telemetrix4RpiPico) pico_sdk_init() add_executable(Telemetrix4RpiPico - Telemetrix4RpiPico.cpp + src/Telemetrix4RpiPico.cpp ) +target_include_directories(Telemetrix4RpiPico PUBLIC ${CMAKE_CURRENT_LIST_DIR}/include/) # target_compile_options(Telemetrix4RpiPico PRIVATE -Wall -Wextra -Wpedantic -Werror) -pico_generate_pio_header(Telemetrix4RpiPico ${CMAKE_CURRENT_LIST_DIR}/Telemetrix4RpiPico.pio) +pico_generate_pio_header(Telemetrix4RpiPico ${CMAKE_CURRENT_LIST_DIR}/src/Telemetrix4RpiPico.pio) pico_enable_stdio_usb(Telemetrix4RpiPico 1) pico_enable_stdio_uart(Telemetrix4RpiPico 1) pico_add_extra_outputs(Telemetrix4RpiPico) diff --git a/Telemetrix4RpiPico.cpp b/src/Telemetrix4RpiPico.cpp similarity index 99% rename from Telemetrix4RpiPico.cpp rename to src/Telemetrix4RpiPico.cpp index d0956f3..1a79776 100644 --- a/Telemetrix4RpiPico.cpp +++ b/src/Telemetrix4RpiPico.cpp @@ -28,7 +28,7 @@ * *************************************************************************/ -#include "include/Telemetrix4RpiPico.hpp" +#include "Telemetrix4RpiPico.hpp" // #include "pico/stdio_uart.h" /******************************************************************* diff --git a/Telemetrix4RpiPico.pio b/src/Telemetrix4RpiPico.pio similarity index 100% rename from Telemetrix4RpiPico.pio rename to src/Telemetrix4RpiPico.pio From 0d0f1f7cb326555d8180603c76d7732bf46eb3c0 Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Tue, 12 Sep 2023 14:07:36 +0200 Subject: [PATCH 08/33] start veml --- include/Telemetrix4RpiPico.hpp | 12 +++++++++++- src/Telemetrix4RpiPico.cpp | 6 ++++++ 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/include/Telemetrix4RpiPico.hpp b/include/Telemetrix4RpiPico.hpp index 4569cbe..bc87de5 100644 --- a/include/Telemetrix4RpiPico.hpp +++ b/include/Telemetrix4RpiPico.hpp @@ -524,10 +524,20 @@ class ADXL345_Sensor : public Sensor { private: void init_sequence(); - int num; int i2c_port; int i2c_addr = 83; }; + +class VEML6040_Sensor : public Sensor { + public: + VEML6040_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]); + void readSensor(); + private: + void init_sequence(); + int i2c_port = 0; + int i2c_addr = 0x10; +} + void sensor_new(); void addSensor(uint8_t data[], size_t len); void readSensors(); diff --git a/src/Telemetrix4RpiPico.cpp b/src/Telemetrix4RpiPico.cpp index 1a79776..98ecf4e 100644 --- a/src/Telemetrix4RpiPico.cpp +++ b/src/Telemetrix4RpiPico.cpp @@ -1081,6 +1081,8 @@ void sensor_new() { } else if (type == SENSOR_TYPES::ADXL345) { send_debug_info(5, 92); sensor = new ADXL345_Sensor(sensor_data); + } else if (type == SENSOR_TYPES::VEML6040) { + sensor = new VEML6040_Sensor(sensor_data); } sensor->type = type; sensor->num = sensor_num; @@ -1164,6 +1166,10 @@ void ADXL345_Sensor::readSensor() { this->writeSensorData(out); } +VEML6040_Sensor::VEML6040_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A] ) { + +} + void Sensor::writeSensorData(std::vector data) { std::vector out = { 0, // write len From 3be3f19c60d6c161272bf6b08c81e3aa21c90793 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Tue, 12 Sep 2023 14:37:41 +0200 Subject: [PATCH 09/33] Add veml --- include/Telemetrix4RpiPico.hpp | 10 ++++++---- src/Telemetrix4RpiPico.cpp | 34 +++++++++++++++++++++++++++++++++- 2 files changed, 39 insertions(+), 5 deletions(-) diff --git a/include/Telemetrix4RpiPico.hpp b/include/Telemetrix4RpiPico.hpp index bc87de5..69da201 100644 --- a/include/Telemetrix4RpiPico.hpp +++ b/include/Telemetrix4RpiPico.hpp @@ -519,7 +519,7 @@ class GPS_Sensor : public Sensor { }; class ADXL345_Sensor : public Sensor { public: - ADXL345_Sensor( uint8_t settings[SENSORS_MAX_SETTINGS_A]); + ADXL345_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]); void readSensor(); private: @@ -529,14 +529,16 @@ class ADXL345_Sensor : public Sensor { }; class VEML6040_Sensor : public Sensor { - public: +public: VEML6040_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]); void readSensor(); - private: + +private: void init_sequence(); + void readColor(uint8_t colorOffset, std::vector &single_color_data); int i2c_port = 0; int i2c_addr = 0x10; -} +}; void sensor_new(); void addSensor(uint8_t data[], size_t len); diff --git a/src/Telemetrix4RpiPico.cpp b/src/Telemetrix4RpiPico.cpp index 98ecf4e..1e12bd4 100644 --- a/src/Telemetrix4RpiPico.cpp +++ b/src/Telemetrix4RpiPico.cpp @@ -1166,8 +1166,40 @@ void ADXL345_Sensor::readSensor() { this->writeSensorData(out); } -VEML6040_Sensor::VEML6040_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A] ) { +VEML6040_Sensor::VEML6040_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]) { + this->init_sequence(); +} + +void VEML6040_Sensor::init_sequence() { + bool ok = write_i2c(this->i2c_port, this->i2c_addr, + { + 0, // register 0 + 0b000 << 4 | // timing 40ms + 0b0 << 2 | // no trigger + 0b0 << 1 | // auto mode + 0b0, // enable sensor + 0b0 // reserved H byte + }); +} +void VEML6040_Sensor::readSensor() { + if (!this->stop) { + return; + } + std::vector data(8); + std::vector single_color_data(2); + bool ok = true; + for (uint8_t reg = 0x08; reg <= 0x0B; + reg++) { // read the 4 registers and add the data to the full data vector + ok &= read_i2c(this->i2c_port, this->i2c_addr, {reg}, 2, single_color_data); + data.push_back(single_color_data[0]); + data.push_back(single_color_data[1]); + } + + this->writeSensorData(data); + if (!ok) { + this->stop = true; + } } void Sensor::writeSensorData(std::vector data) { From 456d2c5036f84392b7451bb09225aecef6a7256f Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Tue, 12 Sep 2023 14:38:12 +0200 Subject: [PATCH 10/33] Add i2c port --- src/Telemetrix4RpiPico.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/Telemetrix4RpiPico.cpp b/src/Telemetrix4RpiPico.cpp index 1e12bd4..ba4b5d3 100644 --- a/src/Telemetrix4RpiPico.cpp +++ b/src/Telemetrix4RpiPico.cpp @@ -1167,6 +1167,7 @@ void ADXL345_Sensor::readSensor() { } VEML6040_Sensor::VEML6040_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]) { + this->i2c_port = settings[0]; this->init_sequence(); } From a09ec086eaded55285abe6fca0e8254c5355628f Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Tue, 12 Sep 2023 15:33:28 +0200 Subject: [PATCH 11/33] Done veml --- scripts/upload.sh | 5 +++++ src/Telemetrix4RpiPico.cpp | 16 ++++++++-------- 2 files changed, 13 insertions(+), 8 deletions(-) create mode 100755 scripts/upload.sh diff --git a/scripts/upload.sh b/scripts/upload.sh new file mode 100755 index 0000000..b1ace25 --- /dev/null +++ b/scripts/upload.sh @@ -0,0 +1,5 @@ +#!/bin/bash + +cd build/ +sudo picotool load Telemetrix4RpiPico.uf2 -F +sudo picotool reboot \ No newline at end of file diff --git a/src/Telemetrix4RpiPico.cpp b/src/Telemetrix4RpiPico.cpp index ba4b5d3..8e17f70 100644 --- a/src/Telemetrix4RpiPico.cpp +++ b/src/Telemetrix4RpiPico.cpp @@ -1079,7 +1079,6 @@ void sensor_new() { if (type == GPS) { sensor = new GPS_Sensor(sensor_data); } else if (type == SENSOR_TYPES::ADXL345) { - send_debug_info(5, 92); sensor = new ADXL345_Sensor(sensor_data); } else if (type == SENSOR_TYPES::VEML6040) { sensor = new VEML6040_Sensor(sensor_data); @@ -1103,7 +1102,7 @@ void GPS_Sensor::readSensor() { // TODO: read uart, forward to serial } -int write_i2c(int i2c_port, int addr, const std::vector &bytes) { +int write_i2c(int i2c_port, int addr, const std::vector &bytes, bool nostop=false) { i2c_inst_t *i2c; if (i2c_port) { @@ -1113,7 +1112,7 @@ int write_i2c(int i2c_port, int addr, const std::vector &bytes) { } int i2c_sdk_call_return_value = i2c_write_blocking_until(i2c, (uint8_t)addr, bytes.data(), bytes.size(), - false, make_timeout_time_ms(50)); + nostop, make_timeout_time_ms(50)); return i2c_sdk_call_return_value == bytes.size(); } @@ -1122,7 +1121,7 @@ int read_i2c(int i2c_port, int addr, const std::vector &write_bytes, // write i2c if (!write_bytes.empty()) { - write_i2c(i2c_port, addr, write_bytes); + write_i2c(i2c_port, addr, write_bytes, true); } i2c_inst_t *i2c; @@ -1184,17 +1183,18 @@ void VEML6040_Sensor::init_sequence() { } void VEML6040_Sensor::readSensor() { - if (!this->stop) { + if (this->stop) { return; } - std::vector data(8); + std::vector data; + data.reserve(8); std::vector single_color_data(2); bool ok = true; for (uint8_t reg = 0x08; reg <= 0x0B; reg++) { // read the 4 registers and add the data to the full data vector ok &= read_i2c(this->i2c_port, this->i2c_addr, {reg}, 2, single_color_data); - data.push_back(single_color_data[0]); - data.push_back(single_color_data[1]); + data.push_back(single_color_data[0] ); + data.push_back(single_color_data[1] ); } this->writeSensorData(data); From b71b021bcee74d5b3f42195742492b8fcba661d6 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Tue, 12 Sep 2023 16:28:51 +0200 Subject: [PATCH 12/33] Add VL53L0 --- CMakeLists.txt | 2 + include/Telemetrix4RpiPico.hpp | 16 +- include/VL53L0.hpp | 180 ++++++ include/i2c_helpers.hpp | 11 + src/Telemetrix4RpiPico.cpp | 579 +++++++++++------ src/VL53L0.cpp | 1095 ++++++++++++++++++++++++++++++++ src/i2c_helpers.cpp | 47 ++ 7 files changed, 1724 insertions(+), 206 deletions(-) create mode 100644 include/VL53L0.hpp create mode 100644 include/i2c_helpers.hpp create mode 100644 src/VL53L0.cpp create mode 100644 src/i2c_helpers.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9d46765..fda85d7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,6 +7,8 @@ project(Telemetrix4RpiPico) pico_sdk_init() add_executable(Telemetrix4RpiPico src/Telemetrix4RpiPico.cpp + src/VL53L0.cpp + src/i2c_helpers.cpp ) target_include_directories(Telemetrix4RpiPico PUBLIC ${CMAKE_CURRENT_LIST_DIR}/include/) # target_compile_options(Telemetrix4RpiPico PRIVATE -Wall -Wextra -Wpedantic -Werror) diff --git a/include/Telemetrix4RpiPico.hpp b/include/Telemetrix4RpiPico.hpp index 69da201..39bb7cc 100644 --- a/include/Telemetrix4RpiPico.hpp +++ b/include/Telemetrix4RpiPico.hpp @@ -6,6 +6,7 @@ #define TELEMETRIX4RPIPICO_TELEMETRIX4RPIPICO_H #include "Telemetrix4RpiPico.pio.h" +#include "i2c_helpers.hpp" #include "hardware/adc.h" #include "hardware/clocks.h" #include "hardware/i2c.h" @@ -21,6 +22,7 @@ #include #include #include +#include "VL53L0.hpp" /************************** FORWARD REFERENCES *********************** We define all functions here as extern to provide allow forward referencing. @@ -535,11 +537,23 @@ class VEML6040_Sensor : public Sensor { private: void init_sequence(); - void readColor(uint8_t colorOffset, std::vector &single_color_data); int i2c_port = 0; int i2c_addr = 0x10; }; +class VL53L0X_Sensor : public Sensor { +public: + VL53L0X_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]); + void readSensor(); + +private: +VL53L0X sensor; + void init_sequence(); + int i2c_port = 0; + int i2c_addr = 0x52; +}; + + void sensor_new(); void addSensor(uint8_t data[], size_t len); void readSensors(); diff --git a/include/VL53L0.hpp b/include/VL53L0.hpp new file mode 100644 index 0000000..4992a21 --- /dev/null +++ b/include/VL53L0.hpp @@ -0,0 +1,180 @@ +#ifndef VL53L0X_h +#define VL53L0X_h +#include +#include +#include +#include "i2c_helpers.hpp" +class VL53L0X +{ + public: + // register addresses from API vl53l0x_device.h (ordered as listed there) + enum regAddr + { + SYSRANGE_START = 0x00, + + SYSTEM_THRESH_HIGH = 0x0C, + SYSTEM_THRESH_LOW = 0x0E, + + SYSTEM_SEQUENCE_CONFIG = 0x01, + SYSTEM_RANGE_CONFIG = 0x09, + SYSTEM_INTERMEASUREMENT_PERIOD = 0x04, + + SYSTEM_INTERRUPT_CONFIG_GPIO = 0x0A, + + GPIO_HV_MUX_ACTIVE_HIGH = 0x84, + + SYSTEM_INTERRUPT_CLEAR = 0x0B, + + RESULT_INTERRUPT_STATUS = 0x13, + RESULT_RANGE_STATUS = 0x14, + + RESULT_CORE_AMBIENT_WINDOW_EVENTS_RTN = 0xBC, + RESULT_CORE_RANGING_TOTAL_EVENTS_RTN = 0xC0, + RESULT_CORE_AMBIENT_WINDOW_EVENTS_REF = 0xD0, + RESULT_CORE_RANGING_TOTAL_EVENTS_REF = 0xD4, + RESULT_PEAK_SIGNAL_RATE_REF = 0xB6, + + ALGO_PART_TO_PART_RANGE_OFFSET_MM = 0x28, + + I2C_SLAVE_DEVICE_ADDRESS = 0x8A, + + MSRC_CONFIG_CONTROL = 0x60, + + PRE_RANGE_CONFIG_MIN_SNR = 0x27, + PRE_RANGE_CONFIG_VALID_PHASE_LOW = 0x56, + PRE_RANGE_CONFIG_VALID_PHASE_HIGH = 0x57, + PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT = 0x64, + + FINAL_RANGE_CONFIG_MIN_SNR = 0x67, + FINAL_RANGE_CONFIG_VALID_PHASE_LOW = 0x47, + FINAL_RANGE_CONFIG_VALID_PHASE_HIGH = 0x48, + FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT = 0x44, + + PRE_RANGE_CONFIG_SIGMA_THRESH_HI = 0x61, + PRE_RANGE_CONFIG_SIGMA_THRESH_LO = 0x62, + + PRE_RANGE_CONFIG_VCSEL_PERIOD = 0x50, + PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI = 0x51, + PRE_RANGE_CONFIG_TIMEOUT_MACROP_LO = 0x52, + + SYSTEM_HISTOGRAM_BIN = 0x81, + HISTOGRAM_CONFIG_INITIAL_PHASE_SELECT = 0x33, + HISTOGRAM_CONFIG_READOUT_CTRL = 0x55, + + FINAL_RANGE_CONFIG_VCSEL_PERIOD = 0x70, + FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI = 0x71, + FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO = 0x72, + CROSSTALK_COMPENSATION_PEAK_RATE_MCPS = 0x20, + + MSRC_CONFIG_TIMEOUT_MACROP = 0x46, + + SOFT_RESET_GO2_SOFT_RESET_N = 0xBF, + IDENTIFICATION_MODEL_ID = 0xC0, + IDENTIFICATION_REVISION_ID = 0xC2, + + OSC_CALIBRATE_VAL = 0xF8, + + GLOBAL_CONFIG_VCSEL_WIDTH = 0x32, + GLOBAL_CONFIG_SPAD_ENABLES_REF_0 = 0xB0, + GLOBAL_CONFIG_SPAD_ENABLES_REF_1 = 0xB1, + GLOBAL_CONFIG_SPAD_ENABLES_REF_2 = 0xB2, + GLOBAL_CONFIG_SPAD_ENABLES_REF_3 = 0xB3, + GLOBAL_CONFIG_SPAD_ENABLES_REF_4 = 0xB4, + GLOBAL_CONFIG_SPAD_ENABLES_REF_5 = 0xB5, + + GLOBAL_CONFIG_REF_EN_START_SELECT = 0xB6, + DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD = 0x4E, + DYNAMIC_SPAD_REF_EN_START_OFFSET = 0x4F, + POWER_MANAGEMENT_GO1_POWER_FORCE = 0x80, + + VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV = 0x89, + + ALGO_PHASECAL_LIM = 0x30, + ALGO_PHASECAL_CONFIG_TIMEOUT = 0x30, + }; + + enum vcselPeriodType { VcselPeriodPreRange, VcselPeriodFinalRange }; + + uint8_t last_status; // status of last I2C transmission + + VL53L0X(); + + void setBus(uint8_t i2c_bus) { this->bus = bus; } + uint8_t getBus() { return bus; } + + void setAddress(uint8_t new_addr); + inline uint8_t getAddress() { return address; } + + bool init(bool io_2v8 = true); + + void writeReg(uint8_t reg, uint8_t value); + void writeReg16Bit(uint8_t reg, uint16_t value); + void writeReg32Bit(uint8_t reg, uint32_t value); + uint8_t readReg(uint8_t reg); + uint16_t readReg16Bit(uint8_t reg); + uint32_t readReg32Bit(uint8_t reg); + + void writeMulti(uint8_t reg, uint8_t const * src, uint8_t count); + void readMulti(uint8_t reg, uint8_t * dst, uint8_t count); + + bool setSignalRateLimit(float limit_Mcps); + float getSignalRateLimit(); + + bool setMeasurementTimingBudget(uint32_t budget_us); + uint32_t getMeasurementTimingBudget(); + + bool setVcselPulsePeriod(vcselPeriodType type, uint8_t period_pclks); + uint8_t getVcselPulsePeriod(vcselPeriodType type); + + void startContinuous(uint32_t period_ms = 0); + void stopContinuous(); + uint16_t readRangeContinuousMillimeters(); + uint16_t readRangeSingleMillimeters(); + + inline void setTimeout(uint16_t timeout) { io_timeout = timeout; } + inline uint16_t getTimeout() { return io_timeout; } + bool timeoutOccurred(); + + private: + // TCC: Target CentreCheck + // MSRC: Minimum Signal Rate Check + // DSS: Dynamic Spad Selection + + struct SequenceStepEnables + { + bool tcc, msrc, dss, pre_range, final_range; + }; + + struct SequenceStepTimeouts + { + uint16_t pre_range_vcsel_period_pclks, final_range_vcsel_period_pclks; + + uint16_t msrc_dss_tcc_mclks, pre_range_mclks, final_range_mclks; + uint32_t msrc_dss_tcc_us, pre_range_us, final_range_us; + }; + + uint8_t bus; + uint8_t address; + uint16_t io_timeout; + bool did_timeout; + uint16_t timeout_start_ms; + + uint8_t stop_variable; // read by init and used when starting measurement; is StopVariable field of VL53L0X_DevData_t structure in API + uint32_t measurement_timing_budget_us; + + bool getSpadInfo(uint8_t * count, bool * type_is_aperture); + + void getSequenceStepEnables(SequenceStepEnables * enables); + void getSequenceStepTimeouts(SequenceStepEnables const * enables, SequenceStepTimeouts * timeouts); + + bool performSingleRefCalibration(uint8_t vhv_init_byte); + + static uint16_t decodeTimeout(uint16_t value); + static uint16_t encodeTimeout(uint32_t timeout_mclks); + static uint32_t timeoutMclksToMicroseconds(uint16_t timeout_period_mclks, uint8_t vcsel_period_pclks); + static uint32_t timeoutMicrosecondsToMclks(uint32_t timeout_period_us, uint8_t vcsel_period_pclks); +}; + +#endif + + diff --git a/include/i2c_helpers.hpp b/include/i2c_helpers.hpp new file mode 100644 index 0000000..6748269 --- /dev/null +++ b/include/i2c_helpers.hpp @@ -0,0 +1,11 @@ + +#pragma once + +#include +#include +#include "hardware/i2c.h" + +int write_i2c(int i2c_port, int addr, const std::vector &bytes, bool nostop = false); + +int read_i2c(int i2c_port, int addr, const std::vector &write_bytes, + int bytes_to_read, std::vector &read_bytes); \ No newline at end of file diff --git a/src/Telemetrix4RpiPico.cpp b/src/Telemetrix4RpiPico.cpp index 8e17f70..662fb02 100644 --- a/src/Telemetrix4RpiPico.cpp +++ b/src/Telemetrix4RpiPico.cpp @@ -75,11 +75,13 @@ uint actual_number_of_pixels; // scan delay uint8_t scan_delay = 10; -static inline void put_pixel(uint32_t pixel_grb) { +static inline void put_pixel(uint32_t pixel_grb) +{ pio_sm_put_blocking(pio0, 0, pixel_grb << 8u); } -static inline uint32_t urgb_u32(uint8_t r, uint8_t g, uint8_t b) { +static inline uint32_t urgb_u32(uint8_t r, uint8_t g, uint8_t b) +{ return ((uint32_t)(r) << 8) | ((uint32_t)(g) << 16) | (uint32_t)(b); } @@ -126,7 +128,13 @@ int sonar_report_message[] = {4, SONAR_DISTANCE, SONAR_TRIG_PIN, M_WHOLE_VALUE, // dht report message int dht_report_message[] = { - 6, DHT_REPORT, 0, 0, 0, 0, 0, + 6, + DHT_REPORT, + 0, + 0, + 0, + 0, + 0, }; /***************************************************************** @@ -178,7 +186,8 @@ command_descriptor command_table[] = {{&serial_loopback}, /************************************************************ * Loop back the received character */ -void serial_loopback() { +void serial_loopback() +{ loop_back_report_message[LOOP_BACK_DATA] = command_buffer[DATA_TO_LOOP_BACK]; serial_write(loop_back_report_message, sizeof(loop_back_report_message) / sizeof(int)); @@ -190,7 +199,8 @@ void serial_loopback() { * @param value: 16 bit value */ // A method to send debug data across the serial link -void send_debug_info(uint id, uint value) { +void send_debug_info(uint id, uint value) +{ debug_info_report_message[DEBUG_ID] = id; debug_info_report_message[DEBUG_VALUE_HIGH_BYTE] = (value & 0xff00) >> 8; debug_info_report_message[DEBUG_VALUE_LOW_BYTE] = value & 0x00ff; @@ -203,8 +213,10 @@ void send_debug_info(uint id, uint value) { * @param blinks - number of blinks * @param delay - delay in milliseconds */ -void led_debug(int blinks, uint delay) { - for (int i = 0; i < blinks; i++) { +void led_debug(int blinks, uint delay) +{ + for (int i = 0; i < blinks; i++) + { gpio_put(LED_PIN, 1); sleep_ms(delay); gpio_put(LED_PIN, 0); @@ -219,13 +231,15 @@ void led_debug(int blinks, uint delay) { /************************************************************************ * Set a Pins mode */ -void set_pin_mode() { +void set_pin_mode() +{ uint pin; uint mode; pin = command_buffer[SET_PIN_MODE_GPIO_PIN]; mode = command_buffer[SET_PIN_MODE_MODE_TYPE]; - switch (mode) { + switch (mode) + { case DIGITAL_INPUT: case DIGITAL_INPUT_PULL_UP: case DIGITAL_INPUT_PULL_DOWN: @@ -234,10 +248,12 @@ void set_pin_mode() { command_buffer[SET_PIN_MODE_DIGITAL_IN_REPORTING_STATE]; gpio_init(pin); gpio_set_dir(pin, GPIO_IN); - if (mode == DIGITAL_INPUT_PULL_UP) { + if (mode == DIGITAL_INPUT_PULL_UP) + { gpio_pull_up(pin); } - if (mode == DIGITAL_INPUT_PULL_DOWN) { + if (mode == DIGITAL_INPUT_PULL_DOWN) + { gpio_pull_down(pin); } break; @@ -246,7 +262,8 @@ void set_pin_mode() { gpio_init(pin); gpio_set_dir(pin, GPIO_OUT); break; - case PWM_OUTPUT: { + case PWM_OUTPUT: + { /* Here we will set the operating frequency to be 50 hz to simplify support PWM as well as servo support. */ @@ -274,7 +291,8 @@ void set_pin_mode() { } case ANALOG_INPUT: // if the temp sensor was selected, then turn it on - if (pin == ADC_TEMPERATURE_REGISTER) { + if (pin == ADC_TEMPERATURE_REGISTER) + { adc_set_temp_sensor_enabled(true); } the_analog_pins[pin].reporting_enabled = @@ -292,7 +310,8 @@ void set_pin_mode() { /********************************************************** * Set a digital output pin's value */ -void digital_write() { +void digital_write() +{ uint pin; uint value; pin = command_buffer[DIGITAL_WRITE_GPIO_PIN]; @@ -303,7 +322,8 @@ void digital_write() { /********************************************** * Set A PWM Pin's value */ -void pwm_write() { +void pwm_write() +{ uint pin; uint16_t value; @@ -317,15 +337,19 @@ void pwm_write() { /*************************************************** * Control reporting */ -void modify_reporting() { +void modify_reporting() +{ int pin = command_buffer[MODIFY_REPORTING_PIN]; - switch (command_buffer[MODIFY_REPORTING_TYPE]) { + switch (command_buffer[MODIFY_REPORTING_TYPE]) + { case REPORTING_DISABLE_ALL: - for (int i = 0; i < MAX_DIGITAL_PINS_SUPPORTED; i++) { + for (int i = 0; i < MAX_DIGITAL_PINS_SUPPORTED; i++) + { the_digital_pins[i].reporting_enabled = false; } - for (int i = 0; i < MAX_ANALOG_PINS_SUPPORTED; i++) { + for (int i = 0; i < MAX_ANALOG_PINS_SUPPORTED; i++) + { the_analog_pins[i].reporting_enabled = false; } break; @@ -336,12 +360,14 @@ void modify_reporting() { the_analog_pins[pin].reporting_enabled = false; break; case REPORTING_DIGITAL_ENABLE: - if (the_digital_pins[pin].pin_mode != PIN_MODE_NOT_SET) { + if (the_digital_pins[pin].pin_mode != PIN_MODE_NOT_SET) + { the_digital_pins[pin].reporting_enabled = true; } break; case REPORTING_DIGITAL_DISABLE: - if (the_digital_pins[pin].pin_mode != PIN_MODE_NOT_SET) { + if (the_digital_pins[pin].pin_mode != PIN_MODE_NOT_SET) + { the_digital_pins[pin].reporting_enabled = false; } break; @@ -353,7 +379,8 @@ void modify_reporting() { /*********************************************************************** * Retrieve the current firmware version */ -void get_firmware_version() { +void get_firmware_version() +{ serial_write(firmware_report_message, sizeof(firmware_report_message) / sizeof(int)); } @@ -361,7 +388,8 @@ void get_firmware_version() { /************************************************************** * Retrieve the Pico's Unique ID */ -void get_pico_unique_id() { +void get_pico_unique_id() +{ // get the unique id pico_unique_board_id_t board_id; pico_get_unique_board_id(&board_id); @@ -380,7 +408,8 @@ void get_pico_unique_id() { /******************************************** * Stop reporting for all input pins */ -void stop_all_reports() { +void stop_all_reports() +{ stop_reports = true; sleep_ms(20); stdio_flush(); @@ -389,7 +418,8 @@ void stop_all_reports() { /********************************************** * Enable reporting for all input pins */ -void enable_all_reports() { +void enable_all_reports() +{ stdio_flush(); stop_reports = false; sleep_ms(20); @@ -398,20 +428,25 @@ void enable_all_reports() { /****************************************** * Use the watchdog time to reset the board. */ -void reset_board() { +void reset_board() +{ watchdog_reboot(0, 0, 0); watchdog_enable(10, 1); } -void i2c_begin() { +void i2c_begin() +{ // get the GPIO pins associated with this i2c instance uint sda_gpio = command_buffer[I2C_SDA_GPIO_PIN]; uint scl_gpio = command_buffer[I2C_SCL_GPIO_PIN]; // set the i2c instance - 0 or 1 - if (command_buffer[I2C_PORT] == 0) { + if (command_buffer[I2C_PORT] == 0) + { i2c_init(i2c0, 100 * 1000); - } else { + } + else + { i2c_init(i2c1, 100 * 1000); } gpio_set_function(sda_gpio, GPIO_FUNC_I2C); @@ -420,7 +455,8 @@ void i2c_begin() { gpio_pull_up(scl_gpio); } -void i2c_read() { +void i2c_read() +{ // The report_message offsets: // 0 = packet length - this must be calculated @@ -449,19 +485,24 @@ void i2c_read() { i2c_inst_t *i2c; // Determine the i2c port to use. - if (command_buffer[I2C_PORT]) { + if (command_buffer[I2C_PORT]) + { i2c = i2c1; - } else { + } + else + { i2c = i2c0; } // If there is an i2c register specified, set the register pointer - if (command_buffer[I2C_READ_NO_STOP_FLAG] != I2C_NO_REGISTER_SPECIFIED) { + if (command_buffer[I2C_READ_NO_STOP_FLAG] != I2C_NO_REGISTER_SPECIFIED) + { i2c_sdk_call_return_value = i2c_write_blocking(i2c, (uint8_t)command_buffer[I2C_DEVICE_ADDRESS], (const uint8_t *)&command_buffer[I2C_READ_REGISTER], 1, (bool)command_buffer[I2C_READ_NO_STOP_FLAG]); - if (i2c_sdk_call_return_value == PICO_ERROR_GENERIC) { + if (i2c_sdk_call_return_value == PICO_ERROR_GENERIC) + { return; } } @@ -471,7 +512,8 @@ void i2c_read() { i2c, (uint8_t)command_buffer[I2C_DEVICE_ADDRESS], data_from_device, (size_t)(command_buffer[I2C_READ_LENGTH]), (bool)command_buffer[I2C_READ_NO_STOP_FLAG]); - if (i2c_sdk_call_return_value == PICO_ERROR_GENERIC) { + if (i2c_sdk_call_return_value == PICO_ERROR_GENERIC) + { i2c_report_message[I2C_PACKET_LENGTH] = I2C_ERROR_REPORT_LENGTH; // length of the packet i2c_report_message[I2C_REPORT_ID] = I2C_READ_FAILED; // report ID @@ -484,7 +526,8 @@ void i2c_read() { } // copy the data returned from i2c device into the report message buffer - for (int i = 0; i < i2c_sdk_call_return_value; i++) { + for (int i = 0; i < i2c_sdk_call_return_value; i++) + { i2c_report_message[i + I2C_READ_START_OF_DATA] = data_from_device[i]; } // length of the packet @@ -513,14 +556,18 @@ void i2c_read() { serial_write((int *)i2c_report_message, num_of_bytes_to_send); } -void i2c_write() { +void i2c_write() +{ // i2c instance pointer i2c_inst_t *i2c; // Determine the i2c port to use. - if (command_buffer[I2C_PORT]) { + if (command_buffer[I2C_PORT]) + { i2c = i2c1; - } else { + } + else + { i2c = i2c0; } @@ -542,7 +589,8 @@ void i2c_write() { serial_write(i2c_report_message, I2C_ERROR_REPORT_NUM_OF_BYTE_TO_SEND); } -void init_neo_pixels() { +void init_neo_pixels() +{ // initialize the pico support a NeoPixel string uint offset = pio_add_program(np_pio, &ws2812_program); ws2812_init(np_pio, np_sm, offset, command_buffer[NP_PIN_NUMBER], 800000, @@ -551,7 +599,8 @@ void init_neo_pixels() { actual_number_of_pixels = command_buffer[NP_NUMBER_OF_PIXELS]; // set the pixels to the fill color - for (uint i = 0; i < actual_number_of_pixels; i++) { + for (uint i = 0; i < actual_number_of_pixels; i++) + { pixel_buffer[i][RED] = command_buffer[NP_RED_FILL]; pixel_buffer[i][GREEN] = command_buffer[NP_GREEN_FILL]; pixel_buffer[i][BLUE] = command_buffer[NP_BLUE_FILL]; @@ -560,7 +609,8 @@ void init_neo_pixels() { sleep_ms(1); } -void set_neo_pixel() { +void set_neo_pixel() +{ // set a single neopixel in the pixel buffer pixel_buffer[command_buffer[NP_PIXEL_NUMBER]][RED] = command_buffer[NP_SET_RED]; @@ -568,59 +618,76 @@ void set_neo_pixel() { command_buffer[NP_SET_GREEN]; pixel_buffer[command_buffer[NP_PIXEL_NUMBER]][BLUE] = command_buffer[NP_SET_BLUE]; - if (command_buffer[NP_SET_AUTO_SHOW]) { + if (command_buffer[NP_SET_AUTO_SHOW]) + { show_neo_pixels(); } } -void show_neo_pixels() { +void show_neo_pixels() +{ // show the neopixels in the buffer - for (int i = 0; i < actual_number_of_pixels; i++) { + for (int i = 0; i < actual_number_of_pixels; i++) + { put_pixel(urgb_u32(pixel_buffer[i][RED], pixel_buffer[i][GREEN], pixel_buffer[i][BLUE])); } } -void clear_all_neo_pixels() { +void clear_all_neo_pixels() +{ // set all the neopixels in the buffer to all zeroes - for (int i = 0; i < actual_number_of_pixels; i++) { + for (int i = 0; i < actual_number_of_pixels; i++) + { pixel_buffer[i][RED] = 0; pixel_buffer[i][GREEN] = 0; pixel_buffer[i][BLUE] = 0; } - if (command_buffer[NP_CLEAR_AUTO_SHOW]) { + if (command_buffer[NP_CLEAR_AUTO_SHOW]) + { show_neo_pixels(); } } -void fill_neo_pixels() { +void fill_neo_pixels() +{ // fill all the neopixels in the buffer with the // specified rgb values. - for (int i = 0; i < actual_number_of_pixels; i++) { + for (int i = 0; i < actual_number_of_pixels; i++) + { pixel_buffer[i][RED] = command_buffer[NP_FILL_RED]; pixel_buffer[i][GREEN] = command_buffer[NP_FILL_GREEN]; pixel_buffer[i][BLUE] = command_buffer[NP_FILL_BLUE]; } - if (command_buffer[NP_FILL_AUTO_SHOW]) { + if (command_buffer[NP_FILL_AUTO_SHOW]) + { show_neo_pixels(); } } -void sonar_callback(uint gpio, uint32_t events) { - if (events & GPIO_IRQ_EDGE_FALL) { +void sonar_callback(uint gpio, uint32_t events) +{ + if (events & GPIO_IRQ_EDGE_FALL) + { // stop time - for (int i = 0; i <= sonar_count; i++) { + for (int i = 0; i <= sonar_count; i++) + { hc_sr04_descriptor *sonar = &the_hc_sr04s.sonars[i]; - if (gpio == sonar->echo_pin) { + if (gpio == sonar->echo_pin) + { sonar->last_time_diff = time_us_32() - sonar->start_time; return; } } - } else if (events & GPIO_IRQ_EDGE_RISE) { + } + else if (events & GPIO_IRQ_EDGE_RISE) + { // start time - for (int i = 0; i <= sonar_count; i++) { + for (int i = 0; i <= sonar_count; i++) + { hc_sr04_descriptor *sonar = &the_hc_sr04s.sonars[i]; - if (gpio == sonar->echo_pin) { + if (gpio == sonar->echo_pin) + { sonar->start_time = time_us_32(); return; } @@ -628,7 +695,8 @@ void sonar_callback(uint gpio, uint32_t events) { } } -void init_quadrature_encoder(int A, int B, encoder_t *enc) { +void init_quadrature_encoder(int A, int B, encoder_t *enc) +{ gpio_init(A); gpio_set_dir(A, GPIO_IN); gpio_set_pulls(A, false, false); @@ -640,7 +708,8 @@ void init_quadrature_encoder(int A, int B, encoder_t *enc) { enc->B = B; enc->type = QUADRATURE; } -void init_single_encoder(int A, encoder_t *enc) { +void init_single_encoder(int A, encoder_t *enc) +{ gpio_init(A); gpio_set_dir(A, GPIO_IN); gpio_set_pulls(A, false, false); @@ -648,14 +717,18 @@ void init_single_encoder(int A, encoder_t *enc) { enc->A = A; enc->type = SINGLE; } -bool encoder_callback(repeating_timer_t *timer) { +bool encoder_callback(repeating_timer_t *timer) +{ (void)timer; - if (encoders.next_encoder_index == 0) { + if (encoders.next_encoder_index == 0) + { return true; } - for (int i = 0; i < encoders.next_encoder_index; i++) { + for (int i = 0; i < encoders.next_encoder_index; i++) + { encoder_t *enc = &encoders.encoders[i]; - if (enc->type == QUADRATURE) { + if (enc->type == QUADRATURE) + { bool a = gpio_get(enc->A); bool b = gpio_get(enc->B); @@ -663,15 +736,21 @@ bool encoder_callback(repeating_timer_t *timer) { int new_gray_code_step = a << 1 | b; int new_bin_step = new_gray_code_step >> 1 ^ new_gray_code_step; int diff = new_bin_step - enc->last_state; - if (diff > -2 && diff < 2) { + if (diff > -2 && diff < 2) + { enc->step += diff; - } else { + } + else + { enc->step += (diff < 0) ? 1 : -1; } enc->last_state = new_bin_step; - } else { + } + else + { bool a = gpio_get(enc->A); - if (a != enc->last_state) { + if (a != enc->last_state) + { enc->step++; enc->last_state = a; } @@ -680,7 +759,8 @@ bool encoder_callback(repeating_timer_t *timer) { return true; } -bool create_encoder_timer() { +bool create_encoder_timer() +{ int hz = 2000; /* blue encoder motor: - 110 rpm = ~2 rot/s @@ -689,39 +769,51 @@ bool create_encoder_timer() { - requires at least 1 scan per step */ if (!add_repeating_timer_us(-1000000 / hz, encoder_callback, NULL, - &encoders.trigger_timer)) { + &encoders.trigger_timer)) + { printf("Failed to add timer\n"); return false; } return true; } -void encoder_new() { +void encoder_new() +{ ENCODER_TYPES type = (ENCODER_TYPES)command_buffer[ENCODER_TYPE]; uint pin_a = command_buffer[ENCODER_PIN_A]; uint pin_b = command_buffer[ENCODER_PIN_B]; // both cases will have a pin B - if (encoders.next_encoder_index == 0) { + if (encoders.next_encoder_index == 0) + { bool timer = create_encoder_timer(); - if (!timer) { + if (!timer) + { return; } - } else if (encoders.next_encoder_index > MAX_ENCODERS) { + } + else if (encoders.next_encoder_index > MAX_ENCODERS) + { return; } encoder_t *new_encoder = &encoders.encoders[encoders.next_encoder_index]; - if (type == SINGLE) { + if (type == SINGLE) + { init_single_encoder(pin_a, new_encoder); - } else { + } + else + { init_quadrature_encoder(pin_a, pin_b, new_encoder); } encoders.next_encoder_index++; } int encoder_report_message[] = {3, ENCODER_REPORT, 0, 0}; -void scan_encoders() { - for (int i = 0; i < encoders.next_encoder_index; i++) { +void scan_encoders() +{ + for (int i = 0; i < encoders.next_encoder_index; i++) + { encoder_t *enc = &encoders.encoders[i]; - if (enc->step != 0) { + if (enc->step != 0) + { encoder_report_message[ENCODER_REPORT_PIN_A] = (uint8_t)enc->A; encoder_report_message[ENCODER_REPORT_STEP] = enc->step; enc->step = 0; @@ -731,32 +823,37 @@ void scan_encoders() { } bool sonar_timer_callback( - repeating_timer_t *rt) { // periodically trigger all sonars at the same time + repeating_timer_t *rt) +{ // periodically trigger all sonars at the same time gpio_set_mask(the_hc_sr04s.trigger_mask); busy_wait_us(10); gpio_clr_mask(the_hc_sr04s.trigger_mask); return true; } -void sonar_new() { +void sonar_new() +{ // add the sonar to the sonar struct to be processed within // the main loop uint trig_pin = command_buffer[SONAR_TRIGGER_PIN]; uint echo_pin = command_buffer[SONAR_ECHO_PIN]; // for the first HC-SR04, add the program. - if (sonar_count == -1) { + if (sonar_count == -1) + { // Init timer int hz = 10; // negative timeout means exact delay (rather than delay between callbacks) if (!add_repeating_timer_us(-1000000 / hz, sonar_timer_callback, NULL, - &the_hc_sr04s.trigger_timer)) { + &the_hc_sr04s.trigger_timer)) + { printf("Failed to add timer\n"); return; } } sonar_count++; - if (sonar_count > MAX_SONARS) { + if (sonar_count > MAX_SONARS) + { return; } the_hc_sr04s.sonars[sonar_count].trig_pin = trig_pin; @@ -771,18 +868,22 @@ void sonar_new() { echo_pin, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true, &sonar_callback); } -bool repeating_timer_callback(struct repeating_timer *t) { +bool repeating_timer_callback(struct repeating_timer *t) +{ (void)t; // printf("Repeat at %lld\n", time_us_64()); timer_fired = true; return true; } -void dht_new() { - if (dht_count > MAX_DHTS) { +void dht_new() +{ + if (dht_count > MAX_DHTS) + { return; } - if (dht_count == -1) { + if (dht_count == -1) + { // first time through start repeating timer add_repeating_timer_ms(2000, repeating_timer_callback, NULL, &timer); } @@ -794,15 +895,19 @@ void dht_new() { gpio_init(dht_pin); } -void init_spi() { +void init_spi() +{ spi_inst_t *spi_port; uint spi_baud_rate; uint cs_pin; // initialize the spi port - if (command_buffer[SPI_PORT] == 0) { + if (command_buffer[SPI_PORT] == 0) + { spi_port = spi0; - } else { + } + else + { spi_port = spi1; } @@ -819,7 +924,8 @@ void init_spi() { gpio_set_function(command_buffer[SPI_CLK_PIN], GPIO_FUNC_SPI); // initialize chip select GPIO pins - for (int i = 0; i < command_buffer[SPI_CS_LIST_LENGTH]; i++) { + for (int i = 0; i < command_buffer[SPI_CS_LIST_LENGTH]; i++) + { cs_pin = command_buffer[SPI_CS_LIST + i]; // Chip select is active-low, so we'll initialise it to a driven-high state gpio_init(cs_pin); @@ -828,7 +934,8 @@ void init_spi() { } } -void spi_cs_control() { +void spi_cs_control() +{ uint8_t cs_pin; uint8_t cs_state; @@ -841,7 +948,8 @@ void spi_cs_control() { void set_scan_delay() { scan_delay = command_buffer[SCAN_DELAY]; } -void read_blocking_spi() { +void read_blocking_spi() +{ // The report_message offsets: // 0 = packet length - this must be calculated // 1 = SPI_READ_REPORT @@ -854,9 +962,12 @@ void read_blocking_spi() { uint8_t repeated_transmit_byte; uint8_t data[command_buffer[SPI_READ_LEN]]; - if (command_buffer[SPI_PORT] == 0) { + if (command_buffer[SPI_PORT] == 0) + { spi_port = spi0; - } else { + } + else + { spi_port = spi1; } @@ -876,20 +987,25 @@ void read_blocking_spi() { spi_report_message[SPI_REPORT_ID] = SPI_REPORT; spi_report_message[SPI_REPORT_PORT] = command_buffer[SPI_PORT]; spi_report_message[SPI_REPORT_NUMBER_OF_DATA_BYTES] = data_length; - for (int i = 0; i < data_length; i++) { + for (int i = 0; i < data_length; i++) + { spi_report_message[SPI_DATA + i] = data[i]; } serial_write((int *)spi_report_message, SPI_DATA + data_length); } -void write_blocking_spi() { +void write_blocking_spi() +{ spi_inst_t *spi_port; uint cs_pin; size_t data_length; - if (command_buffer[SPI_PORT] == 0) { + if (command_buffer[SPI_PORT] == 0) + { spi_port = spi0; - } else { + } + else + { spi_port = spi1; } @@ -898,15 +1014,19 @@ void write_blocking_spi() { spi_write_blocking(spi_port, &command_buffer[SPI_WRITE_DATA], data_length); } -void set_format_spi() { +void set_format_spi() +{ spi_inst_t *spi_port; uint data_bits = command_buffer[SPI_NUMBER_OF_BITS]; spi_cpol_t cpol = (spi_cpol_t)command_buffer[SPI_CLOCK_PHASE]; spi_cpha_t cpha = (spi_cpha_t)command_buffer[SPI_CLOCK_POLARITY]; - if (command_buffer[SPI_PORT] == 0) { + if (command_buffer[SPI_PORT] == 0) + { spi_port = spi0; - } else { + } + else + { spi_port = spi1; } spi_set_format(spi_port, data_bits, cpol, cpha, (spi_order_t)1); @@ -930,7 +1050,8 @@ void servo_detach() {} /*************************************************** * Retrieve the next command and process it */ -void get_next_command() { +void get_next_command() +{ int packet_size; int packet_data; command_descriptor command_entry; @@ -942,21 +1063,28 @@ void get_next_command() { // The first byte is the command ID and the following bytes // are the associated data bytes packet_size = getchar_timeout_us(100); - if (packet_size == PICO_ERROR_TIMEOUT) { + if (packet_size == PICO_ERROR_TIMEOUT) + { // no data, let the main loop continue to run to handle inputs return; - } else { + } + else + { // get the rest of the packet - for (int i = 0; i < packet_size; i++) { - for (int retries = 10; retries > 0; retries--) { + for (int i = 0; i < packet_size; i++) + { + for (int retries = 10; retries > 0; retries--) + { packet_data = getchar_timeout_us(100); - if (packet_data != PICO_ERROR_TIMEOUT) { + if (packet_data != PICO_ERROR_TIMEOUT) + { break; } sleep_ms(1); } - if (packet_data == PICO_ERROR_TIMEOUT) { + if (packet_data == PICO_ERROR_TIMEOUT) + { return; // failed message } command_buffer[i] = (uint8_t)packet_data; @@ -980,7 +1108,8 @@ void get_next_command() { * Scan all pins set as digital inputs * and generate a report. */ -void scan_digital_inputs() { +void scan_digital_inputs() +{ int value; // report message @@ -990,14 +1119,18 @@ void scan_digital_inputs() { // index 2 = pin number // index 3 = value - for (int i = 0; i < MAX_DIGITAL_PINS_SUPPORTED; i++) { + for (int i = 0; i < MAX_DIGITAL_PINS_SUPPORTED; i++) + { if (the_digital_pins[i].pin_mode == DIGITAL_INPUT || the_digital_pins[i].pin_mode == DIGITAL_INPUT_PULL_UP || - the_digital_pins[i].pin_mode == DIGITAL_INPUT_PULL_DOWN) { - if (the_digital_pins[i].reporting_enabled) { + the_digital_pins[i].pin_mode == DIGITAL_INPUT_PULL_DOWN) + { + if (the_digital_pins[i].reporting_enabled) + { // if the value changed since last read value = gpio_get(the_digital_pins[i].pin_number); - if (value != the_digital_pins[i].last_value) { + if (value != the_digital_pins[i].last_value) + { the_digital_pins[i].last_value = value; digital_input_report_message[DIGITAL_INPUT_GPIO_PIN] = i; digital_input_report_message[DIGITAL_INPUT_GPIO_VALUE] = value; @@ -1008,7 +1141,8 @@ void scan_digital_inputs() { } } -void scan_analog_inputs() { +void scan_analog_inputs() +{ uint16_t value; // report message @@ -1021,12 +1155,15 @@ void scan_analog_inputs() { int differential; - for (uint8_t i = 0; i < MAX_ANALOG_PINS_SUPPORTED; i++) { - if (the_analog_pins[i].reporting_enabled) { + for (uint8_t i = 0; i < MAX_ANALOG_PINS_SUPPORTED; i++) + { + if (the_analog_pins[i].reporting_enabled) + { adc_select_input(i); value = adc_read(); differential = abs(value - the_analog_pins[i].last_value); - if (differential >= the_analog_pins[i].differential) { + if (differential >= the_analog_pins[i].differential) + { // trigger value achieved, send out the report the_analog_pins[i].last_value = value; // input_message[1] = the_analog_pins[i].pin_number; @@ -1039,9 +1176,11 @@ void scan_analog_inputs() { } } -void scan_sonars() { +void scan_sonars() +{ uint32_t current_time = time_us_32(); - for (int i = 0; i <= sonar_count; i++) { + for (int i = 0; i <= sonar_count; i++) + { hc_sr04_descriptor *sonar = &the_hc_sr04s.sonars[i]; if ((current_time - sonar->start_time) > @@ -1049,7 +1188,8 @@ void scan_sonars() { { sonar->last_time_diff = 0; } - if (sonar->last_time_diff > 30000) { + if (sonar->last_time_diff > 30000) + { sonar->last_time_diff = 0; // HC-SR04 has max range of 4 / 5m, with a // timeout pulse longer than 35ms } @@ -1065,97 +1205,81 @@ void scan_sonars() { // SENSORSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSS -void sensor_new() { +void sensor_new() +{ const SENSOR_TYPES type = (SENSOR_TYPES)command_buffer[2]; const uint8_t sensor_num = command_buffer[1]; uint8_t sensor_data[SENSORS_MAX_SETTINGS_A]; std::copy(command_buffer + 3, command_buffer + 3 + SENSORS_MAX_SETTINGS_A, sensor_data); - if (type >= SENSOR_TYPES::MAX_SENSORS) { + if (type >= SENSOR_TYPES::MAX_SENSORS) + { return; } Sensor *sensor = nullptr; - if (type == GPS) { + if (type == GPS) + { sensor = new GPS_Sensor(sensor_data); - } else if (type == SENSOR_TYPES::ADXL345) { + } + else if (type == SENSOR_TYPES::ADXL345) + { sensor = new ADXL345_Sensor(sensor_data); - } else if (type == SENSOR_TYPES::VEML6040) { + } + else if (type == SENSOR_TYPES::VEML6040) + { sensor = new VEML6040_Sensor(sensor_data); } + else if (type == SENSOR_TYPES::TOF_VL53) + { + sensor = new VL53L0X_Sensor(sensor_data); + } sensor->type = type; sensor->num = sensor_num; sensors.push_back(sensor); } -void readSensors() { - for (auto &sensor : sensors) { +void readSensors() +{ + for (auto &sensor : sensors) + { sensor->readSensor(); } } -GPS_Sensor::GPS_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]) { +GPS_Sensor::GPS_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]) +{ // TODO: read uart, forward to serial } -void GPS_Sensor::readSensor() { +void GPS_Sensor::readSensor() +{ // TODO: read uart, forward to serial } -int write_i2c(int i2c_port, int addr, const std::vector &bytes, bool nostop=false) { - i2c_inst_t *i2c; - - if (i2c_port) { - i2c = i2c1; - } else { - i2c = i2c0; - } - int i2c_sdk_call_return_value = - i2c_write_blocking_until(i2c, (uint8_t)addr, bytes.data(), bytes.size(), - nostop, make_timeout_time_ms(50)); - return i2c_sdk_call_return_value == bytes.size(); -} - -int read_i2c(int i2c_port, int addr, const std::vector &write_bytes, - int bytes_to_read, std::vector &read_bytes) { - - // write i2c - if (!write_bytes.empty()) { - write_i2c(i2c_port, addr, write_bytes, true); - } - i2c_inst_t *i2c; - - if (i2c_port) { - i2c = i2c1; - } else { - i2c = i2c0; - } - read_bytes.resize(bytes_to_read, 0); - auto i2c_sdk_return = - i2c_read_blocking_until(i2c, addr, read_bytes.data(), bytes_to_read, - false, make_timeout_time_ms(50)); - - return i2c_sdk_return == bytes_to_read; -} - -ADXL345_Sensor::ADXL345_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]) { +ADXL345_Sensor::ADXL345_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]) +{ // assume i2c is done this->i2c_port = settings[0]; this->init_sequence(); } -void ADXL345_Sensor::init_sequence() { +void ADXL345_Sensor::init_sequence() +{ // write 83, 43, 0 bool ok = write_i2c(this->i2c_port, this->i2c_addr, {43, 0}); // write 83, 45, 8 ok &= write_i2c(this->i2c_port, this->i2c_addr, {45, 8}); // write 83, 49, 8 ok &= write_i2c(this->i2c_port, this->i2c_addr, {49, 8}); - if (!ok) { + if (!ok) + { this->stop = true; } } -void ADXL345_Sensor::readSensor() { - if (this->stop) { +void ADXL345_Sensor::readSensor() +{ + if (this->stop) + { return; } // read 83, 50, 6bytes @@ -1165,12 +1289,14 @@ void ADXL345_Sensor::readSensor() { this->writeSensorData(out); } -VEML6040_Sensor::VEML6040_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]) { +VEML6040_Sensor::VEML6040_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]) +{ this->i2c_port = settings[0]; this->init_sequence(); } -void VEML6040_Sensor::init_sequence() { +void VEML6040_Sensor::init_sequence() +{ bool ok = write_i2c(this->i2c_port, this->i2c_addr, { 0, // register 0 @@ -1182,8 +1308,10 @@ void VEML6040_Sensor::init_sequence() { }); } -void VEML6040_Sensor::readSensor() { - if (this->stop) { +void VEML6040_Sensor::readSensor() +{ + if (this->stop) + { return; } std::vector data; @@ -1191,19 +1319,35 @@ void VEML6040_Sensor::readSensor() { std::vector single_color_data(2); bool ok = true; for (uint8_t reg = 0x08; reg <= 0x0B; - reg++) { // read the 4 registers and add the data to the full data vector + reg++) + { // read the 4 registers and add the data to the full data vector ok &= read_i2c(this->i2c_port, this->i2c_addr, {reg}, 2, single_color_data); - data.push_back(single_color_data[0] ); - data.push_back(single_color_data[1] ); + data.push_back(single_color_data[0]); + data.push_back(single_color_data[1]); } this->writeSensorData(data); - if (!ok) { + if (!ok) + { this->stop = true; } } +VL53L0X_Sensor::VL53L0X_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]) +{ + this->sensor.setBus(settings[0]); + this->sensor.init(); + this->sensor.startContinuous(); +} + +void VL53L0X_Sensor::readSensor() +{ + auto dist = this->sensor.readRangeContinuousMillimeters(); + std::vector data = {(uint8_t)(dist >> 8), (uint8_t)(dist & 0xFF)}; + this->writeSensorData(data); +} -void Sensor::writeSensorData(std::vector data) { +void Sensor::writeSensorData(std::vector data) +{ std::vector out = { 0, // write len SENSOR_REPORT, // write type @@ -1216,31 +1360,38 @@ void Sensor::writeSensorData(std::vector data) { serial_write(out); } -void serial_write(std::vector data) { - for (auto i : data) { +void serial_write(std::vector data) +{ + for (auto i : data) + { putchar(i); } stdio_flush(); } // DHTS sensor? -void scan_dhts() { +void scan_dhts() +{ // read the next dht device // one device is read each cycle - if (dht_count >= 0) { - if (timer_fired) { + if (dht_count >= 0) + { + if (timer_fired) + { timer_fired = false; int the_index = the_dhts.next_dht_index; read_dht(the_dhts.dhts[the_index].data_pin); the_dhts.next_dht_index++; - if (the_dhts.next_dht_index > dht_count) { + if (the_dhts.next_dht_index > dht_count) + { the_dhts.next_dht_index = 0; } } } } -void read_dht(uint dht_pin) { +void read_dht(uint dht_pin) +{ int data[5] = {0, 0, 0, 0, 0}; uint last = 1; uint j = 0; @@ -1258,9 +1409,11 @@ void read_dht(uint dht_pin) { gpio_set_dir(dht_pin, GPIO_IN); sleep_us(1); - for (uint i = 0; i < DHT_MAX_TIMINGS; i++) { + for (uint i = 0; i < DHT_MAX_TIMINGS; i++) + { uint count = 0; - while (gpio_get(dht_pin) == last) { + while (gpio_get(dht_pin) == last) + { count++; sleep_us(1); if (count == 255) @@ -1270,7 +1423,8 @@ void read_dht(uint dht_pin) { if (count == 255) break; - if ((i >= 4) && (i % 2 == 0)) { + if ((i >= 4) && (i % 2 == 0)) + { data[j / 8] <<= 1; if (count > 46) data[j / 8] |= 1; @@ -1278,16 +1432,20 @@ void read_dht(uint dht_pin) { } } if ((j >= 40) && - (data[4] == ((data[0] + data[1] + data[2] + data[3]) & 0xFF))) { + (data[4] == ((data[0] + data[1] + data[2] + data[3]) & 0xFF))) + { humidity = (float)((data[0] << 8) + data[1]) / 10; - if (humidity > 100) { + if (humidity > 100) + { humidity = data[0]; } temp_celsius = (float)(((data[2] & 0x7F) << 8) + data[3]) / 10; - if (temp_celsius > 125) { + if (temp_celsius > 125) + { temp_celsius = data[2]; } - if (data[2] & 0x80) { + if (data[2] & 0x80) + { temp_celsius = -temp_celsius; } @@ -1298,7 +1456,9 @@ void read_dht(uint dht_pin) { nearest = roundf(humidity * 100) / 100; humidity_int_part = (int)nearest; humidity_dec_part = (int)((nearest - humidity_int_part) * 100); - } else { + } + else + { // bad data return zeros temp_int_part = temp_dec_part = humidity_int_part = humidity_dec_part = 0; } @@ -1316,8 +1476,10 @@ void read_dht(uint dht_pin) { * @param buffer * @param num_of_bytes_to_send */ -void serial_write(const int *buffer, int num_of_bytes_to_send) { - for (int i = 0; i < num_of_bytes_to_send; i++) { +void serial_write(const int *buffer, int num_of_bytes_to_send) +{ + for (int i = 0; i < num_of_bytes_to_send; i++) + { putchar((buffer[i]) & 0x00ff); } stdio_flush(); @@ -1327,7 +1489,8 @@ void serial_write(const int *buffer, int num_of_bytes_to_send) { * MAIN FUNCTION ****************************************************************/ -int main() { +int main() +{ stdio_init_all(); stdio_set_translate_crlf(&stdio_usb, false); stdio_set_translate_crlf(&stdio_uart, false); @@ -1340,7 +1503,8 @@ int main() { adc_init(); // create an array of pin_descriptors for 100 pins // establish the digital pin array - for (uint8_t i = 0; i < MAX_DIGITAL_PINS_SUPPORTED; i++) { + for (uint8_t i = 0; i < MAX_DIGITAL_PINS_SUPPORTED; i++) + { the_digital_pins[i].pin_number = i; the_digital_pins[i].pin_mode = PIN_MODE_NOT_SET; the_digital_pins[i].reporting_enabled = false; @@ -1348,14 +1512,16 @@ int main() { } // establish the analog pin array - for (uint8_t i = 0; i < MAX_ANALOG_PINS_SUPPORTED; i++) { + for (uint8_t i = 0; i < MAX_ANALOG_PINS_SUPPORTED; i++) + { the_analog_pins[i].reporting_enabled = false; the_analog_pins[i].last_value = -1; } // initialize the sonar structures sonar_data the_hc_sr04s = {.next_sonar_index = 0, .trigger_mask = 0}; - for (int i = 0; i < MAX_SONARS; i++) { + for (int i = 0; i < MAX_SONARS; i++) + { the_hc_sr04s.sonars[i].trig_pin = the_hc_sr04s.sonars[i].echo_pin = (uint)-1; } @@ -1371,12 +1537,15 @@ int main() { // infinite loop uint32_t last_scan = 0; - while (true) { + while (true) + { watchdog_update(); get_next_command(); - if (!stop_reports) { - if (time_us_32() - last_scan >= (scan_delay * 1000)) { + if (!stop_reports) + { + if (time_us_32() - last_scan >= (scan_delay * 1000)) + { last_scan = time_us_32(); scan_digital_inputs(); scan_analog_inputs(); diff --git a/src/VL53L0.cpp b/src/VL53L0.cpp new file mode 100644 index 0000000..2c03b8f --- /dev/null +++ b/src/VL53L0.cpp @@ -0,0 +1,1095 @@ + +#include "VL53L0.hpp" +// Adapted by Arend-Jan van Hilten for the RP2040 from the https://github.com/pololu/vl53l0x-arduino library + +// Most of the functionality of this library is based on the VL53L0X API +// provided by ST (STSW-IMG005), and some of the explanatory comments are quoted +// or paraphrased from the API source code, API user manual (UM2039), and the +// VL53L0X datasheet. + +uint16_t millis() +{ + return time_us_32() / 1000; +} + +// Defines ///////////////////////////////////////////////////////////////////// + +// The Arduino two-wire interface uses a 7-bit number for the address, +// and sets the last bit correctly based on reads and writes +#define ADDRESS_DEFAULT 0b0101001 + +// Record the current time to check an upcoming timeout against +#define startTimeout() (timeout_start_ms = millis()) + +// Check if timeout is enabled (set to nonzero value) and has expired +#define checkTimeoutExpired() (io_timeout > 0 && ((uint16_t)(millis() - timeout_start_ms) > io_timeout)) + +// Decode VCSEL (vertical cavity surface emitting laser) pulse period in PCLKs +// from register value +// based on VL53L0X_decode_vcsel_period() +#define decodeVcselPeriod(reg_val) (((reg_val) + 1) << 1) + +// Encode VCSEL pulse period register value from period in PCLKs +// based on VL53L0X_encode_vcsel_period() +#define encodeVcselPeriod(period_pclks) (((period_pclks) >> 1) - 1) + +// Calculate macro period in *nanoseconds* from VCSEL period in PCLKs +// based on VL53L0X_calc_macro_period_ps() +// PLL_period_ps = 1655; macro_period_vclks = 2304 +#define calcMacroPeriod(vcsel_period_pclks) ((((uint32_t)2304 * (vcsel_period_pclks) * 1655) + 500) / 1000) + +// Constructors //////////////////////////////////////////////////////////////// + +VL53L0X::VL53L0X() + : bus(0), address(ADDRESS_DEFAULT), io_timeout(0) // no timeout + , + did_timeout(false) +{ +} + +// Public Methods ////////////////////////////////////////////////////////////// + +void VL53L0X::setAddress(uint8_t new_addr) +{ + writeReg(I2C_SLAVE_DEVICE_ADDRESS, new_addr & 0x7F); + address = new_addr; +} + +// Initialize sensor using sequence based on VL53L0X_DataInit(), +// VL53L0X_StaticInit(), and VL53L0X_PerformRefCalibration(). +// This function does not perform reference SPAD calibration +// (VL53L0X_PerformRefSpadManagement()), since the API user manual says that it +// is performed by ST on the bare modules; it seems like that should work well +// enough unless a cover glass is added. +// If io_2v8 (optional) is true or not given, the sensor is configured for 2V8 +// mode. +bool VL53L0X::init(bool io_2v8) +{ + // check model ID register (value specified in datasheet) + if (readReg(IDENTIFICATION_MODEL_ID) != 0xEE) + { + return false; + } + + // VL53L0X_DataInit() begin + + // sensor uses 1V8 mode for I/O by default; switch to 2V8 mode if necessary + if (io_2v8) + { + writeReg(VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, + readReg(VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV) | 0x01); // set bit 0 + } + + // "Set I2C standard mode" + writeReg(0x88, 0x00); + + writeReg(0x80, 0x01); + writeReg(0xFF, 0x01); + writeReg(0x00, 0x00); + stop_variable = readReg(0x91); + writeReg(0x00, 0x01); + writeReg(0xFF, 0x00); + writeReg(0x80, 0x00); + + // disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks + writeReg(MSRC_CONFIG_CONTROL, readReg(MSRC_CONFIG_CONTROL) | 0x12); + + // set final range signal rate limit to 0.25 MCPS (million counts per second) + setSignalRateLimit(0.25); + + writeReg(SYSTEM_SEQUENCE_CONFIG, 0xFF); + + // VL53L0X_DataInit() end + + // VL53L0X_StaticInit() begin + + uint8_t spad_count; + bool spad_type_is_aperture; + if (!getSpadInfo(&spad_count, &spad_type_is_aperture)) + { + 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]; + readMulti(GLOBAL_CONFIG_SPAD_ENABLES_REF_0, ref_spad_map, 6); + + // -- VL53L0X_set_reference_spads() begin (assume NVM values are valid) + + writeReg(0xFF, 0x01); + writeReg(DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00); + writeReg(DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C); + writeReg(0xFF, 0x00); + writeReg(GLOBAL_CONFIG_REF_EN_START_SELECT, 0xB4); + + uint8_t first_spad_to_enable = spad_type_is_aperture ? 12 : 0; // 12 is the first aperture spad + uint8_t spads_enabled = 0; + + for (uint8_t i = 0; i < 48; i++) + { + if (i < first_spad_to_enable || 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] >> (i % 8)) & 0x1) + { + spads_enabled++; + } + } + + writeMulti(GLOBAL_CONFIG_SPAD_ENABLES_REF_0, ref_spad_map, 6); + + // -- VL53L0X_set_reference_spads() end + + // -- VL53L0X_load_tuning_settings() begin + // DefaultTuningSettings from vl53l0x_tuning.h + + writeReg(0xFF, 0x01); + writeReg(0x00, 0x00); + + writeReg(0xFF, 0x00); + writeReg(0x09, 0x00); + writeReg(0x10, 0x00); + writeReg(0x11, 0x00); + + writeReg(0x24, 0x01); + writeReg(0x25, 0xFF); + writeReg(0x75, 0x00); + + writeReg(0xFF, 0x01); + writeReg(0x4E, 0x2C); + writeReg(0x48, 0x00); + writeReg(0x30, 0x20); + + writeReg(0xFF, 0x00); + writeReg(0x30, 0x09); + writeReg(0x54, 0x00); + writeReg(0x31, 0x04); + writeReg(0x32, 0x03); + writeReg(0x40, 0x83); + writeReg(0x46, 0x25); + writeReg(0x60, 0x00); + writeReg(0x27, 0x00); + writeReg(0x50, 0x06); + writeReg(0x51, 0x00); + writeReg(0x52, 0x96); + writeReg(0x56, 0x08); + writeReg(0x57, 0x30); + writeReg(0x61, 0x00); + writeReg(0x62, 0x00); + writeReg(0x64, 0x00); + writeReg(0x65, 0x00); + writeReg(0x66, 0xA0); + + writeReg(0xFF, 0x01); + writeReg(0x22, 0x32); + writeReg(0x47, 0x14); + writeReg(0x49, 0xFF); + writeReg(0x4A, 0x00); + + writeReg(0xFF, 0x00); + writeReg(0x7A, 0x0A); + writeReg(0x7B, 0x00); + writeReg(0x78, 0x21); + + writeReg(0xFF, 0x01); + writeReg(0x23, 0x34); + writeReg(0x42, 0x00); + writeReg(0x44, 0xFF); + writeReg(0x45, 0x26); + writeReg(0x46, 0x05); + writeReg(0x40, 0x40); + writeReg(0x0E, 0x06); + writeReg(0x20, 0x1A); + writeReg(0x43, 0x40); + + writeReg(0xFF, 0x00); + writeReg(0x34, 0x03); + writeReg(0x35, 0x44); + + writeReg(0xFF, 0x01); + writeReg(0x31, 0x04); + writeReg(0x4B, 0x09); + writeReg(0x4C, 0x05); + writeReg(0x4D, 0x04); + + writeReg(0xFF, 0x00); + writeReg(0x44, 0x00); + writeReg(0x45, 0x20); + writeReg(0x47, 0x08); + writeReg(0x48, 0x28); + writeReg(0x67, 0x00); + writeReg(0x70, 0x04); + writeReg(0x71, 0x01); + writeReg(0x72, 0xFE); + writeReg(0x76, 0x00); + writeReg(0x77, 0x00); + + writeReg(0xFF, 0x01); + writeReg(0x0D, 0x01); + + writeReg(0xFF, 0x00); + writeReg(0x80, 0x01); + writeReg(0x01, 0xF8); + + writeReg(0xFF, 0x01); + writeReg(0x8E, 0x01); + writeReg(0x00, 0x01); + writeReg(0xFF, 0x00); + writeReg(0x80, 0x00); + + // -- VL53L0X_load_tuning_settings() end + + // "Set interrupt config to new sample ready" + // -- VL53L0X_SetGpioConfig() begin + + writeReg(SYSTEM_INTERRUPT_CONFIG_GPIO, 0x04); + writeReg(GPIO_HV_MUX_ACTIVE_HIGH, readReg(GPIO_HV_MUX_ACTIVE_HIGH) & ~0x10); // active low + writeReg(SYSTEM_INTERRUPT_CLEAR, 0x01); + + // -- VL53L0X_SetGpioConfig() end + + measurement_timing_budget_us = getMeasurementTimingBudget(); + + // "Disable MSRC and TCC by default" + // MSRC = Minimum Signal Rate Check + // TCC = Target CentreCheck + // -- VL53L0X_SetSequenceStepEnable() begin + + writeReg(SYSTEM_SEQUENCE_CONFIG, 0xE8); + + // -- VL53L0X_SetSequenceStepEnable() end + + // "Recalculate timing budget" + setMeasurementTimingBudget(measurement_timing_budget_us); + + // VL53L0X_StaticInit() end + + // VL53L0X_PerformRefCalibration() begin (VL53L0X_perform_ref_calibration()) + + // -- VL53L0X_perform_vhv_calibration() begin + + writeReg(SYSTEM_SEQUENCE_CONFIG, 0x01); + if (!performSingleRefCalibration(0x40)) + { + return false; + } + + // -- VL53L0X_perform_vhv_calibration() end + + // -- VL53L0X_perform_phase_calibration() begin + + writeReg(SYSTEM_SEQUENCE_CONFIG, 0x02); + if (!performSingleRefCalibration(0x00)) + { + return false; + } + + // -- VL53L0X_perform_phase_calibration() end + + // "restore the previous Sequence Config" + writeReg(SYSTEM_SEQUENCE_CONFIG, 0xE8); + + // VL53L0X_PerformRefCalibration() end + + return true; +} + +// Write an 8-bit register +void VL53L0X::writeReg(uint8_t reg, uint8_t value) +{ + write_i2c(this->bus, this->address, {reg, value}); + // bus->beginTransmission(address); + // bus->write(reg); + // bus->write(value); + // last_status = bus->endTransmission(); +} + +// Write a 16-bit register +void VL53L0X::writeReg16Bit(uint8_t reg, uint16_t value) +{ + write_i2c(this->bus, this->address, {reg, (uint8_t)(value >> 8), (uint8_t)(value & 0xFF)}); + + // bus->beginTransmission(address); + // bus->write(reg); + // bus->write((uint8_t)(value >> 8)); // value high byte + // bus->write((uint8_t)(value)); // value low byte + // last_status = bus->endTransmission(); +} + +// Write a 32-bit register +void VL53L0X::writeReg32Bit(uint8_t reg, uint32_t value) +{ + write_i2c(this->bus, this->address, {reg, (uint8_t)(value >> 24 & 0xFF), (uint8_t)(value >> 16 & 0xFF), (uint8_t)(value >> 8 & 0xFF), (uint8_t)(value & 0xFF)}); + + // bus->beginTransmission(address); + // bus->write(reg); + // bus->write((uint8_t)(value >> 24)); // value highest byte + // bus->write((uint8_t)(value >> 16)); + // bus->write((uint8_t)(value >> 8)); + // bus->write((uint8_t)(value)); // value lowest byte + // last_status = bus->endTransmission(); +} + +// Read an 8-bit register +uint8_t VL53L0X::readReg(uint8_t reg) +{ + // uint8_t value; + std::vector data; + data.resize(1, 0); + read_i2c(this->bus, this->address, {reg}, 1, data); + // bus->beginTransmission(address); + // bus->write(reg); + // last_status = bus->endTransmission(); + + // bus->requestFrom(address, (uint8_t)1); + // value = bus->read(); + + return data[0]; +} + +// Read a 16-bit register +uint16_t VL53L0X::readReg16Bit(uint8_t reg) +{ + std::vector data; + data.resize(2, 0); + read_i2c(this->bus, this->address, {reg}, 2, data); + return data[0] << 8 | data[1]; + // uint16_t value; + + // bus->beginTransmission(address); + // bus->write(reg); + // last_status = bus->endTransmission(); + + // bus->requestFrom(address, (uint8_t)2); + // value = (uint16_t)bus->read() << 8; // value high byte + // value |= bus->read(); // value low byte + + // return value; +} + +// Read a 32-bit register +uint32_t VL53L0X::readReg32Bit(uint8_t reg) +{ + std::vector data; + data.resize(4, 0); + read_i2c(this->bus, this->address, {reg}, 4, data); + return data[0] << 24 | data[1] << 16 | data[2] << 24 | data[3]; + // uint32_t value; + + // bus->beginTransmission(address); + // bus->write(reg); + // last_status = bus->endTransmission(); + + // bus->requestFrom(address, (uint8_t)4); + // value = (uint32_t)bus->read() << 24; // value highest byte + // value |= (uint32_t)bus->read() << 16; + // value |= (uint16_t)bus->read() << 8; + // value |= bus->read(); // value lowest byte + + // return value; +} + +// Write an arbitrary number of bytes from the given array to the sensor, +// starting at the given register +void VL53L0X::writeMulti(uint8_t reg, uint8_t const *src, uint8_t count) +{ + std::vector data; + data.reserve(count + 1); + data.push_back(reg); + while (count-- > 0) + { + data.push_back(*(src++)); + } + write_i2c(this->bus, this->address, data); + + // bus->beginTransmission(address); + // bus->write(reg); + + // while (count-- > 0) + // { + // bus->write(*(src++)); + // } + + // last_status = bus->endTransmission(); +} + +// Read an arbitrary number of bytes from the sensor, starting at the given +// register, into the given array +void VL53L0X::readMulti(uint8_t reg, uint8_t *dst, uint8_t count) +{ + std::vector data; + data.resize(count, 0); + read_i2c(this->bus, this->address, {reg}, count, data); + for (int i = 0; i < count; i++) + { + *(dst++) = data[i]; + } + + // bus->beginTransmission(address); + // bus->write(reg); + // last_status = bus->endTransmission(); + + // bus->requestFrom(address, count); + + // while (count-- > 0) + // { + // *(dst++) = bus->read(); + // } +} + +// Set the return signal rate limit check value in units of MCPS (mega counts +// per second). "This represents the amplitude of the signal reflected from the +// target and detected by the device"; setting this limit presumably determines +// the minimum measurement necessary for the sensor to report a valid reading. +// Setting a lower limit increases the potential range of the sensor but also +// seems to increase the likelihood of getting an inaccurate reading because of +// unwanted reflections from objects other than the intended target. +// Defaults to 0.25 MCPS as initialized by the ST API and this library. +bool VL53L0X::setSignalRateLimit(float limit_Mcps) +{ + if (limit_Mcps < 0 || limit_Mcps > 511.99) + { + return false; + } + + // Q9.7 fixed point format (9 integer bits, 7 fractional bits) + writeReg16Bit(FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, limit_Mcps * (1 << 7)); + return true; +} + +// Get the return signal rate limit check value in MCPS +float VL53L0X::getSignalRateLimit() +{ + return (float)readReg16Bit(FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT) / (1 << 7); +} + +// 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 VL53L0X::setMeasurementTimingBudget(uint32_t budget_us) +{ + 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; + + 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; + } + + writeReg16Bit(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, + encodeTimeout(final_range_timeout_mclks)); + + // set_sequence_step_timeout() end + + measurement_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 VL53L0X::getMeasurementTimingBudget() +{ + 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); + } + + measurement_timing_budget_us = budget_us; // store for internal 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 VL53L0X::setVcselPulsePeriod(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: + writeReg(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x18); + break; + + case 14: + writeReg(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x30); + break; + + case 16: + writeReg(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x40); + break; + + case 18: + writeReg(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x50); + break; + + default: + // invalid period + return false; + } + writeReg(PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); + + // apply new VCSEL period + writeReg(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); + + writeReg16Bit(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); + + writeReg(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: + writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x10); + writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); + writeReg(GLOBAL_CONFIG_VCSEL_WIDTH, 0x02); + writeReg(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C); + writeReg(0xFF, 0x01); + writeReg(ALGO_PHASECAL_LIM, 0x30); + writeReg(0xFF, 0x00); + break; + + case 10: + writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x28); + writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); + writeReg(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03); + writeReg(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09); + writeReg(0xFF, 0x01); + writeReg(ALGO_PHASECAL_LIM, 0x20); + writeReg(0xFF, 0x00); + break; + + case 12: + writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x38); + writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); + writeReg(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03); + writeReg(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08); + writeReg(0xFF, 0x01); + writeReg(ALGO_PHASECAL_LIM, 0x20); + writeReg(0xFF, 0x00); + break; + + case 14: + writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x48); + writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); + writeReg(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03); + writeReg(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07); + writeReg(0xFF, 0x01); + writeReg(ALGO_PHASECAL_LIM, 0x20); + writeReg(0xFF, 0x00); + break; + + default: + // invalid period + return false; + } + + // apply new VCSEL period + writeReg(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; + } + + writeReg16Bit(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(measurement_timing_budget_us); + + // "Perform the phase calibration. This is needed after changing on vcsel period." + // VL53L0X_perform_phase_calibration() begin + + uint8_t sequence_config = readReg(SYSTEM_SEQUENCE_CONFIG); + writeReg(SYSTEM_SEQUENCE_CONFIG, 0x02); + performSingleRefCalibration(0x0); + writeReg(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 VL53L0X::getVcselPulsePeriod(vcselPeriodType type) +{ + if (type == VcselPeriodPreRange) + { + return decodeVcselPeriod(readReg(PRE_RANGE_CONFIG_VCSEL_PERIOD)); + } + else if (type == VcselPeriodFinalRange) + { + return decodeVcselPeriod(readReg(FINAL_RANGE_CONFIG_VCSEL_PERIOD)); + } + else + { + return 255; + } +} + +// Start continuous ranging measurements. If period_ms (optional) is 0 or not +// given, continuous back-to-back mode is used (the sensor takes measurements as +// often as possible); otherwise, continuous timed mode is used, with the given +// inter-measurement period in milliseconds determining how often the sensor +// takes a measurement. +// based on VL53L0X_StartMeasurement() +void VL53L0X::startContinuous(uint32_t period_ms) +{ + writeReg(0x80, 0x01); + writeReg(0xFF, 0x01); + writeReg(0x00, 0x00); + writeReg(0x91, stop_variable); + writeReg(0x00, 0x01); + writeReg(0xFF, 0x00); + writeReg(0x80, 0x00); + + if (period_ms != 0) + { + // continuous timed mode + + // VL53L0X_SetInterMeasurementPeriodMilliSeconds() begin + + uint16_t osc_calibrate_val = readReg16Bit(OSC_CALIBRATE_VAL); + + if (osc_calibrate_val != 0) + { + period_ms *= osc_calibrate_val; + } + + writeReg32Bit(SYSTEM_INTERMEASUREMENT_PERIOD, period_ms); + + // VL53L0X_SetInterMeasurementPeriodMilliSeconds() end + + writeReg(SYSRANGE_START, 0x04); // VL53L0X_REG_SYSRANGE_MODE_TIMED + } + else + { + // continuous back-to-back mode + writeReg(SYSRANGE_START, 0x02); // VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK + } +} + +// Stop continuous measurements +// based on VL53L0X_StopMeasurement() +void VL53L0X::stopContinuous() +{ + writeReg(SYSRANGE_START, 0x01); // VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT + + writeReg(0xFF, 0x01); + writeReg(0x00, 0x00); + writeReg(0x91, 0x00); + writeReg(0x00, 0x01); + writeReg(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 VL53L0X::readRangeContinuousMillimeters() +{ + startTimeout(); + while ((readReg(RESULT_INTERRUPT_STATUS) & 0x07) == 0) + { + if (checkTimeoutExpired()) + { + did_timeout = true; + return 65535; + } + } + + // assumptions: Linearity Corrective Gain is 1000 (default); + // fractional ranging is not enabled + uint16_t range = readReg16Bit(RESULT_RANGE_STATUS + 10); + + writeReg(SYSTEM_INTERRUPT_CLEAR, 0x01); + + return range; +} + +// Performs a single-shot range measurement and returns the reading in +// millimeters +// based on VL53L0X_PerformSingleRangingMeasurement() +uint16_t VL53L0X::readRangeSingleMillimeters() +{ + writeReg(0x80, 0x01); + writeReg(0xFF, 0x01); + writeReg(0x00, 0x00); + writeReg(0x91, stop_variable); + writeReg(0x00, 0x01); + writeReg(0xFF, 0x00); + writeReg(0x80, 0x00); + + writeReg(SYSRANGE_START, 0x01); + + // "Wait until start bit has been cleared" + startTimeout(); + while (readReg(SYSRANGE_START) & 0x01) + { + if (checkTimeoutExpired()) + { + did_timeout = true; + return 65535; + } + } + + return readRangeContinuousMillimeters(); +} + +// Did a timeout occur in one of the read functions since the last call to +// timeoutOccurred()? +bool VL53L0X::timeoutOccurred() +{ + bool tmp = did_timeout; + did_timeout = false; + return tmp; +} + +// 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 VL53L0X::getSpadInfo(uint8_t *count, bool *type_is_aperture) +{ + uint8_t tmp; + + writeReg(0x80, 0x01); + writeReg(0xFF, 0x01); + writeReg(0x00, 0x00); + + writeReg(0xFF, 0x06); + writeReg(0x83, readReg(0x83) | 0x04); + writeReg(0xFF, 0x07); + writeReg(0x81, 0x01); + + writeReg(0x80, 0x01); + + writeReg(0x94, 0x6b); + writeReg(0x83, 0x00); + startTimeout(); + while (readReg(0x83) == 0x00) + { + if (checkTimeoutExpired()) + { + return false; + } + } + writeReg(0x83, 0x01); + tmp = readReg(0x92); + + *count = tmp & 0x7f; + *type_is_aperture = (tmp >> 7) & 0x01; + + writeReg(0x81, 0x00); + writeReg(0xFF, 0x06); + writeReg(0x83, readReg(0x83) & ~0x04); + writeReg(0xFF, 0x01); + writeReg(0x00, 0x01); + + writeReg(0xFF, 0x00); + writeReg(0x80, 0x00); + + return true; +} + +// Get sequence step enables +// based on VL53L0X_GetSequenceStepEnables() +void VL53L0X::getSequenceStepEnables(SequenceStepEnables *enables) +{ + uint8_t sequence_config = readReg(SYSTEM_SEQUENCE_CONFIG); + + enables->tcc = (sequence_config >> 4) & 0x1; + enables->dss = (sequence_config >> 3) & 0x1; + enables->msrc = (sequence_config >> 2) & 0x1; + enables->pre_range = (sequence_config >> 6) & 0x1; + enables->final_range = (sequence_config >> 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 VL53L0X::getSequenceStepTimeouts(SequenceStepEnables const *enables, SequenceStepTimeouts *timeouts) +{ + timeouts->pre_range_vcsel_period_pclks = getVcselPulsePeriod(VcselPeriodPreRange); + + timeouts->msrc_dss_tcc_mclks = readReg(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(readReg16Bit(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(readReg16Bit(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); +} + +// 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 VL53L0X::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() +uint16_t VL53L0X::encodeTimeout(uint32_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); + } + else + { + return 0; + } +} + +// Convert sequence step timeout from MCLKs to microseconds with given VCSEL period in PCLKs +// based on VL53L0X_calc_timeout_us() +uint32_t VL53L0X::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 VL53L0X::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); +} + +// based on VL53L0X_perform_single_ref_calibration() +bool VL53L0X::performSingleRefCalibration(uint8_t vhv_init_byte) +{ + writeReg(SYSRANGE_START, 0x01 | vhv_init_byte); // VL53L0X_REG_SYSRANGE_MODE_START_STOP + + startTimeout(); + while ((readReg(RESULT_INTERRUPT_STATUS) & 0x07) == 0) + { + if (checkTimeoutExpired()) + { + return false; + } + } + + writeReg(SYSTEM_INTERRUPT_CLEAR, 0x01); + + writeReg(SYSRANGE_START, 0x00); + + return true; +} \ No newline at end of file diff --git a/src/i2c_helpers.cpp b/src/i2c_helpers.cpp new file mode 100644 index 0000000..4b2b6a0 --- /dev/null +++ b/src/i2c_helpers.cpp @@ -0,0 +1,47 @@ + +#include "i2c_helpers.hpp" + +int write_i2c(int i2c_port, int addr, const std::vector &bytes, bool nostop) +{ + i2c_inst_t *i2c; + + if (i2c_port) + { + i2c = i2c1; + } + else + { + i2c = i2c0; + } + int i2c_sdk_call_return_value = + i2c_write_blocking_until(i2c, (uint8_t)addr, bytes.data(), bytes.size(), + nostop, make_timeout_time_ms(50)); + return i2c_sdk_call_return_value == bytes.size(); +} + +int read_i2c(int i2c_port, int addr, const std::vector &write_bytes, + int bytes_to_read, std::vector &read_bytes) +{ + + // write i2c + if (!write_bytes.empty()) + { + write_i2c(i2c_port, addr, write_bytes, true); + } + i2c_inst_t *i2c; + + if (i2c_port) + { + i2c = i2c1; + } + else + { + i2c = i2c0; + } + read_bytes.resize(bytes_to_read, 0); + auto i2c_sdk_return = + i2c_read_blocking_until(i2c, addr, read_bytes.data(), bytes_to_read, + false, make_timeout_time_ms(50)); + + return i2c_sdk_return == bytes_to_read; +} From 79648eac68442eb8f89c7748a8a33b1ecde1581f Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Wed, 13 Sep 2023 10:15:45 +0200 Subject: [PATCH 13/33] Add mpu9250 --- include/MPU9250.hpp | 1031 ++++++++++++++++++++++++++ include/MPU9250/MPU9250RegisterMap.h | 151 ++++ include/MPU9250/QuaternionFilter.h | 242 ++++++ include/Telemetrix4RpiPico.hpp | 70 +- src/Telemetrix4RpiPico.cpp | 26 + 5 files changed, 1500 insertions(+), 20 deletions(-) create mode 100644 include/MPU9250.hpp create mode 100644 include/MPU9250/MPU9250RegisterMap.h create mode 100644 include/MPU9250/QuaternionFilter.h diff --git a/include/MPU9250.hpp b/include/MPU9250.hpp new file mode 100644 index 0000000..325b6c4 --- /dev/null +++ b/include/MPU9250.hpp @@ -0,0 +1,1031 @@ +#pragma once +#ifndef MPU9250_H +#define MPU9250_H +// Code for the MPU9250 copied and adapted from https://github.com/hideakitai/MPU9250 for the RP2040. +#include +#include +#include +#include "i2c_helpers.hpp" +#include "MPU9250/MPU9250RegisterMap.h" +#include "MPU9250/QuaternionFilter.h" + +const float DEG_TO_RAD = (PI / 180.0); + + + +enum class ACCEL_FS_SEL { + A2G, + A4G, + A8G, + A16G +}; +enum class GYRO_FS_SEL { + G250DPS, + G500DPS, + G1000DPS, + G2000DPS +}; +enum class MAG_OUTPUT_BITS { + M14BITS, + M16BITS +}; + +enum class FIFO_SAMPLE_RATE : uint8_t { + SMPL_1000HZ, + SMPL_500HZ, + SMPL_333HZ, + SMPL_250HZ, + SMPL_200HZ, + SMPL_167HZ, + SMPL_143HZ, + SMPL_125HZ, +}; + +enum class GYRO_DLPF_CFG : uint8_t { + DLPF_250HZ, + DLPF_184HZ, + DLPF_92HZ, + DLPF_41HZ, + DLPF_20HZ, + DLPF_10HZ, + DLPF_5HZ, + DLPF_3600HZ, +}; + +enum class ACCEL_DLPF_CFG : uint8_t { + DLPF_218HZ_0, + DLPF_218HZ_1, + DLPF_99HZ, + DLPF_45HZ, + DLPF_21HZ, + DLPF_10HZ, + DLPF_5HZ, + DLPF_420HZ, +}; + +static constexpr uint8_t MPU9250_WHOAMI_DEFAULT_VALUE {0x71}; +static constexpr uint8_t MPU9255_WHOAMI_DEFAULT_VALUE {0x73}; +static constexpr uint8_t MPU6500_WHOAMI_DEFAULT_VALUE {0x70}; + +struct MPU9250Setting { + ACCEL_FS_SEL accel_fs_sel {ACCEL_FS_SEL::A16G}; + GYRO_FS_SEL gyro_fs_sel {GYRO_FS_SEL::G2000DPS}; + MAG_OUTPUT_BITS mag_output_bits {MAG_OUTPUT_BITS::M16BITS}; + FIFO_SAMPLE_RATE fifo_sample_rate {FIFO_SAMPLE_RATE::SMPL_200HZ}; + uint8_t gyro_fchoice {0x03}; + GYRO_DLPF_CFG gyro_dlpf_cfg {GYRO_DLPF_CFG::DLPF_41HZ}; + uint8_t accel_fchoice {0x01}; + ACCEL_DLPF_CFG accel_dlpf_cfg {ACCEL_DLPF_CFG::DLPF_45HZ}; +}; + +class MPU9250_ { + static constexpr uint8_t MPU9250_DEFAULT_ADDRESS {0x68}; // Device address when ADO = 0 + static constexpr uint8_t AK8963_ADDRESS {0x0C}; // Address of magnetometer + static constexpr uint8_t AK8963_WHOAMI_DEFAULT_VALUE {0x48}; + uint8_t mpu_i2c_addr {MPU9250_DEFAULT_ADDRESS}; + + // settings + MPU9250Setting setting; + // TODO: this should be configured!! + static constexpr uint8_t MAG_MODE {0x06}; // 0x02 for 8 Hz, 0x06 for 100 Hz continuous magnetometer data read + float acc_resolution {0.f}; // scale resolutions per LSB for the sensors + float gyro_resolution {0.f}; // scale resolutions per LSB for the sensors + float mag_resolution {0.f}; // scale resolutions per LSB for the sensors + + // Calibration Parameters + float acc_bias[3] {0., 0., 0.}; // acc calibration value in ACCEL_FS_SEL: 2g + float gyro_bias[3] {0., 0., 0.}; // gyro calibration value in GYRO_FS_SEL: 250dps + float mag_bias_factory[3] {0., 0., 0.}; + float mag_bias[3] {0., 0., 0.}; // mag calibration value in MAG_OUTPUT_BITS: 16BITS + float mag_scale[3] {1., 1., 1.}; + float magnetic_declination = -7.51; // Japan, 24th June + + // Temperature + int16_t temperature_count {0}; // temperature raw count output + float temperature {0.f}; // Stores the real internal chip temperature in degrees Celsius + + // Self Test + float self_test_result[6] {0.f}; // holds results of gyro and accelerometer self test + + // IMU Data + float a[3] {0.f, 0.f, 0.f}; + float g[3] {0.f, 0.f, 0.f}; + float m[3] {0.f, 0.f, 0.f}; + float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion + float rpy[3] {0.f, 0.f, 0.f}; + float lin_acc[3] {0.f, 0.f, 0.f}; // linear acceleration (acceleration with gravity component subtracted) + QuaternionFilter quat_filter; + size_t n_filter_iter {1}; + + // Other settings + bool has_connected {false}; + bool b_ahrs {true}; + bool b_verbose {false}; + + // I2C + uint8_t i2c_err_; + +public: + int bus = 0; + + static constexpr uint16_t CALIB_GYRO_SENSITIVITY {131}; // LSB/degrees/sec + static constexpr uint16_t CALIB_ACCEL_SENSITIVITY {16384}; // LSB/g + + bool setup(const uint8_t addr, const MPU9250Setting& mpu_setting = MPU9250Setting(), int bus = 0) { + // addr should be valid for MPU + if ((addr < MPU9250_DEFAULT_ADDRESS) || (addr > MPU9250_DEFAULT_ADDRESS + 7)) { + // // Serialprint("I2C address 0x"); + // // Serialprint(addr, HEX); + // // Serialprintln(" is not valid for MPU. Please check your I2C address."); + return false; + } + mpu_i2c_addr = addr; + setting = mpu_setting; + this->bus = bus; + + if (isConnectedMPU9250()) { + initMPU9250(); + if (isConnectedAK8963()) + initAK8963(); + else { + if (b_verbose) + // Serialprintln("Could not connect to AK8963"); + has_connected = false; + return false; + } + } else { + if (b_verbose) + // Serialprintln("Could not connect to MPU9250"); + has_connected = false; + return false; + } + has_connected = true; + return true; + } + + void sleep(bool b) { + uint8_t c = read_byte(mpu_i2c_addr, PWR_MGMT_1); // read the value, change sleep bit to match b, write byte back to register + if (b) { + c = c | 0x40; // sets the sleep bit + } else { + c = c & 0xBF; // mask 1011111 keeps all the previous bits + } + write_byte(mpu_i2c_addr, PWR_MGMT_1, c); + } + + void verbose(const bool b) { + b_verbose = b; + } + + void ahrs(const bool b) { + b_ahrs = b; + } + + void calibrateAccelGyro() { + calibrate_acc_gyro_impl(); + } + + void calibrateMag() { + calibrate_mag_impl(); + } + + bool isConnected() { + has_connected = isConnectedMPU9250() && isConnectedAK8963(); + return has_connected; + } + + bool isConnectedMPU9250() { + uint8_t c = read_byte(mpu_i2c_addr, WHO_AM_I_MPU9250); + if (b_verbose) { + // Serialprint("MPU9250 WHO AM I = "); + // Serialprintln(c, HEX); + } + bool b = (c == MPU9250_WHOAMI_DEFAULT_VALUE); + b |= (c == MPU9255_WHOAMI_DEFAULT_VALUE); + b |= (c == MPU6500_WHOAMI_DEFAULT_VALUE); + return b; + } + + bool isConnectedAK8963() { + uint8_t c = read_byte(AK8963_ADDRESS, AK8963_WHO_AM_I); + if (b_verbose) { + // Serialprint("AK8963 WHO AM I = "); + // Serialprintln(c, HEX); + } + return (c == AK8963_WHOAMI_DEFAULT_VALUE); + } + + bool isSleeping() { + uint8_t c = read_byte(mpu_i2c_addr, PWR_MGMT_1); + return (c & 0x40) == 0x40; + } + + bool available() { + return has_connected && (read_byte(mpu_i2c_addr, INT_STATUS) & 0x01); + } + + bool update() { + if (!available()) return false; + + update_accel_gyro(); + update_mag(); + + // Madgwick function needs to be fed North, East, and Down direction like + // (AN, AE, AD, GN, GE, GD, MN, ME, MD) + // Accel and Gyro direction is Right-Hand, X-Forward, Z-Up + // Magneto direction is Right-Hand, Y-Forward, Z-Down + // So to adopt to the general Aircraft coordinate system (Right-Hand, X-Forward, Z-Down), + // we need to feed (ax, -ay, -az, gx, -gy, -gz, my, -mx, mz) + // but we pass (-ax, ay, az, gx, -gy, -gz, my, -mx, mz) + // because gravity is by convention positive down, we need to ivnert the accel data + + // get quaternion based on aircraft coordinate (Right-Hand, X-Forward, Z-Down) + // acc[mg], gyro[deg/s], mag [mG] + // gyro will be convert from [deg/s] to [rad/s] inside of this function + // quat_filter.update(-a[0], a[1], a[2], g[0] * DEG_TO_RAD, -g[1] * DEG_TO_RAD, -g[2] * DEG_TO_RAD, m[1], -m[0], m[2], q); + + float an = -a[0]; + float ae = +a[1]; + float ad = +a[2]; + float gn = +g[0] * DEG_TO_RAD; + float ge = -g[1] * DEG_TO_RAD; + float gd = -g[2] * DEG_TO_RAD; + float mn = +m[1]; + float me = -m[0]; + float md = +m[2]; + + for (size_t i = 0; i < n_filter_iter; ++i) { + quat_filter.update(an, ae, ad, gn, ge, gd, mn, me, md, q); + } + + if (!b_ahrs) { + temperature_count = read_temperature_data(); // Read the adc values + temperature = ((float)temperature_count) / 333.87 + 21.0; // Temperature in degrees Centigrade + } else { + update_rpy(q[0], q[1], q[2], q[3]); + } + return true; + } + + float getRoll() const { return rpy[0]; } + float getPitch() const { return rpy[1]; } + float getYaw() const { return rpy[2]; } + + float getEulerX() const { return rpy[0]; } + float getEulerY() const { return -rpy[1]; } + float getEulerZ() const { return -rpy[2]; } + + float getQuaternionX() const { return q[1]; } + float getQuaternionY() const { return q[2]; } + float getQuaternionZ() const { return q[3]; } + float getQuaternionW() const { return q[0]; } + + float getAcc(const uint8_t i) const { return (i < 3) ? a[i] : 0.f; } + float getGyro(const uint8_t i) const { return (i < 3) ? g[i] : 0.f; } + float getMag(const uint8_t i) const { return (i < 3) ? m[i] : 0.f; } + float getLinearAcc(const uint8_t i) const { return (i < 3) ? lin_acc[i] : 0.f; } + + float getAccX() const { return a[0]; } + float getAccY() const { return a[1]; } + float getAccZ() const { return a[2]; } + float getGyroX() const { return g[0]; } + float getGyroY() const { return g[1]; } + float getGyroZ() const { return g[2]; } + float getMagX() const { return m[0]; } + float getMagY() const { return m[1]; } + float getMagZ() const { return m[2]; } + float getLinearAccX() const { return lin_acc[0]; } + float getLinearAccY() const { return lin_acc[1]; } + float getLinearAccZ() const { return lin_acc[2]; } + + float getAccBias(const uint8_t i) const { return (i < 3) ? acc_bias[i] : 0.f; } + float getGyroBias(const uint8_t i) const { return (i < 3) ? gyro_bias[i] : 0.f; } + float getMagBias(const uint8_t i) const { return (i < 3) ? mag_bias[i] : 0.f; } + float getMagScale(const uint8_t i) const { return (i < 3) ? mag_scale[i] : 0.f; } + + float getAccBiasX() const { return acc_bias[0]; } + float getAccBiasY() const { return acc_bias[1]; } + float getAccBiasZ() const { return acc_bias[2]; } + float getGyroBiasX() const { return gyro_bias[0]; } + float getGyroBiasY() const { return gyro_bias[1]; } + float getGyroBiasZ() const { return gyro_bias[2]; } + float getMagBiasX() const { return mag_bias[0]; } + float getMagBiasY() const { return mag_bias[1]; } + float getMagBiasZ() const { return mag_bias[2]; } + float getMagScaleX() const { return mag_scale[0]; } + float getMagScaleY() const { return mag_scale[1]; } + float getMagScaleZ() const { return mag_scale[2]; } + + float getTemperature() const { return temperature; } + + void setAccBias(const float x, const float y, const float z) { + acc_bias[0] = x; + acc_bias[1] = y; + acc_bias[2] = z; + write_accel_offset(); + } + void setGyroBias(const float x, const float y, const float z) { + gyro_bias[0] = x; + gyro_bias[1] = y; + gyro_bias[2] = z; + write_gyro_offset(); + } + void setMagBias(const float x, const float y, const float z) { + mag_bias[0] = x; + mag_bias[1] = y; + mag_bias[2] = z; + } + void setMagScale(const float x, const float y, const float z) { + mag_scale[0] = x; + mag_scale[1] = y; + mag_scale[2] = z; + } + void setMagneticDeclination(const float d) { magnetic_declination = d; } + + void selectFilter(QuatFilterSel sel) { + quat_filter.select_filter(sel); + } + + void setFilterIterations(const size_t n) { + if (n > 0) n_filter_iter = n; + } + + bool selftest() { + return self_test_impl(); + } + +private: + void initMPU9250() { + acc_resolution = get_acc_resolution(setting.accel_fs_sel); + gyro_resolution = get_gyro_resolution(setting.gyro_fs_sel); + mag_resolution = get_mag_resolution(setting.mag_output_bits); + + // reset device + write_byte(mpu_i2c_addr, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device + sleep_ms(100); + + // wake up device + write_byte(mpu_i2c_addr, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors + sleep_ms(100); // Wait for all registers to reset + + // get stable time source + write_byte(mpu_i2c_addr, PWR_MGMT_1, 0x01); // Auto select clock source to be PLL gyroscope reference if ready else + sleep_ms(200); + + // Configure Gyro and Thermometer + // Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively; + // minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot + // be higher than 1 / 0.0059 = 170 Hz + // GYRO_DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both + // With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz + uint8_t mpu_config = (uint8_t)setting.gyro_dlpf_cfg; + write_byte(mpu_i2c_addr, MPU_CONFIG, mpu_config); + + // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV) + uint8_t sample_rate = (uint8_t)setting.fifo_sample_rate; + write_byte(mpu_i2c_addr, SMPLRT_DIV, sample_rate); // Use a 200 Hz rate; a rate consistent with the filter update rate + // determined inset in CONFIG above + + // Set gyroscope full scale range + // Range selects FS_SEL and GFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3 + uint8_t c = read_byte(mpu_i2c_addr, GYRO_CONFIG); // get current GYRO_CONFIG register value + c = c & ~0xE0; // Clear self-test bits [7:5] + c = c & ~0x03; // Clear Fchoice bits [1:0] + c = c & ~0x18; // Clear GYRO_FS_SEL bits [4:3] + c = c | (uint8_t(setting.gyro_fs_sel) << 3); // Set full scale range for the gyro + c = c | (uint8_t(~setting.gyro_fchoice) & 0x03); // Set Fchoice for the gyro + write_byte(mpu_i2c_addr, GYRO_CONFIG, c); // Write new GYRO_CONFIG value to register + + // Set accelerometer full-scale range configuration + c = read_byte(mpu_i2c_addr, ACCEL_CONFIG); // get current ACCEL_CONFIG register value + c = c & ~0xE0; // Clear self-test bits [7:5] + c = c & ~0x18; // Clear ACCEL_FS_SEL bits [4:3] + c = c | (uint8_t(setting.accel_fs_sel) << 3); // Set full scale range for the accelerometer + write_byte(mpu_i2c_addr, ACCEL_CONFIG, c); // Write new ACCEL_CONFIG register value + + // Set accelerometer sample rate configuration + // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for + // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz + c = read_byte(mpu_i2c_addr, ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value + c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0]) + c = c | (~(setting.accel_fchoice << 3) & 0x08); // Set accel_fchoice_b to 1 + c = c | (uint8_t(setting.accel_dlpf_cfg) & 0x07); // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz + write_byte(mpu_i2c_addr, ACCEL_CONFIG2, c); // Write new ACCEL_CONFIG2 register value + + // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates, + // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting + + // Configure Interrupts and Bypass Enable + // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared, + // clear on read of INT_STATUS, and enable I2C_BYPASS_EN so additional chips + // can join the I2C bus and all can be controlled by the Arduino as master + write_byte(mpu_i2c_addr, INT_PIN_CFG, 0x22); + write_byte(mpu_i2c_addr, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt + sleep_ms(100); + } + + void initAK8963() { + // First extract the factory calibration for each magnetometer axis + uint8_t raw_data[3]; // x/y/z gyro calibration data stored here + write_byte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer + sleep_ms(10); + write_byte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode + sleep_ms(10); + read_bytes(AK8963_ADDRESS, AK8963_ASAX, 3, &raw_data[0]); // Read the x-, y-, and z-axis calibration values + mag_bias_factory[0] = (float)(raw_data[0] - 128) / 256. + 1.; // Return x-axis sensitivity adjustment values, etc. + mag_bias_factory[1] = (float)(raw_data[1] - 128) / 256. + 1.; + mag_bias_factory[2] = (float)(raw_data[2] - 128) / 256. + 1.; + write_byte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer + sleep_ms(10); + // Configure the magnetometer for continuous read and highest resolution + // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register, + // and enable continuous mode data acquisition MAG_MODE (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates + write_byte(AK8963_ADDRESS, AK8963_CNTL, (uint8_t)setting.mag_output_bits << 4 | MAG_MODE); // Set magnetometer data resolution and sample ODR + sleep_ms(10); + + if (b_verbose) { + // Serialprintln("Mag Factory Calibration Values: "); + // Serialprint("X-Axis sensitivity offset value "); + // Serialprintln(mag_bias_factory[0], 2); + // Serialprint("Y-Axis sensitivity offset value "); + // Serialprintln(mag_bias_factory[1], 2); + // Serialprint("Z-Axis sensitivity offset value "); + // Serialprintln(mag_bias_factory[2], 2); + } + } + +public: + void update_rpy(float qw, float qx, float qy, float qz) { + // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. + // In this coordinate system, the positive z-axis is down toward Earth. + // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise. + // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. + // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. + // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. + // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be + // applied in the correct order which for this configuration is yaw, pitch, and then roll. + // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. + float a12, a22, a31, a32, a33; // rotation matrix coefficients for Euler angles and gravity components + a12 = 2.0f * (qx * qy + qw * qz); + a22 = qw * qw + qx * qx - qy * qy - qz * qz; + a31 = 2.0f * (qw * qx + qy * qz); + a32 = 2.0f * (qx * qz - qw * qy); + a33 = qw * qw - qx * qx - qy * qy + qz * qz; + rpy[0] = atan2f(a31, a33); + rpy[1] = -asinf(a32); + rpy[2] = atan2f(a12, a22); + rpy[0] *= 180.0f / PI; + rpy[1] *= 180.0f / PI; + rpy[2] *= 180.0f / PI; + rpy[2] += magnetic_declination; + if (rpy[2] >= +180.f) + rpy[2] -= 360.f; + else if (rpy[2] < -180.f) + rpy[2] += 360.f; + + lin_acc[0] = a[0] + a31; + lin_acc[1] = a[1] + a32; + lin_acc[2] = a[2] - a33; + } + + void update_accel_gyro() { + int16_t raw_acc_gyro_data[7]; // used to read all 14 bytes at once from the MPU9250 accel/gyro + read_accel_gyro(raw_acc_gyro_data); // INT cleared on any read + + // Now we'll calculate the accleration value into actual g's + a[0] = (float)raw_acc_gyro_data[0] * acc_resolution; // get actual g value, this depends on scale being set + a[1] = (float)raw_acc_gyro_data[1] * acc_resolution; + a[2] = (float)raw_acc_gyro_data[2] * acc_resolution; + + temperature_count = raw_acc_gyro_data[3]; // Read the adc values + temperature = ((float)temperature_count) / 333.87 + 21.0; // Temperature in degrees Centigrade + + // Calculate the gyro value into actual degrees per second + g[0] = (float)raw_acc_gyro_data[4] * gyro_resolution; // get actual gyro value, this depends on scale being set + g[1] = (float)raw_acc_gyro_data[5] * gyro_resolution; + g[2] = (float)raw_acc_gyro_data[6] * gyro_resolution; + } + +private: + void read_accel_gyro(int16_t* destination) { + uint8_t raw_data[14]; // x/y/z accel register data stored here + read_bytes(mpu_i2c_addr, ACCEL_XOUT_H, 14, &raw_data[0]); // Read the 14 raw data registers into data array + destination[0] = ((int16_t)raw_data[0] << 8) | (int16_t)raw_data[1]; // Turn the MSB and LSB into a signed 16-bit value + destination[1] = ((int16_t)raw_data[2] << 8) | (int16_t)raw_data[3]; + destination[2] = ((int16_t)raw_data[4] << 8) | (int16_t)raw_data[5]; + destination[3] = ((int16_t)raw_data[6] << 8) | (int16_t)raw_data[7]; + destination[4] = ((int16_t)raw_data[8] << 8) | (int16_t)raw_data[9]; + destination[5] = ((int16_t)raw_data[10] << 8) | (int16_t)raw_data[11]; + destination[6] = ((int16_t)raw_data[12] << 8) | (int16_t)raw_data[13]; + } + +public: + void update_mag() { + int16_t mag_count[3] = {0, 0, 0}; // Stores the 16-bit signed magnetometer sensor output + + // Read the x/y/z adc values + if (read_mag(mag_count)) { + // Calculate the magnetometer values in milliGauss + // Include factory calibration per data sheet and user environmental corrections + // mag_bias is calcurated in 16BITS + float bias_to_current_bits = mag_resolution / get_mag_resolution(MAG_OUTPUT_BITS::M16BITS); + m[0] = (float)(mag_count[0] * mag_resolution * mag_bias_factory[0] - mag_bias[0] * bias_to_current_bits) * mag_scale[0]; // get actual magnetometer value, this depends on scale being set + m[1] = (float)(mag_count[1] * mag_resolution * mag_bias_factory[1] - mag_bias[1] * bias_to_current_bits) * mag_scale[1]; + m[2] = (float)(mag_count[2] * mag_resolution * mag_bias_factory[2] - mag_bias[2] * bias_to_current_bits) * mag_scale[2]; + } + } + +private: + bool read_mag(int16_t* destination) { + const uint8_t st1 = read_byte(AK8963_ADDRESS, AK8963_ST1); + if (st1 & 0x01) { // wait for magnetometer data ready bit to be set + uint8_t raw_data[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition + read_bytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &raw_data[0]); // Read the six raw data and ST2 registers sequentially into data array + if (MAG_MODE == 0x02 || MAG_MODE == 0x04 || MAG_MODE == 0x06) { // continuous or external trigger read mode + if ((st1 & 0x02) != 0) // check if data is not skipped + return false; // this should be after data reading to clear DRDY register + } + + uint8_t c = raw_data[6]; // End data read by reading ST2 register + if (!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data + destination[0] = ((int16_t)raw_data[1] << 8) | raw_data[0]; // Turn the MSB and LSB into a signed 16-bit value + destination[1] = ((int16_t)raw_data[3] << 8) | raw_data[2]; // Data stored as little Endian + destination[2] = ((int16_t)raw_data[5] << 8) | raw_data[4]; + return true; + } + } + return false; + } + + int16_t read_temperature_data() { + uint8_t raw_data[2]; // x/y/z gyro register data stored here + read_bytes(mpu_i2c_addr, TEMP_OUT_H, 2, &raw_data[0]); // Read the two raw data registers sequentially into data array + return ((int16_t)raw_data[0] << 8) | raw_data[1]; // Turn the MSB and LSB into a 16-bit value + } + + // Function which accumulates gyro and accelerometer data after device initialization. It calculates the average + // of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers. + // ACCEL_FS_SEL: 2g (maximum sensitivity) + // GYRO_FS_SEL: 250dps (maximum sensitivity) + void calibrate_acc_gyro_impl() { + set_acc_gyro_to_calibration(); + collect_acc_gyro_data_to(acc_bias, gyro_bias); + write_accel_offset(); + write_gyro_offset(); + sleep_ms(100); + initMPU9250(); + sleep_ms(1000); + } + + void set_acc_gyro_to_calibration() { + // reset device + write_byte(mpu_i2c_addr, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device + sleep_ms(100); + + // get stable time source; Auto select clock source to be PLL gyroscope reference if ready + // else use the internal oscillator, bits 2:0 = 001 + write_byte(mpu_i2c_addr, PWR_MGMT_1, 0x01); + write_byte(mpu_i2c_addr, PWR_MGMT_2, 0x00); + sleep_ms(200); + + // Configure device for bias calculation + write_byte(mpu_i2c_addr, INT_ENABLE, 0x00); // Disable all interrupts + write_byte(mpu_i2c_addr, FIFO_EN, 0x00); // Disable FIFO + write_byte(mpu_i2c_addr, PWR_MGMT_1, 0x00); // Turn on internal clock source + write_byte(mpu_i2c_addr, I2C_MST_CTRL, 0x00); // Disable I2C master + write_byte(mpu_i2c_addr, USER_CTRL, 0x00); // Disable FIFO and I2C master modes + write_byte(mpu_i2c_addr, USER_CTRL, 0x0C); // Reset FIFO and DMP + sleep_ms(15); + + // Configure MPU6050 gyro and accelerometer for bias calculation + write_byte(mpu_i2c_addr, MPU_CONFIG, 0x01); // Set low-pass filter to 188 Hz + write_byte(mpu_i2c_addr, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz + write_byte(mpu_i2c_addr, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity + write_byte(mpu_i2c_addr, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity + + // Configure FIFO to capture accelerometer and gyro data for bias calculation + write_byte(mpu_i2c_addr, USER_CTRL, 0x40); // Enable FIFO + write_byte(mpu_i2c_addr, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150) + sleep_ms(40); // accumulate 40 samples in 40 milliseconds = 480 bytes + } + + void collect_acc_gyro_data_to(float* a_bias, float* g_bias) { + // At end of sample accumulation, turn off FIFO sensor read + uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data + write_byte(mpu_i2c_addr, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO + read_bytes(mpu_i2c_addr, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count + uint16_t fifo_count = ((uint16_t)data[0] << 8) | data[1]; + uint16_t packet_count = fifo_count / 12; // How many sets of full gyro and accelerometer data for averaging + + for (uint16_t ii = 0; ii < packet_count; ii++) { + int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; + read_bytes(mpu_i2c_addr, FIFO_R_W, 12, &data[0]); // read data for averaging + accel_temp[0] = (int16_t)(((int16_t)data[0] << 8) | data[1]); // Form signed 16-bit integer for each sample in FIFO + accel_temp[1] = (int16_t)(((int16_t)data[2] << 8) | data[3]); + accel_temp[2] = (int16_t)(((int16_t)data[4] << 8) | data[5]); + gyro_temp[0] = (int16_t)(((int16_t)data[6] << 8) | data[7]); + gyro_temp[1] = (int16_t)(((int16_t)data[8] << 8) | data[9]); + gyro_temp[2] = (int16_t)(((int16_t)data[10] << 8) | data[11]); + + a_bias[0] += (float)accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases + a_bias[1] += (float)accel_temp[1]; + a_bias[2] += (float)accel_temp[2]; + g_bias[0] += (float)gyro_temp[0]; + g_bias[1] += (float)gyro_temp[1]; + g_bias[2] += (float)gyro_temp[2]; + } + a_bias[0] /= (float)packet_count; // Normalize sums to get average count biases + a_bias[1] /= (float)packet_count; + a_bias[2] /= (float)packet_count; + g_bias[0] /= (float)packet_count; + g_bias[1] /= (float)packet_count; + g_bias[2] /= (float)packet_count; + + if (a_bias[2] > 0L) { + a_bias[2] -= (float)CALIB_ACCEL_SENSITIVITY; + } // Remove gravity from the z-axis accelerometer bias calculation + else { + a_bias[2] += (float)CALIB_ACCEL_SENSITIVITY; + } + } + + void write_accel_offset() { + // Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain + // factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold + // non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature + // compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that + // the accelerometer biases calculated above must be divided by 8. + + uint8_t read_data[2] = {0}; + int16_t acc_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases + read_bytes(mpu_i2c_addr, XA_OFFSET_H, 2, &read_data[0]); // Read factory accelerometer trim values + acc_bias_reg[0] = ((int16_t)read_data[0] << 8) | read_data[1]; + read_bytes(mpu_i2c_addr, YA_OFFSET_H, 2, &read_data[0]); + acc_bias_reg[1] = ((int16_t)read_data[0] << 8) | read_data[1]; + read_bytes(mpu_i2c_addr, ZA_OFFSET_H, 2, &read_data[0]); + acc_bias_reg[2] = ((int16_t)read_data[0] << 8) | read_data[1]; + + int16_t mask_bit[3] = {1, 1, 1}; // Define array to hold mask bit for each accelerometer bias axis + for (int i = 0; i < 3; i++) { + if (acc_bias_reg[i] % 2) { + mask_bit[i] = 0; + } + acc_bias_reg[i] -= (int16_t)acc_bias[i] >> 3; // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g + if (mask_bit[i]) { + acc_bias_reg[i] = acc_bias_reg[i] & ~mask_bit[i]; // Preserve temperature compensation bit + } else { + acc_bias_reg[i] = acc_bias_reg[i] | 0x0001; // Preserve temperature compensation bit + } + } + + uint8_t write_data[6] = {0}; + write_data[0] = (acc_bias_reg[0] >> 8) & 0xFF; + write_data[1] = (acc_bias_reg[0]) & 0xFF; + write_data[2] = (acc_bias_reg[1] >> 8) & 0xFF; + write_data[3] = (acc_bias_reg[1]) & 0xFF; + write_data[4] = (acc_bias_reg[2] >> 8) & 0xFF; + write_data[5] = (acc_bias_reg[2]) & 0xFF; + + // Push accelerometer biases to hardware registers + write_byte(mpu_i2c_addr, XA_OFFSET_H, write_data[0]); + write_byte(mpu_i2c_addr, XA_OFFSET_L, write_data[1]); + write_byte(mpu_i2c_addr, YA_OFFSET_H, write_data[2]); + write_byte(mpu_i2c_addr, YA_OFFSET_L, write_data[3]); + write_byte(mpu_i2c_addr, ZA_OFFSET_H, write_data[4]); + write_byte(mpu_i2c_addr, ZA_OFFSET_L, write_data[5]); + } + + void write_gyro_offset() { + // Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup + uint8_t gyro_offset_data[6] {0}; + gyro_offset_data[0] = (-(int16_t)gyro_bias[0] / 4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format + gyro_offset_data[1] = (-(int16_t)gyro_bias[0] / 4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases + gyro_offset_data[2] = (-(int16_t)gyro_bias[1] / 4 >> 8) & 0xFF; + gyro_offset_data[3] = (-(int16_t)gyro_bias[1] / 4) & 0xFF; + gyro_offset_data[4] = (-(int16_t)gyro_bias[2] / 4 >> 8) & 0xFF; + gyro_offset_data[5] = (-(int16_t)gyro_bias[2] / 4) & 0xFF; + + // Push gyro biases to hardware registers + write_byte(mpu_i2c_addr, XG_OFFSET_H, gyro_offset_data[0]); + write_byte(mpu_i2c_addr, XG_OFFSET_L, gyro_offset_data[1]); + write_byte(mpu_i2c_addr, YG_OFFSET_H, gyro_offset_data[2]); + write_byte(mpu_i2c_addr, YG_OFFSET_L, gyro_offset_data[3]); + write_byte(mpu_i2c_addr, ZG_OFFSET_H, gyro_offset_data[4]); + write_byte(mpu_i2c_addr, ZG_OFFSET_L, gyro_offset_data[5]); + } + + // mag calibration is executed in MAG_OUTPUT_BITS: 16BITS + void calibrate_mag_impl() { + // set MAG_OUTPUT_BITS to maximum to calibrate + MAG_OUTPUT_BITS mag_output_bits_cache = setting.mag_output_bits; + setting.mag_output_bits = MAG_OUTPUT_BITS::M16BITS; + initAK8963(); + collect_mag_data_to(mag_bias, mag_scale); + + if (b_verbose) { + // Serialprintln("Mag Calibration done!"); + + // Serialprintln("AK8963 mag biases (mG)"); + // Serialprint(mag_bias[0]); + // Serialprint(", "); + // Serialprint(mag_bias[1]); + // Serialprint(", "); + // Serialprint(mag_bias[2]); + // Serialprintln(); + // Serialprintln("AK8963 mag scale (mG)"); + // Serialprint(mag_scale[0]); + // Serialprint(", "); + // Serialprint(mag_scale[1]); + // Serialprint(", "); + // Serialprint(mag_scale[2]); + // Serialprintln(); + } + + // restore MAG_OUTPUT_BITS + setting.mag_output_bits = mag_output_bits_cache; + initAK8963(); + } + + void collect_mag_data_to(float* m_bias, float* m_scale) { + if (b_verbose) + // Serialprintln("Mag Calibration: Wave device in a figure eight until done!"); + sleep_ms(4000); + + // shoot for ~fifteen seconds of mag data + uint16_t sample_count = 0; + if (MAG_MODE == 0x02) + sample_count = 128; // at 8 Hz ODR, new mag data is available every 125 ms + else if (MAG_MODE == 0x06) // in this library, fixed to 100Hz + sample_count = 1500; // at 100 Hz ODR, new mag data is available every 10 ms + + int32_t bias[3] = {0, 0, 0}, scale[3] = {0, 0, 0}; + int16_t mag_max[3] = {-32767, -32767, -32767}; + int16_t mag_min[3] = {32767, 32767, 32767}; + int16_t mag_temp[3] = {0, 0, 0}; + for (uint16_t ii = 0; ii < sample_count; ii++) { + read_mag(mag_temp); // Read the mag data + for (int jj = 0; jj < 3; jj++) { + if (mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj]; + if (mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj]; + } + if (MAG_MODE == 0x02) sleep_ms(135); // at 8 Hz ODR, new mag data is available every 125 ms + if (MAG_MODE == 0x06) sleep_ms(12); // at 100 Hz ODR, new mag data is available every 10 ms + } + + if (b_verbose) { + // Serialprintln("mag x min/max:"); + // Serialprintln(mag_min[0]); + // Serialprintln(mag_max[0]); + // Serialprintln("mag y min/max:"); + // Serialprintln(mag_min[1]); + // Serialprintln(mag_max[1]); + // Serialprintln("mag z min/max:"); + // Serialprintln(mag_min[2]); + // Serialprintln(mag_max[2]); + } + + // Get hard iron correction + bias[0] = (mag_max[0] + mag_min[0]) / 2; // get average x mag bias in counts + bias[1] = (mag_max[1] + mag_min[1]) / 2; // get average y mag bias in counts + bias[2] = (mag_max[2] + mag_min[2]) / 2; // get average z mag bias in counts + + float bias_resolution = get_mag_resolution(MAG_OUTPUT_BITS::M16BITS); + m_bias[0] = (float)bias[0] * bias_resolution * mag_bias_factory[0]; // save mag biases in G for main program + m_bias[1] = (float)bias[1] * bias_resolution * mag_bias_factory[1]; + m_bias[2] = (float)bias[2] * bias_resolution * mag_bias_factory[2]; + + // Get soft iron correction estimate + //*** multiplication by mag_bias_factory added in accordance with the following comment + //*** https://github.com/kriswiner/MPU9250/issues/456#issue-836657973 + scale[0] = (float)(mag_max[0] - mag_min[0]) * mag_bias_factory[0] / 2; // get average x axis max chord length in counts + scale[1] = (float)(mag_max[1] - mag_min[1]) * mag_bias_factory[1] / 2; // get average y axis max chord length in counts + scale[2] = (float)(mag_max[2] - mag_min[2]) * mag_bias_factory[2] / 2; // get average z axis max chord length in counts + + float avg_rad = scale[0] + scale[1] + scale[2]; + avg_rad /= 3.0; + + m_scale[0] = avg_rad / ((float)scale[0]); + m_scale[1] = avg_rad / ((float)scale[1]); + m_scale[2] = avg_rad / ((float)scale[2]); + } + + // Accelerometer and gyroscope self test; check calibration wrt factory settings + bool self_test_impl() // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass + { + uint8_t raw_data[6] = {0, 0, 0, 0, 0, 0}; + int32_t gAvg[3] = {0}, aAvg[3] = {0}, aSTAvg[3] = {0}, gSTAvg[3] = {0}; + float factoryTrim[6]; + uint8_t FS = 0; + + write_byte(mpu_i2c_addr, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz + write_byte(mpu_i2c_addr, MPU_CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz + write_byte(mpu_i2c_addr, GYRO_CONFIG, FS << 3); // Set full scale range for the gyro to 250 dps + write_byte(mpu_i2c_addr, ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz + write_byte(mpu_i2c_addr, ACCEL_CONFIG, FS << 3); // Set full scale range for the accelerometer to 2 g + + for (int ii = 0; ii < 200; ii++) { // get average current values of gyro and acclerometer + + read_bytes(mpu_i2c_addr, ACCEL_XOUT_H, 6, &raw_data[0]); // Read the six raw data registers into data array + aAvg[0] += (int16_t)(((int16_t)raw_data[0] << 8) | raw_data[1]); // Turn the MSB and LSB into a signed 16-bit value + aAvg[1] += (int16_t)(((int16_t)raw_data[2] << 8) | raw_data[3]); + aAvg[2] += (int16_t)(((int16_t)raw_data[4] << 8) | raw_data[5]); + + read_bytes(mpu_i2c_addr, GYRO_XOUT_H, 6, &raw_data[0]); // Read the six raw data registers sequentially into data array + gAvg[0] += (int16_t)(((int16_t)raw_data[0] << 8) | raw_data[1]); // Turn the MSB and LSB into a signed 16-bit value + gAvg[1] += (int16_t)(((int16_t)raw_data[2] << 8) | raw_data[3]); + gAvg[2] += (int16_t)(((int16_t)raw_data[4] << 8) | raw_data[5]); + } + + for (int ii = 0; ii < 3; ii++) { // Get average of 200 values and store as average current readings + aAvg[ii] /= 200; + gAvg[ii] /= 200; + } + + // Configure the accelerometer for self-test + write_byte(mpu_i2c_addr, ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g + write_byte(mpu_i2c_addr, GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s + sleep_ms(25); // Delay a while to let the device stabilize + + for (int ii = 0; ii < 200; ii++) { // get average self-test values of gyro and acclerometer + + read_bytes(mpu_i2c_addr, ACCEL_XOUT_H, 6, &raw_data[0]); // Read the six raw data registers into data array + aSTAvg[0] += (int16_t)(((int16_t)raw_data[0] << 8) | raw_data[1]); // Turn the MSB and LSB into a signed 16-bit value + aSTAvg[1] += (int16_t)(((int16_t)raw_data[2] << 8) | raw_data[3]); + aSTAvg[2] += (int16_t)(((int16_t)raw_data[4] << 8) | raw_data[5]); + + read_bytes(mpu_i2c_addr, GYRO_XOUT_H, 6, &raw_data[0]); // Read the six raw data registers sequentially into data array + gSTAvg[0] += (int16_t)(((int16_t)raw_data[0] << 8) | raw_data[1]); // Turn the MSB and LSB into a signed 16-bit value + gSTAvg[1] += (int16_t)(((int16_t)raw_data[2] << 8) | raw_data[3]); + gSTAvg[2] += (int16_t)(((int16_t)raw_data[4] << 8) | raw_data[5]); + } + + for (int ii = 0; ii < 3; ii++) { // Get average of 200 values and store as average self-test readings + aSTAvg[ii] /= 200; + gSTAvg[ii] /= 200; + } + + // Configure the gyro and accelerometer for normal operation + write_byte(mpu_i2c_addr, ACCEL_CONFIG, 0x00); + write_byte(mpu_i2c_addr, GYRO_CONFIG, 0x00); + sleep_ms(25); // Delay a while to let the device stabilize + + // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg + uint8_t self_test_data[6]; + self_test_data[0] = read_byte(mpu_i2c_addr, SELF_TEST_X_ACCEL); // X-axis accel self-test results + self_test_data[1] = read_byte(mpu_i2c_addr, SELF_TEST_Y_ACCEL); // Y-axis accel self-test results + self_test_data[2] = read_byte(mpu_i2c_addr, SELF_TEST_Z_ACCEL); // Z-axis accel self-test results + self_test_data[3] = read_byte(mpu_i2c_addr, SELF_TEST_X_GYRO); // X-axis gyro self-test results + self_test_data[4] = read_byte(mpu_i2c_addr, SELF_TEST_Y_GYRO); // Y-axis gyro self-test results + self_test_data[5] = read_byte(mpu_i2c_addr, SELF_TEST_Z_GYRO); // Z-axis gyro self-test results + + // Retrieve factory self-test value from self-test code reads + factoryTrim[0] = (float)(2620 / 1 << FS) * (pow(1.01, ((float)self_test_data[0] - 1.0))); // FT[Xa] factory trim calculation + factoryTrim[1] = (float)(2620 / 1 << FS) * (pow(1.01, ((float)self_test_data[1] - 1.0))); // FT[Ya] factory trim calculation + factoryTrim[2] = (float)(2620 / 1 << FS) * (pow(1.01, ((float)self_test_data[2] - 1.0))); // FT[Za] factory trim calculation + factoryTrim[3] = (float)(2620 / 1 << FS) * (pow(1.01, ((float)self_test_data[3] - 1.0))); // FT[Xg] factory trim calculation + factoryTrim[4] = (float)(2620 / 1 << FS) * (pow(1.01, ((float)self_test_data[4] - 1.0))); // FT[Yg] factory trim calculation + factoryTrim[5] = (float)(2620 / 1 << FS) * (pow(1.01, ((float)self_test_data[5] - 1.0))); // FT[Zg] factory trim calculation + + // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response + // To get percent, must multiply by 100 + for (int i = 0; i < 3; i++) { + self_test_result[i] = 100.0 * ((float)(aSTAvg[i] - aAvg[i])) / factoryTrim[i] - 100.; // Report percent differences + self_test_result[i + 3] = 100.0 * ((float)(gSTAvg[i] - gAvg[i])) / factoryTrim[i + 3] - 100.; // Report percent differences + } + + if (b_verbose) { + // Serialprint("x-axis self test: acceleration trim within : "); + // Serialprint(self_test_result[0], 1); + // Serialprintln("% of factory value"); + // Serialprint("y-axis self test: acceleration trim within : "); + // Serialprint(self_test_result[1], 1); + // Serialprintln("% of factory value"); + // Serialprint("z-axis self test: acceleration trim within : "); + // Serialprint(self_test_result[2], 1); + // Serialprintln("% of factory value"); + // Serialprint("x-axis self test: gyration trim within : "); + // Serialprint(self_test_result[3], 1); + // Serialprintln("% of factory value"); + // Serialprint("y-axis self test: gyration trim within : "); + // Serialprint(self_test_result[4], 1); + // Serialprintln("% of factory value"); + // Serialprint("z-axis self test: gyration trim within : "); + // Serialprint(self_test_result[5], 1); + // Serialprintln("% of factory value"); + } + + bool b = true; + for (uint8_t i = 0; i < 6; ++i) { + b &= fabs(self_test_result[i]) <= 14.f; + } + return b; + } + + float get_acc_resolution(const ACCEL_FS_SEL accel_af_sel) const { + switch (accel_af_sel) { + // Possible accelerometer scales (and their register bit settings) are: + // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). + // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: + case ACCEL_FS_SEL::A2G: + return 2.0 / 32768.0; + case ACCEL_FS_SEL::A4G: + return 4.0 / 32768.0; + case ACCEL_FS_SEL::A8G: + return 8.0 / 32768.0; + case ACCEL_FS_SEL::A16G: + return 16.0 / 32768.0; + default: + return 0.; + } + } + + float get_gyro_resolution(const GYRO_FS_SEL gyro_fs_sel) const { + switch (gyro_fs_sel) { + // Possible gyro scales (and their register bit settings) are: + // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11). + // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: + case GYRO_FS_SEL::G250DPS: + return 250.0 / 32768.0; + case GYRO_FS_SEL::G500DPS: + return 500.0 / 32768.0; + case GYRO_FS_SEL::G1000DPS: + return 1000.0 / 32768.0; + case GYRO_FS_SEL::G2000DPS: + return 2000.0 / 32768.0; + default: + return 0.; + } + } + + float get_mag_resolution(const MAG_OUTPUT_BITS mag_output_bits) const { + switch (mag_output_bits) { + // Possible magnetometer scales (and their register bit settings) are: + // 14 bit resolution (0) and 16 bit resolution (1) + // Proper scale to return milliGauss + case MAG_OUTPUT_BITS::M14BITS: + return 10. * 4912. / 8190.0; + case MAG_OUTPUT_BITS::M16BITS: + return 10. * 4912. / 32760.0; + default: + return 0.; + } + } + + void write_byte(uint8_t address, uint8_t subAddress, uint8_t data) { + + i2c_err_ = write_i2c(this->bus, address, {subAddress, data}); + + // wire->beginTransmission(address); // Initialize the Tx buffer + // wire->write(subAddress); // Put slave register address in Tx buffer + // wire->write(data); // Put data in Tx buffer + // i2c_err_ = wire->endTransmission(); // Send the Tx buffer + if (i2c_err_) print_i2c_error(); + } + + uint8_t read_byte(uint8_t address, uint8_t subAddress) { + // uint8_t data = 0; // `data` will store the register data + std::vector data; + data.resize(1,0); + read_i2c(this->bus, address, {subAddress}, 1, data); + return data[0]; + + // wire->beginTransmission(address); // Initialize the Tx buffer + // wire->write(subAddress); // Put slave register address in Tx buffer + // i2c_err_ = wire->endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive + // if (i2c_err_) print_i2c_error(); + // wire->requestFrom(address, (size_t)1); // Read one byte from slave register address + // if (wire->available()) data = wire->read(); // Fill Rx buffer with result + // return data; // Return data read from slave register + } + + void read_bytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t* dest) { + + std::vector data; + data.resize(count, 0); + read_i2c(this->bus, address, {subAddress}, count, data); + for(int i = 0; i < count && i < data.size(); i++) { + dest[i] = data[i]; + } + + + // wire->beginTransmission(address); // Initialize the Tx buffer + // wire->write(subAddress); // Put slave register address in Tx buffer + // i2c_err_ = wire->endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive + // if (i2c_err_) print_i2c_error(); + // uint8_t i = 0; + // wire->requestFrom(address, count); // Read bytes from slave register address + // while (wire->available()) { + // dest[i++] = wire->read(); + // } // Put read results in the Rx buffer + } + + void print_i2c_error() { + if (i2c_err_ == 7) return; // to avoid stickbreaker-i2c branch's error code + // Serialprint("I2C ERROR CODE : "); + // Serialprintln(i2c_err_); + } +}; + +using MPU9250 = MPU9250_; + +#endif // MPU9250_H \ No newline at end of file diff --git a/include/MPU9250/MPU9250RegisterMap.h b/include/MPU9250/MPU9250RegisterMap.h new file mode 100644 index 0000000..47040ae --- /dev/null +++ b/include/MPU9250/MPU9250RegisterMap.h @@ -0,0 +1,151 @@ +#pragma once +#ifndef MPU9250REGISTERMAP_H +#define MPU9250REGISTERMAP_H + +//Magnetometer Registers +// #define AK8963_ADDRESS 0x0C +#define AK8963_WHO_AM_I 0x00 // should return 0x48 +#define AK8963_INFO 0x01 +#define AK8963_ST1 0x02 // data ready status bit 0 +#define AK8963_XOUT_L 0x03 // data +#define AK8963_XOUT_H 0x04 +#define AK8963_YOUT_L 0x05 +#define AK8963_YOUT_H 0x06 +#define AK8963_ZOUT_L 0x07 +#define AK8963_ZOUT_H 0x08 +#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2 +#define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0 +#define AK8963_ASTC 0x0C // Self test control +#define AK8963_I2CDIS 0x0F // I2C disable +#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value +#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value +#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value + +#define SELF_TEST_X_GYRO 0x00 +#define SELF_TEST_Y_GYRO 0x01 +#define SELF_TEST_Z_GYRO 0x02 + +// #define X_FINE_GAIN 0x03 // [7:0] fine gain +// #define Y_FINE_GAIN 0x04 +// #define Z_FINE_GAIN 0x05 +// #define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer +// #define XA_OFFSET_L_TC 0x07 +// #define YA_OFFSET_H 0x08 +// #define YA_OFFSET_L_TC 0x09 +// #define ZA_OFFSET_H 0x0A +// #define ZA_OFFSET_L_TC 0x0B + +#define SELF_TEST_X_ACCEL 0x0D +#define SELF_TEST_Y_ACCEL 0x0E +#define SELF_TEST_Z_ACCEL 0x0F + +#define SELF_TEST_A 0x10 + +#define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope +#define XG_OFFSET_L 0x14 +#define YG_OFFSET_H 0x15 +#define YG_OFFSET_L 0x16 +#define ZG_OFFSET_H 0x17 +#define ZG_OFFSET_L 0x18 +#define SMPLRT_DIV 0x19 +#define MPU_CONFIG 0x1A +#define GYRO_CONFIG 0x1B +#define ACCEL_CONFIG 0x1C +#define ACCEL_CONFIG2 0x1D +#define LP_ACCEL_ODR 0x1E +#define WOM_THR 0x1F + +#define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms +#define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0] +#define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms + +#define FIFO_EN 0x23 +#define I2C_MST_CTRL 0x24 +#define I2C_SLV0_ADDR 0x25 +#define I2C_SLV0_REG 0x26 +#define I2C_SLV0_CTRL 0x27 +#define I2C_SLV1_ADDR 0x28 +#define I2C_SLV1_REG 0x29 +#define I2C_SLV1_CTRL 0x2A +#define I2C_SLV2_ADDR 0x2B +#define I2C_SLV2_REG 0x2C +#define I2C_SLV2_CTRL 0x2D +#define I2C_SLV3_ADDR 0x2E +#define I2C_SLV3_REG 0x2F +#define I2C_SLV3_CTRL 0x30 +#define I2C_SLV4_ADDR 0x31 +#define I2C_SLV4_REG 0x32 +#define I2C_SLV4_DO 0x33 +#define I2C_SLV4_CTRL 0x34 +#define I2C_SLV4_DI 0x35 +#define I2C_MST_STATUS 0x36 +#define INT_PIN_CFG 0x37 +#define INT_ENABLE 0x38 +#define DMP_INT_STATUS 0x39 // Check DMP interrupt +#define INT_STATUS 0x3A +#define ACCEL_XOUT_H 0x3B +#define ACCEL_XOUT_L 0x3C +#define ACCEL_YOUT_H 0x3D +#define ACCEL_YOUT_L 0x3E +#define ACCEL_ZOUT_H 0x3F +#define ACCEL_ZOUT_L 0x40 +#define TEMP_OUT_H 0x41 +#define TEMP_OUT_L 0x42 +#define GYRO_XOUT_H 0x43 +#define GYRO_XOUT_L 0x44 +#define GYRO_YOUT_H 0x45 +#define GYRO_YOUT_L 0x46 +#define GYRO_ZOUT_H 0x47 +#define GYRO_ZOUT_L 0x48 +#define EXT_SENS_DATA_00 0x49 +#define EXT_SENS_DATA_01 0x4A +#define EXT_SENS_DATA_02 0x4B +#define EXT_SENS_DATA_03 0x4C +#define EXT_SENS_DATA_04 0x4D +#define EXT_SENS_DATA_05 0x4E +#define EXT_SENS_DATA_06 0x4F +#define EXT_SENS_DATA_07 0x50 +#define EXT_SENS_DATA_08 0x51 +#define EXT_SENS_DATA_09 0x52 +#define EXT_SENS_DATA_10 0x53 +#define EXT_SENS_DATA_11 0x54 +#define EXT_SENS_DATA_12 0x55 +#define EXT_SENS_DATA_13 0x56 +#define EXT_SENS_DATA_14 0x57 +#define EXT_SENS_DATA_15 0x58 +#define EXT_SENS_DATA_16 0x59 +#define EXT_SENS_DATA_17 0x5A +#define EXT_SENS_DATA_18 0x5B +#define EXT_SENS_DATA_19 0x5C +#define EXT_SENS_DATA_20 0x5D +#define EXT_SENS_DATA_21 0x5E +#define EXT_SENS_DATA_22 0x5F +#define EXT_SENS_DATA_23 0x60 +#define MOT_DETECT_STATUS 0x61 +#define I2C_SLV0_DO 0x63 +#define I2C_SLV1_DO 0x64 +#define I2C_SLV2_DO 0x65 +#define I2C_SLV3_DO 0x66 +#define I2C_MST_DELAY_CTRL 0x67 +#define SIGNAL_PATH_RESET 0x68 +#define MOT_DETECT_CTRL 0x69 +#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP +#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode +#define PWR_MGMT_2 0x6C +#define DMP_BANK 0x6D // Activates a specific bank in the DMP +#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank +#define DMP_REG 0x6F // Register in DMP from which to read or to which to write +#define DMP_REG_1 0x70 +#define DMP_REG_2 0x71 +#define FIFO_COUNTH 0x72 +#define FIFO_COUNTL 0x73 +#define FIFO_R_W 0x74 +#define WHO_AM_I_MPU9250 0x75 // Should return 0x71 +#define XA_OFFSET_H 0x77 +#define XA_OFFSET_L 0x78 +#define YA_OFFSET_H 0x7A +#define YA_OFFSET_L 0x7B +#define ZA_OFFSET_H 0x7D +#define ZA_OFFSET_L 0x7E + +#endif // MPU9250REGISTERMAP_H \ No newline at end of file diff --git a/include/MPU9250/QuaternionFilter.h b/include/MPU9250/QuaternionFilter.h new file mode 100644 index 0000000..c72e38e --- /dev/null +++ b/include/MPU9250/QuaternionFilter.h @@ -0,0 +1,242 @@ +#pragma once +#ifndef QUATERNIONFILTER_H +#define QUATERNIONFILTER_H +#include +#include +#include +#include +const float PI = 3.1415926; + + +enum class QuatFilterSel { + NONE, + MADGWICK, + MAHONY, +}; +class QuaternionFilter { + // for madgwick + float GyroMeasError = PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (start at 40 deg/s) + float GyroMeasDrift = PI * (0.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) + float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta + float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value + + // for mahony + float Kp = 30.0; + float Ki = 0.0; + + QuatFilterSel filter_sel{QuatFilterSel::MADGWICK}; + double deltaT{0.}; + uint32_t newTime{0}, oldTime{0}; + +public: + void select_filter(QuatFilterSel sel) { + filter_sel = sel; + } + + void update(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float* q) { + newTime = time_us_32(); + deltaT = newTime - oldTime; + oldTime = newTime; + deltaT = fabs(deltaT * 0.001 * 0.001); + + switch (filter_sel) { + case QuatFilterSel::MADGWICK: + madgwick(ax, ay, az, gx, gy, gz, mx, my, mz, q); + break; + case QuatFilterSel::MAHONY: + mahony(ax, ay, az, gx, gy, gz, mx, my, mz, q); + break; + default: + no_filter(ax, ay, az, gx, gy, gz, mx, my, mz, q); + break; + } + } + + void no_filter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float* q) { + float q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3]; // variable for readability + q[0] += 0.5f * (-q1 * gx - q2 * gy - q3 * gz) * deltaT; + q[1] += 0.5f * (q0 * gx + q2 * gz - q3 * gy) * deltaT; + q[2] += 0.5f * (q0 * gy - q1 * gz + q3 * gx) * deltaT; + q[3] += 0.5f * (q0 * gz + q1 * gy - q2 * gx) * deltaT; + float recipNorm = 1.0 / sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + q[0] *= recipNorm; + q[1] *= recipNorm; + q[2] *= recipNorm; + q[3] *= recipNorm; + } + + // Madgwick Quaternion Update + void madgwick(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float* q) { + double q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3]; // short name local variable for readability + double recipNorm; + double s0, s1, s2, s3; + double qDot1, qDot2, qDot3, qDot4; + double hx, hy; + double _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; + + // Rate of change of quaternion from gyroscope + qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); + qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); + qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); + qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); + + // Normalise accelerometer measurement + double a_norm = ax * ax + ay * ay + az * az; + if (a_norm == 0.) return; // handle NaN + recipNorm = 1.0 / sqrt(a_norm); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Normalise magnetometer measurement + double m_norm = mx * mx + my * my + mz * mz; + if (m_norm == 0.) return; // handle NaN + recipNorm = 1.0 / sqrt(m_norm); + mx *= recipNorm; + my *= recipNorm; + mz *= recipNorm; + + // Auxiliary variables to avoid repeated arithmetic + _2q0mx = 2.0f * q0 * mx; + _2q0my = 2.0f * q0 * my; + _2q0mz = 2.0f * q0 * mz; + _2q1mx = 2.0f * q1 * mx; + _2q0 = 2.0f * q0; + _2q1 = 2.0f * q1; + _2q2 = 2.0f * q2; + _2q3 = 2.0f * q3; + _2q0q2 = 2.0f * q0 * q2; + _2q2q3 = 2.0f * q2 * q3; + q0q0 = q0 * q0; + q0q1 = q0 * q1; + q0q2 = q0 * q2; + q0q3 = q0 * q3; + q1q1 = q1 * q1; + q1q2 = q1 * q2; + q1q3 = q1 * q3; + q2q2 = q2 * q2; + q2q3 = q2 * q3; + q3q3 = q3 * q3; + + // Reference direction of Earth's magnetic field + hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3; + hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3; + _2bx = sqrt(hx * hx + hy * hy); + _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3; + _4bx = 2.0f * _2bx; + _4bz = 2.0f * _2bz; + + // Gradient decent algorithm corrective step + s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + recipNorm = 1.0 / sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + // Apply feedback step + qDot1 -= beta * s0; + qDot2 -= beta * s1; + qDot3 -= beta * s2; + qDot4 -= beta * s3; + + // Integrate rate of change of quaternion to yield quaternion + q0 += qDot1 * deltaT; + q1 += qDot2 * deltaT; + q2 += qDot3 * deltaT; + q3 += qDot4 * deltaT; + + // Normalise quaternion + recipNorm = 1.0 / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + + q[0] = q0; + q[1] = q1; + q[2] = q2; + q[3] = q3; + } + + // Mahony accelleration filter + // Mahony scheme uses proportional and integral filtering on + // the error between estimated reference vector (gravity) and measured one. + // Madgwick's implementation of Mayhony's AHRS algorithm. + // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms + // Free parameters in the Mahony filter and fusion scheme, + // Kp for proportional feedback, Ki for integral + // float Kp = 30.0; + // float Ki = 0.0; + // with MPU-9250, angles start oscillating at Kp=40. Ki does not seem to help and is not required. + // with MPU-6050, some instability observed at Kp=100 Now set to 30. + void mahony(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float* q) { + float recipNorm; + float vx, vy, vz; + float ex, ey, ez; //error terms + float qa, qb, qc; + static float ix = 0.0, iy = 0.0, iz = 0.0; //integral feedback terms + float tmp; + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + tmp = ax * ax + ay * ay + az * az; + if (tmp > 0.0) { + // Normalise accelerometer (assumed to measure the direction of gravity in body frame) + recipNorm = 1.0 / sqrt(tmp); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Estimated direction of gravity in the body frame (factor of two divided out) + vx = q[1] * q[3] - q[0] * q[2]; + vy = q[0] * q[1] + q[2] * q[3]; + vz = q[0] * q[0] - 0.5f + q[3] * q[3]; + + // Error is cross product between estimated and measured direction of gravity in body frame + // (half the actual magnitude) + ex = (ay * vz - az * vy); + ey = (az * vx - ax * vz); + ez = (ax * vy - ay * vx); + + // Compute and apply to gyro term the integral feedback, if enabled + if (Ki > 0.0f) { + ix += Ki * ex * deltaT; // integral error scaled by Ki + iy += Ki * ey * deltaT; + iz += Ki * ez * deltaT; + gx += ix; // apply integral feedback + gy += iy; + gz += iz; + } + + // Apply proportional feedback to gyro term + gx += Kp * ex; + gy += Kp * ey; + gz += Kp * ez; + } + + // Integrate rate of change of quaternion, q cross gyro term + deltaT = 0.5 * deltaT; + gx *= deltaT; // pre-multiply common factors + gy *= deltaT; + gz *= deltaT; + qa = q[0]; + qb = q[1]; + qc = q[2]; + q[0] += (-qb * gx - qc * gy - q[3] * gz); + q[1] += (qa * gx + qc * gz - q[3] * gy); + q[2] += (qa * gy - qb * gz + q[3] * gx); + q[3] += (qa * gz + qb * gy - qc * gx); + + // renormalise quaternion + recipNorm = 1.0 / sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + q[0] = q[0] * recipNorm; + q[1] = q[1] * recipNorm; + q[2] = q[2] * recipNorm; + q[3] = q[3] * recipNorm; + } +}; + +#endif // QUATERNIONFILTER_H \ No newline at end of file diff --git a/include/Telemetrix4RpiPico.hpp b/include/Telemetrix4RpiPico.hpp index 39bb7cc..1c361ee 100644 --- a/include/Telemetrix4RpiPico.hpp +++ b/include/Telemetrix4RpiPico.hpp @@ -23,6 +23,7 @@ #include #include #include "VL53L0.hpp" +#include "MPU9250.hpp" /************************** FORWARD REFERENCES *********************** We define all functions here as extern to provide allow forward referencing. @@ -115,7 +116,7 @@ void encoder_new(); #define SET_PIN_MODE 1 #define DIGITAL_WRITE 2 #define PWM_WRITE 3 -#define MODIFY_REPORTING \ +#define MODIFY_REPORTING \ 4 // mode(all, analog, or digital), pin, enable or disable #define GET_FIRMWARE_VERSION 5 #define GET_PICO_UNIQUE_ID 6 @@ -329,7 +330,11 @@ const uint DHT_MAX_TIMINGS = 85; // Max encoder devices #define MAX_ENCODERS 4 -typedef enum { SINGLE = 1, QUADRATURE = 2 } ENCODER_TYPES; +typedef enum +{ + SINGLE = 1, + QUADRATURE = 2 +} ENCODER_TYPES; // encoder reports are identified by pin A #define ENCODER_REPORT_PIN_A 2 @@ -421,7 +426,8 @@ const int SENSOR_REPORT = 20; #define I2C_NO_REGISTER_SPECIFIED 254 // a descriptor for digital pins -typedef struct { +typedef struct +{ uint pin_number; uint pin_mode; uint reporting_enabled; // If true, then send reports if an input pin @@ -429,14 +435,16 @@ typedef struct { } pin_descriptor; // a descriptor for analog pins -typedef struct analog_pin_descriptor { +typedef struct analog_pin_descriptor +{ uint reporting_enabled; // If true, then send reports if an input pin int last_value; // Last value read for input mode int differential; // difference between current and last value needed } analog_pin_descriptor; // This structure describes an HC-SR04 type device -typedef struct hc_sr04_descriptor { +typedef struct hc_sr04_descriptor +{ uint trig_pin; // trigger pin uint echo_pin; // echo pin uint32_t start_time; @@ -445,7 +453,8 @@ typedef struct hc_sr04_descriptor { // this structure holds an index into the sonars array // and the sonars array -typedef struct sonar_data { +typedef struct sonar_data +{ int next_sonar_index; repeating_timer_t trigger_timer; uint32_t trigger_mask; @@ -453,7 +462,8 @@ typedef struct sonar_data { } sonar_data; // this structure describes a DHT type device -typedef struct dht_descriptor { +typedef struct dht_descriptor +{ uint data_pin; // data pin absolute_time_t previous_time; /* for possible future use @@ -464,13 +474,15 @@ typedef struct dht_descriptor { // this structure holds an index into the dht array // and the dhts array -typedef struct dht_data { +typedef struct dht_data +{ int next_dht_index; dht_descriptor dhts[MAX_DHTS]; } dht_data; // encoder type -typedef struct { +typedef struct +{ ENCODER_TYPES type; int A; int B; @@ -478,13 +490,15 @@ typedef struct { int last_state; } encoder_t; -typedef struct { +typedef struct +{ int next_encoder_index; repeating_timer_t trigger_timer; encoder_t encoders[MAX_ENCODERS]; } encoder_data; encoder_data encoders; -typedef struct { +typedef struct +{ // a pointer to the command processing function void (*command_func)(void); } command_descriptor; @@ -493,8 +507,9 @@ typedef struct { /****SENSORS*/ /*****************************************************************************/ -enum SENSOR_TYPES : uint8_t { // Max 255 sensors, but will always fit in a - // single byte! +enum SENSOR_TYPES : uint8_t +{ // Max 255 sensors, but will always fit in a + // single byte! GPS = 0, LOAD_CELL = 1, MPU_9250 = 2, @@ -503,7 +518,8 @@ enum SENSOR_TYPES : uint8_t { // Max 255 sensors, but will always fit in a ADXL345 = 5, // 3 axis accel MAX_SENSORS }; -class Sensor { +class Sensor +{ public: virtual void readSensor(); bool stop = false; @@ -512,14 +528,16 @@ class Sensor { SENSOR_TYPES type = SENSOR_TYPES::MAX_SENSORS; }; const int SENSORS_MAX_SETTINGS_A = 6; -class GPS_Sensor : public Sensor { +class GPS_Sensor : public Sensor +{ public: GPS_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]); void readSensor(); private: }; -class ADXL345_Sensor : public Sensor { +class ADXL345_Sensor : public Sensor +{ public: ADXL345_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]); void readSensor(); @@ -530,7 +548,8 @@ class ADXL345_Sensor : public Sensor { int i2c_addr = 83; }; -class VEML6040_Sensor : public Sensor { +class VEML6040_Sensor : public Sensor +{ public: VEML6040_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]); void readSensor(); @@ -541,18 +560,29 @@ class VEML6040_Sensor : public Sensor { int i2c_addr = 0x10; }; -class VL53L0X_Sensor : public Sensor { +class VL53L0X_Sensor : public Sensor +{ public: VL53L0X_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]); void readSensor(); private: -VL53L0X sensor; - void init_sequence(); + VL53L0X sensor; int i2c_port = 0; int i2c_addr = 0x52; }; +class MPU9250_Sensor : public Sensor +{ +public: + MPU9250_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]); + void readSensor(); + +private: + MPU9250 sensor; + void init_sequence(); + int i2c_port = 0; +}; void sensor_new(); void addSensor(uint8_t data[], size_t len); diff --git a/src/Telemetrix4RpiPico.cpp b/src/Telemetrix4RpiPico.cpp index 662fb02..7e7358d 100644 --- a/src/Telemetrix4RpiPico.cpp +++ b/src/Telemetrix4RpiPico.cpp @@ -1233,6 +1233,8 @@ void sensor_new() else if (type == SENSOR_TYPES::TOF_VL53) { sensor = new VL53L0X_Sensor(sensor_data); + } else if (type == SENSOR_TYPES::MPU_9250) { + sensor = new MPU9250_Sensor(sensor_data); } sensor->type = type; sensor->num = sensor_num; @@ -1346,6 +1348,30 @@ void VL53L0X_Sensor::readSensor() this->writeSensorData(data); } +MPU9250_Sensor::MPU9250_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]) { + this->sensor.bus = settings[0]; + this->sensor.setup(0x68); +} +void MPU9250_Sensor::readSensor() { + if (this->sensor.update()) { + std::vector float_data; + for(int i = 0; i < 3; i++) { + float_data.push_back(this->sensor.getAcc(i)); + } + for(int i = 0; i < 3; i++) { + float_data.push_back(this->sensor.getGyro(i)); + } + for(int i = 0; i < 3; i++) { + float_data.push_back(this->sensor.getMag(i)); + } + const unsigned char* bytes = reinterpret_cast(&float_data[0]); + static_assert(sizeof(float) == 4); + std::vector data(bytes, bytes+sizeof(float)*float_data.size()); + this->writeSensorData(data); + } +} + + void Sensor::writeSensorData(std::vector data) { std::vector out = { From 3a1cf8db7b24db7bbdae575180209f700cded487 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Wed, 13 Sep 2023 11:46:45 +0200 Subject: [PATCH 14/33] add hx711 --- CMakeLists.txt | 1 + include/HX711.hpp | 85 +++++++++++ include/Telemetrix4RpiPico.hpp | 10 +- src/HX711.cpp | 268 +++++++++++++++++++++++++++++++++ src/Telemetrix4RpiPico.cpp | 45 ++++-- 5 files changed, 399 insertions(+), 10 deletions(-) create mode 100644 include/HX711.hpp create mode 100644 src/HX711.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index fda85d7..89ed5dd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,6 +9,7 @@ add_executable(Telemetrix4RpiPico src/Telemetrix4RpiPico.cpp src/VL53L0.cpp src/i2c_helpers.cpp + src/HX711.cpp ) target_include_directories(Telemetrix4RpiPico PUBLIC ${CMAKE_CURRENT_LIST_DIR}/include/) # target_compile_options(Telemetrix4RpiPico PRIVATE -Wall -Wextra -Wpedantic -Werror) diff --git a/include/HX711.hpp b/include/HX711.hpp new file mode 100644 index 0000000..ead4788 --- /dev/null +++ b/include/HX711.hpp @@ -0,0 +1,85 @@ +/** + * + * HX711 library for Arduino + * https://github.com/bogde/HX711 + * + * MIT License + * (c) 2018 Bogdan Necula + * +**/ +#pragma once +#include +#include "pico/stdlib.h" + +typedef uint8_t byte ; +class HX711 +{ + private: + byte PD_SCK; // Power Down and Serial Clock Input Pin + byte DOUT; // Serial Data Output Pin + byte GAIN; // amplification factor + long OFFSET = 0; // used for tare weight + float SCALE = 1; // used to return weight in grams, kg, ounces, whatever + + public: + + HX711(); + + virtual ~HX711(); + + // Initialize library with data output pin, clock input pin and gain factor. + // Channel selection is made by passing the appropriate gain: + // - With a gain factor of 64 or 128, channel A is selected + // - With a gain factor of 32, channel B is selected + // The library default is "128" (Channel A). + void begin(byte dout, byte pd_sck, byte gain = 128); + + // Check if HX711 is ready + // from the datasheet: When output data is not ready for retrieval, digital output pin DOUT is high. Serial clock + // input PD_SCK should be low. When DOUT goes to low, it indicates data is ready for retrieval. + bool is_ready(); + + // Wait for the HX711 to become ready + void wait_ready(unsigned long delay_ms = 0); + bool wait_ready_retry(int retries = 3, unsigned long delay_ms = 0); + bool wait_ready_timeout(unsigned long timeout = 1000, unsigned long delay_ms = 0); + + // set the gain factor; takes effect only after a call to read() + // channel A can be set for a 128 or 64 gain; channel B has a fixed 32 gain + // depending on the parameter, the channel is also set to either A or B + void set_gain(byte gain = 128); + + // waits for the chip to be ready and returns a reading + long read(); + + // returns an average reading; times = how many times to read + long read_average(byte times = 10); + + // returns (read_average() - OFFSET), that is the current value without the tare weight; times = how many readings to do + double get_value(byte times = 1); + + // returns get_value() divided by SCALE, that is the raw value divided by a value obtained via calibration + // times = how many readings to do + float get_units(byte times = 1); + + // set the OFFSET value for tare weight; times = how many times to read the tare value + void tare(byte times = 10); + + // set the SCALE value; this value is used to convert the raw data to "human readable" data (measure units) + void set_scale(float scale = 1.f); + + // get the current SCALE + float get_scale(); + + // set OFFSET, the value that's subtracted from the actual reading (tare weight) + void set_offset(long offset = 0); + + // get the current OFFSET + long get_offset(); + + // puts the chip into power down mode + void power_down(); + + // wakes up the chip after power down mode + void power_up(); +}; diff --git a/include/Telemetrix4RpiPico.hpp b/include/Telemetrix4RpiPico.hpp index 1c361ee..5ebcc0f 100644 --- a/include/Telemetrix4RpiPico.hpp +++ b/include/Telemetrix4RpiPico.hpp @@ -24,6 +24,7 @@ #include #include "VL53L0.hpp" #include "MPU9250.hpp" +#include "HX711.hpp" /************************** FORWARD REFERENCES *********************** We define all functions here as extern to provide allow forward referencing. @@ -580,10 +581,17 @@ class MPU9250_Sensor : public Sensor private: MPU9250 sensor; - void init_sequence(); int i2c_port = 0; }; +class HX711_Sensor : public Sensor { + public: + HX711_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]); + void readSensor(); + private: + HX711 sensor; +}; + void sensor_new(); void addSensor(uint8_t data[], size_t len); void readSensors(); diff --git a/src/HX711.cpp b/src/HX711.cpp new file mode 100644 index 0000000..e378237 --- /dev/null +++ b/src/HX711.cpp @@ -0,0 +1,268 @@ +/** + * + * HX711 library for Arduino, modified for the rp2040 + * https://github.com/bogde/HX711 + * + * MIT License + * (c) 2018 Bogdan Necula + * + **/ +#include "HX711.hpp" + +#include +#include +#include +#include + +// Define macro designating whether we're running on a reasonable +// fast CPU and so should slow down sampling from GPIO. +#define FAST_CPU 1 + +const int LSBFIRST = 0; +const int MSBFIRST = 1; +#if FAST_CPU +// Make shiftIn() be aware of clockspeed for +// faster CPUs like ESP32, Teensy 3.x and friends. +// See also: +// - https://github.com/bogde/HX711/issues/75 +// - https://github.com/arduino/Arduino/issues/6561 +// - https://community.hiveeyes.org/t/using-bogdans-canonical-hx711-library-on-the-esp32/539 +uint8_t shiftInSlow(uint8_t dataPin, uint8_t clockPin, uint8_t bitOrder) +{ + uint8_t value = 0; + uint8_t i; + + for (i = 0; i < 8; ++i) + { + gpio_put(clockPin, 1); + sleep_us(1); + if (bitOrder == LSBFIRST) + value |= gpio_get(dataPin) << i; + else + value |= gpio_get(dataPin) << (7 - i); + gpio_put(clockPin, 0); + sleep_us(1); + } + return value; +} +#define SHIFTIN_WITH_SPEED_SUPPORT(data, clock, order) shiftInSlow(data, clock, order) +#else +#define SHIFTIN_WITH_SPEED_SUPPORT(data, clock, order) shiftIn(data, clock, order) +#endif + +#if ARCH_ESPRESSIF +// ESP8266 doesn't read values between 0x20000 and 0x30000 when DOUT is pulled up. +#define DOUT_MODE INPUT +#else +#define DOUT_MODE INPUT_PULLUP +#endif + +HX711::HX711() +{ +} + +HX711::~HX711() +{ +} + +void HX711::begin(byte dout, byte pd_sck, byte gain) +{ + PD_SCK = pd_sck; + DOUT = dout; + gpio_set_dir(PD_SCK, GPIO_OUT); + gpio_set_dir(DOUT, GPIO_IN); + + // pinMode(PD_SCK, OUTPUT); + // pinMode(DOUT, DOUT_MODE); + + set_gain(gain); +} + +bool HX711::is_ready() +{ + return gpio_get(DOUT) == 0; +} + +void HX711::set_gain(byte gain) +{ + switch (gain) + { + case 128: // channel A, gain factor 128 + GAIN = 1; + break; + case 64: // channel A, gain factor 64 + GAIN = 3; + break; + case 32: // channel B, gain factor 32 + GAIN = 2; + break; + } +} + +long HX711::read() +{ + + // Wait for the chip to become ready. + wait_ready(); + + // Define structures for reading data into. + unsigned long value = 0; + uint8_t data[3] = {0}; + uint8_t filler = 0x00; + + // Protect the read sequence from system interrupts. If an interrupt occurs during + // the time the PD_SCK signal is high it will stretch the length of the clock pulse. + // If the total pulse time exceeds 60 uSec this will cause the HX711 to enter + // power down mode during the middle of the read sequence. While the device will + // wake up when PD_SCK goes low again, the reset starts a new conversion cycle which + // forces DOUT high until that cycle is completed. + // + // The result is that all subsequent bits read by shiftIn() will read back as 1, + // corrupting the value returned by read(). The ATOMIC_BLOCK macro disables + // interrupts during the sequence and then restores the interrupt mask to its previous + // state after the sequence completes, insuring that the entire read-and-gain-set + // sequence is not interrupted. The macro has a few minor advantages over bracketing + // the sequence between `noInterrupts()` and `interrupts()` calls. + auto status = save_and_disable_interrupts(); + + // Pulse the clock pin 24 times to read the data. + data[2] = SHIFTIN_WITH_SPEED_SUPPORT(DOUT, PD_SCK, MSBFIRST); + data[1] = SHIFTIN_WITH_SPEED_SUPPORT(DOUT, PD_SCK, MSBFIRST); + data[0] = SHIFTIN_WITH_SPEED_SUPPORT(DOUT, PD_SCK, MSBFIRST); + + // Set the channel and the gain factor for the next reading using the clock pin. + for (unsigned int i = 0; i < GAIN; i++) + { + gpio_put(PD_SCK, 1); + + sleep_ms(1); + + gpio_put(PD_SCK, 0); + + sleep_ms(1); + } + + // Enable interrupts again. + restore_interrupts(status); + // Replicate the most significant bit to pad out a 32-bit signed integer + if (data[2] & 0x80) + { + filler = 0xFF; + } + else + { + filler = 0x00; + } + + // Construct a 32-bit signed integer + value = (static_cast(filler) << 24 | static_cast(data[2]) << 16 | static_cast(data[1]) << 8 | static_cast(data[0])); + + return static_cast(value); +} + +void HX711::wait_ready(unsigned long delay_ms) +{ + // Wait for the chip to become ready. + // This is a blocking implementation and will + // halt the sketch until a load cell is connected. + while (!is_ready()) + { + // Probably will do no harm on AVR but will feed the Watchdog Timer (WDT) on ESP. + // https://github.com/bogde/HX711/issues/73 + sleep_ms(delay_ms); + } +} + +bool HX711::wait_ready_retry(int retries, unsigned long delay_ms) +{ + // Wait for the chip to become ready by + // retrying for a specified amount of attempts. + // https://github.com/bogde/HX711/issues/76 + int count = 0; + while (count < retries) + { + if (is_ready()) + { + return true; + } + sleep_ms(delay_ms); + count++; + } + return false; +} + +bool HX711::wait_ready_timeout(unsigned long timeout, unsigned long delay_ms) +{ + // Wait for the chip to become ready until timeout. + // https://github.com/bogde/HX711/pull/96 + auto millisStarted = time_us_32(); + while (time_us_32() - millisStarted < timeout) + { + if (is_ready()) + { + return true; + } + sleep_ms(delay_ms); + } + return false; +} + +long HX711::read_average(byte times) +{ + long sum = 0; + for (byte i = 0; i < times; i++) + { + sum += read(); + // Probably will do no harm on AVR but will feed the Watchdog Timer (WDT) on ESP. + // https://github.com/bogde/HX711/issues/73 + sleep_ms(0); + } + return sum / times; +} + +double HX711::get_value(byte times) +{ + return read_average(times) - OFFSET; +} + +float HX711::get_units(byte times) +{ + return get_value(times) / SCALE; +} + +void HX711::tare(byte times) +{ + double sum = read_average(times); + set_offset(sum); +} + +void HX711::set_scale(float scale) +{ + SCALE = scale; +} + +float HX711::get_scale() +{ + return SCALE; +} + +void HX711::set_offset(long offset) +{ + OFFSET = offset; +} + +long HX711::get_offset() +{ + return OFFSET; +} + +void HX711::power_down() +{ + gpio_put(PD_SCK, 0); + gpio_put(PD_SCK, 1); +} + +void HX711::power_up() +{ + gpio_put(PD_SCK, 0); +} \ No newline at end of file diff --git a/src/Telemetrix4RpiPico.cpp b/src/Telemetrix4RpiPico.cpp index 7e7358d..248206d 100644 --- a/src/Telemetrix4RpiPico.cpp +++ b/src/Telemetrix4RpiPico.cpp @@ -1233,9 +1233,15 @@ void sensor_new() else if (type == SENSOR_TYPES::TOF_VL53) { sensor = new VL53L0X_Sensor(sensor_data); - } else if (type == SENSOR_TYPES::MPU_9250) { + } + else if (type == SENSOR_TYPES::MPU_9250) + { sensor = new MPU9250_Sensor(sensor_data); + } else if (type == SENSOR_TYPES::LOAD_CELL) + { + sensor = new HX711_Sensor(sensor_data); } + sensor->type = type; sensor->num = sensor_num; @@ -1348,30 +1354,51 @@ void VL53L0X_Sensor::readSensor() this->writeSensorData(data); } -MPU9250_Sensor::MPU9250_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]) { +MPU9250_Sensor::MPU9250_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]) +{ this->sensor.bus = settings[0]; this->sensor.setup(0x68); } -void MPU9250_Sensor::readSensor() { - if (this->sensor.update()) { +void MPU9250_Sensor::readSensor() +{ + if (this->sensor.update()) + { std::vector float_data; - for(int i = 0; i < 3; i++) { + for (int i = 0; i < 3; i++) + { float_data.push_back(this->sensor.getAcc(i)); } - for(int i = 0; i < 3; i++) { + for (int i = 0; i < 3; i++) + { float_data.push_back(this->sensor.getGyro(i)); } - for(int i = 0; i < 3; i++) { + for (int i = 0; i < 3; i++) + { float_data.push_back(this->sensor.getMag(i)); } - const unsigned char* bytes = reinterpret_cast(&float_data[0]); + const unsigned char *bytes = reinterpret_cast(&float_data[0]); static_assert(sizeof(float) == 4); - std::vector data(bytes, bytes+sizeof(float)*float_data.size()); + std::vector data(bytes, bytes + sizeof(float) * float_data.size()); + this->writeSensorData(data); + } +} + +HX711_Sensor::HX711_Sensor(uint8_t sensor_data[SENSORS_MAX_SETTINGS_A]) { + this->sensor.begin(sensor_data[0], sensor_data[1]); +} +void HX711_Sensor::readSensor() { + if(this->sensor.is_ready()){ + auto reading = this->sensor.read(); + static_assert(sizeof(reading) == 4); + std::vector data(reinterpret_cast(&reading), + reinterpret_cast(&reading) + sizeof(reading)); this->writeSensorData(data); } } + + void Sensor::writeSensorData(std::vector data) { std::vector out = { From 2839ea98fc5581781c1e07204e5e35b5748ae35b Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Wed, 13 Sep 2023 13:37:04 +0200 Subject: [PATCH 15/33] fix hx711 --- include/HX711.hpp | 2 +- scripts/upload.sh | 2 +- src/HX711.cpp | 21 ++++++++++++++------- src/Telemetrix4RpiPico.cpp | 26 ++++++++++++++------------ 4 files changed, 30 insertions(+), 21 deletions(-) diff --git a/include/HX711.hpp b/include/HX711.hpp index ead4788..2005ece 100644 --- a/include/HX711.hpp +++ b/include/HX711.hpp @@ -40,7 +40,7 @@ class HX711 bool is_ready(); // Wait for the HX711 to become ready - void wait_ready(unsigned long delay_ms = 0); + void wait_ready(unsigned long delay_ms = 1); bool wait_ready_retry(int retries = 3, unsigned long delay_ms = 0); bool wait_ready_timeout(unsigned long timeout = 1000, unsigned long delay_ms = 0); diff --git a/scripts/upload.sh b/scripts/upload.sh index b1ace25..1e2db84 100755 --- a/scripts/upload.sh +++ b/scripts/upload.sh @@ -1,5 +1,5 @@ #!/bin/bash -cd build/ +# cd build/ sudo picotool load Telemetrix4RpiPico.uf2 -F sudo picotool reboot \ No newline at end of file diff --git a/src/HX711.cpp b/src/HX711.cpp index e378237..07793c0 100644 --- a/src/HX711.cpp +++ b/src/HX711.cpp @@ -35,13 +35,13 @@ uint8_t shiftInSlow(uint8_t dataPin, uint8_t clockPin, uint8_t bitOrder) for (i = 0; i < 8; ++i) { gpio_put(clockPin, 1); - sleep_us(1); + busy_wait_us(1); if (bitOrder == LSBFIRST) value |= gpio_get(dataPin) << i; else value |= gpio_get(dataPin) << (7 - i); gpio_put(clockPin, 0); - sleep_us(1); + busy_wait_us(1); } return value; } @@ -69,7 +69,9 @@ void HX711::begin(byte dout, byte pd_sck, byte gain) { PD_SCK = pd_sck; DOUT = dout; + gpio_init(PD_SCK); gpio_set_dir(PD_SCK, GPIO_OUT); + gpio_init(DOUT); gpio_set_dir(DOUT, GPIO_IN); // pinMode(PD_SCK, OUTPUT); @@ -103,7 +105,10 @@ long HX711::read() { // Wait for the chip to become ready. - wait_ready(); + if (!wait_ready_timeout(100, 10)) + { + return 0; + } // Define structures for reading data into. unsigned long value = 0; @@ -135,11 +140,11 @@ long HX711::read() { gpio_put(PD_SCK, 1); - sleep_ms(1); + busy_wait_us(1); gpio_put(PD_SCK, 0); - sleep_ms(1); + busy_wait_us(1); } // Enable interrupts again. @@ -195,8 +200,10 @@ bool HX711::wait_ready_timeout(unsigned long timeout, unsigned long delay_ms) { // Wait for the chip to become ready until timeout. // https://github.com/bogde/HX711/pull/96 - auto millisStarted = time_us_32(); - while (time_us_32() - millisStarted < timeout) + + timeout *= 1000; + auto microsStarted = time_us_32(); + while (time_us_32() - microsStarted < timeout) { if (is_ready()) { diff --git a/src/Telemetrix4RpiPico.cpp b/src/Telemetrix4RpiPico.cpp index 248206d..d0bdef5 100644 --- a/src/Telemetrix4RpiPico.cpp +++ b/src/Telemetrix4RpiPico.cpp @@ -1237,11 +1237,12 @@ void sensor_new() else if (type == SENSOR_TYPES::MPU_9250) { sensor = new MPU9250_Sensor(sensor_data); - } else if (type == SENSOR_TYPES::LOAD_CELL) + } + else if (type == SENSOR_TYPES::LOAD_CELL) { sensor = new HX711_Sensor(sensor_data); } - + sensor->type = type; sensor->num = sensor_num; @@ -1383,22 +1384,24 @@ void MPU9250_Sensor::readSensor() } } -HX711_Sensor::HX711_Sensor(uint8_t sensor_data[SENSORS_MAX_SETTINGS_A]) { - this->sensor.begin(sensor_data[0], sensor_data[1]); +HX711_Sensor::HX711_Sensor(uint8_t sensor_data[SENSORS_MAX_SETTINGS_A]) +{ + auto dout = sensor_data[0]; + auto sck = sensor_data[1]; + this->sensor.begin(dout, sck); } -void HX711_Sensor::readSensor() { - if(this->sensor.is_ready()){ +void HX711_Sensor::readSensor() +{ + if (this->sensor.is_ready()) + { auto reading = this->sensor.read(); static_assert(sizeof(reading) == 4); - std::vector data(reinterpret_cast(&reading), - reinterpret_cast(&reading) + sizeof(reading)); + std::vector data(reinterpret_cast(&reading), + reinterpret_cast(&reading) + sizeof(reading)); this->writeSensorData(data); } } - - - void Sensor::writeSensorData(std::vector data) { std::vector out = { @@ -1587,7 +1590,6 @@ int main() led_debug(2, 250); watchdog_enable(1000, 1); // Add watchdog requiring trigger every 1s - // infinite loop uint32_t last_scan = 0; while (true) From d61926ae40f69b59948e63e3ab52b24d2f41b4ba Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Wed, 13 Sep 2023 14:13:13 +0200 Subject: [PATCH 16/33] add gps --- include/Telemetrix4RpiPico.hpp | 1 + src/Telemetrix4RpiPico.cpp | 26 +++++++++++++++++++++++++- 2 files changed, 26 insertions(+), 1 deletion(-) diff --git a/include/Telemetrix4RpiPico.hpp b/include/Telemetrix4RpiPico.hpp index 5ebcc0f..963736a 100644 --- a/include/Telemetrix4RpiPico.hpp +++ b/include/Telemetrix4RpiPico.hpp @@ -536,6 +536,7 @@ class GPS_Sensor : public Sensor void readSensor(); private: + uart_inst_t * uart_id = uart0; }; class ADXL345_Sensor : public Sensor { diff --git a/src/Telemetrix4RpiPico.cpp b/src/Telemetrix4RpiPico.cpp index d0bdef5..c454538 100644 --- a/src/Telemetrix4RpiPico.cpp +++ b/src/Telemetrix4RpiPico.cpp @@ -1260,10 +1260,34 @@ void readSensors() GPS_Sensor::GPS_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]) { // TODO: read uart, forward to serial + auto rx = settings[0]; + this->uart_id = uart0; + + const auto DATA_BITS = 8; + const auto STOP_BITS = 1; + const auto PARITY = UART_PARITY_NONE; + + uart_init(uart_id, 9600); + // gpio_set_function(UART_TX_PIN, GPIO_FUNC_UART); // No need to send anything to the GPS receiver + gpio_set_function(rx, GPIO_FUNC_UART); + uart_set_hw_flow(uart_id, false, false); + + // Set our data format + uart_set_format(uart_id, DATA_BITS, STOP_BITS, PARITY); + + // Turn off FIFO's - we want to do this character by character + uart_set_fifo_enabled(uart_id, true); } void GPS_Sensor::readSensor() { - // TODO: read uart, forward to serial + // TODO: is this fast enough? fifo is 32 bytes long, unknown speed of gps module + // baud = 9600 -> max ~960 bytes/s. Sensor interval is 100Hz, should be okay. + std::vector data; + while (uart_is_readable(this->uart_id)) + { + data.push_back(uart_getc(this->uart_id)); + } + this->writeSensorData(data); } ADXL345_Sensor::ADXL345_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]) From 0914a12eb9427269eb58b00956d6bd6064cb56df Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Wed, 13 Sep 2023 16:20:05 +0200 Subject: [PATCH 17/33] gps done! --- src/Telemetrix4RpiPico.cpp | 30 +++++++++++++++++++----------- 1 file changed, 19 insertions(+), 11 deletions(-) diff --git a/src/Telemetrix4RpiPico.cpp b/src/Telemetrix4RpiPico.cpp index c454538..1daa745 100644 --- a/src/Telemetrix4RpiPico.cpp +++ b/src/Telemetrix4RpiPico.cpp @@ -1217,7 +1217,6 @@ void sensor_new() return; } Sensor *sensor = nullptr; - if (type == GPS) { sensor = new GPS_Sensor(sensor_data); @@ -1259,35 +1258,44 @@ void readSensors() GPS_Sensor::GPS_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]) { - // TODO: read uart, forward to serial auto rx = settings[0]; + // auto tx = settings[1]; + auto uart = settings[2]; this->uart_id = uart0; + if (uart) + { + this->uart_id = uart1; + } const auto DATA_BITS = 8; const auto STOP_BITS = 1; const auto PARITY = UART_PARITY_NONE; uart_init(uart_id, 9600); - // gpio_set_function(UART_TX_PIN, GPIO_FUNC_UART); // No need to send anything to the GPS receiver + // gpio_set_function(tx, GPIO_FUNC_UART); // No need to send anything to the GPS receiver gpio_set_function(rx, GPIO_FUNC_UART); uart_set_hw_flow(uart_id, false, false); - // Set our data format - uart_set_format(uart_id, DATA_BITS, STOP_BITS, PARITY); - - // Turn off FIFO's - we want to do this character by character uart_set_fifo_enabled(uart_id, true); + + uart_set_format(uart_id, DATA_BITS, STOP_BITS, PARITY); + send_debug_info(100, 43); } + void GPS_Sensor::readSensor() { - // TODO: is this fast enough? fifo is 32 bytes long, unknown speed of gps module + // Is this fast enough? fifo is 32 bytes long, unknown speed of gps module // baud = 9600 -> max ~960 bytes/s. Sensor interval is 100Hz, should be okay. + std::vector data; - while (uart_is_readable(this->uart_id)) + if (uart_is_readable(this->uart_id)) { - data.push_back(uart_getc(this->uart_id)); + while (uart_is_readable(this->uart_id)) + { + data.push_back(uart_getc(this->uart_id)); + } + this->writeSensorData(data); } - this->writeSensorData(data); } ADXL345_Sensor::ADXL345_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]) From 519580f2f436b0737c7458434ab984c66b631b9a Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Wed, 13 Sep 2023 17:00:37 +0200 Subject: [PATCH 18/33] start ping --- include/Telemetrix4RpiPico.hpp | 3 +++ src/Telemetrix4RpiPico.cpp | 19 ++++++++++++++++--- 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/include/Telemetrix4RpiPico.hpp b/include/Telemetrix4RpiPico.hpp index 963736a..dccbf6d 100644 --- a/include/Telemetrix4RpiPico.hpp +++ b/include/Telemetrix4RpiPico.hpp @@ -403,6 +403,7 @@ typedef enum #define ENCODER_REPORT 14 #define DEBUG_PRINT 99 const int SENSOR_REPORT = 20; +const int POING_REPORT = 32; /*************************************************************** * INPUT PIN REPORTING CONTROL SUB COMMANDS ***************************************************************/ @@ -504,6 +505,8 @@ typedef struct void (*command_func)(void); } command_descriptor; +void ping(); + /*****************************************************************************/ /****SENSORS*/ /*****************************************************************************/ diff --git a/src/Telemetrix4RpiPico.cpp b/src/Telemetrix4RpiPico.cpp index 1daa745..206f9ec 100644 --- a/src/Telemetrix4RpiPico.cpp +++ b/src/Telemetrix4RpiPico.cpp @@ -177,7 +177,8 @@ command_descriptor command_table[] = {{&serial_loopback}, {&spi_cs_control}, {&set_scan_delay}, {&encoder_new}, - {&sensor_new}}; + {&sensor_new}, + {&ping}}; /*************************************************************************** * DEBUGGING FUNCTIONS @@ -1203,6 +1204,18 @@ void scan_sonars() } } +void ping() { + auto special_num = command_buffer[1]; + std::vector out = { + 0, // write len + POING_REPORT, // write type + special_num + }; + out[0] = out.size() - 1; // dont count the packet length + serial_write(out); + // watchdog_update(); +} + // SENSORSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSS void sensor_new() @@ -1621,12 +1634,12 @@ int main() // starting afresh led_debug(2, 250); - watchdog_enable(1000, 1); // Add watchdog requiring trigger every 1s + // watchdog_enable(1000, 1); // Add watchdog requiring trigger every 1s // infinite loop uint32_t last_scan = 0; while (true) { - watchdog_update(); + // watchdog_update(); get_next_command(); if (!stop_reports) From bda6dae02be4238f43dcdcd467f70ba78a51afcd Mon Sep 17 00:00:00 2001 From: Arend-Jan Date: Wed, 13 Sep 2023 17:54:59 +0200 Subject: [PATCH 19/33] fix ping --- src/Telemetrix4RpiPico.cpp | 29 ++++++++++++++++++++--------- 1 file changed, 20 insertions(+), 9 deletions(-) diff --git a/src/Telemetrix4RpiPico.cpp b/src/Telemetrix4RpiPico.cpp index 206f9ec..0641415 100644 --- a/src/Telemetrix4RpiPico.cpp +++ b/src/Telemetrix4RpiPico.cpp @@ -1204,16 +1204,29 @@ void scan_sonars() } } -void ping() { +void ping() +{ + static bool watchdog_enabled = false; + static uint8_t random = -1; + auto special_num = command_buffer[1]; + if (!watchdog_enabled) + { + watchdog_enable(1000, 1); // Add watchdog requiring trigger every 1s + watchdog_enabled = true; + srand(time_us_32()); + random = rand() % 100; // create some random number to let computer side know it is the same run + } std::vector out = { - 0, // write len - POING_REPORT, // write type - special_num - }; + 0, // write len + POING_REPORT, // write type + special_num, + random + }; out[0] = out.size() - 1; // dont count the packet length serial_write(out); - // watchdog_update(); + + watchdog_update(); } // SENSORSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSS @@ -1292,13 +1305,12 @@ GPS_Sensor::GPS_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]) uart_set_fifo_enabled(uart_id, true); uart_set_format(uart_id, DATA_BITS, STOP_BITS, PARITY); - send_debug_info(100, 43); } void GPS_Sensor::readSensor() { // Is this fast enough? fifo is 32 bytes long, unknown speed of gps module - // baud = 9600 -> max ~960 bytes/s. Sensor interval is 100Hz, should be okay. + // baud = 9600 -> max ~960 bytes/s. Sensor interval is 50-100Hz -> 1500-3200 bytes/s, is okay. std::vector data; if (uart_is_readable(this->uart_id)) @@ -1634,7 +1646,6 @@ int main() // starting afresh led_debug(2, 250); - // watchdog_enable(1000, 1); // Add watchdog requiring trigger every 1s // infinite loop uint32_t last_scan = 0; while (true) From 2ffc6e07be0cb1ba5541aa5e72d6b8fd8916d191 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 18 Sep 2023 08:32:19 +0200 Subject: [PATCH 20/33] VL check --- src/Telemetrix4RpiPico.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/Telemetrix4RpiPico.cpp b/src/Telemetrix4RpiPico.cpp index 0641415..b5ce85c 100644 --- a/src/Telemetrix4RpiPico.cpp +++ b/src/Telemetrix4RpiPico.cpp @@ -1401,7 +1401,11 @@ void VEML6040_Sensor::readSensor() VL53L0X_Sensor::VL53L0X_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]) { this->sensor.setBus(settings[0]); - this->sensor.init(); + bool ok = this->sensor.init(); + if(!ok) { + this->stop = true; + return; + } this->sensor.startContinuous(); } From 346eb303bdab91a9fc4456b14964f50364da058f Mon Sep 17 00:00:00 2001 From: Arend-Jan van Hilten Date: Thu, 21 Sep 2023 11:12:13 +0200 Subject: [PATCH 21/33] Add tof sensor timeout check --- src/Telemetrix4RpiPico.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/Telemetrix4RpiPico.cpp b/src/Telemetrix4RpiPico.cpp index b5ce85c..72d5b34 100644 --- a/src/Telemetrix4RpiPico.cpp +++ b/src/Telemetrix4RpiPico.cpp @@ -1401,6 +1401,7 @@ void VEML6040_Sensor::readSensor() VL53L0X_Sensor::VL53L0X_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]) { this->sensor.setBus(settings[0]); + this->sensor.setTimeout(100); // Don't block the reading when there is no sensor bool ok = this->sensor.init(); if(!ok) { this->stop = true; @@ -1411,7 +1412,13 @@ VL53L0X_Sensor::VL53L0X_Sensor(uint8_t settings[SENSORS_MAX_SETTINGS_A]) void VL53L0X_Sensor::readSensor() { + if(this->stop) { + return; + } auto dist = this->sensor.readRangeContinuousMillimeters(); + if(dist == 65535) { + this->stop = true; + } std::vector data = {(uint8_t)(dist >> 8), (uint8_t)(dist & 0xFF)}; this->writeSensorData(data); } From 2c460b59a25a273058d760b03391e2915de20357 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 9 Oct 2023 13:23:16 +0200 Subject: [PATCH 22/33] Fix digitalread not always sending a message from setup --- Telemetrix4RpiPico.c | 1 + 1 file changed, 1 insertion(+) diff --git a/Telemetrix4RpiPico.c b/Telemetrix4RpiPico.c index f9772f1..f883309 100644 --- a/Telemetrix4RpiPico.c +++ b/Telemetrix4RpiPico.c @@ -247,6 +247,7 @@ void set_pin_mode() case DIGITAL_INPUT_PULL_DOWN: the_digital_pins[pin].pin_mode = mode; the_digital_pins[pin].reporting_enabled = command_buffer[SET_PIN_MODE_DIGITAL_IN_REPORTING_STATE]; + the_digital_pins[pin].last_value = 0xFF; gpio_init(pin); gpio_set_dir(pin, GPIO_IN); if (mode == DIGITAL_INPUT_PULL_UP) From bf238f51b478e4ea4680e300d460b9f2d64bb459 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 9 Oct 2023 14:27:01 +0200 Subject: [PATCH 23/33] Add building of uf2 to pipeline --- .github/workflows/deploy.yml | 69 ++++++++++++++++++++++++++++++++++++ Dockerfile | 33 +++++++++++++++++ build.sh | 9 +++++ 3 files changed, 111 insertions(+) create mode 100644 .github/workflows/deploy.yml create mode 100644 Dockerfile create mode 100755 build.sh diff --git a/.github/workflows/deploy.yml b/.github/workflows/deploy.yml new file mode 100644 index 0000000..27b1762 --- /dev/null +++ b/.github/workflows/deploy.yml @@ -0,0 +1,69 @@ +name: Deploy new version + +# Only deploy when a new tag is pushed +on: [push, pull_request] + +# Must match the project() name in CMakeLists.txt +env: + APP_NAME: Telemetrix4RpiPico + +# Allow this workflow to write back to the repository +permissions: + contents: write + +# Build binary and send to releases +jobs: + build-deploy: + runs-on: ubuntu-latest + name: Build and deploy + steps: + + - name: Check out this repository + uses: actions/checkout@v3 + - name: Set up Docker Buildx + id: buildx + uses: docker/setup-buildx-action@v1 + + - name: Cache register + uses: actions/cache@v2 + with: + path: /tmp/.buildx-cache + key: ${{ runner.os }}-buildx-${{ hashFiles('**/Dockerfile') }} + + - name: Build Docker image + uses: docker/build-push-action@v2 + with: + context: ./ + file: ./Dockerfile + builder: ${{ steps.buildx.outputs.name }} + load: true + tags: pico-builder-container:latest + cache-from: type=local,src=/tmp/.buildx-cache + cache-to: type=local,dest=/tmp/.buildx-cache + + - name: Build Docker image + run: docker build -t pico-builder-image . + + - name: Build + run: docker run -v ./:/project/src/ pico-builder-image + + # - name: Copy out .uf2 file + # run: docker cp pico-builder-container:/project/src/build/${APP_NAME}.uf2 ./${APP_NAME}.uf2 + - name: Copy uf2 file + run: cp ./build2/${APP_NAME}.uf2 ./ && rm -rf ./build2 + + - name: Put environment variable into the env context + run: echo "app_name=$APP_NAME" >> $GITHUB_ENV + - name: Archive production artifacts + uses: actions/upload-artifact@v3 + with: + name: dist + path: | + ${APP_NAME}.uf2 + + # - name: Push to release + # uses: softprops/action-gh-release@v1 + # if: startsWith(github.ref, 'refs/tags/') + # with: + # files: ${{ env.app_name }}.uf2 + # body_path: CHANGELOG.md \ No newline at end of file diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..c07ed18 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,33 @@ +# Fetch ubuntu image +FROM ubuntu:22.04 + +# Install prerequisites +RUN \ + apt update && \ + apt install -y git python3 && \ + apt install -y cmake gcc-arm-none-eabi libnewlib-arm-none-eabi build-essential + +# Install Pico SDK +RUN \ + mkdir -p /project/src/ && \ + cd /project/ && \ + git clone https://github.com/raspberrypi/pico-sdk.git --branch master && \ + cd pico-sdk/ && \ + git submodule update --init && \ + cd / + +# Set the Pico SDK environment variable +ENV PICO_SDK_PATH=/project/pico-sdk/ + +# Copy in our source files +COPY build.sh /root/ +RUN chmod +x /root/build.sh +# Build project +# RUN \ +# mkdir -p /project/src/build && \ +# cd /project/src/build && \ +# cmake .. && \ +# make + +# Command that will be invoked when the container starts +ENTRYPOINT ["/root/build.sh"] \ No newline at end of file diff --git a/build.sh b/build.sh new file mode 100755 index 0000000..cef9f3e --- /dev/null +++ b/build.sh @@ -0,0 +1,9 @@ +#!/bin/bash +cd /project/src/ +ls +ls .. +mkdir build2/ +ls +cd build2 +cmake -DCMAKE_BUILD_TYPE=Release .. +make \ No newline at end of file From 5bebfba9b1feac78939818a31dd7197ae79bdb79 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 9 Oct 2023 14:29:25 +0200 Subject: [PATCH 24/33] Test push --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index cf9b62c..80f8180 100644 --- a/README.md +++ b/README.md @@ -29,3 +29,5 @@ PC. A complete [User's Guide](https://mryslab.github.io/telemetrix-rpi-pico/) is install and use both the server and client. To install the server, follow these [installation instructions](https://mryslab.github.io/telemetrix-rpi-pico/install_pico_server/). + +Test push From 3c44fba29309a28d83e84421d30260be01883ca9 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 9 Oct 2023 14:34:21 +0200 Subject: [PATCH 25/33] Update components --- .github/workflows/deploy.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/deploy.yml b/.github/workflows/deploy.yml index 27b1762..c72c3cc 100644 --- a/.github/workflows/deploy.yml +++ b/.github/workflows/deploy.yml @@ -25,13 +25,13 @@ jobs: uses: docker/setup-buildx-action@v1 - name: Cache register - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: /tmp/.buildx-cache key: ${{ runner.os }}-buildx-${{ hashFiles('**/Dockerfile') }} - name: Build Docker image - uses: docker/build-push-action@v2 + uses: docker/build-push-action@v5 with: context: ./ file: ./Dockerfile From e410587b1403f90faf670ad184953ce25815b96a Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 9 Oct 2023 14:35:33 +0200 Subject: [PATCH 26/33] Remove extra build --- .github/workflows/deploy.yml | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/.github/workflows/deploy.yml b/.github/workflows/deploy.yml index c72c3cc..eb3d34e 100644 --- a/.github/workflows/deploy.yml +++ b/.github/workflows/deploy.yml @@ -40,12 +40,9 @@ jobs: tags: pico-builder-container:latest cache-from: type=local,src=/tmp/.buildx-cache cache-to: type=local,dest=/tmp/.buildx-cache - - - name: Build Docker image - run: docker build -t pico-builder-image . - name: Build - run: docker run -v ./:/project/src/ pico-builder-image + run: docker run -v ./:/project/src/ pico-builder-container # - name: Copy out .uf2 file # run: docker cp pico-builder-container:/project/src/build/${APP_NAME}.uf2 ./${APP_NAME}.uf2 From 93728e51619e2aaba7631e42dcd2c8bbdc30df90 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 9 Oct 2023 14:36:33 +0200 Subject: [PATCH 27/33] dont rm build folder --- .github/workflows/deploy.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/deploy.yml b/.github/workflows/deploy.yml index eb3d34e..910bca9 100644 --- a/.github/workflows/deploy.yml +++ b/.github/workflows/deploy.yml @@ -47,7 +47,7 @@ jobs: # - name: Copy out .uf2 file # run: docker cp pico-builder-container:/project/src/build/${APP_NAME}.uf2 ./${APP_NAME}.uf2 - name: Copy uf2 file - run: cp ./build2/${APP_NAME}.uf2 ./ && rm -rf ./build2 + run: cp ./build2/${APP_NAME}.uf2 ./ - name: Put environment variable into the env context run: echo "app_name=$APP_NAME" >> $GITHUB_ENV From 2bd41d8663e8791163f48de5c9518ab9e2888868 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 9 Oct 2023 14:45:22 +0200 Subject: [PATCH 28/33] change app_name --- .github/workflows/deploy.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/deploy.yml b/.github/workflows/deploy.yml index 910bca9..e293b62 100644 --- a/.github/workflows/deploy.yml +++ b/.github/workflows/deploy.yml @@ -56,7 +56,7 @@ jobs: with: name: dist path: | - ${APP_NAME}.uf2 + ${app_name}.uf2 # - name: Push to release # uses: softprops/action-gh-release@v1 From ebc9ae060405d93e7091649cffe2e52ae7d6414a Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 9 Oct 2023 14:59:13 +0200 Subject: [PATCH 29/33] Fix app_name --- .github/workflows/deploy.yml | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/.github/workflows/deploy.yml b/.github/workflows/deploy.yml index e293b62..77cd51b 100644 --- a/.github/workflows/deploy.yml +++ b/.github/workflows/deploy.yml @@ -44,10 +44,8 @@ jobs: - name: Build run: docker run -v ./:/project/src/ pico-builder-container - # - name: Copy out .uf2 file - # run: docker cp pico-builder-container:/project/src/build/${APP_NAME}.uf2 ./${APP_NAME}.uf2 - name: Copy uf2 file - run: cp ./build2/${APP_NAME}.uf2 ./ + run: cp ./build2/Telemetrix4RpiPico.uf2 ./ - name: Put environment variable into the env context run: echo "app_name=$APP_NAME" >> $GITHUB_ENV @@ -56,7 +54,7 @@ jobs: with: name: dist path: | - ${app_name}.uf2 + Telemetrix4RpiPico.uf2 # - name: Push to release # uses: softprops/action-gh-release@v1 From 030a5aee00a46f51766fe9cd598f55ce005972f0 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 9 Oct 2023 15:17:21 +0200 Subject: [PATCH 30/33] Release test --- .github/workflows/deploy.yml | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/.github/workflows/deploy.yml b/.github/workflows/deploy.yml index 77cd51b..2012dc3 100644 --- a/.github/workflows/deploy.yml +++ b/.github/workflows/deploy.yml @@ -56,9 +56,8 @@ jobs: path: | Telemetrix4RpiPico.uf2 - # - name: Push to release - # uses: softprops/action-gh-release@v1 - # if: startsWith(github.ref, 'refs/tags/') - # with: - # files: ${{ env.app_name }}.uf2 - # body_path: CHANGELOG.md \ No newline at end of file + - name: Push to release + uses: softprops/action-gh-release@v1 + if: startsWith(github.ref, 'refs/tags/') + with: + files: Telemetrix4RpiPico.uf2 \ No newline at end of file From 15eaad40020d6ba83c205c40c111c21c396dbc96 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 9 Oct 2023 16:17:41 +0200 Subject: [PATCH 31/33] Remove edit from other branch --- Telemetrix4RpiPico.c | 1 - 1 file changed, 1 deletion(-) diff --git a/Telemetrix4RpiPico.c b/Telemetrix4RpiPico.c index f883309..f9772f1 100644 --- a/Telemetrix4RpiPico.c +++ b/Telemetrix4RpiPico.c @@ -247,7 +247,6 @@ void set_pin_mode() case DIGITAL_INPUT_PULL_DOWN: the_digital_pins[pin].pin_mode = mode; the_digital_pins[pin].reporting_enabled = command_buffer[SET_PIN_MODE_DIGITAL_IN_REPORTING_STATE]; - the_digital_pins[pin].last_value = 0xFF; gpio_init(pin); gpio_set_dir(pin, GPIO_IN); if (mode == DIGITAL_INPUT_PULL_UP) From ec544e2ef85c531e249535b57f443bcca4e9b3b0 Mon Sep 17 00:00:00 2001 From: "Arend-Jan van Hilten (TU)" Date: Mon, 9 Oct 2023 16:19:43 +0200 Subject: [PATCH 32/33] Remove edit from other branch --- Telemetrix4RpiPico.c | 1 - 1 file changed, 1 deletion(-) diff --git a/Telemetrix4RpiPico.c b/Telemetrix4RpiPico.c index f883309..f9772f1 100644 --- a/Telemetrix4RpiPico.c +++ b/Telemetrix4RpiPico.c @@ -247,7 +247,6 @@ void set_pin_mode() case DIGITAL_INPUT_PULL_DOWN: the_digital_pins[pin].pin_mode = mode; the_digital_pins[pin].reporting_enabled = command_buffer[SET_PIN_MODE_DIGITAL_IN_REPORTING_STATE]; - the_digital_pins[pin].last_value = 0xFF; gpio_init(pin); gpio_set_dir(pin, GPIO_IN); if (mode == DIGITAL_INPUT_PULL_UP) From 0e36e2a5f3156fce881ee5b27e6349ad516cebe4 Mon Sep 17 00:00:00 2001 From: Arend-Jan van Hilten Date: Wed, 11 Oct 2023 10:24:12 +0200 Subject: [PATCH 33/33] Fix wrong pull-request Pull request #13 probably removed the change from #12 --- Telemetrix4RpiPico.c | 1 + 1 file changed, 1 insertion(+) diff --git a/Telemetrix4RpiPico.c b/Telemetrix4RpiPico.c index f9772f1..f883309 100644 --- a/Telemetrix4RpiPico.c +++ b/Telemetrix4RpiPico.c @@ -247,6 +247,7 @@ void set_pin_mode() case DIGITAL_INPUT_PULL_DOWN: the_digital_pins[pin].pin_mode = mode; the_digital_pins[pin].reporting_enabled = command_buffer[SET_PIN_MODE_DIGITAL_IN_REPORTING_STATE]; + the_digital_pins[pin].last_value = 0xFF; gpio_init(pin); gpio_set_dir(pin, GPIO_IN); if (mode == DIGITAL_INPUT_PULL_UP)