From 9e3c942171a9dbc98165a93542f8df59b93e6464 Mon Sep 17 00:00:00 2001 From: ywh <2227158009@qq.com> Date: Mon, 24 Aug 2026 19:35:01 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96=E4=BA=86=E9=AB=98=E9=A2=91?= =?UTF-8?q?=E8=AE=A1=E7=AE=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Core/Src/main.c | 53 ++- .../Third_Party/Micrium/Config/app_cfg.h | 11 +- PLSR/Inc/plsr.h | 2 + PLSR/Src/plsr.c | 326 ++++++++++++------ PLSR/Src/plsr_planner.c | 177 ++++++++++ PLSR/Src/plsr_planner.h | 30 ++ 6 files changed, 472 insertions(+), 127 deletions(-) diff --git a/Core/Src/main.c b/Core/Src/main.c index 13b859c..8edbacc 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -47,7 +47,8 @@ DMA_HandleTypeDef hdma_usart1_rx; DMA_HandleTypeDef hdma_usart1_tx; /* USER CODE BEGIN PV */ -static OS_STK AppTaskStartStk[APP_TASK_START_STK_SIZE]; +static OS_STK AppTaskPlsrStk[APP_TASK_PLSR_STK_SIZE]; +static OS_STK AppTaskModbusStk[APP_TASK_MODBUS_STK_SIZE]; /* USER CODE END PV */ /* Private function prototypes -----------------------------------------------*/ @@ -56,7 +57,8 @@ static void MX_GPIO_Init(void); static void MX_DMA_Init(void); static void MX_USART1_UART_Init(void); /* USER CODE BEGIN PFP */ -static void AppTaskStart(void *pArg); +static void AppTaskPlsr(void *pArg); +static void AppTaskModbus(void *pArg); extern void PlsrPlatformForceSafeOutputsFromFault(void); /* USER CODE END PFP */ @@ -64,7 +66,31 @@ extern void PlsrPlatformForceSafeOutputsFromFault(void); /* Private user code ---------------------------------------------------------*/ /* USER CODE BEGIN 0 */ -static void AppTaskStart(void *pArg) +static void AppTaskPlsr(void *pArg) +{ + INT32U lastPollTick; + INT32U currentTick; + + (void)pArg; + lastPollTick = OSTimeGet() - 1U; + + while (1) + { + currentTick = OSTimeGet(); + if (currentTick != lastPollTick) + { + lastPollTick = currentTick; + PlsrPoll1ms(); + } + + if (PlsrServiceProfileProducer(APP_PLSR_PRODUCER_BUDGET) == 0U) + { + OSTimeDly(1U); + } + } +} + +static void AppTaskModbus(void *pArg) { (void)pArg; @@ -76,12 +102,10 @@ static void AppTaskStart(void *pArg) while (1) { ModbusSlavePoll(); - PlsrPoll1ms(); if (ModbusSlaveIsIdle() != 0U) { PlsrServicePersistence(); } - // OSTimeDly(1U); } } @@ -129,10 +153,21 @@ int main(void) OSInit(); - osError = OSTaskCreateExt(AppTaskStart, 0, - &AppTaskStartStk[APP_TASK_START_STK_SIZE - 1u], - APP_TASK_START_PRIO, APP_TASK_START_PRIO, - &AppTaskStartStk[0], APP_TASK_START_STK_SIZE, 0, + osError = OSTaskCreateExt(AppTaskPlsr, 0, + &AppTaskPlsrStk[APP_TASK_PLSR_STK_SIZE - 1u], + APP_TASK_PLSR_PRIO, APP_TASK_PLSR_PRIO, + &AppTaskPlsrStk[0], APP_TASK_PLSR_STK_SIZE, 0, + (OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR)); + + if (osError != OS_ERR_NONE) + { + Error_Handler(); + } + + osError = OSTaskCreateExt(AppTaskModbus, 0, + &AppTaskModbusStk[APP_TASK_MODBUS_STK_SIZE - 1u], + APP_TASK_MODBUS_PRIO, APP_TASK_MODBUS_PRIO, + &AppTaskModbusStk[0], APP_TASK_MODBUS_STK_SIZE, 0, (OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR)); if (osError != OS_ERR_NONE) diff --git a/Middlewares/Third_Party/Micrium/Config/app_cfg.h b/Middlewares/Third_Party/Micrium/Config/app_cfg.h index 9d31e9b..e8be7f6 100644 --- a/Middlewares/Third_Party/Micrium/Config/app_cfg.h +++ b/Middlewares/Third_Party/Micrium/Config/app_cfg.h @@ -5,14 +5,17 @@ * uC/OS-II优先级数值越小，任务优先级越高。 * 每个任务必须使用不同的优先级；0通常留给最紧急的系统任务。 */ -#define APP_TASK_START_PRIO 5u -#define APP_TASK_TEST_PRIO 6u +#define APP_TASK_PLSR_PRIO 4u +#define APP_TASK_MODBUS_PRIO 5u /* * 任务栈大小的单位是OS_STK元素。当前OS_STK为32位。 */ -#define APP_TASK_START_STK_SIZE 512u -#define APP_TASK_TEST_STK_SIZE 256u +#define APP_TASK_PLSR_STK_SIZE 512u +#define APP_TASK_MODBUS_STK_SIZE 512u + +/* Maximum profile items planned by one high-priority producer pass. */ +#define APP_PLSR_PRODUCER_BUDGET 32u /* The short-profile pulse IRQ needs more than the port's 128-word default. */ #define OS_CPU_EXCEPT_STK_SIZE 512u diff --git a/PLSR/Inc/plsr.h b/PLSR/Inc/plsr.h index bbcfeba..f62a69a 100644 --- a/PLSR/Inc/plsr.h +++ b/PLSR/Inc/plsr.h @@ -60,6 +60,8 @@ typedef enum uint8_t PlsrInit(void); void PlsrPoll1ms(void); +/* Returns nonzero while another immediate bounded producer pass is useful. */ +uint8_t PlsrServiceProfileProducer(uint16_t itemBudget); void PlsrServicePersistence(void); PLSR_MB_RESULT PlsrModbusReadHolding(uint16_t startAddress, diff --git a/PLSR/Src/plsr.c b/PLSR/Src/plsr.c index 0332763..88ba80e 100644 --- a/PLSR/Src/plsr.c +++ b/PLSR/Src/plsr.c @@ -56,13 +56,12 @@ || (PLSR_PROFILE_QUEUE_CAPACITY > 65535U)) #error "PLSR profile queue capacity must be a power of two not exceeding 65535" #endif -/* A profile item can represent only one pulse. At the 100 kHz limit these - thresholds provide 4 ms of startup data, refill toward 8 ms, and cap each - 1 ms producer pass at 2 ms worth of newly planned items. */ +/* At the 100 kHz limit these thresholds prefill and refill toward 8 ms of + queued profile data. The caller bounds each producer service pass. */ #define PLSR_PROFILE_STARTUP_TARGET (800U) #define PLSR_PROFILE_STARTUP_BUDGET (800U) #define PLSR_PROFILE_REFILL_TARGET (800U) -#define PLSR_PROFILE_REFILL_BUDGET (200U) +#define PLSR_PROFILE_PRODUCER_BATCH_ITEMS (16U) #define PLSR_TIMED_START_BUILD_BUDGET (32U) #define PLSR_TIMED_START_READY_ITEMS PLSR_PROFILE_STARTUP_BUDGET #define PLSR_PROFILE_LOW_WATER (100U) @@ -473,6 +472,9 @@ static uint8_t PlsrProfileQueueFill(uint16_t targetCount, static uint8_t PlsrTimerSettingsEqual( const PLSR_PLATFORM_TIMER_SETTING *first, const PLSR_PLATFORM_TIMER_SETTING *second); +static uint8_t PlsrProfileEntryTryMerge( + PLSR_PROFILE_ENTRY *previous, + const PLSR_PROFILE_ENTRY *entry); static uint8_t PlsrProfileQueueAppendLocked( PLSR_PROFILE_QUEUE *queue, const PLSR_PROFILE_ENTRY *entry); @@ -2663,6 +2665,33 @@ static uint8_t PlsrTimerSettingsEqual( && (first->compare == second->compare)) ? 1U : 0U; } +static uint8_t PlsrProfileEntryTryMerge( + PLSR_PROFILE_ENTRY *previous, + const PLSR_PROFILE_ENTRY *entry) +{ + uint32_t runLimit; + + if ((previous == NULL) || (entry == NULL) + || (previous->startsNextSegment != 0U) + || (entry->startsNextSegment != 0U) + || (PlsrTimerSettingsEqual(&previous->setting, + &entry->setting) == 0U)) + { + return 0U; + } + runLimit = PlsrProfileRunLimit( + previous->setting.actualFrequencyHz); + if ((entry->repeatCount > runLimit) + || (previous->repeatCount > (runLimit - entry->repeatCount)) + || (previous->repeatCount + > (0xFFFFFFFFUL - entry->repeatCount))) + { + return 0U; + } + previous->repeatCount += entry->repeatCount; + return 1U; +} + static uint8_t PlsrProfileQueueAppendLocked( PLSR_PROFILE_QUEUE *queue, const PLSR_PROFILE_ENTRY *entry) @@ -2679,20 +2708,8 @@ static uint8_t PlsrProfileQueueAppendLocked( { previous = &queue->entries[ (writeIndex - 1UL) & PLSR_PROFILE_QUEUE_MASK]; - if ((previous->startsNextSegment == 0U) - && (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))) - { - previous->repeatCount += entry->repeatCount; + if (PlsrProfileEntryTryMerge(previous, entry) != 0U) + { return 1U; } } @@ -3145,21 +3162,33 @@ static uint32_t PlsrProfileTimingNow(void) return *((volatile uint32_t *)0xE0001004UL); } -static void PlsrProfileRecordProducerCycles(uint32_t startCycles) +static void PlsrProfileRecordProducerCycles(uint32_t startCycles, + uint16_t itemCount) { uint32_t elapsed = PlsrProfileTimingNow() - startCycles; uint32_t total = PlsrProfileProducerTotalCycles; + uint32_t perItemCycles; - if (PlsrProfileProducerItemCount != 0xFFFFFFFFUL) + if (itemCount == 0U) { - PlsrProfileProducerItemCount++; + return; + } + if (PlsrProfileProducerItemCount + <= (0xFFFFFFFFUL - (uint32_t)itemCount)) + { + PlsrProfileProducerItemCount += itemCount; + } + else + { + PlsrProfileProducerItemCount = 0xFFFFFFFFUL; } PlsrProfileProducerTotalCycles = (elapsed > (0xFFFFFFFFUL - total)) ? 0xFFFFFFFFUL : total + elapsed; - if (elapsed > PlsrProfileProducerMaxItemCycles) + perItemCycles = (elapsed + itemCount - 1UL) / itemCount; + if (perItemCycles > PlsrProfileProducerMaxItemCycles) { - PlsrProfileProducerMaxItemCycles = elapsed; + PlsrProfileProducerMaxItemCycles = perItemCycles; } } #endif @@ -3167,23 +3196,29 @@ static void PlsrProfileRecordProducerCycles(uint32_t startCycles) static uint8_t PlsrProfileQueueFill(uint16_t targetCount, uint16_t *itemBudget) { - PLSR_SHORT_PROFILE candidate;//规划账本的工作副本。在临界区外算，算完再写回 - PLSR_PROFILE_ENTRY entry;//这一圈要入队的那一项 + PLSR_SHORT_PROFILE candidate; + PLSR_PROFILE_ENTRY entries[PLSR_PROFILE_PRODUCER_BATCH_ITEMS]; + PLSR_PROFILE_ENTRY entry; uint32_t generation; - uint32_t producerEpoch;//拷出去时记下的版本，写回来要对得上 - uint32_t criticalState;//开关中断用 - uint32_t underrunDebt;//硬件已经「借用」了多少还没规划的脉冲 + uint32_t producerEpoch; + uint32_t criticalState; + uint32_t underrunDebt; uint32_t queueReadIndex; uint32_t queueWriteIndex; - uint32_t queueCount;//当前深度 + uint32_t queueCount; uint32_t currentGeneration; uint32_t currentProducerEpoch; - PLSR_PROFILE_QUEUE *queue;//队列指针 + uint32_t targetSpace; + PLSR_PROFILE_QUEUE *queue; + uint16_t batchLimit; + uint16_t generatedItems; + uint16_t entryCount; + uint16_t index; uint8_t queueBank; - uint8_t currentBank;//当前活队列 + uint8_t currentBank; uint8_t queueActive; uint8_t generatorComplete; - uint8_t payingUnderrunDebt;//这一圈是还债还是真入 + uint8_t payingUnderrunDebt; #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) uint32_t startCycles; @@ -3191,57 +3226,90 @@ static uint8_t PlsrProfileQueueFill(uint16_t targetCount, if (itemBudget == NULL) { - //没有预算指针，没法扣次数，当失败 return 0U; } if ((targetCount == 0U) || (targetCount > PLSR_PROFILE_QUEUE_CAPACITY)) { - //非法目标就按整队列来。正常调用是 400/800，走不到这里 targetCount = PLSR_PROFILE_QUEUE_CAPACITY; } while (1) { - criticalState = PlsrPlatformEnterCritical();//进入临界区 - queueBank = PlsrProfileQueueBank;//现在活着的那一套队列是 0 还是 1。先记下来 - queue = &PlsrProfileQueues[queueBank];//拿到这套队列的指针。后面 queue->xxx 都是这套 - queueActive = queue->active;//这套还在给当前运动供货吗。段停完/作废会变成 0 - generatorComplete = queue->generatorComplete;//规划器是不是已经把这段所有脉冲都吐完了 - queueWriteIndex = queue->writeIndex;//任务下一次要写的位置（累计值，不是 0～1023 的下标） - queueReadIndex = queue->readIndex;//IRQ 下一次要读的位置 - queueCount = queueWriteIndex - queueReadIndex;//当前有多少格。无符号减法，环形也能得到深度 - if ((queueActive == 0U)//不供货了，不用再算 - || (generatorComplete != 0U)//这段已经吐完，不用再算 - || (queueCount >= targetCount)//已经够深（调用方一般是 800 或 400） - || (*itemBudget == 0U))//这一拍允许新算的次数用光了 - - { - //关闭临界区 +#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ + && !defined(PLSR_HOST_TEST) + startCycles = PlsrProfileTimingNow(); +#endif + criticalState = PlsrPlatformEnterCritical(); + queueBank = PlsrProfileQueueBank; + queue = &PlsrProfileQueues[queueBank]; + queueActive = queue->active; + generatorComplete = queue->generatorComplete; + queueWriteIndex = queue->writeIndex; + queueReadIndex = queue->readIndex; + queueCount = queueWriteIndex - queueReadIndex; + if ((queueActive == 0U) + || (generatorComplete != 0U) + || (queueCount >= targetCount) + || (*itemBudget == 0U)) + { PlsrPlatformExitCritical(criticalState); return 1U; } - /*先扣一次「本拍还能算几项」。 - 哪怕后面这项被扔掉，次数也花掉了，防止一拍里死循环*/ - (*itemBudget)--; - generation = queue->generation;//记下现在的世代。改频、借债会 generation++ - producerEpoch = queue->producerEpoch;//记下队列现在的世代。改频、借债会 generation++ - underrunDebt = queue->underrunDebtPulses;//硬件已经用末频「预支」了多少脉冲，规划还没跟上 - payingUnderrunDebt = (underrunDebt != 0UL) ? 1U : 0U;//这一圈是 还债 还是 入队。有债=1 + + generation = queue->generation; + producerEpoch = queue->producerEpoch; + underrunDebt = queue->underrunDebtPulses; + payingUnderrunDebt = (underrunDebt != 0UL) ? 1U : 0U; + batchLimit = *itemBudget; + if (batchLimit > PLSR_PROFILE_PRODUCER_BATCH_ITEMS) + { + batchLimit = PLSR_PROFILE_PRODUCER_BATCH_ITEMS; + } + targetSpace = targetCount - queueCount; + if ((uint32_t)batchLimit > targetSpace) + { + batchLimit = (uint16_t)targetSpace; + } + if (payingUnderrunDebt != 0U) + { + /* Debt changes the queue generation and must be paid against the + latest published amount, so keep this exceptional path single. */ + batchLimit = 1U; + } PlsrCopyShortProfile(&candidate, &queue->producerProfile); - //把规划账本拷到本地 candidate。等会儿在门外改它，不能边算边让 IRQ 看到半成品 PlsrPlatformExitCritical(criticalState); -#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ - && !defined(PLSR_HOST_TEST) - startCycles = PlsrProfileTimingNow(); -#endif - if (PlsrShortProfileTakeRunLimited( &candidate, &entry, - (payingUnderrunDebt != 0U) ? underrunDebt : 0xFFFFFFFFUL) == 0U) - + generatedItems = 0U; + entryCount = 0U; + while ((generatedItems < batchLimit) + && (candidate.active != 0U)) + { + if (PlsrShortProfileTakeRunLimited( + &candidate, &entry, + (payingUnderrunDebt != 0U) + ? underrunDebt : 0xFFFFFFFFUL) == 0U) + { + return 0U; + } + generatedItems++; + if ((entryCount == 0U) + || (PlsrProfileEntryTryMerge( + &entries[entryCount - 1U], &entry) == 0U)) + { + entries[entryCount] = entry; + entryCount++; + } + if (payingUnderrunDebt != 0U) + { + break; + } + } + if ((generatedItems == 0U) || (entryCount == 0U)) { return 0U; } - //正常才继续 + *itemBudget = (uint16_t)(*itemBudget - generatedItems); + criticalState = PlsrPlatformEnterCritical(); currentBank = PlsrProfileQueueBank; queueActive = queue->active; @@ -3254,29 +3322,38 @@ static uint8_t PlsrProfileQueueFill(uint16_t targetCount, || (queueActive == 0U) || (currentGeneration != generation) || (currentProducerEpoch != producerEpoch) - || (queueCount >= PLSR_PROFILE_QUEUE_CAPACITY)) + || ((payingUnderrunDebt == 0U) + && ((queueCount + entryCount) + > PLSR_PROFILE_QUEUE_CAPACITY))) { PlsrPlatformExitCritical(criticalState); continue; } - PlsrCopyShortProfile(&queue->producerProfile, &candidate); + if (payingUnderrunDebt != 0U) { underrunDebt = queue->underrunDebtPulses; - if (underrunDebt < entry.repeatCount) + if (underrunDebt < entries[0].repeatCount) { PlsrPlatformExitCritical(criticalState); continue; } queue->underrunDebtPulses = - underrunDebt - entry.repeatCount; + underrunDebt - entries[0].repeatCount; } - else if (PlsrProfileQueueAppendLocked( - queue, &entry) == 0U) + else { - PlsrPlatformExitCritical(criticalState); - continue; + for (index = 0U; index < entryCount; index++) + { + if (PlsrProfileQueueAppendLocked( + queue, &entries[index]) == 0U) + { + PlsrPlatformExitCritical(criticalState); + return 0U; + } + } } + PlsrCopyShortProfile(&queue->producerProfile, &candidate); queueWriteIndex = queue->writeIndex; queueReadIndex = queue->readIndex; queueCount = queueWriteIndex - queueReadIndex; @@ -3293,7 +3370,7 @@ static uint8_t PlsrProfileQueueFill(uint16_t targetCount, PlsrPlatformExitCritical(criticalState); #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) - PlsrProfileRecordProducerCycles(startCycles); + PlsrProfileRecordProducerCycles(startCycles, generatedItems); #endif } } @@ -3307,6 +3384,7 @@ static PLSR_PLATFORM_QUEUE_RESULT PlsrProfileQueueCommitNext( uint32_t writeIndex; uint32_t actualFrequencyHz; uint32_t repeatRemaining; + uint16_t queueDepth; PLSR_PLATFORM_QUEUE_RESULT result; repeatRemaining = PlsrProfileQueue.repeatRemaining; @@ -3347,10 +3425,10 @@ static PLSR_PLATFORM_QUEUE_RESULT PlsrProfileQueueCommitNext( nextReadIndex = readIndex + 1UL; PlsrProfileQueue.readIndex = nextReadIndex; PlsrProfileQueue.repeatRemaining = entry->repeatCount - 1UL; + queueDepth = (uint16_t)(PlsrProfileQueue.writeIndex - nextReadIndex); if (PlsrProfileQueue.generatorComplete == 0U) { - PlsrProfileQueueRecordDepth( - (uint16_t)(PlsrProfileQueue.writeIndex - nextReadIndex)); + PlsrProfileQueueRecordDepth(queueDepth); } PlsrDeferredFrequencyPending = 0U; PlsrQueuedFrequencyHz = actualFrequencyHz; @@ -6076,6 +6154,53 @@ static uint8_t PlsrServiceCountedExecutor(void) return 1U; } +uint8_t PlsrServiceProfileProducer(uint16_t itemBudget) +{ + uint8_t executorMode; + + if ((PlsrInitialized == 0U) || (itemBudget == 0U)) + { + return 0U; + } + + executorMode = PlsrExecutor.mode; + if (((executorMode == PLSR_EXEC_STEP_TABLE) + || (executorMode == PLSR_EXEC_STREAM) + || (executorMode == PLSR_EXEC_AB_LEGACY)) + && (PlsrProfileQueue.active != 0U) + && (PlsrProfileQueue.generatorComplete == 0U) + && (PlsrProfileQueueFill(PLSR_PROFILE_REFILL_TARGET, + &itemBudget) == 0U)) + { + PlsrEnterError(PLSR_ERROR_TIMER); + return 0U; + } + + executorMode = PlsrExecutor.mode; + if (((executorMode == PLSR_EXEC_STEP_TABLE) + || (executorMode == PLSR_EXEC_STREAM)) + && ((PlsrStageCountedHandoff() == 0U) + || (PlsrProfileQueueFill(PLSR_PROFILE_REFILL_TARGET, + &itemBudget) == 0U))) + { + PlsrEnterError(PLSR_ERROR_TIMER); + return 0U; + } + + executorMode = PlsrExecutor.mode; + if (((executorMode == PLSR_EXEC_STEP_TABLE) + || (executorMode == PLSR_EXEC_STREAM) + || (executorMode == PLSR_EXEC_AB_LEGACY)) + && (PlsrProfileQueue.active != 0U) + && (PlsrProfileQueue.generatorComplete == 0U) + && (PlsrProfileQueueCount() < PLSR_PROFILE_REFILL_TARGET)) + { + return 1U; + } + + return 0U; +} + void PlsrPoll1ms(void) { uint8_t extLevel;//这一拍读到的 EXT 引脚电平。后面才用，这段没用 @@ -6089,7 +6214,6 @@ void PlsrPoll1ms(void) uint32_t newTargetHz;//改频后的新目标。后面才用 uint32_t pulseDirReplanPulses = 0UL;//重规划还剩多少脉冲。先 0 uint32_t pollEpoch;//这一拍看到的段世代号，防止用过期段的 EXT/改频 - uint16_t fillBudget = PLSR_PROFILE_REFILL_BUDGET;//这段会用。 本拍最多往队列里新算多少项。宏是 200 PLSR_PLATFORM_SERVICE_RESULT pulseDirReplanResult = PLSR_PLATFORM_SERVICE_READY;//重规划结果，先当成功 @@ -6098,6 +6222,17 @@ void PlsrPoll1ms(void) return; } +#if !defined(PLSR_HOST_TEST) + /* Run the target throughput benchmark only while idle. It blocks task + context while measuring but never starts pulse output. */ + if ((PlsrPlannerBenchmarkRequest != 0UL) + && (PlsrIsBusy() == 0U)) + { + PlsrPlannerBenchmarkService(); + return; + } +#endif + if (PlsrPollCommandMailbox() != 0U) { /*看邮箱有没有 START / STOP / CLEAR。 @@ -6135,43 +6270,6 @@ void PlsrPoll1ms(void) return;//这段波已经结束，本拍不要再补队列。Fill 是给还在跑的段用的 } - if (PlsrProfileQueueCount() <= PLSR_PROFILE_LOW_WATER)//当前队列还剩多少 格子 - { - //浅到 ≤100，本拍预算从 200 改成 400。急救，多炒一倍菜。 - //没浅，仍是 200 - fillBudget = PLSR_PROFILE_STARTUP_BUDGET; - } - - if (((PlsrExecutor.mode == PLSR_EXEC_STEP_TABLE) - || (PlsrExecutor.mode == PLSR_EXEC_STREAM) - || (PlsrExecutor.mode == PLSR_EXEC_AB_LEGACY))//这三种才会用 profile 队列。IDLE 不补 - && (PlsrProfileQueue.active != 0U)//当前 bank 还在给这段供货。段停完后 active 会被清掉 - && (PlsrProfileQueue.generatorComplete == 0U)//本段规划还没把所有脉冲吐完。吐完了第一次 Fill 没意义 - && (PlsrProfileQueueFill(PLSR_PROFILE_REFILL_TARGET,&fillBudget) == 0U)) - /*去算、去塞，直到格子到 800 或预算用完。返回 0 = Generate/入队失败*/ - - { - PlsrEnterError(PLSR_ERROR_TIMER); - return;//活队列补失败，当定时器/规划故障停机 - } - if (((PlsrExecutor.mode == PLSR_EXEC_STEP_TABLE) - || (PlsrExecutor.mode == PLSR_EXEC_STREAM))//这里 没有 AB_LEGACY。PULSE/DIR 的 counted 下一段才走这条 - && ((PlsrStageCountedHandoff() == 0U) - /*本段已经 generatorComplete 时，把下一段预热的第一/第二 run 接到队尾。 - 还不到接的时候它返回 1（成功但空操作）。 - 返回 0 才是真失败（队列塞不下、handoff 非法等*/ - || (PlsrProfileQueueFill(PLSR_PROFILE_REFILL_TARGET, - &fillBudget) == 0U))) - /*handoff 成功或空操作之后，再用 剩下的预算 再填一次。 - handoff 刚占了格子，或第一次没填满。 - - 两个子条件用 或：handoff 失败 或者 第二次 Fill 失败，都进错误。 - 短路：handoff 已经失败，第二次 Fill 不会跑。*/ - { - PlsrEnterError(PLSR_ERROR_TIMER);//接不上下一段或再补失败，同样停 - return; - } - /* This work is bounded and targets only the inactive bank. It is done after servicing the live queue so preparation cannot starve output. */ PlsrServiceTimedStartPreparation(); diff --git a/PLSR/Src/plsr_planner.c b/PLSR/Src/plsr_planner.c index 24bec37..dd299a3 100644 --- a/PLSR/Src/plsr_planner.c +++ b/PLSR/Src/plsr_planner.c @@ -5,6 +5,23 @@ #define PLSR_PLANNER_Q32_ONE (4294967296ULL) +#if !defined(PLSR_HOST_TEST) +#define PLSR_PLANNER_BENCHMARK_DEMCR_ADDRESS (0xE000EDFCUL) +#define PLSR_PLANNER_BENCHMARK_DWT_CTRL (0xE0001000UL) +#define PLSR_PLANNER_BENCHMARK_DWT_CYCCNT (0xE0001004UL) +#define PLSR_PLANNER_BENCHMARK_TRCENA (1UL << 24U) +#define PLSR_PLANNER_BENCHMARK_CYCCNTENA (1UL << 0U) +#define PLSR_PLANNER_BENCHMARK_PULSES (100000UL) +#define PLSR_PLANNER_BENCHMARK_BATCH_CALLS (128UL) +#define PLSR_PLANNER_BENCHMARK_TARGET_HZ (100000UL) + +volatile uint32_t PlsrPlannerBenchmarkRequest; +volatile uint32_t PlsrPlannerBenchmarkRunning; +volatile uint32_t PlsrPlannerBenchmarkRunCount; +volatile uint32_t PlsrPlannerBenchmarkCoreClockHz = 168000000UL; +volatile PLSR_PLANNER_BENCHMARK_RESULT PlsrPlannerBenchmarkResults[3]; +#endif + #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) #define PLSR_PLANNER_CYCCNT_ADDRESS (0xE0001004UL) @@ -923,6 +940,166 @@ uint16_t PlsrPlannerGenerate(PLSR_PLANNER_CONTEXT *context,//规划账本：三 return produced; } +#if !defined(PLSR_HOST_TEST) +static uint32_t PlsrPlannerBenchmarkCyclesNow(void) +{ + return *((volatile uint32_t *)PLSR_PLANNER_BENCHMARK_DWT_CYCCNT); +} + +static void PlsrPlannerBenchmarkEnableCounter(void) +{ + *((volatile uint32_t *)PLSR_PLANNER_BENCHMARK_DEMCR_ADDRESS) |= + PLSR_PLANNER_BENCHMARK_TRCENA; + *((volatile uint32_t *)PLSR_PLANNER_BENCHMARK_DWT_CYCCNT) = 0UL; + *((volatile uint32_t *)PLSR_PLANNER_BENCHMARK_DWT_CTRL) |= + PLSR_PLANNER_BENCHMARK_CYCCNTENA; +} + +static uint32_t PlsrPlannerBenchmarkCyclesQ16(uint32_t cycles, + uint32_t pulses) +{ + if (pulses == 0UL) + { + return 0UL; + } + return (uint32_t)((((uint64_t)cycles << 16U) + pulses / 2UL) + / pulses); +} + +static void PlsrPlannerBenchmarkMode(uint16_t curveMode) +{ + PLSR_MOTION_BLOCK block; + PLSR_PLANNER_CONTEXT context; + PLSR_STREAM_ITEM item; + PLSR_PLANNER_BENCHMARK_RESULT result; + PLSR_PLANNER_STATUS status; + uint64_t totalCycles = 0ULL; + uint32_t minimumBlockQ16 = 0xFFFFFFFFUL; + uint32_t maximumBlockQ16 = 0UL; + uint32_t generateCalls = 0UL; + uint8_t failed = 0U; + + (void)memset(&block, 0, sizeof(block)); + (void)memset(&context, 0, sizeof(context)); + (void)memset(&item, 0, sizeof(item)); + (void)memset(&result, 0, sizeof(result)); + + block.entryHz = 1UL; + block.cruiseHz = PLSR_PLANNER_BENCHMARK_TARGET_HZ; + block.exitHz = 1UL; + block.pulseBudget = PLSR_PLANNER_BENCHMARK_PULSES; + block.referenceSpeedHz = PLSR_PLANNER_BENCHMARK_TARGET_HZ; + block.accelerationTimeMs = 1000U; + block.decelerationTimeMs = 1000U; + block.curveMode = curveMode; + block.pulseOutput = 0U; + block.boundary = PLSR_BOUNDARY_STOP; + + status = PlsrPlannerBegin(&context, &block, 0UL, 0ULL); + result.curveMode = curveMode; + result.beginStatus = (uint32_t)status; + if (status == PLSR_PLANNER_OK) + { + while (context.active != 0U) + { + uint32_t batchCalls = 0UL; + uint32_t startedAt = PlsrPlannerBenchmarkCyclesNow(); + uint32_t elapsed; + uint32_t blockQ16; + + while ((batchCalls < PLSR_PLANNER_BENCHMARK_BATCH_CALLS) + && (context.active != 0U)) + { + if (PlsrPlannerGenerate(&context, &item, 1U) == 0U) + { + failed = 1U; + break; + } + batchCalls++; + } + elapsed = PlsrPlannerBenchmarkCyclesNow() - startedAt; + if (batchCalls != 0UL) + { + blockQ16 = PlsrPlannerBenchmarkCyclesQ16(elapsed, + batchCalls); + totalCycles += elapsed; + generateCalls += batchCalls; + if (blockQ16 < minimumBlockQ16) + { + minimumBlockQ16 = blockQ16; + } + if (blockQ16 > maximumBlockQ16) + { + maximumBlockQ16 = blockQ16; + } + } + if (failed != 0U) + { + break; + } + } + } + + result.plannedPulses = context.generatedPulses; + result.generateCalls = generateCalls; + result.totalCycles = totalCycles; + result.minimumBlockCyclesPerPulseQ16 = + (minimumBlockQ16 == 0xFFFFFFFFUL) ? 0UL : minimumBlockQ16; + result.maximumBlockCyclesPerPulseQ16 = maximumBlockQ16; + if ((context.generatedPulses != 0UL) && (totalCycles != 0ULL)) + { + uint64_t averageQ16 = ((totalCycles << 16U) + + context.generatedPulses / 2UL) + / context.generatedPulses; + uint64_t estimatedHz = + ((uint64_t)PlsrPlannerBenchmarkCoreClockHz + * context.generatedPulses + totalCycles / 2ULL) + / totalCycles; + uint64_t targetCyclesQ16 = + ((uint64_t)PlsrPlannerBenchmarkCoreClockHz << 16U) + / PLSR_PLANNER_BENCHMARK_TARGET_HZ; + + result.averageCyclesPerPulseQ16 = + (averageQ16 > 0xFFFFFFFFULL) ? 0xFFFFFFFFUL + : (uint32_t)averageQ16; + result.estimatedPulsesPerSecond = + (estimatedHz > 0xFFFFFFFFULL) ? 0xFFFFFFFFUL + : (uint32_t)estimatedHz; + result.passes100k = (averageQ16 <= targetCyclesQ16) ? 1UL : 0UL; + } + result.completed = ((failed == 0U) + && (context.generatedPulses + == PLSR_PLANNER_BENCHMARK_PULSES) + && (context.active == 0U)) ? 1UL : 0UL; + PlsrPlannerBenchmarkResults[curveMode] = result; +} + +void PlsrPlannerBenchmarkService(void) +{ + uint16_t curveMode; + + if ((PlsrPlannerBenchmarkRequest == 0UL) + || (PlsrPlannerBenchmarkRunning != 0UL)) + { + return; + } + PlsrPlannerBenchmarkRequest = 0UL; + PlsrPlannerBenchmarkRunning = 1UL; + (void)memset((void *)PlsrPlannerBenchmarkResults, 0, + sizeof(PlsrPlannerBenchmarkResults)); + PlsrPlannerBenchmarkEnableCounter(); + for (curveMode = 0U; curveMode < 3U; curveMode++) + { + PlsrPlannerBenchmarkMode(curveMode); + } + if (PlsrPlannerBenchmarkRunCount != 0xFFFFFFFFUL) + { + PlsrPlannerBenchmarkRunCount++; + } + PlsrPlannerBenchmarkRunning = 0UL; +} +#endif + static uint64_t PlsrPlannerRampDurationUs(uint32_t pulseCount, uint32_t fromHz, uint32_t toHz) diff --git a/PLSR/Src/plsr_planner.h b/PLSR/Src/plsr_planner.h index 78d4da8..d028252 100644 --- a/PLSR/Src/plsr_planner.h +++ b/PLSR/Src/plsr_planner.h @@ -127,6 +127,36 @@ extern volatile PLSR_PLANNER_TIMING PlsrPlannerTiming; void PlsrPlannerTimingReset(void); #endif +#if !defined(PLSR_HOST_TEST) +/* Target-board throughput benchmark. One benchmark run exercises the exact + runtime calling convention (Generate capacity == 1) for a 100000-pulse + triangular profile in each curve mode. Q16 cycle values retain fractional + cycles without using floating point in the target. */ +typedef struct +{ + uint32_t curveMode; + uint32_t beginStatus; + uint32_t completed; + uint32_t plannedPulses; + uint32_t generateCalls; + uint64_t totalCycles; + uint32_t averageCyclesPerPulseQ16; + uint32_t minimumBlockCyclesPerPulseQ16; + uint32_t maximumBlockCyclesPerPulseQ16; + uint32_t estimatedPulsesPerSecond; + uint32_t passes100k; +} PLSR_PLANNER_BENCHMARK_RESULT; + +extern volatile uint32_t PlsrPlannerBenchmarkRequest; +extern volatile uint32_t PlsrPlannerBenchmarkRunning; +extern volatile uint32_t PlsrPlannerBenchmarkRunCount; +extern volatile uint32_t PlsrPlannerBenchmarkCoreClockHz; +extern volatile PLSR_PLANNER_BENCHMARK_RESULT + PlsrPlannerBenchmarkResults[3]; + +void PlsrPlannerBenchmarkService(void); +#endif + PLSR_PLANNER_STATUS PlsrPlannerBegin(PLSR_PLANNER_CONTEXT *context, const PLSR_MOTION_BLOCK *block, uint32_t appliedHz,