From e44045f42262de9a45c3d4f39bd2ad98beee2b92 Mon Sep 17 00:00:00 2001 From: corvus Date: Wed, 22 Sep 2010 22:16:48 +0000 Subject: [PATCH] OP-165 : Guidance Module Creating GuidanceModule together with PositionDesired UAVObject (as discussed), so dschin and me can work on it :-) Will compile and (on sim_posix) execute, but PID logic is yet untested and preliminary. git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1722 ebee16cc-31ac-478f-84a7-5cbb03baadba --- .../Doc/Architecture/OPModuleArchitecture.dia | Bin 8711 -> 8899 bytes flight/OpenPilot/Makefile | 2 + flight/OpenPilot/Makefile.posix | 4 +- flight/OpenPilot/Modules/Guidance/guidance.c | 280 +++++++++++++++++ .../OpenPilot/Modules/Guidance/inc/guidance.h | 43 +++ .../OpenPilot/UAVObjects/guidancesettings.c | 132 ++++++++ .../UAVObjects/inc/guidancesettings.h | 131 ++++++++ .../OpenPilot/UAVObjects/inc/positionactual.h | 6 +- .../UAVObjects/inc/positiondesired.h | 97 ++++++ .../OpenPilot/UAVObjects/inc/systemalarms.h | 6 +- flight/OpenPilot/UAVObjects/positiondesired.c | 111 +++++++ flight/OpenPilot/UAVObjects/uavobjectsinit.c | 4 + .../plugins/uavobjects/guidancesettings.cpp | 209 +++++++++++++ .../src/plugins/uavobjects/guidancesettings.h | 118 ++++++++ .../plugins/uavobjects/guidancesettings.py | 286 ++++++++++++++++++ .../src/plugins/uavobjects/positionactual.cpp | 6 + .../src/plugins/uavobjects/positionactual.h | 6 +- .../src/plugins/uavobjects/positionactual.py | 22 +- .../plugins/uavobjects/positiondesired.cpp | 136 +++++++++ .../src/plugins/uavobjects/positiondesired.h | 84 +++++ .../src/plugins/uavobjects/positiondesired.py | 108 +++++++ .../src/plugins/uavobjects/systemalarms.cpp | 1 + ground/src/plugins/uavobjects/systemalarms.h | 6 +- ground/src/plugins/uavobjects/systemalarms.py | 3 +- ground/src/plugins/uavobjects/uavobjects.pro | 4 + .../src/plugins/uavobjects/uavobjectsinit.cpp | 4 + .../uavobjectdefinition/guidancesettings.xml | 30 ++ .../uavobjectdefinition/positionactual.xml | 2 + .../uavobjectdefinition/positiondesired.xml | 12 + .../uavobjectdefinition/systemalarms.xml | 2 +- 30 files changed, 1843 insertions(+), 12 deletions(-) create mode 100644 flight/OpenPilot/Modules/Guidance/guidance.c create mode 100644 flight/OpenPilot/Modules/Guidance/inc/guidance.h create mode 100644 flight/OpenPilot/UAVObjects/guidancesettings.c create mode 100644 flight/OpenPilot/UAVObjects/inc/guidancesettings.h create mode 100644 flight/OpenPilot/UAVObjects/inc/positiondesired.h create mode 100644 flight/OpenPilot/UAVObjects/positiondesired.c create mode 100644 ground/src/plugins/uavobjects/guidancesettings.cpp create mode 100644 ground/src/plugins/uavobjects/guidancesettings.h create mode 100644 ground/src/plugins/uavobjects/guidancesettings.py create mode 100644 ground/src/plugins/uavobjects/positiondesired.cpp create mode 100644 ground/src/plugins/uavobjects/positiondesired.h create mode 100644 ground/src/plugins/uavobjects/positiondesired.py create mode 100644 ground/src/shared/uavobjectdefinition/guidancesettings.xml create mode 100644 ground/src/shared/uavobjectdefinition/positiondesired.xml diff --git a/flight/Doc/Architecture/OPModuleArchitecture.dia b/flight/Doc/Architecture/OPModuleArchitecture.dia index d5e35d63fca553f928075e401942342467053434..d6a539ce46ba416dee39c832ae38ee6127c10413 100644 GIT binary patch delta 7113 zcmZ{nWl$U5*6!htL-FF8G(d5eqQwcty%Z_Misc#GsZSZg5{HlRzXuB$*v|&G#n$b`v zI(;9u>sea@!mP{&Pdb;{-Ct859yVx%x=X8D$jW`j%bko1lw_8zuKsE+K2+a<-_^e~ zy5B8T{vZtzY#)zny{%|P+Xg~%vZ%p#epi)>w;ZwFtyi{sJZs*b8QOXQU_URvQz#YM z{ju9)98nh2=@9s^eQFW2KBxJ$(>xtcB&1ss zQM52=6W~#NE1HH}FsrrnINX`BwMbUZN=0kt?D?hkZ!f=ptpQs_YCv$^?V=XjTpJGt zUYn-gM51yBmI}alN6l_iO}rl`<2+lKTRe;_T1FQ>aw#`E*}+qWKR&GUHI& zN^Q8UeRKEG9x85KOEh3TxMSC$7|m!rA{Q zZb39i;OY(0M4M330K^z9*nY3w>?)Za)KgE#5shd2XZO!swmmfW;f>I9`|P01T}wO~ zcU|23jpDnN%N5rCNY=x%-Al{9o8rbrxd3q~^0ff7w43qtBYj0|(W3p}x1oHpvA2}N zTvElwadw0&FyFu&w&tRpMozv10%CAUd{Z^schge`sbT2PVk&uy_)|D>HoCsfa9kLD z>%R?m-j9dH{c{h2sMYaaEus6*W6rfhv(c4{Bc~%*g1+?>FH)V-W4hU+Zwh*q6TTF8XBsG~2+J z91bS|JrCM`V!uxbSCg$<{uY_|kzVgHM~AG`c2ebj;==uf!k?E3XWFO1wR~q@)Y|N? zvRIvv*mTIN&)WP$T3AQ!f6BFrlaLg5o)<4bEL={Il%_zwjFO>rY_w=Bc%gVS0ryc?YMEN?ok| zmoyr-RvQ>Z{sj(38DBPc?)EyDC{lg28j1kW%!B!y>!F$r<56^Z>Cfgahpe^6$X*C5 z9K?*q>+V?)X10)$SkhPv$+u_N_3XYEWhm(hX_+{p52r z8%t3=>C7BUm&&68OcJy0=qN*rbmSYz zd|(O8G1Om&v?#KUqp3ZVF1Iv*Z|rDIY#D>-c@i9C_7;wR!Q;xUP6>$W&3iJa*Kn;pi#@ zkbG)t)9As+pKCbTSoy9*43`i=LFg>?By*aH}Su)!yIZco{;5eFus2* z&}Jj2-k81S^<6D99v}HkAZScsnzwwF*E&mb^G=xP@b}ogvb@VWPGhxkgW*9~=yCvO z$!amVRgv*G*6Ow8L9!M+-B;QnsSx(g3>Nib|m0XQx685g~uWoMdD0Yha z82zA=;j#&_{r(o)YEvc7R0?v0B=Pt%uE5xQ9Bomq5%I6*L&%-MkeCy`7+%=y!^CR1*FF(!j`p=I&qvw)h zy%D>Jj*Ql#9XKxVg=G?hAh(5mFsBb%B&g6_e7~)z&CT}dI)X;*@p2u5 zB*f$ThhNH^^5%wT2-918Da0KhW_3;G8`c4a0rnYYV2=81D)kSN z8Y~1NBzxhtq$KYa#Vh6zJiWs0WUgUFNp|JuYaVj6h1X!qa1dcOr2ofPv8-cwo*_kY zcoHRdYpOy8cI~uY?evx)=R3=Hh4aNN=tkhQ2}s= z?-XimKLCknNG!RX{*#ZESaj1~297&}wuM08$F%@q(uQi?>qeYkULGY z60Mb2rdqDATJ2jQE}0fRhCPPJ6~p_dG&j#VVgHagX_LembDMAhB3ILB=NnLlBPulH z*@cwx>p=4PRC3`rnOFk{;{_cl9<8zwEnJq#-dnaq2b)HE`?e>M?<(fxR zE}|qJr8=@<1<-#3XGLo0XNbGRt!IIBfhVH|-0=m$Mg80KEE=#7K^Kn5;qgX$J&*-Q z?(y#E*8VNGyati)O(@M2&LOLBgA`W9vhdg6p}-ffVr`_=BIPsic9kjacK1nxo`r%& zr$@m#B)Oeli40n}G>6ww_K-mIt#OZ!yFhGQgN7 z5o)6EU7-n;7=rSHsRPj`ef;Zap5A&Jp;8{|VbMCQES{!q}rau0pR% z3^Q>Tw8{*aE^8Ss$ha2VQ>8ff0ffn}>!2nWr4Wa4@KO=d_DHrfU_(9mh&u5)>wdg~ z$hAZ<>`VyGkL5nzS3lCQPC=kAZyi`R?J+7Z2YyR${d#%tZ0*M&n7iNF$FPB#eg9fh zl?AcfU&TIRQz!mI6InOJ==5+TsfzPlhWB}=X7_!rBch9aqlAbth^Fh#+Lh%!|2O_} z$OZ5%|9Sz$-f*N$W1FYO4f3VnY-ZfPEcU@G56;sa6^AjaC0~ zo`v&*WATeKW7M2#q42i{44rP==1kr0s^=y|v(+>i9?Q%ZWee+G<=}Wm;x-6_V~t zRKGuR@aZFO`*+1;0z5hR;$*T~5M>`L*eVp%NjP5=>?A@J{{vWJA<2yIrJaMVW_;nA z*3E%g|EG00$7FvQE-{_Mk_{pCrBNMe->8r|-6~SAU%*PKpyOja3~j?o;hM&I1`uv^ z>`Os+E)+7EH%0W>;c@9QmUxayRksnZht5Q3$H|=Y`reK&ptjpLnR%WWxEtxydX}6{ z@m4r?lC}+8R6#-8cy-_!>7x=O5IYwM>Kts3 zJ8G@P>=x|8>~6>(t%gNjl$rk%B4_&JQcSlqn6;x>4gyMtj*!R)(77GtsG4S8%+$9mO5`B7rkLc&JswWe;3lgJZI;uWR&Fu%yFyH7KQB#@ z(w(*fFVW0i#z0;CBPiM0U+4-oDt0Yu6idl?K-?W{H*Tgz(C&WcvT>_vq1N1J59dPj znHGFShzH}FR=9O)>){Vh3i|hH$kh~*)kV=dCn;g{P~oL{(!~ku3aTa})1ZEa%f2hT zPqSB3_PzLxm`DDTgbM(ptP%BsOcZIw{ zv~-=Gs$RI_8qaHcb+_dp@&q)WZ{zl{rVu!es<`PJ3Et6%e-DsQ6%b7 zylKb&ES^D^0cvG%POGrjUp+F{6jS0;dR&*l*Il(IwdgH>=iM>KV@g#+{`*tOs}7v6;lDxPX_h{plW{b0xtK};>R1M6AEJH$V=SH#>Jz$Y_-o+yh2A&#Fny)#823k0Dw46wa*9 zSfL@`%taYql1^dm-P8(A+8U2tu8t0f7JXNNPA@0!TmfD|%#KPH#A9{7kL9HAUox3% zDNt4Qr@2sL5nVaN>=#j;2i7h!uTY+wdAXOEj&61mZ#YRoFX>MskJN9i!lwfy3|zpf zEOqbs&K^Q4E=8k*YUo=TO!ik9Yn4iWDF$gc?Y6xvf~D$=;9ps?!kX%8ZwlDN;N4Tq z8T;`Eesxih^Fis!Ey3%(jm$7NlCBs30XOoO|D#~|O25kKj9A>R)(>`pGMnO&nv7adE{Kh`H=SjYH5*_{2 z*`!rFBwWy&NT|s5q*S1w7e-4%3aJUxiU@j+9J!6!U_q_tX+(BzC}vn%w6UzgXTU;A zOE@6-x57n2?R|o}5e$;L5mnCo=w*)7^1Nj}%<}t-vTOj8itsCSL$*6Qs>TL|sObQL zM=JBH@4T4xe=ySL;?!f2>2rHl>er(+47&!(3VTX^vW;OL-Y#IH=wb~eYO1>UUvVFf zg5^G;IbzYuyh_*jXAfeqUK$W&lKgw5O7HQB`{;z|lAPVw4{YVLPvbO(B(Ath@K=Q? z6@*H|pqhVKAhmw>Rv#oKY^7woh9j~Sw6=TpFUtla zi&vZ9rxlMpR^>SRj1LX=Z9>OuNQz9FE9*J+n?H=J8@&`;>TWdTO;>w#s z9BFQLY<6@?oPMD>?3diyoSne1oZ<0sqTe6qwzqig8JBxk;eFyL_EZQ%K=X z?F|UB#a~y%{@L_}_!<83@Tn3RBl+J4#SHR`F9Ce~p`$$Bmj1{wNEA5Z>9bozX!V4u z)VfR)F4DZaV1JIJ;$ir+_Fidl+C7>27)N~HF4LY%T z8#TQTYJD;-^uc{#b={H`c*a8Ba%6CoKRtO&M^^42^%S0VH14zs|E7cF{IBIg@4DsSf)A zl;h+hq6u95W+h!w-4bnnAV3R(fvGHaU_#e3<#Lavnyq4x>vT2>K13yQUx>k>1;Z|} zjnjwXi8EHkXI*IxG93il0NI>=(+5hXe_~~Do+7ygxHYl8zHd~CK7Ng$oR81S0Q@fC zCl)klP0Mnq?om2^5G7!HQfrcV+03j+Ab-#yy(7?qb9;UX-;9TpF(T&L1G7^7&O;uY z>Jz8ZX!1djiY?m--n!?-K382iS?OEhTvbQ|Kn#VC@(g@q-~h|7_R7%E=JxBBkf-z6 zR52R5Sy85LVrWR7Y*3p3ns(DoF*0}BYiT=YBW6lFPy)79^^Q32Re++OSA}kI{eJ_V zx!LZdU(>=z2$_T-Xme<#Dj3V4gxBLMpi?k#L>i$1ABeo+!JO!g8Zh)mE&IRj%>5>H-5@ z)B);E4QzEl%X>DmDX({~`24z{vII47^CrK+iaD2j{COp+MzQQS*itE6P~ZO25Y)l) zt?}=8cFn6R)A>_X-K?h}!tvN{`K_t=xpPjF!|#{ajB{O}7D8T?&IuE%f$q{WPeIl0 z@8P$r9=|$OH`cz1Ep!nGu~e~p^WyrC1s@3N8R~)$76sKmvD6p$Le)JtOX7Xx3c;S+ zMd=VZ+l``)B9cDMdnoLHi3=DrL>ykgQ?*fsIxM4&C|^!n`xX>qT^Z1BSI2Nt^`1lg z8%&rP2w5xc>X$&~ z7qnc}nS;0L1KqC@V^z7BOMMHrlKu;zp|J~A97`{HG`~bV)O#e?N>XXh@kpr}+M3YS zenI@Y4+c^_iqw1j#4Oqpl>yaUs;U_-l0?|m2Z@iHRcYy$vCu6h#o{fp=yO>#^Y7&s z?&r~(&hK8AzrP#lRTDfkI9&GNvhe5qY!xl^XG;Ivi}0b7!SJdh9G1;O5BXvin0DBT zO5qV@OV*n|)}Ku2t7Kxcu~q#jmOmn7{8St?Mq7FZvrY5%GH;b)WDaVelN?{RIk;A| zIaoHvdg^Tuz}vv9k@8*NFjC6|gU*NzA#HzP`n!C|FC%GsD`ABKFQ>A+ytr#*>z#;v z9!ZQO=qRy7@&5tjU!<5ZPsrAETC&wPHIbZ{C4lWU{fM zw^2fT#U}OE;|Zct^N9ZjKI&DDkatRNfZ+Op_QqbEh0ZS4q^++jY#pqTM$@!?RQhX; zZlix%od?`93pE{9bp)IejBW>4811SD)GZQ>zn9B`YRX6Ap-FjEi_fU*a#*DaeKb98 ze)!uYJ5kwGh5Y$g@`^=gJuyz)e8?>0oZrK?txAG3Myw95c#6Z}b7}+&3{)zC`e5BF zPQoAW{`8HP`tfnYSOIh}0RXm8ppQ};3a*U5YW>}q-8{%l$$)pQ73ht0q?sR+%5X&o zi@xbpg|3=+m19|Uc8{Ae)WF;Tv=E+++7R?$nJ^P0ixs?Cs0a?6q}o-aa4$43MI{PL zO38-)9I2UJ%McsAjfuU`uD3|>3#8f=56kxMFIx$67Pp^s_wiH&aTD-7>y_9MRM-v1 z_qvoZOWm|q`iPaz@*-cW>D{by&Tu^o@*SFF;!t#Ag@t*u299%jg~Rdn6!MkVEfV$S zP3VUr-VM%rfUszam!r2f_AhPBc;{~Y0L}a7G*z!NRQdLVrO_81kXE%p`Ru;nG*Va^ zzoq7ApyP#)CchDf=3bg7Z<<$o!-j=P7m~}KCjau8=csO5T+cv-ZvNO$zQ}h{dV@r_ zxUY)#i;SxFp-YQvoZV)kj-bZkMFYBb?rUY1_UDFrch?ocR*zErTR*={KFF2H-Z~J4 HVPX6ai+&Il delta 6827 zcmZ|SWl+`8x(0AU>6T9E5)kPIY3UZ}*dPtE*))G7rDGG)(kV#Crn^HxxN znS0Kix#yhq;ho<+U)P#>-qj-?E(g#>VIckEprFD{;IT_ekcX4vm#u&eQt9X|xB9>f zFYz-+B;NrFUr)kUq-aK#o_s~Urja!(Fr|2OWm9gAL=No-`g%#WghJ<14gCIw8DbX_ zz2f8VM!UK5J6 z)2_TY))8y}3&g?BMO01e{YGC@m03sPkv&nWw@eNA9m`PEQ)KO{01F{H6&8wf&c4^PHypme|0x0n%bI5MM;(n%P0x;)zTW>TH037+S2Md6nWPqbz;m1v$y1-%z3(o8EkrbKy6%>R~-yx4Jg?a z-Q2QcPnN1DSjy?1F^2Kl(1k^-65HqQU-B)iF7F47f$jthj^>O-Z@_6}Aj0Aue`v9H zHuG6i!w1r7Yfmw#Oy&Z8L!}Xyj~kGZ0&35Fuyqr(`}z46PF3d=wS2)qQ;B}OOu~75 zErX zPiV)B_g|+ulG|L&RWwca9o3L!WZ!OjsL`oD{0geARJA_S(;jtxn*N@FX9Jj>I`(63 z%A^G}2YBqJZ5KZ!;0znuu36vAb;%ni$w*zZKV97mVpP*BiTd5#&$&L@aa~QUj;V(r zXy-ryd_}jqMDJquFDV$`PWCCf0Stzq-9O+dRSRFDxQW{BU&Js$=I0W*nXVvkl?G#X z{pu7XkvU%@xRrb@j!N{z76(+GZhMTl?2n`9j)BIbJfL)FK;uwJ01c)X0@a@-SySGr z)qbKRa&yz{%rhmBD^k{=FE@*~x zYGsE)p7#I(n~Q1ZZTi7xMH`fMKm_~Y#HAMGB^cDj!#UDEw9(){;f{~?9Wj!ko6eu z##%g-#r{SixZDwF_ZCXr7wpjIcg^Ex`Qq`8Os9ee~<}sAT0|h+W73%~` zSn*o=rT+Bm-A4%x@Xt7<-XkfW8G>AB7y2pC6r3K?lmY&uR5vLHNnAHptozECPNzYE zgXo{Z!@elRF`YwF4P%|H*8|fuZ6B`8>1ppDQ$ecz5IdI)-vUlr3gR6qXk7rZ;c5Oo3&wv|^BTjR(jZ?&SXBLWc zAeCz|5;YR~GJ7Z54`ffQd1Oc|rdVS})SU|P%u-hyKIjN-a1Cm4GFvyAz>oAT7D=-t z1p80muonQEn>0Da6uito$*; zelF^_w%B&m*sFqRJ_H?>{T+9Se_P)7w{*lat12>Z$rud2ws#F7k&Aa_C2EEaRr8X+ z9ZCozFv#A`ge4KN{;JWP*;hFFS{*#=+qSYJVIr2qC}-X^K>551)0%2#HsD+LV%>w` zAkCft63s$d`TJNEL@}GUwk5YDr`PsMKU23yI2n>{f(_%>Wb|`x8nI4-~)-Mt*>z z;y9tbfRmWT1VJG4k#@_hK>~U)RH36b?%nR zIoTl}A&GU4u9J*zsUk@TTRL+(l17xZ{YaS^C5pLLHj`%} zZq>Hh`{K)-37Jw>;&qi?SoI(}ag$ugW+F-_%wHy^kNAy38KKTkge(5l`K0S_S;QDj z6lU4SQx;R!nU0jAu_G!d9j7^)IGW1*VP!d+40OyOZJ^TvK4fub|6GNc-~aCCpL(fO z(}g{TJaua_LaTfGmG4}PHVq8i@JJPo?wG{csZ09QfH~t8aD4=)yJGmU+OZ;;@<^R; za}MrZ6$vnTD|3kcovtq6KS98net|r^Rzq=E_T4}iI5P$MQVJUg8}d$Ig034$L)-D% z#W>HkPDgmE+5xl%BL2^TsXSiDCh>SdiZH&;GKqX;9=62V0;T?7Yw{#-O;e_>9N=bB zh(W35Ra@rv{^?0YeFpGbrtUUbk1?jji=z2kh0H)$-?Kejp*{e?L!gB_Sg6< zK$P>_jL2a)wjNL>LN`2T9tt{H_4m5-pI{JIVSx@?Cg2d=EiedkolhuV{<<_#KCQYM z&UGsD?RhxhAa8;6gI>E)nKfX(Ai7_GNLHIP@tPWCd+a*9D4kBVduPuBXr)fyGwj23 z#5Cc-m@LzqquiuU!Nv`fl5&V5C+F8$s#~vMEf1mfJHRbjCfSwb-@U86dHBJ^{blgU z<4&sQgOa}sPc8mpC8;GM%};!71E}YjKC>m#jhkiQ&5@Az)T4=Op-L4}YlqLN#RcWH zRJ^lW5l5O+TWXsEYoirkx#5I2n*BJ`6rffPR3@g>9?hL(KoXsdKbCst&70PD(b48h zbH4^W0Tbv5*WhK5{`8v*;OU_Q8RdZ$vo7w6FaT{*)J%1PaU6GMLNKL~@WMM=tQFZ= z1IHQo8Y?+pA7P@FG-T}(n6U^-5czdK>k*5k>GMI;2O4nKXm`}kvR;Bs?N8QzFY8Qa z&-0C^yyKj~oCi>!C`-~y>o+uCd!$v~D9)Gwu5acL7(^S@fjKLu7p&{WviE*S0omjM ztEf^+MfpD_s!6XR7NZJWcHjxSZ|I`sG7qzLGD0H{q3wB
  • o~&j@wd*6RKBFFn$f zVu;?!uhe7-*t4BpTw~wAo-ON)OMBwS7h^xICA|Kn?$BpP#@L#>K9m0DtwBRUW>!8e zpwP;F&oW9EE!DK)|wxIlA{e6E~ zu$74nkEiRaS<5OtiAf&7Vb?Zq?g`CTVg%tx}ikI z|7jUrvBjLpx=7He>AW4$QkOr{;5vUK%{-@l)Jh8Tnu>J%*AiK}9Wi@lvKd9lzIm&???NO2@kZ>h_0ID>$do zL>9#5xI;#rVrKS^y)Mx%ojmZp^TUGNR{K<>^*GYgR<-w4bQmXa4dp@)q!ID0m2|Dl zn#Fh^X|vajLw+zby}R2)>BP%yaSAkNr&tXcLs^VN=vptn0Gt zynFv8Fe)UjE7UU67`@xWeGF6?c zl7=8>2FVE{8Gjk!*+}d|gXD;AB`WJUJh1y0Db1SqA?W;=ue(Ewl5@T!Szj_q%eMg* zvdypKoQYhyF2ym)yVaBmNk>19m*9!8e3s36Hj1jdPEZHEuRhO`!sBGrdyWTFs^oAT z{Gz!p5neHqpu4Ua_4e(G|MOS`w^#kv;jVQeDQB#s3?UxzhV*|M!buF&E@DZ{vd?e{ z{{Sya=}-T+^@0;MI`Mu4_w*qD?KQ2QU4-%xx&PnJL)~*k&zdt773JC_vY_e={$!_U z$gNh#llm*2V=-Q$2V+ZN<{#a?X-xiMB$AMw;dfg_cTE6yN9FIi1qeIF_qYbIcDcQs zrA*P-6<9QD-DJHDe(h|mEbqV!*vlqf7&4RS&*}5N-ItIKFf`B--M4!iK=YBV0{j!r z((z8)j^w%cA1tsAp;i75mi{-EuX=GAm-%=a0>@4| zICmEF_+s~C+iRN^qFLsw6obg08dB`< zNkp-5PI?~g8L|xQonRpCH}u{ME9UHx$}hXHR8}Wd*%+oQJ;&r$$;mhsn#)@SLXM;b z;KhYsd;W-WNq2Hm-I%3$E-lyDavei<75ni)vR7q`aA)n#=_=Z3eU`(bh>)(qVqDwg zcq-%%eR&<~3I3oc5G}AE^kW<*G9GvQf!w>?XY|*whm6h8vPwf-&}dz9%vkYlwa8=W z)u6bo+yqo2TIjboXUoDl;wW)h4xG~0P;N3(Kx|KUKKy58D}eE6@x(oBJ&!G!bhA@uw##~s2^%x!b;X|&hvj2;61MqBk z!?V!?RC9eE4)z8seJY;OgP8bK3!&lC$dFNq)Kl9KcZid+{+D7^Xg?V#G7EHtj0xyq ze2W_sGPkmolI2hXjPh^1(=TNwO^bIs(9z*+Sf-! zSJQNSpTT>yycq9Z2fGeK{QRKUVxgJ+!4tcBF}NBqmsxI&_%k~IFN;ltQ7cUbVPhuxGZ?Rh%8PxQ(awN~qb{|_KPcR<>xaFci zGlPe-e9lq}J`jQ)7y!4|hept9a#_F`D5Dh!;W&;>I&WcVw3#2p$z|nd&?a9w#HK=b zmw#x1H%K^cHZ`+EzVSX>4KWy?S-_)s5gHxI#t-w+7IqeP=83)Ak{7L3_Y$tZeoD7V zGnx?AFcmo;&NI5+wyn3)h>{`9NtO}CyPlAd*LvBdc$We86RC$dwhtS@!XrvVD1()!IgM*v5yG}2j?-v^WsY@Ih zA}#`nBTk=>fI_jy_N#)5xj9o#w)cE~HvRJW>2hJ8?jn!reLgPrgiQrsZhGdS$L=9d z^-RamI>r;El68SdVLWY+ZJ?(>H8Q~;(q*ulleI=`6ns@__VdDicUFM{qEuwnctbIN z@+lpc6VWj`Efs}MW_RA#{sJd4FWTwI%`iuZVm|keh%$F@9geZ88v;T01?`Fem)!Dz zB9)PH-irTa%OLZ|!g7C?1CccIO8C52;KFlBj|?DTIvMk~SjJ>c9H54RlKVw)Bl`;3 zxLU+ccsJ4hPO*gX*)t3bS}p0*QTb<$HN6?}=85-C>Cx!*4pQX-0KAV?M!cH7C(XZ3 z5TmKAvZ+nhHN~cux+ifdyQO5^Uv$t6{qyqxZDUgIs~@~QUhmD^2w{U3ua#J!9Ib<_ zpwofB_g|mx=5)k60?-@J8!b(M1jk*^MqYaG9W#;7qrIof_xk!WdtODntDHhCl0f5# zH~XWRm!Ta)Nz^k>uFJVsnLli{N26m{ZhtC}={BbdKK`LpsXI1Fck_Nf%yroiuIhN$!AkrG$=*p;({I`)DTv>WvEWYGGfoqE)Om`u%r3L}r6>&fCCBv3m@5V0?1ZoiuCF7C@d!1r)Xkss+=`LX)7T&DZI z{OkPO8ILyJA~jGtFYm^~`W5Iw?ktxT>q@}Yf3z=rk&GN1xMj+TZzo$)yYVqdlsKX?D)RtGoDPNmmgC$9vW`k{o#aEy@i)$tjkMqn z=Zy+>J>rs#=~^G)7U8=4(DpAFRz>5#%ay#OgdFS?Q_qa@e0=iblA40-u%~~G)tT2j zZ!U;CP3?Wg5H9lr#P?ncCS#6bsB4wiy%ACB{twnJX@D94yug3cnFx;uurA8B$;gbPHr{$pkGKd>e z@TzjX>}tP^JhXfjvBr@3(I=^; z-I4wcll8yX`=B;Tja3oFA{z-coc8Liv@SV_$& zNgpmIp905|^+KVEiNP^mxFVfXn0l=sh3F#zMa$JJQYg)=DYg@~4ros|IrjyrbeJBd z*aSrj>4QDQ)TW@W^#$?|)QMlSvpQxVH{SG@TH$guL>@{r^#2FoU(wt`w+W=$b@y=& zUh%UD%iiEy4XZz=vFC|StpF^=(iwxUFa1H5B2}kiq1t93oyGz3pCHCOS0}}r$CXg9 zu(|SKBk0i7EMCcN3{GT9Aj@eNy%EWNve*cEFmBjv9Q#@1%x3ddK(X9^%L%V`&XSpD zpnOt~j%Z#Fq%fp3g>^)TZKy#&8wx9G_A{4wGhC1L#z5L%WTp_m#AUEAgWI}t^g6Zq zjfp{Kk{s2-RxTn43iB*imX!STXAL&RE^+F2&r>C}oSD8`1C^ww5EM^5R~q(IEN}pf zLnb9ieLz2F2jt~uH?ue`=dvLdW3AAA$caJitS{4o$@5In)7iU>(RyTP3_Uq8fI^Gy zLz%vN8a1*QfDR6q2h+?0mxzElbO&dmOGQwAKTnkW^q23cN|+JFw2e;V=CaWC?*cxfBK;4yu3RDj diff --git a/flight/OpenPilot/Makefile b/flight/OpenPilot/Makefile index b598f969b..7eeca005f 100644 --- a/flight/OpenPilot/Makefile +++ b/flight/OpenPilot/Makefile @@ -188,6 +188,8 @@ SRC += $(OPUAVOBJ)/vtolsettings.c SRC += $(OPUAVOBJ)/vtolstatus.c SRC += $(OPUAVOBJ)/mixersettings.c SRC += $(OPUAVOBJ)/mixerstatus.c +SRC += $(OPUAVOBJ)/guidancesettings.c +SRC += $(OPUAVOBJ)/positiondesired.c #SRC += $(OPUAVOBJ)/lesstabilizationsettings.c endif diff --git a/flight/OpenPilot/Makefile.posix b/flight/OpenPilot/Makefile.posix index a7700e269..16cea3414 100644 --- a/flight/OpenPilot/Makefile.posix +++ b/flight/OpenPilot/Makefile.posix @@ -53,7 +53,7 @@ FLASH_TOOL = OPENOCD USE_THUMB_MODE = YES # List of modules to include -MODULES = Telemetry Stabilization/experimental/Stabilization Navigation ManualControl +MODULES = Telemetry Stabilization/experimental/Stabilization ManualControl Guidance #MODULES = Telemetry GPS ManualControl Actuator Altitude Attitude Stabilization #MODULES = Telemetry Example #MODULES = Telemetry MK/MKSerial @@ -164,6 +164,8 @@ SRC += $(OPUAVOBJ)/vtolsettings.c SRC += $(OPUAVOBJ)/vtolstatus.c SRC += $(OPUAVOBJ)/mixersettings.c SRC += $(OPUAVOBJ)/mixerstatus.c +SRC += $(OPUAVOBJ)/guidancesettings.c +SRC += $(OPUAVOBJ)/positiondesired.c endif ## PIOS Hardware (posix) diff --git a/flight/OpenPilot/Modules/Guidance/guidance.c b/flight/OpenPilot/Modules/Guidance/guidance.c new file mode 100644 index 000000000..da71c6a24 --- /dev/null +++ b/flight/OpenPilot/Modules/Guidance/guidance.c @@ -0,0 +1,280 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup GuidanceModule Guidance Module + * @brief Guidance PID loops in an airframe type independent manner + * @note This object updates the @ref AttitudeDesired "Attitude Desired" based on the + * PID loops on the craft position, speed and course + * @{ + * + * @file guidance.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Attitude guidance module. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "openpilot.h" +#include "guidance.h" +#include "guidancesettings.h" +#include "attitudedesired.h" +#include "positiondesired.h" +#include "positionactual.h" +#include "attitudeactual.h" +#include "manualcontrolcommand.h" +#include "systemsettings.h" + + +// Private constants +#define STACK_SIZE configMINIMAL_STACK_SIZE +#define TASK_PRIORITY (tskIDLE_PRIORITY+4) +#define LATERAL_INTEGRAL_LIMIT 0.5 +#define COURSE_INTEGRAL_LIMIT 0.5 +#define ENERGY_INTEGRAL_LIMIT 0.5 +#define SPEED_INTEGRAL_LIMIT 0.5 +#define DEG2RAD ( M_PI / 180.0 ) +#define GEE 9.81 + +// Private types + +// Private variables +static xTaskHandle taskHandle; + +// Private functions +static void guidanceTask(void* parameters); +static float bound(float val, float min, float max); +static float angleDifference(float val); + +/** + * Module initialization + */ +int32_t GuidanceInitialize() +{ + // Initialize variables + + // Start main task + xTaskCreate(guidanceTask, (signed char*)"Guidance", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle); + + return 0; +} + +/** + * Module task + */ +static void guidanceTask(void* parameters) +{ + GuidanceSettingsData guidanceSettings; + AttitudeDesiredData attitudeDesired; + AttitudeActualData attitudeActual; + PositionActualData positionActual; + PositionDesiredData positionDesired; + + ManualControlCommandData manualControl; + SystemSettingsData systemSettings; + portTickType lastSysTime; + // declarations... + float lateralError, lateralErrorLast, lateralDerivative, lateralIntegralLimit, lateralIntegral; + float courseError, courseErrorLast, courseDerivative, courseIntegralLimit, courseIntegral; + float speedError, speedErrorLast, speedDerivative, speedIntegralLimit, speedIntegral; + float energyError, energyErrorLast, energyDerivative, energyIntegralLimit, energyIntegral; + float sinAlpha, cosAlpha; + + // Initialize + lateralIntegral = 0.0; + courseIntegral = 0.0; + speedIntegral = 0.0; + energyIntegral = 0.0; + + lateralErrorLast = 0.0; + courseErrorLast = 0.0; + speedErrorLast = 0.0; + energyErrorLast = 0.0; + + // Main task loop + lastSysTime = xTaskGetTickCount(); + while (1) + { + // Read settings and other objects + GuidanceSettingsGet(&guidanceSettings); + SystemSettingsGet(&systemSettings); + ManualControlCommandGet(&manualControl); + PositionDesiredGet(&positionDesired); + PositionActualGet(&positionActual); + AttitudeActualGet(&attitudeActual); + + // lateral PID Loop + // error is the distance between the current position and the imaginary line going through PositionDesired.NED + // at PositionDesired.Heading degrees course + + sinAlpha = sin(positionDesired.Heading); + cosAlpha = cos(positionDesired.Heading); + lateralError = ( + - sinAlpha * ( positionDesired.NED[0] - positionActual.NED[0] ) + + cosAlpha * ( positionDesired.NED[1] - positionActual.NED[1] ) + ) / ( sinAlpha*sinAlpha + cosAlpha*cosAlpha ); + lateralDerivative = lateralError - lateralErrorLast; + lateralErrorLast = lateralError; + lateralIntegralLimit = LATERAL_INTEGRAL_LIMIT / guidanceSettings.LateralKi; + lateralIntegral = bound(lateralIntegral+lateralError*guidanceSettings.UpdatePeriod, + -lateralIntegralLimit,lateralIntegralLimit); + attitudeDesired.Yaw = angleDifference( bound( ( guidanceSettings.LateralKp*lateralError + + guidanceSettings.LateralKi*lateralIntegral + + guidanceSettings.LateralKd*lateralDerivative + ),-90,90 + ) + positionDesired.Heading + ); + + if (attitudeDesired.Yaw<0) attitudeDesired.Yaw+=360; + + + + // course PID Loop + // very straighforward + courseError = angleDifference( attitudeDesired.Yaw - positionActual.Heading ); + courseDerivative = courseError - courseErrorLast; + courseErrorLast = courseError; + courseIntegralLimit = COURSE_INTEGRAL_LIMIT / guidanceSettings.CourseKi; + courseIntegral = bound(courseIntegral+courseError*guidanceSettings.UpdatePeriod, + -courseIntegralLimit,courseIntegralLimit); + attitudeDesired.Roll = bound( ( guidanceSettings.CourseKp*courseError + + guidanceSettings.CourseKi*courseIntegral + + guidanceSettings.CourseKd*courseDerivative + ),-guidanceSettings.RollMax,guidanceSettings.RollMax + ); + + + // speed PID loop + // since desired value is given as groundspeed, but our limits affect airspeed, translation is necessary + // we assume a constant (wind) offset between the two + // (this is not completely correct since there might be an air pressure dependent linear relationship, too + // but this puts only a linear multiplication factor on our error) + speedError = bound( + positionDesired.Groundspeed + + (positionActual.Airspeed-positionActual.Groundspeed) + ,guidanceSettings.SpeedMin,guidanceSettings.SpeedMax + ) - positionActual.Airspeed; + speedDerivative = speedError - speedErrorLast; + speedErrorLast = speedError; + speedIntegralLimit = SPEED_INTEGRAL_LIMIT / guidanceSettings.SpeedKi; + speedIntegral = bound(speedIntegral+speedError*guidanceSettings.UpdatePeriod, + -speedIntegralLimit,speedIntegralLimit); + attitudeDesired.Pitch = bound( -( guidanceSettings.SpeedKp*speedError + + guidanceSettings.SpeedKi*speedIntegral + + guidanceSettings.SpeedKd*speedDerivative + ),guidanceSettings.PitchMin,guidanceSettings.PitchMax + ); + + // energy PID loop - flight energy = mass_factor*(speed^2+g*altitude) - using mass_factor=1 + // the desired energy is calculated from the desired airspeed (which has been calculated above) + // and the desired altitude + energyError = ( + ( speedError + positionActual.Airspeed ) * ( speedError + positionActual.Airspeed ) + + GEE * -positionDesired.NED[3] + ) - ( + positionActual.Airspeed * positionActual.Airspeed + positionActual.Climbrate * positionActual.Climbrate + + GEE * -positionActual.NED[3] + ); + energyDerivative = energyError - energyErrorLast; + energyErrorLast = energyError; + energyIntegralLimit = SPEED_INTEGRAL_LIMIT / guidanceSettings.EnergyKi; + energyIntegral = bound(energyIntegral+energyError*guidanceSettings.UpdatePeriod, + -energyIntegralLimit,energyIntegralLimit); + attitudeDesired.Throttle = bound( ( guidanceSettings.EnergyKp*energyError + + guidanceSettings.EnergyKi*energyIntegral + + guidanceSettings.EnergyKd*energyDerivative + ),guidanceSettings.ThrottleMin,guidanceSettings.ThrottleMax + ); + + + // adapt roll in case of negative pitch demand + if (attitudeDesired.Pitch < attitudeActual.Pitch) { + if (attitudeDesired.Pitch <= attitudeActual.Pitch - guidanceSettings.PitchRollEpsilon) { + // in case of heavy push, reverse roll + attitudeDesired.Roll = -attitudeDesired.Roll; + } else { + // otherwise linear interpolation between Roll and -Roll + attitudeDesired.Roll -= 2.0 * attitudeDesired.Roll * + ( attitudeActual.Pitch - attitudeDesired.Pitch ) + / guidanceSettings.PitchRollEpsilon; + } + } + + // Write actuator desired (if not in manual mode) + if ( manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO ) + { + AttitudeDesiredSet(&attitudeDesired); + } + else + { + lateralIntegral = 0.0; + courseIntegral = 0.0; + speedIntegral = 0.0; + energyIntegral = 0.0; + + lateralErrorLast = 0.0; + courseErrorLast = 0.0; + speedErrorLast = 0.0; + energyErrorLast = 0.0; + } + + // Clear alarms + AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE); + + // Wait until next update + vTaskDelayUntil(&lastSysTime, guidanceSettings.UpdatePeriod / portTICK_RATE_MS ); + } +} + +/** + * Bound input value between limits + */ +static float bound(float val, float min, float max) +{ + if ( val < min ) + { + val = min; + } + else if ( val > max ) + { + val = max; + } + return val; +} + +/** + * Fix result of angular differences + */ +static float angleDifference(float val) +{ + while ( val < -180.0 ) + { + val += 360.0; + } + while ( val > 180.0 ) + { + val -= 360.0; + } + return val; +} + +/** + * @} + * @} + */ diff --git a/flight/OpenPilot/Modules/Guidance/inc/guidance.h b/flight/OpenPilot/Modules/Guidance/inc/guidance.h new file mode 100644 index 000000000..8c9969628 --- /dev/null +++ b/flight/OpenPilot/Modules/Guidance/inc/guidance.h @@ -0,0 +1,43 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup GuidanceModule Guidance Module + * @brief Guidance PID loops in an airframe type independent manner + * @note This object updates the @ref AttitudeDesired "Attitude Desired" based on the + * PID loops on the craft position, speed and course + * @{ + * + * @file guidance.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief guidance module. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef GUIDANCE_H +#define GUIDANCE_H + +int32_t GuidanceInitialize(); + +#endif // GUIDANCE_H + +/** + * @} + * @} + */ diff --git a/flight/OpenPilot/UAVObjects/guidancesettings.c b/flight/OpenPilot/UAVObjects/guidancesettings.c new file mode 100644 index 000000000..32a2a51d1 --- /dev/null +++ b/flight/OpenPilot/UAVObjects/guidancesettings.c @@ -0,0 +1,132 @@ +/** + ****************************************************************************** + * @addtogroup UAVObjects OpenPilot UAVObjects + * @{ + * @addtogroup GuidanceSettings GuidanceSettings + * @brief PID settings used by the Guidance module to combine the @ref PositionActual and @ref PositionDesired to compute @ref AttitudeDesired + * + * Autogenerated files and functions for GuidanceSettings Object + * @{ + * + * @file guidancesettings.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Implementation of the GuidanceSettings object. This file has been + * automatically generated by the UAVObjectGenerator. + * + * @note Object definition file: guidancesettings.xml. + * This is an automatically generated file. + * DO NOT modify manually. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "openpilot.h" +#include "guidancesettings.h" + +// Private variables +static UAVObjHandle handle; + +// Private functions +static void setDefaults(UAVObjHandle obj, uint16_t instId); + +/** + * Initialize object. + * \return 0 Success + * \return -1 Failure + */ +int32_t GuidanceSettingsInitialize() +{ + // Register object with the object manager + handle = UAVObjRegister(GUIDANCESETTINGS_OBJID, GUIDANCESETTINGS_NAME, GUIDANCESETTINGS_METANAME, 0, + GUIDANCESETTINGS_ISSINGLEINST, GUIDANCESETTINGS_ISSETTINGS, GUIDANCESETTINGS_NUMBYTES, &setDefaults); + + // Done + if (handle != 0) + { + return 0; + } + else + { + return -1; + } +} + +/** + * Initialize object fields and metadata with the default values. + * If a default value is not specified the object fields + * will be initialized to zero. + */ +static void setDefaults(UAVObjHandle obj, uint16_t instId) +{ + GuidanceSettingsData data; + UAVObjMetadata metadata; + + // Initialize object fields to their default values + UAVObjGetInstanceData(obj, instId, &data); + memset(&data, 0, sizeof(GuidanceSettingsData)); + data.UpdatePeriod = 10; + data.RollMax = 35; + data.PitchMax = 35; + data.PitchMin = -35; + data.PitchRollEpsilon = 10; + data.ThrottleMax = 1; + data.ThrottleMin = 0; + data.SpeedMax = 100; + data.SpeedMin = 10; + data.SpeedKp = 0.04; + data.SpeedKi = 4e-06; + data.SpeedKd = 0.01; + data.EnergyKp = 0.04; + data.EnergyKi = 4e-06; + data.EnergyKd = 0.01; + data.LateralKp = 0.04; + data.LateralKi = 4e-06; + data.LateralKd = 0.01; + data.CourseKp = 0.04; + data.CourseKi = 4e-06; + data.CourseKd = 0.01; + + UAVObjSetInstanceData(obj, instId, &data); + + // Initialize object metadata to their default values + metadata.access = ACCESS_READWRITE; + metadata.gcsAccess = ACCESS_READWRITE; + metadata.telemetryAcked = 1; + metadata.telemetryUpdateMode = UPDATEMODE_ONCHANGE; + metadata.telemetryUpdatePeriod = 0; + metadata.gcsTelemetryAcked = 1; + metadata.gcsTelemetryUpdateMode = UPDATEMODE_ONCHANGE; + metadata.gcsTelemetryUpdatePeriod = 0; + metadata.loggingUpdateMode = UPDATEMODE_NEVER; + metadata.loggingUpdatePeriod = 0; + UAVObjSetMetadata(obj, &metadata); +} + +/** + * Get object handle + */ +UAVObjHandle GuidanceSettingsHandle() +{ + return handle; +} + +/** + * @} + */ + diff --git a/flight/OpenPilot/UAVObjects/inc/guidancesettings.h b/flight/OpenPilot/UAVObjects/inc/guidancesettings.h new file mode 100644 index 000000000..3e55a849b --- /dev/null +++ b/flight/OpenPilot/UAVObjects/inc/guidancesettings.h @@ -0,0 +1,131 @@ +/** + ****************************************************************************** + * @addtogroup UAVObjects OpenPilot UAVObjects + * @{ + * @addtogroup GuidanceSettings GuidanceSettings + * @brief PID settings used by the Guidance module to combine the @ref PositionActual and @ref PositionDesired to compute @ref AttitudeDesired + * + * Autogenerated files and functions for GuidanceSettings Object + + * @{ + * + * @file guidancesettings.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Implementation of the GuidanceSettings object. This file has been + * automatically generated by the UAVObjectGenerator. + * + * @note Object definition file: guidancesettings.xml. + * This is an automatically generated file. + * DO NOT modify manually. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef GUIDANCESETTINGS_H +#define GUIDANCESETTINGS_H + +// Object constants +#define GUIDANCESETTINGS_OBJID 2093064904U +#define GUIDANCESETTINGS_NAME "GuidanceSettings" +#define GUIDANCESETTINGS_METANAME "GuidanceSettingsMeta" +#define GUIDANCESETTINGS_ISSINGLEINST 1 +#define GUIDANCESETTINGS_ISSETTINGS 1 +#define GUIDANCESETTINGS_NUMBYTES sizeof(GuidanceSettingsData) + +// Object access macros +/** + * @function GuidanceSettingsGet(dataOut) + * @brief Populate a GuidanceSettingsData object + * @param[out] dataOut + */ +#define GuidanceSettingsGet(dataOut) UAVObjGetData(GuidanceSettingsHandle(), dataOut) +#define GuidanceSettingsSet(dataIn) UAVObjSetData(GuidanceSettingsHandle(), dataIn) +#define GuidanceSettingsInstGet(instId, dataOut) UAVObjGetInstanceData(GuidanceSettingsHandle(), instId, dataOut) +#define GuidanceSettingsInstSet(instId, dataIn) UAVObjSetInstanceData(GuidanceSettingsHandle(), instId, dataIn) +#define GuidanceSettingsConnectQueue(queue) UAVObjConnectQueue(GuidanceSettingsHandle(), queue, EV_MASK_ALL_UPDATES) +#define GuidanceSettingsConnectCallback(cb) UAVObjConnectCallback(GuidanceSettingsHandle(), cb, EV_MASK_ALL_UPDATES) +#define GuidanceSettingsCreateInstance() UAVObjCreateInstance(GuidanceSettingsHandle()) +#define GuidanceSettingsRequestUpdate() UAVObjRequestUpdate(GuidanceSettingsHandle()) +#define GuidanceSettingsRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(GuidanceSettingsHandle(), instId) +#define GuidanceSettingsUpdated() UAVObjUpdated(GuidanceSettingsHandle()) +#define GuidanceSettingsInstUpdated(instId) UAVObjUpdated(GuidanceSettingsHandle(), instId) +#define GuidanceSettingsGetMetadata(dataOut) UAVObjGetMetadata(GuidanceSettingsHandle(), dataOut) +#define GuidanceSettingsSetMetadata(dataIn) UAVObjSetMetadata(GuidanceSettingsHandle(), dataIn) +#define GuidanceSettingsReadOnly(dataIn) UAVObjReadOnly(GuidanceSettingsHandle()) + +// Object data +typedef struct { + uint16_t UpdatePeriod; + float RollMax; + float PitchMax; + float PitchMin; + float PitchRollEpsilon; + float ThrottleMax; + float ThrottleMin; + float SpeedMax; + float SpeedMin; + float SpeedKp; + float SpeedKi; + float SpeedKd; + float EnergyKp; + float EnergyKi; + float EnergyKd; + float LateralKp; + float LateralKi; + float LateralKd; + float CourseKp; + float CourseKi; + float CourseKd; + +} __attribute__((packed)) GuidanceSettingsData; + +// Field information +// Field UpdatePeriod information +// Field RollMax information +// Field PitchMax information +// Field PitchMin information +// Field PitchRollEpsilon information +// Field ThrottleMax information +// Field ThrottleMin information +// Field SpeedMax information +// Field SpeedMin information +// Field SpeedKp information +// Field SpeedKi information +// Field SpeedKd information +// Field EnergyKp information +// Field EnergyKi information +// Field EnergyKd information +// Field LateralKp information +// Field LateralKi information +// Field LateralKd information +// Field CourseKp information +// Field CourseKi information +// Field CourseKd information + + +// Generic interface functions +int32_t GuidanceSettingsInitialize(); +UAVObjHandle GuidanceSettingsHandle(); + +#endif // GUIDANCESETTINGS_H + +/** + * @} + * @} + */ diff --git a/flight/OpenPilot/UAVObjects/inc/positionactual.h b/flight/OpenPilot/UAVObjects/inc/positionactual.h index ba218f6f3..b3febb0b3 100644 --- a/flight/OpenPilot/UAVObjects/inc/positionactual.h +++ b/flight/OpenPilot/UAVObjects/inc/positionactual.h @@ -41,7 +41,7 @@ #define POSITIONACTUAL_H // Object constants -#define POSITIONACTUAL_OBJID 2253458392U +#define POSITIONACTUAL_OBJID 3881752126U #define POSITIONACTUAL_NAME "PositionActual" #define POSITIONACTUAL_METANAME "PositionActualMeta" #define POSITIONACTUAL_ISSINGLEINST 1 @@ -78,6 +78,8 @@ typedef struct { float GeoidSeparation; float Heading; float Groundspeed; + float Airspeed; + float Climbrate; int8_t Satellites; float PDOP; float HDOP; @@ -97,6 +99,8 @@ typedef enum { POSITIONACTUAL_STATUS_NOGPS=0, POSITIONACTUAL_STATUS_NOFIX=1, POS // Field GeoidSeparation information // Field Heading information // Field Groundspeed information +// Field Airspeed information +// Field Climbrate information // Field Satellites information // Field PDOP information // Field HDOP information diff --git a/flight/OpenPilot/UAVObjects/inc/positiondesired.h b/flight/OpenPilot/UAVObjects/inc/positiondesired.h new file mode 100644 index 000000000..a8259bf6a --- /dev/null +++ b/flight/OpenPilot/UAVObjects/inc/positiondesired.h @@ -0,0 +1,97 @@ +/** + ****************************************************************************** + * @addtogroup UAVObjects OpenPilot UAVObjects + * @{ + * @addtogroup PositionDesired PositionDesired + * @brief The desired position that @ref GuidanceModule will try and achieve if FlightMode is Auto. Comes from @ref PathPlannerModule. + * + * Autogenerated files and functions for PositionDesired Object + + * @{ + * + * @file positiondesired.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Implementation of the PositionDesired object. This file has been + * automatically generated by the UAVObjectGenerator. + * + * @note Object definition file: positiondesired.xml. + * This is an automatically generated file. + * DO NOT modify manually. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef POSITIONDESIRED_H +#define POSITIONDESIRED_H + +// Object constants +#define POSITIONDESIRED_OBJID 2182398544U +#define POSITIONDESIRED_NAME "PositionDesired" +#define POSITIONDESIRED_METANAME "PositionDesiredMeta" +#define POSITIONDESIRED_ISSINGLEINST 1 +#define POSITIONDESIRED_ISSETTINGS 0 +#define POSITIONDESIRED_NUMBYTES sizeof(PositionDesiredData) + +// Object access macros +/** + * @function PositionDesiredGet(dataOut) + * @brief Populate a PositionDesiredData object + * @param[out] dataOut + */ +#define PositionDesiredGet(dataOut) UAVObjGetData(PositionDesiredHandle(), dataOut) +#define PositionDesiredSet(dataIn) UAVObjSetData(PositionDesiredHandle(), dataIn) +#define PositionDesiredInstGet(instId, dataOut) UAVObjGetInstanceData(PositionDesiredHandle(), instId, dataOut) +#define PositionDesiredInstSet(instId, dataIn) UAVObjSetInstanceData(PositionDesiredHandle(), instId, dataIn) +#define PositionDesiredConnectQueue(queue) UAVObjConnectQueue(PositionDesiredHandle(), queue, EV_MASK_ALL_UPDATES) +#define PositionDesiredConnectCallback(cb) UAVObjConnectCallback(PositionDesiredHandle(), cb, EV_MASK_ALL_UPDATES) +#define PositionDesiredCreateInstance() UAVObjCreateInstance(PositionDesiredHandle()) +#define PositionDesiredRequestUpdate() UAVObjRequestUpdate(PositionDesiredHandle()) +#define PositionDesiredRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(PositionDesiredHandle(), instId) +#define PositionDesiredUpdated() UAVObjUpdated(PositionDesiredHandle()) +#define PositionDesiredInstUpdated(instId) UAVObjUpdated(PositionDesiredHandle(), instId) +#define PositionDesiredGetMetadata(dataOut) UAVObjGetMetadata(PositionDesiredHandle(), dataOut) +#define PositionDesiredSetMetadata(dataIn) UAVObjSetMetadata(PositionDesiredHandle(), dataIn) +#define PositionDesiredReadOnly(dataIn) UAVObjReadOnly(PositionDesiredHandle()) + +// Object data +typedef struct { + float NED[3]; + float Heading; + float Groundspeed; + +} __attribute__((packed)) PositionDesiredData; + +// Field information +// Field NED information +/* Number of elements for field NED */ +#define POSITIONDESIRED_NED_NUMELEM 3 +// Field Heading information +// Field Groundspeed information + + +// Generic interface functions +int32_t PositionDesiredInitialize(); +UAVObjHandle PositionDesiredHandle(); + +#endif // POSITIONDESIRED_H + +/** + * @} + * @} + */ diff --git a/flight/OpenPilot/UAVObjects/inc/systemalarms.h b/flight/OpenPilot/UAVObjects/inc/systemalarms.h index 6acc183c5..0324cd93f 100644 --- a/flight/OpenPilot/UAVObjects/inc/systemalarms.h +++ b/flight/OpenPilot/UAVObjects/inc/systemalarms.h @@ -71,7 +71,7 @@ // Object data typedef struct { - uint8_t Alarm[10]; + uint8_t Alarm[11]; } __attribute__((packed)) SystemAlarmsData; @@ -80,9 +80,9 @@ typedef struct { /* Enumeration options for field Alarm */ typedef enum { SYSTEMALARMS_ALARM_OK=0, SYSTEMALARMS_ALARM_WARNING=1, SYSTEMALARMS_ALARM_ERROR=2, SYSTEMALARMS_ALARM_CRITICAL=3 } SystemAlarmsAlarmOptions; /* Array element names for field Alarm */ -typedef enum { SYSTEMALARMS_ALARM_OUTOFMEMORY=0, SYSTEMALARMS_ALARM_STACKOVERFLOW=1, SYSTEMALARMS_ALARM_CPUOVERLOAD=2, SYSTEMALARMS_ALARM_EVENTSYSTEM=3, SYSTEMALARMS_ALARM_SDCARD=4, SYSTEMALARMS_ALARM_TELEMETRY=5, SYSTEMALARMS_ALARM_MANUALCONTROL=6, SYSTEMALARMS_ALARM_ACTUATOR=7, SYSTEMALARMS_ALARM_STABILIZATION=8, SYSTEMALARMS_ALARM_AHRSCOMMS=9 } SystemAlarmsAlarmElem; +typedef enum { SYSTEMALARMS_ALARM_OUTOFMEMORY=0, SYSTEMALARMS_ALARM_STACKOVERFLOW=1, SYSTEMALARMS_ALARM_CPUOVERLOAD=2, SYSTEMALARMS_ALARM_EVENTSYSTEM=3, SYSTEMALARMS_ALARM_SDCARD=4, SYSTEMALARMS_ALARM_TELEMETRY=5, SYSTEMALARMS_ALARM_MANUALCONTROL=6, SYSTEMALARMS_ALARM_ACTUATOR=7, SYSTEMALARMS_ALARM_STABILIZATION=8, SYSTEMALARMS_ALARM_GUIDANCE=9, SYSTEMALARMS_ALARM_AHRSCOMMS=10 } SystemAlarmsAlarmElem; /* Number of elements for field Alarm */ -#define SYSTEMALARMS_ALARM_NUMELEM 10 +#define SYSTEMALARMS_ALARM_NUMELEM 11 // Generic interface functions diff --git a/flight/OpenPilot/UAVObjects/positiondesired.c b/flight/OpenPilot/UAVObjects/positiondesired.c new file mode 100644 index 000000000..64533698d --- /dev/null +++ b/flight/OpenPilot/UAVObjects/positiondesired.c @@ -0,0 +1,111 @@ +/** + ****************************************************************************** + * @addtogroup UAVObjects OpenPilot UAVObjects + * @{ + * @addtogroup PositionDesired PositionDesired + * @brief The desired position that @ref GuidanceModule will try and achieve if FlightMode is Auto. Comes from @ref PathPlannerModule. + * + * Autogenerated files and functions for PositionDesired Object + * @{ + * + * @file positiondesired.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Implementation of the PositionDesired object. This file has been + * automatically generated by the UAVObjectGenerator. + * + * @note Object definition file: positiondesired.xml. + * This is an automatically generated file. + * DO NOT modify manually. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "openpilot.h" +#include "positiondesired.h" + +// Private variables +static UAVObjHandle handle; + +// Private functions +static void setDefaults(UAVObjHandle obj, uint16_t instId); + +/** + * Initialize object. + * \return 0 Success + * \return -1 Failure + */ +int32_t PositionDesiredInitialize() +{ + // Register object with the object manager + handle = UAVObjRegister(POSITIONDESIRED_OBJID, POSITIONDESIRED_NAME, POSITIONDESIRED_METANAME, 0, + POSITIONDESIRED_ISSINGLEINST, POSITIONDESIRED_ISSETTINGS, POSITIONDESIRED_NUMBYTES, &setDefaults); + + // Done + if (handle != 0) + { + return 0; + } + else + { + return -1; + } +} + +/** + * Initialize object fields and metadata with the default values. + * If a default value is not specified the object fields + * will be initialized to zero. + */ +static void setDefaults(UAVObjHandle obj, uint16_t instId) +{ + PositionDesiredData data; + UAVObjMetadata metadata; + + // Initialize object fields to their default values + UAVObjGetInstanceData(obj, instId, &data); + memset(&data, 0, sizeof(PositionDesiredData)); + + UAVObjSetInstanceData(obj, instId, &data); + + // Initialize object metadata to their default values + metadata.access = ACCESS_READWRITE; + metadata.gcsAccess = ACCESS_READWRITE; + metadata.telemetryAcked = 0; + metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC; + metadata.telemetryUpdatePeriod = 1000; + metadata.gcsTelemetryAcked = 0; + metadata.gcsTelemetryUpdateMode = UPDATEMODE_MANUAL; + metadata.gcsTelemetryUpdatePeriod = 0; + metadata.loggingUpdateMode = UPDATEMODE_NEVER; + metadata.loggingUpdatePeriod = 0; + UAVObjSetMetadata(obj, &metadata); +} + +/** + * Get object handle + */ +UAVObjHandle PositionDesiredHandle() +{ + return handle; +} + +/** + * @} + */ + diff --git a/flight/OpenPilot/UAVObjects/uavobjectsinit.c b/flight/OpenPilot/UAVObjects/uavobjectsinit.c index 477cf91b5..7a8f47204 100644 --- a/flight/OpenPilot/UAVObjects/uavobjectsinit.c +++ b/flight/OpenPilot/UAVObjects/uavobjectsinit.c @@ -49,6 +49,7 @@ #include "gpsposition.h" #include "gpssatellites.h" #include "gpstime.h" +#include "guidancesettings.h" #include "homelocation.h" #include "manualcontrolcommand.h" #include "manualcontrolsettings.h" @@ -58,6 +59,7 @@ #include "navigationsettings.h" #include "objectpersistence.h" #include "positionactual.h" +#include "positiondesired.h" #include "stabilizationsettings.h" #include "systemalarms.h" #include "systemsettings.h" @@ -94,6 +96,7 @@ void UAVObjectsInitializeAll() GPSPositionInitialize(); GPSSatellitesInitialize(); GPSTimeInitialize(); + GuidanceSettingsInitialize(); HomeLocationInitialize(); ManualControlCommandInitialize(); ManualControlSettingsInitialize(); @@ -103,6 +106,7 @@ void UAVObjectsInitializeAll() NavigationSettingsInitialize(); ObjectPersistenceInitialize(); PositionActualInitialize(); + PositionDesiredInitialize(); StabilizationSettingsInitialize(); SystemAlarmsInitialize(); SystemSettingsInitialize(); diff --git a/ground/src/plugins/uavobjects/guidancesettings.cpp b/ground/src/plugins/uavobjects/guidancesettings.cpp new file mode 100644 index 000000000..4d7494ff1 --- /dev/null +++ b/ground/src/plugins/uavobjects/guidancesettings.cpp @@ -0,0 +1,209 @@ +/** + ****************************************************************************** + * + * @file guidancesettings.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @see The GNU Public License (GPL) Version 3 + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup UAVObjectsPlugin UAVObjects Plugin + * @{ + * + * @note Object definition file: guidancesettings.xml. + * This is an automatically generated file. + * DO NOT modify manually. + * + * @brief The UAVUObjects GCS plugin + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#include "guidancesettings.h" +#include "uavobjectfield.h" + +const QString GuidanceSettings::NAME = QString("GuidanceSettings"); + +/** + * Constructor + */ +GuidanceSettings::GuidanceSettings(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAME) +{ + // Create fields + QList fields; + QStringList UpdatePeriodElemNames; + UpdatePeriodElemNames.append("0"); + fields.append( new UAVObjectField(QString("UpdatePeriod"), QString("ms"), UAVObjectField::UINT16, UpdatePeriodElemNames, QStringList()) ); + QStringList RollMaxElemNames; + RollMaxElemNames.append("0"); + fields.append( new UAVObjectField(QString("RollMax"), QString("degrees"), UAVObjectField::FLOAT32, RollMaxElemNames, QStringList()) ); + QStringList PitchMaxElemNames; + PitchMaxElemNames.append("0"); + fields.append( new UAVObjectField(QString("PitchMax"), QString("degrees"), UAVObjectField::FLOAT32, PitchMaxElemNames, QStringList()) ); + QStringList PitchMinElemNames; + PitchMinElemNames.append("0"); + fields.append( new UAVObjectField(QString("PitchMin"), QString("degrees"), UAVObjectField::FLOAT32, PitchMinElemNames, QStringList()) ); + QStringList PitchRollEpsilonElemNames; + PitchRollEpsilonElemNames.append("0"); + fields.append( new UAVObjectField(QString("PitchRollEpsilon"), QString("degrees"), UAVObjectField::FLOAT32, PitchRollEpsilonElemNames, QStringList()) ); + QStringList ThrottleMaxElemNames; + ThrottleMaxElemNames.append("0"); + fields.append( new UAVObjectField(QString("ThrottleMax"), QString("%"), UAVObjectField::FLOAT32, ThrottleMaxElemNames, QStringList()) ); + QStringList ThrottleMinElemNames; + ThrottleMinElemNames.append("0"); + fields.append( new UAVObjectField(QString("ThrottleMin"), QString("%"), UAVObjectField::FLOAT32, ThrottleMinElemNames, QStringList()) ); + QStringList SpeedMaxElemNames; + SpeedMaxElemNames.append("0"); + fields.append( new UAVObjectField(QString("SpeedMax"), QString("m/s"), UAVObjectField::FLOAT32, SpeedMaxElemNames, QStringList()) ); + QStringList SpeedMinElemNames; + SpeedMinElemNames.append("0"); + fields.append( new UAVObjectField(QString("SpeedMin"), QString("m/s"), UAVObjectField::FLOAT32, SpeedMinElemNames, QStringList()) ); + QStringList SpeedKpElemNames; + SpeedKpElemNames.append("0"); + fields.append( new UAVObjectField(QString("SpeedKp"), QString(""), UAVObjectField::FLOAT32, SpeedKpElemNames, QStringList()) ); + QStringList SpeedKiElemNames; + SpeedKiElemNames.append("0"); + fields.append( new UAVObjectField(QString("SpeedKi"), QString(""), UAVObjectField::FLOAT32, SpeedKiElemNames, QStringList()) ); + QStringList SpeedKdElemNames; + SpeedKdElemNames.append("0"); + fields.append( new UAVObjectField(QString("SpeedKd"), QString(""), UAVObjectField::FLOAT32, SpeedKdElemNames, QStringList()) ); + QStringList EnergyKpElemNames; + EnergyKpElemNames.append("0"); + fields.append( new UAVObjectField(QString("EnergyKp"), QString(""), UAVObjectField::FLOAT32, EnergyKpElemNames, QStringList()) ); + QStringList EnergyKiElemNames; + EnergyKiElemNames.append("0"); + fields.append( new UAVObjectField(QString("EnergyKi"), QString(""), UAVObjectField::FLOAT32, EnergyKiElemNames, QStringList()) ); + QStringList EnergyKdElemNames; + EnergyKdElemNames.append("0"); + fields.append( new UAVObjectField(QString("EnergyKd"), QString(""), UAVObjectField::FLOAT32, EnergyKdElemNames, QStringList()) ); + QStringList LateralKpElemNames; + LateralKpElemNames.append("0"); + fields.append( new UAVObjectField(QString("LateralKp"), QString(""), UAVObjectField::FLOAT32, LateralKpElemNames, QStringList()) ); + QStringList LateralKiElemNames; + LateralKiElemNames.append("0"); + fields.append( new UAVObjectField(QString("LateralKi"), QString(""), UAVObjectField::FLOAT32, LateralKiElemNames, QStringList()) ); + QStringList LateralKdElemNames; + LateralKdElemNames.append("0"); + fields.append( new UAVObjectField(QString("LateralKd"), QString(""), UAVObjectField::FLOAT32, LateralKdElemNames, QStringList()) ); + QStringList CourseKpElemNames; + CourseKpElemNames.append("0"); + fields.append( new UAVObjectField(QString("CourseKp"), QString(""), UAVObjectField::FLOAT32, CourseKpElemNames, QStringList()) ); + QStringList CourseKiElemNames; + CourseKiElemNames.append("0"); + fields.append( new UAVObjectField(QString("CourseKi"), QString(""), UAVObjectField::FLOAT32, CourseKiElemNames, QStringList()) ); + QStringList CourseKdElemNames; + CourseKdElemNames.append("0"); + fields.append( new UAVObjectField(QString("CourseKd"), QString(""), UAVObjectField::FLOAT32, CourseKdElemNames, QStringList()) ); + + // Initialize object + initializeFields(fields, (quint8*)&data, NUMBYTES); + // Set the default field values + setDefaultFieldValues(); +} + +/** + * Get the default metadata for this object + */ +UAVObject::Metadata GuidanceSettings::getDefaultMetadata() +{ + UAVObject::Metadata metadata; + metadata.flightAccess = ACCESS_READWRITE; + metadata.gcsAccess = ACCESS_READWRITE; + metadata.gcsTelemetryAcked = 1; + metadata.gcsTelemetryUpdateMode = UAVObject::UPDATEMODE_ONCHANGE; + metadata.gcsTelemetryUpdatePeriod = 0; + metadata.flightTelemetryAcked = 1; + metadata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_ONCHANGE; + metadata.flightTelemetryUpdatePeriod = 0; + metadata.loggingUpdateMode = UAVObject::UPDATEMODE_NEVER; + metadata.loggingUpdatePeriod = 0; + return metadata; +} + +/** + * Initialize object fields with the default values. + * If a default value is not specified the object fields + * will be initialized to zero. + */ +void GuidanceSettings::setDefaultFieldValues() +{ + data.UpdatePeriod = 10; + data.RollMax = 35; + data.PitchMax = 35; + data.PitchMin = -35; + data.PitchRollEpsilon = 10; + data.ThrottleMax = 1; + data.ThrottleMin = 0; + data.SpeedMax = 100; + data.SpeedMin = 10; + data.SpeedKp = 0.04; + data.SpeedKi = 4e-06; + data.SpeedKd = 0.01; + data.EnergyKp = 0.04; + data.EnergyKi = 4e-06; + data.EnergyKd = 0.01; + data.LateralKp = 0.04; + data.LateralKi = 4e-06; + data.LateralKd = 0.01; + data.CourseKp = 0.04; + data.CourseKi = 4e-06; + data.CourseKd = 0.01; + +} + +/** + * Get the object data fields + */ +GuidanceSettings::DataFields GuidanceSettings::getData() +{ + QMutexLocker locker(mutex); + return data; +} + +/** + * Set the object data fields + */ +void GuidanceSettings::setData(const DataFields& data) +{ + QMutexLocker locker(mutex); + // Get metadata + Metadata mdata = getMetadata(); + // Update object if the access mode permits + if ( mdata.gcsAccess == ACCESS_READWRITE ) + { + this->data = data; + emit objectUpdatedAuto(this); // trigger object updated event + emit objectUpdated(this); + } +} + +/** + * Create a clone of this object, a new instance ID must be specified. + * Do not use this function directly to create new instances, the + * UAVObjectManager should be used instead. + */ +UAVDataObject* GuidanceSettings::clone(quint32 instID) +{ + GuidanceSettings* obj = new GuidanceSettings(); + obj->initialize(instID, this->getMetaObject()); + return obj; +} + +/** + * Static function to retrieve an instance of the object. + */ +GuidanceSettings* GuidanceSettings::GetInstance(UAVObjectManager* objMngr, quint32 instID) +{ + return dynamic_cast(objMngr->getObject(GuidanceSettings::OBJID, instID)); +} diff --git a/ground/src/plugins/uavobjects/guidancesettings.h b/ground/src/plugins/uavobjects/guidancesettings.h new file mode 100644 index 000000000..50ceeda31 --- /dev/null +++ b/ground/src/plugins/uavobjects/guidancesettings.h @@ -0,0 +1,118 @@ +/** + ****************************************************************************** + * + * @file guidancesettings.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @see The GNU Public License (GPL) Version 3 + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup UAVObjectsPlugin UAVObjects Plugin + * @{ + * + * @note Object definition file: guidancesettings.xml. + * This is an automatically generated file. + * DO NOT modify manually. + * + * @brief The UAVUObjects GCS plugin + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef GUIDANCESETTINGS_H +#define GUIDANCESETTINGS_H + +#include "uavdataobject.h" +#include "uavobjectmanager.h" + +class UAVOBJECTS_EXPORT GuidanceSettings: public UAVDataObject +{ + Q_OBJECT + +public: + // Field structure + typedef struct { + quint16 UpdatePeriod; + float RollMax; + float PitchMax; + float PitchMin; + float PitchRollEpsilon; + float ThrottleMax; + float ThrottleMin; + float SpeedMax; + float SpeedMin; + float SpeedKp; + float SpeedKi; + float SpeedKd; + float EnergyKp; + float EnergyKi; + float EnergyKd; + float LateralKp; + float LateralKi; + float LateralKd; + float CourseKp; + float CourseKi; + float CourseKd; + + } __attribute__((packed)) DataFields; + + // Field information + // Field UpdatePeriod information + // Field RollMax information + // Field PitchMax information + // Field PitchMin information + // Field PitchRollEpsilon information + // Field ThrottleMax information + // Field ThrottleMin information + // Field SpeedMax information + // Field SpeedMin information + // Field SpeedKp information + // Field SpeedKi information + // Field SpeedKd information + // Field EnergyKp information + // Field EnergyKi information + // Field EnergyKd information + // Field LateralKp information + // Field LateralKi information + // Field LateralKd information + // Field CourseKp information + // Field CourseKi information + // Field CourseKd information + + + // Constants + static const quint32 OBJID = 2093064904U; + static const QString NAME; + static const bool ISSINGLEINST = 1; + static const bool ISSETTINGS = 1; + static const quint32 NUMBYTES = sizeof(DataFields); + + // Functions + GuidanceSettings(); + + DataFields getData(); + void setData(const DataFields& data); + Metadata getDefaultMetadata(); + UAVDataObject* clone(quint32 instID); + + static GuidanceSettings* GetInstance(UAVObjectManager* objMngr, quint32 instID = 0); + +private: + DataFields data; + + void setDefaultFieldValues(); + +}; + +#endif // GUIDANCESETTINGS_H diff --git a/ground/src/plugins/uavobjects/guidancesettings.py b/ground/src/plugins/uavobjects/guidancesettings.py new file mode 100644 index 000000000..01b9e7364 --- /dev/null +++ b/ground/src/plugins/uavobjects/guidancesettings.py @@ -0,0 +1,286 @@ +## +############################################################################## +# +# @file guidancesettings.py +# @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. +# @brief Implementation of the GuidanceSettings object. This file has been +# automatically generated by the UAVObjectGenerator. +# +# @note Object definition file: guidancesettings.xml. +# This is an automatically generated file. +# DO NOT modify manually. +# +# @see The GNU Public License (GPL) Version 3 +# +#############################################################################/ +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY +# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License +# for more details. +# +# You should have received a copy of the GNU General Public License along +# with this program; if not, write to the Free Software Foundation, Inc., +# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +# + + +import uavobject + +import struct +from collections import namedtuple + +# This is a list of instances of the data fields contained in this object +_fields = [ \ + uavobject.UAVObjectField( + 'UpdatePeriod', + 'H', + 1, + [ + '0', + ], + { + } + ), + uavobject.UAVObjectField( + 'RollMax', + 'f', + 1, + [ + '0', + ], + { + } + ), + uavobject.UAVObjectField( + 'PitchMax', + 'f', + 1, + [ + '0', + ], + { + } + ), + uavobject.UAVObjectField( + 'PitchMin', + 'f', + 1, + [ + '0', + ], + { + } + ), + uavobject.UAVObjectField( + 'PitchRollEpsilon', + 'f', + 1, + [ + '0', + ], + { + } + ), + uavobject.UAVObjectField( + 'ThrottleMax', + 'f', + 1, + [ + '0', + ], + { + } + ), + uavobject.UAVObjectField( + 'ThrottleMin', + 'f', + 1, + [ + '0', + ], + { + } + ), + uavobject.UAVObjectField( + 'SpeedMax', + 'f', + 1, + [ + '0', + ], + { + } + ), + uavobject.UAVObjectField( + 'SpeedMin', + 'f', + 1, + [ + '0', + ], + { + } + ), + uavobject.UAVObjectField( + 'SpeedKp', + 'f', + 1, + [ + '0', + ], + { + } + ), + uavobject.UAVObjectField( + 'SpeedKi', + 'f', + 1, + [ + '0', + ], + { + } + ), + uavobject.UAVObjectField( + 'SpeedKd', + 'f', + 1, + [ + '0', + ], + { + } + ), + uavobject.UAVObjectField( + 'EnergyKp', + 'f', + 1, + [ + '0', + ], + { + } + ), + uavobject.UAVObjectField( + 'EnergyKi', + 'f', + 1, + [ + '0', + ], + { + } + ), + uavobject.UAVObjectField( + 'EnergyKd', + 'f', + 1, + [ + '0', + ], + { + } + ), + uavobject.UAVObjectField( + 'LateralKp', + 'f', + 1, + [ + '0', + ], + { + } + ), + uavobject.UAVObjectField( + 'LateralKi', + 'f', + 1, + [ + '0', + ], + { + } + ), + uavobject.UAVObjectField( + 'LateralKd', + 'f', + 1, + [ + '0', + ], + { + } + ), + uavobject.UAVObjectField( + 'CourseKp', + 'f', + 1, + [ + '0', + ], + { + } + ), + uavobject.UAVObjectField( + 'CourseKi', + 'f', + 1, + [ + '0', + ], + { + } + ), + uavobject.UAVObjectField( + 'CourseKd', + 'f', + 1, + [ + '0', + ], + { + } + ), +] + + +class GuidanceSettings(uavobject.UAVObject): + ## Object constants + OBJID = 2093064904 + NAME = "GuidanceSettings" + METANAME = "GuidanceSettingsMeta" + ISSINGLEINST = 1 + ISSETTINGS = 1 + + def __init__(self): + uavobject.UAVObject.__init__(self, + self.OBJID, + self.NAME, + self.METANAME, + 0, + self.ISSINGLEINST) + + for f in _fields: + self.add_field(f) + + def __str__(self): + s = ("0x%08X (%10u) %-30s %3u bytes format '%s'\n" + % (self.OBJID, self.OBJID, self.NAME, self.get_struct().size, self.get_struct().format)) + for f in self.get_tuple()._fields: + s += ("\t%s\n" % f) + return (s) + +def main(): + # Instantiate the object and dump out some interesting info + x = GuidanceSettings() + print (x) + +if __name__ == "__main__": + #import pdb ; pdb.run('main()') + main() diff --git a/ground/src/plugins/uavobjects/positionactual.cpp b/ground/src/plugins/uavobjects/positionactual.cpp index ec04580af..4ebd9fde6 100644 --- a/ground/src/plugins/uavobjects/positionactual.cpp +++ b/ground/src/plugins/uavobjects/positionactual.cpp @@ -68,6 +68,12 @@ PositionActual::PositionActual(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, QStringList GroundspeedElemNames; GroundspeedElemNames.append("0"); fields.append( new UAVObjectField(QString("Groundspeed"), QString("m/s"), UAVObjectField::FLOAT32, GroundspeedElemNames, QStringList()) ); + QStringList AirspeedElemNames; + AirspeedElemNames.append("0"); + fields.append( new UAVObjectField(QString("Airspeed"), QString("m/s"), UAVObjectField::FLOAT32, AirspeedElemNames, QStringList()) ); + QStringList ClimbrateElemNames; + ClimbrateElemNames.append("0"); + fields.append( new UAVObjectField(QString("Climbrate"), QString("m/s"), UAVObjectField::FLOAT32, ClimbrateElemNames, QStringList()) ); QStringList SatellitesElemNames; SatellitesElemNames.append("0"); fields.append( new UAVObjectField(QString("Satellites"), QString(""), UAVObjectField::INT8, SatellitesElemNames, QStringList()) ); diff --git a/ground/src/plugins/uavobjects/positionactual.h b/ground/src/plugins/uavobjects/positionactual.h index 5edeaf221..c30ee81e7 100644 --- a/ground/src/plugins/uavobjects/positionactual.h +++ b/ground/src/plugins/uavobjects/positionactual.h @@ -50,6 +50,8 @@ public: float GeoidSeparation; float Heading; float Groundspeed; + float Airspeed; + float Climbrate; qint8 Satellites; float PDOP; float HDOP; @@ -69,6 +71,8 @@ public: // Field GeoidSeparation information // Field Heading information // Field Groundspeed information + // Field Airspeed information + // Field Climbrate information // Field Satellites information // Field PDOP information // Field HDOP information @@ -82,7 +86,7 @@ public: // Constants - static const quint32 OBJID = 2253458392U; + static const quint32 OBJID = 3881752126U; static const QString NAME; static const bool ISSINGLEINST = 1; static const bool ISSETTINGS = 0; diff --git a/ground/src/plugins/uavobjects/positionactual.py b/ground/src/plugins/uavobjects/positionactual.py index 581e6be7b..d1cd76465 100644 --- a/ground/src/plugins/uavobjects/positionactual.py +++ b/ground/src/plugins/uavobjects/positionactual.py @@ -111,6 +111,26 @@ _fields = [ \ { } ), + uavobject.UAVObjectField( + 'Airspeed', + 'f', + 1, + [ + '0', + ], + { + } + ), + uavobject.UAVObjectField( + 'Climbrate', + 'f', + 1, + [ + '0', + ], + { + } + ), uavobject.UAVObjectField( 'Satellites', 'b', @@ -180,7 +200,7 @@ _fields = [ \ class PositionActual(uavobject.UAVObject): ## Object constants - OBJID = 2253458392 + OBJID = 3881752126 NAME = "PositionActual" METANAME = "PositionActualMeta" ISSINGLEINST = 1 diff --git a/ground/src/plugins/uavobjects/positiondesired.cpp b/ground/src/plugins/uavobjects/positiondesired.cpp new file mode 100644 index 000000000..e45bafdec --- /dev/null +++ b/ground/src/plugins/uavobjects/positiondesired.cpp @@ -0,0 +1,136 @@ +/** + ****************************************************************************** + * + * @file positiondesired.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @see The GNU Public License (GPL) Version 3 + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup UAVObjectsPlugin UAVObjects Plugin + * @{ + * + * @note Object definition file: positiondesired.xml. + * This is an automatically generated file. + * DO NOT modify manually. + * + * @brief The UAVUObjects GCS plugin + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#include "positiondesired.h" +#include "uavobjectfield.h" + +const QString PositionDesired::NAME = QString("PositionDesired"); + +/** + * Constructor + */ +PositionDesired::PositionDesired(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAME) +{ + // Create fields + QList fields; + QStringList NEDElemNames; + NEDElemNames.append("0"); + NEDElemNames.append("1"); + NEDElemNames.append("2"); + fields.append( new UAVObjectField(QString("NED"), QString("m"), UAVObjectField::FLOAT32, NEDElemNames, QStringList()) ); + QStringList HeadingElemNames; + HeadingElemNames.append("0"); + fields.append( new UAVObjectField(QString("Heading"), QString("degrees"), UAVObjectField::FLOAT32, HeadingElemNames, QStringList()) ); + QStringList GroundspeedElemNames; + GroundspeedElemNames.append("0"); + fields.append( new UAVObjectField(QString("Groundspeed"), QString("m/s"), UAVObjectField::FLOAT32, GroundspeedElemNames, QStringList()) ); + + // Initialize object + initializeFields(fields, (quint8*)&data, NUMBYTES); + // Set the default field values + setDefaultFieldValues(); +} + +/** + * Get the default metadata for this object + */ +UAVObject::Metadata PositionDesired::getDefaultMetadata() +{ + UAVObject::Metadata metadata; + metadata.flightAccess = ACCESS_READWRITE; + metadata.gcsAccess = ACCESS_READWRITE; + metadata.gcsTelemetryAcked = 0; + metadata.gcsTelemetryUpdateMode = UAVObject::UPDATEMODE_MANUAL; + metadata.gcsTelemetryUpdatePeriod = 0; + metadata.flightTelemetryAcked = 0; + metadata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC; + metadata.flightTelemetryUpdatePeriod = 1000; + metadata.loggingUpdateMode = UAVObject::UPDATEMODE_NEVER; + metadata.loggingUpdatePeriod = 0; + return metadata; +} + +/** + * Initialize object fields with the default values. + * If a default value is not specified the object fields + * will be initialized to zero. + */ +void PositionDesired::setDefaultFieldValues() +{ + +} + +/** + * Get the object data fields + */ +PositionDesired::DataFields PositionDesired::getData() +{ + QMutexLocker locker(mutex); + return data; +} + +/** + * Set the object data fields + */ +void PositionDesired::setData(const DataFields& data) +{ + QMutexLocker locker(mutex); + // Get metadata + Metadata mdata = getMetadata(); + // Update object if the access mode permits + if ( mdata.gcsAccess == ACCESS_READWRITE ) + { + this->data = data; + emit objectUpdatedAuto(this); // trigger object updated event + emit objectUpdated(this); + } +} + +/** + * Create a clone of this object, a new instance ID must be specified. + * Do not use this function directly to create new instances, the + * UAVObjectManager should be used instead. + */ +UAVDataObject* PositionDesired::clone(quint32 instID) +{ + PositionDesired* obj = new PositionDesired(); + obj->initialize(instID, this->getMetaObject()); + return obj; +} + +/** + * Static function to retrieve an instance of the object. + */ +PositionDesired* PositionDesired::GetInstance(UAVObjectManager* objMngr, quint32 instID) +{ + return dynamic_cast(objMngr->getObject(PositionDesired::OBJID, instID)); +} diff --git a/ground/src/plugins/uavobjects/positiondesired.h b/ground/src/plugins/uavobjects/positiondesired.h new file mode 100644 index 000000000..62ec1683c --- /dev/null +++ b/ground/src/plugins/uavobjects/positiondesired.h @@ -0,0 +1,84 @@ +/** + ****************************************************************************** + * + * @file positiondesired.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @see The GNU Public License (GPL) Version 3 + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup UAVObjectsPlugin UAVObjects Plugin + * @{ + * + * @note Object definition file: positiondesired.xml. + * This is an automatically generated file. + * DO NOT modify manually. + * + * @brief The UAVUObjects GCS plugin + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef POSITIONDESIRED_H +#define POSITIONDESIRED_H + +#include "uavdataobject.h" +#include "uavobjectmanager.h" + +class UAVOBJECTS_EXPORT PositionDesired: public UAVDataObject +{ + Q_OBJECT + +public: + // Field structure + typedef struct { + float NED[3]; + float Heading; + float Groundspeed; + + } __attribute__((packed)) DataFields; + + // Field information + // Field NED information + /* Number of elements for field NED */ + static const quint32 NED_NUMELEM = 3; + // Field Heading information + // Field Groundspeed information + + + // Constants + static const quint32 OBJID = 2182398544U; + static const QString NAME; + static const bool ISSINGLEINST = 1; + static const bool ISSETTINGS = 0; + static const quint32 NUMBYTES = sizeof(DataFields); + + // Functions + PositionDesired(); + + DataFields getData(); + void setData(const DataFields& data); + Metadata getDefaultMetadata(); + UAVDataObject* clone(quint32 instID); + + static PositionDesired* GetInstance(UAVObjectManager* objMngr, quint32 instID = 0); + +private: + DataFields data; + + void setDefaultFieldValues(); + +}; + +#endif // POSITIONDESIRED_H diff --git a/ground/src/plugins/uavobjects/positiondesired.py b/ground/src/plugins/uavobjects/positiondesired.py new file mode 100644 index 000000000..d49ed374e --- /dev/null +++ b/ground/src/plugins/uavobjects/positiondesired.py @@ -0,0 +1,108 @@ +## +############################################################################## +# +# @file positiondesired.py +# @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. +# @brief Implementation of the PositionDesired object. This file has been +# automatically generated by the UAVObjectGenerator. +# +# @note Object definition file: positiondesired.xml. +# This is an automatically generated file. +# DO NOT modify manually. +# +# @see The GNU Public License (GPL) Version 3 +# +#############################################################################/ +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY +# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License +# for more details. +# +# You should have received a copy of the GNU General Public License along +# with this program; if not, write to the Free Software Foundation, Inc., +# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +# + + +import uavobject + +import struct +from collections import namedtuple + +# This is a list of instances of the data fields contained in this object +_fields = [ \ + uavobject.UAVObjectField( + 'NED', + 'f', + 3, + [ + '0', + '1', + '2', + ], + { + } + ), + uavobject.UAVObjectField( + 'Heading', + 'f', + 1, + [ + '0', + ], + { + } + ), + uavobject.UAVObjectField( + 'Groundspeed', + 'f', + 1, + [ + '0', + ], + { + } + ), +] + + +class PositionDesired(uavobject.UAVObject): + ## Object constants + OBJID = 2182398544 + NAME = "PositionDesired" + METANAME = "PositionDesiredMeta" + ISSINGLEINST = 1 + ISSETTINGS = 0 + + def __init__(self): + uavobject.UAVObject.__init__(self, + self.OBJID, + self.NAME, + self.METANAME, + 0, + self.ISSINGLEINST) + + for f in _fields: + self.add_field(f) + + def __str__(self): + s = ("0x%08X (%10u) %-30s %3u bytes format '%s'\n" + % (self.OBJID, self.OBJID, self.NAME, self.get_struct().size, self.get_struct().format)) + for f in self.get_tuple()._fields: + s += ("\t%s\n" % f) + return (s) + +def main(): + # Instantiate the object and dump out some interesting info + x = PositionDesired() + print (x) + +if __name__ == "__main__": + #import pdb ; pdb.run('main()') + main() diff --git a/ground/src/plugins/uavobjects/systemalarms.cpp b/ground/src/plugins/uavobjects/systemalarms.cpp index d3c514df2..4c5cbd95a 100644 --- a/ground/src/plugins/uavobjects/systemalarms.cpp +++ b/ground/src/plugins/uavobjects/systemalarms.cpp @@ -52,6 +52,7 @@ SystemAlarms::SystemAlarms(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAM AlarmElemNames.append("ManualControl"); AlarmElemNames.append("Actuator"); AlarmElemNames.append("Stabilization"); + AlarmElemNames.append("Guidance"); AlarmElemNames.append("AHRSComms"); QStringList AlarmEnumOptions; AlarmEnumOptions.append("OK"); diff --git a/ground/src/plugins/uavobjects/systemalarms.h b/ground/src/plugins/uavobjects/systemalarms.h index f2d38974e..527fcf8b1 100644 --- a/ground/src/plugins/uavobjects/systemalarms.h +++ b/ground/src/plugins/uavobjects/systemalarms.h @@ -43,7 +43,7 @@ class UAVOBJECTS_EXPORT SystemAlarms: public UAVDataObject public: // Field structure typedef struct { - quint8 Alarm[10]; + quint8 Alarm[11]; } __attribute__((packed)) DataFields; @@ -52,9 +52,9 @@ public: /* Enumeration options for field Alarm */ typedef enum { ALARM_OK=0, ALARM_WARNING=1, ALARM_ERROR=2, ALARM_CRITICAL=3 } AlarmOptions; /* Array element names for field Alarm */ - typedef enum { ALARM_OUTOFMEMORY=0, ALARM_STACKOVERFLOW=1, ALARM_CPUOVERLOAD=2, ALARM_EVENTSYSTEM=3, ALARM_SDCARD=4, ALARM_TELEMETRY=5, ALARM_MANUALCONTROL=6, ALARM_ACTUATOR=7, ALARM_STABILIZATION=8, ALARM_AHRSCOMMS=9 } AlarmElem; + typedef enum { ALARM_OUTOFMEMORY=0, ALARM_STACKOVERFLOW=1, ALARM_CPUOVERLOAD=2, ALARM_EVENTSYSTEM=3, ALARM_SDCARD=4, ALARM_TELEMETRY=5, ALARM_MANUALCONTROL=6, ALARM_ACTUATOR=7, ALARM_STABILIZATION=8, ALARM_GUIDANCE=9, ALARM_AHRSCOMMS=10 } AlarmElem; /* Number of elements for field Alarm */ - static const quint32 ALARM_NUMELEM = 10; + static const quint32 ALARM_NUMELEM = 11; // Constants diff --git a/ground/src/plugins/uavobjects/systemalarms.py b/ground/src/plugins/uavobjects/systemalarms.py index 854802913..0d944722b 100644 --- a/ground/src/plugins/uavobjects/systemalarms.py +++ b/ground/src/plugins/uavobjects/systemalarms.py @@ -40,7 +40,7 @@ _fields = [ \ uavobject.UAVObjectField( 'Alarm', 'b', - 10, + 11, [ 'OutOfMemory', 'StackOverflow', @@ -51,6 +51,7 @@ _fields = [ \ 'ManualControl', 'Actuator', 'Stabilization', + 'Guidance', 'AHRSComms', ], { diff --git a/ground/src/plugins/uavobjects/uavobjects.pro b/ground/src/plugins/uavobjects/uavobjects.pro index ce35dc1b1..21a950489 100644 --- a/ground/src/plugins/uavobjects/uavobjects.pro +++ b/ground/src/plugins/uavobjects/uavobjects.pro @@ -40,6 +40,8 @@ HEADERS += uavobjects_global.h \ gpsposition.h \ gpstime.h \ gpssatellites.h \ + guidancesettings.h \ + positiondesired.h \ positionactual.h \ flightbatterystate.h \ homelocation.h \ @@ -84,6 +86,8 @@ SOURCES += uavobject.cpp \ gpsposition.cpp \ gpstime.cpp \ gpssatellites.cpp \ + guidancesettings.cpp \ + positiondesired.cpp \ positionactual.cpp \ flightbatterystate.cpp \ homelocation.cpp \ diff --git a/ground/src/plugins/uavobjects/uavobjectsinit.cpp b/ground/src/plugins/uavobjects/uavobjectsinit.cpp index c264562ff..d41397879 100644 --- a/ground/src/plugins/uavobjects/uavobjectsinit.cpp +++ b/ground/src/plugins/uavobjects/uavobjectsinit.cpp @@ -51,6 +51,7 @@ #include "gpsposition.h" #include "gpssatellites.h" #include "gpstime.h" +#include "guidancesettings.h" #include "homelocation.h" #include "manualcontrolcommand.h" #include "manualcontrolsettings.h" @@ -60,6 +61,7 @@ #include "navigationsettings.h" #include "objectpersistence.h" #include "positionactual.h" +#include "positiondesired.h" #include "stabilizationsettings.h" #include "systemalarms.h" #include "systemsettings.h" @@ -96,6 +98,7 @@ void UAVObjectsInitialize(UAVObjectManager* objMngr) objMngr->registerObject( new GPSPosition() ); objMngr->registerObject( new GPSSatellites() ); objMngr->registerObject( new GPSTime() ); + objMngr->registerObject( new GuidanceSettings() ); objMngr->registerObject( new HomeLocation() ); objMngr->registerObject( new ManualControlCommand() ); objMngr->registerObject( new ManualControlSettings() ); @@ -105,6 +108,7 @@ void UAVObjectsInitialize(UAVObjectManager* objMngr) objMngr->registerObject( new NavigationSettings() ); objMngr->registerObject( new ObjectPersistence() ); objMngr->registerObject( new PositionActual() ); + objMngr->registerObject( new PositionDesired() ); objMngr->registerObject( new StabilizationSettings() ); objMngr->registerObject( new SystemAlarms() ); objMngr->registerObject( new SystemSettings() ); diff --git a/ground/src/shared/uavobjectdefinition/guidancesettings.xml b/ground/src/shared/uavobjectdefinition/guidancesettings.xml new file mode 100644 index 000000000..45b8419a9 --- /dev/null +++ b/ground/src/shared/uavobjectdefinition/guidancesettings.xml @@ -0,0 +1,30 @@ + + + PID settings used by the Guidance module to combine the @ref PositionActual and @ref PositionDesired to compute @ref AttitudeDesired + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ground/src/shared/uavobjectdefinition/positionactual.xml b/ground/src/shared/uavobjectdefinition/positionactual.xml index d6798bf47..4ee4adf07 100644 --- a/ground/src/shared/uavobjectdefinition/positionactual.xml +++ b/ground/src/shared/uavobjectdefinition/positionactual.xml @@ -8,6 +8,8 @@ + + diff --git a/ground/src/shared/uavobjectdefinition/positiondesired.xml b/ground/src/shared/uavobjectdefinition/positiondesired.xml new file mode 100644 index 000000000..e609989be --- /dev/null +++ b/ground/src/shared/uavobjectdefinition/positiondesired.xml @@ -0,0 +1,12 @@ + + + The desired position that @ref GuidanceModule will try and achieve if FlightMode is Auto. Comes from @ref PathPlannerModule. + + + + + + + + + diff --git a/ground/src/shared/uavobjectdefinition/systemalarms.xml b/ground/src/shared/uavobjectdefinition/systemalarms.xml index c280e79ba..1f9b49634 100644 --- a/ground/src/shared/uavobjectdefinition/systemalarms.xml +++ b/ground/src/shared/uavobjectdefinition/systemalarms.xml @@ -2,7 +2,7 @@ Alarms from OpenPilot to indicate failure conditions or warnings. Set by various modules. + elementnames="OutOfMemory,StackOverflow,CPUOverload,EventSystem,SDCard,Telemetry,ManualControl,Actuator,Stabilization,Guidance,AHRSComms"/>