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 zcmaHSV{qV2@MesSZQIVqwry-|o4?rH*mgFyv9WF2*^Pbs-hH`$)z!_HQCC;>)HBaa z_w=;Kg1&WvA}Gm%L!g6zz<_|vpr|Dx$N(@fF#k10K>n}tZx+PP%+-j|)6OJC87d?QvU*WeU0v6%#Jpsi)YwP0by%WBd%m~UPDIANNSz#!`EI-G`vJ?t zM?KaY+(H=L8c1?6vGGsp@uLDz#f_OiP@Anp)(`KKu!Cxe^%lVVFn^=|Vfp+n_fmke zY&FjyLaP1@d<#TX;8|?+{Z?Y+=tG793q{qo`HaW^Be4p=RR=&GY|9xQX9!0OZ|7}a7wxgsY zYuhXz-5Wka!KP7X5x!|ctcb(n_u)iQ~A5bI~TCzAKXU%vsa9HL1T^XnMNQO zNse_yqr-34XNpj?h*@#G6Pj`4;QFL=Gex{f<{IjkKbx_UK`DR78@sdcf%pNyy<=^; z-3<^x-k9Ex%2_(Y&LB=}ZawF4rY2GUUfQl|>&7dw6VfmRY2oPNg7v&|nEM1*l7)sT z!0x%42L(VtY#=~D{tqnvUs!S1c`A=&ojFjo0<{?=S($ZaQY=A`VdQ59XaSEXv{{r<|^ zl3vJ~x=^J*9L0Wmtr#Ie@$_CP`RH`&CE{7WDWgsg(4y_qVa$7*=v^r?6I(0LGqXbn{*Y<&rQIoi=WRB5 z6pyNojFZQs&Ch9L?U`BkDP#0Gwb51WwKMY~Qo^Do3xZ>G`OC0UyxLHurpvtIQTAnh zXXphPa7Ut~MSHLG)}V>`eQ(5S!&>xJgYF&nSV%`qgj%~lcHM?E6x5^kG< zyt|x|IkEggj}}EA?837jOO~-zyv-bDqDT0s$~V%ixvYm2lrR-)TggVIEP(NsB;INH z!!q0HJSxp6GWZj^R!jxiPpI@(=y$U7S=?`5;6@p-9bJ-n;ozAGMoscGd|Df-94Rqt z2wscy1{Mit6f-L+RP>3XrV41sFy&d4jOAX-<&_{Cj|>8B5Vvm4_0QJ{W1?eLXBT{CgE?G&2YfWlWODZog@7{VDkzM=yO~)!Mnz&9rhNMhWEpT+{pgf2|qUh zWX~X&-RlGWY{>@oBZm&QR0o8*vYame$k%;Vqy2HO%r2Of9QnHmA9$7kRRB5z)lZHy*9^kpoUyoFyA4KOW;Pc^Ae2gPn;!r9Fkue)5ac)7bmRBE zP1aBA#&iN~+8nirRrehBV;!Dp+>Ur_fXWRYe5F`ej&)*XKjrD`Zd!?f(f=!_K!{>n zf|UHQvNqWc?Mra?Wj(sU!}Y2|(Uaw{yYi|)zCSbSvnWrDI6`DE;Q*7XbVw6%0)?yR z6Yk>q+?+r^1G6jaI4;YZ;YmW1JlxV1+U+4PVvcO491(FY`;TKITf&p~lI&7j# zy}Ba~MGVWQFgEclC3AX9N|mrVU#`mLA_daDxK(Vrnp;#>Aw404MxIw~?MSS|+lU}V zO6q~8-e6%d?kTstZ##i)nt?xhG(Znk?Qy$V3b6w^!#Id)OZLGv-K-@wxHK^ydv*5n*TZJZQI#wqh2 zau$Mi31xvuu}y;_vI|2Tqx04FpL`wNfG>$|k=;l9#0#&HSDd-|*1|0CAR||4v0{7;JZG+=+iwXh8D+qeHlVbZFvW?`mf6`X3u! z`E5BLjQ-s2&I2oGCg{8X34N`#&dY%1UB>H4|HEC+hUavd1C;R?i+--6o61ahej4i9}vI7m4p&uKWd z>=C^bPGtZ2&Fj*CJTvN9KWcoihVEr+^KSOjjQe!FYtru4 zRcwvE>Rh@v>hWvM>edyU)_kkl4z=jL+fnan-15_R@7c`52MoHifHk*n>=}v`g&mJC zbHLg2Vaxor{bI&b2FF?h&Sq?FQJ+t-Lqp9r?~}qFlV~`SY{A z58nlx%rkp$fW0a{>jhIGP!J2UaRWMZ1^MOmzM#?7?=>O7>(Qy>jNanYl$LhCrn5Yv z^L9P_?ApZG|LTf~`4o8ndna7OiKO2PH$$N`#Th*^4VZhmdwzI#*EnD>Q4_fXon6$2 zb}u|!Z!Q;SdEcYAe9^K6Z20{+^{1_WuOP9^?((?sZ(XeheoZAGv zeA3=edq4l))i%c8g`e#m4xJ_44Ow?))gX@A5@AkEH-el{hPZd=xMD)`J)O?2z$)o4W!7-J2D~s!nG@8ITi0!jmjWMESJO z29HCu(Wr)WrYGyt&(GD7zMNwu-2@)bj^+59psF*lX0PY@tApD}ot>@kg7pRZ<{Q6O zhbV7af%c(#{1pqWz)FX2m#F@C2iq_-DbtTYi@(WhlWx)Bv~zEiE%2_|p81@o?G^L= zV{mGVtg~XfEpI^e4r7~k<>=k%)yUr^tLJ|3_;emA>+E1;$oA{;z%{ru5lBqK+GF$k zzO}h^DcIUU4nV{lXKfkxO#K)Ntlg;oNyvUODmlDjVBx zz|IF+g@O4xKm5_WPW{vU;m1koPi$NqUbI@2>7(|ND9yH$G45f{iKqTK>~FE)57agz z?|d|M-q@~(rcd3UDm3o%W_hq*jCk$TS%l0?vyM%->kEpz$2w)h>N`{Cii$t5n< z1|7W0R>+#;*XLj#kOTf34j2*&D=3-<-KQu5+u7vgg^tj?w^6J~aWI(S8{F}QWrhYH zGmJ~M6#8-gRrfkmy#xEDREKm+`MATIbwIg`v*XhwX7Jj5#Ni*4>yM=9)bo}4#pS2x z@O76*bW(M>a%QdDQ|)l%X#Q!`lj(rTrbL^EwZXuB;8dU058;<-4`3eI-+s!rMECaR z4No4#Wyd?5EL<6P$RO-Xd^&%Y;9eu{&sBQy6aVEzE((T5e$gf-{MEJsZafEN?0O4g z?1UL1S@kdxgw1EQb@RuP#4p8%PZ2wQxlFTtr1AK*wTJ>-@WWin`O6yjxwXj5)={e~ zll0@tk^>hXv-?6c576z>v!#np-2KDW39G0zEjX2yV)XK9)t-IR;EcQzKGREMm@3Lm zne>lSzdmoiD*nI;kPFQj2K%^7ocUl^*h%V3Sa%42(XIaM_4o7aHMj=#TsM}obAyhH z7Hn)V^T;YAQ!05yB1~)Py@$tthUBs(Dr0+|efjWa+X;5`17ISzk&V&(Q((pjQ5rNO z6V4Gx1QP$%*9as|DbMMjx9uwYc0Qs~1{JyP{G_fq-nbqLK0#954B`avnk|oZtPj1o zp1nP9I=$u2@4$Ofe4247fWysT=i)nc?%P-ER=t7+rv|#l;5pkPmXG^Z_VRIW6^R%t zX!O_=`Zfx*5|zc@9U%QJ0e34;{hjpHK%}kEYuu0jt|NFg&VOOeqlNK4)cS}AZM*;I z5XifaSo=dji{>lk2miK4E0>|)@7F?J$Bbp9yRG9YK_WP_&kJ0EqS2@rHhne0s?OIs z-^`Qviy`Y)aim!*zF3Tw)jWglx`)suyruf2HxCg2gjowAkE;Ea$Rea*r{nDvW_FWl zCC?8>KaPTbN$EqA?=`c{u;Ex#+(m22Jm9<|WK(%i|vJGfJov9=C-N zFoXj9ZP`+LzU_K^%dM-(y}%YQGN;%k|6vU_6495lOicx;kit1^P_P` zjP3zaYm5!FV!)w+FPzcD!6>_vy_Xl|+ap_Z9vH@{M6w+V$xjB#=|mb3L7|I9{L#DP z0IK6Y{8AyWU&Pqcpy0c=UUiTY+9ZIP7kPR@y~O|~esYr1U?6TbeN8zjY=L`{2#V2) zm64`&z8Sn_4oD_ZfH|ZlA%{@MXbuXDM(vRwFckTm{Xjc8b7$l%PsqAl z(;URbYx3PJ=Lxl!htNhpL(oaoPUN`QZ8nWna1r&gOvhv;X!#5OBKnB4)gd6FQh{g@ z9A5T2-(0#JC)(&2s#X>FGSoE+1YR#XmTEt;lnhBl>$ruAn+-~+CckqpAJMYhFf?v4by_jBRkGvlxcfwp3u}eA49hF%l79; zL*5hoNW)}~Qn`gCS+#}K1a$J-H3}hUs6BGW3lA2QoNyfV0VtcYhbs0_YiQV^eSw)R(H~C= z$cZ+Llqwv_lBiZ5x#xh?P`xck6GJSViO2+XJ*SJhmf;&c<_=Q+@1p%~=N5zeAk-kDuhMh>?JQD{)GzQQ|V9QGahA z6ZOmZcZ8JvNFRgw@+(&^#yH7h!)WsYE(52P^VqL-YkTq1k9D*5JIVSf?9%kH>b?C9 zJXFaAH92yuz5BMv;)USVh4hwO^r+B&5U=3|IJm z>glGP7nLIi!iR5e{Z5ur3M6@!9VV(HEY?jGQ+h)@Dq29+-M!Seb7D|1j)-{IK06~u zh*N9j!%4F^nrDna90Hteao|4rn`9D`Hqu&}+@swNLs?blTw5oYMGyxolaExlYf=!` zPz`k6v!VZ+7kaA=^7js+QSVjckRx4WFE%XMjqtLeE#E~42en;t6fRr_qq0E?swar8 z-%4J0703XV6#jF>*T+onduy(V|82#F*A30ZLCuf&V^$_gnYFM3yYho6ZqEz$A?X%x zDoC%dHI&aEbfH*|X}i{0ta+2zYEU{B<70mV;OvT|f3ELprLmW-#0OH#{tT@^!-i8f z8YZ98;IP{Ww_;++wHfA_3<>ku94Rk0Mdf#e4>$#0nfKBT@+TlpM4czk2CmRPPce;I zSoD6f0#SMv0z0F@?^PCxz=bW!d1v$+tyEUHsd?Cb4b8as`1 zf~)3VFBoCb?_%#*Eelae^<~uC!C_E3#f?J8PYD|;K4=JeZ$`wScY`%Cv?K9-|7{(d zP6V%@;ZKxAQy#&2du127JjRs)TAtNTPU|!PX~z7+$3%*Vj>WwFFE@Vs;9q#Cj^*)l z>OznoG5v>#(kl|S=f;J2srcBqU4fq7OmqmIzK!*w({g6YSRK{4%#ai9D5NVIsnA^! zw0hV{I57y+hl-8yq#JbE9KX0yH&^609Onj#4twljnqk~EoGIHuy4uG}5|2P#oTAr& z-Hwy6x+J%16Lbbwc<24Kf|x&EuO&=uJZemvRB1Asa+A#_h!dAFtnI`1%o;H1kbE(( z3}B`G4h_Bek;2ZSI1I=ZWpuw4C1SYDbLSIKh$hU26Xsm>B8${A2hATo16zg783oa| zhI1cX<6Kp$HS;#Z2XZEOM)x11(yf9Jq1igmedS3y>kfiQ9P8C* zLYfLXmS(rdq$9Z|Vj~E#h@tuTzhGxE8)%)N&<4*pS#$BI{P<5fp;w^eet@C_T87hw zXHa#A74Vb}AYqagMiw=ZJ+NG~jLDD1!UWa?q&LHX!elg`OeUKO)Kt=AVK88?PHvGXMcS3 z2u8tnQhP#~@QWqPi-VYoIuiK+`L&ERDCEt$B`?32COxq|AbQ#Eu19x4h~ke=CGG>P zDbFNmH+3?o<}>?-(1m4?+Q|6x$JzB`y*lJ}UB_7^SBM(gwn$_;2V0nYkMKrK)&@eo zR(*KoB;xhb&o^O_9wNEubDBkHf{vPwM}FN)r&oKRpiv4CIR5jO?H}iW8NJUE?;lB2 zz9m#4GZ%_aq;F>i17F`hSH#M{Ur-?@B22EjJ`~s!KGyZik$|eQcFQ)7^AgjIldzl)5O-2piYk?pC{o`Oib#0&Mv*1J2^GWpN3@+PlGq z6mEj&*>e1n_HIib$*f^PH^5;KSe18=A zJv&xQM{XilU4CMWT~;NGnlkOgDGt70bT#lwdY3vBgLlfvO=Z{hR1$cAYCcYr-5N#r zWgwM23t6=iC;*a3PbiNm#OzJs34<29Rm7BnMib=J>3^kc?e$@HBMc+ z_a{B%(@zVBB2VxhxoOms|gYtQQFh*oZ7`o_@fw*e9SWlRx`O zEHU~mY)lce7t@0zqw3V^qKMp#mO2S^?0eu(4!E%@_(w+(sd_K;YxYK6V~0|3wA3Z| z!L#S)bP!<3bx5gG&^oje4iFbaZ+<9R_w-UO(U>Frj?Ly!^TVdJELE~esm|Xkuzu!+ zq~T$SDg69z!myOa+ZDoxKPm$Euh1Gpg9a=+>Xfu|k{QS1DuRR>AAZq!SnGB&!4+kl z`l3?aY_g6|e_pxA+ekKLAj zb2MPcht*?E=@nsG7tx(G zMii8Xj+-XE-Z{g$71t6>Z7=I%WCo5Yl>KmD9fj-mT^XGIGit~S5$MyMiThK_iYk*6 zF##bd-?Ad_;KX_D4snA?&N43m%tlSxEZz!OnDn8sOTiZqVSvUc;QUW6K_LN+Hkdfp zO}33a&JAlD)-+mNSo|2)1jX(>60#V^w@XJ&J6KdyL`N=}*Lknl9^+D@ZszD--bns% z-311=i%HXxX_a&#RBUa@6B)|}T(s8Gb8ZoBTl8Fd@bAoi?O&}AITRI;x~_tFc%Hg8)e7p8WnkD_1eOIBElD1A^Ijjl!rBp zB?w1@zB)Q7#pPPc7n0|snu(L*1gOFX6gIl+Nr9oQj!h{`46BQ>}5NI;<~4pZ#_ zET?i60kUH@pO^hk*C+KtE~sU8{L7fOlEe24YsfJ9A<(!@~G*=2II*S87R zd%f?Z>B^Scl23{|USRiZ>y3*;W}K7w&!qEHZq?6psK}`P*irt(8uwgMit+AJew^{T z2v<>Nzv7-bN$z}i{&umFC*b-ZdB|u+HC#^}vydE0&&-_(0Ftj02m^kn^Fff!_R&7a1 zGt(hTTR$3*i4BAR&BQ#OB^us`Vjgv@lNAj1-(L($4PRt>o0$2|e?OFE|1TPEPwq$L zxF6v^7M`uVk(b+tBnK#32<$5#NTWweZW8kETT=MRV6q}sM9A(xNl&O^)}o$^q2w%I+gxVUttoCwD z3AnPy4Q)6NO#_!3*>a0WE<^O2q?GF~P`6OgY3<;XaB4VOl!21U+?-G&P-J6r!bzzBo(F;lUxoD@wvJGH8l?o0rs(qViHyvYNg(PV z1aKbmyjJ|IY?62U5lsm5{BM+ws+x zb!Kd=*G54haXKvG_?i-7G%=|WoXR5F2c0SuwSv6ze|O8IR*Hw!rD$3Fw6e4HTQ{kF zqE3VP&HkA_wcRthU1Ka)VJ42W2sHe&%kA5y?%`qb8ErtDG%>Gj??1bps&xlSobIYr z5#C0}a5#GAA3nVN5T*bT~9|fj%LAgT-*c)#hj(ppkgN}tF?*|tc zK{K8wrtwH})pj7{7HvYouiZPNA_>pL6p@wT14T}}X%tJlKm9Vf>h5@z-%O{vh>OaF zzBYR+2^u*K^2BORUKGHU*G&{CvTIkFm z!#Yk^{1M_LImP&fFEzvF0&60k^u(sBaiVZKDJ|#jb0#p&hyv;n$sYq~#|_uKg^h%c600z2~iS|=^gqCz@eAn-j%pHCbEDIqhQ0|z_? z3Qz0&ak(+!H57u0e!nZ1s&(CqJ9Ft{iB=+?X23OxJ7sGUGdl-L>|H}`S2+dtG*pN6 z5j=cbjK!A8nD?kj9*zxG3v(>gBp#H>C$%VzT1u6)Oaz7!IKSyvqOnvdR9A(MZi1c= zYHRYA&RAs%l=(>IEs!G}ieX0ZT>Xmeaus!4ekFlbZHm&(so59E5iAQLfO0U&hJkiR zZcj|MKmEHbljJN?tRRu$A}uzayI!=+R4=);CpyA^egc)wBpu(T)xpj{#$Qp#BQ@~S zghn9XS`6fPqU@bpX3*&LRlidIU@SkB&ckq9Q+bI2J4VdoQE}}F&3ns>E zpfL48KBy0Q@rF=P&eM3qUh?z-=FyN~8ugQD@*W-5(e!#!>bG)2 z8Au?#sKuD5Vbg$)S>}#JZuEmQdpw2m@LBpi zc8wh@Ylz~d($NPMk2Lf9`7QM#HMVnOAu~YpNhqze`iJ?Pm=Ct`D(b^PRG2_uODwAj zua!F!`TeYU;e5(>i}|ThHL~RVzfiKoY!@^o&LP^miF8Ma^tZ&24Y=r|4gG$F)K8JN z%{K%27H?;NlFc4zz&Q zlPS{0w(Ociv^uAL%%2BgJjWbA>v8hh*B?JU$a}WK$$PUE`N#i)n6|s^$x>=qT%NRwM~)#ZD@rQ$zhxa%RI8rmkto>76Q7 zFJY(yN3j^)#DQ~ar(46Qf9t&MSxxD}c7 zu!O7$sx1rVRLP^4wdPxMecHepWPrGPylR|P^cRyEt{r7b8NvZuwB-btAj|z&Cy8p{ z$@mz@qQ+QCVJsU$kNn9~fzG5{UQMuRWPV$d;D{Y7a4l5Hzrjq%jDzKHqH6~)(?!EF z=Oih=^262o0d|4pIOOi9aS$22j`$TC>-K!5D@cB-1p)O=pXqUUDqppcSXze|M~?W0 z7L%E->Rsx?Upau~(wY(QMN%!Pt7@Nxk8HIqP^;lPA~|4<@8sa(MDIQJWONO#{95(h zlb1)!Kx{EIoMA^eIF_ZQ7BL^pV#0Tbl}<;>Mkr`6d%CY4=A62(m<>5=+4_4WmrnU* z+n6}tlO%o*Dm7BD4`)r)ps0U0)8H9S6ozZ1R8E*dBd<0FTc!laKS@UvWzN%*RF07S|{zmSmcx?m*2!1VK2dhDMi#(v~+mJVQy5Wa2&d$jl+d($A+JjCzzt5)IyXs zUq|*s5tHw7D~$Mcnx)wE%)>)KAx)o3W5RsPNMOGS!`y0tX%0Rp|Kr%~TiQ4)NO}|q zZJ>#$9_^YKTiq+b%8fD{wos5sSDFqZvsi7F;3#M`*W56q6Z4n-LQO*C2Lr7hUFdDM z5JcPKdf>&G(kUb#dYA*G$B*z0Ij*=~lNo62)73aX5HuH0c}m^fh{pq~-P!2zK`f>7u{HivxBC0QGW5k1q(XkMuM zCbT@PH?|3jRDyp0RKM{|22?Ua(h+E@a+}fQ-fG>hJv&ZAfd-T6qot3?+Rx);_lQD< zFjr(rmR0G|N)<13u-zjbi)=W6c%OxCOrBC9w3>%ymypy6n%&2=&+M1$FeoIWJlkC4 zix_bwJDL*!6LGa57t|Vu7TS3)f`}UU#)Q**jUKD&oN!E)?<^$id4GmVn0`-62@9DB z$-Bo`jHX^TJ{q4CB@ub5Nvb9pnO~9o>Pl-y71;*?ON#QUmSfCjW~%5@mlc_!NQ3fLvGb%tf{DXF_GipnC>LL!L9bRv`k0gjxOlal~uH_r^-Q z3eI-mR-l2_8T%F*C4Y}}=V5QD!`xa2tG07-sOtD9T8qfzI8}A;VHtcvqR5#gq|XMr zjJ|k$>gO^O6AHh)jN~hF_ubeYkMF4z;*;K-PxBH*GP%2!K+vk^)6vnS!U0cnBqk%1 zPQem=9g0!cO53%`N{^!0pczRRbsl@6lGOr0ouiHc-6WetNkS)N(VjNCS>UR(2W&iB zBOq_A=v7-KwqL85`iq!23%^l4^ew-V;=1%~F%Xt^zLax>S97%IGFvBI40E=Z!YUS0 zOzD?-xE^SDg>T~3?0vE>=2)h<+#Yk6dZSm47K2skz4FtCLUX3cyaks#H{*B&SDFFG zv4>Q1lYYiN4lYPNndQz6g{NgMcKbY>DYg=mf#&`aT4oDe23XH56rg>5HFKrD7J z_Wv@Z1;ccYrpiiMV~Tp{O`Z{?C4~arQMPT_TC5|b!4Bm18%b?*{=^Rx+rMKUlqCf? zRdA1q{%yw0`YWd^rM^R1e_iwWKRtu zn)K~4;$o9)L>J*PI2OXoF-&Q?^_GVfP&CQxGdZJX`Pri^xroJ)k~_#S&YA(4KUI)? zcof>PAovc;n3*4VbI-aw_1roF#k|FrF4d!d=f3AP6uVq|p}sR=F&S}ku@+ZT&Q<%+ zAx=3?sQHPAj(f_2y2NU0#;86B3rDCvSGJ0%5tE_O^aNfOPpV7S!?T8m)NY~L8C1a! zv1_YAZ1J*;kkeAaK4iESQ>p`KD3=MiGX1y)Sjuj=USLCpmg z?*{^Yy?zB5@px@^w8sj_Cc~DQCi;Vy14?xU6xW4=w%VW;viVxTC{O@D?P_a0c(HP_ zvv#~zD@W0ec!Bv~OH6gTVx2U0Ag3lSV!0GMLudoik_}>6t(gWs`uDHI=OR8St!(Dy z_@TA36}4G=tcHGiOPcr`+r4_Wv)DDuM!2NP2eK)~Z|p4fkb@(9!d`u*L=Q4aAlPN7tWuOXm z`|K|&c@v5DY$q!hGdHh3@$B$8GNc@Ajc#tHcd-&oA8b+s;oH!_U~&%1HD?y`v2*v`MS9N#u= zdPVO^5P@EOvwHA`vhiGGJJlz||BXo^ldto-JXco2is#T?b59(pDke#5%s{K!J7vjS zlMKZjqWX99+LBb08;AASQk+T^<##pkT)c}cT-aw|IbK0r6k_^`5f`~(QvQ<0vi%#c zadub>1ubDHXL~i8v7)X?)&4Jry(_{x`7|H>YNIS&gdeyBb{r8Fqc$Q~#i)CBs)MgN zP{R|yprUmVG?Jk!cn~*K2_%sM*Z)=rq_(GHujfv4wx~)|AUGVNQ2cRVLE7)q(#Jj2 z*{l!;8sWeV70R=XguZi4?vv;{$Vv@ClV8fKPIBsIC3Qn@%ro|wrBU@fC5+RNUWs)F zn%C?4GVaG7L0p(DU7)C}h%KUuVQl%VB1w|ShuaSakveU!Jr*X3+o~;k1eq`Tqk_E4e8(yjdXW%A&kvWO&E?s=%`wUu6a(jwCUR`u{ zIYW%l@Pv}+IsQ!He~yWxT8}ofWQ4;RvIpRt%V}e-4bJX z{96AF(U;(VnbuOIr>iUUARuwipdi@)<3o~&s0_v|1P%xY1WpnKg)o3UAuT4X=9zt! z^X7ss>EGvLPUKzP^iv29J95QH=tngf3|HO^7kYUA<4bu^P!%bO`U}Oh;tTX&Ku`Tr zO~uJr)7HdAx_tm=WM~ZA1TiKajTn7WR*H{8dtRn5s7665iUo8B8K4@ z0D&lwzz!f@gaJnm!TldX@(kDkZB%P$Iju?#R?*D6V9vdt`EmMG-pj}1a+P7 z0Fu24PPkd&9@WTzqG%ac3LE(EqBksyR&RC5WM=Ro&0!}JN{<1I8XvgOyX)C*#BmQ0K*w=Vpw={ z;lD+0A)MPPui*c-w!p;^?DZ#i$p?NLP%OcWL+I0CbIo6nf^bvAe?uLMCWBrJXg5jZ=NcGqTqhufF1=<4%7i$mV-F zuNyisFD!RJGPqyAORnk}fP0$1n1`@jY*I)0+T{Q-NjHRC|>F*a5GQWq}Vz#Bd z6r;g55yA8n10gtH9tZASOPh?n-j{PS?6xAMAbdTz8(kz9%6jp5Z*?waZ!arms8lMn zM1$`Kf8p~P`4+;|df$AM-JTxT08qU{hKFbLo_YyLO{t#=sXJPQe50~E@)NvSDI07m z5}*~-N&&AbjowBwIylg$v!0RVVn!g%LIvgM-_F-u&gAhf-W(>Y&tqDf#A@HU7uHb1 zU$;Cs-i|MR5do~6tU6*;`Bk&Zz`sX^X}1>(Dzow%z%$qdANA$Tt^n(gdQ>QPmh1d%DFWSYBegjf;X^4H>}$K}k=DH(i% zp0(}vIq&#r)Yze|i#Kn(Cwn`Z$W$|gsb9;zgPu46CxT<9htpfWHa6r*${mKb@5dXi z4cxe~GO@8J<=5?9&gb_Kz`M)TITz2Kj3JakXq^4p=I$G-Q0M!HSL5HY@=a|`%u*(} zpC~l6!W=nwjTOX%pk>2k2jZSm;^9%a_4<=R5fTBBR8Os+M~QS_Z~ZMRpRenhd9JqS zfnU1V0dV=G$Bn8(?=j>P7e0?iWecJoK=zcGe}mo3Z68IO^2#a%pyoMS0N z{tL!~^5+$SKCjexP6px1r!C2Mmy3BUB@lJ>VZu$FKS|dW_2Hpqfw~P1>aY@0c0ZjAWulH_@3qx1el%kbJ)+92fO99{JC$W>e4Zzot_v2u2_?SYb z9^^`x24i=4F+N^cTNS#c$Gh!jGCWZWKNtA8NYVj!{1CzZT+{L+YpPg0uZhm<28Yg0 zd;@Pk`*YnrGD;@}SG=Rt-V99J@!e4g2>YP-pV+uaV2tIfxI*lhI~_N*pZ7yR z^=2Kx70vW9G~>IX^-^gG!AzEZXx_G_@0R9MR5U(~EOw+=!EjanCg>u?&x81zqXw&{ zKm{8qatFiDhMx9cYNKgP(61|HF%%l~3gM3yCT_80!Qj6pGzL-3Upjup-`sQmBF4Ck zxI$DM;@sB<#PNkqK^#B9zrLbBfZ*0Wz0v7VjI6f(=_w|nkS~WdKbX&*4qt?ZrX!nH z5uboNbfS5jhoXE8e{$CzL>=&r5A{3QN}tmka7oNkEAc)1F;hh{Q|XKLMD?$uk)%&EP6ke zMugU(NVtOZkxmMdyyco($h%?DhZ6y-v*PizCP)4${O>5mx2M6Plj*WXZL_Z)3yr&< z_cb?6o)?7RWzi}(hg}ygnl4_4y}i4rq2bdd!WxN>t>>D|^POo!E&+9SyL0qSu1hSc#)CkJNWU2EI{r|K>rBnqx(M&HG!PPPd~GmHYZCcXll96ldP zm`czZBc$|KrxO#~eY$ueE&A({!FhxTxF-A;dz9@8B zek@sd+Q63)5bu}2Uer~P1xW%>YdQLX15fBA)|= z*B+NxlFz1zevlRUUdg&c@CoO4I2@Msm`jr&l>$G$l=(!SAau*9r5HdEre=FKPs+Il zR`4>^6TY=IglW-q)KT9sUY>~9vfVe&hKgq__y|>Cibek@vOa%Y81i>&dgUC(C>9qQ zTJA5!bnOyaHCn4|=3&(6M}?htajMfpmC?TqsqQbjo&1Kz&(V zHKlL}U|e1kLpc|i~KE3|`N z8DhtRWLp~4g@F;`PJQCDBFkHc=$Cr>?{41o@7;ehA&$pY;29-&fWscEk0`R-PPtqwNwbK z^p_k^bQb@G-6L!B`P%dK0XYLSC{OTXO-P_*Lhat` zI`{sh>v+4&scGWu)fZ!)wjOV^T`XvUaVO~dUAyTlu+JC-p7o#nG2~`9{0T=$V3>$y zO)~-{IB~rGkX)4~Oyf~a^pI!fbcDqkAE_%@Krga&df6>~mdC%_6cB=|0$$ElFye(_!Zm^M*4S)KoYvCvT}Lpaa9la}Q)hCW_g7`|Ld)(p-(8{Sih*);D?oe7H~$ z{=L_*y~5&sPCvOtIc}IWsR&IGiVhdVtXB0lgt#Xqc5*CfR2>o1JAtpGREvomi@ND|bZt9WQ+h;9Z$uN2OF3 znL30!*L0Z9)OIEn2{+q5)i;xpn3^m;PPiq$zRoAOsH}N8J;2rbL?;oMv+3 zA-p6(y;rcuYD`V9*SXZ?4yAEPdQ*K9Rn?8CvfvLc*u!`v`-1m`q3o2t>o$Kfmv^o6 zU?j=CM@@?|Knl zy)>kO*(u$A8zmPTM5f+SZliq51S)IJWv~XE`lv{u*vzJ7mpozFib`6FvZz;(KXEG6 zC2#lgD(P%Lr}ITiEhyamtI*5;YZ*dI$~^?FvxunRfAw+QQB5vO{7ZsJi=nq55J030 z3IvpnAs|weA_5ALj&zhFr+4BRHj=bBCWD-s1TDz0S>d~c5nFl?Z2g%)75q25fjT( zf9a@Dg33cJZHf6ti?WGQjBD*+_=|Fwe#DE~O!bF*t3SduAZVN~#PUWao?=pbq=r8f zdGC0>U~HJIbD~6T^;KiV(FLsesMc`7fg15V`k;;q6nL2I3$f!{#o7crPZuOa zi{whFp`r9cKzbpk9p`QX$!Sz|m7wWu+Nu>SZc9J^Acx?TX0uB51PfS{j+Ir0cNsSW zGBCGvBc*vl{ky6KdyZSr4>!X_<*3(73Ff^P4%cQm&cG-2Wf!^q4AP);8BU&QhisX} zfRA0E<9rx9%Y=?>Kk2JLS^*^H(#zXBl}%U&_@!<6H-~$TdD)ABtALnKe!=PqHjG6;tPFD~lNi{j;o;Dx%o}%2IUOucPm}#P~&EF5Q0cId62KAqHT* zt{o*sS8L`Ze?KUTl~sw$`8*nnDH#Y%9R;nH0!3;a0lJ245@XNZ3if-`Q5+i~Rq3v5 zLEpk}uyWqHF2eK;)W|CLYpYfmg*UYFo7MA4e2+8uq8KqTEOvH3ib&APZ;()>oDBD3 zs16b}vJCI){^;w%CCyyH6#pj!Z)vea_3SCl>o+C^?uEQxu!z2iw0y`Pb4T>grkz>P zO`2+n2;mOmOI}OUA@6~_5ya;1T5G6mt`_}V>b=dIyy-5>w11rS2YA-m# zoU6{=J)m33$Vi{=iOo%mC2iArx%pH2n+M;*=H@Osw61Iwo;&lYiYbvwC0$2pZ{%ap zfN4ayonC-a;)PaDP66g2YyXV9>V;dN5*2ZQ$8pt=x>G2Lu5ZtI{yc*}T71#T*ri*O z<49J93S?fpYd%ca!Nz~T$H!rxZ5R%{2*Xa|h;1Ya=_ZFj>_^?e9glWB#@4lX|Z-G@@rId?LGuf}KNlSyW)0gwejI^ovZb zS{Z)Ki5^Cy;)~kyWi2Z+-Sb!1Pfb;J&*^P+Knb^&FT5DzP zMnol^m+~KuY$@D0=Pb(HlvO4Z%$MxH>dg7Wt%vV)-yHQ;j%+EltEx`V(M>7ho6D7? z#f}T@nuBy1mTyU5POuNfW6t;(HdPB_a#C%*`8Gm0A$9f&OLF~)U;KoLVl?u8(~f09 zGGz3~L`GQGOSKLQxo4`Vi>!RfV&Srvn1M2`gFLu%aL7Z$$48z!@YU=pmS%}{C2p)7 z=%1B4CM41Tk?sks!TQIvViDPt+~F}b(X}`cEXP=kMGZ_) zA4mG74TIHn9?f3$Ve1b(zq}tTc;R8gRe3H^nOWCw{!bg`B+j%FQqJ7``q#K`I)Sui zL7oo6&oo6R=MnM_W@@o`-}o=~s+_`jKVM-9t#gn1o?WGuV5I8!+y}()6sn6yM30j* z@sOtE@>1^)GG*pU>&4iE<*`d`>uVn;2kShKBcFi1`;sm0l7UUMWxHF?`&xN=1|wmmiBk zDh@&t+nR%SjZ9@6>(>@+ik`p{=%1Y$6X&Y)&pr556>c5iDSE83?oLN{(R!LsU4(Qj z2A%+F-s4%g;eB!?;$@k5cvj%{(`YZbMbVedRwDiY7tC;4|bG~-=r}Os% z&BJAUGBMk3=SRmb;P{x?VJfHr^!``4^VW)v@&St%UN=_Bw07O5zI7^?oO%K}!wypj z#jOuFH0L7_b{5+hQupPDA&=8t#VNu2hgwnkJj z5PNFl77Umd^m3(?CP*3x!m~DlvUnA5rx*rjqeM$2z9MtuS5HOd?&0kdbv}f5ASGpB zD4lZXO*OVpRR-YxoN_dbY;iH4-VmljQLb$jZ~rWc;uAmtRf9SJX@ar&QjWwz=o9`J zRwK{QAj?t9Q%ihUcB}JoC76P-yB`)1F3MQiKhYUw@=X9(47b_SD3!&s^A?2&b`At-)k<)M#jCJIb z{;C0QRQ-&VkG{pyFpyi5XZvlT#_#EsB>tOVwRiQ-GAB%4u`4cZE;V!#|CZ+sauSm# z?wBuPC)g44f%}3(lY%smlq~ILKAXO;{C4KF-oo{mFJ7; z7o?p$fnjXDQgH9GsxYf*^6hM6Z^cSac%^D|*f4E1!s6h&bzVY*=w#g)p7zAH@M}kl zz;$6KO{?sZ1pF}Z*_GCW_e|R=p1#xI?8MWPx^dI1ZTmp>4kx*(4yC3%#F$E4|xX zxaEq_-Sld&mHWbC9G4cS-aJ_1x8Qfay@Y zoodH`%YEy?*@fX3JM=x@)xXGc)A_SCUBvmYh7J?Y=A4{rn|Mn6y4-TTcjZ;9_Io)4 z_LIU%7r+775r0x4h0Gmo{@i&Gb{I@POcGnH+!NZ)mXSQq7)SED;UGjS6x+woFPDWz zC2eU|Q!#8`NT9J?qr%k`{#kZB7E*96OOw%e<*P+-+1Ta+&jcTa@k1bZS>QoHYI}1; z4(jXvNMqy%VO%dtAUauMDR9MG+nf%X-jy_UgMM6R=NcVUUB^dl|Vf7;zeWN4>T;{&*;waqhO0w-mGwTGl&=#L-viva2jfZrqj0-o)cNHL58s zrEYUn_>yi;YSh#!lob?qtxd8>(dJ+1XQ&T1i{I1Nz)@)GW&3pprnXyO{`PIDyIT)* zA1@1dR9)2uJ}^^Az5k&k@X^F1b6hY)!Dp?+M2uU}f2##-C-mBvk}@;%H&!>M%M8?V zGy5j2Yjy45z&dwlKsxlm)ye5=y;O+}={}t52o1_#DJjzuh&aZ59*I51qlTop^4ylf zNeVeC-|KVS3*ONC*+@429lvpdjCu+42;BrFOUjK}L)t$w9--)XYeEOdqTDTkDG7h6 z16=rVvUcMW5yDO(1KbKY?@((Zl|3SE0XSiRo)Ypv(X>@RzNB7KRRicK&YH zj}nyCId~7v|6L*i$fxK(n~OBTM_us82-DS(&|Dotox1oBa)C@v>k#T?x#2%O{x$DI z!Es$eI+~RLM^pU|p^+}(6q;M)H?7~W^0@vv0w7kGV29@Ymz`w{68iek3D^G+?CBCb zF?|1Bh{J!+=n;--{G7*QGU=d4fXfME{~avr7DLPpGE(1|6aditHi!m2a+q)VI@$aH YZ2OlU!BqnZH8ml`Xq==hzIv$lAGA*-%m4rY delta 14485 zcma*O1yo%5llcXxMpcZw7*QrxAuyKE@#a*DfaaVQQ23ba7+;_mKJDDct8?(^RF zj(_}noO9So=9+7+WG7@~ClBoqt+5a&stQoh*dP!*2!t=qlZf&f`0G}3AZjYe5E=yf z{e39~*;}}ouzK0s35&h_kb79TxLP|ph){B{vQv^YHJ|1%KB{|Ogh z`|o%K{#U%_E+(G;GWl-{4tBQR7)tU#=>`Awv~p64{{3kFZb4DuKibjE#o~`(Sj+(v zHw#ftb`DOKmk$?*2D^YD7q1|PfH2$t0{+Gco0|!~#Po*&7YjQF3+JB(oPyjh27ed+ z>B7R?+U=!_u$a5M{NYMeRfAuSn_Y>EQ~fXQKM=sbA^&M<=51zY;VSyKl)nl8%Kj5; zW#M4)!tdxJs%UNMVxjU!CuHMd<)v0KF;h|3HuB=Ud$zg86%VHwH$HU2Q#=>vO&dY25m(>4p^QYUtdn}9V?+)PVW^G3P zM|C-x{w9BU-rODBUd9S8K1w!G;lEl_@Xz}HJM|X@_Man#=%3>fJUsmGaS3b*Nd~C7 zZt$W9oYo9^-H(f+OUZHK(K<=Vy-TA)K-sFE%l&NJ7m004Mq3&D!IsLOJ`N(JfR%^V z*QBN|E*+xk%o-lJaGp|tb|g>K;05?2#d$Txd+_gkbj(~m`jS{=4GFf#lpISmmd+l3 z`UMErkQJugH$_Jyt%@3`#KOp-y!wE{Pqx03p|k+{8KQhyz-;AME!%%8nF*utrD~0C z=AgX$7a*c1^I?0N)0GKArY^;MB%mH=o%0{nP0v-!CGM@9I~Q+586#p;NIr(eDl4gA@fy;Pbo*weHlOVB8+9Hut z#dKavHAP%YeM_FL$kwt=ePaFIUi-kVX1#XZ02K)2=Vuh>=T0B)UiY4T5-)Ag*tgxq zyI#6Wj_AkvCgm0Cl!>&n?`r|fzPrVa7RK!Bz;lRXNFI+{Yl_k1C%i|K=*!WcH$w`D z847f&;h|8J#Q0S$-etcjm3O<90QL$Aqd_u^EF!-TR9ugXVA!q~#=M*_CFW#6H7*H1 z7egVKvpFyaQQ$`E4_kA%9&u#OBao~Jlj(Jw5gl0uS&*~(kTL*kUI6flNL~8 z7jLe$JUoP$;dLu_-mlVipRr7>jA79|md#~$TUi{S!~CvN(}-@m-t=opj4Z>U9@#BI zW2>7|i7eO(eiio%Ky*O5_CD9N|z;y?i zM8HH6vxNIt)j|V{&g#2bWQayCrd4){L&qm+n3J%q*04bpaxc9%y>JL<8gI0cm%%r^ zexY|Gd08pTw4ZSha)w)y>|BoQMR92-KG$RofM1=y*-m>4xL__#vO{OKzuvdkMGX!x z(i)rKjG+&9Ip!`X9&!}y;uCg5rK@g~>y;&^aA~HOs1zdS#5n8syho|**uPsC5PX9f zE8(UlO?@r1=Jiat*C>WVor40VXJl&8Rk`P)h^eq|z}tt_)GOF4RNT87!mHfa^>hmS3FsQW`|NZM`h2Yy8$?z>0QMT1O(KA#jodF#Z*boVU zPh?1)&lfwezp%pv#rBc$W67iDbM%u*wz(YfpT7Iri5+HQd~{S*fP>GLt&aNkGQ-lN z0D=Bv$di1Ta{Y55z1)HQ;E(}><)lS^kFGKG+D3UBPxTKGKrg{!^0XxrUe3{vxKB3V zOa`5TwpOahs4|cCTN;=2+hOn)aY6woDxnN_xn$;Qx=aERJs%CO=ksR)GQjKEq6t}) zZc%vjhoh`HaqN3@k>Q>F`;+6nF*1N`pz!we=_K5)PR{CUrZ=yzHVtrSz(4#$=#EJI z*!`(w$Y023u6oscN_cd&sklJ>?QQEbC*kHd=-j}D)_ zxzdG2MdQfk`4?A|B7OiqrY@DRV-+r$;m4C7GRA1Xa()iq#^R&}`_F6EXExiDr8H$W z259@gZvL2`eo`Xa*)~rrfE|<@{eJ!=`baR&&%eu-#US@on0@S|bn%CaC1QI)@6Pe7 z(9z?z%Kej-sqXVF*&%K+u>k&|8IPG89qk`MHqA8+!wDv_tO+G?>7zsY>-q9JX!TGb{Y7YgP2t z^-C&TGnbiNu)2iRE(wD82J<_ynu`W`&6RA&U8u4eyhhVNcO7s0$VJw1hT!RDE^Qj# z#yv8V>Q8C~w){d*Uz(ycw+nf9v-{d%RTry#omUs0V%U|DKar!jdGC+kbx{;v445Jo z?jShe9J;imuh0}mb{{O?lpQR4w6&^OJuGJK<7!g5V&En_DUOvc{(Se`X_4#%g-^3+ zkg)|(#edcU*xx8%ik7%$?H4-tTU91za}B2uRF!r|jdZI8ehhOrc~Gbih3nOIEmNTR z(S+2qW;SLKs2&bR^dItNj(a0no|%I+B2Lykn%ZNRrRwoIR1eBbQ<(4kymOpu4jlSAl9GRSB)p6^CWTl_FW0rj%x?_!WKC$7x;k-F$H|VZ+u(IO?_jzPU*Ho>leFUx_ zp9ZNm3EVEH-OOgnXGq%h)+gYEdf-AqfKKh2Wi5e?jU`*mc+KnN;8?C zcg5_I+|Q=mvhh+i)lq5s+Rd}=rU-_{%rbVBBbC}RgQ`A1QL!M8K7?=WudbD1G|dZa zpL_6+PVFzGMp{7+Zz-fNllD__eLW78Qwf?&^9CgvLT@BWYXoK#Lmz@IO zpMI(3)_T>c{XBnXb4Y^A#@ixFI;z^)S8&N;s;`q`ZCLRdm6U8~h_zNxly3M<;r&&u zK5cv6Q2)c(W8p^q5aNXA`tjcL(=EXGtBS2omD_zByD=-^eb#H9q)^jw)caiV;h+1~ z*9>|m5MD!S!Fpp7%JIoO}wUuVpY-VtoSSCBCRjF0~*B- z0!ed4M@)(p3dwUNN@_u=h`66Ge}$wXmZ?Q6g!~-ba^hYRt9!y9g{1d*cdfan-Juij zx@c{f+NYc73{@I9H`RH83f9-D^`2t*L?nVKI6vAX`37y@kcf$C$|!GUY5@yGu5%ki z@r|6Po?lJxYx_}R8Q6V?@<~hr#FnWCjm(8-7^w=(Vp2 z4QOzj{JE$mxQgdm^NU0gZP|0r$oiU%2#KbOG*0QHD|q<)7*R7Fp)dgO&w9qVcw1|r z+~6+ISi2OEW?zkwrrEY^sW+TvNYHb9)QPY>>K6JDn^T6GN%W?`f>mxL-NoW#i%RgH!EYr*v6dV6pgM@^Y`32Ws{L-_G- z5jLr5b97zBR(KvLHk~%vPtB9HutZ#+1bY*r-nv=Dabr{}K7CY73pU7z!;4TnQbkoC zT1`%_hl&JU0-x5d*meW*T(Sqdc+wqnr<*!{fQUU%D=rkI3dsqS{L14`YO|D3P2S@5 z`k+@q!_!t=G0yaTm+4oTYkSxl$I=MYCuiBYO_YyFs^M9ojIi@aFtNSbh3uGwOyCiO zWWxR|b*e&)=;*6Yr_mxqy%`&-((`i|s=PlIAPAXwo51o>MR4Y0 zNYuqqVJ1afQhtF4HXnnqZlp9?%vQqGL?58;m zwnr$pTtoccgXdZ_GyQc~*+DGJ-nNQV>MqLB1k#SMZ}PmZ1?J z4H`MFYdP?mKA(iD8}Uy2H|NYra}V{6MMC8fh)b(sx}O2og0i>+_hW#6T8jJKR-T zyGYdj&O1tY&b>yiZxOwb!F?p3wG-K0Z2@w35LNQ%=nBcS zIe=Y}fEDKPEIrY(9_4=-(y+eag%6zxwdBN*22X8=1&{T`XrXc$mqg)quVCTuZ9<`b>zV6y$5zWB51nTl;U z5i++5qVaY4(e@gsTTzj?tj@9%W2XDkcz_JnxgEyYHGV)2&dRJ7n@e_OdS=q&HsL$i zB;Y|ACQb-)mylt|o9WB^CWnYtbq7IBAQN?5?8@T8U+bk?^& z5sLBLQnL^;G3qNuzV%DTo0LJ*=oN`r7=S}SLB`H03RP6qW|gW)N2ZCC94JO~C*G!@ z;XrzYoh~iDNh)zv%`f9XwdJALBmvE*8u|81E4$;#(DGB*1)4Gtx65 zI`OS}D)C#Fk*ZB<07?|ks}F)Fc53#~@bH(clCGk~!&i>MJ5O!OzLdUSS!aNXGD~#2 zaQ5s;1DQ6godB6m)#5fgr8mvWIQcU~oi|qb?4$_!GH(MQT`fn3HGrKFSWOl7kjP6lUqI0+vdczub9WJ+1D`m^Xlu zxH+#)&ohBfSsa2$BS@PV0Wq2OD}$+PQ&n+^UEf6%^|op>qyQ+e<<6Hn>Nn>0j=Kri z>T~AzS&rYIyErZiPa7(MIKB+8{OG-4Id=+#v zXU{hyMDpArR@Yrz6pH$AnC55srJJ3!K2#4P6sT<|wG7(ee$&DlbkctOPz7ZVB4*#<;89{;-1wL<9-?PRWkjtZN?z z+C&Kt3(jUg?A6NSd(g3E@hBx_sX;{%4Sn`}KX~f2;@ZGC@fYBH6rCab$#kKn` zh>Tq6ru&9%+lcup|Mg%An_Kt(jf4 z$i$tI?#%E$SM=&CsCvt6=sHzUV)H8$bhU5eJcTB#r{J66-ApNvlnqJHPAPZmr}Lfu z6`|!<;pKEsYbl9$>qB2h$=%~bWWo%mwuvjVKqY%P?>kp+Pc#AM@b}_Q+^(dWewVaa@(oP6$PK%rn#)j0@~5y%nNTN;O++`rS85Hy${s9=h{m3+fZJ7`ktZd` zzsztiPitnj5+<4L%q6%EgTWMYm%AhYkD>I^E>n-cMwheGd6ws!&{3Oi)t2kGQ{Aj{ zGkaT!i3gZ2jI`|&-KKnvlZF;?H}zxAnZPi(aL_6zl{n~mb?~{nIJyC)4U2pUzC3s2 zo9ouZdk*>(XqLG2xRbemO2fz*F~2hd3A1RiGMIK60Gk0S!@URX2rr3#)!!hU%mt zK@pxypO?EtII&Cy9}N6vKaNwFA)7wzn}h^eVML$LxSABNk5wU|tS#P_i8qz@K~7;g zI6na_3S8kB*RA&RkgsVF%X-7=wB7IKHX`cE`iD!bv>(@4$ z;==-a1$6Ov`i@dAE6cTCB2zlAe~=L8qjmsU{^%{KNb^ov@Gng=KkRL9AFnKJJfZNl z**e-4zPISSKhV8wZ6R!CFa#C42#|ozLu^&>)hL8=Li~W#PBw+c< z0HF~Ru|XGxN!ijFLDX2JzE_BXnMy}~` z$jaFAQ5;7VrnRwogXc(2ijUa+^I$TOV>VW%c+7{1StKfFj}zCOoW6s^+BMl~A?QL; z|F-WiQaMGZO`$os#Xomr2Rck`b~!o6)6-r`<=t1JZVv~df?mVv zV)V1WMs55EowI8bV$BXp$1dR3_`^@1NjlJhG@X~JIrY1l)_te zoRNVxKghgn!<^I_f@g2OZS>bcb|x_Q9yaW{?5^8MNxP9zVkiPmt}tWBezU-SWK5*c z_>A^F+&#ishQ$=#PwQbzYLg-bQ`A;dMkrJ>J;clK2raXY!b@PbIMd@CSNYg0t8Cq( zuTOGi!kfq4h}0U1u!%U6NVe{|wk;j4?~}gJK<`FvM$~&}XaQ}B1~e3)Y&s0DP(|PC z`w;ie(L%HmOY?ApLez6nTRIRs6HuNn<1mA7WJHK)Zv167SrwyzMM(OgQvMo_k1+PI zpP5yywz{)K?tb-+8|0_qX_;_Q^e^LQ(1p9g7AC~$o~aq#wUW083b$8Xekj=PI@-h? z*2M@s8{P~|wPOfh>UGpB5@6Dd8&tUyLor&R%)UfH7aK57un1@~R{84xknG8x9h-S5Ie+qQhgPn=!!mca=EO=)S)mz?N_I`|UDVM@^jm)%%Rj0km zm+!(C;BkdQnfoaotRT{<3FkPRvE!X_3Tt>k>MJxDUgl7gW02tF~0OS3ATN zwOObZLpZvd6IvrsSR(;^D!~PNer78D)!~A7;i2wZQ0TjSPrB1&NP#^g(8~Bw@wR6g z;njj8ryc66OT@I%VFC&&ygtkHPu4x4;&(}(^TfcW2>?wIN!(lp&F~y@u?w-dM)uZ` zxzi~ES@BMSd~W_YYI+C>0vdNSQ&C;y$xKoRvXKs?t&`oGRYOG}`px(Aprmkn!}YHI za;dfGnC$P$2^V!P>sk-T-(ev4@A{a$EXB_SYQKQFRmtDC&?VBsER=R>kqe1nIDJ=* znGw{wzLjj%BJ%l0wck5i?BoB*KE4P)10UW_3)?AFqC^9#8UxkQEq+h~AcsjzlKw$H zbJEDL)bu5^kwhF77-b5lgOohOri9`AjzJ5vMtR1{7kMavfadTM&I?w#m(&^afF4r5 zFmYPAS7)HXnS>t!PPsP-?6gKr%#UCc16K8JpXA|e{k%N0-6PMRmd1+Jl0tejB2r@NQCMZii)kAHkt$kM6xN=Nf0?k;*w8aN@h_7(5p>yN!LMjS6b8iaXmUyF(xrGaa= z?|Tj65mtf*`(IQ>S1QPE?1YhMrM?i81NL}wnGOv0)fXe}YsJrkSs|Z`9+Zr8s(FXa z(YHjATojt6P6Q_}F5?=Z7T299&+FHtCiq1Y#vAj6s=z<68>|~JPk_+Zg>>k*7{Pk^ z&AhV|Jka4Wgpf2{DmTM8d-V0^^gcf^d!xhN zro&2z<0oom?>vvfme{gmwS79D9fFq=bmed{h5{I_sAl*0z82!X(JtkcZVn4%)TGvR z!F1b{?=jdtJ8&`PBgQ>(Y;t-~>8dg26LvmYKB`ze(4=35Yd0~#iW-Qr)x$VQx=DCI z>+3wcSr5Ds_Z@Ohib2fjI{k2s4qP-<9`}z09dp7=<};K{k-}1@X8rx1a8Hb^eClwy1K)&0boo#gr9kzO`u4KR#KRwh|SgV0arSL z`U3KhFmm|}cIC5eQWqqaX^x_2Zqhb7*OC!g^a>|M4*tOCw|KU=lWPtNz@^*Kts$s) z=famoF@nx6aumYJ5P~QTebGyX=OOzpCW`NKNZCj9(O&B)gh4-r!P<6r=sQeW=^F7i z#fURS?Fw$0R>WSY(DRQU5KZ3%p?4zM?WG}{_mg*znE4q?3PY)uDn_s|yYlkOo^B@b zm`Sb^c$7Et)U=C!QXP9}(S}b%B*4+IHfDZPHr2S3oMj&UN>E{C?W3E&XUjQ5rkKwB z{EU0|cVN0Ecl)0O$wu z(h?e8S;yJ79$WO7q2~gfk<~gft)3i^fvNsPP8{5Sa z^vn_d7XbJNq1xZq*B976w-9rF}kpt?ov+mBxs&89BkuH9n51iUzhvE1=8TbD`fKIUxyn;9pTzw zWh`WXa+nA-xe3WY@rqe%erR9qNbzOo3se>TW0G$B8j*zRBPAl9kS$G!l{{2ga+e$# z5(fNfvhO13y=EpocQk>x3iItD3k4*CgRZ-fV{icJ9$^3-HUas(ZKQ3w;p4f)1{X=j zQE0+1O-}}(<`Y^HhgNm#iVjamcLLlF5qWBxczsIb81qkkcb5}MukYmU;9Oha^o^GZ zb5-81Y~<08iylR&4|I!5@S2ot1z&VDhgDKs>Y1mh(<}ouDdnszQ~VegVB^Jrp77jw zHdM{ru5M8hz^GenSah9H>Ae6f`}L>5MWU~c%d>QF`@tv62Io1MWJ|-&9pNZ$1Nm0( zA#7&Em|%HIiH{+|KkUws?X7GA@npNgT^ppUxs zE#&U*oQ>mTNBhxB5Gg1hHez-4ljrqXDc3uN3dy8ovLiUZtO;dMMcB%j<^rWh08T{e zqi>uo%;q#)F?;!3U_TVF-(Fu$L70Q0!7&9o`ldp+;Vt_10`$bU562r5iSJ|TUIua1 z;i2f@5>zqGNKrPK6$fC@X>>j?x{^WCeuDV0X64h{t$oYnV@ydiQOx+-N-N0aI(O~P zny0*u09|D;el~kgzBaAJRR;Yg394xVhrJsE`OX<6hK$^%@{|c&K~a_K2c%1gv!9A< z9>AidT#0+b?cragNk>EI%RHX(sfroUYHYu6BG9|Q{Wi-e?mMf37*!gI$xDt)fxb6! z%MgVOGG2yD$z$7k<>@WBz8&|OMqv0_OS~2m3%>2fne1I&fL+YixP(IP93$ztDQIZI z6#&ykqZg@HlZFC|{a?kCf`*k#BcbR>zhd2b^y`#iD=|m&i%MyxY8TqL_7pPj^vj@I#i*A>VkaIx<&b-<88ltd`}y z@wbT`OBWn1W-!7!QX#$BaJ(&kq;e$8D<*2Jqol@pf}KqT5z434ZMuWGf^!U z1F6QEX%JE4wf5IGJmAHrA~ZC%TeR^~=*{slW!Xz(JgCMG6^4&Ai&{d94fj&_hPR$y zR$Blv;AKW784WS*f%EoQ-(Rnn&4BHe0~|xCbe*fRzk78Y?;CEeOjHzp=o#GHGuPq# zD`&=rGmfnB3Tlme0TybmjHr=U-Fm)F!(-Sme!&HEdaP__$)5eVxKSghG2dy`BkvoO zCLJGh2>P8!oK6}twTTy#wXI*s)!UJVc16;p{X<3Ov;hImbW zWAACicYfUF=qnI2gCxNO$);RmgEs!sAN4l2bVdU^7eL|)KQK=h7K4o^6WiaaMF8@$ zpASOCmp%^<*ze_`1d7qkGSWc!KXl*awR`@28;gV7vjB2uCmd`FMyocNRCC}k7+w%O zf+&~?a*q%HR7>zH@uL0Ka*7eX9o^W@0<@lFJ($mt?XL?1qr=pr}vk=i9vr9$T*4 zI4{cE-|XVZBn%wf?>ZKE7YcDCFAE^k!M#oV=`-U9DGWpALbqo!$O{sev-;I_%mR+{ z8cXqgoPwl4+?6*cUZYB=L<_ODg^D@d$LcmyaLo$9w~w4bsDz}!?{M_p?76hIVxn+E zkP;P|urGrv90IA!7`lk;`P|r_j#`UZOTcqBMkdEEW`s1MshLYfyLSM zz4smK!O}5a!^}qfYOmHf4RR+4WG$a3q1_PTL6+;#KBPHtv@?KjR&mKjeC>9sQ-YG` zk?ey82~PFR8Tx{h3Bmu2i&42F)2%)SOFk)o%y^C4ze^I?=(%Yb4>Ws$CEw!Ehd*f^ zD5xIB>g$E_{cJo)GO02NMB?&V!N{CLo~6g)ZiXmPM9h%$WhwJpY4U=xzBYd|3oDnS zFQf}iw&Cc`3)PPlWYTBo;7>&V5fURY+865a{>tm4U}8Qlt17X)0)=YMBUJs(8ACB0 zXQiqQi2NiPv{6S<1w36qP#F$a$-Ztt3Ze-MpJ@)v{?04zr->dcI#ahXEE_?Dqo0%& zqgP8u!m&kTD69KvzT^{10#!NM%@9U#Z=^$vEesvW$I7A4Wz~@012u0PsXbHY@Xynm zz;U|YX5#A3H+dh=*0*<$p&Ivcy1Jh)t1OHG6e#^LZ6~+r4!}>B=hURlYwh=d!1raf zl?3qi607ckwZ%71o}h&NTvX@I#KI@!KlG^To%b*{buzd*<{U48hFsbG(FFAAGvhVG z^1((Z==a0`XuY1gy#*W5@@b@S^gKkR@S-8xf*N~jB_3+4W8g$>L}@0UJ9x11)QJv} z+=6{r^)Ud%vihK*>98N-0m7IwIpB7(SD!`2l4kEJ5;W>7r)S=94l(i!Om78|Aw6^6 zacYvI^+7RJbZ2cA@mX8>6Xq|>?+{s^cJd%{G^t{cH@v$3rat|Uwv~v}Mn5_;1 zjm@9WTHka_sk8~ApU#X&6f_3)RosiTXOitcH0?XDQ+X95Y4hd`uu+*uFn(66xPxze zPYt+49P$s$D|iSCqY)qM9(!(`5HvG)?O=5nqnoXF->R{X^W~td$FFV5!z!3svW-^} zM0b6QBkR*#;H*Ut!1EALUF<~K1s-UB+<+o{fa>o*y(NoI)xXg7kqvA)jGuFv)OPtt zFHyjx&S4t8kN2t&2F(M*Xy))fd9^bNLI4GW5C0p7FphTwa6P)nsI+6u&&2u>GpAj; ztn9GoSrX*^=^_VorM2yWgb#jsy7A8&n}QRjptoFP{N=hET&kMO-@F6y;3)z7z zLC^V%c3F;f@tlzk#lYrMLS4}gb?oO4-VL3uo<6dKtG=&^KzQteK7%sfCRD0+JOQ`~ zJe;h$8F@Ja6}-v43G)L~o#X`q55=COhVL}KhBytVx>eri8ZAe!@SK(&S%6YceJfPEZGwN9ED7D!Ny*1o(tcyFrVhFAta~Gz^y1l_)ueTV4kCk(O zDwx5hp;g0t0Ys#^@@@;P5^-GLk5NSsh`WRm#`hi@vUV`cNsoX|p}FJ-#d?OwhRR;K zJ}rOufF0u5*+7t_Qt#Y`12|c$zC+lV2O7`g5evKbvU_)yFP)Z&JiIjwv@;3R{?-p- z7qxO)>{(Sa@1}}z!BI9xg5vjI9us&E8TIny)AB7Cc$v)5Y*1Y7+{5Ng1!}>n@Evby zF|+4TYgg3h$8ENDL8O$ZHGOW<%s)J~N=XuUj;K~pwaz5-pF`xI)&Ufp52f3Bt-x{Y-2cPGu3F!*1T_dL!r;-s)|W`d%|gkG#Ou*Lcw(UhTJ)!x z>$yiKA+^J<1kbrN0ccZZkV7soL%%*^C|rEU&Cb>eX@gx+W({z`NxRtGQ0Y<~I89%L ztqq98_c)}C8_5F9H~m^;R=HL|rHynr%R7g7{J|bH+d1JY5P_p@lW@4~J+$zQU*9sX zibgn6mp!=`qm{7-&b;q85}TDtZJRhC=#~wI2wVu$)lAR10<`l9UpGq{BUM%=ZFDJ@ z8_puhk-bdPWyU%mGVo&&z(fhXSd>mPk~gf6XQb!^OYV3{qIq9Hpgvg2?0Id<&*F~1 zg5Zm`ti4s{B-`)V82rUkAp(;Oh0ldH=F!uy11Pew zJBJ@LIz6B;rV|vd*4g?n2*Pk(g5ds83cQtGp4;7wdZD^72rmtk)d*x9jI{Ue^)V2- z7yDC{h_l<%u(o?v1W+wC4VZge-Zs^UOVa>~7H42IK#TOHL1=Sx+tL`?q3XLSEa>dl z%GUXsv8H=5qm!9?3U^>>CB4$QP;D~=Vm=at?e~)rTv+xP;&y!U`LAY=0t2kDPUYK{ zwEkvPP#{_#sD^9^WT^k|HHUwe99|YB*#5mL@q{+YF|w^NHF5rX6SDj(W$9{3)7V?52lu5z1dWipIE%JgL^>M)} z%J3+Es{6Y>e&ezDz|JrD2R+39azp$dZWQ=lh_S#O$|(PfWksI|GL-Md1??Xme<$1d zzyvD9jQ?2|zu6%FqNMu|>T^D@gaOAt&Gef=jvuU|Lj0GfzuVH=fQaPfEjP&3&HLrt Z(*IH4;5q{$NPG$KcLO?D`#(0k|33l|ZFv9y 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;