From 261fb02bdc5c309cd1e3065154e6a10ed8ec5877 Mon Sep 17 00:00:00 2001 From: Steven Krukowski Date: Fri, 29 Jun 2018 07:15:27 -0700 Subject: [PATCH 1/2] Lateral changes: * added anti-windup to integrators * Retuned gains * Updated README.md --- Diagrams/sideslip_hold.PNG | Bin 0 -> 8682 bytes README.md | 258 +++++++++++++++++++++++++++++++++++-- fixed_wing_project.py | 6 +- plane_control.py | 67 ++++++---- 4 files changed, 297 insertions(+), 34 deletions(-) create mode 100644 Diagrams/sideslip_hold.PNG diff --git a/Diagrams/sideslip_hold.PNG b/Diagrams/sideslip_hold.PNG new file mode 100644 index 0000000000000000000000000000000000000000..d42955fc8b0bb750a44c8f5567bf84fd0208d814 GIT binary patch literal 8682 zcmd6t_dnZT`1iGn#Ga-0sy(9AioL0#N*7h3XzkTV8>0wf&)T81R8epkOdHF|ekfxBv%U zqe0Zbb+@{`0eHF)U~Q~NQ8~!F3cR`Grh8MDf}%Q^?jMW_cn|h9xgS75!Pt5JxX|NM z_Lzb~D9qGA*9PJ62SYW=@pC-p*~w3rAaN-Pm)8U;Wnna$7ES?|e$7fPA>|Z76&H7} z>}9wxT%0id5Iw!&quGNzjGpiAM=p2Mx4C_+ow3_&30N-JojfMB4CSbqgM@Tqbp@;L0$Zx(I zjmu_)sIL;hhk`IpJ-jZi;G!r=53OoKxUHiAJ3HQN`%~3+S`&79Bz|C`1bp?F6O5#l z5F=S?9ld4XQSJ{t{!>3C@;wDR)Zcb?>hUP_s3hdyb`QBnAZ(GJLm2Zv9fke*JWhI> zZc$ShcB=MYM~b_B@@<6c8_D}Jk{C+BSsP|~O(8sMX1in$gylRMZpvl-_rUOH9)1^0g)<@nYx9s#vle;({ z5iGS&7SrBWwf-&NCDIAOR8*RdwmO-nE0(8&WaM(<}+#`mHt;;|yYfVST!(J_MK;x!AurI(Y(v$`s_b-mCGEt=~t`COT-b z*3%!NJnDgEEl0oe;QK%C+NF!#Bmpzl-$K+bwJh97D}NMr@@#`eXiWf&Ow`TKviCR5 zb52t8TrJ6}>rUjEY!9b=Xg!*4NnlxN+W69El+6DJCy!z#97l;tz2bAr_v=m&6|4$5 zJzD+)%q7;&r{!ns@kY~x13_)mnAZtGgzh%)4}*OhC3o@CKr;mH#S79 zloF_%k`7|ARlsoS@T@mP$8wdu+l%y&cSc8^;1sB&6aw_INvG1o{6QbM;qRh zYP1FdKA4p8PA2Vqq!IeBG0s8c*JlqmH_Wq4zOaAWwIC?NCWe_I;-Gnf!O46F!QcHU}}AqUf#_Qf|sxDDM3qeN~O`PRc?WEf5Xv00mk-TX9S z(TF??_v2A?O}LTk@2x7ZC|#D;@y3|&{@O)Xtr-3nWk9%dr(nlH@+2$w5@$7>#e_(K zIL9?SaeGeMmLpvZCf{u=5VDvcxSZHbwY}dOa{Rl1qxduxJC1&h%@Queil`!pGLiCQ zl?_DNCLq|o7QdzrLQA$)WA(wv(66Q`_AUba8bN90Hmu;(%c7*~#rCkC-P3=&LpPN1 znU_QJ4{K1G+BUsb43XKpm|*J%{}U@qw9vy#PxZM?ZNN>;69$0y(wNEqRtXQ$Bw&HC zyt)j>fo!G@dy^onaG{!bREv>~H9^wv@r?^kt7w&_7xSvssQ+!tPM!@ro0j!=lFxu{ zG<4byn-H#&oXEOBr&5w6U}1jxKyozIi1)sCO`w!N3KMnvU-0;( z#!oN+tHo+3@J>Mp`F;?G2-yuLz-h*qu7i_vRXMjx2$wW`_2=3x0WIH%sK%$w5G}sJOk2RLnJLp;zDA zlOk_+XP01Cxq6ayQZ!r{E+*GL<m-`~3Nb-2OI_pd%A4tX3t`|1zJPuO z@4LMt7Esv(({F+>c?&gM(yh|uowI*%)08r-qDkW=9=;U1D+yc*xM%99f)DKhe@rwI z-_GscyZ_DBhv3FrB`*8el)&4YU9U7RM&{1uv>h2t z>aRrScYJy};)>8h&&oBCoc>JK2XR7R#(jV4CI>a|-rNC0k2?gOjo#WgwjNu(0X>)w zdZg0Ma(Z$wJ9{F(u)LUV37?IbMC>J0(eNYMTV~{SI^P%%Kc9UY^dI1KvLl z142<%gfV!i+PUY;;%)9?d-AY9c`onG`-Fj*pd zGbH84?9srWk^TjdJ+|`vPzV~#;#Z*Rs!``vKRxE%yloVO?BNRvoaY{7>8^a#7P?Wd zdN>z}K|5|i;_9d2dAlmD5yoxhYrr7=YqVRGqa*DX!HKygCtcO zZ%q71k>FK&l0w%r_v-H~b9E{FZ`utxzk|*bYUe=(1y$xid`#$9QbZ3T6ew5<{LJI! zTMUf1nPcvj(MTh^mOT7`fj^d>7Re{L$FVWmHmip zjxZpn%T0X?2o7rQ5KYB$idSNHo96d^7%TLu8n4{l`qjJym?E-s$Rl; z*xoI9o>UR01zWz6W+FAJ$^zShd$*mCr!)hO-$2?&ba-LQ8h6SKi!!j3xFss~)B}ax zvN`50p5v#I!$Dg{yU*A5iau0+>AlJnE6Mh5*8Rgr`)uM@k>tQ+7FyiGRT;dW?3b0| z>)@YMQJ78q;cHs-?sK3*XtGvIir++`kJ^rM1)$BAhuc5z*bT%&>rd9x^#1dKMWGnV zJj^ZAOk2DjFCznHWrI7tK=ATP+`e%~nUPd9o{GCl*$NMm0;`nDl36f~G%4~3e8=hIlX(i9AE{Co@gKqE@Qxx^;RAX&lSjsVa zB;sm1E18JrAd43ua8HYA^vzAuay(c?#YpBt5e^p}{)W@=XE)vD-gIGiKYB7=iKz`N za8A}#uEjYFv^X()5vR~wYN{ni}{-*GZ9IycbJ~~cODj9m=`?hROIP2A=SP+zk zdoO>Rd!@qOKuszTQMwuRpfwKUWZl699od%YCVY@3fkcWfXguOBFsyQD$zR@B7(a4# zK=9DXU|-TB7Vd;==cE7Qq+~yyW0Rz{k`&GcKEO3LM!>xHmb@8PK2(VwXTd(MmbriQL z0b;{6JaK4Wy7aIK{_%k=z4oR1UTOEx5AE0P6FLoIt0}<&#IAkn=(5a9QGC@dg~8p_ zQI}B3pw8i#*hXRpn|L7vrblWqX5OAVv(jYSSCKP(5QW(_GkH2%cFUWE%c3~Q43RXs zx2`8dEW@+FcM?Pg!2%cCv}+e&ozbsCcnb_r7&B_d%evx{?HLXCo<6XKK&TmcSMd4S z9117a#M_L){4jLs&Aczemu|beihZc4iIaxLFTBqbA#qiwiWq+h=ed4?TPz9d+^`a; z>jMG7t~5VO(-wuOiMJpREMz)ot+J`Ejm#lb<&u(_VfsV&^})2z+WYvdYQ3Kr(+_a=#c%?|ait=|!H+1#P^gPTU+9lDeewArbMtrCfn2o( z7V{Srk^6twtAa|NwJKGyFM*(Ze49yWxabWOmoXte{N?Sz7c z`z6xH`f8#uPt|91GmFdvx;X9z)}}c@An$L8h2Dwt*H_@WMrMkw0Z9m$f}nIU4T46k zX7o(a-sv}$CBL{UnU)=N53^4~^K}Obr21>hb?*^`G*TLe)+TSNOb2%BxNY@>LnzPs z+^&8e?n=J{b0huW^hJS!tX$1LYA;Y%<@DrSnQvg(amh3#M4!d$+rX9;(;YbSlYue4 zeU;4N!cqF^7H1uEL@`@*%U_5tOn(0t7OS`dmON}hY+Xc8U-O7FQ-CG!Xpu}=KmU#T zM+f`h-@ztJ)n?1yNedEOF8Z;^Mh@QMA;K;zW|XvxxcKD1HT{5Po6+&j`6xu3a#hA> zi_qhC@iX8q>{W0`VX#2`0@s+T0@*#qL_(~O`9ZP09An5aS$35H3F|@HKP`EcFUaxB z?y3yQ{5#lB@S+SUSJkhWjWKiDxCBVG+dHRGt`Z=bkyBOb7X28;L20R&-vfC~f01=* z?M0x@=W(^fSBKKetwpu`Ou{;bnX{W><~A^SPWs@!OxcIQT2F!E7p093;4J2a$zNIq zRWGZEz!D=3L<;f6#{O`>G88s|*SS04+jM*ks^yX>15hdNO|?9+`q4eZ-?fFaFv{-| z91W9iTuiIO(MmTk6h@4E#$rV(Mw@HKK?-`!Oyv?>gjUVKidVUSZm7_mj`XJu z_82WoWpmD~t8I@73HUmA+cwhJvPGrDdsHESr(?%!P;EI-cs@NC27A|^hPj}iD%zK= zbuA=0-PV{evBQzdsN&ul*|Xbrb~3~_q`qFU(8>hMZWwQSJ5u#|?16IN$}qJG+?Af6 zlVKrp{N{V<84b#7jPD9wlplK?4sicEl#S@YQ$O*1^m6+ueV4gR685%ihtGN*Cp>Mq zqbIUElVR)p;5L$BT4pS8WJa+^+Dz;lKRAtgi-ZI3*OYqjvlXz0G5XaxK_n%Di1!RTFkyz|TN zYO(XFuXU9q8M=@z-{~o-!Ux3ffmUn!`68N5QpSiBwPr1|26GDgd7rI#mQE7BKYU$~~L@|da&-fG?L*&Jw;y-LCl zt_pzsKXZe}1VP8w`eYXVI<-b0Xbx%YE{0TEy*XQi%J=u1c})tzEGtoE7a;@52)B9eOb*+Cr_f}8WQSq%^zJ5yI8Dg~UJsF#gq-0O8c^bZ+ zD61o0kj(=E%R@Z1Pp!3d)q9DK1@Wr1z)8nQ2G@LT?lwg|lZNDygAx=bN2->axbV|` z-g?xUAG+ZECQ-R}I^Jm5*-ZqRFU>1`=U&z?I};p#xU$!;ey+DA?RKRznrjzjo858B z_{CNH5r`H_c|l?MWYrNBBoQEzK7?f979309?6kpjgdZd|u)SHhZmu5>hWhOT#mCTq z1R{iMj1h7*;dYe%L$&8G{pXsAube;ageb#kDlZwJ!EqIL;=^b_7&0@5S<(;S5SZGXPC+_AHI7{5iiGL=3Z29jBrJ=aI z7>SFDsjveX;~Y0yl`eq3C?w%jx|HT)7YjidmF31>G*Va#k}zzYHr9N_3U9VyO)43? zE2;|&4@*tgLu)I3RfyF8M9TCkknM{a_V@U~NCR|Y?JJ$4aysJLD3hYg&0OD|Ph7m> zJujabHv@-Ni9q!P3R6yeTQ-(Pcld-OB*OM0qCW$|x+zaRVx&e&w1K?~Q$$|9wAz`>pMc={3#lrZ zO8sH%+R3k#-YW1}u!t4sz{pDGV(t}ey@`vlhB{54Gzl>%!d*#)r$D~PB%#ZWubHT; z*Iv-0eEe^F3aq`QSxgHxx{yrkqbBnfv7?D32Y&E+&Y%d^iX19gOfq92?wZD_r-;lQ zLk@Rzt#h!TSeHS*8Ks`Cgm`P^P-&<25?3?RTVR;mWz&e6Wlh0m@V&IRx`d{n&5bcL z2;*wrX}Pu1-}h}dpRq&?s1`MJY>F?`FY9bQ=R3LlJ=vNp0UM$KQB~h$O*cQj@rqC# z!w|`%K*Z4o=1O3?*lY=_ueo?G%Tg6ZvPBdvDAWpS2^bR8jO0my=G6CE7sS|ga`>nj zH-i@vRp8O`?k1vgh!TWrNwcIYx9GhqR=k|mPt15E#Vf>2>7X+boXfA}sd6w;m|(Vr zRNxTxts-elBqZHPt)gu}wH@lVpx!OKkp5XD^zzrk?sT{OIf>^q#f0ovShq{1EssCm z(B;iQC<*J+bmVp21|j1SlZfJZNl`qw%K*rg0iFmlUgLeu>%l8NSsm@p^ttZ1m&Jak zPecP1D)JT2?rGjOBW#OFK8pWU#;uFSSy@`1m0#MIBY9RBn&(D^phT_&afR}-aogIT zDxXBR_7nB>GWV{!&Y&cZWN2A@$G+ZDus9T;*V+2T1QR7)3D5Psc2puI#cWA7Eus{f z%1|I`jx@eZG5|&929+D_$bPJ>!zyaJxc54~Hc0iQ<-9(xmH7C{b(nkqk+bi2E`$uX z$hW?b=252^n~(2p5?XRco3M(}pbqAX{q^(q1m+))AQzhpx*qhKgxX59f+{h@-Fy+^J0%TqRP==vfe&s~4{;9sw@`fl`sD=hy}R>| zHqUGCjV4qCGX4~Bnp$d&}#`0>c%DyJj#tW#IY6F7TD$AHO-UK?z*~cY*nuZ(|JbJ3B8G5Lr@k|< zx^wr3C0l>6yILd~K#3aVk|YL0T!DYYal~QwMnsfj`~|ZLi9)>waO2@qp#pKe4XU`m zJ9?9}=d@m)35*ODB`M5Dd?mf@Su=tMI2SV4@od@39ldtm2}Z;jb49lpGQ+u-?xJ6Q zy$l$fg6%QJ&FI_B8Qou3+oQnBb$%iHc9+O6##ZE>PyG)3f zgZWo6Sp{bn;7-p?2n!-GA3=bR7F`MTObuzq20Z=pNf}@=K;Q|*S|wy_>JS=P@BYd| zwLny)t5%0G_RTvqL^<1ddDRIvkFVGk~7Y= z8Uw(1)^n|<43JnNP!B;Y9AKmw@Kd2;VVETtAg%^Bq9jROoE!=r_y2b%FR_~~2Q$hp zz3h4F&&pf&e%v7UN>>p+RJFQSwUBdLPXDfxyLo&F0MU0jS6y0m2ccv@3;G4XS2uy+ zt?-b;`55wdV}S`kL-8yO-iZw%Pq>i*)n}6PQ84U3O$s0~WB_>*>SseeW=h3H!3rxqmL)!lt;idlip2#JSV;CKYP_pbl1(i@bDNZP4xQw z^G@ATrrj@q0>korS{uE-td`b&6U!q2#*sp|0GVj`LA|%vdR1HN$NHHU8!h7xgC5!; z|9tlP_<5`*Q`YW5{AJmS=exhhK0Uo{2UOo)iPy)lVmEW|0b<#C@3rUUJNI?>$LA;H zkIg&1b;LO7U$oK{bElP`ewb}26f8e$4h5zn=&TR97251m>pC#ywsk*PuCQ%TeZ9eX zK3i2QDIk(T*|(>^dROyM>zub7dj}i0%Fi}=P1bGj`pT)w=NAG*rvoEBcxBlKDDmq6 zW$a#05}#MrLwNPWpy{9LK^vY;f9mqi87_P_Bxo{VNb{q|c;(nfJC#WQedgZ+^t2E! zfNj;D<5@=4ISS}jJLR9bNT#}t=_b2me$5KEp#u0b;G3$Q zMbD3xvfTXLeV@z(?+)bTbMxjN%!Y-Dy`zqy?x0h@uBrqGZ~NvJ42%PNt0Vt43_aN& z|F0pS`8LU6xWP>(yy|KgOj5G(kf0+3BEiLrIHkW#%@7%*i>D9Q?V58+Q6OZSLJql5 z^hWB^ij35fk*{?7=3HEHj;>aU4ObTX9l7@CLk%btbhxs&1zFKl#o;eC04G_PGjrgn zQlFnwj}<{JG_%uQJ0+8+(*PvyK;}3;#XocbgRgoX)>&1x(5h7ZxIH^Gdn788OL+48n zYA&h-N~4R;;osX4js_4hE{+5`BWmZ!?;VGZD2Nf))r1jXtND|_VMMnO#RA*zb76Ms*MWV78NuLM7LP1x8)%P5gEj$T=aB zk)fOW4PW3B$QswdYyHUW9xzBxL@zn>F8F;Ny0cSH0e!0Ezpl2V@N9EBgQM ft~of;83jwuw_yE7`|H5}Gbl_AEetC493%b@eelAp literal 0 HcmV?d00001 diff --git a/README.md b/README.md index 8d09ca4..36f7408 100644 --- a/README.md +++ b/README.md @@ -355,28 +355,270 @@ Tips: The lateral/directional scenarios are designed to incrementally implement control loops to command the aircrafts airspeed, pitch, and altitude using the elevator and the throttle. When running these scenarios from Python, a Unity based longitudinal controller will maintain altitude and airspeed. #### Scenario #6: Stabilized Roll Angle #### +![roll](Diagrams/roll_loop.png) +The objective of this scenario is to tune/design a controller to maintain a constant roll angle using PD control as shown above. The aircraft will start at 45 degree roll angle and the controller should return the aircraft to wings level (0 degree roll). In this scenario roll angle and roll rate will be used to calculate a desired aileron command. + +To complete this scenario: + +- The roll angle must be within +/- 5 degrees within 5s +- The roll angle must maintain within those bounds for 5 seconds + +This controller should be implemented in plane_control.py, by filling in the following functions: +~~~py + +"""Used to calculate the commanded aileron based on the roll error + + Args: + phi_cmd: commanded roll in radians + phi: roll angle in radians + roll_rate: in radians/sec + T_s: timestep in sec + + Returns: + aileron: in percent full aileron [-1,1] +""" +def roll_attitude_hold_loop(self, + phi_cmd, # commanded roll + phi, # actual roll + roll_rate, + T_s = 0.0): + aileron = 0 + # STUDENT CODE HERE + return aileron +~~~ + +Tips: + +- Increase the proportional until you start seeing small oscillations near wings level. Add a derivative term to smooth the dynamic response +- Due to the high drag of the wings, the derivative term may not be necessary to complete the objectives. The derivative term is required for a complete solution +- The aircraft should be near symmetric, but if you are seeing a small steady state error near wings level, increase your proportional gain to decrease this error. Do not add an integral term! -TBD #### Scenario #7: Coordinated Turn #### +![turn](Diagrams/sideslip_loop.png) -TBD +The objective of this scenario is to tune/design a controller to regulate the sideslip of the aircraft during a banked turn using PI control as shown above. The aircraft will be commanded to a 45 degree bank angle, which will cause the aircraft to sideslip. The coordinated turn assumptions used in the course hold assume that the turn is coordinated and the sideslip is near zero. The sideslip (approximated from the aircraft velocity and heading) will be used to calculate the rudder command. Ensure to implement anti-windup for the integrator. -#### Scenario #8: Constant Course #### +To complete this scenario: -TBD +- The sideslip must be within +/- 0.5 degrees within 25s +- The airspeed must maintain the airspeed within those bounds for 5 seconds +- The integrator uses a discrete integration technique. The timestep used in the Unity simulation is smaller than the python timestep, therefore you may experience integration issues when moving to the Python simulation. You may need to lower your dependence on the integral term when executing python control (smaller gain) or try a higher order integration method (trapezoidal instead of Euler). + +This controller should be implemented in plane_control.py, by filling in the following functions: + +~~~py + +"""Used to calculate the commanded rudder based on the sideslip + + Args: + beta: sideslip angle in radians + T_s: timestep in sec + + Returns: + rudder: in percent full rudder [-1,1] +""" +def sideslip_hold_loop(self, + beta, # sideslip angle + T_s): + rudder = 0 + # STUDENT CODE HERE + return rudder +~~~ + +Tips: + +- Increase the proportional gain to meet the objectives. Increase the integral gain to remove any remaining steady state error. +- You may not be able to drive the steady state error complete to zero, but the smaller it is, the easier the following scenarios will be. + +#### Scenario #8: Constant Course/Yaw Hold #### + +![course](Diagrams/sideslip_loop.png) + +The objective of this scenario is to tune/design a controller to maintain a constant course/yaw angle using PI control as shown above. The target yaw is zero degrees; the aircraft will start with a 45 degree yaw. The current vehicle heading will be used to calculate a roll command. Ensure to implement anti-windup for the integrator. The commanded roll rate should be saturated between at a 60 degrees maximum. The feed-forward roll should be included in this solution. The feed-forward for this scenario but his term is used in the orbit scenario. + +To complete this scenario: + +- The yaw must be within +/- 5 degrees of the target course (0 degrees) within 10s +- The airspeed must maintain the airspeed within those bounds for 5 seconds + +This controller should be implemented in plane_control.py, by filling in the following functions: + +~~~py + +"""Used to calculate the commanded roll angle from the course/yaw angle + Args: + yaw_cmd: commanded yaw in radians + yaw: roll angle in radians + roll_rate: in radians/sec + T_s: timestep in sec + roll_ff: feed-forward roll command (for orbit scenario) + + Returns: + roll_cmd: commanded roll in radians +""" +def yaw_hold_loop(self, + yaw_cmd, # desired heading + yaw, # actual heading + T_s, + roll_ff=0): + roll_cmd = 0 + # STUDENT CODE HERE + return roll_cmd + +~~~ + +Tips: + +- Ensure the course error is appropriately between -PI and PI (i.e. a heading of 350 degrees should have 10 degrees of error, not -350!) +- Increase the proportional gain to have the dynamic response desired (speedy with little/no overshoot). Add a small integral gain to remove any steady state error. +- Since there are no disturbances, an integral gain should not be required to complete this scenario but should be included in the final solution. The integral gain will also help complete the following scenarios. +- The integrator uses a discrete integration technique. The timestep used in the Unity simulation is smaller than the python timestep, therefore you may experience integration issues when moving to the Python simulation. You may need to lower your dependence on the integral term when executing python control (smaller gain) or try a higher order integration method (trapezoidal instead of Euler). #### Scenario #9: Straight Line Following #### -TBD +The objective of this scenario is to tune/design a controller to track the aircraft to the desired line. The line will be defined as an origin point and course. You'll first calculate the crosstrack error from the line and use that to generate a commanded heading (based on an arctan based trajectory towards the line). The infite course approach angle used in the simulation is perpendicular to the line (PI/2), but feel free to play around with different values in your python implementation. The aircraft will start 20 meters to offset from the line. +To complete this scenario: + +- The crosstrack error from the line must be within +/- 3 meters within 25s +- The crosstrack error must maintain within those bounds for 5s + +This controller should be implemented in plane_control.py, by filling in the following functions: + +~~~py + +"""Used to calculate the desired course angle based on cross-track error + from a desired line + + Args: + line_origin: point on the desired line in meters [N, E, D] + line_course: heading of the line in radians + local_position: vehicle position in meters [N, E, D] + + Returns: + course_cmd: course/yaw cmd for the vehicle in radians +""" +def straight_line_guidance(self, line_origin, line_course, + local_position): + course_cmd = 0 + # STUDENT CODE HERE + return course_cmd + +~~~ + +Tips: + +- Make sure to include the course of the line in your final course command. You will still be able to complete this scenario while forgetting it because the course command is 0 degrees, but it will hurt you in future scenarios. +- When using the Unity simulation, a blue arrow is provided on the compass heading showing your desired heading angle. If that heading moves faster than the aircraft can change its heading, your line following gain may be too high. If aircraft heading oscillates around that value, you may need to return to the previous scenario and adjust your proportional yaw gain. If the aircraft heading lags behind the blue arrow, you may need to adjust your integral yaw gain in the previous scenario. +- #### Scenario #10: Orbit Following #### +The objective of this scenario is to tune/design a controller to track the vehicle to a circular orbit. This controller requires two parts. The first generates a course command based on the current radius from the orbit origin and the aircraft heading. The second part calculates the feed-forward roll required for desired turning radius of the orbit (assuming a coordinate turn). Although this scenario is a clockwise orbit, the controller should be able to handle counter clockwise turns also. The aircraft will start with zero roll angle on the orbit. -TBD +To complete this scenario: + +- The radius from the orbit center (North = 0 meters, East = 500 meters) must be within +/- 5 meters of the target radius (500 meters) within 15s +- The radius from the orbit center must maintain within those bounds for 5 seconds + +This controller should be implemented in plane_control.py, by filling in the following functions: + +~~~py + +"""Used to calculate the desired course angle based on radius error from + a specified orbit center + + Args: + orbit_center: in meters [N, E, D] + orbit_radius: desired radius in meters + local_position: vehicle position in meters [N, E, D] + yaw: vehicle heading in radians + clockwise: specifies whether to fly clockwise (increasing yaw) + + Returns: + course_cmd: course/yaw cmd for the vehicle in radians +""" +def orbit_guidance(self, orbit_center, orbit_radius, local_position, yaw, + clockwise = True): + course_cmd = 0 + # STUDENT CODE HERE + return course_cmd + +"""Used to calculate the feedforward roll angle for a constant radius + coordinated turn + + Args: + speed: the aircraft speed during the turn in meters/sec + radius: turning radius in meters + cw: true=clockwise turn, false = counter-clockwise turn + + Returns: + roll_ff: feed-forward roll in radians +""" +def coordinated_turn_ff(self, speed, radius, cw): + roll_ff = 0 + # STUDENT CODE HERE + return roll_ff + +~~~ + +Tips: + +- Increase the gain to ensure the course command guides the vehicle back to the orbit within the threshold +- You will not be able to complete this scenario without including the feed-foward term in your course controller +- When using the Unity simulation, a blue arrow is provided on the compass heading showing your desired heading angle. If that heading moves faster than the aircraft can change its heading, your line following gain may be too high. If aircraft heading oscillates around that value, you may need to return to the previous scenario and adjust your proportional yaw gain. If the aircraft heading lags behind the blue arrow, you may need to adjust your integral yaw gain in the previous scenario. ### Scenario #11: Lateral/Directional Challenge ### -TBD +The objective of this challenge is to successfully fly through a series of virtual gates in the sky. All the positions given in this scenario are relative to the vehicle start location. +To do this, tune/implement a lateral state machine using the vehicle position and heading to generate a course command and feed-forward roll. Your state machine should include function calls to the straight_line_guidance, orbit_guidance, and coordinated_turn_ff functions: + + The first leg (before Gate #1) should implement a line following controller between North = 0 meters, East = 20 meters and Gate #1. + The second leg (between Gate#1 and Gate #2) should implement an orbit following controller around the orbit center North = 500 meters, East = -380 meters (radius = 400 meters) + The third leg (between Gate #2 and Gate #3) should implement an orbit following controller around the orbit center North = 600 meters, East = -380 (radius = 300 meters) + The final leg (between Gate #3 and Gate #4) should implement a line following controller between the two gates. + +To complete the challenge, you must be within +/-5 meter lateral distance when arriving at the gate. + +The gate locations: + +- Gate #1: North = 500m, East = 20m +- Gate #2: North = 900m, East = -380m +- Gate #3: North = 600m, East = -680m +- Gate #4: North = 100m, East = -680m + +This controller should be implemented in plane_control.py, by filling in the following functions: + +~~~py + +"""Used to calculate the desired course angle and feed-forward roll + depending on which phase of lateral flight (orbit or line following) the + aicraft is in + + Args: + local_position: vehicle position in meters [N, E, D] + yaw: vehicle heading in radians + airspeed_cmd: in meters/sec + + Returns: + roll_ff: feed-forward roll in radians + yaw_cmd: commanded yaw/course in radians +""" +def path_manager(self, local_position, yaw, airspeed_cmd): + + roll_ff = 0 + yaw_cmd = 0 + # STUDENT CODE HERE + + return(roll_ff,yaw_cmd) + +~~~ + +Tips: + +- If you see a slow response when going from line following to orbit follow (or vice versa), reset your integrators to 0 when transitioning to a new phase of flight. +- If you having trouble with the last straight line leg, did you include the course command in your line following controller? +- If you having trouble with the orbit segments, did you implement the changes necessary to do a counter clockwise orbit? +- There are not any gains to tune for this scenario, so if the vehicle seems to be in the correct phase of flight, but you are still not hitting the gates, go back to previous scenarios to exceed the scenario requirements, not just meet minimum requirements. ### Scenario #12: Full 3D Challenge ### @@ -384,7 +626,7 @@ TBD ## Evaluation ## -The longitudinal, lateral/directional, and full 3D challenges will be evaluated for successful completion of the objectives. Success completion of the controller +The longitudinal, lateral/directional, and full 3D challenges will be evaluated for successful completion of the objectives. Success implementation includes the control loops as described in the scenarios plus gains tuned to complete the three challenges. ### Inconsistent Results ### diff --git a/fixed_wing_project.py b/fixed_wing_project.py index 4a79016..461e4d3 100644 --- a/fixed_wing_project.py +++ b/fixed_wing_project.py @@ -153,8 +153,8 @@ def attitude_callback(self): (self.scenario == Scenario.ORBIT) | (self.scenario == Scenario.LATERAL)): self.roll_cmd = self.lateral_autopilot.yaw_hold_loop( - self.yaw_cmd, self.attitude[2], dt) - self.roll_cmd = self.roll_cmd + self.roll_ff + self.yaw_cmd, self.attitude[2], dt, self.roll_ff) + if((self.scenario == Scenario.ROLL) | (self.scenario == Scenario.TURN) | @@ -273,4 +273,4 @@ def run_scenario(self,scenario): #conn = WebSocketConnection('ws://127.0.0.1:5760') drone = FixedWingProject(conn) time.sleep(2) - drone.run_scenario(Scenario.LONGITUDINAL) \ No newline at end of file + drone.run_scenario(Scenario.LATERAL) \ No newline at end of file diff --git a/plane_control.py b/plane_control.py index ca4a54e..88b5979 100644 --- a/plane_control.py +++ b/plane_control.py @@ -196,17 +196,17 @@ def __init__(self): self.integrator_yaw = 0.0 self.integrator_beta = 0.0 self.gate = 1 + self.max_roll = 60*np.pi/180.0 """Used to calculate the commanded aileron based on the roll error Args: - phi_c: commanded roll in radians + phi_cmd: commanded roll in radians phi: roll angle in radians roll_rate: in radians/sec T_s: timestep in sec - phi_ff: feed-forward roll angle in radians Returns: aileron: in percent full aileron [-1,1] @@ -215,8 +215,7 @@ def roll_attitude_hold_loop(self, phi_cmd, # commanded roll phi, # actual roll roll_rate, - T_s = 0.0, - phi_ff = 0.0): + T_s = 0.0): aileron = 0 # STUDENT CODE HERE @@ -242,15 +241,15 @@ def roll_attitude_hold_loop(self, def yaw_hold_loop(self, yaw_cmd, # desired heading yaw, # actual heading - T_s - ): + T_s, + roll_ff=0): roll_cmd = 0 # STUDENT CODE HERE # START SOLUTION - gain_p_yaw = 2.0 - gain_i_yaw = 0.01 + gain_p_yaw = 1.6 + gain_i_yaw = 0.02 yaw_error = yaw_cmd-yaw while(yaw_error < np.pi): yaw_error = yaw_error + 2*np.pi @@ -259,11 +258,21 @@ def yaw_hold_loop(self, yaw_error = yaw_error - 2*np.pi self.integrator_yaw = self.integrator_yaw + yaw_error*T_s - if(self.integrator_yaw > 100): - self.integrator_yaw = 100 - elif(self.integrator_yaw < -100): - self.integrator_yaw = -100 - roll_cmd = gain_p_yaw*yaw_error+gain_i_yaw*self.integrator_yaw + + roll_cmd_unsat = gain_p_yaw*yaw_error+roll_ff + if(np.abs(roll_cmd_unsat) > self.max_roll): + roll_cmd_unsat = np.sign(roll_cmd_unsat)*self.max_roll + roll_cmd_unsat = roll_cmd_unsat + gain_i_yaw*self.integrator_yaw + + if(np.abs(roll_cmd_unsat) > self.max_roll): + roll_cmd = np.sign(roll_cmd_unsat)*self.max_roll + else: + roll_cmd = roll_cmd_unsat + + if(gain_i_yaw != 0): + self.integrator_yaw = self.integrator_yaw + (T_s/gain_i_yaw)*(roll_cmd-roll_cmd_unsat) + + # END SOLUTION return roll_cmd @@ -284,10 +293,22 @@ def sideslip_hold_loop(self, # STUDENT CODE HERE # START SOLUTION - gain_p_beta = 1.0 - gain_i_beta = 1.0 + gain_p_beta = -1.0 + gain_i_beta = -1.0 self.integrator_beta = self.integrator_beta+(0.0-beta)*T_s - rudder = -1.0*(gain_p_beta*(0.0-beta)+gain_i_beta*self.integrator_beta) + rudder_unsat = gain_p_beta*(0.0-beta) + if(np.abs(rudder_unsat) > 1): + rudder_unsat = 1*np.sign(rudder_unsat) + + rudder_unsat = rudder_unsat + gain_i_beta*self.integrator_beta + + if(np.abs(rudder_unsat)>1): + rudder = 1*np.sign(rudder_unsat) + else: + rudder = rudder_unsat + + if(gain_i_beta != 0): + self.integrator_beta = self.integrator_beta + (T_s/gain_i_beta)*(rudder-rudder_unsat) #END SOLUTION return rudder @@ -335,7 +356,7 @@ def orbit_guidance(self, orbit_center, orbit_radius, local_position, yaw, # STUDENT CODE HERE # START SOLUTION - gain_orbit = 1.5 + gain_orbit = 6 radius = np.linalg.norm(orbit_center[0:2]-local_position[0:2]) course_cmd = np.pi / 2 + np.arctan( gain_orbit * (radius - orbit_radius) / orbit_radius); @@ -359,7 +380,7 @@ def orbit_guidance(self, orbit_center, orbit_radius, local_position, yaw, Args: speed: the aircraft speed during the turn in meters/sec - radius: turing radius in meters + radius: turning radius in meters cw: true=clockwise turn, false = counter-clockwise turn Returns: @@ -406,12 +427,12 @@ def path_manager(self, local_position, yaw, airspeed_cmd): self.integrator_yaw = 0.0 else: roll_ff = 0.0 - line_origin = np.array([0.0, 50.0, -450.0]) + line_origin = np.array([0.0, 20.0, -450.0]) line_course = 0.0 yaw_cmd = self.straight_line_guidance(line_origin, line_course, local_position) if(self.gate == 2): - if(local_position[1] < -350): + if(local_position[1] < -380): self.gate = self.gate+1 print('Gate 2 Complete') print('Yaw Int = ',self.integrator_yaw) @@ -419,7 +440,7 @@ def path_manager(self, local_position, yaw, airspeed_cmd): else: radius = 400 cw = False - orbit_center = np.array([500.0, -350.0, -450.0]) + orbit_center = np.array([500.0, -380.0, -450.0]) roll_ff = self.coordinated_turn_ff(airspeed_cmd, radius, cw) yaw_cmd = self.orbit_guidance(orbit_center, radius, local_position, yaw, cw) @@ -432,7 +453,7 @@ def path_manager(self, local_position, yaw, airspeed_cmd): else: radius = 300 cw = False - orbit_center = np.array([600.0, -350.0, -450.0]) + orbit_center = np.array([600.0, -380.0, -450.0]) roll_ff = self.coordinated_turn_ff(airspeed_cmd, radius, cw) yaw_cmd = self.orbit_guidance(orbit_center, radius, local_position, yaw, cw) @@ -443,7 +464,7 @@ def path_manager(self, local_position, yaw, airspeed_cmd): self.integrator_yaw = 0.0 else: roll_ff = 0.0 - line_origin = np.array([600.0, -650.0, -450.0]) + line_origin = np.array([600.0, -680.0, -450.0]) line_course = np.pi yaw_cmd = self.straight_line_guidance(line_origin, line_course, local_position) From 04951354c7f687561b9cbc4bb4426c462f41e4a0 Mon Sep 17 00:00:00 2001 From: Steven Krukowski Date: Fri, 29 Jun 2018 07:19:27 -0700 Subject: [PATCH 2/2] Fixed formatting issues --- README.md | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 36f7408..6888287 100644 --- a/README.md +++ b/README.md @@ -355,7 +355,9 @@ Tips: The lateral/directional scenarios are designed to incrementally implement control loops to command the aircrafts airspeed, pitch, and altitude using the elevator and the throttle. When running these scenarios from Python, a Unity based longitudinal controller will maintain altitude and airspeed. #### Scenario #6: Stabilized Roll Angle #### + ![roll](Diagrams/roll_loop.png) + The objective of this scenario is to tune/design a controller to maintain a constant roll angle using PD control as shown above. The aircraft will start at 45 degree roll angle and the controller should return the aircraft to wings level (0 degree roll). In this scenario roll angle and roll rate will be used to calculate a desired aileron command. To complete this scenario: @@ -378,10 +380,10 @@ This controller should be implemented in plane_control.py, by filling in the fol aileron: in percent full aileron [-1,1] """ def roll_attitude_hold_loop(self, - phi_cmd, # commanded roll - phi, # actual roll - roll_rate, - T_s = 0.0): + phi_cmd, # commanded roll + phi, # actual roll + roll_rate, + T_s = 0.0): aileron = 0 # STUDENT CODE HERE return aileron @@ -395,7 +397,8 @@ Tips: #### Scenario #7: Coordinated Turn #### -![turn](Diagrams/sideslip_loop.png) + +![turn](Diagrams/sideslip_hold.PNG) The objective of this scenario is to tune/design a controller to regulate the sideslip of the aircraft during a banked turn using PI control as shown above. The aircraft will be commanded to a 45 degree bank angle, which will cause the aircraft to sideslip. The coordinated turn assumptions used in the course hold assume that the turn is coordinated and the sideslip is near zero. The sideslip (approximated from the aircraft velocity and heading) will be used to calculate the rudder command. Ensure to implement anti-windup for the integrator. @@ -433,7 +436,7 @@ Tips: #### Scenario #8: Constant Course/Yaw Hold #### -![course](Diagrams/sideslip_loop.png) +![course](Diagrams/course_hold.png) The objective of this scenario is to tune/design a controller to maintain a constant course/yaw angle using PI control as shown above. The target yaw is zero degrees; the aircraft will start with a 45 degree yaw. The current vehicle heading will be used to calculate a roll command. Ensure to implement anti-windup for the integrator. The commanded roll rate should be saturated between at a 60 degrees maximum. The feed-forward roll should be included in this solution. The feed-forward for this scenario but his term is used in the orbit scenario.