From a707e8cdb32b4b1b9c66b3e4010b56a2b0188a99 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Dec 2013 18:58:28 +0100 Subject: [PATCH 01/11] Added missing folder --- ROMFS/px4fmu_logging/logging/conv.zip | Bin 0 -> 10087 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 ROMFS/px4fmu_logging/logging/conv.zip diff --git a/ROMFS/px4fmu_logging/logging/conv.zip b/ROMFS/px4fmu_logging/logging/conv.zip new file mode 100644 index 0000000000000000000000000000000000000000..7cb837e5666f51eca7ed803dfef4581f01bec1f3 GIT binary patch literal 10087 zcmaKyV{j$hx~*5-u{!S9){1T0PC9lvwryj@w#|-hvt!%n;MBKo)vbNczUR!USvCL6 zr|PX5f5tn8q6`G&cK`tJ4S+6!D1z1=V&H-R05G!v07!rz04Eb0dvg{86BkqR>C zB~ze>zMP@ZywT)pwGv=ACtIk`^KopEWMsjj2|fj6?9~2;J1@a?$ljm*+Vh5&H+CZf zl@{{X8f|kMU}ThyNFY&7Wk;TnV4_?O2F6%au-fc+0ZpG>&T;lqW){Fp^Fk6-JsQcP z9gLxuxPjmP{`i`dm@GpyDXAX6FaeL2dT_YpCjpr=7sjGHXUtV`fkg-Q3v@mDd}9qa zDiEI;bP3K+SUM)5xNfy2^JvoCOKHOO;xRp>}EV#S$1HjA1+c7>qN?fdixHtZrcGkiM+*}7O92A$z-w%7qJYb zhA4$fD8X!W7GSO-^5JgdG@6@OyS92Y0u+-F+$W8Y{3n2i!c&CwqL36dC5aS;aXnaD z=Fi-kQ_3+xUryq%T%=t5{`<3=^eENEXmT5uG20UyfIb!*{#;WL--8D6tau?6;zXiu zb~jjNCqh&rX2Dk!$F=_F58dw1%O7vPUJqN*t%0sVxzh?W)7voi)7#*OT9vQid4{Ez z!#MWuR{|BcOP$YHr7L}hrxUp|wdB*S+iM4v=g)-t`h{ltUGDk>J}SQVT@R|I=U;j` z6FEOVH$>zUyH>7&e{-#dx^%A7qjwi5?ODYsdrG=?lChA+K`P5g;|f`KNkNGY(tCMb zN69Nl(v#HnLH(Ri6x-z8hh(!cK~H$4KKlnzi$zGKVGth=zFJNVM~`I7e`fgmE9R7mvn1f0h?;Hrwsbb^Ixi!M!-CS9%20i6MV7o= z>gWtqg9OW0Ig$MX{Hk96&%k zG-)X`BGDE_l_9C_2j^kMgYF4cL%xq_oR#2DHFVrxsVH8cdR-!yo`chH%i`s`@DG&U zy|KBc@J$kB)#zlw$1y$S>anC+USWYl6dGn5GqPd&R0`9OEzu^MOnPj3i^Hlhls2%x zqL*tm^qCu4K5j-mA^C`$n-_M#Q)o~vepQs?W2Aysfn{@>8_1FPO#{Q1oygJe@dlj#03C7KSpMdyK$J{MomA~zpdw6;HdaIuUaSzgr?7Pio3ihpliJ|V=~ zwIXRks*-5{x1{THM*~+MdN; zCPa7mvpyZx*p%;d z0)=nC&TZljG$*v$7tEJ8fH&8VhS`qQei9uXBRa>&O05kRKJ(`D!EqA3gD#6^Wfes&u-yn z!rQ>5#c!FL>h}3*W5=4_San9B$jQSak+c>r#oBOTZ0_HuB9i{__hLHTjW=e#s;FUz z3g+>Sn^u^BN~gy}80kU_$AN@(*25x+ARQ+*!9%e8J;mH=7`rChLXfFxz!`6Jj7?97 zpxC7D*zA~AJtaETn7UX&SOe4ONJl+#XkymOM00^D?7)JH;Jgd*!IblpaqQF~Diim> zdKaZaVWNP_-f}gp-+7N^9EKuhYSEoFH}YyR1%+0aIOf4dcV8g_%l&4o{Y}+Ybt$HX zO?$;!bLlSTs*_1U2lo=!w0EDMai;Zr6>|8@>CG#(?w7`B1eJq4f(3msPX(WY|Jdj% zR0(7KOu$1xsy2N$()t_1MT?D&tsH~TdP&D6ax!LOMO)Zoe|yExu4aUm`k?B?L4lU^ zuZMLlnI&nD4!cSprfmZ0ocHinum#Ye(ZeqrO0&J9jcYkNqe?!~k!m_0qIMkwxeuc| z)uET%L@9Vvlle`S7~t4sD&aPzj-9FgkIORSKOz$ghNBDcSxgcc^xk)5bcJLdEE&Sq ziE-Vp`Ns_K2RM0spBKazXAbK10@p^lsU9Fz*w?aZO>;B8Vy)|O&1 zQzU&;TXfrLE!4PbZVIBevsMTSTb0t3L#11xTHJirr9xLVO$;74Tzn4SIbS^&^!6lQ z5ot-i{hoC}*eYQP?54ueJq-BQjH{P5!kqeNRjydL(kdr!kqAc75amJ_O~L#D^vok5 zp>ND{5^QZV>9%CBDmf5GGvu0?hY9o|<_C-&F5cEPJAw7ej$xaG~!vhF(0n= z5{mIQyxOyDY5zvnrd^(7cV)a6Vb&Z zk^3V0D)vr{-^YIx|f-e?D};PyCQGk5D9qoMaMP~mf6e&BKGHbxDf|cks(uUxQ3Sa0w zz^Z?Q&a8ObO|}BHLS=B@va4nwRN9=}6`d3kZ*iIu8($5Syjl_-x4ZT{u|EuhCxLNO zYL)U=^Ns-3E1U4`S7XbckT1M5@4US#xE81PJb9QR0eer9!eMt?&nH?yz?IcB#4`7OEc;fngr&L~q%gEn|&pUGx#Bn}18kp~zCb#-n8 zkv5>~Hq+s@sOz>CO-n*>Cfmea3+h8l7kEX?|2ec}PWpA>KF1v)jA>4l3XC?A84+rt zrUvNT4jP*AO*-xcb)^KYD2@=>+uZWAu(6M}zh1O=zJiK8guynlv>;;#B|DZ;wsU?w zy5BByy0b62eG81+--0BFZT`cjS^tLxmDz*mJt)tDqbxe8S_H4;(Jc^brTAxQ&0%Zn zm^qW~6Xg+D30Lni3*XpC=!-5i(JQ-%H-l4(_Yz93Z3VQ!MGUCS=7N)G^7zPt{x@@4 z(5*D)R*c7-4h2*od$axVsh7u={(Db$ho$-4BR=;&Th>x(J9??zfR?k1o9s4jqi0#B z!y8f-z4^Isrc2}0#fI)%{>!%n|Nni(rt5^LOPL%vzCTg#5t|Bu*oexr?i_inEF{ zPaqdLi-#$_;O=deu>*~1hLtnSb1+iMeixpPf-qOqOPGrOcCXE~- zUIN(QKwQaqUHSb99#j{nE)pJE__=;G?hNiIXjp}{qx&Xuq>EOgkTbE2u97i*0dhQCM36-;V;cLIXGzt~ z#wN5f0ZcM&F0F^^KapBd1p&?;Euiz%T1#!tkkgFmZ-kTTanqp#PYTRJ0iiwycM>7 zs2E{FG1Lw{j$A5)*el*_eRwwRgc^?!pV+?sZUbKOQd-A<2xojhxPWhhjUhrjEgMMc z(UO&6n(mL1QkH@DXWfiTJx}VZz>QVBJyXytWKq9F(-SS?6A`6kSFBx$x(L#V12P%F zdVqis3&|4-`IE}f7X4OEa%)y3m<_g-OkVuNgX(Nm+R({xKBfa4Tx|Cw)!hA!mq@rH zzEmYjM3p}FqS+Oix2rA-{sXM^f;BXfZS^`8+zZx2h+p?{u1H_+3hT3%rx7#X{B3;> z_H)U@e?lC;Casyvnwn!>sWwu*HXn*6AtC}i9FK+0+3>XGQLfYr^=yRi4>{K5L?sDP|892BVZ=XJ$&p1&Mq4b#&;&TEv!mUQ{Ny{ zU&6^$!9vHAmw3y2b<~0qc11PZp+7{y{RFCD1HNv;62k`zLs4M5iS6RW)RCF~0NucPbF2w8mXIdX!S+#Z@(PvaXPT+xAoFVQJ{=wQ9vJ3o28b;_liBY)*< zIuZVH61wg!-O-;uBVcp7YNvIWw=Vj_ZLy43jT=Jd7YY!@?e+Oa$euog<`4QJ(01@L zn>waoyYg;9gh{oG7i(npF%K;*5fnI* zof-e0qbWMwYU^`}yVArjO6BbPZU2wS)wm#mNNH;I2%xbnX#AT7X%lTxe*(0C@;5BQ zoUrB-VGVz1ZruJmSs%=2Od@LAeudE3KAW72GLo;gWsFYL&w0~?-pvx};MI}KAuj_x+Sxyu>2Kr2Z zyrp(KPWEJzAHZHij}CmUYECxj*BTM}jDu;4z%nloM!pq9C-S$qZc?4U$(<9In>~u= z>mliH%N)J^St=a(u9fEL7Ed?dh|#F4d$G0V5Yjq~*KGr@+ECruudU8oiLA8?sZ~vm zHkJ{{RIQf_b&e2tJuhQkyv8~#rsSHnYksAMx_;|ZjL@4vl&gjf(iBm#v#_jA`*Be@ zkS5q$iRpV^)Al^#w5ii*SpI9$0M^@pneO$30v}QJ{v|ec7ade~ggf$pArbx6vg(ei z7qM$23tHuqk-)le5)^4L2QY?=V#MgBrqIR<5xNF6KLV--;d7)F7pdt4`uvievcOg< zSO8xDZdc>Prk$mYi z)o<8_4a=zM1>?y>W#Jc+sNMy#(n>|a7fi|QbJka)_EXJFFs!P|{T}a--obROjyG19 zy-^c-QrcTIGwpD+EG6s96nbM#a0Th&%c6F)O>OuLjM42CFMXuN^h|_-7cNIM^cV0% zJV=K2`jRqY^d3x)wHuML(t+6(cXN+b_DrpIZiWhO>P=}sGw%h_)a^K2^Cvp`#IPBK zrB%v?P)AjSIWN)^wW~38kfFL@#V{hcO^YD}bE;J^wCNJH5w}|GUG^&rK5<%D&uqeV z+JB&k2C8%}W11=c$x~NjFkF^qt3^fmF8F@IAd51br`sDSUhV6jltAkH3;#X?bf~90 zEH72lVp1cno>QL|IEj+6OFy+#HJDOYWxenE zYBttInI}E`S77TYtYGbsG|7_sQz;|QH}{I3B1x~nBg1*#a=we$S)zQUdB=Xqx#l`F zSlB;8Zbq>3sJgPLuLlBT6Wb!*+C}t~GDT}!r4i0ie=Ws=*Bx1cwf)GvzMox^@v4<^ zgl`mTn%f`NkzsweVgFFTxEP(x1%>J5Y_)BRe?R^*85Bhr=p;8mLI?iuv4z~;sEPY=jh? zd$Z;AJCHK2)#>ihRla9!daSjR%VX0wb8KwjGK<$H>3$c6CC8a!ALtcJ)AW*TGTYus z)W#aX+!~F@06b$ZLCipGcwgC=rH5ZB9B;gacRv98{L7W1!+Y2J1A*5?)akg1F3PO{ z4f(_cdobjiPw26FpL%L~Mit9FoY}NnaW%!S4ifG4kKLY)#D*kkrI`=;sbU6Of;C-{ z5nq$zO%y`)#B}rz`PN>KhN&_X&8Go1kNru!=ytA^pa#yfB}|EKnw0Uso*`ZCLgVx8 z;^oUyD@oH>+af^iL{q_ zOfpr&!=(x`XaU7%4Fbe2W9W>WS6OG@%?pI!qsPydIUSow(cdoDvX0FBBMW5_Lhv&n zR9-wPJIQ$_mfJe|9Hc0Fir5U;FmW0MfyPL?+wVCBV*7lVps&#Z#Z#>rIWFbW`X;bt z4y?DmxGFo7!i}__Tc4?O^y5_&gWuzWiGJ;-2XywM&+%vY&Wdcuh4isw)kQn{O&ap{ z|F)OLT&wOkaYHElwl-V+&*faBR|OyN58sM9(Z_%J%9e1ord$gP8T-6BX%{;g_7(d! z;yXkwAleB@>OG)0aK-)juqJe6MR-N_mn!!Hd*YWpQ;(26??zQcWSHL- zWpTQ00g_MjQUZ(4`FUndt;47oyvkriiNkX5;T_hfC=@8v!aETjh+<{B))U{B;^S$I zl{Y9A6BEL}Zg=b#srH#~VY=OBn|{eDNM#EtkeTS4#i$`V?~wOwcCh}0G&E$Frfx_F z|M0h9+q|5kyxXR$d<99ndDv+G#I~dbVm^`xw(Fc|p-}RS7wOg?0MdbgRQh=IA~(Z` zgB8nI+csG`@2@(;JWy+;YV6?JTt8vg^32aIeqA%WXWL@pJlsp4Z32ymuKTbMZ;hQ~ z5k<_lDpht{<-JKDCYMZN5|P~V4}fIPs}8B7?n+h*5NatNGmWqp-I$aftiNBZ(5=?0 z)omN~`AR_j#mBmM?x-8>N)S7`yN99{K~bx2Gpa*^SoQcy>Ac(_bLzUVY$Kd?>;!N9)4^!%rG8$ggbsszCik_ryq&eprj)`;Jjt@m{gM>cDjHI3SBT zrq}na#k&VAP~5HWVm8a2Bz4}EYReJJ+0s8= zLgb$E11>)KS>3kZBr1Foyz?m*h$`xU^<@W6TSuM+SC$aE>ohpyl+$5<8o0mvS_{hr zf!Gpzv$kItYeNDF|2 z>L8CJ6GGAZgK_?{qt`T>hB(JGP}mWW0H$J}=(k*hne>sOLqkd=LG0#V7`9tZ2#t{8 zxMnKnq6E@LW9(%U7ILHj+H#a;szyu0WcbXmz-{;+KPz=-f$p1ttooYT1Pv*5IgGLG zYiNQOD-@y@<2Yo#Oh1d5CnM;x@^=k(M-uhnW{pNv8c4CaYnVPJ^$#Kx)DX(uWRh!J z_`zr~k49sqP?Cp0i;Eot-{tkLF@MgKukk!lxQxcvTQaJcZb8c{C~0d(MWko&LkmX5 z*k|y=N4tnA|1{!y`$oB%a7;_8@hW=J^ff#Y8kAD^gt6s6%DmZ|%?F0m`*}ZqF#UR0 zfuVakRY}tv!!O*aJignI`R=`Q>Q;i7kAcaghU?w9V2RSO=!$umH1y3{|EO>JFV$U5 zJQ{(Q`%>=+j9%5w`JjQ(q8A>+(XvQKXygb%({)*mK=%W^A6)Wku2G=Lt!ZJeFMNjs z(#1OA3k7&USKhL#@hAL#~ExcXb6!W#Ehju1E_0jn5WRj=(tKmxhs6S}_&0ANf zwrfF~h~vl#pVyAMXmwE*9fn88V#SMh#R<{^w&3|<4gTKOT2@E$6+$q5Pn9HP> z)dm)ow!cm>#{&|T6+je5DrkE`=n|$Sk#r(E>BtX0Pz5kms$`;jRj33?*%>|g0Y+?s zaNpa=Uz6xaL(P`bXfsm%N;j6Hvq|VDM)VHd;1v0oZ;Oq`7$}Pd?a-5-URLQCXfz;R z0rb9n^!4$^296Do8+@1jxOs5u6pbqp$31)WVcYT6wKENux_5)L0$WiMhen@+MjkYr z@720fXR_1!R~Q*xsunf8O4$(X8uQ9zxbi=zhGq(FtGz$-43f2q*@8qPcwj}1CF)*1 zQ9ixo7G*hKMr*0A3Ues*S%qN8{3!`=4C6m%N}UxOm9a4yBLN8J6@eClEw<4L44_NPcD58jRfhkQL2kkS&q z$S#Xhou5x*6!edsAkZts#_k9zRkgn*uZ{{i-R;!C516t)K=;x(=j)*b|BEx*(d*`H z@@;pR7)_enFNO@`agZxsnbp&u;V4M5g0d=b63&P*Db$lu)M@s21pM2T+$#Ir@0|s# zLOcQCWwK2bKILm(+2#_DG$xE>pX!r@^>CkSr}mMUVG?10*PK#<4pbQqvh|k9gV{J9zUw0 z*1&;yL2``%I}+RwcIC0%Ea&@61iW=s=BC`%P4L}9H)-eD{$QTm#eY~!C<mkR>LYR!`s%Mm*b4^ayBo(km9X`UmFGP81sl@-XDvFcdb9P z2utE(%i_MS6?go1*07nXjq;<_=4S>9%CW0%Gi16z?|S}1$qhDN^bo|V_F76Jy8&(Y z02`)a7Dr)7N2Scqn`o9LLcg=DX9WRQ=+fivTuKFAy&%fF#0b2%0d;r;pjW_g_7&F~ zja5}wZDDD*13;?P9CJdt_c{b{kxKP%R!?CaBtvJy~x`x!zfT;-v` z>EQx{wE(KXzvkK-zjb3MO?V-5e|m$-L%pu@{Jw}O;L!ULz26P=SPIvacrn#AYo#~G zhi=LzhVOXyG@d+f_&SF#gZU5`nQe6?xZ#QTT7$#?`nO&CS~r470WI9-Ef@fx3;_VZ z0e%3K#e_uV#2B32o&U9LAJc$UejEOel8~CRj{Q2*ci)-Xmt@L#BIm7~c=x=jv-rF` zp-xN21*tt0nOOWad7>|0O8LkRDy;2kHy*9(H~5k5?K++=Lnn71zfX^K0)13=btUIn zl;jk}BXG*BByr=!j0zTQ7l)iGEH24oUUrwM{U(St=`$J4sbW0yp6?c7)U4zsL>9lq zh*M4y>|1pbr4di#wS=P64J(xruHsP_o`%$SXi|h}P}*XUn!!X=h9-Hv++Wb*DV|9~ z64T}tlBl?05cWn45LpU!jDiT$w6wfnYVOIcUmE$KEhngcj(!>{)Bn0^eWAEmIo^7+ zq#H<~PYD8Ab(N1!M1OBYM))KR)mFk1#{N!Bsi1o#Bkt|7(XbERGUquO83FySRrV`z z?hbi8+LM42G0W|$4+J^XevL}$4`;GRl? zcRx=2hr}HA2sRPz?6~6empqD=rvW2YQ~*8m6N||D3yO)1vJxR4*Fiy^_^bYP9;*p< z5#<*^Q3ed05aNI8mth0`IbZ+)peW#<*MD!DLH(b$*?()A{X6qNW1oL#b|V1(Jp_UR u{3`_dUvU4a?0?5mlL7u8okIcsrSt#bK|%jB3 Date: Mon, 23 Dec 2013 18:44:07 +0100 Subject: [PATCH 02/11] Added channel count to log format --- src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 2adb13f5c2..e94b1e13cb 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1193,6 +1193,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_RC_MSG; /* Copy only the first 8 channels of 14 */ memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel)); + log_msg.body.log_RC.channel_count = buf.rc.chan_count; LOGBUFFER_WRITE_AND_COUNT(RC); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 90093a407c..ab4dc9b00d 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -159,6 +159,7 @@ struct log_STAT_s { #define LOG_RC_MSG 11 struct log_RC_s { float channel[8]; + uint8_t channel_count; }; /* --- OUT0 - ACTUATOR_0 OUTPUT --- */ @@ -281,7 +282,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"), - LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), + LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), From 107bb54b33dd4360fd5fee538f7a87b79279b8ab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Dec 2013 20:38:09 +0100 Subject: [PATCH 03/11] Robustifiying PPM parsing --- src/drivers/stm32/drv_hrt.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 36226c941f..f105251f0a 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -342,7 +342,7 @@ static void hrt_call_invoke(void); #define PPM_MAX_CHANNELS 20 /* Number of same-sized frames required to 'lock' */ -#define PPM_CHANNEL_LOCK 2 /* should be less than the input timeout */ +#define PPM_CHANNEL_LOCK 4 /* should be less than the input timeout */ __EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS]; __EXPORT unsigned ppm_decoded_channels = 0; From a5023329920d5ce45c5bf48ae61d621947cdb349 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Dec 2013 15:10:24 +0100 Subject: [PATCH 04/11] Greatly robustified PPM parsing, needs cross-checking with receiver models --- src/drivers/stm32/drv_hrt.c | 39 ++++++++++++++++++++++-------- src/modules/systemlib/ppm_decode.h | 1 + 2 files changed, 30 insertions(+), 10 deletions(-) diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index f105251f0a..5bfbe04b8a 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -282,6 +282,10 @@ static void hrt_call_invoke(void); * Note that we assume that M3 means STM32F1 (since we don't really care about the F2). */ # ifdef CONFIG_ARCH_CORTEXM3 +# undef GTIM_CCER_CC1NP +# undef GTIM_CCER_CC2NP +# undef GTIM_CCER_CC3NP +# undef GTIM_CCER_CC4NP # define GTIM_CCER_CC1NP 0 # define GTIM_CCER_CC2NP 0 # define GTIM_CCER_CC3NP 0 @@ -332,10 +336,10 @@ static void hrt_call_invoke(void); /* * PPM decoder tuning parameters */ -# define PPM_MAX_PULSE_WIDTH 550 /* maximum width of a valid pulse */ +# define PPM_MAX_PULSE_WIDTH 700 /* maximum width of a valid pulse */ # define PPM_MIN_CHANNEL_VALUE 800 /* shortest valid channel signal */ # define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */ -# define PPM_MIN_START 2500 /* shortest valid start gap */ +# define PPM_MIN_START 2400 /* shortest valid start gap (only 2nd part of pulse) */ /* decoded PPM buffer */ #define PPM_MIN_CHANNELS 5 @@ -345,6 +349,7 @@ static void hrt_call_invoke(void); #define PPM_CHANNEL_LOCK 4 /* should be less than the input timeout */ __EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS]; +__EXPORT uint16_t ppm_frame_length = 0; __EXPORT unsigned ppm_decoded_channels = 0; __EXPORT uint64_t ppm_last_valid_decode = 0; @@ -362,7 +367,8 @@ static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS]; struct { uint16_t last_edge; /* last capture time */ uint16_t last_mark; /* last significant edge */ - unsigned next_channel; + uint16_t frame_start; /* the frame width */ + unsigned next_channel; /* next channel index */ enum { UNSYNCH = 0, ARM, @@ -447,7 +453,6 @@ hrt_ppm_decode(uint32_t status) /* how long since the last edge? - this handles counter wrapping implicitely. */ width = count - ppm.last_edge; - ppm.last_edge = count; ppm_edge_history[ppm_edge_next++] = width; @@ -491,6 +496,7 @@ hrt_ppm_decode(uint32_t status) ppm_buffer[i] = ppm_temp_buffer[i]; ppm_last_valid_decode = hrt_absolute_time(); + } } @@ -500,13 +506,14 @@ hrt_ppm_decode(uint32_t status) /* next edge is the reference for the first channel */ ppm.phase = ARM; + ppm.last_edge = count; return; } switch (ppm.phase) { case UNSYNCH: /* we are waiting for a start pulse - nothing useful to do here */ - return; + break; case ARM: @@ -515,14 +522,23 @@ hrt_ppm_decode(uint32_t status) goto error; /* pulse was too long */ /* record the mark timing, expect an inactive edge */ - ppm.last_mark = count; - ppm.phase = INACTIVE; - return; + ppm.last_mark = ppm.last_edge; + + /* frame length is everything including the start gap */ + ppm_frame_length = (uint16_t)(ppm.last_edge - ppm.frame_start); + ppm.frame_start = ppm.last_edge; + ppm.phase = ACTIVE; + break; case INACTIVE: + + /* we expect a short pulse */ + if (width > PPM_MAX_PULSE_WIDTH) + goto error; /* pulse was too long */ + /* this edge is not interesting, but now we are ready for the next mark */ ppm.phase = ACTIVE; - return; + break; case ACTIVE: /* determine the interval from the last mark */ @@ -543,10 +559,13 @@ hrt_ppm_decode(uint32_t status) ppm_temp_buffer[ppm.next_channel++] = interval; ppm.phase = INACTIVE; - return; + break; } + ppm.last_edge = count; + return; + /* the state machine is corrupted; reset it */ error: diff --git a/src/modules/systemlib/ppm_decode.h b/src/modules/systemlib/ppm_decode.h index 6c5e15345e..5a1ad84da9 100644 --- a/src/modules/systemlib/ppm_decode.h +++ b/src/modules/systemlib/ppm_decode.h @@ -57,6 +57,7 @@ __BEGIN_DECLS * PPM decoder state */ __EXPORT extern uint16_t ppm_buffer[PPM_MAX_CHANNELS]; /**< decoded PPM channel values */ +__EXPORT extern uint16_t ppm_frame_length; /**< length of the decoded PPM frame (includes gap) */ __EXPORT extern unsigned ppm_decoded_channels; /**< count of decoded channels */ __EXPORT extern hrt_abstime ppm_last_valid_decode; /**< timestamp of the last valid decode */ From edffade8cec2ea779040e97fb5478e0e9db12031 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Dec 2013 15:11:48 +0100 Subject: [PATCH 05/11] Added PPM frame length feedback in IO comms and status command - allows to warn users about badly formatted PPM frames --- src/drivers/px4io/px4io.cpp | 10 ++++++++++ src/modules/px4iofirmware/controls.c | 10 +++++++--- src/modules/px4iofirmware/protocol.h | 3 ++- src/modules/px4iofirmware/registers.c | 6 ++++-- 4 files changed, 23 insertions(+), 6 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index b80844c5b4..010272c6f4 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1724,6 +1724,16 @@ PX4IO::print_status() printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); printf("\n"); + + if (raw_inputs > 0) { + int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA); + printf("RC data (PPM frame len) %u us\n", frame_len); + + if ((frame_len - raw_inputs * 2000 - 3000) < 0) { + printf("WARNING WARNING WARNING! This RC receiver does not allow safe frame detection.\n"); + } + } + uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID); printf("mapped R/C inputs 0x%04x", mapped_inputs); diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 194d8aab98..58af77997a 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -50,7 +50,7 @@ #define RC_CHANNEL_HIGH_THRESH 5000 #define RC_CHANNEL_LOW_THRESH -5000 -static bool ppm_input(uint16_t *values, uint16_t *num_values); +static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len); static perf_counter_t c_gather_dsm; static perf_counter_t c_gather_sbus; @@ -125,7 +125,7 @@ controls_tick() { * disable the PPM decoder completely if we have S.bus signal. */ perf_begin(c_gather_ppm); - bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count); + bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_status[PX4IO_P_STATUS_RC_DATA]); if (ppm_updated) { /* XXX sample RSSI properly here */ @@ -319,7 +319,7 @@ controls_tick() { } static bool -ppm_input(uint16_t *values, uint16_t *num_values) +ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len) { bool result = false; @@ -343,6 +343,10 @@ ppm_input(uint16_t *values, uint16_t *num_values) /* clear validity */ ppm_last_valid_decode = 0; + /* store PPM frame length */ + if (num_values) + *frame_len = ppm_frame_length; + /* good if we got any channels */ result = (*num_values > 0); } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 11e3803976..500e0ed4b3 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -124,7 +124,8 @@ #define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ #define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ #define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ -#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 1000: perfect reception */ +#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ +#define PX4IO_P_STATUS_RC_DATA 10 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ /* array of post-mix actuator outputs, -10000..10000 */ #define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 3f9e111baa..6aa3a5cd26 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -89,7 +89,9 @@ uint16_t r_page_status[] = { [PX4IO_P_STATUS_IBATT] = 0, [PX4IO_P_STATUS_VSERVO] = 0, [PX4IO_P_STATUS_VRSSI] = 0, - [PX4IO_P_STATUS_PRSSI] = 0 + [PX4IO_P_STATUS_PRSSI] = 0, + [PX4IO_P_STATUS_NRSSI] = 0, + [PX4IO_P_STATUS_RC_DATA] = 0 }; /** @@ -114,7 +116,7 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT]; uint16_t r_page_raw_rc_input[] = { [PX4IO_P_RAW_RC_COUNT] = 0, - [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + 24)] = 0 // XXX ensure we have enough space to decode beefy RX, will be replaced by patch soon + [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 // XXX ensure we have enough space to decode beefy RX, will be replaced by patch soon }; /** From 8d2950561d1889ab1d4c2fc5d832a2984048487d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Dec 2013 15:15:15 +0100 Subject: [PATCH 06/11] Changed RSSI range to 0..255 --- src/drivers/drv_rc_input.h | 2 +- src/modules/px4iofirmware/controls.c | 6 +++--- src/modules/px4iofirmware/sbus.c | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 7b18b5b15b..66771faaa4 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -89,7 +89,7 @@ struct rc_input_values { /** number of channels actually being seen */ uint32_t channel_count; - /** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 1000: full reception */ + /** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 255: full reception */ int32_t rssi; /** Input source */ diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 58af77997a..ed29c83394 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -94,7 +94,7 @@ controls_tick() { * other. Don't do that. */ - /* receive signal strenght indicator (RSSI). 0 = no connection, 1000: perfect connection */ + /* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */ uint16_t rssi = 0; perf_begin(c_gather_dsm); @@ -108,7 +108,7 @@ controls_tick() { else r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11; - rssi = 1000; + rssi = 255; } perf_end(c_gather_dsm); @@ -129,7 +129,7 @@ controls_tick() { if (ppm_updated) { /* XXX sample RSSI properly here */ - rssi = 1000; + rssi = 255; r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; } diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 388502b403..3dcfe7f5b2 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -280,7 +280,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint *rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet) } - *rssi = 1000; + *rssi = 255; return true; } From 05ec96b0f74b5d6011b683e747aae3bc37cbfbb9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 4 Nov 2013 14:05:52 +0100 Subject: [PATCH 07/11] Startup script for simple logging --- ROMFS/px4fmu_common/init.d/800_sdlogger | 60 +++++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 6 +++ 2 files changed, 66 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/800_sdlogger diff --git a/ROMFS/px4fmu_common/init.d/800_sdlogger b/ROMFS/px4fmu_common/init.d/800_sdlogger new file mode 100644 index 0000000000..955fe0e2ad --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/800_sdlogger @@ -0,0 +1,60 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 init to log only + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + + param save +fi + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + commander start + + sh /etc/init.d/rc.io + # Set PWM values for DJI ESCs +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +sh /etc/init.d/rc.sensors + +gps start + +attitude_estimator_ekf start + +position_estimator_inav start + +if [ -d /fs/microsd ] +then + if [ $BOARD == fmuv1 ] + then + sdlog2 start -r 50 -e -b 16 + else + sdlog2 start -r 200 -e -b 16 + fi +fi + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f551c1aa8c..06102b707e 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -345,6 +345,12 @@ then set MODE custom fi + if param compare SYS_AUTOSTART 800 + then + sh /etc/init.d/800_sdlogger + set MODE custom + fi + # Start any custom extensions that might be missing if [ -f /fs/microsd/etc/rc.local ] then From db46672bc4bfc4956baeb3f4d15d2fccf0ef3377 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Dec 2013 12:02:57 +0100 Subject: [PATCH 08/11] Paranoid PPM shape checking --- src/drivers/stm32/drv_hrt.c | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 5bfbe04b8a..a6b005d274 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -336,17 +336,18 @@ static void hrt_call_invoke(void); /* * PPM decoder tuning parameters */ -# define PPM_MAX_PULSE_WIDTH 700 /* maximum width of a valid pulse */ +# define PPM_MIN_PULSE_WIDTH 200 /* minimum width of a valid first pulse */ +# define PPM_MAX_PULSE_WIDTH 600 /* maximum width of a valid first pulse */ # define PPM_MIN_CHANNEL_VALUE 800 /* shortest valid channel signal */ -# define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */ -# define PPM_MIN_START 2400 /* shortest valid start gap (only 2nd part of pulse) */ +# define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */ +# define PPM_MIN_START 2300 /* shortest valid start gap (only 2nd part of pulse) */ /* decoded PPM buffer */ #define PPM_MIN_CHANNELS 5 #define PPM_MAX_CHANNELS 20 /* Number of same-sized frames required to 'lock' */ -#define PPM_CHANNEL_LOCK 4 /* should be less than the input timeout */ +#define PPM_CHANNEL_LOCK 4 /* should be less than the input timeout */ __EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS]; __EXPORT uint16_t ppm_frame_length = 0; @@ -518,8 +519,8 @@ hrt_ppm_decode(uint32_t status) case ARM: /* we expect a pulse giving us the first mark */ - if (width > PPM_MAX_PULSE_WIDTH) - goto error; /* pulse was too long */ + if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) + goto error; /* pulse was too short or too long */ /* record the mark timing, expect an inactive edge */ ppm.last_mark = ppm.last_edge; @@ -533,8 +534,8 @@ hrt_ppm_decode(uint32_t status) case INACTIVE: /* we expect a short pulse */ - if (width > PPM_MAX_PULSE_WIDTH) - goto error; /* pulse was too long */ + if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) + goto error; /* pulse was too short or too long */ /* this edge is not interesting, but now we are ready for the next mark */ ppm.phase = ACTIVE; From d5c857615b8714a49d09ae8410b0ae8d6be4a1be Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Dec 2013 12:14:15 +0100 Subject: [PATCH 09/11] Pure formatting cleanup of drv_hrt.c, no code / functionality changes --- src/drivers/stm32/drv_hrt.c | 76 +++++++++++++++++++------------------ 1 file changed, 39 insertions(+), 37 deletions(-) diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index a6b005d274..b7c9b89a45 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -168,7 +168,7 @@ # error HRT_TIMER_CLOCK must be greater than 1MHz #endif -/* +/** * Minimum/maximum deadlines. * * These are suitable for use with a 16-bit timer/counter clocked @@ -276,7 +276,7 @@ static void hrt_call_invoke(void); * Specific registers and bits used by PPM sub-functions */ #ifdef HRT_PPM_CHANNEL -/* +/* * If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it. * * Note that we assume that M3 means STM32F1 (since we don't really care about the F2). @@ -336,18 +336,18 @@ static void hrt_call_invoke(void); /* * PPM decoder tuning parameters */ -# define PPM_MIN_PULSE_WIDTH 200 /* minimum width of a valid first pulse */ -# define PPM_MAX_PULSE_WIDTH 600 /* maximum width of a valid first pulse */ -# define PPM_MIN_CHANNEL_VALUE 800 /* shortest valid channel signal */ -# define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */ -# define PPM_MIN_START 2300 /* shortest valid start gap (only 2nd part of pulse) */ +# define PPM_MIN_PULSE_WIDTH 200 /**< minimum width of a valid first pulse */ +# define PPM_MAX_PULSE_WIDTH 600 /**< maximum width of a valid first pulse */ +# define PPM_MIN_CHANNEL_VALUE 800 /**< shortest valid channel signal */ +# define PPM_MAX_CHANNEL_VALUE 2200 /**< longest valid channel signal */ +# define PPM_MIN_START 2300 /**< shortest valid start gap (only 2nd part of pulse) */ /* decoded PPM buffer */ #define PPM_MIN_CHANNELS 5 #define PPM_MAX_CHANNELS 20 -/* Number of same-sized frames required to 'lock' */ -#define PPM_CHANNEL_LOCK 4 /* should be less than the input timeout */ +/** Number of same-sized frames required to 'lock' */ +#define PPM_CHANNEL_LOCK 4 /**< should be less than the input timeout */ __EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS]; __EXPORT uint16_t ppm_frame_length = 0; @@ -364,12 +364,12 @@ unsigned ppm_pulse_next; static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS]; -/* PPM decoder state machine */ +/** PPM decoder state machine */ struct { - uint16_t last_edge; /* last capture time */ - uint16_t last_mark; /* last significant edge */ - uint16_t frame_start; /* the frame width */ - unsigned next_channel; /* next channel index */ + uint16_t last_edge; /**< last capture time */ + uint16_t last_mark; /**< last significant edge */ + uint16_t frame_start; /**< the frame width */ + unsigned next_channel; /**< next channel index */ enum { UNSYNCH = 0, ARM, @@ -391,7 +391,7 @@ static void hrt_ppm_decode(uint32_t status); # define CCER_PPM 0 #endif /* HRT_PPM_CHANNEL */ -/* +/** * Initialise the timer we are going to use. * * We expect that we'll own one of the reduced-function STM32 general @@ -437,7 +437,7 @@ hrt_tim_init(void) } #ifdef HRT_PPM_CHANNEL -/* +/** * Handle the PPM decoder state machine. */ static void @@ -577,7 +577,7 @@ error: } #endif /* HRT_PPM_CHANNEL */ -/* +/** * Handle the compare interupt by calling the callout dispatcher * and then re-scheduling the next deadline. */ @@ -606,6 +606,7 @@ hrt_tim_isr(int irq, void *context) hrt_ppm_decode(status); } + #endif /* was this a timer tick? */ @@ -624,7 +625,7 @@ hrt_tim_isr(int irq, void *context) return OK; } -/* +/** * Fetch a never-wrapping absolute time value in microseconds from * some arbitrary epoch shortly after system start. */ @@ -671,7 +672,7 @@ hrt_absolute_time(void) return abstime; } -/* +/** * Convert a timespec to absolute time */ hrt_abstime @@ -685,7 +686,7 @@ ts_to_abstime(struct timespec *ts) return result; } -/* +/** * Convert absolute time to a timespec. */ void @@ -696,7 +697,7 @@ abstime_to_ts(struct timespec *ts, hrt_abstime abstime) ts->tv_nsec = abstime * 1000; } -/* +/** * Compare a time value with the current time. */ hrt_abstime @@ -711,7 +712,7 @@ hrt_elapsed_time(const volatile hrt_abstime *then) return delta; } -/* +/** * Store the absolute time in an interrupt-safe fashion */ hrt_abstime @@ -726,7 +727,7 @@ hrt_store_absolute_time(volatile hrt_abstime *now) return ts; } -/* +/** * Initalise the high-resolution timing module. */ void @@ -741,7 +742,7 @@ hrt_init(void) #endif } -/* +/** * Call callout(arg) after interval has elapsed. */ void @@ -754,7 +755,7 @@ hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, v arg); } -/* +/** * Call callout(arg) at calltime. */ void @@ -763,7 +764,7 @@ hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, v hrt_call_internal(entry, calltime, 0, callout, arg); } -/* +/** * Call callout(arg) every period. */ void @@ -782,13 +783,13 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte irqstate_t flags = irqsave(); /* if the entry is currently queued, remove it */ - /* note that we are using a potentially uninitialised - entry->link here, but it is safe as sq_rem() doesn't - dereference the passed node unless it is found in the - list. So we potentially waste a bit of time searching the - queue for the uninitialised entry->link but we don't do - anything actually unsafe. - */ + /* note that we are using a potentially uninitialised + entry->link here, but it is safe as sq_rem() doesn't + dereference the passed node unless it is found in the + list. So we potentially waste a bit of time searching the + queue for the uninitialised entry->link but we don't do + anything actually unsafe. + */ if (entry->deadline != 0) sq_rem(&entry->link, &callout_queue); @@ -802,7 +803,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte irqrestore(flags); } -/* +/** * If this returns true, the call has been invoked and removed from the callout list. * * Always returns false for repeating callouts. @@ -813,7 +814,7 @@ hrt_called(struct hrt_call *entry) return (entry->deadline == 0); } -/* +/** * Remove the entry from the callout list. */ void @@ -896,17 +897,18 @@ hrt_call_invoke(void) /* if the callout has a non-zero period, it has to be re-entered */ if (call->period != 0) { // re-check call->deadline to allow for - // callouts to re-schedule themselves + // callouts to re-schedule themselves // using hrt_call_delay() if (call->deadline <= now) { call->deadline = deadline + call->period; } + hrt_call_enter(call); } } } -/* +/** * Reschedule the next timer interrupt. * * This routine must be called with interrupts disabled. From c5ef295f68fc475e053d9ab368881743c22f1667 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Dec 2013 15:46:50 +0100 Subject: [PATCH 10/11] Hotfix: Reduce mag influence on att estimate --- .../attitude_estimator_ekf_params.c | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 52dac652be..e1280445b5 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -52,11 +52,13 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f); PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f); /* gyro measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V3_R0, 0.0008f); -PARAM_DEFINE_FLOAT(EKF_ATT_V3_R1, 10000.0f); -PARAM_DEFINE_FLOAT(EKF_ATT_V3_R2, 1.0f); -/* accelerometer measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V3_R3, 0.0f); +PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f); +/* accel measurement noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f); +/* mag measurement noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f); +/* offset estimation - UNUSED */ +PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f); /* offsets in roll, pitch and yaw of sensor plane and body */ PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f); From dd5549da46eb2914b8e710cd656ec0f44c7ce892 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Dec 2013 15:50:28 +0100 Subject: [PATCH 11/11] Hotfix: Better dead zone defaults --- src/modules/sensors/sensor_params.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 78d4b410a8..f826470600 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -104,49 +104,49 @@ PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC1_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC1_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC2_MIN, 1000); PARAM_DEFINE_FLOAT(RC2_TRIM, 1500); PARAM_DEFINE_FLOAT(RC2_MAX, 2000); PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC2_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC2_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC3_MIN, 1000); PARAM_DEFINE_FLOAT(RC3_TRIM, 1500); PARAM_DEFINE_FLOAT(RC3_MAX, 2000); PARAM_DEFINE_FLOAT(RC3_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC3_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC3_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC4_MIN, 1000); PARAM_DEFINE_FLOAT(RC4_TRIM, 1500); PARAM_DEFINE_FLOAT(RC4_MAX, 2000); PARAM_DEFINE_FLOAT(RC4_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC4_DZ, 30.0f); +PARAM_DEFINE_FLOAT(RC4_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC5_MIN, 1000); PARAM_DEFINE_FLOAT(RC5_TRIM, 1500); PARAM_DEFINE_FLOAT(RC5_MAX, 2000); PARAM_DEFINE_FLOAT(RC5_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC5_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC5_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC6_MIN, 1000); PARAM_DEFINE_FLOAT(RC6_TRIM, 1500); PARAM_DEFINE_FLOAT(RC6_MAX, 2000); PARAM_DEFINE_FLOAT(RC6_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC6_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC6_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC7_MIN, 1000); PARAM_DEFINE_FLOAT(RC7_TRIM, 1500); PARAM_DEFINE_FLOAT(RC7_MAX, 2000); PARAM_DEFINE_FLOAT(RC7_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC7_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC7_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC8_MIN, 1000); PARAM_DEFINE_FLOAT(RC8_TRIM, 1500); PARAM_DEFINE_FLOAT(RC8_MAX, 2000); PARAM_DEFINE_FLOAT(RC8_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC8_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC8_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC9_MIN, 1000); PARAM_DEFINE_FLOAT(RC9_TRIM, 1500);