From 0e29f2505a599d473244b0bb7e4b309d392ebb3c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 20 Feb 2013 10:38:06 -0800 Subject: [PATCH] Checkpoint: New hierarchical states --- Documentation/flight_mode_state_machine.odg | Bin 18105 -> 24059 bytes apps/commander/commander.c | 117 ++-- apps/commander/state_machine_helper.c | 678 +++++++++----------- apps/commander/state_machine_helper.h | 4 +- apps/uORB/topics/vehicle_status.h | 89 ++- 5 files changed, 392 insertions(+), 496 deletions(-) diff --git a/Documentation/flight_mode_state_machine.odg b/Documentation/flight_mode_state_machine.odg index b630ecb40242a9a52a796c30ff0dbc283a4de7e7..a50366bcb3e51c2c6571597de89d7b00d746b28d 100644 GIT binary patch delta 20506 zcmaHyQ*h=D@TK24nb@}NOl;f6#J2g1&53PiGO?YBZQIUF?5%J2X8%=N+gIIH=c?Y;1p#N)NK_HO5xtlSQm%Uw_dVhR2 z3rhDBePX8;3v_5ORL!E8hK8PfsYU5FnTfAj>#$^t&V280dr?`BVhsu;mb>k)?+2_8 zANAOC@C)JeYoL@85|f`a<41*HN*gnOp*LHJZ64ky;Re-{>Meo!VSz@2!;1M`o~1w) zxf<&21 zur#tH=#%s~k!X}~S`3?GC@McYQ*MTDja&DR6l3#Ul&##y;bgob6zQIn@bt87hfX4WbjFDNawU~EyM z@_04-Ip|{W6Kllu!bz^ovDsSZiH-j{-we_6Km6lBNXKA~-tsB48URcu%)WX@t4d->`IQI#mEC&Ny zh|_a54+elhwvZsu|H0z_#R38d1adWZbF+4^bp4NqEnNfGb1oGBo8O~eud{+E5HD6g zz&&B%5MAL4gEk)@6;|OZlA4KRkoI=8*qkPyvQco|d8fn#?ap$Fmux(n4__Db{wNxX zNJn?=X(&zr+iC5VYir}xS!N2gOSk2AM?qh z1T+m4+w;CjjkH+otYQWQdVs_5U%m%FQY1n8Y9)(E{n=XxtH~w zp%)au1DT2r{k_UZlQ#B~4{?bB{`0*-vl27B7lt?2eEfG_t`U)4j;i=R+7W4&NZS-L37giufCAlcW_lJ4rtvh9 zPI}LF{Ti!gQh|oqOh^2D={Ek}IkGIMQ83>*a6X#G4L4VMv8^7;u zvVGb#W)R}g<*G-ndgO8(>+(+HbtKpT)b0cjDb)5?rY0bjX=!j#*R zWE6*0zf@xi}S@vB1QKS53sn)hP03-P`P_P z;V+)gEeH)VvAV*KW10Dd9$b8gg+5cPz=VFo`n8Ymm7|S# z)O5t7ievj0#U-7kW=(HNs}VI9$XDB5q(XU=w2DvH@QBGNW+aBvD)6bV9f_Cv7!#&S zOFz)o8!jxyKjl^QZ6~sC+&<$1kd894&m?<6vihlO2ASI#jwnVrZPx4dXq$<xKv_27Z6J0JO{Q`($(yr;r*B0Z(vv0K>@w9`HY)-c2sP08O_kK5C zR)mUn)=8mC!lH+Edp|Ugh0VxzGjBKuG`;U8dpe8dHd!N)q9e_=VHr9B5Jc`{dGMb` zV>F4{VB2$wV^^hIbYHXj4Qy>+^g(YKEv`|t#!1l>+;X2G z7hxFJFjmM^yL4z``*5T&dOw|j$=A^hgwmKsUIbW(@2&Y5K3_*uNY6Y-?g8_-EO;Uq zjRNg-LgOdfEbqAq4IfQEx#5-n zmdnBD&+YDf@WN)o&I?f3>u;OmSca|`o;m-74`euhS+yD5))!w;uHsv!j~qp4-j zn576}htG#rVESrGN$9gv=-15+cH>KD=fmZhanJfuBuik((yrX~SbSIjJ@!27>Dky7B}5tEsQ*aPV7qB*pG z;pKjFy*SJN9<$|#o+D^0;Ll|+ZS!XZnRRxT-vfZRJiJq)`S&0eDr|?2eY@e(Cg|;( z{(jo~`S0#`W5Qj;+1}yMS@PYGO=os3(x@FV*2Hup=!7cNqeIsX3yS~gbZ)JHg#uNp zHT&hjr?Hy)wC9Zsv@>8ED|k(mW{nwM{H7*kcYR${K5!MOr)*gu|4%9A<)pWhv|6q6 zM+Xr8!$$)+M*~#bu$m0}qV@R8HK|FeBEoF%3JvvcR+OqcU5I2sCq_gk*-%Id>7Nar zhv=ix4H?W&Hf5imt0R56$H;n#yj~s42{*yjXW-4=&+}IYw^6z~Ti=E13k@tbeyt8s z-LwMjL-hnJmfAs8j^8fP0`3mBVQEvRAAyzt)7K`w;=^f|-e^1EU9COqIbX*+_WQ@+ z)E0SX<#t>CfZ84AHr>k6yYs7YfNOTo{owKGJaYEg!N`!^*W-a(NLdn)l#ac}?*Dyj zbLwSxkHj6KfYGU=K6F%EE`CwSFVh+7VDE0o~!By zo6oJ8U)+Gh%FUO(;D*29rC@Eo2kg-u)yrn)``&S{9cW(8lAF_cFdNT1H^D%4WBU!* z`9QBUv{)BFIJzA8DYO;P1-CQ)H<#Gq)4M+FYdU7<3(j!r=JwRn0r=tow%(_=Zocp6 zyY<8k2WZ!vd&bzL{q6qn=b{QAF)4{CUMR7V5QU>!N!e#*7P^!Dcu zPaeeQBsiWdT$yypBJN9mx_p-6T_f$!Re1}L{Nq9?4uL^=(IFxF)wTj*G6!wqb_+6b z#)_1xewYZx;kVwp`D;b$pX$r6gp;sbuGK!$c>LO0ObH?MVIl4EWrO$JT5N9Tq}`Q8 z_Hkv!iAR9deIb?)=ymDa(Z?k1{$=lkQ_`Lmno3VKe)+U+&$(%ELD`9z>7_MF6XT&u z{&c$g`n>t7{0lEgAv|Xk;_Etb=8IElFJ&NU(;@OjzxuN`z~8Ia@EXi(-9*~n9VR|T zsIkG^GrOEzx%3s8D7~fk9)aK)irbF3oc($B<->=4C&bAgfQ{NlF+mSVg&iY8ZP1EJ zJVzuIOwx){HmuC^5Fm5Q^}+L0^bMBoW|SaA&}wbaFn0To zjzN6;NWXswYSVtD{t(#KY~?oc|MObJ=ajjOe7AL6Ekq1&{&|5XSUegX%Wj}9RNeVn z=a+Soa4}@lDuFy}%^!!^vYKz$UH1^Sguhgu{N^bNfLOE<^Qqf!i7i75cRJo)VP`j) zSMvRF4dN*UmXtrV_+PWyj2h0x0&07b{PaJlHK;#+Ujkmnd|3_gjibqp+E{GedYd0k zjcptGl18P}8N_8bEWNxLMyx85Q0#Uervbs$C)wXLIAN7O&b{8h;{YH}?|MUx1gyMP zV3G)6M$L#AvS{3f#J6yl6y@&PsVE3hU!@~Omyh{3V%YQ!Z&AvkzC7;vKBJ5_;dxsW z2}>j>(3T^;=hv>!zudZt(hF<>BXde^3LiG$BawZ%%QVzbim6<~hJ~v!rS|o@my#-- z;3{h3i;G7^7ILP)gjDX{K5u&C=N}bH?YjZpDa+0WDk>W?S{t|}%*}^O>pN>hIOMzQ zBF^S_7L0-@rQx_lLeJF?`v@QO9sbD-&(FUmD9 z!zMCJz>yu|DdED+sQTTIFYEQ9=je?!^P07%?yiXMW_n!6Lxg@5VeF>X!==My|KtSX zkis>~5S)+L>Ui6~^C=+OWkRWnO^j_IIwe?&QdkU>$6uF&Uo(l_xuoI43!g+|x4YAn z$Lbv*x5nDSC;oPy(uyh>n#Vc2vU-j2ZQi(7-}oX;0isGMNy4c ztc|r~3d|9#azR8JES*uayrJvz zO>>YJuPJx4TqiW%p28dbjKL?-J5l4}w>h-dA;mPyvK^CEU==R{ix?v=)`x(oY9*3o zNJROc0t=Z6+!*6uXxh~f%h1=TkodhA*lPVK(!Sg_{kk1U+W9O#=4cM6c-?PODLpn( ztAAn*rD6@S@VYsE+#xik@4P>!3w_~z_hx>XJ@R>!+6i}C#3|#la8hCODq(3j1X!k? zgw*~Pzt{!!$Ezb)e{~Bb&uRjfd5^yh$2QQ!Z~w^j^PSl{@2|iEuVKz5XXn-(1l_+{ z&oPzCzc4bWhhwe{Yc)}QaQHx?|+{x7pM8ArTKCmLYjOu;6~Q>M5W4^B86t{nRgCI57pa&ni%8Yoo+30WWZ1;VZE+1n2_6|$Pi5#Lh6~6g*M>G zT24RfOMf%fN$9xhC65tZs>s8ht_cS^YscLTGlqS)IYeVk6OTPQGO0O*#_C-h4*R=x zpYZbf8IaOyyFEZR!)rHk=wdQwHPy#a*PLy5{U_8p;P^@2ngkiZVl_Uc(=2rz(X79B zh>iYb@+VT-VWf}IV)>Oj4|AM!v0=1%0gsW(+GXt5x{ZTG*~hwh`<+z%6i!*jSk2!4 z20oh9g1S5f_TGJ4RLMff>Ow|K9!7N7!a|4I;C#?>MP|}^6`bk1Q_Qx>a7K$87xsKa zleu@wx=0{vQ`Hm@IIs??Eq?oX`iZ)s!^y}?q^{&S!t21_h_TMrl(&bq2M5CU8HS2v zBZIr4Asr7Nc8Z{z2DhNT9Gv0s>4)mgw>|0a!P-QS5+IxzPp$Hc1{8Y&Ka5D)@N_b z1bJ$sayV%oPy37+giDCKEdku8e3MFM)Hqc1sTC z9;$`Oe>Mtu^TuekMfu)AJnFNG5_+VE;?0gNw-Hf3wB@(x=%~I+fy#}?Xk0!>N&N)5 z^+(zJt`Y^nmL_)RaLmR`CA$`WU|(@C#p89sF(lLC zLk;EqwTAlngFXz~DSg)_n=OA5M;%(%a(wJxAiRCC%+K{*?R1Xvm4qM~xu0Q`=s55y z#={g-nw<6<5!TGCdA7s6lcC|hno<=e)i9KXC zhvJQB92T?9(Y-#UbXMkDs5PxcbQ!`2I)B0F0N(F>cu5Lj1p|RKA#W3Qg&;pB#9&9( zUm7$3$mqH2>a>R}_GNS|ks|jSTd-2HHsun%Q$TjO5Ib^SDOM!Ho4o<_eDIQdM0V=pKi9sW%dvjt={Tu9wp&iNZ`)}(I z^rHBM4S%C0oAL?I+pD@L6fmz0(etf$a$BbXD07w{zNXT|^sE-`|9A-62mc{JcPx*e z(-eVz#P%N|$*f4)otqTlrxD=bbp?6(Fw-M?`8C#yP0O3BV0YBuu|Q3+gwrDaGDz^KJ0OTZHD#GbfIbob+wO|CLMvfI>)R5 zyB#Otb;<5ErWlNF2rm0;g|UCVUrU+UdDWRWsncaQktQx<+1iKiSu|lYp!j27 z8Nti?9UFQJqC{LqaT!r8%jy3pNyc(pJ>9C(gO*M-{7Q4O%>W2DOSK1KA4;0J@OddZ*Wm<(G!*X<=`zn%k*BynBIoE5> zgtZiPt;}za$wu-_#YYh1kirTGe!Px@_F#^A?cfN4Q;c`d>zUR}uWHD((sZ~LJwx59+%_b9;^X!np6_|MRL%_{GWp+E`{4zVt~vp+t1 zgrea(X}q9K1;i8QB|v6kPQ<=I!EYv7RElQ3(wAS%lU_KUkiG18*Q2{2;)LT<$@@SX zsxwKtP2EiD`K-Pn3=vu6HgbW2aSnqx?+*E0w{bS972<}rEmGOe!4_t}Bm7a*wSh42 zRbM`N$prn3^G!J9hbV4_+-6bQ;G?GFkze;R88x1$=v0D)POaH54v%xdjQ(e-&yQqk zzf$VZnG2;SvbQtCfv<0$E8-R3FQ`$H5GPmN9ts_Z9_xDaHkpsJ8Fg(5CXLCz!Fa@* zF+Grcxh2450byT2N9!&3fn)lI(pSI>%qPN#-Pg)z^J^UShd!|$0JgDUc}@DhqqIPH znyBn{{REtT_}tw#<&XjG7gP2dP&MBDz03{3CX*)cwID$7{&BFMwzg0FS@AYs@pZo-Fth$UdFsOBVgxC@^4Pp5+almqbgQfGa%*i*7X1Sr z?DTQM?h={bJ5_G;cnp3m=`v+B6g${$hmk%`l_=!f7SnBG*CEhHxScq6WqAo=>)l{S zjxfddYB_#Me>bkz<>xm>U;OiYFUMmjIM%EvFi(n2VcN~W!;*tqVv<5T`7^ucet#6@ zJtt0ESAHT-Lt$c!LryiEhAREUIUb>3Y&GagW|t-mlW)q{U3J&(R0?>2Zaz+z+Zx62 zV5Lt!j6w+<~;E(pcA1^OG2V2@chda79)5y86vt8IthNMvN*`YGX!`Wd_ zTB3$dQos5oGn`Tml}2PUIkGUGCN-aFMUje|+`kI)m;uhaWrbJ9B}c2&Jw>jI8>cQk z`ja0D7^aNScWC9zBSSZv5U$ZeF>fyIT^CHYD{Ov3)r*D(ZbX$ePe0&V?h{g%E1dl# zksSRNKBk1#i{(k0S$%4KQA}Y@N0SUT_C4q)C;V77!lRR@biFsmHAkbKiDMZAdfF1g z;MsF?1_&5(8&d8RvI*;i2PA|rnjebSy}VUQHRp)FjyV69I4rICc7^y6fQHEPE3DSYumRhiCN=$>bjGQqnlQ1}S3qnY&ZeDQXhlW0 zzPOAphdg;2P3c5`h^LS@m9gL6B4gjwV%&?s;4jc(W$WH(8&MQx|D(}qc`NwP@+Uea zqV#}biT@MQk=h0uElau7xjcQ^geaCy4g_L24gw;a9g$|RSJOqCbev~}s9c-gKiGw9 zl^oxga+fvH!ehL!_1!o)+omVZ+)={snI^giA`Jd%*2z4A1&f@=co_X1sPZPVi32Qtqe~89W~;E4D#*H!uzRhO`XMs zl!%yIU{#raaN@FdhqS>gZ(W)%2@w+$)s;`-bJ;6#z`WF~n>o5yFjhER zcZG%PV%D-^UL{)y6JJ~MLcz9$5c_TAHMfYqEp{$5cza$<_$od2GPP2jM>f|40y2fF zB{+quRL&aTz03}~h^&i`xo`RjEIAVHT-b7teCo3uC7pe<_a(<~xTo-s%kRQFZ=Iq4 zQ86#sn_g{sww$TtVJ>dhNAZV-`p@Ga&c#6YM%%NwMMoW3zqW9ki1LS3ioFRE=VK3J z3&GQ3td34fbGw!Chvqx0W#Ohe18N9?MU5W%(%|T;V^fMzS?atKX-uq^WhZ~teGH}H ztdk}rb>(N0hu0>x_%`%)Qso%ku1i*rH(4^}Y~MwOb0e?B;gsOuw*UMnaQt(jL9V~! zG^H^y3@enZO_7N!14pW&z^dUlnWRD!OxuN+fdr#W_scMK_ML^xSlvA%3Q(+$$5KB4 z&#hWTgzA_r;N!T{^G&;u4{n(q|1zPg;`IB%9x_UKNV1Tf75CegGBuKNah+W4^=rcO zS?@b(y0W9O;+N)05Zpc6dgJDl9p@tXJL&S2SN$^sIx4z9Zd4$t)+3LMa=g1t0C&7D z(oKxTzochQil@Lspk2K53AjE;88V(xi_q7=>ZCmRW*uE0f}+I}e&&im{8qeL&HlNX9_OWHPSzt>6mw(|o~JW6H`NjW4q z^O#rNf0z5{FR5C;Z3x4+hxXWwO7VOWf|+)aaQpDqFtj+klF?-AIDiJ3I{^=pfDxSH zhYu%Kz7KoR-+yQ^fya&B5=@(6&M&MPx4BtR`to=+Gyl{LjciF3Ck_G(4UMFi)ml>1 z&2@>>*N+Bd2Hq*1AFTl@wI zyh?b6v+8ITiH(u>v%H3T>Fn^* zHiBm;*dv}&T`%azb1mBY;w9=f^#F{)4U>@yk!R&29kYnx(~xA{<5U?%tNNsdwC8$# zMTFdMjZdS~C}yfy#=CrTH}$KHI(L(SUPT)vAE-F}OjM&R7S5NdBG>LSVRptZtG%34 zf^Hn~LmMtb)4=6Mj{G9B>kz{x8P)m=^euEudOO45l44Fy#CjZr8#(n! zAUBVJN+8BRQ9d27Da@xaDaGleWY@z(0v;YI#c8dBLx5wfvV@dPmPWakfQM%UwB`-I zQqLkfYk=ddTOXT~Q#@+Wf+%e5bDmC4eL~}|^=FKB;P!lU%p01zT)JFYg8Z{NmBYn_ zh;4a#{}AKkTCcQ0RZ4MMu@c1SB^6RFW5DL(;$f^_wRh14mli$JBcP}#BBvsO-0dW# zIDJDp0i1_Eua!KjnC2gU#1O$gODRs%bKbvIa-cTxYuqd6@w&Jew%7U&=H%>8`k^Q7 z)5*HHu+5B(_1Y>bCQXM&9$!--jwU5HLQq@A_+n6pp;b~;{qM6}dZlDoLz<4&Upptq zpmmeRH~KV0!2Ix#U~0Q(a=X?ljB^Y zH&E()SFMKlHadpO*)tDW;e0?J_Ta?L!&j^Qvn+3vNL5KvaoSe@w-4HPU4PyG-X~?v zC$_FHWnFySc3HeIIN{`C(fZMux-m3(;3D|#Um{dH`9^sO?$|#!$$z7dJPzeu4gjMy zYduUrF_T-qf?$bi_VCr6+hCEEO^UqpG0ejP=?e7|WPY%(B6GWt{GlY=jgKy8fnDxF z$3n5sgDb3%IqwtmcocsBbUOnp->e*!}h7dkk6 zAM~VFrXJ;Uw_XDX5W#IB}!qIfzfBk$qt7lUvi571`2Qh$hW zuub(}d5EFh;l1Wn3J+z0?FzH!5$}Q<(;l^>IKDcf3p&NXVeQA6W+YfLPLtd z%_Y#>@rcXKX&WMtteN|uI&zgfZ%!uaul?HK=tGXc?Y)N&ur#X5Tn@nnhAV6%DJg!v zU$+bEnNb@@4A<8;Woe)t3o-=gm02?&e0-aaS4*#yN2Gb zb`I)ks0r^QeE7B)ha;Og?^&BN92cSW?2@!lqO}B1Pmo|eKV*+XRTJO zsg4-k1Un(p(c&wcvCa}K_m$3Hpg=wp$BN{=`W4sZCg!yKN(!ge6s?zAyDyk4R31zS z?P!_<3*&*(o|NKn`fph_*+sNOQ8Lq2MtnSPy?B|qUTSMkY((Jv1UiFRCZSEcgM*P= zpt6ovdf=rAolwxN1jzM5-8;9+q}A=Kd8PTmRBeds)*=8Gt zglV=GLW18wY37S^P#^l@1F5KzulcaM5WZjbF>1cgl}2nBRU;!CO8rJMKpw+!BeFhx zJ{SbAXFIn%^cnu`AC%nqu16FbHO0C2iz=a@`((OL%ykX-BB1H_Q22qq&nYQaE8nHN zAmTe~XbgNGR{V<@w|+7Hr@ER_vK7mWSP!1Vu*mn8ePaK84iCS9bD48ZLuJG0;l~@h z1lB^b{C7f%YloiPDXjyKGkBaZxuwcP(Vg;iwSYVrXf9x>>bTi>9eXJq5FEhpjOsf9h9MBhScz0e~gP3eu#@; z#e(2_JC`K!&v#X-VVaSU^n=>JARGD#BD*6={#^?9$M16aIHk`Ua?W~1{s!sp41+U! zyhREK*#^A!jUB9OND^f-F$a~8wDbD~E%lO(@2TxL&apEL!@n2AbY8~GvteHxi!Za4K9P&zYik#PPzUz;}pMNfBf{M=-G~-=*>|Q82<+{Yj@w1 zqt@gPQ6klUjQ0V;@G^*pP2r6`a@?-&3RO#tLfya7Q`O(BND=)OKdFLA3kyKaoef`@ zx~8RIaIR8QAe_61wV`HWK0c}!);{PWzJ2U7-U3=hWCqrksU=D)h0yBC@C=9s={E6$ zQ^kEv>jv-qM2XL}OI<8ixfjV6!QUn)zzKNsr_gWApiH~n z8i4ba-^qV|yDt7VwHv_iX7t8~lDn#Lce^%|wT0|vEQLIoPMszcYk`_hg_1CKD@#($ z8F~4``7dg4we%3adz?$?c(}B?#w#SSunLOrI|Ig3iCPwg*^gby?@Zc3`P!QWKk{yU zq$^Yr^<7ZtW&46Z!tsOco^HgUcnINKRP2DxrazcY*|=ztr?||*JRym?i}(d%Z9&9d z7<%Y&D@^lh@WLs8+i|^;-TT(gU`HPEKwvjNW4} zCby8PuT{T21qJj>q!uHi84kpQV>vn+QH#NBW&+1JnGEC{#KQLSr~8^=uBrRV+0e6= zt$$bY8B||(jY;!8$rATq(j$fY@HW&9N(Of`4PFt%;ds`{6-1e|3hHBU<;sA) z!YOJhZ6rC1brgS8afL4TqR3yT*-A~%yu5^zG7M?7rYyHigbtgqEUlJU77&99KaS16 zrH`|LGNM6rL8fB*bZg@5b*}&$59)CELSYttSq7}^VvTj8laTRTbHk8s>_3hRbxF}5 zjCA_+VYl7FkZq6aK^JGrr%?PD;f_$AKO#2dx#N3HXJBwnSL6Lb=&oJ}I70^Ev1@+= z3mxFYTlwKs_$YA;5Q!n%M&xB@+$J!ttxXcKaKk^qJ|mAq0>grZpfP|PPKDr0@-|Fk z2IiO1{4kGA7zH>V98*^5M1%gRev_F@=oG}{Bd}JLHsi^?)w*2=4%~)9O=h)6D__yI zpT{X4kwuK*ZYWZ$t1_cis@@pjyGOj1Iq(3eH430s>_o~%wSHu%VESV?Wa)=L=WKE#Fp$jia8Ho#XGea3X zJJD&Fn`+_*0R}$Lt}(~$aBg3J%vLAS6~naEt&Y+q>i%BHDI&8Wg`Uuvt6JaB#E5oT z1k)M3n^^4Ka?DKH#_vu-%V+BGFtM41M_w#2;PNe6>c!&0_l!_Ryl3>S!i0>8vj+R) zNY_a3jaBxQTf@j2EuxR()HS__kzh_bPwUt^ureywU2AZqF&@<#p8xBNp1!<8=k7EvRk$=1otC@W@AGn{+DWo$`JX2%_wX9A=C7Bh?sSc} zyPerHoI$t74N))_!XX6HIi{*P_9DUN;2Ozzy&3#`qc|snf?t)#C_`v*Mx4N!G243= zv70CbS?*x&|6@!Kf$bhmlasQ+67$rbJR?j`4gp**+$Aj94Q<&lH2A2NFFA( z|HM70NC|SO;vGGQ5~zjSE&TtA%l;|P;?*%;p@>F#5Bzb zIzLRvof<|q88~3Z$EDPYEh1oYE<{vdn$dRaFApoCYLPo+aYfGxa70^ilSm+^bdY17 zH3PDLtD*SuDRtsN1dhvCSs(aw&w9M|Ji3A1utkLa z-a@lCtVS5(&{2on;$t15pre9&$aE{A(g4s=FB9=*3d#I}jEht^SE7-neK=`1jN`_C zGIx>g_}!38vT~lG_%}^cRFZ+qbB^G(R8@^w<+&Q0S-@)&l`Mt|mxVY)7g2l2i{1T^ zC;m_~BPVe4eoLu;TSP$!>=BRGl+F;$%E4I=uvv|*G3EHkD#_IJ%ve|=w3Krtb^z8^ z8!c0g78>N)Jr&X?w-y=*TJk&i3pmltjhZQXiGs8iz#P2H??cfYGI^3q{*?Um8~CGP zyOU!Ct<)*Fmw|d+8;$xM&X%{I_dGKbi9zBtj!>VSUVa+hZHgr}1g0yp(*nwc0V^bW zPnl+d1nEed0}<(}g(jXSd4ArFMh>_$vnl}5NlyHXT7{WF@qI_N)DC4mhGQ}MwG&m? z@4g`Blsi$7Qb1j9b{X}y#T4K#x#}Gvr}$m;vP*;#J2rjt`59!JuA5@W>L)=@El>Qb zCT4q3Yk}40flxrdUr|;fL5BnVu@b7usAZ;!;o#+fN`n#AZQ-EpcW?{&{BOWGNRS}? zYHK`Xv1+pO_xNw^TqS#w1(t&?akc5nb+WjD+}ila(Ev%jbnOeNcMoUL8W-M#xHaw6i%k#li0yD{K9wZ!)8asm=jkO%RDWsO7c{hs);Csj_DgRm4b@jI5T3;V+pZUbMaN-dVL`=(@yq_ zx!nk#O~iJy82i_qG!KULqgOUpRI$iaiA$owQQ%8{zOsn81_S~EEmTqz7k0?yoZybV z+#hmSY`J<*Dl`4c@j~Ff1g?nZ7ZLMZVR@aLqvcS2V+oJ;)w(SH$TSiH-^S^>>mq(* zyTHcJc8#&faVRG+ZGH)hE!{?6-)JUK~g-a`kiJqhIM*ktW7 zL+u)$)Flfoa#Rn zg-cqi_HTS9IpHmobVOlX?KS8oN_wW%`@b0Xu88Us(tQnTjI;F+f8Y_?b4FT@+KOVA zpzYbK4Zh|=4^RApj?qKZOo6H7McPm!ltK<#|5p>3)}Dd0o;S_aq9#L$=y-@q`PY#Z zdB02B0Pj$Dvr+$f_d11PAfvUP9zKAA{x#hcxEJdmiVSnI@Y{1*}x_p!Y4HG$R zZ#Wk-wQ=B}*tjak!2qj}&cm8?wmy{hk#omI9`C|LQS%2Sf3HEOL#^oOf)Hs?HOxj~AX(j0NvW0(5hZw0xGn@fgF zbte)QgVJ)qzggPm%+0#PwXrfpGpuAxTtB7IPL77X*UDO{Sz20dOW{ z#6{G-a;|dUTrs2q`g|>jeQKJ13d7?>tr!dcs3C{t&Y$7Ni0FTOsR$0PCL`5&p`2EF zf%ymMYh0?UI-6+OnYzlf58#drjp3Lg#U`MWU`)zM^Hb`~%XY3WT~C*2F*b*1=B{O) z<>lq&equ<(9oK(c<=wOr?_|CN0L4{R;6Wrc;vZPTB)90spkQTk1Ys1LP%v20I2Mpd z1u8TNEaw0SvQQEykYo`S0womhe~1(qaRMo10%DY*{tlD2LJopzG;)B4ZyoQP2%gZ& zDq6v{WXu1uGo@y231QnwPbS!a5?Qo%=O##sU^18?hL(yDUU@ll_S2p#? zB$2r3&%OcB>@T#ZD5f?6iMyY>zrYrUyPdBr%+;LLW$igQeY@B|f;SO-8XgAld_!|K zWX90f1&*McO$egRN{{G97BuYnZ|qLr6d-Q9w?f*cTM`$VH>7N#C=qk|>kQ04lCR9B@@n=bl<_+E7O>wppP6sFU(|l{fxoy*N+`MndXl6V(4^jR?yX zV`@}%a}lsbVJVW=s-PI~wzj~{8RGpnZ^;*78&E36ibw3zWp^uBkcM~=n*}859=gJIZI_dV)Ao(*_?!FYhv{x}7a}-Ay)EdH z4=^33gm!DHU;h02S5;51vS&?CoXc z47F;dwphsh;4cDxW4|KU-##}V<+rEDwE%SQkkR28gO`3Ha#Px8V%m;&5&x*%j=}_A zcIpPZsw7w?jdI|tYNL;_tS&Ch>8w{&g}5=OS-7wQzM-n#hc?~&3SBVllbp< zo`p5kh}SJo&bQ->U&H_#7n`m)bwTy43P8D1IPLyoNo`(n{;7xpYutGL5myOx_I-ke{!&=jY=~|ocgugJLrWQcp@}rb~wG|XKPE5tkPj* z_kO(L*1&@oCmR=sT5;Xp<#K)x3B0>bopbZ<$r?c$hQ&LqZSKCY33tAKcsKqVtJu`h z!YX5i|A|UVC&HO~*H}qH1Xeywejwo`EfEooS8p&G94Q$XMg7$Jd6Y!|_153A^7*>1 zmG5SE9`vP$69`{GcHF2o^d3twapC)TRK6hg0pv`X2Q=8v-1bqnsjRF*0!qHTt3rR> zDma%i6~16SseWD&8t_Sv=VlVEeA-h@VCq=mmno|#gzjZeP- zI}9`Yl=gk0*)A41#3O^C0kAB3zuvnsFO1yWQj1p_*^_V>DbWRzYSzGO$ay&0Ic=f9&C6!FFAKe2U{#2oPjMw^LI@Ps+Ab~TdycfkRT8-137&{e0{}y03oe=`lHifnAvUn(^JgEp8f<1jdptV%hphp$`9!< z4%2S-HFZs#8#kvTv?d12eX)y6$}f;HVoY`BpbAG4&*U%Q1E~`p;{vT-QZ|cJzw@d| zJZ3+SR+P@MSfrBdkzN{#qUD-L*r#F9mkSZQv-0t@Hdo;(;@>Fcx2M74lj-tC9rLdq zOU=8V_q8|7UKd0V;=S^PTBKu7P!TyK@Zs zXG8*B_kYF!Rx1!XI^`i6W7>F@vm*=eu08IETYXmmiiWS9F>v**lWT(23a5mtO{m16 zKq!C~p*P_8b={dJ!lYsbih^WL2&fKDyOx34z#WEuvrfm34f*IRb zAO@3O5Jw)7KJaA>B={Gs7k3qALy-bB+D`u9A#%WCWXub)69+g-73{%FeGD3emWcRJ zY9Q9yywT|!c%Pv9dme9F9ynB!7FOF}8%9e)=QS}mCujvvW}a`rHZ%294r;8D7d8X6 z;CcsqWgS^R7R00;D$b)1`4~6^2A+mf!wV^5v z@I?60y0$wJ<~PmSwAtt>D$;r9iE~W%GITva@~^9 zU)EPmshom1fe+#XF=faV$Rm*}-)!#rp}U8wM5jP@lEh#EQYw!sAWy3|c#EGG3_!nP zJJ^*GP8=x5%CIgRoCt5~lYk9H!6ww8%qw7b^QM3Ao?M$Sf_(KCn9c7iehCh2d>q`s zw%mA>sUd_c9kMtv3Zg0RjA>2E+28T3fp?X~d==dYee#?;U2@8P&gyN|}RB%_$ zmTE7$Jjvd4*!#x5;Z6oO6-~=4Sg8r>!t(Rp1DR1t;o#jKHN8I(D*lPAGcf0ui%e0SX$n%b7{*R%HvWDW(KBeY8%nzAs^guhY83Ih3|<&IjMcuZ2{!2 z@7m`f$WnWcT9)O2^5f&NMZ2={8;nK#hXw8S*G_)i*M`tq>5uDA`$+C8QJHa0}V1HgyLm;zWNg=-LDJE^=`x0ScDy4eQ*dPe+ zd6eN3a5O+>tl=5LP@Tz@j#WJM=`fr@B%NdPVIH$bmlk;*_;em9cY9sr`BJ0ABZ(pg zrk8$F5g`QS=bBpLi;d=Glch-4+M%#F-3)yYi_Zvt~V=5a24KI^c%^)!w+Qnx%1gBK%4T>}rU|u?2RvFf1 z)C@>N+|msd7YX&Bs+R58?!P`h4A+$+-mk`)^_tt?nPLm9hFpHU4k;C~a0);FY3`iNnTCN5N*E#?+4cR36o<|j|k7py;wu7tE zU0DK`!tOD%KfEiBC_Xy!MFtB_BJ zdD2w}iWpjib#;I7ap90+tYC=!i;larSiE}vg2vr@)BKNvKQEg{I>IfU^F=)r`K#&Y zJl0K$VwDKu4CGDROx0fTkfGJ^yA=%$^=Q(p9nDv@OcrGqFX-(YErl*DT(@ss-z~gy>1!23JcV+)w&KC) zm%u?2e3-4SzheBgR(5uN#$hYJj7MsPyI4gE;xd=Rh5==#U;<6wf%D>3IzNQiij$E` zw+7p(tPEw)qE^>psF1z2-(iop{UOT;6nq_ubE9o|9d<2LejZHVYZk2OE+d`4LBlmj zt_L5*{=S;qGgev|+H!=-sKjgiD6$#%tKFK`eW$J-IZx&t-bEDX9JbA(z{ZFh9%7{4 zWNOw*^C8dm&>I$C*ODu%IciA~&7-$7!rQ!5_c^3RhZmp#dUylcug`cNADJ_`pYyWT z(#nm<6Mt3GZzQ~>aQlk02xC)LnRF0uqThxy`-xi*pV__z%H159Qc719?Vi&+lEe=; zD@iLI*V;9PXfiB5lCU{J-efP>W20D9%#D!v2&@Fz__ zEeaArW2YuFLc89oc9_e)QsKGI%$q10CUb)kDC0QFgE|KVKR0-B>a{&@&AviumS|V} z_WF_DW!W==!VMs)o`4#ZuU#G0M_ZM{0uYTW`>;PQw9L=FnIBuDJKF)-RZq-(tNNob zk_2YvO*Ei!zWI7~*QffbOsD%Q=QIz?ZY4$0Zgx6L?8shaN)dX*sL@$4$8GY|MVazU zQ%7cx(W#waYP6{#RI>|fn)2-N4*jme12;6_S%E$9<4JUOF%NG_?C_YG>{^)&lBF+3 z@eEE~^b{RaeLxgfp_&*z zaw0OVHn=IVywqzzy39;*s~B~(Hh!aRYxB$WP#xwhJPq4>DAD3B5zs_kw!izj&-G{_ ze1Pl6a=rZC`~g&uf2$eQT3nYp+)L;=jEOZb>%N7al$0=7n--cakL62V>_03h6;@nX zdm##{I0}w$YYy5sG?8|w-(0pXN`u7FzPd0j#!=^&d-Sy`%*r1la;CEGVMlk-R;qU$ zUMd<1jl*g@=32hzb$%WHwoEK6D`4+sHcW|gw7y&A;hCqM;jn?0{wQ!2!y;nA$JXv* z{$YSwn6!5$a?kDR*!VRxFC!~NnP(7j_#NuJyRNNt#N>(7iB>eJ-LkH4oe5Gqod{@; z*w4Teb_echEaG9d=6gs|_s!?QFVbDbZbUfjwPPEq9_M5H@4iSd*Zg#weAN-KN%y@! z+3^}8RarptX{UNhzaD&>n}JmE`IS+>LOE}?!sne_#j4x1ybi108PSGYix^3&4Xy?f zNVYZi0BsCZsO204Hs>A`O%4N{<} zulIGF4AR^o(I6kjGqK`%@!c0A8@f@T*x4|hs;Wc~bFEn6SqQ)isG9b3*L{AMz-}jE zV@MGNvZFL=L1Oa)->#QZ1xf*dIOayI3{Kh0DT>b7Fx~==tH|8`-BVV6bo}~6nGfO~ zOiCIYPA4D!P>t$Skp?)Yl1_)R+lj2?(;7gO$;!1XW9?o=kbMQnz^Y(-AXOkbU($hC z2u|aRVm8Et1X_$)Tv+8rv07e@DM1#DKN?tuxhSEi|3YMx$u$AkVyN|jdZ`SGmAeSM zLOwBX2`KYv8@su0N&+8a$-Er|QCyITeGMFuss;|YWgJv#ftU6?@t<{h<*qZ#Z4xw^ zq|jz?WjlTTO64q#DFr0Guh~pHLkKhg+Mu)Jn!+b4Z|rsC=@GK0NKvtT=pIu7WNGm7 z8^|@8DJSrCVqJ&6@wC&;#c;FXyyTi3AS)grl9}6>kfE%p{rAY25(!br>ONiql@v?j zmOtX@q+?EE)LeFp=My!couilrHF2QDSH}U4+POt>lyQ$a`CFJzjKtV*ezr%5q0>kN zgn9I<-iAJRME#tlx1Rax2#{NoXS1|iWZ2=8!LJE6@=Mf`Z#w{clqvk$wryCKe$rmJ=bXH6^3?C zIrfh0F*@>WQT@^l*6x;$`37zFmqJ5%R4>O+M~@J16?fIR6VJuR#ltqYQ#j1lvxhTZ zC&^#Lnp7Z?TqSg6lV4*wh zNkW|{D&hU}jnDjT%|=Iv+tyYoFsb6sZxoT&7L%Pnyw8mHU=lt@O@diNh=-o&JG^p- zyLb7G5Eo@}qeMZ?D6%YXOZr)g$g9(c3n^hguT*)4$mp|r1P}DLA0TXg6j{f?7z@2# zV%K~3IM8bqA^Yjop6gG9MA>ev%zSvZ%FcHaYqZ<}PJv0Wof1SDn zZ=wK`;aFRhjzO0vRzvg4BX54v_WV%$Cc{bN$I^5i?adrALcE-Fex_~mCGq=O%iZ4f zcdc5VW%XIl3ng5`4nj`(kqXJAA8PUCE@C0a$>c>QutdwIQFpeC=3$~7E`^4oK}1U) zmM>Fp*96BTY^XL;kSyOwSR>g+`P)f+^Q<@&sNhbP2EEVvck`gK@ttL^NnRxVKmhh8 z|11WKc9!o8*H@5ITgu)3iEohuK77ZmRMWOjOlxO-n2%5)SV zaiHbPpVUza$TbBqGFzkX3#6P$kH*c_-qVDZG9?SkUIOUPW#5DB=Con?X;vqqx$zlu ztZPRP>;h4JVcWh38MzP$aTfcDa`Rx~#c(A3!UHESNpK&ytak{GrmfInRbG|Yek7f} zgF|_Lf%*_1l zjqTYoebwB|zDcWE9a|`{#o6hf4nA^qa{68`Sz=9k0;M=b#p9=#kZA$n&v0Iaqt0-t z!l`b(wjpzpgpbMfdY|=#HuO#z%EW%+Gis1lD`6a^nIvaQx>svJ{WK$)hlaZ*WN19X z-2#{q_mezAhn-bQ##XQ>M*)pZbw7;-%e7yy0qxv4m%H5LAQvwE)$#j*!MzkjLm`)b zYQ9ZTd081CCv9*2X}A%0&y)>WkN+D=njc;5`YwS);pH4kzCrpt~%`dSc< zcZB%=s|ou`?A+2_$wd$0V#jDUO{C7%B2|l^Kk6j}jLc<7H=O~YL`M4emxn-!U_9w_ zBYw$@5r~s<8-j70rI z=)}@b^#a|$Sz*60VgTSss8JO%IVQ>dH%RB09`<*;4gQlQrl$o0GXjkN;g0=xVdxmN z^LM*@nxLe{#(li}-z6ddVEuar001sh1aCEglL*t*;NV4xZH})XQ)} ze|h}(zK<1WbqMJQW&#vJ@jsM?I)n=dPT@bS{$S;C{1HO54#5_|{f~9lkZ|z(V<#N{ zLvf%(z#w`5^COPuUeYC;QUA4%7f3kRUY7us6+-^5WaacK-sjPbAC$ delta 14485 zcma*O1yCJZvo^eOcXxMpcY*{D65J&?Y+MI|yX@fZ8XST{fB*>)Jh;2N1Ooh(WB0uG zz4g~ORl90ducy18?lrS&SI_Fl4yd*`C^U5?7+72o2oVG#l;ul8dky?`D?J!J9c&B> z0{#9Wf3}Wo2|2x7&R9=CpD#&lZCUTt&@!ywYHWVn;^A> zII^g-wY9Bb+lRMjQeM)af2PZe-Kc~c~J-|-F&Sq}5Zo*Dxj#h5M?iRw% zfQyyW-#Wtoh714U=}(EbgRPUj81*}McNbv}4lge+b}t@wXRr+imynPU#~-uQe?cuR z{=vC;fF1roSz2&dIavK><;KCq&c*RJ_BTb~|EBfdtbT+4&B+C9PE&lhT`FjP$MgLew3$WE6!LV5Z zX6{zv+?-t8Y+T%IJX~6wLc%=!!dyb49RCaW8z*XMA^gV;HzyYl8z&bV_n!*f!n{JF z9Dh&#>B7p=*4@_0hRxCg{D&)XbuB>!UQQJrZq2`J|A7Gh4f#(^3m*#yD>w1Ksr+s5 zFY%vP8!IO(u$j9vSX|lG9Bifb$4kh;!_H5uVrHSHscYgb$WQsNDH9j4vz?WNJBJmw zkQp}@FB`yZZf<2Mz|F?V&&_3N#m{CYB*4clXu&3E&B@Pi`4`pyar39!zu#Ckx8E;- zo4c(Ay2Ivh z>$EMo0=3fGrw;v-njeON{&Dk@D2R$l0$q!)vm(&srV2ls4#eWxQ!rMCesH7-W=w#H zso)i14Yg?*OUj06y0S+GFI}gVV4W$GwD;iC)5cUp)&q&;DdKd1IoTaTVuM zt>yElpZ6^56v+#C~M+IYH@H1Xs$bHz^;dWzzlB-Yk84YZ}+u;3%VzGPruIqzc&oO~PL&W>hyP9%9 zJAh|MMBni=Tq(!Dsyz~)1KMCcho&{YS^ydG{OV{1qGQU1$i?^dpCXOpCro~ zwGQm}2yT||Qz8fOzR7rpxn!a29{5=SbMNkOV??q0I|-bkm{KO<*PCMv1c@Ior22CW z7c5W$V~2xWYWb*?rLlh1NOn7J$>iT{CqleK!|70rqlzgXf|NJnBbjy@M6s_H%1F7H z&`nDtE+jC^YbHWU8d23L3c(+U>SR~3$+;69`59ucSLRx4p11T957a66QoF=}Q*xL7 zcd|knoRTed)<;KBv;6KAt_Rin9<$bIRk3XPC-Ql$?yF0K4A|e*YMU_4H=2JfOHgDw zHK4jjYHjyWt5Ag4AgFJV*ou^{_ca z8=Bq&J6(mad8FLL?@5Ea2YeprvxNsu9Pt}rWJ=tSv6gBqENY6jste9EPAubP)-zcn z6@72?Te+WsRYKAn+Pk>VSZi8!WVF)KrPs}s_Ny;Y-SW56`$k4>r#PJe;iYlKN`Xq7 zUD)%Hc$6e&SfOmZ{rWrmTGUh+7yl=M>wEopaxvQ@BN?}Tm9|u$JeFi>USA}uo3+SE z7&RvJ%i(P9)&+uL-%dNeVztEl%exP$Fi~|rk78pbo}`UmAh50~xI+F$uCG#b+%5e# z4(jtJf8O7Gl0`XxjP6YXs&E5YU5^JVM+l|dKQagtX2@08F2(WgNF6B}7>d~Awi3wIix-4;HiM(J(vrVjxLL+g!B z$_nJR&p+&bG(S6ah5j=hQtn7=vIF?oQ5>Is@^fwWAmr8Ao1OHx02q6DiW4@gWcPwKF_=LBxWY}4_TR_wuouQ^lp--NY3f#gdRV6~njdecY^?+8@d2qit zDEtOHPRdp}s6KoEZfRKRE@c}dW-dt9@wNzu+r^Ft)iN3@hOW6eU4d;BB* zlN}_BNw2WIjV3C(+_U434xDi}0@)@_ECfX-mgBFK&R)-yOF?56U?B8<{wzcRc)wUR zqlz;uiH`knmbWC0dv7T=vU~7wda^%G0ZCSzTZGzxtXS#Th-s`RKSZtckH1ycGyLS?t&z zsd2PoJ(|f0w0p7CoUajEQ>9qa8EgPm8U-XZfBZ#f5*~x9ZsJ(l(exvlW5oSwWNGrr z>5~snhN!rBJjH_G(yB`255U(PTm?T~3C@Z*nfj19PXCqrbHok~H$5a^LAxQV#gQVl zIjbp9H{f;4$AXO0Qu(g-1$rUe;Jldk3#T#1!twqA-S%uod1s=WbyRL^Nc z;#dA}3=K*s)bN4j(hf^|yu2J%@ho;5yv>D5Bwj{&dUa}L@uK0kA&9}lVtD(-!!^iz zZf9>_c7`w{#!ex#*Xn{|CW6XyKb_jeDIv8}R!7D`4dewmHPb%|gRBz5zvG8}j%iJI zfT*;rXp=R)>p|?*d|NP+hW%9*9mG(D>6`g4tTQ z#jQHXR8LiWMl9=jBs6EaTfSI)Bi-MRnw>6d&snwpRNLrrdH9%WwEAhGtcSjF#iG4q zoZJ&}=SukJRIb0sLmHc;uOr5za1CJ8@9Z}3W3v?Y^X^c-_+EI$$&}HTAoZ7RphSan znD^ev{@no}{4+yN;sw1XS*SFy^kO`(Y$>ne+2?xf)!x$Re3iS=O58W&LBB`OAHv-X z=CqGI^#b3Q`#-Er*dC^+{L~+}?*GseXQKCs1OE;8?O}&ePlJ<<13#qSGgG#V*6liy$q{) zL1^c~Q*dnhU@_CG2 zG=TX0OCzt&yI$kx#XGwrGJFpHR&nw%^{)QHD=u?Gy;NJ{%Gc=R6vM;pb;{xlBX5cx zuImixJMxDI9?zeOHXDYKC%raK_Ftaw0OnuS9QEqF9uv4t*@5q~U-Ko0nNOfU{`-U#L*i3d=;s|5Tx-6_JTtp%t$b@pp31jTe!LT-z7@C?dPhzh}!k z;{ls=-%W4F(lOJ_V64{2y`{+yRI%IWZFG)PZk(g<9&>pL|{`PSCzp(J^j{VB>u z{d3H}1stB*!c4HHE)-oXqECi2jaCMJ8EiFuA^c!+S>RuIGxxNdP? z`!_e09JvcbE}}(dy2z>+a@e^71=Su5cDwH`r>7|$*R&QJZiF*V8XO_zT(#vsrR-I+ zjS(k%#kl0=Eiv_#+Y$MoxD5Ife+@78qEbmiGTbew2HO@f=gl#hgp4r>9r$1qE^lJV zC^dCMSPcb*UK%n?DMEUO68lZ)3+Z0mk|_`D-5%P6K@!d&o%k@2Iy5&>`YT^Bx!qby zJ!PBU`-4Fx9bbD%{pIqhVx6nSKs7GXnF~cvQz{U0H7I9(| zvp`0XQiun#HED{lVq&g8oyCX^_hoLX%P!1gsq_C>gd&>pVG3Zp8c{DL|1b+>X$CJq z6UkkGB~>3!gPk0CMg0X4*m?@axs}mswO9?;OrY6_tiFz^K7ao;tM>yr=K+*5wnng~ z8|*u|0v%MUIB7YbB+6>F0|j1W<0q`2=W(|@}JZiza!}<{cK?acXrGl zE=*!=9c^NppHV|m3XGVMTm?8~=Gdx=OqJH=?MjxpvUY#djjYW|E?v^TyE5b-1&?R$ z7%QiEe!$}rcYxP;3#HOg`Y44|7|RK-w^5dt6V{WJfg+I%_7hXMMjUIk z4vYL~)WmgD$3@Wm`7}(^gnuTWC3jw$hy4T8y!iM8KGZvK;*2_u9&NcD++L#qeSBam zg~jPC-UC%d9^qY`g^md1ey#!t<9@2C;E_nUaWMfOF7^pkzoP2zO32Pi#2G$~_a zt7J2l0B&U>PPpfb>}2akbih?;BCP`sBF-0+#i|v2GNrry!o{Pv ziS5D;wixG7mFy(f;-Vj?y+}_7i9ut1A>O(!dbrqR@2JnuI%;9=#Kq#XyUI^ZSsuy~019~54mel0gh2%a8;d$z9{JUo*(uMv z#P1NZz(-ZMcoFD5Vy0mqmM;riTw*#kokX=kEVS`)t51&8W%{euK;a!vs^{la#MK z2&@IxPMZ2*)^8;oIxS{aOYDE&Ygek&9D4U`WYqYAYa84W%6~d0k&P4IVa+Q#%)HRp z)zI-wEWvk2%SOz?Y^WUd)<1(_N)AJ-Pb_kA5CI7d6*spyOj%u*U8XVvl`cwpumstI zbcc$L3*{AVhOFckxzurupqvxUp1VqX8_?mwPWF)m>-O=Ue~~rd2icNkML~cRDH13h z90nTN*ZB$y8jXE%u0>}t3YHBrqIi9(3Z?!$6Y5tm&U@igBAkmf(JM(JuqMe&g#DUs zw0BZ`@>|Pv(zk9Cb-T1cv}nFpAB0aGG#q0P5wF;#-NZ{quAM`6pW9XasQtdO&jOX@ z)|dZ(3CG3T8>VZfy!U$&m`=-UdRuS&xor0lTB{+I++# znU}CA5+4f=C0Z^*7X96S^oBj=2b<8YH{IzOD8}gzWK^{`Y^zo&8_}sE<5s$ zcB)o@4?fFVjoMK#!=o1yTb0Q4^($Z)j{{rwP6|sb7bBD;+}cMCB9pjqwfnwiM$?Zu ze-JHcYeAQhZ<3I@BovuWm_8{IYAXF#CQJ90y7DroAy^#!u6iuA5Gb_gEs!}LFy-}$ zzYW~(cNO$qNjR9lJSmPyA1;MDxeIkP4ENt!SVhCy`W&p$VNGNXeeFe8q6Y*>OEsT< z6?V7eEU+L(@!BQT)L&W>iT-eu?r;62hm*V^%m6A3sB0{<4&LN_)5;!vI&dS7bY(~J zqa0cN&6;nj0fpaK+09O*s63ye@vVXExrX7A*~At1FfRTt-Egjgf~jTL%N;5QMoKs( zynaUGsYCByC6wX}jk0rCIy%6_bBgqlw&n>^AC>_g0cF;gT&NmWY$4OvW-l1bskEp# zVtr`|K5VEdb9*Ke4vKzMw?|s!ER|}JjwR7mCQ&#fgs!&*whu*C`TuGH_(`sU&A>z(kV_fA)Gmbmp?^*4rUNF5zmMx2Op(QVpKX z#?oUk`EsJCO4Ncgugw}L2+D6>vJ~hL$iWF@8Y=$O3o+;=Pd+1H_3l|e?+JP-q4{Ls zuMKVuBhRWit4u4bZtwxx6(ElB-*DjIwCWW!q1<|I<7V$ruFoD(e!T=@^+$f`5x!S* z>$wl6pj5f-x#id~VZAPRJyZ(4p!sq__3I4birdD(9siLc`b<7hS2Zae%qyfFJj3!U z0QVQa+FVayPtZWEz(MH1q;sI#gVMVvrTpg4&Xb&?8p&U$Q}nt3?r!Scyj6~OF74#C zb4ylP__MNInLZcF-ra@OZ&{7qrVC5$euaUq5A0m0F+>fN{4#x7sD+Yqpou!D74H1? zzB9few*D%*lHp}5BlT`$`0E(O#R=S3|M5(_Jx4PKx{WlKl$z;Tm~M_?h{V&IZ33FM zOwEMdM3!v-ZuWX6;6q*f3(B0Xs`lHf&0eR6>}fdx{$x=W!DQi;(VTK_{_a%?7gK?g zel81n=)DHmq$^%28#vX)r45E69wpzd88p_iPt;&`3lt%>1+dQcwQc}f63M*kBc%t2 z<@^>Ucf1!Zd|TUb#ObIG=}-AX%;KHQ1OL!)oe8`MdGlimK(w?_lrD);NR4ZjqQ8u^ z!!o>>*OvUdwB52_Q0iq~_&v>hrdo0Ul|$;JCUIO6rWv71TQE+}P;q1o?p!6pp6aY3 zIVIs$rbk733#*MN*<4p1(M>oUwuFbm6%k|{t&e_%cH%Xrf`i_RqQIn{#!Q>8!hnPN zR=vB$+bV1Vz4mhW33&u%^H%{7w z5716~%Q*jmyjoGs`T%<_3kx3oyK}9f*A}<@O6!lqOCXcRc&C9X3SD#;9{)S}mQlx<#RTAfZx5*mQn^e4IK-_1JeP1|4nz1iiHY)}$iAQPwW(g8s6)fpTD>ioY%c4E zp2l%v zdJQOdW@m2uDo>z`(%ad+A#kQ7Cq(Y~c{r8CH5VsWGVaU5Di)o)&yDXv$=FF|>y~1( z7<{R0c-MavrJAbOuGA9J8j!cS3mdL6w~~_U<>e@&_Uk+li_)Y0D?n5``#)T{MVulSa;S|@Bt=`b-!Gg$F!*q~>T~>?= zYSHa_?x-NU9~9p9;Vv4DA#=CiHV5jVyAoOZjvDvCdm9civhEbrSjvEl8{9am{~T}- z6&ocoF{}H4@PKrlX*Eso({{v~)~r~`9KFq)83x_L0Qo8+QpcjR=n9xC$@09wS3U90 zF5j@~@0VVk^x^X`A-6>#ZYIqlk*|NLYtO*w|70jK*tc1i8U5ZBR!CQ>5d#eCoIBw5gUj~%G?vTv z>xOxvw~91kiO2SG!)k?!YNdcrrT7r9&n#uXI>7{&o|=AzMSd#}LPQ50T$eDZQZ1MoEOcl0gdr_}5-urO z_6OzcX%o|O^OvwDGD&n`j3t5rTKXK98jkxr7CqcL^*Osh)R7PphSN_3Z+O){a#!p_ zMrg&Nq#4mZy}?FTGC?E+)xKbe%Q`KoAd*QeMBS%jijTYP^UCZ_ucBajIy+8lD*5fG zSf%M6LuxR@1+(o4^?=>#wTF-kHu@Vcne5O}ep~eHP>+zdwYD-~($v^i8irv5NmbhO z04aX_!gZb2A;k8|;51+>aGh54eff=4;yC=GnyU2fAXWqVs-2Jvb~pT89?vF$-UdQ3f9652izFjX)AuEac3iw#VflSvlbNd2ci}2s* zmhsEBga-A($hqBTA8s&#%jT++f$`uIZn&uert)cWcd$F z>+pcR$EKI9m26jzJXhAOkA#oOofCr~&S4o^@$pEbzBeiStLBONOCBNq&z zfdxEg%OI+$LmW}>Jn?@4fPWC`1O5H|K^^mpu@}cqLDE(UN%T*g)(PatRzbREm0GVP zVu$f*FQP;o0tFzzfZ${W?jjCEu675c1$hq;stY3GyG(g?MJ;exgdt6?k3?05g zrA+yCv}w{Ap$k#PK?SHsNI+9t&`eaXSalYL4>XRIEdeu_YR0Ez{f>1KDfK66WC9U; zx=B z%!}{|5sOdlBTG8GPM!Ds@3F z_}OndT+w};!BK`7y@m=79&0S-DEYN&{F(X_axLC4?J7&d*x-(G^A1r8AS zW1|Rt%{|JYt~))Ioxs7BGt5-wQ`zwV7K2vTBeNR?H2o*259>C*eLcE&EWW1Hbdx2_ zuWfXK!8dv9_qKc$^+cFzLkV*^`-*kxt!{Fdx5+TglX#pxSg7}|APH2|cD3g$;2MUe z!Z0vHN|N(TQu`1dBlTL+2VtM^I$bsf%24jweKV24CH}WLW=X#}HRR~B zFl>HGd@9WS$vdWKRFLTkLTWz8_A4(R;f^lWqWnAqKNpvxYnjSQ_{$nH)T#S{4PvK>iiy&btrGzfO}a@-_%NV)PhZ z?Bggd7~pV^z$h5#9D&WxFKV?YkkK@Flkutmc1s1+61VT6S4$))12=KB>QNeEh|~F zt6cWAQqL$PluxLOE9W9#bQhM4!Nj3~l$R;VA^bc13gX!H!q4~78i>QzFQMP~synmR z;NMjsO0AXWzX`C58_y6PD`7IhIaVXT-E_Vyd7^P9&MzTps;8zUjB*>Xy68X{b4pvl zhszTsO!B29VS{(MArJZnV8pi$uHw;Q_B(19VYayu_RqtBLsj9{nW9T&9z!LXp=SIy z&GJ)MQ|=Y%+sj|0H7B&YpHhDR#Sfw%9)>Ixw!Wv$PRSYOUWw=(wjq4^##|}soY9sV z=k{ISoaZe@2Ao(~{+i0GzPFp(w9Kf97EUumV?*0WMod1>s*A)lz`{KoLdPo&lKeFl z18QD@yuHQYQyxleRR;HFs8q`GI-_IHQ|7N)k%!EQxMIR{H{R>M=45&(i+!LsuuD1! zUD}QO%qg?ct(SvorrPOH(Gzu!H+FoGrRQP{430aDi89!&iE&l=D^miPrVo|IPjpK< zB1?@AG7rXgUSHN)0SW*qeq1^RYQ_`q?TKN4K_7<^#~l|WmRjW|Pwimu`Xs?G!cvu_ zIO51Fq@{Ph)Ad*GtQ~heMbkCRI`1Mp%zQaX6ThbILc5mdh;hOq7<*>Ce0JH9^Q5Fn zE4Zn^Wz94H8;mxC0Bb1by;!_nI#fgX;wLhkVXbX^ZwTHXU~$y|jJuvxOPkj6OPU?p zEqxM$?@{P*U$oxhWRZA1UyfGC*!CTq1Vv9FEqdVz8Ca}Ab}uQup#z=8`IoFkgzrhH zC6sev)a5|Ero46ZG7-2qX?OM$ik(G~Vu9vRt+m6LxXD{L*xc3|3+h?~NhvaV(XXV-jyvmVX3P#uQwJ-8nZ_s{A4q(%*=C44QsSes zA-TU_&lY5SrGE3GIR9X)n=6YrXy~B(MCe@@)Ul#GfXaaIHtDDDtTVJI90izR-)x8< zB&lHYtNVlv67M~p>i0AaO@+LtXi2(GlURijYHJS@dv<`+W1-}h9Z2XHHH%aQO-I=2 z?6=i>WoyGi<&GpHE;8v@j!-lV(v&j>i|zZ~I-ZT$O4v#vayREQF>?2^Zj!sd`1E$4 zP+!FZ#PZ9V{H|8&<3k5*F!oXdnZU>?~jR9@eQ&j`cmryjuBh~V+8&QJk!Xsu|f^xp|OZsbLhKSGB zZ;r@ElHeI8XU7`UF_3X>(;3U_e_AO0gqBECfpI&G719^w6l)L1K=!d}_;Yy;w9jDe z8)sUtw0XjdjAlr@{K?MyK*)pxT3)dh4Vf>uO>V#O=iC zrd~Sva0xU?j!@h~eAx}L0Oaz9;Nh9@ACiHh*t5Bi4vJTw#U+yG?kf|u8meYy-*68z z^9{~y2UDQDaNl!lQ)2YPuvGSBZ&x*+r5nJBJocDDyWz6P`y)*Cn>R4 zc3@ilOdWqI9kt4RXwSMtx1@dw(f}HP6wvvn*K9F#+0o9TM;t3er=O7Wx zQQ-NG!C&Fj>-r3M5=Yypqkc5&9d!A5;bXZW6!>Ez6kF0IKz$;LoTBWa-I@8R2YzZF zDp9;^4U3ec9twjaK)}|}d|Rcg8ESyRf=?VY4)aygo4j|5;~^~lJHJasH8Xk3)-14D zl}|KrUZ=cEX!<}4fFq9t2N#q)MMW`45BE;Iwoi##Si5&|I!!SxHhS(fI41;h(KZs+ zw-n)(%&j>ls)=H{zr|DZYcF!wVFnU-3aKx3q3i*V^gnJvk-k99_n$t}#b=sd82Tv& zx1A;~c+Bd$17en`;L_%?P2MMXSBiobfDsH!MBn_nStSvGipf{-jZ-++J0gT$eN=S% zan=`7!^qjQZhdx6_={{Q%7F~ALx!@tjv(Sk|9t&~m(4BVNpsL!9tyz<{Y@Tq?Uipn zK?I0YLxaZ@?>J!R1H`+n$GiE?$%kX%3#ehP>4!TH3Wn~7&(_YK*do;5*G3{eb;F*+ zSnd$3)Ht64yhNTZHa*Pz+(Ak{ls?1-f$A=bLP19o&oU$TT3(+4crpHlX-y9Cbl?yb{x3VYtEc7fbfY5z{$^v`+^6hJT z)_wOqZ}x<{#fN4_99*)5&FtSZB->+14jSf)$@K9wT;)3>+y%muz^7x>z%Ecbv z8V5O;1?hep0C9@jxGeRqX;}8q#DeiuEm2?u16Ia`-a|)stzpp%EE;*6&C+dBUGF}? z=T8Uez^e(IZ0oRc=F;j`)*8ldwRJc41gAdg&K>48DWhO9}5xQ)xra{LEm<0RtuQ zBfhb^c<;c6(V5PjWpK-_jc#OyUr776_|D5K$j>#~d$!<45oI_6I{1cCnfN&vMX>|} zhDR?Pv>>a2bV~z|m}Ha=_|=eka5I20Z2>(Dh8zC%5liXvJAO`%PG~#)sw#UR7%%;D zYg4UTeef(}4ZbcgiqP|jI({@8u-x)*i(TVc1C=!~;H~T)5eSBO((U9%tU?8iwND}7 zbN13BGJk!`x+Wg!Ok4i!QG!v<88rKTz(itBF0Fm?kf=vK3@T_bTwgmQ_ZrYGAb#B< zZHiJ=mAu)lT46kgqChcW4w4)1ddwt@LxPYb_Tf;w%u3&~Kb@0f5-ocWB#Y;N0fGAA zsdE-|sXt3P{|ZJd-nR8oo0sl*;9v@nOoIwaHWs}Q*<8R(_kL}tJ=BR!I8M(!DI-CQ zrmnD&4miFW!19U;#CWeFn*dBYfhy1oEh%GrId+jNer_KXu7JW>OYcEEy0cg9S0T^s%)s04+YrID);41A^Z3}+ATQ4VXgb_MF#tWvmqwATtsQGq zSf}dm=J25NU#r^}>!#WsCCn}s9;v)RWmSwS7b0~nP{;)+Q1;(XNAck~V@W#*DHpz4 zJP8f5!@E@MSknhs(7=G`ePJ4Npip7{|7#BalpIVQ;yC_Ym3YP&;~L#jnx4G)eGFOg zm8xuwwCVf*XcmJss|dsVZq8sMGYO{??w+#HFbo)$}`k zKA|k1^uNzkqyFm@H3&2d3j(=Wx&Q9Nar@oNrmh49jSIs4yUj%U-=(78XRiO}T0nvX z>l0D_Hvd0WBM^x2e-I!L2#pTXu2265=-+aRe-A*`{K*Dve?-A)KmuLF4}nr8hu9k+ z|07!Bhd3MJL)26e(f(xjcRqgOaReZ)s^pMI1LXg5L;4?XlmsA9syL8NRkZ(QWz~=b zI!plKY)Joy$KS&a0SJ*ADf55k;n+a diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 6b026f287e..8b9e7c49cd 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1785,103 +1785,90 @@ int commander_thread_main(int argc, char *argv[]) if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - /* - * Check if manual control modes have to be switched - */ - if (!isfinite(sp_man.mode_switch)) { - warnx("mode sw not finite"); + /* + * Check if manual control modes have to be switched + */ + if (!isfinite(sp_man.mode_switch)) { + warnx("mode sw not finite"); - /* this switch is not properly mapped, set attitude stabilized */ - if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + /* no valid stick position, go to default */ - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = true; - } else { - /* assuming a fixed wing, fall back to direct pass-through */ - current_status.flag_control_attitude_enabled = false; - current_status.flag_control_rates_enabled = false; - } + } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { - } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { + /* bottom stick position, go to manual mode */ + current_status.mode_switch = MODE_SWITCH_MANUAL; - /* this switch is not properly mapped, set attitude stabilized */ - if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { - /* assuming a rotary wing, fall back to m */ - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = true; - } else { + /* top stick position, set auto/mission for all vehicle types */ + current_status.mode_switch = MODE_SWITCH_AUTO; - /* assuming a fixed wing, set to direct pass-through as requested */ - current_status.flag_control_attitude_enabled = false; - current_status.flag_control_rates_enabled = false; - } - - } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { - - /* top stick position, set auto/mission for all vehicle types */ - current_status.flag_control_position_enabled = true; - current_status.flag_control_velocity_enabled = true; - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = true; - - } else { - /* center stick position, set seatbelt/simple control */ - current_status.flag_control_velocity_enabled = true; - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = true; - } + } else { + /* center stick position, set seatbelt/simple control */ + current_status.mode_switch = MODE_SWITCH_SEATBELT; + } // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); /* - * Check if land/RTL is requested - */ + * Check if land/RTL is requested + */ if (!isfinite(sp_man.return_switch)) { /* this switch is not properly mapped, set default */ - current_status.flag_land_requested = false; // XXX default? + current_status.return_switch = RETURN_SWITCH_NONE; } else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) { /* bottom stick position, set altitude hold */ - current_status.flag_land_requested = false; + current_status.return_switch = RETURN_SWITCH_NONE; } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { /* top stick position */ - current_status.flag_land_requested = true; + current_status.return_switch = RETURN_SWITCH_RETURN; } else { /* center stick position, set default */ - current_status.flag_land_requested = false; // XXX default? + current_status.return_switch = RETURN_SWITCH_NONE; } /* check mission switch */ if (!isfinite(sp_man.mission_switch)) { /* this switch is not properly mapped, set default */ - current_status.flag_mission_activated = false; + current_status.mission_switch = MISSION_SWITCH_NONE; } else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) { /* top switch position */ - current_status.flag_mission_activated = true; + current_status.mission_switch = MISSION_SWITCH_MISSION; } else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) { /* bottom switch position */ - current_status.flag_mission_activated = false; + current_status.mission_switch = MISSION_SWITCH_NONE; } else { /* center switch position, set default */ - current_status.flag_mission_activated = false; // XXX default? + current_status.mission_switch = MISSION_SWITCH_NONE; // XXX default? + } + + /* Now it's time to handle the stick inputs */ + + if (current_status.arming_state == ARMING_STATE_ARMED) { + + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + do_navigation_state_update(stat_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_MANUAL ); + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { + do_navigation_state_update(stat_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_SEATBELT ); + } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { + if (current_status.navigation_state == NAVIGATION_STATE_MANUAL) { + do_navigation_state_update(stat_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_MISSION ); + } + } } /* handle the case where RC signal was regained */ @@ -1890,17 +1877,19 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); } else { - if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); + if (current_status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); + } } /* - * Check if left stick is in lower left position --> switch to standby state. - * Do this only for multirotors, not for fixed wing aircraft. - */ -// if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || -// (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || -// (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) -// ) && + * Check if left stick is in lower left position --> switch to standby state. + * Do this only for multirotors, not for fixed wing aircraft. + */ + // if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) + // ) && if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); @@ -1969,8 +1958,8 @@ int commander_thread_main(int argc, char *argv[]) /* decide about attitude control flag, enable in att/pos/vel */ bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); + sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || + sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); /* decide about rate control flag, enable it always XXX (for now) */ bool rates_ctrl_enabled = true; diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index aae119d35a..2e49354714 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -54,421 +54,329 @@ /** - * Transition from one navigation state to another + * Transition from one sytem state to another */ -int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state) +void state_machine_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - bool valid_path = false; - bool valid_transition = false; + int ret = ERROR; + system_state_t new_system_state; + + + /* + * Firstly evaluate mode switch position + * */ + + /* Always accept manual mode */ + if (current_status->mode_switch == MODE_SWITCH_MANUAL) { + new_system_state = SYSTEM_STATE_MANUAL; + + /* Accept SEATBELT if there is a local position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { + + if (current_status->flag_local_position_valid) { + new_system_state = SYSTEM_STATE_SEATBELT; + } else { + /* or just fall back to manual */ + mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position"); + new_system_state = SYSTEM_STATE_MANUAL); + } + + /* Accept AUTO if there is a global position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_AUTO) { + if (current_status->flag_local_position_valid) { + new_system_state = SYSTEM_STATE_SEATBELT); + } else { + mavlink_log_critical(mavlink_fd, "REJ AUTO: no global position"); + + /* otherwise fallback to seatbelt or even manual */ + if (current_status->flag_local_position_valid) { + new_system_state = SYSTEM_STATE_SEATBELT; + } else { + mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position"); + new_system_state = SYSTEM_STATE_MANUAL; + } + } + } + + /* + * Next up, check for + */ + + + + /* Update the system state in case it has changed */ + if (current_status->system_state != new_system_state) { + + /* Check if the transition is valid */ + if (system_state_update(current_status->system_state, new_system_state) == OK) { + current_status->system_state = new_system_state; + state_machine_publish(status_pub, current_status, mavlink_fd); + } else { + mavlink_log_critical(mavlink_fd, "System state transition not valid"); + } + } + + return; +} + +/* + * This functions does not evaluate any input flags but only checks if the transitions + * are valid. + */ +int system_state_update(system_state_t current_system_state, system_state_t new_system_state) { + int ret = ERROR; - if (current_status->navigation_state == new_state) { - warnx("Navigation state not changed"); + /* only check transition if the new state is actually different from the current one */ + if (new_system_state != current_system_state) { - } else { + switch (new_system_state) { + case SYSTEM_STATE_INIT: - switch (new_state) { - case NAVIGATION_STATE_STANDBY: + /* transitions back to INIT are possible for calibration */ + if (current_system_state == SYSTEM_STATE_MANUAL + || current_system_state == SYSTEM_STATE_SEATBELT + || current_system_state == SYSTEM_STATE_AUTO) { - if (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { + ret = OK; + } - if (!current_status->flag_system_armed) { - mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); - valid_transition = true; - } else { - mavlink_log_critical(mavlink_fd, "Rejected STANDBY state: armed"); - } - valid_path = true; + break; + + case SYSTEM_STATE_MANUAL: + + /* transitions from INIT or from other modes */ + if (current_system_state == SYSTEM_STATE_INIT + || current_system_state == SYSTEM_STATE_SEATBELT + || current_system_state == SYSTEM_STATE_AUTO) { + + ret = OK; + } + + break; + + case SYSTEM_STATE_SEATBELT: + + /* transitions from INIT or from other modes */ + if (current_system_state == SYSTEM_STATE_INIT + || current_system_state == SYSTEM_STATE_MANUAL + || current_system_state == SYSTEM_STATE_AUTO) { + + ret = OK; + } + + break; + + case SYSTEM_STATE_AUTO: + + /* transitions from INIT or from other modes */ + if (current_system_state == SYSTEM_STATE_INIT + || current_system_state == SYSTEM_STATE_MANUAL + || current_system_state == SYSTEM_STATE_SEATBELT) { + + ret = OK; } break; - case NAVIGATION_STATE_MANUAL: + case SYSTEM_STATE_REBOOT: - if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { + /* from INIT you can go straight to REBOOT */ + if (current_system_state == SYSTEM_STATE_INIT) { - /* only check for armed flag when coming from STANDBY XXX does that make sense? */ - if (current_status->flag_system_armed) { - mavlink_log_critical(mavlink_fd, "Switched to MANUAL state"); - valid_transition = true; - } else { - mavlink_log_critical(mavlink_fd, "Rejected MANUAL state: disarmed"); - } - valid_path = true; - } else if (current_status->navigation_state == NAVIGATION_STATE_SEATBELT - || current_status->navigation_state == NAVIGATION_STATE_LOITER - || current_status->navigation_state == NAVIGATION_STATE_MISSION - || current_status->navigation_state == NAVIGATION_STATE_RTL - || current_status->navigation_state == NAVIGATION_STATE_LAND - || current_status->navigation_state == NAVIGATION_STATE_TAKEOFF - || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { - mavlink_log_critical(mavlink_fd, "Switched to MANUAL state"); - valid_transition = true; - valid_path = true; + ret = OK; } break; - case NAVIGATION_STATE_SEATBELT: + case SYSTEM_STATE_IN_AIR_RESTORE: - if (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_LOITER - || current_status->navigation_state == NAVIGATION_STATE_MISSION) { + /* from INIT you can go straight to an IN AIR RESTORE */ + if (current_system_state == SYSTEM_STATE_INIT) { - mavlink_log_critical(mavlink_fd, "Switched to SEATBELT state"); - valid_transition = true; - valid_path = true; - } - break; - case NAVIGATION_STATE_DESCENT: - - if (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - - mavlink_log_critical(mavlink_fd, "Switched to DESCENT state"); - valid_transition = true; - valid_path = true; - } - break; - - case NAVIGATION_STATE_LOITER: - - /* Check for position lock when coming from MANUAL or SEATBELT */ - if (current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - mavlink_log_critical(mavlink_fd, "Switched to LOITER state"); - valid_transition = true; - if (current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Switched to LOITER state"); - valid_transition = true; - - } else { - mavlink_log_critical(mavlink_fd, "Rejected LOITER state: no pos lock"); - } - valid_path = true; - } else if (current_status->navigation_state == NAVIGATION_STATE_MISSION) { - mavlink_log_critical(mavlink_fd, "Switched to LOITER state"); - valid_transition = true; - valid_path = true; - } - break; - - case NAVIGATION_STATE_AUTO_READY: - - /* coming from STANDBY pos and home lock are needed */ - if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { - - if (!current_status->flag_system_armed) { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: disarmed"); - - } else if (!current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos lock"); - - } else if (!current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); - - } else { - mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); - valid_transition = true; - } - valid_path = true; - /* coming from LAND home lock is needed */ - } else if (current_status->navigation_state == NAVIGATION_STATE_LAND) { - - if (!current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); - } else { - mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); - valid_transition = true; - } - valid_path = true; - } - break; - - case NAVIGATION_STATE_MISSION: - - /* coming from TAKEOFF or RTL is always possible */ - if (current_status->navigation_state == NAVIGATION_STATE_TAKEOFF - || current_status->navigation_state == NAVIGATION_STATE_RTL) { - mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); - valid_transition = true; - valid_path = true; - - /* coming from MANUAL or SEATBELT requires home and pos lock */ - } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - - if (!current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos lock"); - - } else if (!current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock"); - mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); - valid_transition = true; - } else { - - } - valid_path = true; - - /* coming from loiter a home lock is needed */ - } else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) { - if (current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); - valid_transition = true; - } else { - mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock"); - } - valid_path = true; - } - break; - - case NAVIGATION_STATE_RTL: - - /* coming from MISSION is always possible */ - if (current_status->navigation_state == NAVIGATION_STATE_MISSION) { - mavlink_log_critical(mavlink_fd, "Switched to RTL state"); - valid_transition = true; - valid_path = true; - - /* coming from MANUAL or SEATBELT requires home and pos lock */ - } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - - if (!current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos lock"); - } else if (!current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock"); - } else { - mavlink_log_critical(mavlink_fd, "Switched to RTL state"); - valid_transition = true; - } - valid_path = true; - - /* coming from loiter a home lock is needed */ - } else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) { - if (current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Switched to RTL state"); - valid_transition = true; - } else { - mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock"); - } - valid_path = true; - } - break; - - case NAVIGATION_STATE_TAKEOFF: - - if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { - - /* TAKEOFF is straight forward from AUTO READY */ - mavlink_log_critical(mavlink_fd, "Switched to TAKEOFF state"); - valid_transition = true; - valid_path = true; - } - break; - - case NAVIGATION_STATE_LAND: - if (current_status->navigation_state == NAVIGATION_STATE_RTL - || current_status->navigation_state == NAVIGATION_STATE_LOITER - || current_status->navigation_state == NAVIGATION_STATE_MISSION) { - - mavlink_log_critical(mavlink_fd, "Switched to LAND state"); - valid_transition = true; - valid_path = true; + ret = OK; } break; default: - warnx("Unknown navigation state"); break; } } - if (valid_transition) { - current_status->navigation_state = new_state; - state_machine_publish(status_pub, current_status, mavlink_fd); -// publish_armed_status(current_status); - ret = OK; - } - - if (!valid_path){ - mavlink_log_critical(mavlink_fd, "REJECTING invalid navigation state transition"); - } - return ret; } -/** - * Transition from one arming state to another - */ -int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state) -{ - bool valid_transition = false; - int ret = ERROR; +///** +// * Transition from one arming state to another +// */ +//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state) +//{ +// bool valid_transition = false; +// int ret = ERROR; +// +// warnx("Current state: %d, requested state: %d", current_status->arming_state, new_state); +// +// if (current_status->arming_state == new_state) { +// warnx("Arming state not changed"); +// valid_transition = true; +// +// } else { +// +// switch (new_state) { +// +// case ARMING_STATE_INIT: +// +// if (current_status->arming_state == ARMING_STATE_STANDBY) { +// +// mavlink_log_critical(mavlink_fd, "Switched to INIT ARMING STATE"); +// valid_transition = true; +// } +// break; +// +// case ARMING_STATE_STANDBY: +// +// // TODO check for sensors +// // XXX check if coming from reboot? +// if (current_status->arming_state == ARMING_STATE_INIT) { +// +// if (current_status->flag_system_sensors_initialized) { +// current_status->flag_system_armed = false; +// mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); +// valid_transition = true; +// } else { +// mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init."); +// } +// +// } else if (current_status->arming_state == ARMING_STATE_ARMED) { +// +// current_status->flag_system_armed = false; +// mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); +// valid_transition = true; +// } +// break; +// +// case ARMING_STATE_ARMED: +// +// if (current_status->arming_state == ARMING_STATE_STANDBY +// || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { +// current_status->flag_system_armed = true; +// mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state"); +// valid_transition = true; +// } +// break; +// +// case ARMING_STATE_ABORT: +// +// if (current_status->arming_state == ARMING_STATE_ARMED) { +// +// mavlink_log_critical(mavlink_fd, "Switched to MISSION ABORT arming state"); +// valid_transition = true; +// } +// break; +// +// case ARMING_STATE_ERROR: +// +// if (current_status->arming_state == ARMING_STATE_ARMED +// || current_status->arming_state == ARMING_STATE_ABORT +// || current_status->arming_state == ARMING_STATE_INIT) { +// +// mavlink_log_critical(mavlink_fd, "Switched to ERROR arming state"); +// valid_transition = true; +// } +// break; +// +// case ARMING_STATE_REBOOT: +// +// if (current_status->arming_state == ARMING_STATE_STANDBY +// || current_status->arming_state == ARMING_STATE_ERROR) { +// +// valid_transition = true; +// mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); +// usleep(500000); +// up_systemreset(); +// /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ +// } +// break; +// +// case ARMING_STATE_IN_AIR_RESTORE: +// +// if (current_status->arming_state == ARMING_STATE_INIT) { +// mavlink_log_critical(mavlink_fd, "Switched to IN-AIR-RESTORE arming state"); +// valid_transition = true; +// } +// break; +// default: +// warnx("Unknown arming state"); +// break; +// } +// } +// +// if (valid_transition) { +// current_status->arming_state = new_state; +// state_machine_publish(status_pub, current_status, mavlink_fd); +//// publish_armed_status(current_status); +// ret = OK; +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING invalid arming state transition"); +// } +// +// return ret; +//} - warnx("Current state: %d, requested state: %d", current_status->arming_state, new_state); - - if (current_status->arming_state == new_state) { - warnx("Arming state not changed"); - valid_transition = true; - - } else { - - switch (new_state) { - - case ARMING_STATE_INIT: - - if (current_status->arming_state == ARMING_STATE_STANDBY) { - - mavlink_log_critical(mavlink_fd, "Switched to INIT ARMING STATE"); - valid_transition = true; - } - break; - - case ARMING_STATE_STANDBY: - - // TODO check for sensors - // XXX check if coming from reboot? - if (current_status->arming_state == ARMING_STATE_INIT) { - - if (current_status->flag_system_sensors_initialized) { - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); - valid_transition = true; - } else { - mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init."); - } - - } else if (current_status->arming_state == ARMING_STATE_ARMED) { - - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); - valid_transition = true; - } - break; - - case ARMING_STATE_ARMED: - - if (current_status->arming_state == ARMING_STATE_STANDBY - || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { - current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state"); - valid_transition = true; - } - break; - - case ARMING_STATE_ABORT: - - if (current_status->arming_state == ARMING_STATE_ARMED) { - - mavlink_log_critical(mavlink_fd, "Switched to MISSION ABORT arming state"); - valid_transition = true; - } - break; - - case ARMING_STATE_ERROR: - - if (current_status->arming_state == ARMING_STATE_ARMED - || current_status->arming_state == ARMING_STATE_ABORT - || current_status->arming_state == ARMING_STATE_INIT) { - - mavlink_log_critical(mavlink_fd, "Switched to ERROR arming state"); - valid_transition = true; - } - break; - - case ARMING_STATE_REBOOT: - - if (current_status->arming_state == ARMING_STATE_STANDBY - || current_status->arming_state == ARMING_STATE_ERROR) { - - valid_transition = true; - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ - } - break; - - case ARMING_STATE_IN_AIR_RESTORE: - - if (current_status->arming_state == ARMING_STATE_INIT) { - mavlink_log_critical(mavlink_fd, "Switched to IN-AIR-RESTORE arming state"); - valid_transition = true; - } - break; - default: - warnx("Unknown arming state"); - break; - } - } - - if (valid_transition) { - current_status->arming_state = new_state; - state_machine_publish(status_pub, current_status, mavlink_fd); -// publish_armed_status(current_status); - ret = OK; - } else { - mavlink_log_critical(mavlink_fd, "REJECTING invalid arming state transition"); - } - - return ret; -} - -/** - * Transition from one hil state to another - */ -int do_hil_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) -{ - bool valid_transition = false; - int ret = ERROR; - - warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); - - if (current_status->hil_state == new_state) { - warnx("Hil state not changed"); - valid_transition = true; - - } else { - - switch (new_state) { - - case HIL_STATE_OFF: - - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { - - current_status->flag_hil_enabled = false; - mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); - valid_transition = true; - } - break; - - case HIL_STATE_ON: - - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { - - current_status->flag_hil_enabled = true; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); - valid_transition = true; - } - break; - - default: - warnx("Unknown hil state"); - break; - } - } - - if (valid_transition) { - current_status->hil_state = new_state; - state_machine_publish(status_pub, current_status, mavlink_fd); - ret = OK; - } else { - mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); - } - - return ret; -} +///** +// * Transition from one hil state to another +// */ +//int do_hil_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) +//{ +// bool valid_transition = false; +// int ret = ERROR; +// +// warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); +// +// if (current_status->hil_state == new_state) { +// warnx("Hil state not changed"); +// valid_transition = true; +// +// } else { +// +// switch (new_state) { +// +// case HIL_STATE_OFF: +// +// if (current_status->arming_state == ARMING_STATE_INIT +// || current_status->arming_state == ARMING_STATE_STANDBY) { +// +// current_status->flag_hil_enabled = false; +// mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); +// valid_transition = true; +// } +// break; +// +// case HIL_STATE_ON: +// +// if (current_status->arming_state == ARMING_STATE_INIT +// || current_status->arming_state == ARMING_STATE_STANDBY) { +// +// current_status->flag_hil_enabled = true; +// mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); +// valid_transition = true; +// } +// break; +// +// default: +// warnx("Unknown hil state"); +// break; +// } +// } +// +// if (valid_transition) { +// current_status->hil_state = new_state; +// state_machine_publish(status_pub, current_status, mavlink_fd); +// ret = OK; +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); +// } +// +// return ret; +//} diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index bf9296caf0..57b3db8f19 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -47,9 +47,9 @@ #include #include -int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); +int navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); -int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); +//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 27a471f131..20cb25cc02 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -60,27 +60,48 @@ /* State Machine */ typedef enum { - NAVIGATION_STATE_STANDBY=0, - NAVIGATION_STATE_MANUAL, - NAVIGATION_STATE_SEATBELT, - NAVIGATION_STATE_DESCENT, - NAVIGATION_STATE_LOITER, - NAVIGATION_STATE_AUTO_READY, - NAVIGATION_STATE_MISSION, - NAVIGATION_STATE_RTL, - NAVIGATION_STATE_LAND, - NAVIGATION_STATE_TAKEOFF -} navigation_state_t; + SYSTEM_STATE_INIT=0, + SYSTEM_STATE_MANUAL, + SYSTEM_STATE_SEATBELT, + SYSTEM_STATE_AUTO, + SYSTEM_STATE_REBOOT, + SYSTEM_STATE_IN_AIR_RESTORE +} system_state_t; typedef enum { - ARMING_STATE_INIT = 0, - ARMING_STATE_STANDBY, - ARMING_STATE_ARMED, - ARMING_STATE_ABORT, - ARMING_STATE_ERROR, - ARMING_STATE_REBOOT, - ARMING_STATE_IN_AIR_RESTORE -} arming_state_t; + MANUAL_STANDBY = 0, + MANUAL_READY, + MANUAL_IN_AIR +} manual_state_t; + +typedef enum { + SEATBELT_STANDBY, + SEATBELT_READY, + SEATBELT, + SEATBELT_ASCENT, + SEATBELT_DESCENT, +} seatbelt_state_t; + +typedef enum { + AUTO_STANDBY, + AUTO_READY, + AUTO_LOITER, + AUTO_MISSION, + AUTO_RTL, + AUTO_TAKEOFF, + AUTO_LAND, +} auto_state_t; + +//typedef enum { +// ARMING_STATE_INIT = 0, +// ARMING_STATE_STANDBY, +// ARMING_STATE_ARMED_GROUND, +// ARMING_STATE_ARMED_AIRBORNE, +// ARMING_STATE_ERROR_GROUND, +// ARMING_STATE_ERROR_AIRBORNE, +// ARMING_STATE_REBOOT, +// ARMING_STATE_IN_AIR_RESTORE +//} arming_state_t; typedef enum { HIL_STATE_OFF = 0, @@ -100,7 +121,7 @@ enum VEHICLE_MODE_FLAG { typedef enum { MODE_SWITCH_MANUAL = 0, - MODE_SWITCH_ASSISTED, + MODE_SWITCH_SEATBELT, MODE_SWITCH_AUTO } mode_switch_pos_t; @@ -114,26 +135,6 @@ typedef enum { MISSION_SWITCH_MISSION } mission_switch_pos_t; -//enum VEHICLE_FLIGHT_MODE { -// VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */ -// VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */ -// VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */ -// VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ -//}; -// -//enum VEHICLE_MANUAL_CONTROL_MODE { -// VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */ -// VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */ -// VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */ -//}; -// -//enum VEHICLE_MANUAL_SAS_MODE { -// VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */ -// VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */ -// VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */ -// VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */ -//}; - /** * Should match 1:1 MAVLink's MAV_TYPE ENUM */ @@ -180,8 +181,8 @@ struct vehicle_status_s uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ // uint64_t failsave_highlevel_begin; TO BE COMPLETED - navigation_state_t navigation_state; /**< current navigation state */ - arming_state_t arming_state; /**< current arming state */ + system_state_t system_state; /**< current system state */ +// arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ @@ -194,6 +195,7 @@ struct vehicle_status_s return_switch_pos_t return_switch; mission_switch_pos_t mission_switch; + bool flag_system_armed; /**< true is motors / actuators are armed */ bool flag_system_emergency; bool flag_system_in_air_restore; /**< true if we can restore in mid air */ @@ -212,9 +214,6 @@ struct vehicle_status_s bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ - bool flag_land_requested; /**< true if land requested */ - bool flag_mission_activated; /**< true if in mission mode */ - bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ bool flag_preflight_accel_calibration;