From f4a22165048e96a8a8bd1d45bd3a5a7ce944293f Mon Sep 17 00:00:00 2001 From: ywh <2227158009@qq.com> Date: Tue, 18 Aug 2026 10:27:31 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8F=98=E9=A2=91=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../bin_to_time_freq.cpython-314.pyc | Bin 16966 -> 18091 bytes HostComputer/bin_to_time_freq.py | 45 +++- PLSR/Src/plsr.c | 211 ++++++++++++++++-- PLSR/Src/plsr_platform.h | 1 + PLSR/Src/plsr_platform_f407.c | 59 +++-- 5 files changed, 270 insertions(+), 46 deletions(-) diff --git a/HostComputer/__pycache__/bin_to_time_freq.cpython-314.pyc b/HostComputer/__pycache__/bin_to_time_freq.cpython-314.pyc index caf358e7f4f08de9e3520a46c8ad5266522d05e5..d31ac38ef3a8d540d8fa4d378a5dd444f8d4af49 100644 GIT binary patch delta 3875 zcmb7HeQaCR6@Sn6d;WUOBK=T2b|5qOy^&mOUk@AY7x+YaH=oMJPK+Fn zr;qWOSW4tZ_hxeW-I-K&Bp(-|GZ(bua&MQMAb2US<#oKCH}FQ@#GCml-ojgX8*k?w zyc2~j)s!t`$QaRgE$vm?SZSEV1`@<^2|;A^8C^t=er&0yS$Req(IMVp4=Y$Dt76py zB+GocHB7L!#AGTWOSt1=NIo+G!Z9?e+o zQeB@3@%1M3nAJ=g&?g?3gw>;eg{;7^Vg*3_)VjlCT&Ln2R3g2p6BUg%3D`Vgt3vPD z7*OHCDm3QKs;YD zSm#}w0J`CJp!e-2Qjh)sySl>QX54CtXlr>y5NtKlI9eO@t9Gzt9l_SHBwGtVH~c&- z*(%Fe%MtN-2b63bThBHOP^^z_3@gi;j^n79z@855)6>KN!8Q%3Sbucm%1#^D0IY0g zTf(w3ZGZ+1!LXt{%b3|#c01d~`dTHKDo)GkI1A_CoE;R~ZY7e6j2$R?&fXzqLwKH5 z!ra;+VLQZ`O5~b2Ggrk~dpWzfRLORVOf?9S>_&0Sh=K;r$k{+j+978*iF8fG?jbst ztp(TW(HW;-7v_|mD{ceYZD$zVi8-4&W*rC9ws7iowA;?WeVSFu8n3=u6mO6EFVJbaE_SZ_qhNO>ZS@rMBYcj&8Xc- zMwA@U^$I~iPj+n}h-Q$ew5%k$e9H(Tx*RWp=%djkRn-HMit&{`E9ImSy6bs@7~D({ zE&|V*E2^L$Ae+TnR3-s$*`;`%m95dQ!1`NO^uz5VIa#}OZ3<>^_8shxlqCFENuE2x=b?(0=&{()NHSTXPl$aEN7F5~0&YSZ)}kHB!iMo!WswJw zt)|oKi7$X)4gq{sAc_Xl1?PklRiCBDB}KjEqIZIsv`mFAxfXT7aoIWns1LIUtt)DeDET|hkHrr=i!9{a$+OTL2&4%|an)Z$Neq=OX2tF5_G|wA- zmqw?|lcOz*#^BlAe_BHy-(6JbmK3%*h3%qcQu;&Z)^6-a>kPt$_T08wN#J^jL zYJB#Yq1qt{#o)|CZ+os#!ZENMo%Gg_8uWs<*)I&kTu5Sg7@#B_N*onNV3gAU_{tsv zETCK7Hu+`DkdVk#_c!{c!m^;S&8X^|NZ%ts7aqlM3PS+{-bgqN;Fk(d;_<`iaKm=; zEp(yb44sC@rdx>(RBHB z(X+mQ6jq=fl?^TU>@b$$u`EkRG>B>33g_M1xK(FbSA(=@s5vCVyP=rFf@T|awPwgw zoU>+5^0l0mNA>7cMQydbl9!EeF5HerMB5B3WCWTBndIBVxei`EGdkJCNT9N<48=|L zEFA%HA1oFRt3e+$?KQ)$^0lXnRB>`t?+=q_QPl5|7W&-iK2MG0(0O?3{Gh21(feF3M_F zNA!Iy_|Y^>t(Q{6>ns^OW=^FS%q2{8%ntu^WA$!G=1$* zPaWuFKqsw?Uko@=N5GCs0Szg7S<9*`=NI1=@OJY$l7U@k6UTHNhq&o0aYOr?og|J- zM>PY1VZ^|w*M;c+h;Hxt==wDW1r~fG^kcXm16&wFco1OEQ2=P2J4d|GmA@X{bq#|m zbfzU#FJ{(iQauJD!Wj&YV|W6?F|^cj=(O+13LP6#P+l7&h3L`gQ7$mL7quvlTm@*q5e`O0I-Sc(^hp{mMZC~k*- z6=d`iSRXDPLJmV7%>}E-0{U}sll&bZWiq zr$^5Zo;&sQsUmu|eOu=owc?z&0- zvV6)iExqEL9=aBJeR#HZ*OyYVW;gi-L27!*kFm6Sj&V==rc9Ru)5h1W*LoM*b}o8% z7QIbn(i_gJ&Y9|K4==X&T;KKUdvDzP_JLV8ve?_d*k08?dpNq-7G3m4Ka-N)0a0rV zZERnn#gaIIjG-;$7wAB!jqb+Z4{|7pHuQs#QVC&!YDe}-agL$up`=beK!{%eVug&z z(bkU56fG9^;f~b))tk|UeZO)&{EFr3i>Ww#LJNj&3|leac$H))Qv6sBe<}J3$ko|N z-+^H+g}Bc9ZRJoE8Qr=4XZM%A@gd{At(Fv5U!%%ZmK?AqVi+0R8Dvnlb*xyQw&!yyoMo*0l!Lj+x;S@ z{RG1l1_6V3_f`(Hd;x)xa{>5#zM<8(Y^T_o%H7VL<)>enO=j{jY_Bj6D-w9A8(jUL zf1_mhTbmp;voZ_5fYu4?MdL+w(llk7?!7L3ujjmV-f-~D{=%Umqd6~oW^du1qQ+R* zQ`8s=duC?CZ<3qu&?>6qHUV(QW~FNX5u&KJJK7d%!L`N{mgq!oD3Oc{@VXGfJledYzUe9+&SGP6@G8%B`AG1wlt>GI z1iG4>d2+{PQeDE*A7Z$HHuh`{;**L3B}FWj=2P)lOn}Fa5JDKlPa;J({3jhr#vkM97%&lM7P&O_}1UJ-4NV%<Ax3wYw-X8 delta 3029 zcma)7eN0=|6@Skk&jvq#e_=jt2Mopr8=GJkNJvH+np9KGweqO4#Ew-1p@G;uI|-y| zAn6zF)P(MAQx$2cqcsv$yEcauE|Lw>3e=5y>#N3vt ztWP9T>Hfq-@Ia0ywQj(sJfgFx!{Pq797=Du`1LCla2a1N#s1L z3E;q!r-3@;$h=N4=bWQl7vCXlV!#hsX%tL z4K+rjs0ld&6?uE7a10V!920-Re(@clA{1(lRHBwc+Y2mmqE>9jh1>y2o{KQpURyw( zUnO*?9rYp)YVwE@dQQoy{37HvQ)pbCl;foVFFVW>UR4kwr{=Vrj?;4n&ghpTpCGBk zS~T(tbH%2XhBI1f{~`TD|7PE;dFFrd~5>Lj(VDfc8A-V6HR&X=*Wp;Q#tX-*l3EydmYxo z11~tdvhQLl^%VtQ`|4rcv6OwkxrKHd!z4dJV4Ogb08b!+!71iXlKCur)v}X*4|<&E zm>|B?y>j@x*+Q$}w$s^WKy>Lf9R49i)ly*t5|y%~iSW9x0er14+5nMOzu15zaHQ2D zX2L31?lZtITbr@PSc~C1u4Y;dySzRIQQIgi#hnJCjmkD1ZXVLoc9WD1O$Ohis3AVs zN>T2T_$B4=zRPBo7gE)Xk3pW%J;XcMFq`0xS4rz&a<>tF=2ftW3G0!}fmOo7x8?b7 zhfG4DU2*$$LsO*(3Trqy{KXxhFM+kqCY}#j;rEU8qM;1#8wU7$BMY@IxhScC3vDi! zqToj1on(hTDz%2>!ETCroQ@B`pV~Z6Zn@2|rCM9Nm0@kjC7h=WhMq6gZl|%2N;uMP z5$kYU!41wpom{46~@mCmt*OD0ld>9NG|{0%haZ{S(7 zbB4e)fzudrqWBaZ(>ZB+DmszMNhV^cV>wY0lga6vBtDi(=jdbn2+^MQzs6dZS?daG2X&{p%6PrzM*L>yC(kYg2XD2YTiWoN_Q10C0PN~KJR`kY`R190 z_%;8J#umP{!agQgu3ctpm+AtG+pl-uNZ;)LWO}K4aJ6o5g++px7|%3aZM`P?iEOcR z)zi0X?_0M!GO4T11^$EQ-g)lb=NAVteZ&5#>tok1+Ll6ttKUGY9<;~R$~HSKVYd>+_mU;aP);&E5t~Zk-b*>a9qeTO zh-ax$7zrNGm4)*+to^Pmdn@=P&E&rYjosg$A>YXr<^9f z9!vNJ0=NND`~p#|NX1SLN0YJiQJyq{oH#k2=KF~FJb{-9WC*-X;2i?91RnDH2ZXy! z!1@-!hsw?27}+X0nCYoje}b*`ozm%Ay4PSbtQttzmOWJy$!tljGnwDB0FcRsudlZHZmXgY| zH9L<f z(&Iht#8aLqx0zcm+Z4L?V7-e0V2+2gG}$wtXU6eeY?BiQci{ eqa^Iy=@qf%w-Wiwd+&-VN%cQB`r(DWE&l~W`j5H* diff --git a/HostComputer/bin_to_time_freq.py b/HostComputer/bin_to_time_freq.py index b3424e2..4f26e4f 100644 --- a/HostComputer/bin_to_time_freq.py +++ b/HostComputer/bin_to_time_freq.py @@ -111,8 +111,12 @@ def plot_show(path, fs, thresh, ymax, save_path=None): fig, ax = plt.subplots(figsize=(15, 7)) fig.subplots_adjust(bottom=0.10, top=0.92) - # 全脉冲散点 - ax.plot(t * 1000, freq, '.', ms=1.5, color='C0', alpha=0.5, zorder=1) + # 全脉冲散点：复用 artist，重绘时按可见范围抽稀（大文件流畅） + MAX_VISIBLE_POINTS = 5000 # 可见范围内最多绘制的点数，超过则等间隔抽稀 + (scatter,) = ax.plot([], [], '.', ms=1.5, color='C0', alpha=0.5, 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）" % ( @@ -145,6 +149,19 @@ def plot_show(path, fs, thresh, ymax, save_path=None): zorder=6, visible=False) hover_last = {'idx': -1, 'visible': False} + # 可见范围内抽稀绘制散点（大文件性能优化） + def update_scatter(): + x0, x1 = ax.get_xlim() + mask = (x_full >= x0) & (x_full <= x1) + n_vis = int(np.count_nonzero(mask)) + if n_vis > MAX_VISIBLE_POINTS: + # 等间隔抽稀：取 n_vis 中的 MAX_VISIBLE_POINTS 个 + step = (n_vis + MAX_VISIBLE_POINTS - 1) // MAX_VISIBLE_POINTS + idx = np.flatnonzero(mask)[::step] + else: + idx = np.flatnonzero(mask) + scatter.set_data(x_full[idx], y_full[idx]) + def on_scroll(event): if event.inaxes is not ax or event.xdata is None: return @@ -167,6 +184,7 @@ def plot_show(path, fs, thresh, ymax, save_path=None): if n1 - n0 < 1e-6: return ax.set_xlim(n0, n1) + update_scatter() fig.canvas.draw_idle() def on_press(event): @@ -187,28 +205,29 @@ def plot_show(path, fs, thresh, ymax, save_path=None): dy = event.ydata - state['press_y'] ax.set_xlim(x0 - dx, x1 - dx) ax.set_ylim(y0 - dy, y1 - dy) + update_scatter() fig.canvas.draw_idle() return # 悬停：找可见范围内距鼠标最近的脉冲点（屏幕像素距离 < 20px 才高亮） x0, x1 = ax.get_xlim() - tx_ms = t * 1000 - mask = (tx_ms >= x0) & (tx_ms <= x1) + mask = (x_full >= x0) & (x_full <= x1) if not np.any(mask): - hl_marker.set_visible(False) - hl_text.set_visible(False) + if hover_last['visible']: + hl_marker.set_visible(False) + hl_text.set_visible(False) + fig.canvas.draw_idle() hover_last['visible'] = False - fig.canvas.draw_idle() return - px, py = ax.transData.transform(np.column_stack([tx_ms[mask], freq[mask]])).T + px, py = ax.transData.transform(np.column_stack([x_full[mask], y_full[mask]])).T dist = np.hypot(px - event.x, py - event.y) k = int(np.argmin(dist)) if dist[k] <= 20.0: idx = int(np.flatnonzero(mask)[k]) - hl_marker.set_data([tx_ms[idx]], [freq[idx]]) + hl_marker.set_data([x_full[idx]], [y_full[idx]]) hl_text.set_text("脉冲 #%d\nt = %.3f ms\nf = %.0f Hz" - % (idx + 1, tx_ms[idx], freq[idx])) - hl_text.set_position((tx_ms[idx] + (x1 - x0) * 0.01, - freq[idx] + (ax.get_ylim()[1] - ax.get_ylim()[0]) * 0.02)) + % (idx + 1, x_full[idx], y_full[idx])) + hl_text.set_position((x_full[idx] + (x1 - x0) * 0.01, + y_full[idx] + (ax.get_ylim()[1] - ax.get_ylim()[0]) * 0.02)) hl_marker.set_visible(True) hl_text.set_visible(True) if hover_last['idx'] != idx or not hover_last['visible']: @@ -229,8 +248,10 @@ def plot_show(path, fs, thresh, ymax, save_path=None): if event.dblclick: 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) + update_scatter() fig.canvas.draw_idle() + update_scatter() fig.canvas.mpl_connect('scroll_event', on_scroll) fig.canvas.mpl_connect('button_press_event', on_press) fig.canvas.mpl_connect('button_release_event', on_release) diff --git a/PLSR/Src/plsr.c b/PLSR/Src/plsr.c index 54fd3b9..00ffa36 100644 --- a/PLSR/Src/plsr.c +++ b/PLSR/Src/plsr.c @@ -73,6 +73,9 @@ #define PLSR_REPLAN_PREFIX_ITEMS (512U) #define PLSR_REPLAN_PREFIX_TIME_US (5000ULL) #define PLSR_REPLAN_BUILD_ITEMS (400U) +/* Cap equal-frequency counted runs so an online update never waits behind an + arbitrarily long steady run. At low speed one complete pulse is the limit. */ +#define PLSR_COUNTED_RUN_MAX_TIME_US (2000ULL) #define PLSR_BOUNDARY_EVENT_CAPACITY (2U) #if ((PLSR_BOUNDARY_EVENT_CAPACITY == 0U) \ || ((PLSR_BOUNDARY_EVENT_CAPACITY \ @@ -171,6 +174,7 @@ typedef struct typedef struct { uint64_t magnitude; + uint64_t commandMagnitude; uint32_t firstFrequencyHz; uint32_t secondFrequencyHz; uint32_t firstRequestedFrequencyHz; @@ -191,6 +195,7 @@ typedef struct typedef struct { uint64_t magnitude; + uint64_t commandMagnitude; uint32_t firstFrequencyHz; uint32_t completedPulses; uint8_t nextSegment; @@ -269,6 +274,9 @@ static PLSR_CONFIG PlsrActiveConfig; static PLSR_CONFIG PlsrCandidateConfig; static volatile int32_t PlsrPosition; static volatile uint64_t PlsrRemainingPulses; +/* Full configured displacement still available in the active segment. ACT + may publish a shorter execution profile without discarding this ceiling. */ +static volatile uint64_t PlsrCommandRemainingPulses; static volatile uint8_t PlsrPulseActive; static volatile uint8_t PlsrCutRequested; static volatile uint8_t PlsrActTimedCutPlanned; @@ -424,10 +432,15 @@ static uint8_t PlsrShortProfileTakeRunLimited( PLSR_SHORT_PROFILE *profile, PLSR_PROFILE_ENTRY *entry, uint32_t maximumRepeats); +static uint32_t PlsrProfileRunLimit(uint32_t frequencyHz); static uint8_t PlsrLimitProfileToActTime( PLSR_SHORT_PROFILE *profile, uint16_t actTimeMs, uint8_t *timedCutPlanned); +static uint8_t PlsrLimitProfileToActTimeUs( + PLSR_SHORT_PROFILE *profile, + uint32_t actTimeUs, + uint8_t *timedCutPlanned); static void PlsrProfileRecordPlannerStatus( const PLSR_SHORT_PROFILE *profile); static PLSR_PLATFORM_SERVICE_RESULT PlsrReplanPulseDir( @@ -1894,9 +1907,9 @@ static uint8_t PlsrPrepareShortProfile(PLSR_SHORT_PROFILE *profile, block.exitHz = 0UL; block.pulseBudget = (uint32_t)pulseCount; block.referenceSpeedHz = PlsrActiveConfig.defaultSpeedHz; - block.accelerationTimeMs = PlsrActiveConfig.accelerationTimeMs; - block.decelerationTimeMs = PlsrActiveConfig.decelerationTimeMs; - block.curveMode = PlsrActiveConfig.curveMode; + block.accelerationTimeMs = frequencyConfig->accelerationTimeMs; + block.decelerationTimeMs = frequencyConfig->decelerationTimeMs; + block.curveMode = frequencyConfig->curveMode; block.pulseOutput = (uint8_t)PlsrActiveConfig.pulseOutput; block.boundary = PLSR_BOUNDARY_STOP; @@ -1916,7 +1929,9 @@ static uint8_t PlsrPrepareShortProfile(PLSR_SHORT_PROFILE *profile, } else if (nextSegment == 0U) { - block.exitHz = PlsrEffectiveStopFrequency(targetFrequencyHz); + block.exitHz = (frequencyConfig->decelerationTimeMs == 0U) + ? targetFrequencyHz + : PlsrActiveConfig.stopSpeedHz; } else if (segment->waitType == PLSR_EXT_OR_COMPLETE) { @@ -1956,6 +1971,27 @@ static uint8_t PlsrShortProfileTakeRun(PLSR_SHORT_PROFILE *profile, return PlsrShortProfileTakeRunLimited(profile, entry, 0xFFFFFFFFUL); } +static uint32_t PlsrProfileRunLimit(uint32_t frequencyHz) +{ + uint64_t limit; + + if (PlsrActiveConfig.outputMode != PLSR_OUTPUT_PULSE_DIR) + { + return 0xFFFFFFFFUL; + } + if (frequencyHz == 0UL) + { + return 1UL; + } + limit = ((uint64_t)frequencyHz * PLSR_COUNTED_RUN_MAX_TIME_US + + 999999ULL) / 1000000ULL; + if (limit == 0ULL) + { + return 1UL; + } + return (limit > 0xFFFFFFFFULL) ? 0xFFFFFFFFUL : (uint32_t)limit; +} + static uint8_t PlsrShortProfileTakeRunLimited( PLSR_SHORT_PROFILE *profile, PLSR_PROFILE_ENTRY *entry, @@ -1978,6 +2014,15 @@ static uint8_t PlsrShortProfileTakeRunLimited( } profile->pendingRepeats = profile->pendingItem.repeatCount; } + { + uint32_t runLimit = PlsrProfileRunLimit( + profile->pendingItem.setting.actualFrequencyHz); + + if (maximumRepeats > runLimit) + { + maximumRepeats = runLimit; + } + } repeatCount = profile->pendingRepeats; if (repeatCount > profile->pulseCount - profile->nextPeriod) { @@ -2017,6 +2062,15 @@ static uint8_t PlsrLimitProfileToActTime( PLSR_SHORT_PROFILE *profile, uint16_t actTimeMs, uint8_t *timedCutPlanned) +{ + return PlsrLimitProfileToActTimeUs( + profile, (uint32_t)actTimeMs * 1000UL, timedCutPlanned); +} + +static uint8_t PlsrLimitProfileToActTimeUs( + PLSR_SHORT_PROFILE *profile, + uint32_t actTimeUs, + uint8_t *timedCutPlanned) { PLSR_PLANNER_TIME_PREDICTION prediction; @@ -2025,8 +2079,7 @@ static uint8_t PlsrLimitProfileToActTime( return 0U; } *timedCutPlanned = 0U; - if (PlsrPlannerPredictTime(&profile->planner, - (uint32_t)actTimeMs * 1000UL, + if (PlsrPlannerPredictTime(&profile->planner, actTimeUs, &prediction) == 0U) { return 0U; @@ -2062,8 +2115,11 @@ static PLSR_PLATFORM_SERVICE_RESULT PlsrReplanPulseDir( PLSR_SHORT_PROFILE replacement; PLSR_SHORT_PROFILE candidate; PLSR_PROFILE_ENTRY entry; + uint64_t committedTimeUs; uint64_t prefixTimeUs = 0ULL; + uint64_t prefixTimeLimitUs = PLSR_REPLAN_PREFIX_TIME_US; uint64_t prefixPulses = 0ULL; + uint64_t remainingActTimeUs = 0ULL; uint32_t committedPulses; uint32_t tailFrequencyHz; uint32_t sourceGeneration; @@ -2078,6 +2134,7 @@ static PLSR_PLATFORM_SERVICE_RESULT PlsrReplanPulseDir( uint32_t sourceIndex; uint32_t criticalState; uint32_t epoch; + uint32_t segmentElapsedMs; uint8_t segmentNumber; uint8_t handoffBank; uint8_t sourceBank; @@ -2085,6 +2142,8 @@ static PLSR_PLATFORM_SERVICE_RESULT PlsrReplanPulseDir( uint8_t boundaryWriteIndex; uint8_t pipelineStartsNextSegment = 0U; uint8_t tailWasClipped = 0U; + uint8_t actTimeLimited = 0U; + uint8_t actTimedCutPlanned = 0U; uint16_t fillBudget = PLSR_REPLAN_BUILD_ITEMS; criticalState = PlsrPlatformEnterCritical(); @@ -2097,6 +2156,7 @@ static PLSR_PLATFORM_SERVICE_RESULT PlsrReplanPulseDir( snapshotWriteIndex = source->writeIndex; epoch = PlsrSegmentEpoch; segmentNumber = PlsrCurrentSegment; + segmentElapsedMs = PlsrSegmentElapsedMs; handoffBank = PlsrPreparedHandoffBank; boundaryWriteIndex = PlsrCountedBoundaryWriteIndex; if ((PlsrPulseActive == 0U) || (segmentNumber == 0U) @@ -2111,9 +2171,23 @@ static PLSR_PLATFORM_SERVICE_RESULT PlsrReplanPulseDir( PlsrPlatformExitCritical(criticalState); return PLSR_PLATFORM_SERVICE_DEFERRED; } + if ((forceTerminalExit == 0U) + && (PlsrActiveConfig.segments[segmentNumber - 1U].waitType + == PLSR_ACT_TIME)) + { + uint32_t actTimeMs = + PlsrActiveConfig.segments[segmentNumber - 1U].actTimeMs; + + actTimeLimited = 1U; + if (actTimeMs > segmentElapsedMs) + { + remainingActTimeUs = + (uint64_t)(actTimeMs - segmentElapsedMs) * 1000ULL; + } + } if (PlsrPlatformFinitePipelineSnapshot( (uint8_t)PlsrActiveConfig.pulseOutput, - &committedPulses, &tailFrequencyHz, + &committedPulses, &committedTimeUs, &tailFrequencyHz, &pipelineStartsNextSegment) == 0U) { PlsrPlatformExitCritical(criticalState); @@ -2133,6 +2207,19 @@ static PLSR_PLATFORM_SERVICE_RESULT PlsrReplanPulseDir( totalPulses = committedPulses; } availablePlanPulses = totalPulses - committedPulses; + if (actTimeLimited != 0U) + { + if (committedTimeUs >= remainingActTimeUs) + { + prefixTimeLimitUs = 0ULL; + actTimedCutPlanned = 1U; + } + else if ((remainingActTimeUs - committedTimeUs) + < prefixTimeLimitUs) + { + prefixTimeLimitUs = remainingActTimeUs - committedTimeUs; + } + } destination->readIndex = 0UL; destination->writeIndex = 0UL; @@ -2150,7 +2237,7 @@ static PLSR_PLATFORM_SERVICE_RESULT PlsrReplanPulseDir( while ((sourceIndex != snapshotWriteIndex) && (prefixCount < PLSR_REPLAN_PREFIX_ITEMS) && (prefixPulses < availablePlanPulses) - && (prefixTimeUs < PLSR_REPLAN_PREFIX_TIME_US)) + && (prefixTimeUs < prefixTimeLimitUs)) { uint32_t remainingPrefixPulses = availablePlanPulses - (uint32_t)prefixPulses; @@ -2170,7 +2257,7 @@ static PLSR_PLATFORM_SERVICE_RESULT PlsrReplanPulseDir( return PLSR_PLATFORM_SERVICE_FAILED; } remainingPrefixTimeUs = - PLSR_REPLAN_PREFIX_TIME_US - prefixTimeUs; + prefixTimeLimitUs - prefixTimeUs; timeLimitedPulses = (remainingPrefixTimeUs * frequencyHz + 999999ULL) / 1000000ULL; if (timeLimitedPulses == 0ULL) @@ -2198,6 +2285,13 @@ static PLSR_PLATFORM_SERVICE_RESULT PlsrReplanPulseDir( } replacementPulses = availablePlanPulses - (uint32_t)prefixPulses; + if ((actTimeLimited != 0U) + && ((committedTimeUs + prefixTimeUs) >= remainingActTimeUs)) + { + replacementPulses = 0UL; + totalPulses = committedPulses + (uint32_t)prefixPulses; + actTimedCutPlanned = 1U; + } replacementStartIndex = prefixCount; if (replacementPulses != 0UL) { @@ -2211,6 +2305,24 @@ static PLSR_PLATFORM_SERVICE_RESULT PlsrReplanPulseDir( { return PLSR_PLATFORM_SERVICE_FAILED; } + if (actTimeLimited != 0U) + { + uint64_t consumedTimeUs = committedTimeUs + prefixTimeUs; + uint64_t replacementTimeUs = + (remainingActTimeUs > consumedTimeUs) + ? remainingActTimeUs - consumedTimeUs : 0ULL; + + if ((replacementTimeUs > 0xFFFFFFFFULL) + || (PlsrLimitProfileToActTimeUs( + &replacement, (uint32_t)replacementTimeUs, + &actTimedCutPlanned) == 0U)) + { + return PLSR_PLATFORM_SERVICE_FAILED; + } + replacementPulses = replacement.pulseCount; + totalPulses = committedPulses + (uint32_t)prefixPulses + + replacementPulses; + } PlsrCopyShortProfile(&destination->producerProfile, &replacement); destination->active = 1U; candidate = replacement; @@ -2229,6 +2341,11 @@ static PLSR_PLATFORM_SERVICE_RESULT PlsrReplanPulseDir( && (PlsrTimerSettingsEqual( &destination->entries[prefixCount - 1UL].setting, &entry.setting) != 0U) + && (entry.repeatCount <= PlsrProfileRunLimit( + entry.setting.actualFrequencyHz)) + && (destination->entries[prefixCount - 1UL].repeatCount + <= (PlsrProfileRunLimit(entry.setting.actualFrequencyHz) + - entry.repeatCount)) && (destination->entries[prefixCount - 1UL].repeatCount <= (0xFFFFFFFFUL - entry.repeatCount))) { @@ -2278,7 +2395,7 @@ static PLSR_PLATFORM_SERVICE_RESULT PlsrReplanPulseDir( PlsrRamp.active = 0U; PlsrHandoffPlan.valid = 0U; PlsrCountedHandoffStaged = 0U; - PlsrActTimedCutPlanned = 0U; + PlsrActTimedCutPlanned = actTimedCutPlanned; PlsrProfileQueueBank = destinationBank; PlsrCopyShortProfile(&PlsrShortProfile, &destination->producerProfile); @@ -2489,6 +2606,12 @@ static uint8_t PlsrProfileQueueAppendLocked( && (entry->startsNextSegment == 0U) && (PlsrTimerSettingsEqual( &previous->setting, &entry->setting) != 0U) + && (entry->repeatCount <= PlsrProfileRunLimit( + previous->setting.actualFrequencyHz)) + && (previous->repeatCount + <= (PlsrProfileRunLimit( + previous->setting.actualFrequencyHz) + - entry->repeatCount)) && (previous->repeatCount <= (0xFFFFFFFFUL - entry->repeatCount))) { @@ -3476,6 +3599,10 @@ static PLSR_PLATFORM_SERVICE_RESULT PlsrTryStartPreparedTimedSegment( PlsrDirectionDelayActive = 0U; PlsrDirectionDelayRemainingMs = 0U; PlsrRemainingPulses = magnitude; + PlsrCommandRemainingPulses = + (resolvedDisplacement < 0) + ? (uint64_t)(-resolvedDisplacement) + : (uint64_t)resolvedDisplacement; PlsrCountPositive = positive; PlsrActTimedCutPlanned = PlsrTimedStart.nextActTimedCut; PlsrTimedStart.pendingActivation = 1U; @@ -3782,6 +3909,7 @@ static uint8_t PlsrStartSegment(uint8_t segmentNumber, criticalState = PlsrPlatformEnterCritical(); PlsrRemainingPulses = magnitude; + PlsrCommandRemainingPulses = magnitude; PlsrCountPositive = positive; PlsrPlatformExitCritical(criticalState); @@ -3890,6 +4018,7 @@ static void PlsrFinishCompleted(void) PlsrDisarmWaitSignal(); PlsrPlatformStopPulse((uint8_t)PlsrActiveConfig.pulseOutput); PlsrRemainingPulses = 0UL; + PlsrCommandRemainingPulses = 0ULL; PlsrPulseActive = 0U; PlsrCutRequested = 0U; PlsrActTimedCutPlanned = 0U; @@ -3931,6 +4060,7 @@ static void PlsrFinishStopped(void) PlsrDiagnosticFinishSegment(0U); PlsrPlatformStopPulse((uint8_t)PlsrActiveConfig.pulseOutput); PlsrRemainingPulses = 0UL; + PlsrCommandRemainingPulses = 0ULL; PlsrPulseActive = 0U; PlsrCutRequested = 0U; PlsrActTimedCutPlanned = 0U; @@ -3972,6 +4102,7 @@ static void PlsrEnterError(PLSR_ERROR error) PlsrDiagnosticFinishSegment(0U); PlsrPlatformStopPulse((uint8_t)PlsrActiveConfig.pulseOutput); PlsrRemainingPulses = 0UL; + PlsrCommandRemainingPulses = 0ULL; PlsrPulseActive = 0U; PlsrCutRequested = 0U; PlsrActTimedCutPlanned = 0U; @@ -4123,6 +4254,7 @@ static uint8_t PlsrBuildHandoffPlan(uint8_t sourceSegment, plan->magnitude = (displacement < 0) ? (uint64_t)(-displacement) : (uint64_t)displacement; + plan->commandMagnitude = plan->magnitude; if (carryFrequencyHz == 0UL) { carryFrequencyHz = @@ -4419,6 +4551,14 @@ static uint8_t PlsrSyncCountedProgress(void) PlsrPosition = (int32_t)nextPosition; } PlsrRemainingPulses = remaining - delta; + if (delta >= PlsrCommandRemainingPulses) + { + PlsrCommandRemainingPulses = 0ULL; + } + else + { + PlsrCommandRemainingPulses -= delta; + } PlsrCountedObservedPublished = observed; PlsrPositionCheckpointDirty = 1U; return 1U; @@ -4569,6 +4709,7 @@ void PlsrExecCountedSegmentBoundaryIrq(uint8_t pulseOutput, event = &PlsrCountedBoundaryEvents[ writeIndex & (PLSR_BOUNDARY_EVENT_CAPACITY - 1U)]; event->magnitude = PlsrHandoffPlan.magnitude; + event->commandMagnitude = PlsrHandoffPlan.commandMagnitude; event->firstFrequencyHz = PlsrHandoffPlan.firstFrequencyHz; event->completedPulses = completedPulses; event->nextSegment = PlsrHandoffPlan.nextSegment; @@ -4701,6 +4842,7 @@ static void PlsrExecServiceCountedBoundaryEvent(void) PlsrSegmentEpoch++; PlsrCurrentSegment = nextSegment; PlsrRemainingPulses = event.magnitude; + PlsrCommandRemainingPulses = event.commandMagnitude; PlsrCountPositive = event.positive; PlsrCurrentFrequencyHz = event.firstFrequencyHz; PlsrBoundaryFrequencyHz = event.firstFrequencyHz; @@ -4938,6 +5080,7 @@ static PLSR_PLATFORM_QUEUE_RESULT PlsrTryContinuousHandoff(void) PlsrSegmentEpoch); PlsrCurrentSegment = nextSegment; PlsrRemainingPulses = magnitude; + PlsrCommandRemainingPulses = PlsrHandoffPlan.commandMagnitude; PlsrCountPositive = positive; PlsrActTimedCutPlanned = PlsrHandoffPlan.nextActTimedCut; PlsrBoundaryFrequencyHz = PlsrCurrentFrequencyHz; @@ -5426,6 +5569,7 @@ uint8_t PlsrInit(void) sizeof(PlsrPreparedHandoffPlans)); PlsrPreparedHandoffBank = 0U; PlsrRemainingPulses = 0UL; + PlsrCommandRemainingPulses = 0ULL; PlsrPulseActive = 0U; PlsrCutRequested = 0U; PlsrActTimedCutPlanned = 0U; @@ -5644,6 +5788,7 @@ static uint8_t PlsrExecuteStop(void) drainPulses = 1ULL; } PlsrRemainingPulses = drainPulses; + PlsrCommandRemainingPulses = drainPulses; PlsrPlatformExitCritical(criticalState); if (PlsrReplanPulseDir(stopTargetHz, (uint32_t)drainPulses, 1U) @@ -5683,6 +5828,7 @@ static void PlsrExecuteClear(void) PlsrPosition = 0L; PlsrPositionValid = 1U; PlsrRemainingPulses = 0UL; + PlsrCommandRemainingPulses = 0ULL; PlsrPlatformExitCritical(criticalState); PlsrPlatformCheckpointPosition(0L, 1U, 0U); PlsrCountOverflowPending = 0U; @@ -6031,7 +6177,9 @@ void PlsrPoll1ms(void) PlsrBoundaryRampStarted = 0U; if (PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR) { - uint64_t remaining = PlsrRemainingPulses; + uint64_t remaining = + (activeSegment->waitType == PLSR_ACT_TIME) + ? PlsrCommandRemainingPulses : PlsrRemainingPulses; if (PlsrCountedHandoffStaged != 0U) { @@ -6039,6 +6187,12 @@ void PlsrPoll1ms(void) Leave both untouched; the boundary consumer retargets this pending update to the segment that hardware enters. */ } + else if ((activeSegment->waitType == PLSR_ACT_TIME) + && (PlsrSegmentElapsedMs >= activeSegment->actTimeMs)) + { + /* The normal deadline path below owns the full-pulse cut. */ + PlsrFrequencyUpdatePending = 0U; + } else if ((remaining == 0ULL) || (remaining > 0xFFFFFFFFULL)) { PlsrFrequencyUpdatePending = 0U; @@ -6235,6 +6389,10 @@ void PlsrPulseTimerIrq(uint8_t pulseOutput) { remainingPulses--; PlsrRemainingPulses = remainingPulses; + if (PlsrCommandRemainingPulses != 0ULL) + { + PlsrCommandRemainingPulses--; + } } if ((remainingPulses == 0UL) || (PlsrCutRequested != 0U) @@ -6467,6 +6625,7 @@ PLSR_MB_RESULT PlsrModbusWriteHolding(uint16_t startAddress, uint32_t criticalState; uint8_t handoffBank = 0U; uint8_t updateActiveFrequencies = 0U; + uint8_t updateActivePlannerParameters = 0U; uint8_t drainedToDifferentSegment = 0U; uint8_t segmentBeforeDrain; uint32_t drainedSegmentTargetHz = 0UL; @@ -6622,8 +6781,19 @@ PLSR_MB_RESULT PlsrModbusWriteHolding(uint16_t startAddress, updateActiveFrequencies = 1U; } } + if ((PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR) + && ((PlsrCandidateConfig.accelerationTimeMs + != PlsrShadowConfig.accelerationTimeMs) + || (PlsrCandidateConfig.decelerationTimeMs + != PlsrShadowConfig.decelerationTimeMs) + || (PlsrCandidateConfig.curveMode + != PlsrShadowConfig.curveMode))) + { + updateActivePlannerParameters = 1U; + } } - if (updateActiveFrequencies != 0U) + if ((updateActiveFrequencies != 0U) + || (updateActivePlannerParameters != 0U)) { handoffBank = PlsrBuildHandoffPlanBank(&PlsrCandidateConfig); } @@ -6646,9 +6816,18 @@ PLSR_MB_RESULT PlsrModbusWriteHolding(uint16_t startAddress, } } PlsrShadowConfig = PlsrCandidateConfig; - if (updateActiveFrequencies != 0U) + if ((updateActiveFrequencies != 0U) + || (updateActivePlannerParameters != 0U)) { PlsrInvalidateTimedStartLocked(); + if (updateActivePlannerParameters != 0U) + { + PlsrActiveConfig.accelerationTimeMs = + PlsrCandidateConfig.accelerationTimeMs; + PlsrActiveConfig.decelerationTimeMs = + PlsrCandidateConfig.decelerationTimeMs; + PlsrActiveConfig.curveMode = PlsrCandidateConfig.curveMode; + } for (index = 0U; index < PlsrActiveConfig.segmentCount; index++) { uint8_t frequencyChanged = @@ -6672,7 +6851,8 @@ PLSR_MB_RESULT PlsrModbusWriteHolding(uint16_t startAddress, } } PlsrPreparedHandoffBank = handoffBank; - if (PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR) + if ((updateActiveFrequencies != 0U) + && (PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR)) { /* Publish the new prepared-plan bank without touching the stream or handoff metadata already visible to the counter IRQ. A @@ -6694,7 +6874,7 @@ PLSR_MB_RESULT PlsrModbusWriteHolding(uint16_t startAddress, PlsrFrequencyUpdatePending = 1U; } } - else + else if (updateActiveFrequencies != 0U) { PlsrHandoffPlan.valid = 0U; PlsrProfileQueueInvalidateGeneration(); @@ -6781,6 +6961,7 @@ void PlsrTestSetPosition(int32_t position, uint8_t positionValid) PlsrPosition = position; PlsrPositionValid = (positionValid != 0U) ? 1U : 0U; PlsrRemainingPulses = 0UL; + PlsrCommandRemainingPulses = 0ULL; PlsrPositionCheckpointDirty = 0U; PlsrPlatformExitCritical(criticalState); PlsrPlatformCheckpointPosition(position, PlsrPositionValid, 0U); diff --git a/PLSR/Src/plsr_platform.h b/PLSR/Src/plsr_platform.h index 8a93a8f..84b1d77 100644 --- a/PLSR/Src/plsr_platform.h +++ b/PLSR/Src/plsr_platform.h @@ -94,6 +94,7 @@ uint8_t PlsrPlatformFiniteRetargetReady(uint8_t pulseOutput, uint32_t *activeFrequencyHz); uint8_t PlsrPlatformFinitePipelineSnapshot(uint8_t pulseOutput, uint32_t *committedPulses, + uint64_t *committedTimeUs, uint32_t *tailFrequencyHz, uint8_t *startsNextSegment); uint8_t PlsrPlatformFiniteProgress(uint8_t pulseOutput, diff --git a/PLSR/Src/plsr_platform_f407.c b/PLSR/Src/plsr_platform_f407.c index a5d4944..e3d515a 100644 --- a/PLSR/Src/plsr_platform_f407.c +++ b/PLSR/Src/plsr_platform_f407.c @@ -552,21 +552,31 @@ uint8_t PlsrPlatformFiniteRetargetReady(uint8_t pulseOutput, uint8_t PlsrPlatformFinitePipelineSnapshot(uint8_t pulseOutput, uint32_t *committedPulses, + uint64_t *committedTimeUs, uint32_t *tailFrequencyHz, uint8_t *startsNextSegment) { uint32_t remaining; + uint32_t frequencyHz; if ((pulseOutput > 3U) || (committedPulses == NULL) - || (tailFrequencyHz == NULL) || (startsNextSegment == NULL) + || (committedTimeUs == NULL) || (tailFrequencyHz == NULL) + || (startsNextSegment == NULL) || (PlsrHostFiniteActive[pulseOutput] == 0U)) { return 0U; } remaining = PlsrHostFiniteTarget[pulseOutput] - PlsrHostFiniteEmitted[pulseOutput]; + frequencyHz = PlsrHostFrequency[pulseOutput]; + if (frequencyHz == 0UL) + { + return 0U; + } *committedPulses = remaining; - *tailFrequencyHz = PlsrHostQueuedFrequency[pulseOutput]; + *committedTimeUs = ((uint64_t)remaining * 1000000ULL + + frequencyHz - 1UL) / frequencyHz; + *tailFrequencyHz = frequencyHz; *startsNextSegment = 0U; return 1U; } @@ -3604,16 +3614,19 @@ uint8_t PlsrPlatformFiniteRetargetReady(uint8_t pulseOutput, uint8_t PlsrPlatformFinitePipelineSnapshot(uint8_t pulseOutput, uint32_t *committedPulses, + uint64_t *committedTimeUs, uint32_t *tailFrequencyHz, uint8_t *startsNextSegment) { - TIM_TypeDef *counter; uint32_t criticalState; - uint32_t blockCount; + uint32_t completed; + uint32_t frequencyHz; uint32_t remaining; + uint64_t timeUs; if ((pulseOutput > 3U) || (committedPulses == NULL) - || (tailFrequencyHz == NULL) || (startsNextSegment == NULL)) + || (committedTimeUs == NULL) || (tailFrequencyHz == NULL) + || (startsNextSegment == NULL)) { return 0U; } @@ -3625,23 +3638,22 @@ uint8_t PlsrPlatformFinitePipelineSnapshot(uint8_t pulseOutput, PlsrPlatformExitCritical(criticalState); return 0U; } - counter = PlsrCounters[PlsrCounterIndexByOutput[pulseOutput]]; - blockCount = (uint16_t)counter->CNT; - if (blockCount >= PlsrFiniteCounterPreload[pulseOutput]) - { - blockCount -= PlsrFiniteCounterPreload[pulseOutput]; - } - else + completed = PlsrFiniteCompletedPulsesSnapshot(pulseOutput); + remaining = PlsrFiniteTargetPulses[pulseOutput]; + if (completed > remaining) { - blockCount = 0UL; + completed = remaining; } - remaining = PlsrFiniteRemainingPulses[pulseOutput]; - if (blockCount > remaining) + remaining -= completed; + frequencyHz = PlsrTimerActiveFrequencyHz[pulseOutput]; + if (frequencyHz == 0UL) { - blockCount = remaining; + PlsrPlatformExitCritical(criticalState); + return 0U; } - remaining -= blockCount; - *tailFrequencyHz = PlsrTimerActiveFrequencyHz[pulseOutput]; + timeUs = ((uint64_t)remaining * 1000000ULL + + frequencyHz - 1UL) / frequencyHz; + *tailFrequencyHz = frequencyHz; *startsNextSegment = 0U; if (PlsrFiniteStreamNextValid[pulseOutput] != 0U) { @@ -3649,12 +3661,21 @@ uint8_t PlsrPlatformFinitePipelineSnapshot(uint8_t pulseOutput, remaining = (nextPulses > (0xFFFFFFFFUL - remaining)) ? 0xFFFFFFFFUL : remaining + nextPulses; - *tailFrequencyHz = + frequencyHz = PlsrFiniteStreamNextSetting[pulseOutput].actualFrequencyHz; + if (frequencyHz == 0UL) + { + PlsrPlatformExitCritical(criticalState); + return 0U; + } + timeUs += ((uint64_t)nextPulses * 1000000ULL + + frequencyHz - 1UL) / frequencyHz; + *tailFrequencyHz = frequencyHz; *startsNextSegment = PlsrFiniteStreamNextStartsSegment[pulseOutput]; } *committedPulses = remaining; + *committedTimeUs = timeUs; PlsrPlatformExitCritical(criticalState); return 1U; }