From ebe0285ce7964ac1a81a65bae417e978cf366466 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 21 Feb 2013 13:06:56 -0800 Subject: [PATCH] Checkpoint: navigation state machine as discussed with Lorenz --- Documentation/flight_mode_state_machine.odg | Bin 24059 -> 23463 bytes apps/commander/commander.c | 16 +- apps/commander/state_machine_helper.c | 397 +++++++++++++++----- apps/commander/state_machine_helper.h | 3 +- apps/controllib/fixedwing.cpp | 8 +- apps/mavlink/mavlink.c | 55 ++- apps/uORB/topics/vehicle_status.h | 86 ++--- 7 files changed, 393 insertions(+), 172 deletions(-) diff --git a/Documentation/flight_mode_state_machine.odg b/Documentation/flight_mode_state_machine.odg index a50366bcb3e51c2c6571597de89d7b00d746b28d..cbdad91c9c0e539eb5b02b700e4b9d8eab5efd50 100644 GIT binary patch delta 18537 zcmb4}MNpm%5TrlcgG+FC2=2k%J-EBO!;8DS6WrZ{2KNv?+=9Eiv-R)pVQZ`Quyg6T z_f$_Gp4Tpj@^%OmWjQElED#7D1Y$_kNJ3GB`cDi90EL7c7Od{aAEci3Ms~D_I1Q?( z8XESkY1y(p5M*gH?Dc+rVTjaXr=H zP=Wca(Fe6|h{x$J+3(*GCJ@xz&N^G1|JW1oR&khpSumPUu2_T@Gh1^NW2Ca#aw$yY z%!V#}2KJT_p37nTYKY)N!5ma->9qcV6^wFtMM$ZC*&XoQag}35H=XN^NM+IV8aB6U z%LF{^t!LLetZc>993~qNYFz0`XUe|YI|w(#gE=B@AA`AijvO@ZmNTmYoU&gpipp!T z-fOJ|9@0)jENZ&Aunu`c>P~CzcP)RHKW>?z0GoAfekx4h^E+y&(pghsH=Ue?B9mE0 z-Q&?Eb>m3&TV=U3JZEl>M&9Z>BUlU%$IxPUyv3 z!W~XJylE0JSYgd9Y*u$n6O`6M%m{*()vru&u2w&bp5c_`;NYEnzIe|=fI#-pAkhCZ zA08h5Kl4RF|HlBxAP~sa+|A9}!P1r4%ieBZ=b!5i7h1rjVbb+op+70?O>q#yI$AQv zR8lCc%lW;+GEzlSE3wR%?Y?@mt%)E7!msXpV1Y#O^j=5L)>Zx5&bK?9#^N7n$>fgS zt-Dp8&;tF5H`bHB#2cE--Ad;q%+73u5FBF2$?zRj(#n9WQj(MpZ^rcgZh8j%rZ{<} zoeM^gUDeMh)~oxbgM%{**)~FN)6~9}?krVj@Z;g_^Yz;v-tc@qCq9ah|F3*}q(uDa z3iU0M&>UMXYp6X;KBnlV+@4Ni^gaZ897b$8nED^p6|_KXR#)-8YSnuacbge1OxC5oO7)9Hyl2bmI!Z>B?|JKI%|@AjC)>fzk1+bk zSS6y`@Hj;#B}^_ux~V?zoQ&3sS5CtU4%kz8Qi|A~d$HEYd$EbMrMmgk@;EsF_egBZ zKeIyqPUpN^hxo%2ze*eDL;F1=20VA=DO}zx{|uZS(!}|wyvd61AnucOi*!uG+@77w zE?J>s$B<(TcH=rk(WTAyZnef)&Yv+}SN}9tsyl0h5t1|$?pVr4qb@}Ls1@$D-xd4$ zxR;pX6&g;3Y7kXI`4T9H}>Xy)lN z_g(1CBfS`FAPh)K_5gd(EAey*1Wk8Z&XJW5>ui}2}>pF=P@ zGl3#$h(Y$0U?jQB$AI10h&4@c48FI_iv-%$_L0K``O*(9;N>GY;8DD|uCmU9ZNsnl~s@)#y)x01_#{gVP z>ox4vK83cPn=-`u78Xtge@~KKrmYgMrq)!>0s@ri@I}fzdmXUx*3W5!E};1O&tPK1 zjibLQ*5kHT4jq-tE|+fRO_7qG%G#e%c;9bgUw%eTeGSR!3F;oL_7Ho3MyT?#wdPHX z!QdbzA@ER%mLByvEKxQ?p=S82z{hlK?E3}Ke?n0Cs4(oYq?WV zL^3~$<5GaXac*=al?hnMWGyZ&X2W=tEQ!z5@QTSPW+sNxDe$YWz(~{vSP-R2OW)Hq zE?V1+`bw!AIgVx7d%YnCds0&QC0+?r)ghke%U@3Yi2R1U-g3@`P$zx0vjb>0jW@PK zXlIMV32{%CW4))-Pj;&&ZpDX_m2^(Z0POoLN2ql#@pOZcY)-c2=}>FT4j&E zqb&bUR}@cU&EgvvP1;!Gj>}EPYSaHDcbO(;W2S#K2XBVqSCdFO9;cj&MeYfKCHqA5Tqw3om0`Da3Fz)JrP?o4Ay#0MvhRw<;*yISma0nUMWoahGBX zX9%#;#N(SmP(EppGq5w?HgzLXFImV+@C}caM3w5J=?VP>edLpIo;Oxy}LR% z2E1JVd$;xn`ZA~gZPZ#Sq24Yq7u5dXf8q|Q{;}q{e}u}dl(W!v@7A^4tE)hAu;$sd z(sjS<*V=cY@Abg{slU&^wy|V>IQc%ly#RDQXXr1*T5TzHGh!uPy3@6>v~9-JQCxVYXpKR^6aKvOVm*7NK%nd{mf z(9*lD?Z3GTwmAOtD`#US!}YD{oGGVa?Y$HAcBAF=eYNR5S3> zk>$9J_0&n5xgki~9X_^Hj+$3`3XXuDYEA$vSn}S|sHvcPZb(S1hb;-pdy-(@+ByP3 zBAgXxruS>5eVd(6i+$G3J5+AZgtIcA-2g%AAEx<$!FplYH|nt5@9Uuy&G#-Sr^o-WKl*ZzF9H0;uxXN86hE>W z&IsGi5!*q`t?igE|GuSPQQ9v}4(nejzU#ZjeO604nQT2O+$Nk(?>O4#-uwO1^9vk* zZZAJ~aQQxFCgR&INe+J9`$eJ$RP3^}p>KV}Sd^l|L!T&Ablg5j)Z?O1Z=leU@?bd?dv+tju>)&TC{8!_~ zs((yX+9Qt-k1Lr^L6J$s@g@m27b^Qw)YE>J^#3|n?9`tLUh@=Q-+LBC{T{&=z0KCR zCS2fR{^{sTyUo8fB!~Wk`RIcDla%~xJ{W(v^Jktn{zQ^|e@&r976AaQclJ}y0TRRq zh|Qr7UrX0@Xn|edg!D1FzV_~sgumd+ZMvDqe>WuM38&}4$L+3P+B=`V{ba^{E#b_R z(A1CHX%kr4{aceRDoX)o5BAKTO5o|Db(BqD+~b=2$$CoQV(j_JLvVw+ta_f*Vei|a zVb%5pDRFJ~5)`>36u`>o1xhM_+4&po`<)cWo>USJl@#YN@M$dx|91hvPRVF+Uio$| z-HP%h=7(=b0RNnF$Y9Z<<FFRYN%RxP9w5 zk+H8Fj}R`j{?_;T3fQ~_qarBZEo}u%ySxxf+W3lQZ2EE^0RYVF&Qd+<<UrW0$N2$6SI(svCjq ze@X@EN3&0u7D~Du34X86FFZmYqy>rw5tq`j*WX@~_U{;QRDKJcmg{#WXe+=~Lk2%B z3T94GJAC4FP+VLg9)zUb7$Pk0+PlZxE=bkzc0y%X1Me*V?0a8s8@+b=>bBXVYsPrM z68G)Ds!=D)y+1k(YCLrBKzSozE#sK?;{_yLL#-YaBK}T*1{wsFdF7pq^yw^#_fp$c zp8%SEcX?mQ(A?KLP(fMf+xfdB^Pa3ha^rd*x3_yzpq_Wv*Yvp^c5XGnH$$#8W1_u& zhc}w)fY$w?yGmN{~5hOtnUx| zXnfc4+WcVk?G`s8#>tuX!L%!y@0#Df67*7lt8CB^rOw3JFX*v)#+{NiuBb`7eCjKt zOuyWcKt~tw^Ks%$8hCc;^r~@eBn$Zb^%-9b^f~3cEI`Mj2l3acC?e+4+sF2ZHsZ>y znY1p)6?woEl|@!mPD)ItkLUnJ2am!30HV_K9N9IW*6cqXrwa1(;;Qo)RSX@cE8qig zHwN*+hJ>xnvoWnO=C&4-VeSdrg;K-)1O3IXm$fhV)VAaO%PG_`xNdwp6aT3_X94)+C|Yl@hdto0%8x z`(HJBLWN$l?`DQu~9Nj`w%d8^XbqY{)NzGc-Cn)f9mcOBCWQ*3{_TvU(Ld5hxXQRwsL zpHKYr?{bI3p7NT6uHf(4liiuL<^=KWh4I{nTeV;;nw670ti5+jsi%-yEQ5ai;A;B` zKQ>e4uU2_Ct{UwVTPC?X-|!F5&8I9_Biniq^vp6B_G?b6_y(Pjse;fD+Ol>F&oh zGgMKcbf2%jd!_|mj@Qqu#JRA85uuvCTzqQ|gS_hGfbSh{(JTb{ZQr&PzeckhP|%1a zyU~xq3o4kTsn_>E+v9&s33#PX;88!5IE7^09HOWuj9!ud z7V&gj3|Uy)Zi8OvL+aQ#danih<Pgmd%i z(1gCS?qqt)ib?u0W_%Ts{Nqd&uWJ1zXWMK$ULw;m1D?8=i49t~^;Bzi#I3bX<2+dpuuj#$$5gl$|1ubudI|4h}H{)722_c`T)IkHr=;Dp%wt%F#vwu{viR6y^mAp=pjbp*5oYg zsUYCjq4ukLllO+h`|>v1Lid!X)|8TAMGkKIJ0ZWGbx|K$phMr(NWj*3wH~m;rDQM` zZ&of&-HlfA>Y#0%dV{Bae|CXB&aQ zlzODxaN_z#rq2B&=}RZ+*HiQHpzT;v>|%N2^!2+^T&eJ_L%X3l6PJ-RwF`<`g?h4L6y%Wi@En9S6S(+imTZB9;UT)TJ zWG)n@S7;>{Y2?8e;^AghCJ+=FQ(m-Fg%j~3kE(BZA`7{p^>^-h55fdfh#J?h zyDC$~KLnXHP%9oZm1sR_s&`5v^mAYfC`qm7Ub&8MhMp|Es{~miOKbo~1-3E)riMFZ zQ^9=AHi6_Baf-^ROym*R24Brf4ai@bT%ww#zmQx=^6V|zk@9Qc%&kANpA@y}#VGg6 z%$+hwYrHHSb8>KQB{fnQBeBV$*A+Qx-Ru7FU|#a@+RJ7@vx;i6$Okk%wRRwl>q#i- zl2Fm9iDV&5_ada$XiEbI<_!^;qS4dEPUTq}7{2U8nGH1-c#5O-!_c&44B@NqDh%uT zPYf@G6}2P_Ba<=ota91#rn^nCe_vl@TzA-a7Z?-IyxRb zrdW+xGr7IrB==Z}Rd%bRdWTc!p9i{aoPJGKRGuVy?u?&{CgKD(wh6P`(!KZ&>1PlY zD(w&)*axvl*wsdMkD}WpbgOgp`LPu2pmM^RFK{S@QgUgng)uzH&$~Qn3s%!*f_T5H zTIw|F;WUly5L&Se8Cgji-8Nth73+^m*mR3w>kMopOJ#Z_?5yhwq)p-=axT%%!#c`q z75CI1i(hn%V}SvDRFmUo`JAb1h|)UNrJwMNaaKrp(&5}1k#eKDUVO$>b)tM&G%SbS z!JM#qcx9i)b%={M_*qfGasXD4E@=vTY>(9eeycKpK5u~pvILVlbl1U?}?#+*sfdJ+UDz*aAvsXUzEAcR zmoqn25u0NI!pdvi6~)NxS9I`@oiCZwZgY#`W&fT~K6fiBD;=Z9HD}_J&oN`sySj2P zQKe^7=t?upbr?Rbr?ZR7b|G#@BkLtAeMJomRpc%cFJ&-6- zD45~QpLT>9y35Nw)-Zk5rd*U1hvm&yi@pzWs1g!bED;DTUq7;=<|xTwU|@ni>&I0; z7S_eWECh?$tJWMCBNKA3oj-AE%QR%|z5ZwN6lvUNqV1+h{&ZKhOHZqkHIY;pJV5v| zsI?A){rbh@*m71WS7-Jn@x>f?2A<0zdcDBs9*rAuC6`c&gnz$1C$ihTf-#?hj*q9L z1Z)JN$gd;Uilqo^OO(b%M}jn1lSkQ1_HF}(`u6xZtm_8ZS8lv3h@UsdKH*ncz7P>; zb|WBbPsyhlDdko&I#oAv;1>XqF#*a zGQ)I-#QFb$HjS>?JOjOh=%FSluW0Ek~|W*4jrhb{B~GlYNe zN59)yYGV)A<`JD#p6c%4u2Dq8P0yQFqf%~_yPN|r&YfIf!#7%80GD4yAiBLUkEW|^ zhKrPh+*}Hp!?AH*7nk_l-1?Dm>tr<BpwiK`-Xbof+-3W|z?Dwa+z(Qb$u96N@(GE9W% z1Gu39+_||H1An&n0Ib6c-O?siU0&X>Y98Kq5fcU^IVZTboCP5oOQg-eTQ;g0vAMbW zcxg6Ie2&4TJB^MBDQdH->VL()>3v71pgCNy)XqZ_%E1z{IRkeCf1w~g$Hu)&!vloU zsDsnXqlqWfq!ecumfY(tR<6`ib3JxujQ6lCEpj5U)KsjNfvdp86mL<*k2ZK-iDI)$ zQBlPt#jr@~3+h^_WN&h}kVG{drNJyyF2~>m%<#Irgyvz+XiU*4b;yOW(UqVCwUSen zT|~;XR6$0;nYuOmR9=a@-?;-ar%Oo*t{mQm0ZW;46Vb$0<}k))U!QsECAJKb{PS@@ z98WxhHKA&LKrWj;1R6JjSwtoGn480FQ|Y-`@sO~+ks_W)5R7M>5q9IS8Db`tnG`-AT4m}-Krk% z4-DHT(XuQ~=c{QITOEAj&6K=?s1xr+dgCzC^=w4~mUQu%B&_t}w;dHXYq;mIMlz8N zfW4=TCxu9J0(#vs*+6YJ{>P5nXm!e@z_bFv3g>9)M{;?OrN?x7`Fl5I&+kUtCh{o0 zI2*7m>WUb+5%=fP`b>G(khyD4w$7h~9xf5r$aJiKeo&NgWlyP=JRjn2tuF0Hvr{3X?hGp<{qNdg`Wr3cLTn6O!8lRwBDKu2Z`{GKU(8Tzn>x!l z(Q6{Q`?{VzxD4YQhMYDcS3xI(J1CuQra1N1cd6^`Nqpcvg}s#WDrzxZt!w|~9OK5X zBS>4FBi}`UetUs049=y^G{jemz*%?1(GA2g@rpNpnX^qcXN~U|{$dHv{td@WQlAri zRWS)0uf#KzI_ew7SQ%EGW?9674dpk&U)`;9$GK>gDf~2@l;q~4&{4K)oSl^1hbG5! zsrh>*VVrX*-E`Y0R#|kivBip*ySWDRNc7%wG^TSS^Xvsb-)#A=)XvwjilkhtZCDf^A6L$!);59{^GiN#zk z%*yB@WL(A(@!M1~l}XD85N;ReuFx|(|FHCZ`6KkBnq;Ju^>zm(Qcub!KmtKo0~NYL zpkntZ2Gkmt55yuGcG7w-6c}OEuGS* zR+sZA<&j^Vs}|!9O|FXXv2L}cGWRYBh#m}k6Pk7MPbT<(+?p%}jI9SSJy`$A6bP?L z+PCC!bm=9x@s?E?M?>a0>|)f9ohp-rw<2fV`&sY~#CEf375@m%gVjbsG+WuaR7vfV z;^Ntz(2J!Vx!3dqq{0yv3R&dIy%U-^EeT=4No+H43%l40-P{8mbq@rknHuHoPrsHO z>9rosP)|4IRvcqAxb$PM??nil^89TkUTa^r-aROLHzO$fa+SVM+C0YzrOSN#}Q}E~hm?U&02q-oH{TitAWbfpBg>mCKJs#3p zJL*4*`HGglE?~t#illjaGomf>WTA9_chZs&z9k52t%XUDZQqjVx8WP4UndAo6ZbW3 z7$C?^4q#D6v|dcWF)NOmAZ)zw9HW7e2iTx3x!&D)uWe1{ab!K6G>C&8Nz^wb8P)l1iSqcf=I})dz8xcgWDn!%7}#ghU?hy)%8~~2M`g}9la~jV zOF0yKCb%3s{w_aeY)umk)o3s+cN!OV(B#cgk_{sQhxy7dHT_DA4$3Tl+5{=pD`A;m zpXp^WLu)Gug6$qJ#bJ1f5L{I3)Uqk;3K+Mz*uzVN*$o0+V98o5xP>E}pnxmdHfsEO zePLb{I9G#Qn=fCkxYCC``oWVXdYLIzd|EgV`#<@42HUpuybs!o1h)8Q!b+7rj}!$4 z>%%($#N|wkNVSFS)u{tlcd3jt^g9<2_o%~JehbX;wkt(dG5jV=c9 z`)k@aW%|751|9h%+WSmqK3ZEi&&;pg43vK*ixVgs3oU0K^di`pH^-ddO~o)ACAsC| zU+RBJe$H|gD3^;(Asm)MS@Jf!w=X8dwzsqbM4*a&Z8&N%maONX%0$lCBh`$GvSurS z)6(1yoWXBB2KmR|!ifx$M$zt~M7d}Svmj#a3)Slc5@>eW61q5fxUl*TyqH_u;ySIe zzjaqylY@UhoStTt8L#`vz(|bQdEJKX*0Lhp1~rp$Jm$9MSFM#D+hS+4Y_#e2<{^Lq zN)6_0J1Y%>v?Hj*c(GyJd1f=K(&rtSH0|zoEU`EaTJ?mfRO+g|P_>FO-h-WrvKGX` z%w|V)jn(E!<-Tk9c`i9h1%86j7N{k*A7itYE!=O&nJX0-9XsCGQgM%!Uy-L2h^A?% zwZ6#p?xXpmiz`%JTt!z|E*+d6xusG9y;{_9zx%t~I-Du`*YmYAQINTVvNGuO=qEu& z^pF+M@hHxfW|-6cCzE^Pcm#?OHg%aMt6DO|>Xk^K?^Ma1-<14zJ~ga0cFI1)N7O+`U|28!ic}H%BlJ)F52u< z+e*rR*6c(9&y>|E!MB4~>x>s1*ks%!u*HSQ9m`8dfU}O$G_52dMse~oZ2o;3082K;e)r45AGPIG5%Ltxi zc@-zhlAF@>_*6xPQOZPA!DXr^;L6lIN=Qt~Nb#dD?$MOSE~yd5`#S(PQM6t^`yWvy zNT@;>nJZB7>6Ik5FA@Ql&!HlY%%e^@)VoTGW_eRLevtAPF6{M8<+11j*AX7-Nh8A4 zt{WD7uU*Bht>Fv(0)2Qq6v5M|@1g>JXg%A*F=j&Sd0%>{){mK^+>uTH;9th#E(dV4 zn?Jx_jPm78TnDnUw-5rT2f~5l579QM(uwgSTXN-|nlooT)m=krMC(LPmgcAWWeUvQ z2{qA#ux}#JRvlyQ?%u764cUvsr_AoxRh@gG{vACx2&dnkO9i+n!sZByl~3q>TJC;$ z(!GDD^2tt(za#T`PZ^9#BD{DmdbFvQqm?;p@Plr=zL=Er>7tP$=8&;33Y(14wBkfM ztE()wZM!gC>QxdSG$#wEE#N3pwg%JYX<);p(tuE!6+>%4dHV+Bt`T$AVV~Ta1>+Dy z<-gePt5{PAnF3)kxm@%1ioq=7-UcXC~3qDpP#4QJNxSP%3~+b-M5yj(s`Ad8Mf*zu;uu70Lhl z!>=#P9_y6EJw$>1IRq%@JUTUaxKw|AQDJv09=z*!oq0dJ|D42wC+c2I-yT z2hlLw7$HXQmuilG9NOy8=Ar=0C?!2LOsmX~&!NH#VJRDd2t>dMWvo zCZfd(#^(Woo9RbE6GE_S=NDCa@_u#K8sq9JpV$LlTuF;IqkR!s1$Z;QqcU0_lhKy> ziiYoa*c!I8A)W!9Dy$v__`wjfDPv!C$W;2e#rKfyg*fdb7g@6d6y^Z=YJPcC@f8&7 zUIF-7=857Vl;1%oX}J{}Aw|38j5F-dxbpM>+0=NmFX~B3D?$t=tU<#0<}9=TmqVYU5%Fh6jSFeb9yAO+~;Irn{W{ zLlSiD>!AAI3tk0ju>{}#SMeW zXjH~VzUqeg3}Sc-2Vq_Ba76pHWtRVy7vJye?vXby>PzT5}mmaJ>V(-5aQ~MblygJlES6-W0R?L7Puf}&T zEiWf;s45HgX27*oU$oOzP{p#u#Yunh{STg$DrUpiiS*2U5gW19 zhZeF8isiU;PGgRS^%t6%lYl3AexZvRkL~?O6k8V2CsqL0IRvzMLETGJ^U|XFQ}erm z5!ZK07WM&4kj|5+DAd10M^@M;IJ<>MJo_CKpMBYY6J2*fqw?bOtM5P!<-@S#=TP|0(Z?v(&I7Zr!(m|>P_afwf# zS+Jw~kQ6vwP$9GcB z-#KDn@DbT*Me@yH|4MIjtoqLpF~WhwZE2>>ohP?-_A5>?XEuv@4D`-+I_b#$Nh}?M z{0!1m=D?Y(z?OtS{|}?nViV=cG^`1i!7xeHX%)a1x%E&ne%l`WaOSPsVC?zL+%MRC zDMYc~I`mcVwSfP7K1Tr6U@dKDKYq(f_3n8?ml zu66)%ymuGnjL`$w9<(z0k&mq$+=O5XHy&>`nZoGcKpi|I_HH(GP)Mcy(GB{R1vhoy zPm4MxDCG+M@s#V$0)976w|a#!HF|aB4%Hto_NJu{o*7h`L&<_?ZM2*cn}j5=96O$O zTY|;>&ExSs=}TJTe35{>JajzW+2WcYI+=p&*!jyx0BcU)U^`AVY6SOY{beug;OPo@ z(64oJ+tlOgGU1ctKhBU>^Vjt$BM3x$0`b4G>i^Buiza9}>1E=8K$$ox;T$3Wj-iaW zh`LwqdEU9}yk!2UF8=f~@tP@tZ{Y9&Sq>;XFl8`tV+~20nT#x_RsoxNnf}5>J)W~SFH|$z$frY@}l=RYW+nOl*ob-ui zHPwANZL#(DyO4l?0m<`15?~I16ipm^_!1f(I)EISG?GIU6f8~g|5}L2WiWCghrsen z@DPk*8wOb`RNfO5422c&{qa%IOYmcA4w{0$nJ{xg6+lTnNuil(N=K9I6_D`naoeG@ zNECRG70eg?*luG^l*svxI0=ww2LQ32%_d#GW5p-rR;L@ZdPQ$xc+SNx(W40H&1@5y zwfcxN{N^t%&u)<^Jt$QPmY1@{=+XkaCx&(QsQHf;G9z?g^Uzd-Upq}XLc>hP<%+RP zE1tk~Z#(4AWC)TqDT_>vAPX`#(>{$*U#h8z6po!xAEo)eBB0*8j-+HR zYzSh;zz^oz%n*#dKZ|xRxk%pIC7>c64=e-ejrtq4v1g0Kk?1F2k5|;Qo)}1}Uz<9iGVL>eag!L$x?I6eGKYF^65| zguWKI*itdV_5Q0Dk^@*418wyXLBlpINXTE7kWx#A>?Prj7F?Ox#t`IWeEJRy2xl}V zP&~Rb+bqump0`S!=%<@ZA+&VEA>8m}M2RFUBa_U^{z$<3b5q*_VD7*#yOjGzGEj?U zjsoV}EXP;g$unW>5dE2FYvi!HQC;dyv41m=Rfo#%3E~gQ_CX1@=h~k)+S0HN&0C+R zvp5~>qgG8u5HmF-P)d6uP|W|STLugqBq@iK7xS*F2fpbR8!hQ*9ccN-NUSNco2{7Y zJB6Pkv~(2^3Uo;Zo-!v2Z*l&2d8Tq8i;a-3V#`BIHv8(uX~p$PMI|>6$5^_FlS4zc zuK0I@Chb^E&w96eyQVMB=%(&&?csfEv+cSha;|F=Ws4rCr%!Ci6>Fd4W)nn2jlWaa zeGTM}2aU(%)^fI)@4%x5XfmIcPCA?5vLbuj9?~KJ$LQqmxqY7?%UvHy-`jzZ!P`Ug z#q|4CeHcg&gwmLp!-Rc6r5(@7x&q*3IXR4^~R7K3VGe0_V<5YEod%rZ+Y=VX$m-a{4E@q zbq#JOp5jlbIQ`7!3@qlW9>os5Ai=|4#P`RiSRw|fCi?qNFgqTeg_$=5Ue zZ_v~k-QCt~p$2ILqlPiBp5*=!Eg<#Zr||f#c$s3@(D&3mp5c=8!&<&QSOqYHA799B zkX1{xRAVI0_a51ExH;qX@(tHgtZdc71128aXnsba8jkhcdc2kwD8wtJ&O%&awZs4s zLlh%&TSnzC5mC!CIOnU~CDK0?{)QivbZa6AvC+w8L+iBiE0vaXcf_zrl0(p554H7t zUM422g3~b%nfX-d+p^ zIU)o{4P^)O_qYEz+B{NY!X}RUpI!iU_gKzi_Md%r-DJFiT&uk~mK*rBv2jC^tp5F&Y`G!6_3KgVl)%eYOhCes78Upw}HTn_ZnGWOZAO3Fe{BB7UOasUGK6(65e+>Kd-U zz!rOIFBQY4!ZJhNV-zeQEdsZfByKRIMx#5d$2=bTl}hFfUX!K(&OQ zdVD`2&P{T?VzeIWD@qQ8f9wO_JF{1IxH-^w;?i;<){7ecc^c6EY~ zZ`Dy*{^ut!FFHVo`OBdFd!RN_f7CuqBi&W?5l^SN!Qi}_0F5#rz6BHe%Dd3$Y9QER z#0?uM9W*DQ{$?;J4f;!`9L>VqQjsMXT$V9*;(^(|8$`R_-*YV)BfL(O@biH!chhHT zll-CG&=%IQ(p=N_%9|fgtQkrDkso6-m%$}X=a&14E(fS=@cqsKWArH}N}S8^ORiRm z4&vaSQxZSdFwuM|5@MHOco`@+K<*;LRl>8N$2UpG7R>$A)9x3R$?1Eyu>9)_Osh!4 z>98F6sqV}MJ+&O$&o^^Zv+WCWo_aDpt4Na0o5XN5VG+=t?GM&w`hfHma=r0c&+_;} zG8ZNgDjJ~E7fAl!>k*?5%+5m$iIMou9RXY5>#gL@6Jn4kSb5v5=97_tgOJ~G3#?RL zA3Sipbp^au+a=Q*h0`$gzv)@LT<5+12+(Wp6uRA_c3GokxIViZnlEn5hBTT|`sa7& z_qi!<$}USOe>Jqh5LK4`wvOcjsm-{T2#lSI0U_Vcymn8X9k&eWRR~^$Xkg0?|A8%T z_4cl?>|=~U*xe_P`-f!&I;+G6B+S^iWA>(#=^o;H?L94-JfAFOe6wq$sGt}s1i3P}!2r|x? z1Cx$#{wH(N0cRe))pFif4e*1b4!BL)oExRNeC(5HKfq{PX&~Z_U$e8R$)<=m6;u#( zN%H#d+qPQGI=TDDQo26wWElxONhss#>G2B*qmm7nV>FL-WS@zp-5S9%9d5j@n}v6y z1@eBrE;!Fj_V#Hp!_g#NF%+F*eV1>Kz^7N}fa~pk2vnX_yrQNrCpOwn6euxf*rq-1 z1&w{V<(R;5B|=XSKe@%_UGofjZuOj!w=(r+|GH97aX3J(+)|d?0jJDzf@zvmJmI|_}CmRu-3_H-`PhXKXeE4s~-S86fjD!W50Y%bjRXK*r%a69ft#+(H0Cw;2 z&Qq@I?n53G*)3SdKmmp_(pq`Zith*WLV%omyh4GqSM>JpYU@liNHlSLM979Cn3d41 z2<)2@L{Z5-9grxB^4}`Styx83-0TAJU(NK2gm$dq%1|aCVWa|GkYUZgR=wiN1t&2D z5Od9hlkG80UJE2ed?y?u?dl)THqWSm$XFFO3 zx%C$h^shvik#pbI28Hi#wXsq-p*_SXLDTeMO!Zx#f7O#nGKKUzoKZt40V#lmK<{n~^+72bAN=8R+$!SIkfk4aN&MFos5xQm*3~0AHg`b~Xli zLUc&_{bFu_;)nanJe72vlOxK49$Fbgd4gl1wJ{WE1r6=K-HxQL|GrzQX+d-Oc-QOV zd3UxkX<2ajuiBbkXTWSf99O0YQ#FcI9!7W>eydb-Vi@uW2E6O}_k7>0n(Ncr z;|p20`uL4x?ScZ!;vh&-fD^%QsDM>?;ndKB#K@SzaFa!QqvYXO#CKYf1JYpkkhJ6A z)X+d668oYN+QNwyN!>mp-pr_Nr)BsaD%1*EAPfm&dHbPMZP>34XthPr>CsL%eXM_j zhZ|}Xr_gc6C!X!8{QO%hXUlbQ#orj*-(Lx;>RSoZ`~vsevhN90yScu&0G^%m_*e#r z-`YP#85IA>3;OZB`9GFf)(8<6G{#8w^YdT}b6$^FJmJ=F`dRL|iK$gJwi4Cz^f;V; zWBx+yU05(f|Ixh-9RFHTp=N;;W>9o|8Fsv@=63v8`?rHIyAlO=+V(ziY3398q&$N3 zgdMmtK^4A`a}8}}W3F@>y}+n;gjB>YjvD~8m1=W}Itu!UUkDNH!~|+gul;HwEsRtL zvxQHLvw4HhO8FD=60)H$fPfsatX|430)AT`^mDQO#6qqE2%^pE4Mc=M1Bp7W1o@Uv ztBF$xiyA@ylyU=LTHs4S(6HhZTiHfP!t3r3WWR9&8v_2P2n$D|1(MFeLxe)Je({h} zh6yBwiPUw7Kub;Yl=Fbo06k$Ni0F$dYi^p8RX_?ux67I)&{k(d>w+NV;a_-r(WD`^ z7L~g(fdtl5%Fv%R0@I?2(8^0P#PA0w50I|q)j`@6cD$9Bk4s<5lfsPP`(IK({$5U} zye`TVM&}*`5x08EG1k6|7py$FvN&G2fyMR4pvHo3VN>v~03^|fu84L{FN7POF^(55 z$jRhAT#cNEyv6!Ge7BRHHfPb1XVV?011InaAm0>3!r7X7VZ9P3=1`Ui5j!32$J{Uv zvV}k+_X|Zh8K?)Hsedji-V}wn-xrLh^7EO5&{#fnr_V(UCT6s859$YoNcOwm3-K^r zTB`-ENd_81sP9&$bfY=FOs(s+b^B=Stj0ZOyk?uSI$ijAEX}0<`Q+%loPOZrI{J{Y!VM zhKLRX{cONWRbE!wA|;Ne`GhzttPE(AcN__BI9Z3$hNiDzrMuF3@8)eaUtBkb?M|(& zoF78L?@9C`uYFDE-Ok0YDD+EU`A_`-fW|ND8iQxK^23l+=0m0C+7FI~OH zK!W^4GIfyV`HUJJ-M0(!*d#TG`LoXsr)1nPW6;HC$^Tz7j)wI+gW3K_Xfpca{BvBj zw25R!rV`MSfJM~GTm%b~!^WGNWMTY|HqJdBs=SZmXC{Mnduj)R8JAKf402slE;Cw% zx$(*+J0@xu<5m&IC}x&rdWu{QDP1ttq{xm-D|hlxa%omX+Zrrl*Cp3t#5}LDJxcqW zf6njsIp6dBd|$uw&-s1N>%7&vg6Lk(Iev&b9Wu-njv&wPf=%@I8ohAFStjE zuu%ru2pTcR7Z2#Nn7PmGs+mC}CEvhPJLwu03qyL6Jz|@S%>i4}B|1jP5=e1c+TKN0 zUFoZNCSPh&ev(}Bk&gL_enfG^-hke z+!$@m2w^-HJwu0NuSWI%T;||y9Yyc+QLp`#7r%T|*jHfH-ij4GQfR$60?;Aq{Pb?RuGJT`G{ud zRY%#nBg1f3*7v2UKTp*xPLK0-ifChwQWlEUC&N&rST0s@4sUBMV2)1wP>^5G*t<4| zhPx0Q77AwV;#LQyy4C|4;Pp;!iQ?V;$)P!mz4Mv7CGQ?P(M;1l;bWYUeYG`=!1J^* z??fIRsWF<-zR-*)rz zg>iDAPJ3efAi`=U^`?AfQEl$fvF*9imc&>GC56a@XDR5LDi1=huN|N1Z3uCcS?(9( z^D$ZCJKB1)H!hh~OgwOQWhgC96@Lg0{#8Uco0gTS=NdAY(BiP3c-8TtM71>4DaNoaM()zCkOU-T z^{(IK`#RBKOh8kcVR*l2z(dS-1p<5n=Vyn5q>5OK(jOQ^ zeb`^&y-V8?%pvJ&rY%b~+aeKbNt{=*5jHH^%7D190H(D=Y4W|u(M<-LZ+QnkvD$yktwX>Cr?=pR_Qf;QQllxiq zowKF#hw;FBt=~GqJj-wF=hYi^6V(WM6bW_xtm0k%%k z19fu_SDq~GX79_FeLSv#Pc@bRD8*y`3m&om zk+rsvY8P~Fe~}M8Qp^c%GK)0-3E;8_S>sewB`KU;l8MN^gKz#^Zw9vA2*>B!$6-=l z>vc)tx@D!FW@9VU^p{Pl8=DGYf@{i^Z<2OIt7@X;8~+TfuVr#oWEUR=K@{>r6AV`0 zjOth7ToLbSI(7aFD_K;XLirn}Y<>(Lx8p_rKbpHHpZ9wg#BlL~Cf3duB0+yRJCeUG zaqvsnf`kDdlJ-lOFrWc>C+JI60JX`oALo{v`dg|3nD$Z6C4aM_<0w19CGyvsV?)aS z#^xJZG{s$VF7Kq|Y$PlXJn9bFxKi(Jt|;M=Y?4aAb#HxdiSqf+{1QOPr-KUJiYsv? zK%mNi@+RBl^Y*;_r`rGkI03T1)6kY=|J0}l0JSYK$zauYXq_(^DcW0N+zFXfqmBCa zQIXIL0tg{%fgx16O{t9lp7K!zDP&p2zia#WfnXyZLIlIfTHq>G?jJU&EjCyp2qDA4 zTC(C7S%f}F=oun-%13X*Q8K+p1l1|1{}VSz1Su4h!PX^`*g!vep|^OdkU%``@2AbA kAvHru0B|Of8S1|QhWTQp!BQF$D(wk&(a_RVFG-sJ0`4+`FaQ7m delta 19105 zcmZ6xQ*fpYur2(?$;7s8XJXqnCbrEdHYc`i+Y?NjiEVQx_Nj0G`{F;RZ@Ri`RdrQ& z^{RET%G<%-y1@{Y zXz1COT9j^+nfR);k4m=b{OPZ^7nSuW)}TOQx!dmfe!%+h(TF_{zYtEp21+R*G5JX| zaa0JVv@!b^db6F_=HYz`Zdg63(elrzK(oPN#h+cCr9c(A8s1^VG=o`$HpuFr-(kTk zRs^BPz_^OjOF3A74IxYD0tcm99*uusMHL-R6MV`~b_bkRSQ=Rp^eK9rNHj?78c(lk z+5`hVJPvlIjWw2(4VxOC3tidDW^1Kv7?c`IYu$ktUyUn}5t7=8AXCckZQtkPeEU{o*VOnrrmWw1OeX@@!+8UH-ei(?nv$EJ_pI z01VUE!SzYmW~xMs>@{?40K18?VVOYJ8;6U?fy4p8vtwhm-3t&x-I(2v%Uij?%_7Zc zZ9V65r6tq+TH3De=*2I!7uGZbwQ=@v!+Bjf&VNEE%fY}F;`Ckp0Rw?-Awi)3@AR;+ zu>YN26!iZK1W>XkI1;d>Yv6j$h2noxKkoH9Cx`;^V)X;u6BZ896|OL7^YKw(6}}>= zl}H9@Z%2#GX%Z?M1=pQ-T1?RHET?$M#(!Cqg#pDViGipz~xGJD?LIJL$B|>Prdtv=B zJ^zqm_i`Ag^7-f1LUzP`Cx7;&{H|FIL#raF;VIC zUODCHbowRoS)nDfL0_A$N0%x8ZL)u**j#+AP~XCic7SF%YwVIw4^^>P%y?=wSA$vO zYQ=El579Ta9X_B~^>G^K#;mmypGo5(%l1pBTj9>feCj9xO#=ltpI1kK%htv#tKn1D z_;Y%rr^b6{_C>UmRa*{(YkXO2R3%Ykq*~WwQTZtMvc5C&g5rTpMTh=g<)cX(`^ks6 z!~p;K-lSQH8Qu@Wn`=H%pO{j?!ipyF2FTWcVq)rSemW6|7W>|Hp(})=z=nCYd>=6WQvKu&L+;Obz125XYAY0nEN&<$s%XVGAqNE7L%_w3ZKv1%n1XqwA(#Lt)R z;P0O&`!!_y3IiB$UbY~(#i}3n7nMZ}!iV0-{oRQ;HpLCW^}?dw$m( z((#$FVO4#KJ^Gi>?#p^ip{Ltb zmlEK`de~caRj4qS75!P9FHRCEx|eu>#a%X{g*1uE-S-K9@qBJUXpo836Mh_@?Zfya zsYMZCHdRV}(b7HR?ZOo&lD;`xG+qWn#=`1yCW=mR) zsI@@8+V&z9%A=%Re5Qs+OinQ)F_cz;Pkrr3ywt~-FjZRmfws|bVKM$GuVP?3k$vO# z85hz~M)sLxFGyBDb1EHDr3hk&&xdwk=4x6==(Ag>_T~n=`K7!2 z;quJ5Z~dtG!3L(Ey~C%~Un~C8>8?emS5K)u=Bj(?-nh@dJ-b&=Xh!R;dOOUr|87U4 zuX)Sgz@u+7pTMve$OhKjdvRtdR}^Z2d+M z=g0{QTSd;;Mrkoi#6p{Xn^#0TTiXGj@817(eKU1toIpJsG)`*qzFk=?iJ5ogz?CIMGh%q*wC`@cJpIxb{`e)w5R z%u^GU^+r#U_r?<`#lbMFt1L*FdIkbP_<$iO$ILrSYv*m}LBWNq&&t))U^J@i} zb#9m61Ktuiyi=n2_aGK3Y=@40yW!Fy=Se%k-}@2RJ)a;6z{LKW)KrR#{c_;bTupu2_eKWV8M2KPye3Ms#tbih zQxmegzAh>sx{B0Owk(kUrxf#Y(%(&5t=9dcEBpuGqXC?w0jeEXErtWpdi>>@)Ff3A zVYYXLhI%(EO4Z#iM6#e0BchXRC?tjS&nC}9^zrDX4CW`Bvd_=ev4PxUWW7XQudd~U zo8am*@K*2VKUW90QMx-@--Q|r4JouS)|vI3uj3v2{bP7~i@dvXyCZ){?GAIBZsq9R`PDeUHM{SA z`1te>a`xH5*ofWNr$w_ivoa*J6`vB5>&rY^ObRu zkL_7;79&GyiTr(U+^$!D*yq(V?8`HG&j1+iYIrg23t|p!J$7PT@v`><=l7f6dmBN^ zE4lLN?10~{?6S`@<^Va&f_{*h9r;Y{EI$3v1UGZmu1%IK-qhi4J6eoSH@YJ^~nm)RrQ0-=hkd3H{h^x^JOo%;cs{;SeNesdvr(j zvYGk5cie9W`Xgt_&FMUxjpv=4V4%9O{RZrOpjR4NtP3Cx&x=(5^Z6jIl}k+xy|qMHN6|QW8A;1r@i`h`xex%m?A%?m(Qa_;*|`{MT3cl^4`CqAjZTsgDR>#K1*aw z9)$GIa=5t>d6zAFF13sMNr8-@6~PM-PVRN6}!NZNFXe9^D| z><{qw>NmUw^IA8Nws(h#j}dBaGWX0bCs!_gMJ7sb>%T`Jc!uJ(BQ9ruo_qQ5Vc!XH z@`sJuMlnGTNQM1PgxaJPm3WRwDww1dm3WOv>YVzV@p;>+Dq!y`CT&;=o1n;^+ zR}%skHoV%H?<4Jx_%L?B{-a|M-#$|P4?%6(uhbs`+nVj%M*hEEi};)}myz$bj;n=; z;mtoU@C1v;qhr|()P<_MUmN_gP7*FgY}zG|=dAhTFxyu14SO3N!j|xt8k65VML{gu zi22l=x5SpAg*#nuuds8Q%q#i+xCZf*0!zvtTKunB9Y#&(VgYqPUy`5x2ek(ENBt$> zWz3h=6yH3a+^mho)~mPq;ndu*kuPafN}WMmcEi%on_KA_i-8!Tz!)LO@k9w z>Eqn%{W}f-^7L*r)JVX}YX_!?m{Bt#Ml71QA@METB}KV=b}9-&)K}?9(dA?QjTyGQ z!&{WHs4q`=zRv>6Xp^3|MUk*Xf&v{m(tCcL`uxl7t0?`z7BDui)S>WU13nfxkh@Gn z4W*dMHELM68dGZDsCy}?(haVnCce0MRAeD%S}UY-_x5?yAOGi3q13)tciOW1fr`q8 zjMfHj33Kz|()!NY5Dxk7x`?y&odu&H%Bd!eSh zuL;UC4A>5|^8R<6$|f>PhUBox3=?oc3a5JhwH{{Dkz36#*W6iu)?diKK;=9>C z7xIu~R1{(qC{?k^-y4Wd36`Q17DMIn&B7=WU{CG^#uJ8B8-6S_K%ml4o)? zx-#HM^zhAn(Ai2_ku=|`%T#TQ)uyF#T7QIBRXh9cUi#ZP2^ctMWP;m(y)hHysg26v zlzBYuGiDGjA?~&WaG&x`Dw$aac`aT3(SC=qyt;e7qZ`~Zn3Ij!SGw0NIhcE-4krKE zDB#T-qumzedl&Jz&nimjksgXSJGR_LL^&|B<+teQsJ=^q%8kcpTs}-m{RFx7OWFIb z5(QhD;5qW^W48ajJ*9FIjOq&lil=s&f z>gNyoFl?vvU7KvS{3#rDXkE*R-~R&P?Tcl8uJ3B6bCjl4Jf6vGT%b2X)U755I)fP3q}J0yx;lok`%%Uh5~Cs-X`q|L4Hh#!H%r88Z;oI z=bo$6KC;-C@!yFQx!>4=m6Ek7m*|}WvcrYgk@HH~!eu^THh&!Z`FNDYÐksRuL) zMOqHJIylwHK~_~nx^h4s*Mq3Q(nbWW5e9f7$z$E@vkorE_e zyVsavFuEbQ?5`EZ{`Gz>WoGA9XWpbvm)(?~YBfcgyo_b*9KC1Jgw25Bk9}nXFB^1h z>Mw{AaT&*DM6oQV|D_}u%WaYOClQr+(tI>=-c>)USUqdl;^8x>UBm)l62jOT&3kl< zcXhLu6Syqg81*AVTuy##RiJUAid_znnZ)#DYVDwPI}dN{APkFn4%L&;xDLIOybacN z=OFwDjdHdaw2y#9Li$p-=rDSqU`Al__%SZiE(95tqx(Eik*vG!D1^+pUUMd_rKoFV zetS$dmS-wHh8TwwRzLvM!p&hd(K$n-51((c<>6EN3!HMntiZ(o0K?EWnkhPiZaAz& zpmGF-OIaFQ)}Myf=mdDL?=Yu`auFKR)_|qT#w} zyr4}5#1sEWfXu|4h21YPMWRW|_1qvoO4C1`Iw~gWxI#lnEj6M$4%FU!n{{~`Q#-N z^fS&k;gBDqxEX-lR#DpEqn6{b+IyLd8c$SoDnUZ0_Uspj$N5?P&r+Ws$<%(O)S5~OXNE&x-#%BwE52V)qa-0tt-3uFIuJcJ^yzIfA7?Y_+7e6|lYfKph&N+;Ao+4j zfXf2HzJRXwTkZqL^be)4fESoggfY9XmCx4KIO-35Vm$x=+gPx?CVk&gS|B`4RCc?5 z0!}}C?rvLh$T}~k?Khxmya)T4n}AIwP2g)mfZ+Y(U_WhbpZK%zL(J<=Y}cU~_?7We zJ1^R9#E^w;x-oc23i-Chblco>I6%0aIDch%31aKtU`LKH#rJAE zeo22fZq()HH%4Fl^?WbKV<`B$RZ-v%DK>>^F9Q!t4r+->3hmU-?7sW`ag_I*IBi|| z$vh2($=@7us^K(L=_k(d2!mp)L02-nG+~&0(}1zN>aN?V6z~AudYmq|HICuONG5d_ zx@s+0m`rv;bxbMlV1_^xyx6NMt{gm`D6hfrqr@y?euj__y%i zN?83^o}`)8r`8w66y|g^$zZ>~2mR!P{|!_lJUWR=H+o}Sb2RIjIF>=6r!652pFOu` zfJWR#l)Hs&!n)xB2_cNuhvIcFZ- z$dhN#luq3~n%BS$T?3FlRUTQYX9^ES#D;%!7!ou}1 zYgsX`k}ZUZuPu3@VB1270rgg1^NZ-)V&^i$x97!#uhPF?rdO)-$mUx>nL^bPoI+J9 zXU*?kW`{jQ*2Tx%H-iM090_+WY`I51joFTp&c50Ek`p)F)A+~bcj4W)&d~p;n3wEL zueLnf&Q$U+7q=Uu_(Mbe|KK3b$3XW-+q1bvM;%$ewsD+@@`qK50dIoD`Pie_Lh!U0 ztK(DB+-_z3q4~~gS-7drY6yWv%^v#F;OMKrrxl~J)OjbZT-rtLw@K!Q=Gt2IoWduQP?R(H>cQml^0Qa=FCty)Ed z>Y6Ly7h^8-;lN@gBOIV3pqm{;9@ zm;2~1soJ1z2*B{|p)+=)Qaqo8V75~v+&;WL3@y&CWIWkAjs}@K0S}UZ5uD z4|~zye`qm*$IadnOq*fOFRU52xmi&9@_03~|I`hQY)KU-4+0Dgjii^=+EUZab&1p0 zkA`I9f*`@Nu#V@5NB5yw#+@4EghB%L7lYFx7Mb5BXMyhm_aoU3?J-vl6#m3c`;h^^ zBXU$W^796e<&`W2_f-yLFruV3i3IkoDE(!z*pO?`~b=91n&S9VjSP>A8lM=BVdL`^2cWm~|KMy0-O^BX4cD&ZN; zYM@ypHU^OQw1*gB$k9iq!knI--@gxe3I2 zk9bOTy`Z1Swdfp(muT4312Bd+OvWlio|TVu%pyilLy~omQ)Lvb8k3sRo*VTQ5pusZ zKaJ0zn5kkJ@AA#xG_E%5+)V{~6>XG!pyCWNQ32yD7S5NdBG>M-VRptZtNonQf^Hn~ zBO5LwGnX4V@{7o>BMh5lRO>I$x6m=^oe)#->bTleK~gF_T+n0C>fA_06`2kLR~1>B zZb`*Xo)8JQ>>WtxF8L!PBVk&Y;A1h(l*ia9wE>v0fn)+=pNl~SBhtOPN7NrhC)7_hmxcmT%gReKjbaB0zFeFBP#B62Dc$h}TdiZeH) zljot&YbDPrruoMoF+{M>Qi?P5ocC{)9H=e)8u!Y1ye=+=oprv$IXU}Ne&|X2bh0ik zY_q?A_uDEeCe4IL9$!--jwdBILr`1B_+n6pp;b~;{ZCmgy;3r&Ax+2Xubl(r7_@KF z_(q?G2$&xp5=?LRO>Nhi$XA+6ATKtNQ=hnh+tfQeOgWq%DY|T)C-E?{$?fk&Z-wMCcJr{g@zP^n@ga% z;}Mse(>6sQSu+nnb>%8~-keM}Ui)>x(T5y?+k1~5U};p9xd4aY0;3hSk(3m*@7JBe zdM4F>*~UwxPg$ZzhjzU{5_pn5pEw0kL1nrG z4S5a~oi+sE@n9inDuxjMdRHk^@41(7;Woe)t3o-=gl~~>&e0-aaS4*#yN2Gbb`I)m zstF$;eE7B)hXcrF{_(6!8I2234|gikA{mw~AhRrsUP_a)N*YPz`esmt&RVTlQynqB z33fuHqs3P?Yn>%n?kkXYzz=MKY`9*mPzQ)?&4r17pQFD zl^%L&K_?V+E6Mdj-8;9+q}3g$d8PTmRBed&;*b!kl}2nBRU;!CO8rJML>|L&BeFhvJ{$zE zXFI<<@(F~0`v)aAvFj1VMon?<{h~@J=suP16LVd|y{PH;Q22qq&nYQaC*PyHAmTe` zXbgNGRs4$?w|+74tGb#}vK`BeSP!1Vu*mn8eRBVO9uL2XbD48ZLuJG0;l~@h1lB^b z{C7f%YlptvX{`f~>bT?>9eXJq5FEHpjOsf1C*g+hSccZV2q0veu#@;#e(2_ zCzmAg&v#X-QJS%k^n<#;ARGEgBD*6={#^?9$M16aIHk`Ua?W~1{s!sp48yZ~yhREK z*#-cweRCJ<8j?hrOw2*$BkiC4g0@D{I=lJbp|hH(2Q5d!enDI65~`z@M1PdTa7zN!goiQSH0WPM z^Au&r@}Am>;~YE7Fj~JLu6r&?ftF9Bx8MO7vk>ZZ`zZ?;`c3#AT%OzXt3>NAK}ayJ zI?`!BnIT_n%dI)aXmA|l8$^v;rq>Vtg~|ps_B1}@61517gDSksVy9Av|9n|_3n9?rbhUK# zTMp=CPnZY)VQPPIWu*Il&0YS^BT5XMYnQrMu5vGuErP#IO@b5f=1-&Fm_eELx;34z z)F=O{cU`PEwHw0kW%R~}lDn#Lce^%|wT0|uEQLIoNu41SYlE6eg_1CKD@#($8GHG~ z`7dgCwe%3acY;gkc(k;)#w#SSunLOrI|Ig3iCPwg*^gby?@Zc3`P!QWKk{w?AL$BJ zM12<&df9>Ck8u28yQdp*C>}yM7ZtnimS31o*|=ztr?||*JRym?i}(d%9YMri7<%Y& zD@=b7VO_V)T!hY_5pKECMmz=BxaQSMlm+uldyh*QZTv!G=h?$EzmU#s9Ea;5$*5R3II3#M(|K$@4ze zNu!&1Ge0JknaW2nqJHvLVlb;z)DUhOTin(qI^o0#UJF+VY_Jfq;9@(T=-DI4 z_RzAG&q zbLL8HXfvDZsokYL{F4V*FKw9okkv{XsypWpqT1~W)$90=NDtT&x;eSIF#3C>%=vp2l5@t}@|FBE3cmu0}pF4kBlItdxiw>FLF#{T2DP?r?_!APf1 zA9mX-4B7Fx9&~Z0dSM!}$% z6xipZUc^bNIMAJ8Bd-?ZgWKcL!@BQ9k9kM_S4 zM(Yo-E9n^lkx{hL!7w-m5#Os;vt1ES6tQHUbju+=RFXBzn|knql##eFH#3yMvlE?0 zxv3_95Mbc*>>6|2j^+*w#%y&HT`|m9-RdY^qVDg7oFXzCQs@btxvCBPOpNH1MKGPk zyNSi#Eyv8HZT{{gw0x!>4-=bNc;v+b11{gDrCux!1m8148S$Rcw+j<8Ce9h`Pas_* zy*F3cS8{dU3O3QX;M~HX7VMGjJnSuXS=i`e*LN?DR3HCDZxel-pswjZEJsL85vt-R^rS!U+qy)VLgAq;7`*EBbaXVOc);5l zg~i0ot6R9l(12>(v(j;Gy3(g4K5R}JPLt13q-?!FldFLV(;}BlMM^Jh*_l4RS?H#_ z2W&jsAfjxn=+`78F$F>;EBEmlC8sb+10k@z<7skIHdtbw`cr3w>B(We(RLj<+H7NG zA&wLd8_6B>0VEHT+rQ!-RHOvCRPm0ULkZNvZ5JShIZMS@wuuSwY2(1PI-AvZ-H^e@ zWGK3jgYfle4Psj61)U!z?+vAOgo_tgH|G`DZ=eMjl$lMC4XY7G01h2>$SpqBF$y{=xQ9%)5-JUJ)XPM?*+MeEAmbv{ z&6Q|mX&+A74db|ppUgd^JAOChlB}F(DE=)o6qRJ)@|2(M8lA@?v+t>foc_0+fA5@f;NYLRxf2@RRF>0G_0T>Qm4yZI3QQZ~} zI_iVl$p6$E2MH3SUu{iI;yqwS3_K)v3CUyH_pF7gL$4oVlvq9@&zC`ztd=_^SQQP6GBalTEW)AE zxowz99-PpG-G6$tn9&ec*Ym`Zl$I9t1#yL%5vx{nc{Ae1>tY{LuBD(o--+|osNOE0xdni9+eIMxCy_lO5yO8 z0p(aO`S)|Y1+dQ(NQF!5qEytkVoqFDEPrTORg$O6R82%7bWGonsT5TFCYTZX9!rR= zT8pm=*Bc9onRc>Y%#n`{@qXS~l@o>cC2&PNwM5MGh2;%$j+P^h%_ThASL?F) zV>3tyd>g0hu8a81odQeC2^}+LR}5Z+kr*{Ms|RnW8_&gd(*wc+-Z2RLnhFU6}?Q+-#5$iu(L#)Era zPEeE(gPeI{!b54AQn;kG>iouMk`vxWNkSzH zWCPxo*X5%OXqd=Rd&Bvd>5T&i#pYEx4hC3-bbyC7>1=%@?IY)oi#%jq!st@t48qhu zDmSUqwTFLVpHbUceox8QyNBLhcg_oYy1L!}a0wmFI3pIFVJMeOErzw;bilciDBo`T z)LQ$uQUQbYh+X!E`NE^^`){O`%%=BNih`9NsyromTI1GwM8CM2=W@=2mYbxwDa{c_ zJ%C;6fAtD-88?>k9gf-G@FBh6BRUh2SC7TPh*nEipwTtPS1}(|rm3=P8sXJ6&C20DU=1TxjTvY`gL{cOEfhA0Gi*5`G zRwhRfMzIM6gB6Wq0f|(gLX*I94uK#GC2;~t7GWV!Lh=4DkOCu4Acagoj55^UQPOtE zVNi`m4)E};>zxz96IxkCE4Yqq`CoRX)SN9LYzGUd7CQ8{E?Yy#5{J|d7y8*00PQcY zi>xpd&G!W6f`#^EQ;$s%iJSf$7y`}JqCG`1bqGk@{oJhuTO93mzOpb^b5@tN=iv11 zVFL-?MDS^N7{K$5%-fI|Lths-f^s$?h&C%dqMKRJu>X8xclxFPaof8U(lOJPxX`*G zWeXk8gg~m#4?{v4DeH(X+K$W*#C@fU`$aT?v1kq|ySp%L3JyV6iH?Ds=$kX48Uc!5 zOJX!{=>BesbKP)C`1#|Y2D2N#v3*>HrTx|P(_&d2`Fl>jon^zJkkv<>l*g;Qi8t%T zp;9msQa78R!8dC}SgsgTqoSLOfGrA3k-T;V#elc91#Zp|@4tCVz6jfZQYltE;(#u@ zTfu@fq`Nx78`|#}a+tNi)^mI&+WgT%T`$?zZ+If-^MQf-d<0(@{!jx0c4`&%b|F_4G=f?0jacWv&8RUJ0sv%05>z-Y+O+e~q%oZp(Np#ei=jf$J*;L2|u34&A$zwU~InFXv|3Z$(K%`g!s+ zyGkvT_v7>3>R!y8~KE|@TxG<-4UQrd|#-LW=!U~LU=WDKK z3iuaqj#D+~vF$D5_3u0jYp4;gTb`V6#}~E402>#Zt~hl;^_&Voxl%ae{$fdOUUB4P zT13+-zC^#Est5&0rPEQ5n!)AG*C0*H#<2-Uz{<6lgDe(8_JU&A&;F(zRhLT=n^8%n zd)P&a7t=3)Ena$F&i+tux;h@bzLjp&F(ztOAjXyzJTnwS#9J!mSXb4_wlHFLF@y_nKln-vY)*j zpzKgtS%n0Ye0f)e{<>9gE@diw!Fp2tydpHAtZMUY(*S45JZlvfNrvKJ@N>RnFl9#j!NZtAUWC+gP2_FeQdcS%T z8i|n>{vLUDTFEv({Q~ST%9UTJ1aCTG4B z^lN<*Ke^ig{G9kc4u(gMDPGl=Onxr}Oit1pMCaMx>)x?(dTIEQ9p%neV8)*Rj#^N}7h~|m)>RU7 z%oiAMB}Ty$=D^zNx~c!X9|3AM8wjsxXO3Z*-j!^Y%1Q}mvkk)Xx3&DXw4S142x#SS zqQnbFs|z;47Ab!oB-|V|S+@i!+DcP68htkPb=Innr!&L6u9U}8YBDHBJX)H%$B~CX z)J|#+qguRl)h68B^VE`HUPfLaDUERL8vqgnB4!|`Pl&Itm=7SNeNTUUCJZyXV}E9v znK<;zam^q0bEnG>v8m?wtsI+~2p&Fa+eg1EhJx65 zPPExhuW{KLs#5tO9mZkC&Az6gg>&QPbd1)-V0j>RaY^|FGDeK4!5mcKNaC6N1$-cN z!(&{aHA>26k?MC}wTQ>;=h2GNITnjll0DK(Ls7I{^9cJiE&6gHVs}?Qp4R0mJVpE) zr~LLbTzoQ9-mGK()n}=B_w&B)hS}?a2%?-%7cDGerc^{T>9PG> zi{(#u`iN^_!`<#YgZ>$jK+pZJ-vFx>2pygB5REZyBFov41$ft?D*#2q*UcKZ z`Zmb5z-on4!qp{IVo)FyK#R~DaMWIRr-?ABn1P}onG*u4gVQelh(^R46iH~5YQuFy z3)^Cu5sy+f)stYxHW!G&q!+}IN2CvZ83PIa1?$BT6thG^cw~9749l%Yc-3r;gY^yncSmpwY+8#2;I;m&-z_4e?G&qQ z%Z5}wd8q7IIxyG@7W%%uD&B$yr;iUJb?g2v=udSR>W8ws2%-tdR2lY6*Q96t#0aPF zURJ(@AV?AnM&zr#V4IKXVn)2OW+E-u_{rSJ1N%{IGRkim zvOQrTP=HW8Rfr1i%DGbQMVBYpn=X6b*f-p%;Fh8pc?ByqL0wpW-g_W3DoNb-+h_mr zh}LQ(-H&i;@`0)Q=fj1crW*VWxEBfA9$FC#gbf7=<3mvN}+cf7aceA^E ze&vqXpzCFT5uzsx{HTojB1@NO=b9eNg~r~rGVx~Hx8`O_3QLRC*BP(W&(Gxq4~;E9 zw-2~_e_Z+U*nPL8fjhKgiuotj#$W(9OMu%0_EoO&nqOxoB?kcW<>jq6`n31a2z@$q0V3kp3(9V-1#;_v?Jxa+mUi6oZ+8shZkGba}`JH{4+Y zvP0o};z&;Fz;#Cex$C?3c?h!9-lLXfIiURbcx=(Bto#OJ5&vO9yZyD3ANRE(R4?^A z-YsfN;EKV?q%bxuL332BSYmG5 zwo8$?Y)vgAO;y}4B#<SYhp7asc>G77bv4Km$x?@oS?b3T9hxkhB_qdlK4{ds*COHA zyB6Nm4xt>;)LSf;^>Oz<>sq1GtfIl3;7ou#VBG2<55-R*tfGnwh0Fi^lV66 z-HNHfsv;c;r&{S@tV9W#>>vgH6Zcd+?VAfuL@*3R&0&0!EHgj1o0;CXWYpRQ{wT(`#m zoNH?;9)^Rtr@*z;d`Xd4 z0~V{&=Kj_>BxQ}L^1?Im->+f(^*|-4DX0pNnREpPo9)zL4_!ew4&nVlHN6eOy7ot= zM#t95yjak}^ko!6tOze-F_8|jz7Y~C?wR|Q+t&lJAS^?~a6 z?@|&CD*?INfaz@jU9SxYVW6xTZ7#9{C*>#__c5fa&=EW6D%=DM&&QAs{R)sQY4;uP z=HEsf7$Mi|Cu4ezS341i*xO{_{fsV1mZ=(G5`jJl_dx9qqSyElK0UkY?LfeR(Se?D zflAU+&(wX&ENy6VK#>x%aBUQ0i~l2wJl2Q)V(9G@aN>ZyEdV15B1^x|Q@HWwtlUC= zdcPkD<*JZDIm=CXYU%i*N!~#nSp7KA7E?1t=Ii!AMAj$hb9VNh71YpBM#wj}HM*74 zJ?B1WR(|@r3OhTKw;uWTT+7G4(S@D_D_p1`__4Jbw4xgkZmHxim?S%bhev_2Zsu1M zDOURo2*MUz6WiRW!%flrL|FW=KNm;!`_6b{r|B>&jSE#$Bn);gH+>#PYi;iLIp<~l ziM@#kE{_=Kgfh?=E*r*w3Z6_pOw%*l+Dxs2K)eF2jK2l^uQa#D=rq%?BnaaYNs@c5 zv)J+LeK=4Wm6VJ*h4hM=E})uPw=97L;+Zr)T{)VHWqLWuB`D|6H0tH$cv^eDeq}IJ zqFHH1Sd>b7f1(&i)kuNR@CS_j7Pa3aV#@d4%S(Gr&72BIlf+s5Hlu#kKJwYqV+@tF zat4#7``y{&{m)nz@1w;txaV@N7C1){g*lif)`AJu&gPp*+27J@2t_|;nt}3QFUTCc zgjj45BTb2jXtSr82-?KlQ!UK&{w?2xJ#B#){M4bhA2sP<+faK&Vbg7*lSbUtBBb(I zWa$jyTq+o#R)VieMEl^7EcG0yMr*R3cY&4?hUuioe_zVH+(0C-)aYvBb4vj1e)rG5NhAL+R};p39uA`hpMjQeS^} zRK_3fR}a?uTnX{>f%#9CU(PH6L-4Kd&y9v#SBy5INb{+#an&--&-uODO8xY-dQOkH*Q3Gr!h)s5~pcI!f7l2d7m9km~;Z9P7q zG4wDAlRnz9aOb|}p0DHAwLoQyQ7H{iBIj)!^n9wP#@vf;LXxF!@95p8RCO+Y@m+b0 z8=baQ@=?{9#+IV#8=+OPx(NhWGQ;eW%9u-t+C=Pw(rqz0;j_1L;U>yG)h(uHB(F4G zfOpn+#DmLaMh!lNd3xEJ7SQNanoBlB!ff8#xf?xQ>0Fdp=Z^hA_BQifaBe%rRg_#ryixcyuHo^pFB zw2vS(E5T-Zl!k>)s?=~nhEMZ_R0>eGFHacpBwtw6-wia0g690^{)P}rKE9tuw5dP4 z;B+#2sMt{Kjc~nPk4YaNU^qs(I)JJn#@bI#>jkh>QdLl~IbL1e@b3mdM=-EY1a7*K zUcX>aTa(131(z(6w9~BUhu`&zK%O*e*WhAMeP4rjK6_*9vAzc=K53H)EiX~09V#}y z#scJly+=EcRqfAG6}`54*oei0yw=&)6Bq>%4<_zIBXofXrd%yUpNS9gjbDG#CdC=S zEX$r*QF#3MrQfG@g&<^CT3T0krfBzz3gT;1JwW+G+S+JEudQDTrK}-%k%#84NnW?( zMBV`+;JsjP;Kr7OLaYzxF8C%UUe1Qbig14KyhDH{%De7QEFs+KJ>NM7_cI}i|7(y{ zYE%a>iec`;1NZfb^2j3aSJA!^R{)P8QTzj@u)xbCkx(CXq6(yM-i*nCK1KlkS;BI2 z@R!-(*mg^_k%i3g6x*y$m+FYXds(SI!snQMFeXT!bsXf$uwdb^8uGVO85k`bC1f!; zkTttUtZZx{Ah86WBw@^Eo-S^u>nw>5_P+v7f`Ak9^Fo)v^t4C zfQKb&zwa*04P@E)^5YW6l!{IX^iSrKIC18(*;tm>iRp5WGz^r!iI8SR?|jdDYkc@q)( zn^FJLIamdb5)~F0rg1&4AdJwlMSNg0LHeoPol8edHRK$wp3b!mF@8jYJulP{iUrvz@$ z+$_$8yF)k&!PH}DqlNFsRc#G+8Zvu0>IdURazb6Dhr}-KA28UF5i_^DJeZ|#B_9~o z*})SUc>E}GPln6h4Y|dbXq#1{j+e3KGb@58E^BBh{PlJ4#WjH<<_N=ftPz|6*Hu`f zPNX=dl1UAM)*_ErZqz9v;Lx4qY1-K3ciHXj$Qb{hIjPnk`#$O`OPt$SXF&~>WAQn= z^3(n|Suryxq`BUQHwQ5TgsS<{bB~h~616BEk<>bvU2X!pe-4anF-9{N|%GM`y0|V4RrwN&ABd+ z?|BUjRS7{~0x~vhSblu*v{wZ2bg>|9C%!bE4_2Kd1_z-R=n^~$NK7+qe;TU z2dE-umbqtJ!ZUboN4|U!w6s_<0{Tkn=@sP-jB7`68J9at!qfW)WD_GnxX`H*ik7nR zx$pH19=px!J~unNaAtC5sMKn2e)j8rw@R`n4477KqGy5^Px<J*mAwe4-8~NxUo7IiQCsMyU|3Dt2OIsQR~XwGy?rIJJrez*8`4}V zt;MforK2Q}k6CYf;{u$4!R_a;i`1A+Ch3f_jre$=wytW9qW+L{T>;=BnSRHSq98vN zD?x8iL{ys?$40}iLmV@ug)`I#@@~#fT)AM2Z2jON(n|kloy3o3o|Rt&M{_I>@6bFv zsqqkhc*Cv}K<0QP6V2!R|7pU!i6T<3Ilbr%V%ri+<{*K*zi6*>8D1Mte0%uFw#}AWg%m04O;f{ zvD1MqL2Z8HZrFu(8um+;Kj{z>e;{uFz#a?$H20t&Z$D4k_Ry2Qhs{A?SwQwTAm`5+ zkoc;|<7_R8qRLYC|4xGdKz@~vAKRUxw%l711p&?{vHi_4>=p`i*~F(Hz(XW6~ISnRqX^ayUkv6IeNh!0e-%eOispy z8o1-v9e>qYIu?@IREI6Uy>%?va~4V~J*~ik*sTr-u(u@}OVS|zO7V*$`U9Fkkoo^}Ot3M-gBL3j54aHPK@GrMUk9bB&T{ diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 8b9e7c49cd..4e2b4907ba 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1319,7 +1319,7 @@ int commander_thread_main(int argc, char *argv[]) /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); - current_status.navigation_state = NAVIGATION_STATE_STANDBY; + current_status.navigation_state = NAVIGATION_STATE_INIT; current_status.arming_state = ARMING_STATE_INIT; current_status.hil_state = HIL_STATE_OFF; current_status.flag_system_armed = false; @@ -1857,19 +1857,7 @@ int commander_thread_main(int argc, char *argv[]) } /* 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 ); - } - } - } + navigation_state_update(stat_pub, ¤t_status, mavlink_fd); /* handle the case where RC signal was regained */ if (!current_status.rc_signal_found_once) { diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 2e49354714..e6344f1a84 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -56,60 +56,195 @@ /** * Transition from one sytem state to another */ -void state_machine_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { int ret = ERROR; - system_state_t new_system_state; + navigation_state_t new_navigation_state; + /* do the navigation state update depending on the arming state */ + switch (current_status->arming_state) { - /* - * Firstly evaluate mode switch position - * */ + /* evaluate the navigation state when disarmed */ + case ARMING_STATE_STANDBY: - /* Always accept manual mode */ - if (current_status->mode_switch == MODE_SWITCH_MANUAL) { - new_system_state = SYSTEM_STATE_MANUAL; + /* Always accept manual mode */ + if (current_status->mode_switch == MODE_SWITCH_MANUAL) { + new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; - /* Accept SEATBELT if there is a local position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { + /* 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); - } + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; + } else { + /* or just fall back to manual */ + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; + } - /* 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"); + /* 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_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; + } 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; + /* otherwise fallback to seatbelt or even manual */ + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; + } else { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; + } + } } - } - } - /* - * Next up, check for - */ + break; + + /* evaluate the navigation state when armed */ + case ARMING_STATE_ARMED: + + /* Always accept manual mode */ + if (current_status->mode_switch == MODE_SWITCH_MANUAL) { + new_navigation_state = NAVIGATION_STATE_MANUAL; + + /* Accept SEATBELT if there is a local position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT + && current_status->return_switch == RETURN_SWITCH_NONE) { + + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT; + } else { + /* or just fall back to manual */ + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + + /* Accept SEATBELT_DESCENT if there is a local position estimate and the return switch is on */ + } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT + && current_status->return_switch == RETURN_SWITCH_RETURN) { + + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; + } else { + /* descent not possible without baro information, fall back to manual */ + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + + /* Accept LOITER if there is a global position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_AUTO + && current_status->return_switch == RETURN_SWITCH_NONE + && current_status->mission_switch == MISSION_SWITCH_NONE) { + + if (current_status->flag_global_position_valid) { + new_navigation_state = NAVIGATION_STATE_AUTO_LOITER; + } else { + mavlink_log_critical(mavlink_fd, "Rej. LOITER: no global position"); + + /* otherwise fallback to SEATBELT or even manual */ + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT; + } else { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + } + + /* Accept MISSION if there is a global position estimate and home position */ + } else if (current_status->mode_switch == MODE_SWITCH_AUTO + && current_status->return_switch == RETURN_SWITCH_NONE + && current_status->mission_switch == MISSION_SWITCH_MISSION) { + + if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { + new_navigation_state = NAVIGATION_STATE_AUTO_MISSION; + } else { + + /* spit out what exactly is unavailable */ + if (current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. MISSION: no home position"); + } else if (current_status->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position"); + } else { + mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position and no home position"); + } + + /* otherwise fallback to SEATBELT or even manual */ + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT; + } else { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + } + + /* Go to RTL or land if global position estimate and home position is available */ + } else if (current_status->mode_switch == MODE_SWITCH_AUTO + && current_status->return_switch == RETURN_SWITCH_RETURN + && (current_status->mission_switch == MISSION_SWITCH_NONE || current_status->mission_switch == MISSION_SWITCH_MISSION)) { + + if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { + + /* after RTL go to LAND */ + if (current_status->flag_system_returned_to_home) { + new_navigation_state = NAVIGATION_STATE_AUTO_LAND; + } else { + new_navigation_state = NAVIGATION_STATE_AUTO_RTL; + } + + } else { + + /* spit out what exactly is unavailable */ + if (current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no home position"); + } else if (current_status->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position"); + } else { + mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position and no home position"); + } + + /* otherwise fallback to SEATBELT_DESCENT or even MANUAL */ + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; + } else { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + } + } + break; + + case ARMING_STATE_ARMED_ERROR: + + // TODO work out fail-safe scenarios + break; + + case ARMING_STATE_STANDBY_ERROR: + + // TODO work out fail-safe scenarios + break; + + case ARMING_STATE_REBOOT: + + // XXX I don't think we should end up here + break; + + case ARMING_STATE_IN_AIR_RESTORE: + + // XXX not sure what to do here + break; + default: + break; + } /* Update the system state in case it has changed */ - if (current_status->system_state != new_system_state) { + if (current_status->navigation_state != new_navigation_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; + if (check_navigation_state_transition(current_status->navigation_state, new_navigation_state) == OK) { + current_status->navigation_state = new_navigation_state; state_machine_publish(status_pub, current_status, mavlink_fd); } else { mavlink_log_critical(mavlink_fd, "System state transition not valid"); @@ -123,74 +258,164 @@ void state_machine_update(int status_pub, struct vehicle_status_s *current_statu * 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 check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state) { int ret = ERROR; /* only check transition if the new state is actually different from the current one */ - if (new_system_state != current_system_state) { + if (new_navigation_state != current_navigation_state) { - switch (new_system_state) { - case SYSTEM_STATE_INIT: + switch (new_navigation_state) { + case NAVIGATION_STATE_INIT: /* 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) { - - ret = OK; - } - - 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) { + if (current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { ret = OK; } break; - case SYSTEM_STATE_REBOOT: + case NAVIGATION_STATE_MANUAL_STANDBY: - /* from INIT you can go straight to REBOOT */ - if (current_system_state == SYSTEM_STATE_INIT) { + /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ + if (current_navigation_state == NAVIGATION_STATE_INIT + || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_navigation_state == NAVIGATION_STATE_MANUAL) { ret = OK; } break; - case SYSTEM_STATE_IN_AIR_RESTORE: + case NAVIGATION_STATE_MANUAL: - /* from INIT you can go straight to an IN AIR RESTORE */ - if (current_system_state == SYSTEM_STATE_INIT) { + /* all transitions possible */ + ret = OK; + break; + + case NAVIGATION_STATE_SEATBELT_STANDBY: + + /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ + if (current_navigation_state == NAVIGATION_STATE_INIT + || current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_SEATBELT: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ + if (current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || current_navigation_state == NAVIGATION_STATE_MANUAL + || current_navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_READY + || current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_SEATBELT_DESCENT: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ + if (current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_MANUAL + || current_navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_READY + || current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_STANDBY: + + /* transitions from INIT or from other STANDBY modes or from AUTO READY */ + if (current_navigation_state == NAVIGATION_STATE_INIT + || current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_READY) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_READY: + + /* transitions from AUTO_STANDBY or AUTO_LAND */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_LAND) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_TAKEOFF: + + /* only transitions from AUTO_READY */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_READY) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_LOITER: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_MANUAL) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_MISSION: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_MANUAL) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_RTL: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_MANUAL) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_LAND: + /* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER) { ret = OK; } diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index 57b3db8f19..1c0564d077 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -47,10 +47,11 @@ #include #include -int navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); +void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); //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); +int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp index 9a69195352..584ca2306a 100644 --- a/apps/controllib/fixedwing.cpp +++ b/apps/controllib/fixedwing.cpp @@ -294,8 +294,9 @@ void BlockMultiModeBacksideAutopilot::update() for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) _actuators.control[i] = 0.0f; +#warning this if is incomplete, should be based on flags // only update guidance in auto mode - if (_status.navigation_state == NAVIGATION_STATE_MISSION) { + if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { // update guidance _guide.update(_pos, _att, _posCmd, _lastPosCmd); } @@ -304,8 +305,9 @@ void BlockMultiModeBacksideAutopilot::update() // once the system switches from manual or auto to stabilized // the setpoint should update to loitering around this position +#warning should be base on flags // handle autopilot modes - if (_status.navigation_state == NAVIGATION_STATE_MISSION || + if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION || _status.navigation_state == NAVIGATION_STATE_MANUAL) { // update guidance @@ -340,7 +342,7 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_THR] : _manual.throttle; } - +#warning should be base on flags } else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) { // if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index e25f1be27f..34b267d56b 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -205,36 +205,40 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) } /* autonomous mode */ - if (v_status.navigation_state == NAVIGATION_STATE_MISSION - || v_status.navigation_state == NAVIGATION_STATE_TAKEOFF - || v_status.navigation_state == NAVIGATION_STATE_RTL - || v_status.navigation_state == NAVIGATION_STATE_LAND - || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { + if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND + || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER + || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION + || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY + || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL + || v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; } /* set arming state */ - if (v_status.flag_system_armed) { + if (v_status.arming_state == ARMING_STATE_ARMED + || v_status.arming_state == ARMING_STATE_ARMED_ERROR) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } - if (v_status.navigation_state == NAVIGATION_STATE_MANUAL) { + if (v_status.navigation_state == NAVIGATION_STATE_MANUAL + || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT) { - - *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; - } - - if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT) { - - *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; - } +// if (v_status.system_state == NAVIGATION_STATE_SEATBELT) { +// +// *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; +// } +// +// if (v_status.system_state == NAVIGATION_STATE_SEATBELT) { +// +// *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; +// } /** @@ -248,19 +252,30 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_state = MAV_STATE_CALIBRATING; - } else if (v_status.arming_state == ARMING_STATE_ERROR || v_status.arming_state == ARMING_STATE_ABORT) { + } else if (v_status.flag_system_emergency) { *mavlink_state = MAV_STATE_EMERGENCY; - } else if (v_status.arming_state == ARMING_STATE_ARMED || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE) { + // XXX difference between active and armed? is AUTO_READY active? + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND + || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER + || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION + || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL + || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || v_status.navigation_state == NAVIGATION_STATE_MANUAL) { *mavlink_state = MAV_STATE_ACTIVE; - } else if (v_status.arming_state == ARMING_STATE_STANDBY) { + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY // XXX correct? + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { *mavlink_state = MAV_STATE_STANDBY; - } else if (v_status.arming_state == ARMING_STATE_INIT) { + } else if (v_status.navigation_state == NAVIGATION_STATE_INIT) { *mavlink_state = MAV_STATE_UNINIT; } else { diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 20cb25cc02..29baff34bd 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -60,13 +60,20 @@ /* State Machine */ typedef enum { - SYSTEM_STATE_INIT=0, - SYSTEM_STATE_MANUAL, - SYSTEM_STATE_SEATBELT, - SYSTEM_STATE_AUTO, - SYSTEM_STATE_REBOOT, - SYSTEM_STATE_IN_AIR_RESTORE -} system_state_t; + NAVIGATION_STATE_INIT=0, + NAVIGATION_STATE_MANUAL_STANDBY, + NAVIGATION_STATE_MANUAL, + NAVIGATION_STATE_SEATBELT_STANDBY, + NAVIGATION_STATE_SEATBELT, + NAVIGATION_STATE_SEATBELT_DESCENT, + NAVIGATION_STATE_AUTO_STANDBY, + NAVIGATION_STATE_AUTO_READY, + NAVIGATION_STATE_AUTO_TAKEOFF, + NAVIGATION_STATE_AUTO_LOITER, + NAVIGATION_STATE_AUTO_MISSION, + NAVIGATION_STATE_AUTO_RTL, + NAVIGATION_STATE_AUTO_LAND +} navigation_state_t; typedef enum { MANUAL_STANDBY = 0, @@ -75,50 +82,20 @@ typedef enum { } 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; + ARMING_STATE_INIT = 0, + ARMING_STATE_STANDBY, + ARMING_STATE_ARMED, + ARMING_STATE_ARMED_ERROR, + ARMING_STATE_STANDBY_ERROR, + ARMING_STATE_REBOOT, + ARMING_STATE_IN_AIR_RESTORE +} arming_state_t; typedef enum { HIL_STATE_OFF = 0, HIL_STATE_ON } hil_state_t; -enum VEHICLE_MODE_FLAG { - VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, - VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, - VEHICLE_MODE_FLAG_HIL_ENABLED = 32, - VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16, - VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, - VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, - VEHICLE_MODE_FLAG_TEST_ENABLED = 2, - VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 -}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ - typedef enum { MODE_SWITCH_MANUAL = 0, MODE_SWITCH_SEATBELT, @@ -135,6 +112,17 @@ typedef enum { MISSION_SWITCH_MISSION } mission_switch_pos_t; +enum VEHICLE_MODE_FLAG { + VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, + VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, + VEHICLE_MODE_FLAG_HIL_ENABLED = 32, + VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16, + VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, + VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, + VEHICLE_MODE_FLAG_TEST_ENABLED = 2, + VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 +}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ + /** * Should match 1:1 MAVLink's MAV_TYPE ENUM */ @@ -181,8 +169,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 - system_state_t system_state; /**< current system state */ -// arming_state_t arming_state; /**< current arming state */ + navigation_state_t navigation_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 */ @@ -203,7 +191,9 @@ struct vehicle_status_s bool flag_system_arming_requested; bool flag_system_disarming_requested; bool flag_system_reboot_requested; - bool flag_system_on_ground; + bool flag_system_returned_to_home; + + bool flag_auto_enabled; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */