From dd4a42e3b975246bb3add460cb184c47bfa2243e Mon Sep 17 00:00:00 2001 From: ywh <2227158009@qq.com> Date: Tue, 25 Aug 2026 21:29:26 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E4=BA=86=E6=98=BE=E7=A4=BA?= =?UTF-8?q?=E9=A2=91=E8=B0=B1=E7=9A=84=E8=84=9A=E6=9C=AC=20=E5=92=8Cs?= =?UTF-8?q?=E6=9B=B2=E7=BA=BF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 5 + .../bin_to_time_freq.cpython-314.pyc | Bin 22489 -> 29867 bytes HostComputer/bin_to_time_freq.py | 125 ++++++++++- HostComputer/bin_to_time_freq1.py | 209 +++++++++++++++--- PLSR/Src/plsr_planner.c | 171 +++++++++++--- 5 files changed, 442 insertions(+), 68 deletions(-) diff --git a/.gitignore b/.gitignore index 3e1eceb..fb93ce1 100644 --- a/.gitignore +++ b/.gitignore @@ -30,3 +30,8 @@ tmp/ .codex-tmp/ Document/PLSR_document/波形/ Document/PLSR_document/上位机图片/ +.codex_docx_qa_final5/ +.codex_docx_qa_final/ +.codex_docx_qa_final2/ +.codex_docx_qa_final4/ +.codex_docx_qa_final3/ diff --git a/HostComputer/__pycache__/bin_to_time_freq.cpython-314.pyc b/HostComputer/__pycache__/bin_to_time_freq.cpython-314.pyc index 5189aeeeb822d84d8f80a09405c67b85c20ee499..4f9b7d82423c9fc291bed64e304b779ae6b24d86 100644 GIT binary patch delta 9297 zcmbU{2~=CxmH$gZ`dSd1gv2fhFbD*g-8;rM7`)<-^Xa4%Dv($bf#fe?W0H>K#EHQP z0sF_X;w8pgO*R{cCc#_mq&=OSww^+OSpD^xHm3;8v{T!OyQI@Oo%_BJ5{}(=<{fl( z-@W(Uci(;6eOLVA0{-?aF3*&QN-!wb5BEOxqiqM|YJB2Anq_4uDND`QR+$>(MTBnmkNL4@4G z;TeGrcY=JNc>BKZz3Q48ae1!}c#nQ`_v(?^QwL^V+2_4*Y5MR7vxk2$b7U`YXWbuo zFI=5IG^8;#<(M3IuXfK|K6UqMj|6E#h6Xfh{((xTPv_;AgLHoK9@6^Bd!Tpr%7Gxg z!NUPFe&gCR71R)H^S_+a9S$>{ z!#ow25(29fTQ$tY{VO311(cREs+24|k||5c6NV!~81uc97CefXPSN8LOn7RFQ-R1T z0D2)Su{(^8R=cCM(M}^(Y>16+ZFkU%kxU^X2;0o2R%5$fLKh)gF-LQ>&}MrJ9f2rI z5%@MD;|_Wo`WCdb*6*SzK<#KNE<%O#0$l^uaF(!}Elm!y-9a0nescJSsad#60I0u+ z0pVkq;42-$7>TX8fZ4V+QJ}>roY_ZYqN-FW#h2{Du;xs(Lz#<&%IT}eqIxY-z!geR zfcvl@T8N*9G?-*u3+Y?~jO6*bEqG9yc|CQ!C6SM4#m^2jqj+`Ll4Xw&npe_-GnL{* zTzjxu>kOC(tuipouP1HJqHh>DZ~mhVvg7W1hYe;;c+3|oqn&kFO=EB;1^y1CHN%D=)izm;J6Mk5F9EY{p*7bG zF!BCU)4K@Jnwtw~$2r=%IxLt<2}mi_naI^j0XJwchPfH86OfW@ocU#hhS?~S$nxix zge{EudAMet8%@p6MUYJR0vTP+?+qb-%L20-Bp{4fjMT^|wSURfVod0&L|`St0EsPC z>GRFzktKU!9mG*FRNR6Asu$J`^V=N5s8}ivtqDdIrg7G6_pg5{E`8w=0kL!p2~x4i zm|oU#3sAUmXOF)z^KK7Zy0b@Kox6RXyX4(-#Cy>-`~2X{eeR0yKA!ip zdC#1i-M4Rg;KSK{ecqQ}o8JGLcmLqst4Do3O&@c4hx*{Aox0?ne(e~91Cpt0*JoZG zoH=&Fd;SfdX+AU2ZF>n~gG+D^&7AA?DMBV+at?cs??V>R=os!mTh38$H#cg$2Vd6s zY}RR#KxSLKzH9@~tOy$wyS8nKqhQ&Fv0-JpBaM#^*(r@I@l^c2?5X+Foz7!fk7(o%Kit-_7FfthB{w zX{z^$aYN*W#`0TDoor~6(PC*ZHa=Z%F*i9_`FxdbZEkU}B8Syl-(=p!;+-tdjy_;z$Om3lMMUw8^=Xdtwd%J)>8nb3xzIK#Y z>k)e z;weJfUHXy`%!^R;Z#%|!mmaeZEIaBPSLcjJEOu@1M8>+Sorkhr8*fWP`%3ngxN`?G zk6VYzC#1_=>u$^BeOvc$b(an-Kkgi=nUJk?l|2w*AgSN4?=OAH;97fI5q)_5!S(L7 zL-^U8$@tu%+#%!p?c<6v*Jh7WH>u2VZSjODdY^T7jmh<{^&W+GQjy_Ww+Fb%lS+fD zY|oZ^P9Y|ibnoch-e20^aHy=e)?Ga*(@YW?FdICbxIfXI>{f!W#izuB+S9U8b=J5z z+qLHJGbOUV?ETs9e0M6$NX)62!Q6KX$F;emF?r*Xd@ww~cRo_U6B^lDdrPJrlWEWB z&Sjj;7?)+a$~-c8Z>5`eoB#{CT6YJky9@l3(C~g`?~`D#>9~F}G}%?|`6eY&W%dFPg$Ts9Gr?b?9m zsAzvtf9AL}5o}hUQop0Sknv{5xVG??cIg=WNte3Tc~qJK#UbZl+|c$({jy2bGS`M~ z$CyBITOE5sdPF)b8(hac88>P(sMGDOpIHrtz zqsFK;>Ws<86l1D!kulAfZqyqyjG0CQlbV*Km0FW+GHY5LM&gxt1pR7i&TN&2eX zG%_mqKCMn86%W;w3+tdYo|hlOjOn5U&|4DH6dLWn$dfRzbB|sbrV6IO-H)$~V*aXE zBtWECry$k-kuH#0v_LBCF;aTeqcVn!CF96=f1rx`t@3jc$i#;@narN_SZ5NSVu3@D6wfLJrY(;-8e~bMgcw@@sE~s5%~_h`D+uR)(8? zjY!qWRXKD01%1_`HyKx|@|YcpSf?$5k~Be}9FJ`m+a$HFY2ho7e;DO zZ0P4irhOMpH4{49fKxWTt=h0WG=?H=uX* zT!GF5?koRiI*;NyH;`G6=zRXk=hUi!vf!d2Ou}aYFkXTuBwE2H$xS(I55AprT!LVE+s^#NG%m^ zhRg#@vzp8cURP91aLpD=g;$mM8wb9r*t+M)$8`|P9aB_j)mk);t-7Ga5N?K7K6DDa zfHFTug$E=_JQ*YNNt`TzPa%AYNIXYi)%y-)XEv6Nk;QdlSbs~%rF9{&)|VB}r6+d5 z4zi}e$yDZ79Qdw7m%#Em32CTZ@28S0$d#Zcg{h_zioY@f--wRnhzcq$ zuu2dY9p`hrN-F-bybV<1V|g2?gvarWl~huI7qqT61?Db@x*4vv|HoCwlD1J)1;cgE(tPu0 zsQj*k#j{vGZ8mjcI5T_uvifdon_1 z*w>*iX{<@j&TocAdLi8lUo5|^ zsS$vg1H0&cL=)|7wYN4{%q-RwO2bRZWUv`cD;HvFm18s;xW2HsYD zIK5bc;|)yylD|;MMEWEGrw}-ez!?BwHvMB%`4oYFL*Sm%Y$PH^Y9RQ5u4NpXpf>U&$FB z?K3>!xFE;E%bRD~O^+bOJR4MOyIFw=(&jAwv~OzBc*qo%p&v#0v{o89e_O= zK)~xh$Aj;E?q9RkfLn*HYd_QwYJiRC|U-ajQg);bD?w-EE z?5K!_Mc7?oz)v#oS6IQk)XIyQH6ZcNo>~7{xgrY&f_6OpBmyb`KtNv{0dS6vfIf}> z7L&6f4}Xzq*pQmE&}LR_hwb8Mb)d~>wbMn+_cwgo=}YFIL=2bj;nF-@vSDtrE#_u( zyNP}u1mI@}n*|PnPRNtkjXTX~OVlEINTbDSholC_f@y7ZaQT z1!^=(0MXwC!18St2mK<#LL8k)Pz08uTng;sq5{yCTi?`Su>g`+@8mFv&0=+cr6{{Y z7(2~&pEPaV#R`p`Ttz@ztqzut22X%0&O-VY>hqTf+(qCk1pbTwgm=Cz9JC~UhiLzW z!0!>D5V)3xAX=agFZe8=L29T8KBz#7NLjEtxcKr#V$O2pS~>%Qp=oGke*stN4U&|vQX(CtNl4p#{GmhoE9Y>cA;;*lI4DnYY`QfzwG><&8 z@9CbWkLgZiyq+;GPwnP=q^i%PNuId0?oGEN^@G~M>LJ4o#bjRDcw~9EXeK)00UuLj z4(1Lv4?TOsG+Df1B5I>2In8;GkBP&llxnw%sXeoHP;`3Jq;he$#1o%%V$rxkQqEO$d2_^bg%t~`qqEC9I2}bQdc-3D|$#>bfVjFxV^s} z>~TUzTYI*;*A6H~7cHICE*sfCsV%*+Zcl8`2RjM~OOZ0BNE!H-aYas#xLeTO$Q;|8xLF$cnKaR(igTL| zJu^@}sY>sz@W`S#(<=s(ySGlrvTw^G`f~;_+7-%`0IHVYAdHDI%1ICkPJivG%nfHWP2(hnX zPsNKHe3>NJ67wY#Fm#)KB@2PyMRmj0tttFyGL+(>-(YPLQa*=CYa^Kd-j<{aBg6e6 zus`Uv$>w+wl`Dwv$=G+fEl zRHtPsYu>K;j{?mJ8SCIB!N-QR24VVWTF9KOu3G;#h;<3jzeR8`uS*vEtNje8 z22-r=k|UA85BFR>=!bmf>*^|}4y1iYyQse2WNoakX9ecYRtK#|3@!@bL+nM1;K<<) zbXM4EZ)&wbrW*;MlSm)N;zm{gv4WiySZ$D^FG8$h1kk}BIP!frfEuZaK_C|21V^_b zkoyig-`J&5F zLjMhhq7T_pUQTC$#ar}pIuE|-Yy^;p*%0^uV~u<>Ju~ zTJX($JnYNw3KQ|w_`NkaCXjV^P4HuWAs}8Xzt6`6v46hzj110(2N;fvfr_+<|6)fM Vcz;T10`_r&s5FiL@gf}H{{l2;E-U~5 delta 3442 zcma)8eQ*=U6~EP;q?2XIUy?0<%Eq4xHea$am=BwlfNh9t&YC0E)>W)hf8+KCCtr0Gm)-#f)0 z$@CB2=ziLL`}V!JZ{OS9bMKKipCitNPDd68^wP&i_rAFLq|-xQJy{?3XG_*1E>1L2 za!8`lvPn+KX0)=TtRhac@5z?zE4Fi71m>4z7%a$Z)Xk`eBmC()B))!~RxZ zb0{C#E&!iJ&r&s6i>X>HtWs2^n^QEF)MyjREJKDR5sk#;q%Vv5k=JbSv|$=mhiMLS zmLtG=Ih|;<2mgiPSZI{?K~u#T)*)3Q(XG(U)kQTLAJn3%MuX5lot|uHA}v;E&Ja4z z!*}PVh2vyi&Xe=y0@)?InVK3U9Slaiv7X6XYb)MLJC>o0o&O zsf+IvQdwRuVC%KB{hJEh?GoQgGA2e%9H$g0`F#a!a*dn)bwxg(*O$+xS1k1w2dt8# z(LCb{$EEB6yHeKYQp)@Cd-G5e@9guiCGE{7m~wojkGR-d54zal^&-2xvY5~BEkYqi z+SXQiq=LqhIaPfg$@Td@OSP4=zV$@{==S2Y9C!0)K6*LeRT-*&*_KW6+4&_dcDK!4 znpMRGT~g7^TIS2M3)>8ta+JyW|1!Nk)amv}#Zrk>+KO=oGOW#vlu6}Z7+J_3=`AUQ zU|Q-k@?2ykrgvXKZxfs9C@QXKg^(h7x!sF1Esm4C0n4dckEwSltLiGKEtqpvBGsH! zDG{j(z6J1AOJuR2lo*>(e5fX9lWL_psXl0y8YEwuPZwt(ZFc8 z?n!QSzoEm54e83@pk*j1Hi6TD{p@LXYW0I^5&C2z4hX+K6q)O zHd6V{3qQR1{A2GOJ#p)+zhHasZ?ZU0OAdSb{*A7^(A3R|SW=-weuG|aXLmQ0u(FL! zL7qkh zMh~VhZ#+dB;eh0N;2;-=qFQJy9!t;$;7~WpIl^F33ni5#Xi{hyo9tRhj}>Dit})4N>yN>3IcA$EIDG5h2zZwJsvsv>9zBM1)xfZy~}NTjDPtFBsd!k_VUXH`i%2s| z1`8oZj|Mx)uGyNQMhV4Ha{|CfqR2oz9FK+f(YeUrBVaB(KI`0$&~rG1H@ zV-mFXHNgjW&% zj4+9S**Y&aFJt!=gx?_aA)G?^;`RbhtpI@6(%{UC$#^z(%qP0F8f0`^Dz0cDToj## z{vUM{kn%Mrh^R#e!`x)mCgiB2w*BaJcSbwddbDAf;q?>%q5qmfJ zh$H4rg#iwKYKC4#gJ3YjhMXfrMTFaUtywJ!bjWraCDB(~@ z7ouY^jn?4+o}4-l2PyqKcKz73#MHr9JZijDjY#{lbRz2l)V`_>^42kSyYhSTFNjBg?!b2{C!e z$zG`zg3Nb3onGPR7J!Q$PiKvAv1M&%4eM0P%1DpTK|ewJcM#Bb-JBdrq{gY6omOjm zw;?--fT?0w)WuLJDJP<#5dA&!a4(G4xh}3wD3OtPbS<@lSMcLhJQ86i3AruQ5aG^d zmalbohfE~r!}Vql=^@k0IU#%Ez*WBZO+h@e;Wp0+CI6b5@oTg4-1 TvSyu!yXdj5TfkqeB7pw`Jjyqk diff --git a/HostComputer/bin_to_time_freq.py b/HostComputer/bin_to_time_freq.py index eecbece..02ebaa6 100644 --- a/HostComputer/bin_to_time_freq.py +++ b/HostComputer/bin_to_time_freq.py @@ -18,14 +18,23 @@ python bin_to_time_freq.py xxx.bin 3125000 --save=out.png python bin_to_time_freq.py xxx.bin 3125000 --selftest python bin_to_time_freq.py xxx.bin 3125000 --reverse --cycles=8 + python bin_to_time_freq.py xxx.bin 3125000 --d1 --d2 + python bin_to_time_freq.py xxx.bin 3125000 --d1 --dwindow-ms=10 + python bin_to_time_freq.py xxx.bin 3125000 --d1 --d2 --d1-window-ms=5 --d2-window-ms=15 选项: --thresh=128 电平阈值（默认128） --cycles=8 用连续多少个周期估计频率（默认8；1为逐周期） --reverse 反相电平后再检测脉冲（低电平作为脉冲高电平） --ymax=12000 频率轴上限（默认自适应） + --d1 另开一张图：频率对时间的一阶导数 df/dt（红） + --d2 另开一张图：频率对时间的二阶导数 d²f/dt²（黑） + --dwindow-ms=5 一、二阶导数的公共拟合窗口，单位 ms（默认5） + --d1-window-ms 原函数 -> 一阶导数的拟合窗口（默认继承 --dwindow-ms） + --d2-window-ms 一阶导数 -> 二阶导数的拟合窗口（默认继承 --dwindow-ms） --save=路径 保存 PNG 后退出（不弹窗） --selftest 仅打印统计，不画图 + --help, -h 显示这份中文帮助后退出 交互（弹窗模式）: 滚轮 缩放 X 轴（向上放大/向下缩小，以鼠标位置为中心） @@ -166,7 +175,61 @@ def selftest(path, fs, thresh, cycles, reverse=False): print("波形活动时长: %.1f ms" % (meta['activity_duration'] * 1000)) -def plot_show(path, fs, thresh, ymax, cycles, reverse, save_path=None): +def local_linear_derivative(t_sec, values, window_s=5e-3, min_points=7): + """对非等间隔时间点做局部线性拟合，返回 d(values)/dt。 + + 直接对相邻频率点差分会放大采样量化抖动；局部最小二乘斜率 + 使用真实时间间隔，并对这种抖动做平均。 + """ + t = np.asarray(t_sec, dtype=np.float64) + y = np.asarray(values, dtype=np.float64) + if t.ndim != 1 or y.ndim != 1 or len(t) != len(y): + raise ValueError("t_sec 和 values 必须是等长一维数组") + if len(t) < 2: + raise ValueError("至少需要两个点才能计算导数") + if window_s <= 0: + raise ValueError("导数拟合窗口必须大于0") + if np.any(np.diff(t) <= 0): + raise ValueError("时间点必须严格递增") + + n = len(t) + min_points = min(n, max(2, int(min_points))) + half_window = window_s / 2.0 + left = np.searchsorted(t, t - half_window, side='left') + right = np.searchsorted(t, t + half_window, side='right') + + # 低频段在固定时间窗口内可能点数太少，至少补足 min_points 个点。 + idx = np.arange(n) + fallback_left = np.clip(idx - min_points // 2, 0, n - min_points) + fallback_right = fallback_left + min_points + too_few = (right - left) < min_points + left[too_few] = fallback_left[too_few] + right[too_few] = fallback_right[too_few] + + # 移动时间原点减少长时采集时前缀和相减的精度损失。 + x = t - (t[0] + t[-1]) / 2.0 + sx = np.concatenate(([0.0], np.cumsum(x))) + sy = np.concatenate(([0.0], np.cumsum(y))) + sxx = np.concatenate(([0.0], np.cumsum(x * x))) + sxy = np.concatenate(([0.0], np.cumsum(x * y))) + count = (right - left).astype(np.float64) + sum_x = sx[right] - sx[left] + sum_y = sy[right] - sy[left] + sum_xx = sxx[right] - sxx[left] + sum_xy = sxy[right] - sxy[left] + denominator = count * sum_xx - sum_x * sum_x + numerator = count * sum_xy - sum_x * sum_y + + derivative = np.empty(n, dtype=np.float64) + good = np.abs(denominator) > np.finfo(np.float64).eps + derivative[good] = numerator[good] / denominator[good] + derivative[~good] = np.gradient(y, t)[~good] + return derivative + + +def plot_show(path, fs, thresh, ymax, cycles, reverse, save_path=None, + draw_d1=False, draw_d2=False, d1_window_ms=5.0, + d2_window_ms=None): t, freq, hi_w, meta = load_time_freq(path, fs, thresh, cycles, reverse) import matplotlib.pyplot as plt @@ -183,8 +246,8 @@ def plot_show(path, fs, thresh, ymax, cycles, reverse, save_path=None): y_full = freq ax.set_xlim(0.0, meta['activity_duration'] * 1000 * 1.02) ax.set_ylim(0, ymax if ymax > 0 else freq.max() * 1.08) - ax.set_title("%s 时频曲线（%d 个测量点，%d 周期平均，活动时长 %.0f ms）" % ( - os.path.basename(path), meta['pulses'], meta['cycles'], + ax.set_title("%s 时频曲线（检测到脉冲 %d，频率测量点 %d，%d 周期平均，活动时长 %.0f ms）" % ( + os.path.basename(path), meta['detected_pulses'], meta['pulses'], meta['cycles'], meta['activity_duration'] * 1000)) ax.set_xlabel("时间 (ms)") ax.set_ylabel("频率 (Hz)") @@ -195,10 +258,53 @@ def plot_show(path, fs, thresh, ymax, cycles, reverse, save_path=None): ax.grid(True, which='minor', alpha=0.15) ax.minorticks_on() + fig_der = None + draw_d1 = draw_d1 and (len(freq) >= 2) + draw_d2 = draw_d2 and (len(freq) >= 3) + if draw_d1 or draw_d2: + f64 = freq.astype(np.float64) + if d2_window_ms is None: + d2_window_ms = d1_window_ms + d1_full = local_linear_derivative(t, f64, d1_window_ms * 1e-3) + d2_full = (local_linear_derivative(t, d1_full, d2_window_ms * 1e-3) + if draw_d2 else None) + plot_count = int(draw_d1) + int(draw_d2) + fig_der, axes_der = plt.subplots(plot_count, 1, figsize=(15, 4.5 * plot_count), + sharex=True, squeeze=False) + fig_der.subplots_adjust(bottom=0.14, top=0.90) + row = 0 + if draw_d1: + ax_der = axes_der[row, 0] + ax_der.plot(x_full, d1_full, '-', lw=0.9, color='red', label='df/dt') + ax_der.set_ylabel("一阶导数 (Hz/s)") + ax_der.legend(loc='upper right') + row += 1 + if draw_d2: + ax_der = axes_der[row, 0] + ax_der.plot(x_full, d2_full, '-', lw=0.9, color='black', label='d²f/dt²') + ax_der.set_ylabel("二阶导数 (Hz/s²)") + ax_der.legend(loc='upper right') + for ax_der in axes_der[:, 0]: + ax_der.set_xlim(0.0, meta['activity_duration'] * 1000 * 1.02) + ax_der.grid(True, which='major', alpha=0.35) + ax_der.minorticks_on() + axes_der[-1, 0].set_xlabel("时间 (ms)") + axes_der[0, 0].set_title( + "时频曲线导数（一阶窗口 %.3g ms / 二阶窗口 %.3g ms）" % + (d1_window_ms, d2_window_ms)) + if save_path: + scatter.set_data(x_full, y_full) + root, ext = os.path.splitext(save_path) fig.savefig(save_path, dpi=130) print("已保存: %s" % save_path) + if fig_der is not None: + der_path = root + "_d" + ext + fig_der.savefig(der_path, dpi=130) + print("已保存: %s" % der_path) plt.close(fig) + if fig_der is not None: + plt.close(fig_der) return # 交互：滚轮缩放 X（Ctrl+滚轮缩放 Y）/ 左键双向拖拽平移 / 双击复位 / 悬停高亮 @@ -323,8 +429,9 @@ def plot_show(path, fs, thresh, ymax, cycles, reverse, save_path=None): fig.canvas.mpl_connect('button_press_event', on_double) print("打开窗口：滚轮缩放X / Ctrl+滚轮缩放Y / 左键双向拖拽平移 / 双击复位") - print("测量点 %d 个，使用 %d 周期平均，活动时长 %.1f ms" % ( - meta['pulses'], meta['cycles'], meta['activity_duration'] * 1000)) + print("检测到脉冲 %d，频率测量点 %d，使用 %d 周期平均，活动时长 %.1f ms" % ( + meta['detected_pulses'], meta['pulses'], meta['cycles'], + meta['activity_duration'] * 1000)) plt.show() @@ -344,6 +451,11 @@ def main(): reverse = '--reverse' in opts ymax = float(opts.get('--ymax', 0)) save_path = opts.get('--save', '') + draw_d1 = '--d1' in opts + draw_d2 = '--d2' in opts + derivative_window_ms = float(opts.get('--dwindow-ms', 5.0)) + d1_window_ms = float(opts.get('--d1-window-ms', derivative_window_ms)) + d2_window_ms = float(opts.get('--d2-window-ms', derivative_window_ms)) if not os.path.isfile(path): print("错误：文件不存在 - %s" % path) @@ -352,7 +464,8 @@ def main(): if '--selftest' in opts: selftest(path, fs, thresh, cycles, reverse) else: - plot_show(path, fs, thresh, ymax, cycles, reverse, save_path or None) + plot_show(path, fs, thresh, ymax, cycles, reverse, save_path or None, + draw_d1, draw_d2, d1_window_ms, d2_window_ms) if __name__ == '__main__': diff --git a/HostComputer/bin_to_time_freq1.py b/HostComputer/bin_to_time_freq1.py index 4f26e4f..8a599f7 100644 --- a/HostComputer/bin_to_time_freq1.py +++ b/HostComputer/bin_to_time_freq1.py @@ -4,24 +4,38 @@ 数据格式: 逻辑分析仪导出的单通道数字采样，每字节 1 个采样点； 电平 >= 阈值(默认128) 判为高（脉冲），否则为低。 + --reverse 时先把高低电平对调，再按同样规则找脉冲。 频率定义: 每脉冲频率 = 采样率 / 脉冲周期； 周期 = 本脉冲高电平起点 -> 下一脉冲高电平起点； - 最后一个脉冲无下一脉冲，按高电平宽度 x2 近似（50% 占空比）。 + 仅使用实际检测到的相邻上升沿，不猜测末脉冲周期。 时间定义: 脉冲时间 = 高电平中点（与低电平/空闲段无关）。 用法: - python bin_to_time_freq.py [采样率Hz] [选项] + python bin_to_time_freq1.py [采样率Hz] [选项] 示例: - python bin_to_time_freq.py "Document/PLSR_document/波形/10段.bin" 6250000 - python bin_to_time_freq.py xxx.bin 6250000 --ymax=12000 - python bin_to_time_freq.py xxx.bin 6250000 --save=out.png - python bin_to_time_freq.py xxx.bin 6250000 --selftest + python bin_to_time_freq1.py "Document/PLSR_document/波形/10段.bin" 6250000 + python bin_to_time_freq1.py xxx.bin 6250000 --ymax=12000 + python bin_to_time_freq1.py xxx.bin 6250000 --line + python bin_to_time_freq1.py xxx.bin 6250000 --reverse + python bin_to_time_freq1.py xxx.bin 6250000 --d1 --d2 + python bin_to_time_freq1.py xxx.bin 6250000 --d1 --dwindow-ms=10 + python bin_to_time_freq1.py xxx.bin 6250000 --d1 --d2 --d1-window-ms=5 --d2-window-ms=15 + python bin_to_time_freq1.py xxx.bin 6250000 --save=out.png + python bin_to_time_freq1.py xxx.bin 6250000 --selftest 选项: --thresh=128 电平阈值（默认128） --ymax=12000 频率轴上限（默认自适应） + --line 将脉冲点连成折线（默认只画散点） + --reverse 高低电平对调后再算频率点（低电平当脉冲） + --d1 另开一张图：频率对时间的一阶导数 df/dt（红） + --d2 另开一张图：频率对时间的二阶导数 d²f/dt²（黑） + --dwindow-ms=5 一、二阶导数的公共拟合窗口，单位 ms（默认5） + --d1-window-ms 原函数 -> 一阶导数的拟合窗口（默认继承 --dwindow-ms） + --d2-window-ms 一阶导数 -> 二阶导数的拟合窗口（默认继承 --dwindow-ms） --save=路径 保存 PNG 后退出（不弹窗） --selftest 仅打印统计，不画图 + --help, -h 显示这份中文帮助后退出 交互（弹窗模式）: 滚轮 缩放 X 轴（向上放大/向下缩小，以鼠标位置为中心） @@ -46,13 +60,20 @@ except Exception: pass -def load_time_freq(path, fs, threshold=128): +def load_time_freq(path, fs, threshold=128, reverse=False): """读取 bin，返回 (t_sec, freq_hz, hi_width_sec, meta)。 - t_sec 以第一个脉冲为 0 时刻；hi_width 为每脉冲高电平宽度。""" + t_sec 以第一个脉冲为 0 时刻；hi_width 为每脉冲高电平宽度。 + reverse=True 时先把高低电平对调，再按高电平找脉冲。""" data = np.fromfile(path, dtype=np.uint8) if data.size == 0: raise ValueError("文件为空: %s" % path) + if fs <= 0: + raise ValueError("采样率必须大于0") + if not 0 <= threshold <= 255: + raise ValueError("阈值必须在0到255之间") samples = (data >= threshold).astype(np.int8) + if reverse: + samples = 1 - samples # run 级压缩 changes = np.flatnonzero(np.diff(samples) != 0) + 1 @@ -60,68 +81,132 @@ def load_time_freq(path, fs, threshold=128): ends = np.concatenate((changes, [len(samples)])) runs = np.column_stack((starts, ends, samples[starts])) - # 脉冲 = 高电平 run，且其后紧跟低电平 run + # 每个高电平 run 的起点都是一个上升沿。如果采集从高电平开始， + # 第一个 run 的真实上升沿在文件外，不能用来测周期。 hi = np.flatnonzero(runs[:, 2] == 1) - hi = hi[hi + 1 < len(runs)] - lo_ok = runs[hi + 1, 2] == 0 - hi = hi[lo_ok] + if hi.size and runs[hi[0], 0] == 0: + hi = hi[1:] + if hi.size < 2: + raise ValueError("至少需要两个完整的上升沿才能计算频率: %s" % path) t_start = runs[hi, 0].astype(np.int64) hi_width = (runs[hi, 1] - runs[hi, 0]).astype(np.int64) - # 周期：高到高；末脉冲按 2x 高电平宽度近似 - next_start = np.append(t_start[1:], [t_start[-1] + 2 * hi_width[-1]]) - period = next_start - t_start - freq = fs / np.maximum(period, 1) + # 只用相邻实测上升沿求周期，不根据占空比猜测末脉冲频率。 + period = np.diff(t_start) + freq = float(fs) / np.maximum(period, 1) # 时间：高电平中点，去起始偏移 - t_sec = (t_start + hi_width / 2.0) / fs + t_sec = (t_start[:-1] + hi_width[:-1] / 2.0) / fs t_sec = t_sec - t_sec[0] meta = { 'total_samples': int(len(samples)), 'duration': len(samples) / fs, 'pulses': int(len(freq)), + 'detected_pulses': int(len(t_start)), 'offset_ms': t_start[0] / fs * 1000, } - return t_sec, freq, hi_width / fs, meta - - -def selftest(path, fs, thresh): + return t_sec, freq, hi_width[:-1] / fs, meta + + +def local_linear_derivative(t_sec, values, window_s=5e-3, min_points=7): + """对非等间隔时间点做局部线性拟合，返回 d(values)/dt。 + + 直接对相邻单周期频率做差分会放大整数采样点带来的量化抖动； + 局部最小二乘斜率使用真实时间间隔，并对这种抖动做平均。 + """ + t = np.asarray(t_sec, dtype=np.float64) + y = np.asarray(values, dtype=np.float64) + if t.ndim != 1 or y.ndim != 1 or len(t) != len(y): + raise ValueError("t_sec 和 values 必须是等长一维数组") + if len(t) < 2: + raise ValueError("至少需要两个点才能计算导数") + if window_s <= 0: + raise ValueError("导数拟合窗口必须大于0") + if np.any(np.diff(t) <= 0): + raise ValueError("时间点必须严格递增") + + n = len(t) + min_points = min(n, max(2, int(min_points))) + half_window = window_s / 2.0 + left = np.searchsorted(t, t - half_window, side='left') + right = np.searchsorted(t, t + half_window, side='right') + + # 低频段在固定时间窗口内可能点数太少，至少补足 min_points 个点。 + idx = np.arange(n) + fallback_left = np.clip(idx - min_points // 2, 0, n - min_points) + fallback_right = fallback_left + min_points + too_few = (right - left) < min_points + left[too_few] = fallback_left[too_few] + right[too_few] = fallback_right[too_few] + + # 前缀和使每个窗口的最小二乘斜率可以 O(1) 计算。 + # 先把时间原点移到数据中心，降低长时采集时前缀和相减的精度损失。 + x = t - (t[0] + t[-1]) / 2.0 + sx = np.concatenate(([0.0], np.cumsum(x))) + sy = np.concatenate(([0.0], np.cumsum(y))) + sxx = np.concatenate(([0.0], np.cumsum(x * x))) + sxy = np.concatenate(([0.0], np.cumsum(x * y))) + count = (right - left).astype(np.float64) + sum_x = sx[right] - sx[left] + sum_y = sy[right] - sy[left] + sum_xx = sxx[right] - sxx[left] + sum_xy = sxy[right] - sxy[left] + denominator = count * sum_xx - sum_x * sum_x + numerator = count * sum_xy - sum_x * sum_y + + derivative = np.empty(n, dtype=np.float64) + good = np.abs(denominator) > np.finfo(np.float64).eps + derivative[good] = numerator[good] / denominator[good] + derivative[~good] = np.gradient(y, t)[~good] + return derivative + + +def selftest(path, fs, thresh, reverse=False): import statistics - t, freq, hi_w, meta = load_time_freq(path, fs, thresh) + t, freq, hi_w, meta = load_time_freq(path, fs, thresh, reverse) print("文件: %s" % path) print("总采样: %d (%.3f s @ %.2f MS/s)" % (meta['total_samples'], meta['duration'], fs / 1e6)) print("起始采集偏移: %.1f ms" % meta['offset_ms']) - print("脉冲总数: %d" % meta['pulses']) + print("检测到脉冲: %d，频率测量点: %d" % ( + meta['detected_pulses'], meta['pulses'])) if meta['pulses']: print("频率 min=%.0f max=%.0f 中位=%.0f Hz" % ( freq.min(), freq.max(), statistics.median(freq))) - print("末脉冲: t=%.3f ms, 高电平 %.3f ms, %.0f Hz" % ( + print("末测量点: t=%.3f ms, 高电平 %.3f ms, %.0f Hz" % ( t[-1] * 1000, hi_w[-1] * 1000, freq[-1])) print("波形活动时长: %.1f ms" % ((t[-1] + hi_w[-1] / 2) * 1000)) -def plot_show(path, fs, thresh, ymax, save_path=None): - t, freq, hi_w, meta = load_time_freq(path, fs, thresh) +def plot_show(path, fs, thresh, ymax, save_path=None, draw_line=False, + reverse=False, draw_d1=False, draw_d2=False, + d1_window_ms=5.0, d2_window_ms=None): + t, freq, hi_w, meta = load_time_freq(path, fs, thresh, reverse) import matplotlib.pyplot as plt from matplotlib.ticker import MaxNLocator + draw_d1 = draw_d1 and (len(freq) >= 2) + draw_d2 = draw_d2 and (len(freq) >= 3) fig, ax = plt.subplots(figsize=(15, 7)) fig.subplots_adjust(bottom=0.10, top=0.92) # 全脉冲散点：复用 artist，重绘时按可见范围抽稀（大文件流畅） MAX_VISIBLE_POINTS = 5000 # 可见范围内最多绘制的点数，超过则等间隔抽稀 - (scatter,) = ax.plot([], [], '.', ms=1.5, color='C0', alpha=0.5, zorder=1) + (scatter,) = ax.plot([], [], '.', ms=1.5, color='C0', alpha=0.5, zorder=2) + line = None + if draw_line: + (line,) = ax.plot([], [], '-', lw=0.9, color='C0', alpha=0.85, zorder=1) tx_ms = t * 1000 x_full = tx_ms y_full = freq ax.set_xlim(0.0, (t[-1] + hi_w[-1]) * 1000 * 1.02) ax.set_ylim(0, ymax if ymax > 0 else freq.max() * 1.08) - ax.set_title("%s 时频曲线（%d 脉冲，活动时长 %.0f ms）" % ( - os.path.basename(path), meta['pulses'], - (t[-1] + hi_w[-1] / 2) * 1000)) + title_extra = "，高低对调" if reverse else "" + ax.set_title("%s 时频曲线（检测到脉冲 %d，频率测量点 %d，活动时长 %.0f ms%s）" % ( + os.path.basename(path), meta['detected_pulses'], meta['pulses'], + (t[-1] + hi_w[-1] / 2) * 1000, title_extra)) ax.set_xlabel("时间 (ms)") ax.set_ylabel("频率 (Hz)") # 更密的刻度 @@ -131,10 +216,53 @@ def plot_show(path, fs, thresh, ymax, save_path=None): ax.grid(True, which='minor', alpha=0.15) ax.minorticks_on() + fig_der = None + if draw_d1 or draw_d2: + f64 = freq.astype(np.float64) + if d2_window_ms is None: + d2_window_ms = d1_window_ms + d1_full = local_linear_derivative(t, f64, d1_window_ms * 1e-3) + d2_full = (local_linear_derivative(t, d1_full, d2_window_ms * 1e-3) + if draw_d2 else None) + plot_count = int(draw_d1) + int(draw_d2) + fig_der, axes_der = plt.subplots(plot_count, 1, figsize=(15, 4.5 * plot_count), + sharex=True, squeeze=False) + fig_der.subplots_adjust(bottom=0.14, top=0.90) + row = 0 + if draw_d1: + ax_der = axes_der[row, 0] + ax_der.plot(x_full, d1_full, '-', lw=0.9, color='red', label='df/dt') + ax_der.set_ylabel("一阶导数 (Hz/s)") + ax_der.legend(loc='upper right') + row += 1 + if draw_d2: + ax_der = axes_der[row, 0] + ax_der.plot(x_full, d2_full, '-', lw=0.9, color='black', label='d²f/dt²') + ax_der.set_ylabel("二阶导数 (Hz/s²)") + ax_der.legend(loc='upper right') + for ax_der in axes_der[:, 0]: + ax_der.set_xlim(0.0, (t[-1] + hi_w[-1]) * 1000 * 1.02) + ax_der.grid(True, which='major', alpha=0.35) + ax_der.minorticks_on() + axes_der[-1, 0].set_xlabel("时间 (ms)") + axes_der[0, 0].set_title( + "时频曲线导数（一阶窗口 %.3g ms / 二阶窗口 %.3g ms）" % + (d1_window_ms, d2_window_ms)) + if save_path: + scatter.set_data(x_full, y_full) + if line is not None: + line.set_data(x_full, y_full) + root, ext = os.path.splitext(save_path) fig.savefig(save_path, dpi=130) print("已保存: %s" % save_path) + if fig_der is not None: + der_path = root + "_d" + ext + fig_der.savefig(der_path, dpi=130) + print("已保存: %s" % der_path) plt.close(fig) + if fig_der is not None: + plt.close(fig_der) return # 交互：滚轮缩放 X（Ctrl+滚轮缩放 Y）/ 左键双向拖拽平移 / 双击复位 / 悬停高亮 @@ -161,6 +289,8 @@ def plot_show(path, fs, thresh, ymax, save_path=None): else: idx = np.flatnonzero(mask) scatter.set_data(x_full[idx], y_full[idx]) + if line is not None: + line.set_data(x_full[idx], y_full[idx]) def on_scroll(event): if event.inaxes is not ax or event.xdata is None: @@ -259,8 +389,9 @@ def plot_show(path, fs, thresh, ymax, save_path=None): fig.canvas.mpl_connect('button_press_event', on_double) print("打开窗口：滚轮缩放X / Ctrl+滚轮缩放Y / 左键双向拖拽平移 / 双击复位") - print("脉冲 %d 个，活动时长 %.1f ms" % ( - meta['pulses'], (t[-1] + hi_w[-1] / 2) * 1000)) + print("检测到脉冲 %d，频率测量点 %d，活动时长 %.1f ms" % ( + meta['detected_pulses'], meta['pulses'], + (t[-1] + hi_w[-1] / 2) * 1000)) plt.show() @@ -269,7 +400,7 @@ def main(): opts = {a.split('=', 1)[0]: (a.split('=', 1)[1] if '=' in a else True) for a in sys.argv[1:] if a.startswith('--')} - if len(args) < 1 or '--help' in opts or '-h' in opts: + if len(args) < 1 or '--help' in opts or '-h' in sys.argv[1:]: print(__doc__) sys.exit(0) @@ -278,15 +409,23 @@ def main(): thresh = int(opts.get('--thresh', 128)) ymax = float(opts.get('--ymax', 0)) save_path = opts.get('--save', '') + draw_line = '--line' in opts + reverse = '--reverse' in opts + draw_d1 = '--d1' in opts + draw_d2 = '--d2' in opts + derivative_window_ms = float(opts.get('--dwindow-ms', 5.0)) + d1_window_ms = float(opts.get('--d1-window-ms', derivative_window_ms)) + d2_window_ms = float(opts.get('--d2-window-ms', derivative_window_ms)) if not os.path.isfile(path): print("错误：文件不存在 - %s" % path) sys.exit(1) if '--selftest' in opts: - selftest(path, fs, thresh) + selftest(path, fs, thresh, reverse) else: - plot_show(path, fs, thresh, ymax, save_path or None) + plot_show(path, fs, thresh, ymax, save_path or None, draw_line, + reverse, draw_d1, draw_d2, d1_window_ms, d2_window_ms) if __name__ == '__main__': diff --git a/PLSR/Src/plsr_planner.c b/PLSR/Src/plsr_planner.c index dd299a3..754b909 100644 --- a/PLSR/Src/plsr_planner.c +++ b/PLSR/Src/plsr_planner.c @@ -63,34 +63,150 @@ void PlsrPlannerTimingReset(void) } #endif -static const uint32_t PlsrPlannerSmoothIntegralQ24[65] = +#define PLSR_PLANNER_CURVE_TABLE_BITS (9U) +#define PLSR_PLANNER_CURVE_TABLE_INTERVALS (1UL << PLSR_PLANNER_CURVE_TABLE_BITS) +#define PLSR_PLANNER_CURVE_TABLE_SIZE (PLSR_PLANNER_CURVE_TABLE_INTERVALS + 1UL) +#define PLSR_PLANNER_CURVE_DERIVATIVE_SHIFT (8U + PLSR_PLANNER_CURVE_TABLE_BITS) + +/* curveMode 1: fixed seven-section jerk profile. Each entry/exit ramp + uses a 1:2:1 constant-jerk/constant-acceleration/constant-jerk ratio. + The table stores the integral of the normalized frequency blend. */ +static const uint32_t PlsrPlannerSmoothIntegralQ24[PLSR_PLANNER_CURVE_TABLE_SIZE] = { - 0UL, 64UL, 504UL, 1688UL, 3968UL, 7688UL, 13176UL, 20752UL, - 30720UL, 43376UL, 59000UL, 77864UL, 100224UL, 126328UL, 156408UL, - 190688UL, 229376UL, 272672UL, 320760UL, 373816UL, 432000UL, - 495464UL, 564344UL, 638768UL, 718848UL, 804688UL, 896376UL, - 993992UL, 1097600UL, 1207256UL, 1323000UL, 1444864UL, 1572864UL, - 1707008UL, 1847288UL, 1993688UL, 2146176UL, 2304712UL, 2469240UL, - 2639696UL, 2816000UL, 2998064UL, 3185784UL, 3379048UL, 3577728UL, - 3781688UL, 3990776UL, 4204832UL, 4423680UL, 4647136UL, 4875000UL, - 5107064UL, 5343104UL, 5582888UL, 5826168UL, 6072688UL, 6322176UL, - 6574352UL, 6828920UL, 7085576UL, 7344000UL, 7603864UL, 7864824UL, - 8126528UL, 8388608UL + 0UL, 0UL, 1UL, 3UL, 7UL, 14UL, 24UL, 38UL, + 57UL, 81UL, 111UL, 148UL, 192UL, 244UL, 305UL, 375UL, + 455UL, 546UL, 648UL, 762UL, 889UL, 1029UL, 1183UL, 1352UL, + 1536UL, 1736UL, 1953UL, 2187UL, 2439UL, 2710UL, 3000UL, 3310UL, + 3641UL, 3993UL, 4367UL, 4764UL, 5184UL, 5628UL, 6097UL, 6591UL, + 7111UL, 7658UL, 8232UL, 8834UL, 9465UL, 10125UL, 10815UL, 11536UL, + 12288UL, 13072UL, 13889UL, 14739UL, 15623UL, 16542UL, 17496UL, 18486UL, + 19513UL, 20577UL, 21679UL, 22820UL, 24000UL, 25220UL, 26481UL, 27783UL, + 29127UL, 30514UL, 31944UL, 33418UL, 34937UL, 36501UL, 38111UL, 39768UL, + 41472UL, 43224UL, 45025UL, 46875UL, 48775UL, 50726UL, 52728UL, 54782UL, + 56889UL, 59049UL, 61263UL, 63532UL, 65856UL, 68236UL, 70673UL, 73167UL, + 75719UL, 78330UL, 81000UL, 83730UL, 86521UL, 89373UL, 92287UL, 95264UL, + 98304UL, 101408UL, 104577UL, 107811UL, 111111UL, 114478UL, 117912UL, 121414UL, + 124985UL, 128625UL, 132335UL, 136116UL, 139968UL, 143892UL, 147889UL, 151959UL, + 156103UL, 160322UL, 164616UL, 168986UL, 173433UL, 177957UL, 182559UL, 187240UL, + 192000UL, 196840UL, 201761UL, 206763UL, 211847UL, 217014UL, 222264UL, 227598UL, + 233017UL, 238521UL, 244110UL, 249785UL, 255545UL, 261390UL, 267321UL, 273337UL, + 279438UL, 285625UL, 291897UL, 298254UL, 304697UL, 311225UL, 317838UL, 324537UL, + 331321UL, 338190UL, 345145UL, 352185UL, 359310UL, 366521UL, 373817UL, 381198UL, + 388665UL, 396217UL, 403854UL, 411577UL, 419385UL, 427278UL, 435257UL, 443321UL, + 451470UL, 459705UL, 468025UL, 476430UL, 484921UL, 493497UL, 502158UL, 510905UL, + 519737UL, 528654UL, 537657UL, 546745UL, 555918UL, 565177UL, 574521UL, 583950UL, + 593465UL, 603065UL, 612750UL, 622521UL, 632377UL, 642318UL, 652345UL, 662457UL, + 672654UL, 682937UL, 693305UL, 703758UL, 714297UL, 724921UL, 735630UL, 746425UL, + 757305UL, 768270UL, 779321UL, 790457UL, 801678UL, 812985UL, 824377UL, 835854UL, + 847417UL, 859065UL, 870798UL, 882617UL, 894521UL, 906510UL, 918585UL, 930745UL, + 942990UL, 955321UL, 967737UL, 980238UL, 992825UL, 1005497UL, 1018254UL, 1031097UL, + 1044025UL, 1057038UL, 1070137UL, 1083321UL, 1096590UL, 1109945UL, 1123385UL, 1136910UL, + 1150521UL, 1164217UL, 1177998UL, 1191865UL, 1205817UL, 1219854UL, 1233977UL, 1248185UL, + 1262478UL, 1276857UL, 1291321UL, 1305870UL, 1320505UL, 1335225UL, 1350030UL, 1364921UL, + 1379897UL, 1394958UL, 1410105UL, 1425337UL, 1440654UL, 1456057UL, 1471545UL, 1487118UL, + 1502777UL, 1518521UL, 1534350UL, 1550265UL, 1566265UL, 1582350UL, 1598521UL, 1614777UL, + 1631118UL, 1647545UL, 1664057UL, 1680654UL, 1697337UL, 1714105UL, 1730958UL, 1747897UL, + 1764921UL, 1782030UL, 1799225UL, 1816505UL, 1833870UL, 1851321UL, 1868857UL, 1886478UL, + 1904185UL, 1921977UL, 1939854UL, 1957817UL, 1975865UL, 1993998UL, 2012217UL, 2030521UL, + 2048910UL, 2067385UL, 2085945UL, 2104590UL, 2123321UL, 2142137UL, 2161038UL, 2180025UL, + 2199097UL, 2218254UL, 2237497UL, 2256825UL, 2276238UL, 2295737UL, 2315321UL, 2334990UL, + 2354745UL, 2374585UL, 2394510UL, 2414521UL, 2434617UL, 2454798UL, 2475065UL, 2495417UL, + 2515854UL, 2536377UL, 2556985UL, 2577678UL, 2598457UL, 2619321UL, 2640270UL, 2661305UL, + 2682425UL, 2703630UL, 2724921UL, 2746297UL, 2767758UL, 2789305UL, 2810937UL, 2832654UL, + 2854457UL, 2876345UL, 2898318UL, 2920377UL, 2942521UL, 2964750UL, 2987065UL, 3009465UL, + 3031950UL, 3054521UL, 3077177UL, 3099918UL, 3122745UL, 3145657UL, 3168654UL, 3191737UL, + 3214905UL, 3238158UL, 3261497UL, 3284921UL, 3308430UL, 3332025UL, 3355705UL, 3379470UL, + 3403321UL, 3427257UL, 3451278UL, 3475385UL, 3499577UL, 3523854UL, 3548217UL, 3572665UL, + 3597198UL, 3621817UL, 3646521UL, 3671310UL, 3696185UL, 3721145UL, 3746190UL, 3771321UL, + 3796537UL, 3821838UL, 3847225UL, 3872697UL, 3898254UL, 3923897UL, 3949625UL, 3975438UL, + 4001337UL, 4027321UL, 4053390UL, 4079545UL, 4105785UL, 4132110UL, 4158521UL, 4185017UL, + 4211598UL, 4238265UL, 4265017UL, 4291854UL, 4318777UL, 4345785UL, 4372878UL, 4400057UL, + 4427321UL, 4454670UL, 4482104UL, 4509622UL, 4537223UL, 4564907UL, 4592673UL, 4620520UL, + 4648448UL, 4676456UL, 4704543UL, 4732709UL, 4760953UL, 4789274UL, 4817672UL, 4846146UL, + 4874695UL, 4903319UL, 4932017UL, 4960788UL, 4989632UL, 5018548UL, 5047535UL, 5076593UL, + 5105721UL, 5134918UL, 5164184UL, 5193518UL, 5222919UL, 5252387UL, 5281921UL, 5311520UL, + 5341184UL, 5370912UL, 5400703UL, 5430557UL, 5460473UL, 5490450UL, 5520488UL, 5550586UL, + 5580743UL, 5610959UL, 5641233UL, 5671564UL, 5701952UL, 5732396UL, 5762895UL, 5793449UL, + 5824057UL, 5854718UL, 5885432UL, 5916198UL, 5947015UL, 5977883UL, 6008801UL, 6039768UL, + 6070784UL, 6101848UL, 6132959UL, 6164117UL, 6195321UL, 6226570UL, 6257864UL, 6289202UL, + 6320583UL, 6352007UL, 6383473UL, 6414980UL, 6446528UL, 6478116UL, 6509743UL, 6541409UL, + 6573113UL, 6604854UL, 6636632UL, 6668446UL, 6700295UL, 6732179UL, 6764097UL, 6796048UL, + 6828032UL, 6860048UL, 6892095UL, 6924173UL, 6956281UL, 6988418UL, 7020584UL, 7052778UL, + 7084999UL, 7117247UL, 7149521UL, 7181820UL, 7214144UL, 7246492UL, 7278863UL, 7311257UL, + 7343673UL, 7376110UL, 7408568UL, 7441046UL, 7473543UL, 7506059UL, 7538593UL, 7571144UL, + 7603712UL, 7636296UL, 7668895UL, 7701509UL, 7734137UL, 7766778UL, 7799432UL, 7832098UL, + 7864775UL, 7897463UL, 7930161UL, 7962868UL, 7995584UL, 8028308UL, 8061039UL, 8093777UL, + 8126521UL, 8159270UL, 8192024UL, 8224782UL, 8257543UL, 8290307UL, 8323073UL, 8355840UL, + 8388608UL }; -static const uint32_t PlsrPlannerSineIntegralQ24[65] = +static const uint32_t PlsrPlannerSineIntegralQ24[PLSR_PLANNER_CURVE_TABLE_SIZE] = { - 0UL, 53UL, 421UL, 1420UL, 3362UL, 6560UL, 11321UL, 17949UL, - 26744UL, 38000UL, 52007UL, 69047UL, 89393UL, 113314UL, 141066UL, - 172899UL, 209052UL, 249753UL, 295221UL, 345662UL, 401269UL, - 462225UL, 528698UL, 600845UL, 678806UL, 762711UL, 852672UL, - 948789UL, 1051146UL, 1159812UL, 1274841UL, 1396271UL, 1524127UL, - 1658415UL, 1799129UL, 1946244UL, 2099722UL, 2259509UL, 2425536UL, - 2597719UL, 2775958UL, 2960141UL, 3150138UL, 3345809UL, 3546997UL, - 3753534UL, 3965237UL, 4181913UL, 4403356UL, 4629347UL, 4859658UL, - 5094050UL, 5332273UL, 5574071UL, 5819175UL, 6067312UL, 6318200UL, - 6571549UL, 6827065UL, 7084448UL, 7343394UL, 7603596UL, 7864741UL, - 8126517UL, 8388608UL + 0UL, 0UL, 1UL, 3UL, 7UL, 13UL, 22UL, 35UL, + 53UL, 75UL, 103UL, 137UL, 178UL, 226UL, 282UL, 347UL, + 421UL, 505UL, 599UL, 705UL, 822UL, 951UL, 1094UL, 1250UL, + 1420UL, 1604UL, 1805UL, 2021UL, 2254UL, 2503UL, 2771UL, 3057UL, + 3362UL, 3687UL, 4032UL, 4398UL, 4785UL, 5194UL, 5626UL, 6081UL, + 6560UL, 7063UL, 7592UL, 8146UL, 8726UL, 9333UL, 9967UL, 10630UL, + 11321UL, 12041UL, 12791UL, 13571UL, 14382UL, 15225UL, 16100UL, 17008UL, + 17949UL, 18923UL, 19933UL, 20977UL, 22057UL, 23173UL, 24325UL, 25516UL, + 26744UL, 28010UL, 29316UL, 30661UL, 32046UL, 33472UL, 34939UL, 36449UL, + 38000UL, 39595UL, 41233UL, 42915UL, 44642UL, 46414UL, 48232UL, 50096UL, + 52007UL, 53966UL, 55972UL, 58027UL, 60131UL, 62284UL, 64487UL, 66742UL, + 69047UL, 71404UL, 73813UL, 76275UL, 78790UL, 81359UL, 83982UL, 86660UL, + 89393UL, 92182UL, 95028UL, 97930UL, 100890UL, 103908UL, 106984UL, 110119UL, + 113314UL, 116568UL, 119882UL, 123258UL, 126695UL, 130194UL, 133755UL, 137379UL, + 141066UL, 144817UL, 148632UL, 152512UL, 156457UL, 160468UL, 164545UL, 168688UL, + 172899UL, 177177UL, 181523UL, 185937UL, 190421UL, 194973UL, 199596UL, 204289UL, + 209052UL, 213886UL, 218792UL, 223770UL, 228820UL, 233943UL, 239140UL, 244409UL, + 249753UL, 255172UL, 260665UL, 266234UL, 271878UL, 277599UL, 283396UL, 289270UL, + 295221UL, 301250UL, 307358UL, 313543UL, 319808UL, 326151UL, 332575UL, 339078UL, + 345662UL, 352326UL, 359072UL, 365899UL, 372808UL, 379799UL, 386873UL, 394029UL, + 401269UL, 408592UL, 416000UL, 423491UL, 431068UL, 438729UL, 446475UL, 454307UL, + 462225UL, 470229UL, 478320UL, 486497UL, 494762UL, 503114UL, 511554UL, 520082UL, + 528698UL, 537403UL, 546197UL, 555080UL, 564053UL, 573116UL, 582268UL, 591511UL, + 600845UL, 610269UL, 619785UL, 629392UL, 639090UL, 648881UL, 658763UL, 668739UL, + 678806UL, 688967UL, 699221UL, 709568UL, 720008UL, 730543UL, 741171UL, 751894UL, + 762711UL, 773623UL, 784629UL, 795731UL, 806928UL, 818220UL, 829608UL, 841092UL, + 852672UL, 864348UL, 876121UL, 887990UL, 899955UL, 912018UL, 924178UL, 936435UL, + 948789UL, 961241UL, 973790UL, 986438UL, 999183UL, 1012026UL, 1024968UL, 1038007UL, + 1051146UL, 1064383UL, 1077718UL, 1091153UL, 1104686UL, 1118319UL, 1132051UL, 1145882UL, + 1159812UL, 1173841UL, 1187971UL, 1202200UL, 1216528UL, 1230956UL, 1245485UL, 1260113UL, + 1274841UL, 1289669UL, 1304597UL, 1319626UL, 1334754UL, 1349983UL, 1365312UL, 1380742UL, + 1396271UL, 1411902UL, 1427632UL, 1443464UL, 1459395UL, 1475428UL, 1491560UL, 1507793UL, + 1524127UL, 1540561UL, 1557096UL, 1573732UL, 1590467UL, 1607304UL, 1624240UL, 1641278UL, + 1658415UL, 1675654UL, 1692992UL, 1710431UL, 1727970UL, 1745610UL, 1763349UL, 1781189UL, + 1799129UL, 1817169UL, 1835309UL, 1853548UL, 1871888UL, 1890328UL, 1908867UL, 1927505UL, + 1946244UL, 1965082UL, 1984019UL, 2003055UL, 2022190UL, 2041425UL, 2060758UL, 2080191UL, + 2099722UL, 2119351UL, 2139080UL, 2158906UL, 2178831UL, 2198854UL, 2218974UL, 2239193UL, + 2259509UL, 2279923UL, 2300434UL, 2321042UL, 2341747UL, 2362550UL, 2383449UL, 2404444UL, + 2425536UL, 2446724UL, 2468008UL, 2489388UL, 2510864UL, 2532435UL, 2554101UL, 2575863UL, + 2597719UL, 2619670UL, 2641715UL, 2663855UL, 2686088UL, 2708416UL, 2730837UL, 2753351UL, + 2775958UL, 2798659UL, 2821451UL, 2844337UL, 2867314UL, 2890384UL, 2913545UL, 2936797UL, + 2960141UL, 2983575UL, 3007100UL, 3030716UL, 3054421UL, 3078216UL, 3102101UL, 3126075UL, + 3150138UL, 3174290UL, 3198530UL, 3222858UL, 3247274UL, 3271777UL, 3296368UL, 3321045UL, + 3345809UL, 3370659UL, 3395595UL, 3420617UL, 3445724UL, 3470915UL, 3496192UL, 3521552UL, + 3546997UL, 3572525UL, 3598137UL, 3623831UL, 3649608UL, 3675467UL, 3701408UL, 3727430UL, + 3753534UL, 3779718UL, 3805983UL, 3832327UL, 3858752UL, 3885255UL, 3911838UL, 3938498UL, + 3965237UL, 3992054UL, 4018948UL, 4045919UL, 4072966UL, 4100090UL, 4127289UL, 4154564UL, + 4181913UL, 4209337UL, 4236836UL, 4264407UL, 4292052UL, 4319770UL, 4347560UL, 4375422UL, + 4403356UL, 4431361UL, 4459436UL, 4487581UL, 4515797UL, 4544081UL, 4572435UL, 4600857UL, + 4629347UL, 4657904UL, 4686529UL, 4715220UL, 4743977UL, 4772800UL, 4801688UL, 4830641UL, + 4859658UL, 4888739UL, 4917883UL, 4947090UL, 4976359UL, 5005690UL, 5035082UL, 5064536UL, + 5094050UL, 5123623UL, 5153256UL, 5182948UL, 5212698UL, 5242506UL, 5272372UL, 5302294UL, + 5332273UL, 5362308UL, 5392398UL, 5422543UL, 5452742UL, 5482995UL, 5513301UL, 5543660UL, + 5574071UL, 5604534UL, 5635047UL, 5665612UL, 5696227UL, 5726891UL, 5757604UL, 5788366UL, + 5819175UL, 5850032UL, 5880936UL, 5911886UL, 5942882UL, 5973923UL, 6005009UL, 6036139UL, + 6067312UL, 6098529UL, 6129787UL, 6161088UL, 6192430UL, 6223813UL, 6255236UL, 6286698UL, + 6318200UL, 6349740UL, 6381317UL, 6412933UL, 6444585UL, 6476273UL, 6507997UL, 6539755UL, + 6571549UL, 6603376UL, 6635236UL, 6667129UL, 6699054UL, 6731011UL, 6762999UL, 6795017UL, + 6827065UL, 6859142UL, 6891247UL, 6923381UL, 6955542UL, 6987730UL, 7019944UL, 7052183UL, + 7084448UL, 7116737UL, 7149050UL, 7181386UL, 7213745UL, 7246126UL, 7278528UL, 7310951UL, + 7343394UL, 7375857UL, 7408339UL, 7440839UL, 7473358UL, 7505893UL, 7538445UL, 7571012UL, + 7603596UL, 7636194UL, 7668806UL, 7701431UL, 7734070UL, 7766721UL, 7799383UL, 7832057UL, + 7864741UL, 7897435UL, 7930138UL, 7962850UL, 7995570UL, 8028297UL, 8061031UL, 8093771UL, + 8126517UL, 8159267UL, 8192022UL, 8224781UL, 8257543UL, 8290307UL, 8323073UL, 8355840UL, + 8388608UL }; static uint32_t PlsrPlannerAbsDifference(uint32_t first, uint32_t second) @@ -294,7 +410,7 @@ static uint64_t PlsrPlannerCurveIntegralQ32(uint64_t progressQ32, } table = (curveMode == 1U) ? PlsrPlannerSmoothIntegralQ24 : PlsrPlannerSineIntegralQ24; - scaled = progressQ32 * 64ULL; + scaled = progressQ32 * PLSR_PLANNER_CURVE_TABLE_INTERVALS; index = (uint32_t)(scaled >> 32U); fraction = (uint32_t)scaled; first = (uint64_t)table[index] << 8U; @@ -365,10 +481,11 @@ static uint32_t PlsrPlannerInstantFrequency( table = (context->block.curveMode == 1U) ? PlsrPlannerSmoothIntegralQ24 : PlsrPlannerSineIntegralQ24; - scaled = progressQ32 * 64ULL; + scaled = progressQ32 * PLSR_PLANNER_CURVE_TABLE_INTERVALS; index = (uint32_t)(scaled >> 32U); curveProgressQ32 = - (uint64_t)(table[index + 1UL] - table[index]) << 14U; + (uint64_t)(table[index + 1UL] - table[index]) + << PLSR_PLANNER_CURVE_DERIVATIVE_SHIFT; } if (context->rampToHz >= context->rampFromHz) {