yuwenhao
/
Modbus-rtu
1
0
Forka 0
Codice Problemi 0 Pull Requests 0 Rilasci 0 Wiki Attività
Sfoglia il codice sorgente

优化了高频计算

codex/plsr-2026-minimal
ywh 3 settimane fa
parent
a6c667d485
commit
9e3c942171
6 ha cambiato i file con 472 aggiunte e 127 eliminazioni
Visualizzazione separata
Opzioni Diff
Mostra statistiche Scarica il file Patch Scarica il file Diff
  1. +44
    -9
      Core/Src/main.c
  2. +7
    -4
      Middlewares/Third_Party/Micrium/Config/app_cfg.h
  3. +2
    -0
      PLSR/Inc/plsr.h
  4. +212
    -114
      PLSR/Src/plsr.c
  5. +177
    -0
      PLSR/Src/plsr_planner.c
  6. +30
    -0
      PLSR/Src/plsr_planner.h

+ 44
- 9
Core/Src/main.c Vedi File

@@ -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)


+ 7
- 4
Middlewares/Third_Party/Micrium/Config/app_cfg.h Vedi File

@@ -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


+ 2
- 0
PLSR/Inc/plsr.h Vedi File

@@ -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,


+ 212
- 114
PLSR/Src/plsr.c Vedi File

@@ -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();


+ 177
- 0
PLSR/Src/plsr_planner.c Vedi File

@@ -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)


+ 30
- 0
PLSR/Src/plsr_planner.h Vedi File

@@ -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,


Scrivi Anteprima
Caricamento…
Annulla
Salva