From 6ee58edc7d5becbeeae5c3e57cef85d490d65e01 Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Fri, 14 Feb 2025 11:32:40 +0300 Subject: [PATCH] Add LocCorr_new: another variant of LocCorr (instead of pusirobot used multistepper; some errors fixed); I hope, sometimes I will finish loccorrNG --- LocCorr_new/15x12.png | Bin 0 -> 12585 bytes LocCorr_new/CANSERVER_pusirobo | 9 + LocCorr_new/CMakeLists.txt | 89 +++ LocCorr_new/DEBUG.log.analyze | 9 + LocCorr_new/FindBASLER.cmake | 36 ++ LocCorr_new/FindFLYCAP.cmake | 40 ++ LocCorr_new/Readme.md | 6 + LocCorr_new/basler.c | 335 ++++++++++ LocCorr_new/basler.h | 27 + LocCorr_new/binmorph.c | 500 +++++++++++++++ LocCorr_new/binmorph.h | 64 ++ LocCorr_new/cameracapture.c | 275 ++++++++ LocCorr_new/cameracapture.h | 53 ++ LocCorr_new/cclabling.h | 136 ++++ LocCorr_new/cclabling_ori.h | 128 ++++ LocCorr_new/cmdlnopts.c | 118 ++++ LocCorr_new/cmdlnopts.h | 77 +++ LocCorr_new/config.c | 407 ++++++++++++ LocCorr_new/config.h | 140 +++++ LocCorr_new/debug.c | 57 ++ LocCorr_new/debug.h | 57 ++ LocCorr_new/dilation.h | 79 +++ LocCorr_new/draw.c | 128 ++++ LocCorr_new/draw.h | 46 ++ LocCorr_new/ec_filter.h | 102 +++ LocCorr_new/fc_filter.h | 74 +++ LocCorr_new/fits.c | 127 ++++ LocCorr_new/fits.h | 32 + LocCorr_new/grasshopper.c | 275 ++++++++ LocCorr_new/grasshopper.h | 28 + LocCorr_new/imagefile.c | 582 +++++++++++++++++ LocCorr_new/imagefile.h | 72 +++ LocCorr_new/improc.c | 351 +++++++++++ LocCorr_new/improc.h | 41 ++ LocCorr_new/inotify.c | 129 ++++ LocCorr_new/inotify.h | 28 + LocCorr_new/loccorr.conf | 36 ++ LocCorr_new/main.c | 234 +++++++ LocCorr_new/median.c | 446 +++++++++++++ LocCorr_new/median.h | 31 + LocCorr_new/socket.c | 421 +++++++++++++ LocCorr_new/socket.h | 29 + LocCorr_new/stbimpl.c | 29 + LocCorr_new/steppers.c | 1068 ++++++++++++++++++++++++++++++++ LocCorr_new/steppers.h | 39 ++ 45 files changed, 6990 insertions(+) create mode 100644 LocCorr_new/15x12.png create mode 100644 LocCorr_new/CANSERVER_pusirobo create mode 100644 LocCorr_new/CMakeLists.txt create mode 100755 LocCorr_new/DEBUG.log.analyze create mode 100644 LocCorr_new/FindBASLER.cmake create mode 100644 LocCorr_new/FindFLYCAP.cmake create mode 100644 LocCorr_new/Readme.md create mode 100644 LocCorr_new/basler.c create mode 100644 LocCorr_new/basler.h create mode 100644 LocCorr_new/binmorph.c create mode 100644 LocCorr_new/binmorph.h create mode 100644 LocCorr_new/cameracapture.c create mode 100644 LocCorr_new/cameracapture.h create mode 100644 LocCorr_new/cclabling.h create mode 100644 LocCorr_new/cclabling_ori.h create mode 100644 LocCorr_new/cmdlnopts.c create mode 100644 LocCorr_new/cmdlnopts.h create mode 100644 LocCorr_new/config.c create mode 100644 LocCorr_new/config.h create mode 100644 LocCorr_new/debug.c create mode 100644 LocCorr_new/debug.h create mode 100644 LocCorr_new/dilation.h create mode 100644 LocCorr_new/draw.c create mode 100644 LocCorr_new/draw.h create mode 100644 LocCorr_new/ec_filter.h create mode 100644 LocCorr_new/fc_filter.h create mode 100644 LocCorr_new/fits.c create mode 100644 LocCorr_new/fits.h create mode 100644 LocCorr_new/grasshopper.c create mode 100644 LocCorr_new/grasshopper.h create mode 100644 LocCorr_new/imagefile.c create mode 100644 LocCorr_new/imagefile.h create mode 100644 LocCorr_new/improc.c create mode 100644 LocCorr_new/improc.h create mode 100644 LocCorr_new/inotify.c create mode 100644 LocCorr_new/inotify.h create mode 100644 LocCorr_new/loccorr.conf create mode 100644 LocCorr_new/main.c create mode 100644 LocCorr_new/median.c create mode 100644 LocCorr_new/median.h create mode 100644 LocCorr_new/socket.c create mode 100644 LocCorr_new/socket.h create mode 100644 LocCorr_new/stbimpl.c create mode 100644 LocCorr_new/steppers.c create mode 100644 LocCorr_new/steppers.h diff --git a/LocCorr_new/15x12.png b/LocCorr_new/15x12.png new file mode 100644 index 0000000000000000000000000000000000000000..7758d6cee6ba87185cf1aad4cbbea242ee21b9c3 GIT binary patch literal 12585 zcmeHM2UHWvnhv7UJA#085kUeZBtYm8dT&Zc)94T&K|qRvD2M_|?+8+)swllHh;$3R z7pc;b-ZprB_j-5VzTJ1uzH|1SbKwA)neYGRuQS8U1ASdJDsonG002Oxp{{I5_)A0h zp(G{z0>lfHO9B9_8Mlm0@rDRbE?0M~jlC0w3-9fU;lg;?+W-JwgM~@nH+g}yUyo-Q zaY4x7DFvdH4yu`*4T{SxT0@7MLQ8$cLTy(cYoV1dfj4d+c43Z4j@^qIg5tIE1gs7y zpvHY(hD94MR@8j%9eN!V&Q=}nRaPv|GKa^>j2-U2s+`?y`6g4=)wNmiz^~@mFMalb zv&=)zI=dzBn%jOg^33?@{d=kBynL2@!)9qxDZIA}E+?n0*sA2l9$*}&23WB&A~6xi z4YqN}V%X~W;TT+=jL0%Miw`vrKT@OHZ!@vSf%S}|W(=Y=0lxq9octafc= z!_E&{{b@PQ>q^9Ej`I$MN%84x=4ost-<;dKN_Ss=5;;D?Sm?zsihB7h`7L4B+>Yrv zDr8qyXE!4#vbMKHT>NTWrpo#b_qUeYLR-H9mk%a4_o|l;F_up<6jz3&*5lJKQW2v&->Y=$F@+ir4t$K0P@zs8<$Jlb|y>iZ2PpP)1g2{zV zjNI|+@%m=-z1~RY!WfWSjV$%eP`g;}uK6dP6SP$vNs|Vzb)3fTA06n0Z+eNkh%t-z z7x@c+dUhBB6p_EO&wG7BYQTBJOZd)%a!)iUQ~#w&vD_f~#iC^?lQh%87t@N31`kIU zM-=|<)k=koi3`Pdd^RKombWYCI@_M|hlj}RK61>%N?r+4-}$Of3F^*>;C>&E4Cm|Z zkJM{@8SPF_S+($c9z<+`b)}+dtVimL^aFBb#>kd@PgeJT_yYbEVhZ>v1YawL4)g8 z3d%pX<`+~B4mn1c)ZJRDnmt;E?t67Ot9ieJ`}>5&;ry-MlIv#p-fy;i=_S)feELmP zR}m8Kx^BtuNa+z=5E6b>6=rMdO-Xa#-pX=osfwoSa881u^*Cm1*t3hJV);0D_L1dw z2k!RX63Gjyb9a4woNUtvRBL4Bo7rsai&n#X#40S9UsS$4Sei1QcaU9(L6{AfBe(9L zK3?gWcF3MyKMZ{}&sW4a)Of1fDJS3BKl|a(eyxq&&E<#RGHwWUDOLI;`!{J66>@+n zo)h7Rn5)(x`t5Q?bps>&8MpUcI-|C&N2LvR`+Wwa&Bhft4i{CV2iWLV1}qS~m&Bbv zY_b$x_4?)xLY*pY17qmo+7@4<##`n5c-}ZB-yQ1&ZK=XjY7OkyKKMNNcHmZ64a~}G zbH;^>^C$~+Iw#Zye2?Y({K`BcyCs$JoFVSGq0}LzGNjZN1@jDZ((g?g3Xd>5tnIHp z;}r42L5g~>DcRqFdHGs=k!)lo{V3!PU^nA_v z;+Vzoxg^VXV_RUvh}(tjiO%>q>QqK%K8>MdBilh?t+H!FmAVH;4J?4?;`EM}vAmniTyH7DT(qlft_1)?}~=PM`7N?PKU`771QVI~uXmf`<)- z_Hl1D5;{7_a!TKQCS~z@P*6Eshwj0)ppRsetXt4#+e_x!H0I zInVAd`syW}9=r2u=e%9dd$M+7UOrPAr$tvhm1Chw(K(6Dg6>QCubw_kdw=FVx%Oj# zw0HKtIB5w_6gQx<}{cXkhOmbs^#K?I-~`Ybeche%(+hI@P> zT9029C0XXi3i=qZ8bOK+9kvCZG(H-3vqJF7zVgXXws9Qh=y`lkUUT%6D@Txs(AJ=} zg~OdQ^yeI9@|`F7A~;#TmQ>?O8y1&<{lp zeWgtZf<9ylr>eP01xqjQKYbbRIaL_le20#1S2;wCk4|T?L?I-HKVxN(nnx#zVP>=0 zke}F6@+r|y>tb5yn}KO`pIybeRhA!ojI>~%{j%c01L<^84OwttytJ?>OH7oHctSxJ zu~skJhQ84i3}&%reRX?f683@|?JuPG#ZT$w*mEj6QAX0xdsh8I{inxkPYsoIM zie7dWa9YwP*}oS-#j5j(RXu`ySBIk_I6rcO%Fsh%T@L$lPl-mkQe3uQfPUMb8B9h_ zQ5r_(GLWQDr@E*(*DXtT=Eg{k`x13}g~i3Om4|txUdE0{D{0oDEZ-)a-G)igT!Z_o#yv%uDRm#!%jKnd^Tzy$9u0G5AHaS~1RdwBObUdz#Um`ag zH3HwKoC&pzCy!YR8Kf!NjOVWX_8Q&#zldmKI6Ok1Y?9i1;!I=tE7l)>Ga1NWrXf%FTy9`QcU=edhg1^In{3 zguAEaR&L2%!Bgft?yeCp`ENwGqq%krLzXJVVM-ex#$)d^uAD!}D`eFU5<*k8Mx$Kc zat`(imG>prJS=IVA>|ZM8rNiX9xWPz?IV|yID%DG6&H_nL;SC5CebA@^GPA+NebU+ zeB!OUP|N?=r4`H0;-cD!YnkAB#(rwn=&b{L=jp(0)z8N9fayb3^2ZIar>OnZkzI$( z0~k3xGsh8km@td6w=&Zqkf9?g^z(VPGkcpj!&+SAoXA_i#ns}EjL#33o;wyXnkEFZ zs-zNOh0_!6IeOZ2UnC8H7kh$Zj;B?_J7ips7Vqk74sh(h?Kg+f^M%CT%*QO9A+}I% zhw~ThWzu)bWwohgs@4*7t++NlV?vEpdzKmcf6fA|=fBiI zZo`oISX8a4ZET_d278 z8x0f|0_96rKd^mFE6kp-RO7Y1*`-S5W_~AC8SLdkSpetRZDXK_;FOlk=@f5PwZJb%^w3@Nl1YEM!|O#ivdcLi;8!woE7t^X zLuIG29pCfVK2*XXhr+j!5>*EkcGf-kB?t1dfT$VvA%o-FkzoA;kAO<`E+xp~{btcz zrIKy|RT7c)SDZ!n?%eHHFdD9&X{VFZ$l|oi5jeZ8fz7`!FKD4+*%LuuzgtnAPqBvz(!RBg)0;PGP9=k%FP;ys?NQ&dEcq-3@bBI0M=flcu1E?_X{ zUWRV$;1yyO{Mpspb5A|^&tX0CnRKfc6Q)RetV`Ulr>4&)0IxmeP%iAOA zC>C=&(yEUr{apH}rD$Mg`SpUtMn^^pPTKGBug~#?h-?*=K@h$k4nAsKf|-{C1U1$a zfQDw1rrN7-;v&eJMs4J&YQAE~F%-K8oYs_0 z!rb~uIho9lrIlddI{Vo(wd2BDWz%JKQEA3bzZYqq$C!=7x4o>92Ke0$U~aX?LxC?# zWuER+%PMy?pV!k$w-{f^<+X`1nzXe`Zo6(q}G+l{ROS#|}3!@6G*e@rr&2_DSfB7id>ZOtyZxN7js@*ggB19w zV2g|lQGRkenBx_5iyXaHj#BhKN2$y-w+D`uA+cQMaIm~-*=AzcR5lJ${?wQQJbrCg zaQKN}*f}LZ;&~bXOG##&-jg>E5ZjqA9x*DNE!zztr&26tT4xP>TH5nOWk6nxHe#Q< zy(Z3<6nuGLV40hA9&kH}pT)YWpZ6I_(r1CCZv%ne=Bh?~rm^84Rf>Z!npN;UB|tG zN-75_OX3xS3u;?LhXZ;~^;vor%~&?@-Q>$T#*XKipO23~=hWQNePo{T${u73%y)gA zp1p9{C)-Y3G&P-v=-p=}2aBo?WLvjv05rQ9mpOIwS?VRl0Ec0#bB?}8w5f^aV-}Y} zHC8kOZ{3old|DbzUK+oz&BC*yDw$rN<9b@qlH5$nM#zXgo^IxJqe4kgwd(WtU-9VE z-$lgsO|_3g=g)I3iGNM_1dvImDPit>l)XE`ORLOtT!@ z`OGA4w46+9BXP_3rF&RLR*z^+4a}4{@HG zk1xGUE|Nppp_Ab+S&An$6FEYPk0Cs?^VIyOzB9PqYIJMCx@pT{-6$yxcV!;0O?};O zuNgz*x~ff=61-DjuVEWj)E{inCjOCsBCDKGSFD)z!!44;Ik(RTs4JHKkq z`!2j#-{nn$0t9X03p6C2qek6mMbSz>Lq%M&Y_I>w?i*oe-xA@Tcgm3bl|wj1!=rlZ5@FFQ0~H;I6m(hWcj`YKCVoHa5>mrMzZ9UXVJQu9X-e zeefLCEak^y2_gt=hd=!Pae$na3$D}X0LLtrA%}!}V$MWDK zZHktn|JV_Ex+yJOya?n_x37Twj4D+zv}IL12+#-k!{<`OgG%8lM^Y4Xui4M`B)z&( zl+DkBSClfH+WN{it`e38%O6Yj_!zXO5pefiQ?j3kaY{ZEZ>=w9`s9;v>f!A8;l^`c zk_4VpyN1_hV@o8uDhD}UM)ufH>sXDGEo(Sc8^gOw9Lw`Q;~j;}&n1q-cZ~W|5U+P3 z&bs`7tv6;(L<#ISIo%yNLK9EbPn9zVT`Znwe1B$|ev_yo-5(>CM*hO^#jPctH9`5s zX#x4^Yr7Amsn=;Vkll9s>hx5-Ev2)=TEt0J-!1wOQCFZh3(hG#mWt^xB%zDfrF1 zqmdYLmbEXgGv5W8Y9*Y=d;@^DU5zjZ-ptw5IRxE}5SWLy6`T4<&eVC6DvK|Q8H`1Z zs7urn;mx)j5PHMJ#4fku{CqgfsQl8$fkf%>Yaz)!cZx5kexjY&a_~zd1|UM^oSqZ_nYohUCKv64 z08&VFVEr^_3ow3*qoi^ zUS5aJU6akM`xyZOW;~ncd#gtLsvTG6Yb#QyjadAm?|${s-GAOyO^xtPVfF2#Y#sDX zu{w0EY^)xPj+V*qab2-@lZ2h|YWK?)HE=iA)QC%c{z>qaJ{@jBPf=f4yM6YQc8c|W zIh+)>E`ROM_==6zwdOCOT+SB9%jkAdr^GZ2Q#d?mZZu9$ZY)D}oSD$fA z=3n(tPDI~<)#NqOKPCrIv|d3RfVr=*ESt;6NlVUMX#2M=1Ku5Y0nz^e#STSmkC|oeR3vq|4U|s z6!a9Z?&$?H{z#?IWOI*%T0VBa&yX^w&J4-6?u9}DHI*d|c?mt#>2j~Tb{VhN=_yBs z(CD#N9hPD5c5AAl5BIAY-fj3->8Y)d7re8jiF@OcUpEySl~Nc#o9uNvJd)9)%$hQ& zUwXrjBm6nmSux<^P5NY#g)w6_eWrI1zi4gZ)SyeOKuIw7{l&qo2BOi|v-&YZYTw2} znR@okK2;*~q>D{#z5Ibgf_YUO+@Dq6#_8N?<#|8BO}Z<0!Mrip09rdvAFpI<@2)AT zaqiR$L{A|NSytADRK10CvtzAAuP<~-s&>%v6GG=LeG7PNK$cyQ&tA2~(_m~RHspnL zfq-DP;P^xAgA2^MOjI<}If&+O?FObVqNcplE=25q-7%qZI?_6=){)YZkgRUD1>^<=H=?2$$*NhfK)=4MhFTM}m={sw)|Eqw`_=)L6O`WsV>#+rvez>gUgPgSz+Ot3pza@=CA?V?@!G@T`WkTf-(IT-Z&b-Ck6&rN(`US2 z_a@^iK2I_Pezq_0VTxs*60K09X~RPF=o?dQq`F}!c06hB9iH~|SUt18>sH;Th6W*P z$pg+{+|ltU1(k%j#;Z3Z8s;D@Rqa_E}965bORzL}S{ zzagYH^;VrZuN&=6))`PPEh5$?D2^p*c(S9osH(yGAg!^=fZnXMN|C;+JtC*3V^6Y1 zvdpiCnuGuQni~rUrKzoG{?Cc|rnAy36s6)t==?dsN&5#q4v!YmAT%HZdk6p# zu&AgcP!tRV!$f|SCyeUo{4VW+`{_l3KSjI{t|A~|Q4we7KS|*5DmVX_@9#?Bj0tb7 zB8C_o*25izQMrk6!Snt)sjHI*?$A9_2XZA1u4{h0Yz88r z?D|9E1pO-#jrtwu>f!G61A|71V4N_{1VwNJX3(GTczc^a0`w<)PDcKVB82IF=l>J> zZ~pqB%MZUwDPvI{CzEO@%krG)D}~0Q?9oy`K7yfO8zc$?0g6l5fPfGTAv`c~FcOGH zNkVNf5;iC(82k&Bh6@gla6w^Cs0if3_5>cZ1i}VkgMtAG*^2=}Y@lEu3@Jv4F~|lY z29?A}f-%TnDD>Rz36+3w`c+USeqiyfv;q}LWvAr|K2#+{%8c3Y*4FN&K z#3e<=#KfR~fUaTOafBp4;RJ~a|Fj#8l2RowA_&Q5?~Jg;h`74g{t!6{ixj~af@2XU zwLxJ1(NC~N3hs_U;IZz;SgeyQ&xx*FCzL-*ic98au}EoSQ9lHKfHCNkdiz;%N(ft# zA59sNe+2$7Oh$HCPnZ84&mYj=Srpyzo>+HBJ$F5%0|tfv$2@-z{5z8&VXedA-Muyb z2c!NIPUg3CRVT<|-MxRA-w5OOTkE$4$;tkQtGKv+ECNyp)Nk&`A#P&OKSDt8<8MPK zJA{iZhOpZHEVaM&+y9+328p8}2tpMA#U*XTfDi;B_awz2P#{WD42h6LN+2N+i9fUB zur_#4ggZvjmXHI4I1@_iN1VAX|HwXpKchYEFoYbrA}S^&D$4U)cz=fbUt~dDhZQ-iXx$~KWq5^+a4GMEG7ZMz<}Ziae_TauniE2 zhDZRxNDNd`90@`Z){K8Adn7Rkqy%Ak1cGfO5rkz^k`Ny-gwRBZf+f%}6hcA*^KWGj z7%T=xN}zyHG#CPeKnV4N6qi5%MKNM%hz-&PEe=NgJK0017=rpRFi=v0a4>=p_Cg>` z3@Q#3lf*!f;xMQL0{Wj;oB!YS1A>B(=#w2)!Uh8+)Q=4sh!m5A01@H{1PCOFfk{Xr z{;%UBBXV+J{M+&^b8@88(UJOfftNWsa!F~N?5su}uC7k@823Lf{D0dB{~O#d@;^7- z|4IB;*l*HstgAQSP-2JI^K|)#)BhddZw%V@D2xjZ`;S!r74n-~emPeYbpEZ6a9$@I z|3!X3|Nq?fPEH2@!mpp(>A!FW0`*^${8Rq@%UyrD>!0$#KL!3PyZ&<5Kjndc3j9}g z{hzsu{Er(%j0@o(S OK\nU maxspeed=OK +mesg U maxspeed 12800 -> OK\nU maxspeed=OK +register V 0x582 stepper -> OK\nV maxspeed=OK +mesg V maxspeed 12800 -> OK\nV maxspeed=OK +mesg U relmove 1600 -> OK\nU rotdir=OK\nU relsteps=OK +mesg U setzero -> OK\nU curpos=OK +mesg V setzero -> OK\nV curpos=OK diff --git a/LocCorr_new/CMakeLists.txt b/LocCorr_new/CMakeLists.txt new file mode 100644 index 0000000..859912f --- /dev/null +++ b/LocCorr_new/CMakeLists.txt @@ -0,0 +1,89 @@ +cmake_minimum_required(VERSION 3.0) +set(PROJ loccorr) +set(MINOR_VERSION "1") +set(MID_VERSION "0") +set(MAJOR_VERSION "0") +set(VERSION "${MAJOR_VERSION}.${MID_VERSION}.${MINOR_VERSION}") + +project(${PROJ} VERSION ${VERSION} LANGUAGES C) + +# default flags +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=gnu99") + +set(CMAKE_COLOR_MAKEFILE ON) + +# here is one of two variants: all .c in directory or .c files in list +aux_source_directory(${CMAKE_CURRENT_SOURCE_DIR} SOURCES) + +set(CMAKE_VERBOSE_MAKEFILE "ON") + +# list of options +option(DEBUG "Compile in debug mode" OFF) + +# default flags +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -W -Wextra -std=gnu99") +set(CMAKE_COLOR_MAKEFILE ON) + +# cmake -DDEBUG=yes -> debugging +if(DEBUG) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Og -g3 -ggdb -fno-builtin-strlen -Werror") + add_definitions(-DEBUG) + set(CMAKE_BUILD_TYPE DEBUG) + set(CMAKE_VERBOSE_MAKEFILE "ON") +else() + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3 -march=native -fdata-sections -ffunction-sections") + SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--gc-sections") + set(CMAKE_BUILD_TYPE RELEASE) +endif() + +message("Build type: ${CMAKE_BUILD_TYPE}") + +###### pkgconfig ###### +# pkg-config modules (for pkg-check-modules) +set(MODULES usefull_macros cfitsio improc) + +# find packages: +SET(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}) +find_package(PkgConfig REQUIRED) +find_package(FLYCAP REQUIRED) +find_package(BASLER REQUIRED) +pkg_check_modules(${PROJ} REQUIRED ${MODULES}) + +include(FindOpenMP) +if(OPENMP_FOUND) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") + add_definitions(-DOMP_FOUND) +endif() +if(NOT DEFINED PROCESSOR_COUNT) + set(PROCESSOR_COUNT 2) # by default 2 cores + execute_process(COMMAND getconf _NPROCESSORS_ONLN OUTPUT_VARIABLE PROCESSOR_COUNT OUTPUT_STRIP_TRAILING_WHITESPACE) +endif() +message("In multithreaded operations will use ${PROCESSOR_COUNT} threads") + + +# change wrong behaviour with install prefix +if(CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT AND CMAKE_INSTALL_PREFIX MATCHES "/usr/local") +else() + message("Change default install path to /usr/local") + set(CMAKE_INSTALL_PREFIX "/usr/local") +endif() +message("Install dir prefix: ${CMAKE_INSTALL_PREFIX}") + +# exe file +add_executable(${PROJ} ${SOURCES}) +# -I +include_directories(${${PROJ}_INCLUDE_DIRS} ${FLYCAP_INCLUDE_DIRS} ${BASLER_INCLUDE_DIRS}) +# -L +link_directories(${${PROJ}_LIBRARY_DIRS} ${FLYCAP_LIBRARY_DIRS} ${BASLER_LIBRARY_DIRS}) +# -D +add_definitions(${CFLAGS} -DLOCALEDIR=\"${LOCALEDIR}\" + -DPACKAGE_VERSION=\"${VERSION}\" -DGETTEXT_PACKAGE=\"${PROJ}\" + -DMINOR_VERSION=\"${MINOR_VERSION}\" -DMID_VERSION=\"${MID_VERSION}\" + -DMAJOR_VERSION=\"${MAJOR_VERSION}\" -DTHREAD_NUMBER=${PROCESSOR_COUNT}) + +# -l +target_link_libraries(${PROJ} ${${PROJ}_LIBRARIES} ${FLYCAP_LIBRARIES} ${BASLER_LIBRARIES} -lm) + +# Installation of the program +INSTALL(TARGETS ${PROJ} DESTINATION "bin") diff --git a/LocCorr_new/DEBUG.log.analyze b/LocCorr_new/DEBUG.log.analyze new file mode 100755 index 0000000..4803787 --- /dev/null +++ b/LocCorr_new/DEBUG.log.analyze @@ -0,0 +1,9 @@ +#!/bin/sh + +ALLOC=$(grep ALLOCSZ DEBUG.log) +FREE=$(grep FREESZ DEBUG.log) + +echo "Total: $(echo "$ALLOC" | wc -l) allocations and $(echo "$FREE" | wc -l) frees" +AL=$(echo "$ALLOC" | sed 's/ALLOCSZ(\([[:digit:]]*\))/\1/' | awk 'BEGIN{x=0;} {x=x+$1;} END{print x;}') +FR=$(echo "$FREE" | sed 's/FREESZ(\([[:digit:]]*\))/\1/' | awk 'BEGIN{x=0;} {x=x+$1;} END{print x;}') +echo "Allocated: $AL bytes, Freed: $FR bytes, difference: $((AL - FR)) bytes" diff --git a/LocCorr_new/FindBASLER.cmake b/LocCorr_new/FindBASLER.cmake new file mode 100644 index 0000000..5324401 --- /dev/null +++ b/LocCorr_new/FindBASLER.cmake @@ -0,0 +1,36 @@ +# - Try to find libpylonc.so +# Once done this will define +# +# BASLER_FOUND - system has lib +# BASLER_INCLUDE_DIR - include directory +# BASLER_LIBRARIES - Link these to use lib + +# Copyright (c) 2021, Edward V. Emelianov +# +# Redistribution and use is allowed according to the terms of the GPLv2/GPLv3. + +include(GNUInstallDirs) + +find_path(BASLER_INCLUDE_DIR pylonc/PylonC.h + PATHS /usr/pylon/include /opt/pylon/include /usr/local/pylon/include /usr/include /usr/local/include /opt/include /opt/local/include +) +find_path(BASLER_LIBRARY_DIR libpylonc.so + PATHS /usr/pylon/lib /opt/pylon/lib /usr/local/pylon/lib /lib /lib64 /usr/lib /usr/lib64 /opt/lib /opt/lib64 /usr/local/lib /usr/local/lib64 +) +find_library(BASLER_LIBRARY NAMES pylonc + PATHS /opt/pylon/lib /usr/pylon/lib /usr/local/pylon/lib /lib /lib64 /usr/lib /usr/lib64 /opt/lib /opt/lib64 /usr/local/lib /usr/local/lib64 +) + +find_package_handle_standard_args(BASLER DEFAULT_MSG BASLER_INCLUDE_DIR BASLER_LIBRARY BASLER_LIBRARY_DIR) + +if(BASLER_FOUND) + set(BASLER_INCLUDE_DIRS ${BASLER_INCLUDE_DIR}) + set(BASLER_LIBRARIES ${BASLER_LIBRARY}) + set(BASLER_LIBRARY_DIRS ${BASLER_LIBRARY_DIR}) +# message("BASLER include dir = ${BASLER_INCLUDE_DIRS}") +# message("BASLER lib = ${BASLER_LIBRARIES}") +# message("BASLER libdir = ${BASLER_LIBRARY_DIRS}") + mark_as_advanced(BASLER_INCLUDE_DIRS BASLER_LIBRARIES BASLER_LIBRARY_DIRS) +else() + message("BASLER not found") +endif(BASLER_FOUND) diff --git a/LocCorr_new/FindFLYCAP.cmake b/LocCorr_new/FindFLYCAP.cmake new file mode 100644 index 0000000..db436f9 --- /dev/null +++ b/LocCorr_new/FindFLYCAP.cmake @@ -0,0 +1,40 @@ +# - Try to find libflycapture +# Once done this will define +# +# FLYCAP_FOUND - system has libflycapture +# FLYCAP_INCLUDE_DIR - include directory +# FLYCAP_LIBRARIES - Link these to use libflycapture + +# Copyright (c) 2021, Edward V. Emelianov +# +# Redistribution and use is allowed according to the terms of the GPLv2/GPLv3. + +include(GNUInstallDirs) + +find_path(FLYCAP_INCLUDE_DIR FlyCapture2.h + PATH_SUFFIXES libflycapture flycapture + PATHS /usr/include /usr/local/include /opt/include /opt/local/include +) +find_path(FLYCAP_LIBRARY_DIR libflycapture.so + PATHS /lib /lib64 /usr/lib /usr/lib64 /opt/lib /opt/lib64 /usr/local/lib /usr/local/lib64 +) +find_library(FLYCAP_LIBRARY NAMES flycapture + PATHS /lib /lib64 /usr/lib /usr/lib64 /opt/lib /opt/lib64 /usr/local/lib /usr/local/lib64 +) +find_library(FLYCAP_LIBRARYC NAMES flycapture-c + PATHS /lib /lib64 /usr/lib /usr/lib64 /opt/lib /opt/lib64 /usr/local/lib /usr/local/lib64 +) + +find_package_handle_standard_args(FLYCAP DEFAULT_MSG FLYCAP_INCLUDE_DIR FLYCAP_LIBRARY FLYCAP_LIBRARYC FLYCAP_LIBRARY_DIR) + +if(FLYCAP_FOUND) + set(FLYCAP_INCLUDE_DIRS ${FLYCAP_INCLUDE_DIR}) + set(FLYCAP_LIBRARIES ${FLYCAP_LIBRARY} ${FLYCAP_LIBRARYC}) + set(FLYCAP_LIBRARY_DIRS ${FLYCAP_LIBRARY_DIR}) +# message("FLYCAP include dir = ${FLYCAP_INCLUDE_DIRS}") +# message("FLYCAP lib = ${FLYCAP_LIBRARIES}") +# message("FLYCAP libdir = ${FLYCAP_LIBRARY_DIRS}") + mark_as_advanced(FLYCAP_INCLUDE_DIRS FLYCAP_LIBRARIES FLYCAP_LIBRARY_DIRS) +else() + message("FLYCAP not found") +endif(FLYCAP_FOUND) diff --git a/LocCorr_new/Readme.md b/LocCorr_new/Readme.md new file mode 100644 index 0000000..4c4cc21 --- /dev/null +++ b/LocCorr_new/Readme.md @@ -0,0 +1,6 @@ +# Local corrector + +Takes fresh images from CMOS, single file or directory using inotify, find objects, calculate their centroids and send messages to corrector over serial-proxy. + +Used corrector controller - `multistepper`. +Used CMOS - Basler or Grasshopper. \ No newline at end of file diff --git a/LocCorr_new/basler.c b/LocCorr_new/basler.c new file mode 100644 index 0000000..bd65759 --- /dev/null +++ b/LocCorr_new/basler.c @@ -0,0 +1,335 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include + +#include "basler.h" +#include "debug.h" +#include "imagefile.h" + +static PYLON_DEVICE_HANDLE hDev; +static int isopened = FALSE; +static size_t payloadsize = 0; // size of imgBuf +static unsigned char *imgBuf = NULL; +static float expostime = 0.; // current exposition time +static PYLON_DEVICECALLBACK_HANDLE hCb; + +typedef struct{ + int64_t min; + int64_t max; + int64_t incr; + int64_t val; +} int64_values; + +typedef struct{ + double min; + double max; + double val; +} float_values; + +static char* describeError(GENAPIC_RESULT reserr){ + static char buf[1024]; + char* errMsg, *bptr = buf; + size_t length, l = 1023; + GenApiGetLastErrorMessage(NULL, &length); + errMsg = MALLOC(char, length); + GenApiGetLastErrorMessage(errMsg, &length); + size_t ll = snprintf(bptr, l, "%s (%d); ", errMsg, (int)reserr); + if(ll > 0){l -= ll; bptr += ll;} + FREE(errMsg); + GenApiGetLastErrorDetail(NULL, &length); + errMsg = MALLOC(char, length); + GenApiGetLastErrorDetail(errMsg, &length); + snprintf(bptr, l, "%s", errMsg); + FREE(errMsg); + return buf; +} + +#define PYLONFN(fn, ...) do{register GENAPIC_RESULT reserr; if(GENAPI_E_OK != (reserr=fn(__VA_ARGS__))){ \ + WARNX(#fn "(): %s", describeError(reserr)); return FALSE;}}while(0) + +static void disconnect(){ + FNAME(); + if(!isopened) return; + FREE(imgBuf); + PylonDeviceDeregisterRemovalCallback(hDev, hCb); + PylonDeviceClose(hDev); + PylonDestroyDevice(hDev); + PylonTerminate(); + isopened = FALSE; +} + +/** + * @brief chkNode - get node & check it for read/write + * @param phNode (io) - pointer to node + * @param nodeType - type of node + * @param wr - ==TRUE if need to check for writeable + * @return TRUE if node found & checked + */ +static int chkNode(NODE_HANDLE *phNode, const char *featureName, EGenApiNodeType nodeType, int wr){ + if(!isopened || !phNode || !featureName) return FALSE; + NODEMAP_HANDLE hNodeMap; + EGenApiNodeType nt; + _Bool bv; + PYLONFN(PylonDeviceGetNodeMap, hDev, &hNodeMap); + PYLONFN(GenApiNodeMapGetNode, hNodeMap, featureName, phNode); + if(*phNode == GENAPIC_INVALID_HANDLE) return FALSE; + PYLONFN(GenApiNodeGetType, *phNode, &nt); + if(nodeType != nt) return FALSE; + PYLONFN(GenApiNodeIsReadable, *phNode, &bv); + if(!bv) return FALSE; // not readable + if(wr){ + PYLONFN(GenApiNodeIsWritable, *phNode, &bv); + if(!bv) return FALSE; // not writeable + } + return TRUE; +} + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-function" + +// getters of different types of data +static int getBoolean(const char *featureName, _Bool *val){ + if(!isopened || !featureName) return FALSE; + NODE_HANDLE hNode; + if(!chkNode(&hNode, featureName, BooleanNode, FALSE)) return FALSE; + if(!val) return TRUE; + PYLONFN(GenApiBooleanGetValue, hNode, val); + //DBG("Get boolean: %s = %s", featureName, val ? "true" : "false"); + return TRUE; +} +static int getInt(char *featureName, int64_values *val){ + if(!isopened || !featureName) return FALSE; + NODE_HANDLE hNode; + if(!chkNode(&hNode, featureName, IntegerNode, FALSE)) return FALSE; + if(!val) return TRUE; + PYLONFN(GenApiIntegerGetMin, hNode, &val->min); + PYLONFN(GenApiIntegerGetMax, hNode, &val->max); + PYLONFN(GenApiIntegerGetInc, hNode, &val->incr); + PYLONFN(GenApiIntegerGetValue, hNode, &val->val); + //DBG("Get integer %s = %ld: min = %ld, max = %ld, incr = %ld", featureName, val->val, val->min, val->max, val->incr); + return TRUE; +} +static int getFloat(char *featureName, float_values *val){ + if(!isopened || !featureName) return FALSE; + NODE_HANDLE hNode; + if(!chkNode(&hNode, featureName, FloatNode, FALSE)) return FALSE; + if(!val) return TRUE; + PYLONFN(GenApiFloatGetMin, hNode, &val->min); + PYLONFN(GenApiFloatGetMax, hNode, &val->max); + PYLONFN(GenApiFloatGetValue, hNode, &val->val); + //DBG("Get float %s = %g: min = %g, max = %g", featureName, val->val, val->min, val->max); + return TRUE; +} + +// setters of different types of data +static int setBoolean(char *featureName, _Bool val){ + if(!isopened || !featureName) return FALSE; + NODE_HANDLE hNode; + if(!chkNode(&hNode, featureName, BooleanNode, TRUE)) return FALSE; + PYLONFN(GenApiBooleanSetValue, hNode, val); + return TRUE; +} +static int setInt(char *featureName, int64_t val){ + if(!isopened || !featureName) return FALSE; + NODE_HANDLE hNode; + if(!chkNode(&hNode, featureName, IntegerNode, TRUE)) return FALSE; + PYLONFN(GenApiIntegerSetValue, hNode, val); + return TRUE; +} +static int setFloat(char *featureName, float val){ + if(!isopened || !featureName) return FALSE; + NODE_HANDLE hNode; + if(!chkNode(&hNode, featureName, FloatNode, TRUE)) return FALSE; + PYLONFN(GenApiFloatSetValue, hNode, val); + return TRUE; +} +#pragma GCC diagnostic pop + +static void disableauto(){ + if(!isopened) return; + const char *features[] = {"EnumEntry_TriggerSelector_AcquisitionStart", + "EnumEntry_TriggerSelector_FrameBurstStart", + "EnumEntry_TriggerSelector_FrameStart"}; + const char *triggers[] = {"AcquisitionStart", "FrameBurstStart", "FrameStart"}; + for(int i = 0; i < 3; ++i){ + if(PylonDeviceFeatureIsAvailable(hDev, features[i])){ + PylonDeviceFeatureFromString(hDev, "TriggerSelector", triggers[i]); + PylonDeviceFeatureFromString(hDev, "TriggerMode", "Off"); + } + } + PylonDeviceFeatureFromString(hDev, "GainAuto", "Off"); + PylonDeviceFeatureFromString(hDev, "ExposureAuto", "Off"); + PylonDeviceFeatureFromString(hDev, "ExposureMode", "Timed"); + PylonDeviceFeatureFromString(hDev, "SequencerMode", "Off"); +} + +static void GENAPIC_CC removalCallbackFunction(_U_ PYLON_DEVICE_HANDLE hDevice){ + disconnect(); +} + +static int connect(){ + FNAME(); + size_t numDevices; + disconnect(); + PylonInitialize(); + PYLONFN(PylonEnumerateDevices, &numDevices); + if(!numDevices){ + WARNX("No cameras found"); + return FALSE; + } + PYLONFN(PylonCreateDeviceByIndex, 0, &hDev); + isopened = TRUE; + PYLONFN(PylonDeviceOpen, hDev, PYLONC_ACCESS_MODE_CONTROL | PYLONC_ACCESS_MODE_STREAM | PYLONC_ACCESS_MODE_EXCLUSIVE); + disableauto(); + PYLONFN(PylonDeviceFeatureFromString, hDev, "PixelFormat", "Mono8"); + PYLONFN(PylonDeviceFeatureFromString, hDev, "CameraOperationMode", "LongExposure"); + PYLONFN(PylonDeviceFeatureFromString, hDev, "UserSetSelector", "HighGain"); // set high gain selector + PYLONFN(PylonDeviceFeatureFromString, hDev, "AcquisitionMode", "SingleFrame"); + PYLONFN(PylonDeviceExecuteCommandFeature, hDev, "UserSetLoad"); // load high gain mode + PYLON_STREAMGRABBER_HANDLE hGrabber; + PYLONFN(PylonDeviceGetStreamGrabber, hDev, 0, &hGrabber); + PYLONFN(PylonStreamGrabberOpen, hGrabber); +// PYLON_WAITOBJECT_HANDLE hWaitStream; +// PYLONFN(PylonStreamGrabberGetWaitObject, hStreamGrabber, &hWaitStream); + PYLONFN(PylonStreamGrabberGetPayloadSize, hDev, hGrabber, &payloadsize); + PylonStreamGrabberClose(hGrabber); + PylonDeviceRegisterRemovalCallback(hDev, removalCallbackFunction, &hCb); + imgBuf = MALLOC(unsigned char, payloadsize); + //PYLONFN(PylonDeviceExecuteCommandFeature, hDev, "AcquisitionStart"); + return TRUE; +} + +static Image *capture(){ + FNAME(); + static int toohot = FALSE; + if(!isopened || !imgBuf) return NULL; + float_values f; + if(!getFloat("DeviceTemperature", &f)) WARNX("Can't get temperature"); + else{ + LOGDBG("Basler temperature: %.1f", f.val); + DBG("Temperature: %.1f", f.val); + if(f.val > 80.){ + WARNX("Device too hot"); + if(!toohot){ + LOGWARN("Device too hot"); + toohot = TRUE; + } + }else if(toohot && f.val < 75.){ + LOGDBG("Device temperature is normal"); + toohot = FALSE; + } + } + PylonGrabResult_t grabResult; + _Bool bufferReady; + GENAPIC_RESULT res = PylonDeviceGrabSingleFrame(hDev, 0, imgBuf, payloadsize, + &grabResult, &bufferReady, 500 + (uint32_t)expostime); + if(res != GENAPI_E_OK || !bufferReady){ + WARNX("res != GENAPI_E_OK || !bufferReady"); + return NULL; + } + if(grabResult.Status != Grabbed){ + WARNX("grabResult.Status != Grabbed"); + return NULL; + } + Image *oIma = u8toImage(imgBuf, grabResult.SizeX, grabResult.SizeY, grabResult.SizeX + grabResult.PaddingX); + return oIma; +} + +// Basler have no "brightness" parameter +static int setbrightness(_U_ float b){ + FNAME(); + return TRUE; +} + +static int setexp(float e){ + FNAME(); + if(!isopened) return FALSE; + e *= 1000.; + if(!setFloat("ExposureTime", e)){ + LOGWARN("Can't set expose time %g", e); + WARNX("Can't set expose time %g", e); + return FALSE; + } + float_values f; + if(!getFloat("ExposureTime", &f)) return FALSE; + expostime = (float)f.val / 1000.; + return TRUE; +} + +static int setgain(_U_ float e){ + FNAME(); + if(!isopened) return FALSE; + if(!setFloat("Gain", e)){ + LOGWARN("Can't set gain %g", e); + WARNX("Can't set gain %g", e); + return FALSE; + } + return TRUE; +} + +static int changeformat(frameformat *fmt){ + FNAME(); + if(!isopened) return FALSE; + setInt("Width", fmt->w); + setInt("Height", fmt->h); + setInt("OffsetX", fmt->xoff); + setInt("OffsetY", fmt->yoff); + int64_values i; + if(getInt("Width", &i)) fmt->w = i.val; + if(getInt("Height", &i)) fmt->h = i.val; + if(getInt("OffsetX", &i)) fmt->xoff = i.val; + if(getInt("OffsetY", &i)) fmt->yoff = i.val; + return TRUE; +} + +static int geometrylimits(frameformat *max, frameformat *step){ + FNAME(); + if(!isopened || !max || !step) return FALSE; + int64_values i; + if(!getInt("Width", &i)) return FALSE; + max->w = i.max; step->w = i.incr; + if(!getInt("Height", &i)) return FALSE; + max->h = i.max; step->h = i.incr; + if(!getInt("OffsetX", &i)) return FALSE; + max->xoff = i.max; step->xoff = i.incr; + if(!getInt("OffsetY", &i)) return FALSE; + max->yoff = i.max; step->yoff = i.incr; + return TRUE; +} + +static float gainmax(){ + FNAME(); + float_values v; + if(!getFloat("Gain", &v)) return 0.; + return (float)v.max; +} + +// exported object +camera Basler = { + .disconnect = disconnect, + .connect = connect, + .capture = capture, + .setbrightness = setbrightness, + .setexp = setexp, + .setgain = setgain, + .setgeometry = changeformat, + .getgeomlimits = geometrylimits, + .getmaxgain = gainmax, +}; + diff --git a/LocCorr_new/basler.h b/LocCorr_new/basler.h new file mode 100644 index 0000000..42e2f4d --- /dev/null +++ b/LocCorr_new/basler.h @@ -0,0 +1,27 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#pragma once +#ifndef BASLER_H__ +#define BASLER_H__ + +#include "cameracapture.h" // `camera` +#define BASLER_CAPT_NAME "basler" + +extern camera Basler; + +#endif // BASLER_H__ diff --git a/LocCorr_new/binmorph.c b/LocCorr_new/binmorph.c new file mode 100644 index 0000000..f322a23 --- /dev/null +++ b/LocCorr_new/binmorph.c @@ -0,0 +1,500 @@ +/* + * binmorph.c - functions for morphological operations on binary image + * + * Copyright 2015 Edward V. Emelianoff + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + */ + +#include +#include +#include // memcpy +#include +#include + +#include "binmorph.h" +#include "debug.h" +#include "imagefile.h" + +// global arrays for erosion/dilation masks +static uint8_t *ER = NULL, *DIL = NULL; + +/* + * =================== AUXILIARY FUNCTIONS ===================> + */ + +/** + * This function inits masks arrays for erosion and dilation + * You may call it yourself or it will be called when one of + * `erosion` or `dilation` functions will be ran first time + */ +static void morph_init(){ + if(ER) return; + ER = MALLOC(uint8_t, 256); + DIL = MALLOC(uint8_t, 256); + OMP_FOR() + for(int i = 0; i < 256; i++){ + ER[i] = i & ((i << 1) | 1) & ((i >> 1) | (0x80)); // don't forget that << and >> set borders to zero + DIL[i] = i | (i << 1) | (i >> 1); + } +} + +/* + * =================== MORPHOLOGICAL OPERATIONS ===================> + */ + +/** + * Remove all non-4-connected pixels + * @param image (i) - input image + * @param W, H - size of binarized image (in pixels) + * @return allocated memory area with converted input image + */ +uint8_t *il_filter4(uint8_t *image, int W, int H){ + //FNAME(); + if(W < MINWIDTH || H < MINHEIGHT) return NULL; + uint8_t *ret = MALLOC(uint8_t, W*H); + int W0 = (W + 7) / 8; // width in bytes + int w = W0-1, h = H-1; + { + // top of image, y = 0 + #define IM_UP + #include "fc_filter.h" + #undef IM_UP + } + { + // mid of image, y = 1..h-1 + #include "fc_filter.h" + } + { + // image bottom, y = h + #define IM_DOWN + #include "fc_filter.h" + #undef IM_DOWN + } + return ret; +} + +/** + * Remove all non-8-connected pixels (single points) + * @param image (i) - input image + * @param W, H - size of binarized image (in pixels) + * @return allocated memory area with converted input image + */ +uint8_t *il_filter8(uint8_t *image, int W, int H){ + //FNAME(); + if(W < MINWIDTH || H < MINHEIGHT) return NULL; + uint8_t *ret = MALLOC(uint8_t, W*H); + int W0 = (W + 7) / 8; // width in bytes + int w = W0-1, h = H-1; + { + #define IM_UP + #include "ec_filter.h" + #undef IM_UP + } + { + #include "ec_filter.h" + } + { + #define IM_DOWN + #include "ec_filter.h" + #undef IM_DOWN + } + return ret; +} + +/** + * Make morphological operation of il_dilation + * @param image (i) - input image + * @param W, H - size of image (pixels) + * @return allocated memory area with il_dilation of input image + */ +uint8_t *il_dilation(uint8_t *image, int W, int H){ + //FNAME(); + if(W < MINWIDTH || H < MINHEIGHT) return NULL; + int W0 = (W + 7) / 8; // width in bytes + int w = W0-1, h = H-1, rest = 7 - (W - w*8); + uint8_t lastmask = ~(1< + */ +#if 0 +/** + * Logical AND of two images + * @param im1, im2 (i) - two images + * @param W, H - their size (of course, equal for both images) + * @return allocated memory area with image = (im1 AND im2) + */ +uint8_t *imand(uint8_t *im1, uint8_t *im2, int W, int H){ + uint8_t *ret = MALLOC(uint8_t, W*H); + int y; + OMP_FOR() + for(y = 0; y < H; y++){ + int x, S = y*W; + uint8_t *rptr = &ret[S], *p1 = &im1[S], *p2 = &im2[S]; + for(x = 0; x < W; x++) + *rptr++ = *p1++ & *p2++; + } + return ret; +} + +/** + * Substitute image 2 from image 1: reset to zero all bits of image 1 which set to 1 on image 2 + * @param im1, im2 (i) - two images + * @param W, H - their size (of course, equal for both images) + * @return allocated memory area with image = (im1 AND (!im2)) + */ +uint8_t *substim(uint8_t *im1, uint8_t *im2, int W, int H){ + uint8_t *ret = MALLOC(uint8_t, W*H); + int y; + OMP_FOR() + for(y = 0; y < H; y++){ + int x, S = y*W; + uint8_t *rptr = &ret[S], *p1 = &im1[S], *p2 = &im2[S]; + for(x = 0; x < W; x++) + *rptr++ = *p1++ & (~*p2++); + } + return ret; +} +#endif +/* + * <=================== LOGICAL OPERATIONS =================== + */ + +/* + * =================== CONNECTED COMPONENTS LABELING ===================> + */ + +//#define TESTMSGS +#ifdef TESTMSGS +#define TEST(...) printf(__VA_ARGS__) +#else +#define TEST(...) +#endif + +// check table and rename all "oldval" into "newval" +static inline void remark(size_t newval, size_t oldval, size_t *assoc){ + TEST("\tnew = %zd, old=%zd; ", newval, oldval); + // find the least values + do{newval = assoc[newval];}while(assoc[newval] != newval); + do{oldval = assoc[oldval];}while(assoc[oldval] != oldval); + TEST("\trealnew = %zd, realold=%zd ", newval, oldval); + // now change larger value to smaller + if(newval > oldval){ + assoc[newval] = oldval; + TEST("change %zd to %zd\n", newval, oldval); + }else{ + assoc[oldval] = newval; + TEST("change %zd to %zd\n", oldval, newval); + } +} + +/** + * label 4-connected components on image + * (slow algorythm, but easy to parallel) + * + * @param I (i) - image ("packed") + * @param W,H - size of the image (W - width in pixels) + * @param CC (o) - connected components boxes + * @return an array of labeled components + */ +size_t *il_cclabel4(uint8_t *Img, int W, int H, il_ConnComps **CC){ + size_t *assoc; + if(W < MINWIDTH || H < MINHEIGHT) return NULL; + uint8_t *f = il_filter4(Img, W, H); // remove all non 4-connected pixels + //DBG("convert to size_t"); + size_t *labels = bin2ST(f, W, H); + FREE(f); + //DBG("Calculate"); + size_t Nmax = W*H/4; // max number of 4-connected labels + assoc = MALLOC(size_t, Nmax); // allocate memory for "remark" array + size_t last_assoc_idx = 1; // last index filled in assoc array + for(int y = 0; y < H; ++y){ + bool found = false; + size_t *ptr = &labels[y*W]; + size_t curmark = 0; // mark of pixel to the left + for(int x = 0; x < W; ++x, ++ptr){ + if(!*ptr){found = false; continue;} // empty pixel + size_t U = (y) ? ptr[-W] : 0; // upper mark + if(found){ // there's a pixel to the left + if(U && U != curmark){ // meet old mark -> remark one of them in assoc[] + TEST("(%d, %d): remark %zd --> %zd\n", x, y, U, curmark); + remark(U, curmark, assoc); + curmark = U; // change curmark to upper mark (to reduce further checks) + } + }else{ // new mark -> change curmark + found = true; + if(U) curmark = U; // current mark will copy upper value + else{ // current mark is new value + curmark = last_assoc_idx++; + assoc[curmark] = curmark; + TEST("(%d, %d): new mark=%zd\n", x, y, curmark); + } + } + *ptr = curmark; + } + } + size_t *indexes = MALLOC(size_t, last_assoc_idx); // new indexes + size_t cidx = 1; + TEST("\n\n\nRebuild indexes\n\n"); + for(size_t i = 1; i < last_assoc_idx; ++i){ + TEST("%zd\t%zd ",i,assoc[i]); + // search new index + register size_t realnew = i, newval = 0; + do{ + realnew = assoc[realnew]; + TEST("->%zd ", realnew); + if(indexes[realnew]){ // find least index + newval = indexes[realnew]; + TEST("real: %zd ", newval); + break; + } + }while(assoc[realnew] != realnew); + if(newval){ + TEST(" ==> %zd\n", newval); + indexes[i] = newval; + continue; + } + TEST("new index %zd\n", cidx); + // enter next label + indexes[i] = cidx++; + } + // cidx now is amount of detected objects + 1 - size of output array (0th idx is not used) + //DBG("amount after rebuild: %zd", cidx-1); + #ifdef TESTMSGS + printf("\n\n\nI\tASS[I]\tIDX[I]\n"); + for(size_t i = 1; i < last_assoc_idx; ++i) + printf("%zd\t%zd\t%zd\n",i,assoc[i],indexes[i]); + #endif + il_Box *boxes = MALLOC(il_Box, cidx); + OMP_FOR() + for(size_t i = 1; i < cidx; ++i){ // init borders + boxes[i].xmin = W; + boxes[i].ymin = H; + } +#pragma omp parallel shared(boxes) + { + il_Box *l_boxes = MALLOC(il_Box, cidx); + for(size_t i = 1; i < cidx; ++i){ // init borders + l_boxes[i].xmin = W; + l_boxes[i].ymin = H; + } + #pragma omp for nowait + for(int y = 0; y < H; ++y){ + size_t *lptr = &labels[y*W]; + for(int x = 0; x < W; ++x, ++lptr){ + if(!*lptr) continue; + register size_t mark = indexes[*lptr]; + *lptr = mark; + il_Box *b = &l_boxes[mark]; + ++b->area; + if(b->xmax < x) b->xmax = x; + if(b->xmin > x) b->xmin = x; + if(b->ymax < y) b->ymax = y; + if(b->ymin > y) b->ymin = y; + } + } + #pragma omp critical + for(size_t i = 1; i < cidx; ++i){ + il_Box *ob = &boxes[i], *ib = &l_boxes[i]; + if(ob->xmax < ib->xmax) ob->xmax = ib->xmax; + if(ob->xmin > ib->xmin) ob->xmin = ib->xmin; + if(ob->ymax < ib->ymax) ob->ymax = ib->ymax; + if(ob->ymin > ib->ymin) ob->ymin = ib->ymin; + ob->area += ib->area; + } + FREE(l_boxes); + } + FREE(assoc); + FREE(indexes); +#ifdef TESTMSGS + for(size_t i = 1; i < cidx; ++i){ + printf("%8zd\t%6d\t(%4d..%4d, %4d..%4d)\t%.2f\n", i, boxes[i].area, + boxes[i].xmin, boxes[i].xmax, boxes[i].ymin, boxes[i].ymax, + (1.+boxes[i].xmax-boxes[i].xmin)/(1.+boxes[i].ymax-boxes[i].ymin)); + }printf("\n\n"); +#endif + if(CC){ + *CC = MALLOC(il_ConnComps, 1); + (*CC)->Nobj = cidx; (*CC)->boxes = boxes; + }else{ + FREE(boxes); + } + return labels; +} + +#if 0 +// label 8-connected components, look cclabel4 +size_t *cclabel8(size_t *labels, int W, int H, size_t *Nobj){ + if(W < MINWIDTH || H < MINHEIGHT) return NULL; + #define LABEL_8 + #include "cclabling.h" + #undef LABEL_8 + return labels; +} +#endif + +/* + * <=================== CONNECTED COMPONENTS LABELING =================== + */ + + +/* + * <=================== template ===================> + */ diff --git a/LocCorr_new/binmorph.h b/LocCorr_new/binmorph.h new file mode 100644 index 0000000..381cff0 --- /dev/null +++ b/LocCorr_new/binmorph.h @@ -0,0 +1,64 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once +#ifndef BINMORPH_H__ +#define BINMORPH_H__ + +#include +#include +#include +#include "fits.h" + +// minimal image size for morph operations +#define MINWIDTH (9) +#define MINHEIGHT (3) + +// simple box with given borders +typedef struct{ + uint16_t xmin; + uint16_t xmax; + uint16_t ymin; + uint16_t ymax; + uint32_t area; // total amount of object pixels inside the box +} il_Box; + +typedef struct{ + size_t Nobj; + il_Box *boxes; +} il_ConnComps; + + +// morphological operations: +uint8_t *il_dilation(uint8_t *image, int W, int H); +uint8_t *il_dilationN(uint8_t *image, int W, int H, int N); +uint8_t *il_erosion(uint8_t *image, int W, int H); +uint8_t *il_erosionN(uint8_t *image, int W, int H, int N); +uint8_t *il_openingN(uint8_t *image, int W, int H, int N); +uint8_t *il_closingN(uint8_t *image, int W, int H, int N); +uint8_t *il_topHat(uint8_t *image, int W, int H, int N); +uint8_t *il_botHat(uint8_t *image, int W, int H, int N); + +// clear non 4-connected pixels +uint8_t *il_filter4(uint8_t *image, int W, int H); +// clear single pixels +uint8_t *il_filter8(uint8_t *image, int W, int H); + +size_t *il_cclabel4(uint8_t *Img, int W, int H, il_ConnComps **CC); + +#endif // BINMORPH_H__ diff --git a/LocCorr_new/cameracapture.c b/LocCorr_new/cameracapture.c new file mode 100644 index 0000000..843b780 --- /dev/null +++ b/LocCorr_new/cameracapture.c @@ -0,0 +1,275 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include // FLT_EPSILON +#include +#include +#include + +#include "cmdlnopts.h" +#include "config.h" +#include "debug.h" +#include "fits.h" +#include "grasshopper.h" +#include "imagefile.h" +#include "improc.h" +#include "median.h" + +// pointer to selected camera +static camera *theCam = NULL; + +static float gain = 0., gainmax = 0.; +static float exptime = 100.; +static float brightness = 0.; +static int connected = FALSE; + +static frameformat curformat; +static frameformat maxformat; +static frameformat stepformat; + +static void changeformat(){ + if(!theCam) return; + if(maxformat.h < 1 || maxformat.w < 1){ + WARNX("Bad max format data"); + return; + } + if(stepformat.h < 1 || stepformat.w < 1){ + WARNX("Bad step format data"); + LOGWARN("Bad step format data"); + return; + } + if(stepformat.xoff < 1) stepformat.xoff = 1; + if(stepformat.yoff < 1) stepformat.yoff = 1; + curformat.h = (theconf.height < maxformat.h) ? theconf.height : maxformat.h; + curformat.h -= curformat.h % stepformat.h; + curformat.w = (theconf.width < maxformat.w) ? theconf.width : maxformat.w; + curformat.w -= curformat.w % stepformat.w; + curformat.xoff = (theconf.xoff + curformat.w <= maxformat.w) ? theconf.xoff : maxformat.w - curformat.w; + curformat.xoff -= curformat.xoff % stepformat.xoff; + curformat.yoff = (theconf.yoff + curformat.h <= maxformat.h) ? theconf.yoff : maxformat.h - curformat.h; + curformat.yoff -= curformat.yoff % stepformat.yoff; + if(theCam->setgeometry(&curformat)){ // now we can change config values to real + theconf.height = curformat.h; + theconf.width = curformat.w; + theconf.xoff = curformat.xoff; + theconf.yoff = curformat.yoff; + } +} + +/** + * @brief setCamera - set active camera & initialize it + * @param cptr - pointer to new camera + * @return FALSE if failed + */ +int setCamera(camera *cptr){ + camdisconnect(); + theCam = cptr; + connected = theCam->connect(); + if(!connected) return FALSE; + gainmax = theCam->getmaxgain(); + gain = theconf.gain; + brightness = theconf.brightness; + if(!theCam->getgeomlimits(&maxformat, &stepformat)){ + WARNX("Can't detect camera format limits"); + LOGWARN("Can't detect camera format limits"); + return TRUE; + } + changeformat(); + LOGMSG("Camera connected, max gain: %.1f, max (W,H): (%d,%d)", gainmax, maxformat.w, maxformat.h); + return TRUE; +} + +void camdisconnect(){ + if(!connected) return; + connected = FALSE; + if(theCam) theCam->disconnect(); +} + +static void calcexpgain(float newexp){ + DBG("recalculate exposition: oldexp=%g, oldgain=%g, newexp=%g", exptime, gain, newexp); + while(newexp*1.25 > theconf.minexp){ // increase gain first + if(gain < gainmax - 0.9999){ + gain += 1.; + newexp /= 1.25; + }else break; + } + while(newexp < theconf.minexp){ + if(1.25*newexp < theconf.maxexp && gain > 0.9999){ + gain -= 1.; + newexp *= 1.25; + }else break; + } + if(newexp < theconf.minexp) newexp = theconf.minexp; + else if(newexp > theconf.maxexp) newexp = theconf.maxexp; + exptime = newexp; + DBG("New values: exp=%g, gain=%g", exptime, gain); +} + +//convertedImage.pData, convertedImage.cols, convertedImage.rows, convertedImage.stride +static void recalcexp(Image *I){ + // check if user changed exposition values + if(exptime < theconf.minexp){ + exptime = theconf.minexp; + return; + } + else if(exptime > theconf.maxexp){ + exptime = theconf.maxexp; + return; + } + size_t *histogram = get_histogram(I); + if(!histogram){ + WARNX("Can't calculate histogram"); + return; + } + int idx100; + size_t sum100 = 0; + for(idx100 = 255; idx100 >= 0; --idx100){ + sum100 += histogram[idx100]; + if(sum100 > 100) break; + } + FREE(histogram); + DBG("Sum100=%zd, idx100=%d", sum100, idx100); + if(idx100 > 230 && idx100 < 253) return; // good values + if(idx100 > 253){ // exposure too long + calcexpgain(0.7*exptime); + }else{ // exposure too short + if(idx100 > 5) + calcexpgain(exptime * 230. / (float)idx100); + else + calcexpgain(exptime * 50.); + } +} + +int camcapture(void (*process)(Image*)){ + FNAME(); + static float oldexptime = 0.; + static float oldgain = -1.; + static float oldbrightness = -1.; + Image *oIma = NULL; + while(1){ + if(stopwork){ + DBG("STOP"); + break; + } + if(!theCam){ // wait until camera be powered on + LOGERR("camcapture(): camera not initialized"); + ERRX("Not initialized"); + } + if(!connected){ + DBG("Disconnected"); + connected = theCam->connect(); + sleep(1); + changeformat(); + continue; + } + if(fabsf(oldbrightness - brightness) > FLT_EPSILON){ // new brightness + DBG("Change brightness to %g", brightness); + if(theCam->setbrightness(brightness)){ + oldbrightness = brightness; + }else{ + WARNX("Can't change brightness to %g", brightness); + } + } + if(exptime > theconf.maxexp) exptime = theconf.maxexp; + else if(exptime < theconf.minexp) exptime = theconf.minexp; + if(fabsf(oldexptime - exptime) > FLT_EPSILON){ // new exsposition value + DBG("Change exptime to %.2fms\n", exptime); + if(theCam->setexp(exptime)){ + oldexptime = exptime; + }else{ + WARNX("Can't change exposition time to %gms", exptime); + } + } + if(gain > gainmax) gain = gainmax; + if(fabsf(oldgain - gain) > FLT_EPSILON){ // change gain + DBG("Change gain to %g\n", gain); + if(theCam->setgain(gain)){ + oldgain = gain; + }else{ + WARNX("Can't change gain to %g", gain); + } + } + // change format + if(abs(curformat.h - theconf.height) || abs(curformat.w - theconf.width) || abs(curformat.xoff - theconf.xoff) || abs(curformat.yoff - theconf.yoff)){ + changeformat(); + } + if(!(oIma = theCam->capture())){ + WARNX("Can't grab image"); + camdisconnect(); + continue; + } + if(theconf.expmethod == EXPAUTO) recalcexp(oIma); + else{ + if(fabs(theconf.fixedexp - exptime) > FLT_EPSILON) + exptime = theconf.fixedexp; + if(fabs(theconf.gain - gain) > FLT_EPSILON) + gain = theconf.gain; + if(fabs(theconf.brightness - brightness) > FLT_EPSILON) + brightness = theconf.brightness; + } + if(process){ + if(theconf.medfilt){ + Image *X = get_median(oIma, theconf.medseed); + if(X){ + FREE(oIma->data); + FREE(oIma); + oIma = X; + } + } + process(oIma); + } + FREE(oIma->data); + FREE(oIma); + } + if(oIma){ + FREE(oIma->data); + FREE(oIma); + } + camdisconnect(); + DBG("CAMCAPTURE: out"); + return 1; +} + +/** + * @brief camstatus - return JSON with image status + * @param messageid - value of "messageid" + * @param buf - buffer for string + * @param buflen - length of `buf` + * @return buf + */ +char *camstatus(const char *messageid, char *buf, int buflen){ + if(!buf || buflen < 2) return NULL; + if(!messageid) messageid = "unknown"; + static char *impath = NULL; + if(!impath){ + if(!(impath = realpath(GP->outputjpg, impath))){ + WARN("realpath() (%s)", impath); + impath = strdup(GP->outputjpg); + } + DBG("path: %s", impath); + } + float xc, yc; + getcenter(&xc, &yc); + snprintf(buf, buflen, "{ \"%s\": \"%s\", \"camstatus\": \"%sconnected\", \"impath\": \"%s\", \"imctr\": %llu, " + "\"fps\": %.3f, \"expmethod\": \"%s\", \"exposition\": %g, \"gain\": %g, \"brightness\": %g, " + "\"xcenter\": %.1f, \"ycenter\": %.1f }\n", + MESSAGEID, messageid, connected ? "" : "dis", impath, ImNumber, getFramesPerS(), + (theconf.expmethod == EXPAUTO) ? "auto" : "manual", exptime, gain, brightness, + xc, yc); + return buf; +} diff --git a/LocCorr_new/cameracapture.h b/LocCorr_new/cameracapture.h new file mode 100644 index 0000000..ae02b46 --- /dev/null +++ b/LocCorr_new/cameracapture.h @@ -0,0 +1,53 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#pragma once +#ifndef CAMERACAPTURE_H__ +#define CAMERACAPTURE_H__ + +#include "imagefile.h" // Image + + +// format of single frame +typedef struct{ + int w; int h; // width & height + int xoff; int yoff; // X and Y offset +} frameformat; + +typedef struct{ + void (*disconnect)(); // disconnect & cleanup + int (*connect)(); // connect & init + Image* (*capture)(); // capture an image + // setters: brightness, exptime, gain + int (*setbrightness)(float b); + int (*setexp)(float e); + int (*setgain)(float g); + float (*getmaxgain)(); // get max available gain value + // geometry (if TRUE, all args are changed to suitable values) + int (*setgeometry)(frameformat *fmt); + // get limits of geometry: maximal values and steps + int (*getgeomlimits)(frameformat *max, frameformat *step); + //int (*getgeometry)(frameformat *fmt); +} camera; + +int setCamera(camera *cptr); +void camdisconnect(); +int camcapture(void (*process)(Image *)); +char *camstatus(const char *messageid, char *buf, int buflen); + + +#endif // CAMERACAPTURE_H__ diff --git a/LocCorr_new/cclabling.h b/LocCorr_new/cclabling.h new file mode 100644 index 0000000..e01f3c7 --- /dev/null +++ b/LocCorr_new/cclabling.h @@ -0,0 +1,136 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +//#define TESTMSGS + +#ifdef TESTMSGS +#define TEST(...) printf(__VA_ARGS__) +#else +#define TEST(...) +#endif + +size_t Nmax = W*H/4; // max number of 4-connected labels +size_t *assoc = MALLOC(size_t, Nmax); // allocate memory for "remark" array +size_t last_assoc_idx = 1; // last index filled in assoc array +// was: 35 +// check table and rename all "oldval" into "newval" +inline void remark(size_t newval, size_t oldval){ + TEST("\tnew = %zd, old=%zd; ", newval, oldval); + // find the least values + do{newval = assoc[newval];}while(assoc[newval] != newval); + do{oldval = assoc[oldval];}while(assoc[oldval] != oldval); + TEST("\trealnew = %zd, realold=%zd ", newval, oldval); + // now change larger value to smaller + if(newval > oldval){ + assoc[newval] = oldval; + TEST("change %zd to %zd\n", newval, oldval); + }else{ + assoc[oldval] = newval; + TEST("change %zd to %zd\n", oldval, newval); + } +} + +for(int y = 0; y < H; ++y){ + bool found = false; + size_t *ptr = &labels[y*W]; + size_t curmark = 0; // mark of pixel to the left + for(int x = 0; x < W; ++x, ++ptr){ + if(!*ptr){found = false; continue;} // empty pixel + size_t U = (y) ? ptr[-W] : 0; // upper mark + if(found){ // there's a pixel to the left + if(U && U != curmark){ // meet old mark -> remark one of them in assoc[] + TEST("(%d, %d): remark %zd --> %zd\n", x, y, U, curmark); + remark(U, curmark); + curmark = U; // change curmark to upper mark (to reduce further checks) + } + }else{ // new mark -> change curmark + found = true; + if(U) curmark = U; // current mark will copy upper value + else{ // current mark is new value + curmark = last_assoc_idx++; + assoc[curmark] = curmark; + TEST("(%d, %d): new mark=%zd\n", x, y, curmark); + } + } + *ptr = curmark; + } +} +size_t *indexes = MALLOC(size_t, last_assoc_idx); // new indexes +size_t cidx = 1; +TEST("\n\n\nRebuild indexes\n\n"); +for(size_t i = 1; i < last_assoc_idx; ++i){ + TEST("%zd\t%zd ",i,assoc[i]); + // search new index + register size_t realnew = i, newval = 0; + do{ + realnew = assoc[realnew]; + TEST("->%zd ", realnew); + if(indexes[realnew]){ // find least index + newval = indexes[realnew]; + TEST("real: %zd ", newval); + break; + } + }while(assoc[realnew] != realnew); + if(newval){ + TEST(" ==> %zd\n", newval); + indexes[i] = newval; + continue; + } + TEST("new index %zd\n", cidx); + // enter next label + indexes[i] = cidx++; +} +/* +// rebuild indexes +for(size_t i = 1; i < last_assoc_idx; ++i){ + printf("%zd\t%zd ",i,assoc[i]); + if(i == assoc[i]){ + if(i == cidx){ + printf("- keep\n"); + }else{ + printf("- change to %zd\n", cidx); + size_t _2change = assoc[i]; + assoc[i] = cidx; + for(size_t j = i+1; j < last_assoc_idx; ++j){ + if(assoc[j] == _2change){ + printf("\t%zd\t%zd -> %zd\n", j, assoc[j], cidx); + assoc[j] = cidx; + } + } + } + ++cidx; + }else printf("\n"); +}*/ + +--cidx; // cidx now is amount of detected objects +DBG("amount after rebuild: %zd", cidx); + +#ifdef TESTMSGS +printf("\n\n\nI\tASS[I]\tIDX[I]\n"); +for(size_t i = 1; i < last_assoc_idx; ++i) + printf("%zd\t%zd\t%zd\n",i,assoc[i],indexes[i]); +#endif + +int wh = W*H; +OMP_FOR() +for(int i = 0; i < wh; ++i) labels[i] = indexes[labels[i]]; + +if(Nobj) *Nobj = cidx; + +FREE(assoc); +FREE(indexes); diff --git a/LocCorr_new/cclabling_ori.h b/LocCorr_new/cclabling_ori.h new file mode 100644 index 0000000..76fe3bb --- /dev/null +++ b/LocCorr_new/cclabling_ori.h @@ -0,0 +1,128 @@ +/* + * cclabling_1.h - inner part of functions cclabel4 and cclabel8 + * + * Copyright 2015 Edward V. Emelianoff + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + */ + +//double t0 = dtime(); + + size_t N = 0, // current label + Nmax = W*H/4; // max number of labels + int w = W - 1, h = H - 1; + int y; + size_t *assoc = MALLOC(size_t, Nmax); // allocate memory for "remark" array + size_t last_assoc_idx = 0; // last index filled in assoc array + size_t currentnum = 0; // current pixel number + inline void remark(size_t old, size_t new){ // remark in assoc[] pixel with value old to assoc[new] or vice versa + size_t New = assoc[new], Old = assoc[old]; + if(Old == New){ + return; + } + // now we must check Old: if Old use its value + upmark = U[-1]; + }else // check point above only if there's nothing in left up + #endif + if(U[0]) upmark = U[0]; + #ifdef LABEL_8 + if(x < w && U[1]){ // there's something in upper right + if(upmark){ // to the left of it was pixels + remark(U[1], upmark); + }else + upmark = U[1]; + } + #endif + } + if(!upmark){ // there's nothing above - set current pixel to incremented counter + #ifdef LABEL_8 // check, whether pixel is not single + size_t *D = (y < h) ? &ptr[W] : NULL; + if( !(x && ((D && D[-1]) /*|| ptr[-1]*/)) // no left neighbours + && !(x < w && ((D && D[1]) || ptr[1])) // no right neighbours + && !(D && D[0])){ // no neighbour down + *ptr = 0; // erase this hermit! + continue; + } + #else + // no neighbour down & neighbour to the right -> hermit + if((y < h && ptr[W] == 0) && (x < w && ptr[1] == 0)){ + *ptr = 0; // erase this hermit! + continue; + } + #endif + upmark = ++N; + assoc[upmark] = ++currentnum; // refresh "assoc" + last_assoc_idx = upmark + 1; + } + *ptr = upmark; + curmark = upmark; + }else{ // there was something to the left -> we must chek only U[1] + if(U){ + if(x < w && U[1]){ // there's something in upper right + remark(U[1], curmark); + } + } + *ptr = curmark; + } + } + } +#if 0 + // Step 2: rename markers + // first correct complex assotiations in assoc + OMP_FOR() + for(y = 0; y < H; y++){ + size_t *ptr = &labels[y*W]; + for(int x = 0; x < W; x++, ptr++){ + size_t p = *ptr; + if(!p){continue;} + *ptr = assoc[p]; + } + } +#endif + FREE(assoc); + if(Nobj) *Nobj = currentnum; +//printf("%6.4f\t%zd\n", dtime()-t0, currentnum); diff --git a/LocCorr_new/cmdlnopts.c b/LocCorr_new/cmdlnopts.c new file mode 100644 index 0000000..6e98116 --- /dev/null +++ b/LocCorr_new/cmdlnopts.c @@ -0,0 +1,118 @@ +/* geany_encoding=koi8-r + * cmdlnopts.c - the only function that parse cmdln args and returns glob parameters + * + * Copyright 2013 Edward V. Emelianoff + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + */ +#include +#include +#include +#include +#include + +#include "cmdlnopts.h" +#include "config.h" +#include "debug.h" +#include "improc.h" + +/* + * here are global parameters initialisation + */ +static int help; +glob_pars *GP = NULL; + +// DEFAULTS +// default global parameters +static glob_pars G = { + .pidfile = DEFAULT_PIDFILE, + .configname = DEFAULT_CONFFILE, + .steppersport = DEFAULT_STEPPERSPORT, + .throwpart = DEFAULT_THROWPART, +// .medradius = 1., + .ndilations = DEFAULT_DILATIONS, + .nerosions = DEFAULT_EROSIONS, + .minarea = DEFAULT_MINAREA, + .maxarea = DEFAULT_MAXAREA, + .intensthres = DEFAULT_INTENSTHRES, + .maxexp = EXPOS_MAX, + .minexp = EXPOS_MIN, + .xtarget = -1., + .ytarget = -1., + .Naveraging = DEFAULT_NAVERAGE, + .ioport = DEFAULT_IOPORT, + .outputjpg = DEFAULT_OUTPJPEG, +}; + +/* + * Define command line options by filling structure: + * name has_arg flag val type argptr help +*/ +static myoption cmdlnopts[] = { +// common options + {"maxexp", NEED_ARG, NULL, 0, arg_double, APTR(&G.maxexp), _("maximal exposition time (ms), default: 500")}, + {"minexp", NEED_ARG, NULL, 0, arg_double, APTR(&G.minexp), _("minimal exposition time (ms), default: 0.001")}, + {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&help), _("show this help")}, + {"logfile", NEED_ARG, NULL, 'l', arg_string, APTR(&G.logfile), _("file to save logs (default: none)")}, + {"pidfile", NEED_ARG, NULL, 'P', arg_string, APTR(&G.pidfile), _("pidfile (default: " DEFAULT_PIDFILE ")")}, + {"verbose", NO_ARGS, NULL, 'v', arg_none, APTR(&G.verb), _("increase verbosity level of log file (each -v increased by 1)")}, + {"input", NEED_ARG, NULL, 'i', arg_string, APTR(&G.inputname), _("file or directory name for monitoring (or grasshopper/basler for capturing)")}, + {"blackp", NEED_ARG, NULL, 'b', arg_double, APTR(&G.throwpart), _("fraction of black pixels to throw away when make histogram eq")}, +// {"radius", NEED_ARG, NULL, 'r', arg_int, APTR(&G.medradius), _("radius of median filter (r=1 -> 3x3, r=2 -> 5x5 etc.)")}, + {"equalize", NO_ARGS, NULL, 'e', arg_int, APTR(&G.equalize), _("make historam equalization of saved jpeg")}, + {"ndilat", NEED_ARG, NULL, 'D', arg_int, APTR(&G.ndilations),_("amount of dilations after thresholding (default: 2)")}, + {"neros", NEED_ARG, NULL, 'E', arg_int, APTR(&G.nerosions), _("amount of erosions after dilations (default: 2)")}, + {"minarea", NEED_ARG, NULL, 'I', arg_int, APTR(&G.minarea), _("minimal object pixels amount (default: 400)")}, + {"maxarea", NEED_ARG, NULL, 'A', arg_int, APTR(&G.maxarea), _("maximal object pixels amount (default: 150000)")}, + {"intthres",NEED_ARG, NULL, 'T', arg_double, APTR(&G.intensthres),_("threshold by total object intensity when sorting = |I1-I2|/(I1+I2), default: 0.01")}, + {"xoff", NEED_ARG, NULL, 'x', arg_int, APTR(&G.xoff), _("X offset at grabbed image")}, + {"yoff", NEED_ARG, NULL, 'y', arg_int, APTR(&G.yoff), _("Y offset at grabbed image")}, + {"width", NEED_ARG, NULL, 'W', arg_int, APTR(&G.width), _("grabbed subimage width")}, + {"height", NEED_ARG, NULL, 'H', arg_int, APTR(&G.height), _("grabbed subimage height")}, + {"xtarget", NEED_ARG, NULL, 'X', arg_double, APTR(&G.xtarget), _("target point X coordinate")}, + {"ytarget", NEED_ARG, NULL, 'Y', arg_double, APTR(&G.ytarget), _("target point Y coordinate")}, + {"logXY", NEED_ARG, NULL, 'L', arg_string, APTR(&G.logXYname), _("file to log XY coordinates of selected star")}, + {"confname",NEED_ARG, NULL, 'c', arg_string, APTR(&G.configname),_("name of configuration file (default: ./loccorr.conf)")}, + {"canport", NEED_ARG, NULL, 'C', arg_string, APTR(&G.steppersport),_("port of local pusirobot CAN server (default: 4444)")}, + {"naverage",NEED_ARG, NULL, 'N', arg_int, APTR(&G.Naveraging),_("amount of images to average processing (min 2, max 25)")}, + {"ioport", NEED_ARG, NULL, 0, arg_int, APTR(&G.ioport), _("port for IO communication")}, + {"jpegout", NEED_ARG, NULL, 'j', arg_string, APTR(&G.outputjpg), _("output jpeg file location (default: '" DEFAULT_OUTPJPEG "')")}, + end_option +}; + +/** + * Parse command line options and return dynamically allocated structure + * to global parameters + * @param argc - copy of argc from main + * @param argv - copy of argv from main + * @return allocated structure with global parameters + */ +glob_pars *parse_args(int argc, char **argv){ + size_t hlen = 1024; + char helpstring[1024], *hptr = helpstring; + snprintf(hptr, hlen, "Usage: %%s [args]\n\n\tWhere args are:\n"); + // format of help: "Usage: progname [args]\n" + change_helpstring(helpstring); + // parse arguments + parseargs(&argc, &argv, cmdlnopts); + if(help) showhelp(-1, cmdlnopts); + if(argc > 0){ + WARNX("Extra parameters!"); + showhelp(-1, cmdlnopts); + } + return &G; +} + diff --git a/LocCorr_new/cmdlnopts.h b/LocCorr_new/cmdlnopts.h new file mode 100644 index 0000000..f35e733 --- /dev/null +++ b/LocCorr_new/cmdlnopts.h @@ -0,0 +1,77 @@ +/* geany_encoding=koi8-r + * cmdlnopts.h - comand line options for parceargs + * + * Copyright 2013 Edward V. Emelianoff + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + */ + +#pragma once +#ifndef CMDLNOPTS_H__ +#define CMDLNOPTS_H__ + +// default values +#define DEFAULT_PIDFILE "/tmp/loccorr.pid" +#define DEFAULT_CONFFILE "./loccorr.conf" +#define DEFAULT_OUTPJPEG "./outpWcrosses.jpg" +#define DEFAULT_STEPPERSPORT (4444) +#define DEFAULT_IOPORT (12345) +#define DEFAULT_MAXAREA (150000) +#define DEFAULT_MINAREA (400) +#define DEFAULT_EROSIONS (2) +#define DEFAULT_DILATIONS (2) +#define DEFAULT_THROWPART (0.5) +#define DEFAULT_INTENSTHRES (0.01) +#define DEFAULT_NAVERAGE (5) +#define DEFAULT_MAXUSTEPS (16000) +#define DEFAULT_MAXVSTEPS (16000) +#define DEFAULT_NEROSIONS (3) +#define DEFAULT_NDILATIONS (3) + +/* + * here are some typedef's for global data + */ +typedef struct{ + char *pidfile; // name of PID file + char *logfile; // logging to this file + char *inputname; // input for monitor file or directory name + char *logXYname; // file to log XY coordinates of first point + char *configname; // name of configuration file (default: ./loccorr.conf) + char *outputjpg; // output jpeg name + int steppersport; // port of local motors CAN server + int equalize; // make historam equalization of saved jpeg +// int medradius; // radius of median filter (r=1 -> 3x3, r=2 -> 5x5 etc.) + int verb; // logfile verbosity level + int ndilations; // amount of erosions (default: 2) + int nerosions; // amount of dilations (default: 2) + int minarea; // minimal object pixels amount (default: 400) + int maxarea; // maximal object pixels amount (default: 150000) + int Naveraging; // amount of images to average processing (min 2, max 25, default 5) + int xoff; int yoff; // offset by X and Y axes + int width; int height; // target width and height of image + int ioport; // port for IO commands + double throwpart; // fraction of black pixels to throw away when make histogram eq + double intensthres; // threshold by total object intensity when sorting = |I1-I2|/(I1+I2), default: 0.01 + double maxexp; // max exposition time (ms) + double minexp; // min exposition time (ms) + double xtarget; double ytarget;// target point coordinates +} glob_pars; + +extern glob_pars *GP; // for GP->pidfile need in `signals` + +glob_pars *parse_args(int argc, char **argv); + +#endif // CMDLNOPTS_H__ diff --git a/LocCorr_new/config.c b/LocCorr_new/config.c new file mode 100644 index 0000000..44d98e5 --- /dev/null +++ b/LocCorr_new/config.c @@ -0,0 +1,407 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include +#include + +#include "cmdlnopts.h" +#include "config.h" +#include "debug.h" + +static char *conffile = NULL; // configuration file name + +configuration theconf = { + .maxUpos=DEFAULT_MAXUSTEPS, + .minUpos=0, + .maxVpos=DEFAULT_MAXVSTEPS, + .minVpos=0, + .maxFpos=Fmaxsteps-1, + .minFpos=-Fmaxsteps+1, + .minarea=DEFAULT_MINAREA, + .maxarea=DEFAULT_MAXAREA, + .maxwh=1.1, + .minwh=0.9, + .Nerosions=DEFAULT_NEROSIONS, + .Ndilations=DEFAULT_NDILATIONS, + .xoff=0, + .yoff=0, + .width=0, + .height=0, + .equalize=1, + .naverage=DEFAULT_NAVERAGE, + .stpserverport=DEFAULT_STEPPERSPORT, + .starssort=0, + .Kxu=0, + .Kyu=0, + .Kxv=0, + .Kyv=0, + .xtarget=-1, + .ytarget=-1, + .throwpart=DEFAULT_THROWPART, + .maxexp=EXPOS_MAX - 1., + .minexp=EXPOS_MIN, + .fixedexp=EXPOS_MIN*2, + .gain=20., + .intensthres=DEFAULT_INTENSTHRES, + .medseed=MIN_MEDIAN_SEED, +}; + +// {"", PAR_DOUBLE, (void*)&theconf., 0}, +static confparam parvals[] = { + {"maxarea", PAR_INT, (void*)&theconf.maxarea, 0, MINAREA, MAXAREA, + "maximal area (in square pixels) of recognized star image"}, + {"minarea", PAR_INT, (void*)&theconf.minarea, 0, MINAREA, MAXAREA, + "minimal area (in square pixels) of recognized star image"}, + {"minwh", PAR_DOUBLE, (void*)&theconf.minwh, 0, MINWH, 1., + "minimal value of W/H roundness parameter"}, + {"maxwh", PAR_DOUBLE, (void*)&theconf.maxwh, 0, 1., MAXWH, + "maximal value of W/H roundness parameter"}, + {"ndilat", PAR_INT, (void*)&theconf.Ndilations, 0, 1., MAX_NDILAT, + "amount of dilations on binarized image"}, + {"neros", PAR_INT, (void*)&theconf.Nerosions, 0, 1., MAX_NEROS, + "amount of erosions after dilations"}, + {"xoffset", PAR_INT, (void*)&theconf.xoff, 0, 0., MAX_OFFSET, + "X offset of subimage"}, + {"yoffset", PAR_INT, (void*)&theconf.yoff, 0, 0., MAX_OFFSET, + "Y offset of subimage"}, + {"width", PAR_INT, (void*)&theconf.width, 0, 0., MAX_OFFSET, + "subimage width"}, + {"height", PAR_INT, (void*)&theconf.height, 0, 0., MAX_OFFSET, + "subimage height"}, + {"equalize", PAR_INT, (void*)&theconf.equalize, 0, 0., 1., + "make histogram equalization"}, + {"expmethod", PAR_INT, (void*)&theconf.expmethod, 0, 0., 1., + "exposition method: 0 - auto, 1 - fixed"}, + {"naverage", PAR_INT, (void*)&theconf.naverage, 0, 1., NAVER_MAX, + "calculate mean position by N images"}, + {"umax", PAR_INT, (void*)&theconf.maxUpos, 0, -MAXSTEPS, MAXSTEPS, + "maximal value of steps on U semi-axe"}, + {"umin", PAR_INT, (void*)&theconf.minUpos, 0, -MAXSTEPS, MAXSTEPS, + "minimal value of steps on U semi-axe"}, + {"vmax", PAR_INT, (void*)&theconf.maxVpos, 0, -MAXSTEPS, MAXSTEPS, + "maximal value of steps on V semi-axe"}, + {"vmin", PAR_INT, (void*)&theconf.minVpos, 0, -MAXSTEPS, MAXSTEPS, + "minimal value of steps on V semi-axe"}, + {"focmax", PAR_INT, (void*)&theconf.maxFpos, 0, 0., Fmaxsteps, + "maximal focus position in microsteps"}, + {"focmin", PAR_INT, (void*)&theconf.minFpos, 0, -Fmaxsteps, 0., + "minimal focus position in microsteps"}, + {"stpservport", PAR_INT, (void*)&theconf.stpserverport, 0, 0., 65535., + "port number of steppers' server"}, + {"Kxu", PAR_DOUBLE, (void*)&theconf.Kxu, 0, KUVMIN, KUVMAX, + "dU = Kxu*dX + Kyu*dY"}, + {"Kyu", PAR_DOUBLE, (void*)&theconf.Kyu, 0, KUVMIN, KUVMAX, + "dU = Kxu*dX + Kyu*dY"}, + {"Kxv", PAR_DOUBLE, (void*)&theconf.Kxv, 0, KUVMIN, KUVMAX, + "dV = Kxv*dX + Kyv*dY"}, + {"Kyv", PAR_DOUBLE, (void*)&theconf.Kyv, 0, KUVMIN, KUVMAX, + "dV = Kxv*dX + Kyv*dY"}, + {"xtarget", PAR_DOUBLE, (void*)&theconf.xtarget, 0, 1., MAX_OFFSET, + "X coordinate of target position"}, + {"ytarget", PAR_DOUBLE, (void*)&theconf.ytarget, 0, 1., MAX_OFFSET, + "Y coordinate of target position"}, + {"eqthrowpart", PAR_DOUBLE, (void*)&theconf.throwpart, 0, 0., MAX_THROWPART, + "a part of low intensity pixels to throw away when histogram equalized"}, + {"minexp", PAR_DOUBLE, (void*)&theconf.minexp, 0, 0., EXPOS_MAX, + "minimal exposition time"}, + {"maxexp", PAR_DOUBLE, (void*)&theconf.maxexp, 0, 0., EXPOS_MAX, + "maximal exposition time"}, + {"fixedexp", PAR_DOUBLE, (void*)&theconf.fixedexp, 0, EXPOS_MIN, EXPOS_MAX, + "fixed (in manual mode) exposition time"}, + {"intensthres", PAR_DOUBLE, (void*)&theconf.intensthres, 0, 0., 1., + "threshold by total object intensity when sorting = |I1-I2|/(I1+I2)"}, + {"gain", PAR_DOUBLE, (void*)&theconf.gain, 0, GAIN_MIN, GAIN_MAX, + "gain value in manual mode"}, + {"brightness", PAR_DOUBLE, (void*)&theconf.brightness, 0, BRIGHT_MIN, BRIGHT_MAX, + "brightness value"}, + {"starssort", PAR_INT, (void*)&theconf.starssort, 0, 0., 1., + "stars sorting algorithm: by distance from target (0) or by intensity (1)"}, + {"medfilt", PAR_INT, (void*)&theconf.medfilt, 0, 0., 1., + "use median filter (1) or not (0)"}, + {"medseed", PAR_INT, (void*)&theconf.medseed, 0, MIN_MEDIAN_SEED, MAX_MEDIAN_SEED, + "median filter radius"}, + {"fixedbg", PAR_INT, (void*)&theconf.fixedbkg, 0, 0., 1., + "don't calculate background, use fixed value instead"}, + {"fbglevel", PAR_INT, (void*)&theconf.fixedbkgval, 0, FIXED_BK_MIN, FIXED_BK_MAX, + "fixed background level"}, + {NULL, 0, NULL, 0, 0., 0., NULL} +}; + +// return pointer to buff with size l filled with list of all commands (+help messages + low/high values) +char *get_cmd_list(char *buff, int l){ + if(!buff || l < 1) return NULL; + int L = l; + char *ptr = buff; + confparam *par = parvals; + while(L > 0 && par->name){ + int s = snprintf(ptr, L, "%s=newval - %s (from %g to %g)\n", par->name, par->help, par->minval, par->maxval); + if(s < 1) break; + L -= s; ptr += s; + ++par; + } + return buff; +} + +static char *omitspaces(char *v){ + if(!v) return NULL; + while(*v && (*v == ' ' || *v == '\t')) ++v; + char *ptr = strchr(v, ' '); + if(ptr) *ptr = 0; + ptr = strchr(v, '\t'); + if(ptr) *ptr = 0; + return v; +} + +// Read key/value from `pair` (key = value) +// RETURNed value should be FREEd +char *get_keyval(const char *pair, char value[128]){ + char key[128]; + char val[128]; + if(!pair || !*pair) return strdup("#"); // empty line + char *keyptr = key, *valptr = val; + int x = sscanf(pair, "%127[^=]=%127[^\n]%*c", key, val); + //DBG("x=%d, key='%s', val='%s'", x, key, val); + if(x < 0 || x > 2) return NULL; // wrong data or EOF + if(x == 0) return strdup("#"); // empty line + keyptr = omitspaces(key); + if(x == 2){ // param = value + valptr = omitspaces(val); + sprintf(value, "%s", valptr); + return strdup(keyptr); + } + if(*keyptr == '#' || *keyptr == '%'){ // comment + *value = 0; + return strdup("#"); + } + return NULL; +} + +// Read key/value from file +static char *read_key(FILE *file, char value[128]){ + char *line = NULL; + size_t n = 0; + int got = getline(&line, &n, file); + if(!line) return NULL; + if(got < 0){ + free(line); + return NULL; + } + char *kv = get_keyval(line, value); + return kv; +} + +static int str2int(int *num, const char *str){ + long res; + char *endptr; + if(!str) return 0; + res = strtol(str, &endptr, 0); + if(endptr == str || *str == '\0' || *endptr != '\0'){ + return FALSE; + } + if(res > INT_MAX || res < INT_MIN) return FALSE; + if(num) *num = (int)res; + return TRUE; +} + +/** + * @brief chk_keyval - check key for presence in theconf and calculate its value + * @param key (i) - keyword + * @param val (i) - value + * @param result - result calculated from `val` + * @return pointer to confparam if found & checked + */ +confparam *chk_keyval(const char *key, const char *val, key_value *result){ + if(!key || !val || !result) return NULL; + confparam *par = parvals; + while(par->name){ + if(strcmp(key, par->name) == 0){ + //DBG("key='%s', par->name='%s'", key, par->name); + result->type = par->type; + switch(par->type){ + case PAR_INT: + //DBG("INTEGER"); + if(!str2int(&result->val.intval, val)){ + WARNX("Wrong integer value '%s' of parameter '%s'", val, key); + return NULL; + } + if(result->val.intval < par->minval || result->val.intval > par->maxval) + WARNX("Value (%d) of parameter %s out of range %g..%g", + result->val.intval, par->name, par->minval, par->maxval); + else return par; + break; + case PAR_DOUBLE: + //DBG("DOUBLE"); + if(!str2double(&result->val.dblval, val)){ + WARNX("Wrong double value '%s' of parameter '%s'", val, key); + return NULL; + } + //DBG("val: %g, min: %g, max: %g", result->val.dblval, par->minval, par->maxval); + if(result->val.dblval < par->minval || result->val.dblval > par->maxval) + WARNX("Value (%g) of parameter %s out of range %g..%g", + result->val.dblval, par->name, par->minval, par->maxval); + else return par; + break; + } + return NULL; + } + ++par; + } + return NULL; +} + +/** + * @brief chkconfig - check configuration file and init variables + * @param confname - name of file + * @return FALSE if configuration file wrong or absent + */ +int chkconfig(const char *confname){ + DBG("Config name: %s", confname); + if(conffile){ free(conffile); conffile = NULL; } + conffile = strdup(confname); + FILE *f = fopen(confname, "r"); + int ret = TRUE; + if(!f){ + WARN("Can't open %s", confname); + return FALSE; + } + char *key, val[128]; + confparam *par = parvals; + while(par->name){ + par->got = 0; + ++par; + } + while((key = read_key(f, val))){ + if(*key == '#'){ + free(key); + key = NULL; + continue; // comment + } + //DBG("key: %s", key); + key_value kv; + par = chk_keyval(key, val, &kv); + if(!par){ + WARNX("Parameter '%s' is wrong or out of range", key); + free(key); + key = NULL; + continue; + } + switch(par->type){ + case PAR_INT: + *((int*)par->ptr) = kv.val.intval; + break; + case PAR_DOUBLE: + *((double*)par->ptr) = kv.val.dblval; + break; + } + ++par->got; + free(key); + key = NULL; + } + fclose(f); + int found = 0; + par = parvals; + while(par->name){ + //DBG("parvals[]={%s, %d, %d(%g), %d}", par->name, par->type, *((int*)par->ptr), *((double*)par->ptr), par->got); + int k = par->got; + if(!k){ + ++par; + continue; + } + if(k > 1){ + WARNX("parameter '%s' meets %d times", par->name, k); + ret = FALSE; + } + ++found; ++par; + } + DBG("chkconfig(): found %d", found); + return ret; +} + +/** + * @brief saveconf - try to save configuration into file + * @param confname - config file name + * @return FALSE if failed + */ +int saveconf(const char *confname){ + if(!confname){ + if(!conffile){ + WARNX("no conffile given"); + return FALSE; + } + confname = conffile; + } + FILE *f = fopen(confname, "w"); + if(!f){ + WARN("Can't open %s", confname); + LOGERR("Can't open %s to store configuration", confname); + return FALSE; + } + confparam *par = parvals; + while(par->name){ + par->got = 1; + switch(par->type){ + case PAR_INT: + fprintf(f, "%s = %d\n", par->name, *((int*)par->ptr)); + //DBG("%s = %d", par->name, *((int*)par->ptr)); + break; + case PAR_DOUBLE: + fprintf(f, "%s = %.3f\n", par->name, *((double*)par->ptr)); + //DBG("%s = %.3f", par->name, *((double*)par->ptr)); + break; + } + ++par; + } + DBG("%s saved", confname); + LOGDBG("Configuration file '%s' saved (my PID=%zd)", confname, getpid()); + fclose(f); + return TRUE; +} + +// return buffer filled with current configuration +char *listconf(const char *messageid, char *buf, int buflen){ + int L; + char *ptr = buf; + confparam *par = parvals; + L = snprintf(ptr, buflen, "{ \"%s\": \"%s\", ", MESSAGEID, messageid); + buflen -= L; ptr += L; + while(par->name && buflen > 0){ + switch(par->type){ + case PAR_INT: + L = snprintf(ptr, buflen, "\"%s\": %d", par->name, *((int*)par->ptr)); + break; + case PAR_DOUBLE: + L = snprintf(ptr, buflen, "\"%s\": %.3f", par->name, *((double*)par->ptr)); + break; + default: + L = 0; + } + ++par; + if(L > -1){ + buflen -= L; ptr += L; + }else{ + buf[buflen-1] = 0; break; + } + if(par->name){ // put comma + L = snprintf(ptr, buflen, ", "); + if(L > -1){buflen -= L; ptr += L;} + } + } + snprintf(ptr, buflen, " }\n"); + return buf; +} diff --git a/LocCorr_new/config.h b/LocCorr_new/config.h new file mode 100644 index 0000000..bea5700 --- /dev/null +++ b/LocCorr_new/config.h @@ -0,0 +1,140 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#pragma once +#ifndef CONFIG_H__ +#define CONFIG_H__ + +// default configuration borders +// min/max total steps range +#define MAXSTEPS (50000) +#define Fmaxsteps (64000) +// steps per pixel +#define COEFMIN (0.1) +#define COEFMAX (10000) +// area +#define MINAREA (4) +#define MAXAREA (2500000) +#define MAX_NDILAT (100) +#define MAX_NEROS (100) +#define MAX_THROWPART (0.9) +#define MAX_OFFSET (10000) +// min/max exposition in ms +#define EXPOS_MIN (0.1) +#define EXPOS_MAX (4001.) +#define GAIN_MIN (0.) +#define GAIN_MAX (100.) +#define BRIGHT_MIN (0.) +#define BRIGHT_MAX (10.) +// max average images counter +#define NAVER_MAX (25) +// coefficients to convert dx,dy to du,dv +#define KUVMIN (-5000.) +#define KUVMAX (5000.) +// default coefficient for corrections (move to Kdu, Kdv instead of du, dv) +#define KCORR (0.90) +// min/max median seed +#define MIN_MEDIAN_SEED (1) +#define MAX_MEDIAN_SEED (7) +// fixed background +#define FIXED_BK_MIN (0) +#define FIXED_BK_MAX (250) + +// exposition methods: 0 - auto, 1 - fixed +#define EXPAUTO (0) +#define EXPMANUAL (1) + +// roundness parameter +#define MINWH (0.3) +#define MAXWH (3.) + +// messageID field name +#define MESSAGEID "messageid" + +typedef struct{ + int maxUpos; // min/max positions + int minUpos; + int maxVpos; + int minVpos; + int maxFpos; // min/max F position (in microsteps) - user can't change this + int minFpos; + int minarea; // min/max area of star image + int maxarea; + int Nerosions; // amount of erosions/dilations + int Ndilations; + int xoff; // subimage offset + int yoff; + int width; // subimage size + int height; + int equalize; // !=0 to equalize output image histogram + int naverage; // amount of images for average calculation (>1) + int stpserverport; // steppers' server port + int starssort; // stars sorting algorithm: by distance from target (0) or by intensity (1) + int expmethod; // 0 - auto, 1 - fixed + int medfilt; // == 1 to make median filter before calculations + int medseed; // median seed + int fixedbkg; // don't calculate background, use fixed value instead + int fixedbkgval; // value of bk + // dU = Kxu*dX + Kyu*dY; dV = Kxv*dX + Kyv*dY + double Kxu; double Kyu; + double Kxv; double Kyv; + double minwh; double maxwh; // roundness parameters + double xtarget; // target (center) values (in absolute coordinates! screen coords = target - offset) + double ytarget; + double throwpart; // part of values to throw avay @ histogram equalisation + double maxexp; // minimal and maximal exposition (in ms) + double minexp; + double fixedexp; // exptime in manual mode + double gain; // gain value in manual mode + double brightness; // brightness @camera + double intensthres; // threshold for stars intensity comparison: fabs(Ia-Ib)/(Ia+Ib) > thres -> stars differs +} configuration; + +typedef enum{ + PAR_INT, + PAR_DOUBLE +} partype; + +typedef struct{ + const char *name; // parameter name + partype type; // type of parameter's value + void *ptr; // pointer to value in `theconf` + int got; // counter of parameter in config file + double minval; // min/max values + double maxval; + const char *help; // help message +} confparam; + +typedef union{ + double dblval; + int intval; +} dblint; + +typedef struct{ + dblint val; + partype type; +} key_value; + +extern configuration theconf; +char *get_cmd_list(char *buff, int l); +int chkconfig(const char *confname); +int saveconf(const char *confname); +char *get_keyval(const char *pair, char value[128]); +confparam *chk_keyval(const char *key, const char *val, key_value *result); +char *listconf(const char *messageid, char *buf, int buflen); + +#endif // CONFIG_H__ diff --git a/LocCorr_new/debug.c b/LocCorr_new/debug.c new file mode 100644 index 0000000..6e813f1 --- /dev/null +++ b/LocCorr_new/debug.c @@ -0,0 +1,57 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#ifdef EBUG + +#include +#include + +#include "debug.h" + +#define DEBUGLOG "DEBUG.log" + +sl_log *debuglog = NULL; + +void makedebuglog(){ + unlink(DEBUGLOG); + debuglog = sl_createlog(DEBUGLOG, LOGLEVEL_ANY, 0); +} + +/** + * @brief my_malloc - memory allocator for logger + * @param N - number of elements to allocate + * @param S - size of single element (typically sizeof) + * @return pointer to allocated memory area + */ +void *my_malloc(size_t N, size_t S){ + size_t NS = N*S + sizeof(size_t); + sl_putlogt(0, debuglog, LOGLEVEL_ERR, "ALLOCSZ(%zd)", N*S); + void *p = malloc(NS); + if(!p) ERR("malloc"); + memset(p, 0, NS); + *((size_t*)p) = N*S; + return (p + sizeof(size_t)); +} + +void my_free(void *ptr){ + void *orig = ptr - sizeof(size_t); + sl_putlogt(0, debuglog, LOGLEVEL_ERR, "FREESZ(%zd)", *(size_t*)orig); + free(orig); +} + +#endif diff --git a/LocCorr_new/debug.h b/LocCorr_new/debug.h new file mode 100644 index 0000000..00c41f5 --- /dev/null +++ b/LocCorr_new/debug.h @@ -0,0 +1,57 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#pragma once +#ifndef DEBUG_H__ +#define DEBUG_H__ + +#include + +#ifndef TRUE_INLINE +#define TRUE_INLINE __attribute__((always_inline)) static inline +#endif + +#ifdef EBUG + +extern sl_log *debuglog; +void makedebuglog(); +void *my_malloc(size_t N, size_t S); +void my_free(void *ptr); + +//#undef FNAME +#undef ALLOC +#undef MALLOC +#undef FREE +#define _LOG(...) do{if(!debuglog) makedebuglog(); sl_putlogt(1, debuglog, LOGLEVEL_ERR, __VA_ARGS__);}while(0) +#define DBGLOG(...) do{_LOG("%s (%s, line %d)", __func__, __FILE__, __LINE__); \ + sl_putlogt(0, debuglog, LOGLEVEL_ERR, __VA_ARGS__);}while(0) +//#define FNAME() _LOG("%s (%s, line %d)", __func__, __FILE__, __LINE__) + +#define _str(x) #x +#define ALLOC(type, var, size) DBGLOG("%s *%s = ALLOC(%d)", _str(type), _str(var), size*sizeof(type)); \ + type * var = ((type *)my_malloc(size, sizeof(type))) +#define MALLOC(type, size) ((type *)my_malloc(size, sizeof(type))); DBGLOG("ALLOC()") +#define FREE(ptr) do{if(ptr){DBGLOG("FREE(%s)", _str(ptr)); my_free(ptr); ptr = NULL;}}while(0); + +#else + +#define DBGLOG(...) + +#endif + + +#endif // DEBUG_H__ diff --git a/LocCorr_new/dilation.h b/LocCorr_new/dilation.h new file mode 100644 index 0000000..3326980 --- /dev/null +++ b/LocCorr_new/dilation.h @@ -0,0 +1,79 @@ +/* + * dilation.h - inner part of function `dilation` + * + * Copyright 2013 Edward V. Emelianoff + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + */ + +/* + * HERE'S NO ANY "FILE-GUARDS" BECAUSE FILE IS MULTIPLY INCLUDED! + * I use this because don't want to dive into depths of BOOST + * + * multi-including with different defines before - is a simplest way to + * modify simple code for avoiding extra if-then-else + */ + + +#if ! defined IM_UP && ! defined IM_DOWN // image without upper and lower borders +OMP_FOR() +for(int y = 1; y < h; y++) +#endif +{ +#ifdef IM_UP + int y = 0; +#endif +#ifdef IM_DOWN + int y = h; +#endif + uint8_t *iptr = &image[W0*y]; + uint8_t *optr = &ret[W0*y]; + uint8_t p = DIL[*iptr] + #ifndef IM_UP + | iptr[-W0] + #endif + #ifndef IM_DOWN + | iptr[W0] + #endif + ; + if(iptr[1] & 0x80) p |= 1; + *optr = p; + optr++; iptr++; + for(int x = 1; x < w; x++, iptr++, optr++){ + p = DIL[*iptr] + #ifndef IM_UP + | iptr[-W0] + #endif + #ifndef IM_DOWN + | iptr[W0] + #endif + ; + if(iptr[1] & 0x80) p |= 1; + if(iptr[-1] & 1) p |= 0x80; + *optr = p; + } + p = DIL[*iptr] + #ifndef IM_UP + | iptr[-W0] + #endif + #ifndef IM_DOWN + | iptr[W0] + #endif + ; + if(iptr[-1] & 1) p |= 0x80; + *optr = p & lastmask; // clear outern pixel +} + diff --git a/LocCorr_new/draw.c b/LocCorr_new/draw.c new file mode 100644 index 0000000..0854178 --- /dev/null +++ b/LocCorr_new/draw.c @@ -0,0 +1,128 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +// simplest interface to draw lines & ellipses +#include "debug.h" +#include "draw.h" +#include "fits.h" + + +// base colors: +const uint8_t + C_R[] = {255, 0, 0}, + C_G[] = {0, 255, 0}, + C_B[] = {0, 0, 255}, + C_K[] = {0, 0, 0}, + C_W[] = {255,255,255}; + +void Img3_free(il_Img3 **im){ + if(!im || !*im) return; + FREE((*im)->data); + FREE(*im); +} + +void il_Pattern_free(il_Pattern **p){ + if(!p || !*p) return; + FREE((*p)->data); + FREE(*p); +} + +// make a single-channel (opaque) mask for cross; allocated here!!! +il_Pattern *il_Pattern_cross(int h, int w){ + int s = h*w, hmid = h/2, wmid = w/2; + uint8_t *data = MALLOC(uint8_t, s); + il_Pattern *p = MALLOC(il_Pattern, 1); + p->data = data; + p->h = h; p->w = w; + uint8_t *ptr = &data[wmid]; + for(int y = 0; y < h; ++y, ptr += w) *ptr = 255; + ptr = &data[hmid*w]; + for(int x = 0; x < w; ++x, ++ptr) *ptr = 255; + return p; +} + +// complicated cross +il_Pattern *il_Pattern_xcross(int h, int w){ + int s = h*w, hmid = h/2, wmid = w/2; + uint8_t *data = MALLOC(uint8_t, s); + il_Pattern *p = MALLOC(il_Pattern, 1); + p->data = data; + p->h = h; p->w = w; + data[hmid*w + wmid] = 255; // point @ center + if(h < 7 || w < 7) return p; + int idxy1 = (hmid-3)*w, idxy2 = (hmid+3)*w; + int idxx1 = wmid-3, idxx2 = wmid+3; + for(int i = 0; i < wmid - 3; ++i){ + data[idxy1+i] = data[idxy1+w-1-i] = 255; + data[idxy2+i] = data[idxy2+w-1-i] = 255; + } + for(int i = 0; i < hmid - 3; ++i){ + data[idxx1 + i*w] = data[idxx1 + (h-1-i)*w] = 255; + data[idxx2 + i*w] = data[idxx2 + (h-1-i)*w] = 255; + } + return p; +} + +/** + * @brief draw3_pattern - draw pattern @ 3-channel image + * @param img (io) - image + * @param p (i) - the pattern + * @param xc, yc - coordinates of pattern center @ image + * @param colr - color to draw pattern (when opaque == 255) + */ +void il_Pattern_draw3(il_Img3 *img, const il_Pattern *p, int xc, int yc, const uint8_t colr[]){ + if(!img || !p) return; + int xul = xc - p->w/2, yul = yc - p->h/2; + int xdr = xul+p->w-1, ydr = yul+p->h-1; + int R = img->w, D = img->h; // right and down border coordinates + 1 + if(ydr < 0 || xdr < 0 || xul > R-1 || yul > D-1) return; // box outside of image + + int oxlow, oxhigh, oylow, oyhigh; // output limit coordinates + int ixlow, iylow; // intput limit coordinates + if(xul < 0){ + oxlow = 0; ixlow = -xul; + }else{ + oxlow = xul; ixlow = 0; + } + if(yul < 0){ + oylow = 0; iylow = -yul; + }else{ + oylow = yul; iylow = 0; + } + if(xdr < R){ + oxhigh = xdr; + }else{ + oxhigh = R; + } + if(ydr < D){ + oyhigh = ydr; + }else{ + oyhigh = D; + } + OMP_FOR() + for(int y = oylow; y < oyhigh; ++y){ + uint8_t *in = &p->data[(iylow+y-oylow)*p->w + ixlow]; // opaque component + uint8_t *out = &img->data[(y*img->w + oxlow)*3]; // 3-colours + for(int x = oxlow; x < oxhigh; ++x, ++in, out += 3){ + float opaque = ((float)*in)/255.; + for(int c = 0; c < 3; ++c){ + out[c] = (uint8_t)(colr[c] * opaque + out[c]*(1.-opaque)); + } + } + } +} diff --git a/LocCorr_new/draw.h b/LocCorr_new/draw.h new file mode 100644 index 0000000..c5d57e5 --- /dev/null +++ b/LocCorr_new/draw.h @@ -0,0 +1,46 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#pragma once +#ifndef DRAW_H__ +#define DRAW_H__ + +#include + +// 3-channel image for saving into jpg/png +typedef struct{ + uint8_t *data; // image data + int w; // width + int h; // height +} il_Img3; + +// opaque pattern for drawing +typedef struct{ + uint8_t *data; + int w; + int h; +} il_Pattern; + +// base colors +extern const uint8_t C_R[], C_G[], C_B[], C_K[], C_W[]; + +void il_Pattern_free(il_Pattern **p); +void il_Pattern_draw3(il_Img3 *img, const il_Pattern *p, int xc, int yc, const uint8_t colr[]); +il_Pattern *il_Pattern_cross(int h, int w); +il_Pattern *il_Pattern_xcross(int h, int w); + +#endif // DRAW_H__ diff --git a/LocCorr_new/ec_filter.h b/LocCorr_new/ec_filter.h new file mode 100644 index 0000000..b110f92 --- /dev/null +++ b/LocCorr_new/ec_filter.h @@ -0,0 +1,102 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +// inner part of function `filter8` + +#if ! defined IM_UP && ! defined IM_DOWN +OMP_FOR() +for(int y = 1; y < h; y++) +#endif +{ +#ifdef IM_UP + int y = 0; +#endif +#ifdef IM_DOWN + int y = h; +#endif + uint8_t *iptr = &image[W0*y]; + uint8_t *optr = &ret[W0*y]; + // x=0 + register uint8_t inp = *iptr; + register uint8_t p = (inp << 1) | (inp >> 1) + #ifndef IM_UP + | iptr[-W0] | (iptr[-W0]<<1) | (iptr[-W0]>>1) + #endif + #ifndef IM_DOWN + | iptr[W0] | (iptr[W0]<<1) | (iptr[W0]>>1) + #endif + ; + if((iptr[1] + #ifndef IM_UP + | iptr[-W0+1] + #endif + #ifndef IM_DOWN + | iptr[W0+1] + #endif + )& 0x80) p |= 1; + *optr++ = inp & p; + ++iptr; + for(int x = 1; x < w; ++x){ + inp = *iptr; + p = (inp << 1) | (inp >> 1) + #ifndef IM_UP + | iptr[-W0] | (iptr[-W0]<<1) | (iptr[-W0]>>1) + #endif + #ifndef IM_DOWN + | iptr[W0] | (iptr[W0]<<1) | (iptr[W0]>>1) + #endif + ; + if((iptr[1] + #ifndef IM_UP + | iptr[-W0+1] + #endif + #ifndef IM_DOWN + | iptr[W0+1] + #endif + )& 0x80) p |= 1; + if((iptr[-1] + #ifndef IM_UP + | iptr[-W0-1] + #endif + #ifndef IM_DOWN + | iptr[W0-1] + #endif + )& 1) p |= 0x80; + *optr++ = inp & p; + ++iptr; + } + // x = W0-1 + inp = *iptr; + p = (inp << 1) | (inp >> 1) + #ifndef IM_UP + | iptr[-W0] | (iptr[-W0]<<1) | (iptr[-W0]>>1) + #endif + #ifndef IM_DOWN + | iptr[W0] | (iptr[W0]<<1) | (iptr[W0]>>1) + #endif + ; + if((iptr[-1] + #ifndef IM_UP + | iptr[-W0-1] + #endif + #ifndef IM_DOWN + | iptr[W0-1] + #endif + )& 1) p |= 0x80; + *optr = inp & p; +} diff --git a/LocCorr_new/fc_filter.h b/LocCorr_new/fc_filter.h new file mode 100644 index 0000000..4a97789 --- /dev/null +++ b/LocCorr_new/fc_filter.h @@ -0,0 +1,74 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +// inner part of function `filter4` + +#if ! defined IM_UP && ! defined IM_DOWN +OMP_FOR() +for(int y = 1; y < h; y++) +#endif +{ +#ifdef IM_UP + int y = 0; +#endif +#ifdef IM_DOWN + int y = h; +#endif + uint8_t *iptr = &image[W0*y]; + uint8_t *optr = &ret[W0*y]; + // x=0 + register uint8_t inp = *iptr; + register uint8_t p = (inp << 1) | (inp >> 1) + #ifndef IM_UP + | iptr[-W0] + #endif + #ifndef IM_DOWN + | iptr[W0] + #endif + ; + if(iptr[1] & 0x80) p |= 1; + *optr++ = inp & p; + ++iptr; + for(int x = 1; x < w; ++x){ + inp = *iptr; + p = (inp << 1) | (inp >> 1) + #ifndef IM_UP + | iptr[-W0] + #endif + #ifndef IM_DOWN + | iptr[W0] + #endif + ; + if(iptr[1] & 0x80) p |= 1; + if(iptr[-1] & 1) p |= 0x80; + *optr++ = inp & p; + ++iptr; + } + // x = W0-1 + inp = *iptr; + p = (inp << 1) | (inp >> 1) + #ifndef IM_UP + | iptr[-W0] + #endif + #ifndef IM_DOWN + | iptr[W0] + #endif + ; + if(iptr[-1] & 1) p |= 0x80; + *optr = inp & p; +} diff --git a/LocCorr_new/fits.c b/LocCorr_new/fits.c new file mode 100644 index 0000000..b3cd3e7 --- /dev/null +++ b/LocCorr_new/fits.c @@ -0,0 +1,127 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#include +#include +#include +#include + +#include "debug.h" +#include "fits.h" + +static int fitsstatus = 0; + +/* + * Macros for error processing when working with cfitsio functions + */ +#define TRYFITS(f, ...) \ +do{ fitsstatus = 0; \ + f(__VA_ARGS__, &fitsstatus); \ + if(fitsstatus){ \ + fits_report_error(stderr, fitsstatus); \ + return FALSE;} \ +}while(0) +#define FITSFUN(f, ...) \ +do{ fitsstatus = 0; \ + int ret = f(__VA_ARGS__, &fitsstatus); \ + if(ret || fitsstatus) \ + fits_report_error(stderr, fitsstatus); \ +}while(0) +#define WRITEKEY(...) \ +do{ fitsstatus = 0; \ + fits_write_key(__VA_ARGS__, &fitsstatus); \ + if(status) fits_report_error(stderr, status);\ +}while(0) + +void Image_free(Image **img){ + FREE((*img)->data); + FREE(*img); +} + +// I->data should be allocated!!! +static inline void convflt2ima(float *f, Image *I){ + if(!I || !I->data || !f) return; + float min = *f, max = min; + int wh = I->height * I->width; + #pragma omp parallel shared(min, max) + { + float min_p = min, max_p = min; + #pragma omp for nowait + for(int i = 0; i < wh; ++i){ + if(f[i] < min_p) min_p = f[i]; + else if(f[i] > max_p) max_p = f[i]; + } + #pragma omp critical + { + if(min > min_p) min = min_p; + if(max < max_p) max = max_p; + } + } + float W = 255./(max - min); + #pragma omp for + for(int i = 0; i < wh; ++i){ + I->data[i] = (Imtype)(W*(f[i] - min)); + } + I->maxval = 255; + I->minval = 0; +} + +bool FITS_read(const char *filename, Image **fits){ + FNAME(); + bool ret = TRUE; + fitsfile *fp; + int hdunum; + int naxis, dtype; + long naxes[2]; + Image *img = MALLOC(Image, 1); + + TRYFITS(fits_open_file, &fp, filename, READONLY); + FITSFUN(fits_get_num_hdus, fp, &hdunum); + DBG("Got %d HDUs", hdunum); + if(hdunum < 1){ + WARNX(_("Can't read HDU")); + ret = FALSE; + goto returning; + } + // get image dimensions + TRYFITS(fits_get_img_param, fp, 2, &dtype, &naxis, naxes); + DBG("Image have %d axis", naxis); + if(naxis > 2){ + WARNX(_("Images with > 2 dimensions are not supported")); + ret = FALSE; + goto returning; + } + img->width = naxes[0]; + img->height = naxes[1]; + DBG("got image %ldx%ld pix, bitpix=%d", naxes[0], naxes[1], dtype); + size_t sz = naxes[0] * naxes[1]; + img->data = MALLOC(Imtype, sz); + float *targ = MALLOC(float, sz); + int stat = 0; + TRYFITS(fits_read_img, fp, TFLOAT, 1, sz, NULL, targ, &stat); + if(stat) WARNX(_("Found %d pixels with undefined value"), stat); + convflt2ima(targ, img); + FREE(targ); + DBG("ready"); + +returning: + FITSFUN(fits_close_file, fp); + if(!ret || !fits){ + Image_free(&img); + }else *fits = img; + return ret; +} diff --git a/LocCorr_new/fits.h b/LocCorr_new/fits.h new file mode 100644 index 0000000..2d3fd76 --- /dev/null +++ b/LocCorr_new/fits.h @@ -0,0 +1,32 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once +#ifndef FITS_H__ +#define FITS_H__ + +#include +#include +#include + +#include "imagefile.h" + +void Image_free(Image **ima); +bool FITS_read(const char *filename, Image **fits); + +#endif // FITS_H__ diff --git a/LocCorr_new/grasshopper.c b/LocCorr_new/grasshopper.c new file mode 100644 index 0000000..c9aac7a --- /dev/null +++ b/LocCorr_new/grasshopper.c @@ -0,0 +1,275 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include +#include // FLT_EPSILON +#include +#include +#include + +#include "config.h" +#include "debug.h" +#include "grasshopper.h" +#include "imagefile.h" + +static fc2Context context; +static fc2PGRGuid guid; +static fc2Error err = FC2_ERROR_OK; + +#ifndef Stringify +#define Stringify(x) #x +#endif + +#define FC2FN(fn, ...) do{err = FC2_ERROR_OK; if(FC2_ERROR_OK != (err=fn(context __VA_OPT__(,) __VA_ARGS__))){ \ + WARNX(Stringify(fn) "(): %s", fc2ErrorToDescription(err)); return FALSE;}}while(0) + +static void disconnect(){ + fc2DestroyContext(context); +} + +/** + * @brief setfloat - set absolute property value (float) + * @param t - type of property + * @param f - new value + * @return 1 if all OK + */ +static int setfloat(fc2PropertyType t, float f){ + fc2Property prop = {0}; + prop.type = t; + fc2PropertyInfo i = {0}; + i.type = t; + FC2FN(fc2GetProperty, &prop); + FC2FN(fc2GetPropertyInfo, &i); + if(!prop.present || !i.present) return 0; + if(prop.autoManualMode){ + if(!i.manualSupported){ + WARNX("Can't set auto-only property"); + return FALSE; + } + prop.autoManualMode = false; + } + if(!prop.absControl){ + if(!i.absValSupported){ + WARNX("Can't set non-absolute property to absolute value"); + return FALSE; + } + prop.absControl = true; + } + if(!prop.onOff){ + if(!i.onOffSupported){ + WARNX("Can't set property ON"); + return FALSE; + } + prop.onOff = true; + } + if(prop.onePush && i.onePushSupported) prop.onePush = false; + prop.valueA = prop.valueB = 0; + prop.absValue = f; + FC2FN(fc2SetProperty, &prop); + // now check + FC2FN(fc2GetProperty, &prop); + if(fabsf(prop.absValue - f) > 0.02f){ + WARNX("Can't set property! Got %g instead of %g.", prop.absValue, f); + return FALSE; + } + return TRUE; +} + +static int setbrightness(float b){ + return setfloat(FC2_BRIGHTNESS, b); +} +static int setexp(float e){ + return setfloat(FC2_SHUTTER, e); +} +static int setgain(float e){ + return setfloat(FC2_GAIN, e); +} +// TODO: +set saturation, hue, sharpness ? + + +static int propOnOff(fc2PropertyType t, BOOL onOff){ + fc2Property prop = {0}; + prop.type = t; + fc2PropertyInfo i = {0}; + i.type = t; + FC2FN(fc2GetPropertyInfo, &i); + FC2FN(fc2GetProperty, &prop); + if(!prop.present || !i.present) return 0; + if(prop.onOff == onOff) return 0; + if(!i.onOffSupported){ + WARNX("Property doesn't support state OFF"); + return 0; + } + prop.onOff = onOff; + FC2FN(fc2SetProperty, &prop); + FC2FN(fc2GetProperty, &prop); + if(prop.onOff != onOff){ + WARNX("Can't change property OnOff state"); + return 0; + } + return 1; +} + +#define autoExpOff() propOnOff(FC2_AUTO_EXPOSURE, false) +#define whiteBalOff() propOnOff(FC2_WHITE_BALANCE, false) +#define gammaOff() propOnOff(FC2_GAMMA, false) +#define trigModeOff() propOnOff(FC2_TRIGGER_MODE, false) +#define trigDelayOff() propOnOff(FC2_TRIGGER_DELAY, false) +#define frameRateOff() propOnOff(FC2_FRAME_RATE, false) + +static int geometrylimits(frameformat *max, frameformat *step){ + fc2Format7Info f = {.mode = FC2_MODE_0}; + BOOL b; + fc2Format7Info i = {0}; + FC2FN(fc2GetFormat7Info, &i, &b); + if(!b || !max || !step) return FALSE; + max->h = f.maxHeight; max->w = f.maxWidth; + max->xoff = f.maxWidth - f.offsetHStepSize; + max->yoff = f.maxHeight - f.offsetVStepSize; + step->w = f.imageHStepSize; + step->h = f.imageVStepSize; + step->xoff = f.offsetHStepSize; + step->yoff = f.offsetVStepSize; + return TRUE; +} + +static int getformat(frameformat *fmt){ + if(!fmt) return FALSE; + unsigned int packsz; float pc; + fc2Format7ImageSettings f7; + FC2FN(fc2GetFormat7Configuration, &f7, &packsz, &pc); + fmt->h = f7.height; fmt->w = f7.width; + fmt->xoff = f7.offsetX; fmt->yoff = f7.offsetY; + return TRUE; +} + +static int changeformat(frameformat *fmt){ + if(!fmt) return FALSE; + BOOL b; + fc2Format7ImageSettings f7; + f7.mode = FC2_MODE_0; + f7.offsetX = fmt->xoff; + f7.offsetY = fmt->yoff; + f7.width = fmt->w; + f7.height = fmt->h; + DBG("offx=%d, offy=%d, w=%d, h=%d ", f7.offsetX, f7.offsetY, f7.width, f7.height); + f7.pixelFormat = FC2_PIXEL_FORMAT_MONO8; + fc2Format7PacketInfo f7p; + FC2FN(fc2ValidateFormat7Settings, &f7, &b, &f7p); + if(!b) return FALSE; // invalid + FC2FN(fc2SetFormat7Configuration, &f7, f7p.recommendedBytesPerPacket); + getformat(fmt); // change values to currently used + return TRUE; +} + +static int connect(){ + FNAME(); + unsigned int numCameras = 0; + if(FC2_ERROR_OK != (err = fc2CreateContext(&context))){ + WARNX("fc2CreateContext(): %s", fc2ErrorToDescription(err)); + return FALSE; + } + FC2FN(fc2GetNumOfCameras, &numCameras); + if(numCameras == 0){ + WARNX("No cameras detected!"); + camdisconnect(); + return FALSE; + } + DBG("Found %d camera[s]", numCameras); + if(numCameras > 1){ + WARNX("Found %d cameras, will use first", numCameras); + } + FC2FN(fc2GetCameraFromIndex, 0, &guid); + FC2FN(fc2Connect, &guid); + // turn off all shit + autoExpOff(); + whiteBalOff(); + gammaOff(); + trigModeOff(); + trigDelayOff(); + frameRateOff(); + return TRUE; +} + +static int GrabImage(fc2Image *convertedImage){ + if(!convertedImage) return FALSE; + int ret = FALSE; + fc2Image rawImage; + // start capture + FC2FN(fc2StartCapture); + err = fc2CreateImage(&rawImage); + if(err != FC2_ERROR_OK){ + WARNX("Error in fc2CreateImage: %s", fc2ErrorToDescription(err)); + fc2StopCapture(context); + return FALSE; + } + // Retrieve the image + err = fc2RetrieveBuffer(context, &rawImage); + if(err != FC2_ERROR_OK){ + WARNX("Error in fc2RetrieveBuffer: %s", fc2ErrorToDescription(err)); + goto rtn; + } + // Convert image to gray + err = fc2ConvertImageTo(FC2_PIXEL_FORMAT_MONO8, &rawImage, convertedImage); + if(err != FC2_ERROR_OK){ + WARNX("Error in fc2ConvertImageTo: %s", fc2ErrorToDescription(err)); + goto rtn; + } + ret = TRUE; +rtn: + fc2StopCapture(context); + fc2DestroyImage(&rawImage); + return ret; +} + +// capture image, memory allocated HERE +static Image *capture(){ + FNAME(); + Image *oIma = NULL; + fc2Image convertedImage; + err = fc2CreateImage(&convertedImage); + if(err != FC2_ERROR_OK){ + WARNX("capture_grasshopper(): can't create image, %s", fc2ErrorToDescription(err)); + return NULL; + } + if(!GrabImage(&convertedImage)){ + WARNX("Can't grab image"); + return NULL; + } + oIma = u8toImage(convertedImage.pData, convertedImage.cols, convertedImage.rows, convertedImage.stride); + fc2DestroyImage(&convertedImage); + return oIma; +} + +static float maxgain(){ + return GAIN_MAX; +} + +// exported object +camera GrassHopper = { + .disconnect = disconnect, + .connect = connect, + .capture = capture, + .setbrightness = setbrightness, + .setexp = setexp, + .setgain = setgain, + .setgeometry = changeformat, + .getgeomlimits = geometrylimits, + .getmaxgain = maxgain, +}; diff --git a/LocCorr_new/grasshopper.h b/LocCorr_new/grasshopper.h new file mode 100644 index 0000000..30edfb4 --- /dev/null +++ b/LocCorr_new/grasshopper.h @@ -0,0 +1,28 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#pragma once +#ifndef GRASSHOPPER_H__ +#define GRASSHOPPER_H__ + +#include "cameracapture.h" // `camera` + +#define GRASSHOPPER_CAPT_NAME "grasshopper" + +extern camera GrassHopper; + +#endif // GRASSHOPPER_H__ diff --git a/LocCorr_new/imagefile.c b/LocCorr_new/imagefile.c new file mode 100644 index 0000000..63d00d6 --- /dev/null +++ b/LocCorr_new/imagefile.c @@ -0,0 +1,582 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "basler.h" +#include "cameracapture.h" +#include "cmdlnopts.h" +#include "config.h" +#include "debug.h" +#include "draw.h" +#include "fits.h" +#include "grasshopper.h" +#include "imagefile.h" +#include "median.h" + +typedef struct{ + const char signature[8]; + uint8_t len; + InputType it; +} imsign; + +const imsign signatures[] = { + {"BM", 2, T_BMP}, + {"SIMPLE", 6, T_FITS}, + {{0x1f, 0x8b, 0x08}, 3, T_GZIP}, + {"GIF8", 4, T_GIF}, + {{0xff, 0xd8, 0xff, 0xdb}, 4, T_JPEG}, + {{0xff, 0xd8, 0xff, 0xe0}, 4, T_JPEG}, + {{0xff, 0xd8, 0xff, 0xe1}, 4, T_JPEG}, + {{0x89, 0x50, 0x4e, 0x47}, 4, T_PNG}, + // {{0x49, 0x49, 0x2a, 0x00}, 4, T_TIFF}, + {"", 0, T_WRONG} +}; + +#ifdef EBUG +static char *hexdmp(const char sig[8]){ + static char buf[128]; + char *bptr = buf; + bptr += sprintf(bptr, "[ "); + for(int i = 0; i < 7; ++i){ + bptr += sprintf(bptr, "%02X ", (uint8_t)sig[i]); + } + bptr += sprintf(bptr, "]"); + return buf; +} +#endif + +/** + * @brief imtype - check image type of given file + * @param f - opened image file structure + * @return image type or T_WRONG + */ +static InputType imtype(FILE *f){ + char signature[8]; + int x = fread(signature, 1, 7, f); + DBG("x=%d", x); + if(7 != x){ + WARN("Can't read file signature"); + return T_WRONG; + } + signature[7] = 0; + const imsign *s = signatures; + DBG("Got signature: %s (%s)", hexdmp(signature), signature); + while(s->len){ + DBG("Check %s", s->signature); + if(0 == memcmp(s->signature, signature, s->len)){ + DBG("Found signature %s", s->signature); + return s->it; + } + ++s; + } + return T_WRONG; +} + +/** + * @brief chkinput - check file/directory name + * @param name - name of file or directory + * @return type of `name` + */ +InputType chkinput(const char *name){ + DBG("input name: %s", name); + if(0 == strcmp(name, GRASSHOPPER_CAPT_NAME)) return T_CAPT_GRASSHOPPER; + else if(0 == strcmp(name, BASLER_CAPT_NAME)) return T_CAPT_BASLER; + struct stat fd_stat; + stat(name, &fd_stat); + if(S_ISDIR(fd_stat.st_mode)){ + DBG("%s is a directory", name); + DIR *d = opendir(name); + if(!d){ + WARN("Can't open directory %s", name); + return T_WRONG; + } + closedir(d); + return T_DIRECTORY; + } + FILE *f = fopen(name, "r"); + if(!f){ + WARN("Can't open file %s", name); + return T_WRONG; + } + InputType tp = imtype(f); + DBG("Image type of %s is %d", name, tp); + fclose(f); + return tp; +} + +/** + * @brief u8toImage - convert uint8_t data to Image structure (flipping upside down for FITS coordinates) + * @param data - original image data + * @param width - image width + * @param height - image height + * @param stride - image width with alignment + * @return Image structure (fully allocated, you can FREE(data) after it) + */ +Image *u8toImage(const uint8_t *data, int width, int height, int stride){ + FNAME(); + Image *outp = Image_new(width, height); + // flip image updown for FITS coordinate system + OMP_FOR() + for(int y = 0; y < height; ++y){ + Imtype *Out = &outp->data[(height-1-y)*width]; + const uint8_t *In = &data[y*stride]; + for(int x = 0; x < width; ++x){ + *Out++ = (Imtype)(*In++); + } + } + Image_minmax(outp); + return outp; +} + +/** + * @brief im_load - load image file + * @param name - filename + * @return Image structure or NULL + */ +static inline Image *im_load(const char *name){ + int width, height, channels; + uint8_t *img = stbi_load(name, &width, &height, &channels, 1); + if(!img){ + WARNX("Error in loading the image %s\n", name); + return NULL; + } + Image *I = u8toImage(img, width, height, width); + free(img); + return I; +} + +/** + * @brief Image_read - read image from any supported file type + * @param name - path to image + * @return image or NULL if failed + */ +Image *Image_read(const char *name){ + InputType tp = chkinput(name); + if(tp == T_DIRECTORY || tp == T_WRONG){ + WARNX("Bad file type to read"); + return NULL; + } + Image *outp = NULL; + if(tp == T_FITS || tp == T_GZIP){ + if(!FITS_read(name, &outp)){ + WARNX("Can't read %s", name); + return NULL; + } + }else outp = im_load(name); + return outp; +} + +/** + * @brief Image_new - allocate memory for new struct Image & Image->data + * @param w, h - image size + * @return data allocated here + */ +Image *Image_new(int w, int h){ + if(w < 1 || h < 1) return NULL; + Image *outp = MALLOC(Image, 1); + outp->width = w; + outp->height = h; + outp->data = MALLOC(Imtype, w*h); + return outp; +} + +/** + * @brief Image_sim - allocate memory for new empty Image with similar size & data type + * @param i - sample image + * @return data allocated here (with empty keylist & zeros in data) + */ +Image *Image_sim(const Image *i){ + if(!i) return NULL; + Image *outp = Image_new(i->width, i->height); + return outp; +} + +/** + * @brief get_histogram - calculate image histogram + * @param I - orig + * @return + */ +size_t *get_histogram(const Image *I){ + if(!I || !I->data) return NULL; + size_t *histogram = MALLOC(size_t, 256); + int wh = I->width * I->height; +#pragma omp parallel +{ + size_t histogram_private[256] = {0}; + #pragma omp for nowait + for(int i = 0; i < wh; ++i){ + ++histogram_private[I->data[i]]; + } + #pragma omp critical + { + for(int i = 0; i < 256; ++i) histogram[i] += histogram_private[i]; + } +} + return histogram; +} + + +/** + * @brief calc_background - Simple background calculation by histogram + * @param img (i) - input image (here will be modified its top2proc field) + * @param bk (o) - background value + * @return 0 if error + */ +int calc_background(const Image *img, Imtype *bk){ + if(!img || !img->data || !bk) return FALSE; + if(img->maxval == img->minval){ + WARNX("Zero or overilluminated image!"); + return FALSE; + } + if(theconf.fixedbkg){ + if(theconf.fixedbkg > img->minval){ + WARNX("Image values too small"); + return FALSE; + } + *bk = theconf.fixedbkg; + return TRUE; + } + size_t *histogram = get_histogram(img); + + size_t modeidx = 0, modeval = 0; + for(int i = 0; i < 256; ++i) + if(modeval < histogram[i]){ + modeval = histogram[i]; + modeidx = i; + } + //DBG("Mode=%g @ idx%d (N=%d)", ((Imtype)modeidx / 255.)*ampl, modeidx, modeval); + ssize_t diff2[256] = {0}; + for(int i = 2; i < 254; ++i) diff2[i] = (histogram[i+2]+histogram[i-2]-2*histogram[i])/4; + //green("HISTO:\n"); + //for(int i = 0; i < 256; ++i) printf("%d:\t%d\t%d\n", i, histogram[i], diff2[i]); + FREE(histogram); + if(modeidx < 2) modeidx = 2; + if(modeidx > 253){ + WARNX("Overilluminated image"); + return FALSE; // very bad image: overilluminated + } + size_t borderidx = modeidx; + for(int i = modeidx; i < 254; ++i){ // search bend-point by second derivate + if(diff2[i] <= 0 && diff2[i+1] <= 0){ + borderidx = i; break; + } + } + //DBG("borderidx=%d -> %d", borderidx, (borderidx+modeidx)/2); + //*bk = (borderidx + modeidx) / 2; + *bk = borderidx; + return TRUE; +} + + +/** + * @brief linear - linear transform for preparing file to save as JPEG or other type (mirror image upside down!) + * @param I - input image + * @param nchannels - 1 or 3 colour channels + * @return allocated here image for jpeg/png storing + */ +uint8_t *linear(const Image *I, int nchannels){ // only 1 and 3 channels supported! + if(!I || !I->data || (nchannels != 1 && nchannels != 3)) return NULL; + int width = I->width, height = I->height; + size_t stride = width*nchannels, S = height*stride; + uint8_t *outp = MALLOC(uint8_t, S); + float min = (float)I->minval, max = (float)I->maxval, W = 255./(max - min); + //DBG("make linear transform %dx%d, %d channels", I->width, I->height, nchannels); + if(nchannels == 3){ + OMP_FOR() + for(int y = 0; y < height; ++y){ + uint8_t *Out = &outp[(height-1-y)*stride]; + Imtype *In = &I->data[y*width]; + for(int x = 0; x < width; ++x){ + Out[0] = Out[1] = Out[2] = (uint8_t)(W*((float)(*In++) - min)); + Out += 3; + } + } + }else{ + OMP_FOR() + for(int y = 0; y < height; ++y){ + uint8_t *Out = &outp[(height-1-y)*width]; + Imtype *In = &I->data[y*width]; + for(int x = 0; x < width; ++x){ + *Out++ = (uint8_t)(W*((float)(*In++) - min)); + } + } + } + return outp; +} + +/** + * @brief equalize - hystogram equalization (mirror image upside down!) + * @param I - input image + * @param nchannels - 1 or 3 colour channels + * @param throwpart - which part of black pixels (from all amount) to throw away + * @return allocated here image for jpeg/png storing + */ +uint8_t *equalize(const Image *I, int nchannels, double throwpart){ + if(!I || !I->data || (nchannels != 1 && nchannels != 3)) return NULL; + int width = I->width, height = I->height; + size_t stride = width*nchannels, S = height*stride; + size_t *orig_histo = get_histogram(I); // original hystogram (linear) + if(!orig_histo) return NULL; + uint8_t *outp = MALLOC(uint8_t, S); + uint8_t eq_levls[256] = {0}; // levels to convert: newpix = eq_levls[oldpix] + int s = width*height; + int Nblack = 0, bpart = (int)(throwpart * (double)s); + int startidx; + // remove first part of black pixels + for(startidx = 0; startidx < 256; ++startidx){ + Nblack += orig_histo[startidx]; + if(Nblack >= bpart) break; + } + ++startidx; + /* remove last part of white pixels + for(stopidx = 255; stopidx > startidx; --stopidx){ + Nwhite += orig_hysto[stopidx]; + if(Nwhite >= wpart) break; + }*/ + //DBG("Throw %d (real: %d black) pixels, startidx=%d", bpart, Nblack, startidx); + double part = (double)(s + 1. - Nblack) / 256., N = 0.; + for(int i = startidx; i < 256; ++i){ + N += orig_histo[i]; + eq_levls[i] = (uint8_t)(N/part); + } + //for(int i = stopidx; i < 256; ++i) eq_levls[i] = 255; +#if 0 + DBG("Original / new histogram"); + for(int i = 0; i < 256; ++i) printf("%d\t%d\t%d\n", i, orig_hysto[i], eq_levls[i]); +#endif + if(nchannels == 3){ + OMP_FOR() + for(int y = 0; y < height; ++y){ + uint8_t *Out = &outp[(height-1-y)*stride]; + Imtype *In = &I->data[y*width]; + for(int x = 0; x < width; ++x){ + Out[0] = Out[1] = Out[2] = eq_levls[*In++]; + Out += 3; + } + } + }else{ + OMP_FOR() + for(int y = 0; y < height; ++y){ + uint8_t *Out = &outp[(height-1-y)*width]; + Imtype *In = &I->data[y*width]; + for(int x = 0; x < width; ++x){ + *Out++ = eq_levls[*In++]; + } + } + } + FREE(orig_histo); + return outp; +} + +/** + * @brief Image_write_jpg - save image as JPG file (flipping upside down) + * @param I - image + * @param name - filename + * @param eq == 0 to write linear, != 0 to write equalized image + * @return 0 if failed + */ +int Image_write_jpg(const Image *I, const char *name, int eq){ + if(!I || !I->data) return 0; + uint8_t *outp = NULL; + if(eq) + outp = equalize(I, 1, theconf.throwpart); + else + outp = linear(I, 1); + if(!outp) return 0; + //DBG("Try to write %s", name); + char *tmpnm = MALLOC(char, strlen(name) + 5); + sprintf(tmpnm, "%s-tmp", name); + int r = stbi_write_jpg(tmpnm, I->width, I->height, 1, outp, 95); + if(r){ + if(rename(tmpnm, name)){ + WARN("rename()"); + r = 0; + } + } + FREE(tmpnm); + FREE(outp); + return r; +} + +// calculate extremal values of image data and store them in it +void Image_minmax(Image *I){ + if(!I || !I->data) return; + Imtype min = *(I->data), max = min; + int wh = I->width * I->height; +#ifdef EBUG + double t0 = dtime(); +#endif + #pragma omp parallel shared(min, max) + { + int min_p = min, max_p = min; + #pragma omp for nowait + for(int i = 0; i < wh; ++i){ + Imtype pixval = I->data[i]; + if(pixval < min_p) min_p = pixval; + else if(pixval > max_p) max_p = pixval; + } + #pragma omp critical + { + if(min > min_p) min = min_p; + if(max < max_p) max = max_p; + } + } + I->maxval = max; + I->minval = min; + DBG("Image_minmax(): Min=%d, Max=%d, time: %gms", min, max, (dtime()-t0)*1e3); +} + +/* + * =================== CONVERT IMAGE TYPES ===================> + */ + +/** + * @brief bin2Im - convert binarized image into floating + * @param image - binarized image + * @param W, H - its size (in pixels!) + * @return Image structure + */ +Image *bin2Im(const uint8_t *image, int W, int H){ + Image *ret = Image_new(W, H); + int stride = (W + 7) / 8, s1 = (stride*8 == W) ? stride : stride - 1; + OMP_FOR() + for(int y = 0; y < H; y++){ + Imtype *optr = &ret->data[y*W]; + const uint8_t *iptr = &image[y*stride]; + for(int x = 0; x < s1; x++){ + register uint8_t inp = *iptr++; + for(int i = 0; i < 8; ++i){ + *optr++ = (inp & 0x80) ? 1. : 0; + inp <<= 1; + } + } + int rest = W - s1*8; + if(rest){ + register uint8_t inp = *iptr; + for(int i = 0; i < rest; ++i){ + *optr++ = (inp & 0x80) ? 1. : 0; + inp <<= 1; + } + } + } + ret->minval = 0; + ret->maxval = 1; + return ret; +} + +/** + * Convert floatpoint image into pseudo-packed (1 char == 8 pixels), all values > bk will be 1, else - 0 + * @param im (i) - image to convert + * @param stride (o) - new width of image + * @param bk - background level (all values < bk will be 0, other will be 1) + * @return allocated memory area with "packed" image + */ +uint8_t *Im2bin(const Image *im, Imtype bk){ + if(!im) return NULL; + int W = im->width, H = im->height; + if(W < 2 || H < 2) return NULL; + int y, W0 = (W + 7) / 8, s1 = (W/8 == W0) ? W0 : W0 - 1; + uint8_t *ret = MALLOC(uint8_t, W0 * H); + OMP_FOR() + for(y = 0; y < H; ++y){ + Imtype *iptr = &im->data[y*W]; + uint8_t *optr = &ret[y*W0]; + for(int x = 0; x < s1; ++x){ + register uint8_t o = 0; + for(int i = 0; i < 8; ++i){ + o <<= 1; + if(*iptr++ > bk) o |= 1; + } + *optr++ = o; + } + int rest = W - s1*8; + if(rest){ + register uint8_t o = 0; + for(int x = 0; x < rest; ++x){ + o <<= 1; + if(*iptr++ > bk) o |= 1; + } + *optr = o << (8 - rest); + } + } + return ret; +} + +#if 0 +UNUSED function! Need to be refactored +// convert size_t labels into Image +Image *ST2Im(const size_t *image, int W, int H){ + Image *ret = Image_new(W, H); + OMP_FOR() + for(int y = 0; y < H; ++y){ + Imtype *optr = &ret->data[y*W]; + const size_t *iptr = &image[y*W]; + for(int x = 0; x < W; ++x){ + *optr++ = (Imtype)*iptr++; + } + } + Image_minmax(ret); + return ret; +} +#endif + +/** + * Convert "packed" image into size_t array for conncomp procedure + * @param image (i) - input image + * @param W, H - size of image in pixels + * @return allocated memory area with copy of an image + */ +size_t *bin2ST(const uint8_t *image, int W, int H){ + size_t *ret = MALLOC(size_t, W * H); + int W0 = (W + 7) / 8, s1 = W0 - 1; + OMP_FOR() + for(int y = 0; y < H; y++){ + size_t *optr = &ret[y*W]; + const uint8_t *iptr = &image[y*W0]; + for(int x = 0; x < s1; ++x){ + register uint8_t inp = *iptr++; + for(int i = 0; i < 8; ++i){ + *optr++ = (inp & 0x80) ? 1 : 0; + inp <<= 1; + } + } + int rest = W - s1*8; + if(rest){ + register uint8_t inp = *iptr; + for(int i = 0; i < rest; ++i){ + *optr++ = (inp & 0x80) ? 1 : 0; + inp <<= 1; + } + } + } + return ret; +} + + +/* + * <=================== CONVERT IMAGE TYPES =================== + */ diff --git a/LocCorr_new/imagefile.h b/LocCorr_new/imagefile.h new file mode 100644 index 0000000..6cc9217 --- /dev/null +++ b/LocCorr_new/imagefile.h @@ -0,0 +1,72 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#pragma once +#ifndef IMAGEFILE_H__ +#define IMAGEFILE_H__ + +#include +#include // size_t + +#ifndef OMP_FOR +#define Stringify(x) #x +#define OMP_FOR(x) _Pragma(Stringify(omp parallel for x)) +#endif + +typedef uint8_t Imtype; // maybe float or double only + +typedef struct{ + int width; // width + int height; // height + Imtype *data; // picture data + Imtype minval; // extremal data values + Imtype maxval; +} Image; + +// input file/directory type +typedef enum{ + T_WRONG, + T_DIRECTORY, + T_BMP, + T_FITS, + T_GZIP, + T_GIF, + T_JPEG, + T_PNG, + T_CAPT_GRASSHOPPER, // capture grasshopper + T_CAPT_BASLER +} InputType; + +void Image_minmax(Image *I); +uint8_t *linear(const Image *I, int nchannels); +uint8_t *equalize(const Image *I, int nchannels, double throwpart); +InputType chkinput(const char *name); +Image *Image_read(const char *name); +Image *Image_new(int w, int h); +Image *Image_sim(const Image *i); +void Image_free(Image **I); +int Image_write_jpg(const Image *I, const char *name, int equalize); +size_t *get_histogram(const Image *I); +int calc_background(const Image *img, Imtype *bk); + +Image *u8toImage(const uint8_t *data, int width, int height, int stride); +Image *bin2Im(const uint8_t *image, int W, int H); +uint8_t *Im2bin(const Image *im, Imtype bk); +size_t *bin2ST(const uint8_t *image, int W, int H); +//Image *ST2Im(const size_t *image, int W, int H); + +#endif // IMAGEFILE_H__ diff --git a/LocCorr_new/improc.c b/LocCorr_new/improc.c new file mode 100644 index 0000000..5cfd88f --- /dev/null +++ b/LocCorr_new/improc.c @@ -0,0 +1,351 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include +#include +#include +#include + +#include "basler.h" +#include "binmorph.h" +#include "cameracapture.h" +#include "cmdlnopts.h" +#include "config.h" +#include "debug.h" +#include "draw.h" +#include "grasshopper.h" +#include "fits.h" +#include "improc.h" +#include "inotify.h" +#include "median.h" +#include "steppers.h" + +volatile atomic_ullong ImNumber = 0; // GLOBAL: counter of processed images +volatile atomic_bool stopwork = FALSE; // GLOBAL: suicide + +// GLOBAL: get image information +char *(*imagedata)(const char *messageid, char *buf, int buflen) = NULL; + +static FILE *fXYlog = NULL; +static double tstart = 0.; // time of logging start +static double FPS = 0.; // frames per second +static float xc = -1., yc = -1.; // center coordinates + +typedef struct{ + uint32_t area; // object area in pixels + double Isum; // total object's intensity over background + double WdivH; // width of object's box divided by height + double xc; // centroid coordinates + double yc; + double xsigma; // STD by horizontal and vertical axes + double ysigma; +} object; + +// functions for Qsort +static int compIntens(const void *a, const void *b){ // compare by intensity + const object *oa = (const object*)a; + const object *ob = (const object*)b; + double idiff = (oa->Isum - ob->Isum)/(oa->Isum + ob->Isum); + if(fabs(idiff) > theconf.intensthres) return (idiff > 0) ? -1:1; + double r2a = oa->xc * oa->xc + oa->yc * oa->yc; + double r2b = ob->xc * ob->xc + ob->yc * ob->yc; + return (r2a < r2b) ? -1 : 1; +} +static int compDist(const void *a, const void *b){ // compare by distanse from target + const object *oa = (const object*)a; + const object *ob = (const object*)b; + double xtg = theconf.xtarget - theconf.xoff, ytg = theconf.ytarget - theconf.yoff; + double xa = oa->xc - xtg, xb = ob->xc - xtg, + ya = oa->yc - ytg, yb = ob->yc - ytg; + double r2a = xa*xa + ya*ya; + double r2b = xb*xb + yb*yb; + return (r2a < r2b) ? -1 : 1; +} + +static void XYnewline(){ + if(!fXYlog) return; + fprintf(fXYlog, "\n"); + fflush(fXYlog); +} + +static void getDeviation(object *curobj){ + int averflag = 0; + static double Xc[NAVER_MAX+1], Yc[NAVER_MAX+1]; + double xx = curobj->xc, yy = curobj->yc, xsum2 = 0., ysum2 = 0.; + double Sx = 0., Sy = 0.; + static int counter = 0; + Xc[counter] = curobj->xc; Yc[counter] = curobj->yc; + if(fXYlog){ // make log record + fprintf(fXYlog, "%.2f\t%.1f\t%.1f\t%.1f\t%.1f\t%.1f\t", + dtime(), curobj->xc, curobj->yc, + curobj->xsigma, curobj->ysigma, curobj->WdivH); + } + //DBG("counter = %d", counter); + if(++counter < theconf.naverage){ + goto process_corrections; + } + // it's time to calculate average deviations + xx = 0.; yy = 0.; + for(int i = 0; i < counter; ++i){ + double x = Xc[i], y = Yc[i]; + xx += x; yy += y; + xsum2 += x*x; ysum2 += y*y; + } + xx /= counter; yy /= counter; + Sx = sqrt(xsum2/counter - xx*xx); + Sy = sqrt(ysum2/counter - yy*yy); + counter = 0; +#ifdef EBUG + green("\n Average centroid: X=%.1f (+-%.1f), Y=%.1f (+-%.1f)\n", xx, Sx, yy, Sy); +#endif + LOGDBG("getDeviation(): Average centroid: X=%.1f (+-%.1f), Y=%.1f (+-%.1f)", xx, Sx, yy, Sy); + averflag = 1; + if(fXYlog) fprintf(fXYlog, "%.1f\t%.1f\t%.1f\t%.1f", xx, yy, Sx, Sy); +process_corrections: + if(theSteppers && theSteppers->proc_corr && averflag){ + if(Sx > XY_TOLERANCE || Sy > XY_TOLERANCE){ + LOGDBG("Bad value - not process"); // don't run processing for bad data + }else + theSteppers->proc_corr(xx, yy); + }else{ + LOGERR("Lost connection with stepper server"); + ERRX("Lost connection with stepper server"); + } + XYnewline(); +} + +void process_file(Image *I){ + static double lastTproc = 0.; +/* +#ifdef EBUG + double t0 = dtime(), tlast = t0; +#define DELTA(p) do{double t = dtime(); DBG("---> %s @ %gms (delta: %gms)", p, (t-t0)*1e3, (t-tlast)*1e3); tlast = t;}while(0) +#else +*/ +#define DELTA(x) +//#endif + // I - original image + // mean - local mean + // std - local STD + /**** read original image ****/ + DELTA("Imread"); + if(!I){ + WARNX("No image"); + return; + } + int W = I->width, H = I->height; + //save_fits(I, "fitsout.fits"); + //DELTA("Save original"); + Imtype bk; + if(calc_background(I, &bk)){ + DBG("backgr = %d", bk); + DELTA("Got background"); + uint8_t *ibin = Im2bin(I, bk); + DELTA("Made binary"); + if(ibin){ + //savebin(ibin, W, H, "binary.fits"); + //DELTA("save binary.fits"); + uint8_t *er = il_erosionN(ibin, W, H, theconf.Nerosions); + FREE(ibin); + DELTA("Erosion"); + //savebin(er, W, H, "erosion.fits"); + //DELTA("Save erosion"); + uint8_t *opn = il_dilationN(er, W, H, theconf.Ndilations); + FREE(er); + DELTA("Opening"); + //savebin(opn, W, H, "opening.fits"); + //DELTA("Save opening"); + il_ConnComps *cc = NULL; + size_t *S = il_cclabel4(opn, W, H, &cc); + FREE(opn); + if(cc->Nobj > 1){ // Nobj = amount of objects + 1 + DBGLOG("Nobj=%d", cc->Nobj-1); + object *Objects = MALLOC(object, cc->Nobj-1); + int objctr = 0; + for(size_t i = 1; i < cc->Nobj; ++i){ + il_Box *b = &cc->boxes[i]; + double wh = ((double)b->xmax - b->xmin)/(b->ymax - b->ymin); + //DBG("Obj# %zd: wh=%g, area=%d", i, wh, b->area); + if(wh < theconf.minwh || wh > theconf.maxwh) continue; + if((int)b->area < theconf.minarea || (int)b->area > theconf.maxarea) continue; + double xc = 0., yc = 0.; + double x2c = 0., y2c = 0., Isum = 0.; + for(size_t y = b->ymin; y <= b->ymax; ++y){ + size_t idx = y*W + b->xmin; + size_t *maskptr = &S[idx]; + Imtype *Iptr = &I->data[idx]; + for(size_t x = b->xmin; x <= b->xmax; ++x, ++maskptr, ++Iptr){ + if(*maskptr != i) continue; + double intens = (double) (*Iptr - bk); + if(intens < 0.) continue; + double xw = x * intens, yw = y * intens; + xc += xw; + yc += yw; + x2c += xw * x; + y2c += yw * y; + Isum += intens; + } + } + xc /= Isum; yc /= Isum; + x2c = x2c/Isum - xc*xc; + y2c = y2c/Isum - yc*yc; + Objects[objctr++] = (object){ + .area = b->area, .Isum = Isum, + .WdivH = wh, .xc = xc, .yc = yc, + .xsigma = sqrt(x2c), .ysigma = sqrt(y2c) + }; + } + DELTA("Labeling"); + DBGLOG("T%.2f, N=%d\n", dtime(), objctr); + if(objctr > 1){ + if(theconf.starssort) + qsort(Objects, objctr, sizeof(object), compIntens); + else + qsort(Objects, objctr, sizeof(object), compDist); + } + if(objctr){ +#ifdef EBUG + object *o = Objects; + green("%6s\t%6s\t%6s\t%6s\t%6s\t%6s\t%6s\t%6s\t%8s\n", + "N", "Area", "Mv", "W/H", "Xc", "Yc", "Sx", "Sy", "Area/r^2"); + for(int i = 0; i < objctr; ++i, ++o){ + // 1.0857 = 2.5/ln(10) + printf("%6d\t%6d\t%6.1f\t%6.1f\t%6.1f\t%6.1f\t%6.1f\t%6.1f\t%8.1f\n", + i, o->area, 20.-1.0857*log(o->Isum), o->WdivH, o->xc, o->yc, + o->xsigma, o->ysigma, o->area/o->xsigma/o->ysigma); + } +#endif + getDeviation(Objects); // calculate dX/dY and process corrections + } + { // prepare image and save jpeg + uint8_t *outp = NULL; + if(theconf.equalize) + outp = equalize(I, 3, theconf.throwpart); + else + outp = linear(I, 3); + static il_Pattern *cross = NULL, *crossL = NULL; + if(!cross) cross = il_Pattern_xcross(33, 33); + if(!crossL) crossL = il_Pattern_xcross(51, 51); + il_Img3 i3 = {.data = outp, .w = I->width, .h = H}; + // draw fiber center position + il_Pattern_draw3(&i3, crossL, theconf.xtarget-theconf.xoff, H-(theconf.ytarget-theconf.yoff), C_R); + if(objctr){ // draw crosses @ objects' centers + int H = I->height; + // draw current star centroid + il_Pattern_draw3(&i3, cross, Objects[0].xc, H-Objects[0].yc, C_G); + // add offset to show in target system + xc = Objects[0].xc + theconf.xoff; + yc = Objects[0].yc + theconf.yoff; + // draw other centroids + for(int i = 1; i < objctr; ++i) + il_Pattern_draw3(&i3, cross, Objects[i].xc, H-Objects[i].yc, C_B); + }else{xc = -1.; yc = -1.;} + char *tmpnm = MALLOC(char, strlen(GP->outputjpg) + 5); + sprintf(tmpnm, "%s-tmp", GP->outputjpg); + if(stbi_write_jpg(tmpnm, I->width, I->height, 3, outp, 95)){ + if(rename(tmpnm, GP->outputjpg)){ + WARN("rename()"); + LOGWARN("can't save %s", GP->outputjpg); + } + } + FREE(tmpnm); + FREE(outp); + } + FREE(Objects); + }else{ + xc = -1.; yc = -1.; + Image_write_jpg(I, GP->outputjpg, theconf.equalize); + } + DBGLOG("Image saved"); + FREE(S); + FREE(cc); + } + }else Image_write_jpg(I, GP->outputjpg, theconf.equalize); + ++ImNumber; + if(lastTproc > 1.) FPS = 1. / (dtime() - lastTproc); + lastTproc = dtime(); + DELTA("End"); +} + +static char *localimages(const char *messageid, int isdir, char *buf, int buflen){ + static char *impath = NULL; + if(!impath){ + if(!realpath(GP->outputjpg, impath)) impath = strdup(GP->outputjpg); + } + snprintf(buf, buflen, "{ \"%s\": \"%s\", \"camstatus\": \"watch %s\", \"impath\": \"%s\", \"xcenter\": %.1f, \"ycenter\": %.1f }", + MESSAGEID, messageid, isdir ? "directory" : "file", impath, xc, yc); + return buf; +} +static char *watchdr(const char *messageid, char *buf, int buflen){ + return localimages(messageid, 1, buf, buflen); +} +static char *watchfl(const char *messageid, char *buf, int buflen){ + return localimages(messageid, 0, buf, buflen); +} + +int process_input(InputType tp, char *name){ + DBG("process_input(%d, %s)", tp, name); + if(tp == T_DIRECTORY){ + imagedata = watchdr; + return watch_directory(name, process_file); + }else if(tp == T_CAPT_GRASSHOPPER || tp == T_CAPT_BASLER){ + camera *cam = &GrassHopper; + if(tp == T_CAPT_BASLER) cam = &Basler; + if(!setCamera(cam)){ + WARNX("The camera disconnected"); + LOGWARN("The camera disconnected"); + } + imagedata = camstatus; + return camcapture(process_file); + } + imagedata = watchfl; + return watch_file(name, process_file); +} + +/** + * @brief openXYlog - open file to log XY values + * @param name - filename + */ +void openXYlog(const char *name){ + closeXYlog(); + fXYlog = fopen(name, "a"); + if(!fXYlog){ + char *e = strerror(errno); + WARNX("Can't create file %s: %s", name, e); + LOGERR("Can't create file %s: %s", name, e); + return; + } + time_t t = time(NULL); + fprintf(fXYlog, "# Start at: %s", ctime(&t)); + fprintf(fXYlog, "# time Xc\tYc\t\tSx\tSy\tW/H\taverX\taverY\tSX\tSY\n"); + fflush(fXYlog); + tstart = dtime(); +} +void closeXYlog(){ + if(!fXYlog) return; + fclose(fXYlog); + fXYlog = NULL; +} + +double getFramesPerS(){ return FPS; } + +void getcenter(float *x, float *y){ + if(x) *x = xc; + if(y) *y = yc; +} diff --git a/LocCorr_new/improc.h b/LocCorr_new/improc.h new file mode 100644 index 0000000..fcd7659 --- /dev/null +++ b/LocCorr_new/improc.h @@ -0,0 +1,41 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#pragma once +#ifndef IMPROC_H__ +#define IMPROC_H__ + +#include + +#include "imagefile.h" + +// tolerance of deviations by X and Y axis (if sigmaX or sigmaY greater, values considered to be wrong) +#define XY_TOLERANCE (5.) + +extern volatile atomic_bool stopwork; +extern volatile atomic_ullong ImNumber; + +extern char *(*imagedata)(const char *messageid, char *buf, int buflen); + +void process_file(Image *I); +int process_input(InputType tp, char *name); +void openXYlog(const char *name); +void closeXYlog(); +double getFramesPerS(); +void getcenter(float *x, float *y); + +#endif // IMPROC_H__ diff --git a/LocCorr_new/inotify.c b/LocCorr_new/inotify.c new file mode 100644 index 0000000..6d46ed9 --- /dev/null +++ b/LocCorr_new/inotify.c @@ -0,0 +1,129 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include +#include +#include // strlen +#include +#include +#include + +#include "debug.h" +#include "imagefile.h" + +static char filenm[FILENAME_MAX]; + +// handle inotify event IN_CLOSE_WRITE +/** + * @brief handle_events - handle inotify event IN_CLOSE_WRITE + * @param name - directory or file name + * @param fd - inotify init descriptor + * @return 1 if file was changed and ready to be processed, 0 if not changed, -1 if removed + */ +static int changed(const char *name, int fd, uint32_t mask){ + struct inotify_event buf[10] = {0}; + ssize_t len = read(fd, buf, sizeof buf); + if(len == -1 && errno != EAGAIN){ + WARN("inotify read()"); + return -1; + } + if(len < 1) return 0; // not ready + uint32_t bm = buf[0].mask; + buf[0].mask = 0; + for(int i = 0; i < 10; ++i) + DBG("CHANGED: %s (%d)\n", buf[i].name, bm); + if(bm & mask){ + if(name){ + snprintf(filenm, FILENAME_MAX, "%s/%s", name, buf[0].name); // full path + }else{ + snprintf(filenm, FILENAME_MAX, "%s", buf[0].name); // file name + } + return 1; + } + DBG("IN_IGNORED"); + return -1; // IN_IGNORED (file removed) +} + +static int initinot(const char *name, int *filed, uint32_t mask){ + static int fd = -1, wd = -1; + if(wd > -1){ + inotify_rm_watch(fd, wd); + wd = -1; fd = -1; + } + if(fd == -1) fd = inotify_init();//1(IN_NONBLOCK); + if(fd == -1){ + WARN("inotify_init()"); + return 0; + } + if(-1 == (wd = inotify_add_watch(fd, name, mask))){ + WARN("inotify_add_watch()"); + return 0; + } + if(filed) *filed = fd; + return wd; +} + +static int watch_any(const char *name, void (*process)(Image*), uint32_t mask){ + int fd = -1, wd = -1; + while(1){ + if(wd < 1 || fd < 1){ + wd = initinot(name, &fd, mask); + if(wd < 1){ + usleep(1000); + continue; + } + } + int ch; + if(mask == IN_CLOSE_WRITE) + ch = changed(NULL, fd, mask); // only file name + else + ch = changed(name, fd, mask); // full path + if(ch == -1){ sleep(1); wd = -1; continue; } // removed + if(ch == 1){ // changed + if(process){ + Image *I = Image_read(filenm); + process(I); + } + } + } + close(fd); + return 0; +} + + + +int watch_file(const char *name, void (*process)(Image*)){ + FNAME(); + if(!name){ + WARNX("Need filename"); + return 1; + } + return watch_any(name, process, IN_CLOSE_WRITE); +} + +int watch_directory(char *name, void (*process)(Image*)){ + FNAME(); + if(!name){ + WARNX("Need directory name"); + return 1; + } + int l = strlen(name) - 1; + if(name[l] == '/') name[l] = 0; + return watch_any(name, process, IN_CLOSE_WRITE|IN_ISDIR); +} diff --git a/LocCorr_new/inotify.h b/LocCorr_new/inotify.h new file mode 100644 index 0000000..19caecc --- /dev/null +++ b/LocCorr_new/inotify.h @@ -0,0 +1,28 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once +#ifndef INOTIFY_H__ +#define INOTIFY_H__ + +#include "fits.h" // Image* + +int watch_file(const char *name, void (*process)(Image*)); +int watch_directory(char *name, void (*process)(Image*)); + +#endif // INOTIFY_H__ diff --git a/LocCorr_new/loccorr.conf b/LocCorr_new/loccorr.conf new file mode 100644 index 0000000..8c05aa5 --- /dev/null +++ b/LocCorr_new/loccorr.conf @@ -0,0 +1,36 @@ +maxarea = 4000 +minarea = 100 +minwh = 0.800 +maxwh = 1.300 +ndilat = 3 +neros = 3 +xoffset = 0 +yoffset = 0 +width = 1920 +height = 1200 +equalize = 0 +expmethod = 0 +naverage = 3 +umax = 400 +vmax = 230 +focmax = 4500 +focmin = 20 +stpservport = 4444 +Kxu = 45.155 +Kyu = 68.838 +Kxv = 43.771 +Kyv = -70.428 +xtarget = 1039.600 +ytarget = 602.820 +eqthrowpart = 0.900 +minexp = 50.000 +maxexp = 1000.000 +fixedexp = 1000.000 +intensthres = 0.010 +gain = 36.000 +brightness = 0.000 +starssort = 0 +medfilt = 0 +medseed = 1 +fixedbg = 0 +fbglevel = 100 diff --git a/LocCorr_new/main.c b/LocCorr_new/main.c new file mode 100644 index 0000000..8381e40 --- /dev/null +++ b/LocCorr_new/main.c @@ -0,0 +1,234 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include +#include // signal +#include +#include // strdup +#include //prctl +#include // wait + +#include "cmdlnopts.h" +#include "config.h" +#include "debug.h" +#include "improc.h" +#include "steppers.h" +#include "socket.h" + +static InputType tp; +static pid_t childpid; + +/** + * We REDEFINE the default WEAK function of signal processing + */ +void signals(int sig){ + DBG("SIGN: %d, child PID: %d", sig, childpid); + if(childpid){ + DBG("Father -> just exit"); + exit(sig); // father -> do nothin @ end + } + if(sig){ + signal(sig, SIG_IGN); + DBG("Get signal %d.", sig); + } + stopwork = TRUE; + saveconf(NULL); + if(GP && GP->pidfile){ // remove unnesessary PID file + DBG("unlink(GP->pidfile)"); + unlink(GP->pidfile); + } + if(theSteppers && theSteppers->stepdisconnect) theSteppers->stepdisconnect(); + DBG("closeXYlog()"); + closeXYlog(); + DBG("EXIT %d", sig); + LOGERR("Exit with status %d", sig); + exit(sig); +} + +void iffound_default(pid_t pid){ + ERRX("Another copy of this process found, pid=%d. Exit.", pid); +} + +static void *procinp_thread(_U_ void* arg){ + int p = process_input(tp, GP->inputname); + LOGDBG("process_input=%d", p); + return NULL; +} + +static InputType chk_inp(const char *name){ + if(!name) ERRX("Point file or directory name to monitor"); + InputType itp = chkinput(GP->inputname); + if(T_WRONG == itp) return T_WRONG; + green("\n%s is a ", name); + switch(itp){ + case T_DIRECTORY: + printf("directory"); + break; + case T_JPEG: + printf("jpeg"); + break; + case T_PNG: + printf("png"); + break; + case T_GIF: + printf("gif"); + break; + case T_FITS: + printf("fits"); + break; + case T_BMP: + printf("bmp"); + break; + case T_GZIP: + printf("maybe fits.gz?"); + break; + case T_CAPT_GRASSHOPPER: + printf("capture grasshopper camera"); + break; + case T_CAPT_BASLER: + printf("capture basler camera"); + break; + default: + printf("Unsupported type\n"); + return T_WRONG; + } + printf("\n"); + return itp; +} + +int main(int argc, char *argv[]){ + initial_setup(); + char *self = strdup(argv[0]); + GP = parse_args(argc, argv); + if(GP->throwpart < 0. || GP->throwpart > 0.99){ + ERRX("Fraction of black pixels should be in [0., 0.99]"); + } + if(GP->Naveraging < 1 || GP->Naveraging > NAVER_MAX) + ERRX("Averaging amount should be from 1 to %d", NAVER_MAX); + tp = chk_inp(GP->inputname); + if(tp == T_WRONG) ERRX("Enter correct image file or directory name"); + // check ability of saving file + { + FILE *f = fopen(GP->outputjpg, "w"); + if(!f) ERR("Can't create %s", GP->outputjpg); + fclose(f); + } + if(GP->logfile){ + sl_loglevel lvl = LOGLEVEL_ERR; // default log level - errors + int v = GP->verb; + while(v--){ // increase loglevel for each "-v" + if(++lvl == LOGLEVEL_ANY) break; + } + OPENLOG(GP->logfile, lvl, 1); + DBG("Opened log file @ level %d", lvl); + } + int C = chkconfig(GP->configname); + if(!C){ + LOGWARN("Wrong/absent configuration file"); + WARNX("Wrong/absent configuration file"); + } + // change `theconf` parameters to user values + { + if(GP->maxarea != DEFAULT_MAXAREA || theconf.maxarea == 0) theconf.maxarea = GP->maxarea; + if(GP->minarea != DEFAULT_MINAREA || theconf.minarea == 0) theconf.minarea = GP->minarea; + if(GP->xtarget > 0.) theconf.xtarget = GP->xtarget; + if(GP->ytarget > 0.) theconf.ytarget = GP->ytarget; + if(GP->nerosions != DEFAULT_EROSIONS || theconf.Nerosions == 0){ + if(GP->nerosions < 1 || GP->nerosions > MAX_NEROS) ERRX("Amount of erosions should be from 1 to %d", MAX_NEROS); + theconf.Nerosions = GP->nerosions; + } + if(GP->ndilations != DEFAULT_DILATIONS || theconf.Ndilations == 0){ + if(GP->ndilations < 1 || GP->ndilations > MAX_NDILAT) ERRX("Amount of erosions should be from 1 to %d", MAX_NDILAT); + theconf.Ndilations = GP->ndilations; + } + if(fabs(GP->throwpart - DEFAULT_THROWPART) > DBL_EPSILON || theconf.throwpart < DBL_EPSILON){ + if(GP->throwpart < 0. || GP->throwpart > MAX_THROWPART) ERRX("'thworpart' should be from 0 to %g", MAX_THROWPART); + theconf.throwpart = GP->throwpart; + } + if(GP->xoff && GP->xoff < MAX_OFFSET) theconf.xoff = GP->xoff; + if(GP->yoff && GP->yoff < MAX_OFFSET) theconf.yoff = GP->yoff; + if(GP->width && GP->width < MAX_OFFSET) theconf.width = GP->width; + if(GP->height && GP->height < MAX_OFFSET) theconf.height = GP->height; + if(fabs(GP->minexp - EXPOS_MIN) > DBL_EPSILON || theconf.minexp < DBL_EPSILON){ + if(GP->minexp < DBL_EPSILON || GP->minexp > EXPOS_MAX) ERRX("Minimal exposition should be > 0 and < %g", EXPOS_MAX); + theconf.minexp = GP->minexp; + } + if(fabs(GP->maxexp - EXPOS_MAX) > DBL_EPSILON || theconf.maxexp < theconf.minexp){ + if(GP->maxexp < theconf.minexp) ERRX("Maximal exposition should be greater than minimal"); + theconf.maxexp = GP->maxexp; + } + if(GP->equalize && theconf.equalize == 0) theconf.equalize = 1; + if(fabs(GP->intensthres - DEFAULT_INTENSTHRES) > DBL_EPSILON){ + if(GP->intensthres < DBL_EPSILON || GP->intensthres > 1.-DBL_EPSILON) ERRX("'intensthres' should be from 0 to 1"); + theconf.intensthres = GP->intensthres; + } + if(GP->Naveraging != DEFAULT_NAVERAGE || theconf.naverage < 1){ + if(GP->Naveraging < 1 || GP->Naveraging > NAVER_MAX) ERRX("N images for averaging should be from 1 to %d", NAVER_MAX); + theconf.naverage = GP->Naveraging; + } + if(GP->steppersport != DEFAULT_STEPPERSPORT || theconf.stpserverport == 0){ + if(GP->steppersport < 1 || GP->steppersport > 65535) ERRX("Wrong steppers' server port: %d", GP->steppersport); + theconf.stpserverport = GP->steppersport; + } + } + check4running(self, GP->pidfile); + DBG("%s started, snippets library version is %s\n", self, sl_libversion()); + free(self); self = NULL; + signal(SIGTERM, signals); // kill (-15) - quit + signal(SIGHUP, SIG_IGN); // hup - ignore + signal(SIGINT, signals); // ctrl+C - quit + signal(SIGQUIT, signals); // ctrl+\ - quit + signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z + while(1){ // guard for dead processes + childpid = fork(); + if(childpid){ // father + LOGMSG("create child with PID %d\n", childpid); + DBG("Created child with PID %d\n", childpid); + wait(NULL); + LOGMSG("child %d died\n", childpid); + WARNX("Child %d died\n", childpid); + sleep(5); + }else{ // son + //prctl(PR_SET_PDEATHSIG, SIGTERM); // send SIGTERM to child when parent dies + break; // go out to normal functional + } + } + if(!(theSteppers = steppers_connect())){ + LOGERR("Steppers server unavailable, can't run"); + ERRX("Steppers server unavailable, can't run"); + } + if(GP->logXYname) openXYlog(GP->logXYname); + LOGMSG("Start application..."); + LOGDBG("xtag=%g, ytag=%g", theconf.xtarget, theconf.ytarget); + openIOport(GP->ioport); + pthread_t inp_thread; + if(pthread_create(&inp_thread, NULL, procinp_thread, NULL)){ + LOGERR("pthread_create() for image input failed"); + ERR("pthread_create()"); + } + while(1){ + if(stopwork || pthread_kill(inp_thread, 0) == ESRCH){ + DBG("close"); + pthread_join(inp_thread, NULL); + DBG("out"); + return 0; + } + }; + return 0; +} diff --git a/LocCorr_new/median.c b/LocCorr_new/median.c new file mode 100644 index 0000000..503eab7 --- /dev/null +++ b/LocCorr_new/median.c @@ -0,0 +1,446 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +// FOR MEDIATOR: +// Copyright (c) 2011 ashelly.myopenid.com under + + +// TODO: resolve problem with borders + +#include +#include +#include +#include + +#include "config.h" +#include "debug.h" +#include "imagefile.h" +#include "median.h" + + +#define ELEM_SWAP(a, b) {register Imtype t = a; a = b; b = t;} +#define PIX_SORT(a, b) {if (p[a] > p[b]) ELEM_SWAP(p[a], p[b]);} + +static inline Imtype mean(Imtype a, Imtype b){ + register uint16_t x = ((uint16_t)a + (uint16_t)b) / 2; + return (Imtype)x; +} + +static Imtype opt_med2(Imtype *p){ + return mean(p[0], p[1]); +} +static Imtype opt_med3(Imtype *p){ + PIX_SORT(0, 1); PIX_SORT(1, 2); PIX_SORT(0, 1); + return(p[1]) ; +} +static Imtype opt_med4(Imtype *p){ + PIX_SORT(0, 2); PIX_SORT(1, 3); + PIX_SORT(0, 1); PIX_SORT(2, 3); + return mean(p[1], p[2]); +} +static Imtype opt_med5(Imtype *p){ + PIX_SORT(0, 1); PIX_SORT(3, 4); PIX_SORT(0, 3); + PIX_SORT(1, 4); PIX_SORT(1, 2); PIX_SORT(2, 3) ; + PIX_SORT(1, 2); + return(p[2]) ; +} +// even values are from "FAST, EFFICIENT MEDIAN FILTERS WITH EVEN LENGTH WINDOWS", J.P. HAVLICEK, K.A. SAKADY, G.R.KATZ +static Imtype opt_med6(Imtype *p){ + PIX_SORT(1, 2); PIX_SORT(3, 4); + PIX_SORT(0, 1); PIX_SORT(2, 3); PIX_SORT(4, 5); + PIX_SORT(1, 2); PIX_SORT(3, 4); + PIX_SORT(0, 1); PIX_SORT(2, 3); PIX_SORT(4, 5); + PIX_SORT(1, 2); PIX_SORT(3, 4); + return mean(p[2], p[3]); +} +static Imtype opt_med7(Imtype *p){ + PIX_SORT(0, 5); PIX_SORT(0, 3); PIX_SORT(1, 6); + PIX_SORT(2, 4); PIX_SORT(0, 1); PIX_SORT(3, 5); + PIX_SORT(2, 6); PIX_SORT(2, 3); PIX_SORT(3, 6); + PIX_SORT(4, 5); PIX_SORT(1, 4); PIX_SORT(1, 3); + PIX_SORT(3, 4); return (p[3]); +} +// optimal Batcher's sort for 8 elements (http://myopen.googlecode.com/svn/trunk/gtkclient_tdt/include/fast_median.h) +static Imtype opt_med8(Imtype *p){ + PIX_SORT(0, 4); PIX_SORT(1, 5); PIX_SORT(2, 6); + PIX_SORT(3, 7); PIX_SORT(0, 2); PIX_SORT(1, 3); + PIX_SORT(4, 6); PIX_SORT(5, 7); PIX_SORT(2, 4); + PIX_SORT(3, 5); PIX_SORT(0, 1); PIX_SORT(2, 3); + PIX_SORT(4, 5); PIX_SORT(6, 7); PIX_SORT(1, 4); + PIX_SORT(3, 6); + return mean(p[3], p[4]); +} +static Imtype opt_med9(Imtype *p){ + PIX_SORT(1, 2); PIX_SORT(4, 5); PIX_SORT(7, 8); + PIX_SORT(0, 1); PIX_SORT(3, 4); PIX_SORT(6, 7); + PIX_SORT(1, 2); PIX_SORT(4, 5); PIX_SORT(7, 8); + PIX_SORT(0, 3); PIX_SORT(5, 8); PIX_SORT(4, 7); + PIX_SORT(3, 6); PIX_SORT(1, 4); PIX_SORT(2, 5); + PIX_SORT(4, 7); PIX_SORT(4, 2); PIX_SORT(6, 4); + PIX_SORT(4, 2); return(p[4]); +} +static Imtype opt_med16(Imtype *p){ + PIX_SORT(0, 8); PIX_SORT(1, 9); PIX_SORT(2, 10); PIX_SORT(3, 11); + PIX_SORT(4, 12); PIX_SORT(5, 13); PIX_SORT(6, 14); PIX_SORT(7, 15); + PIX_SORT(0, 4); PIX_SORT(1, 5); PIX_SORT(2, 6); PIX_SORT(3, 7); + PIX_SORT(8, 12); PIX_SORT(9, 13); PIX_SORT(10, 14); PIX_SORT(11, 15); + PIX_SORT(4, 8); PIX_SORT(5, 9); PIX_SORT(6, 10); PIX_SORT(7, 11); + PIX_SORT(0, 2); PIX_SORT(1, 3); PIX_SORT(4, 6); PIX_SORT(5, 7); + PIX_SORT(8, 10); PIX_SORT(9, 11); PIX_SORT(12, 14); PIX_SORT(13, 15); + PIX_SORT(2, 8); PIX_SORT(3, 9); PIX_SORT(6, 12); PIX_SORT(7, 13); + PIX_SORT(2, 4); PIX_SORT(3, 5); PIX_SORT(6, 8); PIX_SORT(7, 9); + PIX_SORT(10, 12); PIX_SORT(11, 13); PIX_SORT(0, 1); PIX_SORT(2, 3); + PIX_SORT(4, 5); PIX_SORT(6, 7); PIX_SORT(8, 9); PIX_SORT(10, 11); + PIX_SORT(12, 13); PIX_SORT(14, 15); PIX_SORT(1, 8); PIX_SORT(3, 10); + PIX_SORT(5, 12); PIX_SORT(7, 14); PIX_SORT(5, 8); PIX_SORT(7, 10); + return mean(p[7], p[8]); +} +static Imtype opt_med25(Imtype *p){ + PIX_SORT(0, 1) ; PIX_SORT(3, 4) ; PIX_SORT(2, 4) ; + PIX_SORT(2, 3) ; PIX_SORT(6, 7) ; PIX_SORT(5, 7) ; + PIX_SORT(5, 6) ; PIX_SORT(9, 10) ; PIX_SORT(8, 10) ; + PIX_SORT(8, 9) ; PIX_SORT(12, 13); PIX_SORT(11, 13) ; + PIX_SORT(11, 12); PIX_SORT(15, 16); PIX_SORT(14, 16) ; + PIX_SORT(14, 15); PIX_SORT(18, 19); PIX_SORT(17, 19) ; + PIX_SORT(17, 18); PIX_SORT(21, 22); PIX_SORT(20, 22) ; + PIX_SORT(20, 21); PIX_SORT(23, 24); PIX_SORT(2, 5) ; + PIX_SORT(3, 6) ; PIX_SORT(0, 6) ; PIX_SORT(0, 3) ; + PIX_SORT(4, 7) ; PIX_SORT(1, 7) ; PIX_SORT(1, 4) ; + PIX_SORT(11, 14); PIX_SORT(8, 14) ; PIX_SORT(8, 11) ; + PIX_SORT(12, 15); PIX_SORT(9, 15) ; PIX_SORT(9, 12) ; + PIX_SORT(13, 16); PIX_SORT(10, 16); PIX_SORT(10, 13) ; + PIX_SORT(20, 23); PIX_SORT(17, 23); PIX_SORT(17, 20) ; + PIX_SORT(21, 24); PIX_SORT(18, 24); PIX_SORT(18, 21) ; + PIX_SORT(19, 22); PIX_SORT(8, 17) ; PIX_SORT(9, 18) ; + PIX_SORT(0, 18) ; PIX_SORT(0, 9) ; PIX_SORT(10, 19) ; + PIX_SORT(1, 19) ; PIX_SORT(1, 10) ; PIX_SORT(11, 20) ; + PIX_SORT(2, 20) ; PIX_SORT(2, 11) ; PIX_SORT(12, 21) ; + PIX_SORT(3, 21) ; PIX_SORT(3, 12) ; PIX_SORT(13, 22) ; + PIX_SORT(4, 22) ; PIX_SORT(4, 13) ; PIX_SORT(14, 23) ; + PIX_SORT(5, 23) ; PIX_SORT(5, 14) ; PIX_SORT(15, 24) ; + PIX_SORT(6, 24) ; PIX_SORT(6, 15) ; PIX_SORT(7, 16) ; + PIX_SORT(7, 19) ; PIX_SORT(13, 21); PIX_SORT(15, 23) ; + PIX_SORT(7, 13) ; PIX_SORT(7, 15) ; PIX_SORT(1, 9) ; + PIX_SORT(3, 11) ; PIX_SORT(5, 17) ; PIX_SORT(11, 17) ; + PIX_SORT(9, 17) ; PIX_SORT(4, 10) ; PIX_SORT(6, 12) ; + PIX_SORT(7, 14) ; PIX_SORT(4, 6) ; PIX_SORT(4, 7) ; + PIX_SORT(12, 14); PIX_SORT(10, 14); PIX_SORT(6, 7) ; + PIX_SORT(10, 12); PIX_SORT(6, 10) ; PIX_SORT(6, 17) ; + PIX_SORT(12, 17); PIX_SORT(7, 17) ; PIX_SORT(7, 10) ; + PIX_SORT(12, 18); PIX_SORT(7, 12) ; PIX_SORT(10, 18) ; + PIX_SORT(12, 20); PIX_SORT(10, 20); PIX_SORT(10, 12) ; + return (p[12]); +} +#undef PIX_SORT +#define PIX_SORT(a, b) {if (a > b) ELEM_SWAP(a, b);} +/** + * quick select - algo for approximate median calculation for array idata of size n + */ +static Imtype quick_select(Imtype *idata, int n){ + int low, high; + int median; + int middle, ll, hh; + Imtype *arr = MALLOC(Imtype, n); + memcpy(arr, idata, n*sizeof(Imtype)); + low = 0 ; high = n-1 ; median = (low + high) / 2; + for(;;){ + if(high <= low) // One element only + break; + if(high == low + 1){ // Two elements only + PIX_SORT(arr[low], arr[high]) ; + break; + } + // Find median of low, middle and high Imtypes; swap into position low + middle = (low + high) / 2; + PIX_SORT(arr[middle], arr[high]) ; + PIX_SORT(arr[low], arr[high]) ; + PIX_SORT(arr[middle], arr[low]) ; + // Swap low Imtype (now in position middle) into position (low+1) + ELEM_SWAP(arr[middle], arr[low+1]) ; + // Nibble from each end towards middle, swapping Imtypes when stuck + ll = low + 1; + hh = high; + for(;;){ + do ll++; while (arr[low] > arr[ll]); + do hh--; while (arr[hh] > arr[low]); + if(hh < ll) break; + ELEM_SWAP(arr[ll], arr[hh]) ; + } + // Swap middle Imtype (in position low) back into correct position + ELEM_SWAP(arr[low], arr[hh]) ; + // Re-set active partition + if (hh <= median) low = ll; + if (hh >= median) high = hh - 1; + } + Imtype ret = arr[median]; + FREE(arr); + return ret; +} +#undef PIX_SORT +#undef ELEM_SWAP + +/** + * calculate median of array idata with size n + */ +Imtype calc_median(Imtype *idata, int n){ + if(!idata || n < 1){ + WARNX("calc_median(): wrong dta"); return 0.; + } + typedef Imtype (*medfunc)(Imtype *p); + medfunc fn = NULL; + const medfunc fnarr[] = {opt_med2, opt_med3, opt_med4, opt_med5, opt_med6, + opt_med7, opt_med8, opt_med9}; + if(n == 1) return *idata; + if(n < 10) fn = fnarr[n - 2]; + else if(n == 16) fn = opt_med16; + else if(n == 25) fn = opt_med25; + if(fn){ + return fn(idata); + }else{ + return quick_select(idata, n); + } +} + +#define ImtypeLess(a,b) ((a)<(b)) +#define ImtypeMean(a,b) (((a)+(b))/2) + +typedef struct Mediator_t{ + Imtype* data; // circular queue of values + int* pos; // index into `heap` for each value + int* heap; // max/median/min heap holding indexes into `data`. + int N; // allocated size. + int idx; // position in circular queue + int ct; // count of Imtypes in queue +} Mediator; + +/*--- Helper Functions ---*/ + +#define minCt(m) (((m)->ct-1)/2) //count of Imtypes in minheap +#define maxCt(m) (((m)->ct)/2) //count of Imtypes in maxheap + +//returns 1 if heap[i] < heap[j] +static inline int mmless(Mediator* m, int i, int j){ + return ImtypeLess(m->data[m->heap[i]],m->data[m->heap[j]]); +} + +//swaps Imtypes i&j in heap, maintains indexes +static inline int mmexchange(Mediator* m, int i, int j){ + int t = m->heap[i]; + m->heap[i] = m->heap[j]; + m->heap[j] = t; + m->pos[m->heap[i]] = i; + m->pos[m->heap[j]] = j; + return 1; +} + +//swaps Imtypes i&j if i1 && i < minCt(m) && mmless(m, i+1, i)) ++i; + if(!mmCmpExch(m,i,i/2)) break; + } +} + +//maintains maxheap property for all Imtypes below i/2. (negative indexes) +static inline void maxSortDown(Mediator* m, int i){ + for(; i >= -maxCt(m); i*=2){ + if(i<-1 && i > -maxCt(m) && mmless(m, i, i-1)) --i; + if(!mmCmpExch(m,i/2,i)) break; + } +} + +//maintains minheap property for all Imtypes above i, including median +//returns true if median changed +static inline int minSortUp(Mediator* m, int i){ + while (i > 0 && mmCmpExch(m, i, i/2)) i /= 2; + return (i == 0); +} + +//maintains maxheap property for all Imtypes above i, including median +//returns true if median changed +static inline int maxSortUp(Mediator* m, int i){ + while (i < 0 && mmCmpExch(m, i/2, i)) i /= 2; + return (i == 0); +} + +/*--- Public Interface ---*/ + +//creates new Mediator: to calculate `nImtypes` running median. +//mallocs single block of memory, caller must free. +static Mediator* MediatorNew(int nImtypes){ + int size = sizeof(Mediator) + nImtypes*(sizeof(Imtype)+sizeof(int)*2); + Mediator* m = malloc(size); + m->data = (Imtype*)(m + 1); + m->pos = (int*) (m->data + nImtypes); + m->heap = m->pos + nImtypes + (nImtypes / 2); //points to middle of storage. + m->N = nImtypes; + m->ct = m->idx = 0; + while (nImtypes--){ //set up initial heap fill pattern: median,max,min,max,... + m->pos[nImtypes] = ((nImtypes+1)/2) * ((nImtypes&1)? -1 : 1); + m->heap[m->pos[nImtypes]] = nImtypes; + } + return m; +} + +//Inserts Imtype, maintains median in O(lg nImtypes) +static void MediatorInsert(Mediator* m, Imtype v){ + int isNew=(m->ctN); + int p = m->pos[m->idx]; + Imtype old = m->data[m->idx]; + m->data[m->idx]=v; + m->idx = (m->idx+1) % m->N; + m->ct+=isNew; + if(p>0){ //new Imtype is in minHeap + if (!isNew && ImtypeLess(old,v)) minSortDown(m,p*2); + else if (minSortUp(m,p)) maxSortDown(m,-1); + }else if (p<0){ //new Imtype is in maxheap + if (!isNew && ImtypeLess(v,old)) maxSortDown(m,p*2); + else if (maxSortUp(m,p)) minSortDown(m, 1); + }else{ //new Imtype is at median + if (maxCt(m)) maxSortDown(m,-1); + if (minCt(m)) minSortDown(m, 1); + } +} + +//returns median Imtype (or average of 2 when Imtype count is even) +static Imtype MediatorMedian(Mediator* m){ + Imtype v = m->data[m->heap[0]]; + if ((m->ct&1) == 0) v = ImtypeMean(v, m->data[m->heap[-1]]); + return v; +} + +#if 0 +// median + min/max +static Imtype MediatorStat(Mediator* m, Imtype *minval, Imtype *maxval){ + Imtype v = m->data[m->heap[0]]; + if ((m->ct&1) == 0) v = ImtypeMean(v, m->data[m->heap[-1]]); + Imtype min = v, max = v; + int i; + for(i = -maxCt(m); i < 0; ++i){ + int v = m->data[m->heap[i]]; + if(v < min) min = v; + } + *minval = min; + for(i = 1; i <= minCt(m); ++i){ + int v = m->data[m->heap[i]]; + if(v > max) max = v; + } + *maxval = max; + return v; +} +#endif + +/** + * filter image by median (seed*2 + 1) x (seed*2 + 1) + */ +Image *get_median(const Image *img, int seed){ + if(seed < 1) return NULL; + size_t w = img->width, h = img->height; + Image *out = Image_sim(img); + Imtype *med = out->data, *inputima = img->data; + + size_t blksz = seed * 2 + 1, fullsz = blksz * blksz; +#ifdef EBUG + double t0 = dtime(); +#endif + OMP_FOR(shared(inputima, med)) + for(size_t x = seed; x < w - seed; ++x){ + size_t xx, yy, xm = x + seed + 1, y, ymax = blksz - 1, xmin = x - seed; + Mediator* m = MediatorNew(fullsz); + // initial fill + for(yy = 0; yy < ymax; ++yy) + for(xx = xmin; xx < xm; ++xx) + MediatorInsert(m, inputima[xx + yy*w]); + ymax = 2*seed*w; + xmin += ymax; + xm += ymax; + ymax = h - seed; + size_t medidx = x + seed * w; + for(y = seed; y < ymax; ++y, xmin += w, xm += w, medidx += w){ + for(xx = xmin; xx < xm; ++xx) + MediatorInsert(m, inputima[xx]); + med[medidx] = MediatorMedian(m); + } + free(m); + } + Image_minmax(out); + DBG("time for median filtering %zdx%zd of image %zdx%zd: %gs", blksz, blksz, w, h, + dtime() - t0); + return out; +} + +/** + * @brief get_stat - calculate floating statistics in (seed*2+1)^2 + * @param in (i) - input image + * @param seed - radius of box + * @param mean (o) - mean by box (excluding borders) + * @param std (o) - STD by box (excluding borders) + * @retur 0 if error + */ +int get_stat(const Image *in, int seed, Image **mean, Image **std){ + if(!in) return FALSE; + if(seed < 1 || seed > (in->width - 1)/2 || seed > (in->height - 1)/2) return FALSE; +#ifdef EBUG + double t0 = dtime(); +#endif + Image *M = NULL, *S = NULL; + if(mean) M = Image_sim(in); + if(std) S = Image_sim(in); + int ymax = in->height - seed, xmax = in->width - seed; + int hsz = (seed*2 + 1), sz = hsz * hsz, w = in->width; + OMP_FOR() + for(int y = seed; y < ymax; ++y){ // dumb calculations + int startidx = y*w + seed; + Imtype *om = (M) ? &M->data[startidx] : NULL; + Imtype *os = (S) ? &S->data[startidx] : NULL; + for(int x = seed; x < xmax; ++x){ + double sum = 0, sum2 = 0; + int yb = y + seed + 1, xm = x - seed; + for(int yy = y - seed; yy < yb; ++yy){ + Imtype *ptr = &in->data[yy * w + xm]; + for(int xx = 0; xx < hsz; ++xx){ + double d = *ptr++; + sum += d; + sum2 += d*d; + } + } + //DBG("sum=%g, sum2=%g, sz=%d", sum, sum2, sz); + sum /= sz; + if(om){ + *om++ = (Imtype)sum; + //DBG("mean (%d, %d): %g", x, y, sum); + } + if(os) *os++ = (Imtype)sqrt(sum2/sz - sum*sum); + } + } + if(mean){ + Image_minmax(M); + *mean = M; + } + if(std){ + Image_minmax(S); + *std = S; + } + DBG("time for mean/sigma computation: %gs", dtime() - t0); + return TRUE; +} diff --git a/LocCorr_new/median.h b/LocCorr_new/median.h new file mode 100644 index 0000000..d8014d1 --- /dev/null +++ b/LocCorr_new/median.h @@ -0,0 +1,31 @@ +/* + * median.h + * + * Copyright 2015 Edward V. Emelianov + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + */ +#pragma once +#ifndef __MEDIAN_H__ +#define __MEDIAN_H__ + +#include "fits.h" + +Imtype calc_median(Imtype *idata, int n); +Image *get_median(const Image *img, int seed); +int get_stat(const Image *in, int seed, Image **mean, Image **std); + +#endif // __MEDIAN_H__ diff --git a/LocCorr_new/socket.c b/LocCorr_new/socket.c new file mode 100644 index 0000000..dd77d1e --- /dev/null +++ b/LocCorr_new/socket.c @@ -0,0 +1,421 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + + +#include // inet_ntop +#include // basename +#include // INT_xxx +#include // addrinfo +#include +#include +#include // pthread_kill +#include +#include +#include +#include // syscall +#include // daemon + +#include "cmdlnopts.h" +#include "config.h" +#include "debug.h" +#include "improc.h" +#include "socket.h" +#include "steppers.h" + +// buffer size for received data +#define BUFLEN (1024) +// buffer size for answer +#define ANSBUFLEN (32768) +// Max amount of connections +#define BACKLOG (10) + +/* +TODO3: add 'FAIL error text' if not OK and instead all "wrong message" +*/ + + +// additional commands list - getters +typedef struct{ + const char *command; + char *(*handler)(const char *messageid, char *buf, int buflen); + const char *help; +} getter; +// setters +typedef struct{ + const char *command; + char *(*handler)(const char *val, char *buf, int buflen); + const char *help; +} setter; + +static char *helpmsg(const char *messageid, char *buf, int buflen); +static char *stepperstatus(const char *messageid, char *buf, int buflen); +static char *getimagedata(const char *messageid, char *buf, int buflen); +static getter getterHandlers[] = { + {"help", helpmsg, "List avaiable commands"}, + {"settings", listconf, "List current configuration"}, + {"stpserv", stepperstatus, "Get status of steppers server"}, + {"imdata", getimagedata, "Get image data (status, path, FPS, counter)"}, + {NULL, NULL, NULL} +}; + +static char *setstepperstate(const char *state, char *buf, int buflen); +static char *setfocusstate(const char *state, char *buf, int buflen); +static char *moveU(const char *val, char *buf, int buflen); +static char *moveV(const char *val, char *buf, int buflen); + +static setter setterHandlers[] = { + {"stpstate", setstepperstate, "Set given steppers' server state"}, + {"focus", setfocusstate, "Move focus to given value"}, + {"moveU", moveU, "Relative moving by U axe"}, + {"moveV", moveV, "Relative moving by V axe"}, + {NULL, NULL, NULL} +}; + +static char *retFAIL(char *buf, int buflen){ + snprintf(buf, buflen, FAIL); + return buf; +} + +/**************** functions to process commands ****************/ +// getters +static char *helpmsg(_U_ const char *messageid, char *buf, int buflen){ + if(get_cmd_list(buf, buflen)){ + int l = strlen(buf), L = buflen - l; + char *ptr = buf + l; + getter *g = getterHandlers; + while(L > 0 && g->command){ + int s = snprintf(ptr, L, "%s - %s\n", g->command, g->help); + if(s < 1) break; + L -= s; ptr += s; + ++g; + } + setter *sh = setterHandlers; + while(L > 0 && sh->command){ + int s = snprintf(ptr, L, "%s=newval - %s\n", sh->command, sh->help); + if(s < 1) break; + L -= s; ptr += s; + ++sh; + } + return buf; + } + return NULL; +} +static char *stepperstatus(const char *messageid, char *buf, int buflen){ + if(theSteppers && theSteppers->stepstatus) return theSteppers->stepstatus(messageid, buf, buflen); + return retFAIL(buf, buflen); +} +static char *getimagedata(const char *messageid, char *buf, int buflen){ + if(imagedata) return imagedata(messageid, buf, buflen); + return retFAIL(buf, buflen); +} + +// setters +static char *setstepperstate(const char *state, char *buf, int buflen){ + DBG("set steppersstate to %s", state); + if(theSteppers && theSteppers->setstepstatus) return theSteppers->setstepstatus(state, buf, buflen); + return retFAIL(buf, buflen); +} +static char *setfocusstate(const char *state, char *buf, int buflen){ + DBG("move focus to %s", state); + if(theSteppers && theSteppers->movefocus) return theSteppers->movefocus(state, buf, buflen); + return retFAIL(buf, buflen); +} +static char *moveU(const char *val, char *buf, int buflen){ + if(theSteppers && theSteppers->moveByU) return theSteppers->moveByU(val, buf, buflen); + return retFAIL(buf, buflen); +} +static char *moveV(const char *val, char *buf, int buflen){ + if(theSteppers && theSteppers->moveByV) return theSteppers->moveByV(val, buf, buflen); + return retFAIL(buf, buflen); +} + +/* +static char *rmnl(const char *msg, char *buf, int buflen){ + strncpy(buf, msg, buflen); + char *nl = strchr(buf, '\n'); + if(nl) *nl = 0; + return buf; +} +*/ + +/** + * @brief processCommand - command parser + * @param msg - incoming message + * @param ans - buffer for answer + * @param anslen - length of `ans` + * @return NULL if no answer or pointer to ans + */ +static char *processCommand(const char msg[BUFLEN], char *ans, int anslen){ + char value[BUFLEN]; + char *kv = get_keyval(msg, value); + confparam *par; + if(kv){ + DBG("got KEY '%s' with value '%s'", kv, value); + key_value result; + par = chk_keyval(kv, value, &result); + free(kv); kv = NULL; + if(par){ + switch(par->type){ + case PAR_INT: + DBG("FOUND! Integer, old=%d, new=%d", *((int*)par->ptr), result.val.intval); + *((int*)par->ptr) = result.val.intval; + break; + case PAR_DOUBLE: + DBG("FOUND! Double, old=%g, new=%g", *((double*)par->ptr), result.val.dblval); + *((double*)par->ptr) = result.val.dblval; + break; + default: + snprintf(ans, anslen, FAIL); + return ans; + } + snprintf(ans, anslen, OK); + return ans; + }else{ + setter *s = setterHandlers; + while(s->command){ + int l = strlen(s->command); + if(strncasecmp(msg, s->command, l) == 0) + return s->handler(value, ans, anslen); + ++s; + } + } + }else{ + getter *g = getterHandlers; + while(g->command){ + int l = strlen(g->command); + if(strncasecmp(msg, g->command, l) == 0) + return g->handler(g->command, ans, anslen); + ++g; + } + } + snprintf(ans, anslen, FAIL); + return ans; +} + +/**************** SERVER FUNCTIONS ****************/ +/** + * Send data over socket (and add trailing '\n' if absent) + * @param sock - socket fd + * @param textbuf - zero-trailing buffer with data to send + * @return amount of sent bytes + */ +static size_t send_data(int sock, const char *textbuf){ + ssize_t Len = strlen(textbuf); + if(Len != write(sock, textbuf, Len)){ + WARN("write()"); + LOGERR("send_data(): write() failed"); + return 0; + }else{ + LOGDBG("send_data(): sent '%s'", textbuf); + } + if(textbuf[Len-1] != '\n') Len += write(sock, "\n", 1); + return (size_t)Len; +} + +/** + * @brief handle_socket - read and process data from socket + * @param sock - socket fd + * @return 0 if all OK, 1 if socket closed + */ +static int handle_socket(int sock){ + FNAME(); + char buff[BUFLEN]; + char ansbuff[ANSBUFLEN]; + ssize_t rd = read(sock, buff, BUFLEN-1); + if(rd < 1){ + DBG("read() == %zd", rd); + return 1; + } + // add trailing zero to be on the safe side + buff[rd] = 0; + // now we should check what do user want + // here we can process user data + DBG("user %d send '%s'", sock, buff); + LOGDBG("user %d send '%s'", sock, buff); + //pthread_mutex_lock(&mutex); + char *ans = processCommand(buff, ansbuff, ANSBUFLEN-1); // run command parser + if(ans){ + send_data(sock, ans); // send answer + } + //pthread_mutex_unlock(&mutex); + return 0; +} + +// main socket server +static void *server(void *asock){ + DBG("server(): getpid: %d, tid: %lu",getpid(), syscall(SYS_gettid)); + int sock = *((int*)asock); + if(listen(sock, BACKLOG) == -1){ + LOGERR("server(): listen() failed"); + WARN("listen"); + return NULL; + } + int nfd = 1; + struct pollfd poll_set[BACKLOG+1]; + memset(poll_set, 0, sizeof(poll_set)); + poll_set[0].fd = sock; + poll_set[0].events = POLLIN; + while(1){ + if(stopwork){ + DBG("server() exit @ global stop"); + return NULL; + } + poll(poll_set, nfd, 1); // poll for 1ms + for(int fdidx = 0; fdidx < nfd; ++fdidx){ // poll opened FDs + if((poll_set[fdidx].revents & POLLIN) == 0) continue; + poll_set[fdidx].revents = 0; + if(fdidx){ // client + int fd = poll_set[fdidx].fd; + //int nread = 0; + //ioctl(fd, FIONREAD, &nread); + if(handle_socket(fd)){ // socket closed - remove it from list + close(fd); + DBG("Client with fd %d closed", fd); + LOGMSG("Client %d disconnected", fd); + // move last to free space + poll_set[fdidx] = poll_set[nfd - 1]; + --nfd; + } + }else{ // server + socklen_t size = sizeof(struct sockaddr_in); + struct sockaddr_in their_addr; + int newsock = accept(sock, (struct sockaddr*)&their_addr, &size); + if(newsock <= 0){ + LOGERR("server(): accept() failed"); + WARN("accept()"); + continue; + } + struct in_addr ipAddr = their_addr.sin_addr; + char str[INET_ADDRSTRLEN]; + inet_ntop(AF_INET, &ipAddr, str, INET_ADDRSTRLEN); + DBG("Connection from %s, give fd=%d", str, newsock); + LOGMSG("Got connection from %s, fd=%d", str, newsock); + if(nfd == BACKLOG + 1){ + LOGWARN("Max amount of connections: disconnect %s (%d)", str, newsock); + send_data(newsock, "Max amount of connections reached!"); + WARNX("Limit of connections reached"); + close(newsock); + }else{ + memset(&poll_set[nfd], 0, sizeof(struct pollfd)); + poll_set[nfd].fd = newsock; + poll_set[nfd].events = POLLIN; + ++nfd; + } + } + } // endfor + } + LOGERR("server(): UNREACHABLE CODE REACHED!"); +} + +// data gathering & socket management +static void daemon_(int sock){ + if(sock < 0) return; + pthread_t sock_thread;//, canserver_thread; + if(pthread_create(&sock_thread, NULL, server, (void*) &sock)){ + LOGERR("daemon_(): pthread_create() failed"); + ERR("pthread_create()"); + } + do{ + if(stopwork){ + DBG("kill"); + pthread_join(sock_thread, NULL); + return; + } + if(pthread_kill(sock_thread, 0) == ESRCH){ // died + WARNX("Sockets thread died"); + LOGERR("Sockets thread died"); + pthread_join(sock_thread, NULL); + if(pthread_create(&sock_thread, NULL, server, (void*) &sock)){ + LOGERR("daemon_(): new pthread_create(sock_thread) failed"); + ERR("pthread_create(sock_thread)"); + } + } + usleep(1000); // sleep a little or thread's won't be able to lock mutex + }while(1); + LOGERR("daemon_(): UNREACHABLE CODE REACHED!"); +} + +/** + * open sockets + * // should be called only once!!! + */ +static void *connect2sock(void *data){ + FNAME(); + char port[10]; + int portN = *((int*)data); + snprintf(port, 10, "%d", portN); + DBG("get port: %s", port); + int sock = -1; + struct addrinfo hints, *res, *p; + memset(&hints, 0, sizeof(hints)); + hints.ai_family = AF_INET; + hints.ai_socktype = SOCK_STREAM; + hints.ai_flags = AI_PASSIVE; + if(getaddrinfo("127.0.0.1", port, &hints, &res) != 0){ // accept only local connections + LOGERR("daemonize(): getaddrinfo() failed"); + ERR("getaddrinfo"); + } + struct sockaddr_in *ia = (struct sockaddr_in*)res->ai_addr; + char str[INET_ADDRSTRLEN]; + inet_ntop(AF_INET, &(ia->sin_addr), str, INET_ADDRSTRLEN); + // loop through all the results and bind to the first we can + for(p = res; p != NULL; p = p->ai_next){ + if((sock = socket(p->ai_family, p->ai_socktype, p->ai_protocol)) == -1){ + LOGWARN("openIOport(): socket() failed"); + WARN("socket"); + continue; + } + int reuseaddr = 1; + if(setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &reuseaddr, sizeof(int)) == -1){ + LOGERR("openIOport(): setsockopt() failed"); + ERR("setsockopt"); + } + if(bind(sock, p->ai_addr, p->ai_addrlen) == -1){ + close(sock); + LOGERR("openIOport(): bind() failed"); + WARN("bind"); + continue; + } + break; // if we get here, we have a successfull connection + } + if(p == NULL){ + LOGERR("openIOport(): failed to bind socket, exit"); + // looped off the end of the list with no successful bind + ERRX("failed to bind socket"); + } + freeaddrinfo(res); + daemon_(sock); + close(sock); + LOGWARN("openIOport(): close @ global stop"); + return NULL; +} + + +// run socket thread +void openIOport(int portN){ + static int portnum = 0; + if(portnum) return; + portnum = portN; + pthread_t connthread; + DBG("open port: %d", portN); + if(pthread_create(&connthread, NULL, connect2sock, (void*) &portnum)){ + LOGERR("openIOport(): pthread_create() failed"); + ERR("pthread_create()"); + } + pthread_detach(connthread); +} diff --git a/LocCorr_new/socket.h b/LocCorr_new/socket.h new file mode 100644 index 0000000..00b6756 --- /dev/null +++ b/LocCorr_new/socket.h @@ -0,0 +1,29 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once +#ifndef __SOCKET_H__ +#define __SOCKET_H__ + +// standard answers +#define OK "OK\n" +#define FAIL "FAILED\n" + +void openIOport(int portN); + +#endif // __SOCKET_H__ diff --git a/LocCorr_new/stbimpl.c b/LocCorr_new/stbimpl.c new file mode 100644 index 0000000..4c76251 --- /dev/null +++ b/LocCorr_new/stbimpl.c @@ -0,0 +1,29 @@ +/* + * This file is part of the loccorr project. + * Copyright 2021 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +// implementation of STB library (to accelerate compilation) +#define STBI_NO_PSD +#define STBI_NO_TGA +#define STBI_NO_HDR +#define STBI_NO_PIC +#define STBI_NO_PNM + +#define STB_IMAGE_IMPLEMENTATION +#include +#define STB_IMAGE_WRITE_IMPLEMENTATION +#include diff --git a/LocCorr_new/steppers.c b/LocCorr_new/steppers.c new file mode 100644 index 0000000..ffb5287 --- /dev/null +++ b/LocCorr_new/steppers.c @@ -0,0 +1,1068 @@ +/* + * This file is part of the loccorr project. + * Copyright 2024 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#define _GNU_SOURCE +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "config.h" +#include "debug.h" +#include "improc.h" // global variable stopwork +#include "steppers.h" +#include "socket.h" + +// buffer for socket +#define BUFLEN (256) + +// max time to wait answer "OK" from server +#define WAITANSTIME (0.3) + + +// amount of consequent center coordinates coincidence in `process_targetstate` +#define NCONSEQ (2) +// tolerance of coordinates coincidence (pix) +#define COORDTOLERANCE (0.5) + +typedef enum{ + STP_DISCONN, + STP_RELAX, + STP_SETUP, + STP_GOTOTHEMIDDLE, + STP_FINDTARGET, + STP_FIX, + STP_UNDEFINED +} STPstate; + +typedef enum{ + SETUP_NONE, // no setup + SETUP_INIT, // the starting - move U&V to 0 + SETUP_WAITUV0, // wait & move U&V to middle + SETUP_WAITUVMID, // wait + SETUP_WAITU0, // move U->0 + SETUP_WAITUMAX, // move U->max + SETUP_WAITV0, // V->0 + SETUP_WAITVMAX, // V->max + SETUP_FINISH +} setupstatus; +static _Atomic setupstatus sstatus = SETUP_NONE; // setup state + +//static int errctr = 0; // sending messages error counter (if > MAX_ERR_CTR, set state to disconnected) + +steppersproc *theSteppers = NULL; + +// stepper numbers +typedef enum{ + Ustepper = 0, + Vstepper = 2, + Fstepper = 1, + } stepperno; + +const char *motornames[NMOTORS] = { + [Ustepper] = "Umotor", + [Vstepper] = "Vmotor", + [Fstepper] = "Fmotor", +}; + +// a list of steppers commands +typedef enum{ + CMD_ABSPOS, + CMD_EMSTOP, + CMD_ESW, + CMD_GOTO, + CMD_GOTOZ, + CMD_RELPOS, + CMD_STATE, + CMD_STOP, + CMD_AMOUNT +} steppercmd; + +typedef enum{ + ERR_OK, // 0 - all OK + ERR_BADPAR, // 1 - parameter's value is wrong + ERR_BADVAL, // 2 - wrong parameter's value + ERR_WRONGLEN, // 3 - wrong message length + ERR_BADCMD, // 4 - unknown command + ERR_CANTRUN, // 5 - can't run given command due to bad parameters or other + ERR_AMOUNT // amount of error codes +} errcodes; + + +static const char *stp_commands[CMD_AMOUNT] = { + [CMD_ABSPOS] = "abspos", + [CMD_EMSTOP] = "emstop", + [CMD_ESW] = "esw", + [CMD_GOTO] = "goto", + [CMD_GOTOZ] = "gotoz", + [CMD_RELPOS] = "relpos", + [CMD_STATE] = "state", + [CMD_STOP] = "stop", +}; + +static const char* errtxt[ERR_AMOUNT] = { + [ERR_OK] = "OK", + [ERR_BADPAR] = "BADPAR", + [ERR_BADVAL] = "BADVAL", + [ERR_WRONGLEN] = "WRONGLEN", + [ERR_BADCMD] = "BADCMD", + [ERR_CANTRUN] = "CANTRUN", +}; + +static STPstate state = STP_DISCONN; // server state +// this flag set to TRUE when next Xc,Yc available +static volatile atomic_bool coordsRdy = FALSE; +static double Xtarget = 0., Ytarget = 0.; + +// Values to change U,V and F by hands +// flag & new focus value +static volatile atomic_bool chfocus = FALSE; +static volatile atomic_int newfocpos = 0, dUmove = 0, dVmove = 0; + +static volatile atomic_int sockfd = -1; // server file descriptor +static volatile atomic_bool motorsoff = FALSE; // flag to disconnect + +// mutex for message sending/receiving +static pthread_mutex_t mesg_mutex = PTHREAD_MUTEX_INITIALIZER; + +// current steps counters (zero at the middle) +static volatile atomic_int motposition[NMOTORS] = {0}; +// relative position change after current moving ends (from external command) +static volatile atomic_int motrelsteps[NMOTORS] = {0}; +// current motor state +static volatile atomic_int motstates[NMOTORS] = {0}; +static uint8_t fixerr = 0; // ==1 if can't fixed + +// motor states: +typedef enum{ + STATE_RELAX, + STATE_ACCEL, + STATE_MOVE, + STATE_MVSLOW, + STATE_DECEL, + STATE_STALL, + STATE_ERR, + STATE_NUM +} motstate; +static const char *str_states[STATE_NUM] = { + [STATE_RELAX] = "relax", + [STATE_ACCEL] = "accelerated", + [STATE_MOVE] = "moving", + [STATE_MVSLOW] = "slow moving", + [STATE_DECEL] = "decelerated", + [STATE_STALL] = "stalled", + [STATE_ERR] = "error", +}; + +TRUE_INLINE int relaxed(int nmotor){return motstates[nmotor] == STATE_RELAX;} +#define Uposition (motposition[Ustepper]) +#define Vposition (motposition[Vstepper]) +#define Fposition (motposition[Fstepper]) + +static int nth_motor_setter(steppercmd idx, int n, int p); + +static void stp_disc(){ + motorsoff = TRUE; +} + +static void stp_disconnect(){ + DBG("Try to disconnect"); + if(sockfd > -1){ + DBG("sockfd closed"); + close(sockfd); + } + state = STP_DISCONN; + sockfd = -1; + LOGWARN("Canserver disconnected"); +} + +// check if nmot is U/V/F and return FALSE if not +static int chkNmot(int nmot){ + if(nmot == Ustepper || nmot == Vstepper || nmot == Fstepper) return TRUE; + return FALSE; +} + +/** + * wait for answer from socket + * @return FALSE in case of error or timeout, TRUE if socket is ready + */ +static int canread(){ + if(sockfd < 0) return FALSE; + fd_set fds; + struct timeval timeout; + int rc; + // wait not more than 10ms + timeout.tv_sec = 0; + timeout.tv_usec = 10000; + FD_ZERO(&fds); + FD_SET(sockfd, &fds); + do{ + rc = select(sockfd+1, &fds, NULL, NULL, &timeout); + if(rc < 0){ + if(errno != EINTR){ + WARN("select()"); + return FALSE; + } + continue; + } + break; + }while(1); + if(FD_ISSET(sockfd, &fds)){ + //DBG("CANREAD"); + return TRUE; + } + return FALSE; +} + +// clear all data received earlier +static void clrbuf(){ + char buf[256]; + while(canread()){ + ssize_t got = recv(sockfd, buf, 256, 0); + DBG("cleared %zd bytes of trash", got); + if(got <= 0){ // disconnect or error + LOGERR("Server disconnected"); + ERRX("Server disconnected"); + } + } +} + +// read message (if exists) and return its length (or -1 if none) +// There's could be many strings of data!!! +static ssize_t read_message(char *msg, size_t msglen){ + if(!msg || msglen == 0) return -1; + if(pthread_mutex_lock(&mesg_mutex)){ + WARN("pthread_mutex_lock()"); + LOGWARN("read_message(): pthread_mutex_lock() err"); + return 0; + } + double t0 = dtime(); + size_t gotbytes = 0; + --msglen; // for trailing zero + while(dtime() - t0 < WAITANSTIME && gotbytes < msglen && sockfd > 0){ + if(!canread()) continue; + int n = recv(sockfd, msg+gotbytes, msglen, 0); + if(n <= 0){ // disconnect or error + LOGERR("Server disconnected"); + ERRX("Server disconnected"); + } + if(n == 0) break; + gotbytes += n; + msglen -= n; + if(msg[gotbytes-1] == '\n') break; + t0 = dtime(); + } + //DBG("Dt=%g, gotbytes=%zd, sockfd=%d, msg='%s'", dtime()-t0,gotbytes,sockfd,msg); + pthread_mutex_unlock(&mesg_mutex); + if(msg[gotbytes-1] != '\n'){ + //DBG("No newline at the end"); + return 0; + } + msg[gotbytes] = 0; + return gotbytes; +} + +static errcodes parsing(steppercmd idx, int nmot, int ival){ + int goodidx = chkNmot(nmot); + switch(idx){ + case CMD_ABSPOS: + if(goodidx) motposition[nmot] = ival; + else return ERR_BADPAR; + break; + case CMD_RELPOS: + if(goodidx) motrelsteps[nmot] = ival; + else return ERR_BADPAR; + break; + case CMD_STATE: + if(!goodidx) return ERR_BADPAR; + motstates[nmot] = ival; + if(chkNmot(nmot)){ // one of our motors - check err or stall + if(ival == STATE_STALL || ival == STATE_ERR){ + WARNX("BAD status of motor %d", nmot); + LOGWARN("BAD status of motor %d", nmot); + nth_motor_setter(CMD_EMSTOP, nmot, 1); // tty to clear error + } + } + break; + case CMD_EMSTOP: + case CMD_ESW: + case CMD_GOTO: + case CMD_GOTOZ: + case CMD_STOP: + break; + default: return ERR_BADCMD; + } + return ERR_OK; +} + +// check if message is error text +static errcodes getecode(const char *msg){ + errcodes e; + for(e = 0; e < ERR_AMOUNT; ++e){ + if(0 == strcmp(msg, errtxt[e])) break; + } + DBG("ERRcode: %d, (%s)", e, (e != ERR_AMOUNT) ? errtxt[e] : "undef"); + LOGDBG("ERRcode: %d, (%s)", e, (e != ERR_AMOUNT) ? errtxt[e] : "undef"); + return e; +} + +/** + * @brief read_parse - read answer (till got or WAITANSTIME timeout) and parse it + * @param idx - index of message sent (to compare answer) + * @return + */ +static errcodes read_and_parse(steppercmd idx){ + char value[128], msg[1024]; + double t0 = dtime(); + while(dtime() - t0 < WAITANSTIME*10.){ + ssize_t got = read_message(msg, 1024); + if(got < 1) continue; + //LOGDBG("GOT from stepper server:\n%s\n", msg); + char *saveptr, *tok = msg; + for(;; tok = NULL){ + char *token = strtok_r(tok, "\n", &saveptr); + if(!token) break; + //LOGDBG("Got line: %s", token); + char *key = get_keyval(token, value); + if(key){ + int ival = atoi(value); + //LOGDBG("key = %s, value = %s (%d)", key, value, ival); + size_t l = strlen(key); + size_t numpos = strcspn(key, "0123456789"); + int parno = -1; + if(numpos < l){ + parno = atoi(key + numpos); + key[numpos] = 0; + } + //DBG("numpos=%zd, parno=%d", numpos, parno); + if(parno > -1){ // got motor number + if(!chkNmot(parno)){ + DBG("Not our business"); + free(key); continue; + } + } + // search index in commands + if(0 == strcmp(stp_commands[idx], key)){ // found our + free(key); + //LOGDBG("OK, idx=%d, cmd=%s", idx, stp_commands[idx]); + return parsing(idx, parno, ival); + } + free(key); + }else{ + DBG("GOT NON-setter %s", token); + errcodes e = getecode(token); + if(e != ERR_AMOUNT) return e; // ERR_AMOUNT means some other message + } + } + } + DBG("No answer detected to our command"); + LOGDBG("read_and_parse(): no answer detected to our command"); + return ERR_CANTRUN; // didn't get anwer need +} + +/** + * @brief send_message - send character string `msg` to serial server, get and parse answer + * @param msg - message (for setters could be like "N=M" or "=M") or NULL (for getters) + * @return FALSE if failed (should reconnect) + */ +static errcodes send_message(steppercmd idx, char *msg){ + // FNAME(); + if(sockfd < 0) return ERR_CANTRUN; + char buf[256]; + size_t msglen; + if(!msg) msglen = snprintf(buf, 255, "%s\n", stp_commands[idx]); + else msglen = snprintf(buf, 255, "%s%s\n", stp_commands[idx], msg); + //DBG("Send message '%s', len %zd", buf, msglen); + if(pthread_mutex_lock(&mesg_mutex)){ + WARN("pthread_mutex_lock()"); + LOGWARN("send_message(): pthread_mutex_lock() err"); + return FALSE; + } + clrbuf(); + if(send(sockfd, buf, msglen, 0) != (ssize_t)msglen){ + WARN("send()"); + LOGWARN("send_message(): send() failed"); + return ERR_WRONGLEN; + } + //LOGDBG("Message '%s' sent", buf); + pthread_mutex_unlock(&mesg_mutex); + return read_and_parse(idx); +} + +// send command cmd to n'th motor with param p, @return FALSE if failed +static int nth_motor_setter(steppercmd idx, int n, int p){ + if(idx < 0 || idx >= CMD_AMOUNT) return FALSE; + char buf[256]; + if(n < 0){ // setter without number + snprintf(buf, 255, "=%d", p); + DBG("nth_motor_setter(): set %s=%d", stp_commands[idx], p); + LOGDBG("nth_motor_setter(): set %s=%d", stp_commands[idx], p); + }else if(n < NMOTORS){ + snprintf(buf, 255, "%d=%d", n, p); + DBG("nth_motor_setter(): get %s%d=%d", stp_commands[idx], n, p); + LOGDBG("nth_motor_setter(): set %s%d=%d", stp_commands[idx], n, p); + }else{ + WARNX("Wrong motno %d", n); + LOGWARN("Wrong motno %d (cmd=%s, setter=%d)", n, stp_commands[idx], p); + } + if(ERR_OK != send_message(idx, buf)) return FALSE; + return TRUE; +} +// and simplest getter +static int nth_motor_getter(steppercmd idx, int n){ + if(idx < 0 || idx >= CMD_AMOUNT) return FALSE; + char buf[32], *msg = NULL; + if(n > -1 && n < NMOTORS){ + sprintf(buf, "%d", n); + //DBG("nth_motor_getter(): %s%d", stp_commands[idx], n); + //LOGDBG("nth_motor_getter(): %s%d", stp_commands[idx], n); + msg = buf; + }else{ + WARNX("Wrong motno %d", n); + LOGWARN("nth_motor_getter(): wrong motno %d (cmd=%s)", n, stp_commands[idx]); + } + if(ERR_OK != send_message(idx, msg)) return FALSE; + return TRUE; +} + +// send getter to all motors; return FALSE if failed +static int chkmots(steppercmd cmd){ + if( nth_motor_getter(cmd, Ustepper) && + nth_motor_getter(cmd, Vstepper) && + nth_motor_getter(cmd, Fstepper)) return TRUE; + return FALSE; +} + +static void chkall(){ + chkmots(CMD_STATE); + chkmots(CMD_ABSPOS); + chkmots(CMD_RELPOS); +} + +/** + * @brief stp_connect_server - try connect to a local steppers CAN server + * @return FALSE if failed + */ +static int stp_connect_server(){ + DBG("STP_connect(%d)", theconf.stpserverport); + char port[10]; + snprintf(port, 10, "%d", theconf.stpserverport); + stp_disconnect(); + struct addrinfo hints = {0}, *res, *p; + hints.ai_family = AF_INET; + hints.ai_socktype = SOCK_STREAM; + hints.ai_flags = AI_PASSIVE; + if(getaddrinfo(NULL, port, &hints, &res) != 0){ + WARN("getaddrinfo()"); + return FALSE; + } + // loop through all the results and connect to the first we can + for(p = res; p; p = p->ai_next){ + if((sockfd = socket(p->ai_family, p->ai_socktype, p->ai_protocol)) == -1){ + WARN("socket"); + continue; + } + if(connect(sockfd, p->ai_addr, p->ai_addrlen) == -1){ + WARN("connect()"); + close(sockfd); + continue; + } + break; // if we get here, we have a successfull connection + } + if(!p){ + WARNX("Can't connect to socket"); + LOGWARN("Can't connect to steppers server"); + sockfd = -1; + return FALSE; + } + freeaddrinfo(res); + // register and set max speed; don't check `register` answer as they could be registered already + state = STP_RELAX; + sstatus = SETUP_NONE; + LOGMSG("Connected to stepper server"); + return TRUE; +} + +static void *stp_process_states(_U_ void *arg); +static pthread_t processingthread; + +static void process_movetomiddle_stage(){ + switch(sstatus){ + case SETUP_INIT: // initial moving + if( !nth_motor_setter(CMD_EMSTOP, Ustepper, 1) || + !nth_motor_setter(CMD_EMSTOP, Vstepper, 1) || + !nth_motor_setter(CMD_EMSTOP, Fstepper, 1) ) break; + if(nth_motor_setter(CMD_GOTOZ, Ustepper, 1) && + nth_motor_setter(CMD_GOTOZ, Vstepper, 1) && + nth_motor_setter(CMD_GOTOZ, Fstepper, 1)){ + LOGMSG("process_movetomiddle_stage(): SETUP_WAITUV0"); + sstatus = SETUP_WAITUV0; + } + break; + case SETUP_WAITUV0: // wait for all coordinates moving to zero + if(!relaxed(Ustepper) || !relaxed(Vstepper) || !relaxed(Fstepper)) break; // didn't reach yet + // now all motors are stopped -> send positions to zero + if( !nth_motor_setter(CMD_ABSPOS, Ustepper, 1) || + !nth_motor_setter(CMD_ABSPOS, Vstepper, 1) || + !nth_motor_setter(CMD_ABSPOS, Fstepper, 1)) break; + DBG("Reached UVF0!"); + // goto + if(nth_motor_setter(CMD_GOTO, Ustepper, (theconf.maxUpos + theconf.minUpos)/2) && + nth_motor_setter(CMD_GOTO, Vstepper, (theconf.maxVpos + theconf.minVpos)/2) && + nth_motor_setter(CMD_GOTO, Fstepper, (theconf.maxFpos + theconf.minFpos)/2)){ + LOGMSG("process_movetomiddle_stage(): SETUP_WAITUVMID"); + sstatus = SETUP_WAITUVMID; + } + break; + case SETUP_WAITUVMID: // wait for the middle + if(!relaxed(Ustepper) || !relaxed(Vstepper) || !relaxed(Fstepper)) break; + // if motors ready, relsteps should be 0 + if(motrelsteps[Ustepper] || motrelsteps[Vstepper] || motrelsteps[Fstepper]){ + WARNX("Come to wrong pos: U=%d, V=%d, F=%d", Uposition, Vposition, Fposition); + LOGWARN("Come to wrong pos: U=%d, V=%d, F=%d", Uposition, Vposition, Fposition); + sstatus = SETUP_WAITUV0; + } + DBG("Reached middle position"); + LOGMSG("Reached middle position"); + // fallthrough + default: + sstatus = SETUP_NONE; + state = STP_RELAX; + } +} + +/** + * @brief process_setup_stage - process all stages of axes setup + */ +static void process_setup_stage(){ + DBG("PROCESS: %d\n", sstatus); + // coordinates for corrections calculation + static double X0U, Y0U, XmU, YmU; + static double X0V, Y0V, XmV, YmV; + switch(sstatus){ + case SETUP_INIT: // initial moving; don't move F (as it should be focused already) + if( !nth_motor_setter(CMD_EMSTOP, Ustepper, 1) || + !nth_motor_setter(CMD_EMSTOP, Vstepper, 1) ) break; + if(nth_motor_setter(CMD_GOTOZ, Ustepper, 1) && + nth_motor_setter(CMD_GOTOZ, Vstepper, 1) ){ + LOGMSG("process_setup_stage(): SETUP_WAITUV0"); + sstatus = SETUP_WAITUV0; + } + break; + case SETUP_WAITUV0: // wait for both coordinates moving to zero + if(!relaxed(Ustepper) || !relaxed(Vstepper)) break; + // set current position to 0 + if( !nth_motor_setter(CMD_ABSPOS, Ustepper, 1) || + !nth_motor_setter(CMD_ABSPOS, Vstepper, 1)) break; + DBG("ZERO border reached"); + // goto middle + if(nth_motor_setter(CMD_GOTO, Ustepper, (theconf.maxUpos+theconf.minUpos)/2) && + nth_motor_setter(CMD_GOTO, Vstepper, (theconf.maxVpos+theconf.minVpos)/2)){ + LOGMSG("process_setup_stage(): SETUP_WAITUVMID"); + sstatus = SETUP_WAITUVMID; + }else{ + WARNX("Can't move U/V to middle"); + LOGWARN("Can't move U/V to middle"); + sstatus = SETUP_INIT; + } + break; + case SETUP_WAITUVMID: // wait for the middle + if(!relaxed(Ustepper) || !relaxed(Vstepper)) break; + DBG("The middle reached"); + // now move U to zero + if(nth_motor_setter(CMD_GOTO, Ustepper, theconf.minUpos)){ + LOGMSG("process_setup_stage(): SETUP_WAITU0"); + sstatus = SETUP_WAITU0; + }else{ + LOGWARN("Can't move U to min"); + sstatus = SETUP_INIT; + } + break; + case SETUP_WAITU0: // wait while U moves to zero + if(!coordsRdy) return; + coordsRdy = FALSE; + X0U = Xtarget; Y0U = Ytarget; + DBG("got X0U=%.1f, Y0U=%.1f", X0U, Y0U); + LOGDBG("got X0U=%.1f, Y0U=%.1f", X0U, Y0U); + // move U to max + if(nth_motor_setter(CMD_GOTO, Ustepper, theconf.maxUpos)){ + LOGMSG("process_setup_stage(): SETUP_WAITUMAX"); + sstatus = SETUP_WAITUMAX; + }else{ + LOGWARN("Can't move U to max"); + sstatus = SETUP_INIT; + } + break; + case SETUP_WAITUMAX: // wait while U moves to UVworkrange + if(!coordsRdy) return; + coordsRdy = FALSE; + XmU = Xtarget; YmU = Ytarget; + LOGDBG("got XmU=%.1f, YmU=%.1f", XmU, YmU); + // now move U to zero and V to min + if(nth_motor_setter(CMD_GOTO, Ustepper, (theconf.maxUpos+theconf.minUpos)/2) && + nth_motor_setter(CMD_GOTO, Vstepper, theconf.minVpos)){ + LOGMSG("process_setup_stage(): SETUP_WAITV0"); + sstatus = SETUP_WAITV0; + }else{ + LOGWARN("Can't move U -> mid OR/AND V -> min"); + sstatus = SETUP_INIT; + } + break; + case SETUP_WAITV0: // wait while V moves to 0 + if(!coordsRdy) return; + coordsRdy = FALSE; + X0V = Xtarget; Y0V = Ytarget; + LOGDBG("got X0V=%.1f, Y0V=%.1f", X0V, Y0V); + if(nth_motor_setter(CMD_GOTO, Vstepper, theconf.maxVpos)){ + LOGMSG("process_setup_stage(): SETUP_WAITVMAX"); + sstatus = SETUP_WAITVMAX; + }else{ + LOGWARN("Can't move V -> max"); + sstatus = SETUP_INIT; + } + break; + case SETUP_WAITVMAX: // wait while V moves to UVworkrange + if(!coordsRdy) return; + coordsRdy = FALSE; + XmV = Xtarget; YmV = Ytarget; + LOGDBG("got XmV=%.1f, YmV=%.1f", XmV, YmV); + // calculate + double dxU = XmU - X0U, dyU = YmU - Y0U, dxV = XmV - X0V, dyV = YmV - Y0V; + LOGDBG("dxU=%.1f, dyU=%.1f, dxV=%.1f, dyV=%.1f", dxU, dyU, dxV, dyV); + double sqU = sqrt(dxU*dxU + dyU*dyU), sqV = sqrt(dxV*dxV + dyV*dyV); + LOGDBG("sqU=%g, sqV=%g", sqU, sqV); + if(sqU < DBL_EPSILON || sqV < DBL_EPSILON) goto endmoving; + // TODO: check configuration !!111111 + // proportion coefficients for axes + double KU = (theconf.maxUpos - theconf.minUpos) / sqU; + double KV = (theconf.maxVpos - theconf.minVpos) / sqV; + double sa = dyU/sqU, ca = dxU/sqU, sb = dyV/sqV, cb = dxV/sqV; // sin(alpha) etc + LOGDBG("KU=%.4f, KV=%.4f, sa=%.4f, ca=%.4f, sb=%.4f, cb=%.4f", + KU, KV, sa, ca, sb, cb); + /* + * [dX dY] = M*[dU dV], M = [ca/KU cb/KV; sa/KU sb/KV] ===> + * [dU dV] = inv(M)*[dX dY], + * inv(M) = 1/(ca/KU*sb/KV - sa/KU*cb/KV)*[sb/KV -cb/KV; -sa/KU ca/KU] + */ + double mul = 1. / (ca/KU*sb/KV - sa/KU*cb/KV); + theconf.Kxu = mul*sb/KV; + theconf.Kyu = -mul*cb/KV; + theconf.Kxv = -mul*sa/KU; + theconf.Kyv = mul*ca/KU; + LOGMSG("process_setup_stage(): Kxu=%g, Kyu=%g; Kxv=%g, Kyv=%g", theconf.Kxu, theconf.Kyu, theconf.Kxv, theconf.Kyv); + DBG("Now save new configuration"); + saveconf(NULL); // try to store configuration + // fallthrough + endmoving: + if(nth_motor_setter(CMD_GOTO, Vstepper, (theconf.maxVpos+theconf.minVpos)/2)) sstatus = SETUP_FINISH; + break; + case SETUP_FINISH: // goto middle again + if(!relaxed(Ustepper) || !relaxed(Vstepper)) break; + sstatus = SETUP_NONE; + state = STP_RELAX; + break; + default: // SETUP_NONE - do nothing + return; + } +} + +// process target finding stage (target should be fixed for at least NCONSEQ frames within COORDTOLERANCE) +// return TRUE if finished +static int process_targetstage(double X, double Y){ + static double xprev = 0., yprev = 0.; + static int nhit = 0; + if(fabs(X - xprev) > COORDTOLERANCE || fabs(Y - yprev) > COORDTOLERANCE){ + DBG("tolerance too bad: dx=%g, dy=%g", X-xprev, Y-yprev); + nhit = 0; + xprev = X; yprev = Y; + return FALSE; + }else if(++nhit < NCONSEQ){ + DBG("nhit = %d", nhit); + return FALSE; + } + theconf.xtarget = X + theconf.xoff; + theconf.ytarget = Y + theconf.yoff; + DBG("Got target coordinates: (%.1f, %.1f)", X, Y); + LOGMSG("Got target coordinates: (%.1f, %.1f)", X, Y); + saveconf(NULL); + nhit = 0; xprev = 0.; yprev = 0.; + return TRUE; +} + +/** + * @brief try2correct - try to correct position + * @param dX - delta of X-coordinate in image space + * @param dY - delta of Y-coordinate in image space + * @return FALSE if failed (motors are moving etc) or correction out of limits + */ +static int try2correct(double dX, double dY){ + if(!relaxed(Ustepper) || !relaxed(Vstepper)) return FALSE; + double dU, dV; + // dU = KU*(dX*cosXU + dY*sinXU); dV = KV*(dX*cosXV + dY*sinXV) + dU = KCORR*(theconf.Kxu * dX + theconf.Kyu * dY); + dV = KCORR*(theconf.Kxv * dX + theconf.Kyv * dY); + int Unew = Uposition + (int)dU, Vnew = Vposition + (int)dV; + if(Unew > theconf.maxUpos || Unew < theconf.minUpos || + Vnew > theconf.maxVpos || Vnew < theconf.minVpos){ + // TODO: here we should signal that the limit reached and move by telescope + LOGWARN("Correction failed, curpos: %d, %d, should move to %d, %d", + Uposition, Vposition, Unew, Vnew); + return FALSE; + } + LOGDBG("try2correct(): move from (%d, %d) to (%d, %d) (abs: %d, %d), delta (%.1f, %.1f)", + Uposition, Vposition, Unew, Vnew, Uposition + (int)(dU/KCORR), + Vposition + (int)(dV/KCORR), dU, dV); + int ret = TRUE; + int usteps = (int)dU, vsteps = (int)dV; + if(usteps) ret = nth_motor_setter(CMD_RELPOS, Ustepper, usteps); + if(ret && vsteps) ret = nth_motor_setter(CMD_RELPOS, Vstepper, vsteps); + if(!ret) LOGWARN("Canserver: cant run corrections"); + return ret; +} + +// global variable proc_corr +/** + * @brief stp_process_corrections - get XY corrections (in pixels) and move motors to fix them + * @param X, Y - centroid (x,y) in screen coordinate system + * This function called from improc.c each time the corrections calculated (ONLY IF Xtarget/Ytarget > -1) + */ +static void stp_process_corrections(double X, double Y){ + static bool coordstrusted = TRUE; + if(!relaxed(Ustepper) || !relaxed(Vstepper)){ // don't process coordinates when moving + coordstrusted = FALSE; + coordsRdy = FALSE; + return; + } + if(!coordstrusted){ // don't trust first coordinates after moving finished + coordstrusted = TRUE; + coordsRdy = FALSE; + return; + } + //DBG("got centroid data: %g, %g", X, Y); + Xtarget = X; Ytarget = Y; + coordsRdy = TRUE; +} + +// try to change state; @return TRUE if OK +static int stp_setstate(STPstate newstate){ + if(newstate == state) return TRUE; + if(newstate == STP_DISCONN){ + stp_disc(); + return TRUE; + } + if(state == STP_DISCONN){ + if(!stp_connect_server()) return FALSE; + } + if(newstate == STP_SETUP || newstate == STP_GOTOTHEMIDDLE){ + sstatus = SETUP_INIT; + }else sstatus = SETUP_NONE; + state = newstate; + return TRUE; +} + +// get current status (global variable stepstatus) +// return JSON string with different parameters +static char *stp_status(const char *messageid, char *buf, int buflen){ + // FNAME(); + int l; + char *bptr = buf; + const char *s = NULL, *stage = NULL; + l = snprintf(bptr, buflen, "{ \"%s\": \"%s\", \"status\": ", MESSAGEID, messageid); + buflen -= l; bptr += l; + switch(state){ + case STP_DISCONN: + l = snprintf(bptr, buflen, "\"disconnected\""); + break; + case STP_RELAX: + l = snprintf(bptr, buflen, "\"ready\""); + break; + case STP_SETUP: + case STP_GOTOTHEMIDDLE: + s = (state == STP_SETUP) ? "setup" : "gotomiddle"; + switch(sstatus){ + case SETUP_INIT: + stage = "init"; + break; + case SETUP_WAITUV0: + stage = "waituv0"; + break; + case SETUP_WAITUVMID: + stage = "waituvmid"; + break; + case SETUP_WAITU0: + stage = "waitu0"; + break; + case SETUP_WAITUMAX: + stage = "waitumax"; + break; + case SETUP_WAITV0: + stage = "waitv0"; + break; + case SETUP_WAITVMAX: + stage = "waitvmax"; + break; + case SETUP_FINISH: + stage = "finishing"; + break; + default: + stage = "unknown"; + } + l = snprintf(bptr, buflen, "{ \"%s\": \"%s\" }", s, stage); + break; + case STP_FINDTARGET: + l = snprintf(bptr, buflen, "\"findtarget\""); + break; + case STP_FIX: + l = snprintf(bptr, buflen, "\"%s\"", fixerr ? "fixoutofrange" : "fixing"); + break; + default: + l = snprintf(bptr, buflen, "\"unknown\""); + } + buflen -= l; bptr += l; + if(state != STP_DISCONN){ + l = snprintf(bptr, buflen, ", "); + buflen -= l; bptr += l; + for(int i = 0; i < NMOTORS; ++i){ + if(!motornames[i]) continue; // this motor not used + l = snprintf(bptr, buflen, "\"%s\": { \"status\": \"%s\", \"position\": %d }, ", + motornames[i], str_states[motstates[i]], motposition[i]); + buflen -= l; bptr += l; + } + } + snprintf(bptr, buflen, " }\n"); + return buf; +} + +typedef struct{ + const char *str; + STPstate state; +} strstate; +// commands from client to change status +static strstate stringstatuses[] = { + {"disconnect", STP_DISCONN}, + {"relax", STP_RELAX}, + {"setup", STP_SETUP}, + {"middle", STP_GOTOTHEMIDDLE}, + {"findtarget", STP_FINDTARGET}, + {"fix", STP_FIX}, + {NULL, 0} +}; + +// try to set new status (global variable stepstatus) +static char *set_stpstatus(const char *newstatus, char *buf, int buflen){ + // FNAME(); + strstate *s = stringstatuses; + STPstate newstate = STP_UNDEFINED; + while(s->str){ + if(strcasecmp(s->str, newstatus) == 0){ + newstate = s->state; + break; + } + ++s; + } + if(newstate != STP_UNDEFINED){ + if(stp_setstate(newstate)){ + snprintf(buf, buflen, OK); + return buf; + }else{ + snprintf(buf, buflen, FAIL); + return buf; + } + } + int L = snprintf(buf, buflen, "status '%s' undefined, allow: ", newstatus); + char *ptr = buf; + s = stringstatuses; + while(L > 0){ + buflen -= L; + ptr += L; + L = snprintf(ptr, buflen, "'%s' ", s->str); + if((++s)->str == NULL) break; + } + ptr[L-1] = '\n'; + return buf; +} + +// MAIN THREAD +static void *stp_process_states(_U_ void *arg){ + // FNAME(); + static bool first = TRUE; // flag for logging when can't reconnect + while(!stopwork){ + usleep(10000); + // check for disconnection flag + if(motorsoff){ + motorsoff = FALSE; + stp_disconnect(); + sleep(1); + continue; + } + // check for moving + if(state == STP_DISCONN){ + DBG("DISCONNECTED - try to connect"); + sleep(1); + stp_connect_server(); + continue; + } + // check request to change focus + if(chfocus){ + DBG("Try to move F to %d", newfocpos); + if(nth_motor_setter(CMD_GOTO, Fstepper, newfocpos)) chfocus = FALSE; + } + if(dUmove){ + DBG("Try to move U by %d", dUmove); + if(nth_motor_setter(CMD_RELPOS, Ustepper, dUmove)) dUmove = 0; + } + if(dVmove){ + DBG("Try to move V by %d", dVmove); + if(nth_motor_setter(CMD_RELPOS, Vstepper, dVmove)) dVmove = 0; + } + static double t0 = -1.; + if(t0 < 0.) t0 = dtime(); + if(state != STP_DISCONN){ + if(dtime() - t0 >= 0.1){ // each 0.1s check state if steppers aren't disconnected + t0 = dtime(); + chkall(); + } + if(!relaxed(Ustepper) && !relaxed(Vstepper)) continue; + first = TRUE; + } + // if we are here, all U/V moving is finished + switch(state){ // STProbo state machine + case STP_DISCONN: + if(!stp_connect_server()){ + WARNX("Can't reconnect"); + if(first){ + LOGWARN("Can't reconnect"); + first = FALSE; + } + sleep(5); + } + break; + case STP_SETUP: // setup axes (before this state set Xtarget/Ytarget in improc.c) + process_setup_stage(); + break; + case STP_GOTOTHEMIDDLE: + process_movetomiddle_stage(); + break; + case STP_FINDTARGET: // calculate target coordinates + if(coordsRdy){ + coordsRdy = FALSE; + if(process_targetstage(Xtarget, Ytarget)) + state = STP_RELAX; + } + break; + case STP_FIX: // process corrections + if(coordsRdy){ + coordsRdy = FALSE; + DBG("GET AVERAGE -> correct\n"); + double xtg = theconf.xtarget - theconf.xoff, ytg = theconf.ytarget - theconf.yoff; + double xdev = xtg - Xtarget, ydev = ytg - Ytarget; + double corr = sqrt(xdev*xdev + ydev*ydev); + if(theconf.xtarget < 1. || theconf.ytarget < 1. || corr < COORDTOLERANCE){ + DBG("Target coordinates not defined or correction too small, targ: (%.1f, %.1f); corr: %.1f, %.1f (abs: %.1f)", + theconf.xtarget, theconf.ytarget, xdev, ydev, corr); + break; + } + LOGDBG("Current position: U=%d, V=%d, deviations: dX=%.1f, dy=%.1f", + Uposition, Vposition, xdev, ydev); + if(!try2correct(xdev, ydev)){ + LOGWARN("failed to correct"); + fixerr = 1; + // TODO: do something here + DBG("FAILED"); + }else fixerr = 0; + } + break; + default: // STP_RELAX + break; + } + } + DBG("thread stopped"); + return NULL; +} + +// change focus (global variable movefocus) +static char *set_pfocus(const char *newstatus, char *buf, int buflen){ + int newval = atoi(newstatus); + if(newval < theconf.minFpos || newval > theconf.maxFpos){ + snprintf(buf, buflen, FAIL); + LOGDBG("Failed to move F -> %d", newval); + DBG("Failed to move F -> %d", newval); + }else{ + snprintf(buf, buflen, OK); + newfocpos = newval; + chfocus = TRUE; + } + return buf; +} +// move by U and V axis +static char *Umove(const char *val, char *buf, int buflen){ + int d = atoi(val); + int Unfixed = Uposition + d; + if(Unfixed > theconf.maxUpos || Unfixed < theconf.minUpos){ + snprintf(buf, buflen, FAIL); + LOGDBG("Failed to move U -> %d", Unfixed); + DBG("Failed to move U -> %d", Unfixed); + return buf; + } + dUmove = d; + snprintf(buf, buflen, OK); + return buf; +} +static char *Vmove(const char *val, char *buf, int buflen){ + int d = atoi(val); + int Vnfixed = Vposition + d; + if(Vnfixed > theconf.maxVpos || Vnfixed < theconf.minVpos){ + snprintf(buf, buflen, FAIL); + LOGDBG("Failed to move V -> %d", Vnfixed); + DBG("Failed to move V -> %d", Vnfixed); + return buf; + } + dVmove = d; + snprintf(buf, buflen, OK); + return buf; +} + +static steppersproc steppers = { + .stepdisconnect = stp_disc, + .proc_corr = stp_process_corrections, + .stepstatus = stp_status, + .setstepstatus = set_stpstatus, + .movefocus = set_pfocus, + .moveByU = Umove, + .moveByV = Vmove, +}; + +/** + * @brief STP_connect - run a thread processed steppers status + * @return FALSE if failed to connect immediately + */ +steppersproc* steppers_connect(){ + DBG("Try to connect"); + if(!stp_connect_server()) return NULL; + if(pthread_create(&processingthread, NULL, stp_process_states, NULL)){ + LOGERR("pthread_create() for steppers server failed"); + ERR("pthread_create()"); + } + return &steppers; +} diff --git a/LocCorr_new/steppers.h b/LocCorr_new/steppers.h new file mode 100644 index 0000000..42a149f --- /dev/null +++ b/LocCorr_new/steppers.h @@ -0,0 +1,39 @@ +/* + * This file is part of the loccorr project. + * Copyright 2024 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +// set state to `disconnect` after this amount of errors in `moving_finished` +#define MAX_ERR_CTR (15) + +// amount of ALL motors +#define NMOTORS (8) + +typedef struct{ + void (*proc_corr)(double, double); + char *(*stepstatus)(const char *messageid, char *buf, int buflen); + char *(*setstepstatus)(const char *newstatus, char *buf, int buflen); + char *(*movefocus)(const char *newstatus, char *buf, int buflen); + char *(*moveByU)(const char *val, char *buf, int buflen); + char *(*moveByV)(const char *val, char *buf, int buflen); + void (*stepdisconnect)(); +} steppersproc; + +steppersproc *steppers_connect(); +extern steppersproc* theSteppers; +