From c6b58491bbd7390650723c65b4d4e9ec0922c8de Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Oct 2013 22:18:44 +0200 Subject: [PATCH 1/5] Work queue in RGB driver without work_cancel() --- src/drivers/rgbled/rgbled.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index fedff769b8..aeb7e2f787 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -248,6 +248,11 @@ RGBLED::led_trampoline(void *arg) void RGBLED::led() { + if (!_should_run) { + _running = false; + return; + } + switch (_mode) { case RGBLED_MODE_BLINK_SLOW: case RGBLED_MODE_BLINK_NORMAL: @@ -471,11 +476,6 @@ RGBLED::set_mode(rgbled_mode_t mode) work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); } - /* if it should stop, then cancel the workq */ - if (!should_run && _running) { - _running = false; - work_cancel(LPWORK, &_work); - } } } From fbe595a591734bffa95d28125b8e0bda117d7314 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Oct 2013 23:10:12 +0200 Subject: [PATCH 2/5] Fixed some stupid compile errors --- src/drivers/rgbled/rgbled.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index aeb7e2f787..ea87b37d94 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -103,6 +103,7 @@ private: bool _running; int _led_interval; + bool _should_run; int _counter; void set_color(rgbled_color_t ledcolor); @@ -136,6 +137,7 @@ RGBLED::RGBLED(int bus, int rgbled) : _brightness(1.0f), _running(false), _led_interval(0), + _should_run(false), _counter(0) { memset(&_work, 0, sizeof(_work)); @@ -414,10 +416,10 @@ RGBLED::set_mode(rgbled_mode_t mode) { if (mode != _mode) { _mode = mode; - bool should_run = false; switch (mode) { case RGBLED_MODE_OFF: + _should_run = false; send_led_enable(false); break; @@ -428,7 +430,7 @@ RGBLED::set_mode(rgbled_mode_t mode) break; case RGBLED_MODE_BLINK_SLOW: - should_run = true; + _should_run = true; _counter = 0; _led_interval = 2000; _brightness = 1.0f; @@ -436,7 +438,7 @@ RGBLED::set_mode(rgbled_mode_t mode) break; case RGBLED_MODE_BLINK_NORMAL: - should_run = true; + _should_run = true; _counter = 0; _led_interval = 500; _brightness = 1.0f; @@ -444,7 +446,7 @@ RGBLED::set_mode(rgbled_mode_t mode) break; case RGBLED_MODE_BLINK_FAST: - should_run = true; + _should_run = true; _counter = 0; _led_interval = 100; _brightness = 1.0f; @@ -452,14 +454,14 @@ RGBLED::set_mode(rgbled_mode_t mode) break; case RGBLED_MODE_BREATHE: - should_run = true; + _should_run = true; _counter = 0; _led_interval = 25; send_led_enable(true); break; case RGBLED_MODE_PATTERN: - should_run = true; + _should_run = true; _counter = 0; _brightness = 1.0f; send_led_enable(true); @@ -471,7 +473,7 @@ RGBLED::set_mode(rgbled_mode_t mode) } /* if it should run now, start the workq */ - if (should_run && !_running) { + if (_should_run && !_running) { _running = true; work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); } From 39336fd58533c85ef6bad93b80dd51e62b6eba3a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Oct 2013 10:46:28 +0200 Subject: [PATCH 3/5] Better defaults for RC SCALE --- ROMFS/px4fmu_common/init.d/1000_rc_fw.hil | 2 ++ ROMFS/px4fmu_common/init.d/100_mpx_easystar | 2 ++ ROMFS/px4fmu_common/init.d/101_hk_bixler | 2 ++ ROMFS/px4fmu_common/init.d/31_io_phantom | 2 ++ 4 files changed, 8 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil index 6e29bd6f80..5e4028bbb8 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil @@ -33,6 +33,8 @@ then param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 param set SYS_AUTOCONFIG 0 param save diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar index 1aa1ef494e..4f843e9aa1 100644 --- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar @@ -29,6 +29,8 @@ then param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 param set SYS_AUTOCONFIG 0 param save diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/101_hk_bixler index 4d8a24c4ae..cef86c34d0 100644 --- a/ROMFS/px4fmu_common/init.d/101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/101_hk_bixler @@ -29,6 +29,8 @@ then param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 param set SYS_AUTOCONFIG 0 param save diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 8fe94452f8..6528337454 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -29,6 +29,8 @@ then param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 param set FW_L1_PERIOD 17 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 param set SYS_AUTOCONFIG 0 param save From 8ed0796448e49ae34dbfdf31c056e402834c0b8f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Oct 2013 22:17:53 +0200 Subject: [PATCH 4/5] L1 control diagram illustrating corner case --- Documentation/l1_control.odg | Bin 0 -> 11753 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 Documentation/l1_control.odg diff --git a/Documentation/l1_control.odg b/Documentation/l1_control.odg new file mode 100644 index 0000000000000000000000000000000000000000..69910c677459aae4664a08b1bbeb6aaed48721ad GIT binary patch literal 11753 zcmeHtbzGEd)Bg&BfOJYIEgefY(jwhR!?H9i>@Hn`Gzf?wAkxw$-5^RRh)8#LhjjiH z&+*acJf7!w-uLf!{4CykUo+pCYwqiwz2=%xRY1N;2mqh~03lk6QmM%7tt*v4e+-GzI71zTzS|?@U}_JB3Bf@^zZxd=E4R>h zMSt9Jv$J-v6{WF)!ySbA6$47K~tWe(y1 z+kvmA4&wl_137;1UR(H2jk@Oj)g(tK7={4+?&p7s@vF?QSP%&0Uq)a1;}3UzH=T=< zlZWGv??0@D*<0KFQ;pZWY#@leBf?HIJMgbcO&zT5|0&Eh@AsMh3SbE}H%A!qpAQ9L zGdG7Nhq)=-l+DE&>`ME?@;~PB-)AfMm)V*_O_*Vs*fkLs5pV@D{&1`L98fVl)sxqv)uTxMoq zbAB#1PChQ6Ihc>lRFI#SO8~?sV8O}9Xa1+^pIZLV`mc8zcD?XnaBC3NcUM8ouCWmd z&DjAiO2f@Z!yzW}=W`VL!}CAB{R`py?LPt#8XDU7h=vGa{~}Dp6$}i&b|dUM2uW^P{*3dBL}#D(%V2 z-=Zn2_Pq-RZI1e5x>QPT2)o~)X4;GF7bp^Dcqh8gWDI|uvzq3z<+XO{82U}{AZ;^~ z1m&HUlE1n~h)8dLtzC(+>+6R*eE9O@j6o|jrgSFHp?Wv;N2b=#1+x;?*lsaHU+Xe=EnSlQ>i4@vV^ObbCOji}iS^Lp$>^*(LFcS)#Si z{D;Tu@fpXJPXc&5z1>iJ<8RMxbU#{JIVLQiA(j%^cl{juXyu_}-Fn@F!$46JK`(%zFlZx!Hbioe1S57SDI#^Eoasg#TaZ5 zMq9ILdPw4CL$&{*^KpAFjv8+`Y5Bt`{4MTiQQ{X`fwQ)fI#*D(k7gNBWCiA)4tKB^ z88F&EYKKnCp8BnkHf^+OmW;DorWS+|Ws!5yuo#}W?1lzJS?Y|Tl|t(rW=pZN)1tDNt@T75_onIAd+KnvUv|=C zc*9wHx^E^ZThEa1WFp~!@iAOibY{ZR8`>WV*Y@Ndi3I96&-YI-8wA~_Ug1DDen(pj zAtq{F2vDP3xYJ78KAL30#P>}9Gqz$ZJG1mgka7%+?Z_3`JIba6PJuieD-lt)XlreK zF5bu;%YV6(JUd~;bo3kzy>^Iytt#+}atFC8UXs#k)8pd~^y96fs$f2UUf^le86!Y~ zFbqSm*bhY01l5gvw|ii$+c9i21dI)x;pO3noIK0lQ|Wcp4wdSWW|j@xN?=>46U}lBo4hoYnA)ra=Ra>?R-mG7AfV} z=!z*$?WOA;U37*Cs^sw16LTY6wt93B8!fSaK$kq>O)dIix)`5uv`m}fO>mtQI%`OJ z@#`g!qalHudFAx|F=>JiHQ9meWgl6~3Bm1o?CezP5$L&AI!rzgh?k_%OxLN#(XA;< z#sYx?f+)`1C9c;GD4V**Z-@sr6|2-Cp=zK>x}<3@TAobY&Z$32#d4Us>V#jRsw&(< z!&QkJ!$3jo`Dg)vzmK7}5it}5ae!ZM`4Ay=({LO*{t?f+M@P}LJnvwv<})TLGUH3l z`pU|TrTH+21|XYqX2&Q13e0r^1%D42WgXf$w!0XxR3l_DaRf5{t zCM7h>MHnBTIOxzVRRq=XHJ;Z!R{T<&)OIhS(Uw2;wWWG(D)T^U3iH7B82jX&hl-c| zg(%u$*O_ZlLub6k8GfB;Zi7hY{b`G=$x4;}3TLf5nokYu6fi#w6!hHPJjgv2it41= z&h^Y(OCk4Un_GpFSfvF!e^q@-{(Ntf3jYD2CuWFIsQ%*n>G6H>>E^SxdhC~5vVuZ| zg|hN$b)QfZ%}Co`WCH3uV}_J6KIKutZ%Hwbe0odbl)WtRatTbH=IDX<#OPsouXAn0 zkw!u^Uw-(+k!!xcu;wixWG*#GhRMaVz5?a>&{aG7>^G-U_to?CZ_8r6O~0M|QkhDN zKaj<^qLNopC)Btt9f`B1+g}8qhX197Wh1c%5k1CSkj2=gD=%_Neiu?1wye&pWaq*& z6lUWoWn1Nl+yR_w{RuOY5=N_97B|YP^xKKdYuuV?bva*P=xXe*f}_LW7>$iH#hsew z#W|hK>IMx(`JrAigWK;{VQoEc)s4{%@9?@h=H++UEU_cWOhf8E-pUT2=q{!$seN5r z>&6p9Inenq!g{*fJIlJ7PmTK>BWnwmP{Ro$!RDnZ)-9j z5N@kd2!KyJ~wju{0Fhg|mP9?Nnkjv#=GD`@2numWP$(6CUQA|TMH zHdEhL+1L0c3^CrLhVMnwo|8JGF%v#5hIiKgEXkMV3j-K?2ZZ$5Sn`SQv#tp&Pc<OvTk>=Z)y(D$Oaf z-a9%L&!&*Be0^z$G8IN`tGSsfVwEH3or&WUz@;6P7lnCs&0>6#3FWO!vw`WY!HST4 z-fc4!v(^nroMm{9C~;5ndnJ)g&o&q*KwLp2RU~!MM}vaMd2z#Z2b;w(P@o2VuI`;t z(VPII8a?4~x_8#8Zm4Eyi(~QmcJq}raGj@5LU}AiP;0};?@dvg6fIRowO1x%bbs=eS&Qj^6o+AO+LFM6Gzx`bDfB2#5x(^y0oJ z6O<45OiN>)N+-Etg@)HHA|FtgNiD5{Pw6A#r5u~HnMrM)!l)uFC%wYsPDU>GG{pO{ z^d#PJ&)DhI=8T?W+^Oqku*dY#T#>;NUVZbRzsi9fyV$%-pi}u0J%99iU|t3$wAS9B z%LhZ!l_*?3LeuKip*G5KCh@QXF(!HJc;}{vCd7MFiZ*mo*eL;Hs!!{2k!6)Buy)dJ zf%mp%jUfdKjGrmL|AD(+9r#I;MyPCJ;N5;W<_2!35p^Ds0@JW!hdzy5Ijc#mPWL%o z|GFo7jIGndwqQZ+I?T>zMk<){z@rU>V$hc>V zPE08E)_phoj0QUlQGw9~ zPB!TMZRWkj=v9p7dnQAkbX&8AJth-r@=MHiiOdY+g{n3*I} z@hz*opo&iqd}UCBX@A^ObFs3ojv_r5xY_-zD>`GXgHXKjW0(w&NsNHufr=Hm zD7Bw8o@zWB$^KO9M>nv<=m4V=MbKF-;N(ljae`vs)Ja4GL+@- z#)->l57O?A_Vyo*d<_+JkgMKgm-06PeTi@qhE=E1(98$fjimd*xg5;x$v+jkvF(8- zqPN$Sil8^kdYFI%k36F54Vb!R zO~=+8Kedijk9_zPZsUPEt*>#%!pETB^uW^+Ot@F2$eqQ26662KtKfa6LWM!7VZ12x zs47MWa{&L_liGVNBMWK~N?5R@Qor`Q>v)CsVOWf6Lrh}!Lly}FWWYQr!SsOcLecWW z&tu%S;vZm#-VA1$5tCm&gnZ-u@Zc-pNlf0^`-y$kjE!|ec6myZdYMu4$J8IHFyK zrueLewq=&?AAE7{NRHC2Jx-s(CMN41avYl2&*t@pb#QQHar*Uj2)$K(hu356TvFk$ z3H6|QMOGx?r3<}qBl`rleuIoLd?y9HKnBM)NKm(V{0nQgpyElhlUEFtHMw2ikhn_}T zM+t`Yi!bwLE5?Qob8K6o;F3_%vQOcu2ENGK(7dTPt{0|wqHak<0tSzLhF-8N&Yx!MoFO#y%jUQ1x%oK1BY{m@&<=MWFT6( z6PJjz{~;)Uz$LAFYlTk!aKI}Px&M;AfbKCxo8KQ{_G4fj)u$>5?fMvmGG8Odk+&#s2ID(ibiQAMK z-6OcKK7*El%ZFkjv^SR|&|2|0ZPEPN z6GdxI!%7CmyprN;=$dmBw!OpSo0fgwG#raxXB7q6a#nW{2QPilA6dnZv*yKzx+W>8 z0KoO<4kD}2v~spLb1=2GgK@xrT(UblSca-9%VA-VVIWwrjE6#6zS`Xj((HH2a#;FfsyMIi1r% z_Xg7Jd&O?@t~!$vkduvr?;XAJRGO0Rl&beusMy1OiHsk&M5m^v-dlH_)=#$O>+eMw zhl`=|2G^d~9TW|5b?eu44YLnF6GJ_S1oMh6fZy-MXGYzdRD(=$4Rr5&*5e{zLD`hz zK5)qS<;B@$l)1*Kn?!l&6rn6h4TZO&R^-J>G(Sl~+4;#T$bgrnL1<)g@0-t#=04Eu z6OZbJNlkFPOB_yml({^`;7EL?krLhH*@@eg+tv2P`mhe{WX?Q&1@!ejR>%@34!t@v z0n?Sfj{0j>LT|gQ6!9S9Kut9KXZ#D4(t)F+Hhq;lS8XLSJnj1)tvR z{a#R}VSjq1qV1TKkQc`F{Kxo2iwBb>>*{NAgr*CkKmp0IdoOLm-{@^q)Q9zU_{bW& zxqO{}v|m~@Mf-+8!|I->aG2O-=}^0zfa6p>NxxpUyVV;3xJz$`R}tD$vf`)UUhGuK z{nCfYZ86;Tja~dXxZ^oGgZ1QnlM;8yW*S)Z??aDW3dEMk$!fPYi?GGS#S8pW_x6tH zqZ|djKl6K_77yuVPl#zH+vGApF&2L!GKeN=Tpz`C0k^x zfVfvck)r(Ma@7_=O_iOr107vMLcV;S=Z6is8kF|#1m?5T-l^gDv_+5$qd<0by>I74 z*E2g5Jc0d{Td;5e?PeA4eey@gFr0mxs__Vxi={AhIjq}Y4OmU#$J9VaD zXuHR_4;&ZG4puuI=~=uVgD%wH$?l@yL!A-W~3QxUrtTSEw#MqtAXI?rHE^p7{`|c(cX}tO}CAaQA zDvVFLO=;ZRq)nO5l<6~!V0EONM8=uK>Kn3@s|qSQw>(%nJQELvh}aKAu4Z1oUd{Nn za67(WxR&Quc37r+^|0SD5VK;j4k^Q2WHcm#t$r6KSlj{a-h)Jp7KuvsE zub$j?@+w!!cbI#g@#RtPWUcOL$o;GK&8_C6t2sX>!h+9~^%t}GM@dsBY@aBFVVW-w zOM56Cs#SK@x5P1wAPTE$_e~ z+mRD{Om9x+RfN9)7Z85ija!_g*KzMl60{*%$>ef+S2d37p53(%oNSPOAFLoYigl(d zNx^osF+a%k#$7of+w)NEvtd_)h@Qi^oTxpu??nE`5)wkWlB6Tk z7qIlCBVcEWg3C9t`;m=tUC~KLYOT_CiIH`_l!2?JescRjY5vT6!yVWxZc{$7~uH^Pn( zM`uSw!O{7DC>N!Uz;^#bx$An2GtA5s`d{QIerG2XVu>hZ{(mH|fAat8o@-10 zyT0E^UAyF`)b9#E48PH33IZYOx8KWM93W>X^v?rLWW?F$t?NoP;Cg#;O@IH8erJq@ z9RyJU{Nb|S8FRgnfx+Do)wu69DE;op`5L^YWhP8EzK7TqJ{u|4!u^dS8)l_9Tu@uS zY}869#J1giM97#wnWFQNI+$XQ*!^jPVswLCn0O46e%WF~y=R~BJG0h&`~opz@v_cW z{L>-}c2E^*8F#1o!2Wkh(KJiwY(xB09F#r}hNt+eo83S3$F`d%?$msdr&gn{j?g6F zWY5pSn{umUd`Y2Gp+_vhFsR z1rH11xp5Eat|Da@yLWbwiECnJQM8G0l)mva65-VIYS`ss>dM-N@FlHU@GY8c28Xx3 zAMP8Ptr=voZK=@4KMTO)eY_al{}lp<(d_D8#TKutt1Yw8$Vxq>7tWO3C1zl>0cdptL`q-%k^?z!2Fl72Zop6cRGzgG%l zn#9&DO

yd{$-KBM33nLbWKjb_j9GK5s16nwR3-p{lKyLhc z3rl)StFzi^Irdp38T?kvO%gcI{-Yx*tw1DS&i5?F(J{A2kzyhl3=`;Wm}p-Jl4oPm z+iMQf$a`qDBq!qR-$){v)F?aYTz`$)L10=MkM>GWCDDpL-ZP3Uz^?>8ws||RRG6`~ zPP9|qh$Qs!Eo3Tp#?Z7!0f~J2E>mA;1M**e5_1$g-(VqCU&i9h7u|ae;@fjb;vS^P zs)oc?za4Iqqq%>&-I+|jtxqi=eWSHPlk@7i_=H;vU4nFrIdOuM$xD#{AYpzfD~6bR zleaNF%GEQkZ>lRW5@bJcsEViAF`6`tD?7j^p=hww3V@)ojVwgq=N2S(L1GY5t-t8c{68F zT1kwB!~i82ZxhIIu|L@{2M9+E&Y(y6nySxK%)6AN7EQg$gl8wU@YG<;H{7l)Aj8V5 zlHi@kjTbB?h?#pe+BMS}k#%Rp$#cF4g`Jzs^I5UCY}S)xX1c%ywLII>P{DaeTSk(Qzks~LCmbq0U7_)rxz8U(@mAVinSbv2>{~r&qG5>&XBk`rL4qa;4`}!s zLf&4^wHAw7pKt}Ei&^BaM!fEFd@oYnBX4t$guYwJE2#8xsIMd}SeyD^lM-g;z`Gh-RFGS^;S!EI3CmciD z1>swFu@*s@CL)yz4$1cBsb#WejSe@b8`ohtM% zi&j*&8w|Ic58YqDx3c-Av)pJ}Xm-EkAO$nbioWh`g{_)O5hy8!#VVHu2}-o+)c92g z_2({@M~Va`=VB0*UkG8mRf#Rvjt*L_m@OpXQ^p-f& zTh|SqWweIox8v>jLFprkW4R+u+Ojm`C&fr<^9F8g+`egO8QIGEAc(-5ItN`<(-OA@ zoJE5#3h!OPsBV1Us8#2k8SaSMfguzlRZRA@UWmKXN{d@|p%rbgMyzc=6OUTUbJFeV z^vrlX0A@6|@Ve@gwcj1YL95}q&K6#^O3_)v$vy3z3qE6~rDUo4pxetjr!g~>;ro~r z89nbLQbVj{((ch=p>g1(kGA1rD5W$D1T2wAubj%zz@32s8LQ_qH2Z62Xx&D1VkI2G zJ+~KE*TboKeA7i5Yp5hSUgUh3F~c>y@Wrd7ts+Vi*L1sU|LWa@>Sm$VyX>&H^Z|G6 zm_xR5^VI7=_I_r;&g-Zf?_B5mP6ypA~< zq}LLja`~}Z)uz15Me@DvI64s2y+NZx!0X0CpSauU)QFlEuu6S!(pb8}w_Q%ELZ?}X zhA|@Pnru+$;lXxw(qVjg(bFs{Q_^NKi{UJ*Z_SU!&_s)ImQ2#$b($Ax?a#37q?T;w zU|2)Mjcdk2i4A=qlXK%!7iN+dR6{!H_IpNoPiXM;QI>M5|D;C!#UeaV8UFNmO$WaV zEACzW5=q2VwrW4DE#smPaJK?UtQx|DRf4J~=XY6>!#i-f1AUXXf9j_3X`{F$rSG%q zM6DH>tec_$+u}rj*jlVlEZ^5FTtVDgn`}|#l^o~?p2K}*(oMwgiAwa5#hK5`c$O5q z)c*c?R|Zd`CL|k4N8*NXDOc8S1)9~{o-PSXQ~Qgk?(H&Vo;Y?6!?d$$ zeS!haU!_xedLEe*sUK4Su`a{#YMQG>MxB5@B#Fpw8&K(G4!QjmOI1$=G#Bw6_z;!S zyj$sU*^tTS`~2k_!{PmoX=ZjPqU)YUy(5PEqmP5-WgRoNKJ@gkGNe^oeaWO2$aEBu zUAB)v&9qFJ=RJc%p@x7$N%VdK?XE zM&384UGujTDKcQp=lKI8)za%VO|f))WXg?e4nQ6HX|J=MD7s=ey@ zt+zjt#pR^7%V=3cae=kC8QC32q4|okZ{m{VdY5#gQ+=c6x^s~G$1dr2`yC?0en&}0 zQ-V!iS(d{d(e-5khJCLc7iqLOe4xT?JYu2Q@+-L>8w|gO_rTqp3wOob|tR9V!LTK>Yy=s>;Xz=|K`--d^9>H#3)py zIDuR%D!dYrZ;knti^!i>PtBTuVdI!Jfx~%^I^Pk|k5wugjI7>J=oTD5qiDulf~537 z)9azit)Y9?V+W%(!5|mo+z2Iqc4{1f32LA+7FG5s^GnyLMpEc&dqw^93e?;52Wl%6 z^V3NeQR^-Bk4PENMaYZe(O52Wdf#Vm@y`uYPI@C>J2fA$*nxio0GL9)cIpizLcrg* zXI^hlf6|X;&A&?hwFmR3gc{=Vx@YQoBl<(?$0Pi;9pJig>L>LfdO3e>qx!4luSb5@ z?E*iE7_s;LrFr0o;NP7H{uHz${`>R6zoPt<0syXau%Gnc?ti8n|B;dXyT^YD0|3`K z*-!HR4cM<4+CRmq{0+{p8QVX_+58R8uNmCm<6P%(KZ*X{e>w@#=l4rS_xDKG3DHk7 z{teR44DX+!#Qg^4XU6vj%8xaBt@}sXcAfM6q^{qf{7nY<-`3N=Pml(&y#AWP{>=aW z-Lzk0<#h)5lXMX%Kl8yK+J8*x*KlwhNq^Eq#=nWHstPEGBXs}(7x9k|@vbH_U4Q$3 DeGk#2 literal 0 HcmV?d00001 From 4532eca4ef6f6170765062ccd2ca7f29814e0b3a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Oct 2013 22:55:16 +0200 Subject: [PATCH 5/5] Covered corner case in L1 controller not adressed so far in existing concepts --- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 33 +++++++++++++++++------- 1 file changed, 23 insertions(+), 10 deletions(-) diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index c5c0c7a3c1..daf136d495 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -130,8 +130,12 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float alongTrackDist = vector_A_to_airplane * vector_AB; /* estimate airplane position WRT to B */ - math::Vector2f vector_B_to_airplane_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); - float bearing_wp_b = atan2f(-vector_B_to_airplane_unit.getY() , -vector_B_to_airplane_unit.getX()); + math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); + + /* calculate angle of airplane position vector relative to line) */ + + // XXX this could probably also be based solely on the dot product + float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB); /* extension from [2], fly directly to A */ if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) { @@ -148,21 +152,30 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c /* bearing from current position to L1 point */ _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); -// XXX this can be useful as last-resort guard, but is currently not needed -#if 0 - } else if (absf(bearing_wp_b) > math::radians(80.0f)) { - /* extension, fly back to waypoint */ + /* + * If the AB vector and the vector from B to airplane point in the same + * direction, we have missed the waypoint. At +- 90 degrees we are just passing it. + */ + } else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) { + /* + * Extension, fly back to waypoint. + * + * This corner case is possible if the system was following + * the AB line from waypoint A to waypoint B, then is + * switched to manual mode (or otherwise misses the waypoint) + * and behind the waypoint continues to follow the AB line. + */ /* calculate eta to fly to waypoint B */ /* velocity across / orthogonal to line */ - xtrack_vel = ground_speed_vector % (-vector_B_to_airplane_unit); + xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit); /* velocity along line */ - ltrack_vel = ground_speed_vector * (-vector_B_to_airplane_unit); + ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit); eta = atan2f(xtrack_vel, ltrack_vel); /* bearing from current position to L1 point */ - _nav_bearing = bearing_wp_b; -#endif + _nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX()); + } else { /* calculate eta to fly along the line between A and B */