From 74439b4f17d3d3c0915804fc819bed9134a44dd8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 23 Feb 2013 09:30:50 +1100 Subject: [PATCH] Tools: removed unused/unmaintained ArduTracker this has not compiled for a long time. --- Tools/ArduTracker/.ArduTracker.pde.swo | Bin 40960 -> 0 bytes Tools/ArduTracker/APM_Config.h.reference | 927 ----------- Tools/ArduTracker/APM_Config_mavlink_hil.h | 55 - Tools/ArduTracker/APM_Config_xplane.h | 23 - Tools/ArduTracker/ArduTracker.pde | 879 ----------- Tools/ArduTracker/Attitude.pde | 380 ----- Tools/ArduTracker/GCS.h | 266 ---- Tools/ArduTracker/GCS_Ardupilot.pde | 157 -- Tools/ArduTracker/GCS_DebugTerminal.pde | 1622 -------------------- Tools/ArduTracker/GCS_Mavlink.pde | 610 -------- Tools/ArduTracker/GCS_Standard.pde | 276 ---- Tools/ArduTracker/GCS_Xplane.pde | 113 -- Tools/ArduTracker/HIL.h | 141 -- Tools/ArduTracker/HIL_Mavlink.pde | 154 -- Tools/ArduTracker/HIL_Xplane.pde | 151 -- Tools/ArduTracker/Log.pde | 611 -------- Tools/ArduTracker/Mavlink_Common.h | 219 --- Tools/ArduTracker/Terminal_commands.txt | 118 -- Tools/ArduTracker/climb_rate.pde | 92 -- Tools/ArduTracker/command_description.txt | 76 - Tools/ArduTracker/commands.pde | 239 --- Tools/ArduTracker/commands_process.pde | 466 ------ Tools/ArduTracker/config.h | 609 -------- Tools/ArduTracker/control_modes.pde | 48 - Tools/ArduTracker/createTags | 68 - Tools/ArduTracker/debug.pde | 70 - Tools/ArduTracker/defines.h | 312 ---- Tools/ArduTracker/events.pde | 225 --- Tools/ArduTracker/global_data.h | 58 - Tools/ArduTracker/global_data.pde | 188 --- Tools/ArduTracker/navigation.pde | 241 --- Tools/ArduTracker/param_init.pde | 114 -- Tools/ArduTracker/param_table.c | 230 --- Tools/ArduTracker/param_table.h | 142 -- Tools/ArduTracker/paramgen.awk | 126 -- Tools/ArduTracker/paramgen.in | 257 ---- Tools/ArduTracker/radio.pde | 199 --- Tools/ArduTracker/sensors.pde | 70 - Tools/ArduTracker/setup.pde | 229 --- Tools/ArduTracker/system.pde | 511 ------ Tools/ArduTracker/test.pde | 520 ------- Tools/ArduTracker/updateParams | 5 - 42 files changed, 11797 deletions(-) delete mode 100644 Tools/ArduTracker/.ArduTracker.pde.swo delete mode 100644 Tools/ArduTracker/APM_Config.h.reference delete mode 100644 Tools/ArduTracker/APM_Config_mavlink_hil.h delete mode 100644 Tools/ArduTracker/APM_Config_xplane.h delete mode 100644 Tools/ArduTracker/ArduTracker.pde delete mode 100644 Tools/ArduTracker/Attitude.pde delete mode 100644 Tools/ArduTracker/GCS.h delete mode 100644 Tools/ArduTracker/GCS_Ardupilot.pde delete mode 100644 Tools/ArduTracker/GCS_DebugTerminal.pde delete mode 100644 Tools/ArduTracker/GCS_Mavlink.pde delete mode 100644 Tools/ArduTracker/GCS_Standard.pde delete mode 100644 Tools/ArduTracker/GCS_Xplane.pde delete mode 100644 Tools/ArduTracker/HIL.h delete mode 100644 Tools/ArduTracker/HIL_Mavlink.pde delete mode 100644 Tools/ArduTracker/HIL_Xplane.pde delete mode 100644 Tools/ArduTracker/Log.pde delete mode 100644 Tools/ArduTracker/Mavlink_Common.h delete mode 100644 Tools/ArduTracker/Terminal_commands.txt delete mode 100644 Tools/ArduTracker/climb_rate.pde delete mode 100644 Tools/ArduTracker/command_description.txt delete mode 100644 Tools/ArduTracker/commands.pde delete mode 100644 Tools/ArduTracker/commands_process.pde delete mode 100644 Tools/ArduTracker/config.h delete mode 100644 Tools/ArduTracker/control_modes.pde delete mode 100755 Tools/ArduTracker/createTags delete mode 100644 Tools/ArduTracker/debug.pde delete mode 100644 Tools/ArduTracker/defines.h delete mode 100644 Tools/ArduTracker/events.pde delete mode 100644 Tools/ArduTracker/global_data.h delete mode 100644 Tools/ArduTracker/global_data.pde delete mode 100644 Tools/ArduTracker/navigation.pde delete mode 100644 Tools/ArduTracker/param_init.pde delete mode 100644 Tools/ArduTracker/param_table.c delete mode 100644 Tools/ArduTracker/param_table.h delete mode 100644 Tools/ArduTracker/paramgen.awk delete mode 100644 Tools/ArduTracker/paramgen.in delete mode 100644 Tools/ArduTracker/radio.pde delete mode 100644 Tools/ArduTracker/sensors.pde delete mode 100644 Tools/ArduTracker/setup.pde delete mode 100644 Tools/ArduTracker/system.pde delete mode 100644 Tools/ArduTracker/test.pde delete mode 100755 Tools/ArduTracker/updateParams diff --git a/Tools/ArduTracker/.ArduTracker.pde.swo b/Tools/ArduTracker/.ArduTracker.pde.swo deleted file mode 100644 index 85f431f62962ef7d6c765e0f56de471e3d9896bf..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 40960 zcmeI5d30n~edmjvve|QnB@DxJ8*HhqwbgB#ZnuqEQg_SNhNRx`*fpi9r&7_SDp5@W_wM?;_x|p0ySJ8xj!sTTmkf-g_&g$&`rT)pGkxV>?M&TvJe8{E8(BR* z(`SA6nNqm-|3w83&Q&YVh;#MYV0LYFaFj=j)oku$Tupu-Sj)#}SLSxAZ3=8t;4CQ6 zs4w?l`q0$KuHm8ccbpgf)}xM`MW*c}+Z5QQz%~W8DX>j}Z3=8tV4DKl6xgP~|4$UC zuRS>RYdUmaumgx3 ztoIKDF9xsof8USqSAZeD=>7I*n*!Su*rvcX1-2=$O@VC+Y*S#H0^1bWroc7@wkfbp zfqRbv*?cM`UH^+x!twvh2Jp6rr&4bPZw9{u4ucOqES35b@CNXD5QClI`wvZ}-Ux01 ze*%tx1K>vwNu_=Wz6L%H-VR;~UI=ak*Ml6G0wW*=K6!2`bt`x_*bRPhPAc^tunxw+ z!@>74JbVwl9lQ;^4!i{11a1H;U=~~s`avJ~BF2l4f$P9J7y&<^)87W)0PhAb12M>h z-vSQ?@5E^FHc$a2un+tzhK&z{8^IFDfNQ~1!4+T-JPy1IgUBnvAAskAYrz5V+u#mt z9-jg41}_8aU=~~ghQR~DKVv8P0r(hrCy?FbMz98sfump-*a6bu!Qj3?Ifu ztJO+1I#RC2xk{m2ti}20@Z?lxdUkvws>Stsv0NBPogbHKaVj-BmpL@Ikl}rrPhqXr zGjn8Ws&}7WjE_z0MU>C2?o;~Wa;u#E`y^5-V03&eG%cIY?Mn>~(t&b~-o>kpQnp^H*6yrf3llR7 zv-1ln(v9rRJb~KN zi&weq7c9ipVzx94z0aZuvV{_k9GIFtZo_rgim-g4kcG+lgLkJL$`{I*nVuNkTn4mG zc0Srf4035Ot8unuLsE-N1P2u>Wt&6IsWLsb=hEFH>2$aG;^vBWeRIb#cY&puVtyL>)khva57jw~CHLlef)!5G~bxV_ObbR5G zOLy;PNrZW4O?8v6#)R5xE!Z&AHT5ovYK^tE(ngfe*0a%awXz!3SE#9IbC=EzC{@1} zQnX;xx)!Zg^6@|vEo_uYss{QhqbzOARm#i70FX-}tdygC5q3~7Z6xX(4e5NP)0JAit-gG*mR%~v`MxMyv!>Hh*W?+MTZ)!T z#llKGGvZDs?bhtGfk0SE<%VdffMs-hJt`2tv$}$UKEiPwE^$n3eA1@cn z><(6yMm$@O*5fE&DPL%lZKlE+vZt-CS=wl+vAi5tnR*)IQmK-&(Iorcy-RVSSdQw& z)ws{K=^iGt4WYQGYxTV_j^`K4xsuk?4(9*B$_`e4cyMTJq5VOjR9VWFGFtKNFO0Tv zO%G}D6cj3H-84$Ev6E1sTjD;|m|yR`T0Gd+5As#rcbuyQ$!CZbILAmXJT zNn2aVd~=iIq=&1KobOSoxKzznHxebgZNHgFftqv05}b58E@UGlf@rE+p2_JWTjk+Z z_dL_t`if|TW5D7GkNI$J2m^$LqDu6E5IWDoL3B1uOp zoM6&5r#6YLIy}mHsuZ-I#*2kwopELyPsP>ltr%yf97OEdiWK$^>aSMd6q{*(47N@~ z+D8y>(WP9UiIy0}nL^dlrNH9OvrA`ytie-qux~XbCWMgY^b zO^tGUzU-;$;96m|wwBFx^igl(n4Pbq0eW7n_*zDf&PUZoS)2y}{T}|P8;9d;o|)xW zW$m7?Beiod#l)#;|K2q)EPgEkVyc)!T8R511eqRq>_(+g^*d=V63<4Yy|fznoC7JJ zUYqz@W2scEt+3NB!SN~Up!VX0%5r^OtovZ4QHFI<-@Yg=YQspuql(KLQL!HJic*z{ z(pPJtHHT(c-w08>I~14WD)W6#wd7KZD{MB)#p0<)Y`NGMU5Ol6V_B}mB);;1lg%VYRSzETi$)tT4XALkFFF`6 z$LxrG(VQ`n>6nJb)xKyxUcIikR7{a{1tnx}dc4voM91RdQe2|M3a!4fQ7TsSy_zpZ z(~WZFI@L?_q6MMj}Pnf_0yYOO>_NxLglI*qa*MX;V1r-`O9Ltbe_j zudnQlcJGU(5g7MIV;5btFUs|sP}N^qUPjHcw?#-&uJ@Cr)-Neqdcarc^OwQr!DoSJ@M*C9*`~lY1-2=$O@V)# z6u2Rk=0CvyB+FJSrPAK0e|Y4QOD`Q58XD@0B8)>hwoRU0{FLwviKe2sx)JnM$L6_1NLe{E_kTiTP`y^nN5530rZ7?2?goMm^PRzF5f= z%gKcOk$Z|Tv)a=vptqOwPw7(r+~nfe;kM$f`B7)_J{^Xn_Tue1rEgLE{Or`!HBnpZ zAsDJ;XX{j~^TCI-Zg;NXR0$P+XOLtInuJm2zoA33HPErblOv zj85%!qna2CjWwTUL8A2HVNAP=Qxn%j`=df!@0lB&ADzxD&QJ0`{|Zy9vSGjLS14u2{|S&-!3R7SxcR%q!VbSUT+~8IUSVnD8W2 zyEi3S`D~^by|cNx@*1hfCg;bdCN`6rKCwtDbydr7$&9bR8-*3C_Ri_mqM_BXusC{P za%%GFU8=z(lFKv;>+vIHPeb*3=&kuybvj$D*4E-UpK(r){U}83Px_WmvVs<1=CC3|p@zumfOh2SPN#c%N|kb~-Z`9w}8T6>aMypR&XirCN{`F3mmob_wOEE*4Lj6m&mZck7 z=<_g$HpTLB4f_F;!_y4YF(a)ZuF&IFFZrXlwVp=j7v{z@2M-*}Oi#{;dkqf_1>$bC zAbX>WLUCIaL`j`wFh>?++M@>abj>(=T;!8Iesq#{O-zhuoZCmMdSYhuz|_QeYgzjh z45LBe%ofW%VOjS@{jK7g%epVYMVb$rUESI7));P}Nq^WAjFa?=LG1CY1aqs|=lx*7 zV|v>5i!(5EPsh&M9Ut1++KXUTZPkXZZmQ3Af^_Hh;rP2%#@s5K!5?2nf!$En1%VHhp2L?Ouxu}J_%h{80G?P75EZEA6*nHO- z7$a(0m^C~kCoQS*mV;9#G6zm%j!n)_WDaa*V@=iq4{x6VHbzW=5fc;ie=O?adese8 z14k$nVXD~RVkbjHSyTLrb=#-XfwY?n)_l7=Q;WGWRAObNx1L3<-fSAsGfW`Xne_jw z5c8ge?^yc(jr{W=`TIc){3q~m@G$UebpAWQ{{jC9z6-ttz5w0`t^-HFlfY-N2Ph`s zh2Ta|2Q_dU><0s&A3Os582iB2!Pme$!7D%&j04#Yo(vuV&IJzwU&Vg#W$;Dt1@KYu z5%3(a4?GSicHrmWpTHNv-++&S*Mbdj0z3s=3Z4ue1^xjW!v6yA0q+KX2wnrO2M55{ zu_1g7yal`pyb#<7o(Y}~c7w-($AVvBUr=np`@ze>b)W{S;2`*I@N;Ypw}TIXmw`IS zfhBMS`1jyka1Ka=4`8QxKX?sz1~>u6z!Sm0-XA*#cn5eB_zUoSunuOx0JsqRjM#!t zg1-fy0RIyx4&gaq0qg<0!J~lU555T$gYX{kZlHOR0oQ^vf3(rHc}UOeh!!~0dqopO z7hHhscW7o}advuQabjNleAqa7V+|W+thfa)BWTZStY%7;%Gwxm66^_c7!1Ui!q8s6 z4GwmZxss18O=K2J%Y})tREyTJfx_uxldw|h8h__jdNxudL#7NL`cOFasVmKPso$XDg1RVZX%T~ zvM@1!bT%`4WYMLOvpcUHDWX}fslNU#`Y2C-qK|kEDyz1Sxu6yt9Vg>DAuY-1g5=s6 zpIit64W!dZGJRw*PsBJ-K0}&EfMergW>E#kdhF7bbrdUm)SMeK`=UYB8Bo-*T4r|` zl{=lhNUPMoUM(VKs`EB~RX{X%Y&trXEj5^^wxDFvE|!Yd#lBo=U0%t{$Q&JA?$tX) zg}btRta+A&o-4q@%du{|b-J4ro{j9f=&c5>Vfl|~)!?9KAGt=giYAqDF$~t6v+I46 zHr9X~sv3bEIeIe~pU%ZTpg}5aB055zE{j1*p)f~-gGB`VIzH=`U=!+gVJyYfHAM%Y zb%d0}uAGXN8)X+IqZu+eGcmt7IXab@89h3A$Yys^=5-ZyVM)FTZ6$l@%8i<7TY zR^!LyY!kY+8b>!)Uha)*tCb3BM!68E0!5nPWK0|eX{u7l9McTT%xBlFb{=Z_v;@tm z*+ZEFlZ(@%3s?0<+A9{W%IMkr=rLGgM=2rJ#i^ki`XW;>^wj1~>v02) zIDB!Y-m7=aCjUmu|E${+_)6yB=)z)e>ij6Vy5W>C1&htgBvvWPvZB{a_Z(>__;BZ7MJ(WuuC{i=px1lvhZ zZe^tThvSOvqlDdsj!#Z4j2@hjMsSwP+^wyZn9V^;$@G7$520Gg@A;ys;wfZg$Y2(Y zu!x#V)Uhh-gC$T0M9Iu*_B6tJJ}%X>({QK-LM6-j8pEpAcRq$qg-mDPgH{{5e6#K) z_x~nKMp?&(W40d{JIpIS9p~g2MK#|uboe^Wp0!G?R>W5rqyc3%>Bq|gH`o#v18BZk z#0$Pq&#tb~>s8#L#hP1GW<_(RG$P^R`W%viLK5^ATJdSEM7@(5Sk5_q&dc_(-fBh8 zO+z+55+gQk-g6W42Q%{%bF=fdtGY5*(aon2donr6bDFV3CSzHJq5b{DE>)M|+~tg% z{8fg@tSw4OeQh$5Td?gDyfrSQW?oKsN^86DBA7%kmDQx9quN%Jcyl?;akUn;^-3~o zvS=Wl=9i&?<$cXT$842%c`OTdPCq?blc2wSW>k9z zJIht&ozG+}4vk};wv}7;YbU(#Z-2|!3_s;Ai^=_N{bE0G#!#82&TdI);QVSJq^+LG z63DcsKqi~mCPOpZ8>G=>xrc*8T)<;xt-mB+K2msrXo!2fj+BiqrDyX538uB&yM36e zPmZR?jzqZ)?8OMy1ZA53k7}8^So$3L{|or%lj!<4f#(9n|8IcPU<}9(aDN~hz&`=m z0sbER5AY)JJn&p_9Q1+5gGYi#fZNgaWf%AucnkPT@N#fH$bl0;_JJMXd+7PH349dX z29ANBqSyZfd>XtK=$ya?I1LVgUeE(>N0&bW7C{7lhVK43@JjF;Abnov11VeldFI`AU! zOmG}L4a|XqU;^v|JHSK0x#08Y_iqL|e{cj`1}+AV1m}PsGavp8JR7WnrvlB9JwVJV z-EzQmW#(gZN07lr$NOwr?ra){nS{jnAcGT}#C%mrgM-BTLG3xdbmdqfQ zTZ6h*lh7QA(dW#BB}JjCug7tjHcQdi(^&(KqH6q16d^@5dz+M+X2!~%%8|i9G#e1bVejMg01cUH~W_Cg5(xL+pP`JsS46%0n_$!v{I>@ zWFj=y*f#SO2405~)L_$1_n_J7bU;%?HAi+j?FYrVIGyn+4uex%?g_SG_Xh9f+4d{h z;xX@YU1*qzs5UrF#BHd{mb}pw@mQ|ej5mQv(l?pu45UleG}4dUYBIYoU*?Ebmpf-$ z!h|>6HrK3TC$bI>a>4@7`xX;bEp%a`8M9Vo)(OVz=?O|*w6iywn8-|_mYL!>H+pEI zFOu}@0&)+e()(q6lnwl3;vBYXDrN$F~~ns%dMcm>phN= zJgSS9eje0thX>Og-gxZ`6o0J_ezxf`7NEvN<-tvB7t!bo3?5>77-8P=+m>EGjY*ZK` z7G?czGd^_Yv2B4ZQcMI5!cJhZZJn8L2p4B2+@_vx+B{^6V$_hjB@TG{>EH&`CSbvW z7tAXXb~VwejTh%OObzjpmjjAJEXUvBp5$uHwDPmF>qaTWp$OeBha{$Tb zC1@(RBAkXY{eK6dyCIamc(fpIVf2EgOM1Hl8pchK`c0p0*!5B@Wl0n-0}ioX93;9cNVKz9ec1Y8Mp zU%)PKUm#n+r@;Hc&EWY!c7bPs9Jm%d9h?Bi!3E%7unGJayb>rD;23xs_y{(D_k&x( zAA?m;0-H3C;zw6Z|Q79oPka zh_3&S;Je^EUebfb0V2fk%UX2kv05d>Z^d(7cJkoqseb zQQn(@n9UF>B3pv;A~(!$9;TFbp<~STyBjYwx;b7br+CC<m$v{TyJkE!NT4oq%?)R$?L+AO())5TDpNN*W~QK_Gw*n#%-3K2*X|g|yLk|P4xLKc~ttuW54VbPL^hDxLNpGBK$wrs30x>Pk-AM*^VlU>%H=RU zZ>?xfWIwKL?9L?S%qrm(naOcUax{JuLq*_AGDeyAMvpAclH3PM1p&NEu!o#Ly%syA z?v2eJIzhh(EA3gV%p*P}M%1zEL9E#WL(UFF>f@x&RcdEWP2t%*e)!+eCL%i- zO%alj%Ivmkudzeg!Pm_6!c>&QIE*fjd8-x?1!w=d=TB;NpwSB>sZ9Ev(1ZUXXLmAtyi?Jb#ccH#F`BR$dlatp35t zsYOXM2~J`awPj-Y5d*M4+T(bbjB4ZYQln5R_v|?DJT~||M$+64*C}+(dcAgx`SLJ87i!vfYeX_4UB92FrW__UaWt)=)-f>js ze}-1Mx#`R%n<+#wJk+A{O0FV65rgST#2Rv7$87 z#Ftu~dvi3Oyu*@=DoYESl9ag`>klFY38KV;!c=F!E4#b0m`c~H$+lIoGV2WtOm4Dw zY=1Pj$8UFv6~4jre^l|*PyPLWy7T}0=<(kJ()T|K{s<@@AP*M6WndrJ4MxBt!B;8d zec&D7?H~^pzBO;J3-d?MSdOS= zN}FO*!1tV~A%hb|#unnlnL~0&>!I`_v2Ym-*SQvCu+HgMeTkgHwT@C}o@o=jPGO?8 z+em_wlaTTjBQm$=u!6_^zJUGvlwISNJoRknhEe35kJq1t_D7R+*$kl%MS{f@6ET?i9Z?WQ?b5pgqQ3Kpc?%C3xi4ll=^(*ACE%XgvCmT74N zU4NCG(8AK)nN{)MAP@dy)LciiOo?)jQ%!Glq>j{M zLNxUTU36dDyJ7dvY`Xrv?aN1G-q6T;8qf(Dgj>bB=M>Gc^KM1E_Y|LeufDs9;oa9v zBbGl=3PyazM034iIK+MRrPs)ELLM)A`BMOr$R#>i!kS_qV$Kf|{$N@IXLWUM;WQT$ z)f)aBgiaB#ANJ!5B`^OEIb3XgboIzJdT*z6QJoUqv`7Lwo8N4VB*oAK9Zd+gU$toC zGI315nWv)z&08CB)t76lMf2_VGgER^F8N_C=2?YY(?sttO)nl8*OyDemy4tDZBKd{ zyPaO{PCK4N$T@C*e_Gqm|C)u#j-(fZ{etwn(i46`DEDxB)GZYhFhA=khQr>;5_G(| zOxz)4Ef&f;MZn!uJ0q0ZVx@UJBbDAU#d6zm15(e9MQ7~T(Wj>sqVB8q6$_;Os|%6r zOUM6o1o(hG)3?!%wm5Btz@e^qr@=`oibty*RIwv zeiZ*!vS=sf!__R0cl7Ppms+ZD(V>o^CL~-f-DS>hq0TWK?JT^cJ_*0h45<0&GF)Mc zc$17BY=C>B8u8K!nPetP_BMC3Ej5?lm6|fv(O!z_0W{=d$Am*GvbmKa5mqc$wo0AO zBKSirk4V($Fd85nHFPO3Hy@FqUU`n5uV~sdV~1Qqad(%L=4O{I0L7_lSNQf}c3v<` z@|+LC3EdRy(A8MdR#c4W?UqaA6^EWv*d5+lY z=CWX|BSLV^pHosBLM}IBLpOZOj7_UJcRR$sdnbhVkX`0T1KehKZ`|xQ=zUPD|39}z>!0^I=DSx zbn2{I)t1Z3nVL8>I(FhNbj)Qyf~I5GLT1KC=f|yQJ$f?jP81$yMIwzTT133@`UCE! z!@YZT(omOU_oeOsk^YY|)t+E<=0qRMDHn5IJe*PNb4mMBt)kkhin`S9d7pMvmb#9A zlq0*$1Jyg|xG*(t>#Jg0;1*JO6Nmvrj7A6)mX3{;2=o!CM zeJdm&G4)%QiKk`ObhBK?K{0hUZ%j`Ph*E{Wee#rL_3PB~h z$y~!jgLMtO>Hnz3sfO48^&5ZhK%ZX$1uzc|fMM`NAYK0#==9$Jp91d%?*{Jzw*dM5 z_kv%d+rI}q2Rs`TfPDLp1>Z)me>1oSJP^DI9exrV28!c<5V#Hf{bsNNdO;7k5Ii1) z-vKyH!*$pH?dag|0B;Ac1up?t1L^8tLEru|_!RgsD1jY7{&wBz|1)&!XMqQR7otnw z2u^}|up2xE=)V5%fmeeyPyxrl6+ma=UjeQLhk@?oe+W1iNbmj{cn1)D>%cxJCh2s^ z#H$Q9+-FPsBA6E+rV?Uqw3jg9b>cL*!BZQo$sH~^^76>VLn(Q6qO=^y?CnhHj1Q%; ztvr-sqP14zCab0%I_XZ85Ns;Z(CJda;i`)`W!-Znsz_(%cUo0&C`KZV|?_#AgreQ4v`hPuLQTaM9RP~9yP|MR~>O&^#wrtU+pQwMB0Zhmx5E)f1;i+*# zuJp=GgX`ldu1)OK3odQNS{b8nmZdQupxR91T7Zmea-ocsd8?PM%q`xUB-8973YmVq zp(RiS1BMTo&-jq}-O^L1W{+i#&Q2{7l`Va7U}z77fM5rFc>PfyUe6fj;u5zt3}3YC zqLGVs_d$SQGUaL`-DDscGsbvK@PH{D4+gF zfw#oC*^88)WJCJyatEs*-g1r!+X(}A(Uhx^bl!7|5+UV6juT6Ti7@uOC$OSq#@1%w ztv8AMaGa3Em*97%&H}W}Iol6cE=sRvTf>S&qdImgJ(|+b2q=VdLid2tFL9;BjcI8#oPz6_?^P^c{>&YRIdzuuzgXU^Pj5X#XaOE9(h_tE5dM|v0g z(In0p(^I}Kygt2U4y+uyMjykAO{^}}-L?EqcuHGj1~(AYoBXA1D6xAU)UMOeCW}tu zul8_rTf3JJU1HzO*0ghT7~B4hkc`a1?51sEL2oJeF>k`ULZfF1n(e7jv5|cIURtS~6kzwEe3WO(u3h;ugD>ov-OgrXx+`n7Sl3)M#C@Z@=BZ zZ|wwDV}70~_~AIw2CXHf!`&jBHy<)w<%0h)Anyd)RKSt7yOiD0H0}4MORU)t#6ANs zGMXM5VQoa%(|th3G#wW)ljpL2@&e-9dgKP3D(c84I_d!gO3I5e$y|U}Zq)lOTc&b7 zU5vxuShDq?)79De*mL66V>urEUkL|d^2wZBr^yE{i(GR%snKzuV7m4$($3ay_9V*V zswl{_ak#d6xw)K)nfXH}&Rml2|HEjSdXsch;{T8H&u!@V&jMF~pQFp)4sHc60CV6m z;D4jXe+3)`N5EmQ5Bvaq{trP6@?a7SfM251zZaCiBZ2PYy9t~Iqu{Av5Ig~V7oGmC z;4MIB_MZ!mgPq`~==8UNn}B@vimyKhyb+yVarHNW%fWuo2Obap7CrwH;8j5J^#w2u zM!*w*V(ot)JO}7ry{CbT!47Z^NP`E1`+#3y5BL_)@A+K=6l1U0dd1Vf5*!5ILihh` z@J8?q@Mxgl=DQ6Vi_U)$>;TaEE`HjTEAb3(fp34ok7%vI02VPGG`BzrD=v=L-&Ey} zc$-kre#=UyhG&OvTD)OgcjZJXJ3`2CF*D8qW^3EC>P;tJ+FRIpqlj5RSmm1vF9zqD zc~`m~YlR)-B6aV(9sssiiE~0@?ruQKClQ8v{UgVo_${O|9vW;cCezqiZofCfNOd7W zCDK7uqPp~FtwSSD)FdiZjdawJAX(^@wx~9OxqGiwYDb}Mx2P=m3w%so3?;jVX0cFW z*V1)VnqV5avl7qL$W1iZ#G}pVv9N(TWz~8rIb9yTw7>5mBNiC8YNNRsXKar^ys<#O zMXoI_qD+av2w+8|GpX!8?#|A^S>%Hcht(XjxNsLEDv2)<8RqLn_*65@Antyh?CL!u z*O)niJ=%7bu?3c)iMM!0g0a4)E54dGl4N+CpKFA6DIg%T#7rO((p-HVuWHW>;hy0I zHfT~}bW+H99siEa&2(X-TG7$eP>QLm;i@UW^o(ZH1lQYG*gUkGT+Bw;xfH|hj^R5) zq(`Z~5^Ri`?bH0+96!3(IuVh>a1tqQ1YvJIR)mGrN+)e9)~|K9QXpKLeB=HQE$3QS zL*X>U{8Dj8{G6De*lH~}K5}DX`63HTPiJ7T&Q!fS^+c3CU99;oD8jl)fvs{JcR75P za95I5L#XyH@>9_y*Iy$0nLbz-`w~kuFEG@{kFFmz%a#hXxW2N=PwjLFb9x{fVZvy| zFg9}amS|_{^U-*v$OSuGn2`%B|CD>glJtr!^V`z;y8>ufxtxtu!&(-p0?EidNz~;H zqG?44XNsDgb{|wzSd7SQD@wh>Yk*d6?q=Bpsnb6R1HpvgRGj23v)E(g| z*_J6K`($2K2+Hr~WSx}J+Laks`d^?*Q1>vn=q?x*YG*Fa)0Vc%WrK&03AvpguItQc z^&OjQHq3a(dUnIzXWp6GYaYoV3(80svQ0J6>Qk9S4al*I?cAaU(b_deGqIiWJlJA2 zjf}VE)+zfbJa7EK=fxhsL4XAMh+j|bGPk`K>lejcl#Tm=%dT+GPb-b@UW2171779q-vej?5a|=(1j)?o8VfP9JNlT4B;EGDcWdk zt`9CYj8;`}p7ce#*zx$IzRoYTC5tv9296A=uX8Nj4B&*Co3X;e?nKQ2c}^57vDuPnbmi^%_&7* zZ@?s+%)e>g#v}_r6W@`rxhma;MsXp1YjC$N%2_rx7nUT3x_`{CeU_-r<6xQVV%;0+ z?`gi(aXEg=%}07=a@7XHVK1{PJmLU>tb+`i!mnH?j&o-v41CSR&J4)r!i96eP#O*# zaS2tC-?MAi37U7XnuhIesPc42GHsr`G$unYlg1#o3wUQ97Dg_jbQD%bg*GObL6>|M z{OGbJX}QG=>-Jb=XQ3cH$bjAC4^E2jB3FlNbHfk?A1sSU7lAf8jZpqHzE8R0c-<~mCt>!5f3 bwe}mEalRcekb1V&5K+FZBzJz*N=p4dYB>1Z diff --git a/Tools/ArduTracker/APM_Config.h.reference b/Tools/ArduTracker/APM_Config.h.reference deleted file mode 100644 index de47322a20..0000000000 --- a/Tools/ArduTracker/APM_Config.h.reference +++ /dev/null @@ -1,927 +0,0 @@ -// -// Example and reference ArduPilot Mega configuration file -// ======================================================= -// -// This file contains documentation and examples of configuration options -// supported by the ArduPilot Mega software. -// -// Most of these options are just that - optional. You should create -// the APM_Config.h file and use this file as a reference for options -// that you want to change. Don't copy this file directly; the options -// described here and their default values may change over time. -// -// Each item is marked with a keyword describing when you should set it: -// -// REQUIRED -// You must configure this in your APM_Config.h file. The -// software will not compile if the option is not set. -// -// OPTIONAL -// The option has a sensible default (which will be described -// here), but you may wish to override it. -// -// EXPERIMENTAL -// You should configure this option unless you are prepared -// to deal with potential problems. It may relate to a feature -// still in development, or which is not yet adequately tested. -// -// DEBUG -// The option should only be set if you are debugging the -// software, or if you are gathering information for a bug -// report. -// -// NOTE: -// Many of these settings are considered 'factory defaults', and the -// live value is stored and managed in the ArduPilot Mega EEPROM. -// Use the setup 'factoryreset' command after changing options in -// your APM_Config.h file. -// -// Units -// ----- -// -// Unless indicated otherwise, numeric quantities use the following units: -// -// Measurement | Unit -// ------------+------------------------------------- -// angle | degrees -// distance | metres -// speed | metres per second -// servo angle | microseconds -// voltage | volts -// times | seconds -// throttle | percent -// - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// HARDWARE CONFIGURATION AND CONNECTIONS -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// GPS_PROTOCOL REQUIRED -// -// GPS configuration, must be one of: -// -// GPS_PROTOCOL_NONE No GPS attached -// GPS_PROTOCOL_IMU X-Plane interface or ArduPilot IMU. -// GPS_PROTOCOL_MTK MediaTek-based GPS for Mediatek 1.4 -// GPS_PROTOCOL_MTK19 MediaTek-based GPS for firmware 1.6, 1.7, 1.8, 1.9 -// GPS_PROTOCOL_UBLOX UBLOX GPS -// GPS_PROTOCOL_SIRF SiRF-based GPS in Binary mode. NOT TESTED -// GPS_PROTOCOL_NMEA Standard NMEA GPS. NOT SUPPORTED (yet?) -// GPS_PROTOCOL_AUTO 7 -// -//#define GPS_PROTOCOL GPS_PROTOCOL_UBLOX -// - -////////////////////////////////////////////////////////////////////////////// -// AIRSPEED_SENSOR OPTIONAL -// AIRSPEED_RATIO OPTIONAL -// -// Set AIRSPEED_SENSOR to ENABLED if you have an airspeed sensor attached. -// Adjust AIRSPEED_RATIO in small increments to calibrate the airspeed -// sensor relative to your GPS. The calculation and default value are optimized for speeds around 12 m/s -// -// The default assumes that an airspeed sensor is connected. -// -//#define AIRSPEED_SENSOR ENABLED -//#define AIRSPEED_RATIO 1.9936 -// - -////////////////////////////////////////////////////////////////////////////// -// HIL_PROTOCOL OPTIONAL -// HIL_MODE OPTIONAL -// HIL_PORT OPTIONAL -// -// Configuration for Hardware-in-the-loop simulation. In these modes, -// APM is connected via one or more interfaces to simulation software -// running on another system. -// -// HIL_PROTOCOL_XPLANE Configure for the X-plane HIL interface. -// HIL_PROTOCOL_MAVLINK Configure for HIL over MAVLink. -// -// HIL_MODE_DISABLED Configure for standard flight. -// HIL_MODE_ATTITUDE Simulator provides attitude and position information. -// HIL_MODE_SENSORS Simulator provides raw sensor values. -// -// Note that currently HIL_PROTOCOL_XPLANE requires HIL_MODE_ATTITUDE. -// Note that currently HIL_PROTOCOL_MAVLINK requires HIL_MODE_SENSORS. -// -//#define HIL_MODE HIL_MODE_DISABLED -//#define HIL_PORT 0 -// - -////////////////////////////////////////////////////////////////////////////// -// GCS_PROTOCOL OPTIONAL -// GCS_PORT OPTIONAL -// -// The GCS_PROTOCOL option determines which (if any) ground control station -// protocol will be used. Must be one of: -// -// GCS_PROTOCOL_NONE No GCS output -// GCS_PROTOCOL_STANDARD standard APM protocol -// GCS_PROTOCOL_LEGACY legacy ArduPilot protocol -// GCS_PROTOCOL_DEBUGTERMINAL In-flight debug console (experimental) -// GCS_PROTOCOL_MAVLINK QGroundControl protocol -// -// The GCS_PORT option determines which serial port will be used by the -// GCS protocol. The usual values are 0 for the console/USB port, -// or 3 for the telemetry port on the oilpan. Note that some protocols -// will ignore this value and always use the console port. -// -// The default serial port is the telemetry port for GCS_PROTOCOL_STANDARD -// and GCS_PROTOCOL_LEGACY. For all other protocols, the default serial -// port is the FTDI/console port. GCS_PORT normally should not be set -// in your configuration. -// -//#define GCS_PROTOCOL GCS_PROTOCOL_NONE -//#define GCS_PORT 3 -// - -////////////////////////////////////////////////////////////////////////////// -// Serial port speeds. -// -// SERIAL0_BAUD OPTIONAL -// -// Baudrate for the console port. Default is 38400bps. -// -// SERIAL3_BAUD OPTIONAL -// -// Baudrate for the telemetry port. Default is 115200bps. -// -//#define SERIAL0_BAUD 115200 -//#define SERIAL3_BAUD 57600 -// - -////////////////////////////////////////////////////////////////////////////// -// Battery monitoring OPTIONAL -// -// See the manual for details on selecting divider resistors for battery -// monitoring via the oilpan. -// -// BATTERY_EVENT OPTIONAL -// -// Set BATTERY_EVENT to ENABLED to enable battery monitoring. The default is -// DISABLED. -// -// BATTERY_TYPE OPTIONAL if BATTERY_EVENT is set -// -// Set to 0 for 3s LiPo, 1 for 4s LiPo. The default is 0, selecting a 3s -// battery. -// -// LOW_VOLTAGE OPTIONAL if BATTERY_EVENT is set. -// -// Normally derived from BATTERY_TYPE, the automatic value can be overridden -// here. Value in volts at which ArduPilot Mega should consider the -// battery to be "low". -// -// VOLT_DIV_RATIO OPTIONAL -// -// See the manual for details. The default value corresponds to the resistors -// recommended by the manual. -// -//#define BATTERY_EVENT DISABLED -//#define BATTERY_TYPE 0 -//#define LOW_VOLTAGE 11.4 -//#define VOLT_DIV_RATIO 3.0 -// - -////////////////////////////////////////////////////////////////////////////// -// INPUT_VOLTAGE OPTIONAL -// -// In order to have accurate pressure and battery voltage readings, this -// value should be set to the voltage measured on the 5V rail on the oilpan. -// -// See the manual for more details. The default value should be close. -// -//#define INPUT_VOLTAGE 5.0 -// - - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// RADIO CONFIGURATION -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// FLIGHT_MODE OPTIONAL -// FLIGHT_MODE_CHANNEL OPTIONAL -// -// Flight modes assigned to the control channel, and the input channel that -// is read for the control mode. -// -// Use a servo tester, or the ArduPilotMega_demo test program to check your -// switch settings. -// -// ATTENTION: Some ArduPilot Mega boards have radio channels marked 0-7, and -// others have them marked the standard 1-8. The FLIGHT_MODE_CHANNEL option -// uses channel numbers 1-8 (and defaults to 8). -// -// If you only have a three-position switch or just want three modes, set your -// switch to produce 1165, 1425, and 1815 microseconds and configure -// FLIGHT_MODE 1 & 2, 3 & 4 and 5 & 6 to be the same. This is the default. -// -// If you have FLIGHT_MODE_CHANNEL set to 8 (the default) and your control -// channel connected to input channel 8, the hardware failsafe mode will -// activate for any control input over 1750ms. -// -// For more modes (up to six), set your switch(es) to produce any of 1165, -// 1295, 1425, 1555, 1685, and 1815 microseconds. -// -// Flight mode | Switch Setting (ms) -// ------------+--------------------- -// 1 | 1165 -// 2 | 1295 -// 3 | 1425 -// 4 | 1555 -// 5 | 1685 -// 6 | 1815 (FAILSAFE if using channel 8) -// -// The following standard flight modes are available: -// -// Name | Description -// -----------------+-------------------------------------------- -// | -// MANUAL | Full manual control via the hardware multiplexer. -// | -// STABILIZE | Tries to maintain level flight, but can be overridden with radio control inputs. -// | -// FLY_BY_WIRE_A | Autopilot style control via user input, with manual throttle. -// | -// FLY_BY_WIRE_B | Autopilot style control via user input, aispeed controlled with throttle. -// | -// RTL | Returns to the Home location and then LOITERs at a safe altitude. -// | -// AUTO | Autonomous flight based on programmed waypoints. Use the WaypointWriter -// | application or your Ground Control System to edit and upload -// | waypoints and other commands. -// | -// -// -// The following non-standard modes are EXPERIMENTAL: -// -// Name | Description -// -----------------+-------------------------------------------- -// | -// LOITER | Flies in a circle around the current location. -// | -// CIRCLE | Flies in a stabilized 'dumb' circle. -// | -// -// -// If you are using channel 8 for mode switching then FLIGHT_MODE_5 and -// FLIGHT_MODE_6 should be MANUAL. -// -// -//#define FLIGHT_MODE_CHANNEL 8 -// -//#define FLIGHT_MODE_1 RTL -//#define FLIGHT_MODE_2 RTL -//#define FLIGHT_MODE_3 FLY_BY_WIRE_A -//#define FLIGHT_MODE_4 FLY_BY_WIRE_A -//#define FLIGHT_MODE_5 MANUAL -//#define FLIGHT_MODE_6 MANUAL -// - -////////////////////////////////////////////////////////////////////////////// -// THROTTLE_FAILSAFE OPTIONAL -// THROTTLE_FS_VALUE OPTIONAL -// THROTTLE_FAILSAFE_ACTION OPTIONAL -// -// The throttle failsafe allows you to configure a software failsafe activated -// by a setting on the throttle input channel (channel 3). -// -// This can be used to achieve a failsafe override on loss of radio control -// without having to sacrifice one of your FLIGHT_MODE settings, as the -// throttle failsafe overrides the switch-selected mode. -// -// Throttle failsafe is enabled by setting THROTTLE_FAILSAFE to 1. The default -// is for it to be disabled. -// -// If the throttle failsafe is enabled, THROTTLE_FS_VALUE sets the channel value -// below which the failsafe engages. The default is 975ms, which is a very low -// throttle setting. Most transmitters will let you trim the manual throttle -// position up so that you cannot engage the failsafe with a regular stick movement. -// -// Configure your receiver's failsafe setting for the throttle channel to the -// absolute minimum, and use the ArduPilotMega_demo program to check that -// you cannot reach that value with the throttle control. Leave a margin of -// at least 50 microseconds between the lowest throttle setting and -// THROTTLE_FS_VALUE. -// -// The FAILSAFE_ACTION setting determines what APM will do when throttle failsafe -// mode is entered while flying in AUTO mode. This is important in order to avoid -// accidental failsafe behaviour when flying waypoints that take the aircraft -// temporarily out of radio range. -// -// If FAILSAFE_ACTION is 1, when failsafe is entered in AUTO or LOITER modes, -// the aircraft will head for home in RTL mode. If the throttle channel moves -// back up, it will return to AUTO or LOITER mode. -// -// The default behaviour is to ignore throttle failsafe in AUTO and LOITER modes. -// -//#define THROTTLE_FAILSAFE DISABLED -//#define THROTTLE_FS_VALUE 975 -//#define THROTTLE_FAILSAFE_ACTION 2 -// - -////////////////////////////////////////////////////////////////////////////// -// AUTO_TRIM OPTIONAL -// -// ArduPilot Mega can update its trim settings by looking at the -// radio inputs when switching out of MANUAL mode. This allows you to -// manually trim your aircraft before switching to an assisted mode, but it -// also means that you should avoid switching out of MANUAL while you have -// any control stick deflection. -// -// The default is to enable AUTO_TRIM. -// -//#define AUTO_TRIM ENABLED -// - -////////////////////////////////////////////////////////////////////////////// -// THROTTLE_REVERSE OPTIONAL -// -// A few speed controls require the throttle servo signal be reversed. Setting -// this to ENABLED will reverse the throttle output signal. Ensure that your -// throttle needs to be reversed by using the hardware failsafe and the -// ArduPilotMega_demo program before setting this option. -// -// The default is to not reverse the signal. -// -//#define THROTTLE_REVERSE DISABLED -// - -////////////////////////////////////////////////////////////////////////////// -// ENABLE_STICK_MIXING OPTIONAL -// -// If this option is set to ENABLED, manual control inputs are are respected -// while in the autopilot modes (AUTO, RTL, LOITER, CIRCLE etc.) -// -// The default is to enable stick mixing, allowing the pilot to take -// emergency action without switching modes. -// -//#define ENABLE_STICK_MIXING ENABLED -// - -////////////////////////////////////////////////////////////////////////////// -// THROTTLE_OUT DEBUG -// -// When debugging, it can be useful to disable the throttle output. Set -// this option to DISABLED to disable throttle output signals. -// -// The default is to not disable throttle output. -// -//#define THROTTLE_OUT ENABLED -// - - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// STARTUP BEHAVIOUR -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// GROUND_START_DELAY OPTIONAL -// -// If configured, inserts a delay between power-up and the beginning of IMU -// calibration during a ground start. -// -// Use this setting to give you time to position the aircraft horizontally -// for the IMU calibration. -// -// The default is to begin IMU calibration immediately at startup. -// -//#define GROUND_START_DELAY 0 -// - -////////////////////////////////////////////////////////////////////////////// -// ENABLE_AIR_START OPTIONAL -// -// If air start is disabled then you will get a ground start (including IMU -// calibration) every time the AP is powered up. This means that if you get -// a power glitch or reboot for some reason in the air, you will probably -// crash, but it prevents a lot of problems on the ground like unintentional -// motor start-ups, etc. -// -// If air start is enabled then you will get an air start at power up and a -// ground start will be performed if the speed is near zero when we get gps -// lock. -// -// The default is to disable air start. -// -//#define ENABLE_AIR_START 0 -// - - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// FLIGHT AND NAVIGATION CONTROL -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// Altitude measurement and control. -// -// AOA OPTIONAL -// -// The angle in 100ths of a degree that the nose of the aircraft should be -// raised from horizontal in level flight. The default is 1 degree. -// -//#define AOA 100 // note, 100ths of a degree -// -// ALT_EST_GAIN OPTIONAL -// -// The gain of the altitude estimation function; a lower number results -// in slower error correction and smoother output. The default is a -// reasonable starting point. -// -//#define ALT_EST_GAIN 0.01 -// -// ALTITUDE_MIX OPTIONAL -// -// Configures the blend between GPS and pressure altitude. -// 0 = GPS altitude, 1 = Press alt, 0.5 = half and half, etc. -// -// The default is to use only GPS altitude. -// -//#define ALTITUDE_MIX 0 -// - -////////////////////////////////////////////////////////////////////////////// -// AIRSPEED_CRUISE OPTIONAL -// -// The speed in metres per second to maintain during cruise. The default -// is 10m/s, which is a conservative value suitable for relatively small, -// light aircraft. -// -//#define AIRSPEED_CRUISE 10 -// - -////////////////////////////////////////////////////////////////////////////// -// FLY_BY_WIRE_B airspeed control (also used for throttle "nudging" in AUTO) -// -// AIRSPEED_FBW_MIN OPTIONAL -// AIRSPEED_FBW_MAX OPTIONAL -// -// Airspeed corresponding to minimum and maximum throttle in Fly By Wire B mode. -// The defaults are 6 and 30 metres per second. -// -// AIRSPEED_FBW_MAX also sets the maximum airspeed that the cruise airspeed can be "nudged" to in AUTO mode when ENABLE_STICK_MIXING is set. -// In AUTO the cruise airspeed can be increased between AIRSPEED_CRUISE and AIRSPEED_FBW_MAX by positioning the throttle -// stick in the top 1/2 of its range. Throttle stick in the bottom 1/2 provide regular AUTO control. -// -// THROTTLE_ALT_P OPTIONAL -// THROTTLE_ALT_I OPTIONAL -// THROTTLE_ALT_D OPTIONAL -// -// P, I and D terms for the throttle control loop. Defaults are 0.32, 0, 0. -// -// THROTTLE_ALT_INT_MAX OPTIONAL -// -// Maximum throttle input due to the integral. Limits the throttle input -// due to persistent inability to maintain the commanded speed. Helps -// prevent the throttle from staying wide open when the control is reduced -// after a period at maxium speed. -// Default is 20 (20%). -// -//#define AIRSPEED_FBW_MIN 6 -//#define AIRSPEED_FBW_MAX 30 -//#define THROTTLE_ALT_P 0.32 -//#define THROTTLE_ALT_I 0.0 -//#define THROTTLE_ALT_D 0.0 -//#define THROTTLE_ALT_INT_MAX 20 -// - -////////////////////////////////////////////////////////////////////////////// -// Throttle control -// -// THROTTLE_MIN OPTIONAL -// -// The minimum throttle setting to which the autopilot will reduce the -// throttle while descending. The default is zero, which is -// suitable for aircraft with a steady power-off glide. Increase this -// value if your aircraft needs throttle to maintain a stable descent in -// level flight. -// -// THROTTLE_CRUISE OPTIONAL -// -// The approximate throttle setting to achieve AIRSPEED_CRUISE in level flight. -// The default is 45%, which is reasonable for a modestly powered aircraft. -// -// THROTTLE_MAX OPTIONAL -// -// The maximum throttle setting the autopilot will apply. The default is 75%. -// Reduce this value if your aicraft is overpowered, or has complex flight -// characteristics at high throttle settings. -// -//#define THROTTLE_MIN 0 -//#define THROTTLE_CRUISE 45 -//#define THROTTLE_MAX 75 -// - -////////////////////////////////////////////////////////////////////////////// -// Autopilot control limits -// -// HEAD_MAX OPTIONAL -// -// The maximum commanded bank angle in either direction. -// The default is 45 degrees. Decrease this value if your aircraft is not -// stable or has difficulty maintaining altitude in a steep bank. -// -// PITCH_MAX OPTIONAL -// -// The maximum commanded pitch up angle. -// The default is 15 degrees. Care should be taken not to set this value too -// large, as the aircraft may stall. -// -// PITCH_MIN -// -// The maximum commanded pitch down angle. Note that this value must be -// negative. The default is -25 degrees. Care should be taken not to set -// this value too large as it may result in overspeeding the aircraft. -// -//#define HEAD_MAX 45 -//#define PITCH_MAX 15 -//#define PITCH_MIN -25 - -////////////////////////////////////////////////////////////////////////////// -// Attitude control gains -// -// Tuning values for the attitude control PID loops. -// -// The P term is the primary tuning value. This determines how the control -// deflection varies in proportion to the required correction. -// -// The I term is used to help control surfaces settle. This value should -// normally be kept low. -// -// The D term is used to control overshoot. Avoid using or adjusting this -// term if you are not familiar with tuning PID loops. It should normally -// be zero for most aircraft. -// -// Note: When tuning these values, start with changes of no more than 25% at -// a time. -// -// SERVO_ROLL_P OPTIONAL -// SERVO_ROLL_I OPTIONAL -// SERVO_ROLL_D OPTIONAL -// -// P, I and D terms for roll control. Defaults are 0.4, 0, 0. -// -// SERVO_ROLL_INT_MAX OPTIONAL -// -// Maximum control offset due to the integral. This prevents the control -// output from being overdriven due to a persistent offset (e.g. crosstracking). -// Default is 5 degrees. -// -// ROLL_SLEW_LIMIT EXPERIMENTAL -// -// Limits the slew rate of the roll control in degrees per second. If zero, -// slew rate is not limited. Default is to not limit the roll control slew rate. -// (This feature is currently not implemented.) -// -// SERVO_PITCH_P OPTIONAL -// SERVO_PITCH_I OPTIONAL -// SERVO_PITCH_D OPTIONAL -// -// P, I and D terms for the pitch control. Defaults are 0.6, 0, 0. -// -// SERVO_PITCH_INT_MAX OPTIONAL -// -// Maximum control offset due to the integral. This prevents the control -// output from being overdriven due to a persistent offset (e.g. native flight -// AoA). If you find this value is insufficient, consider adjusting the AOA -// parameter. -// Default is 5 degrees. -// -// PITCH_COMP OPTIONAL -// -// Adds pitch input to compensate for the loss of lift due to roll control. -// Default is 0.20 (20% of roll control also applied to pitch control). -// -// SERVO_YAW_P OPTIONAL -// SERVO_YAW_I OPTIONAL -// SERVO_YAW_D OPTIONAL -// -// P, I and D terms for the YAW control. Defaults are 0., 0., 0. -// -// SERVO_YAW_INT_MAX OPTIONAL -// -// Maximum control offset due to the integral. This prevents the control -// output from being overdriven due to a persistent offset (e.g. crosstracking). -// Default is 5 degrees. -// -// RUDDER_MIX OPTIONAL -// -// Roll to yaw mixing. This allows for co-ordinated turns. -// Default is 0.50 (50% of roll control also applied to yaw control.) -// -//#define SERVO_ROLL_P 0.4 -//#define SERVO_ROLL_I 0.0 -//#define SERVO_ROLL_D 0.0 -//#define SERVO_ROLL_INT_MAX 5 -//#define ROLL_SLEW_LIMIT 0 -//#define SERVO_PITCH_P 0.6 -//#define SERVO_PITCH_I 0.0 -//#define SERVO_PITCH_D 0.0 -//#define SERVO_PITCH_INT_MAX 5 -//#define PITCH_COMP 0.2 -//#define SERVO_YAW_P 0.0 // Default is zero. A suggested value if you want to use this parameter is 0.5 -//#define SERVO_YAW_I 0.0 -//#define SERVO_YAW_D 0.0 -//#define SERVO_YAW_INT_MAX 5 -//#define RUDDER_MIX 0.5 -// - -////////////////////////////////////////////////////////////////////////////// -// Navigation control gains -// -// Tuning values for the navigation control PID loops. -// -// The P term is the primary tuning value. This determines how the control -// deflection varies in proportion to the required correction. -// -// The I term is used to control drift. -// -// The D term is used to control overshoot. Avoid adjusting this term if -// you are not familiar with tuning PID loops. -// -// Note: When tuning these values, start with changes of no more than 25% at -// a time. -// -// NAV_ROLL_P OPTIONAL -// NAV_ROLL_I OPTIONAL -// NAV_ROLL_D OPTIONAL -// -// P, I and D terms for navigation control over roll, normally used for -// controlling the aircraft's course. The P term controls how aggressively -// the aircraft will bank to change or hold course. -// Defaults are 0.7, 0.0, 0.02. -// -// NAV_ROLL_INT_MAX OPTIONAL -// -// Maximum control offset due to the integral. This prevents the control -// output from being overdriven due to a persistent offset (e.g. crosstracking). -// Default is 5 degrees. -// -// NAV_PITCH_ASP_P OPTIONAL -// NAV_PITCH_ASP_I OPTIONAL -// NAV_PITCH_ASP_D OPTIONAL -// -// P, I and D terms for pitch adjustments made to maintain airspeed. -// Defaults are 0.65, 0, 0. -// -// NAV_PITCH_ASP_INT_MAX OPTIONAL -// -// Maximum pitch offset due to the integral. This limits the control -// output from being overdriven due to a persistent offset (eg. inability -// to maintain the programmed airspeed). -// Default is 5 degrees. -// -// NAV_PITCH_ALT_P OPTIONAL -// NAV_PITCH_ALT_I OPTIONAL -// NAV_PITCH_ALT_D OPTIONAL -// -// P, I and D terms for pitch adjustments made to maintain altitude. -// Defaults are 0.65, 0, 0. -// -// NAV_PITCH_ALT_INT_MAX OPTIONAL -// -// Maximum pitch offset due to the integral. This limits the control -// output from being overdriven due to a persistent offset (eg. inability -// to maintain the programmed altitude). -// Default is 5 degrees. -// -//#define NAV_ROLL_P 0.7 -//#define NAV_ROLL_I 0.01 -//#define NAV_ROLL_D 0.02 -//#define NAV_ROLL_INT_MAX 5 -//#define NAV_PITCH_ASP_P 0.65 -//#define NAV_PITCH_ASP_I 0.0 -//#define NAV_PITCH_ASP_D 0.0 -//#define NAV_PITCH_ASP_INT_MAX 5 -//#define NAV_PITCH_ALT_P 0.65 -//#define NAV_PITCH_ALT_I 0.0 -//#define NAV_PITCH_ALT_D 0.0 -//#define NAV_PITCH_ALT_INT_MAX 5 -// - -////////////////////////////////////////////////////////////////////////////// -// Energy/Altitude control gains -// -// The Energy/altitude control system uses throttle input to control aircraft -// altitude. -// -// The P term is the primary tuning value. This determines how the throttle -// setting varies in proportion to the required correction. -// -// The I term is used to compensate for small offsets. -// -// The D term is used to control overshoot. Avoid adjusting this term if -// you are not familiar with tuning PID loops. -// -// THROTTLE_TE_P OPTIONAL -// THROTTLE_TE_I OPTIONAL -// THROTTLE_TE_D OPTIONAL -// -// P, I and D terms for throttle adjustments made to control altitude. -// Defaults are 0.5, 0, 0. -// -// THROTTLE_TE_INT_MAX OPTIONAL -// -// Maximum throttle input due to the integral term. This limits the -// throttle from being overdriven due to a persistent offset (e.g. -// inability to maintain the programmed altitude). -// Default is 20%. -// -// THROTTLE_SLEW_LIMIT OPTIONAL -// -// Limits the slew rate of the throttle, in percent per second. Helps -// avoid sudden throttle changes, which can destabilise the aircraft. -// A setting of zero disables the feature. -// Default is zero (disabled). -// -// P_TO_T OPTIONAL -// -// Pitch to throttle feed-forward gain. Used when AIRSPEED_SENSOR -// is DISABLED. Default is 2.5. -// -//#define THROTTLE_TE_P 0.50 -//#define THROTTLE_TE_I 0.0 -//#define THROTTLE_TE_D 0.0 -//#define THROTTLE_TE_INT_MAX 20 -//#define THROTTLE_SLEW_LIMIT 0 -//#define P_TO_T 2.5 -// - -////////////////////////////////////////////////////////////////////////////// -// Crosstrack compensation -// -// XTRACK_GAIN OPTIONAL -// -// Crosstrack compensation in degrees per metre off track. -// Default value is 0.01 degrees per metre. Values lower than 0.001 will -// disable crosstrack compensation. -// -// XTRACK_ENTRY_ANGLE OPTIONAL -// -// Maximum angle used to correct for track following. -// Default value is 30 degrees. -// -//#define XTRACK_GAIN 10 // deg/m * 100 -//#define XTRACK_ENTRY_ANGLE 3000 // deg * 100 -// - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// DEBUGGING -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// Subsystem test and debug. -// -// DEBUG_SUBSYSTEM DEBUG -// -// Selects a subsystem debug mode. Default is 0. -// -// 0 = no debug -// 1 = Debug the Radio input -// 2 = Radio Setup / Servo output -// 4 = Debug the GPS input -// 5 = Debug the GPS input - RAW OUTPUT -// 6 = Debug the IMU -// 7 = Debug the Control Switch -// 8 = Debug the Servo DIP switches -// 9 = Debug the Relay out -// 10 = Debug the Magnetometer Note - The magnetometer is not supported in V1.0 -// 11 = Debug the ABS pressure sensor -// 12 = Debug the stored waypoints -// 13 = Debug the Throttle -// 14 = Debug the Radio Min Max -// 15 = Debug the EEPROM - Hex Dump -// 16 = XBee X-CTU Range and RSSI Test -// 17 = Debug IMU - raw gyro and accel outputs -// 20 = Debug Analog Sensors -// -// -//#define DEBUG_SUBSYSTEM 0 -// - -////////////////////////////////////////////////////////////////////////////// -// DEBUG_LEVEL DEBUG -// -// Selects the lowest level of debug messages passed to the telemetry system. -// Default is SEVERITY_LOW. May be one of: -// -// SEVERITY_LOW -// SEVERITY_MEDIUM -// SEVERITY_HIGH -// SEVERITY_CRITICAL -// -//#define DEBUG_LEVEL SEVERITY_LOW -// - -////////////////////////////////////////////////////////////////////////////// -// Dataflash logging control -// -// Each of these logging options may be set to ENABLED to enable, or DISABLED -// to disable the logging of the respective data. -// -// LOG_ATTITUDE_FAST DEBUG -// -// Logs basic attitude info to the dataflash at 50Hz (uses more space). -// Defaults to DISABLED. -// -// LOG_ATTITUDE_MED OPTIONAL -// -// Logs basic attitude info to the dataflash at 10Hz (uses less space than -// LOG_ATTITUDE_FAST). Defaults to ENABLED. -// -// LOG_GPS OPTIONAL -// -// Logs GPS info to the dataflash at 10Hz. Defaults to ENABLED. -// -// LOG_PM OPTIONAL -// -// Logs IMU performance monitoring info every 20 seconds. -// Defaults to DISABLED. -// -// LOG_CTUN OPTIONAL -// -// Logs control loop tuning info at 10 Hz. This information is useful for tuning -// servo control loop gain values. Defaults to DISABLED. -// -// LOG_NTUN OPTIONAL -// -// Logs navigation tuning info at 10 Hz. This information is useful for tuning -// navigation control loop gain values. Defaults to DISABLED. -// -// LOG_ MODE OPTIONAL -// -// Logs changes to the flight mode upon occurrence. Defaults to ENABLED. -// -// LOG_RAW DEBUG -// -// Logs raw accelerometer and gyro data at 50 Hz (uses more space). -// Defaults to DISABLED. -// -// LOG_CMD OPTIONAL -// -// Logs new commands when they process. -// Defaults to ENABLED. -// -//#define LOG_ATTITUDE_FAST DISABLED -//#define LOG_ATTITUDE_MED ENABLED -//#define LOG_GPS ENABLED -//#define LOG_PM ENABLED -//#define LOG_CTUN DISABLED -//#define LOG_NTUN DISABLED -//#define LOG_MODE ENABLED -//#define LOG_RAW DISABLED -//#define LOG_CMD ENABLED -// - -////////////////////////////////////////////////////////////////////////////// -// Navigation defaults -// -// WP_RADIUS_DEFAULT OPTIONAL -// -// When the user performs a factory reset on the APM, set the waypoint radius -// (the radius from a target waypoint within which the APM will consider -// itself to have arrived at the waypoint) to this value in meters. This is -// mainly intended to allow users to start using the APM without running the -// WaypointWriter first. -// -// LOITER_RADIUS_DEFAULT OPTIONAL -// -// When the user performs a factory reset on the APM, set the loiter radius -// (the distance the APM will attempt to maintain from a waypoint while -// loitering) to this value in meters. This is mainly intended to allow -// users to start using the APM without running the WaypointWriter first. -// -//#define WP_RADIUS_DEFAULT 20 -//#define LOITER_RADIUS_DEFAULT 60 -// - -////////////////////////////////////////////////////////////////////////////// -// Debugging interface -// -// DEBUG_PORT OPTIONAL -// -// The APM will periodically send messages reporting what it is doing; this -// variable determines to which serial port they will be sent. Port 0 is the -// USB serial port on the shield, port 3 is the telemetry port. -// -//#define DEBUG_PORT 0 -// - -// -// Do not remove - this is to discourage users from copying this file -// and using it as-is rather than editing their own. -// -#error You should not copy APM_Config.h.reference - make your own APM_Config.h file with just the options you need. diff --git a/Tools/ArduTracker/APM_Config_mavlink_hil.h b/Tools/ArduTracker/APM_Config_mavlink_hil.h deleted file mode 100644 index 96300c38a1..0000000000 --- a/Tools/ArduTracker/APM_Config_mavlink_hil.h +++ /dev/null @@ -1,55 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -// Enable Autopilot Flight Mode -#define FLIGHT_MODE_CHANNEL 8 -#define FLIGHT_MODE_1 AUTO -#define FLIGHT_MODE_2 RTL -#define FLIGHT_MODE_3 FLY_BY_WIRE_A -#define FLIGHT_MODE_4 FLY_BY_WIRE_B -#define FLIGHT_MODE_5 STABILIZE -#define FLIGHT_MODE_6 MANUAL - -// Hardware in the loop protocol -#define HIL_PROTOCOL HIL_PROTOCOL_MAVLINK - -// HIL_MODE SELECTION -// -// Mavlink supports -// 1. HIL_MODE_ATTITUDE : simulated position, airspeed, and attitude -// 2. HIL_MODE_SENSORS: full sensor simulation -#define HIL_MODE HIL_MODE_ATTITUDE - -// HIL_PORT SELCTION -// -// PORT 1 -// If you would like to run telemetry communications for a groundstation -// while you are running hardware in the loop it is necessary to set -// HIL_PORT to 1. This uses the port that would have been used for the gps -// as the hardware in the loop port. You will have to solder -// headers onto the gps port connection on the apm -// and connect via an ftdi cable. -// The baud rate is set to 115200 in this mode. -// -// PORT 3 -// If you don't require telemetry communication with a gcs while running -// hardware in the loop you may use the telemetry port as the hardware in -// the loop port. -// The buad rate is controlled by SERIAL3_BAUD in this mode. -#define HIL_PORT 1 - -// You can set your gps protocol here for your actual -// hardware and leave it without affecting the hardware -// in the loop simulation -#define GPS_PROTOCOL GPS_PROTOCOL_MTK - -// Ground control station comms -#define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK -#define GCS_PORT 3 - -// Sensors -// All sensors are supported in all modes. -// The magnetometer is not used in -// HIL_MODE_ATTITUDE but you may leave it -// enabled if you wish. -#define AIRSPEED_SENSOR ENABLED -#define MAGNETOMETER ENABLED diff --git a/Tools/ArduTracker/APM_Config_xplane.h b/Tools/ArduTracker/APM_Config_xplane.h deleted file mode 100644 index 9d40a9a961..0000000000 --- a/Tools/ArduTracker/APM_Config_xplane.h +++ /dev/null @@ -1,23 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -#define DEBUG_SUBSYSTEM 0 - -#define FLIGHT_MODE_CHANNEL 8 -#define FLIGHT_MODE_1 AUTO -#define FLIGHT_MODE_2 RTL -#define FLIGHT_MODE_3 FLY_BY_WIRE_A -#define FLIGHT_MODE_4 FLY_BY_WIRE_B -#define FLIGHT_MODE_5 STABILIZE -#define FLIGHT_MODE_6 MANUAL - -#define HIL_PROTOCOL HIL_PROTOCOL_XPLANE -#define HIL_MODE HIL_MODE_ATTITUDE -#define HIL_PORT 0 - -#define GCS_PROTOCOL GCS_PROTOCOL_DEBUGTERMINAL -#define GCS_PORT 3 - -#define AIRSPEED_CRUISE 25 - -#define THROTTLE_FAILSAFE ENABLED -#define AIRSPEED_SENSOR ENABLED diff --git a/Tools/ArduTracker/ArduTracker.pde b/Tools/ArduTracker/ArduTracker.pde deleted file mode 100644 index 3ef01a1891..0000000000 --- a/Tools/ArduTracker/ArduTracker.pde +++ /dev/null @@ -1,879 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/* -ArduPilotMega (unstable development version) -Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short -Thanks to: Chris Anderson, HappyKillMore, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi -Please contribute your ideas! - - -This firmware is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. -*/ - -//////////////////////////////////////////////////////////////////////////////// -// Header includes -//////////////////////////////////////////////////////////////////////////////// - -// AVR runtime -#include -#include -#include -#include - -// Libraries -#include -#include -#include -#include // ArduPilot Mega RC Library -#include // ArduPilot GPS library -#include -#include // ArduPilot Mega Flash Memory Library -#include // ArduPilot Mega Analog to Digital Converter Library -#include // ArduPilot Mega BMP085 Library -#include // ArduPilot Mega Magnetometer Library -#include // ArduPilot Mega Vector/Matrix math Library -#include // ArduPilot Mega IMU Library -#include // ArduPilot Mega DCM Library -#include // PID library - -#include // MAVLink GCS definitions - -// Configuration -#include "config.h" - -// Local modules -#include "defines.h" -#include "global_data.h" -#include "GCS.h" -#include "HIL.h" - -//////////////////////////////////////////////////////////////////////////////// -// Serial ports -//////////////////////////////////////////////////////////////////////////////// -// -// Note that FastSerial port buffers are allocated at ::begin time, -// so there is not much of a penalty to defining ports that we don't -// use. -// -FastSerialPort0(Serial); // FTDI/console -FastSerialPort1(Serial1); // GPS port -FastSerialPort3(Serial3); // Telemetry port - -//////////////////////////////////////////////////////////////////////////////// -// Sensors -//////////////////////////////////////////////////////////////////////////////// -// -// There are three basic options related to flight sensor selection. -// -// - Normal flight mode. Real sensors are used. -// - HIL Attitude mode. Most sensors are disabled, as the HIL -// protocol supplies attitude information directly. -// - HIL Sensors mode. Synthetic sensors are configured that -// supply data from the simulation. -// - -#if HIL_MODE == HIL_MODE_NONE - -// real sensors -AP_ADC_ADS7844 adc; -APM_BMP085_Class pitot; //TODO: 'pitot' is not an appropriate name for a static pressure sensor -AP_Compass_HMC5843 compass; - -// real GPS selection -#if GPS_PROTOCOL == GPS_PROTOCOL_NMEA -AP_GPS_NMEA gps(&Serial1); -#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF -AP_GPS_SIRF gps(&Serial1); -#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX -AP_GPS_UBLOX gps(&Serial1); -#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK -AP_GPS_MTK gps(&Serial1); -#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK19 -AP_GPS_MTK19 gps(&Serial1); -#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE -AP_GPS_NONE gps(NULL); -#else - #error Unrecognised GPS_PROTOCOL setting. -#endif // GPS PROTOCOL - -#elif HIL_MODE == HIL_MODE_SENSORS -// sensor emulators -AP_ADC_HIL adc; -APM_BMP085_HIL_Class pitot; -AP_Compass_HIL compass; -AP_GPS_HIL gps(NULL); - -#elif HIL_MODE == HIL_MODE_ATTITUDE -AP_DCM_HIL dcm; -AP_GPS_HIL gps(NULL); - -#else - #error Unrecognised HIL_MODE setting. -#endif // HIL MODE - -#if HIL_MODE != HIL_MODE_DISABLED - -#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK - HIL_MAVLINK hil; -#elif HIL_PROTOCOL == HIL_PROTOCOL_XPLANE - HIL_XPLANE hil; -#endif // HIL PROTOCOL - -#endif - -#if HIL_MODE != HIL_MODE_ATTITUDE -AP_IMU imu(&adc,getAddress(PARAM_IMU_OFFSET_0)); -AP_DCM dcm(&imu, &gps, &compass); -#endif - -//////////////////////////////////////////////////////////////////////////////// -// GCS selection -//////////////////////////////////////////////////////////////////////////////// -// -#if GCS_PROTOCOL == GCS_PROTOCOL_STANDARD -// create an instance of the standard GCS. -BinComm::MessageHandler GCS_MessageHandlers[] = { - {BinComm::MSG_ANY, receive_message, NULL}, - {BinComm::MSG_NULL, NULL, NULL} -}; -GCS_STANDARD gcs(GCS_MessageHandlers); - -#elif GCS_PROTOCOL == GCS_PROTOCOL_LEGACY -GCS_LEGACY gcs; -#elif GCS_PROTOCOL == GCS_PROTOCOL_DEBUGTERMINAL -GCS_DEBUGTERMINAL gcs; -#elif GCS_PROTOCOL == GCS_PROTOCOL_XPLANE -GCS_XPLANE gcs; // Should become a HIL -#elif GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK -GCS_MAVLINK gcs; -#else -// If we are not using a GCS, we need a stub that does nothing. -GCS_Class gcs; -#endif - -//////////////////////////////////////////////////////////////////////////////// -// Global variables -//////////////////////////////////////////////////////////////////////////////// - -byte control_mode = MANUAL; -boolean failsafe = false; // did our throttle dip below the failsafe value? -boolean ch3_failsafe = false; -byte crash_timer; -byte oldSwitchPosition; // for remembering the control mode switch -boolean reverse_switch = 1; // do we read the reversing switches after startup? - -byte ground_start_count = 6; // have we achieved first lock and set Home? -int ground_start_avg; // 5 samples to avg speed for ground start -boolean ground_start = false; // have we started on the ground? -const char *comma = ","; - -const char* flight_mode_strings[] = { - "Manual", - "Circle", - "Stabilize", - "", - "", - "FBW_A", - "FBW_B", - "", - "", - "", - "Auto", - "RTL", - "Loiter", - "Takeoff", - "Land"}; - - -/* Radio values - Channel assignments - 1 Ailerons (rudder if no ailerons) - 2 Elevator - 3 Throttle - 4 Rudder (if we have ailerons) - 5 Mode - 6 TBD - 7 TBD - 8 TBD -*/ - -uint16_t radio_in[8]; // current values from the transmitter - microseconds -uint16_t radio_out[8]; // Send to the PWM library -int16_t servo_out[8]; // current values to the servos - degrees * 100 (approx assuming servo is -45 to 45 degrees except [3] is 0 to 100 - -uint16_t elevon1_trim = 1500; // TODO: handle in EEProm -uint16_t elevon2_trim = 1500; -uint16_t ch1_temp = 1500; // Used for elevon mixing -uint16_t ch2_temp = 1500; - -int reverse_roll = 1; -int reverse_pitch = 1; -int reverse_rudder = 1; -byte mix_mode = 0; // 0 = normal , 1 = elevons -int reverse_elevons = 1; -int reverse_ch1_elevon = 1; -int reverse_ch2_elevon = 1; - -// for elevons radio_in[CH_ROLL] and radio_in[CH_PITCH] are equivalent aileron and elevator, not left and right elevon - - -float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1? - -// PID controllers -PID pidServoRoll(getAddress(PARAM_RLL2SRV_P),PID::STORE_EEPROM_FLOAT); -PID pidServoPitch(getAddress(PARAM_PTCH2SRV_P),PID::STORE_EEPROM_FLOAT); -PID pidServoRudder(getAddress(PARAM_YW2SRV_P),PID::STORE_EEPROM_FLOAT); -PID pidNavRoll(getAddress(PARAM_HDNG2RLL_P),PID::STORE_EEPROM_FLOAT); -PID pidNavPitchAirspeed(getAddress(PARAM_ARSPD2PTCH_P),PID::STORE_EEPROM_FLOAT); -PID pidNavPitchAltitude(getAddress(PARAM_ALT2PTCH_P),PID::STORE_EEPROM_FLOAT); -PID pidTeThrottle(getAddress(PARAM_ENRGY2THR_P),PID::STORE_EEPROM_FLOAT); -PID pidAltitudeThrottle(getAddress(PARAM_ALT2THR_P),PID::STORE_EEPROM_FLOAT); - -PID *pid_index[] = { - &pidServoRoll, - &pidServoPitch, - &pidServoRudder, - &pidNavRoll, - &pidNavPitchAirspeed, - &pidNavPitchAltitude, - &pidTeThrottle, - &pidAltitudeThrottle -}; - -// GPS variables -// ------------- -const float t7 = 10000000.0; // used to scale values for EEPROM and flash memory storage -float scaleLongUp; // used to reverse longtitude scaling -float scaleLongDown; // used to reverse longtitude scaling -boolean GPS_light = false; // status of the GPS light - -// Location & Navigation -// --------------------- -const float radius_of_earth = 6378100; // meters -const float gravity = 9.81; // meters/ sec^2 -long hold_course = -1; // deg * 100 dir of plane -long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate -long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target -long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target -int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate - - -byte command_must_index; // current command memory location -byte command_may_index; // current command memory location -byte command_must_ID; // current command ID -byte command_may_ID; // current command ID -//byte EEPROM_command // 1 = from the list, 0 = generated - - -// Airspeed -// -------- -int airspeed; // m/s * 100 -int airspeed_nudge = 0; // m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range -float airspeed_error; // m/s * 100 -long energy_error; // energy state error (kinetic + potential) for altitude hold -long airspeed_energy_error; // kinetic portion of energy error - -// Location Errors -// --------------- -long bearing_error; // deg * 100 : 0 to 36000 -long altitude_error; // meters * 100 we are off in altitude -float crosstrack_error; // meters we are off trackline - -// Sensors -// -------- -float airpressure_raw; // Airspeed Sensor - is a float to better handle filtering -int airpressure_offset; // analog air pressure sensor while still -int airpressure; // airspeed as a pressure value -float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter -float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery Voltage of cell 1, initialized above threshold for filter -float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1+2, initialized above threshold for filter -float battery_voltage3 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1+2+3, initialized above threshold for filter -float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1+2+3+4, initialized above threshold for filter - -// Pressure Sensor variables -unsigned long abs_press; -unsigned long abs_press_filt; -unsigned long abs_press_gnd; -int ground_temperature; -int temp_unfilt; -long ground_alt; // Ground altitude from gps at startup in centimeters -long press_alt; // Pressure altitude - -// flight mode specific -// -------------------- -boolean takeoff_complete = true; // Flag for using gps ground course instead of IMU yaw. Set false when takeoff command processes. -boolean land_complete = false; -int landing_pitch; // pitch for landing set by commands -int takeoff_pitch; -int takeoff_altitude; -int landing_distance; // meters; - -// Loiter management -// ----------------- -long old_target_bearing; // deg * 100 -int loiter_total; // deg : how many times to loiter * 360 -int loiter_delta; // deg : how far we just turned -int loiter_sum; // deg : how far we have turned around a waypoint -long loiter_time; // millis : when we started LOITER mode -int loiter_time_max; // millis : how long to stay in LOITER mode - -// these are the values for navigation control functions -// ---------------------------------------------------- -long nav_roll; // deg * 100 : target roll angle -long nav_pitch; // deg * 100 : target pitch angle -long altitude_estimate; // for smoothing GPS output - -int throttle_nudge = 0; // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel - -// Waypoints -// --------- -long wp_distance; // meters - distance between plane and next waypoint -long wp_totalDistance; // meters - distance between old and next waypoint -byte next_wp_index; // Current active command index - -// repeating event control -// ----------------------- -byte event_id; // what to do - see defines -long event_timer; // when the event was asked for in ms -int event_delay; // how long to delay the next firing of event in millis -int event_repeat; // how many times to fire : 0 = forever, 1 = do once, 2 = do twice -int event_value; // per command value, such as PWM for servos -int event_undo_value; // the value used to undo commands -byte repeat_forever; -byte undo_event; // counter for timing the undo - -// delay command -// -------------- -int delay_timeout = 0; // used to delay commands -long delay_start = 0; // used to delay commands - -// 3D Location vectors -// ------------------- -struct Location home; // home location -struct Location prev_WP; // last waypoint -struct Location current_loc; // current location -struct Location next_WP; // next waypoint -struct Location tell_command; // command for telemetry -struct Location next_command; // command preloaded -long target_altitude; // used for -long offset_altitude; // used for -boolean home_is_set = false; // Flag for if we have gps lock and have set the home location - -// patch antenna variables -struct Location trackVehicle_loc; // vehicle location to track with antenna - -// IMU variables -// ------------- -float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm) - -float COGX; // Course overground X axis -float COGY = 1; // Course overground Y axis - -// Performance monitoring -// ---------------------- -long perf_mon_timer; -float imu_health; // Metric based on accel gain deweighting -int G_Dt_max; // Max main loop cycle time in milliseconds -byte gyro_sat_count; -byte adc_constraints; -byte renorm_sqrt_count; -byte renorm_blowup_count; -int gps_fix_count; -byte gcs_messages_sent; - - -// GCS -// --- -char GCS_buffer[53]; -char display_PID = -1; // Flag used by DebugTerminal to indicate that the next PID calculation with this index should be displayed - -// System Timers -// -------------- -unsigned long fast_loopTimer; // Time in miliseconds of main control loop -unsigned long fast_loopTimeStamp = 0; // Time Stamp when fast loop was complete -unsigned long medium_loopTimer; // Time in miliseconds of navigation control loop -byte medium_loopCounter = 0; // Counters for branching from main control loop to slower loops -byte slow_loopCounter = 0; -byte superslow_loopCounter = 0; -unsigned long deltaMiliSeconds; // Delta Time in miliseconds -unsigned long dTnav; // Delta Time in milliseconds for navigation computations -int mainLoop_count; -unsigned long elapsedTime; // for doing custom events -unsigned long GPS_timer; -float load; // % MCU cycles used - -//////////////////////////////////////////////////////////////////////////////// -// Top-level logic -//////////////////////////////////////////////////////////////////////////////// - -void setup() { - - init_ardupilot(); -} - -void loop() -{ - // We want this to execute at 50Hz if possible - // ------------------------------------------- - if (millis()-fast_loopTimer > 19) { - deltaMiliSeconds = millis() - fast_loopTimer; - load = float(fast_loopTimeStamp - fast_loopTimer)/deltaMiliSeconds; - G_Dt = (float)deltaMiliSeconds / 1000.f; - fast_loopTimer = millis(); - - mainLoop_count++; - - // Execute the fast loop - // --------------------- - fast_loop(); - - // Execute the medium loop - // ----------------------- - medium_loop(); - - if (millis()- perf_mon_timer > 20000) { - if (mainLoop_count != 0) { - gcs.send_message(MSG_PERF_REPORT); - if (get(PARAM_LOG_BITMASK) & MASK_LOG_PM) - Log_Write_Performance(); - - resetPerfData(); - } - } - fast_loopTimeStamp = millis(); - } -} - -void fast_loop() -{ - // This is the fast loop - we want it to execute at 50Hz if possible - // ----------------------------------------------------------------- - if (deltaMiliSeconds > G_Dt_max) - G_Dt_max = deltaMiliSeconds; - - // Read radio - // ---------- - read_radio(); - - // check for throtle failsafe condition - // ------------------------------------ - //if (get(PARAM_THR_FAILSAFE)) - //set_failsafe(ch3_failsafe); - - // Read Airspeed - // ------------- -# if AIRSPEED_SENSOR == 1 && HIL_MODE != HIL_MODE_ATTITUDE - //read_airspeed(); -# endif - - //dcm.update_DCM(G_Dt); - -# if HIL_MODE == HIL_MODE_DISABLED - //if (get(PARAM_LOG_BITMASK) & MASK_LOG_ATTITUDE_FAST) - //Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (uint16_t)dcm.yaw_sensor); - - //if (get(PARAM_LOG_BITMASK) & MASK_LOG_RAW) - //Log_Write_Raw(); -#endif // HIL_MODE - - // altitude smoothing - // ------------------ - //if (control_mode != FLY_BY_WIRE_B) - //calc_altitude_error(); - - // inertial navigation - // ------------------ -#if INERTIAL_NAVIGATION == ENABLED - // TODO: implement inertial nav function - //inertialNavigation(); -#endif - - // custom code/exceptions for flight modes - // --------------------------------------- - //update_current_flight_mode(); - - // apply desired roll, pitch and yaw to the plane - // ---------------------------------------------- - //if (control_mode > MANUAL) - //stabilize(); - - // write out the servo PWM values - // ------------------------------ - set_servos_4(); - - - // XXX is it appropriate to be doing the comms below on the fast loop? - -#if HIL_MODE != HIL_MODE_DISABLED - // kick the HIL to process incoming sensor packets - hil.update(); - // send out hil data - hil.send_message(MSG_SERVO_OUT); - //hil.send_message(MSG_ATTITUDE); - //hil.send_message(MSG_LOCATION); - //hil.send_message(MSG_AIRSPEED); -#endif - - // kick the GCS to process uplink data - gcs.update(); -#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK - gcs.data_stream_send(45,1000); -#endif - // XXX this should be absorbed into the above, - // or be a "GCS fast loop" interface -} - -void medium_loop() -{ - // This is the start of the medium (10 Hz) loop pieces - // ----------------------------------------- - switch(medium_loopCounter) { - - // This case deals with the GPS - //------------------------------- - case 0: - medium_loopCounter++; - update_GPS(); - - #if HIL_MODE != HIL_MODE_ATTITUDE && MAGNETOMETER == 1 - //compass.read(); // Read magnetometer - //compass.calculate(dcm.roll,dcm.pitch); // Calculate heading - #endif - - break; - - // This case performs some navigation computations - //------------------------------------------------ - case 1: - medium_loopCounter++; - - - if(gps.new_data){ - dTnav = millis() - medium_loopTimer; - medium_loopTimer = millis(); - } - - // calculate the plane's desired bearing - // ------------------------------------- - //navigate(); - break; - - // command processing - //------------------------------ - case 2: - medium_loopCounter++; - - // perform next command - // -------------------- - //update_commands(); - break; - - // This case deals with sending high rate telemetry - //------------------------------------------------- - case 3: - medium_loopCounter++; - - //if ((get(PARAM_LOG_BITMASK) & MASK_LOG_ATTITUDE_MED) && !(get(PARAM_LOG_BITMASK) & MASK_LOG_ATTITUDE_FAST)) - //Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (uint16_t)dcm.yaw_sensor); - -#if HIL_MODE != HIL_MODE_ATTITUDE - //if (get(PARAM_LOG_BITMASK) & MASK_LOG_CTUN) - //Log_Write_Control_Tuning(); -#endif - - //if (get(PARAM_LOG_BITMASK) & MASK_LOG_NTUN) - //Log_Write_Nav_Tuning(); - - //if (get(PARAM_LOG_BITMASK) & MASK_LOG_GPS) - //Log_Write_GPS(gps.time, current_loc.lat, current_loc.lng, gps.altitude, current_loc.alt, (long) gps.ground_speed, gps.ground_course, gps.fix, gps.num_sats); - -// XXX this should be a "GCS medium loop" interface -#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK - gcs.data_stream_send(5,45); - // send all requested output streams with rates requested - // between 5 and 45 Hz -#else - //gcs.send_message(MSG_ATTITUDE); // Sends attitude data -#endif - break; - - // This case controls the slow loop - //--------------------------------- - case 4: - medium_loopCounter=0; - slow_loop(); - break; - } -} - -void slow_loop() -{ - // This is the slow (3 1/3 Hz) loop pieces - //---------------------------------------- - switch (slow_loopCounter){ - case 0: - slow_loopCounter++; - - - - - superslow_loopCounter++; - //if(superslow_loopCounter >=15) { - // keep track of what page is in use in the log - // *** We need to come up with a better scheme to handle this... - //eeprom_write_word((uint16_t *) EE_LAST_LOG_PAGE, DataFlash.GetWritePage()); - //superslow_loopCounter = 0; - //} - break; - - case 1: - slow_loopCounter++; - - // Read 3-position switch on radio - // ------------------------------- - read_control_switch(); - - // Read Control Surfaces/Mix switches - // ---------------------------------- - if(reverse_switch){ - update_servo_switches(); - } - - // Read main battery voltage if hooked up - does not read the 5v from radio - // ------------------------------------------------------------------------ - #if BATTERY_EVENT == 1 - read_battery(); - #endif - -#if HIL_MODE != HIL_MODE_ATTITUDE - // Read Baro pressure - // ------------------ - //read_airpressure(); -#endif - - break; - - case 2: - slow_loopCounter = 0; - //update_events(); - -// XXX this should be a "GCS slow loop" interface -#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK - gcs.data_stream_send(1,5); - // send all requested output streams with rates requested - // between 1 and 5 Hz -#else - //gcs.send_message(MSG_LOCATION); -#endif - // send a heartbeat - gcs.send_message(MSG_HEARTBEAT); // XXX This is running at 3 1/3 Hz - //but should be at 1 Hz, new loop timer? - // display load - gcs.send_message(MSG_CPU_LOAD, load*100); - break; - } -} - - -void update_GPS(void) -{ - if(gps.status() == 0) - { - gps.init(); // reinitialize dead connections - return; // let it warm up while other stuff is running - } - gps.update(); - update_GPS_light(); - - if (gps.new_data && gps.fix) { - GPS_timer = millis(); -// XXX We should be sending GPS data off one of the regular loops so that we send -// no-GPS-fix data too -#if GCS_PROTOCOL != GCS_PROTOCOL_MAVLINK - gcs.send_message(MSG_LOCATION); -#endif - - // for performance - // --------------- - gps_fix_count++; - - if(ground_start_count > 1){ - ground_start_count--; - ground_start_avg += gps.ground_speed; - - } else if (ground_start_count == 1) { - // We countdown N number of good GPS fixes - // so that the altitude is more accurate - // ------------------------------------- - if (current_loc.lat == 0) { - SendDebugln("!! bad loc"); - ground_start_count = 5; - - } else { - if(ENABLE_AIR_START == 1 && (ground_start_avg / 5) < SPEEDFILT){ - startup_ground(); - - if (get(PARAM_LOG_BITMASK) & MASK_LOG_CMD) - Log_Write_Startup(TYPE_GROUNDSTART_MSG); - - init_home(); - } else if (ENABLE_AIR_START == 0) { - init_home(); - } - - ground_start_count = 0; - } - } - - - current_loc.lng = gps.longitude; // Lon * 10**7 - current_loc.lat = gps.latitude; // Lat * 10**7 - -// XXX this is bogus; should just force get(PARAM_ALT_MIX) to zero for GPS_PROTOCOL_IMU -#if HIL_MODE == HIL_MODE_ATTITUDE - current_loc.alt = gps.altitude; -#else - current_loc.alt = ((1 - get(PARAM_ALT_MIX)) * gps.altitude) + (get(PARAM_ALT_MIX) * press_alt); // alt_MSL centimeters (meters * 100) -#endif - - // Calculate new climb rate - add_altitude_data(millis()/100, gps.altitude/10); - - COGX = cosf(ToRad(gps.ground_course/100.0)); - COGY = sinf(ToRad(gps.ground_course/100.0)); - } -} - -void update_current_flight_mode(void) -{ - if(control_mode == AUTO){ - crash_checker(); - - switch(command_must_ID){ - case CMD_TAKEOFF: - if (hold_course > -1) { - calc_nav_roll(); - } else { - nav_roll = 0; - } - -#if AIRSPEED_SENSOR == ENABLED - calc_nav_pitch(); - if (nav_pitch < (long)takeoff_pitch) nav_pitch = (long)takeoff_pitch; -#else - nav_pitch = (long)((float)gps.ground_speed / (float)get(PARAM_TRIM_AIRSPEED) * (float)takeoff_pitch * 0.5); - nav_pitch = constrain(nav_pitch, 500l, (long)takeoff_pitch); -#endif - - - servo_out[CH_THROTTLE] = get(PARAM_THR_MAX); //TODO: Replace with THROTTLE_TAKEOFF or other method of controlling throttle - // What is the case for doing something else? Why wouldn't you want max throttle for TO? - // ****************************** - - break; - - case CMD_LAND: - calc_nav_roll(); - -#if AIRSPEED_SENSOR == ENABLED - calc_nav_pitch(); - calc_throttle(); -#else - calc_nav_pitch(); // calculate nav_pitch just to use for calc_throttle - calc_throttle(); // throttle based on altitude error - nav_pitch = landing_pitch; // pitch held constant -#endif - - if (land_complete) { - servo_out[CH_THROTTLE] = 0; - } - break; - - default: - hold_course = -1; - calc_nav_roll(); - calc_nav_pitch(); - calc_throttle(); - break; - } - }else{ - switch(control_mode){ - case RTL: - case LOITER: - hold_course = -1; - crash_checker(); - calc_nav_roll(); - calc_nav_pitch(); - calc_throttle(); - break; - - case FLY_BY_WIRE_A: - // fake Navigation output using sticks - nav_roll = ((radio_in[CH_ROLL] - radio_trim(CH_ROLL)) * - get(PARAM_LIM_ROLL) * reverse_roll) / 350; - nav_pitch = ((radio_in[CH_PITCH] - radio_trim(CH_PITCH)) * - 3500l * reverse_pitch) / 350; - nav_roll = constrain(nav_roll, -get(PARAM_LIM_ROLL), get(PARAM_LIM_ROLL)); - nav_pitch = constrain(nav_pitch, -3000, 3000); // trying to give more pitch authority - break; - - case FLY_BY_WIRE_B: - // fake Navigation output using sticks - // We use get(PARAM_PITCH_MIN) because its magnitude is - // normally greater than get(PARAM_get(PARAM_PITCH_MAX)) - nav_roll = ((radio_in[CH_ROLL] - radio_trim(CH_ROLL)) - * get(PARAM_LIM_ROLL) * reverse_roll) / 350; - altitude_error = ((radio_in[CH_PITCH] - radio_trim(CH_PITCH)) - * get(PARAM_LIM_PITCH_MIN) * -reverse_pitch) / 350; - nav_roll = constrain(nav_roll, -get(PARAM_LIM_ROLL), get(PARAM_LIM_ROLL)); - -#if AIRSPEED_SENSOR == ENABLED - airspeed_error = ((int)(get(PARAM_ARSPD_FBW_MAX) - - get(PARAM_ARSPD_FBW_MIN)) * - servo_out[CH_THROTTLE]) + - ((int)get(PARAM_ARSPD_FBW_MIN) * 100); - // Intermediate calculation - airspeed_error is just desired airspeed at this point - airspeed_energy_error = (long)(((long)airspeed_error * - (long)airspeed_error) - - ((long)airspeed * (long)airspeed))/20000; - //Changed 0.00005f * to / 20000 to avoid floating point calculation - airspeed_error = (airspeed_error - airspeed); -#endif - - calc_throttle(); - calc_nav_pitch(); - break; - - case STABILIZE: - nav_roll = 0; - nav_pitch = 0; - // throttle is passthrough - break; - - case CIRCLE: - // we have no GPS installed and have lost radio contact - // or we just want to fly around in a gentle circle w/o GPS - // ---------------------------------------------------- - nav_roll = get(PARAM_LIM_ROLL) / 3; - nav_pitch = 0; - - if (failsafe == true){ - servo_out[CH_THROTTLE] = get(PARAM_TRIM_THROTTLE); - } - break; - - case MANUAL: - // servo_out is for Sim control only - // --------------------------------- - servo_out[CH_ROLL] = reverse_roll * (radio_in[CH_ROLL] - radio_trim(CH_ROLL)) * 9; - servo_out[CH_PITCH] = reverse_pitch * (radio_in[CH_PITCH] - radio_trim(CH_PITCH)) * 9; - servo_out[CH_RUDDER] = reverse_rudder * (radio_in[CH_RUDDER] - radio_trim(CH_RUDDER)) * 9; - break; - //roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000 - - } - } -} diff --git a/Tools/ArduTracker/Attitude.pde b/Tools/ArduTracker/Attitude.pde deleted file mode 100644 index 9b85db028e..0000000000 --- a/Tools/ArduTracker/Attitude.pde +++ /dev/null @@ -1,380 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -//**************************************************************** -// Function that controls aileron/rudder, elevator, rudder (if 4 channel control) and throttle to produce desired attitude and airspeed. -//**************************************************************** - -void stabilize() -{ - float ch1_inf = 1.0f; - float ch2_inf = 1.0f; - float ch4_inf = 1.0f; -#if AIRSPEED_SENSOR == ENABLED - float speed_scaler = STANDARD_SPEED_SQUARED / (airspeed * airspeed); - speed_scaler = constrain(speed_scaler, 0.11f, 9.0f); -#endif -#if AIRSPEED_SENSOR == DISABLED - float speed_scaler; - if (servo_out[CH_THROTTLE] > 0) - speed_scaler = 0.5f + (THROTTLE_CRUISE / servo_out[CH_THROTTLE] / 2.0f); // First order taylor expansion of square root - // Should maybe be to the 2/7 power, but we aren't goint to implement that... - else - speed_scaler = 1.67f; - speed_scaler = constrain(speed_scaler, 0.6f, 1.67f); // This case is constrained tighter as we don't have real speed info -#endif - - - if(crash_timer > 0){ - nav_roll = 0; - } - - // For Testing Only - // roll_sensor = (radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 10; - // Serial.print(" roll_sensor "); - // Serial.print(roll_sensor,DEC); - - // Calculate dersired servo output for the roll - // --------------------------------------------- - servo_out[CH_ROLL] = pidServoRoll.get_pid((nav_roll - dcm.roll_sensor), deltaMiliSeconds, speed_scaler); - servo_out[CH_PITCH] = pidServoPitch.get_pid((nav_pitch + fabsf(dcm.roll_sensor * get(PARAM_KFF_PTCHCOMP)) - (dcm.pitch_sensor - get(PARAM_TRIM_PITCH))), deltaMiliSeconds, speed_scaler); - //Serial.print(" servo_out[CH_ROLL] "); - //Serial.print(servo_out[CH_ROLL],DEC); - - // Mix Stick input to allow users to override control surfaces - // ----------------------------------------------------------- - if ((control_mode < FLY_BY_WIRE_A) || (ENABLE_STICK_MIXING == 1 && control_mode > FLY_BY_WIRE_B)) { - - ch1_inf = (float)radio_in[CH_ROLL] - (float)radio_trim(CH_ROLL); - ch1_inf = fabsf(ch1_inf); - ch1_inf = min(ch1_inf, 400.0); - ch1_inf = ((400.0 - ch1_inf) /400.0); - - ch2_inf = (float)radio_in[CH_PITCH] - radio_trim(CH_PITCH); - ch2_inf = fabsf(ch2_inf); - ch2_inf = min(ch2_inf, 400.0); - ch2_inf = ((400.0 - ch2_inf) /400.0); - - // scale the sensor input based on the stick input - // ----------------------------------------------- - servo_out[CH_ROLL] *= ch1_inf; - servo_out[CH_PITCH] *= ch2_inf; - - // Mix in stick inputs - // ------------------- - servo_out[CH_ROLL] += reverse_roll * (radio_in[CH_ROLL] - radio_trim(CH_ROLL)) * 9; - servo_out[CH_PITCH] += reverse_pitch * (radio_in[CH_PITCH] - radio_trim(CH_PITCH)) * 9; - - //Serial.print(" servo_out[CH_ROLL] "); - //Serial.println(servo_out[CH_ROLL],DEC); - } - - // stick mixing performed for rudder for all cases including FBW unless disabled for higher modes - // important for steering on the ground during landing - // ----------------------------------------------- - if (control_mode <= FLY_BY_WIRE_B || ENABLE_STICK_MIXING == 1) { - ch4_inf = (float)radio_in[CH_RUDDER] - (float)radio_trim(CH_RUDDER); - ch4_inf = fabsf(ch4_inf); - ch4_inf = min(ch4_inf, 400.0); - ch4_inf = ((400.0 - ch4_inf) /400.0); - } - - // Apply output to Rudder - // ---------------------- - calc_nav_yaw(speed_scaler); - servo_out[CH_RUDDER] *= ch4_inf; - servo_out[CH_RUDDER] += reverse_rudder * (radio_in[CH_RUDDER] - radio_trim(CH_RUDDER)) * 9; - - // Call slew rate limiter if used - // ------------------------------ - //#if(ROLL_SLEW_LIMIT != 0) - // servo_out[CH_ROLL] = roll_slew_limit(servo_out[CH_ROLL]); - //#endif -} - -void crash_checker() -{ - if(dcm.pitch_sensor < -4500){ - crash_timer = 255; - } - if(crash_timer > 0) - crash_timer--; -} - - -#if AIRSPEED_SENSOR == DISABLED -void calc_throttle() -{ - int throttle_target = get(PARAM_TRIM_THROTTLE) + throttle_nudge; - - // no airspeed sensor, we use nav pitch to determine the proper throttle output - // AUTO, RTL, etc - // --------------------------------------------------------------------------- - if (nav_pitch >= 0) { - servo_out[CH_THROTTLE] = throttle_target + (get(PARAM_THR_MAX) - throttle_target) * nav_pitch / get(PARAM_LIM_PITCH_MAX); - } else { - servo_out[CH_THROTTLE] = throttle_target - (throttle_target - get(PARAM_THR_MIN)) * nav_pitch / get(PARAM_LIM_PITCH_MIN); - } - - servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], get(PARAM_THR_MIN), get(PARAM_THR_MAX)); -} -#endif - -#if AIRSPEED_SENSOR == ENABLED -void calc_throttle() -{ - // throttle control with airspeed compensation - // ------------------------------------------- - energy_error = airspeed_energy_error + (float)altitude_error * 0.098f; - - // positive energy errors make the throttle go higher - servo_out[CH_THROTTLE] = get(PARAM_TRIM_THROTTLE) + pidTeThrottle.get_pid(energy_error, dTnav); - servo_out[CH_THROTTLE] = max(servo_out[CH_THROTTLE], 0); - - // are we going too slow? up the throttle to get some more groundspeed - // move into PID loop in the future - // lower the contstant value to limit the effect : 50 = default - - //JASON - We really should change this to act on airspeed in this case. Let's visit about it.... - /*int gs_boost = 30 * (1.0 - ((float)gps.ground_speed / (float)airspeed_cruise)); - gs_boost = max(0,gs_boost); - servo_out[CH_THROTTLE] += gs_boost;*/ - servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], - get(PARAM_THR_MIN), get(PARAM_THR_MAX)); -} -#endif - - - -/***************************************** - * Calculate desired roll/pitch/yaw angles (in medium freq loop) - *****************************************/ - -// Yaw is separated into a function for future implementation of heading hold on rolling take-off -// ---------------------------------------------------------------------------------------- -void calc_nav_yaw(float speed_scaler) -{ -#if HIL_MODE != HIL_MODE_ATTITUDE - Vector3f temp = imu.get_accel(); - long error = -temp.y; - - // Control is a feedforward from the aileron control + a PID to coordinate the turn (drive y axis accel to zero) - servo_out[CH_RUDDER] = get(PARAM_KFF_RDDRMIX) * servo_out[CH_ROLL] + pidServoRudder.get_pid(error, deltaMiliSeconds, speed_scaler); -#else - servo_out[CH_RUDDER] = get(PARAM_KFF_RDDRMIX) * servo_out[CH_ROLL]; - // XXX probably need something here based on heading -#endif -} - - -void calc_nav_pitch() -{ - // Calculate the Pitch of the plane - // -------------------------------- -#if AIRSPEED_SENSOR == ENABLED - nav_pitch = -pidNavPitchAirspeed.get_pid(airspeed_error, dTnav); -#else - nav_pitch = pidNavPitchAltitude.get_pid(altitude_error, dTnav); -#endif - nav_pitch = constrain(nav_pitch, get(PARAM_LIM_PITCH_MIN), get(PARAM_LIM_PITCH_MAX)); -} - - -void calc_nav_roll() -{ - - // Adjust gain based on ground speed - We need lower nav gain going in to a headwind, etc. - // This does not make provisions for wind speed in excess of airframe speed - nav_gain_scaler = (float)gps.ground_speed / (STANDARD_SPEED * 100.0); - nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4); - - // negative error = left turn - // positive error = right turn - // Calculate the required roll of the plane - // ---------------------------------------- - nav_roll = pidNavRoll.get_pid(bearing_error, dTnav, nav_gain_scaler); //returns desired bank angle in degrees*100 - nav_roll = constrain(nav_roll,-get(PARAM_LIM_ROLL), get(PARAM_LIM_ROLL)); - -} - - -/***************************************** - * Roll servo slew limit - *****************************************/ -/* -float roll_slew_limit(float servo) -{ - static float last; - float temp = constrain(servo, last-ROLL_SLEW_LIMIT * deltaMiliSeconds/1000.f, last + ROLL_SLEW_LIMIT * deltaMiliSeconds/1000.f); - last = servo; - return temp; -}*/ - -/***************************************** - * Throttle slew limit - *****************************************/ -/*float throttle_slew_limit(float throttle) -{ - static float last; - float temp = constrain(throttle, last-THROTTLE_SLEW_LIMIT * deltaMiliSeconds/1000.f, last + THROTTLE_SLEW_LIMIT * deltaMiliSeconds/1000.f); - last = throttle; - return temp; -} -*/ - -// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. -// Keeps outdated data out of our calculations -void reset_I(void) -{ - pidNavRoll.reset_I(); - pidNavPitchAirspeed.reset_I(); - pidNavPitchAltitude.reset_I(); - pidTeThrottle.reset_I(); - pidAltitudeThrottle.reset_I(); -} - -/***************************************** -* Set the flight control servos based on the current calculated values -*****************************************/ -void set_servos_4(void) -{ - if(control_mode == MANUAL){ - // do a direct pass through of radio values - for(int y=0; y<4; y++) { - radio_out[y] = radio_in[y]; - } - - } else { - // Patch Antenna Control Code - float phi, theta; //,servo_phi, servo_theta; - float x1,x2,y1,y2,x,y,r,z; - - y1 = 110600*current_loc.lat/t7; - x1 = (PI/180)*6378137*(cosf(atanf(0.99664719f*tanf(current_loc.lat/t7*PI/180))))*(current_loc.lng/t7); - - y2 = 110600*trackVehicle_loc.lat/t7; - x2 = (PI/180)*6378137*(cosf(atanf(0.99664719f*tanf(current_loc.lat/t7*PI/180))))*(trackVehicle_loc.lng/t7); - - x = abs(x2 - x1); - y = abs(y2 - y1); - - r = sqrtf(x*x+y*y); - z = trackVehicle_loc.alt/100.0f - current_loc.alt; - - phi = (atanf(z/r)*180/PI); - theta = (atanf(x/y)*180/PI); - // Check to see which quadrant of the angle - - if (trackVehicle_loc.lat >= current_loc.lat && trackVehicle_loc.lng >= current_loc.lng) - { - theta = abs(theta); - } - else if (trackVehicle_loc.lat >= current_loc.lat && trackVehicle_loc.lng <= current_loc.lng) - { - theta = 360 - abs(theta); - } - else if (trackVehicle_loc.lat <= current_loc.lat && trackVehicle_loc.lng >= current_loc.lng) - { - theta = 180 - abs(theta); - } - else if (trackVehicle_loc.lat <= current_loc.lat && trackVehicle_loc.lng <= current_loc.lng) - { - theta = 180 + abs(theta); - } - - // Calibration of the top servo - //servo_phi = (91*phi + 7650)/9; - - // Calibration of the bottom servo - //servo_theta = (2*theta + 5940)/3; - - - Serial.print("target lat: "); Serial.println(current_loc.lat); - Serial.print("tracker lat: "); Serial.println(trackVehicle_loc.lat); - Serial.print("phi: "); Serial.println(phi); - Serial.print("theta: "); Serial.println(theta); - - // Outputing to the servos - servo_out[CH_ROLL] = 10000*phi/90.0f; - servo_out[CH_PITCH] = 10000*theta/360.0f; - servo_out[CH_RUDDER] = 0; - servo_out[CH_THROTTLE] = 0; - - radio_out[CH_ROLL] = radio_trim(CH_ROLL) + (reverse_roll * ((float)servo_out[CH_ROLL] * 0.11111)); - radio_out[CH_PITCH] = radio_trim(CH_PITCH) + (reverse_pitch * ((float)servo_out[CH_PITCH] * 0.11111)); - radio_out[CH_RUDDER] = 0; - radio_out[CH_THROTTLE] = 0; - - /* - if (mix_mode == 0){ - radio_out[CH_ROLL] = radio_trim(CH_ROLL) + (reverse_roll * ((float)servo_out[CH_ROLL] * 0.11111)); - radio_out[CH_PITCH] = radio_trim(CH_PITCH) + (reverse_pitch * ((float)servo_out[CH_PITCH] * 0.11111)); - radio_out[CH_RUDDER] = radio_trim(CH_RUDDER) + (reverse_rudder * ((float)servo_out[CH_RUDDER] * 0.11111)); - }else{ - //Elevon mode - float ch1; - float ch2; - ch1 = reverse_elevons * (servo_out[CH_PITCH] - servo_out[CH_ROLL]); - ch2 = servo_out[CH_PITCH] + servo_out[CH_ROLL]; - radio_out[CH_ROLL] = elevon1_trim + (reverse_ch1_elevon * (ch1 * 0.11111)); - radio_out[CH_PITCH] = elevon2_trim + (reverse_ch2_elevon * (ch2 * 0.11111)); - } - - #if THROTTLE_OUT == 0 - radio_out[CH_THROTTLE] = 0; - #endif - - - // convert 0 to 100% into PWM - servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], 0, 100); - radio_out[CH_THROTTLE] = servo_out[CH_THROTTLE] * ((radio_max(CH_THROTTLE) - radio_min(CH_THROTTLE)) / 100); - radio_out[CH_THROTTLE] += radio_min(CH_THROTTLE); - - //Radio_in: 1763 PWM output: 2000 Throttle: 78.7695999145 PWM Min: 1110 PWM Max: 1938 - - #if THROTTLE_REVERSE == 1 - radio_out[CH_THROTTLE] = radio_max(CH_THROTTLE) + radio_min(CH_THROTTLE) - radio_out[CH_THROTTLE]; - #endif - */ - } - - // send values to the PWM timers for output - // ---------------------------------------- - for(int y=0; y<4; y++) { - APM_RC.OutputCh(y, radio_out[y]); // send to Servos - } - } - -void demo_servos(byte i) { - - while(i > 0){ - gcs.send_text(SEVERITY_LOW,"Demo Servos!"); - APM_RC.OutputCh(1, 1400); - delay(400); - APM_RC.OutputCh(1, 1600); - delay(200); - APM_RC.OutputCh(1, 1500); - delay(400); - i--; - } -} - -int readOutputCh(unsigned char ch) -{ - int pwm; - switch(ch) - { - case 0: pwm = OCR5B; break; // ch0 - case 1: pwm = OCR5C; break; // ch1 - case 2: pwm = OCR1B; break; // ch2 - case 3: pwm = OCR1C; break; // ch3 - case 4: pwm = OCR4C; break; // ch4 - case 5: pwm = OCR4B; break; // ch5 - case 6: pwm = OCR3C; break; // ch6 - case 7: pwm = OCR3B; break; // ch7 - case 8: pwm = OCR5A; break; // ch8, PL3 - case 9: pwm = OCR1A; break; // ch9, PB5 - case 10: pwm = OCR3A; break; // ch10, PE3 - } - pwm >>= 1; // pwm / 2; - return pwm; -} diff --git a/Tools/ArduTracker/GCS.h b/Tools/ArduTracker/GCS.h deleted file mode 100644 index 1c11a1bacb..0000000000 --- a/Tools/ArduTracker/GCS.h +++ /dev/null @@ -1,266 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/// @file GCS.h -/// @brief Interface definition for the various Ground Control System protocols. - -#ifndef __GCS_H -#define __GCS_H - -#include -#include -#include -#include -#include -#include -#include - -/// -/// @class GCS -/// @brief Class describing the interface between the APM code -/// proper and the GCS implementation. -/// -/// GCS' are currently implemented inside the sketch and as such have -/// access to all global state. The sketch should not, however, call GCS -/// internal functions - all calls to the GCS should be routed through -/// this interface (or functions explicitly exposed by a subclass). -/// -class GCS_Class -{ -public: - - /// Startup initialisation. - /// - /// This routine performs any one-off initialisation required before - /// GCS messages are exchanged. - /// - /// @note The stream is expected to be set up and configured for the - /// correct bitrate before ::init is called. - /// - /// @note The stream is currently BetterStream so that we can use the _P - /// methods; this may change if Arduino adds them to Print. - /// - /// @param port The stream over which messages are exchanged. - /// - void init(BetterStream *port) { _port = port; } - - /// Update GCS state. - /// - /// This may involve checking for received bytes on the stream, - /// or sending additional periodic messages. - void update(void) {} - - /// Send a message with a single numeric parameter. - /// - /// This may be a standalone message, or the GCS driver may - /// have its own way of locating additional parameters to send. - /// - /// @param id ID of the message to send. - /// @param param Explicit message parameter. - /// - void send_message(uint8_t id, int32_t param = 0) {} - - /// Send a text message. - /// - /// @param severity A value describing the importance of the message. - /// @param str The text to be sent. - /// - void send_text(uint8_t severity, const char *str) {} - - /// Send acknowledgement for a message. - /// - /// @param id The message ID being acknowledged. - /// @param sum1 Checksum byte 1 from the message being acked. - /// @param sum2 Checksum byte 2 from the message being acked. - /// - void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) {} - - /// Emit an update of the "current" waypoints, often previous, current and - /// next. - /// - void print_current_waypoints() {} - - // - // The following interfaces are not currently implemented as their counterparts - // are not called in the mainline code. XXX ripe for re-specification. - // - - /// Send a text message with printf-style formatting. - /// - /// @param severity A value describing the importance of the message. - /// @param fmt The format string to send. - /// @param ... Additional arguments to the format string. - /// - // void send_message(uint8_t severity, const char *fmt, ...) {} - - /// Log a waypoint - /// - /// @param wp The waypoint to log. - /// @param index The index of the waypoint. - // void print_waypoint(struct Location *wp, uint8_t index) {} - - // test if frequency within range requested for loop - // used by data_stream_send - static bool freqLoopMatch(uint16_t freq, uint16_t freqMin, uint16_t freqMax) - { - if (freq != 0 && freq >= freqMin && freq < freqMax) return true; - else return false; - } - - // send streams which match frequency range - void data_stream_send(uint16_t freqMin, uint16_t freqMax); - -protected: - /// The stream we are communicating over - BetterStream *_port; -}; - -// -// GCS class definitions. -// -// These are here so that we can declare the GCS object early in the sketch -// and then reference it statically rather than via a pointer. -// - -/// -/// @class GCS_STANDARD -/// @brief The default APM GCS protocol -/// -#if GCS_PROTOCOL == GCS_PROTOCOL_STANDARD -class GCS_STANDARD : public GCS_Class -{ -public: - GCS_STANDARD(BinComm::MessageHandler GCS_MessageHandlers[]); - - void update(void); - void init(BetterStream *port); - void send_message(uint8_t id, uint32_t param = 0); - void send_text(uint8_t severity, const char *str); - void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2); - BinComm & getBinComm(void) { return _binComm; } -private: - BinComm _binComm; -}; -#endif // GCS_PROTOCOL_STANDARD - -/// -/// @class GCS_MAVLINK -/// @brief The mavlink protocol for qgroundcontrol -/// -#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK -class GCS_MAVLINK : public GCS_Class -{ -public: - GCS_MAVLINK(); - void update(void); - void init(BetterStream *port); - void send_message(uint8_t id, uint32_t param = 0); - void send_text(uint8_t severity, const char *str); - void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2); - void data_stream_send(uint16_t freqMin, uint16_t freqMax); -private: - void handleMessage(mavlink_message_t * msg); - mavlink_channel_t chan; - uint16_t packet_drops; - uint16_t rawSensorStreamRate; - uint16_t attitudeStreamRate; - uint16_t positionStreamRate; - uint16_t rawControllerStreamRate; - uint16_t rcStreamRate; - uint16_t extraStreamRate[3]; -}; -#endif // GCS_PROTOCOL_MAVLINK - -/// -/// @class GCS_LEGACY -/// @brief Support for the legacy Ardupilot GCS -/// -#if GCS_PROTOCOL == GCS_PROTOCOL_LEGACY -class GCS_LEGACY : public GCS_Class -{ -public: - void send_message(uint8_t id, uint32_t param = 0); - void send_text(uint8_t severity, const char *str); - -private: - void print_attitude(void); - void print_control_mode(void); - void print_position(void); - void print_waypoint(struct Location *cmd,byte index); -}; -#endif // GCS_PROTOTOL_LEGACY - -/// -/// @class GCS_DEBUGTERMINAL -/// @brief A GCS driver with an interactive management interface -/// -#if GCS_PROTOCOL == GCS_PROTOCOL_DEBUGTERMINAL -class GCS_DEBUGTERMINAL : public GCS_Class -{ -public: - GCS_DEBUGTERMINAL() : - report_command(1) - {} - - void update(void); - void send_message(uint8_t id, uint32_t param = 0); - void send_text(uint8_t severity, const char *str); - void print_current_waypoints(void); - void print_commands(); - void print_PID(long PID_error, long dt, float scaler, float derivative, float integrator, float last_error); - -private: - char gcsinputbuffer[120]; // read buffer - byte bufferidx; - - // Reporting flags - byte report_heartbeat; - byte report_attitude; - byte report_location; - byte report_command; - byte report_severity; - byte first_location; - byte report_cpu_load; - - void processgcsinput(char c); - void run_debugt_command(char *buf); - static int strmatch(char* s1, const char* s2); - static long readfloat(char* s, unsigned char* i); - void process_set_cmd(char *buffer, int bufferlen); - static int get_pid_offset_name(char *buffer, int *offset, unsigned char *length); - void print_rlocation(Location *wp); - void print_error(const char *msg); - void print_position(void); - void print_attitude(void); - void print_new_wp_info(); - void print_control_mode(void); - void print_tuning(void); - void print_command(struct Location *cmd, byte index); - void print_command_id(byte id); - void print_gain(unsigned char g); - void print_gains(); - void print_gain_keyword_error(); - void print_commands(unsigned char i1, unsigned char i2); -}; -#endif // GCS_PROTOCOL_DEBUGTERMINAL - -/// -/// @class GCS_XPLANE -/// @brief GCS driver for HIL operation with the X-plane simulator -/// -#if GCS_PROTOCOL == GCS_PROTOCOL_XPLANE -class GCS_XPLANE : public GCS_Class -{ -public: - void send_message(uint8_t id, uint32_t param = 0); - void send_text(uint8_t severity, const char *str); - void print_current_waypoints(void); - -private: - void print_control_mode(void); - void print_waypoint(struct Location *wp, uint8_t index); - void print_waypoints(void); -}; -#endif // GCS_PROTOCOL_XPLANE - - -#endif // __GCS_H diff --git a/Tools/ArduTracker/GCS_Ardupilot.pde b/Tools/ArduTracker/GCS_Ardupilot.pde deleted file mode 100644 index 2d2dab48e7..0000000000 --- a/Tools/ArduTracker/GCS_Ardupilot.pde +++ /dev/null @@ -1,157 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/// -/// @file GCS_Ardupilot.pde -/// @brief GCS driver for the legacy Ardupilot GCS protocol. -/// - -#if GCS_PROTOCOL == GCS_PROTOCOL_LEGACY - -/* -Message Prefixes -!!! Position Low rate telemetry -+++ Attitude High rate telemetry -### Mode Change in control mode -%%% Waypoint Current and previous waypoints -XXX Alert Text alert - NOTE: Alert message generation is not localized to a function -PPP IMU Performance Sent every 20 seconds for performance monitoring - -Message Suffix -*** All messages use this suffix -*/ - -void -GCS_LEGACY::send_text(uint8_t severity, const char *str) -{ - if(severity >= DEBUG_LEVEL){ - _port->print("MSG: "); - _port->println(str); - } -} - -void -GCS_LEGACY::send_message(uint8_t id, uint32_t param) -{ - switch(id) { - case MSG_ATTITUDE: // ** Attitude message - print_attitude(); - break; - case MSG_LOCATION: // ** Location/GPS message - print_position(); - break; - case MSG_MODE_CHANGE: - case MSG_HEARTBEAT: // ** Location/GPS message - print_control_mode(); - break; - case MSG_CPU_LOAD: - //TODO: replace with appropriate message - _port->printf_P(PSTR("MSG: load %ld%%\n"), param); - break; - //case MSG_PERF_REPORT: - // printPerfData(); - } -} - -void -GCS_LEGACY::print_attitude(void) -{ - _port->print("+++"); - _port->print("ASP:"); - _port->print((int)airspeed / 100, DEC); - _port->print(",THH:"); - _port->print(servo_out[CH_THROTTLE], DEC); - _port->print (",RLL:"); - _port->print(roll_sensor / 100, DEC); - _port->print (",PCH:"); - _port->print(pitch_sensor / 100, DEC); - _port->println(",***"); -} - -void -GCS_LEGACY::print_control_mode(void) -{ - switch (control_mode){ - case MANUAL: - _port->println("###MANUAL***"); - break; - case STABILIZE: - _port->println("###STABILIZE***"); - break; - case CIRCLE: - _port->println("###CIRCLE***"); - break; - case FLY_BY_WIRE_A: - _port->println("###FBW A***"); - break; - case FLY_BY_WIRE_B: - _port->println("###FBW B***"); - break; - case AUTO: - _port->println("###AUTO***"); - break; - case RTL: - _port->println("###RTL***"); - break; - case LOITER: - _port->println("###LOITER***"); - break; - case TAKEOFF: - _port->println("##TAKEOFF***"); - break; - case LAND: - _port->println("##LAND***"); - break; - } -} - -void -GCS_LEGACY::print_position(void) -{ - _port->print("!!!"); - _port->print("LAT:"); - _port->print(current_loc.lat/10,DEC); - _port->print(",LON:"); - _port->print(current_loc.lng/10,DEC); //wp_current_lat - _port->print(",SPD:"); - _port->print(gps.ground_speed/100,DEC); - _port->print(",CRT:"); - _port->print(climb_rate,DEC); - _port->print(",ALT:"); - _port->print(current_loc.alt/100,DEC); - _port->print(",ALH:"); - _port->print(next_WP.alt/100,DEC); - _port->print(",CRS:"); - _port->print(yaw_sensor/100,DEC); - _port->print(",BER:"); - _port->print(target_bearing/100,DEC); - _port->print(",WPN:"); - _port->print(get(PARAM_WP_INDEX),DEC);//Actually is the waypoint. - _port->print(",DST:"); - _port->print(wp_distance,DEC); - _port->print(",BTV:"); - _port->print(battery_voltage,DEC); - _port->print(",RSP:"); - _port->print(servo_out[CH_ROLL]/100,DEC); - _port->print(",TOW:"); - _port->print(gps.time); - _port->println(",***"); -} - -void -GCS_LEGACY::print_waypoint(struct Location *cmd,byte index) -{ - _port->print("command #: "); - _port->print(index, DEC); - _port->print(" id: "); - _port->print(cmd->id,DEC); - _port->print(" p1: "); - _port->print(cmd->p1,DEC); - _port->print(" p2: "); - _port->print(cmd->alt,DEC); - _port->print(" p3: "); - _port->print(cmd->lat,DEC); - _port->print(" p4: "); - _port->println(cmd->lng,DEC); -} - -#endif // GCS_PROTOCOL_LEGACY diff --git a/Tools/ArduTracker/GCS_DebugTerminal.pde b/Tools/ArduTracker/GCS_DebugTerminal.pde deleted file mode 100644 index f1b09fe63d..0000000000 --- a/Tools/ArduTracker/GCS_DebugTerminal.pde +++ /dev/null @@ -1,1622 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/* This GCS protocol sends text-based information over the GCS port -*/ - -#if GCS_PROTOCOL == GCS_PROTOCOL_DEBUGTERMINAL - -#define ERR(a) ((DEBUGTERMINAL_VERBOSE)>0?(PSTR(a)):(0)) - -void -GCS_DEBUGTERMINAL::update() -{ - byte numc, i, c; - - numc = _port->available(); - for (i=0;iread(); - processgcsinput(c); - } -} - -void -GCS_DEBUGTERMINAL::processgcsinput(char c) -{ - if (c==8) { - //Received a backspace character; move the buffer index backwards - if (bufferidx > 0) bufferidx--; - } else if (c==10) { - //Received a linefeed; do nothing - } else if (c==13) { - //Received a carriage return; process command - gcsinputbuffer[bufferidx] = 0; - run_debugt_command(gcsinputbuffer); - bufferidx = 0; - } else { - gcsinputbuffer[bufferidx++] = c; - if (bufferidx >= sizeof(gcsinputbuffer)) bufferidx = 0; - } -} - -void -GCS_DEBUGTERMINAL::run_debugt_command(char *buf) -{ - BetterStream *port = _port; - - //*********** Ignore comments *********** - if (strmatch(buf, PSTR("//"))) { - - //*********** Process 'show' commands *********** - } else if (strmatch(buf, PSTR("show "))) { - if (strmatch(buf+5, PSTR("heartbeat"))) - report_heartbeat = 1; - else if (strmatch(buf+5, PSTR("attitude"))) - report_attitude = 1; - else if (strmatch(buf+5, PSTR("location"))) - report_location = 1; - else if (strmatch(buf+5, PSTR("command"))) - report_command = 1; - else if (strmatch(buf+5, PSTR("cpuload"))) - report_cpu_load = 1; - else if (strmatch(buf+5, PSTR("severity"))) - report_severity = atoi(buf+14); - else - print_error(ERR("USAGE: show {heartbeat|attitude|location|command|severity }")); - - //*********** Process 'hide' commands *********** - } else if (strmatch(buf, PSTR("hide "))) { - if (strmatch(buf+5, PSTR("heartbeat"))) - report_heartbeat = 0; - else if (strmatch(buf+5, PSTR("attitude"))) - report_attitude = 0; - else if (strmatch(buf+5, PSTR("location"))) - report_location = 0; - else if (strmatch(buf+5, PSTR("command"))) - report_command = 0; - else if (strmatch(buf+5, PSTR("cpuload"))) - report_cpu_load = 0; - else if (strmatch(buf+5, PSTR("all"))) { - report_heartbeat = 0; - report_attitude = 0; - report_location = 0; - report_command = 0; - report_cpu_load = 0; - } else - print_error(ERR("USAGE: hide {heartbeat|attitude|location|command|all}")); - - //*********** Process 'copy' command *********** - } else if (strmatch(buf, PSTR("copy "))) { - //------- copy cmd ------- - if (strmatch(buf+5, PSTR("cmd "))) { - unsigned char i = 9, index1, index2; - while (buf[i] != 0) { - i++; - if (buf[i] == ' ') break; - } - if (buf[i] == ' ') { - buf[i] = 0; - index1 = atoi(buf+9); - index2 = atoi(buf+i+1); - Location temp = get_wp_with_index(index1); - set_wp_with_index(temp, index2); - port->print_P(PSTR("Copied command index ")); port->print(index1,DEC); port->print_P(PSTR(" to ")); port->println(index2,DEC); - } else { - print_error(ERR("USAGE: copy cmd ")); - } - } - - //*********** Process 'echo' command *********** - } else if (strmatch(buf, PSTR("echo "))) { - port->println(buf+5); - - //*********** Process 'groundstart' command *********** - } else if (strmatch(buf, PSTR("groundstart"))) { - startup_ground(); - - //*********** Process 'inithome' command *********** - } else if (strmatch(buf, PSTR("inithome"))) { - init_home(); - port->println_P(PSTR("Home set.")); - - //------- k -? ------- - } else if (strmatch(buf, PSTR("k -?"))) { - print_error(ERR("USAGE: {print|set} {k{p|i|d}|imax} {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt}")); - print_error(ERR("USAGE: {print|set} kff {pitchcomp|ruddermix|pitchtothrottle}")); - - //*********** Process 'print' commands *********** - } else if (strmatch(buf, PSTR("print "))) { - //------- print airspeedtrim ------- - if (strmatch(buf+6, PSTR("airspeedtrim"))) { - port->printf_P("Trim airspeed = %f\n", (float)get(PARAM_TRIM_AIRSPEED)/100); - - //------- print airspeednudge ------- - } else if (strmatch(buf+6, PSTR("airspeednudge"))) { - port->printf_P("Airspeed nudge = %f\n", (float)airspeed_nudge/100); - - //------- print altitude ------- - } else if (strmatch(buf+6, PSTR("altitude"))) { - recalc_climb_rate(); - port->printf_P(PSTR("Altitude:\n" - " Pressure: %.2fm\n" - " GPS: %.2fm\n" - " Mix ratio: %.3f\n" - " Mix: %.2fm\n" - " Above home: %.1fm\n" - " Climb rate: %.2fm/s\n"), - (float)press_alt / 100, - (float)gps.altitude / 100, - get(PARAM_ALT_MIX), - (((1.0 - get(PARAM_ALT_MIX)) * gps.altitude) + (get(PARAM_ALT_MIX) * press_alt)) / 100, - (float)get_altitude_above_home()/100, - (float)climb_rate/100); - - //------- print attitude ------- - } else if (strmatch(buf+6, PSTR("attitude"))) { - print_attitude(); - - //------- print commands[ -] ------- - } else if (strmatch(buf+6, PSTR("commands"))) { - unsigned char dash, index1, index2; - for (dash=14; dashprint_P(PSTR("EEPROM read: ")); - for (i=0; i<6; i++) { - port->print_P(PSTR("Ch ")); port->print(i,DEC); port->print_P(PSTR(" = ")); port->print(get(uint8_param_t(PARAM_FLIGHT_MODE_1+i)),DEC); port->print_P(PSTR(", ")); - } - port->println(" "); - - //------- print holdalt ------- - } else if (strmatch(buf+6, PSTR("holdalt"))) { - port->print_P(PSTR("Altitude above home set to ")); port->println(get(PARAM_ALT_HOLD_HOME),2); - - //------- print imax {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} ------- - } else if (strmatch(buf+6, PSTR("imax "))) { - int i; - unsigned char j; - if (get_pid_offset_name(buf+11, &i, &j)) { - port->print_P(PSTR("Integrator maximum for ")); - port->print(buf+9); - port->print_P(PSTR(" = ")); - port->println(pid_index[i]->imax(),DEC); - } else - print_gain_keyword_error(); - - //------- print kp {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} ------- - } else if (strmatch(buf+6, PSTR("kp "))) { - int i; - unsigned char j; - if (get_pid_offset_name(buf+9, &i, &j)) { - port->print_P(PSTR("P gain for ")); - port->print(buf+9); - port->print_P(PSTR(" = ")); - port->println(pid_index[i]->kP(),DEC); - } else - print_gain_keyword_error(); - - //------- print ki {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} ------- - } else if (strmatch(buf+6, PSTR("ki "))) { - int i; - unsigned char j; - if (get_pid_offset_name(buf+9, &i, &j)) { - port->print_P(PSTR("I gain for ")); - port->print(buf+9); - port->print_P(PSTR(" = ")); - port->println(pid_index[i]->kI(),DEC); - } else - print_gain_keyword_error(); - - //------- print kd {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} ------- - } else if (strmatch(buf+6, PSTR("kd "))) { - int i; - unsigned char j; - if (get_pid_offset_name(buf+9, &i, &j)) { - port->print_P(PSTR("D gain for ")); - port->print(buf+9); - port->print_P(PSTR(" = ")); - port->println(pid_index[i]->kD(),DEC); - } else - print_gain_keyword_error(); - - //------- print kff {pitchcomp|ruddermix|pitchtothrottle} ------- - } else if (strmatch(buf+6, PSTR("kff "))) { - if (strmatch(buf+10, PSTR("pitchcomp"))) { - port->print_P(PSTR("FF gain for pitchcomp = ")); - port->println(get(PARAM_KFF_PTCHCOMP),DEC); - } else if (strmatch(buf+10, PSTR("ruddermix"))) { - port->print_P(PSTR("FF gain for ruddermix = ")); - port->println(get(PARAM_KFF_RDDRMIX),DEC); - } else if (strmatch(buf+10, PSTR("pitchtothrottle"))) { - port->print_P(PSTR("FF gain for pitchtothrottle = ")); - port->println(get(PARAM_KFF_PTCH2THR),DEC); - /*} else if (strmatch(buf+10, PSTR("throttletopitch"))) { - port->print_P(PSTR("FF gain for throttletopitch = ")); - port->println(kff[CASE_T_TO_P],DEC);*/ - } else - print_gain_keyword_error(); - - //------- print location ------- - } else if (strmatch(buf+6, PSTR("location"))) { - print_position(); - - //------- print navrolllimit ------- - } else if (strmatch(buf+6, PSTR("navrolllimit"))) { - port->print_P(PSTR("Nav roll limit = ")); port->println((float)get(PARAM_LIM_ROLL)/100,2); - - //------- print navsettings ------- - } else if (strmatch(buf+6, PSTR("navsettings"))) { - port->printf_P(PSTR("Navigation settings:\n" -#if AIRSPEED_SENSOR == ENABLED - " Cruise airspeed: %.2f\n" -#else - " Cruise throttle: %d\n" -#endif - " Hold altitude above home: %ld\n" - " Loiter radius: %d\n" - " Waypoint radius: %d\n"), -#if AIRSPEED_SENSOR == ENABLED - (float)get(PARAM_TRIM_AIRSPEED) / 100, -#else - get(PARAM_TRIM_THROTTLE), -#endif - get(PARAM_ALT_HOLD_HOME), - get(PARAM_LOITER_RADIUS), - get(PARAM_WP_RADIUS)); - - //------- print pitchmax ------- - } else if (strmatch(buf+6, PSTR("pitchmax"))) { - port->print_P(PSTR("Maximum pitch = ")); port->println((float)get(PARAM_LIM_PITCH_MAX)/100,2); - - //------- print pitchmin ------- - } else if (strmatch(buf+6, PSTR("pitchmin"))) { - port->print_P(PSTR("Minimum pitch = ")); port->println((float)get(PARAM_LIM_PITCH_MIN)/100,2); - - //------- print pitchtarget ------- - } else if (strmatch(buf+6, PSTR("pitchtarget"))) { - port->print_P(PSTR("Target pitch = ")); port->println((float)get(PARAM_TRIM_PITCH)/100,2); - -#if HIL_MODE != HIL_MODE_ATTITUDE - //------- print pressure ------- - } else if (strmatch(buf+6, PSTR("pressure"))) { - port->println_P(PSTR("BMP085 pressure sensor:")); - port->print_P(PSTR(" Temperature: ")); port->println(pitot.Temp,DEC); - port->print_P(PSTR(" Pressure: ")); port->println(pitot.Press,DEC); -#endif - - //------- print rlocation home ------- - } else if (strmatch(buf+6, PSTR("rlocation home"))) { - print_rlocation(&home); - - //------- print rlocation ------- - //(implication is "relative to next waypoint") - } else if (strmatch(buf+6, PSTR("rlocation"))) { - print_rlocation(&next_WP); - - //------- print speed ------- - } else if (strmatch(buf+6, PSTR("speed"))) { - port->println_P(PSTR("Speed:")); - port->print_P(PSTR(" Ground: ")); port->println((float)gps.ground_speed/100.0,2); -#if AIRSPEED_SENSOR == ENABLED - port->print_P(PSTR(" Air: ")); port->println((float)airspeed/100.0,2); - port->print_P(PSTR(" Cruise: ")); port->println((float)get(PARAM_TRIM_AIRSPEED)/100.0,2); -#endif - - //------- print throttlecruise ------- - } else if (strmatch(buf+6, PSTR("throttlecruise"))) { - port->print_P(PSTR("Throttle cruise = ")); port->print(get(PARAM_TRIM_THROTTLE),DEC); port->println_P(PSTR("%")); - - //------- print throttlemax ------- - } else if (strmatch(buf+6, PSTR("throttlemax"))) { - port->print_P(PSTR("Throttle max = ")); port->print(get(PARAM_THR_MAX),DEC); port->println_P(PSTR("%")); - - //------- print throttlemin ------- - } else if (strmatch(buf+6, PSTR("throttlemin"))) { - port->print_P(PSTR("Throttle min = ")); port->print(get(PARAM_THR_MIN),DEC); port->println_P(PSTR("%")); - - //------- print tuning ------- - } else if (strmatch(buf+6, PSTR("tuning"))) { - print_tuning(); - - } else - print_error(ERR("USAGE: print {altitude|attitude|commands|ctrlmode|curwaypts|flightmodes|k -?|kp|ki|kd|kff|location|navsettings|pressure|rlocation [home]|speed|tuning|}")); - - //*********** Process 'reset' commands *********** - } else if (strmatch(buf, PSTR("reset "))) { - //------- reset commands ------- - if (strmatch(buf+6, PSTR("commands"))) { - reload_commands(); - port->println_P(PSTR("Commands reloaded.")); - } else - print_error(ERR("USAGE: reset commands")); - - //*********** Process 'rtl' command *********** - } else if (strmatch(buf, PSTR("rtl"))) { - return_to_launch(); - port->println_P(PSTR("Returning to launch...")); - - //*********** Process 'set' commands *********** - } else if (strmatch(buf, PSTR("set "))) { - //------- set cmd ------- - if (strmatch(buf+4, PSTR("cmd "))) { - process_set_cmd(buf+8, bufferidx-8); - - //------- set cmdcount ------- - } else if (strmatch(buf+4, PSTR("cmdcount "))) { - set(PARAM_WP_TOTAL, atoi(buf+13)); - port->print_P(PSTR("PARAM_WP_TOTAL = ")); port->println(get(PARAM_WP_TOTAL),DEC); - - //------- set cmdindex ------- - } else if (strmatch(buf+4, PSTR("cmdindex "))) { - set(PARAM_WP_INDEX, atoi(buf+13)); - decrement_WP_index(); - next_command = get_wp_with_index(get(PARAM_WP_INDEX)+1); - port->print_P(PSTR("Command set to index ")); port->print(get(PARAM_WP_INDEX),DEC); - if (next_command.id >= 0x10 && next_command.id <= 0x1F) { //TODO: create a function the defines what type of command each command ID is - command_must_index = 0; - port->println_P(PSTR(" (must)")); - } else if (next_command.id >= 0x20 && next_command.id <= 0x2F) { - command_may_index = 0; - port->println_P(PSTR(" (may)")); - } else - port->println_P(PSTR(" (now)")); - - next_command.id = CMD_BLANK; - if (get(PARAM_WP_INDEX) > get(PARAM_WP_TOTAL)) { - set(PARAM_WP_TOTAL, get(PARAM_WP_INDEX)); - port->print_P(PSTR(" The total number of commands is now ")); - port->println(get(PARAM_WP_TOTAL),DEC); - } - next_WP = current_loc; - update_commands(); - - //------- set cruise ------- - } else if (strmatch(buf+4, PSTR("cruise "))) { - unsigned char j = 4+7; -#if AIRSPEED_SENSOR == 1 - port->print_P(PSTR("airspeed_cruise changed from ")); - port->print((float)get(PARAM_TRIM_AIRSPEED)/100,2); - port->print_P(PSTR(" to ")); - set(PARAM_TRIM_AIRSPEED, (int)(readfloat(buf, &j)/100000)); - set(PARAM_TRIM_AIRSPEED, constrain(get(PARAM_TRIM_AIRSPEED),0,9000)); //TODO: constrain minimum as stall speed, maximum as Vne - port->println(((float)get(PARAM_TRIM_AIRSPEED))/100,2); -#else - port->print_P(PSTR("throttle_cruise changed from ")); - port->print(get(PARAM_TRIM_THROTTLE),DEC); - port->print_P(PSTR(" to ")); - set(PARAM_TRIM_THROTTLE, constrain((int)(readfloat(buf, &j)/10000000),0,200)); - port->println(get(PARAM_TRIM_THROTTLE),DEC); -#endif - //save_user_configs(); - - //------- set holdalt ------- - } else if (strmatch(buf+4, PSTR("holdalt "))) { - int tempalt = atoi(buf+12)*100; - set(PARAM_ALT_HOLD_HOME, (float)tempalt); //save_alt_to_hold((int32_t)tempalt); - port->println_P(PSTR("Hold altitude above home set.")); - - //------- set imax {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} ------- - } else if (strmatch(buf+4, PSTR("imax "))) { - int i; - unsigned char j = 0; - if (get_pid_offset_name(buf+9, &i, &j)) { - buf[9+j] = 0; - port->print_P(PSTR("Integrator maximum for ")); - port->print(buf+9); - port->print_P(PSTR(" changed from ")); - port->print(pid_index[i]->imax(),DEC); - port->print_P(PSTR(" to ")); - pid_index[i]->imax((float)atoi(buf+j)); - pid_index[i]->save_gains(); - port->println(pid_index[i]->imax(),DEC); - } else - print_error(ERR("ERROR: Did not recognize keyword; type set k -? for more information")); - - //------- set kp {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} ------- - } else if (strmatch(buf+4, PSTR("kp "))) { - int i; - unsigned char j = 0; - if (get_pid_offset_name(buf+7, &i, &j)) { - buf[7+j] = 0; - port->print_P(PSTR("P gain for ")); - port->print(buf+7); - port->print_P(PSTR(" changed from ")); - port->print(pid_index[i]->kP(),DEC); - port->print_P(PSTR(" to ")); - j += 7+1; - pid_index[i]->kP(((float)readfloat(buf, &j))/10000000); - pid_index[i]->save_gains(); - port->println(pid_index[i]->kP(),DEC); - } else - print_gain_keyword_error(); - - //------- set ki {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} ------- - } else if (strmatch(buf+4, PSTR("ki "))) { - int i; - unsigned char j = 0; - if (get_pid_offset_name(buf+7, &i, &j)) { - buf[7+j] = 0; - port->print_P(PSTR("I gain for ")); - port->print(buf+7); - port->print_P(PSTR(" changed from ")); - port->print(pid_index[i]->kI(),DEC); - port->print_P(PSTR(" to ")); - j += 7+1; - pid_index[i]->kI(((float)readfloat(buf, &j))/10000000); - pid_index[i]->save_gains(); - port->println(pid_index[i]->kI(),DEC); - } else - print_gain_keyword_error(); - - //------- set kd {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} ------- - } else if (strmatch(buf+4, PSTR("kd "))) { - int i; - unsigned char j = 0; - if (get_pid_offset_name(buf+7, &i, &j)) { - buf[7+j] = 0; - port->print_P(PSTR("D gain for ")); - port->print(buf+7); - port->print_P(PSTR(" changed from ")); - port->print(pid_index[i]->kD(),DEC); - port->print_P(PSTR(" to ")); - j += 7+1; - pid_index[i]->kD(((float)readfloat(buf, &j))/10000000); - pid_index[i]->save_gains(); - port->println(pid_index[i]->kD(),DEC); - } else - print_gain_keyword_error(); - - //------- set kff {pitchcomp|ruddermix|pitchtothrottle} ------- - } else if (strmatch(buf+4, PSTR("kff "))) { - unsigned char j = 0; - if (strmatch(buf+8, PSTR("pitchcomp"))) { - buf[8+9] = 0; - port->print_P(PSTR("FF gain for ")); - port->print(buf+8); - port->print_P(PSTR(" changed from ")); - port->print(get(PARAM_KFF_PTCHCOMP),DEC); - port->print_P(PSTR(" to ")); - j = 8+9+1; - set(PARAM_KFF_PTCHCOMP, ((float)readfloat(buf, &j))/10000000); - port->println(get(PARAM_KFF_PTCHCOMP),DEC); - } else if (strmatch(buf+8, PSTR("ruddermix"))) { - buf[8+9] = 0; - port->print_P(PSTR("FF gain for ")); - port->print(buf+8); - port->print_P(PSTR(" changed from ")); - port->print(get(PARAM_KFF_RDDRMIX),DEC); - port->print_P(PSTR(" to ")); - j = 8+9+1; - set(PARAM_KFF_RDDRMIX, ((float)readfloat(buf, &j))/10000000); - port->println(get(PARAM_KFF_RDDRMIX),DEC); - } else if (strmatch(buf+8, PSTR("pitchtothrottle"))) { - buf[8+15] = 0; - port->print_P(PSTR("FF gain for ")); - port->print(buf+8); - port->print_P(PSTR(" changed from ")); - port->print(get(PARAM_KFF_PTCH2THR),DEC); - port->print_P(PSTR(" to ")); - j = 8+15+1; - set(PARAM_KFF_PTCH2THR, ((float)readfloat(buf, &j))/10000000); - port->println(get(PARAM_KFF_PTCH2THR),DEC); - /*} else if (strmatch(buf+8, PSTR("throttletopitch"))) { - buf[8+15] = 0; - port->print_P(PSTR("FF gain for ")); - port->print(buf+8); - port->print_P(PSTR(" changed from ")); - port->print(kff[CASE_T_TO_P],DEC); - port->print_P(PSTR(" to ")); - j = 8+15+1; - kff[CASE_T_TO_P] = ((float)readfloat(buf, &j))/10000000; - port->println(kff[CASE_T_TO_P],DEC);*/ - } else - print_gain_keyword_error(); - - //------- set loiterradius ------- - } else if (strmatch(buf+4, PSTR("loiterradius "))) { - set(PARAM_LOITER_RADIUS, atoi(buf+17)); - port->print_P(PSTR("Set loiter radius to ")); port->print(get(PARAM_LOITER_RADIUS),DEC); port->println_P(PSTR(" meters")); - - //------- set navrolllimit ------- - } else if (strmatch(buf+4, PSTR("navrolllimit "))) { - unsigned char j = 17; - port->print_P(PSTR("Nav roll limit changed from ")); port->print((float)get(PARAM_LIM_ROLL)/100,2); - port->print_P(PSTR(" to ")); - set(PARAM_LIM_ROLL, readfloat(buf, &j)/100000); - port->println((float)get(PARAM_LIM_ROLL)/100,2); - - //------- set pitchmax ------- - } else if (strmatch(buf+4, PSTR("pitchmax "))) { - unsigned char j = 13; - port->print_P(PSTR("Maximum pitch changed from ")); port->print((float)get(PARAM_LIM_PITCH_MAX)/100,2); - port->print_P(PSTR(" to ")); - set(PARAM_LIM_PITCH_MAX, readfloat(buf, &j)/100000); - port->println((float)get(PARAM_LIM_PITCH_MAX)/100,2); - - //------- set pitchmin ------- - } else if (strmatch(buf+4, PSTR("pitchmin "))) { - unsigned char j = 13; - port->print_P(PSTR("Minimum pitch changed from ")); port->print((float)get(PARAM_LIM_PITCH_MIN)/100,2); - port->print_P(PSTR(" to ")); - set(PARAM_LIM_PITCH_MIN, readfloat(buf, &j)/100000); - port->println((float)get(PARAM_LIM_PITCH_MIN)/100,2); - - //------- set pitchtarget ------- - } else if (strmatch(buf+4, PSTR("pitchtarget "))) { - unsigned char j = 16; - port->print_P(PSTR("Pitch target changed from ")); port->print((float)get(PARAM_TRIM_PITCH)/100,2); - port->print_P(PSTR(" to ")); - set(PARAM_TRIM_PITCH, readfloat(buf, &j)/100000); - port->println((float)get(PARAM_TRIM_PITCH)/100,2); - - //------- set rcin ------- - } else if (strmatch(buf+4, PSTR("rcin"))) { - unsigned char index = buf[8]-'1'; - if (index < 8) { - radio_in[index] = atoi(buf+9); - } else - print_error(ERR("USAGE: set rcin ")); - - //------- set rcout ------- - } else if (strmatch(buf+4, PSTR("rcout"))) { - unsigned char index = buf[9]-'1'; - if (index < 8) { - radio_out[index] = atoi(buf+10); - APM_RC.OutputCh(index, radio_out[index]); - } else - print_error(ERR("USAGE: set rcout ")); - - //------- set relay ------- - } else if (strmatch(buf+4, PSTR("relay "))) { - unsigned char newvalue = atoi(buf+10); - if (newvalue == 0) { - relay_off(); - port->println_P(PSTR("Relay turned off")); - } else if (newvalue == 1) { - relay_on(); - port->println_P(PSTR("Relay turned on")); - } else { - relay_toggle(); - port->println_P(PSTR("Relay toggled")); - } - - //------- set throttlecruise ------- - } else if (strmatch(buf+4, PSTR("throttlecruise "))) { - port->print_P(PSTR("Throttle cruise changed from ")); port->print(get(PARAM_TRIM_THROTTLE),DEC); port->print_P(PSTR("% to ")); - set(PARAM_TRIM_THROTTLE,atoi(buf+19)); - port->print(get(PARAM_TRIM_THROTTLE),DEC); port->println_P(PSTR("%")); - - //------- set throttlefailsafe ------- - } else if (strmatch(buf+4, PSTR("throttlefailsafe "))) { - set(PARAM_THR_FAILSAFE, atoi(buf+21)); - /*if (get(PARAM_THROTTLE_FS) == 0) - throttle_FS_enabled = 0; - else - throttle_FS_enabled = 1;*/ - //save_user_configs(); - port->println_P(PSTR("Throttle failsafe adjusted.")); - - //------- set throttlefailsafeaction ------- - } else if (strmatch(buf+4, PSTR("throttlefailsafeaction "))) { - set(PARAM_THR_FS_ACTION, atoi(buf+27)); - //save_user_configs(); - - //------- set throttlemax ------- - } else if (strmatch(buf+4, PSTR("throttlemax "))) { - port->print_P(PSTR("Throttle max changed from ")); port->print(get(PARAM_THR_MAX),DEC); port->print_P(PSTR("% to ")); - set(PARAM_THR_MAX, atoi(buf+16)); - port->print(get(PARAM_THR_MAX),DEC); port->println_P(PSTR("%")); - //save_user_configs(); - - //------- set throttlemin ------- - } else if (strmatch(buf+4, PSTR("throttlemin "))) { - port->print_P(PSTR("Throttle min changed from ")); port->print(get(PARAM_THR_MIN),DEC); port->print_P(PSTR("% to ")); - set(PARAM_THR_MIN, atoi(buf+16)); - port->print(get(PARAM_THR_MIN),DEC); port->println_P(PSTR("%")); - //save_user_configs(); - - //------- set wpradius ------- - } else if (strmatch(buf+4, PSTR("wpradius "))) { - set(PARAM_WP_RADIUS, atoi(buf+13)); - port->print_P(PSTR("Set waypoint radius to ")); port->print(get(PARAM_WP_RADIUS),DEC); port->println_P(PSTR(" meters")); - - //------- set xtrackentryangle ------- - } else if (strmatch(buf+4, PSTR("xtrackentryangle "))) { - unsigned char j = 21; - set(PARAM_XTRACK_ANGLE, readfloat(buf, &j)/100000); - port->print_P(PSTR("Crosstrack entry angle set to ")); port->println((float)get(PARAM_XTRACK_ANGLE)/100,2); - - //------- set xtrackgain ------- - } else if (strmatch(buf+4, PSTR("xtrackgain "))) { - unsigned char j = 15; - set(PARAM_XTRACK_GAIN, ((float)readfloat(buf, &j))/10000000); - port->print_P(PSTR("Crosstrack gain set to ")); port->println(get(PARAM_XTRACK_GAIN),2); - - } else - print_error(ERR("USAGE: set {cmd|cmdcount|cmdindex|cruise|holdalt|kp|ki|kd|kff|loiterradius|rcin|rcout|wpradius}")); - - //*********** Process 'status' commands *********** - } else if (strmatch(buf, PSTR("status "))) { - //------- status altitude ------- - if (strmatch(buf+7, PSTR("altitude"))) { - port->printf_P(PSTR("Altitude:\n" - " altitude_error = %.2fm\n" - " target_altitude = %.2fm\n" - " next_WP.alt = %.2fm\n" - " wp_distance = %ldm\n" - " wp_totalDistance = %ldm\n" - " offset_altitude = %.2fm\n"), - (float)altitude_error/100, - (float)target_altitude/100, - (float)next_WP.alt/100, - wp_distance, - wp_totalDistance, - (float)offset_altitude/100); - - //------- status climb ------- - } else if (strmatch(buf+7, PSTR("climb"))) { - print_climb_debug_info(); - - //------- status control ------- - } else if (strmatch(buf+7, PSTR("control"))) { - port->printf_P(PSTR("Control status:\n" - " Roll: nav= %.2f, servo_out= %.2f\n" // XXX float? - " Pitch: nav= %.2f, servo_out= %.2f\n" // XXX float? - " Throttle: nav= %d, servo_out= %d\n"), - (float)nav_roll / 100, (float)servo_out[CH_ROLL] / 100, - (float)nav_pitch / 100, (float)servo_out[CH_PITCH] / 100, - get(PARAM_TRIM_THROTTLE), servo_out[CH_THROTTLE]); - - //------- status gps ------- - } else if (strmatch(buf+7, PSTR("gps"))) { - port->printf_P(PSTR("GPS status:\n" - " Fix: %S (%d)\n" - " Satellites: %d\n" - " Fix count: %d\n"), - (gps.fix ? PSTR("Valid") : PSTR("INVALID")), - (int)gps.fix, - (int)gps.num_sats, - gps_fix_count); - - //------- status landing ------- - } else if (strmatch(buf+7, PSTR("landing"))) { - port->printf_P(PSTR("Landing status:" - " land_complete = %d\n" - " landing_pitch = %d\n" - " nav_pitch = %ld\n" - " airspeed_cruise = %d\n" - " throttle_cruise = %d\n" - " hold_course = %ld\n" - " nav_bearing = %ld\n" - " bearing_error = %ld\n"), - (int)land_complete, - landing_pitch, - nav_pitch, - get(PARAM_TRIM_AIRSPEED), - get(PARAM_TRIM_THROTTLE), - hold_course, - nav_bearing, - bearing_error); - - //------- status loiter ------- - } else if (strmatch(buf+7, PSTR("loiter"))) { - port->printf_P(PSTR("Loiter status:" - " Loiter radius: %d\n" - " Progress: %d/%d\n" // XXX original had non-ASCII units char - " Time: %ldms/%dms\n"), - get(PARAM_LOITER_RADIUS), - loiter_sum, loiter_total, - loiter_time, loiter_time_max); - - //------- status navigation ------- - } else if (strmatch(buf+7, PSTR("navigation"))) { - port->printf_P(PSTR("Navigation status:\n" - " From <%.6f, %.6f, %.1f>: "), - (float)prev_WP.lat/10000000.0, - (float)prev_WP.lng/10000000.0, - (float)prev_WP.alt/100.0); - print_rlocation(&prev_WP); - port->printf_P(PSTR(" At <%.6f, %.6f, %.1f>\n"), - (float)current_loc.lat/10000000.0, - (float)current_loc.lng/10000000.0, - (float)current_loc.alt/100.0); - port->printf_P(PSTR(" To <%.6f, %.6f, %.1f>: "), - (float)next_WP.lat/10000000.0, - (float)next_WP.lng/10000000.0, - (float)next_WP.alt/100.0); - print_rlocation(&next_WP); - port->printf_P(PSTR(" Distance: %.1f%% %ldm / %ldm; %.1f vertically\n"), - 100.0*(float)(wp_totalDistance-wp_distance)/(float)wp_totalDistance, - wp_totalDistance-wp_distance, - wp_totalDistance, - (float)altitude_error/100.0); - port->printf_P(PSTR(" Nav bearing: %.2f; error = %.2f\n"), - (float)nav_bearing/100.0, - (float)bearing_error/100.0); - port->printf_P(PSTR(" Ground course: %.1f (current), %.1f (target)\n"), - (float)gps.ground_course/100.0, - (float)target_bearing/100.0); - if (hold_course >= 0) { - port->print_P(PSTR(" HOLD COURSE: ")); port->println(hold_course/100.0,2); - } - - //------- status navpitch ------- - } else if (strmatch(buf+7, PSTR("navpitch"))) { -#if AIRSPEED_SENSOR == ENABLED - port->printf_P(PSTR(">>> nav_pitch = PID[airspeed_error (%.2f) = airspeed_cruise (%.2f) - airspeed (%.2f)]\n"), - (float)airspeed_error / 100, - (float)get(PARAM_TRIM_AIRSPEED) / 100, - (float)airspeed / 100); -#else - port->printf_P(PSTR(">>> nav_pitch = PID[altitude_error (%ld) = target_altitude (%ld) - current_loc.alt (%ld)]\n"), - altitude_error, - target_altitude, - current_loc.alt); -#endif - port->printf_P(PSTR("nav_pitch (%.2f) - pitch_sensor (%.2f) + pitch_comp (%.2f) = %.2f\n"), - (float)nav_pitch / 100, - (float)dcm.pitch_sensor / 100, - fabsf(dcm.roll_sensor * get(PARAM_KFF_PTCHCOMP)) / 100, - (float)(nav_pitch-dcm.pitch_sensor + fabsf(dcm.roll_sensor * get(PARAM_KFF_PTCHCOMP))) / 100); - port->printf_P(PSTR("servo_out[CH_PITCH] (%.2f) = PID[nav_pitch + pitch_comp - pitch_sensor]"), - (float)servo_out[CH_PITCH] / 100); - - //------- status navroll ------- - } else if (strmatch(buf+7, PSTR("navroll"))) { - print_rlocation(&next_WP); - port->printf_P(PSTR("bearing_error (%ld) = nav_bearing (%ld) - yaw_sensor (%ld)\n" - "nav_roll (%ld) - roll_sensor (%ld) = %ld\n" - "servo_out[CH_ROLL] = %d\n"), - bearing_error, nav_bearing, dcm.yaw_sensor, - nav_roll, dcm.roll_sensor, nav_roll - dcm.roll_sensor, - servo_out[CH_ROLL]); - - //------- status pid {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} ------- - } else if (strmatch(buf+7, PSTR("pid "))) { - int i; - unsigned char j; - if (get_pid_offset_name(buf+11, &i, &j)) { - port->print(buf+11); - port->print_P(PSTR(": ")); - display_PID = i; //The next time a PID calculation is performed on this channel, the print_PID routine in this GCS will be called - } else - print_error(ERR("ERROR: Did not recognize keyword")); - - //------- status rcinputch ------- - } else if (strmatch(buf+7, PSTR("rcinputch"))) { - unsigned char i; - port->println_P(PSTR("RC hardware input:")); - for (i=0; i<8; i++) { - port->print_P(PSTR(" Ch")); - port->print(i+1,DEC); - port->print_P(PSTR(": ")); - port->println(APM_RC.InputCh(i)); - } - - //------- status rcin ------- - } else if (strmatch(buf+7, PSTR("rcin"))) { - unsigned char i; - port->println_P(PSTR("RC software input:")); - for (i=0; i<8; i++) { - port->print_P(PSTR(" Ch")); - port->print(i+1,DEC); - port->print_P(PSTR(": ")); - port->println(radio_in[i]); - } - - //------- status rcout ------- - } else if (strmatch(buf+7, PSTR("rcout"))) { - unsigned char i; - port->println_P(PSTR("RC software output:")); - for (i=0; i<8; i++) { - port->print_P(PSTR(" Ch")); - port->print(i+1,DEC); - port->print_P(PSTR(": ")); - port->println(radio_out[i]); - } - - //------- status rcpwm ------- - } else if (strmatch(buf+7, PSTR("rcpwm"))) { - unsigned char i; - port->println_P(PSTR("RC hardware output:")); - for (i=0; i<8; i++) { - port->print_P(PSTR(" Ch")); - port->print(i+1,DEC); - port->print_P(PSTR(": ")); - port->println(readOutputCh(i)); - } - - //------- status rctrim ------- - } else if (strmatch(buf+7, PSTR("rctrim"))) { - unsigned char i; - port->println_P(PSTR("RC trim:")); - for (i=0; i<8; i++) { - port->print_P(PSTR(" Ch")); - port->print(i+1,DEC); - port->print_P(PSTR(": ")); - port->println(radio_trim(i)); - } - port->print_P(PSTR(" elevon1_trim = ")); port->println(elevon1_trim,DEC); - port->print_P(PSTR(" elevon2_trim = ")); port->println(elevon2_trim,DEC); - - //------- status rc ------- - } else if (strmatch(buf+7, PSTR("rc"))) { - unsigned char i; - port->println_P(PSTR("RC:\tCh\tHWin\tSWtrim\tSWin\tServo\tSWout\tHWout\t")); - for (i=0; i<8; i++) { - // XXX might benefit from field widths, since some terminals tab badly - port->printf_P(PSTR("\t%u\t%u\t%u\t%u\t%.2f\t%d\t%d\n"), - (unsigned int)(i + 1), - APM_RC.InputCh(i), - radio_trim(i), - radio_in[i], - (float)servo_out[i] / 100, - radio_out[i], - readOutputCh(i)); - } - - //------- status system ------- - } else if (strmatch(buf+7, PSTR("system"))) { - port->printf_P(PSTR("System status:" - " Ground start: %S (%d)\n" - " Home: %SSET (%d)\n"), - (ground_start ? PSTR("YES") : PSTR("NO")), (int)ground_start, - (home_is_set ? PSTR("") : PSTR("NOT ")), (int)home_is_set); - - //------- status takeoff ------- - /*} else if (strmatch(buf+7, PSTR("takeoff"))) { - port->println_P(PSTR("Takeoff status:")); - port->print_P(PSTR(" takeoff in progress: ")); - if (takeoff_in_progress) - port->println_P(PSTR("YES")); - else { - port->print_P(PSTR("NO; trigger = ")); - port->print(toss_trigger,DEC); - port->print_P(PSTR(", current = ")); - Vector3f temp = ardupilotDCM.get_accels(); - port->println(abs(temp.y),1); - } - port->printf_P(PSTR(" takeoff_pitch = %.2f\n" - " scaler = %.3f\n" - " nav_pitch = %.2f\n" - " throttle = %d\n" - " hold_course = %.2f\n" - " nav_bearing = %ld\n" - " bearing_error = %ld\n" - " current_loc.alt = %ld\n" - " home.alt + takeoff_altitude = %ld"), - (float)takeoff_pitch / 100, - (float)airspeed / (float)airspeed_cruise, - (float)nav_pitch / 100, - servo_out[CH_THROTTLE], - (float)hold_course / 100, - nav_bearing, - bearing_error, - current_loc.alt, - home.alt + takeoff_altitude);*/ - - //------- status telemetry ------- - } else if (strmatch(buf+7, PSTR("telemetry"))) { - port->printf_P(PSTR("Telemetry status:\n" - " %S heartbeat\n" - " %S location\n" - " %S attitude\n" - " %S command\n" - " Severity report level %d\n"), - (report_heartbeat ? PSTR("Show") : PSTR("Hide")), - (report_location ? PSTR("Show") : PSTR("Hide")), - (report_attitude ? PSTR("Show") : PSTR("Hide")), - (report_command ? PSTR("Show") : PSTR("Hide")), - (int)report_severity); - - //------- status throttle ------- - } else if (strmatch(buf+7, PSTR("throttle"))) { -#if AIRSPEED_SENSOR == ENABLED - port->printf_P(PSTR(">>> airspeed_energy_error (%ld) = 0.5 * (airspeed_cruise (%.2f)^2 - airspeed (%.2f)^2)\n" - "energy_error (%ld) = airspeed_energy_error (%ld) + altitude_error*0.098 (%ld)\n" - "servo_out[CH_THROTTLE] (%d) = PID[energy_error]\n"), - airspeed_energy_error, (float)get(PARAM_TRIM_AIRSPEED) / 100, (float)airspeed / 100, - energy_error, airspeed_energy_error, (long)((float)altitude_error * 0.098), - servo_out[CH_THROTTLE]); -#else - port->printf_P(PSTR("altitude_error (%ld) = target_altitude (%ld) - current_loc.alt (%ld)\n" - "servo_out[CH_THROTTLE] (%d) = PID[altitude_error]\n"), - altitude_error, target_altitude, current_loc.alt, - servo_out[CH_THROTTLE]); -#endif - - //------- status waypoints ------- - } else if (strmatch(buf+7, PSTR("waypoints"))) { - port->printf_P(PSTR("Waypoints status:\n" - " Distance: %ld/%ld\n" - " Index: %d/%d\n" - " Next: %d\n"), - wp_distance, wp_totalDistance, - (int)get(PARAM_WP_INDEX), (int)get(PARAM_WP_TOTAL), - (int)next_wp_index); - - } else if (strmatch(buf+7, PSTR("james"))) { - port->println_P(PSTR("James is a monkey")); - } else { - print_error(ERR("USAGE: status {control|gps|landing|loiter|mixing|navigation|navpitch|navroll|rc|rcinputch|rcin|rcout|rcpwm|rctrim|system|takeoff|telemetry|throttle|waypoints}")); - } - - } else { - print_error(ERR("USAGE: {echo |groundstart|inithome|show|hide|print|status|set|reset|rtl}")); - print_error(ERR("Type -? for specific usage guidance")); - } -} - -/* strmatch compares two strings and returns 1 if they match and 0 if they don't - s2 must be stored in program memory (via PSTR) rather than RAM (like standard strings) - s1 must be at least as long as s2 for a valid match - if s1 is longer than s2, then only the beginning of s1 (the length of s2) must match s2 for a valid match */ -int -GCS_DEBUGTERMINAL::strmatch(char* s1, const char* s2) -{ - int i = 0; - char c1 = s1[0], c2 = pgm_read_byte(s2); - - while (c1 != 0 && c2 != 0) { - if (c1 < c2) - return 0; - if (c1 > c2) - return 0; - i++; - c1 = s1[i]; - c2 = pgm_read_byte(s2+i); - } - - if (c2==0) - return 1; - else - return 0; -} - -/* readfloat parses a string written as a float starting at the offset in *i and updates *i as it parses - numbers are multiplied by 10,000,000 and decimals beyond 7 places are discarded - parsing is terminated with either a space or a null, other characters are ignored */ -long -GCS_DEBUGTERMINAL::readfloat(char* s, unsigned char* i) -{ - long result = 0, multiplier = 0; - char c, neg = 0; - - for (;;(*i)++) { - c = s[*i]; - if (c == ' ') - break; - else if (c == 0) - break; - else if (c == '-') - neg = 1-neg; - else if (c == '.') { - result *= 10000000; - multiplier = 1000000; - } else if (c >= '0' && c <= '9') { - if (multiplier == 0) - result = (result*10) + c-'0'; - else { - result += (c-'0')*multiplier; - multiplier /= 10; - } - } - } - - if (multiplier == 0) - result *= 10000000; - - if (neg) - return -result; - else - return result; -} - -/* process_set_cmd processing the parameters of a 'set cmd' command and takes the appropriate action - *buffer is the buffer containing the parameters of the command; it should start after the space after 'set cmd' - bufferlen is the length of the buffer; the routine will stop looking for parameters after the offset index reaches this value -*/ -#define SETPARAM_NONE (0) -#define SETPARAM_ID (1) -#define SETPARAM_P1 (2) -#define SETPARAM_LAT (3) -#define SETPARAM_LNG (4) -#define SETPARAM_ALT (5) -#define SETPARAM_P2 (6) -#define SETPARAM_P3 (7) -#define SETPARAM_P4 (8) - -void -GCS_DEBUGTERMINAL::process_set_cmd(char *buffer, int bufferlen) -{ - BetterStream *port = _port; - unsigned char i, j, err=1, setparam=SETPARAM_NONE; - unsigned char cmdindex=0, p1=0, cmdid; - long lat, lng, alt; - Location temp; - - //Parse the command index - for (i=0; i= '0' && buffer[i] <= '9') - cmdindex = (cmdindex*10) + buffer[i]-'0'; - else - break; - - if (buffer[i] == ' ') { - //Find the end of the command-type string - i++; - for (j=i; j SETPARAM_NONE) { - //Process new parameter value - i = j+1; - if (setparam == SETPARAM_ALT || setparam == SETPARAM_LAT || setparam == SETPARAM_LNG) { - lat = readfloat(buffer, &i); - } else { - unsigned char k; - for (k=i; i= '0' && buffer[k] <= '9')) - break; - buffer[k] = 0; - lat = atol(buffer+i); - i = k; - } - - temp = get_wp_with_index(cmdindex); - if (setparam == SETPARAM_ID) - temp.id = lat; - else if (setparam == SETPARAM_P1) - temp.p1 = lat; - else if (setparam == SETPARAM_ALT) - temp.alt = lat/100000; - else if (setparam == SETPARAM_LAT) - temp.lat = lat; - else if (setparam == SETPARAM_LNG) - temp.lng = lat; - else if (setparam == SETPARAM_P2) - temp.alt = lat; - else if (setparam == SETPARAM_P3) - temp.lat = lat; - else if (setparam == SETPARAM_P4) - temp.lng = lat; - cmdid = temp.id; - p1 = temp.p1; - lat = temp.lat; - lng = temp.lng; - alt = temp.alt; - err = 0; - } else { - //Process param 1 - for (i=j+1; i= '0' && buffer[i] <= '9') - p1 = (p1*10) + buffer[i]-'0'; - else - break; - } - - if (buffer[i] == ' ') { - //Process altitude - i++; - if (strmatch(buffer+i, PSTR("here"))) { - lat = gps.latitude; - lng = gps.longitude; - alt = get_altitude_above_home(); //GPS.altitude; - err = 0; - } else { - alt = readfloat(buffer, &i)/100000; - - if (buffer[i] == ' ') { - //Process latitude - i++; - lat = readfloat(buffer, &i); - if (strmatch(buffer+i, PSTR("here"))) { - lat = gps.latitude; - lng = gps.longitude; - err = 0; - } else { - if (buffer[i] == ' ') { - //Process longitude - i++; - lng = readfloat(buffer, &i); - - //TODO: add other command special cases here - if (cmdid == CMD_LAND_OPTIONS) { - temp.p1 = p1; - temp.alt = alt; - temp.lat = lat / 10000000; - temp.lng = lng / 10000000; - } - - err = 0; - } else - print_error(ERR("Error processing set cmd: Could not find longitude parameter")); - } - } else - print_error(ERR("Error processing set cmd: Could not find latitude parameter")); - } - } else - print_error(ERR("Error processing set cmd: Could not find altitude parameter")); - } - } else - print_error(ERR("Error processing set cmd: Could not find command type")); - } else - print_error(ERR("Error processing set cmd: Could not find command index")); - - if (err == 0) { - temp.id = cmdid; - temp.p1 = p1; - temp.lat = lat; - temp.lng = lng; - temp.alt = alt; - - if (cmdindex >= get(PARAM_WP_TOTAL)) { - set(PARAM_WP_TOTAL, cmdindex+1); - } - set_wp_with_index(temp, cmdindex); - - port->printf_P(PSTR("Set command %d to %d with p1=%d, lat=%ld, lng=%ld, alt=%ld\n"), - (int)cmdindex, (int)temp.id, (int)temp.p1, temp.lat, temp.lng, temp.alt); - } -} - -/* get_pid_offset_name matches a string expressed in *buffer with a pid keyword and returns the k-array - gain offset in *offset, and the length of that string expression in *length. If a good keyword - match is found, 1 is returned; 0 otherwise -*/ -int -GCS_DEBUGTERMINAL::get_pid_offset_name(char *buffer, int *offset, unsigned char *length) -{ - if (strmatch(buffer, PSTR("servoroll"))) { - *length += 9; *offset = CASE_SERVO_ROLL; - } else if (strmatch(buffer, PSTR("servopitch"))) { - *length += 10; *offset = CASE_SERVO_PITCH; - } else if (strmatch(buffer, PSTR("servorudder"))) { - *length += 11; *offset = CASE_SERVO_RUDDER; - } else if (strmatch(buffer, PSTR("navroll"))) { - *length += 7; *offset = CASE_NAV_ROLL; - } else if (strmatch(buffer, PSTR("navpitchasp"))) { - *length += 11; *offset = CASE_NAV_PITCH_ASP; - } else if (strmatch(buffer, PSTR("navpitchalt"))) { - *length += 11; *offset = CASE_NAV_PITCH_ALT; - } else if (strmatch(buffer, PSTR("throttlete"))) { - *length += 10; *offset = CASE_TE_THROTTLE; - } else if (strmatch(buffer, PSTR("throttlealt"))) { - *length += 11; *offset = CASE_ALT_THROTTLE; - } else { - return 0; - } - - return 1; -} - - -/* print_rlocation prints the relative location of the specified waypoint from the plane in easy-to-read cartesian format -*/ -void -GCS_DEBUGTERMINAL::print_rlocation(Location *wp) -{ - //float x, y; - //y = (float)(wp->lat - current_loc.lat) * 0.0111194927f; - //x = (float)(wp->lng - current_loc.lng) * cosf(radians(current_loc.lat)) * 0.0111194927f; - BetterStream *port = _port; - int x, y; - y = (wp->lat - current_loc.lat) * 0.0111194927f; - x = (wp->lng - current_loc.lng) * cosf(radians(current_loc.lat)) * 0.0111194927f; - port->printf_P(PSTR("dP = <%d%c, %d%c, %.1f> meters\n"), - abs(y), (y >= 0 ? 'N' : 'S'), - abs(x), (x >= 0 ? 'E' : 'W'), - (float)(wp->alt - current_loc.alt) / 100); -} - -/* print_error prints an error message if the user sends an invalid command -*/ -void -GCS_DEBUGTERMINAL::print_error(const char *msg) -{ - BetterStream *port = _port; - - if (msg == 0) - port->println_P(PSTR("ERROR: Invalid command")); - else - port->println_P(msg); -} - - -void -GCS_DEBUGTERMINAL::send_text(uint8_t severity, const char *str) -{ - BetterStream *port = _port; - - if(severity >= DEBUG_LEVEL){ - port->print_P(PSTR("MSG: ")); - port->println(str); - } -} - -void -GCS_DEBUGTERMINAL::send_message(uint8_t id, uint32_t param) -{ - switch(id) { - case MSG_MODE_CHANGE: - print_control_mode(); - break; - - case MSG_HEARTBEAT: - if (report_heartbeat) - print_control_mode(); - break; - - case MSG_ATTITUDE: - if (report_attitude) - print_attitude(); - break; - - case MSG_LOCATION: - if (report_location) - print_position(); - if (first_location == 0) { - send_text(0,"First location received"); - first_location = 1; - } - break; - - case MSG_CPU_LOAD: - if (report_cpu_load) { - _port->printf_P(PSTR("MSG: load %ld%%\n"), param); - } - break; - - case MSG_COMMAND_LIST: - struct Location cmd = get_wp_with_index(param); - print_command(&cmd, param); - break; - } -} - -void -GCS_DEBUGTERMINAL::print_current_waypoints() -{ - _port->printf_P(PSTR("Current waypoints:" - " Prev:\t%ld,\t%ld\t%ld\n" - " Cur: \t%ld,\t%ld\t%ld\n" - " Next:%d\t%ld,\t%ld\t%ld\n"), - prev_WP.lat, prev_WP.lng, prev_WP.alt, - current_loc.lat, current_loc.lng, current_loc.alt, - (int)get(PARAM_WP_INDEX), next_WP.lat, next_WP.lng, next_WP.alt); -} - -void -GCS_DEBUGTERMINAL::print_position(void) -{ - recalc_climb_rate(); - _port->printf_P(PSTR("Pos: %ld, %ld, %ldcm, %ldcm/s GS, %d cm/s AS, %d cm above home, %d? climb, %ldm from wp, %d%% throttle, %ld alt err\n"), - current_loc.lat, current_loc.lng, current_loc.alt, - gps.ground_speed, - airspeed, - get_altitude_above_home(), - climb_rate, - wp_distance, - get(PARAM_TRIM_THROTTLE), - altitude_error); -} - -void -GCS_DEBUGTERMINAL::print_attitude(void) -{ - _port->printf_P(PSTR("Att: %u roll_in, %u pitch in, %u throttle_in, " - "%ld roll_sensor, %ld pitch_sensor, " - "%ld ground_course, %ld target_bearing, " - "%ld nav_roll, %d loiter_sum, " - "%d roll servo_out, %d pitch_servo_out\n"), - radio_in[CH_ROLL], radio_in[CH_PITCH], radio_in[CH_THROTTLE], - dcm.roll_sensor, dcm.pitch_sensor, - gps.ground_course, target_bearing, - nav_roll, loiter_sum, - servo_out[CH_ROLL], servo_out[CH_PITCH]); -} - -// required by Groundstation to plot lateral tracking course -void -GCS_DEBUGTERMINAL::print_new_wp_info() -{ - _port->printf_P(PSTR("New waypt (%d): %ld, %ld, %ldm -> " - "%ld, %ld, %ldm; " - "%ldm dist, %u roll trim, %u pitch trim\n"), - (int)get(PARAM_WP_INDEX), - prev_WP.lat, prev_WP.lng, prev_WP.alt, - next_WP.lat, next_WP.lng, next_WP.alt, - wp_totalDistance, - radio_trim(CH_ROLL), radio_trim(CH_PITCH)); -} - -void -GCS_DEBUGTERMINAL::print_control_mode(void) -{ - BetterStream *port = _port; - - switch (control_mode){ - case MANUAL: - port->println_P(PSTR("##MANUAL")); - break; - case STABILIZE: - port->println_P(PSTR("##STABILIZE")); - break; - case FLY_BY_WIRE_A: - port->println_P(PSTR("##FBW A")); - break; - case FLY_BY_WIRE_B: - port->println_P(PSTR("##FBW B")); - break; - case AUTO: - port->println_P(PSTR("##AUTO")); - break; - case RTL: - port->println_P(PSTR("##RTL")); - break; - case LOITER: - port->println_P(PSTR("##LOITER")); - break; - case 98: - port->println_P(PSTR("##Air Start Complete")); - break; - case 99: - port->println_P(PSTR("##Ground Start Complete")); - break; - } -} - -void -GCS_DEBUGTERMINAL::print_tuning(void) -{ - _port->printf_P(PSTR("TUN:%d, %ld, %ld, %d, %ld, %ld\n"), - servo_out[CH_ROLL], nav_roll / 100, dcm.roll_sensor / 100, - servo_out[CH_PITCH], nav_pitch / 100, dcm.pitch_sensor / 100); -} - -void -GCS_DEBUGTERMINAL::print_command_id(byte id) -{ - BetterStream *port = _port; - - switch (id) { - // Command IDs - Must - case CMD_BLANK: port->print_P(PSTR("CMD_BLANK")); break; - case CMD_WAYPOINT: port->print_P(PSTR("waypoint")); break; - case CMD_LOITER: port->print_P(PSTR("loiter")); break; - case CMD_LOITER_N_TURNS: port->print_P(PSTR("loiternturns")); break; - case CMD_LOITER_TIME: port->print_P(PSTR("loitertime")); break; - case CMD_RTL: port->print_P(PSTR("rtl")); break; - case CMD_LAND: port->print_P(PSTR("land")); break; - case CMD_TAKEOFF: port->print_P(PSTR("takeoff")); break; - - // Command IDs - May - case CMD_DELAY: port->print_P(PSTR("delay")); break; - case CMD_CLIMB: port->print_P(PSTR("climb")); break; - case CMD_LAND_OPTIONS: port->print_P(PSTR("landoptions")); break; - - // Command IDs - Now - case CMD_RESET_INDEX: port->print_P(PSTR("resetindex")); break; - case CMD_GOTO_INDEX: port->print_P(PSTR("CMD_GOTO_INDEX")); break; - case CMD_GETVAR_INDEX: port->print_P(PSTR("CMD_GETVAR_INDEX")); break; - case CMD_SENDVAR_INDEX: port->print_P(PSTR("CMD_SENDVAR_INDEX")); break; - case CMD_TELEMETRY: port->print_P(PSTR("CMD_TELEMETRY")); break; - - case CMD_THROTTLE_CRUISE: port->print_P(PSTR("throttlecruise")); break; - case CMD_AIRSPEED_CRUISE: port->print_P(PSTR("CMD_AIRSPEED_CRUISE")); break; - case CMD_RESET_HOME: port->print_P(PSTR("resethome")); break; - - case CMD_KP_GAIN: port->print_P(PSTR("CMD_KP_GAIN")); break; - case CMD_KI_GAIN: port->print_P(PSTR("CMD_KI_GAIN")); break; - case CMD_KD_GAIN: port->print_P(PSTR("CMD_KD_GAIN")); break; - case CMD_KI_MAX: port->print_P(PSTR("CMD_KI_MAX")); break; - case CMD_KFF_GAIN: port->print_P(PSTR("CMD_KFF_GAIN")); break; - - case CMD_RADIO_TRIM: port->print_P(PSTR("CMD_RADIO_TRIM")); break; - case CMD_RADIO_MAX: port->print_P(PSTR("CMD_RADIO_MAX")); break; - case CMD_RADIO_MIN: port->print_P(PSTR("CMD_RADIO_MIN")); break; - case CMD_ELEVON_TRIM: port->print_P(PSTR("CMD_ELEVON_TRIM")); break; - - case CMD_INDEX: port->print_P(PSTR("index")); break; - case CMD_REPEAT: port->print_P(PSTR("CMD_REPEAT")); break; - case CMD_RELAY: port->print_P(PSTR("relay")); break; - case CMD_SERVO: port->print_P(PSTR("servo")); break; - - default: port->print(id,DEC); - } -} - -void -GCS_DEBUGTERMINAL::print_command(struct Location *cmd,byte index) -{ - BetterStream *port = _port; - - port->print_P(PSTR(" command #: ")); - port->print(index, DEC); - port->print_P(PSTR(" id: ")); - print_command_id(cmd->id); - - port->print_P(PSTR(" p1: ")); - port->print(cmd->p1,DEC); - port->print_P(PSTR(" p2/alt: ")); - port->print(cmd->alt,DEC); - port->print_P(PSTR(" p3/lat: ")); - port->print(cmd->lat,DEC); - port->print_P(PSTR(" p4/lng: ")); - port->println(cmd->lng,DEC); -} - -void -GCS_DEBUGTERMINAL::print_commands() -{ - print_commands(1, get(PARAM_WP_TOTAL)); -} - -void -GCS_DEBUGTERMINAL::print_commands(unsigned char i1, unsigned char i2) -{ - BetterStream *port = _port; - - port->println_P(PSTR("Commands in memory:")); - port->print_P(PSTR(" ")); - port->print(get(PARAM_WP_TOTAL), DEC); - port->println_P(PSTR(" commands total")); - // create a location struct to hold the temp Waypoints for printing - //Location tmp; - port->println_P(PSTR(" Home: ")); - struct Location cmd = get_wp_with_index(0); - print_command(&cmd, 0); - port->println_P(PSTR(" Commands: ")); - - for (int i=i1; i<=i2; i++){ - cmd = get_wp_with_index(i); - print_command(&cmd, i); - delay(10); - } -} - -void -GCS_DEBUGTERMINAL::print_gains() -{ - BetterStream *port = _port; - unsigned char i; - - port->println_P(PSTR("PID gains \tP \tI \tD \tIMax)")); - port->print_P(PSTR("Servo roll \t")); print_gain(CASE_SERVO_ROLL); - port->print_P(PSTR("Servo pitch \t")); print_gain(CASE_SERVO_PITCH); - port->print_P(PSTR("Servo yaw \t")); print_gain(CASE_SERVO_RUDDER); - port->print_P(PSTR("Nav roll \t")); print_gain(CASE_NAV_ROLL); - - port->print_P(PSTR("Nav pitch \t")); - if (AIRSPEED_SENSOR) - print_gain(CASE_NAV_PITCH_ASP); - else - print_gain(CASE_NAV_PITCH_ALT); - - port->print_P(PSTR("Nav throttle\t")); - if (AIRSPEED_SENSOR) - print_gain(CASE_TE_THROTTLE); - else - print_gain(CASE_ALT_THROTTLE); - - port->println_P(PSTR("Feed-forward gains")); - port->print_P(PSTR("Pitch compensation\t")); port->println(get(PARAM_KFF_PTCHCOMP),3); - port->print_P(PSTR("Rudder mix \t")); port->println(get(PARAM_KFF_RDDRMIX),3); - port->print_P(PSTR("Pitch to throttle \t")); port->println(get(PARAM_KFF_PTCH2THR),3); - //port->print_P(PSTR("Throttle to pitch \t")); port->println(kff[CASE_T_TO_P],3); -} - -void -GCS_DEBUGTERMINAL::print_gain(unsigned char g) -{ - BetterStream *port = _port; - - port->print(pid_index[g]->kP(),3); - port->print_P(PSTR("\t")); - port->print(pid_index[g]->kI(),3); - port->print_P(PSTR("\t")); - port->print(pid_index[g]->kD(),3); - port->print_P(PSTR("\t")); - port->println(pid_index[g]->imax(),DEC); -} - -void -GCS_DEBUGTERMINAL::print_gain_keyword_error() -{ - print_error(ERR("ERROR: Did not recognize keyword; type k -? for more information")); -} - -void -GCS_DEBUGTERMINAL::print_PID(long PID_error, long dt, float scaler, float derivative, float integrator, float last_error) -{ - BetterStream *port = _port; - - port->print_P(PSTR("P = ")); port->print(pid_index[display_PID]->kP() * scaler * (float)PID_error,2); - port->print_P(PSTR(",\tI = ")); port->print(integrator,2); - port->print_P(PSTR(",\tD = ")); port->print(pid_index[display_PID]->kD() * scaler * derivative,2); - port->print_P(PSTR("\terrors = {")); port->print(PID_error,DEC); - port->print_P(PSTR(", ")); port->print(last_error,DEC); - port->print_P(PSTR("} with dt = ")); port->println(dt,DEC); - - display_PID = -1; -} - -#endif // GCS_PROTOCOL_DEBUGTERMINAL diff --git a/Tools/ArduTracker/GCS_Mavlink.pde b/Tools/ArduTracker/GCS_Mavlink.pde deleted file mode 100644 index 4eda246f17..0000000000 --- a/Tools/ArduTracker/GCS_Mavlink.pde +++ /dev/null @@ -1,610 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK - -#include "Mavlink_Common.h" - -GCS_MAVLINK::GCS_MAVLINK() : - packet_drops(0) -{ -} - -void -GCS_MAVLINK::init(BetterStream * port) -{ - GCS_Class::init(port); - mavlink_comm_1_port = port; - chan = MAVLINK_COMM_1; -} - -void -GCS_MAVLINK::update(void) -{ - // recieve new packets - mavlink_message_t msg; - mavlink_status_t status; - - // process received bytes - while(comm_get_available(chan)) - { - uint8_t c = comm_receive_ch(chan); - - // Try to get a new message - if(mavlink_parse_char(chan, c, &msg, &status)) handleMessage(&msg); - } - - // Update packet drops counter - packet_drops += status.packet_rx_drop_count; - - // send out queued params/ waypoints - mavlink_queued_send(chan); - - // send parameters communication_queued_send(chan); - // stop waypoint sending if timeout - if (global_data.waypoint_sending && - millis() - global_data.waypoint_timelast_send > - global_data.waypoint_send_timeout) - { - send_text(SEVERITY_LOW,"waypoint send timeout"); - global_data.waypoint_sending = false; - } - - // stop waypoint receiving if timeout - if (global_data.waypoint_receiving && - millis() - global_data.waypoint_timelast_receive > - global_data.waypoint_receive_timeout) - { - send_text(SEVERITY_LOW,"waypoint receive timeout"); - global_data.waypoint_receiving = false; - } -} - -void -GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) -{ - if (freqLoopMatch(global_data.streamRateRawSensors,freqMin,freqMax)) - send_message(MSG_RAW_IMU); - - if (freqLoopMatch(global_data.streamRateExtendedStatus,freqMin,freqMax)) - send_message(MSG_EXTENDED_STATUS); - - if (freqLoopMatch(global_data.streamRateRCChannels,freqMin,freqMax)) - send_message(MSG_RADIO_OUT); - - if (freqLoopMatch(global_data.streamRateRawController,freqMin,freqMax)) - send_message(MSG_SERVO_OUT); - - //if (freqLoopMatch(global_data.streamRateRawSensorFusion,freqMin,freqMax)) - - if (freqLoopMatch(global_data.streamRatePosition,freqMin,freqMax)) - send_message(MSG_LOCATION); - - if (freqLoopMatch(global_data.streamRateExtra1,freqMin,freqMax)) - send_message(MSG_GPS_STATUS); - - if (freqLoopMatch(global_data.streamRateExtra2,freqMin,freqMax)) - send_message(MSG_CURRENT_WAYPOINT); - - if (freqLoopMatch(global_data.streamRateExtra3,freqMin,freqMax)) - { - send_message(MSG_LOCAL_LOCATION); - send_message(MSG_ATTITUDE); - } -} - -void -GCS_MAVLINK::send_message(uint8_t id, uint32_t param) -{ - mavlink_send_message(chan,id,param,packet_drops); -} - -void -GCS_MAVLINK::send_text(uint8_t severity, const char *str) -{ - mavlink_send_text(chan,severity,str); -} - -void -GCS_MAVLINK::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) -{ - mavlink_acknowledge(chan,id,sum1,sum2); -} - -void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) -{ - switch (msg->msgid) { - - case MAVLINK_MSG_ID_GLOBAL_POSITION: - { - // decode - mavlink_global_position_t packet; - mavlink_msg_global_position_decode(msg, &packet); - //if (mavlink_check_target(packet.target_system,packet.target_component)) break; - trackVehicle_loc.id = 0; - trackVehicle_loc.p1 = 0; - trackVehicle_loc.alt = packet.alt; - trackVehicle_loc.lat = packet.lat; - trackVehicle_loc.lng = packet.lon; - Serial.println("received global position packet"); - } - - - case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: - { - // decode - mavlink_request_data_stream_t packet; - mavlink_msg_request_data_stream_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - - int freq = 0; // packet frequency - - if (packet.start_stop == 0) freq = 0; // stop sending - else if (packet.start_stop == 1) freq = packet.req_message_rate; // start sending - else break; - - switch(packet.req_stream_id) - { - case MAV_DATA_STREAM_ALL: - global_data.streamRateRawSensors = freq; - global_data.streamRateExtendedStatus = freq; - global_data.streamRateRCChannels = freq; - global_data.streamRateRawController = freq; - global_data.streamRateRawSensorFusion = freq; - global_data.streamRatePosition = freq; - global_data.streamRateExtra1 = freq; - global_data.streamRateExtra2 = freq; - global_data.streamRateExtra3 = freq; - break; - case MAV_DATA_STREAM_RAW_SENSORS: - global_data.streamRateRawSensors = freq; - break; - case MAV_DATA_STREAM_EXTENDED_STATUS: - global_data.streamRateExtendedStatus = freq; - break; - case MAV_DATA_STREAM_RC_CHANNELS: - global_data.streamRateRCChannels = freq; - break; - case MAV_DATA_STREAM_RAW_CONTROLLER: - global_data.streamRateRawController = freq; - break; - case MAV_DATA_STREAM_RAW_SENSOR_FUSION: - global_data.streamRateRawSensorFusion = freq; - break; - case MAV_DATA_STREAM_POSITION: - global_data.streamRatePosition = freq; - break; - case MAV_DATA_STREAM_EXTRA1: - global_data.streamRateExtra1 = freq; - break; - case MAV_DATA_STREAM_EXTRA2: - global_data.streamRateExtra2 = freq; - break; - case MAV_DATA_STREAM_EXTRA3: - global_data.streamRateExtra3 = freq; - break; - default: - break; - } - } - - case MAVLINK_MSG_ID_ACTION: - { - // decode - mavlink_action_t packet; - mavlink_msg_action_decode(msg, &packet); - if (mavlink_check_target(packet.target,packet.target_component)) break; - - // do action - gcs.send_text(SEVERITY_LOW,"action received"); - switch(packet.action) - { - - case MAV_ACTION_LAUNCH: - set_mode(TAKEOFF); - break; - - case MAV_ACTION_RETURN: - set_mode(RTL); - break; - - case MAV_ACTION_EMCY_LAND: - set_mode(LAND); - break; - - case MAV_ACTION_HALT: - loiter_at_location(); - break; - - // can't implement due to APM configuration - // just setting to manual to be safe - case MAV_ACTION_MOTORS_START: - case MAV_ACTION_CONFIRM_KILL: - case MAV_ACTION_EMCY_KILL: - case MAV_ACTION_MOTORS_STOP: - case MAV_ACTION_SHUTDOWN: - set_mode(MANUAL); - break; - - case MAV_ACTION_CONTINUE: - process_next_command(); - break; - - case MAV_ACTION_SET_MANUAL: - set_mode(MANUAL); - break; - - case MAV_ACTION_SET_AUTO: - set_mode(AUTO); - break; - - case MAV_ACTION_STORAGE_READ: - //read_EEPROM_startup(); - //read_EEPROM_airstart_critical(); - //read_command_index(); - //read_EEPROM_flight_modes(); - break; - - case MAV_ACTION_STORAGE_WRITE: - //save_EEPROM_trims(); - //save_EEPROM_waypoint_info(); - //save_EEPROM_gains(); - //save_command_index(); - //save_pressure_data(); - //save_EEPROM_radio_minmax(); - //save_user_configs(); - //save_EEPROM_flight_modes(); - break; - - case MAV_ACTION_CALIBRATE_RC: break; - trim_radio(); - break; - - case MAV_ACTION_CALIBRATE_GYRO: - case MAV_ACTION_CALIBRATE_MAG: - case MAV_ACTION_CALIBRATE_ACC: - case MAV_ACTION_CALIBRATE_PRESSURE: - case MAV_ACTION_REBOOT: // this is a rough interpretation - startup_IMU_ground(); - break; - - case MAV_ACTION_REC_START: break; - case MAV_ACTION_REC_PAUSE: break; - case MAV_ACTION_REC_STOP: break; - - case MAV_ACTION_TAKEOFF: - set_mode(TAKEOFF); - break; - - case MAV_ACTION_NAVIGATE: - set_mode(AUTO); - break; - - case MAV_ACTION_LAND: - set_mode(LAND); - break; - - case MAV_ACTION_LOITER: - set_mode(LOITER); - break; - - default: break; - } - } - break; - - case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: - { - //send_text(SEVERITY_LOW,"waypoint request list"); - - // decode - mavlink_waypoint_request_list_t packet; - mavlink_msg_waypoint_request_list_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - - // Start sending waypoints - mavlink_msg_waypoint_count_send(chan,msg->sysid, - msg->compid,get(PARAM_WP_TOTAL)); - global_data.waypoint_timelast_send = millis(); - global_data.waypoint_sending = true; - global_data.waypoint_receiving = false; - global_data.waypoint_dest_sysid = msg->sysid; - global_data.waypoint_dest_compid = msg->compid; - - } - break; - - case MAVLINK_MSG_ID_WAYPOINT_REQUEST: - { - //send_text(SEVERITY_LOW,"waypoint request"); - - // Check if sending waypiont - if (!global_data.waypoint_sending) break; - - // decode - mavlink_waypoint_request_t packet; - mavlink_msg_waypoint_request_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - - // send waypoint - tell_command = get_wp_with_index(packet.seq); - - // set frame of waypoint - uint8_t frame = MAV_FRAME_GLOBAL; // reference frame - uint8_t action = MAV_ACTION_NAVIGATE; // action - uint8_t orbit_direction = 0; // clockwise(0), counter-clockwise(1) - float orbit = 0; // loiter radius - float param1 = 0, param2 = 0; - - switch(tell_command.id) - { - - case CMD_WAYPOINT: // navigate - action = MAV_ACTION_NAVIGATE; // action - break; - - case CMD_LOITER_TIME: // loiter - orbit = get(PARAM_WP_RADIUS); // XXX setting loiter radius as waypoint acceptance radius - action = MAV_ACTION_LOITER; // action - param1 = get(PARAM_WP_RADIUS); - param2 = tell_command.p1*100; // loiter time - break; - - case CMD_TAKEOFF: // takeoff - action = MAV_ACTION_TAKEOFF; - break; - - case CMD_LAND: // land - action = MAV_ACTION_LAND; - break; - - defaut: - gcs.send_text(SEVERITY_LOW,"command not handled"); - break; - } - - // time that the mav should loiter in milliseconds - uint8_t current = 0; // 1 (true), 0 (false) - if (packet.seq == get(PARAM_WP_INDEX)) current = 1; - float yaw_dir = 0; // yaw orientation in radians, 0 = north XXX: what does this do? - uint8_t autocontinue = 1; // 1 (true), 0 (false) - float x = tell_command.lng/1.0e7; // local (x), global (longitude) - float y = tell_command.lat/1.0e7; // local (y), global (latitude) - float z = tell_command.alt/1.0e2; // local (z), global (altitude) - // note XXX: documented x,y,z order does not match with gps raw - mavlink_msg_waypoint_send(chan,msg->sysid, - msg->compid,packet.seq,frame,action, - orbit,orbit_direction,param1,param2,current,x,y,z,yaw_dir,autocontinue); - - // update last waypoint comm stamp - global_data.waypoint_timelast_send = millis(); - } - break; - - case MAVLINK_MSG_ID_WAYPOINT_ACK: - { - //send_text(SEVERITY_LOW,"waypoint ack"); - - // decode - mavlink_waypoint_ack_t packet; - mavlink_msg_waypoint_ack_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - - // check for error - uint8_t type = packet.type; // ok (0), error(1) - - // turn off waypoint send - global_data.waypoint_sending = false; - } - break; - - case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: - { - //send_text(SEVERITY_LOW,"param request list"); - - // decode - mavlink_param_request_list_t packet; - mavlink_msg_param_request_list_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - - // Start sending parameters - global_data.parameter_i = 0; - } - break; - - case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: - { - //send_text(SEVERITY_LOW,"waypoint clear all"); - - // decode - mavlink_waypoint_clear_all_t packet; - mavlink_msg_waypoint_clear_all_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - - // clear all waypoints - uint8_t type = 0; // ok (0), error(1) - set(PARAM_WP_TOTAL,0); - - // send acknowledgement 3 times to makes sure it is received - for (int i=0;i<3;i++) mavlink_msg_waypoint_ack_send(chan,msg->sysid,msg->compid,type); - - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: - { - //send_text(SEVERITY_LOW,"waypoint set current"); - - // decode - mavlink_waypoint_set_current_t packet; - mavlink_msg_waypoint_set_current_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - - // set current waypoint - set(PARAM_WP_INDEX,packet.seq); - { - Location temp; // XXX this is gross - temp = get_wp_with_index(packet.seq); - set_next_WP(&temp); - } - mavlink_msg_waypoint_current_send(chan,get(PARAM_WP_INDEX)); - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_COUNT: - { - //send_text(SEVERITY_LOW,"waypoint count"); - - // decode - mavlink_waypoint_count_t packet; - mavlink_msg_waypoint_count_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - - // start waypoint receiving - set(PARAM_WP_TOTAL,packet.count); - if (get(PARAM_WP_TOTAL) > MAX_WAYPOINTS) - set(PARAM_WP_TOTAL,MAX_WAYPOINTS); - global_data.waypoint_timelast_receive = millis(); - global_data.waypoint_receiving = true; - global_data.waypoint_sending = false; - global_data.waypoint_request_i = 0; - break; - } - - case MAVLINK_MSG_ID_WAYPOINT: - { - // Check if receiving waypiont - if (!global_data.waypoint_receiving) break; - - // decode - mavlink_waypoint_t packet; - mavlink_msg_waypoint_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - - // check if this is the requested waypoint - if (packet.seq != global_data.waypoint_request_i) break; - - // store waypoint - uint8_t loadAction = 0; // 0 insert in list, 1 exec now - - switch (packet.frame) - { - case MAV_FRAME_GLOBAL: - { - tell_command.lng = 1.0e7f*packet.x; - tell_command.lat = 1.0e7f*packet.y; - tell_command.alt = packet.z*1.0e2f; - break; - } - - case MAV_FRAME_LOCAL: // local (relative to home position) - { - tell_command.lng = 1.0e7f*ToDeg(packet.x/ - (radius_of_earth*cosf(ToRad(home.lat/1.0e7f)))) + home.lng; - tell_command.lat = 1.0e7f*ToDeg(packet.y/radius_of_earth) + home.lat; - tell_command.alt = -packet.z*1.0e2f + home.alt; - break; - } - } - - // defaults - tell_command.id = CMD_BLANK; - - switch (packet.action) - { - - case MAV_ACTION_TAKEOFF: - { - tell_command.id = CMD_TAKEOFF; - break; - } - case MAV_ACTION_LAND: - { - tell_command.id = CMD_LAND; - break; - } - - case MAV_ACTION_NAVIGATE: - { - tell_command.id = CMD_WAYPOINT; - break; - } - - case MAV_ACTION_LOITER: - { - tell_command.id = CMD_LOITER_TIME; - tell_command.p1 = packet.param2/1.0e2; - break; - } - } - - // save waypoint - set_wp_with_index(tell_command, packet.seq); - - // update waypoint receiving state machine - global_data.waypoint_timelast_receive = millis(); - global_data.waypoint_request_i++; - - if (global_data.waypoint_request_i == get(PARAM_WP_TOTAL)) - { - gcs.send_text(SEVERITY_LOW,"flight plane received"); - uint8_t type = 0; // ok (0), error(1) - mavlink_msg_waypoint_ack_send(chan,msg->sysid,msg->compid,type); - global_data.waypoint_receiving = false; - set(PARAM_WP_RADIUS,packet.param1); // XXX takes last waypoint radius for all - //save_EEPROM_waypoint_info(); - } - break; - } - - case MAVLINK_MSG_ID_PARAM_SET: - { - // decode - mavlink_param_set_t packet; - mavlink_msg_param_set_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) - break; - - // set parameter - const char * key = (const char*) packet.param_id; - - // iterate known parameters - // XXX linear search; would be better to sort params & use a binary search - for (uint16_t i = 0; i < global_data.param_count; i++) { - - // compare key with parameter name - if (!strcmp_P(key, getParamName(i))) { - - // sanity-check the new parameter - if (!isnan(packet.param_value) && // not nan - !isinf(packet.param_value)) { // not inf - - setParamAsFloat(i,packet.param_value); - - // Report back new value - char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK - strcpy_P(param_name, getParamName(i)); /// XXX HACK - mavlink_msg_param_value_send(chan, - (int8_t*)param_name, - getParamAsFloat(i), - global_data.param_count,i); - // call load if required - if (i >= PARAM_RLL2SRV_P && i <= PARAM_RLL2SRV_IMAX) pidServoRoll.load_gains(); - if (i >= PARAM_PTCH2SRV_P && i <= PARAM_PTCH2SRV_IMAX) pidServoPitch.load_gains(); - if (i >= PARAM_YW2SRV_P && i <= PARAM_YW2SRV_IMAX) pidServoRudder.load_gains(); - if (i >= PARAM_HDNG2RLL_P && i <= PARAM_HDNG2RLL_IMAX) pidNavRoll.load_gains(); - if (i >= PARAM_ARSPD2PTCH_P && i <= PARAM_ARSPD2PTCH_IMAX) pidNavPitchAirspeed.load_gains(); - if (i >= PARAM_ALT2PTCH_P && i <= PARAM_ALT2PTCH_IMAX) pidNavPitchAltitude.load_gains(); - if (i >= PARAM_ENRGY2THR_P && i <= PARAM_ENRGY2THR_IMAX) pidTeThrottle.load_gains(); - if (i >= PARAM_ALT2THR_P && i <= PARAM_ALT2THR_IMAX) pidAltitudeThrottle.load_gains(); - } - break; - } - } - break; - } // end case - } // end switch -} // end handle mavlink - - -#endif - diff --git a/Tools/ArduTracker/GCS_Standard.pde b/Tools/ArduTracker/GCS_Standard.pde deleted file mode 100644 index bfe8ab764a..0000000000 --- a/Tools/ArduTracker/GCS_Standard.pde +++ /dev/null @@ -1,276 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/// -/// @file GCS_Standard.pde -/// @brief GCS driver for the APM binary protocol -/// - -#if GCS_PROTOCOL == GCS_PROTOCOL_STANDARD - -// constructor -GCS_STANDARD::GCS_STANDARD(BinComm::MessageHandler GCS_MessageHandlers[]) : - _binComm(GCS_MessageHandlers) -{ -} - -void -GCS_STANDARD::init(BetterStream *port) -{ - GCS_Class::init(port); - _binComm.init(port); -} - -void -GCS_STANDARD::update() -{ - _binComm.update(); -} - -void -GCS_STANDARD::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) -{ - _binComm.send_msg_acknowledge(id, sum1, sum2); - gcs_messages_sent++; -} - -void -GCS_STANDARD::send_message(uint8_t id, uint32_t param) -{ - byte mess_buffer[54]; - byte mess_ck_a = 0; - byte mess_ck_b = 0; - int tempint; - int ck; - long templong; - - switch(id) { - case MSG_MODE_CHANGE: - case MSG_HEARTBEAT: - _binComm.send_msg_heartbeat(control_mode, // current control mode - millis() / 1000, // seconds since power-up - battery_voltage1 * 100, // battery voltage * 100 - command_must_index); // command index (waypoint #) - break; - - case MSG_ATTITUDE: - _binComm.send_msg_attitude(roll_sensor, - pitch_sensor, - yaw_sensor); - break; - - case MSG_LOCATION: - _binComm.send_msg_location(current_loc.lat, - current_loc.lng, - GPS.altitude / 100, - GPS.ground_speed, - yaw_sensor, - GPS.time); - break; - - case MSG_PRESSURE: - _binComm.send_msg_pressure(current_loc.alt / 10, - airspeed / 100); - break; - - case MSG_PERF_REPORT: - _binComm.send_msg_perf_report(millis() - perf_mon_timer, - mainLoop_count, - G_Dt_max & 0xff, - gyro_sat_count, - adc_constraints, - renorm_sqrt_count, - renorm_blowup_count, - gps_fix_count, - imu_health * 100, - gcs_messages_sent); - break; - - case MSG_CPU_LOAD: - //TODO: Implement appropriate BinComm routine here - - case MSG_VALUE: - switch(param) { - //case 0x00: templong = roll_mode; break; - //case 0x01: templong = pitch_mode; break; - //case 0x02: templong = throttle_mode; break; - case 0x03: templong = yaw_mode; break; - case 0x04: templong = elevon1_trim; break; - case 0x05: templong = elevon2_trim; break; - /* TODO: implement for new PID lib - case 0x10: templong = integrator[0] * 1000; break; - case 0x11: templong = integrator[1] * 1000; break; - case 0x12: templong = integrator[2] * 1000; break; - case 0x13: templong = integrator[3] * 1000; break; - case 0x14: templong = integrator[4] * 1000; break; - case 0x15: templong = integrator[5] * 1000; break; - case 0x16: templong = integrator[6] * 1000; break; - case 0x17: templong = integrator[7] * 1000; break; - */ - case 0x1a: templong = kff[0]; break; - case 0x1b: templong = kff[1]; break; - case 0x1c: templong = kff[2]; break; - case 0x20: templong = target_bearing; break; - case 0x21: templong = nav_bearing; break; - case 0x22: templong = bearing_error; break; - case 0x23: templong = crosstrack_bearing; break; - case 0x24: templong = crosstrack_error; break; - case 0x25: templong = altitude_error; break; - case 0x26: templong = wp_radius; break; - case 0x27: templong = loiter_radius; break; - // case 0x28: templong = wp_mode; break; - // case 0x29: templong = loop_commands; break; - case 0x2a: templong = nav_gain_scaler; break; - } - _binComm.send_msg_value(param, - templong); - break; - - case MSG_COMMAND_LIST: - tell_command = get_wp_with_index((int)param); - _binComm.send_msg_command_list(param, - wp_total, - tell_command.id, - tell_command.p1, - tell_command.alt, - tell_command.lat, - tell_command.lng); - break; - - case MSG_TRIM_STARTUP: - _binComm.send_msg_trim_startup(radio_trim); - break; - - case MSG_TRIM_MIN: - _binComm.send_msg_trim_min(radio_min); - break; - - case MSG_TRIM_MAX: - _binComm.send_msg_trim_max(radio_max); - break; - - /* TODO: implement for new PID lib - case MSG_PID: // PID Gains message - _binComm.send_msg_pid(param, - kp[param] * 1000000, - ki[param] * 1000000, - kd[param] * 1000000, - integrator_max[param]); - break; - */ - - case MSG_SERVO_OUT: - _binComm.send_msg_servo_out(servo_out); - break; - - case MSG_RADIO_OUT: - _binComm.send_msg_radio_out(radio_out); - break; - - default: - GCS.send_text(SEVERITY_LOW," unknown message ID"); - } - gcs_messages_sent++; -} - -void -GCS_STANDARD::send_text(byte severity, const char *str) -{ - if (severity >= DEBUG_LEVEL) { - char text[50]; // XXX magic numbers - - strncpy(text, str, 50); - _binComm.send_msg_status_text(severity, text); - gcs_messages_sent++; - } -} - -void -receive_message(void * arg, uint8_t id, uint8_t messageVersion, void * messageData) -{ - // process command - switch(id) { - - case MSG_STATUS_TEXT: - { - char str[50]; - uint8_t severity; - GCS.getBinComm().unpack_msg_status_text(severity,str); - SendDebug(str); - SendDebug(" severity: "); SendDebugln(severity); - } - break; - - case MSG_VERSION_REQUEST: - break; - - case MSG_VALUE_REQUEST: - break; - - case MSG_VALUE_SET: - break; - - case MSG_PID_REQUEST: - break; - - case MSG_PID_SET: - break; - - case MSG_EEPROM_REQUEST: - break; - - case MSG_EEPROM_SET: - break; - - case MSG_PIN_REQUEST: - break; - - case MSG_PIN_SET: - break; - - case MSG_DATAFLASH_REQUEST: - break; - - case MSG_DATAFLASH_SET: - break; - - case MSG_COMMAND_REQUEST: - uint16_t commandIndex; - GCS.getBinComm().unpack_msg_command_request(commandIndex); - tell_command = get_wp_with_index(commandIndex); - GCS.getBinComm().send_msg_command_list(commandIndex,uint16_t(wp_total),tell_command.id, - tell_command.p1,tell_command.alt,tell_command.lat,tell_command.lng); - break; - - case MSG_COMMAND_UPLOAD: - uint8_t action; // 0 -insert in list, 1- execute immediately - uint16_t itemNumber; // item number ( i.e. waypoint number) - uint16_t listLength; // list length - struct Location temp; - GCS.getBinComm().unpack_msg_command_upload(action,itemNumber,listLength,temp.id,temp.p1,temp.alt,temp.lat,temp.lng); - wp_total=listLength; - if (action == 0) // insert in list - { - // save waypoint total to eeprom at start of the list - if (itemNumber == 1) save_EEPROM_waypoint_info(); - set_wp_with_index(temp, itemNumber); - } - else if (action == 1) - { - next_command = temp; - process_now(); - } - break; - - case MSG_ACKNOWLEDGE: - break; - - default: - { - char str[50]; - sprintf(str," unknown messageID:%x",id); - GCS.send_text(SEVERITY_LOW,str); - } - } -} - -#endif // GCS_PROTOCOL_STANDARD diff --git a/Tools/ArduTracker/GCS_Xplane.pde b/Tools/ArduTracker/GCS_Xplane.pde deleted file mode 100644 index 64a12fff4d..0000000000 --- a/Tools/ArduTracker/GCS_Xplane.pde +++ /dev/null @@ -1,113 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/// -/// @file GCS_Xplane.pde -/// @brief GCS driver for the X-plane HIL interface. -/// - -#if GCS_PROTOCOL == GCS_PROTOCOL_XPLANE - -void -GCS_XPLANE::send_text(uint8_t severity, const char *str) -{ - if(severity >= DEBUG_LEVEL){ - Serial.print("MSG: "); - Serial.println(str); - } -} - -void -GCS_XPLANE::send_message(uint8_t id, uint32_t param) -{ - switch(id) { - case MSG_HEARTBEAT: - print_control_mode(); - break; - - case MSG_ATTITUDE: - //print_attitude(); - break; - - case MSG_LOCATION: - //print_position(); - break; - - case MSG_CPU_LOAD: - //TODO: implement appropriate routine here if applicable - break; - - case MSG_COMMAND_LIST: - struct Location cmd = get_wp_with_index(param); - print_waypoint(&cmd, param); - break; - - } -} - -void -GCS_XPLANE::print_control_mode(void) -{ - Serial.print("MSG: "); - Serial.print(flight_mode_strings[control_mode]); -} - -void -GCS_XPLANE::print_current_waypoints() -{ - Serial.print("MSG: "); - Serial.print("CUR:"); - Serial.print("\t"); - Serial.print(current_loc.lat,DEC); - Serial.print(",\t"); - Serial.print(current_loc.lng,DEC); - Serial.print(",\t"); - Serial.println(current_loc.alt,DEC); - - Serial.print("MSG: "); - Serial.print("NWP:"); - Serial.print(wp_index,DEC); - Serial.print(",\t"); - Serial.print(next_WP.lat,DEC); - Serial.print(",\t"); - Serial.print(next_WP.lng,DEC); - Serial.print(",\t"); - Serial.println(next_WP.alt,DEC); -} - -void -GCS_XPLANE::print_waypoint(struct Location *cmd,byte index) -{ - Serial.print("MSG: command #: "); - Serial.print(index, DEC); - Serial.print(" id: "); - Serial.print(cmd->id,DEC); - Serial.print(" p1: "); - Serial.print(cmd->p1,DEC); - Serial.print(" p2: "); - Serial.print(cmd->alt,DEC); - Serial.print(" p3: "); - Serial.print(cmd->lat,DEC); - Serial.print(" p4: "); - Serial.println(cmd->lng,DEC); -} - -void -GCS_XPLANE::print_waypoints() -{ - Serial.println("Commands in memory"); - Serial.print("commands total: "); - Serial.println(wp_total, DEC); - // create a location struct to hold the temp Waypoints for printing - //Location tmp; - Serial.println("Home: "); - struct Location cmd = get_wp_with_index(0); - print_waypoint(&cmd, 0); - Serial.println("Commands: "); - - for (int i=1; i -#include -#include -#include -#include -#include - -/// -/// @class HIL -/// @brief Class describing the interface between the APM code -/// proper and the HIL implementation. -/// -/// HIL' are currently implemented inside the sketch and as such have -/// access to all global state. The sketch should not, however, call HIL -/// internal functions - all calls to the HIL should be routed through -/// this interface (or functions explicitly exposed by a subclass). -/// -class HIL_Class -{ -public: - - /// Startup initialisation. - /// - /// This routine performs any one-off initialisation required before - /// HIL messages are exchanged. - /// - /// @note The stream is expected to be set up and configured for the - /// correct bitrate before ::init is called. - /// - /// @note The stream is currently BetterStream so that we can use the _P - /// methods; this may change if Arduino adds them to Print. - /// - /// @param port The stream over which messages are exchanged. - /// - void init(BetterStream *port) { _port = port; } - - /// Update HIL state. - /// - /// This may involve checking for received bytes on the stream, - /// or sending additional periodic messages. - void update(void) {} - - /// Send a message with a single numeric parameter. - /// - /// This may be a standalone message, or the HIL driver may - /// have its own way of locating additional parameters to send. - /// - /// @param id ID of the message to send. - /// @param param Explicit message parameter. - /// - void send_message(uint8_t id, int32_t param = 0) {} - - /// Send a text message. - /// - /// @param severity A value describing the importance of the message. - /// @param str The text to be sent. - /// - void send_text(uint8_t severity, const char *str) {} - - /// Send acknowledgement for a message. - /// - /// @param id The message ID being acknowledged. - /// @param sum1 Checksum byte 1 from the message being acked. - /// @param sum2 Checksum byte 2 from the message being acked. - /// - void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) {} - -protected: - /// The stream we are communicating over - BetterStream *_port; -}; - -// -// HIL class definitions. -// -// These are here so that we can declare the HIL object early in the sketch -// and then reference it statically rather than via a pointer. -// - -/// -/// @class HIL_MAVLINK -/// @brief The mavlink protocol for hil -/// -#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK -class HIL_MAVLINK : public HIL_Class -{ -public: - HIL_MAVLINK(); - void update(void); - void init(BetterStream *port); - void send_message(uint8_t id, uint32_t param = 0); - void send_text(uint8_t severity, const char *str); - void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2); -private: - void handleMessage(mavlink_message_t * msg); - mavlink_channel_t chan; - uint16_t packet_drops; -}; -#endif // HIL_PROTOCOL_MAVLINK - -/// -/// @class HIL_XPLANE -/// @brief The xplane protocol for hil -/// -#if HIL_PROTOCOL == HIL_PROTOCOL_XPLANE -class HIL_XPLANE : public HIL_Class -{ -public: - HIL_XPLANE(); - void update(void); - void init(BetterStream *port); - void send_message(uint8_t id, uint32_t param = 0); - void send_text(uint8_t severity, const char *str); - void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2); -private: - void output_HIL(); - void output_HIL_(); - void output_int(int value); - void output_byte(byte value); - void print_buffer(); - - AP_GPS_IMU * hilPacketDecoder; - byte buf_len; - byte out_buffer[32]; -}; -#endif // HIL_PROTOCOL_XPLANE - -#endif // HIL not disabled - -#endif // __HIL_H - diff --git a/Tools/ArduTracker/HIL_Mavlink.pde b/Tools/ArduTracker/HIL_Mavlink.pde deleted file mode 100644 index 0de8a6eea1..0000000000 --- a/Tools/ArduTracker/HIL_Mavlink.pde +++ /dev/null @@ -1,154 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -#if HIL_MODE != HIL_MODE_DISABLED && HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK - -#include "Mavlink_Common.h" - -HIL_MAVLINK::HIL_MAVLINK() : - packet_drops(0) -{ -} - -void -HIL_MAVLINK::init(BetterStream * port) -{ - HIL_Class::init(port); - mavlink_comm_0_port = port; - chan = MAVLINK_COMM_0; -} - -void -HIL_MAVLINK::update(void) -{ - mavlink_message_t msg; - mavlink_status_t status; - - // process received bytes - while(comm_get_available(chan)) - { - uint8_t c = comm_receive_ch(chan); - - // Try to get a new message - if(mavlink_parse_char(chan, c, &msg, &status)) handleMessage(&msg); - } - - // Update packet drops counter - packet_drops += status.packet_rx_drop_count; -} - -void -HIL_MAVLINK::send_message(uint8_t id, uint32_t param) -{ - mavlink_send_message(chan,id,param,packet_drops); -} - -void -HIL_MAVLINK::send_text(uint8_t severity, const char *str) -{ - mavlink_send_text(chan,severity,str); -} - -void -HIL_MAVLINK::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) -{ - mavlink_acknowledge(chan,id,sum1,sum2); -} - -void -HIL_MAVLINK::handleMessage(mavlink_message_t* msg) -{ - switch (msg->msgid) { - - // handle incoming vehicle position - case MAVLINK_MSG_ID_GLOBAL_POSITION: - { - // decode - mavlink_global_position_t packet; - mavlink_msg_global_position_decode(msg, &packet); - //if (mavlink_check_target(packet.target_system,packet.target_component)) break; - trackVehicle_loc.id = 0; - trackVehicle_loc.p1 = 0; - trackVehicle_loc.alt = packet.alt; - trackVehicle_loc.lat = packet.lat; - trackVehicle_loc.lng = packet.lon; - Serial.println("received global position packet"); - } - - // This is used both as a sensor and to pass the location - // in HIL_ATTITUDE mode. - case MAVLINK_MSG_ID_GPS_RAW: - { - // decode - mavlink_gps_raw_t packet; - mavlink_msg_gps_raw_decode(msg, &packet); - - // set gps hil sensor - gps.setHIL(packet.usec/1000.0,packet.lat,packet.lon,packet.alt, - packet.v,packet.hdg,0,0); - break; - } - - case MAVLINK_MSG_ID_AIRSPEED: - { - // decode - mavlink_airspeed_t packet; - mavlink_msg_airspeed_decode(msg, &packet); - - // set airspeed - airspeed = 100*packet.airspeed; - break; - } - -#if HIL_MODE == HIL_MODE_ATTITUDE - - case MAVLINK_MSG_ID_ATTITUDE: - { - // decode - mavlink_attitude_t packet; - mavlink_msg_attitude_decode(msg, &packet); - - // set gps hil sensor - dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, - packet.pitchspeed,packet.yawspeed); - break; - } - -#elif HIL_MODE == HIL_MODE_SENSORS - - case MAVLINK_MSG_ID_RAW_IMU: - { - // decode - mavlink_raw_imu_t packet; - mavlink_msg_raw_imu_decode(msg, &packet); - - // set imu hil sensors - // TODO: check scaling for temp/absPress - float temp = 70; - float absPress = 1; - //Serial.printf_P(PSTR("\nreceived accel:\t%d\t%d\t%d\n"), packet.xacc, packet.yacc, packet.zacc); - //Serial.printf_P(PSTR("\nreceived gyro:\t%d\t%d\t%d\n"), packet.xgyro, packet.ygyro, packet.zgyro); - - adc.setHIL(packet.xgyro,packet.ygyro,packet.zgyro,temp, - packet.xacc,packet.yacc,packet.zacc,absPress); - compass.setHIL(packet.xmag,packet.ymag,packet.zmag); - break; - } - - case MAVLINK_MSG_ID_RAW_PRESSURE: - { - // decode - mavlink_raw_pressure_t packet; - mavlink_msg_raw_pressure_decode(msg, &packet); - - // set pressure hil sensor - // TODO: check scaling - float temp = 70; - pitot.setHIL(temp,packet.press_diff1); - break; - } -#endif // HIL_MODE - - } // end switch -} -#endif - diff --git a/Tools/ArduTracker/HIL_Xplane.pde b/Tools/ArduTracker/HIL_Xplane.pde deleted file mode 100644 index c2b375922e..0000000000 --- a/Tools/ArduTracker/HIL_Xplane.pde +++ /dev/null @@ -1,151 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -#if HIL_MODE != HIL_MODE_DISABLED && HIL_PROTOCOL == HIL_PROTOCOL_XPLANE - - -void HIL_XPLANE::output_HIL(void) -{ - // output real-time sensor data - Serial.print("AAA"); // Message preamble - output_int((int)(servo_out[CH_ROLL])); // 0 bytes 0, 1 - output_int((int)(servo_out[CH_PITCH])); // 1 bytes 2, 3 - output_int((int)(servo_out[CH_THROTTLE])); // 2 bytes 4, 5 - output_int((int)(servo_out[CH_RUDDER])); // 3 bytes 6, 7 - output_int((int)wp_distance); // 4 bytes 8,9 - output_int((int)bearing_error); // 5 bytes 10,11 - output_int((int)next_WP.alt / 100); // 6 bytes 12, 13 - output_int((int)energy_error); // 7 bytes 14,15 - output_byte(get(PARAM_WP_INDEX)); // 8 bytes 16 - output_byte(control_mode); // 9 bytes 17 - - // print out the buffer and checksum - // --------------------------------- - print_buffer(); -} - -// This is for debugging only!, -// I just move the underscore to keep the above version pristene. -void HIL_XPLANE::output_HIL_(void) -{ - // output real-time sensor data - Serial.print("AAA"); // Message preamble - output_int((int)(servo_out[CH_ROLL])); // 0 bytes 0, 1 - output_int((int)(servo_out[CH_PITCH])); // 1 bytes 2, 3 - output_int((int)(servo_out[CH_THROTTLE])); // 2 bytes 4, 5 - output_int((int)(servo_out[CH_RUDDER])); // 3 bytes 6, 7 - output_int((int)wp_distance); // 4 bytes 8, 9 - output_int((int)bearing_error); // 5 bytes 10,11 - output_int((int)dcm.roll_sensor); // 6 bytes 12,13 - output_int((int)loiter_total); // 7 bytes 14,15 - output_byte(get(PARAM_WP_INDEX)); // 8 bytes 16 - output_byte(control_mode); // 9 bytes 17 - - // print out the buffer and checksum - // --------------------------------- - print_buffer(); -} - -void HIL_XPLANE::output_int(int value) -{ - //buf_len += 2; - out_buffer[buf_len++] = value & 0xff; - out_buffer[buf_len++] = (value >> 8) & 0xff; -} - -void HIL_XPLANE::output_byte(byte value) -{ - out_buffer[buf_len++] = value; -} - -void HIL_XPLANE::print_buffer() -{ - byte ck_a = 0; - byte ck_b = 0; - for (byte i = 0;i < buf_len; i++){ - Serial.print (out_buffer[i]); - } - Serial.print('\r'); - Serial.print('\n'); - buf_len = 0; -} - - - -HIL_XPLANE::HIL_XPLANE() : - buf_len(0) -{ -} - -void -HIL_XPLANE::init(BetterStream * port) -{ - HIL_Class::init(port); - hilPacketDecoder = new AP_GPS_IMU(port); - hilPacketDecoder->init(); -} - -void -HIL_XPLANE::update(void) -{ - hilPacketDecoder->update(); - airspeed = (float)hilPacketDecoder->airspeed; //airspeed is * 100 coming in from Xplane for accuracy - calc_airspeed_errors(); - dcm.setHil(hilPacketDecoder->roll_sensor*M_PI/18000, - hilPacketDecoder->pitch_sensor*M_PI/18000, - hilPacketDecoder->ground_course*M_PI/18000, - 0,0,0); - - // set gps hil sensor - gps.setHIL(hilPacketDecoder->time/1000.0,(float)hilPacketDecoder->latitude/1.0e7,(float)hilPacketDecoder->longitude/1.0e7,(float)hilPacketDecoder->altitude/1.0e2, - (float)hilPacketDecoder->speed_3d/1.0e2,(float)hilPacketDecoder->ground_course/1.0e2,0,0); -} - -void -HIL_XPLANE::send_message(uint8_t id, uint32_t param) -{ - // TODO: split output by actual request - uint64_t timeStamp = micros(); - switch(id) { - - case MSG_HEARTBEAT: - break; - case MSG_EXTENDED_STATUS: - break; - case MSG_ATTITUDE: - break; - case MSG_LOCATION: - break; - case MSG_LOCAL_LOCATION: - break; - case MSG_GPS_RAW: - break; - case MSG_SERVO_OUT: - output_HIL(); - break; - case MSG_AIRSPEED: - break; - case MSG_RADIO_OUT: - break; - case MSG_RAW_IMU: - break; - case MSG_GPS_STATUS: - break; - case MSG_CURRENT_WAYPOINT: - break; - defualt: - break; - } -} - -void -HIL_XPLANE::send_text(uint8_t severity, const char *str) -{ -} - -void -HIL_XPLANE::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) -{ -} - -#endif - diff --git a/Tools/ArduTracker/Log.pde b/Tools/ArduTracker/Log.pde deleted file mode 100644 index 0a69dd1138..0000000000 --- a/Tools/ArduTracker/Log.pde +++ /dev/null @@ -1,611 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -// Code to Write and Read packets from DataFlash log memory -// Code to interact with the user to dump or erase logs - -#define HEAD_BYTE1 0xA3 // Decimal 163 -#define HEAD_BYTE2 0x95 // Decimal 149 -#define END_BYTE 0xBA // Decimal 186 - - -// These are function definitions so the Menu can be constructed before the functions -// are defined below. Order matters to the compiler. -static int8_t print_log_menu(uint8_t argc, const Menu::arg *argv); -static int8_t dump_log(uint8_t argc, const Menu::arg *argv); -static int8_t erase_logs(uint8_t argc, const Menu::arg *argv); -static int8_t select_logs(uint8_t argc, const Menu::arg *argv); - -// This is the help function -// PSTR is an AVR macro to read strings from flash memory -// printf_P is a version of print_f that reads from flash memory -static int8_t help_log(uint8_t argc, const Menu::arg *argv) -{ - Serial.printf_P(PSTR("\n" - "Commands:\n" - " dump dump log \n" - " erase erase all logs\n" - " enable |all enable logging or everything\n" - " disable |all disable logging or everything\n" - "\n")); -} - -// Creates a constant array of structs representing menu options -// and stores them in Flash memory, not RAM. -// User enters the string in the console to call the functions on the right. -// See class Menu in AP_Coommon for implementation details -const struct Menu::command log_menu_commands[] PROGMEM = { - {"dump", dump_log}, - {"erase", erase_logs}, - {"enable", select_logs}, - {"disable", select_logs}, - {"help", help_log} -}; - -// A Macro to create the Menu -MENU2(log_menu, "Log", log_menu_commands, print_log_menu); - -static bool -print_log_menu(void) -{ - int log_start; - int log_end; - byte last_log_num = eeprom_read_byte((uint8_t *) EE_LAST_LOG_NUM); - - Serial.printf_P(PSTR("logs enabled: ")); - if (0 == get(PARAM_LOG_BITMASK)) { - Serial.printf_P(PSTR("none")); - } else { - // Macro to make the following code a bit easier on the eye. - // Pass it the capitalised name of the log option, as defined - // in defines.h but without the LOG_ prefix. It will check for - // the bit being set and print the name of the log option to suit. -#define PLOG(_s) if (get(PARAM_LOG_BITMASK) & LOGBIT_ ## _s) Serial.printf_P(PSTR(" %S"), PSTR(#_s)) - PLOG(ATTITUDE_FAST); - PLOG(ATTITUDE_MED); - PLOG(GPS); - PLOG(PM); - PLOG(CTUN); - PLOG(NTUN); - PLOG(MODE); - PLOG(RAW); - PLOG(CMD); -#undef PLOG - } - Serial.println(); - - if (last_log_num == 0) { - Serial.printf_P(PSTR("\nNo logs available for download\n")); - } else { - - Serial.printf_P(PSTR("\n%d logs available for download\n"), last_log_num); - for(int i=1;i last_log_num)) { - Serial.printf_P(PSTR("bad log number\n")); - return(-1); - } - - dump_log_start = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(dump_log-1)*0x02)); - dump_log_end = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(dump_log)*0x02))-1; - if (dump_log == last_log_num) { - dump_log_end = eeprom_read_word((uint16_t *) EE_LAST_LOG_PAGE); - } - Serial.printf_P(PSTR("Dumping Log number %d, start page %d, end page %d\n"), - dump_log, dump_log_start, dump_log_end); - Log_Read(dump_log_start, dump_log_end); - Serial.printf_P(PSTR("Log read complete\n")); -} - -static int8_t -erase_logs(uint8_t argc, const Menu::arg *argv) -{ - for(int i = 10 ; i > 0; i--) { - Serial.printf_P(PSTR("ATTENTION - Erasing log in %d seconds. Power off now to save log! \n"), i); - delay(1000); - } - Serial.printf_P(PSTR("\nErasing log...\n")); - for(int j = 1; j < 4001; j++) - DataFlash.PageErase(j); - eeprom_write_byte((uint8_t *)EE_LAST_LOG_NUM, 0); - eeprom_write_byte((uint8_t *)EE_LAST_LOG_PAGE, 1); - Serial.printf_P(PSTR("\nLog erased.\n")); -} - -static int8_t -select_logs(uint8_t argc, const Menu::arg *argv) -{ - uint16_t bits; - - if (argc != 2) { - Serial.printf_P(PSTR("missing log type\n")); - return(-1); - } - - bits = 0; - - // Macro to make the following code a bit easier on the eye. - // Pass it the capitalised name of the log option, as defined - // in defines.h but without the LOG_ prefix. It will check for - // that name as the argument to the command, and set the bit in - // bits accordingly. - // - if (!strcasecmp_P(argv[1].str, PSTR("all"))) { - bits = ~(bits = 0); - } else { -#define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= LOGBIT_ ## _s - TARG(ATTITUDE_FAST); - TARG(ATTITUDE_MED); - TARG(GPS); - TARG(PM); - TARG(CTUN); - TARG(NTUN); - TARG(MODE); - TARG(RAW); - TARG(CMD); -#undef TARG - } - - if (!strcasecmp_P(argv[0].str, PSTR("enable"))) { - set(PARAM_LOG_BITMASK, get(PARAM_LOG_BITMASK) | bits); - } else { - set(PARAM_LOG_BITMASK, get(PARAM_LOG_BITMASK) & ~bits); - } - return(0); -} - -int8_t -process_logs(uint8_t argc, const Menu::arg *argv) -{ - log_menu.run(); -} - -// Write an attitude packet. Total length : 10 bytes -void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw) -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_ATTITUDE_MSG); - DataFlash.WriteInt(log_roll); - DataFlash.WriteInt(log_pitch); - DataFlash.WriteInt(log_yaw); - DataFlash.WriteByte(END_BYTE); -} - -// Write a performance monitoring packet. Total length : 19 bytes -void Log_Write_Performance() -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_PERFORMANCE_MSG); - DataFlash.WriteLong(millis()- perf_mon_timer); - DataFlash.WriteInt(mainLoop_count); - DataFlash.WriteInt(G_Dt_max); - DataFlash.WriteByte(gyro_sat_count); - DataFlash.WriteByte(adc_constraints); - DataFlash.WriteByte(renorm_sqrt_count); - DataFlash.WriteByte(renorm_blowup_count); - DataFlash.WriteByte(gps_fix_count); - DataFlash.WriteInt((int)(imu_health*1000)); - DataFlash.WriteByte(END_BYTE); -} - -// Write a command processing packet. Total length : 19 bytes -//void Log_Write_Cmd(byte num, byte id, byte p1, long alt, long lat, long lng) -void Log_Write_Cmd(byte num, struct Location *wp) -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_CMD_MSG); - DataFlash.WriteByte(num); - DataFlash.WriteByte(wp->id); - DataFlash.WriteByte(wp->p1); - DataFlash.WriteLong(wp->alt); - DataFlash.WriteLong(wp->lat); - DataFlash.WriteLong(wp->lng); - DataFlash.WriteByte(END_BYTE); -} - -void Log_Write_Startup(byte type) -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_STARTUP_MSG); - DataFlash.WriteByte(type); - DataFlash.WriteByte(get(PARAM_WP_TOTAL)); - DataFlash.WriteByte(END_BYTE); - - // create a location struct to hold the temp Waypoints for printing - struct Location cmd = get_wp_with_index(0); - Log_Write_Cmd(0, &cmd); - - for (int i=1; i7) logvar = DataFlash.ReadInt(); - else logvar = DataFlash.ReadByte(); - //if(y > 7) logvar = logvar/1000.f; - Serial.print(logvar); - Serial.print(comma); - } - Serial.println(" "); -} - -// Read a command processing packet -void Log_Read_Cmd() -{ - byte logvarb; - long logvarl; - - Serial.print("CMD:"); - for(int i=1;i<4;i++) { - logvarb = DataFlash.ReadByte(); - Serial.print(logvarb,DEC); - Serial.print(comma); - } - for(int i=1;i<4;i++) { - logvarl = DataFlash.ReadLong(); - Serial.print(logvarl,DEC); - Serial.print(comma); - } - Serial.println(" "); -} - -void Log_Read_Startup() -{ - byte logbyte = DataFlash.ReadByte(); - if (logbyte == TYPE_AIRSTART_MSG) - Serial.print("AIR START - "); - else if (logbyte == TYPE_GROUNDSTART_MSG) - Serial.print("GROUND START - "); - else - Serial.print("UNKNOWN STARTUP TYPE -"); - Serial.print(DataFlash.ReadByte(), DEC); - Serial.println(" commands in memory"); -} - -// Read an attitude packet -void Log_Read_Attitude() -{ - int log_roll; - int log_pitch; - uint16_t log_yaw; - - log_roll = DataFlash.ReadInt(); - log_pitch = DataFlash.ReadInt(); - log_yaw = (uint16_t)DataFlash.ReadInt(); - - Serial.print("ATT:"); - Serial.print(log_roll); - Serial.print(comma); - Serial.print(log_pitch); - Serial.print(comma); - Serial.print(log_yaw); - Serial.println(); -} - -// Read a mode packet -void Log_Read_Mode() -{ - byte mode; - - mode = DataFlash.ReadByte(); - Serial.print("MOD:"); - switch (mode) { - case 0: - Serial.println("Manual"); - break; - case 1: - Serial.println("Stab"); - break; - case 5: - Serial.println("FBW_A"); - break; - case 6: - Serial.println("FBW_B"); - break; - case 10: - Serial.println("AUTO"); - break; - case 11: - Serial.println("RTL"); - break; - case 12: - Serial.println("Loiter"); - break; - case 98: - Serial.println("AS_COM"); - break; - case 99: - Serial.println("GS_COM"); - break; - } -} - -// Read a GPS packet -void Log_Read_GPS() -{ - - Serial.print("GPS:"); - Serial.print(DataFlash.ReadLong()); // Time - Serial.print(comma); - Serial.print((int)DataFlash.ReadByte()); // Fix - Serial.print(comma); - Serial.print((int)DataFlash.ReadByte()); // Num of Sats - Serial.print(comma); - Serial.print((float)DataFlash.ReadLong()/t7, 7); // Lattitude - Serial.print(comma); - Serial.print((float)DataFlash.ReadLong()/t7, 7); // Longitude - Serial.print(comma); - Serial.print((float)DataFlash.ReadLong()/100.0); // Baro/gps altitude mix - Serial.print(comma); - Serial.print((float)DataFlash.ReadLong()/100.0); // GPS altitude - Serial.print(comma); - Serial.print((float)DataFlash.ReadLong()/100.0); // Ground Speed - Serial.print(comma); - Serial.println((float)DataFlash.ReadLong()/100.0); // Ground Course - -} - -// Read a raw accel/gyro packet -void Log_Read_Raw() -{ - float logvar; - Serial.print("RAW:"); - for (int y=0;y<6;y++) { - logvar = (float)DataFlash.ReadLong()/t7; - Serial.print(logvar); - Serial.print(comma); - } - Serial.println(" "); -} - -// Read the DataFlash log memory : Packet Parser -void Log_Read(int start_page, int end_page) -{ - byte data; - byte log_step=0; - int packet_count=0; - int page = start_page; - - - DataFlash.StartRead(start_page); - while (page < end_page && page != -1) - { - data = DataFlash.ReadByte(); - switch(log_step) //This is a state machine to read the packets - { - case 0: - if(data==HEAD_BYTE1) // Head byte 1 - log_step++; - break; - case 1: - if(data==HEAD_BYTE2) // Head byte 2 - log_step++; - else - log_step = 0; - break; - case 2: - if(data==LOG_ATTITUDE_MSG){ - Log_Read_Attitude(); - log_step++; - - }else if(data==LOG_MODE_MSG){ - Log_Read_Mode(); - log_step++; - - }else if(data==LOG_CONTROL_TUNING_MSG){ - Log_Read_Control_Tuning(); - log_step++; - - }else if(data==LOG_NAV_TUNING_MSG){ - Log_Read_Nav_Tuning(); - log_step++; - - }else if(data==LOG_PERFORMANCE_MSG){ - Log_Read_Performance(); - log_step++; - - }else if(data==LOG_RAW_MSG){ - Log_Read_Raw(); - log_step++; - - }else if(data==LOG_CMD_MSG){ - Log_Read_Cmd(); - log_step++; - - }else if(data==LOG_STARTUP_MSG){ - Log_Read_Startup(); - log_step++; - }else { - if(data==LOG_GPS_MSG){ - Log_Read_GPS(); - log_step++; - } else { - Serial.print("Error Reading Packet: "); - Serial.print(packet_count); - log_step=0; // Restart, we have a problem... - } - } - break; - case 3: - if(data==END_BYTE){ - packet_count++; - }else{ - Serial.print("Error Reading END_BYTE "); - Serial.println(data,DEC); - } - log_step=0; // Restart sequence: new packet... - break; - } - page = DataFlash.GetPage(); - } - Serial.print("Number of packets read: "); - Serial.println(packet_count); -} - - diff --git a/Tools/ArduTracker/Mavlink_Common.h b/Tools/ArduTracker/Mavlink_Common.h deleted file mode 100644 index 48df48c6fd..0000000000 --- a/Tools/ArduTracker/Mavlink_Common.h +++ /dev/null @@ -1,219 +0,0 @@ -#ifndef Mavlink_Common_H -#define Mavlink_Common_H - -#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLIK || GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK - -uint16_t system_type = MAV_FIXED_WING; - -static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid) -{ - if (sysid != mavlink_system.sysid) - { - return 1; - } - else if (compid != mavlink_system.compid) - { - gcs.send_text(SEVERITY_LOW,"component id mismatch"); - return 0; // XXX currently not receiving correct compid from gcs - } - else return 0; // no error -} - -/** - * @brief Send low-priority messages at a maximum rate of xx Hertz - * - * This function sends messages at a lower rate to not exceed the wireless - * bandwidth. It sends one message each time it is called until the buffer is empty. - * Call this function with xx Hertz to increase/decrease the bandwidth. - */ -static void mavlink_queued_send(mavlink_channel_t chan) -{ - //send parameters one by one - if (global_data.parameter_i < global_data.param_count) - { - char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK - strcpy_P(param_name, getParamName(global_data.parameter_i)); /// XXX HACK - mavlink_msg_param_value_send(chan, - (int8_t*)param_name, - getParamAsFloat(global_data.parameter_i), - global_data.param_count,global_data.parameter_i); - global_data.parameter_i++; - } - - // request waypoints one by one - if (global_data.waypoint_receiving && - global_data.waypoint_request_i < get(PARAM_WP_TOTAL)) - { - mavlink_msg_waypoint_request_send(chan, - global_data.waypoint_dest_sysid, - global_data.waypoint_dest_compid ,global_data.waypoint_request_i); - } -} - -void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, uint16_t packet_drops) -{ - uint64_t timeStamp = micros(); - switch(id) { - - case MSG_HEARTBEAT: - mavlink_msg_heartbeat_send(chan,system_type,MAV_AUTOPILOT_ARDUPILOTMEGA); - break; - - case MSG_EXTENDED_STATUS: - { - uint8_t mode = MAV_MODE_UNINIT; - uint8_t nav_mode = MAV_NAV_VECTOR; - - switch(control_mode) { - case MANUAL: - mode = MAV_MODE_MANUAL; - break; - case CIRCLE: - mode = MAV_MODE_TEST3; - break; - case STABILIZE: - mode = MAV_MODE_GUIDED; - break; - case FLY_BY_WIRE_A: - mode = MAV_MODE_TEST1; - break; - case FLY_BY_WIRE_B: - mode = MAV_MODE_TEST2; - break; - case AUTO: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_WAYPOINT; - break; - case RTL: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_RETURNING; - break; - case LOITER: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_HOLD; - break; - case TAKEOFF: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_LIFTOFF; - break; - case LAND: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_LANDING; - break; - } - uint8_t status = MAV_STATE_ACTIVE; - uint8_t motor_block = false; - - mavlink_msg_sys_status_send(chan,mode,nav_mode,status,load*1000, - battery_voltage1*1000,motor_block,packet_drops); - break; - } - - case MSG_ATTITUDE: - { - Vector3f omega = dcm.get_gyro(); - mavlink_msg_attitude_send(chan,timeStamp,dcm.roll,dcm.pitch,dcm.yaw, - omega.x,omega.y,omega.z); - break; - } - case MSG_LOCATION: - { - float gamma = dcm.pitch; // neglecting angle of attack for now - float yaw = dcm.yaw; - mavlink_msg_global_position_send(chan,timeStamp,current_loc.lat/1.0e7f, - current_loc.lng/1.0e7f,current_loc.alt/1.0e2f,gps.ground_speed/1.0e2f*cosf(gamma)*cosf(yaw), - gps.ground_speed/1.0e2f*cosf(gamma)*sinf(yaw),gps.ground_speed/1.0e2f*sinf(gamma)); - break; - } - case MSG_LOCAL_LOCATION: - { - float gamma = dcm.pitch; // neglecting angle of attack for now - float yaw = dcm.yaw; - mavlink_msg_local_position_send(chan,timeStamp,ToRad((current_loc.lat-home.lat)/1.0e7f)*radius_of_earth, - ToRad((current_loc.lng-home.lng)/1.0e7f)*radius_of_earth*cosf(ToRad(home.lat/1.0e7f)), - (current_loc.alt-home.alt)/1.0e2f, gps.ground_speed/1.0e2f*cosf(gamma)*cosf(yaw), - gps.ground_speed/1.0e2f*cosf(gamma)*sinf(yaw),gps.ground_speed/1.0e2f*sinf(gamma)); - break; - } - case MSG_GPS_RAW: - { - mavlink_msg_gps_raw_send(chan,timeStamp,gps.status(), - gps.latitude/1.0e7f,gps.longitude/1.0e7f,gps.altitude/100.0f, - 2.0f,10.0f,gps.ground_speed/100.0f,gps.ground_course/100.0f); - break; - } - case MSG_AIRSPEED: - { - mavlink_msg_airspeed_send(chan,float(airspeed)/100.0); - break; - } - case MSG_SERVO_OUT: - { - uint8_t rssi = 1; // TODO: can we calculated this? - // receive signal strength 0(0%)-255(100%) - Serial.printf_P(PSTR("sending servo out: %d %d %d %d\n"), - servo_out[0],servo_out[1], servo_out[2], servo_out[3]); - mavlink_msg_rc_channels_scaled_send(chan, - servo_out[0],servo_out[1], - servo_out[2]*100, // account for throttle scaling 0-100 - servo_out[3],servo_out[4],servo_out[5], - servo_out[6],servo_out[7],rssi); - break; - } - case MSG_RADIO_OUT: - { - uint8_t rssi = 1; // TODO: can we calculated this? - // receive signal strength 0(0%)-255(100%) - mavlink_msg_rc_channels_raw_send(chan, - radio_out[0],radio_out[1],radio_out[2], - radio_out[3],radio_out[4],radio_out[5], - radio_out[6],radio_out[7],rssi); - break; - } - -#if HIL_MODE != HIL_MODE_ATTITUDE - case MSG_RAW_IMU: - { - Vector3f accel = imu.get_accel(); - Vector3f gyro = imu.get_gyro(); - //Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z); - //Serial.printf_P(PSTR("sending gyro: %f %f %f\n"), gyro.x, gyro.y, gyro.z); - mavlink_msg_raw_imu_send(chan,timeStamp, - accel.x*1000.0f/gravity,accel.y*1000.0f/gravity,accel.z*1000.0f/gravity, - gyro.x*1000.0f,gyro.y*1000.0f,gyro.z*1000.0f, - compass.mag_x,compass.mag_y,compass.mag_z); - mavlink_msg_raw_pressure_send(chan,timeStamp, - adc.Ch(AIRSPEED_CH),pitot.RawPress,0); - break; - } -#endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE - - case MSG_GPS_STATUS: - { - mavlink_msg_gps_status_send(chan,gps.num_sats,NULL,NULL,NULL,NULL,NULL); - break; - } - - case MSG_CURRENT_WAYPOINT: - { - mavlink_msg_waypoint_current_send(chan,get(PARAM_WP_INDEX)); - break; - } - - defualt: - break; - } -} - -void mavlink_send_text(mavlink_channel_t chan, uint8_t severity, const char *str) -{ - mavlink_msg_statustext_send(chan,severity,(const int8_t*)str); -} - -void mavlink_acknowledge(mavlink_channel_t chan, uint8_t id, uint8_t sum1, uint8_t sum2) -{ -} - -#endif // mavlink in use - -#endif // inclusion guard diff --git a/Tools/ArduTracker/Terminal_commands.txt b/Tools/ArduTracker/Terminal_commands.txt deleted file mode 100644 index 99791fb290..0000000000 --- a/Tools/ArduTracker/Terminal_commands.txt +++ /dev/null @@ -1,118 +0,0 @@ - = {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} -show - heartbeat - attitude - location - command - severity -hide - heartbeat - attitude - location - command - all -copy cmd -echo -groundstart -inithome -k -? -print - altitude - altmaxcruiseerr - aspmaxcruiseerr - attitude - commands - ctrlmode - curwaypts - flightmodes - holdalt - imax - kp - ki - kd - kff {pitchcomp|ruddermix|pitchtothrottle} - kpitchcruise - location - navrolllimit - navsettings - pitchmax - pitchmin - pitchtarget - pressure - rlocation [home] - speed - targetaxis1 - targetaxis2 - targetneutral - throttlecruise - throttlemax - throttlemin - tuning -release - all - ctrlmode - rcin -reset commands -rtl -set - altmaxcruiseerr - aspmaxcruiseerr - cmd {here|{ }} - commandtype = {waypoint|takeoff|land|landoptions|loiter|loiternturns|loitertime|delay|resetindex|airspeedcruise|throttlecruise|resethome|index|rtl|relay|servo} - cmdcount - cmdindex - cruise - ctrlmode {manual|circle|stabilize|fbwa|fbwb|auto|rtl|loiter|takeoff|land|radio} - holdalt - imax - kp - ki - kd - kff {pitchcomp|ruddermix|pitchtothrottle} - kpitchcruise - loiterradius - navrolllimit - pitchmax - pitchmin - pitchtarget - rcin - rcout - target alt - target here - targetaxis1 - targetaxis2 - targetmode - targetneutral - target - throttlecruise - throttlefailsafe - throttlefailsafeaction - throttlemax - throttlemin - wpradius - xtrackentryangle - xtrackgain -status - climb - control - cruise - gps - landing - loiter - mixing - navigation - navpitch - navroll - pid - rc - rcinputch - rcin - rcout - rcpwm - rctrim - system - takeoff - telemetry - throttle - waypoints - \ No newline at end of file diff --git a/Tools/ArduTracker/climb_rate.pde b/Tools/ArduTracker/climb_rate.pde deleted file mode 100644 index 85674ef3b8..0000000000 --- a/Tools/ArduTracker/climb_rate.pde +++ /dev/null @@ -1,92 +0,0 @@ -struct DataPoint { - unsigned long x; - long y; -}; - -DataPoint history[ALTITUDE_HISTORY_LENGTH]; // Collection of (x,y) points to regress a rate of change from -unsigned char index; // Index in history for the current data point - -unsigned long xoffset; -unsigned char n; - -// Intermediate variables for regression calculation -long xi; -long yi; -long xiyi; -unsigned long xi2; - - -void add_altitude_data(unsigned long xl, long y) -{ - unsigned char i; - int dx; - - //Reset the regression if our X variable overflowed - if (xl < xoffset) - n = 0; - - //To allow calculation of sum(xi*yi), make sure X hasn't exceeded 2^32/2^15/length - if (xl - xoffset > 131072/ALTITUDE_HISTORY_LENGTH) - n = 0; - - if (n == ALTITUDE_HISTORY_LENGTH) { - xi -= history[index].x; - yi -= history[index].y; - xiyi -= (long)history[index].x * history[index].y; - xi2 -= history[index].x * history[index].x; - } else { - if (n == 0) { - xoffset = xl; - xi = 0; - yi = 0; - xiyi = 0; - xi2 = 0; - } - n++; - } - - history[index].x = xl - xoffset; - history[index].y = y; - - xi += history[index].x; - yi += history[index].y; - xiyi += (long)history[index].x * history[index].y; - xi2 += history[index].x * history[index].x; - - if (++index >= ALTITUDE_HISTORY_LENGTH) - index = 0; -} - -void recalc_climb_rate() -{ - float slope = ((float)xi*(float)yi - ALTITUDE_HISTORY_LENGTH*(float)xiyi) / ((float)xi*(float)xi - ALTITUDE_HISTORY_LENGTH*(float)xi2); - climb_rate = (int)(slope*100); -} - -void print_climb_debug_info() -{ - unsigned char i, j; - recalc_climb_rate(); - SendDebugln_P("Climb rate:"); - for (i=0; i= ALTITUDE_HISTORY_LENGTH) j -= ALTITUDE_HISTORY_LENGTH; - SendDebug_P(" "); - SendDebug(j,DEC); - SendDebug_P(": "); - SendDebug(history[j].x,DEC); - SendDebug_P(", "); - SendDebugln(history[j].y,DEC); - } - SendDebug_P(" sum(xi) = "); - SendDebugln(xi,DEC); - SendDebug_P(" sum(yi) = "); - SendDebugln(yi,DEC); - SendDebug_P(" sum(xi*yi) = "); - SendDebugln(xiyi,DEC); - SendDebug_P(" sum(xi^2) = "); - SendDebugln(xi2,DEC); - SendDebug_P(" Climb rate = "); - SendDebug((float)climb_rate/100,2); - SendDebugln_P(" m/s"); -} diff --git a/Tools/ArduTracker/command_description.txt b/Tools/ArduTracker/command_description.txt deleted file mode 100644 index 5a2ce9c970..0000000000 --- a/Tools/ArduTracker/command_description.txt +++ /dev/null @@ -1,76 +0,0 @@ -ArduPilotMega 1.0.0 Commands - -Command Structure in bytes -0 0x00 byte Command ID -1 0x01 byte Parameter 1 -2 0x02 int Parameter 2 -3 0x03 .. -4 0x04 long Parameter 3 -5 0x05 .. -6 0x06 .. -7 0x07 .. -8 0x08 long Parameter 4 -9 0x09 .. -10 0x0A .. -11 0x0B .. - -0x00 Reserved -.... -0x0F Reserved - -*********************************** -Commands 0x10 to 0x2F are commands that have a end criteria, eg "reached waypoint" or "reached altitude" -*********************************** -Command ID Name Parameter 1 Altitude Latitude Longitude -0x10 CMD_WAYPOINT - altitude lat lon -0x11 CMD_LOITER (indefinitely) - altitude lat lon -0x12 CMD_LOITER_N_TURNS turns altitude lat lon -0x13 CMD_LOITER_TIME time (seconds*10) altitude lat lon -0x14 CMD_RTL - altitude lat lon -0x15 CMD_LAND - altitude lat lon -0x16 CMD_TAKEOFF takeoff pitch altitude - - - NOTE: for command 0x16 the value takeoff pitch specifies the minimum pitch for the case with airspeed sensor and the target pitch for the case without. - - -*********************************** -May Commands - these commands are optional to finish -Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4 -0x20 CMD_DELAY - - time (milliseconds) - -0x21 CMD_CLIMB rate (cm/sec) alt (finish) - - -0x22 CMD_LAND_OPTIONS distance to WP airspeed m/s, throttle %, pitch deg - - - -*********************************** -Unexecuted commands >= 0x20 are dropped when ready for the next command <= 0x1F so plan/queue commands accordingly! -For example if you had a string of 0x21 commands following a 0x10 command that had not finished when the waypoint was -reached, the unexecuted 0x21 commands would be skipped and the next command <= 0x1F would be loaded -*********************************** -Now Commands - these commands are executed once until no more new now commands are available -0x31 CMD_RESET_INDEX -0x32 CMD_GOTO_INDEX index repeat count -0x33 CMD_GETVAR_INDEX variable ID -0x34 CMD_SENDVAR_INDEX off/on = 0/1 -0x35 CMD_TELEMETRY off/on = 0/1 - -0x40 CMD_THROTTLE_CRUISE speed -0x41 CMD_AIRSPEED_CRUISE speed -0x44 CMD_RESET_HOME altitude lat lon - -0x60 CMD_KP_GAIN array index gain value*100,000 -0x61 CMD_KI_GAIN array index gain value*100,000 -0x62 CMD_KD_GAIN array index gain value*100,000 -0x63 CMD_KI_MAX array index gain value*100,000 -0x64 CMD_KFF_GAIN array index gain value*100,000 - -0x70 CMD_RADIO_TRIM array index value -0x71 CMD_RADIO_MAX array index value -0x72 CMD_RADIO_MIN array index value -0x73 CMD_ELEVON_TRIM array index value (index 0 = elevon 1 trim, 1 = elevon 2 trim) -0x75 CMD_INDEX index - -0x80 CMD_REPEAT type value delay in sec repeat count -0x81 CMD_RELAY (0,1 to change swicth position) -0x82 CMD_SERVO number value - - diff --git a/Tools/ArduTracker/commands.pde b/Tools/ArduTracker/commands.pde deleted file mode 100644 index 1633e9a735..0000000000 --- a/Tools/ArduTracker/commands.pde +++ /dev/null @@ -1,239 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -void init_commands() -{ - //read_EEPROM_waypoint_info(); - set(PARAM_WP_INDEX,0); - command_must_index = 0; - command_may_index = 0; - next_command.id = CMD_BLANK; -} - -void update_auto() -{ - if (get(PARAM_WP_INDEX) == get(PARAM_WP_TOTAL)){ - return_to_launch(); - //wp_index = 0; - } -} - -void reload_commands() -{ - init_commands(); - //read_command_index(); // Get wp_index = command_must_index from EEPROM - if(get(PARAM_WP_INDEX) > 0){ - decrement_WP_index; - } -} - -// Getters -// ------- -struct Location get_wp_with_index(int i) -{ - struct Location temp; - long mem; - - - // Find out proper location in memory by using the start_byte position + the index - // -------------------------------------------------------------------------------- - if (i > get(PARAM_WP_TOTAL)) { - temp.id = CMD_BLANK; - }else{ - // read WP position - mem = (WP_START_BYTE) + (i * WP_SIZE); - temp.id = eeprom_read_byte((uint8_t*)mem); - mem++; - temp.p1 = eeprom_read_byte((uint8_t*)mem); - mem++; - temp.alt = (long)eeprom_read_dword((uint32_t*)mem); - mem += 4; - temp.lat = (long)eeprom_read_dword((uint32_t*)mem); - mem += 4; - temp.lng = (long)eeprom_read_dword((uint32_t*)mem); - } - return temp; -} - -// Setters -// ------- -void set_wp_with_index(struct Location temp, int i) -{ - - i = constrain(i,0,get(PARAM_WP_TOTAL)); - uint32_t mem = WP_START_BYTE + (i * WP_SIZE); - - eeprom_write_byte((uint8_t *) mem, temp.id); - - mem++; - eeprom_write_byte((uint8_t *) mem, temp.p1); - - mem++; - eeprom_write_dword((uint32_t *) mem, temp.alt); - - mem += 4; - eeprom_write_dword((uint32_t *) mem, temp.lat); - - mem += 4; - eeprom_write_dword((uint32_t *) mem, temp.lng); -} - -void increment_WP_index() -{ - if(get(PARAM_WP_INDEX) < get(PARAM_WP_TOTAL)){ - set(PARAM_WP_INDEX, get(PARAM_WP_INDEX) +1); - SendDebug("MSG WP index is incremented to "); - SendDebugln(get(PARAM_WP_INDEX),DEC); - }else{ - SendDebug("MSG Failed to increment WP index of "); - SendDebugln(get(PARAM_WP_INDEX),DEC); - } -} -void decrement_WP_index() -{ - if(get(PARAM_WP_INDEX) > 0){ - set(PARAM_WP_INDEX, get(PARAM_WP_INDEX) -1); - } -} - -long read_alt_to_hold() -{ - byte options = get(PARAM_CONFIG); - - // save the alitude above home option - if(options & HOLD_ALT_ABOVE_HOME){ - int32_t temp = get(PARAM_ALT_HOLD_HOME); - return temp + home.alt; - }else{ - return current_loc.alt; - } -} - -void loiter_at_location() -{ - next_WP = current_loc; -} - -// add a new command at end of command set to RTL. -void return_to_launch(void) -{ - //so we know where we are navigating from - next_WP = current_loc; - - // home is WP 0 - // ------------ - set(PARAM_WP_INDEX,0); - - // Loads WP from Memory - // -------------------- - set_next_WP(&home); - - // Altitude to hold over home - // Set by configuration tool - // ------------------------- - next_WP.alt = read_alt_to_hold(); -} - -struct Location get_LOITER_home_wp() -{ - // read home position - struct Location temp = get_wp_with_index(0); - temp.id = CMD_LOITER; - - temp.alt = read_alt_to_hold(); - return temp; -} - -/* -This function stores waypoint commands -It looks to see what the next command type is and finds the last command. -*/ -void set_next_WP(struct Location *wp) -{ - //GCS.send_text(SEVERITY_LOW,"load WP"); - SendDebug("MSG wp_index: "); - SendDebugln(get(PARAM_WP_INDEX),DEC); - gcs.send_message(MSG_COMMAND_LIST, get(PARAM_WP_INDEX)); - - // copy the current WP into the OldWP slot - // --------------------------------------- - prev_WP = next_WP; - // Load the next_WP slot - // --------------------- - next_WP = *wp; - // offset the altitude relative to home position - // --------------------------------------------- - next_WP.alt += home.alt; - - // used to control FBW and limit the rate of climb - // ----------------------------------------------- - target_altitude = current_loc.alt; - if(prev_WP.id != CMD_TAKEOFF && (next_WP.id == CMD_WAYPOINT || next_WP.id == CMD_LAND)) - offset_altitude = next_WP.alt - prev_WP.alt; - else - offset_altitude = 0; - - - // zero out our loiter vals to watch for missed waypoints - loiter_delta = 0; - loiter_sum = 0; - loiter_total = 0; - - float rads = (abs(next_WP.lat)/t7) * 0.0174532925f; - //377,173,810 / 10,000,000 = 37.717381 * 0.0174532925 = 0.658292482926943 - scaleLongDown = cosf(rads); - scaleLongUp = 1.0f/cosf(rads); - - // this is handy for the groundstation - wp_totalDistance = getDistance(¤t_loc, &next_WP); - wp_distance = wp_totalDistance; - - gcs.print_current_waypoints(); - - target_bearing = get_bearing(¤t_loc, &next_WP); - old_target_bearing = target_bearing; - // this is used to offset the shrinking longitude as we go towards the poles - - // set a new crosstrack bearing - // ---------------------------- - reset_crosstrack(); -} - - -// run this at setup on the ground -// ------------------------------- -void init_home() -{ - SendDebugln("MSG: init home"); - - // Extra read just in case - // ----------------------- - //gps.Read(); - - // block until we get a good fix - // ----------------------------- - while (!gps.new_data || !gps.fix) { - gps.update(); - } - home.id = CMD_WAYPOINT; - home.lng = gps.longitude; // Lon * 10**7 - home.lat = gps.latitude; // Lat * 10**7 - home.alt = gps.altitude; - home_is_set = TRUE; - - // ground altitude in centimeters for pressure alt calculations - // ------------------------------------------------------------ - ground_alt = gps.altitude; - press_alt = gps.altitude; // Set initial value for filter - //save_pressure_data(); - - // Save Home to EEPROM - // ------------------- - set_wp_with_index(home, 0); - - // Save prev loc - // ------------- - prev_WP = home; -} - - - diff --git a/Tools/ArduTracker/commands_process.pde b/Tools/ArduTracker/commands_process.pde deleted file mode 100644 index 1c759774d9..0000000000 --- a/Tools/ArduTracker/commands_process.pde +++ /dev/null @@ -1,466 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -// called by 10 Hz loop -// -------------------- -void update_commands(void) -{ - // This function loads commands into three buffers - // when a new command is loaded, it is processed with process_XXX() - // ---------------------------------------------------------------- - if((home_is_set == FALSE) || (control_mode != AUTO)){ - return; // don't do commands - } - - if(control_mode == AUTO){ - load_next_command(); - process_next_command(); - } - - //verify_must(); - //verify_may(); -} - - -void load_next_command() -{ - // fetch next command if it's empty - // -------------------------------- - if(next_command.id == CMD_BLANK){ - next_command = get_wp_with_index(get(PARAM_WP_INDEX)+1); - if(next_command.id != CMD_BLANK){ - //SendDebug("MSG fetch found new cmd from list at index "); - //SendDebug((get(PARAM_WP_INDEX)+1),DEC); - //SendDebug(" with cmd id "); - //SendDebugln(next_command.id,DEC); - } - } - - // If the preload failed, return or just Loiter - // generate a dynamic command for RTL - // -------------------------------------------- - if(next_command.id == CMD_BLANK){ - // we are out of commands! - gcs.send_text(SEVERITY_LOW,"out of commands!"); - //SendDebug("MSG out of commands, get(PARAM_WP_INDEX): "); - //SendDebugln(get(PARAM_WP_INDEX),DEC); - - - switch (control_mode){ - case LAND: - // don't get a new command - break; - - default: - next_command = get_LOITER_home_wp(); - //SendDebug("MSG Preload RTL cmd id: "); - //SendDebugln(next_command.id,DEC); - break; - } - } -} - -void process_next_command() -{ - // these are waypoint/Must commands - // --------------------------------- - if (command_must_index == 0){ // no current command loaded - if (next_command.id >= 0x10 && next_command.id <= 0x1F ){ - increment_WP_index(); - //save_command_index(); // to Recover from in air Restart - command_must_index = get(PARAM_WP_INDEX); - - //SendDebug("MSG new command_must_id "); - //SendDebug(next_command.id,DEC); - //SendDebug(" index:"); - //SendDebugln(command_must_index,DEC); - if (get(PARAM_LOG_BITMASK) & MASK_LOG_CMD) - Log_Write_Cmd(get(PARAM_WP_INDEX), &next_command); - process_must(); - } - } - - // these are May commands - // ---------------------- - if (command_may_index == 0){ - if (next_command.id >= 0x20 && next_command.id <= 0x2F ){ - increment_WP_index();// this command is from the command list in EEPROM - command_may_index = get(PARAM_WP_INDEX); - //Serial.print("new command_may_index "); - //Serial.println(command_may_index,DEC); - if (get(PARAM_LOG_BITMASK) & MASK_LOG_CMD) - Log_Write_Cmd(get(PARAM_WP_INDEX), &next_command); - process_may(); - } - } - - // these are do it now commands - // --------------------------- - if (next_command.id >= 0x30){ - increment_WP_index();// this command is from the command list in EEPROM - if (get(PARAM_LOG_BITMASK) & MASK_LOG_CMD) - Log_Write_Cmd(get(PARAM_WP_INDEX), &next_command); - process_now(); - } - -} -/* -These functions implement the waypoint commands. -*/ -void process_must() -{ - //SendDebug("process must index: "); - //SendDebugln(command_must_index,DEC); - - gcs.send_text(SEVERITY_LOW,"New cmd: "); - gcs.send_message(MSG_COMMAND_LIST, get(PARAM_WP_INDEX)); - - // clear May indexes - command_may_index = 0; - command_may_ID = 0; - - command_must_ID = next_command.id; - - // loads the waypoint into Next_WP struct - // -------------------------------------- - set_next_WP(&next_command); - - // invalidate command so a new one is loaded - // ----------------------------------------- - next_command.id = 0; - - // reset navigation integrators - // ------------------------- - reset_I(); - - switch(command_must_ID){ - - case CMD_TAKEOFF: // TAKEOFF! - // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 - takeoff_pitch = (int)next_command.p1 * 100; - takeoff_altitude = (int)next_command.alt; - next_WP.lat = home.lat + 1000; // so we don't have bad calcs - next_WP.lng = home.lng + 1000; // so we don't have bad calcs - takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction - break; - - case CMD_WAYPOINT: // Navigate to Waypoint - break; - - case CMD_LAND: // LAND to Waypoint - break; - - case CMD_LOITER: // Loiter indefinitely - break; - - case CMD_LOITER_N_TURNS: // Loiter N Times - loiter_total = next_command.p1 * 360; - break; - - case CMD_LOITER_TIME: - loiter_time = millis(); - loiter_time_max = next_command.p1 * 10000; // seconds * 10 * 1000 - break; - - case CMD_RTL: - break; - } -} - -void process_may() -{ - //Serial.print("process_may cmd# "); - //Serial.println(get(PARAM_WP_INDEX),DEC); - command_may_ID = next_command.id; - - // invalidate command so a new one is loaded - // ----------------------------------------- - next_command.id = 0; - - gcs.send_text(SEVERITY_LOW," New may command loaded:"); - gcs.send_message(MSG_COMMAND_LIST, get(PARAM_WP_INDEX)); - // do the command - // -------------- - switch(command_may_ID){ - - case CMD_DELAY: // Navigate to Waypoint - delay_start = millis(); - delay_timeout = next_command.lat; - break; - - case CMD_LAND_OPTIONS: // Land this puppy - gcs.send_text(SEVERITY_LOW,"Landing options set"); - - // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 - landing_pitch = next_command.lng * 100; - set(PARAM_TRIM_AIRSPEED, next_command.alt * 100); - set(PARAM_TRIM_THROTTLE,next_command.lat); - landing_distance = next_command.p1; - //landing_roll = command.lng; - - SendDebug("MSG: throttle_cruise = "); - SendDebugln(get(PARAM_TRIM_THROTTLE),DEC); - break; - - default: - break; - } -} - -void process_now() -{ - const float t5 = 100000.0; - //Serial.print("process_now cmd# "); - //Serial.println(get(PARAM_WP_INDEX),DEC); - - byte id = next_command.id; - - // invalidate command so a new one is loaded - // ----------------------------------------- - next_command.id = 0; - - gcs.send_text(SEVERITY_LOW, " New now command loaded: "); - gcs.send_message(MSG_COMMAND_LIST, get(PARAM_WP_INDEX)); - - // do the command - // -------------- - switch(id){ - - //case CMD_AP_MODE: - //next_command.p1 = constrain(next_command.p1, MANUAL, LAND); - //set_mode(next_command.p1); - //break; - - case CMD_RESET_INDEX: - init_commands(); - break; - - case CMD_GETVAR_INDEX: - // - break; - - case CMD_SENDVAR_INDEX: - // - break; - - case CMD_TELEMETRY: - // - break; - - case CMD_AIRSPEED_CRUISE: - set(PARAM_TRIM_AIRSPEED,next_command.p1 * 100); - break; - - case CMD_THROTTLE_CRUISE: - set(PARAM_TRIM_THROTTLE,next_command.p1); - break; - - case CMD_RESET_HOME: - init_home(); - break; - - case CMD_KP_GAIN: - pid_index[next_command.p1]->kP(next_command.alt/t5); - pid_index[next_command.p1]->save_gains(); - break; - case CMD_KI_GAIN: - pid_index[next_command.p1]->kI(next_command.alt/t5); - pid_index[next_command.p1]->save_gains(); - break; - case CMD_KD_GAIN: - pid_index[next_command.p1]->kD(next_command.alt/t5); - pid_index[next_command.p1]->save_gains(); - break; - - case CMD_KI_MAX: - pid_index[next_command.p1]->imax(next_command.alt/t5); - pid_index[next_command.p1]->save_gains(); - break; - - case CMD_KFF_GAIN: - { - float val = next_command.alt/t5; - switch(next_command.p1) - { - case CASE_PITCH_COMP: set(PARAM_KFF_PTCHCOMP,val); break; - case CASE_RUDDER_MIX: set(PARAM_KFF_RDDRMIX,val); break; - case CASE_P_TO_T: set(PARAM_KFF_PTCH2THR,val); break; - } - } - break; - - case CMD_RADIO_TRIM: - set_radio_trim(next_command.p1,next_command.alt); - break; - - case CMD_RADIO_MAX: - set_radio_max(next_command.p1,next_command.alt); - break; - - case CMD_RADIO_MIN: - set_radio_min(next_command.p1,next_command.alt); - break; - - case CMD_REPEAT: - new_event(&next_command); - break; - - case CMD_SERVO: - //Serial.print("CMD_SERVO "); - //Serial.print(next_command.p1,DEC); - //Serial.print(" PWM: "); - //Serial.println(next_command.alt,DEC); - APM_RC.OutputCh(next_command.p1, next_command.alt); - break; - - case CMD_INDEX: - command_must_index = 0; - command_may_index = 0; - set(PARAM_WP_INDEX,next_command.p1 - 1); - break; - - case CMD_RELAY: - if (next_command.p1 == 0) { - relay_on(); - } else if (next_command.p1 == 1) { - relay_off(); - }else{ - relay_toggle(); - } - break; - } -} - -// Verify commands -// --------------- -void verify_must() -{ - float power; - - switch(command_must_ID) { - - case CMD_TAKEOFF: // Takeoff! - //Serial.print("verify_must cmd# "); - //Serial.println(command_must_index,DEC); - - if (gps.ground_speed > 300){ - if(hold_course == -1){ - // save our current course to take off - #if MAGNETOMETER == ENABLED - hold_course = dcm.yaw_sensor; - #else - hold_course = gps.ground_course; - #endif - } - } - if(hold_course > -1){ - // recalc bearing error with hold_course; - nav_bearing = hold_course; - // recalc bearing error - calc_bearing_error(); - } - if (current_loc.alt > (home.alt + takeoff_altitude)) { - command_must_index = 0; - hold_course = -1; - takeoff_complete = true; - } - break; - - - - case CMD_LAND: - // we don't verify landing - we never go to a new Must command after Land. - if ( ((wp_distance > 0) && (wp_distance <= (2*gps.ground_speed/100))) || (current_loc.alt <= next_WP.alt + 300) ) - { - land_complete = 1; //Set land_complete if we are within 2 seconds distance or within 3 meters altitude - if(hold_course == -1){ - // save our current course to land - //hold_course = yaw_sensor; - // save the course line of the runway to land - hold_course = crosstrack_bearing; - } - } - if(hold_course > -1){ - // recalc bearing error with hold_course; - nav_bearing = hold_course; - // recalc bearing error - calc_bearing_error(); - } - update_crosstrack(); - - break; - - case CMD_WAYPOINT: // reach a waypoint - hold_course == -1; - update_crosstrack(); - if ((wp_distance > 0) && (wp_distance <= get(PARAM_WP_RADIUS))) { - SendDebug("MSG REACHED_WAYPOINT #"); - SendDebugln(command_must_index,DEC); - // clear the command queue; - command_must_index = 0; - } - // add in a more complex case - // Doug to do - if(loiter_sum > 300){ - gcs.send_text(SEVERITY_MEDIUM," Missed WP"); - command_must_index = 0; - } - break; - - case CMD_LOITER_N_TURNS: // LOITER N times - update_loiter(); - calc_bearing_error(); - if(loiter_sum > loiter_total) { - loiter_total = 0; - gcs.send_text(SEVERITY_LOW," LOITER orbits complete "); - // clear the command queue; - command_must_index = 0; - } - break; - - case CMD_LOITER_TIME: // loiter N milliseconds - update_loiter(); - calc_bearing_error(); - if ((millis() - loiter_time) > loiter_time_max) { - gcs.send_text(SEVERITY_LOW," LOITER time complete "); - command_must_index = 0; - } - break; - - case CMD_RTL: - if (wp_distance <= 30) { - //Serial.println("REACHED_HOME"); - command_must_index = 0; - } - break; - - case CMD_LOITER: // Just plain LOITER - update_loiter(); - calc_bearing_error(); - break; - - default: - gcs.send_text(SEVERITY_HIGH," No current Must commands"); - break; - } -} - -void verify_may() -{ - switch(command_may_ID) { - case CMD_DELAY: - if ((millis() - delay_start) > delay_timeout){ - command_may_index = 0; - delay_timeout = 0; - } - - case CMD_LAND_OPTIONS: - if ((wp_distance > 0) && (wp_distance <= landing_distance)) { - //Serial.print("XXX REACHED_WAYPOINT #"); - //Serial.println(command_must_index,DEC); - // clear the command queue; - command_may_index = 0; - } - break; - } -} - diff --git a/Tools/ArduTracker/config.h b/Tools/ArduTracker/config.h deleted file mode 100644 index 95a8bce8c9..0000000000 --- a/Tools/ArduTracker/config.h +++ /dev/null @@ -1,609 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -// -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// -// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING -// -// DO NOT EDIT this file to adjust your configuration. Create your own -// APM_Config.h and use APM_Config.h.example as a reference. -// -// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING -/// -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// -// Default and automatic configuration details. -// -// Notes for maintainers: -// -// - Try to keep this file organised in the same order as APM_Config.h.example -// - -#include "defines.h" - -/// -/// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that -/// change in your local copy of APM_Config.h. -/// -#include "APM_Config.h" // <== THIS INCLUDE, DO NOT EDIT IT. EVER. -/// -/// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that -/// change in your local copy of APM_Config.h. -/// - -// Just so that it's completely clear... -#define ENABLED 1 -#define DISABLED 0 - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// HARDWARE CONFIGURATION AND CONNECTIONS -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// AIRSPEED_SENSOR -// AIRSPEED_RATIO -// -#ifndef AIRSPEED_SENSOR -# define AIRSPEED_SENSOR DISABLED -#endif -#ifndef AIRSPEED_RATIO -# define AIRSPEED_RATIO 1.9936 // Note - this varies from the value in ArduPilot due to the difference in ADC resolution -#endif - -////////////////////////////////////////////////////////////////////////////// -// HIL_PROTOCOL OPTIONAL -// HIL_MODE OPTIONAL -// HIL_PORT OPTIONAL - -#if HIL_MODE != HIL_MODE_DISABLED - - # undef GPS_PROTOCOL - # define GPS_PROTOCOL GPS_PROTOCOL_NONE - - #ifndef HIL_PROTOCOL - # error Must define HIL_PROTOCOL if HIL_MODE is not disabled - #endif - - #ifndef HIL_PORT - # error Must define HIL_PORT if HIL_PROTOCOL is set - #endif - -#endif - -#if HIL_MODE != HIL_MODE_DISABLED - -// check xplane settings -#if HIL_PROTOCOL == HIL_PROTOCOL_XPLANE - // MAGNETOMETER not supported by XPLANE -# undef MAGNETOMETER -# define MAGNETOMETER DISABLED -# if HIL_MODE != HIL_MODE_ATTITUDE -# error HIL_PROTOCOL_XPLANE requires HIL_MODE_ATTITUDE -# endif -#endif - -#endif // HIL_MODE != HIL_MODE_DISABLED - -////////////////////////////////////////////////////////////////////////////// -// GPS_PROTOCOL -// -// Note that this test must follow the HIL_PROTOCOL block as the HIL -// setup may override the GPS configuration. -// -#ifndef GPS_PROTOCOL -# error XXX -# error XXX You must define GPS_PROTOCOL in APM_Config.h, or select a HIL configuration. -# error XXX -#endif - -////////////////////////////////////////////////////////////////////////////// -// GCS_PROTOCOL -// GCS_PORT -// -#ifndef GCS_PROTOCOL -# define GCS_PROTOCOL GCS_PROTOCOL_NONE -#endif - -#ifndef DEBUGTERMINAL_VERBOSE -# define DEBUGTERMINAL_VERBOSE 1 //Set this to 1 to get more detail in DEBUGTERMINAL messages, or 0 to save flash space -#endif - - -////////////////////////////////////////////////////////////////////////////// -// Serial port speeds. -// -#ifndef SERIAL0_BAUD -# define SERIAL0_BAUD 115200 -#endif -#ifndef SERIAL3_BAUD -# define SERIAL3_BAUD 57600 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// Battery monitoring -// -#ifndef BATTERY_EVENT -# define BATTERY_EVENT DISABLED -#endif -#ifndef BATTERY_TYPE -# define BATTERY_TYPE 0 -#endif -#ifndef LOW_VOLTAGE -# define LOW_VOLTAGE 11.4 -#endif -#ifndef VOLT_DIV_RATIO -# define VOLT_DIV_RATIO 3.0 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// INPUT_VOLTAGE -// -#ifndef INPUT_VOLTAGE -# define INPUT_VOLTAGE 5.0 -#endif - -////////////////////////////////////////////////////////////////////////////// -// MAGNETOMETER -// NOTE - There is no support for using the magnetometer in v1.0 -#ifndef MAGNETOMETER -# define MAGNETOMETER DISABLED -#endif - - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// RADIO CONFIGURATION -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - - -////////////////////////////////////////////////////////////////////////////// -// Radio channel limits -// -// Note that these are not called out in APM_Config.h.example as there -// is currently no configured behaviour for these channels. -// -#ifndef CH5_MIN -# define CH5_MIN 1000 -#endif -#ifndef CH5_MAX -# define CH5_MAX 2000 -#endif -#ifndef CH6_MIN -# define CH6_MIN 1000 -#endif -#ifndef CH6_MAX -# define CH6_MAX 2000 -#endif -#ifndef CH7_MIN -# define CH7_MIN 1000 -#endif -#ifndef CH7_MAX -# define CH7_MAX 2000 -#endif -#ifndef CH8_MIN -# define CH8_MIN 1000 -#endif -#ifndef CH8_MAX -# define CH8_MAX 2000 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// FLIGHT_MODE -// FLIGHT_MODE_CHANNEL -// -#ifndef FLIGHT_MODE_CHANNEL -# define FLIGHT_MODE_CHANNEL 8 -#endif -#if (FLIGHT_MODE_CHANNEL != 5) && (FLIGHT_MODE_CHANNEL != 6) && (FLIGHT_MODE_CHANNEL != 7) && (FLIGHT_MODE_CHANNEL != 8) -# error XXX -# error XXX You must set FLIGHT_MODE_CHANNEL to 5, 6, 7 or 8 -# error XXX -#endif - -#if !defined(FLIGHT_MODE_1) -# define FLIGHT_MODE_1 FLY_BY_WIRE_A -#endif -#if !defined(FLIGHT_MODE_2) -# define FLIGHT_MODE_2 FLY_BY_WIRE_A -#endif -#if !defined(FLIGHT_MODE_3) -# define FLIGHT_MODE_3 STABILIZE -#endif -#if !defined(FLIGHT_MODE_4) -# define FLIGHT_MODE_4 STABILIZE -#endif -#if !defined(FLIGHT_MODE_5) -# define FLIGHT_MODE_5 MANUAL -#endif -#if !defined(FLIGHT_MODE_6) -# define FLIGHT_MODE_6 MANUAL -#endif - - -////////////////////////////////////////////////////////////////////////////// -// THROTTLE_FAILSAFE -// THROTTLE_FS_VALUE -// THROTTLE_FAILSAFE_ACTION -// -#ifndef THROTTLE_FAILSAFE -# define THROTTLE_FAILSAFE DISABLED -#endif -#ifndef THROTTE_FS_VALUE -# define THROTTLE_FS_VALUE 975 -#endif -#ifndef THROTTLE_FAILSAFE_ACTION -# define THROTTLE_FAILSAFE_ACTION 2 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// AUTO_TRIM -// -#ifndef AUTO_TRIM -# define AUTO_TRIM ENABLED -#endif - - -////////////////////////////////////////////////////////////////////////////// -// THROTTLE_REVERSE -// -#ifndef THROTTLE_REVERSE -# define THROTTLE_REVERSE DISABLED -#endif - - -////////////////////////////////////////////////////////////////////////////// -// ENABLE_STICK_MIXING -// -#ifndef ENABLE_STICK_MIXING -# define ENABLE_STICK_MIXING ENABLED -#endif - - -////////////////////////////////////////////////////////////////////////////// -// THROTTLE_OUT -// -#ifndef THROTTE_OUT -# define THROTTLE_OUT ENABLED -#endif - - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// STARTUP BEHAVIOUR -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// GROUND_START_DELAY -// -#ifndef GROUND_START_DELAY -# define GROUND_START_DELAY 0 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// ENABLE_AIR_START -// -#ifndef ENABLE_AIR_START -# define ENABLE_AIR_START DISABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// ENABLE REVERSE_SWITCH -// -#ifndef REVERSE_SWITCH -# define REVERSE_SWITCH ENABLED -#endif - - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// FLIGHT AND NAVIGATION CONTROL -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// Altitude measurement and control. -// -#ifndef AOA -# define AOA 100 // XXX still 100ths of a degree -#endif -#ifndef ALT_EST_GAIN -# define ALT_EST_GAIN 0.01 -#endif -#ifndef ALTITUDE_MIX -# define ALTITUDE_MIX 0 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// AIRSPEED_CRUISE -// -#ifndef AIRSPEED_CRUISE -# define AIRSPEED_CRUISE 1000 // 10 m/s * 100 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// FLY_BY_WIRE_B airspeed control -// -#ifndef AIRSPEED_FBW_MIN -# define AIRSPEED_FBW_MIN 6 -#endif -#ifndef AIRSPEED_FBW_MAX -# define AIRSPEED_FBW_MAX 30 -#endif -#ifndef THROTTLE_ALT_P -# define THROTTLE_ALT_P 0.32 -#endif -#ifndef THROTTLE_ALT_I -# define THROTTLE_ALT_I 0.0 -#endif -#ifndef THROTTLE_ALT_D -# define THROTTLE_ALT_D 0.0 -#endif -#ifndef THROTTLE_ALT_INT_MAX -# define THROTTLE_ALT_INT_MAX 20 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// Throttle control -// -#ifndef THROTTLE_MIN -# define THROTTLE_MIN 0 -#endif -#ifndef THROTTLE_CRUISE -# define THROTTLE_CRUISE 45 -#endif -#ifndef THROTTLE_MAX -# define THROTTLE_MAX 75 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// Autopilot control limits -// -#ifndef HEAD_MAX -# define HEAD_MAX 4500 // deg * 100 -#endif -#ifndef PITCH_MAX -# define PITCH_MAX 1500 // deg * 100 -#endif -#ifndef PITCH_MIN -# define PITCH_MIN -2500 // deg * 100 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// Attitude control gains -// -#ifndef SERVO_ROLL_P -# define SERVO_ROLL_P 0.4 -#endif -#ifndef SERVO_ROLL_I -# define SERVO_ROLL_I 0.0 -#endif -#ifndef SERVO_ROLL_D -# define SERVO_ROLL_D 0.0 -#endif -#ifndef SERVO_ROLL_INT_MAX -# define SERVO_ROLL_INT_MAX 5 -#endif -#ifndef ROLL_SLEW_LIMIT -# define ROLL_SLEW_LIMIT 0 -#endif -#ifndef SERVO_PITCH_P -# define SERVO_PITCH_P 0.6 -#endif -#ifndef SERVO_PITCH_I -# define SERVO_PITCH_I 0.0 -#endif -#ifndef SERVO_PITCH_D -# define SERVO_PITCH_D 0.0 -#endif -#ifndef SERVO_PITCH_INT_MAX -# define SERVO_PITCH_INT_MAX 5 -#endif -#ifndef PITCH_COMP -# define PITCH_COMP 0.2 -#endif -#ifndef SERVO_YAW_P -# define SERVO_YAW_P 0.0 -#endif -#ifndef SERVO_YAW_I -# define SERVO_YAW_I 0.0 -#endif -#ifndef SERVO_YAW_D -# define SERVO_YAW_D 0.0 -#endif -#ifndef SERVO_YAW_INT_MAX -# define SERVO_YAW_INT_MAX 5 -#endif -#ifndef RUDDER_MIX -# define RUDDER_MIX 0.5 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// Navigation control gains -// -#ifndef NAV_ROLL_P -# define NAV_ROLL_P 0.7 -#endif -#ifndef NAV_ROLL_I -# define NAV_ROLL_I 0.0 -#endif -#ifndef NAV_ROLL_D -# define NAV_ROLL_D 0.02 -#endif -#ifndef NAV_ROLL_INT_MAX -# define NAV_ROLL_INT_MAX 5 -#endif -#ifndef NAV_PITCH_ASP_P -# define NAV_PITCH_ASP_P 0.65 -#endif -#ifndef NAV_PITCH_ASP_I -# define NAV_PITCH_ASP_I 0.0 -#endif -#ifndef NAV_PITCH_ASP_D -# define NAV_PITCH_ASP_D 0.0 -#endif -#ifndef NAV_PITCH_ASP_INT_MAX -# define NAV_PITCH_ASP_INT_MAX 5 -#endif -#ifndef NAV_PITCH_ALT_P -# define NAV_PITCH_ALT_P 0.65 -#endif -#ifndef NAV_PITCH_ALT_I -# define NAV_PITCH_ALT_I 0.0 -#endif -#ifndef NAV_PITCH_ALT_D -# define NAV_PITCH_ALT_D 0.0 -#endif -#ifndef NAV_PITCH_ALT_INT_MAX -# define NAV_PITCH_ALT_INT_MAX 5 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// Energy/Altitude control gains -// -#ifndef THROTTLE_TE_P -# define THROTTLE_TE_P 0.50 -#endif -#ifndef THROTTLE_TE_I -# define THROTTLE_TE_I 0.0 -#endif -#ifndef THROTTLE_TE_D -# define THROTTLE_TE_D 0.0 -#endif -#ifndef THROTTLE_TE_INT_MAX -# define THROTTLE_TE_INT_MAX 20 -#endif -#ifndef THROTTLE_SLEW_LIMIT -# define THROTTLE_SLEW_LIMIT 0 -#endif -#ifndef P_TO_T -# define P_TO_T 2.5 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// Crosstrack compensation -// -#ifndef XTRACK_GAIN -# define XTRACK_GAIN 10 // deg/m * 100 -#endif -#ifndef XTRACK_ENTRY_ANGLE -# define XTRACK_ENTRY_ANGLE 3000 // deg * 100 -#endif - - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// DEBUGGING -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// DEBUG_SUBSYSTEM -// -#ifndef DEBUG_SUBSYSTEM -# define DEBUG_SUBSYSTEM 0 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// DEBUG_LEVEL -// -#ifndef DEBUG_LEVEL -# define DEBUG_LEVEL SEVERITY_LOW -#endif - - -////////////////////////////////////////////////////////////////////////////// -// Dataflash logging control -// -#ifndef LOG_ATTITUDE_FAST -# define LOG_ATTITUDE_FAST DISABLED -#endif -#ifndef LOG_ATTITUDE_MED -# define LOG_ATTITUDE_MED ENABLED -#endif -#ifndef LOG_GPS -# define LOG_GPS ENABLED -#endif -#ifndef LOG_PM -# define LOG_PM ENABLED -#endif -#ifndef LOG_CTUN -# define LOG_CTUN DISABLED -#endif -#ifndef LOG_NTUN -# define LOG_NTUN DISABLED -#endif -#ifndef LOG_MODE -# define LOG_MODE ENABLED -#endif -#ifndef LOG_RAW -# define LOG_RAW DISABLED -#endif -#ifndef LOG_CMD -# define LOG_CMD ENABLED -#endif - -#ifndef DEBUG_PORT -# define DEBUG_PORT 0 -#endif - -#if DEBUG_PORT == 0 -# define SendDebug_P(a) Serial.print_P(PSTR(a)) -# define SendDebugln_P(a) Serial.println_P(PSTR(a)) -# define SendDebug Serial.print -# define SendDebugln Serial.println -#elif DEBUG_PORT == 1 -# define SendDebug_P(a) Serial1.print_P(PSTR(a)) -# define SendDebugln_P(a) Serial1.println_P(PSTR(a)) -# define SendDebug Serial1.print -# define SendDebugln Serial1.println -#elif DEBUG_PORT == 2 -# define SendDebug_P(a) Serial2.print_P(PSTR(a)) -# define SendDebugln_P(a) Serial2.println_P(PSTR(a)) -# define SendDebug Serial2.print -# define SendDebugln Serial2.println -#elif DEBUG_PORT == 3 -# define SendDebug_P(a) Serial3.print_P(PSTR(a)) -# define SendDebugln_P(a) Serial3.println_P(PSTR(a)) -# define SendDebug Serial3.print -# define SendDebugln Serial3.println -#endif - -////////////////////////////////////////////////////////////////////////////// -// Navigation defaults -// -#ifndef WP_RADIUS_DEFAULT -# define WP_RADIUS_DEFAULT 20 -#endif -#ifndef LOITER_RADIUS_DEFAULT -# define LOITER_RADIUS_DEFAULT 60 -#endif - -////////////////////////////////////////////////////////////////////////////// -// Developer Items -// - -#ifndef STANDARD_SPEED -# define STANDARD_SPEED 15.0 -#endif -#define STANDARD_SPEED_SQUARED (STANDARD_SPEED * STANDARD_SPEED) -#define STANDARD_THROTTLE_SQUARED (THROTTLE_CRUISE * THROTTLE_CRUISE) -#define THROTTLE2SPEED_SCALER diff --git a/Tools/ArduTracker/control_modes.pde b/Tools/ArduTracker/control_modes.pde deleted file mode 100644 index 71c3b60a11..0000000000 --- a/Tools/ArduTracker/control_modes.pde +++ /dev/null @@ -1,48 +0,0 @@ -void read_control_switch() -{ - byte switchPosition = readSwitch(); - if (oldSwitchPosition != switchPosition){ - - set_mode(flight_mode(switchPosition)); - - oldSwitchPosition = switchPosition; - - // reset navigation integrators - // ------------------------- - reset_I(); - } -} - -byte readSwitch(void){ - uint16_t pulsewidth = APM_RC.InputCh(get(PARAM_FLIGHT_MODE_CH) - 1); - if (pulsewidth > 1230 && pulsewidth <= 1360) return 1; - if (pulsewidth > 1360 && pulsewidth <= 1490) return 2; - if (pulsewidth > 1490 && pulsewidth <= 1620) return 3; - if (pulsewidth > 1620 && pulsewidth <= 1749) return 4; // Software Manual - if (pulsewidth >= 1750) return 5; // Hardware Manual - return 0; -} - -void reset_control_switch() -{ - oldSwitchPosition = 0; - read_control_switch(); - SendDebug("MSG: reset_control_switch"); - SendDebugln(oldSwitchPosition , DEC); -} - -void update_servo_switches() -{ - // up is reverse - // up is elevon - mix_mode = (PINL & 128) ? 1 : 0; - if (mix_mode == 0) { - reverse_roll = (PINE & 128) ? 1 : -1; - reverse_pitch = (PINE & 64) ? 1 : -1; - reverse_rudder = (PINL & 64) ? 1 : -1; - } else { - reverse_elevons = (PINE & 128) ? 1 : -1; - reverse_ch1_elevon = (PINE & 64) ? 1 : -1; - reverse_ch2_elevon = (PINL & 64) ? 1 : -1; - } -} diff --git a/Tools/ArduTracker/createTags b/Tools/ArduTracker/createTags deleted file mode 100755 index 1b9c3aff76..0000000000 --- a/Tools/ArduTracker/createTags +++ /dev/null @@ -1,68 +0,0 @@ -#!/bin/bash -#" Autocompletion enabled vim for arduino pde's - -ctags -RV --language-force=C++ --c++-kinds=+p --fields=+iaS --extra=+q \ - . \ - ../libraries/* \ - /usr/lib/avr/include - -# sample vimrc file -# you'll need to have omnicpp plugin for vim - -#"set compatible - -#" Vim5 and later versions support syntax highlighting. Uncommenting the next -#" line enables syntax highlighting by default. -#syntax on -#filetype on -#au BufNewFile,BufRead *.pde set filetype=cpp - -#" If using a dark background within the editing area and syntax highlighting -#" turn on this option as well -#"set background=dark - -#" Uncomment the following to have Vim jump to the last position when -#" reopening a file -#if has("autocmd") - #au BufReadPost * if line("'\"") > 1 && line("'\"") <= line("$") | exe "normal! g'\"" | endif -#endif - -#" Uncomment the following to have Vim load indentation rules and plugins -#" according to the detected filetype. -#if has("autocmd") - #filetype plugin indent on -#endif - -#" The following are commented out as they cause vim to behave a lot -#" differently from regular Vi. They are highly recommended though. -#set showcmd " Show (partial) command in status line. -#set showmatch " Show matching brackets. -#set ignorecase " Do case insensitive matching -#set smartcase " Do smart case matching -#set incsearch " Incremental search -#set autowrite " Automatically save before commands like :next and :make -#set hidden " Hide buffers when they are abandoned -#set mouse=a " Enable mouse usage (all modes) -#set vb - -#" build tags of your own project with Ctrl-F12 -#map :!ctags -R --sort=yes --c++-kinds=+p --fields=+iaS --extra=+q . - -#" OmniCppComplete -#let OmniCpp_NamespaceSearch = 1 -#let OmniCpp_GlobalScopeSearch = 1 -#let OmniCpp_ShowAccess = 1 -#let OmniCpp_ShowPrototypeInAbbr = 1 " show function parameters -#let OmniCpp_MayCompleteDot = 1 " autocomplete after . -#let OmniCpp_MayCompleteArrow = 1 " autocomplete after -> -#let OmniCpp_MayCompleteScope = 1 " autocomplete after :: -#let OmniCpp_DefaultNamespaces = ["std", "_GLIBCXX_STD"] - -#" arduino syntax -#au BufNewFile,BufRead *.pde setf arduino - -#" automatically open and close the popup menu / preview window -#"au CursorMovedI,InsertLeave * if pumvisible() == 0|silent! pclose|endif -#"set completeopt=menuone,menu,longest,preview -#set ts=4 -#set sw=4 diff --git a/Tools/ArduTracker/debug.pde b/Tools/ArduTracker/debug.pde deleted file mode 100644 index e090a763db..0000000000 --- a/Tools/ArduTracker/debug.pde +++ /dev/null @@ -1,70 +0,0 @@ -#if DEBUG_SUBSYSTEM == 1 -void debug_subsystem() -{ - Serial.println("GPS Subsystem, RAW OUTPUT"); - - while(1){ - if(Serial1.available() > 0) // Ok, let me see, the buffer is empty? - { - delay(60); // wait for it all - while(Serial1.available() > 0){ - byte incoming = Serial1.read(); - //Serial.print(incoming,DEC); - Serial.print(incoming, BYTE); // will output Hex values - //Serial.print(","); - } - Serial.println(" "); - } - - } -} -#endif - -#if DEBUG_SUBSYSTEM == 2 -void debug_subsystem() -{ - Serial.println("Control Switch"); - - Serial.print("Control CH "); - Serial.println(flight_mode_channel, DEC); - - while(1){ - delay(20); - byte switchPosition = readSwitch(); - if (oldSwitchPosition != switchPosition){ - Serial.printf_P(PSTR("Position %d\n"), i, switchPosition); - oldSwitchPosition = switchPosition; - } - } -} -#endif - -#if DEBUG_SUBSYSTEM == 3 -void debug_subsystem() -{ - Serial.println("DIP Switch Test"); - - while(1){ - delay(100); - update_servo_switches(); - if (mix_mode == 0) { - Serial.print("Mix:standard "); - Serial.print("\t reverse_roll: "); - Serial.print(reverse_roll, DEC); - Serial.print("\t reverse_pitch: "); - Serial.print(reverse_pitch, DEC); - Serial.print("\t reverse_rudder: "); - Serial.println(reverse_rudder, DEC); - } else { - Serial.print("Mix:elevons "); - Serial.print("\t reverse_elevons: "); - Serial.print(reverse_elevons, DEC); - Serial.print("\t reverse_ch1_elevon: "); - Serial.print(reverse_ch1_elevon, DEC); - Serial.print("\t reverse_ch2_elevon: "); - Serial.println(reverse_ch2_elevon, DEC); - } - } -} -#endif - diff --git a/Tools/ArduTracker/defines.h b/Tools/ArduTracker/defines.h deleted file mode 100644 index be786750f4..0000000000 --- a/Tools/ArduTracker/defines.h +++ /dev/null @@ -1,312 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -// Internal defines, don't edit and expect things to work -// ------------------------------------------------------- - -#define TRUE 1 -#define FALSE 0 -#define ToRad(x) radians(x) // *pi/180 -#define ToDeg(x) degrees(x) // *180/pi - -#define DEBUG 0 -#define LOITER_RANGE 30 // for calculating power outside of loiter radius - -// GPS baud rates -// -------------- -#define NO_GPS 38400 -#define NMEA_GPS 38400 -#define EM406_GPS 57600 -#define UBLOX_GPS 38400 -#define ARDU_IMU 38400 -#define MTK_GPS 38400 -#define SIM_GPS 38400 - -// GPS type codes - use the names, not the numbers -#define GPS_PROTOCOL_NONE -1 -#define GPS_PROTOCOL_NMEA 0 -#define GPS_PROTOCOL_SIRF 1 -#define GPS_PROTOCOL_UBLOX 2 -#define GPS_PROTOCOL_IMU 3 -#define GPS_PROTOCOL_MTK 4 -#define GPS_PROTOCOL_HIL 5 -#define GPS_PROTOCOL_MTK19 6 -#define GPS_PROTOCOL_AUTO 7 - -// Radio channels -// Note channels are from 0! -// -// XXX these should be CH_n defines from RC.h at some point. -#define CH_ROLL 0 -#define CH_PITCH 1 -#define CH_THROTTLE 2 -#define CH_RUDDER 3 -#define CH_1 0 -#define CH_2 1 -#define CH_3 2 -#define CH_4 3 -#define CH_5 4 -#define CH_6 5 -#define CH_7 6 -#define CH_8 7 - -#define WP_SIZE 14 - -// HIL enumerations -#define HIL_PROTOCOL_XPLANE 1 -#define HIL_PROTOCOL_MAVLINK 2 - -#define HIL_MODE_DISABLED 0 -#define HIL_MODE_ATTITUDE 1 -#define HIL_MODE_SENSORS 2 - -// GCS enumeration -#define GCS_PROTOCOL_STANDARD 0 // standard APM protocol -#define GCS_PROTOCOL_LEGACY 1 // legacy ArduPilot protocol -#define GCS_PROTOCOL_XPLANE 2 // X-Plane HIL simulation -#define GCS_PROTOCOL_DEBUGTERMINAL 3 //Human-readable debug interface for use with a dumb terminal -#define GCS_PROTOCOL_MAVLINK 4 // binary protocol for qgroundcontrol -#define GCS_PROTOCOL_NONE -1 // No GCS output - -// PID enumeration -// --------------- -#define CASE_SERVO_ROLL 0 -#define CASE_SERVO_PITCH 1 -#define CASE_SERVO_RUDDER 2 -#define CASE_NAV_ROLL 3 -#define CASE_NAV_PITCH_ASP 4 -#define CASE_NAV_PITCH_ALT 5 -#define CASE_TE_THROTTLE 6 -#define CASE_ALT_THROTTLE 7 - -// Feedforward cases -// ---------------- -#define CASE_PITCH_COMP 0 -#define CASE_RUDDER_MIX 1 -#define CASE_P_TO_T 2 - -// Auto Pilot modes -// ---------------- -#define MANUAL 0 -#define CIRCLE 1 // When flying sans GPS, and we loose the radio, just circle -#define STABILIZE 2 - -#define FLY_BY_WIRE_A 5 // Fly By Wire A has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical = manual throttle -#define FLY_BY_WIRE_B 6 // Fly By Wire B has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical => desired airspeed - // Fly By Wire B = Fly By Wire A if you have AIRSPEED_SENSOR 0 -#define AUTO 10 -#define RTL 11 -#define LOITER 12 -#define TAKEOFF 13 -#define LAND 14 - - -// Command IDs - Must -#define CMD_BLANK 0x00 // there is no command stored in the mem location requested -#define CMD_WAYPOINT 0x10 -#define CMD_LOITER 0x11 -#define CMD_LOITER_N_TURNS 0x12 -#define CMD_LOITER_TIME 0x13 -#define CMD_RTL 0x14 -#define CMD_LAND 0x15 -#define CMD_TAKEOFF 0x16 - -// Command IDs - May -#define CMD_DELAY 0x20 -#define CMD_CLIMB 0x21 // NOT IMPLEMENTED -#define CMD_LAND_OPTIONS 0x22 // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 - -// Command IDs - Now -//#define CMD_AP_MODE 0x30 -#define CMD_RESET_INDEX 0x31 -#define CMD_GOTO_INDEX 0x32 // NOT IMPLEMENTED -#define CMD_GETVAR_INDEX 0x33 -#define CMD_SENDVAR_INDEX 0x34 -#define CMD_TELEMETRY 0x35 - -#define CMD_THROTTLE_CRUISE 0x40 -#define CMD_AIRSPEED_CRUISE 0x41 -#define CMD_RESET_HOME 0x44 - -#define CMD_KP_GAIN 0x60 -#define CMD_KI_GAIN 0x61 -#define CMD_KD_GAIN 0x62 -#define CMD_KI_MAX 0x63 -#define CMD_KFF_GAIN 0x64 - -#define CMD_RADIO_TRIM 0x70 -#define CMD_RADIO_MAX 0x71 -#define CMD_RADIO_MIN 0x72 -#define CMD_RADIO_MIN 0x72 -#define CMD_ELEVON_TRIM 0x73 - -#define CMD_INDEX 0x75 // sets the current Must index -#define CMD_REPEAT 0x80 -#define CMD_RELAY 0x81 -#define CMD_SERVO 0x82 // move servo N to PWM value - -//repeating events -#define NO_REPEAT 0 -#define CH_4_TOGGLE 1 -#define CH_5_TOGGLE 2 -#define CH_6_TOGGLE 3 -#define CH_7_TOGGLE 4 -#define RELAY_TOGGLE 5 -#define STOP_REPEAT 10 - -// GCS Message ID's -#define MSG_ACKNOWLEDGE 0x00 -#define MSG_HEARTBEAT 0x01 -#define MSG_ATTITUDE 0x02 -#define MSG_LOCATION 0x03 -#define MSG_PRESSURE 0x04 -#define MSG_STATUS_TEXT 0x05 -#define MSG_PERF_REPORT 0x06 -#define MSG_MODE_CHANGE 0x07 //This is different than HEARTBEAT because it occurs only when the mode is actually changed -#define MSG_VERSION_REQUEST 0x08 -#define MSG_VERSION 0x09 -#define MSG_EXTENDED_STATUS 0x0a -#define MSG_CPU_LOAD 0x0b - -#define MSG_COMMAND_REQUEST 0x20 -#define MSG_COMMAND_UPLOAD 0x21 -#define MSG_COMMAND_LIST 0x22 -#define MSG_COMMAND_MODE_CHANGE 0x23 -#define MSG_CURRENT_WAYPOINT 0x24 - -#define MSG_VALUE_REQUEST 0x30 -#define MSG_VALUE_SET 0x31 -#define MSG_VALUE 0x32 - -#define MSG_PID_REQUEST 0x40 -#define MSG_PID_SET 0x41 -#define MSG_PID 0x42 - -#define MSG_TRIM_STARTUP 0x50 -#define MSG_TRIM_MIN 0x51 -#define MSG_TRIM_MAX 0x52 -#define MSG_RADIO_OUT 0x53 - -#define MSG_RAW_IMU 0x60 -#define MSG_GPS_STATUS 0x61 -#define MSG_GPS_RAW 0x62 -#define MSG_AIRSPEED 0x63 - -#define MSG_SERVO_OUT 0x70 - -#define MSG_PIN_REQUEST 0x80 -#define MSG_PIN_SET 0x81 - -#define MSG_DATAFLASH_REQUEST 0x90 -#define MSG_DATAFLASH_SET 0x91 - -#define MSG_EEPROM_REQUEST 0xa0 -#define MSG_EEPROM_SET 0xa1 - -#define MSG_POSITION_CORRECT 0xb0 -#define MSG_ATTITUDE_CORRECT 0xb1 -#define MSG_POSITION_SET 0xb2 -#define MSG_ATTITUDE_SET 0xb3 -#define MSG_LOCAL_LOCATION 0xb4 - -#define SEVERITY_LOW 1 -#define SEVERITY_MEDIUM 2 -#define SEVERITY_HIGH 3 -#define SEVERITY_CRITICAL 4 - -// Logging parameters -#define LOG_ATTITUDE_MSG 0x01 -#define LOG_GPS_MSG 0x02 -#define LOG_MODE_MSG 0X03 -#define LOG_CONTROL_TUNING_MSG 0X04 -#define LOG_NAV_TUNING_MSG 0X05 -#define LOG_PERFORMANCE_MSG 0X06 -#define LOG_RAW_MSG 0x07 -#define LOG_CMD_MSG 0x08 -#define LOG_STARTUP_MSG 0x09 -#define TYPE_AIRSTART_MSG 0x00 -#define TYPE_GROUNDSTART_MSG 0x01 - -#define MASK_LOG_ATTITUDE_FAST 0 -#define MASK_LOG_ATTITUDE_MED 2 -#define MASK_LOG_GPS 4 -#define MASK_LOG_PM 8 -#define MASK_LOG_CTUN 16 -#define MASK_LOG_NTUN 32 -#define MASK_LOG_MODE 64 -#define MASK_LOG_RAW 128 -#define MASK_LOG_CMD 256 - -// Yaw modes -#define YAW_MODE_COORDINATE_TURNS 0 -#define YAW_MODE_HOLD_HEADING 1 -#define YAW_MODE_SLIP 2 - -// Waypoint Modes -// ---------------- -#define ABS_WP 0 -#define REL_WP 1 - -// Command Queues -// --------------- -#define COMMAND_MUST 0 -#define COMMAND_MAY 1 -#define COMMAND_NOW 2 - -// Events -// ------ -#define EVENT_WILL_REACH_WAYPOINT 1 -#define EVENT_SET_NEW_WAYPOINT_INDEX 2 -#define EVENT_LOADED_WAYPOINT 3 -#define EVENT_LOOP 4 - -//GPS_fix -#define VALID_GPS 0x00 -#define BAD_GPS 0x01 -#define FAILED_GPS 0x03 - -// Climb rate calculations -#define ALTITUDE_HISTORY_LENGTH 8 //Number of (time,altitude) points to regress a climb rate from - - -#define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO - -#define AIRSPEED_CH 7 // The external ADC channel for the airspeed sensor -#define BATTERY_PIN1 0 // These are the pins for the voltage dividers -#define BATTERY_PIN2 1 -#define BATTERY_PIN3 2 -#define BATTERY_PIN4 3 -#define RELAY_PIN 47 - -// Hardware Parameters -#define SLIDE_SWITCH_PIN 40 -#define PUSHBUTTON_PIN 41 - -#define A_LED_PIN 37 //36 = B, 37 = A, 35 = C -#define B_LED_PIN 36 -#define C_LED_PIN 35 - -#define HOLD_ALT_ABOVE_HOME 8 // bitmask value - -#define SPEEDFILT 400 // centimeters/second; the speed below which a groundstart will be triggered - -#define EEPROM_MAX_ADDR 4096 - -// log -#define EE_END_OF_PARAMS getAddress(PARAM_LAST_UINT32) -#define EE_LAST_LOG_PAGE EE_END_OF_PARAMS+1 -#define EE_LAST_LOG_NUM EE_END_OF_PARAMS+3 -#define EE_LOG_1_START EE_END_OF_PARAMS+5 -#define WP_START_BYTE EE_END_OF_PARAMS+45 // where in memory home WP is stored + all other WP - // add 19 to account for log files - -// bits in log_bitmask -#define LOGBIT_ATTITUDE_FAST (1<<0) -#define LOGBIT_ATTITUDE_MED (1<<1) -#define LOGBIT_GPS (1<<2) -#define LOGBIT_PM (1<<3) -#define LOGBIT_CTUN (1<<4) -#define LOGBIT_NTUN (1<<5) -#define LOGBIT_MODE (1<<6) -#define LOGBIT_RAW (1<<7) -#define LOGBIT_CMD (1<<8) - diff --git a/Tools/ArduTracker/events.pde b/Tools/ArduTracker/events.pde deleted file mode 100644 index 15d1a50746..0000000000 --- a/Tools/ArduTracker/events.pde +++ /dev/null @@ -1,225 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/* - This event will be called when the failsafe changes - boolean failsafe reflects the current state -*/ -void failsafe_event() -{ - if (failsafe == true){ - - // This is how to handle a failsafe. - switch(control_mode) - { - case MANUAL: // First position - set_mode(STABILIZE); - break; - - case STABILIZE: - case FLY_BY_WIRE_A: // middle position - case FLY_BY_WIRE_B: // middle position - set_mode(RTL); - set(PARAM_TRIM_THROTTLE,THROTTLE_CRUISE); - - case CIRCLE: // middle position - break; - - case AUTO: // middle position - case LOITER: // middle position - if (get(PARAM_THR_FS_ACTION)== 1){ - set_mode(RTL); - set(PARAM_TRIM_THROTTLE,THROTTLE_CRUISE); - } - // 2 = Stay in AUTO and ignore failsafe - break; - - case RTL: // middle position - break; - - default: - set_mode(RTL); - set(PARAM_TRIM_THROTTLE,THROTTLE_CRUISE); - break; - } - }else{ - reset_I(); - } -} - -void low_battery_event(void) -{ - gcs.send_text(SEVERITY_HIGH,"Low Battery!"); - set_mode(RTL); - set(PARAM_TRIM_THROTTLE,THROTTLE_CRUISE); -} - - -/* -4 simultaneous events -int event_original - original time in seconds -int event_countdown - count down to zero -byte event_countdown - ID for what to do -byte event_countdown - how many times to loop, 0 = indefinite -byte event_value - specific information for an event like PWM value -byte counterEvent - undo the event if nescessary - -count down each one - - -new event -undo_event -*/ - -void new_event(struct Location *cmd) -{ - SendDebug("New Event Loaded "); - SendDebugln(cmd->p1,DEC); - - - if(cmd->p1 == STOP_REPEAT){ - SendDebugln("STOP repeat "); - event_id = NO_REPEAT; - event_timer = -1; - undo_event = 0; - event_value = 0; - event_delay = 0; - event_repeat = 0; // convert seconds to millis - event_undo_value = 0; - repeat_forever = 0; - }else{ - // reset the event timer - event_timer = millis(); - event_id = cmd->p1; - event_value = cmd->alt; - event_delay = cmd->lat; - event_repeat = cmd->lng; // convert seconds to millis - event_undo_value = 0; - repeat_forever = (event_repeat == 0) ? 1:0; - } - - /* - Serial.print("event_id: "); - Serial.println(event_id,DEC); - Serial.print("event_value: "); - Serial.println(event_value,DEC); - Serial.print("event_delay: "); - Serial.println(event_delay,DEC); - Serial.print("event_repeat: "); - Serial.println(event_repeat,DEC); - Serial.print("event_undo_value: "); - Serial.println(event_undo_value,DEC); - Serial.print("repeat_forever: "); - Serial.println(repeat_forever,DEC); - Serial.print("Event_timer: "); - Serial.println(event_timer,DEC); - */ - perform_event(); -} - -void perform_event() -{ - if (event_repeat > 0){ - event_repeat --; - } - switch(event_id) { - case CH_4_TOGGLE: - event_undo_value = readOutputCh(4); - - APM_RC.OutputCh(4, event_value); // send to Servos - undo_event = 2; - break; - case CH_5_TOGGLE: - event_undo_value = readOutputCh(5); - APM_RC.OutputCh(5, event_value); // send to Servos - undo_event = 2; - break; - case CH_6_TOGGLE: - event_undo_value = readOutputCh(6); - APM_RC.OutputCh(6, event_value); // send to Servos - undo_event = 2; - break; - case CH_7_TOGGLE: - event_undo_value = readOutputCh(7); - APM_RC.OutputCh(7, event_value); // send to Servos - undo_event = 2; - event_undo_value = 1; - break; - case RELAY_TOGGLE: - - event_undo_value = PORTL & B00000100 ? 1:0; - relay_toggle(); - SendDebug("toggle relay "); - SendDebugln(PORTL,BIN); - undo_event = 2; - break; - - } -} - -void relay_on() -{ - PORTL |= B00000100; -} - -void relay_off() -{ - PORTL &= ~B00000100; -} - -void relay_toggle() -{ - PORTL ^= B00000100; -} - -void update_events() -{ - // repeating events - if(undo_event == 1){ - perform_event_undo(); - undo_event = 0; - }else if(undo_event > 1){ - undo_event --; - } - - if(event_timer == -1) - return; - - if((millis() - event_timer) > event_delay){ - perform_event(); - - if(event_repeat > 0 || repeat_forever == 1){ - event_repeat--; - event_timer = millis(); - }else{ - event_timer = -1; - } - } -} - -void perform_event_undo() -{ - switch(event_id) { - case CH_4_TOGGLE: - APM_RC.OutputCh(4, event_undo_value); // send to Servos - break; - - case CH_5_TOGGLE: - APM_RC.OutputCh(5, event_undo_value); // send to Servos - break; - - case CH_6_TOGGLE: - APM_RC.OutputCh(6, event_undo_value); // send to Servos - break; - - case CH_7_TOGGLE: - APM_RC.OutputCh(7, event_undo_value); // send to Servos - break; - - case RELAY_TOGGLE: - - relay_toggle(); - SendDebug("untoggle relay "); - SendDebugln(PORTL,BIN); - break; - } -} diff --git a/Tools/ArduTracker/global_data.h b/Tools/ArduTracker/global_data.h deleted file mode 100644 index 63ea125448..0000000000 --- a/Tools/ArduTracker/global_data.h +++ /dev/null @@ -1,58 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -#ifndef __GLOBAL_DATA_H -#define __GLOBAL_DATA_H - -#include "param_table.h" - -#define ONBOARD_PARAM_NAME_LENGTH 15 -#define MAX_WAYPOINTS 20 - -#define FIRMWARE_VERSION 12 // <-- change on param data struct modifications - -#ifdef __cplusplus - -/// -// global data structure -// This structure holds all the vehicle parameters. -// TODO: bring in varibles floating around in ArduPilotMega.pde -// -struct global_struct -{ - // parameters - uint16_t parameter_i; // parameter index - uint16_t param_count; // number of params - - // waypoints - uint16_t waypoint_request_i; // request index - uint16_t waypoint_dest_sysid; // where to send requests - uint16_t waypoint_dest_compid; // " - bool waypoint_sending; // currently in send process - bool waypoint_receiving; // currently receiving - uint16_t waypoint_count; - uint32_t waypoint_timelast_send; // milliseconds - uint32_t waypoint_timelast_receive; // milliseconds - uint16_t waypoint_send_timeout; // milliseconds - uint16_t waypoint_receive_timeout; // milliseconds - float junk; //used to return a junk value for interface - - // data stream rates - uint16_t streamRateRawSensors; - uint16_t streamRateExtendedStatus; - uint16_t streamRateRCChannels; - uint16_t streamRateRawController; - uint16_t streamRateRawSensorFusion; - uint16_t streamRatePosition; - uint16_t streamRateExtra1; - uint16_t streamRateExtra2; - uint16_t streamRateExtra3; - - // struct constructor - global_struct(); -} global_data; - -extern "C" const char *param_nametab[]; - -#endif // __cplusplus - -#endif // __GLOBAL_DATA_H diff --git a/Tools/ArduTracker/global_data.pde b/Tools/ArduTracker/global_data.pde deleted file mode 100644 index 396ce54466..0000000000 --- a/Tools/ArduTracker/global_data.pde +++ /dev/null @@ -1,188 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -////////////////////////////////////////////////////////////////////// -// parameter get/set functions -////////////////////////////////////////////////////////////////////// - -// calculate memory starting location for eeprom -static uint16_t floatMemStart = 0; -static uint16_t uint8MemStart = floatMemStart + PARAM_FLOAT_COUNT * sizeof(float); -static uint16_t uint16MemStart = uint8MemStart + PARAM_UINT8_COUNT * sizeof(uint8_t); -static uint16_t int16MemStart = uint16MemStart + PARAM_UINT16_COUNT * sizeof(uint16_t); -static uint16_t uint32MemStart = int16MemStart + PARAM_INT16_COUNT * sizeof(int16_t); - -// read parameters -static uint8_t get(uint8_param_t id) __attribute__((noinline)); -static uint8_t get(uint8_param_t id) -{ - return eeprom_read_byte((uint8_t *)((id-PARAM_FIRST_UINT8)*sizeof(uint8_t)+uint8MemStart)); -} - -static uint16_t get(uint16_param_t id) __attribute__((noinline)); -static uint16_t get(uint16_param_t id) -{ - return eeprom_read_word((uint16_t *)((id-PARAM_FIRST_UINT16)*sizeof(uint16_t)+uint16MemStart)); -} - -static int16_t get(int16_param_t id) __attribute__((noinline)); -static int16_t get(int16_param_t id) -{ - return (int16_t)eeprom_read_word((uint16_t *)((id-PARAM_FIRST_INT16)*sizeof(int16_t)+int16MemStart)); -} - -static float get(float_param_t id) __attribute__((noinline)); -static float get(float_param_t id) -{ - float value; - eeprom_read_block((void*)&value,(const void*)((id-PARAM_FIRST_FLOAT)*sizeof(float)+floatMemStart),sizeof(value)); - return value; -} - -static float get(uint32_param_t id) __attribute__((noinline)); -static float get(uint32_param_t id) -{ - uint32_t value; - eeprom_read_block((void*)&value,(const void*)((id-PARAM_FIRST_UINT32)*sizeof(uint32_t)+uint32MemStart),sizeof(value)); - return value; -} - - - -// write parameters -static void set(uint8_param_t id, uint8_t val) __attribute__((noinline)); -static void set(uint8_param_t id, uint8_t val) -{ - eeprom_write_byte((uint8_t *)((id-PARAM_FIRST_UINT8)*sizeof(uint8_t)+uint8MemStart),val); -} - -static void set(uint16_param_t id, uint16_t val) __attribute__((noinline)); -static void set(uint16_param_t id, uint16_t val) -{ - eeprom_write_word((uint16_t *)((id-PARAM_FIRST_UINT16)*sizeof(uint16_t)+uint16MemStart),val); -} - -static void set(int16_param_t id, int16_t val) __attribute__((noinline)); -static void set(int16_param_t id, int16_t val) -{ - eeprom_write_word((uint16_t *)((id-PARAM_FIRST_INT16)*sizeof(int16_t)+int16MemStart),(uint16_t)val); -} - -static void set(float_param_t id, float val) __attribute__((noinline)); -static void set(float_param_t id, float val) -{ - eeprom_write_block((const void *)&val,(void *)((id-PARAM_FIRST_FLOAT)*sizeof(float)+floatMemStart),sizeof(val)); -} - -static void set(uint32_param_t id, uint32_t val) __attribute__((noinline)); -static void set(uint32_param_t id, uint32_t val) -{ - eeprom_write_block((const void *)&val,(void *)((id-PARAM_FIRST_UINT32)*sizeof(uint32_t)+uint32MemStart),sizeof(val)); -} - -static void setParamAsFloat(uint16_t id, float value) -{ - if (id < PARAM_FIRST_UINT8) set((float_param_t)id,value); - else if (id < PARAM_FIRST_UINT16) set((uint8_param_t)id,(uint8_t)value); - else if (id < PARAM_FIRST_INT16) set((uint16_param_t)id,(uint16_t)value); - else if (id < PARAM_FIRST_UINT32) set((int16_param_t)id,(int16_t)value); - else if (id < PARAM_COUNT) set((uint32_param_t)id,(uint32_t)value); - // XXX: uint32 won't have full bitwise precision -} - -static float getParamAsFloat(uint16_t id) -{ - // name - if (id < PARAM_FIRST_UINT8) return (float)get((float_param_t)id); - else if (id < PARAM_FIRST_UINT16) return (float)get((uint8_param_t)id); - else if (id < PARAM_FIRST_INT16) return (float)get((uint16_param_t)id); - else if (id < PARAM_FIRST_UINT32) return (float)get((int16_param_t)id); - else if (id < PARAM_COUNT) return (float)get((uint32_param_t)id); - // XXX: uint32 won't have full bitwise precision -} - -static const prog_char *getParamName(uint16_t id) -{ - return (const prog_char *)pgm_read_word(¶m_nametab[id]); -} - -global_struct::global_struct() : - // parameters - // note, all values not explicitly initialised here are zeroed - param_count(PARAM_COUNT), - waypoint_send_timeout(1000), // 1 second - waypoint_receive_timeout(1000), // 1 second - - // stream rates - streamRateRawSensors(1), - streamRateExtendedStatus(1), - streamRateRCChannels(1), - streamRateRawController(1), - streamRateRawSensorFusion(1), - streamRatePosition(1), - streamRateExtra1(1), - streamRateExtra2(1), - streamRateExtra3(1) -{ -} - -// Print -static void printParam(BetterStream & serial, uint16_t id) -{ - serial.printf_P(PSTR("id %d %S, %f addr %d\n"), id, getParamName(id), getParamAsFloat(id), getAddress(id)); -} - -static void printAllParams(BetterStream & serial) -{ - for (int i=0;i WP error - distance < 0"); - //Serial.println(wp_distance,DEC); - //print_current_waypoints(); - return; - } - - // check if we have missed the WP - loiter_delta = (target_bearing - old_target_bearing)/100; - - // reset the old value - old_target_bearing = target_bearing; - - // wrap values - if (loiter_delta > 180) loiter_delta -= 360; - if (loiter_delta < -180) loiter_delta += 360; - loiter_sum += abs(loiter_delta); - - calc_bearing_error(); - - // control mode specific updates to nav_bearing - update_navigation(); - } -} - -void update_navigation() -{ - // wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS - // ------------------------------------------------------------------------ - - // distance and bearing calcs only - if(control_mode == AUTO){ - verify_must(); - verify_may(); - }else{ - - switch(control_mode){ - case LOITER: - update_loiter(); - calc_bearing_error(); - break; - - case RTL: - if(wp_distance <= (get(PARAM_LOITER_RADIUS)) + LOITER_RANGE) { - set_mode(LOITER); - }else{ - update_crosstrack(); - } - break; - } - } -} - - -/* -Disabled for now -void calc_distance_error() -{ - //distance_estimate += (float)gps.ground_speed * .0002f * cosf(radians(bearing_error * .01f)); - //distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance); - //wp_distance = max(distance_estimate,10); -} -*/ - -void calc_airspeed_errors() -{ - if(control_mode>=AUTO && airspeed_nudge > 0) { - airspeed_error = get(PARAM_TRIM_AIRSPEED) + airspeed_nudge - airspeed; - airspeed_energy_error = (long)(((long)(get(PARAM_TRIM_AIRSPEED) + airspeed_nudge) * (long)(get(PARAM_TRIM_AIRSPEED) + airspeed_nudge)) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation - } else { - airspeed_error = get(PARAM_TRIM_AIRSPEED) - airspeed; - airspeed_energy_error = (long)(((long)get(PARAM_TRIM_AIRSPEED) * (long)get(PARAM_TRIM_AIRSPEED)) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation - } -} - -void calc_bearing_error() -{ - if(takeoff_complete == true || MAGNETOMETER == ENABLED) { - bearing_error = nav_bearing - dcm.yaw_sensor; - } else { - bearing_error = nav_bearing - gps.ground_course; - } - bearing_error = wrap_180(bearing_error); -} - -void calc_altitude_error() -{ - // limit climb rates - target_altitude = next_WP.alt - (long)(((float)((wp_distance -30) * offset_altitude) / (float)(wp_totalDistance - 30))); - if(prev_WP.alt > next_WP.alt){ - target_altitude = constrain(target_altitude, next_WP.alt, prev_WP.alt); - }else{ - target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt); - } - - /* - // Disabled for now - #if AIRSPEED_SENSOR == 1 - // special thanks to Ryan Beall for this one - float pitch_angle = pitch_sensor - AOA; // pitch_angle = pitch sensor - angle of attack of your plane at level *100 (50 = .5°) - pitch_angle = constrain(pitch_angle, -2000, 2000); - float scale = sinf(radians(pitch_angle * .01)); - altitude_estimate += (float)airspeed * .0002 * scale; - altitude_estimate -= ALT_EST_GAIN * (float)(altitude_estimate - current_loc.alt); - - // compute altitude error for throttle control - altitude_error = target_altitude - altitude_estimate; - #else - altitude_error = target_altitude - current_loc.alt; - #endif - */ - - altitude_error = target_altitude - current_loc.alt; -} - - -long wrap_360(long error) -{ - if (error > 36000) error -= 36000; - if (error < 0) error += 36000; - return error; -} - -long wrap_180(long error) -{ - if (error > 18000) error -= 36000; - if (error < -18000) error += 36000; - return error; -} - - -void update_loiter() -{ - float power; - if (wp_distance <= get(PARAM_LOITER_RADIUS)){ - power = float(wp_distance) / float(get(PARAM_LOITER_RADIUS)); - nav_bearing += (int)(9000.0 * (2.0 + power)); - } else if (wp_distance < (get(PARAM_LOITER_RADIUS) + LOITER_RANGE)){ - power = -((float)(wp_distance - get(PARAM_LOITER_RADIUS) - LOITER_RANGE) / LOITER_RANGE); - power = constrain(power, 0, 1); - nav_bearing -= power * 9000; - } else { - update_crosstrack(); - } - - nav_bearing = wrap_360(nav_bearing); - -/* loiter_delta = (target_bearing - old_target_bearing)/100; - // reset the old value - old_target_bearing = target_bearing; - // wrap values - if (loiter_delta > 170) loiter_delta -= 360; - if (loiter_delta < -170) loiter_delta += 360; - loiter_sum += loiter_delta; -*/ -} - -void update_crosstrack(void) -{ - // Crosstrack Error - // ---------------- - if (abs(target_bearing - crosstrack_bearing) < 4500) { // If we are too far off or too close we don't do track following - crosstrack_error = sinf(radians((target_bearing - crosstrack_bearing)/100)) * wp_distance; // Meters we are off track line - nav_bearing += constrain(crosstrack_error * get(PARAM_XTRACK_GAIN), -get(PARAM_XTRACK_ANGLE), get(PARAM_XTRACK_ANGLE)); - nav_bearing = wrap_360(nav_bearing); - } -} - -void reset_crosstrack() -{ - crosstrack_bearing = get_bearing(¤t_loc, &next_WP); // Used for track following -} - -long get_altitude_above_home(void) -{ - // This is the altitude above the home location - // The GPS gives us altitude at Sea Level - // if you slope soar, you should see a negative number sometimes - // ------------------------------------------------------------- - return current_loc.alt - home.alt; -} - -long getDistance(struct Location *loc1, struct Location *loc2) -{ - if(loc1->lat == 0 || loc1->lng == 0) - return -1; - if(loc2->lat == 0 || loc2->lng == 0) - return -1; - float dlat = (float)(loc2->lat - loc1->lat); - float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown; - return sqrtf(sq(dlat) + sq(dlong)) * .01113195f; -} - -long get_alt_distance(struct Location *loc1, struct Location *loc2) -{ - return abs(loc1->alt - loc2->alt); -} - -long get_bearing(struct Location *loc1, struct Location *loc2) -{ - long off_x = loc2->lng - loc1->lng; - long off_y = (loc2->lat - loc1->lat) * scaleLongUp; - long bearing = 9000 + atan2f(-off_y, off_x) * 5729.57795f; - if (bearing < 0) bearing += 36000; - return bearing; -} diff --git a/Tools/ArduTracker/param_init.pde b/Tools/ArduTracker/param_init.pde deleted file mode 100644 index 039e4dd6ed..0000000000 --- a/Tools/ArduTracker/param_init.pde +++ /dev/null @@ -1,114 +0,0 @@ -// -// THIS FILE WAS AUTOMATICALLY GENERATED - DO NOT EDIT -// -/// @file param_init.pde - -void param_reset_defaults(void) -{ - set(PARAM_HDNG2RLL_P, NAV_ROLL_P); - set(PARAM_HDNG2RLL_I, NAV_ROLL_I); - set(PARAM_HDNG2RLL_D, NAV_ROLL_D); - set(PARAM_HDNG2RLL_IMAX, NAV_ROLL_INT_MAX); - set(PARAM_RLL2SRV_P, SERVO_ROLL_P); - set(PARAM_RLL2SRV_I, SERVO_ROLL_I); - set(PARAM_RLL2SRV_D, SERVO_ROLL_D); - set(PARAM_RLL2SRV_IMAX, SERVO_ROLL_INT_MAX); - set(PARAM_PTCH2SRV_P, SERVO_PITCH_P); - set(PARAM_PTCH2SRV_I, SERVO_PITCH_I); - set(PARAM_PTCH2SRV_D, SERVO_PITCH_D); - set(PARAM_PTCH2SRV_IMAX, SERVO_PITCH_INT_MAX); - set(PARAM_ARSPD2PTCH_P, NAV_PITCH_ASP_P); - set(PARAM_ARSPD2PTCH_I, NAV_PITCH_ASP_I); - set(PARAM_ARSPD2PTCH_D, NAV_PITCH_ASP_D); - set(PARAM_ARSPD2PTCH_IMAX, NAV_PITCH_ASP_INT_MAX); - set(PARAM_YW2SRV_P, SERVO_YAW_P); - set(PARAM_YW2SRV_I, SERVO_YAW_I); - set(PARAM_YW2SRV_D, SERVO_YAW_D); - set(PARAM_YW2SRV_IMAX, SERVO_YAW_INT_MAX); - set(PARAM_ALT2THR_P, THROTTLE_ALT_P); - set(PARAM_ALT2THR_I, THROTTLE_ALT_I); - set(PARAM_ALT2THR_D, THROTTLE_ALT_D); - set(PARAM_ALT2THR_IMAX, THROTTLE_ALT_INT_MAX); - set(PARAM_ENRGY2THR_P, THROTTLE_TE_P); - set(PARAM_ENRGY2THR_I, THROTTLE_TE_I); - set(PARAM_ENRGY2THR_D, THROTTLE_TE_D); - set(PARAM_ENRGY2THR_IMAX, THROTTLE_TE_INT_MAX); - set(PARAM_ALT2PTCH_P, NAV_PITCH_ALT_P); - set(PARAM_ALT2PTCH_I, NAV_PITCH_ALT_I); - set(PARAM_ALT2PTCH_D, NAV_PITCH_ALT_D); - set(PARAM_ALT2PTCH_IMAX, NAV_PITCH_ALT_INT_MAX); - set(PARAM_KFF_PTCHCOMP, PITCH_COMP); - set(PARAM_KFF_RDDRMIX, RUDDER_MIX); - set(PARAM_KFF_PTCH2THR, P_TO_T); - set(PARAM_GND_ALT, 0); - set(PARAM_TRIM_AIRSPEED, AIRSPEED_CRUISE); - set(PARAM_XTRACK_ANGLE, XTRACK_ENTRY_ANGLE); - set(PARAM_LIM_ROLL, HEAD_MAX); - set(PARAM_LIM_PITCH_MAX, PITCH_MAX); - set(PARAM_LIM_PITCH_MIN, PITCH_MIN); - set(PARAM_ALT_MIX, ALTITUDE_MIX); - set(PARAM_ALT_HOLD_HOME, 0); - set(PARAM_ARSPD_RATIO, AIRSPEED_RATIO); - set(PARAM_IMU_OFFSET_0, 0); - set(PARAM_IMU_OFFSET_1, 0); - set(PARAM_IMU_OFFSET_2, 0); - set(PARAM_IMU_OFFSET_3, 0); - set(PARAM_IMU_OFFSET_4, 0); - set(PARAM_IMU_OFFSET_5, 0); - set(PARAM_YAW_MODE, 0); - set(PARAM_WP_MODE, 0); - set(PARAM_WP_TOTAL, WP_SIZE); - set(PARAM_WP_INDEX, 0); - set(PARAM_WP_RADIUS, WP_RADIUS_DEFAULT); - set(PARAM_LOITER_RADIUS, LOITER_RADIUS_DEFAULT); - set(PARAM_ARSPD_FBW_MIN, AIRSPEED_FBW_MIN); - set(PARAM_ARSPD_FBW_MAX, AIRSPEED_FBW_MAX); - set(PARAM_THR_MIN, THROTTLE_MIN); - set(PARAM_THR_MAX, THROTTLE_MAX); - set(PARAM_THR_FAILSAFE, THROTTLE_FAILSAFE); - set(PARAM_THR_FS_ACTION, THROTTLE_FAILSAFE_ACTION); - set(PARAM_TRIM_THROTTLE, THROTTLE_CRUISE); - set(PARAM_CONFIG, 0); - set(PARAM_TRIM_AUTO, AUTO_TRIM); - set(PARAM_SWITCH_ENABLE, REVERSE_SWITCH); - set(PARAM_FLIGHT_MODE_CH, FLIGHT_MODE_CHANNEL); - set(PARAM_FLIGHT_MODE_1, FLIGHT_MODE_1); - set(PARAM_FLIGHT_MODE_2, FLIGHT_MODE_2); - set(PARAM_FLIGHT_MODE_3, FLIGHT_MODE_3); - set(PARAM_FLIGHT_MODE_4, FLIGHT_MODE_4); - set(PARAM_FLIGHT_MODE_5, FLIGHT_MODE_5); - set(PARAM_FLIGHT_MODE_6, FLIGHT_MODE_6); - set(PARAM_FIRMWARE_VER, FIRMWARE_VERSION); - set(PARAM_RADIOTRIM_CH1, 1500); - set(PARAM_RADIOTRIM_CH2, 1500); - set(PARAM_RADIOTRIM_CH3, 1500); - set(PARAM_RADIOTRIM_CH4, 1500); - set(PARAM_RADIOTRIM_CH5, 1500); - set(PARAM_RADIOTRIM_CH6, 1500); - set(PARAM_RADIOTRIM_CH7, 1500); - set(PARAM_RADIOTRIM_CH8, 1500); - set(PARAM_RADIOMIN_CH1, 1000); - set(PARAM_RADIOMIN_CH2, 1000); - set(PARAM_RADIOMIN_CH3, 1000); - set(PARAM_RADIOMIN_CH4, 1000); - set(PARAM_RADIOMIN_CH5, CH5_MIN); - set(PARAM_RADIOMIN_CH6, CH6_MIN); - set(PARAM_RADIOMIN_CH7, CH7_MIN); - set(PARAM_RADIOMIN_CH8, CH8_MIN); - set(PARAM_RADIOMAX_CH1, 2000); - set(PARAM_RADIOMAX_CH2, 2000); - set(PARAM_RADIOMAX_CH3, 2000); - set(PARAM_RADIOMAX_CH4, 2000); - set(PARAM_RADIOMAX_CH5, CH5_MAX); - set(PARAM_RADIOMAX_CH6, CH6_MAX); - set(PARAM_RADIOMAX_CH7, CH7_MAX); - set(PARAM_RADIOMAX_CH8, CH8_MAX); - set(PARAM_LOG_BITMASK, 0); - set(PARAM_TRIM_ELEVON, 1500); - set(PARAM_THR_FS_VALUE, THROTTLE_FS_VALUE); - set(PARAM_XTRACK_GAIN, XTRACK_GAIN); - set(PARAM_GND_TEMP, 0); - set(PARAM_AP_OFFSET, 0); - set(PARAM_TRIM_PITCH, 0); - set(PARAM_GND_ABS_PRESS, 0); -} diff --git a/Tools/ArduTracker/param_table.c b/Tools/ArduTracker/param_table.c deleted file mode 100644 index 7e37cf3c74..0000000000 --- a/Tools/ArduTracker/param_table.c +++ /dev/null @@ -1,230 +0,0 @@ -// -// THIS FILE WAS AUTOMATICALLY GENERATED - DO NOT EDIT -// -/// @file param_table.c - -#pragma pack(push) -#pragma pack(1) - -#include -#include -#include "global_data.h" - -#define PARAM_NAME(_name) static const char _param_ ## _name [] PROGMEM = #_name; -#define PARAM_INDEX(_name) [PARAM_ ## _name] = _param_ ## _name - -PARAM_NAME(THR_FS_VALUE); -PARAM_NAME(XTRACK_GAIN); -PARAM_NAME(GND_TEMP); -PARAM_NAME(AP_OFFSET); -PARAM_NAME(TRIM_PITCH); -PARAM_NAME(GND_ABS_PRESS); -PARAM_NAME(PTCH2SRV_D); -PARAM_NAME(PTCH2SRV_IMAX); -PARAM_NAME(ARSPD2PTCH_P); -PARAM_NAME(ALT2THR_P); -PARAM_NAME(ARSPD2PTCH_I); -PARAM_NAME(ALT2THR_I); -PARAM_NAME(ARSPD2PTCH_D); -PARAM_NAME(ALT2THR_D); -PARAM_NAME(ARSPD2PTCH_IMAX); -PARAM_NAME(ALT2PTCH_D); -PARAM_NAME(ALT2THR_IMAX); -PARAM_NAME(YW2SRV_P); -PARAM_NAME(ALT2PTCH_IMAX); -PARAM_NAME(ENRGY2THR_P); -PARAM_NAME(YW2SRV_I); -PARAM_NAME(KFF_PTCHCOMP); -PARAM_NAME(ENRGY2THR_I); -PARAM_NAME(YW2SRV_D); -PARAM_NAME(LIM_PITCH_MIN); -PARAM_NAME(KFF_RDDRMIX); -PARAM_NAME(ENRGY2THR_D); -PARAM_NAME(YW2SRV_IMAX); -PARAM_NAME(ALT_MIX); -PARAM_NAME(KFF_PTCH2THR); -PARAM_NAME(ENRGY2THR_IMAX); -PARAM_NAME(ALT_HOLD_HOME); -PARAM_NAME(GND_ALT); -PARAM_NAME(ALT2PTCH_P); -PARAM_NAME(YAW_MODE); -PARAM_NAME(ARSPD_RATIO); -PARAM_NAME(TRIM_AIRSPEED); -PARAM_NAME(ALT2PTCH_I); -PARAM_NAME(WP_MODE); -PARAM_NAME(IMU_OFFSET_0); -PARAM_NAME(XTRACK_ANGLE); -PARAM_NAME(WP_TOTAL); -PARAM_NAME(IMU_OFFSET_1); -PARAM_NAME(LIM_ROLL); -PARAM_NAME(THR_FAILSAFE); -PARAM_NAME(WP_INDEX); -PARAM_NAME(IMU_OFFSET_2); -PARAM_NAME(LIM_PITCH_MAX); -PARAM_NAME(THR_FS_ACTION); -PARAM_NAME(WP_RADIUS); -PARAM_NAME(IMU_OFFSET_3); -PARAM_NAME(TRIM_THROTTLE); -PARAM_NAME(LOITER_RADIUS); -PARAM_NAME(IMU_OFFSET_4); -PARAM_NAME(CONFIG); -PARAM_NAME(ARSPD_FBW_MIN); -PARAM_NAME(IMU_OFFSET_5); -PARAM_NAME(FLIGHT_MODE_4); -PARAM_NAME(ARSPD_FBW_MAX); -PARAM_NAME(FLIGHT_MODE_5); -PARAM_NAME(TRIM_AUTO); -PARAM_NAME(THR_MIN); -PARAM_NAME(FLIGHT_MODE_6); -PARAM_NAME(SWITCH_ENABLE); -PARAM_NAME(THR_MAX); -PARAM_NAME(RADIOTRIM_CH7); -PARAM_NAME(FIRMWARE_VER); -PARAM_NAME(FLIGHT_MODE_CH); -PARAM_NAME(RADIOTRIM_CH8); -PARAM_NAME(RADIOTRIM_CH1); -PARAM_NAME(FLIGHT_MODE_1); -PARAM_NAME(RADIOMIN_CH1); -PARAM_NAME(RADIOTRIM_CH2); -PARAM_NAME(FLIGHT_MODE_2); -PARAM_NAME(RADIOMAX_CH1); -PARAM_NAME(RADIOMIN_CH2); -PARAM_NAME(RADIOTRIM_CH3); -PARAM_NAME(FLIGHT_MODE_3); -PARAM_NAME(RADIOMAX_CH2); -PARAM_NAME(RADIOMIN_CH3); -PARAM_NAME(RADIOTRIM_CH4); -PARAM_NAME(RADIOMAX_CH3); -PARAM_NAME(RADIOMIN_CH4); -PARAM_NAME(RADIOTRIM_CH5); -PARAM_NAME(RADIOMAX_CH4); -PARAM_NAME(RADIOMIN_CH5); -PARAM_NAME(RADIOTRIM_CH6); -PARAM_NAME(RADIOMAX_CH5); -PARAM_NAME(RADIOMIN_CH6); -PARAM_NAME(RADIOMAX_CH6); -PARAM_NAME(RADIOMIN_CH7); -PARAM_NAME(RADIOMAX_CH7); -PARAM_NAME(RADIOMIN_CH8); -PARAM_NAME(RADIOMAX_CH8); -PARAM_NAME(LOG_BITMASK); -PARAM_NAME(TRIM_ELEVON); -PARAM_NAME(HDNG2RLL_P); -PARAM_NAME(HDNG2RLL_I); -PARAM_NAME(HDNG2RLL_D); -PARAM_NAME(HDNG2RLL_IMAX); -PARAM_NAME(RLL2SRV_P); -PARAM_NAME(RLL2SRV_I); -PARAM_NAME(RLL2SRV_D); -PARAM_NAME(RLL2SRV_IMAX); -PARAM_NAME(PTCH2SRV_P); -PARAM_NAME(PTCH2SRV_I); - -const char *param_nametab[] PROGMEM = { - PARAM_INDEX(THR_FS_VALUE), - PARAM_INDEX(XTRACK_GAIN), - PARAM_INDEX(GND_TEMP), - PARAM_INDEX(AP_OFFSET), - PARAM_INDEX(TRIM_PITCH), - PARAM_INDEX(GND_ABS_PRESS), - PARAM_INDEX(PTCH2SRV_D), - PARAM_INDEX(PTCH2SRV_IMAX), - PARAM_INDEX(ARSPD2PTCH_P), - PARAM_INDEX(ARSPD2PTCH_I), - PARAM_INDEX(ALT2THR_P), - PARAM_INDEX(ARSPD2PTCH_D), - PARAM_INDEX(ALT2THR_I), - PARAM_INDEX(ARSPD2PTCH_IMAX), - PARAM_INDEX(ALT2THR_D), - PARAM_INDEX(YW2SRV_P), - PARAM_INDEX(ALT2THR_IMAX), - PARAM_INDEX(ALT2PTCH_D), - PARAM_INDEX(YW2SRV_I), - PARAM_INDEX(ENRGY2THR_P), - PARAM_INDEX(ALT2PTCH_IMAX), - PARAM_INDEX(YW2SRV_D), - PARAM_INDEX(ENRGY2THR_I), - PARAM_INDEX(KFF_PTCHCOMP), - PARAM_INDEX(YW2SRV_IMAX), - PARAM_INDEX(ENRGY2THR_D), - PARAM_INDEX(KFF_RDDRMIX), - PARAM_INDEX(LIM_PITCH_MIN), - PARAM_INDEX(ENRGY2THR_IMAX), - PARAM_INDEX(KFF_PTCH2THR), - PARAM_INDEX(ALT_MIX), - PARAM_INDEX(ALT2PTCH_P), - PARAM_INDEX(GND_ALT), - PARAM_INDEX(ALT_HOLD_HOME), - PARAM_INDEX(ALT2PTCH_I), - PARAM_INDEX(TRIM_AIRSPEED), - PARAM_INDEX(ARSPD_RATIO), - PARAM_INDEX(YAW_MODE), - PARAM_INDEX(XTRACK_ANGLE), - PARAM_INDEX(IMU_OFFSET_0), - PARAM_INDEX(WP_MODE), - PARAM_INDEX(LIM_ROLL), - PARAM_INDEX(IMU_OFFSET_1), - PARAM_INDEX(WP_TOTAL), - PARAM_INDEX(LIM_PITCH_MAX), - PARAM_INDEX(IMU_OFFSET_2), - PARAM_INDEX(WP_INDEX), - PARAM_INDEX(THR_FAILSAFE), - PARAM_INDEX(IMU_OFFSET_3), - PARAM_INDEX(WP_RADIUS), - PARAM_INDEX(THR_FS_ACTION), - PARAM_INDEX(IMU_OFFSET_4), - PARAM_INDEX(LOITER_RADIUS), - PARAM_INDEX(TRIM_THROTTLE), - PARAM_INDEX(FLIGHT_MODE_4), - PARAM_INDEX(IMU_OFFSET_5), - PARAM_INDEX(ARSPD_FBW_MIN), - PARAM_INDEX(CONFIG), - PARAM_INDEX(TRIM_AUTO), - PARAM_INDEX(FLIGHT_MODE_5), - PARAM_INDEX(ARSPD_FBW_MAX), - PARAM_INDEX(SWITCH_ENABLE), - PARAM_INDEX(FLIGHT_MODE_6), - PARAM_INDEX(THR_MIN), - PARAM_INDEX(FLIGHT_MODE_CH), - PARAM_INDEX(FIRMWARE_VER), - PARAM_INDEX(RADIOTRIM_CH7), - PARAM_INDEX(THR_MAX), - PARAM_INDEX(FLIGHT_MODE_1), - PARAM_INDEX(RADIOTRIM_CH1), - PARAM_INDEX(RADIOTRIM_CH8), - PARAM_INDEX(FLIGHT_MODE_2), - PARAM_INDEX(RADIOTRIM_CH2), - PARAM_INDEX(RADIOMIN_CH1), - PARAM_INDEX(FLIGHT_MODE_3), - PARAM_INDEX(RADIOTRIM_CH3), - PARAM_INDEX(RADIOMIN_CH2), - PARAM_INDEX(RADIOMAX_CH1), - PARAM_INDEX(RADIOTRIM_CH4), - PARAM_INDEX(RADIOMIN_CH3), - PARAM_INDEX(RADIOMAX_CH2), - PARAM_INDEX(RADIOTRIM_CH5), - PARAM_INDEX(RADIOMIN_CH4), - PARAM_INDEX(RADIOMAX_CH3), - PARAM_INDEX(RADIOTRIM_CH6), - PARAM_INDEX(RADIOMIN_CH5), - PARAM_INDEX(RADIOMAX_CH4), - PARAM_INDEX(RADIOMIN_CH6), - PARAM_INDEX(RADIOMAX_CH5), - PARAM_INDEX(RADIOMIN_CH7), - PARAM_INDEX(RADIOMAX_CH6), - PARAM_INDEX(RADIOMIN_CH8), - PARAM_INDEX(RADIOMAX_CH7), - PARAM_INDEX(RADIOMAX_CH8), - PARAM_INDEX(LOG_BITMASK), - PARAM_INDEX(TRIM_ELEVON), - PARAM_INDEX(HDNG2RLL_P), - PARAM_INDEX(HDNG2RLL_I), - PARAM_INDEX(HDNG2RLL_D), - PARAM_INDEX(HDNG2RLL_IMAX), - PARAM_INDEX(RLL2SRV_P), - PARAM_INDEX(RLL2SRV_I), - PARAM_INDEX(RLL2SRV_D), - PARAM_INDEX(RLL2SRV_IMAX), - PARAM_INDEX(PTCH2SRV_P), - PARAM_INDEX(PTCH2SRV_I), -}; diff --git a/Tools/ArduTracker/param_table.h b/Tools/ArduTracker/param_table.h deleted file mode 100644 index 744aa8e089..0000000000 --- a/Tools/ArduTracker/param_table.h +++ /dev/null @@ -1,142 +0,0 @@ -// -// THIS FILE WAS AUTOMATICALLY GENERATED - DO NOT EDIT -// -/// @file param_table.h - -#define PARAM_FIRST_FLOAT 0 -enum float_param_t { - PARAM_HDNG2RLL_P = PARAM_FIRST_FLOAT, - PARAM_HDNG2RLL_I, - PARAM_HDNG2RLL_D, - PARAM_HDNG2RLL_IMAX, - PARAM_RLL2SRV_P, - PARAM_RLL2SRV_I, - PARAM_RLL2SRV_D, - PARAM_RLL2SRV_IMAX, - PARAM_PTCH2SRV_P, - PARAM_PTCH2SRV_I, - PARAM_PTCH2SRV_D, - PARAM_PTCH2SRV_IMAX, - PARAM_ARSPD2PTCH_P, - PARAM_ARSPD2PTCH_I, - PARAM_ARSPD2PTCH_D, - PARAM_ARSPD2PTCH_IMAX, - PARAM_YW2SRV_P, - PARAM_YW2SRV_I, - PARAM_YW2SRV_D, - PARAM_YW2SRV_IMAX, - PARAM_ALT2THR_P, - PARAM_ALT2THR_I, - PARAM_ALT2THR_D, - PARAM_ALT2THR_IMAX, - PARAM_ENRGY2THR_P, - PARAM_ENRGY2THR_I, - PARAM_ENRGY2THR_D, - PARAM_ENRGY2THR_IMAX, - PARAM_ALT2PTCH_P, - PARAM_ALT2PTCH_I, - PARAM_ALT2PTCH_D, - PARAM_ALT2PTCH_IMAX, - PARAM_KFF_PTCHCOMP, - PARAM_KFF_RDDRMIX, - PARAM_KFF_PTCH2THR, - PARAM_GND_ALT, - PARAM_TRIM_AIRSPEED, - PARAM_XTRACK_ANGLE, - PARAM_LIM_ROLL, - PARAM_LIM_PITCH_MAX, - PARAM_LIM_PITCH_MIN, - PARAM_ALT_MIX, - PARAM_ALT_HOLD_HOME, - PARAM_ARSPD_RATIO, - PARAM_IMU_OFFSET_0, - PARAM_IMU_OFFSET_1, - PARAM_IMU_OFFSET_2, - PARAM_IMU_OFFSET_3, - PARAM_IMU_OFFSET_4, - PARAM_IMU_OFFSET_5, - PARAM_LAST_FLOAT -}; -#define PARAM_FLOAT_COUNT (PARAM_LAST_FLOAT - PARAM_FIRST_FLOAT) - -#define PARAM_FIRST_UINT8 PARAM_LAST_FLOAT -enum uint8_param_t { - PARAM_YAW_MODE = PARAM_FIRST_UINT8, - PARAM_WP_MODE, - PARAM_WP_TOTAL, - PARAM_WP_INDEX, - PARAM_WP_RADIUS, - PARAM_LOITER_RADIUS, - PARAM_ARSPD_FBW_MIN, - PARAM_ARSPD_FBW_MAX, - PARAM_THR_MIN, - PARAM_THR_MAX, - PARAM_THR_FAILSAFE, - PARAM_THR_FS_ACTION, - PARAM_TRIM_THROTTLE, - PARAM_CONFIG, - PARAM_TRIM_AUTO, - PARAM_SWITCH_ENABLE, - PARAM_FLIGHT_MODE_CH, - PARAM_FLIGHT_MODE_1, - PARAM_FLIGHT_MODE_2, - PARAM_FLIGHT_MODE_3, - PARAM_FLIGHT_MODE_4, - PARAM_FLIGHT_MODE_5, - PARAM_FLIGHT_MODE_6, - PARAM_LAST_UINT8 -}; -#define PARAM_UINT8_COUNT (PARAM_LAST_UINT8 - PARAM_FIRST_UINT8) - -#define PARAM_FIRST_UINT16 PARAM_LAST_UINT8 -enum uint16_param_t { - PARAM_FIRMWARE_VER = PARAM_FIRST_UINT16, - PARAM_RADIOTRIM_CH1, - PARAM_RADIOTRIM_CH2, - PARAM_RADIOTRIM_CH3, - PARAM_RADIOTRIM_CH4, - PARAM_RADIOTRIM_CH5, - PARAM_RADIOTRIM_CH6, - PARAM_RADIOTRIM_CH7, - PARAM_RADIOTRIM_CH8, - PARAM_RADIOMIN_CH1, - PARAM_RADIOMIN_CH2, - PARAM_RADIOMIN_CH3, - PARAM_RADIOMIN_CH4, - PARAM_RADIOMIN_CH5, - PARAM_RADIOMIN_CH6, - PARAM_RADIOMIN_CH7, - PARAM_RADIOMIN_CH8, - PARAM_RADIOMAX_CH1, - PARAM_RADIOMAX_CH2, - PARAM_RADIOMAX_CH3, - PARAM_RADIOMAX_CH4, - PARAM_RADIOMAX_CH5, - PARAM_RADIOMAX_CH6, - PARAM_RADIOMAX_CH7, - PARAM_RADIOMAX_CH8, - PARAM_LOG_BITMASK, - PARAM_TRIM_ELEVON, - PARAM_THR_FS_VALUE, - PARAM_LAST_UINT16 -}; -#define PARAM_UINT16_COUNT (PARAM_LAST_UINT16 - PARAM_FIRST_UINT16) - -#define PARAM_FIRST_INT16 PARAM_LAST_UINT16 -enum int16_param_t { - PARAM_XTRACK_GAIN = PARAM_FIRST_INT16, - PARAM_GND_TEMP, - PARAM_AP_OFFSET, - PARAM_TRIM_PITCH, - PARAM_LAST_INT16 -}; -#define PARAM_INT16_COUNT (PARAM_LAST_INT16 - PARAM_FIRST_INT16) - -#define PARAM_FIRST_UINT32 PARAM_LAST_INT16 -enum uint32_param_t { - PARAM_GND_ABS_PRESS = PARAM_FIRST_UINT32, - PARAM_LAST_UINT32 -}; -#define PARAM_UINT32_COUNT (PARAM_LAST_UINT32 - PARAM_FIRST_UINT32) - -#define PARAM_COUNT PARAM_LAST_UINT32 diff --git a/Tools/ArduTracker/paramgen.awk b/Tools/ArduTracker/paramgen.awk deleted file mode 100644 index 2329fa40c7..0000000000 --- a/Tools/ArduTracker/paramgen.awk +++ /dev/null @@ -1,126 +0,0 @@ -# -# Process the parameter specification and produce the parameter enumerations and -# name table. -# -# See paramgen.in for details of the input format -# - -BEGIN { - paramIndex = 0 - typeIndex = 0 - firstInType = 0 - - printf("//\n// THIS FILE WAS AUTOMATICALLY GENERATED - DO NOT EDIT\n//\n") > "param_table.h" - printf("/// @file param_table.h\n\n") > "param_table.h" - - printf("//\n// THIS FILE WAS AUTOMATICALLY GENERATED - DO NOT EDIT\n//\n") > "param_init.pde" - printf("/// @file param_init.pde\n\n") > "param_init.pde" - printf("void param_reset_defaults(void)\n") > "param_init.pde" - printf("{\n") > "param_init.pde" - - printf("//\n// THIS FILE WAS AUTOMATICALLY GENERATED - DO NOT EDIT\n//\n") > "param_table.c" - printf("/// @file param_table.c\n\n") > "param_table.c" - printf("#pragma pack(push)\n") > "param_table.c" - printf("#pragma pack(1)\n\n") > "param_table.c" - printf("#include \n") > "param_table.c" - printf("#include \n") > "param_table.c" - printf("#include \"global_data.h\"\n\n") > "param_table.c" - printf("#define PARAM_NAME(_name) static const char _param_ ## _name [] PROGMEM = #_name;\n") > "param_table.c" - printf("#define PARAM_INDEX(_name) [PARAM_ ## _name] = _param_ ## _name\n\n") > "param_table.c" - - -} - -function END_ENUM() { - printf(" PARAM_LAST_%s\n", currentType) > "param_table.h" - printf("};\n") > "param_table.h" - printf("#define PARAM_%s_COUNT (PARAM_LAST_%s - PARAM_FIRST_%s)\n\n", - currentType, currentType, currentType) > "param_table.h" -} - -# -# skip lines containing comments -# -$1=="#" { next } - -# -# process a type section change -# -$1=="type" { - - newType = toupper($2) - - # if there's a type already open, close it - if (currentType == "") { - # first enum opens at index zero - printf("#define PARAM_FIRST_%s 0\n", newType) > "param_table.h" - } else { - # finalise the preceding enum - END_ENUM() - - # chain the next enum's starting value off the previous - printf("#define PARAM_FIRST_%s PARAM_LAST_%s\n", newType, currentType) > "param_table.h" - } - printf("enum %s_param_t {\n", tolower(newType)) > "param_table.h" - - currentType = newType - firstInType = 1 - - next -} - -# -# process a parameter name -# -NF >= 1 { - - paramName = $1 - paramInitial = $2 - - # emit the parameter inside the enum for param_table.h - if (firstInType) { - printf(" PARAM_%s = PARAM_FIRST_%s,\n", paramName, currentType) > "param_table.h" - firstInType = 0 - } else { - printf(" PARAM_%s,\n", paramName) > "param_table.h" - } - - # emit the call to the initialiser for param_init.pde - if (paramInitial != "") { - printf(" set(PARAM_%s, %s);\n", paramName, paramInitial) > "param_init.pde" - } - - # save name for param_table.c - paramNames[paramIndex] = paramName - paramIndex++ -} - -END { - # - # close out the current enum - # - END_ENUM() - printf("#define PARAM_COUNT PARAM_LAST_%s\n", currentType) > "param_table.h" - - # - # close the initialiser function - # - printf("}\n") > "param_init.pde" - - - # - # Generate param_table.c - # - # emit the PARAM_NAME invocations - for (name in paramNames) { - printf("PARAM_NAME(%s);\n", paramNames[name]) > "param_table.c" - } - - # emit the PARAM_INDEX array - printf("\nconst char *param_nametab[] PROGMEM = {\n") > "param_table.c" - for (name in paramNames) { - printf(" PARAM_INDEX(%s),\n", paramNames[name]) > "param_table.c" - } - printf("};\n") > "param_table.c" -} - diff --git a/Tools/ArduTracker/paramgen.in b/Tools/ArduTracker/paramgen.in deleted file mode 100644 index 4bc159a43d..0000000000 --- a/Tools/ArduTracker/paramgen.in +++ /dev/null @@ -1,257 +0,0 @@ -# -# Parameter database specification -# -# This file describes the global parameters used by the ArduPilot Mega software. -# Definitions here are used to produce the param_table.h, param_table.c and -# param_init.pde files. -# -# Process this file with "awk -f paramgen.awk paramgen.in" -# - -# -# Parameters are grouped by type, with the software expecting that the types -# will be presented in the order float, uint8, uint16, int16, uint32. -# -# The following line formats are recognised -# -# type -# -# Announces the start of the section of the file conatining parameters -# of the type . -# -# -# -# Defines a parameter with the name . This will result in -# the generation of an enumeration named PARAM_, and the -# creation of an entry in the global param_nametab array indexed by the -# enumeration, pointing to a PROGMEM string containing . -# -# -# -# As above, but additionally an entry will be added to the param_init() -# function that will reset the parameter to . -# -# -# ------------------------------------------------------------------ -# -# !!! CHANGE THE FIRMWARE VERSION IF YOU MODIFY THIS FILE !!! -# -# If the firmware version in ROM and the firmware version -# in this file do not match it is assumed that the EEProm is -# in an unknown state and a factory reset is forced. -# This is to prevent the user from reading EEProm values that -# have been remapped to different locations in memory. -# - -# ====================================================================== -type float - - # ------------------------------------------------------------------ - # Gains - - # ------------------------------------------------------------------ - # Roll Control - - # heading error from commnd to roll command deviation from trim - # (bank to turn strategy) - HDNG2RLL_P NAV_ROLL_P - HDNG2RLL_I NAV_ROLL_I - HDNG2RLL_D NAV_ROLL_D - HDNG2RLL_IMAX NAV_ROLL_INT_MAX - - # roll error from command to roll servo deviation from trim - # (tracks commanded bank angle) - RLL2SRV_P SERVO_ROLL_P - RLL2SRV_I SERVO_ROLL_I - RLL2SRV_D SERVO_ROLL_D - RLL2SRV_IMAX SERVO_ROLL_INT_MAX - - # ------------------------------------------------------------------ - # Pitch Control - - # pitch error from command to pitch servo deviation from trim - # (front-side strategy) - PTCH2SRV_P SERVO_PITCH_P - PTCH2SRV_I SERVO_PITCH_I - PTCH2SRV_D SERVO_PITCH_D - PTCH2SRV_IMAX SERVO_PITCH_INT_MAX - - # airspeed error from commnd to pitch servo deviation from trim - # (back-side strategy) - ARSPD2PTCH_P NAV_PITCH_ASP_P - ARSPD2PTCH_I NAV_PITCH_ASP_I - ARSPD2PTCH_D NAV_PITCH_ASP_D - ARSPD2PTCH_IMAX NAV_PITCH_ASP_INT_MAX - - # ------------------------------------------------------------------ - # Yaw Control - - # yaw rate error from commnd to yaw servo deviation from trim - # (stabilizes dutch roll) - YW2SRV_P SERVO_YAW_P - YW2SRV_I SERVO_YAW_I - YW2SRV_D SERVO_YAW_D - YW2SRV_IMAX SERVO_YAW_INT_MAX - - - # ------------------------------------------------------------------ - # Throttle Control - - # altitude error from commnd to throttle servo deviation from trim - # (throttle back-side strategy) - ALT2THR_P THROTTLE_ALT_P - ALT2THR_I THROTTLE_ALT_I - ALT2THR_D THROTTLE_ALT_D - ALT2THR_IMAX THROTTLE_ALT_INT_MAX - - # total energy error from commnd to throttle servo deviation from trim - # (throttle back-side strategy alternative) - ENRGY2THR_P THROTTLE_TE_P - ENRGY2THR_I THROTTLE_TE_I - ENRGY2THR_D THROTTLE_TE_D - ENRGY2THR_IMAX THROTTLE_TE_INT_MAX - - # altitude error from commnd to pitch servo deviation from trim - # (throttle front-side strategy alternative) - ALT2PTCH_P NAV_PITCH_ALT_P - ALT2PTCH_I NAV_PITCH_ALT_I - ALT2PTCH_D NAV_PITCH_ALT_D - ALT2PTCH_IMAX NAV_PITCH_ALT_INT_MAX - - - # feed forward gains - KFF_PTCHCOMP PITCH_COMP - KFF_RDDRMIX RUDDER_MIX - KFF_PTCH2THR P_TO_T - - # misc - GND_ALT 0 - TRIM_AIRSPEED AIRSPEED_CRUISE - XTRACK_ANGLE XTRACK_ENTRY_ANGLE - - # limits - LIM_ROLL HEAD_MAX - LIM_PITCH_MAX PITCH_MAX - LIM_PITCH_MIN PITCH_MIN - - # estimation - ALT_MIX ALTITUDE_MIX - ALT_HOLD_HOME 0 - ARSPD_RATIO AIRSPEED_RATIO - - # ------------------------------------------------------------------ - # IMU Calibration - - IMU_OFFSET_0 0 - IMU_OFFSET_1 0 - IMU_OFFSET_2 0 - IMU_OFFSET_3 0 - IMU_OFFSET_4 0 - IMU_OFFSET_5 0 - -# ====================================================================== -type uint8 - - # not used currently - YAW_MODE 0 - - # waypoints - WP_MODE 0 - WP_TOTAL WP_SIZE - WP_INDEX 0 - WP_RADIUS WP_RADIUS_DEFAULT - LOITER_RADIUS LOITER_RADIUS_DEFAULT - - # fly by wire - ARSPD_FBW_MIN AIRSPEED_FBW_MIN - ARSPD_FBW_MAX AIRSPEED_FBW_MAX - - # throttle - THR_MIN THROTTLE_MIN - THR_MAX THROTTLE_MAX - THR_FAILSAFE THROTTLE_FAILSAFE - THR_FS_ACTION THROTTLE_FAILSAFE_ACTION - TRIM_THROTTLE THROTTLE_CRUISE - - # misc - CONFIG 0 - TRIM_AUTO AUTO_TRIM - SWITCH_ENABLE REVERSE_SWITCH - - # flight modes - FLIGHT_MODE_CH FLIGHT_MODE_CHANNEL - FLIGHT_MODE_1 FLIGHT_MODE_1 - FLIGHT_MODE_2 FLIGHT_MODE_2 - FLIGHT_MODE_3 FLIGHT_MODE_3 - FLIGHT_MODE_4 FLIGHT_MODE_4 - FLIGHT_MODE_5 FLIGHT_MODE_5 - FLIGHT_MODE_6 FLIGHT_MODE_6 - -# ====================================================================== -type uint16 - - FIRMWARE_VER FIRMWARE_VERSION - - # ------------------------------------------------------------------ - # Radio Settings - # - # all radio settings are uint16_t and represent the period of the - # pulse width modulated signal. Typically 1000 ms is the lower limit, - # 1500 is neutral, and 2000 is the upper limit - - # trim (neutral setting) - RADIOTRIM_CH1 1500 - RADIOTRIM_CH2 1500 - RADIOTRIM_CH3 1500 - RADIOTRIM_CH4 1500 - RADIOTRIM_CH5 1500 - RADIOTRIM_CH6 1500 - RADIOTRIM_CH7 1500 - RADIOTRIM_CH8 1500 - - # min (maps to 0%) - RADIOMIN_CH1 1000 - RADIOMIN_CH2 1000 - RADIOMIN_CH3 1000 - RADIOMIN_CH4 1000 - RADIOMIN_CH5 CH5_MIN - RADIOMIN_CH6 CH6_MIN - RADIOMIN_CH7 CH7_MIN - RADIOMIN_CH8 CH8_MIN - - # max (maps to 100%) - RADIOMAX_CH1 2000 - RADIOMAX_CH2 2000 - RADIOMAX_CH3 2000 - RADIOMAX_CH4 2000 - RADIOMAX_CH5 CH5_MAX - RADIOMAX_CH6 CH6_MAX - RADIOMAX_CH7 CH7_MAX - RADIOMAX_CH8 CH8_MAX - - # ------------------------------------------------------------------ - # Misc - - LOG_BITMASK 0 - TRIM_ELEVON 1500 - THR_FS_VALUE THROTTLE_FS_VALUE - -# ====================================================================== -type int16 - - # ------------------------------------------------------------------ - # Misc - - XTRACK_GAIN XTRACK_GAIN - GND_TEMP 0 - AP_OFFSET 0 - TRIM_PITCH 0 - - -# ====================================================================== -type uint32 - - # ------------------------------------------------------------------ - # Misc - - GND_ABS_PRESS 0 diff --git a/Tools/ArduTracker/radio.pde b/Tools/ArduTracker/radio.pde deleted file mode 100644 index 4a6d05e8c0..0000000000 --- a/Tools/ArduTracker/radio.pde +++ /dev/null @@ -1,199 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -//Function that will read the radio data, limit servos and trigger a failsafe -// ---------------------------------------------------------------------------- -byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling - -void read_radio() -{ - ch1_temp = APM_RC.InputCh(CH_ROLL); - ch2_temp = APM_RC.InputCh(CH_PITCH); - - if(mix_mode == 0){ - radio_in[CH_ROLL] = ch1_temp; - radio_in[CH_PITCH] = ch2_temp; - }else{ - radio_in[CH_ROLL] = reverse_elevons * (reverse_ch2_elevon * int(ch2_temp - elevon2_trim) - reverse_ch1_elevon * int(ch1_temp - elevon1_trim)) / 2 + 1500; - radio_in[CH_PITCH] = (reverse_ch2_elevon * int(ch2_temp - elevon2_trim) + reverse_ch1_elevon * int(ch1_temp - elevon1_trim)) / 2 + 1500; - } - - for (int y = 2; y < 8; y++) - radio_in[y] = APM_RC.InputCh(y); - - #if THROTTLE_REVERSE == 1 - radio_in[CH_THROTTLE] = radio_max(CH_THROTTLE) + radio_min(CH_THROTTLE) - radio_in[CH_THROTTLE]; - #endif - - throttle_failsafe(radio_in[CH_THROTTLE]); - servo_out[CH_THROTTLE] = ((float)(radio_in[CH_THROTTLE] - radio_min(CH_THROTTLE)) / - (float)(radio_max(CH_THROTTLE) - radio_min(CH_THROTTLE))) * 100; - servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], 0, 100); - - if (servo_out[CH_THROTTLE] > 50) { - if(AIRSPEED_SENSOR == 1) { - airspeed_nudge = (get(PARAM_ARSPD_FBW_MAX) - get(PARAM_TRIM_AIRSPEED)) * ((servo_out[CH_THROTTLE]-50) * 2); - } else { - throttle_nudge = (get(PARAM_THR_MAX) - get(PARAM_TRIM_THROTTLE)) * ((servo_out[CH_THROTTLE]-50) * 2); - } - } else { - airspeed_nudge = 0; - throttle_nudge = 0; - } -} - -void throttle_failsafe(uint16_t pwm) -{ - if(get(PARAM_THR_FAILSAFE)== 0) - return; - - //check for failsafe and debounce funky reads - // ------------------------------------------ - if (pwm < get(PARAM_THR_FS_VALUE)){ - // we detect a failsafe from radio - // throttle has dropped below the mark - failsafeCounter++; - if (failsafeCounter == 9){ - SendDebug("MSG FS ON "); - SendDebugln(pwm, DEC); - }else if(failsafeCounter == 10) { - ch3_failsafe = true; - //set_failsafe(true); - //failsafeCounter = 10; - }else if (failsafeCounter > 10){ - failsafeCounter = 11; - } - - }else if(failsafeCounter > 0){ - // we are no longer in failsafe condition - // but we need to recover quickly - failsafeCounter--; - if (failsafeCounter > 3){ - failsafeCounter = 3; - } - if (failsafeCounter == 1){ - SendDebug("MSG FS OFF "); - SendDebugln(pwm, DEC); - }else if(failsafeCounter == 0) { - ch3_failsafe = false; - //set_failsafe(false); - //failsafeCounter = -1; - }else if (failsafeCounter <0){ - failsafeCounter = -1; - } - } -} - -void trim_control_surfaces() -{ - // Store control surface trim values - // --------------------------------- - if(mix_mode == 0){ - set_radio_trim(CH_ROLL,radio_in[CH_ROLL]); - set_radio_trim(CH_PITCH,radio_in[CH_PITCH]); - set_radio_trim(CH_RUDDER,radio_in[CH_RUDDER]); - }else{ - elevon1_trim = ch1_temp; - elevon2_trim = ch2_temp; - //Recompute values here using new values for elevon1_trim and elevon2_trim - //We cannot use radio_in[CH_ROLL] and radio_in[CH_PITCH] values from read_radio() because the elevon trim values have changed - uint16_t center = 1500; - set_radio_trim(CH_ROLL,center); - set_radio_trim(CH_PITCH,center); - } - // disabled for now - //save_EEPROM_trims(); -} - -void trim_radio() -{ - for (int y = 0; y < 50; y++) { - read_radio(); - } - - // Store the trim values - // --------------------- - if(mix_mode == 0){ - for (int y = 0; y < 8; y++) set_radio_trim(y,radio_in[y]); - }else{ - elevon1_trim = ch1_temp; - elevon2_trim = ch2_temp; - uint16_t center = 1500; - set_radio_trim(CH_ROLL,center); - set_radio_trim(CH_PITCH,center); - for (int y = 2; y < 8; y++) set_radio_trim(y,radio_in[y]); - } - //save_EEPROM_trims(); -} - - -#if SET_RADIO_LIMITS == 1 -void read_radio_limits() -{ - // set initial servo limits for calibration routine - // ------------------------------------------------- - set_radio_min(CH_ROLL,radio_in[CH_ROLL] - 150); - set_radio_max(CH_ROLL,radio_in[CH_ROLL] + 150); - - set_radio_min(CH_PITCH,radio_in[CH_PITCH] - 150); - set_radio_max(CH_PITCH,radio_in[CH_PITCH] + 150); - - // vars for the radio config routine - // --------------------------------- - int counter = 0; - long reminder; - reminder = millis() - 10000; - - uint16_t tempMin[2] = {radio_min(CH_ROLL), radio_min(CH_PITCH)} ; - uint16_t tempMax[2] = {radio_max(CH_ROLL), radio_max(CH_PITCH)} ; - - // Allows user to set stick limits and calibrate the IR - // ---------------------------------------------------- - while(counter < 50){ - - if (millis() - reminder >= 10000) { // Remind user every 10 seconds what is going on - GCS.send_text(SEVERITY_LOW,"Reading radio limits:"); - GCS.send_text(SEVERITY_LOW,"Move sticks to: upper right and lower Left"); - GCS.send_text(SEVERITY_LOW,"To Continue, hold the stick in the corner for 2 seconds."); - GCS.send_text(SEVERITY_LOW," "); - //print_radio(); - demo_servos(1); - reminder = millis(); - } - - delay(40); - read_radio(); - - // AutoSet servo limits - // -------------------- - if (radio_in[CH_ROLL] > 1000 && radio_in[CH_ROLL] < 2000){ - tempMin[CH_ROLL] = min(radio_in[CH_ROLL], tempMin[CH_ROLL]); - tempMax[CH_ROLL] = max(radio_in[CH_ROLL], tempMax[CH_ROLL]); - } - - if (radio_in[CH_PITCH] > 1000 && radio_in[CH_PITCH]< 2000){ - tempMin[CH_PITCH] = min(radio_in[CH_PITCH], tempMin[CH_PITCH]); - tempMax[CH_PITCH] = max(radio_in[CH_PITCH], tempMax[CH_PITCH]); - } - if(radio_in[CH_PITCH] < (tempMin[CH_PITCH] + 30) || tempMin[CH_PITCH] > (tempMax[CH_PITCH] -30)){ - SendDebug("."); - counter++; - }else{ - if (counter > 0) - counter--; - } - } - - // contstrain min values - // --------------------- - set_radio_min(CH_ROLL,constrain(tempMin[CH_ROLL], 800, 2200)); - set_radio_max(CH_ROLL,constrain(tempMax[CH_ROLL], 800, 2200)); - set_radio_min(CH_PITCH,constrain(tempMin[CH_PITCH], 800, 2200)); - set_radio_max(CH_PITCH,constrain(tempMax[CH_PITCH], 800, 2200)); - - SendDebugln(" "); -} -#endif - - - - diff --git a/Tools/ArduTracker/sensors.pde b/Tools/ArduTracker/sensors.pde deleted file mode 100644 index 555e0f13f7..0000000000 --- a/Tools/ArduTracker/sensors.pde +++ /dev/null @@ -1,70 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -void ReadSCP1000(void) {} - -// Sensors are not available in HIL_MODE_ATTITUDE -#if HIL_MODE != HIL_MODE_ATTITUDE - -void read_airpressure(void){ - double x; - - pitot.Read(); // Get new data from absolute pressure sensor - abs_press = pitot.Press; - abs_press_filt = (abs_press); // + 2l * abs_press_filt) / 3l; // Light filtering - //temperature = (temperature * 9 + temp_unfilt) / 10; We will just use the ground temp for the altitude calculation - - double p = (double)abs_press_gnd / (double)abs_press_filt; - double temp = (float)ground_temperature / 10.f + 273.15f; - x = log(p) * temp * 29271.267f; - //x = log(p) * temp * 29.271267 * 1000; - press_alt = (long)(x / 10) + ground_alt; // Pressure altitude in centimeters - // Need to add comments for theory..... -} - -// in M/S * 100 -void read_airspeed(void) -{ - #if GPS_PROTOCOL != GPS_PROTOCOL_IMU // Xplane will supply the airspeed - airpressure_raw = ((float)adc.Ch(AIRSPEED_CH) * .25) + (airpressure_raw * .75); - airpressure = (int)airpressure_raw - airpressure_offset; - airpressure = max(airpressure, 0); - airspeed = sqrtf((float)airpressure * get(PARAM_ARSPD_RATIO)) * 100; - #endif - - calc_airspeed_errors(); -} - -void zero_airspeed(void) -{ - airpressure_raw = (float)adc.Ch(AIRSPEED_CH); - for(int c = 0; c < 50; c++){ - delay(20); - airpressure_raw = (airpressure_raw * .90) + ((float)adc.Ch(AIRSPEED_CH) * .10); - } - airpressure_offset = airpressure_raw; -} - -#endif // HIL_MODE != HIL_MODE_ATTITUDE - -#if BATTERY_EVENT == 1 -void read_battery(void) -{ - battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9; - battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9; - battery_voltage3 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN3)) * .1 + battery_voltage3 * .9; - battery_voltage4 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN4)) * .1 + battery_voltage4 * .9; - - #if BATTERY_TYPE == 0 - if(battery_voltage3 < LOW_VOLTAGE) - low_battery_event(); - battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream - #endif - - #if BATTERY_TYPE == 1 - if(battery_voltage4 < LOW_VOLTAGE) - low_battery_event(); - battery_voltage = battery_voltage4; // set total battery voltage, for telemetry stream - #endif -} -#endif - diff --git a/Tools/ArduTracker/setup.pde b/Tools/ArduTracker/setup.pde deleted file mode 100644 index ba479a21df..0000000000 --- a/Tools/ArduTracker/setup.pde +++ /dev/null @@ -1,229 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -// Functions called from the setup menu -static int8_t setup_radio(uint8_t argc, const Menu::arg *argv); -static int8_t setup_show(uint8_t argc, const Menu::arg *argv); -static int8_t setup_factory(uint8_t argc, const Menu::arg *argv); -static int8_t setup_flightmodes(uint8_t argc, const Menu::arg *argv); - -// Command/function table for the setup menu -const struct Menu::command setup_menu_commands[] PROGMEM = { - // command function called - // ======= =============== - {"reset", setup_factory}, - {"radio", setup_radio}, - {"modes", setup_flightmodes}, - {"show", setup_show}, -}; - -// Create the setup menu object. -MENU(setup_menu, "setup", setup_menu_commands); - -// Called from the top-level menu to run the setup menu. -int8_t -setup_mode(uint8_t argc, const Menu::arg *argv) -{ - // Give the user some guidance - Serial.printf_P(PSTR("Setup Mode\n" - "\n" - "IMPORTANT: if you have not previously set this system up, use the\n" - "'reset' command to initialize the EEPROM to sensible default values\n" - "and then the 'radio' command to configure for your radio.\n" - "\n")); - - // Run the setup menu. When the menu exits, we will return to the main menu. - setup_menu.run(); -} - -// Print the current configuration. -// Called by the setup menu 'show' command. -static int8_t -setup_show(uint8_t argc, const Menu::arg *argv) -{ - printAllParams(Serial); - return(0); -} - -// Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults). -// Called by the setup menu 'factoryreset' command. -static int8_t -setup_factory(uint8_t argc, const Menu::arg *argv) -{ - - uint8_t i; - int c; - - Serial.printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: ")); - - do { - c = Serial.read(); - } while (-1 == c); - - if (('y' != c) && ('Y' != c)) - return(-1); - - Serial.printf_P(PSTR("\nFACTORY RESET\n\n")); - param_reset_defaults(); - return(0); -} - -// Perform radio setup. -// Called by the setup menu 'radio' command. -static int8_t -setup_radio(uint8_t argc, const Menu::arg *argv) -{ - Serial.println("\n\nRadio Setup:"); - uint8_t i; - - for(i = 0; i < 100;i++){ - delay(20); - read_radio(); - } - - if(radio_in[CH_ROLL] < 500){ - while(1){ - Serial.print("Radio error"); - delay(1000); - // stop here - } - } - - uint16_t tempMin[4], tempMax[4]; - for(i = 0; i < 4; i++){ - tempMin[i] = radio_in[i]; - tempMax[i] = radio_in[i]; - } - - Serial.printf_P(PSTR("\nMove both sticks to each corner. Hit Enter to save: ")); - while(1){ - - delay(20); - // Filters radio input - adjust filters in the radio.pde file - // ---------------------------------------------------------- - read_radio(); - - for(i = 0; i < 4; i++){ - tempMin[i] = min(tempMin[i], radio_in[i]); - tempMax[i] = max(tempMax[i], radio_in[i]); - } - - if(Serial.available() > 0){ - Serial.flush(); - Serial.println("Saving:"); - - for(i = 0; i < 4; i++) - { - set_radio_max(i, tempMax[i]); - set_radio_min(i, tempMin[i]); - } - - for(i = 0; i < 8; i++) - Serial.printf_P(PSTR("CH%d: %d | %d\n"), i + 1, radio_min(i), radio_max(i)); - break; - } - } - - return(0); -} - - -static int8_t -setup_flightmodes(uint8_t argc, const Menu::arg *argv) -{ - byte switchPosition, oldSwitchPosition, mode; - - Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes.")); - print_hit_enter(); - trim_radio(); - - while(1){ - delay(20); - read_radio(); - switchPosition = readSwitch(); - - - // look for control switch change - if (oldSwitchPosition != switchPosition){ - // Override position 5 - if(switchPosition > 4){ - set_flight_mode(switchPosition,MANUAL); - mode = MANUAL; - }else{ - // update our current mode - mode = flight_mode(switchPosition); - } - - // update the user - print_switch(switchPosition, mode); - - // Remember switch position - oldSwitchPosition = switchPosition; - } - - // look for stick input - int radioInputSwitch = radio_input_switch(); - - if (radioInputSwitch != 0){ - - mode += radioInputSwitch; - - while ( - mode != MANUAL && - mode != CIRCLE && - mode != STABILIZE && - mode != FLY_BY_WIRE_A && - mode != FLY_BY_WIRE_B && - mode != AUTO && - mode != RTL && - mode != LOITER && - mode != TAKEOFF && - mode != LAND) - { - if (mode < MANUAL) - mode = LAND; - else if (mode >LAND) - mode = MANUAL; - else - mode += radioInputSwitch; - } - - // Override position 5 - if(switchPosition > 4) - mode = MANUAL; - - // save new mode - set_flight_mode(switchPosition,mode); - - // print new mode - print_switch(switchPosition, mode); - } - - // escape hatch - if(Serial.available() > 0){ - //save_EEPROM_flight_modes(); - return (0); - } - } -} - -void -print_switch(byte p, byte m) -{ - Serial.printf_P(PSTR("Pos %d: "),p); - Serial.println(flight_mode_strings[m]); -} - -int8_t -radio_input_switch(void) -{ - static int8_t bouncer = 0; - - if (int16_t(radio_in[0] - radio_trim(0)) > 200) bouncer = 10; - if (int16_t(radio_in[0] - radio_trim(0)) < -200) bouncer = -10; - if (bouncer >0) bouncer --; - if (bouncer <0) bouncer ++; - - if (bouncer == 1 || bouncer == -1) return bouncer; - else return 0; -} - diff --git a/Tools/ArduTracker/system.pde b/Tools/ArduTracker/system.pde deleted file mode 100644 index 911421f445..0000000000 --- a/Tools/ArduTracker/system.pde +++ /dev/null @@ -1,511 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/***************************************************************************** -The init_ardupilot function processes everything we need for an in - air restart - We will determine later if we are actually on the ground and process a - ground start in that case. - -*****************************************************************************/ - -// Functions called from the top-level menu -extern int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde -extern int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde -extern int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp - -// This is the help function -// PSTR is an AVR macro to read strings from flash memory -// printf_P is a version of print_f that reads from flash memory -static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv) -{ - Serial.printf_P(PSTR("Commands:\n" - " logs log readback/setup mode\n" - " setup setup mode\n" - " test test mode\n" - "\n" - "Move the slide switch and reset to FLY.\n" - "\n")); - return(0); -} - -// Command/function table for the top-level menu. -const struct Menu::command main_menu_commands[] PROGMEM = { -// command function called -// ======= =============== - {"logs", process_logs}, - {"setup", setup_mode}, - {"test", test_mode}, - {"help", main_menu_help} -}; - -// Create the top-level menu object. -MENU(main_menu, "ArduPilotMega", main_menu_commands); - -void init_ardupilot() -{ - - byte last_log_num; - int last_log_start; - int last_log_end; - - // Console serial port - // - // The console port buffers are defined to be sufficiently large to support - // the console's use as a logging device, optionally as the GPS port when - // GPS_PROTOCOL_IMU is selected, and as the telemetry port. - // - // XXX This could be optimised to reduce the buffer sizes in the cases - // where they are not otherwise required. - // - Serial.begin(SERIAL0_BAUD, 128, 128); - - // GPS serial port. - // - // Not used if the IMU/X-Plane GPS is in use. - // - // XXX currently the EM406 (SiRF receiver) is nominally configured - // at 57600, however it's not been supported to date. We should - // probably standardise on 38400. - // - // XXX the 128 byte receive buffer may be too small for NMEA, depending - // on the message set configured. - // - #if HIL_MODE != HIL_MODE_DISABLED && HIL_PORT == 1 // TODO: figure out a better way to do this - // Steal gps port for hardware in the loop - Serial1.begin(115200, 128, 128); - #else - // standard gps running - Serial1.begin(38400, 128, 16); - #endif - - // Telemetry port. - // - // Not used if telemetry is going to the console. - // - // XXX for unidirectional protocols, we could (should) minimize - // the receive buffer, and the transmit buffer could also be - // shrunk for protocols that don't send large messages. - // - Serial3.begin(SERIAL3_BAUD, 128, 128); - - Serial.printf_P(PSTR("\n\n" - "Init ArduPilotMega (unstable development version)\n\n" - "Firmware Version: %d freeRAM: %lu\n"), - FIRMWARE_VERSION, freeRAM()); - - // !!! Check firmware version before loading any - // data from EEPROM !!! - if (FIRMWARE_VERSION != get(PARAM_FIRMWARE_VER)) - { - Serial.printf_P(PSTR("\n\n" - "Firmware Mismatch! ROM Firmware Version: %d\n" - "\nA factory reset is being performed."),get(PARAM_FIRMWARE_VER)); - - // If firmware mismatches a factory reset is forced - param_reset_defaults(); - } - - - APM_RC.Init(); // APM Radio initialization - //read_EEPROM_startup(); // Read critical config information to start - -#if HIL_MODE != HIL_MODE_ATTITUDE - adc.Init(); // APM ADC library initialization - pitot.Init(); // APM Abs Pressure sensor initialization - compass.init(); // I2C initialization -#endif - DataFlash.Init(); // DataFlash log initialization - gps.init(); // GPS Initialization - - // init the GCS -#if GCS_PORT == 3 - gcs.init(&Serial3); -#else - gcs.init(&Serial); -#endif - -// init the HIL -#if HIL_MODE != HIL_MODE_DISABLED - - #if HIL_PORT == 3 - hil.init(&Serial3); - #elif HIL_PORT == 1 - hil.init(&Serial1); - #else - hil.init(&Serial); - #endif - -#endif - - APM_RC.OutputCh(CH_ROLL, radio_trim(CH_ROLL)); // Initialization of servo outputs - APM_RC.OutputCh(CH_PITCH, radio_trim(CH_PITCH)); - APM_RC.OutputCh(CH_THROTTLE, radio_trim(CH_THROTTLE)); - APM_RC.OutputCh(CH_RUDDER, radio_trim(CH_RUDDER)); - - pinMode(C_LED_PIN, OUTPUT); // GPS status LED - pinMode(A_LED_PIN, OUTPUT); // GPS status LED - pinMode(B_LED_PIN, OUTPUT); // GPS status LED - pinMode(SLIDE_SWITCH_PIN, INPUT); // To enter interactive mode - pinMode(PUSHBUTTON_PIN, INPUT); // unused - DDRL |= B00000100; // Set Port L, pin 2 to output for the relay - - // If the switch is in 'menu' mode, run the main menu. - // - // Since we can't be sure that the setup or test mode won't leave - // the system in an odd state, we don't let the user exit the top - // menu; they must reset in order to fly. - // - if (digitalRead(SLIDE_SWITCH_PIN) == 0) { - digitalWrite(A_LED_PIN,HIGH); // turn on setup-mode LED - Serial.printf_P(PSTR("\n" - "Entering interactive setup mode...\n" - "\n" - "If using the Arduino Serial Monitor, ensure Line Ending is set to Carriage Return.\n" - "Type 'help' to list commands, 'exit' to leave a submenu.\n" - "Visit the 'setup' menu for first-time configuration.\n")); - for (;;) { - Serial.printf_P(PSTR("\n" - "Move the slide switch and reset to FLY.\n" - "\n")); - main_menu.run(); - } - } - - - if(get(PARAM_LOG_BITMASK) > 0){ - // Here we will check on the length of the last log - // We don't want to create a bunch of little logs due to powering on and off - // XXX: TODO implement for new struct - //last_log_num = eeprom_read_byte((uint8_t *) EE_LAST_LOG_NUM); - //last_log_start = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(last_log_num - 1) * 0x02)); - //last_log_end = eeprom_read_word((uint16_t *) EE_LAST_LOG_PAGE); - - if(last_log_num == 0) { - // The log space is empty. Start a write session on page 1 - DataFlash.StartWrite(1); - eeprom_write_byte((uint8_t *) EE_LAST_LOG_NUM, (1)); - eeprom_write_word((uint16_t *) EE_LOG_1_START, (1)); - - } else if (last_log_end <= last_log_start + 10) { - // The last log is small. We consider it junk. Overwrite it. - DataFlash.StartWrite(last_log_start); - - } else { - // The last log is valid. Start a new log - if(last_log_num >= 19) { - Serial.println("Number of log files exceeds max. Log 19 will be overwritten."); - last_log_num --; - } - DataFlash.StartWrite(last_log_end + 1); - eeprom_write_byte((uint8_t *) EE_LAST_LOG_NUM, (last_log_num + 1)); - eeprom_write_word((uint16_t *) (EE_LOG_1_START+(last_log_num)*0x02), (last_log_end + 1)); - } - } - - // read in the flight switches - update_servo_switches(); - - if(DEBUG_SUBSYSTEM > 0){ - debug_subsystem(); - - } else if (ENABLE_AIR_START == 1) { - // Perform an air start and get back to flying - gcs.send_text(SEVERITY_LOW," AIR START"); - - // Get necessary data from EEPROM - //---------------- - //read_EEPROM_airstart_critical(); -#if HIL_MODE != HIL_MODE_ATTITUDE - imu.load_gyro_eeprom(); - imu.load_accel_eeprom(); -#endif - - // This delay is important for the APM_RC library to work. - // We need some time for the comm between the 328 and 1280 to be established. - int old_pulse = 0; - while (millis()<=1000 && (abs(old_pulse - APM_RC.InputCh(get(PARAM_FLIGHT_MODE_CH))) > 5 || - APM_RC.InputCh(get(PARAM_FLIGHT_MODE_CH)) == 1000 || - APM_RC.InputCh(get(PARAM_FLIGHT_MODE_CH)) == 1200)) { - old_pulse = APM_RC.InputCh(get(PARAM_FLIGHT_MODE_CH)); - delay(25); - } - if (get(PARAM_LOG_BITMASK) & MASK_LOG_CMD) - Log_Write_Startup(TYPE_AIRSTART_MSG); - reload_commands(); // Get set to resume AUTO from where we left off - - }else { - startup_ground(); - if (get(PARAM_LOG_BITMASK) & MASK_LOG_CMD) - Log_Write_Startup(TYPE_GROUNDSTART_MSG); - } - - // set the correct flight mode - // --------------------------- - reset_control_switch(); -} - -//******************************************************************************** -//This function does all the calibrations, etc. that we need during a ground start -//******************************************************************************** -void startup_ground(void) -{ - gcs.send_text(SEVERITY_LOW," GROUND START"); - - #if(GROUND_START_DELAY > 0) - gcs.send_text(SEVERITY_LOW," With Delay"); - delay(GROUND_START_DELAY * 1000); - #endif - - // Output waypoints for confirmation - // -------------------------------- - for(int i = 1; i < get(PARAM_WP_TOTAL) + 1; i++) { - gcs.send_message(MSG_COMMAND_LIST, i); - } - - // Makes the servos wiggle - // step 1 = 1 wiggle - // ----------------------- - demo_servos(1); - - //IMU ground start - //------------------------ - // - startup_IMU_ground(); - - // read the radio to set trims - // --------------------------- - trim_radio(); - -#if HIL_MODE != HIL_MODE_ATTITUDE -# if AIRSPEED_SENSOR == ENABLED - // initialize airspeed sensor - // -------------------------- - zero_airspeed(); - gcs.send_text(SEVERITY_LOW," zero airspeed calibrated"); -# else - gcs.send_text(SEVERITY_LOW," NO airspeed"); -# endif -#endif - - // Save the settings for in-air restart - // ------------------------------------ - //save_EEPROM_groundstart(); - - // initialize commands - // ------------------- - init_commands(); - - // Makes the servos wiggle - 3 times signals ready to fly - // ----------------------- - demo_servos(3); - - gcs.send_text(SEVERITY_LOW,"\n\n Ready to FLY."); -} - -void set_mode(byte mode) -{ - if(control_mode == mode){ - // don't switch modes if we are already in the correct mode. - return; - } - if(get(PARAM_TRIM_AUTO) > 0 || control_mode == MANUAL) - trim_control_surfaces(); - - control_mode = mode; - crash_timer = 0; - - switch(control_mode) - { - case MANUAL: - break; - - case STABILIZE: - break; - - case FLY_BY_WIRE_A: - break; - - case FLY_BY_WIRE_B: - break; - - case AUTO: - update_auto(); - break; - - case RTL: - return_to_launch(); - break; - - case LOITER: - loiter_at_location(); - break; - - case TAKEOFF: - break; - - case LAND: - break; - - default: - return_to_launch(); - break; - } - - // output control mode to the ground station - gcs.send_message(MSG_MODE_CHANGE); - - if (get(PARAM_LOG_BITMASK) & MASK_LOG_MODE) - Log_Write_Mode(control_mode); -} - -void set_failsafe(boolean mode) -{ - // only act on changes - // ------------------- - if(failsafe != mode){ - - // store the value so we don't trip the gate twice - // ----------------------------------------------- - failsafe = mode; - - if (failsafe == false){ - // We're back in radio contact - // --------------------------- - - // re-read the switch so we can return to our preferred mode - reset_control_switch(); - - // Reset control integrators - // --------------------- - reset_I(); - - }else{ - // We've lost radio contact - // ------------------------ - // nothing to do right now - } - - // Let the user know what's up so they can override the behavior - // ------------------------------------------------------------- - failsafe_event(); - } -} - - -void startup_IMU_ground(void) -{ -#if HIL_MODE != HIL_MODE_ATTITUDE - uint16_t store = 0; - int flashcount = 0; - SendDebugln(" Warming up ADC..."); - delay(500); - - // Makes the servos wiggle twice - about to begin IMU calibration - HOLD LEVEL AND STILL!! - // ----------------------- - demo_servos(2); - SendDebugln(" Beginning IMU calibration; do not move plane"); - delay(1000); - - imu.init_accel(); - imu.init_gyro(); - -# if HIL_MODE == HIL_MODE_SENSORS - hil.update(); // look for inbound hil packets for initialization -# endif - - pitot.Read(); // Get initial data from absolute pressure sensor - abs_press_gnd = pitot.Press; - ground_temperature = pitot.Temp; - delay(20); - // *********** - - for(int i = 0; i < 200; i++){ // We take some readings... - -# if HIL_MODE == HIL_MODE_SENSORS - hil.update(); // look for inbound hil packets -# endif - - pitot.Read(); // Get initial data from absolute pressure sensor - abs_press_gnd = (abs_press_gnd * 9l + pitot.Press) / 10l; - ground_temperature = (ground_temperature * 9 + pitot.Temp) / 10; - - delay(20); - if(flashcount == 5) { - digitalWrite(C_LED_PIN, LOW); - digitalWrite(A_LED_PIN, HIGH); - digitalWrite(B_LED_PIN, LOW); - } - if(flashcount >= 10) { - flashcount = 0; - digitalWrite(C_LED_PIN, HIGH); - digitalWrite(A_LED_PIN, LOW); - digitalWrite(B_LED_PIN, HIGH); - } - flashcount++; - - } - SendDebugln(" Calibration complete."); -#endif // HIL_MODE_ATTITUDE - - digitalWrite(B_LED_PIN, HIGH); // Set LED B high to indicate IMU ready - digitalWrite(A_LED_PIN, LOW); - digitalWrite(C_LED_PIN, LOW); -} - - -void update_GPS_light(void) -{ - // GPS LED on if we have a fix or Blink GPS LED if we are receiving data - // --------------------------------------------------------------------- - switch (gps.status()) { - case(2): - digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix. - break; - - case(1): - if (gps.valid_read == true){ - GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock - if (GPS_light){ - digitalWrite(C_LED_PIN, LOW); - } else { - digitalWrite(C_LED_PIN, HIGH); - } - gps.valid_read = false; - } - break; - - default: - digitalWrite(C_LED_PIN, LOW); - break; - } -} - - - -void resetPerfData(void) { - mainLoop_count = 0; - G_Dt_max = 0; - gyro_sat_count = 0; - adc_constraints = 0; - renorm_sqrt_count = 0; - renorm_blowup_count = 0; - gps_fix_count = 0; - perf_mon_timer = millis(); -} - - -/* - * This function gets the current value of the heap and stack pointers. - * The stack pointer starts at the top of RAM and grows downwards. The heap pointer - * starts just above the static variables etc. and grows upwards. SP should always - * be larger than HP or you'll be in big trouble! The smaller the gap, the more - * careful you need to be. Julian Gall 6 - Feb - 2009. - */ -unsigned long freeRAM() { - uint8_t * heapptr, * stackptr; - stackptr = (uint8_t *)malloc(4); // use stackptr temporarily - heapptr = stackptr; // save value of heap pointer - free(stackptr); // free up the memory again (sets stackptr to 0) - stackptr = (uint8_t *)(SP); // save value of stack pointer - return stackptr - heapptr; -} - diff --git a/Tools/ArduTracker/test.pde b/Tools/ArduTracker/test.pde deleted file mode 100644 index 8fb5b361d4..0000000000 --- a/Tools/ArduTracker/test.pde +++ /dev/null @@ -1,520 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -// These are function definitions so the Menu can be constructed before the functions -// are defined below. Order matters to the compiler. -static int8_t test_radio(uint8_t argc, const Menu::arg *argv); -static int8_t test_adc(uint8_t argc, const Menu::arg *argv); -static int8_t test_gps(uint8_t argc, const Menu::arg *argv); -static int8_t test_imu(uint8_t argc, const Menu::arg *argv); -static int8_t test_gyro(uint8_t argc, const Menu::arg *argv); -static int8_t test_battery(uint8_t argc, const Menu::arg *argv); -static int8_t test_relay(uint8_t argc, const Menu::arg *argv); -static int8_t test_wp(uint8_t argc, const Menu::arg *argv); -static int8_t test_airspeed(uint8_t argc, const Menu::arg *argv); -static int8_t test_pressure(uint8_t argc, const Menu::arg *argv); -static int8_t test_mag(uint8_t argc, const Menu::arg *argv); -static int8_t test_xbee(uint8_t argc, const Menu::arg *argv); -static int8_t test_eedump(uint8_t argc, const Menu::arg *argv); -static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv); -static int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv); -static int8_t test_dipswitches(uint8_t argc, const Menu::arg *argv); - -// Creates a constant array of structs representing menu options -// and stores them in Flash memory, not RAM. -// User enters the string in the console to call the functions on the right. -// See class Menu in AP_Common for implementation details -const struct Menu::command test_menu_commands[] PROGMEM = { - {"radio", test_radio}, - {"battery", test_battery}, - {"relay", test_relay}, - {"waypoints", test_wp}, - {"xbee", test_xbee}, - {"eedump", test_eedump}, - {"modeswitch", test_modeswitch}, - {"dipswitches", test_dipswitches}, - - // Tests below here are for hardware sensors only present - // when real sensors are attached or they are emulated -#if HIL_MODE == HIL_MODE_DISABLED - {"adc", test_adc}, - {"gps", test_gps}, - {"rawgps", test_rawgps}, - {"imu", test_imu}, - {"gyro", test_gyro}, - {"airspeed", test_airspeed}, - {"airpressure", test_pressure}, - {"compass", test_mag}, -#elif HIL_MODE == HIL_MODE_SENSORS - {"adc", test_adc}, - {"gps", test_gps}, - {"imu", test_imu}, - {"gyro", test_gyro}, - {"compass", test_mag}, -#elif HIL_MODE == HIL_MODE_ATTITUDE -#endif - -}; - -// A Macro to create the Menu -MENU(test_menu, "test", test_menu_commands); - -int8_t -test_mode(uint8_t argc, const Menu::arg *argv) -{ - Serial.printf_P(PSTR("Test Mode\n\n")); - test_menu.run(); -} - -void print_hit_enter() -{ - Serial.printf_P(PSTR("Hit Enter to exit.\n\n")); -} - -static int8_t -test_eedump(uint8_t argc, const Menu::arg *argv) -{ - int i, j; - - // hexdump the EEPROM - for (i = 0; i < EEPROM_MAX_ADDR; i += 16) { - Serial.printf_P(PSTR("%04x:"), i); - for (j = 0; j < 16; j++) - Serial.printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j))); - Serial.println(); - } - return(0); -} - -static int8_t -test_radio(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - #if THROTTLE_REVERSE == 1 - Serial.println("Throttle is reversed in config: "); - delay(1000); - #endif - - // read the radio to set trims - // --------------------------- - trim_radio(); - - while(1){ - delay(20); - // Filters radio input - adjust filters in the radio.pde file - // ---------------------------------------------------------- - read_radio(); - update_servo_switches(); - - servo_out[CH_ROLL] = reverse_roll * int(radio_in[CH_ROLL] - radio_trim(CH_ROLL)) * 9; - servo_out[CH_PITCH] = reverse_pitch * int(radio_in[CH_PITCH] - radio_trim(CH_PITCH)) * 9; - servo_out[CH_RUDDER] = reverse_rudder * int(radio_in[CH_RUDDER] - radio_trim(CH_RUDDER)) * 9; - - // write out the servo PWM values - // ------------------------------ - set_servos_4(); - - - for(int y = 4; y < 8; y++) { - radio_out[y] = constrain(radio_in[y], radio_min(y), radio_max(y)); - APM_RC.OutputCh(y, radio_out[y]); // send to Servos - } - Serial.printf_P(PSTR("IN: 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\t"), radio_in[CH_1], radio_in[CH_2], radio_in[CH_3], radio_in[CH_4], radio_in[CH_5], radio_in[CH_6], radio_in[CH_7], radio_in[CH_8]); - Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d\n"), (servo_out[CH_ROLL]/100), (servo_out[CH_PITCH]/100), servo_out[CH_THROTTLE], (servo_out[CH_RUDDER]/100)); - - if(Serial.available() > 0){ - return (0); - } - } -} - - -static int8_t -test_battery(uint8_t argc, const Menu::arg *argv) -{ -#if BATTERY_EVENT == 1 - for (int i = 0; i < 20; i++){ - delay(20); - read_battery(); - } - Serial.printf_P(PSTR("Volts: 1:")); - Serial.print(battery_voltage1, 4); - Serial.print(" 2:"); - Serial.print(battery_voltage2, 4); - Serial.print(" 3:"); - Serial.print(battery_voltage3, 4); - Serial.print(" 4:"); - Serial.println(battery_voltage4, 4); -#else - Serial.printf_P(PSTR("Not enabled\n")); - -#endif - return (0); -} - - -static int8_t -test_relay(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - while(1){ - Serial.println("Relay on"); - relay_on(); - delay(3000); - if(Serial.available() > 0){ - return (0); - } - - Serial.println("Relay off"); - relay_off(); - delay(3000); - if(Serial.available() > 0){ - return (0); - } - } -} - -void -test_wp_print(struct Location *cmd, byte index) -{ - Serial.print("command #: "); - Serial.print(index, DEC); - Serial.print(" id: "); - Serial.print(cmd->id, DEC); - Serial.print(" p1: "); - Serial.print(cmd->p1, DEC); - Serial.print(" p2: "); - Serial.print(cmd->alt, DEC); - Serial.print(" p3: "); - Serial.print(cmd->lat, DEC); - Serial.print(" p4: "); - Serial.println(cmd->lng, DEC); -} - -static int8_t -test_wp(uint8_t argc, const Menu::arg *argv) -{ - delay(1000); - ground_alt = 0; - //read_EEPROM_waypoint_info(); - - // save the alitude above home option - if(get(PARAM_CONFIG) & HOLD_ALT_ABOVE_HOME){ - Serial.printf_P(PSTR("Hold altitude of %ldm\n"), get(PARAM_ALT_HOLD_HOME)/100); - }else{ - Serial.printf_P(PSTR("Hold current altitude\n")); - } - - Serial.printf_P(PSTR("%d waypoints\n"), get(PARAM_WP_TOTAL)); - Serial.printf_P(PSTR("Hit radius: %d\n"), get(PARAM_WP_RADIUS)); - Serial.printf_P(PSTR("Loiter radius: %d\n\n"), get(PARAM_LOITER_RADIUS)); - - for(byte i = 0; i <= get(PARAM_WP_TOTAL); i++){ - struct Location temp = get_wp_with_index(i); - test_wp_print(&temp, i); - } - - return (0); -} - -static int8_t -test_xbee(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - Serial.printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n")); - while(1){ - delay(250); - // Timeout set high enough for X-CTU RSSI Calc over XBee @ 115200 - Serial3.printf_P(PSTR("0123456789:;<=>?@ABCDEFGHIJKLMNO\n")); - //Serial.print("X"); - // Default 32bit data from X-CTU Range Test - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_modeswitch(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - Serial.print("Control CH "); - Serial.println(FLIGHT_MODE_CHANNEL, DEC); - - while(1){ - delay(20); - byte switchPosition = readSwitch(); - if (oldSwitchPosition != switchPosition){ - Serial.printf_P(PSTR("Position %d\n"), switchPosition); - oldSwitchPosition = switchPosition; - } - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_dipswitches(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - while(1){ - delay(100); - update_servo_switches(); - if (mix_mode == 0) { - Serial.print("Mix:standard "); - Serial.print("\t reverse_roll: "); - Serial.print(reverse_roll, DEC); - Serial.print("\t reverse_pitch: "); - Serial.print(reverse_pitch, DEC); - Serial.print("\t reverse_rudder: "); - Serial.println(reverse_rudder, DEC); - } else { - Serial.print("Mix:elevons "); - Serial.print("\t reverse_elevons: "); - Serial.print(reverse_elevons, DEC); - Serial.print("\t reverse_ch1_elevon: "); - Serial.print(reverse_ch1_elevon, DEC); - Serial.print("\t reverse_ch2_elevon: "); - Serial.println(reverse_ch2_elevon, DEC); - } - if(Serial.available() > 0){ - return (0); - } - } -} - -//------------------------------------------------------------------------------------------- -// tests in this section are for real sensors or sensors that have been simulated - -#if HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS -static int8_t -test_adc(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - adc.Init(); - delay(1000); - Serial.printf_P(PSTR("ADC\n")); - delay(1000); - - while(1){ - for (int i=0;i<9;i++) Serial.printf_P(PSTR("%d\t"),adc.Ch(i)); - Serial.println(); - delay(100); - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_gps(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - while(1){ - delay(333); - - // Blink GPS LED if we don't have a fix - // ------------------------------------ - update_GPS_light(); - - gps.update(); - - if (gps.new_data){ - Serial.print("Lat:"); - Serial.print((float)gps.latitude/10000000, 10); - Serial.print(" Lon:"); - Serial.print((float)gps.longitude/10000000, 10); - Serial.printf_P(PSTR("alt %ldm, #sats: %d\n"), gps.altitude/100, gps.num_sats); - }else{ - Serial.print("."); - } - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_imu(uint8_t argc, const Menu::arg *argv) -{ - //Serial.printf_P(PSTR("Calibrating.")); - - imu.init_gyro(); - - print_hit_enter(); - delay(1000); - - while(1){ - delay(20); - if (millis() - fast_loopTimer > 19) { - deltaMiliSeconds = millis() - fast_loopTimer; - G_Dt = (float)deltaMiliSeconds / 1000.f; // used by DCM integrator - fast_loopTimer = millis(); - - // IMU - // --- - dcm.update_DCM(G_Dt); - - #if MAGNETOMETER == 1 - medium_loopCounter++; - if(medium_loopCounter == 5){ - compass.read(); // Read magnetometer - medium_loopCounter = 0; - } - #endif - - // We are using the IMU - // --------------------- - Serial.printf_P(PSTR("r: %d\tp: %d\t y: %d\n"), ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100)); - - } - if(Serial.available() > 0){ - return (0); - } - } -} - - -static int8_t -test_gyro(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - adc.Init(); - delay(1000); - Serial.printf_P(PSTR("Gyro | Accel\n")); - delay(1000); - - while(1){ - Vector3f gyros = imu.get_gyro(); - Vector3f accels = imu.get_accel(); - Serial.printf_P(PSTR("%d\t%d\t%d\t|\t%d\t%d\t%d\n"), (int)gyros.x, (int)gyros.y, (int)gyros.z, (int)accels.x, (int)accels.y, (int)accels.z); - delay(100); - - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_mag(uint8_t argc, const Menu::arg *argv) -{ -#if MAGNETOMETER == DISABLED - Serial.printf_P(PSTR("Compass disabled\n")); - return (0); -#else - print_hit_enter(); - - while(1){ - float heading; - - delay(250); - compass.read(); - heading = compass.calculate_heading(0,0); - Serial.printf_P(PSTR("Heading: (")); - Serial.print(ToDeg(heading)); - Serial.printf_P(PSTR(") XYZ: (")); - Serial.print(compass.mag_x); - Serial.print(comma); - Serial.print(compass.mag_y); - Serial.print(comma); - Serial.print(compass.mag_z); - Serial.println(")"); - } -#endif -} - -#endif // HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS - -//------------------------------------------------------------------------------------------- -// real sensors that have not been simulated yet go here - -#if HIL_MODE == HIL_MODE_DISABLED - -static int8_t -test_airspeed(uint8_t argc, const Menu::arg *argv) -{ -#if AIRSPEED_SENSOR == DISABLED - Serial.printf_P(PSTR("airspeed disabled\n")); - return (0); -#else - print_hit_enter(); - zero_airspeed(); - - while(1){ - delay(20); - read_airspeed(); - Serial.printf_P(PSTR("%dm/s\n"),airspeed/100); - - if(Serial.available() > 0){ - return (0); - } - } -#endif -} - -static int8_t -test_pressure(uint8_t argc, const Menu::arg *argv) -{ - uint32_t sum = 0; - Serial.printf_P(PSTR("Uncalibrated Abs Airpressure\n")); - Serial.printf_P(PSTR("Note - the altitude displayed is relative to the start of this test\n")); - print_hit_enter(); - Serial.printf_P(PSTR("\nCalibrating....\n")); - for (int i=1;i<301;i++) { - read_airpressure(); - if(i>200)sum += abs_press_filt; - delay(10); - } - abs_press_gnd = (double)sum/100.0; - - ground_alt = 0; - while(1){ - delay(100); - read_airpressure(); - //Serial.printf_P(PSTR("Alt: %dm, Raw: %d\n"), press_alt / 100, abs_press_filt); // Someone needs to fix the formatting here for long integers -Serial.print("Altitude: "); -Serial.print(press_alt/100.0,2); -Serial.print(" Raw pressure value: "); -Serial.println(abs_press_filt); - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_rawgps(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - while(1){ - if(Serial1.available() > 0) // Ok, let me see, the buffer is empty? - { - delay(60); // wait for it all - while(Serial1.available() > 0){ - byte incoming = Serial1.read(); - //Serial.print(incoming,DEC); - Serial.print(incoming, BYTE); // will output Hex values - //Serial.print(","); - } - Serial.println(" "); - } - if(Serial.available() > 0){ - return (0); - } - } -} -#endif // HIL_MODE == HIL_MODE_DISABLED diff --git a/Tools/ArduTracker/updateParams b/Tools/ArduTracker/updateParams deleted file mode 100755 index f6b0772fc7..0000000000 --- a/Tools/ArduTracker/updateParams +++ /dev/null @@ -1,5 +0,0 @@ -#!/bin/bash -echo Updating Parameters ... -awk -f paramgen.awk paramgen.in -echo Finished. -echo Did you increment firmware version?