From 003806fcc103a5323b358b42e25137d3b997aa3a Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Fri, 11 Jul 2014 09:36:26 +0530 Subject: [PATCH] HAL_Linux: pwmpru,HAL_Linux_RCOut code doesn't wait for magic flag to send data change pwmpru code to remove frequent wrong PWM outputs Replace pru firmware with new modified pru firmware --- Tools/Linux_HAL_Essentials/pwmpru/pwmpru1.c | 160 +++++++------------- Tools/Linux_HAL_Essentials/pwmpru1 | Bin 11924 -> 11848 bytes libraries/AP_HAL_Linux/RCOutput.cpp | 1 - 3 files changed, 54 insertions(+), 107 deletions(-) diff --git a/Tools/Linux_HAL_Essentials/pwmpru/pwmpru1.c b/Tools/Linux_HAL_Essentials/pwmpru/pwmpru1.c index 95dbe957d9..a28464cb5e 100644 --- a/Tools/Linux_HAL_Essentials/pwmpru/pwmpru1.c +++ b/Tools/Linux_HAL_Essentials/pwmpru/pwmpru1.c @@ -41,6 +41,7 @@ int main(int argc, char *argv[]) u32 delta, deltamin, tnext, hi, lo; u32 *nextp; const u32 *hilop; + u32 period; u32 enmask; /* enable mask */ u32 stmask; /* state mask */ static u32 next_hi_lo[MAX_PWMS][3]; @@ -79,60 +80,67 @@ int main(int argc, char *argv[]) continue; } nextp[0] = cnt; /* next */ - nextp[1] = hilop[0]; /* hi */ - nextp[2] = hilop[1]; /* lo */ + nextp[1] = 200000; /* hi */ + nextp[2] = 208000; /* lo */ + PWM_CMD->periodhi[i][0] = 408000; + PWM_CMD->periodhi[i][1] = 180000; } - + PWM_CMD->enmask = 0; clrmsk = enmask; setmsk = 0; /* guaranteed to be immediate */ deltamin = 0; next = cnt + deltamin; - PWM_CMD->magic = 0; - + PWM_CMD->magic = PWM_REPLY_MAGIC; + while(1) { - /* signalled interrupt from either PRU0 or host */ - if(PWM_CMD->magic == PWM_CMD_MAGIC) { - clrmsk = enmask; - setmsk = 0; - /* guaranteed to be immediate */ + //if(PWM_CMD->magic == PWM_CMD_MAGIC) + { + msk = PWM_CMD->enmask; + for(i=0, nextp = &next_hi_lo[0][0]; iperiodhi[i][1]; + period = PWM_CMD->periodhi[i][0]; + nextp[2] =period - nextp[1]; + + } + PWM_CMD->hilo_read[i][0] = nextp[0]; + PWM_CMD->hilo_read[i][1] = nextp[1]; + + + } + + // guaranteed to be immediate deltamin = 0; - - for (i = 0; i < MAX_PWMS; i++){ - cfg.hilo[i][0] = PWM_CMD->periodhi[i][1]; - cfg.hilo[i][1] = PWM_CMD->periodhi[i][0] - PWM_CMD->periodhi[i][1]; - PWM_CMD->hilo_read[i][0] = cfg.hilo[i][0]; - PWM_CMD->hilo_read[i][1] = cfg.hilo[i][1]; - } - - cfg.enmask = PWM_CMD->enmask; - PWM_CMD->enmask_read = cfg.enmask; - enmask = cfg.enmask; - stmask = 0; /* starting all low */ - - clrmsk = 0; - for (i = 0, msk = 1, nextp = next_hi_lo, hilop = &cfg.hilo[0][0]; - i < MAX_PWMS; - i++, msk <<= 1, nextp += 3, hilop += 2) { - if ((enmask & msk) == 0) { - nextp[1] = PRU_us(100); /* default */ - nextp[2] = PRU_us(100); - continue; - } - nextp[0] = cnt; /* next */ - nextp[1] = hilop[0]; /* hi */ - nextp[2] = hilop[1]; /* lo */ - } - clrmsk = enmask; - setmsk = 0; - /* guaranteed to be immediate */ - deltamin = 0; - - PWM_CMD->magic = PWM_REPLY_MAGIC; + + PWM_CMD->magic = PWM_REPLY_MAGIC; } - PWM_CMD->enmask_read = enmask; + PWM_CMD->enmask_read = enmask; /* if nothing is enabled just skip it all */ if (enmask == 0) continue; @@ -211,71 +219,11 @@ int main(int argc, char *argv[]) #if MAX_PWMS > 12 && (PWM_EN_MASK & BIT(12)) SINGLE_PWM(12); #endif -#if MAX_PWMS > 13 && (PWM_EN_MASK & BIT(13)) - SINGLE_PWM(13); -#endif -#if MAX_PWMS > 14 && (PWM_EN_MASK & BIT(14)) - SINGLE_PWM(14); -#endif -#if MAX_PWMS > 15 && (PWM_EN_MASK & BIT(15)) - SINGLE_PWM(15); -#endif -#if MAX_PWMS > 16 && (PWM_EN_MASK & BIT(16)) - SINGLE_PWM(16); -#endif -#if MAX_PWMS > 17 && (PWM_EN_MASK & BIT(17)) - SINGLE_PWM(17); -#endif -#if MAX_PWMS > 18 && (PWM_EN_MASK & BIT(18)) - SINGLE_PWM(18); -#endif -#if MAX_PWMS > 19 && (PWM_EN_MASK & BIT(19)) - SINGLE_PWM(19); -#endif -#if MAX_PWMS > 20 && (PWM_EN_MASK & BIT(20)) - SINGLE_PWM(20); -#endif -#if MAX_PWMS > 21 && (PWM_EN_MASK & BIT(21)) - SINGLE_PWM(21); -#endif -#if MAX_PWMS > 22 && (PWM_EN_MASK & BIT(22)) - SINGLE_PWM(22); -#endif -#if MAX_PWMS > 23 && (PWM_EN_MASK & BIT(23)) - SINGLE_PWM(23); -#endif -#if MAX_PWMS > 24 && (PWM_EN_MASK & BIT(24)) - SINGLE_PWM(24); -#endif -#if MAX_PWMS > 25 && (PWM_EN_MASK & BIT(25)) - SINGLE_PWM(25); -#endif -#if MAX_PWMS > 26 && (PWM_EN_MASK & BIT(26)) - SINGLE_PWM(26); -#endif -#if MAX_PWMS > 27 && (PWM_EN_MASK & BIT(27)) - SINGLE_PWM(27); -#endif -#if MAX_PWMS > 28 && (PWM_EN_MASK & BIT(28)) - SINGLE_PWM(28); -#endif -#if MAX_PWMS > 29 && (PWM_EN_MASK & BIT(29)) - SINGLE_PWM(29); -#endif -#if MAX_PWMS > 30 && (PWM_EN_MASK & BIT(30)) - SINGLE_PWM(30); -#endif -#if MAX_PWMS > 31 && (PWM_EN_MASK & BIT(31)) - SINGLE_PWM(31); -#endif + /* results in set bits where there are changes */ - delta = ~clrmsk | setmsk; - - if ((delta & 0xffff) != 0) - __R30 = (__R30 & (clrmsk & 0xffff)) | (setmsk & 0xffff); - if ((delta >> 16) != 0) - pru_other_and_or_reg(30, (clrmsk >> 16) | 0xffff0000, setmsk >> 16); + __R30 = (__R30 & (clrmsk & 0xfff)) | (setmsk & 0xfff); + /* loop while nothing changes */ do { cnt = read_PIEP_COUNT(); diff --git a/Tools/Linux_HAL_Essentials/pwmpru1 b/Tools/Linux_HAL_Essentials/pwmpru1 index 8c56222636f7285a500a8ef1413d2acd1ef2b3b1..da2cbd0e5b2f2c3487c8f5595cc336ca04971eeb 100644 GIT binary patch delta 3672 zcmZXX4^R}>9mjufZ}+%=$O=>qa2_Yyf(a0aCPGRYY)r;xuC-1WYcgg;tD~eeI7$-I zNoGmMw$6kgvM4xXCpKf6Os8XPp)(!pG_lggIB5%se@JWFU1M7&jmPbB%%Qc??^|!> zDLcd6@AG}X&)awJcK5XJ>C+o}7KMftqQ-I}vwYu@uj-wz%v4*YlGYg~E7^ERv#bD< zDO}qfm#uDOoKH%my+WpzRGG%JB)B`i%Ib_K(9>uqlTr-tydO28CTnjzV)J=F1(s#9bBF$!`_ib7ouVRDa&kVnkIhtPAWvtUfHOU%W@chq z#>uE($k5sg$op~POgckXouul&N-fExlYirelg}3@ImzvNAB^mCrfI>nGo|{b-EkG; zYVQi!<{zkk1=)_Cab*63-85ZKSpLCQ8a+)DSX0oT7_uMOEOFl!2~KMD70z7n}A^g=1%Y@OyHXoy$C!B-XeG< z@Jem^spm=XB-i`R?VgSkE`sQTs6niNSZULcC;A}zT=DJOMR*!K8D0&%cfGbb-fDO` z@S5PQhqu9|&$FIx=0MDG#SL@BCWrxu%@DUh+-}n~?}Pz(0oVK59IqK(5MC?19q`(1 z3b5=o5QG2ljeD0 zcwyI5;Yqk=ZeC}*tqJFbuBIkZ`4cmF+KtqN%5Rww+@C$TPkU?0zANRdOJyeHg{jyD zs$%ia&G4Vo>8rSs!%@cxNBK!UF+ zG^K1NA(Nu~uBbNI&8*&J2w9ZhVt1L8Z6stA<@fD-{K-XX_XF$ajbMcNHf?U(*-mE_qbgY9HxZ5V=3jbcPN+t@ ztQhs768{(m&6_j*FBI}HH@qUiM;OrwBN2Z1?#){_ZEM}SqouX2{obaww$@$uCR(0- zu4Cism5Ggy?%K8W+4h!A2zR{jU1M+fBWYMMz8!f;!o+Vx?w7`t!A~OfEWlUIkHN3! zeTf;3c`_OXAIqy{yyHc6qr|PCTc zQ8x+9=V}(X7wzmj!-G z2aF5BCe+sj-i|upo86H%aEZXrgG&W|8N6KJ-QY%nUj;V_toI^VF9Zj{tpXngcL@9z zxJ%%p;2wd01wJD13Ggw2Pl1PLS*J4yPR|B33ce`t7acp~o& zh!=oYf`%i9qej2Q1p9%an-U8MH{tmb<%TMx#od^;_!;9c%fjhzN z0{;lSN8oPoA%XXUj~NGxzswF9=Za(OEn}+qodw=iz`rV4vS&`B3lFB_CC@zePn6!l zhmv6ac;yFQ;5G0?f%*6Cjr%a355{x*ndljPadsoP%BQNhgCDqq9}!qZ|5}&%HHA$C zZvlHPoOg1_K2tdV0E~aEjLN>;!LvXK?TiE*!msf6e=Ew!7n}Z)1fn+|hrD zk7tj3eD;5XOSArbNoVvQ-oRIghvj^!fRdc~f?Qrm4j83-@CD8AaUA?@zNC;J8}0q1 z@DuP$$J=bqm0M*7(eccOAEbWr;BmMZ@pCP~ho;6;6=pNp9ZZ>GBRFwul(W}PV$};x8@mys&J7fG9 zubYkE<8{cmQdzG2EFW8I?5ij+Dyvi$G9IcbR}K~sU2(IQkp0kjt*ShCs1SP=BEo+R usoywRRg9DU4ZVFaTrI|EVS!Ont+Jy=Rdu;?3E5M|lhtLc!FaiP;r{{R*AOBA delta 3795 zcmZXX4{RG(9mjv~e0I*UlU&nm%}6}k%`v-e{(`%9TSwddQ;|AIA<_^nG;BmlQ6vhD zpiG5}r50LIHGeMcw9>H-ia^>>ks`KkFjmo^HKQnPX}5ujlm`ViknFoSrfErPzVCdO zK9ZAs_dehIecs)B_b)lWWAqC{^_5zP=x~atIX%4c5i^@DwN51zBby+e+jPR1n$nm` zk&Yt?l^;PS_^d+408JS=of#s_fsZ5}3k~jreFgIitu%+f{5e zUMR3|J|AVat+6YRM~RGVhWNG9Zx|u)VOW4Fy~R0H_`bCj6z(7 z5p;TKnx2~MV8$S|7&o0f4I&?6rPeezREA;qmHe20sd4hhOKIH0XE0&;fHh_y8RQ3bGi5 zs6ecT*a$J<&JqOWUI3l}FAcBRp_hFx054#=;%}FT21FHNJH!mc^$xx6 ziz-Cb^-eGG+TjJ^ZGg84-eV5E?R!CZLDxI6;FNBQd@l?y?0TI`ygqo9@b<#n2XDZkDwZ_MH%}$RN>_Y%iMW?nO-CT^hjfUoP2GeR5A)qK@S#aTfx4q7YpU=cCny4j&4Tk z9NKmXMZyg3x@p$D~3|lVRrF_kMlX%7l_ZkM?5035Fx@{L~hw!fO zYU84I_`&F~{0_LCx4ZH;9d#}rze=BA$;&)4SuFk+PfR3c+h!ytM)MZ0^ukJnl`%0j zukf&!g^`70Vw#`lnwM$Fw3xVL-{lovR)MS{Ca(V;9`dpfvQSLiOWi!^WkF=Yn7A>Y zG`Z@9Dnd0T?(zqDz{>*20x|K(q`2Z`3Nj@o9=|%yyo@1ZG4UvuIQ^kKIzmP<@qRdq znH?a-`0JD!JV(@Y?t851+#%LP2U&8gF7zS`iBCdf$l4bAk+of087(7Gh&{@zo!Y(D zPqj>S9cozMOHY6G>-|(8vOd%rLk6Pevi9?^?&G;|yO9>fL}B~_lp`XQ_3Yi%-}(%l z2Q*@@;d*)$73Ds?aH4V@dUow&mKNEl_V?`Rx4ss+tY{(Y@#w<}#Qo8=N@Hm3SJ4NU zCeBqXV{J9}vCzlRoe>&K*K{yZv0dM(2=CXm_X_VCHc01c&ok?X>58$5x{ZoCUiL{* zs^~Y+uSh%&zAo`QU@U(h}&@pZI+A@RQ%uFn}Mm`2-{_$JzO z5|ctykDq@66=*j~T#a_S#CM^+L1IzcDRGlx5_L(zYP5SL?m)X=;`L}Blz1cBhb4Xj z?UNEeiS{cJi(}!>iG=}dHv!{P@HE=*OS}v1>k{uqdsgCaqCF?^chJ@YcMfm>+$ga) z*>;Id3qeK-j)S*I{1UiJ;-7(gB|Z)AllWEefW&Wt4@)ctd-4vO^m_!S?gVrhd`99w zfyX7D0>3ZuKf%`|#$RV-OI!rcNi1edSN$0>t0;h-bqhfiRf5wJF9T;Jt_N?CI1b(_ z@qOTKiCe&Z5{ud1=d+2&=ph6HegJ+1d`jX^f-gwi37(bsDR6zzA5j&35!@_s4|tQr z;&OF)Y>MZ24}xB);92lKiL>A#i4THLNjwNXBk?it1&Ln(Pf08;_e`0soegW*%hu6` zID6IlWy9&a-@-nBQ5C0j5sX*ZvyESQVrJ2>abkn?9k6qpVxRk1yiSNUWp2e0^~zVG+Cs`~4EZSpnU5_K$);;#Rx$X6eVGK-|h$ zgzy@;gvIAK7xCMozFh9szxRI}q<<``cf@{gsm0j;15SAUGR0(cOB9GR#OrmLvYh4x zixYBr1u2)Y!T%L+)m6d2@exvu^C$O=kli6>`SyuDohS=i-Nc-KiJ*yRDv7 zoON3V-1Se~_3u)jMRHF%#Rja+-g<>KkY11U_4JEwy29GM>ac=u`R!HzLj2+CPkV9a z8c}v=jnDQPJve}a_E^E@CiZu0Rdb4cU~R*C!1@-}m#vquK5tE6owgjTU$gFMNwHJb z=9bi|D|I->2oe7Kr04Myi;mw}oIB+|WXDpstZe48ASx%yYJ}w@r diff --git a/libraries/AP_HAL_Linux/RCOutput.cpp b/libraries/AP_HAL_Linux/RCOutput.cpp index b3e43539d3..0a283da017 100644 --- a/libraries/AP_HAL_Linux/RCOutput.cpp +++ b/libraries/AP_HAL_Linux/RCOutput.cpp @@ -67,7 +67,6 @@ void LinuxRCOutput::disable_ch(uint8_t ch) void LinuxRCOutput::write(uint8_t ch, uint16_t period_us) { sharedMem_cmd->periodhi[chan_pru_map[ch]][1] = TICK_PER_US*period_us; - sharedMem_cmd->magic = PWM_CMD_MAGIC; } void LinuxRCOutput::write(uint8_t ch, uint16_t* period_us, uint8_t len)