From 5265b98bac5b14b5d15da3e1a1327f863b205d65 Mon Sep 17 00:00:00 2001
From: Edward Emelianov <edward.emelianoff@gmail.com>
Date: Fri, 20 May 2022 20:39:22 +0300
Subject: [PATCH] caclulate To for basic range, hangs @ sqrt

---
 F1:F103/MLX90640/MLX90640.bin | Bin 15800 -> 20516 bytes
 F1:F103/MLX90640/Makefile     |   2 +-
 F1:F103/MLX90640/hardware.c   |   5 +-
 F1:F103/MLX90640/hardware.h   |   6 +-
 F1:F103/MLX90640/i2c.c        |  14 ++-
 F1:F103/MLX90640/mlx90640.c   | 231 ++++++++++++++++++++++------------
 F1:F103/MLX90640/mlx90640.h   |  10 +-
 F1:F103/MLX90640/proto.c      |  51 ++++----
 F1:F103/MLX90640/usb.c        |   4 +-
 F1:F103/MLX90640/version.inc  |   4 +-
 10 files changed, 208 insertions(+), 119 deletions(-)

diff --git a/F1:F103/MLX90640/MLX90640.bin b/F1:F103/MLX90640/MLX90640.bin
index e1978e61abd04d3744611732924aba014c6af320..46051afbc775866ce41a28183d38a40e45b57f9b 100755
GIT binary patch
delta 12605
zcmeHtdwkPXw(tJ^@@Sf-ZQ7LDLWN)2f|V8seS-y(215FvDKAAuNTpy=ged4?)HA=d
zWwg$KQ;p*TP@|xuf;?JqeD#F!bv%PQt>_$ljV-knQpUSOTl!1VHs`y4Ey_3_@A=%%
zoqPYew|v%b@3r^ZYpuQZT5CVTYxnN=aw4fF!jhRpQwCV{6CgkNV>Ap{Gb?AP|8dMu
zdHgrHC<`4_{mAI1oFD7|&ES7f_<x;V{O1dE9dl4}w`1T0hjOJLM3-wTJsYj`N`<Fp
zgY&sTckc4K80&JUs^VQqDXvPXalMu5bS)-6qUX(oaAu<8%=DP%Dv}vJEo<uJipgcW
zH_o;0uF=+&(wj=P&d)baDz^-fgwJb|+>bV-Ze;b;n$(R(xqRH6yyJI`9~|YTItEDL
zs*MR{S=LoG`v$+x+czj}{f#rGF4k_P4T>12TS~Czs-2!DZJBj|tn;{IU8-{WmZ8Z?
zIfcOum6$?MhO8iFH$lCNbe1bUllf$InUV;0rDram&UspI;&0>P%F$8@ASPliQ+ocQ
zIHSFUg79tSXfmUa*?`?;U4K!PCbBNR+)&5m8DvvbD)&uSbDY+b9W^_Fi(n?!@{2EL
zW!G>*WGKdSd(?uA`Upp}hgThaPa<xP>Vu}E9R0<xR^aH0VUCs$6RPz5Im*aaDm@pX
zc8U|_uE8)z`@%#}9&xp4?HrAlTm5M544Z5mjgj#)z)yz-8>1q=_C-V^0cM<&tZ_|S
zC$|n(i+P`XBW-VvvZdOaFzS5xa|sKFQY(<D9DO^SRJlFXXmhtjhF*y9`M0GtIgUQn
za;S8mBNjqG8z=t^<QzR1eo-P$jlJ!x)Tlx4`)HjBrd}xVSophMBCsA;DbcKek#;~4
zW;+knOcZY&J`!?Mb2NWM4?XqAD<(G_VcNYSsR@4y7Dc<x*4T368zen8fHos~Op&3g
zVf*Vf<iVC`dt_+m$d5eK5PzSn)O8TP1a@K^nArUgOYk@29CiG<;`Jkww)59SiP>4F
zzd385^NE8TC%t4Qry<i7(@Eo!9hKR2>2-T1who52B!6CC(HewTWE1-N4$H;Z1p9?p
z5*d0l!rMrenO^b0J!LUicx^W6pZGwGbIy^rv-G!jmTfJMsngd@Vm!ow&*1?RNah{(
zs0>w&rfzeY-l=nno9#nqeq^{Q!r1lixJC9O#!mQ^!TF!P&WiQ+G+UAfs~xVb5bDxw
zTGV6Zd43x@CfQ8iNVP|;{ZZ-5xC0U#c)F!syL%mtC|ab2<{LS$c1l*0&s@B3kSuTV
zxmylMyot0Z=wvbIpc|Gd=;mN%%d@JJl9IgJA5H8&@1WbMpz-M4j^4FF?cS4;hGaAy
zlS+k@m;;hdP*8&2d(c~rvcaOD3#DU0v+1OyCkLb?*vd?!8i_G!cdk9DF0Wka<TDEG
z2~PK!LM+k25c8V{LLx{1HI!GLSCP1#nKVyI)JKN=qa=&aUo_0BG`+~BD~+Z{BSWjg
z%raXVer$1s6|(WyH>iv$<*5)d5kfNTNtH&|x-(fX!QAG^ka?6*5&C#To8&basD?LF
zvO{N&EmPF#>L!VkS_khD#fw>om37(*ol{koWY@YTIyt&p*03nV(HWtX-qXornV}kv
z^cCu#YGGugl}{HEjWF)lSty?XEV+gVf{ke0gy>%b?zR%m9hh@o=#C1lFt&^_;LC+J
z+cE#FkWk0bt7UN{bSns_gl?_4wK8QpM<>gzgF$7$tjd@8cykL!OF{xn*B5-gbg<**
zDy^V>jHBNMofxMGy(+Cxb97#44Y1+htCAU5VdzF+gTM%|S)uFgh<hZO^JqyD(KJmK
z@=oy0Aa8m#xI&GTAAMq?f+k8z+nYf`6f{=iY=TWe6M|8;nC5b@`#4xuR&3*GWrR0!
z^c9)IbHP`GW;IVQM!uBvdL`%=Nh;f`L4vmP5zdwc`h1j((|O53ABN9}`O=q?62!%5
zoQcL?F6C+ZIMElf0y@IekK`7f8o*kkSJ02p!!=KbBEJp_wyVo{s*x4hC@ZoCQ%S<8
zmWUGKlOX<;agIkZ4l-`gHD8Ibn}UL9WBOOg!>qyB3M2)O!(XOQP+oyD@sSpV#}Rt4
zpIoJW<fC&@bCz}z-v?MvmcLj&qo}j>UIv^cnPK2c_=FCS1V{tq<t;Z0C<*z%l8KV}
z&zF**=bqrHcpREl!!W-eRS!PAJSi`x&#F+`T7ydHzkGbw`!MS?*vJgJry@)7ePooG
zk*AM=f|<2xbZ>=Kx|(oB!Yo4j-BEkgyYDsgPb;ts&Ry7AIwjN)q#fEldBIfhy(*%6
zlBe4v(HD}$buoR7itFoOPH3}5W?nlQ)Ayj_l(Zs^ryC-Q-BD~SwJI(q8NQZs++*}A
zS()o(`bsFnl&Bt8w{-NL3YBfrc9OlbgWvO|ltL62Qw(F{csef<9Y|8f^zBe`HfNp4
z)9leNrA!m|bgE5hGufg&Jk62W8pr4Od8Dq4_3$)hbOTm;!l>HD0ZWa{#y1&4Szltr
zXj98JiA9a4+K3xe%tk7ij*nPu%4VkK$f$BRXVarbM|hDoBNDI0KD;9G(K#jerBtkE
z%h5H$n4BGr-lMdop!Tn!ug@tnc-j%dnvIMx?m)>HJ}3+L(+E#*1uqkL&5RW7J9UnL
zo(uBymXX=LLMAarXB69u%at`rb#mz}Mm7|S%MC7$PM26ZR7r$}BV(Hf$(UI_vn;7@
zwq56(RW_qM#$`ZKN(LRFfe4|!<8)t3LUVM0i+(+j5-$$yKQs+~t&o!TvYeujslSM@
zG5?7%A4P<6!EV^z8fxsnG1}lF^m|e6{10^cBE-mL7|Jv25}bOw0jbpB8dD{%Ok2n0
zYNwO_d+N2$$G;`M!~Mj)f&C@kVLpvaScwS8^E6qRhqJ86qJw2hmL-qC<5~7Rjc8#J
z!=fbzZ7p^}4@bnLJt=jp!gRbpa;QATnD%01$QsUi@nE@Nj10{WA3$3=+UA5`MvDox
ze4J~JNtMDesmgRrsy3JQM_rw&RX1sy7dIu*c(O~{EbNjtHSdzPn086G>lX~XeUO_<
zjH(c!`iFV?@<>ecyZs8i#-tL_#M9?Sw0m^TX**k`bhi-0)7>Ku`jmyI{*hQlD6oM%
z{q0DyJZubW8i{LGLf9BqKN8>Eh#4)8B<;~ZnNp{In9xP?q|zeAEj+zr<cek%z8gnU
zn&1C{<AxD&0z>s<OxBJ>H>-bOxoX5!aYt2@CpNG}k%;dD3Qco@=Od~1NaCK@fG@@n
z<l<XwiHH~L$OOdmqa#<~``~cG9z`?jBU2O5S1^){*84|N@V$3fJ!oF1Z2nqGbH6ZX
zzJ>D`#K*W!LvY*PVu`QVr=60$!)Lx`PYdtKI<V?Go?bB$jbU{o3Vd%G=J%*=DVU3H
zM8j}+KMTC2!|OCRM3qa*7zK^O$w5=h)9`Q%C@M$PHajQ=hQ}yNhFRJf9O3q4Mq5G8
zd6ZOK5WP=53EJ9yF9mQ4q<<Zb1JSGzCBB)%N}0i@!|}kTj)z_9a4fJ-Mz{j?Zk3#x
zgc-#zF9t)y!Y4bT_86Wv=S6uk%k_KQg?!#)rFsvc54l-FFMe6=IhDeuyQVj*jyDR<
zNn&{#4v^o!IjNkb%EaxvO9`FaKr(QkK=7p^_)^Of5q#F~lI%(4%DQBGj5Db$$r*#-
zSw6~~cIGIdnWK@R_%|!z-K*gBwSet_9e@>RJBHGf0hl!w5!LX176mLyL>4AGY#J;?
zUPPJo(%8}RZ$Xl^U_f<X0i0^eD9$c~(CcM^7e@#^KXU1O^Ev3uG`_(xP`TCgQt1V~
z`4#B|&c`vKv2)M)dSaYaZo=uvgpgKb%tF%npoe5kw`Z3r>kI|X9L(`VPmcX-I2M7&
zC++!0deO~-^6UtkpKpW{w0jX|)$jEyGM+H=G;Y}AG)xn3*Yh-<v-|XOCgCI*&m-#e
zsZr(@w9G~HAquw$nt)y%8@OG`)9&ybxWd_CJJ!jz$k3h?<YM}JiEqDUY%{%-F5Uu8
zV6@c$6O5$Y(<-enrM4st5b=*VnwDPJWNB!*()XM==)U=pmTZX=0BGK9*#rO8J~8IO
zN2F`bhK1-cIpML-hp}Y{(BGiRV>5UvJm!(l4rgdZ`24;>_trfvCnbR}|9&Wp-H=gF
zeG#+|fR=3|zZur<IVq))*5E1=H|=zbI7uxZlU+Q$JM5s_aBg`&d=i`8KCD;~%*aew
zB%3e}>7`X#W=`4pymrbAhhJ~PS$v)vNu|m63Itvoj<T_1H-ZVAc3aqKh^Nnt3w8{b
zUZ}MyZCNc9k^%}X0F7%YcCO!|!j4OgA+}dFg-b8gS+&oAh&&#=9?Y`QUyJ^G!+((0
zRG*YGi3-U@V`74Nnub~aLAs$?`#8=wz>~svV{=^}X1tPz-L|s+DB+}(@8)S#*oB@|
za!<5ug<{ewiL09-{<^Wj7elvzWU)*lg&W2Pmw0)4E>we_xpGfmm}H!k%;cn$W#XFO
z>*q{7{XBF7`lpBMZ7{f=ZLnofARWp9hXOg6@xRM$DKd}PkfTgxJ1Ln&!t!aH>{f~p
zC!6d|kerkjLV(@HcJWAvIo&|eZDZ|~gzeUSL2mbgB?+iexwj{1LERrZDOHoXSh^dB
z+2+Yg=KZ<2gJ%tA))kbI^n!{4`%IV4i3`f>#3+v4FPbICKs6%IG2nKNMV>`skvF5A
z7_-Z-Lfl=2xKn3bjkt>pMcyRFth$VH!O3Mzm0K>pnO&D@&u~tv&^jj~sietbhr)#Z
zEgTs-{aVWS<|_^rAH0HX%ScK`dJ@aDqP!=ILo*I0*zs^tH^KgV&jjc{93D#+g)*a4
zVDum1?w(qN)7TCzSO^hHS-RPTwo75R9d(wE%t%2l<fmppCqmH){0K_TaW!~<j0-2B
zFM;PXVNsLLS4?ziaBCj`F=MDdKB00#dB%3VYtm6JTi=eBwnPkTm4`iqVLQU{6)D?^
zDaFSLqeJn-vGTS0#-qxN7<&e0|Flf`3s7zea~REMsO=ZVE?>w6dfedID+@b7J4GNS
zfa^F3xK(7(zTx?RTe$-1g*OuF92TJI%J4y4l9cFYdRK)Hm7ecl&mnjMK#-A)B#Db@
zIKp|nfklekY;s{u1A7a|<yQ0T-~92?j=Gh*1K1sW9<h_LUU96r{Ck(&wTk3&4djh+
zvdjBd^HJk0SM#adgO$ob^9H6IQ<{U)YFX)Uh$+n!z8E@I`hADG(+wLYhQ$h&Q4(bf
zoeL3>eiLGk8+d7`=U&2n6DHeHszRx5;9lbP5_(JcSJ=(2Y~Y@(kRN>9bV7r-Wpro>
z9vKMT@7`qprgv=QGK?ft*T6lYP@4ni>u<4t(t8Vpo({p5P~?qpEwGLdXIzJ}HC%(Q
z_$PF-%E9nW_`VZT7}ubDK70edZ-tb`)hM41UyJW?VArF3JiHv=L%^;>*(}fGS4c+p
zVj{c8Rf!#+#C}YQdxlVzJc&2!xu<TO+=6!@H$7VaN=s~j7rED3UXl3f(V?BASj#ME
zS>J|djRU1|?PjpQHw28Zaz3za48xnmf`Qt@Wa>B4u-n&isD-9N;gA_Ns}1E#$)J83
zH$g(L32_CW7Y?-SF6}#AKCaq{5VO+idI;y*BvDC5hZ68oVWwN_r`fL=CprWowg`)v
z{#b+;LTnM%)O%anaL*kbx(_470qfyO_ViwFU_BXoxrz+c%I!smlkLg9EH-w8lI#h+
zks;ft!5-Vo4%zJR%tEvhPYXM3BuXPJMg%POj82*;no+F;c+r=Fk^nkUrvN+Zk5>H-
zMmUQLIv&8NKJ*B%E?~?+(V=EwUj&s#R~=iqUBFHOI|A%@=r_PV1f>p?1HmR>?*)0I
zX(OS(58V&!-_VyNqVv08BhWtt)fZPRQsHo!V{;c=ipEDmEE*pOUUKY);9I3rZGY?7
zjuB7e0v{QgFnTxYPXrMX>y?9++LhyRe3v}(R{61GGbkPi;s9W!Yv4w~+klI}H(N#4
z@ItT-4gbQlyA@}p&<0>GU=+G83o*yt2<!;3iV*YLn=!Q3ivCrEmWEcL<$AQ#jknCf
zI|;$l7H@QcTW*N?aar(M92QTEtVG?)IHPB32tF;+lA!4}mUkwHOqtb7s;f+Jfn3at
z(AeNS%v}p|HYebratzOu`yvvT&w9hsw=HLJASHA(n4i<xq5iHEG>Jh=c_|*omP_fd
zZ%mMRwI;Zr;)yyp708Kxv!3?ML))k%GN;A+E&Vgz{=V;-V?Ph@1J3rGK)~-07TLRd
zia`2V*w^p)ZDCnq#h>c<fzF!^){{=wGTNJG|D-3+-VXQ}@FCz&J!1hB8)V#H26r#G
z`+8=8&wG+!d!vW=SSH@In24HfaI*UVP0ZN<Ucf=XYk*Gyoq)3d6JP)^3{cwfP69{)
z2msR+xEZ3%2k22RLAemH3bbr{$%Hf4qRwuZNG_UHz%{_%vfYS{3`jK^$=^t)l7v(T
zfq%*d0Nel<ARoloE;3b1)K%GaCR-YIoF!pp#Y|_Kn{5k-4WSzQOckK`cw!|x20Y{G
zy0Rj1KU(;TNll8^U5g__Z}_UN@ywq*jYqLz@<m=MZ(TDbnX8;7zuxY4t8I6eW!X4f
z0LKzjha1nnjx5_8wB?P?v8il|e7wtndY}X60koRrl>D%p9q1UYjd)H7$vTd%9bx1g
z4UT%w2<iFpktiVF<3h!#iiej~ESIwM94#1LD%UiDNb%ZGlf=>dku83WilY}tJ;lbe
z*K)K4&*8rujk4|OWpD9gVmBe_EEu@%@FlS}ISG{<UecRp`(rO5ClJ{8EPM`=*H>W8
z%8BNq4?q0yEXwnMfpX8?>9<zDP=YODH8D%a<CbJ^U-vqG<jBxfO|j)UbrpOkcTJI#
zZ_n9Ia)RyI+hbnm1gVW=YVkIs5OQGu^kKm!wIRF6Kf25>jVGSf8TZY9Dzbml{ucY|
z1q(}#nsaD}5fhFKJ%Yy`a@5t<cGiWb-QW0Ji<Jn-2O=aR&(+s^dL9v;hcYjgc#<=#
zg=^8A?ds|^;+l#nW582B;=7FK{>&F9p9THvnM4yim1qJeFLg78z^8obTs3QYp``WK
zQ>Sz4cGPK!=Z$Ia^C%jwx*J7n=&J9zxr`C(J<qJ+BxZ`T8Mq#d7>7z2&<St>HUTmL
z`vL70t?AR3aT6Im8_#ItFoy9<n2v+kJkLYfH!x}b>{3?D%$YBK9Ukpvj?Ns`&JILi
zB$pUW9W&15v@bbB%-$}p+S_F(q%UdqGd;xV?P@=As*7`ZyOe^rOJzbyQ0l#1Y6S^#
z<lb)L^%^*@S3yX1H%bE`H+19Kaw2hiyGgURdz@bE(ttrU7<|i<5F=KCNDXFDV3yOq
z>kM_Yot+$+)x`<k?!6`mCpUHr-d#O<^RAwx>Rmk=lsc3;vg>RDA=*p)G~f{p9@D{>
z$Cwndt0zsrt49m|hcFSN35u9RcaLCt3QU)G$Dkv{wd>3Y^7z@mMk4+cHwHBC>WKv-
zF9tB>Xao;2<MjQg??t<eo7_GB17og$6+eXqE7PCp;e=1RsAa}E&g|_X)!weVp&TJg
zyE#mjobYy)67q#byyr+)J?n6KyCeDUbVtO+XW|Rr={`8i+of<VKEo9@c5~#4?nYBX
z=T5=f_1r9P0_yr?y|-Iu^d|6XZ?_uE{@n$pojW@fWKCC`$=Xg%Jk(B{_qR7f7^h!)
z=D7uponGSYY5<RcNMs_{Iku#tVD@(N3Eu7`Lj0T?>*e+ucv9VkvXOIZ9wB5^ca*F-
z*0I{%sdIauB_{8_nS{KdfnZz!I=Rk=I-iRNL-UF=&+5HhjV5;|6LdAk>B#af6{+v+
zjYPhXeE~eCU@Z7_L5|p^Gg&)1leM7_=Hrcj>?Xn^@<2jXbmwD94yqsork`N4p4s^)
zCeYgzgy}i8zPk|p1eSpF`r{y&3FXv@-CSq=nGk4r;_cF*E$w4(m(aQS4Ck_T=FWF_
z?Sm{rR(5l4ap!YiM<OqO$%LCcU5CtG4lb+Z$S=D&{X>bI^M3wWvp0bw$=$qVhe28G
z?Rp<3jTAJV;R_qjL``h$R6z)}j5#S&j;$}wMj;jIO4KKz&es2@bg(#%-s1xrxG^GT
z1$cT_6b62PI@4b0#0>SF-G7#~rbwjTZWa+rIT}1YteyhLtEa@m<41)?O01^i4!=d|
z@LN@n`c?57U$dW&pW<8LC%Tn>UMKo%SBw7jt8wT#<97I0toXEV<;vH5x0mX^&ROX8
z^L)Kup=f}K8vPu12``+{<TJG;P~vd-x!Jg+rd9eAX7lal(waUCXOP<!d6j-c9^byK
zbRB+tV4`ujbMFsu%FhBqT<n*8C~i1Cq`Kfvu>=UG2*ru0T0^S->n(Ae&j$om9LTuo
z=orrORZ?EZ`c9-Q?d!gSqhwLkXAVl@z6ucCFA<KL`#A3CpnJ-sQ%48M6y2#Z&04>S
zIQ%6#B?LPB3zsDhy5rq+xBIOCacF#N{nfha03nt><x-1Z#hKdH`o&6}*<t~sgaP*y
zrsuhihbf6y`mj3)M?>}f8lRB2FA$?>4G6sM3tg_}Yd(;qdpn@xxsZ+;LwrEji5C8}
z@9lt~<A4hx-SAgFi(khzo4=j-oR5RJtF5NCEq<kPN=e!my8O7Wl=$7^U2qh{kP<)M
z@%?7Mf*3=}{zZNT^m;d-8|%hT1RPpf@6|R_8z?9%ABBpI18#%bS3lrR;PSb7RlRIY
zQ+#YyuWtWw%~kDe&6U1X{7PYJd)TjtpX?*Jh_Sh^Sgq<1&7{nd<+H$bOl?VnZe@v&
z8+0d_+L*Z)(NT+Kz{URK1~IiU0TX3LOh}q9#>U&K{an7<x6H37V65>!FN`J}ewt+q
z5~FJ|+;G5Ms|@5S!9xY$s_{^FS-<!LfgS?wK65D}nh&B<tmGNa;hzoQT~mCCRJ3}i
z=%}a8rLM)fR4mnC(`GG79}_$#%(;yzZp3ubzys>>j0$Q%ytea49B#ab|IAz}GT(KK
z>$ukc@ahEvrw=)(f?w<ZN|)kO<X$TIzOQ(!Fm^Y^uplpsV$|%9!=hpJ7q)#zd@cxy
z=qd*~H>LR#R$S>z^BYz^>et2p4nXK%rJRL`Dz(vHxirIHxg4q#r1+MJFr5W!XvJz4
zw|m3<$4t|%FyEu5q_i`v=mD<o3M;PN3!|p^swfw?KM+yAI!MIYLGw1{Cu{vxakpD+
zeQI^7|9zvyzjCR=|DIs>uc>zUKc0CkFrsVny*1dmW%B8NyA%a+m!iPcCPIwOhA21t
zc>?{oV|}ZYZ~H#&)2S?e<x*E0Qw}P`aV4bxfIBHr82?uv!4FKt;)0tJrBG4;kDheN
zqgBXz{etY&stb%KC`*l?*7u!q!F?~5_Nlowerp`!%74n`5-<5W1}ZTSukaz|rGfGw
zmMAV80D6H5xlrZj;~=P?L*44<6)ZNGS|~3F5KJKO@^LI?;yXZLGb00x1-!;5awS9U
z7QdSNoors$V*FnO+cIM9Qz#6`2pS(ZZ>ZhsS7R?`YpKJUruf*3>e{RQ7ByDTZ&i16
zFv~IFuoQ8%u^zQgZ%ehY-c*F6E}rkeiWeF>*jQBuBBn5nQHVC}4}&e2g&PbnW7|1b
z+yC6LelP9sQz@Z991BC1n(-Elq^`(wQ1{A3euKg4Ph5u>N?+tpn6SvNU+?gfX>m~Y
zDMXO?Sn0yRh!S_@ZTu%UY8?Jt{?&==`xL6Nx?{kt$Kzzqt9^XjU44qUbl<BJ@3NWw
zIx<fCllIj=)4um-+P@OXO20ZTrQP9ILAT_)9Da4O!~c%L>R-0i;eQ)pn3i^;gC*yf
z4vU}2sR%}{e6^RgAx&F}@S~3}xOn)`@jeCe9Te7Gp6^|4m~mAVHmsz-b8zu;4{!c%
z5tP~W9b09#W*T{=7Ytl;m|<GBg8AY={$aL;;=t6yY=v`r8K!0{TCiYX<Pd91@BPs?
z#DVt?0aJ<tv4`OeDsiCiP+~7KMTr_)wL_$wj_uhFluIP#N7%KoJ8xQzog3+g)h4xb
zN5DX*W3HxSX_l9FbG+g#)(1#DYgy%c5^LF9n&Uft#TI|J%S<`qT6~F@?0O|p9gHXA
z_!p*Z$*K06@Q9GX{-@mzYz{Zuc0%)YHrxb!-0{fJA8?Jt&o2<bc}~u@iXTTF^>G3b
zZjv-%;#(qV{uUW}H<Eu@+uw`R{veK*#qk|-Y%In^$5i860-e@OU+W-A`zkQHI`(tK
zPo=|ug0J*fyIPQ{%zcdnSwf~y5UJZ?>@Wn1?|jP#LbGeJg(W8cvSn`vQdnZv=|ei?
z5B#$4W3zYo|Cr(MpUO~RGiL6Zfjt)USA2+l^5*4HY$@|5!|J1E^MDx&F`i}_Ljc1h
zZs2TP>JcZUWQwo#iJT`?p8Immp3dB<iv0XSk0#IPd8Hu6^ICo~pI_{$%$vp+7I?Ph
zRq=(n9(&=H8F|I|SI-r;ZmYR-+s4~#vTztCw{O}$$5fbanz>HMs?0CS^&BafoKjQ(
zvUN3^ui7?7ENs2&W>?KE>pe5`()i*$&+7bKeold>Ie#)gr_ghR6~&(R{Av80Il!(|
z<mKjhQVKF_O+~pi^B_cviU}2zrb{L3xVCm4S%FH$Ej1fA1OGc^3TEd)i{)dIg*(^R
z)cyjjenOLH@?;fWsVK<HXA?wOAS+OipX+HVG%5;UhJ&oVpvd!KVQNM}VIg=fTQ8eH
zD9g3}lwZ+o&)LEmiwlYv-HiFdlC{fMSeC6QTVnYK*~N@x%t%bRf9Rjhj8HUPRRl7t
z$5>?W+)$+F@;$c|T{*oP4-BgSwE!33K0p&-FW?B^UBIURM}_AvMY(IM)~~M>YHPOD
zK**xa+t%MH6y3aWn^03*d*}MCTM4nsOEsoNm9XK?Ew>9MVcQm=I4_sTWA9vl%f_v2
z+z&&Dsidl89wQ*cb6c@-fz47@xo9EzdwO)*YBp|KkJ;HaZK>HNY=i0M3TP%d`2HwY
z+xlhkOfGItkzmd><>t;X70k#rd9Ik-=ZU`N+4$vG2{VMPbzA;LfZM^1Uc9CdyJ73Z
zlKVMv>m7G)WBl0D6nlsw9R8ywF%$AODn!ExxJIBN|LGJ&*ar9tFy8dPTj1*QPyS<S
yv%k2!j}GJ~-u~VBzp4l>@;~$U&zJu*f_|p?|I7aw!~9=r{(s&6tNu^f`M&`Es+VX0

delta 7832
zcmb_hdstN0wcqE=3=A(B1igr;XBbozu>%8$iaImkaUKZxsG8IyGXgrKMn@zDjlDA<
zO-Ym75T~~>Xf08btML&WFo{M3No;G=w3VjqHMjB8_ERyMwwnNQW(H>NZyz*m(?9y%
z@4ND?v-W!IwbtHyt^F8}v~%w~?ID#!m_LW83jn!a1M;<hOostuGdv~gpZk2>#=oFF
zveTQYzZtU5{#*NBbpEG+|L^*q|9@xht_n(a0KQo<NHj6ln6l?;V{9dKisCuvM#n1G
zt3yq>C4)VRRgR02(z8zOaG!TCBO2aF2xlZ3&PcmRW(8Hp$IQ%{k!GK4OZOgVdZ{*T
zx#><k_mXp#%S0bB&2seCng&Tyuk)~^?3vblaL=R`6Xm!`-b<&s>2O?FSv$^}RZ{6Z
zG;}rZ(2!L3eMhV-&SIube5}JOC6z=u+FI1s4D%q_(B@5WQx?Q(f&dUmJZJP0;6IY<
zX~Z{|pBOcX5FwTLY<vdyO6L>&x4Cg?;G_eHo){+)-z~+TVjqxD{Ead^k!i?gVD|4s
zw^ZiwOvKxgU7R7=0l(ep<nV$hjvMb=7@eCG8AbdhwtH`6Wji?`GMwc5PV^F;C(P00
z(7Mw<k%*U28quHTs52BRaI`qY(Yz3$yzg4HmQN=>Uj4j>mqk~D9Q`Cn1Z9W&Lac?O
z!$H>JH{f0h>P3zY$n-~`dxC<<v`BaUJfdC?AhV@2NmrxAc#%x!?wX?-CTT^lpU;mB
zcSTZ9cbQfFoNBN)OO!5<>FjGir_^fBG<B4o<1!L_wJ`~veIp$~FEz&S9U9mrXEc^Q
z9UMI^`+Ocgn}YGS1pD{~%a;knAdS~VhChu|o>ppO?67c`Bb+ZXyyLzNMaLtOGSq^&
zgy}?R2KEx7(VwwT-P6%IROvY>MG^iJA}Bia7vjY|{%@U2^ej*4k0K<4*FvbD<A@Ab
zj#$n*$+P}gOJw*zM#q9<9ilNc8|AP}2x}fDjB(T9C=&~%hO?Z{X~iM?+Z|~QirKo$
z*QAgA`z5017e;4Kx^nGyGF6k$NI9t`cgRulBDq9N_Y#skY+R9HcZ6gasjB%0@_NT+
zlY!jkWXHUY3%BWuT*ix*G&e`HWakPv4+NgKr@Ba1%vGZMzBRUG<vGr<-@nJKmH6!8
zKU|Ymf93e<74FbVNk>VyDwe2Ejw9-qfPV%!6HnAj<A_I{97ojO1(3|0_H=h-cu7Pd
zk}N_`cvTrs+ILt}U0dyH$CH-lB;>M#qc;OAJ$eEheJgOPZ|*VVp)oRiE<D!ji1&i@
zjviy>jr5ea#<J0>aK*bOPwpJr<$3v2rE9&t$`bEjw$fm$JK$l)+F-0T(9#D}DMTBi
zTW?uwQ#w;zYb}*FrQ;z>66>Jn=uz4A9%%0jjLpsR<>h7Bt_;`yw9cX6R?odnZ&uhl
z11PubO_$m9j?L}|VID&=((XOQ);QD?o8OQ2#5pSZ7J00TZE-G*Ycez7@!VVXCQ>J}
z*b=Q%In}OQtHu%USZL|)Gg$M#(%S_+!<+FgqMKzcu*6@VW6k*o+4MJwF5acLXss!7
z;BgqKT@HN)LQfB*^a(b>n&Qf`OtN{{$l@KzP+1_WOn^$<{m~^^V_jTc>})b{rzzGE
zxW#L;T=6!68P%j&ZuBMC6qHkpZ)h_Ux^NHC#oFRvc7jD=lc!k!NgwHEyU39!BAm}7
z>Iccns)KDd0wEq@#6Z+N`B;B!Xbpu#eFDk{fPV`<EkGy8QbCsiABC(Ib#POvEoTeY
zEphaWv<aNgBz*%%uS$&DB~_Iv`;Dr6iBB~8IU1A%aF0o^nTL8Gtza65rMCmC*Q~d!
zFY_=XS=eQ6%UxfVx&l1OO5>?pQi*F|xI@CIA>S*{&i5cEgr`o46SG8~)&x}Ia%lbN
zzLrPUs+X+b`C!*#<><>1Udz!sS+WU^jn9-q+AJAi^`u93yB=(^V+q(hr6RBo$=nrC
z;prSnTk8ehEj5Zoz?pDufzIX#Co*|vpn|?+;^}57TGYeBI)uV(j4kIkB&Ul7`iN9P
z|9u5VSD+WO5`)0I0`Uan9|4Pv&>VTy#nVwqDNci#0%@960C&C|eTp1?j2u>!M9|03
zz#0DF=dulM<CnQjM4MupV8Pm>UN~I}D#mq-bgQv^Y_0Xc8CY+&Q5B=jmbetStN``D
z8V!SCm2o|=3*ZLq1`u+|bU#Vt1oDFSyoJ!uBl6}094ipo6M5GDeQT#A>PrTqGgfl3
zUNW1|YmLpeDjHGvrBdU!xc1m-S&g=qZe!8mA+oBa+v{KDCCCeJq=I@jVp1+gcG@%j
zud3ELE)K*HOLzN_*TK<k5H~>jeq>wOw&h)tS)b8zyz!_*6UWgi$aX>23EX7j=zoQ~
z0!Dq7^LP`<CbYeABLe-;$Og7c0<%CjM>3|QYSWHJhD{^Nehok4sGyAuFZ6)O<Wsc*
z1O;g8c|kiD?Qe_-VwY4VbV=p<E~(OJ9*CYbDK@HQSxX8{BrixCg%_mi))%Br`WK`c
z4WXMFx#=WB6(m%%m!l~Wb?e0eg+{Gs9FE3EjP{t;^yfRJ46lIjl?eMs2zzzJ6U!8Z
zF||NAMea66eHo5xRl?aAbvGQ}nuk$kM{?|%wp5pDFQK#Kk&#FdCi4&B39T%nKM7B4
z{pnX0FNc#Er5UsF^Kf)))K?Zi2v4&M9$TXC@K8_HIA22lCdGJsJ&LH-guO|=UrOsc
z#_vz)e>*NYz$KFD<9(>%aPljt=kNr4|2mqqU(w2BNst@}r-1k8@I-w7G<x;(goecZ
z%GNgV@KEcTr0eUkm#|!73CNPK1#~NbO!DBI1~WNOe_)=(+YiT1%Wm#AF5K_ueKU@)
zTgy>*I2tD34=eC}Yn0#5wWeaUHQ^{mhq*RR)L_)dMtS<ARHc46I!of?6f_!Hq%K0a
zh1JkI6y{o4$=br)eojn9>!r~c@NJPyPt4&cC0!C@laz6jF&0brv3_F=$}b!Tg=fM_
ze4idw$`zj-jt8|n%!#Zp-J`LfW<|IHRhtUiaB_)|Q}~7mJISRl_a?L@?l<No>>by(
z+iJ`kw^!5VHSl@QnG<~)EZnPJp7M&qH&i~)C#h`g_P*_wdYjT!Z;6wSj1&i<MPZh=
z$zeic!rYCy$HvaGgb>MEGB|Mjapc0xFb-Qns8iMqj}kgK`rtqg1lfs~-c&JIw$1#d
z21j&rXzUb>Zd!l;6!Z)?dro}Qvc|4-#X8nmCfp#pm6nxOr7PL72B)<B2GN$=%CW7m
zW0;35$zi$?87>SdL}Mq>F0(Cz%nF%GR|c8kM#W&|wu(V7Zc*j;oqgVD`pQ}Pi3OI0
zvM&?vR@dGoIL<r^aGr6x#ps@L!(cPu{3^B-*v3x9T{p5_3oJTEESnR|MaL+ck>@bZ
z#?i>|&Uf=ISCOQI9vRKF=asQqKP+qP8s$*9m6g||2JcS>6uNyzj+T#hNfv#ym>L(I
zRdFYId`q{ch^L9ZSrd{xH5#-yOSfkJWWRzY@+fc3q-diboCPRzl=~(@-LHvC=&w<7
zR5v;w%byjtV5!BWy2~j6E^YuPE&1E2D%{3lc^xM%J9*QQ=*56)Ksz?kcF2lwp!T4>
z4tSpj0#+Gs+B>Dy`bqxxu?Z#(5>J!gbZ@iMNS8OhQRY}_y0^u-ALa47WvoCtq&3DP
z70IPfuXLawJ{^e?IRS%BRvXxGdq+5tUEY5;s*ClYqz(^xx9|5CZD7Uo;fQifcluFi
zo*(gyC3Mf|i}qGqXJDQFvh#|6a+JIe<KB5<q=MFCuN@iTX0+J%Ia`o<w?@7v@2$5+
z*os^jX+-ENjj7MS7CSAc;*Lg-&3;i7w!W6`lW_RlNVLdyb`%tBwV9MS0y+9ygvbi1
z*)wXoS6!tPv;6MmTPwb(<mh&&x#99dV0LQ09nwPa{D|qEtLk>;%Ylbr$_ZIDWT!{w
zh;K`eRysDQu(hf6%q>S(VvKJ~54WQK+KS+_;8;hPjq*{ixYlkRh!#GS^1U2g3@*;#
zlF=2m6?VV0zI9*Kn0lV9K5w*1wBJ`RZs+JsnLB5c=y-9d^(O<IKDPR~4cuOirj3;1
zuJP@WMiGmk;pq4gE8JBfjD^rs2p@0Ys9NSdG!{rmB^KC=tVVH)r;4K$BP^*(L+p0I
zrh2YjE3T{ladjmqlPoI?c}1<5RhA`Ql*$nKoDkQR1Zkcuof$HrJ!eFy$tcUj0{z>F
zfO1Hcm*=l|_uUL-x~5r42DW^5lgF(#GF)<wXeZjzES?ls8j4t<OG6c+bk9wKf#J~D
z-jXYu_!UfC4E_El(Z<@wq3Fj!KT4;O^$X7(8uD)7Eqndum8Wh!*y7uf)bfLt2qxsV
z`6y`IH|48D7upi4;|qW{A9oP-nJBn?56@9TUxBL^L+T9Pp5|8LNq8P5!tR|3cFmde
z25wGtZ@kSPAj0g^$_znFbK}`4XfE&>(Rd^gdO)`E6l^>Z5=8c-WPRD0a|L$XXGp(S
zMTRUJU+ecOSEGP=EukLQ2(H5Pk<bZTP?b<%VXO(AGJny_-nw|!A!A8Kip0g89OA#z
z-oI4A?aj+3U#@Rr&$=@5Fwg$h-Nz2#?~q1(p9Xg=$7DuXV@&VC_Yqn0I9KK&-{mgI
z!~@X-tpBQ;fpEtJk<k62X0r--UK4kWJ3y!}^j%#3%bG4oN=>^zGQ3J++y4B<i<ZRe
z?fo@m?EM-UcFGUToW^d8;yNdUhhH3d&ys+_;DOobIgiQbLg#Jg@y=c);igD~!E-DJ
zg6Hh}Ts;FuTyTc)`XuyY6xRoM`ab9y8P-MomjCJVm;0fAE%+90=Y&RwIzU|k#e*WD
zlc0KoO0C;PXlLj+sE<H(fI1O6Y9Su_Ayng`dN=qwgy)02R$oi#!O&q)XCO;~>}v2;
zP^W@X_f{`e4fSlABN_|x1AH8zmxFBiJi!y@d%Y~kUBUC_>Ed5{e+c~_;wOv9uqyHc
zv|EG9A!Chl$W;B!`zwzeVLdkl+0%qwZaqP4xmEO0LSGL)3;HS0TV(oR@EOosS!j<F
zx+2sJ$_0udP+O=GloM13sFIKe)WhgmT?P3%LKlQ~fU^P|*L_ZI=qYe4=;3}G<DVU>
z2Wbk{<6P_rZ%2C}GYv;mLfb&)2ld}(nL06~&)l+nOSv8yFdHKyR25u=5%ADvV*(v=
zK|fuV4F|Y<CLIXe@ZWZ^{`Z3UbFTMJ?sGsVHuxB}@Z>)3n1>nK7Hmc$&1vNJDWZ)1
z;~F2b^xqmwxoSvQ%7wjSDYrhzQZA{Hdxgw04kR|V`@_TQkY0qo>s@a7{jKFN{#gL-
z;QGB_5vbn<IIRWv?O*}Ezk*Bz?g-}M`?G+;qb)(>Sa2@BJprZG1biqs8{e~_76b1O
z&cxRbsu=hOaw<L>z;a<9ICwwIr2AnWXJP&rmnJ#P_q<%&s;QA|<Cnko&b74QQFIx<
z)U*~6^=7~z;Aj-?phv+Z^T!^+yTN1|h8na6kP65I=mEt53t%<C4cHB623$d(R^WYr
zcEC|U7vMaAL=)i>@MXYtwh=K5IB|c6O!uJk^MFf$KO|a>tEHSC;2vNjA>IffJ)V7#
zjV;H)K(l$#Wz;)K=;yicusTe#*7VvelW*~-=HX|!nCrL{7bK4_xx?$u40$~>C4OPa
zWgN7}2=xZq&By(upHqI(`>nH!E!-_OyPjygE(!e;-j!Ey&l<j2OSlZZ7#{xbV3EDZ
zMzZ3&Q*q0_d$Z8;IiL@4^JWiL{Vdk=`c1Z`w?o|n6))mP68lFke(?H+<dR=F7$@AP
zxBU91-ttSpF91KkiQjm({krdh#CC=l+3)KfI6eT!@80B3<%;jX@YPYSZI8drd{W}|
zlRqQbO9n5UB$?NwR&TfeHUI5NgnogyIibg;LTLgF-NeI~&?bq?hhgC`dfvTq>gGfQ
z)+UX)YLk*d`52}DeV+!isRu|{26fR^+{Hj#M-G$$9|c@N`wIxkpUHJ4397yT?E-&2
zKtlU7;M;&3030frs^wAT*)F}9jw7flq_od<q<h)ff!$EZ#Jw;BP;_Q|89S8E__P8x
zT5x##<QBzQ_cHu^)}1lk_wT|C9_V%99)Gd8^PqMjS9V6;&fC0E;tnhOjo=sfb%0e`
zua{(QtH=`PV-n6r=8Gy(k&g=!v<G`}t%l}XXcPL9mtCM3-74+XO^nA>g_$--J(4d&
zmy!Q2ezhR99&ef~;P*yX+E+<g*zVsQT_LyB{gI-_f-P8~bK$Lb)GGGdh_69+dkx+v
zqa2-$?)&>L+?Mt*uY`VyePGGp+Be6%;=BOu87zKtd0)EtejiI@LUxtzz}P=4M#2Mr
z`SHgecLBcx=qdKSmHD`5*9>eO>xof1lUf!l>Kdxf;&*P{#PVsP!7;(kU*{IibMTj^
zH;|mbWuoImg`mT<7mqy9jI*YSagI1yQw`0G1~NNvS=XST5kBrT5u^($J13;M@uPG%
zP96Ez`Nif$a$+VC@OtrWpT2YP_aa9oAMsnx7L*pBHqN2FxOw3m*^QrS$Z7Y53%A{P
zP(9i0UZ%v3y)8m?2KU`Q)_*5r-#x=Odq$;T1UuXP$387C;+P?H_)$0M{yU|%8Lv-0
z5B;4Pm<GUUzymRh3w9ozxgd(McFpKKICC}^bp-8;Gkm|w`Y8|e!&y6kI>*ia3%7t7
zV$vQqc@AQ{2wV+X14GP0>LQ|^11JU*0Vw9%4SZlxXW^WcTpH77{h97K^kMc!XX4c|
z(|0K6VdgEIV`j)acX9f)k#HX?4VK=ifOZ&SxC5O#&o4ZxDyz6-<o*5Oio{PU=s`v2
z-SHa|KCQSzbS0FOXeg=B1$mm<Z8W==RZv3Q%f|OR6S-b9uj+-=sP5j1?*o~y+GNap
zVspcMePO<SZk3Q#W+*6{wLow?>$cZCw&BT*8*A#aW+4{e5z};qVSbVCW0MwmzK@!w
z@_M~bW1hk5=lhDx<-ESo=PjM0$j{C7wU*4_bLaUknWv`Y>0z$Qxn*X(u)W?{hp$`j
z3!BsVydqzENgi*=^Hr8i<qZbeqM<<UYbXRYMPUGWr6jYec1LyfA_P&ornEE{4Tkf8
zff$=uSDPwVn^r$)uQ4xOM0U9Aoy;F&u68aW|1ik>d;_AgKH=O{BdlC%Dt$=nyJni?
zyKJ7AQt;35=F0IF8063j=KCCMl!b<|(H7>*fzQkJ^^|It<j<Q2!#RtE<&RZXtX;aY
zqWo))iwYsHtC6RId`&igp3k^6N0E#9>{>dTEA(AkI+HW_qQxoY{}8<)pE;3D)IQ;q
zmnR)5mkH1ViUAhDYJdx{8_)`92Z+wv%{A4&axt%JRn3<ASWHfdyf$?;&T2xIZmF-S
z6XtEGtrwit)d+JtA>}nS)q=BL*j(e>UN3CjQX@QByQRM9v3m5CWpy>1YPX}`1G*aX
z&f1IhzH6d(a)q4`(PS-KS_+=9v2N=Vf?lZKD&!Ru`rZ{MMJ=tX+gcavo4Rabr*T;h
zH!eR{Fy`s=@^bVAIl1$FPh0LrWfentmhYTps&9|A*_Ud|FP^zpsMZVBxx(DJb7u-U
z!c%p%^)*7dFs3Tx7Ur-)Jhip1db?0vTUS$6zj-I~YRxN&?JT!F!=<vbgPjzF!+(4d
MBOxE4fn(`^0hI?)<^TWy

diff --git a/F1:F103/MLX90640/Makefile b/F1:F103/MLX90640/Makefile
index 8143eab..07e1d20 100644
--- a/F1:F103/MLX90640/Makefile
+++ b/F1:F103/MLX90640/Makefile
@@ -74,7 +74,7 @@ LDFLAGS		+= -T$(LDSCRIPT)
 
 ###############################################################################
 # Used libraries
-LDLIBS		+=  -lc $(shell $(CC) $(CFLAGS) -print-libgcc-file-name)
+LDLIBS		+= -lm -lc $(shell $(CC) $(CFLAGS) -print-libgcc-file-name)
 
 DEFS		+= -DSTM32$(FAMILY) -DSTM32$(MCU) -DSTM32F10X_$(DENSITY)
 
diff --git a/F1:F103/MLX90640/hardware.c b/F1:F103/MLX90640/hardware.c
index b1ead54..9a39ef7 100644
--- a/F1:F103/MLX90640/hardware.c
+++ b/F1:F103/MLX90640/hardware.c
@@ -28,8 +28,9 @@ static inline void gpio_setup(){
     GPIOC->CRH |= CRH(13, CNF_ODOUTPUT | MODE_SLOW);
     // USB pullup (PA15) - pushpull output
     GPIOA->CRH = CRH(15, CNF_PPOUTPUT | MODE_SLOW);
-    // PB5 is powered MLX sensor (less than 23mA) - pushpull output
-    GPIOB->CRL = CRL(5, CNF_PPOUTPUT | MODE_SLOW);
+    // PB5 is powered MLX sensor (less than 23mA) - OD output
+    MLXPOW_OFF();
+    GPIOB->CRL = CRL(5, CNF_ODOUTPUT | MODE_SLOW);
 }
 
 void hw_setup(){
diff --git a/F1:F103/MLX90640/hardware.h b/F1:F103/MLX90640/hardware.h
index 0a44612..5ee8bd2 100644
--- a/F1:F103/MLX90640/hardware.h
+++ b/F1:F103/MLX90640/hardware.h
@@ -33,9 +33,9 @@
 #define MLXPOW_pin  (1<<5)
 #define USBPU_ON()  pin_set(USBPU_port, USBPU_pin)
 #define USBPU_OFF() pin_clear(USBPU_port, USBPU_pin)
-#define MLXPOW_ON() pin_set(MLXPOW_port, MLXPOW_pin)
-#define MLXPOW_OFF() pin_clear(MLXPOW_port, MLXPOW_pin)
-#define MLXPOW_VAL() pin_read(MLXPOW_port, MLXPOW_pin)
+#define MLXPOW_ON() pin_clear(MLXPOW_port, MLXPOW_pin)
+#define MLXPOW_OFF() pin_set(MLXPOW_port, MLXPOW_pin)
+#define MLXPOW_VAL() ((MLXPOW_port->IDR & MLXPOW_pin)?0:1)
 
 #define LED_blink(x)    pin_toggle(x ## _port, x ## _pin)
 #define LED_on(x)       pin_clear(x ## _port, x ## _pin)
diff --git a/F1:F103/MLX90640/i2c.c b/F1:F103/MLX90640/i2c.c
index 7b8e4e2..fd94621 100644
--- a/F1:F103/MLX90640/i2c.c
+++ b/F1:F103/MLX90640/i2c.c
@@ -49,17 +49,22 @@ static void i2c_DMAr_setup(){
  * @param withDMA == 1 to setup DMA receiver too
  */
 void i2c_setup(uint8_t withDMA){
-    I2C1->CR1 = 0;
-    I2C1->SR1 = 0;
     RCC->APB2ENR |= RCC_APB2ENR_IOPBEN;
     GPIOB->CRL = (GPIOB->CRL & ~(GPIO_CRL_CNF6 | GPIO_CRL_CNF7)) |
             CRL(6, CNF_AFOD | MODE_NORMAL) | CRL(7, CNF_AFOD | MODE_NORMAL);
     RCC->APB1ENR |= RCC_APB1ENR_I2C1EN;
+    I2C1->CR1 = 0; // clear all previous settings
+    I2C1->SR1 = 0;
+    RCC->APB1RSTR |=  RCC_APB1RSTR_I2C1RST; // reset peripherial
+    RCC->APB1RSTR &= ~RCC_APB1RSTR_I2C1RST;
     I2C1->CR2 = 8; // FREQR=8MHz, T=125ns
-    I2C1->TRISE = 9; // (9-1)*125 = 1mks
+    //I2C1->CR2 = 10; // FREQR=10MHz, T=100ns
+    I2C1->TRISE = 9; // (9-1)*125 = 1us
+    //I2C1->TRISE = 4; // (4-1)*100 = 300ns
     I2C1->CCR = 40; // normal mode, 8MHz/2/40 = 100kHz
-    I2C1->CR1 |= I2C_CR1_PE; // enable periph
+    //I2C1->CCR = I2C_CCR_FS | 10; // fast mode, 10MHz/2/10 = 500kHz
     if(withDMA) i2c_DMAr_setup();
+    I2C1->CR1 |= I2C_CR1_PE; // enable periph
 }
 
 void i2c_set_addr7(uint8_t addr){
@@ -162,6 +167,7 @@ i2c_status i2c_7bit_receive_twobytes(uint8_t *data){
     DBG("2 ADDR");
     if(I2C1->SR1 & I2C_SR1_AF){             // NACK
         ret = I2C_NACK;
+        DBG("2 NACK");
         goto eotr;
     }
     DBG("2 ACK");
diff --git a/F1:F103/MLX90640/mlx90640.c b/F1:F103/MLX90640/mlx90640.c
index 6d40ea9..af703c3 100644
--- a/F1:F103/MLX90640/mlx90640.c
+++ b/F1:F103/MLX90640/mlx90640.c
@@ -16,6 +16,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <math.h>
 #include "hardware.h"
 #include "i2c.h"
 #include "mlx90640.h"
@@ -70,14 +71,27 @@ int read_reg(uint16_t reg, uint16_t *val){
 }
 
 // blocking read N uint16_t values starting from `reg`
-// @return N of registers read
-int read_data(uint16_t reg, uint16_t *data, int N){
-    if(N < 1 ) return 0;
-    int i;
-    for(i = 0; i < N; ++i){
-        if(!read_reg(reg+i, data++)) break;
+// @param reg - register to read
+// @param N (io) - amount of bytes to read / bytes read
+// @return `dataarray` or NULL if failed
+uint16_t *read_data(uint16_t reg, uint16_t *N){
+    uint16_t n = *N;
+    if(n < 1 || n > MLX_DMA_MAXLEN) return NULL;
+    uint16_t i, *data = dataarray;
+#ifdef EBUG
+    SEND("Tms="); printu(Tms); newline();
+#endif
+    for(i = 0; i < n; ++i){
+        if(!read_reg(reg++, data++)){
+            DBG("can't read");
+            break;
+        }
     }
-    return i;
+#ifdef EBUG
+    SEND("Tms="); printu(Tms); newline();
+#endif
+    *N = i;
+    return dataarray;
 }
 
 // write register value
@@ -146,7 +160,7 @@ static int get_parameters(){
     SEND("0 Tms="); printu(Tms); newline();
 #endif
     int8_t i8;
-    int16_t i16, *pi16;
+    int16_t i16;
     uint16_t *pu16;
     uint16_t val = CREG_VAL(REG_VDD);
     i8 = (int8_t) (val >> 8);
@@ -169,14 +183,14 @@ static int get_parameters(){
 #ifdef EBUG
     SEND("1 Tms="); printu(Tms); newline();
 #endif
-    int8_t occRow[24];
-    int8_t occColumn[32];
-    occacc(occRow, 24, &CREG_VAL(REG_OCCROW14));
-    occacc(occColumn, 32, &CREG_VAL(REG_OCCCOL14));
-    int8_t accRow[24];
-    int8_t accColumn[32];
-    occacc(accRow, 24, &CREG_VAL(REG_ACCROW14));
-    occacc(accColumn, 32, &CREG_VAL(REG_ACCCOL14));
+    int8_t occRow[MLX_H];
+    int8_t occColumn[MLX_W];
+    occacc(occRow, MLX_H, &CREG_VAL(REG_OCCROW14));
+    occacc(occColumn, MLX_W, &CREG_VAL(REG_OCCCOL14));
+    int8_t accRow[MLX_H];
+    int8_t accColumn[MLX_W];
+    occacc(accRow, MLX_H, &CREG_VAL(REG_ACCROW14));
+    occacc(accColumn, MLX_W, &CREG_VAL(REG_ACCCOL14));
     val = CREG_VAL(REG_APTATOCCS);
     // need to do multiplication instead of bitshift, so:
     float occRemScale = 1<<(val&0x0F),
@@ -205,31 +219,28 @@ static int get_parameters(){
     float accRowScale = 1<<((val & 0x0f00)>>8),
           accColumnScale = 1<<((val & 0x00f0)>>4),
           accRemScale = 1<<(val & 0x0f);
-    pi16 = params.offset;
     pu16 = &CREG_VAL(REG_OFFAK1);
-    float *fp = params.kta;
+    float *kta = params.kta, *offset = params.offset;
 #ifdef EBUG
     SEND("2 Tms="); printu(Tms); newline();
 #endif
-    for(int row = 0; row < 24; ++row){
+    for(int row = 0; row < MLX_H; ++row){
         int idx = (row&1)<<1;
-        for(int col = 0; col < 32; ++col){
+        for(int col = 0; col < MLX_W; ++col){
             // offset
             register uint16_t rv = *pu16++;
             i16 = (rv & 0xFC00) >> 10;
             if(i16 > 0x1F) i16 -= 0x40;
-            register float oft = (float)offavg + occRow[row]*occRowScale + occColumn[col]*occColumnScale + i16*occRemScale;
-            *pi16++ = (int16_t)oft;
+            *offset++ = (float)offavg + (float)occRow[row]*occRowScale + (float)occColumn[col]*occColumnScale + (float)i16*occRemScale;
             // kta
             i16 = (rv & 0xF) >> 1;
             if(i16  > 0x03) i16 -= 0x08;
-            *fp++ = (ktaavg[idx|(col&1)] + i16*mul) / div;
+            *kta++ = (ktaavg[idx|(col&1)] + i16*mul) / div;
             // alpha
             i16 = (rv & 0x3F0) >> 4;
             if(i16 > 0x1F) i16 -= 0x40;
-            oft = (float)a_r + accRow[row]*accRowScale + accColumn[col]*accColumnScale +i16*accRemScale;
+            float oft = (float)a_r + accRow[row]*accRowScale + accColumn[col]*accColumnScale +i16*accRemScale;
             *a++ = oft / diva;
-            //*a++ /= diva;
         }
     }
 #ifdef EBUG
@@ -280,14 +291,14 @@ static int get_parameters(){
     div = 1<<((CREG_VAL(REG_CT34) & 0x0F) + 8); // kstoscale
     val = CREG_VAL(REG_KSTO12);
     i8 = (int8_t)(val & 0xFF);
-    params.ksTo[0] = (float)i8 / div;
+    params.ksTo[0] = 273.15f * i8 / div;
     i8 = (int8_t)(val >> 8);
-    params.ksTo[1] = (float)i8 / div;
+    params.ksTo[1] = 273.15f * i8 / div;
     val = CREG_VAL(REG_KSTO34);
     i8 = (int8_t)(val & 0xFF);
-    params.ksTo[2] = (float)i8 / div;
+    params.ksTo[2] = 273.15f * i8 / div;
     i8 = (int8_t)(val >> 8);
-    params.ksTo[3] = (float)i8 / div;
+    params.ksTo[3] = 273.15f * i8 / div;
     params.CT[0] = 0.f; // 0degr - between ranges 1 and 2
     val = CREG_VAL(REG_CT34);
     mul = ((val & 0x3000)>>12)*10.f; // step
@@ -305,57 +316,82 @@ static int get_parameters(){
     return TRUE;
 }
 
-
-// calculate Vsup, Tamb, gain, off, Vdd, Ta
-static void stage1(){
-    int16_t i16a = (int16_t)IMD_VAL(REG_IVDDPIX);
-    float dvdd = i16a - params.vdd25;
-    dvdd = dvdd / params.kVdd;
-    float vdd = dvdd + 3.3f;
-    SEND("Vd="); float2str(vdd, 2); newline();
-    i16a = (int16_t)IMD_VAL(REG_ITAPTAT);
-    int16_t i16b = (int16_t)IMD_VAL(REG_ITAVBE);
-    float Ta = (float)i16a / (i16a * params.alphaPTAT + i16b); // vptatart
-    Ta *= (float)(1<<18);
-    Ta = (Ta / (1 + params.KvPTAT*dvdd) - params.vPTAT25);
-    Ta = Ta / params.KtPTAT + 25.;
-    SEND("Ta="); float2str(Ta, 2); newline();
-    i16a = (int16_t)IMD_VAL(REG_IGAIN);
-    float Kgain = params.gainEE / (float)i16a;
-    SEND("Kgain="); float2str(Kgain, 2); newline();
-    ;
-    //int idx = (row&1)<<1;
-    //for(int col = 0; col < 32; ++col){
-    //    *fp++ = (ktaavg[idx|(col&1)]
-    // pix_gain = pix*Kgain
-    // pix_os = pix_gain - offset*(1+kta*(Ta-Ta0))*(1+kv*(vdd-vdd0))
-}
-
 /**
  * @brief process_subpage - calculate all parameters from `dataarray` into `mlx_image`
  */
 static void process_subpage(){
     DBG("process_subpage()");
+SEND("Tms="); printu(Tms); newline();
     SEND("subpage="); printu(subpageno); newline();
     (void)subpageno; (void)simpleimage;
-for(int i = 0; i < 32; ++i){
+for(int i = 0; i < MLX_W; ++i){
     printi((int8_t)dataarray[i]); bufputchar(' ');
 } newline();
-    stage1();
-    NL();
+SEND("072a="); printuhex(IMD_VAL(REG_IVDDPIX));
+SEND("\n0720="); printuhex(IMD_VAL(REG_ITAPTAT));
+SEND("\n0700="); printuhex(IMD_VAL(REG_ITAVBE));
+SEND("\n070a="); printuhex(IMD_VAL(REG_IGAIN)); newline();
+    int16_t i16a = (int16_t)IMD_VAL(REG_IVDDPIX);
+    float dvdd = i16a - params.vdd25;
+    dvdd = dvdd / params.kVdd;
+  SEND("Vd="); float2str(dvdd+3.3f, 2); newline();
+    i16a = (int16_t)IMD_VAL(REG_ITAPTAT);
+    int16_t i16b = (int16_t)IMD_VAL(REG_ITAVBE);
+    float dTa = (float)i16a / (i16a * params.alphaPTAT + i16b); // vptatart
+    dTa *= (float)(1<<18);
+    dTa = (dTa / (1 + params.KvPTAT*dvdd) - params.vPTAT25);
+    dTa = dTa / params.KtPTAT; // without 25degr - Ta0
+  SEND("Ta="); float2str(dTa+25., 2); newline();
+    i16a = (int16_t)IMD_VAL(REG_IGAIN);
+    float Kgain = params.gainEE / (float)i16a;
+  SEND("Kgain="); float2str(Kgain, 2); newline();
+    // now make first approximation to image
+    uint16_t pixno = 0;  // current pixel number - for indexing in parameters etc
+    for(int row = 0; row < MLX_H; ++row){
+        int idx = (row&1)<<1; // index for params.kv
+        for(int col = 0; col < MLX_W; ++col, ++pixno){
+            uint8_t sp = (row&1)^(col&1); // subpage of current pixel
+            if(sp != subpageno) continue;
+            register float curval = (float)((int16_t)dataarray[pixno]) * Kgain; // gain compensation
+            curval -= params.offset[pixno] * (1.f + params.kta[pixno]*dTa) *
+                    (1.f + params.kv[idx|(col&1)]*dvdd); // add offset
+            float IRcompens = curval; // IR_compensated
+            curval -= params.cpOffset[subpageno] * (1.f - params.cpKta * dTa) *
+                    (1.f + params.cpKv * dvdd); // CP
+            if(!simpleimage){
+                curval = IRcompens - params.tgc * curval; // IR gradient compens
+                float alphaComp = params.alpha[pixno] - params.tgc * params.cpAlpha[subpageno];
+                alphaComp /= 1.f + params.KsTa * dTa;
+                // calculate To for basic range
+                float Tar = dTa + 273.15f + 25.f;
+                Tar = Tar*Tar*Tar*Tar;
+                float ac3 = alphaComp*alphaComp*alphaComp;
+                float Sx = ac3*IRcompens + alphaComp*ac3*Tar;
+                Sx = params.KsTa * sqrt(sqrt(Sx));
+                float To = IRcompens / (alphaComp * (1.f - params.ksTo[1]) + Sx) + Tar;
+                curval = sqrt(sqrt(To)) - 273.15; // To
+                // TODO: extended
+            }
+            mlx_image[pixno] = curval;
+        }
+    }
+SEND("Tms="); printu(Tms); newline();
+NL();
 }
 
 // start image acquiring for next subpage
 static int startima(){
     DBG("startima()");
+    // write `overwrite` flag twice
     if(!write_reg(REG_CONTROL, reg_control_val[subpageno]) ||
+            !write_reg(REG_STATUS, REG_STATUS_OVWEN) ||
             !write_reg(REG_STATUS, REG_STATUS_OVWEN)) return FALSE;
     return TRUE;
 }
 
 /**
  * @brief parse_buffer - swap bytes in `dataarray` (after receiving or before transmitting data)
- */
+ *
 static void parse_buffer(){
     uint16_t *ptr = dataarray;
     DBG("parse_buffer()");
@@ -371,7 +407,7 @@ static void parse_buffer(){
 #if 0
     sendbuf();
 #endif
-}
+}*/
 
 /**
  * @brief mlx90640_process - main finite-state machine
@@ -382,13 +418,14 @@ void mlx90640_process(){
 #define chktmout() do{if(Tms - Tlast > MLX_TIMEOUT){chstate(M_ERROR); DBG("Timeout! -> M_ERROR"); }}while(0)
     static int errctr = 0;
     static uint32_t Tlast = 0;
+    uint16_t reg, N;
+    /*
     uint8_t gotdata = 0;
-    uint16_t reg;
     if(i2cDMAr == I2C_DMA_READY){ // convert received data into little-endian
         i2cDMAr = I2C_DMA_RELAX;
         parse_buffer();
         gotdata = 1;
-    }
+    }*/
     switch(mlx_state){
         case M_FIRSTSTART: // init working mode by request
             if(write_reg(REG_CONTROL, reg_control_val[0])
@@ -396,14 +433,30 @@ void mlx90640_process(){
                 SEND("REG_CTRL="); printuhex(reg); NL();
                 if(read_reg(REG_STATUS, &reg)){
                     SEND("REG_STATUS="); printuhex(reg); NL();}
-                if(read_data_dma(REG_CALIDATA, REG_CALIDATA_LEN)){
+/*
+#define PARTD  512
+                if(read_data_dma(REG_CALIDATA, PARTD)){
+                    chstate(M_READCONF);
+                    DBG("-> M_READCONF");
+                }else chkerr();
+*/
+                N = REG_CALIDATA_LEN;
+                if(read_data(REG_CALIDATA, &N)){
                     chstate(M_READCONF);
                     DBG("-> M_READCONF");
                 }else chkerr();
             }else chkerr();
         break;
         case M_READCONF:
-            if(gotdata){ // calculate calibration parameters
+            //if(gotdata){ // calculate calibration parameters
+               /* uint16_t *d = &dataarray[PARTD];
+                for(uint16_t r = REG_CALIDATA+PARTD; r < REG_CALIDATA + REG_CALIDATA_LEN; ++r){
+                    if(!read_reg(r, d++)){
+                        chstate(M_FIRSTSTART);
+                        DBG("can't read all confdata -> M_FIRSTSTART");
+                        return;
+                    }
+                }*/
                 if(get_parameters()){
                     chstate(M_RELAX);
                     DBG("-> M_RELAX");
@@ -411,16 +464,15 @@ void mlx90640_process(){
                     chstate(M_FIRSTSTART);
                     DBG("-> M_FIRSTSTART");
                 }
-            }else chktmout();
+            //}else chktmout();
         break;
         case M_STARTIMA:
-            subpageno = 0;
             if(startima()){
                 chstate(M_PROCESS);
                 DBG("-> M_PROCESS");
             }else{
                 chstate(M_ERROR);
-                DBG("can't start sp0 -> M_ERROR");
+                DBG("can't start subpage -> M_ERROR");
             }
         break;
         case M_PROCESS:
@@ -430,7 +482,15 @@ void mlx90640_process(){
                         chstate(M_ERROR);
                         DBG("wrong subpage number -> M_ERROR");
                     }else{ // all OK, run image reading
-                        if(read_data_dma(REG_IMAGEDATA, MLX_PIXARRSZ)){
+                        write_reg(REG_STATUS, 0); // clear rdy bit
+                        /*
+                        if(read_data_dma(REG_IMAGEDATA, PARTD)){
+                            chstate(M_READOUT);
+                            DBG("-> M_READOUT");
+                        }else chkerr();
+                        */
+                        N = MLX_PIXARRSZ;
+                        if(read_data(REG_IMAGEDATA, &N)){
                             chstate(M_READOUT);
                             DBG("-> M_READOUT");
                         }else chkerr();
@@ -439,21 +499,29 @@ void mlx90640_process(){
             }else chkerr();
         break;
         case M_READOUT:
-            if(gotdata){
+            //if(gotdata){
+              /*  uint16_t *d = &dataarray[PARTD];
+                for(uint16_t r = REG_IMAGEDATA+PARTD; r < REG_IMAGEDATA+MLX_PIXARRSZ; ++r){
+                    if(!read_reg(r, d++)){
+                        chstate(M_ERROR);
+                        DBG("can't read all confdata -> M_ERROR");
+                        return;
+                    }
+                }*/
                 process_subpage();
+                subpageno = !subpageno;
+                DBG("Subpage ready");
+                chstate(M_RELAX);
+                /*
                 if(++subpageno > 1){ // image ready
+                    subpageno = 0;
                     chstate(M_RELAX);
                     DBG("Image READY!");
                 }else{
-                    if(startima()){
-                        chstate(M_PROCESS);
-                        DBG("-> M_PROCESS");
-                    }else{
-                        chstate(M_ERROR);
-                        DBG("can't start sp1 -> M_ERROR");
-                    }
-                }
-            }else chktmout();
+                    chstate(M_STARTIMA);
+                    DBG("-> M_STARTIMA");
+                }*/
+            //}else chktmout();
         break;
         case M_POWERON:
             if(Tms - Tlast > MLX_POWON_WAIT){
@@ -494,12 +562,17 @@ void mlx90640_restart(){
 // @param simple ==1 for simplest image processing (without T calibration)
 int mlx90640_take_image(uint8_t simple){
     simpleimage = simple;
-    if(mlx_state != M_RELAX) return FALSE;
+    if(mlx_state == M_ERROR){
+        DBG("Restart I2C");
+        i2c_setup(i2cDMAr != I2C_DMA_NOTINIT);
+    } else if(mlx_state != M_RELAX) return FALSE;
     if(params.kVdd == 0){ // no parameters -> make first run
         mlx_state = M_FIRSTSTART;
         DBG("no params -> M_FIRSTSTART");
         return TRUE;
     }
+    //subpageno = 0;
     mlx_state = M_STARTIMA;
+    DBG("-> M_STARTIMA");
     return TRUE;
 }
diff --git a/F1:F103/MLX90640/mlx90640.h b/F1:F103/MLX90640/mlx90640.h
index 697dc5d..a277f36 100644
--- a/F1:F103/MLX90640/mlx90640.h
+++ b/F1:F103/MLX90640/mlx90640.h
@@ -31,7 +31,9 @@
 #define MLX_POWON_WAIT      2000
 
 // amount of pixels
-#define MLX_PIXNO           (24*32)
+#define MLX_W               (32)
+#define MLX_H               (24)
+#define MLX_PIXNO           (MLX_W*MLX_H)
 // pixels + service data
 #define MLX_PIXARRSZ        (MLX_PIXNO + 64)
 
@@ -48,10 +50,10 @@ typedef struct{
     float cpKta;    // K_Ta_CP
     float KsTa;
     float CT[3]; // range borders (0, 160, 320 degrC?)
-    float ksTo[4]; // K_S_To for each range
+    float ksTo[4]; // K_S_To for each range * 273.15
     float alphacorr[4]; // Alpha_corr for each range
     float alpha[MLX_PIXNO]; // full - with alpha_scale
-    int16_t offset[MLX_PIXNO];
+    float offset[MLX_PIXNO];
     float kta[MLX_PIXNO]; // full K_ta - with scale1&2
     float kv[4];  // full - with scale; 0 - odd row, odd col; 1 - odd row even col; 2 - even row, odd col; 3 - even row, even col
     float cpAlpha[2];   // alpha_CP_subpage 0 and 1
@@ -85,7 +87,7 @@ extern float mlx_image[MLX_PIXNO];
 
 int read_reg(uint16_t reg, uint16_t *val);
 int write_reg(uint16_t reg, uint16_t val);
-int read_data(uint16_t reg, uint16_t *data, int N);
+uint16_t *read_data(uint16_t reg, uint16_t *N);
 int read_data_dma(uint16_t reg, int N);
 void mlx90640_process();
 int mlx90640_take_image(uint8_t simple);
diff --git a/F1:F103/MLX90640/proto.c b/F1:F103/MLX90640/proto.c
index 42c29bd..eb778e3 100644
--- a/F1:F103/MLX90640/proto.c
+++ b/F1:F103/MLX90640/proto.c
@@ -23,8 +23,6 @@
 #include "usb.h"
 #include "version.inc"
 
-#define D16LEN  (256)
-
 extern uint32_t Tms;
 
 static const char* _states[M_STATES_AMOUNT] = {
@@ -51,7 +49,6 @@ static void dumpfarr(float *arr){
 }
 
 static void dumpparams(){
-    int16_t *pi16;
     SEND("\nkVdd="); printi(params.kVdd);
     SEND("\nvdd25="); printi(params.vdd25);
     SEND("\nKvPTAT="); float2str(params.KvPTAT, 4);
@@ -60,10 +57,10 @@ static void dumpparams(){
     SEND("\nalphaPTAT="); float2str(params.alphaPTAT, 2);
     SEND("\ngainEE="); printi(params.gainEE);
     SEND("\nPixel offset parameters:\n");
-    pi16 = params.offset;
+    float *offset = params.offset;
     for(int row = 0; row < 24; ++row){
         for(int col = 0; col < 32; ++col){
-            printi(*pi16++); bufputchar(' ');
+            float2str(*offset++, 2); bufputchar(' ');
         }
         newline();
     }
@@ -94,10 +91,21 @@ static void dumpparams(){
     NL();
 }
 
+static void dumpimage(){
+    float *idata = mlx_image;
+    for(int row = 0; row < MLX_H; ++row){
+        for(int col = 0; col < MLX_W; ++col, ++idata){
+            float2str(*idata,1); bufputchar(' ');
+        }
+        newline();
+    }
+    NL();
+}
+
 const char *parse_cmd(char *buf){
     int32_t Num = 0;
     uint16_t r, d;
-    uint16_t data[D16LEN];
+    uint16_t *data;
     const float pi = 3.1415927f, e = 2.7182818f;
     char *ptr, cmd = *buf++;
     switch(cmd){
@@ -112,7 +120,7 @@ const char *parse_cmd(char *buf){
             if(buf != (ptr = getnum(buf, &Num))){
                 r = Num;
                 if(ptr != getnum(ptr, &Num)){
-                    if(Num < 1) return "N>0";
+                    if(Num < 1 || Num > MLX_DMA_MAXLEN) return "0<N<=832";
                     if(!read_data_dma(r, Num)) return("Can't read");
                     else return "OK";
                 }else return "Need amount";
@@ -143,9 +151,13 @@ const char *parse_cmd(char *buf){
             if(buf != (ptr = getnum(buf, &Num))){
                 r = Num;
                 if(ptr != getnum(ptr, &Num)){
-                    if(Num < 1 || Num > 256) return "N from 0 to 256";
-                    d = read_data(r, data, Num);
-                    if(d < Num){
+                    if(Num < 1 || Num > MLX_DMA_MAXLEN) return "N from 0 to 832";
+                    uint16_t od = d = Num;
+                    if(!(data = read_data(r, &d))){
+                        SEND("Can't read\n");
+                        return NULL;
+                    }
+                    if(d != od){
                         addtobuf("Got only ");
                         printu(d);
                         addtobuf(" values\n");
@@ -190,6 +202,10 @@ const char *parse_cmd(char *buf){
             USB_sendstr("Soft reset\n");
             NVIC_SystemReset();
         break;
+        case 'S':
+            dumpimage();
+            return NULL;
+        break;
         case 'T':
             SEND("Tms="); printu(Tms); NL();
             return NULL;
@@ -201,17 +217,6 @@ const char *parse_cmd(char *buf){
             if(write_reg(r, Num)) return "OK";
             else return "Failed";
         break;
-        case 'W':
-            r = 0;
-            while(r < D16LEN){
-                if(buf == (ptr = getnum(buf, &Num))) break;
-                data[r++] = ((Num & 0xff) << 8) | (Num >> 8);
-                buf = ptr + 1;
-            }
-            if(r == 0) return "Need at least one uint8_t";
-            if(I2C_OK == i2c_7bit_send((uint8_t*)data, r*2, 1)) return "Sent\n";
-            else return "Error\n";
-        break;
         default: // help
             addtobuf(
             "MLX90640 build #" BUILD_NUMBER " @" BUILD_DATE "\n\n"
@@ -219,16 +224,16 @@ const char *parse_cmd(char *buf){
             "'d reg N' - read  registers starting from `reg` using DMA\n"
             "'Ee' - expose image: E - full, e - simple\n"
             "'f' - test float printf (0.00, 3.1, -2.72, -3.142, 2.7183, -INF, NAN)\n"
-            "'g reg N' - read N (<256) registers starting from `reg`\n"
+            "'g reg N' - read N registers starting from `reg`\n"
             "'I' - restart I2C\n"
             "'M' - MLX state\n"
             "'O' - turn On or restart MLX sensor\n"
             "'P' - dump params\n"
             "'r reg' - read `reg`\n"
             "'R' - software reset\n"
+            "'S' - show image\n"
             "'T' - get Tms\n"
             "'w reg dword' - write `dword` to `reg`\n"
-            "'W d0 d1 ...' - write N (<256) 16-bit words directly to I2C\n"
             );
             NL();
             return NULL;
diff --git a/F1:F103/MLX90640/usb.c b/F1:F103/MLX90640/usb.c
index d7e2db2..8a3f90d 100644
--- a/F1:F103/MLX90640/usb.c
+++ b/F1:F103/MLX90640/usb.c
@@ -66,13 +66,15 @@ static int usbwr(const uint8_t *buf, uint16_t l){
     while(--ctra && tx_succesfull == 0){
         IWDG->KR = IWDG_REFRESH;
     }
+    if(tx_succesfull == 0) return 1;
     tx_succesfull = 0;
     EP_Write(3, buf, l);
-    ctra = 1000000;
+  /*  ctra = 1000000;
     while(--ctra && tx_succesfull == 0){
         IWDG->KR = IWDG_REFRESH;
     }
     if(tx_succesfull == 0){usbON = 0; return 1;} // usb is OFF?
+    */
     return 0;
 }
 
diff --git a/F1:F103/MLX90640/version.inc b/F1:F103/MLX90640/version.inc
index 4396fae..0716cf2 100644
--- a/F1:F103/MLX90640/version.inc
+++ b/F1:F103/MLX90640/version.inc
@@ -1,2 +1,2 @@
-#define BUILD_NUMBER "141"
-#define BUILD_DATE "2022-05-19"
+#define BUILD_NUMBER "197"
+#define BUILD_DATE "2022-05-20"