From 0025e9ca909d4e1c8bc63841cd0e61265a328e30 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 13 Aug 2013 07:57:39 +0200 Subject: [PATCH] Hotfix: Copy a current version of the log conversion tools to each log directory --- ROMFS/px4fmu_common/logging/log_converter.zip | Bin 0 -> 10090 bytes ROMFS/px4fmu_common/logging/logconv.m | 586 ------------------ src/modules/sdlog2/sdlog2.c | 9 + 3 files changed, 9 insertions(+), 586 deletions(-) create mode 100644 ROMFS/px4fmu_common/logging/log_converter.zip delete mode 100644 ROMFS/px4fmu_common/logging/logconv.m diff --git a/ROMFS/px4fmu_common/logging/log_converter.zip b/ROMFS/px4fmu_common/logging/log_converter.zip new file mode 100644 index 0000000000000000000000000000000000000000..2641e671068b1ea6867fc6b9fef17ba3889e5262 GIT binary patch literal 10090 zcmaKS1B@ojw(Zw8rfu7{ZQHhuX}hQ0)1J0%ThlhDZS!mU&VOF=-pM`h-t1JWDydpY zB`cM^_FhV|VBlx~000`W!HXbT$i5Zh1rGp7vj6~y06c)JnVq90i=mmjy%U3zx4Ie( z04#n8M&-RFQlKy6sw( zG~pu#ZZL^zi-Eb!LWHX6>euI$RCE!QjQp~R$j&O?*6o#77o+p_ZL7QXci*f!N9y&Y z@0!)D95}<`EyTjeE9&~QL_}j0Gttop;sZ2T=E^BMY%_P$XYvXFemW1bDGf7mZ2Ey{ zhOx^yz0b#YY3XrtfCU-t=;L8`}0fQ z0kH+{Iq$iiOg57utEd3t(=2q!I(nPqVDGFn%lg->oA<$80BZnGHUowU{X5qx3NXKj zOmQ$2B!AsUyrE)yhcRXT z%&j@4j1v0v6G0Tj7vK#&UfN~0EHA^BT|G-$pJgTT(qD8FTS$8BGYkXZ#ul*A$-1~d zLfT!i(8=gTzA$ce`M$_HJ-j}-roVm6A+ZX;t|9mf2=em=5O#3~z$I8#?~(?`WVb+B ze!qzbDDT(2-ZK1U0F_qC7{J||>elns4{501`*j1vVZYlOz5||g0y~e3ODXifo`8DO z2M4`^2Sic69}0zE8y0)9XaQ@1_aQ8+~J-M}htv&+EuNn$}>BA5s=#8F#B z@5{g;JQQA{7*U~Th7)?IAP2m0IyD zL5x06`$&i{gCH*{qW<=xpL@B&$SLcAzC>)e+pH3E!#hT-E!3LAzlwIsjP5OnB`eKD zzk!=d>BqT1VF!k&l}gM{MF&f$6LsZ7fbX#x?S{8l@78e-2&S?@Dh(vjK>sF}-yf?- zY0E?Q)$(-^meu)#dV8z}-BM&eg#CKQ2(oVb3x79HtfNx*5WIbzG>geIQFc)GhE%}? z#SD3mKv&HsQ$%{4a#Yh zTgJNHY<|a8dCidNXlxyVN8rK9ehQgci?4%uc!rLtt+HU_oXd77avzU4W$Q#Eq^V-3 zCzqD9`~9CImM76$ODSX}e5z@gr%j7b%Ce3FI5X`}Hccxo)`<9P47L2T1xV3Fj=N;C z^W(%a74c-!3(6JOCK4#;-+)?P@=Nbyb93`dp+Vf_=;HQYSbw&L$nV3^sKLcSaznr{VKuf(JjKG|_7l2W=r^xwcQ%sOi-erU`}4JGFj zd#iy~xmUaHWl3b~>&xn6omlia^Wzu3_X#vy#1SLpD20$ySUFA==zHUa_n%i274ytZ zY~wvPoNO@2A70xb#Wz{-olpjsyC*St2d^KrtTOC6D_q2(Z%;}B^s2l0{$5521HIA+s!pL@wl2T2`l z60XBS$_40u6m%6E6T&V;O;8c0vvCyM8Z$)aEFL1$iuA|c+v)8qMqzs0jMcxY*{dzZ z*049%tkhK;;_`bMd+6d^;+XgC3oy>apRba|EFIqmXb=3>r=Tq8<7Sq!W!O}{41$xB z_)X2^7qtmBk3iq7>qFS|fZep-W#7!+es7(1)*`5+^HXh@zX~%>9euyrye<~su9%`{ z&BFh(yRoGr1?aY^(aF4RAmiabv>s?Z1!Q{VYC&b4JAL#;mF2RT8Fi+WC6v0t5G@dl z$*sT&pwD4bm7tiSUyw>3DsfAuXZ_*-J(sQ{CGl20j>kf<$sPgW&ytY16HQ~7Vos>} zPclUcRV;a6J*VLP#$oUU0?x?{L2ylG3+#tC(reS?LJ^K{kNBAVi$nWo4p)F;v@dh7 zq=AK>(yvVJb{klDEUO*g&|=+Z>|DE#C@4$wSp;?wvp6~T zmubruoz6>y0T(!8>X{usQ65S(PBfY58gXxi*(*HODYLx|^#*ZJAr0BvVKE}SGY8`2 z$*59M6$9|N*Y)${FT(S*(Fkqy3g2{h6upsjDL6%Hb2$h+o#3a_8 zW5$>oof(=#fE?bNLv`lVmb>|l;B@$iVACEPZ3R|LwRaxn*mAVnXjoY@54Q)Id z#Wf#p&c1%51)tfd){y*1qQY-TK3T|) z@4&5_%hTZ(lWF(}(u@pEsC94mUfy{AXE%fPE1}0pgXpEBSCNAaZ&H`57uVW&Xxqp< zf+IsG(S`{BTAkxMVAtz>Wm53?4!<;}W}xe^^yTyK7Do(Wm{ytw9>;E|JF-g7FYXw2RM;iT8nQy{2{BpP%!8p41Q8|L zJ3Sm6PS%s|l-0hTQ|=>EZ9IGV(?exFUi|@G#VRz9Pcx6nAM2r1ze)_p~mpK zJUELKr9!LGl#ndv!l}VXdm4y(qWB?g^pLOM~xp|cMsBcp}}?wjnH^eiEh=z zJ?h+Nw|mv@b(~8p;Qb@F)1X-v)t)FCRjqPj(2fxSMrfCiDb987R)bBS3O9QlYA1`$ zACJs0@Ca!iFx-7?P&axdl((zMITHY~Eeq%w^BR;|oPkF9rM@5C%BU%^s8E3~8+z4; zoJTIq24e-C%#48(MM8ZgkJbEshPAvuwg*)?Z|K^3bS=Ab8jsjK4;SgV;cBjap01lM z!@G+rY{xdf*lqeYI&@{YW=$9|XWhs?gBT(^o`*n!<$Qw(TCAIlm-vEG`oH>xBM45k z7l*@v8WRA}w*UZO{_2;%>c!O2!Gpp6U($st3rYokV9ORQlI!BUG4* z$e7Qn0!c)_jT!e`4Lx?!FF6fcEH-iJ4W*(uBtaM7uT!VK&)&*2s6RzgQ3?lmh+M`b za~D@<6@SBrjhmcBTSuUM7EHI7+;p&U8HCv_Vk496ehM2#&!l>}1~b9PI!Sk8u3OTi z^9MxJ=BbC(>rl4odTsB|(L=%rOH9on7o$ZvB)7I&$(`jETyQ2C&zoEaL4Q`uX2XCW zXL6v6B4OOn%gx}_;foW?RorV@GDm_Xw;2JCgi$-<@pC+r2{=4>@Ew}Rz@*eGmi!x>#S#qx-Nc$fH#R>8h$hQ1i1u=Xb z`I7IFU(;-$E0vohz4Ys3vI}5ieG{@5<=-I8N(~&1kPgXUY|XisY4nsqv`>-ldFnKX zjF~`WkisNN0PCCxD;X~^(s7G!xyJ!YXFv33N#zpzG~3^tqnbktdPTdA;N~f|bbGr>e_Ld?7Y3i!D8%B3 z1P9|*#P+^ogbCS5C;T{isSs?hc%$|H$+Qz`x?y-wB)(8PT&yM^loSY-wX>~ zlz3V$nAE%FSB80dAaY7s2JY6H1-C|?^jCo=t3-R2kZ;(cL5Y?RO4KI;a>=fEyE1hV zxGN|4PXOxyJbWBDZ#cLKm61K_t-RFctY|1ZOe>j!#ECc6*{Y0@tI>RH2Pmlc?n$bp z*DD{9NJm1c>JL#h`nZc`4@kbQx?gY~Af*?q;nD1?*QubsFy6ufdJl6&2KrZ+pMAWI z82Og3YjZH4OWuJK68JS~&D^%soNLOp(Hgb+5HyKVQJ|5yEOc&0r!5cirM}-TM(v2S zgYSc|m-!U~$kY!Quh?gILMwIaN?4gl5zCCQv^I$DUywvQeP?XF}T| zs`RxC46_U*UCkA(bba`UH@~lrT2aERs6{%J3zZqw^tsx%sP9h0%fK0RTyVG*?2#Z9 z8NgmFsD~;YU1(C)dVKbY39|7jF#(R!* zVEqck;f3y$ zPq#$+%GYuw+;SDZ?kn9fm_8$5cfD$-b(*&=+TyWV#;wK)BMS%zh~V`3K@)PM52FNT zei7(6`C3dJQ?OrsZ$aSt?(@aH@%d%>xq0i#dA+ z2P|L7hL?QHq0hY9$unIyGd~m1vcHe;b5npcQaH-#VbSKCYnRR51++$8J)`zk`u>27 zSE;y**q1=pEc@($!6Y&3_wUMzw0+*pu-2u;tB&?xgv!({;U8Q-S&YKCKX$Z=1V?z5 zoVvLdBh}mDB2ECc-y?WyIT~^fX?QVu5#|ogM}Y|VDHHZE5RVknp9?>!p7Csp#4+Zr ztu2ZSOR_T)*mE>Rr&n!zE(t76`l3|HzS|D`m|Tqy5sa3h)`(&=mCGE5)+BABEgDFK z6jXu6G|Gu+J`vFjgyg{)0LuAcJYf(~+Ycy)$MxIgT$GV~{aHru{PsC-p4hihA``kg za``hg!If?StdxNHnYH~A0b39jsXwgmaet5{;;X?}BcAiMTIJ(CWZ#E^$zK_L&>(5w zXM?_HpsbNEcKJO<8JmQ@9^lm}j}cPgYVdSJzUIN)R4Jt-NUqXH^L zpF0oyYtk^YF;gU__$me0og+0m=rRkJ30WwMDv6@68IFMrnhQ0BPKruNui~*M2>vzs z%83aZVW#}3ESlg8vMgB9os_#7InrmPuHHZLs(%-6Jax)d4(bs&nm?el&C0 zYB?<9!QK`UrGou@j@}j0zw1%rz1vK`$x5zDeu8~^eio4!oTlEkJ-?jY7}?fp9D%<2 zAt}Vq{TT(?YiYo73*GbGE6&#zgN{A!g*ND#Hk_Prv#2o0hG1!q`MU^ev+jO+&tBy1 zHU=)E2@UhFSZUNw%hQu!bwn$oNBEuMzP5t;i3S%4mXnZ6LtUgknXVleaPjN`AnLm$ z_XxIAH4WGeY@R9SGQYT1rn&vMU<6PcD%$4Rf?jiT69^9cp{2wMkjD^TTq?caRDB|O zbgXCQ_hgXc5_JISrxy%qyhpxd&8>i>0_PbqhBVnem~ADP~6% zu3qRZNfBF^4qgx+dFWsqb@`G_{ex0pl#*Ivj;aO^;DDP)J)fMUd}-GHO@p(C<}BBk zw@@IwEoA~kn5?bA*4`NM5Wzs# zTzhNS{Nm?59WphXr7Nu8K4&Yv=icCe^Jm9q_M_YGJi{9 zKsC%Kq>Vq?{rB%1QIqa$;DAZDu#0fN#ln|bf7}M%m70;9mylm?=!obzW_~OL+DH`% z^OEjUev+91$y;wo*g%x4o@1Cgk!$LKZS@w}bFG1|uUv1K$glv_k`}`XmZ%CC2YMl_SMymA&)bwIvarb!^z3 z`Kwt7B@c5bpZYn=Fk&t@es1zVj>&;(vrxn(q*@5cnv&KZ+pt2NkT?alfJXDtW?vKjxkrR(SD8e$(X~Iwf0X2Qo+Be_#DJM?#Q*I4(e7 zFkm}zw^G7=xA2Q^YkF*Do%_XtmvZ~+=)n2So`Mpo)fHGLKOI3h>NTGcvv9#(F`J~C zBz$}oyfY2{dxM3eZMJMAox_Y{6lmxu+DfMvPczfQxFZbpe0{uVdz%O5%5+opZ7O@g zgl;PemN#$37#Wk~9y><^LRZ_wqG5cJHl2Grr6qGHBihRPDdNpRc6^#rod1cJJ*}h3 z+sqXK|1B}wLG-(pw28t!?`1-myQ|44XP1} z#@6|PeUeg8ie_aJ*1OQAp*P!29JZOyJnP(xb0aHq=E~c;gkwE5%Exjy>)7x!-b@|^ zBt4(4#En>98?p4@=n!d(7X@8U2A3WM9!@#c$0SY1u&S3odhC;(%_k|6XfY)p=b=>g z?|DeAZM~Bpf(;#su?~jO?r*vhtyGPf$f`mp!9!uDkXNuX!f$4;!bpCC7?A6FeJtZ2 z%pteXUp>^?X05Qvo6IpJW}>bCj6JhfB^Y=x5)Z}#U-z9aPl7+y*=B?o6mNrt#~Asz zZ|KB$Z|J4($d@QFHwc~xo#%@SmQd}Ler?GYh2I-Y=|;NypCP+%?HVg+AZO+_=GCU7 zR!3Ys3X5$Et$C!c#37h`qRVTG?Yf1&nRJF`n~hE$-{Mb;K)^u$0n(Aeij<|8zlrQ= zJf6nt2cl52b6~t$b!LB2YrI4hr1c%$PF&8wClq@9Gg=>?yw zMceeRet#3wlPBg6fBD{RD(o>xfXGkA<7FA;QpX!Pg+_zkI0y+dR}P57OAO(}pV07+ z^4l4)-(;2{Y351o8b9-`DGaPIvLr7C$>k_^IB?s798BFqR3-?MHneEuh+yKBB$*t| z>q!0Hqt+Izq}sQ!HJL17BV{}(FBmqjUZ%9n5jHtQUQckUm7-aXAZKnHELT&HqIpFT zxyn5|VoOzfgBH%x&gbs5i)6qJ!QJE#1Hx+dJ}V`L>Q{RsjZ%9xp7S-kDu9(Xri1+n zD@d%`WS$VDtH2jXXdFMkVd%#oe-AxE*Nyim;-HRVc5ffP-x?_Pa2mTv6o0Y{28NPYKeEyWlrL@qe6MPKS0M^0yo-vZ{#}YpqM!D;Q z;qBp@t;u=9_BE%p{#8Ca(l0CU;(PNX6on#e4*~88%Tr5Tfm)O4zhgf(Z(h`3^c=98 z|5gn{58-q53*c(G%?~2b+hq!hqh5?+0l*7JQrf!7p?EEY_b1SvDv5y1L0Iz?8y2ruk7gE{@~=#elW z*Jf8I3+To5+Nl%{T#e%wxVQVvXJ3oJ&MW!*FOEiOVXqiWNt7X@_4g2h4{AMx2#KNP zgsMh&wbCQAVjNDziDO64!WGnq1iWb5-cf+Y3tbC($X&r)q3LHnwntFBkf?Cr+X3D|_qavXT^191{}l z+&6Tqh+nr_s;F;5?imwYn=|3asC*Mtx}Cuhs-e}VW_>fIP%1pCvabD85reeKI|GXM zx^T2pM8@l4R~?Ly(5*hW7BwJD`P#lMT^H#Thlap+wx_V;^M1bdom$BxARH!XI4u_R zjs9eSyKbMA;6Ux>0y%2n?V<}wU-{dw-?h#C93x$&ht~G5Md`E=tcqvZhavhSBw#i& z=XcH!-9QD~ppEoO_nWrj**MDBwd!aJspN5~qEN@00)SA-itP@TOJMg<*g@c+r5vhP zS?oA%qz$KK^eYRe$(1vqhE!#<|RG%$z5-xXpSfcCisWW zy`*Ccll5Fe&e0L)h!Cih48L_|sJwI}lssNa22BHekg33ID_SH$ok}8dRHKBKn7hSm zIAr(|fLduM^+2&91}jtDu*OX1GuuIz)g!uzj?gn@n@s3oXh<`byz^HAN~gTi+P-OH zTbUlh0a(-XL(`B*oZr|OirrHaoNXYDR>q_OWzJi4E1th-O}D^Ai|ZI_?YkQzK}y66 zYQ$mr&W)!tdnGHkZ@Z=$P$j<|DCUm1swXX*XvG!9jwu-5(hPhF@mID;S%ya;zam49 z#qU2l7k<8GS7zPaA?+<_i}%U&Y6r(p0OWX?1q#0k=FWGYATjU+?C8=-wYvI$`j)<} z+wDyW&4G?5c6Dk&R!Q{<&KwyOt`50m)QKMfPWeMLx*= zA~T1n_bd=lk(M>=0f6u@_`1cK7@Z*IsB{l&HZow8L^$fXgLCvIDqbK|KRwq#UswvA zgKp0y5{KeONKu`>A110Tq8X#^?iGueuccU*4W(T|AJo=-=Wq*5u`&axvw{31aHt7KRJh14QYYYd zmr-OH;}wc?S5ZY*4#+Y`L6Vcfh=#}x6&%qw2Wu1(kAsCu0jj$27)r%kU!BqL@3)si z-H$E9cnajt*tV~R^rVD$bB`do-NYtc!yiqWrEmu4;O8?o0O7d4bEDmKlTBF0h#8HL z7Oo3H_pjb^HG>B)cv*1g?IYW5?sp1ZQ)vnr^MCaGX2lwCEOJyduN63QxL??6k0woW zOeMq3MPZv?qocsvBj#qzH4{CN6`;4gO2Q1~Hupb3z1*ffGl>06(<`{|#BO;L9rN)V zc<6J^Y#}a8az<Tb#M;9v@9_lzj$FbiE$ICDR%!WyAPif00$ZesseDHQ}qU=mp|b_nl>h{aZWd1?>Q*aTCh66Hnj$TuYLYqvW}|-yqCLf34W|%|H^st&*8guL&zDGrEoYw(xqpby=cz% z>^MHDEUO3h#Qk35$QG}o)Ae_u@^fKK!I60kM3laZ;l;G~F{~|Kx8fCL#z&8gw}i|; zF2X@EA^Ak^1beqeh2-oP2X<@kX*6#;joPHKX`o93Lc z+49RBYc@Qb?gS_s%UnD0X~>vIaQ}rVy1nMzcTM(*N$d9h1^9op_CMY5q6M_D8@GS$ zKPq4V0PNpdyNbB5n7lZHo0r?aitS^X|6iYwx{9vj8WWoTOzrbe$^;^}&71_UysEQ= zygcDfYsLlXJ!IK9{67jrU;dN|(H&Hn+tZ%B+SRXcBiq|`yj@1FUVZ_e-fIK~-#9ds z-DZ)0rYIeOQvOPoFipy+V9{}R%BjNSmOAF+aG%<50{bI#CaX17jBDA0W+hI|N?t-_ zqEf%pEL{xQnlF!%c86|<@ zi8L%JZC){%iU$gQZ^RIRrBK&6gfLB8+ZU?lj@@Ml)IA3g&WTAJ&RL4dvVjUdSp7y^GCibJDgk!R@wRpQSAp0zF$ni z=4eHF5|eVQ~raL9Xle2=2Q7?O=T@r2#)Er9hrV&o5`vu6XJ3o6y!;~7+mME znqd`D{zDoNP(rZ($zq1}*Lns3{ACfKDCnQne>a>#{2#;Fe>0o?JLZ43ef}M@@2{Ed y-K{vUV{kpDae>~GxtosB8}+5H#l;vhu; literal 0 HcmV?d00001 diff --git a/ROMFS/px4fmu_common/logging/logconv.m b/ROMFS/px4fmu_common/logging/logconv.m deleted file mode 100644 index 3750ddae2a..0000000000 --- a/ROMFS/px4fmu_common/logging/logconv.m +++ /dev/null @@ -1,586 +0,0 @@ -% This Matlab Script can be used to import the binary logged values of the -% PX4FMU into data that can be plotted and analyzed. - -%% ************************************************************************ -% PX4LOG_PLOTSCRIPT: Main function -% ************************************************************************ -function PX4Log_Plotscript - -% Clear everything -clc -clear all -close all - -% ************************************************************************ -% SETTINGS -% ************************************************************************ - -% Set the path to your sysvector.bin file here -filePath = 'sysvector.bin'; - -% Set the minimum and maximum times to plot here [in seconds] -mintime=0; %The minimum time/timestamp to display, as set by the user [0 for first element / start] -maxtime=0; %The maximum time/timestamp to display, as set by the user [0 for last element / end] - -%Determine which data to plot. Not completely implemented yet. -bDisplayGPS=true; - -%conversion factors -fconv_gpsalt=1E-3; %[mm] to [m] -fconv_gpslatlong=1E-7; %[gps_raw_position_unit] to [deg] -fconv_timestamp=1E-6; % [microseconds] to [seconds] - -% ************************************************************************ -% Import the PX4 logs -% ************************************************************************ -ImportPX4LogData(); - -%Translate min and max plot times to indices -time=double(sysvector.timestamp) .*fconv_timestamp; -mintime_log=time(1); %The minimum time/timestamp found in the log -maxtime_log=time(end); %The maximum time/timestamp found in the log -CurTime=mintime_log; %The current time at which to draw the aircraft position - -[imintime,imaxtime]=FindMinMaxTimeIndices(); - -% ************************************************************************ -% PLOT & GUI SETUP -% ************************************************************************ -NrFigures=5; -NrAxes=10; -h.figures(1:NrFigures)=0.0; % Temporary initialization of figure handle array - these are numbered consecutively -h.axes(1:NrAxes)=0.0; % Temporary initialization of axes handle array - these are numbered consecutively -h.pathpoints=[]; % Temporary initiliazation of path points - -% Setup the GUI to control the plots -InitControlGUI(); -% Setup the plotting-GUI (figures, axes) itself. -InitPlotGUI(); - -% ************************************************************************ -% DRAW EVERYTHING -% ************************************************************************ -DrawRawData(); -DrawCurrentAircraftState(); - -%% ************************************************************************ -% *** END OF MAIN SCRIPT *** -% NESTED FUNCTION DEFINTIONS FROM HERE ON -% ************************************************************************ - - -%% ************************************************************************ -% IMPORTPX4LOGDATA (nested function) -% ************************************************************************ -% Attention: This is the import routine for firmware from ca. 03/2013. -% Other firmware versions might require different import -% routines. - -function ImportPX4LogData() - % Work around a Matlab bug (not related to PX4) - % where timestamps from 1.1.1970 do not allow to - % read the file's size - if ismac - system('touch -t 201212121212.12 sysvector.bin'); - end - - % ************************************************************************ - % RETRIEVE SYSTEM VECTOR - % ************************************************************************* - % //All measurements in NED frame - % - % uint64_t timestamp; //[us] - % float gyro[3]; //[rad/s] - % float accel[3]; //[m/s^2] - % float mag[3]; //[gauss] - % float baro; //pressure [millibar] - % float baro_alt; //altitude above MSL [meter] - % float baro_temp; //[degree celcius] - % float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1] - % float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512) - % float vbat; //battery voltage in [volt] - % float bat_current - current drawn from battery at this time instant - % float bat_discharged - discharged energy in mAh - % float adc[4]; //remaining auxiliary ADC ports [volt] - % float local_position[3]; //tangent plane mapping into x,y,z [m] - % int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] - % float attitude[3]; //pitch, roll, yaw [rad] - % float rotMatrix[9]; //unitvectors - % float actuator_control[4]; //unitvector - % float optical_flow[4]; //roll, pitch, yaw [-1..1], thrust [0..1] - % float diff_pressure; - pressure difference in millibar - % float ind_airspeed; - % float true_airspeed; - - % Definition of the logged values - logFormat{1} = struct('name', 'timestamp', 'bytes', 8, 'array', 1, 'precision', 'uint64', 'machineformat', 'ieee-le.l64'); - logFormat{2} = struct('name', 'gyro', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{3} = struct('name', 'accel', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{4} = struct('name', 'mag', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{5} = struct('name', 'baro', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{6} = struct('name', 'baro_alt', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{7} = struct('name', 'baro_temp', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{8} = struct('name', 'control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{11} = struct('name', 'bat_current', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{12} = struct('name', 'bat_discharged', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{14} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{15} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le'); - logFormat{16} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{17} = struct('name', 'rot_matrix', 'bytes', 4, 'array', 9, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{18} = struct('name', 'vicon_position', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{19} = struct('name', 'actuator_control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{20} = struct('name', 'optical_flow', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{21} = struct('name', 'diff_pressure', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{22} = struct('name', 'ind_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{23} = struct('name', 'true_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - - % First get length of one line - columns = length(logFormat); - lineLength = 0; - - for i=1:columns - lineLength = lineLength + logFormat{i}.bytes * logFormat{i}.array; - end - - - if exist(filePath, 'file') - - fileInfo = dir(filePath); - fileSize = fileInfo.bytes; - - elements = int64(fileSize./(lineLength)); - - fid = fopen(filePath, 'r'); - offset = 0; - for i=1:columns - % using fread with a skip speeds up the import drastically, do not - % import the values one after the other - sysvector.(genvarname(logFormat{i}.name)) = transpose(fread(... - fid, ... - [logFormat{i}.array, elements], [num2str(logFormat{i}.array),'*',logFormat{i}.precision,'=>',logFormat{i}.precision], ... - lineLength - logFormat{i}.bytes*logFormat{i}.array, ... - logFormat{i}.machineformat) ... - ); - offset = offset + logFormat{i}.bytes*logFormat{i}.array; - fseek(fid, offset,'bof'); - end - - % shot the flight time - time_us = sysvector.timestamp(end) - sysvector.timestamp(1); - time_s = time_us*1e-6; - time_m = time_s/60; - - % close the logfile - fclose(fid); - - disp(['end log2matlab conversion' char(10)]); - else - disp(['file: ' filePath ' does not exist' char(10)]); - end -end - -%% ************************************************************************ -% INITCONTROLGUI (nested function) -% ************************************************************************ -%Setup central control GUI components to control current time where data is shown -function InitControlGUI() - %********************************************************************** - % GUI size definitions - %********************************************************************** - dxy=5; %margins - %Panel: Plotctrl - dlabels=120; - dsliders=200; - dedits=80; - hslider=20; - - hpanel1=40; %panel1 - hpanel2=220;%panel2 - hpanel3=3*hslider+4*dxy+3*dxy;%panel3. - - width=dlabels+dsliders+dedits+4*dxy+2*dxy; %figure width - height=hpanel1+hpanel2+hpanel3+4*dxy; %figure height - - %********************************************************************** - % Create GUI - %********************************************************************** - h.figures(1)=figure('Units','pixels','position',[200 200 width height],'Name','Control GUI'); - h.guistatepanel=uipanel('Title','Current GUI state','Units','pixels','Position',[dxy dxy width-2*dxy hpanel1],'parent',h.figures(1)); - h.aircraftstatepanel=uipanel('Title','Current aircraft state','Units','pixels','Position',[dxy hpanel1+2*dxy width-2*dxy hpanel2],'parent',h.figures(1)); - h.plotctrlpanel=uipanel('Title','Plot Control','Units','pixels','Position',[dxy hpanel1+hpanel2+3*dxy width-2*dxy hpanel3],'parent',h.figures(1)); - - %%Control GUI-elements - %Slider: Current time - h.labels.CurTime=uicontrol(gcf,'style','text','Position',[dxy dxy dlabels hslider],'String','Current time t[s]:','parent',h.plotctrlpanel,'HorizontalAlignment','left'); - h.sliders.CurTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels dxy dsliders hslider],... - 'min',mintime,'max',maxtime,'value',mintime,'callback',@curtime_callback,'parent',h.plotctrlpanel); - temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min'); - set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]); - h.edits.CurTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders dxy dedits hslider],'String',get(h.sliders.CurTime,'value'),... - 'BackgroundColor','white','callback',@curtime_callback,'parent',h.plotctrlpanel); - - %Slider: MaxTime - h.labels.MaxTime=uicontrol(gcf,'style','text','position',[dxy 2*dxy+hslider dlabels hslider],'String','Max. time t[s] to display:','parent',h.plotctrlpanel,'HorizontalAlignment','left'); - h.sliders.MaxTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 2*dxy+hslider dsliders hslider],... - 'min',mintime_log,'max',maxtime_log,'value',maxtime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel); - h.edits.MaxTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 2*dxy+hslider dedits hslider],'String',get(h.sliders.MaxTime,'value'),... - 'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel); - - %Slider: MinTime - h.labels.MinTime=uicontrol(gcf,'style','text','position',[dxy 3*dxy+2*hslider dlabels hslider],'String','Min. time t[s] to dispay :','parent',h.plotctrlpanel,'HorizontalAlignment','left'); - h.sliders.MinTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 3*dxy+2*hslider dsliders hslider],... - 'min',mintime_log,'max',maxtime_log,'value',mintime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel); - h.edits.MinTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 3*dxy+2*hslider dedits hslider],'String',get(h.sliders.MinTime,'value'),... - 'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel); - - %%Current data/state GUI-elements (Multiline-edit-box) - h.edits.AircraftState=uicontrol(gcf,'style','edit','Units','normalized','position',[.02 .02 0.96 0.96],'Min',1,'Max',10,'String','This shows the current aircraft state',... - 'HorizontalAlignment','left','parent',h.aircraftstatepanel); - - h.labels.GUIState=uicontrol(gcf,'style','text','Units','pixels','position',[dxy dxy width-4*dxy hslider],'String','Current state of this GUI',... - 'HorizontalAlignment','left','parent',h.guistatepanel); - -end - -%% ************************************************************************ -% INITPLOTGUI (nested function) -% ************************************************************************ -function InitPlotGUI() - - % Setup handles to lines and text - h.markertext=[]; - templinehandle=0.0;%line([0 1],[0 5]); % Just a temporary handle to init array - h.markerline(1:NrAxes)=templinehandle; % the actual handle-array to the lines - these are numbered consecutively - h.markerline(1:NrAxes)=0.0; - - % Setup all other figures and axes for plotting - % PLOT WINDOW 1: GPS POSITION - h.figures(2)=figure('units','normalized','Toolbar','figure', 'Name', 'GPS Position'); - h.axes(1)=axes(); - set(h.axes(1),'Parent',h.figures(2)); - - % PLOT WINDOW 2: IMU, baro altitude - h.figures(3)=figure('Name', 'IMU / Baro Altitude'); - h.axes(2)=subplot(4,1,1); - h.axes(3)=subplot(4,1,2); - h.axes(4)=subplot(4,1,3); - h.axes(5)=subplot(4,1,4); - set(h.axes(2:5),'Parent',h.figures(3)); - - % PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,... - h.figures(4)=figure('Name', 'Attitude Estimate / Actuators / Airspeeds'); - h.axes(6)=subplot(4,1,1); - h.axes(7)=subplot(4,1,2); - h.axes(8)=subplot(4,1,3); - h.axes(9)=subplot(4,1,4); - set(h.axes(6:9),'Parent',h.figures(4)); - - % PLOT WINDOW 4: LOG STATS - h.figures(5) = figure('Name', 'Log Statistics'); - h.axes(10)=subplot(1,1,1); - set(h.axes(10:10),'Parent',h.figures(5)); - -end - -%% ************************************************************************ -% DRAWRAWDATA (nested function) -% ************************************************************************ -%Draws the raw data from the sysvector, but does not add any -%marker-lines or so -function DrawRawData() - % ************************************************************************ - % PLOT WINDOW 1: GPS POSITION & GUI - % ************************************************************************ - figure(h.figures(2)); - % Only plot GPS data if available - if (sum(double(sysvector.gps_raw_position(imintime:imaxtime,1)))>0) && (bDisplayGPS) - %Draw data - plot3(h.axes(1),double(sysvector.gps_raw_position(imintime:imaxtime,1))*fconv_gpslatlong, ... - double(sysvector.gps_raw_position(imintime:imaxtime,2))*fconv_gpslatlong, ... - double(sysvector.gps_raw_position(imintime:imaxtime,3))*fconv_gpsalt,'r.'); - title(h.axes(1),'GPS Position Data(if available)'); - xlabel(h.axes(1),'Latitude [deg]'); - ylabel(h.axes(1),'Longitude [deg]'); - zlabel(h.axes(1),'Altitude above MSL [m]'); - grid on - - %Reset path - h.pathpoints=0; - end - - % ************************************************************************ - % PLOT WINDOW 2: IMU, baro altitude - % ************************************************************************ - figure(h.figures(3)); - plot(h.axes(2),time(imintime:imaxtime),sysvector.mag(imintime:imaxtime,:)); - title(h.axes(2),'Magnetometers [Gauss]'); - legend(h.axes(2),'x','y','z'); - plot(h.axes(3),time(imintime:imaxtime),sysvector.accel(imintime:imaxtime,:)); - title(h.axes(3),'Accelerometers [m/s²]'); - legend(h.axes(3),'x','y','z'); - plot(h.axes(4),time(imintime:imaxtime),sysvector.gyro(imintime:imaxtime,:)); - title(h.axes(4),'Gyroscopes [rad/s]'); - legend(h.axes(4),'x','y','z'); - plot(h.axes(5),time(imintime:imaxtime),sysvector.baro_alt(imintime:imaxtime),'color','blue'); - if(bDisplayGPS) - hold on; - plot(h.axes(5),time(imintime:imaxtime),double(sysvector.gps_raw_position(imintime:imaxtime,3)).*fconv_gpsalt,'color','red'); - hold off - legend('Barometric Altitude [m]','GPS Altitude [m]'); - else - legend('Barometric Altitude [m]'); - end - title(h.axes(5),'Altitude above MSL [m]'); - - % ************************************************************************ - % PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,... - % ************************************************************************ - figure(h.figures(4)); - %Attitude Estimate - plot(h.axes(6),time(imintime:imaxtime), sysvector.attitude(imintime:imaxtime,:).*180./3.14159); - title(h.axes(6),'Estimated attitude [deg]'); - legend(h.axes(6),'roll','pitch','yaw'); - %Actuator Controls - plot(h.axes(7),time(imintime:imaxtime), sysvector.actuator_control(imintime:imaxtime,:)); - title(h.axes(7),'Actuator control [-]'); - legend(h.axes(7),'0','1','2','3'); - %Actuator Controls - plot(h.axes(8),time(imintime:imaxtime), sysvector.actuators(imintime:imaxtime,1:8)); - title(h.axes(8),'Actuator PWM (raw-)outputs [µs]'); - legend(h.axes(8),'CH1','CH2','CH3','CH4','CH5','CH6','CH7','CH8'); - set(h.axes(8), 'YLim',[800 2200]); - %Airspeeds - plot(h.axes(9),time(imintime:imaxtime), sysvector.ind_airspeed(imintime:imaxtime)); - hold on - plot(h.axes(9),time(imintime:imaxtime), sysvector.true_airspeed(imintime:imaxtime)); - hold off - %add GPS total airspeed here - title(h.axes(9),'Airspeed [m/s]'); - legend(h.axes(9),'Indicated Airspeed (IAS)','True Airspeed (TAS)','GPS Airspeed'); - %calculate time differences and plot them - intervals = zeros(0,imaxtime - imintime); - for k = imintime+1:imaxtime - intervals(k) = time(k) - time(k-1); - end - plot(h.axes(10), time(imintime:imaxtime), intervals); - - %Set same timescale for all plots - for i=2:NrAxes - set(h.axes(i),'XLim',[mintime maxtime]); - end - - set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]); -end - -%% ************************************************************************ -% DRAWCURRENTAIRCRAFTSTATE(nested function) -% ************************************************************************ -function DrawCurrentAircraftState() - %find current data index - i=find(time>=CurTime,1,'first'); - - %********************************************************************** - % Current aircraft state label update - %********************************************************************** - acstate{1,:}=[sprintf('%s \t\t','GPS Pos:'),'[lat=',num2str(double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong),'°, ',... - 'lon=',num2str(double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong),'°, ',... - 'alt=',num2str(double(sysvector.gps_raw_position(i,3))*fconv_gpsalt),'m]']; - acstate{2,:}=[sprintf('%s \t\t','Mags[gauss]'),'[x=',num2str(sysvector.mag(i,1)),... - ', y=',num2str(sysvector.mag(i,2)),... - ', z=',num2str(sysvector.mag(i,3)),']']; - acstate{3,:}=[sprintf('%s \t\t','Accels[m/s²]'),'[x=',num2str(sysvector.accel(i,1)),... - ', y=',num2str(sysvector.accel(i,2)),... - ', z=',num2str(sysvector.accel(i,3)),']']; - acstate{4,:}=[sprintf('%s \t\t','Gyros[rad/s]'),'[x=',num2str(sysvector.gyro(i,1)),... - ', y=',num2str(sysvector.gyro(i,2)),... - ', z=',num2str(sysvector.gyro(i,3)),']']; - acstate{5,:}=[sprintf('%s \t\t','Altitude[m]'),'[Barometric: ',num2str(sysvector.baro_alt(i)),'m, GPS: ',num2str(double(sysvector.gps_raw_position(i,3))*fconv_gpsalt),'m]']; - acstate{6,:}=[sprintf('%s \t','Est. attitude[deg]:'),'[Roll=',num2str(sysvector.attitude(i,1).*180./3.14159),... - ', Pitch=',num2str(sysvector.attitude(i,2).*180./3.14159),... - ', Yaw=',num2str(sysvector.attitude(i,3).*180./3.14159),']']; - acstate{7,:}=sprintf('%s \t[','Actuator Ctrls [-]:'); - for j=1:4 - acstate{7,:}=[acstate{7,:},num2str(sysvector.actuator_control(i,j)),',']; - end - acstate{7,:}=[acstate{7,:},']']; - acstate{8,:}=sprintf('%s \t[','Actuator Outputs [PWM/µs]:'); - for j=1:8 - acstate{8,:}=[acstate{8,:},num2str(sysvector.actuators(i,j)),',']; - end - acstate{8,:}=[acstate{8,:},']']; - acstate{9,:}=[sprintf('%s \t','Airspeed[m/s]:'),'[IAS: ',num2str(sysvector.ind_airspeed(i)),', TAS: ',num2str(sysvector.true_airspeed(i)),']']; - - set(h.edits.AircraftState,'String',acstate); - - %********************************************************************** - % GPS Plot Update - %********************************************************************** - %Plot traveled path, and and time. - figure(h.figures(2)); - hold on; - if(CurTime>mintime+1) %the +1 is only a small bugfix - h.pathline=plot3(h.axes(1),double(sysvector.gps_raw_position(imintime:i,1))*fconv_gpslatlong, ... - double(sysvector.gps_raw_position(imintime:i,2))*fconv_gpslatlong, ... - double(sysvector.gps_raw_position(imintime:i,3))*fconv_gpsalt,'b','LineWidth',2); - end; - hold off - %Plot current position - newpoint=[double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong double(sysvector.gps_raw_position(i,3))*fconv_gpsalt]; - if(numel(h.pathpoints)<=3) %empty path - h.pathpoints(1,1:3)=newpoint; - else %Not empty, append new point - h.pathpoints(size(h.pathpoints,1)+1,:)=newpoint; - end - axes(h.axes(1)); - line(h.pathpoints(:,1),h.pathpoints(:,2),h.pathpoints(:,3),'LineStyle','none','Marker','.','MarkerEdge','black','MarkerSize',20); - - - % Plot current time (small label next to current gps position) - textdesc=strcat(' t=',num2str(time(i)),'s'); - if(isvalidhandle(h.markertext)) - delete(h.markertext); %delete old text - end - h.markertext=text(double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong,double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong,... - double(sysvector.gps_raw_position(i,3))*fconv_gpsalt,textdesc); - set(h.edits.CurTime,'String',CurTime); - - %********************************************************************** - % Plot the lines showing the current time in the 2-d plots - %********************************************************************** - for i=2:NrAxes - if(isvalidhandle(h.markerline(i))) delete(h.markerline(i)); end - ylims=get(h.axes(i),'YLim'); - h.markerline(i)=line([CurTime CurTime] ,get(h.axes(i),'YLim'),'Color','black'); - set(h.markerline(i),'parent',h.axes(i)); - end - - set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]); -end - -%% ************************************************************************ -% MINMAXTIME CALLBACK (nested function) -% ************************************************************************ -function minmaxtime_callback(hObj,event) %#ok - new_mintime=get(h.sliders.MinTime,'Value'); - new_maxtime=get(h.sliders.MaxTime,'Value'); - - %Safety checks: - bErr=false; - %1: mintime must be < maxtime - if((new_mintime>maxtime) || (new_maxtimeCurTime) - set(h.labels.GUIState,'String','Error: Mintime cannot be bigger than CurTime! CurTime set to new mintime.','BackgroundColor','red'); - mintime=new_mintime; - CurTime=mintime; - bErr=true; - end - %3: MaxTime must be >CurTime - if(new_maxtime - %find current time - if(hObj==h.sliders.CurTime) - CurTime=get(h.sliders.CurTime,'Value'); - elseif (hObj==h.edits.CurTime) - temp=str2num(get(h.edits.CurTime,'String')); - if(tempmintime) - CurTime=temp; - else - %Error - set(h.labels.GUIState,'String','Error: You tried to set an invalid current time! Previous value restored.','BackgroundColor','red'); - end - else - %Error - set(h.labels.GUIState,'String','Error: curtime_callback','BackgroundColor','red'); - end - - set(h.sliders.CurTime,'Value',CurTime); - set(h.edits.CurTime,'String',num2str(CurTime)); - - %Redraw time markers, but don't have to redraw the whole raw data - DrawCurrentAircraftState(); -end - -%% ************************************************************************ -% FINDMINMAXINDICES (nested function) -% ************************************************************************ -function [idxmin,idxmax] = FindMinMaxTimeIndices() - for i=1:size(sysvector.timestamp,1) - if time(i)>=mintime; idxmin=i; break; end - end - for i=1:size(sysvector.timestamp,1) - if maxtime==0; idxmax=size(sysvector.timestamp,1); break; end - if time(i)>=maxtime; idxmax=i; break; end - end - mintime=time(idxmin); - maxtime=time(idxmax); -end - -%% ************************************************************************ -% ISVALIDHANDLE (nested function) -% ************************************************************************ -function isvalid = isvalidhandle(handle) - if(exist(varname(handle))>0 && length(ishandle(handle))>0) - if(ishandle(handle)>0) - if(handle>0.0) - isvalid=true; - return; - end - end - end - isvalid=false; -end - -%% ************************************************************************ -% JUST SOME SMALL HELPER FUNCTIONS (nested function) -% ************************************************************************ -function out = varname(var) - out = inputname(1); -end - -%This is the end of the matlab file / the main function -end diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 3713e0b306..0da8ec568e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -604,6 +604,15 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "unable to create logging folder, exiting."); } + const char *converter_in = "/etc/logging/log_converter.zip"; + char* converter_out = malloc(200); + sprintf(converter_out, "%s/log_converter.zip", folder_path); + + if (file_copy(converter_in, converter_out)) { + errx(1, "unable to copy conversion scripts, exiting."); + } + free(converter_out); + /* only print logging path, important to find log file later */ warnx("logging to directory: %s", folder_path);