#include "plsr.h" #include "plsr_internal.h" #include "plsr_planner.h" #include "plsr_platform.h" #include #include #if defined(__ICCARM__) #include #endif #define PLSR_COMMON_FIRST_ADDRESS (0x1000U) #define PLSR_COMMON_LAST_ADDRESS (0x10FFU) #define PLSR_SEGMENT_FIRST_ADDRESS (0x1100U) #define PLSR_SEGMENT_STRIDE (0x0010U) #define PLSR_SEGMENT_DEFINED_WORDS (8U) #define PLSR_WAIT_TIME (0U) #define PLSR_WAIT_SIGNAL (1U) #define PLSR_ACT_TIME (2U) #define PLSR_EXT_SIGNAL (3U) #define PLSR_EXT_OR_COMPLETE (4U) #define PLSR_SEND_COMPLETE (0U) #define PLSR_SEND_SUBSEQUENT (1U) #define PLSR_POSITION_RELATIVE (0U) #define PLSR_POSITION_ABSOLUTE (1U) #define PLSR_COMMAND_START (0x0001U) #define PLSR_COMMAND_STOP (0x0002U) #define PLSR_COMMAND_CLEAR (0x0004U) #define PLSR_DIAG_FLAG_MONITORING (0x0001U) #define PLSR_DIAG_FLAG_COUNT_CHECKED (0x0002U) #define PLSR_DIAG_FLAG_COUNT_PASS (0x0004U) #define PLSR_DIAG_FLAG_FREQUENCY_CHECKED (0x0008U) #define PLSR_DIAG_FLAG_FREQUENCY_PASS (0x0010U) #define PLSR_DIAG_FLAG_CURVE_CHECKED (0x0020U) #define PLSR_DIAG_FLAG_CURVE_PASS (0x0040U) #define PLSR_DIAG_FLAG_FAULT_LATCHED (0x0080U) #define PLSR_DIAG_REASON_NONE (0U) #define PLSR_DIAG_REASON_COUNT (1U) #define PLSR_DIAG_REASON_FREQUENCY (2U) #define PLSR_DIAG_REASON_CURVE (3U) #define PLSR_DIAG_REASON_INVALID_SAMPLE (4U) #define PLSR_CONFIG_SAVE_DELAY_MS (1000U) #define PLSR_POSITION_CHECKPOINT_MS (10U) #define PLSR_PROFILE_QUEUE_CAPACITY (1024U) #define PLSR_PROFILE_QUEUE_MASK (PLSR_PROFILE_QUEUE_CAPACITY - 1U) #if ((PLSR_PROFILE_QUEUE_CAPACITY == 0U) \ || ((PLSR_PROFILE_QUEUE_CAPACITY \ & (PLSR_PROFILE_QUEUE_CAPACITY - 1U)) != 0U) \ || (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. */ #define PLSR_PROFILE_STARTUP_TARGET (400U) #define PLSR_PROFILE_STARTUP_BUDGET (400U) #define PLSR_PROFILE_REFILL_TARGET (800U) #define PLSR_PROFILE_REFILL_BUDGET (200U) #define PLSR_PROFILE_LOW_WATER (100U) #define PLSR_PROFILE_LOW_WATER_CLEAR (200U) #define PLSR_HANDOFF_WARMUP_ITEMS (8U) #define PLSR_REPLAN_PREFIX_ITEMS (512U) #define PLSR_REPLAN_PREFIX_TIME_US (5000ULL) #define PLSR_REPLAN_BUILD_ITEMS (400U) #define PLSR_BOUNDARY_EVENT_CAPACITY (2U) #if ((PLSR_BOUNDARY_EVENT_CAPACITY == 0U) \ || ((PLSR_BOUNDARY_EVENT_CAPACITY \ & (PLSR_BOUNDARY_EVENT_CAPACITY - 1U)) != 0U) \ || (PLSR_BOUNDARY_EVENT_CAPACITY > 127U)) #error "PLSR boundary event capacity must be a power of two up to 127" #endif #define PLSR_QUEUE_DIAG_LOW_WATER (0x0001U) #define PLSR_QUEUE_DIAG_UNDERRUN (0x0002U) #define PLSR_QUEUE_DIAG_PLANNER_CLIPPED (0x0004U) #define PLSR_RAMP_POLL_RESERVE_AREA_DIVISOR (20ULL) /* 功能诊断(计数/频率/曲线检查)总开关。 * 0 = 全部关闭:诊断采集、结算与锁定均不执行,Modbus 诊断寄存器保持全 0, * 诊断控制字写入仍被接受(无操作)。运动核心先行跑通;曲线校准阶段再置 1。 */ #define PLSR_DIAGNOSTIC_ENABLED (0U) /* 计数错误停机检查:暂时关闭。counted 流计数同步与执行器重构对齐期间, * 实际输出与预期的偏差只做数据钳制(delta=remaining),不再进 PLSR_ERROR_COUNT。 */ #define PLSR_COUNT_ERROR_CHECK_ENABLED (0U) typedef struct { uint32_t fromHz; uint32_t toHz; uint32_t durationMs; uint32_t elapsedMs; uint8_t active; } PLSR_RAMP; typedef struct { PLSR_PLANNER_CONTEXT planner; PLSR_STREAM_ITEM pendingItem; uint32_t startHz; uint32_t peakHz; uint32_t endHz; uint32_t pulseCount; uint32_t entryPulses; uint32_t steadyPulses; uint32_t exitPulses; volatile uint32_t nextPeriod; uint32_t pendingRepeats; uint32_t lastRampFrequencyHz; volatile uint8_t active; } PLSR_SHORT_PROFILE; typedef struct { PLSR_PLATFORM_TIMER_SETTING setting; uint32_t requestedFrequencyHz; uint32_t repeatCount; uint8_t startsNextSegment; } PLSR_PROFILE_ENTRY; typedef struct { PLSR_PROFILE_ENTRY entries[PLSR_PROFILE_QUEUE_CAPACITY]; PLSR_SHORT_PROFILE producerProfile; volatile uint32_t readIndex; volatile uint32_t writeIndex; volatile uint32_t generation; uint32_t producerEpoch; volatile uint32_t repeatRemaining; uint8_t producerSegment; uint8_t preparedHandoffBank; volatile uint8_t active; volatile uint8_t generatorComplete; } PLSR_PROFILE_QUEUE; typedef struct { uint64_t magnitude; uint32_t firstFrequencyHz; uint32_t secondFrequencyHz; uint32_t firstRequestedFrequencyHz; uint32_t secondRequestedFrequencyHz; uint32_t firstRepeatCount; uint32_t secondRepeatCount; PLSR_PLATFORM_TIMER_SETTING firstSetting; PLSR_PLATFORM_TIMER_SETTING secondSetting; PLSR_PROFILE_ENTRY warmup[PLSR_HANDOFF_WARMUP_ITEMS]; PLSR_SHORT_PROFILE profile; uint8_t warmupCount; uint8_t nextSegment; uint8_t positive; volatile uint8_t valid; } PLSR_HANDOFF_PLAN; typedef struct { uint64_t magnitude; uint32_t firstFrequencyHz; uint32_t completedPulses; uint8_t nextSegment; uint8_t positive; } PLSR_COUNTED_BOUNDARY_EVENT; typedef enum { PLSR_WORD_OK = 0, PLSR_WORD_ILLEGAL_ADDRESS, PLSR_WORD_ILLEGAL_VALUE } PLSR_WORD_RESULT; typedef enum { PLSR_COMMAND_MAILBOX_EMPTY = 0, PLSR_COMMAND_MAILBOX_PENDING, PLSR_COMMAND_MAILBOX_EXECUTING } PLSR_COMMAND_MAILBOX_STATE; typedef struct { PLSR_CONFIG startConfig; uint16_t command; volatile uint8_t state; } PLSR_COMMAND_MAILBOX; typedef struct { volatile PLSR_EXEC_MODE mode; volatile uint32_t generation; } PLSR_EXECUTOR; typedef struct { uint16_t flags; uint16_t reason; uint16_t lastSegment; uint16_t modeDirection; uint32_t expectedCycles; uint32_t observedCycles; int32_t countError; uint32_t requestedHz; uint32_t expectedTimerHz; uint32_t activeTimerHz; int32_t requestErrorHz; uint32_t curveSampleCount; uint32_t curveMismatchCount; uint32_t curveMaxAbsErrorHz; uint32_t firstMismatchSample; } PLSR_DIAGNOSTIC; static const uint16_t PlsrSineProgressQ16[65] = { 0U, 39U, 158U, 355U, 630U, 982U, 1411U, 1915U, 2494U, 3146U, 3869U, 4662U, 5522U, 6448U, 7438U, 8488U, 9597U, 10762U, 11980U, 13248U, 14563U, 15922U, 17321U, 18758U, 20228U, 21728U, 23256U, 24806U, 26375U, 27960U, 29556U, 31160U, 32767U, 34375U, 35979U, 37575U, 39160U, 40729U, 42279U, 43807U, 45307U, 46777U, 48214U, 49613U, 50972U, 52287U, 53555U, 54773U, 55938U, 57047U, 58097U, 59087U, 60013U, 60873U, 61666U, 62389U, 63041U, 63620U, 64124U, 64553U, 64905U, 65180U, 65377U, 65496U, 65535U }; static PLSR_CONFIG PlsrShadowConfig; static PLSR_CONFIG PlsrActiveConfig; static PLSR_CONFIG PlsrCandidateConfig; static volatile int32_t PlsrPosition; static volatile uint64_t PlsrRemainingPulses; static volatile uint8_t PlsrPulseActive; static volatile uint8_t PlsrCutRequested; static volatile uint8_t PlsrBoundaryPending; static volatile uint8_t PlsrBoundaryWasCut; static volatile uint8_t PlsrCountPositive; static volatile uint8_t PlsrCountOverflowPending; static volatile uint8_t PlsrPositionValid; static volatile uint8_t PlsrPositionCheckpointDirty; static volatile uint8_t PlsrFrequencyUpdatePending; static volatile uint8_t PlsrFrequencyUpdateSegment; static volatile uint8_t PlsrSeamlessHandoffPending; static volatile uint8_t PlsrTimerErrorPending; static volatile uint8_t PlsrDeferredFrequencyPending; static volatile uint32_t PlsrCurrentFrequencyHz; static volatile uint32_t PlsrQueuedFrequencyHz; static volatile uint32_t PlsrBoundaryFrequencyHz; static volatile uint32_t PlsrFrequencyUpdateTargetHz; static volatile uint32_t PlsrDeferredFrequencyHz; static volatile uint32_t PlsrSegmentEpoch; static volatile PLSR_STATUS PlsrRunStatus = PLSR_STATUS_UNINITIALIZED; static PLSR_ERROR PlsrError = PLSR_ERROR_NONE; static PLSR_RAMP PlsrRamp; static PLSR_SHORT_PROFILE PlsrShortProfile; static PLSR_PROFILE_QUEUE PlsrProfileQueues[2]; static volatile uint8_t PlsrProfileQueueBank; #define PlsrProfileQueue (PlsrProfileQueues[PlsrProfileQueueBank]) static PLSR_HANDOFF_PLAN PlsrHandoffPlan; static volatile uint64_t PlsrCountedObservedPublished; static volatile uint8_t PlsrCountedHandoffStaged; /* 段边界/流故障事件:IRQ 只置事件并快照(handoff 计划 + 边界累计计数), 全部簿记由 1ms 任务消费事件时完成(IRQ 内不做状态转换与错误标志写入)。 */ static PLSR_COUNTED_BOUNDARY_EVENT PlsrCountedBoundaryEvents[PLSR_BOUNDARY_EVENT_CAPACITY]; static volatile uint8_t PlsrCountedBoundaryWriteIndex; static volatile uint8_t PlsrCountedBoundaryReadIndex; static volatile uint8_t PlsrCountedBoundaryBusy; static volatile uint8_t PlsrCountedFaultEvent; static PLSR_HANDOFF_PLAN PlsrPreparedHandoffPlans[2][PLSR_SEGMENT_COUNT_MAX]; static volatile uint8_t PlsrPreparedHandoffBank; #ifdef PLSR_HOST_TEST static uint32_t PlsrHostTestEventSequence; static uint32_t PlsrHostTestLastGateEvent; static uint32_t PlsrHostTestLastDiagnosticFinishEvent; #endif static uint8_t PlsrInitialized; static volatile uint8_t PlsrCurrentSegment; static uint8_t PlsrDirectionDelayActive; static uint16_t PlsrDirectionDelayRemainingMs; static volatile uint8_t PlsrSegmentClockStarted; static volatile uint32_t PlsrSegmentElapsedMs; static volatile uint32_t PlsrWaitElapsedMs; static volatile uint8_t PlsrExtPreviousLevel; static volatile uint8_t PlsrExtEdgePending; static volatile uint8_t PlsrStopRequested; static volatile uint8_t PlsrStopPulsesRemaining; static volatile uint8_t PlsrAbStopArmed; static volatile uint8_t PlsrBoundaryRampStarted; static uint8_t PlsrLastDirectionValid; static uint8_t PlsrLastDirectionOutput; static uint8_t PlsrLastDirectionLevel; static uint8_t PlsrPersistenceDirty; static uint16_t PlsrPersistenceDelayMs; static uint8_t PlsrPositionCheckpointElapsedMs; static PLSR_COMMAND_MAILBOX PlsrCommandMailbox; static PLSR_EXECUTOR PlsrExecutor; static volatile PLSR_DIAGNOSTIC PlsrDiagnostic; static volatile uint64_t PlsrDiagnosticSegmentCounterBase; static volatile uint32_t PlsrDiagnosticQueuedExpectedHz; static volatile uint16_t PlsrDiagnosticFailedChecks; static volatile uint16_t PlsrProfileQueueMinimumDepth; static volatile uint16_t PlsrProfileQueueDiagnosticFlags; static volatile uint8_t PlsrProfileQueueLowWaterLatched; static volatile uint32_t PlsrProfileQueueLowWaterEvents; static volatile uint32_t PlsrProfileQueueUnderruns; static volatile uint32_t PlsrPlannerClippedCount; #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) volatile uint32_t PlsrProfileProducerItemCount; volatile uint32_t PlsrProfileProducerTotalCycles; volatile uint32_t PlsrProfileProducerMaxItemCycles; #endif static uint8_t PlsrIsBusy(void); static void PlsrSetDefaults(PLSR_CONFIG *config); static uint8_t PlsrConfigIsValid(const PLSR_CONFIG *config, uint8_t validateActivePath); static uint32_t PlsrResolvedSegmentFrequency(const PLSR_CONFIG *config, uint8_t segmentIndex); static uint16_t PlsrReadConfigWord(const PLSR_CONFIG *config, uint16_t address); static PLSR_WORD_RESULT PlsrWriteConfigWord(PLSR_CONFIG *config, uint16_t address, uint16_t value); static uint8_t PlsrAddressIsDwordHalf(uint16_t address, uint16_t *pairedAddress); static PLSR_MB_RESULT PlsrQueueCommand(uint16_t command); static uint8_t PlsrPollCommandMailbox(void); static void PlsrExecuteStart(void); static uint8_t PlsrExecuteStop(void); static void PlsrExecuteClear(void); static uint8_t PlsrStartSegment(uint8_t segmentNumber, uint8_t allowCarry, uint32_t carryFrequencyHz); static uint32_t PlsrEffectiveStartFrequency(uint32_t segmentFrequencyHz, uint8_t allowCarry, uint8_t directionChanged, uint32_t carryFrequencyHz); static uint32_t PlsrEffectiveStopFrequency(uint32_t segmentFrequencyHz); static uint8_t PlsrBeginSegmentOutput(uint32_t startFrequencyHz); static void PlsrHandleBoundary(uint8_t extEdge); static void PlsrTransitionToNext(uint8_t allowCarry); static void PlsrFinishCompleted(void); static void PlsrFinishStopped(void); static void PlsrEnterError(PLSR_ERROR error); static void PlsrEnterErrorIfEpoch(PLSR_ERROR error, uint32_t expectedEpoch); static void PlsrMarkPersistenceDirty(uint16_t delayMs); static void PlsrCheckpointPosition(uint8_t wasBusy); static void PlsrPollPositionCheckpoint(void); static PLSR_PLATFORM_QUEUE_RESULT PlsrTryContinuousHandoff(void); static uint8_t PlsrPrepareShortProfile(PLSR_SHORT_PROFILE *profile, uint8_t segmentNumber, uint32_t startFrequencyHz, uint32_t targetFrequencyHz, uint64_t pulseCount); static uint8_t PlsrShortProfileTakeRun(PLSR_SHORT_PROFILE *profile, PLSR_PROFILE_ENTRY *entry); static void PlsrProfileRecordPlannerStatus( const PLSR_SHORT_PROFILE *profile); static uint8_t PlsrReplanPulseDir(uint32_t targetHz, uint32_t totalPulses); static void PlsrProfileQueueReset(void); static void PlsrProfileQueueInvalidateGeneration(void); static uint8_t PlsrProfileQueueBegin( const PLSR_SHORT_PROFILE *producerProfile, uint32_t producerEpoch, uint8_t producerSegment, uint8_t preparedHandoffBank); static uint8_t PlsrProfileQueueFill(uint16_t targetCount, uint16_t *itemBudget); static uint8_t PlsrStageCountedHandoff(void); static PLSR_PLATFORM_QUEUE_RESULT PlsrProfileQueueCommitNext( uint8_t pulseOutput); static uint8_t PlsrPrepareFutureHandoffQueue(void); static void PlsrActivateHandoffQueueFromIrq( const PLSR_HANDOFF_PLAN *plan, uint32_t producerEpoch); static void PlsrCopyShortProfile(PLSR_SHORT_PROFILE *destination, const PLSR_SHORT_PROFILE *source); static void PlsrInvalidateHandoffPlans(void); static uint8_t PlsrBuildHandoffPlanBank( const PLSR_CONFIG *frequencyConfig); static uint8_t PlsrSelectPreparedHandoffPlan(PLSR_HANDOFF_PLAN *plan); static PLSR_PLATFORM_QUEUE_RESULT PlsrPrimeHandoff(void); static uint8_t PlsrBuildHandoffPlan(uint8_t sourceSegment, const PLSR_CONFIG *frequencyConfig, uint32_t carryFrequencyHz, PLSR_HANDOFF_PLAN *plan); static void PlsrRefreshCurrentHandoffPlan(uint32_t terminalFrequencyHz); static void PlsrDiagnosticReset(void); static void PlsrDiagnosticBeginSegment(uint8_t segmentNumber, uint64_t expectedCycles, uint8_t directionPositive); static void PlsrDiagnosticFinishSegment(uint8_t completedNormally); static uint8_t PlsrDiagnosticStartPulse(uint8_t pulseOutput, uint32_t requestedFirstHz, uint32_t requestedQueuedHz, uint32_t *actualFirstHz, uint32_t *actualQueuedHz); static PLSR_PLATFORM_QUEUE_RESULT PlsrDiagnosticQueueFrequency( uint8_t pulseOutput, uint32_t requestedHz, uint32_t *actualHz); static void PlsrDiagnosticCheckActiveFrequency(uint32_t activeHz); static uint8_t PlsrArmFinalAbBoundaryLocked(void); #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) static uint32_t PlsrProfileTimingNow(void); #endif static uint8_t PlsrQueueFinalAbBoundaryFromIrq(void); static uint8_t PlsrIsBusy(void) { return ((PlsrRunStatus == PLSR_STATUS_ACCELERATING) || (PlsrRunStatus == PLSR_STATUS_RUNNING) || (PlsrRunStatus == PLSR_STATUS_DECELERATING) || (PlsrRunStatus == PLSR_STATUS_WAITING) || (PlsrRunStatus == PLSR_STATUS_PAUSED)) ? 1U : 0U; } static uint32_t PlsrJoinU32(uint16_t lowWord, uint16_t highWord) { return (uint32_t)lowWord | ((uint32_t)highWord << 16U); } static uint16_t PlsrLowWord(uint32_t value) { return (uint16_t)(value & 0xFFFFUL); } static uint16_t PlsrHighWord(uint32_t value) { return (uint16_t)(value >> 16U); } static uint8_t PlsrFinalAbBoundaryRequired(void) { uint8_t finalBoundary; if (PlsrActiveConfig.outputMode != PLSR_OUTPUT_AB) { return 0U; } finalBoundary = ((PlsrCutRequested != 0U) || ((PlsrStopRequested != 0U) && (PlsrStopPulsesRemaining == 1U)) || ((PlsrRemainingPulses == 1UL) && (PlsrHandoffPlan.valid == 0U))) ? 1U : 0U; return ((PlsrPulseActive != 0U) && (finalBoundary != 0U)) ? 1U : 0U; } static uint8_t PlsrCompleteFinalAbArmLocked(void) { PLSR_PLATFORM_STOP_RESULT result; uint8_t pulseOutput = (uint8_t)PlsrActiveConfig.pulseOutput; result = PlsrPlatformRequestStopLocked(pulseOutput, 1U); if (result == PLSR_PLATFORM_STOP_PENDING) { PlsrQueuedFrequencyHz = PlsrCurrentFrequencyHz; PlsrDiagnosticQueuedExpectedHz = PlsrCurrentFrequencyHz; PlsrDeferredFrequencyHz = PlsrCurrentFrequencyHz; PlsrDeferredFrequencyPending = 0U; PlsrFrequencyUpdatePending = 0U; PlsrShortProfile.active = 0U; PlsrHandoffPlan.valid = 0U; PlsrRamp.active = 0U; return 1U; } PlsrTimerErrorPending = 1U; PlsrPlatformStopPulse(pulseOutput); PlsrAbStopArmed = 0U; PlsrPulseActive = 0U; PlsrBoundaryFrequencyHz = PlsrCurrentFrequencyHz; PlsrBoundaryWasCut = (PlsrCutRequested != 0U) ? 1U : 0U; PlsrCutRequested = 0U; PlsrCurrentFrequencyHz = 0UL; PlsrQueuedFrequencyHz = 0UL; PlsrBoundaryPending = 1U; return 0U; } static uint8_t PlsrArmFinalAbBoundaryLocked(void) { if (PlsrFinalAbBoundaryRequired() == 0U) { return 1U; } if (PlsrAbStopArmed != 0U) { return 1U; } PlsrAbStopArmed = 1U; return PlsrCompleteFinalAbArmLocked(); } static uint8_t PlsrQueueFinalAbBoundaryFromIrq(void) { uint32_t criticalState; if ((PlsrFinalAbBoundaryRequired() == 0U) || (PlsrAbStopArmed != 0U)) { return 1U; } PlsrAbStopArmed = 1U; if (PlsrPlatformQueueFinalArmFromIrq( (uint8_t)PlsrActiveConfig.pulseOutput) != 0U) { return 1U; } criticalState = PlsrPlatformEnterCritical(); if ((PlsrAbStopArmed == 0U) || (PlsrFinalAbBoundaryRequired() == 0U)) { PlsrPlatformExitCritical(criticalState); return 1U; } { uint8_t result = PlsrCompleteFinalAbArmLocked(); PlsrPlatformExitCritical(criticalState); return result; } } void PlsrFinalArmJobIrq(uint8_t pulseOutput) { uint32_t criticalState = PlsrPlatformEnterCritical(); if ((PlsrAbStopArmed == 0U) || (PlsrPulseActive == 0U) || (pulseOutput != (uint8_t)PlsrActiveConfig.pulseOutput) || (PlsrFinalAbBoundaryRequired() == 0U)) { if ((PlsrPulseActive != 0U) && (pulseOutput == (uint8_t)PlsrActiveConfig.pulseOutput)) { PlsrAbStopArmed = 0U; } PlsrPlatformExitCritical(criticalState); return; } (void)PlsrCompleteFinalAbArmLocked(); PlsrPlatformExitCritical(criticalState); } #if PLSR_DIAGNOSTIC_ENABLED static uint32_t PlsrAbsDifferenceU32(uint32_t first, uint32_t second) { return (first >= second) ? (first - second) : (second - first); } static int32_t PlsrDifferenceI32(uint32_t first, uint32_t second) { int64_t difference = (int64_t)first - (int64_t)second; if (difference > INT32_MAX) { return INT32_MAX; } if (difference < INT32_MIN) { return INT32_MIN; } return (int32_t)difference; } #endif #if PLSR_DIAGNOSTIC_ENABLED static void PlsrDiagnosticLatch(uint16_t reason) { PlsrDiagnostic.flags |= PLSR_DIAG_FLAG_FAULT_LATCHED; if (PlsrDiagnostic.reason == PLSR_DIAG_REASON_NONE) { PlsrDiagnostic.reason = reason; } } #endif static void PlsrDiagnosticReset(void) { #if PLSR_DIAGNOSTIC_ENABLED (void)memset((void *)&PlsrDiagnostic, 0, sizeof(PlsrDiagnostic)); PlsrDiagnostic.firstMismatchSample = 0xFFFFFFFFUL; PlsrDiagnosticSegmentCounterBase = 0UL; PlsrDiagnosticQueuedExpectedHz = 0UL; PlsrDiagnosticFailedChecks = 0U; #else /* 关闭状态保持诊断区清零,Modbus 读诊断寄存器恒为 0。 */ (void)memset((void *)&PlsrDiagnostic, 0, sizeof(PlsrDiagnostic)); #endif PlsrProfileQueueMinimumDepth = 0xFFFFU; PlsrProfileQueueDiagnosticFlags = 0U; PlsrProfileQueueLowWaterLatched = 0U; PlsrProfileQueueLowWaterEvents = 0UL; PlsrProfileQueueUnderruns = 0UL; PlsrPlannerClippedCount = 0UL; } static void PlsrDiagnosticBeginSegment(uint8_t segmentNumber, uint64_t expectedCycles, uint8_t directionPositive) { #if PLSR_DIAGNOSTIC_ENABLED if (expectedCycles > 0xFFFFFFFFUL) { PlsrDiagnostic.expectedCycles = 0xFFFFFFFFUL; PlsrDiagnosticLatch(PLSR_DIAG_REASON_INVALID_SAMPLE); } else { PlsrDiagnostic.expectedCycles = (uint32_t)expectedCycles; } PlsrDiagnostic.flags &= (uint16_t)~(PLSR_DIAG_FLAG_COUNT_CHECKED | PLSR_DIAG_FLAG_COUNT_PASS); PlsrDiagnostic.flags |= PLSR_DIAG_FLAG_MONITORING; PlsrDiagnostic.lastSegment = segmentNumber; PlsrDiagnostic.modeDirection = (uint16_t)(PlsrActiveConfig.outputMode | ((uint16_t)directionPositive << 8U)); PlsrDiagnostic.observedCycles = 0UL; PlsrDiagnostic.countError = 0L; PlsrDiagnosticSegmentCounterBase = PlsrPlatformObservedPulses( (uint8_t)PlsrActiveConfig.pulseOutput); #else (void)segmentNumber; (void)expectedCycles; (void)directionPositive; #endif } static void PlsrDiagnosticFinishSegment(uint8_t completedNormally) { #if PLSR_DIAGNOSTIC_ENABLED uint64_t now; uint64_t observed; uint32_t observedCycles; uint32_t expectedCycles; if ((PlsrDiagnostic.flags & PLSR_DIAG_FLAG_MONITORING) == 0U) { return; } #ifdef PLSR_HOST_TEST PlsrHostTestEventSequence++; PlsrHostTestLastDiagnosticFinishEvent = PlsrHostTestEventSequence; #endif now = PlsrPlatformObservedPulses( (uint8_t)PlsrActiveConfig.pulseOutput); observed = (now >= PlsrDiagnosticSegmentCounterBase) ? (now - PlsrDiagnosticSegmentCounterBase) : 0UL; if (observed > 0xFFFFFFFFUL) { PlsrDiagnostic.observedCycles = 0xFFFFFFFFUL; PlsrDiagnosticFailedChecks |= PLSR_DIAG_FLAG_COUNT_PASS; PlsrDiagnostic.flags &= (uint16_t)~PLSR_DIAG_FLAG_COUNT_PASS; PlsrDiagnosticLatch(PLSR_DIAG_REASON_INVALID_SAMPLE); } else { PlsrDiagnostic.observedCycles = (uint32_t)observed; } PlsrDiagnostic.flags &= (uint16_t)~PLSR_DIAG_FLAG_MONITORING; if (completedNormally == 0U) { PlsrDiagnostic.countError = 0L; PlsrDiagnostic.flags &= (uint16_t)~(PLSR_DIAG_FLAG_COUNT_CHECKED | PLSR_DIAG_FLAG_COUNT_PASS); return; } observedCycles = PlsrDiagnostic.observedCycles; expectedCycles = PlsrDiagnostic.expectedCycles; PlsrDiagnostic.countError = PlsrDifferenceI32(observedCycles, expectedCycles); PlsrDiagnostic.flags |= PLSR_DIAG_FLAG_COUNT_CHECKED; if (PlsrDiagnostic.countError != 0L) { PlsrDiagnosticFailedChecks |= PLSR_DIAG_FLAG_COUNT_PASS; PlsrDiagnostic.flags &= (uint16_t)~PLSR_DIAG_FLAG_COUNT_PASS; PlsrDiagnosticLatch(PLSR_DIAG_REASON_COUNT); } else if ((PlsrDiagnosticFailedChecks & PLSR_DIAG_FLAG_COUNT_PASS) == 0U) { PlsrDiagnostic.flags |= PLSR_DIAG_FLAG_COUNT_PASS; } #else (void)completedNormally; #endif } static void PlsrDiagnosticRecordFrequency(uint32_t requestedHz, uint32_t expectedHz, uint32_t actualHz) { #if PLSR_DIAGNOSTIC_ENABLED uint32_t error = PlsrAbsDifferenceU32(actualHz, expectedHz); PlsrDiagnostic.requestedHz = requestedHz; PlsrDiagnostic.expectedTimerHz = expectedHz; PlsrDiagnostic.activeTimerHz = actualHz; /* Request error is divider quantization, not a diagnostic deviation. */ PlsrDiagnostic.requestErrorHz = PlsrDifferenceI32(expectedHz, requestedHz); PlsrDiagnostic.flags |= PLSR_DIAG_FLAG_FREQUENCY_CHECKED; if (actualHz != expectedHz) { PlsrDiagnosticFailedChecks |= PLSR_DIAG_FLAG_FREQUENCY_PASS; PlsrDiagnostic.flags &= (uint16_t)~PLSR_DIAG_FLAG_FREQUENCY_PASS; PlsrDiagnosticLatch(PLSR_DIAG_REASON_FREQUENCY); } else if ((PlsrDiagnosticFailedChecks & PLSR_DIAG_FLAG_FREQUENCY_PASS) == 0U) { PlsrDiagnostic.flags |= PLSR_DIAG_FLAG_FREQUENCY_PASS; } if (error > PlsrDiagnostic.curveMaxAbsErrorHz) { PlsrDiagnostic.curveMaxAbsErrorHz = error; } #else (void)requestedHz; (void)expectedHz; (void)actualHz; #endif } #if PLSR_DIAGNOSTIC_ENABLED static void PlsrDiagnosticRecordCurveSample(uint32_t expectedHz, uint32_t actualHz) { uint32_t error = PlsrAbsDifferenceU32(expectedHz, actualHz); if (PlsrDiagnostic.curveSampleCount != 0xFFFFFFFFUL) { PlsrDiagnostic.curveSampleCount++; } PlsrDiagnostic.flags |= PLSR_DIAG_FLAG_CURVE_CHECKED; if (error == 0UL) { if ((PlsrDiagnosticFailedChecks & PLSR_DIAG_FLAG_CURVE_PASS) == 0U) { PlsrDiagnostic.flags |= PLSR_DIAG_FLAG_CURVE_PASS; } } else { PlsrDiagnosticFailedChecks |= PLSR_DIAG_FLAG_CURVE_PASS; PlsrDiagnostic.flags &= (uint16_t)~PLSR_DIAG_FLAG_CURVE_PASS; if (PlsrDiagnostic.curveMismatchCount != 0xFFFFFFFFUL) { PlsrDiagnostic.curveMismatchCount++; } if (PlsrDiagnostic.firstMismatchSample == 0xFFFFFFFFUL) { PlsrDiagnostic.firstMismatchSample = PlsrDiagnostic.curveSampleCount; } if (error > PlsrDiagnostic.curveMaxAbsErrorHz) { PlsrDiagnostic.curveMaxAbsErrorHz = error; } PlsrDiagnosticLatch(PLSR_DIAG_REASON_CURVE); } } #endif static uint8_t PlsrDiagnosticStartPulse(uint8_t pulseOutput, uint32_t requestedFirstHz, uint32_t requestedQueuedHz, uint32_t *actualFirstHz, uint32_t *actualQueuedHz) { PLSR_PLATFORM_TIMER_SETTING firstSetting; PLSR_PLATFORM_TIMER_SETTING queuedSetting; if ((PlsrPlatformBuildTimerSetting( pulseOutput, (uint8_t)PlsrActiveConfig.outputMode, requestedFirstHz, &firstSetting) == 0U) || (PlsrPlatformBuildTimerSetting( pulseOutput, (uint8_t)PlsrActiveConfig.outputMode, requestedQueuedHz, &queuedSetting) == 0U) || (PlsrPlatformStartPrepared(pulseOutput, &firstSetting, &queuedSetting, actualFirstHz, actualQueuedHz) == 0U)) { PlsrDiagnosticFinishSegment(0U); return 0U; } #if PLSR_DIAGNOSTIC_ENABLED PlsrDiagnosticRecordFrequency(requestedFirstHz, firstSetting.actualFrequencyHz, *actualFirstHz); PlsrDiagnosticRecordCurveSample(firstSetting.actualFrequencyHz, *actualFirstHz); PlsrDiagnosticQueuedExpectedHz = queuedSetting.actualFrequencyHz; #endif return 1U; } static PLSR_PLATFORM_QUEUE_RESULT PlsrDiagnosticQueueFrequency( uint8_t pulseOutput, uint32_t requestedHz, uint32_t *actualHz) { PLSR_PLATFORM_QUEUE_RESULT result; #if PLSR_DIAGNOSTIC_ENABLED uint32_t expectedHz; if (PlsrPlatformExpectedFrequency( pulseOutput, (uint8_t)PlsrActiveConfig.outputMode, requestedHz, &expectedHz) == 0U) { PlsrDiagnosticFinishSegment(0U); return PLSR_PLATFORM_QUEUE_FAILED; } #endif result = PlsrPlatformQueueFrequency(pulseOutput, requestedHz, actualHz); if (result == PLSR_PLATFORM_QUEUE_FAILED) { PlsrDiagnosticFinishSegment(0U); return result; } if (result == PLSR_PLATFORM_QUEUE_STALE) { return result; } #if PLSR_DIAGNOSTIC_ENABLED PlsrDiagnosticRecordFrequency(requestedHz, expectedHz, *actualHz); PlsrDiagnosticQueuedExpectedHz = expectedHz; #endif return PLSR_PLATFORM_QUEUE_APPLIED; } static void PlsrDiagnosticCheckActiveFrequency(uint32_t activeHz) { #if PLSR_DIAGNOSTIC_ENABLED uint16_t platformFault = PlsrPlatformDiagnosticFault(); uint32_t observedHz = activeHz; if (platformFault == PLSR_DIAG_REASON_FREQUENCY) { observedHz = PlsrPlatformActiveFrequency( (uint8_t)PlsrActiveConfig.pulseOutput); if (observedHz == 0UL) { observedHz = activeHz; } PlsrDiagnosticFailedChecks |= PLSR_DIAG_FLAG_FREQUENCY_PASS; PlsrDiagnostic.flags |= PLSR_DIAG_FLAG_FREQUENCY_CHECKED; PlsrDiagnostic.flags &= (uint16_t)~PLSR_DIAG_FLAG_FREQUENCY_PASS; PlsrDiagnosticLatch(PLSR_DIAG_REASON_FREQUENCY); } if ((platformFault != PLSR_DIAG_REASON_NONE) && (PlsrDiagnosticQueuedExpectedHz != 0UL)) { PlsrDiagnostic.activeTimerHz = observedHz; if (platformFault == PLSR_DIAG_REASON_CURVE) { if (PlsrDiagnostic.curveSampleCount != 0xFFFFFFFFUL) { PlsrDiagnostic.curveSampleCount++; } PlsrDiagnostic.flags |= PLSR_DIAG_FLAG_CURVE_CHECKED; PlsrDiagnostic.flags &= (uint16_t)~PLSR_DIAG_FLAG_CURVE_PASS; PlsrDiagnosticFailedChecks |= PLSR_DIAG_FLAG_CURVE_PASS; if (PlsrDiagnostic.curveMismatchCount != 0xFFFFFFFFUL) { PlsrDiagnostic.curveMismatchCount++; } if (PlsrDiagnostic.firstMismatchSample == 0xFFFFFFFFUL) { PlsrDiagnostic.firstMismatchSample = PlsrDiagnostic.curveSampleCount; } PlsrDiagnosticLatch(PLSR_DIAG_REASON_CURVE); } else if (platformFault == PLSR_DIAG_REASON_FREQUENCY) { PlsrDiagnosticRecordCurveSample(PlsrDiagnosticQueuedExpectedHz, observedHz); } } #else (void)activeHz; #endif } static void PlsrSetDefaults(PLSR_CONFIG *config) { uint8_t index; (void)memset(config, 0, sizeof(*config)); config->pulseOutput = 0U; config->directionOutput = 0U; config->waitInput = 0U; config->extInput = 0U; config->sendMode = PLSR_SEND_COMPLETE; config->directionDelayMs = 10U; config->directionNegativeLogic = 0U; config->curveMode = 0U; config->positionMode = PLSR_POSITION_RELATIVE; config->segmentCount = 1U; config->startSegment = 1U; config->defaultSpeedHz = 1000UL; config->startSpeedHz = 100UL; config->stopSpeedHz = 100UL; config->accelerationTimeMs = 100U; config->decelerationTimeMs = 100U; config->outputMode = PLSR_OUTPUT_PULSE_DIR; for (index = 0U; index < PLSR_SEGMENT_COUNT_MAX; index++) { config->segments[index].frequencyHz = 1000UL; config->segments[index].pulses = (index == 0U) ? 1000L : 0L; config->segments[index].waitType = PLSR_EXT_OR_COMPLETE; config->segments[index].waitTimeMs = 0U; config->segments[index].actTimeMs = 0U; config->segments[index].jumpSegment = 0U; } } static uint8_t PlsrConfigIsValid(const PLSR_CONFIG *config, uint8_t validateActivePath) { uint8_t index; if ((config->pulseOutput > 3U) || (config->directionOutput > 3U) || (config->waitInput > 1U) || (config->extInput > 1U) || (config->sendMode > PLSR_SEND_SUBSEQUENT) || (config->directionNegativeLogic > 1U) || (config->curveMode > 2U) || (config->positionMode > PLSR_POSITION_ABSOLUTE) || (config->outputMode > PLSR_OUTPUT_AB) || ((config->outputMode == PLSR_OUTPUT_AB) && ((config->pulseOutput & 1U) != 0U)) || (config->segmentCount == 0U) || (config->segmentCount > PLSR_SEGMENT_COUNT_MAX) || (config->startSegment == 0U) || (config->startSegment > PLSR_SEGMENT_COUNT_MAX) || (config->defaultSpeedHz == 0UL) || (config->defaultSpeedHz > PLSR_FREQUENCY_MAX_HZ) || (config->startSpeedHz > PLSR_FREQUENCY_MAX_HZ) || (config->stopSpeedHz > PLSR_FREQUENCY_MAX_HZ)) { return 0U; } if ((validateActivePath != 0U) && (config->startSegment > config->segmentCount)) { return 0U; } for (index = 0U; index < PLSR_SEGMENT_COUNT_MAX; index++) { const PLSR_SEGMENT_CONFIG *segment = &config->segments[index]; if ((segment->frequencyHz > PLSR_FREQUENCY_MAX_HZ) || (segment->waitType > PLSR_EXT_OR_COMPLETE) || (segment->jumpSegment > PLSR_SEGMENT_COUNT_MAX)) { return 0U; } if ((validateActivePath != 0U) && (index < config->segmentCount) && (segment->jumpSegment > config->segmentCount)) { return 0U; } } return 1U; } static uint16_t PlsrReadConfigWord(const PLSR_CONFIG *config, uint16_t address) { uint16_t offset; uint8_t segmentIndex; const PLSR_SEGMENT_CONFIG *segment; switch (address) { case 0x1000U: return config->pulseOutput; case 0x1001U: return config->directionOutput; case 0x1002U: return config->waitInput; case 0x1003U: return config->extInput; case 0x1004U: return config->sendMode; case 0x1005U: return config->directionDelayMs; case 0x1006U: return config->directionNegativeLogic; case 0x1007U: return config->curveMode; case 0x1008U: return config->positionMode; case 0x1009U: return config->segmentCount; case 0x100AU: return config->startSegment; case 0x100BU: return PlsrLowWord(config->defaultSpeedHz); case 0x100CU: return PlsrHighWord(config->defaultSpeedHz); case 0x100DU: return PlsrLowWord(config->startSpeedHz); case 0x100EU: return PlsrHighWord(config->startSpeedHz); case 0x1010U: return PlsrLowWord(config->stopSpeedHz); case 0x1011U: return PlsrHighWord(config->stopSpeedHz); case 0x1012U: return config->accelerationTimeMs; case 0x1013U: return config->decelerationTimeMs; default: break; } if ((address >= PLSR_SEGMENT_FIRST_ADDRESS) && (address <= PLSR_CONFIG_LAST_ADDRESS)) { offset = (uint16_t)(address - PLSR_SEGMENT_FIRST_ADDRESS); segmentIndex = (uint8_t)(offset / PLSR_SEGMENT_STRIDE); offset = (uint16_t)(offset % PLSR_SEGMENT_STRIDE); segment = &config->segments[segmentIndex]; switch (offset) { case 0U: return PlsrLowWord(segment->frequencyHz); case 1U: return PlsrHighWord(segment->frequencyHz); case 2U: return PlsrLowWord((uint32_t)segment->pulses); case 3U: return PlsrHighWord((uint32_t)segment->pulses); case 4U: return segment->waitType; case 5U: return segment->waitTimeMs; case 6U: return segment->actTimeMs; case 7U: return segment->jumpSegment; default: return 0U; } } /* 0x100F, 0x1014..0x10FF, and segment padding read as zero. */ return 0U; } static PLSR_WORD_RESULT PlsrWriteConfigWord(PLSR_CONFIG *config, uint16_t address, uint16_t value) { uint16_t offset; uint8_t segmentIndex; PLSR_SEGMENT_CONFIG *segment; switch (address) { case 0x1000U: config->pulseOutput = value; return PLSR_WORD_OK; case 0x1001U: config->directionOutput = value; return PLSR_WORD_OK; case 0x1002U: config->waitInput = value; return PLSR_WORD_OK; case 0x1003U: config->extInput = value; return PLSR_WORD_OK; case 0x1004U: config->sendMode = value; return PLSR_WORD_OK; case 0x1005U: config->directionDelayMs = value; return PLSR_WORD_OK; case 0x1006U: config->directionNegativeLogic = value; return PLSR_WORD_OK; case 0x1007U: config->curveMode = value; return PLSR_WORD_OK; case 0x1008U: config->positionMode = value; return PLSR_WORD_OK; case 0x1009U: config->segmentCount = value; return PLSR_WORD_OK; case 0x100AU: config->startSegment = value; return PLSR_WORD_OK; case 0x100BU: config->defaultSpeedHz = PlsrJoinU32(value, PlsrHighWord(config->defaultSpeedHz)); return PLSR_WORD_OK; case 0x100CU: config->defaultSpeedHz = PlsrJoinU32(PlsrLowWord(config->defaultSpeedHz), value); return PLSR_WORD_OK; case 0x100DU: config->startSpeedHz = PlsrJoinU32(value, PlsrHighWord(config->startSpeedHz)); return PLSR_WORD_OK; case 0x100EU: config->startSpeedHz = PlsrJoinU32(PlsrLowWord(config->startSpeedHz), value); return PLSR_WORD_OK; case 0x100FU: return (value == 0U) ? PLSR_WORD_OK : PLSR_WORD_ILLEGAL_VALUE; case 0x1010U: config->stopSpeedHz = PlsrJoinU32(value, PlsrHighWord(config->stopSpeedHz)); return PLSR_WORD_OK; case 0x1011U: config->stopSpeedHz = PlsrJoinU32(PlsrLowWord(config->stopSpeedHz), value); return PLSR_WORD_OK; case 0x1012U: config->accelerationTimeMs = value; return PLSR_WORD_OK; case 0x1013U: config->decelerationTimeMs = value; return PLSR_WORD_OK; default: break; } if ((address >= 0x1014U) && (address <= PLSR_COMMON_LAST_ADDRESS)) { return (value == 0U) ? PLSR_WORD_OK : PLSR_WORD_ILLEGAL_VALUE; } if ((address < PLSR_SEGMENT_FIRST_ADDRESS) || (address > PLSR_CONFIG_LAST_ADDRESS)) { return PLSR_WORD_ILLEGAL_ADDRESS; } offset = (uint16_t)(address - PLSR_SEGMENT_FIRST_ADDRESS); segmentIndex = (uint8_t)(offset / PLSR_SEGMENT_STRIDE); offset = (uint16_t)(offset % PLSR_SEGMENT_STRIDE); segment = &config->segments[segmentIndex]; switch (offset) { case 0U: segment->frequencyHz = PlsrJoinU32(value, PlsrHighWord(segment->frequencyHz)); return PLSR_WORD_OK; case 1U: segment->frequencyHz = PlsrJoinU32(PlsrLowWord(segment->frequencyHz), value); return PLSR_WORD_OK; case 2U: segment->pulses = (int32_t)PlsrJoinU32( value, PlsrHighWord((uint32_t)segment->pulses)); return PLSR_WORD_OK; case 3U: segment->pulses = (int32_t)PlsrJoinU32( PlsrLowWord((uint32_t)segment->pulses), value); return PLSR_WORD_OK; case 4U: segment->waitType = value; return PLSR_WORD_OK; case 5U: segment->waitTimeMs = value; return PLSR_WORD_OK; case 6U: segment->actTimeMs = value; return PLSR_WORD_OK; case 7U: segment->jumpSegment = value; return PLSR_WORD_OK; default: return (value == 0U) ? PLSR_WORD_OK : PLSR_WORD_ILLEGAL_VALUE; } } static uint8_t PlsrAddressIsDwordHalf(uint16_t address, uint16_t *pairedAddress) { uint16_t offset; switch (address) { case 0x100BU: case 0x100DU: case 0x1010U: *pairedAddress = (uint16_t)(address + 1U); return 1U; case 0x100CU: case 0x100EU: case 0x1011U: *pairedAddress = (uint16_t)(address - 1U); return 1U; default: break; } if ((address >= PLSR_SEGMENT_FIRST_ADDRESS) && (address <= PLSR_CONFIG_LAST_ADDRESS)) { offset = (uint16_t)((address - PLSR_SEGMENT_FIRST_ADDRESS) % PLSR_SEGMENT_STRIDE); if ((offset == 0U) || (offset == 2U)) { *pairedAddress = (uint16_t)(address + 1U); return 1U; } if ((offset == 1U) || (offset == 3U)) { *pairedAddress = (uint16_t)(address - 1U); return 1U; } } return 0U; } static uint8_t PlsrAddressIsProduct(uint16_t address) { return (((address >= PLSR_CONFIG_FIRST_ADDRESS) && (address <= PLSR_CONFIG_LAST_ADDRESS)) || (address == PLSR_OUTPUT_MODE_ADDRESS) || ((address >= PLSR_STATUS_FIRST_ADDRESS) && (address <= PLSR_STATUS_LAST_ADDRESS)) || ((address >= PLSR_DIAGNOSTIC_FIRST_ADDRESS) && (address <= PLSR_DIAGNOSTIC_LAST_ADDRESS)) || ((address >= PLSR_QUEUE_DIAGNOSTIC_FIRST_ADDRESS) && (address <= PLSR_QUEUE_DIAGNOSTIC_LAST_ADDRESS)) || (address == PLSR_CONTROL_ADDRESS) || (address == PLSR_DIAGNOSTIC_CONTROL_ADDRESS)) ? 1U : 0U; } static PLSR_MB_RESULT PlsrClassifyRange(uint16_t startAddress, uint16_t quantity) { uint32_t address; uint32_t endAddress; uint8_t foundProduct = 0U; uint8_t foundOther = 0U; if (quantity == 0U) { return PLSR_MB_ILLEGAL_VALUE; } endAddress = (uint32_t)startAddress + (uint32_t)quantity - 1UL; if (endAddress > 0xFFFFUL) { return PLSR_MB_ILLEGAL_ADDRESS; } for (address = startAddress; address <= endAddress; address++) { if (PlsrAddressIsProduct((uint16_t)address) != 0U) { foundProduct = 1U; } else { foundOther = 1U; } } if (foundProduct == 0U) { return PLSR_MB_NOT_HANDLED; } return (foundOther != 0U) ? PLSR_MB_ILLEGAL_ADDRESS : PLSR_MB_OK; } static uint32_t PlsrCurveProgressQ16(uint32_t elapsed, uint32_t duration, uint16_t curveMode) { uint32_t linear; if ((duration == 0UL) || (elapsed >= duration)) { return 65535UL; } linear = (uint32_t)(((uint64_t)elapsed * 65535UL) / duration); if (curveMode == 1U) { uint64_t x = linear; uint64_t x2 = (x * x) / 65535UL; return (uint32_t)((x2 * (196605UL - 2UL * x)) / 65535UL); } if (curveMode == 2U) { uint32_t scaled = linear * 64UL; uint32_t index = scaled / 65535UL; uint32_t fraction = scaled % 65535UL; uint32_t first; uint32_t second; if (index >= 64UL) { return 65535UL; } first = PlsrSineProgressQ16[index]; second = PlsrSineProgressQ16[index + 1UL]; return first + (uint32_t)(((uint64_t)(second - first) * fraction) / 65535UL); } return linear; } static uint32_t PlsrRampDurationMs(uint32_t fromHz, uint32_t toHz) { uint32_t gap; uint32_t baseTimeMs; uint64_t duration; if (fromHz == toHz) { return 0UL; } gap = (fromHz > toHz) ? (fromHz - toHz) : (toHz - fromHz); baseTimeMs = (toHz > fromHz) ? PlsrActiveConfig.accelerationTimeMs : PlsrActiveConfig.decelerationTimeMs; if (baseTimeMs == 0UL) { return 0UL; } duration = ((uint64_t)gap * baseTimeMs + PlsrActiveConfig.defaultSpeedHz - 1UL) / PlsrActiveConfig.defaultSpeedHz; if (duration > 0xFFFFFFFFUL) { return 0xFFFFFFFFUL; } return (uint32_t)duration; } static uint32_t PlsrEffectiveStartFrequency(uint32_t segmentFrequencyHz, uint8_t allowCarry, uint8_t directionChanged, uint32_t carryFrequencyHz) { if ((allowCarry != 0U) && (directionChanged == 0U) && (carryFrequencyHz != 0UL)) { return carryFrequencyHz; } if (PlsrActiveConfig.accelerationTimeMs == 0U) { return segmentFrequencyHz; } return PlsrActiveConfig.startSpeedHz; } static uint32_t PlsrEffectiveStopFrequency(uint32_t segmentFrequencyHz) { return (PlsrActiveConfig.decelerationTimeMs == 0U) ? segmentFrequencyHz : PlsrActiveConfig.stopSpeedHz; } static void PlsrRampStart(uint32_t fromHz, uint32_t toHz) { PlsrRamp.fromHz = fromHz; PlsrRamp.toHz = toHz; PlsrRamp.durationMs = PlsrRampDurationMs(fromHz, toHz); PlsrRamp.elapsedMs = 0UL; PlsrRamp.active = (PlsrRamp.durationMs != 0UL) ? 1U : 0U; if (toHz > fromHz) { PlsrRunStatus = PLSR_STATUS_ACCELERATING; } else if (toHz < fromHz) { PlsrRunStatus = PLSR_STATUS_DECELERATING; } else { PlsrRunStatus = PLSR_STATUS_RUNNING; } } static uint8_t PlsrApplyFrequencyPair(uint32_t requestedFirstFrequencyHz, uint32_t requestedQueuedFrequencyHz, uint32_t expectedEpoch) { uint32_t criticalState; uint32_t actualFirstFrequencyHz; uint32_t actualQueuedFrequencyHz; uint32_t hardwareFirstFrequencyHz = requestedFirstFrequencyHz; uint32_t hardwareQueuedFrequencyHz = requestedQueuedFrequencyHz; PLSR_HANDOFF_PLAN candidatePlan; uint8_t haveCandidatePlan = 0U; criticalState = PlsrPlatformEnterCritical(); if ((PlsrSegmentEpoch != expectedEpoch) || (PlsrBoundaryPending != 0U) || (PlsrRemainingPulses == 0UL)) { PlsrPlatformExitCritical(criticalState); return 1U; } if (hardwareFirstFrequencyHz == 0UL) { if (PlsrPulseActive == 0U) { PlsrCurrentFrequencyHz = 0UL; PlsrQueuedFrequencyHz = 0UL; PlsrPlatformExitCritical(criticalState); return 1U; } hardwareFirstFrequencyHz = 1UL; } if (hardwareQueuedFrequencyHz == 0UL) { hardwareQueuedFrequencyHz = 1UL; } if (PlsrPulseActive != 0U) { if (PlsrAbStopArmed != 0U) { PlsrPlatformExitCritical(criticalState); return 1U; } if (PlsrHandoffPlan.valid != 0U) { PlsrPlatformExitCritical(criticalState); return 1U; } PlsrDeferredFrequencyHz = hardwareQueuedFrequencyHz; PlsrDeferredFrequencyPending = 1U; PlsrPlatformExitCritical(criticalState); return 1U; } else { if (PlsrSelectPreparedHandoffPlan(&candidatePlan) != 0U) { hardwareQueuedFrequencyHz = candidatePlan.firstFrequencyHz; haveCandidatePlan = 1U; } if (PlsrDiagnosticStartPulse( (uint8_t)PlsrActiveConfig.pulseOutput, hardwareFirstFrequencyHz, hardwareQueuedFrequencyHz, &actualFirstFrequencyHz, &actualQueuedFrequencyHz) == 0U) { PlsrPlatformExitCritical(criticalState); return 0U; } PlsrPulseActive = 1U; PlsrCurrentFrequencyHz = actualFirstFrequencyHz; PlsrHandoffPlan.valid = 0U; if (haveCandidatePlan != 0U) { PlsrHandoffPlan = candidatePlan; PlsrHandoffPlan.firstFrequencyHz = actualQueuedFrequencyHz; PlsrHandoffPlan.valid = 1U; } } PlsrQueuedFrequencyHz = actualQueuedFrequencyHz; if (PlsrArmFinalAbBoundaryLocked() == 0U) { PlsrPlatformExitCritical(criticalState); return 0U; } PlsrPlatformExitCritical(criticalState); return 1U; } static PLSR_PLATFORM_QUEUE_RESULT PlsrApplyFrequency( uint32_t requestedFrequencyHz, uint32_t expectedEpoch) { return (PlsrApplyFrequencyPair(requestedFrequencyHz, requestedFrequencyHz, expectedEpoch) != 0U) ? PLSR_PLATFORM_QUEUE_APPLIED : PLSR_PLATFORM_QUEUE_FAILED; } static uint8_t PlsrStopDrainPulseCount(void) { uint8_t deferredPending = PlsrDeferredFrequencyPending; uint32_t deferredFrequencyHz = PlsrDeferredFrequencyHz; uint32_t queuedFrequencyHz = PlsrQueuedFrequencyHz; uint32_t currentFrequencyHz = PlsrCurrentFrequencyHz; if ((deferredPending != 0U) && (deferredFrequencyHz != queuedFrequencyHz)) { return 3U; } PlsrDeferredFrequencyPending = 0U; return (currentFrequencyHz == queuedFrequencyHz) ? 1U : 2U; } static uint8_t PlsrArmStopDrainLocked(void) { uint32_t drainPulses = PlsrStopDrainPulseCount(); PlsrStopPulsesRemaining = (uint8_t)drainPulses; return PlsrArmFinalAbBoundaryLocked(); } static PLSR_PLATFORM_QUEUE_RESULT PlsrCommitDeferredFrequency( uint8_t pulseOutput) { uint32_t requestedFrequencyHz; uint32_t actualFrequencyHz; PLSR_PLATFORM_QUEUE_RESULT result; if (PlsrDeferredFrequencyPending == 0U) { return PLSR_PLATFORM_QUEUE_APPLIED; } requestedFrequencyHz = PlsrDeferredFrequencyHz; result = PlsrDiagnosticQueueFrequency(pulseOutput, requestedFrequencyHz, &actualFrequencyHz); if (result != PLSR_PLATFORM_QUEUE_APPLIED) { return result; } if ((PlsrDeferredFrequencyPending != 0U) && (PlsrDeferredFrequencyHz == requestedFrequencyHz)) { PlsrDeferredFrequencyPending = 0U; } PlsrQueuedFrequencyHz = actualFrequencyHz; return PLSR_PLATFORM_QUEUE_APPLIED; } static uint8_t PlsrRampAdvance(uint32_t expectedEpoch) { uint32_t progress; uint32_t frequency; uint32_t gap; uint32_t criticalState; uint32_t fromHz; uint32_t toHz; uint32_t durationMs; uint32_t elapsedMs; PLSR_PLATFORM_QUEUE_RESULT applyResult; criticalState = PlsrPlatformEnterCritical(); if ((PlsrSegmentEpoch != expectedEpoch) || (PlsrRamp.active == 0U)) { PlsrPlatformExitCritical(criticalState); return 1U; } if (PlsrRamp.elapsedMs < PlsrRamp.durationMs) { PlsrRamp.elapsedMs++; } fromHz = PlsrRamp.fromHz; toHz = PlsrRamp.toHz; durationMs = PlsrRamp.durationMs; elapsedMs = PlsrRamp.elapsedMs; PlsrPlatformExitCritical(criticalState); progress = PlsrCurveProgressQ16(elapsedMs, durationMs, PlsrActiveConfig.curveMode); if (toHz >= fromHz) { gap = toHz - fromHz; frequency = fromHz + (uint32_t)(((uint64_t)gap * progress) / 65535UL); } else { gap = fromHz - toHz; frequency = fromHz - (uint32_t)(((uint64_t)gap * progress) / 65535UL); } applyResult = PlsrApplyFrequency(frequency, expectedEpoch); if (applyResult == PLSR_PLATFORM_QUEUE_FAILED) { return 0U; } if (applyResult == PLSR_PLATFORM_QUEUE_STALE) { return 1U; } criticalState = PlsrPlatformEnterCritical(); if (PlsrSegmentEpoch != expectedEpoch) { PlsrPlatformExitCritical(criticalState); return 1U; } if (elapsedMs >= durationMs) { PlsrRamp.active = 0U; if (PlsrStopRequested == 0U) { PlsrRunStatus = PLSR_STATUS_RUNNING; } else { (void)PlsrArmStopDrainLocked(); } } PlsrPlatformExitCritical(criticalState); return 1U; } static uint8_t PlsrGetNextSegment(uint8_t *nextSegment) { const PLSR_SEGMENT_CONFIG *segment = &PlsrActiveConfig.segments[PlsrCurrentSegment - 1U]; if (segment->jumpSegment != 0U) { *nextSegment = (uint8_t)segment->jumpSegment; return 1U; } if (PlsrCurrentSegment < PlsrActiveConfig.segmentCount) { *nextSegment = (uint8_t)(PlsrCurrentSegment + 1U); return 1U; } return 0U; } static uint32_t PlsrResolvedSegmentFrequency(const PLSR_CONFIG *config, uint8_t segmentIndex) { uint32_t frequencyHz = config->segments[segmentIndex].frequencyHz; return (frequencyHz == 0UL) ? config->defaultSpeedHz : frequencyHz; } static int64_t PlsrSegmentDisplacement(uint8_t segmentNumber, int32_t referencePosition) { int32_t configured = PlsrActiveConfig.segments[segmentNumber - 1U].pulses; if (PlsrActiveConfig.positionMode == PLSR_POSITION_ABSOLUTE) { return (int64_t)configured - (int64_t)referencePosition; } return configured; } static uint8_t PlsrPredictNextDirection(uint8_t nextSegment, uint8_t *positive) { uint32_t criticalState; uint64_t remaining; int32_t position; uint32_t predictedBits; int32_t predictedPosition; int64_t displacement; criticalState = PlsrPlatformEnterCritical(); remaining = PlsrRemainingPulses; position = PlsrPosition; PlsrPlatformExitCritical(criticalState); predictedBits = (uint32_t)position; if (PlsrCountPositive != 0U) { predictedBits += (uint32_t)remaining; } else { predictedBits -= (uint32_t)remaining; } predictedPosition = (int32_t)predictedBits; displacement = PlsrSegmentDisplacement(nextSegment, predictedPosition); if (displacement == 0) { return 0U; } *positive = (displacement > 0) ? 1U : 0U; return 1U; } static uint64_t PlsrRemainingSnapshot(void) { uint32_t criticalState = PlsrPlatformEnterCritical(); uint64_t remaining = PlsrRemainingPulses; PlsrPlatformExitCritical(criticalState); return remaining; } static uint64_t PlsrRampPulseEstimate(uint32_t fromHz, uint32_t toHz, uint32_t rateBoundHz) { uint32_t duration = PlsrRampDurationMs(fromHz, toHz); uint64_t sum = (uint64_t)fromHz + toHz; uint64_t discreteArea = sum * duration; uint64_t rampArea; uint64_t pollReserve = 0ULL; uint64_t candidateReserve = ((uint64_t)rateBoundHz + 999UL) / 1000UL; uint64_t endpointCorrection = 0ULL; /* RampAdvance applies samples at 1/duration through duration/duration. For all supported symmetric curves this right-endpoint sum differs from the continuous trapezoid by half the endpoint delta. */ if (toHz >= fromHz) { discreteArea += (uint64_t)toHz - fromHz; } else { discreteArea -= (uint64_t)fromHz - toHz; endpointCorrection = ((uint64_t)fromHz - toHz + 1999UL) / 2000UL; } rampArea = (discreteArea + 1999UL) / 2000UL; if (candidateReserve <= rampArea / PLSR_RAMP_POLL_RESERVE_AREA_DIVISOR) { pollReserve = candidateReserve + endpointCorrection; } /* A full poll reserve on shorter ramps would dominate their area and leave stop-speed tail pulses. */ return rampArea + pollReserve; } static uint8_t PlsrPrepareShortProfile(PLSR_SHORT_PROFILE *profile, uint8_t segmentNumber, uint32_t startFrequencyHz, uint32_t targetFrequencyHz, uint64_t pulseCount) { PLSR_MOTION_BLOCK block; PLSR_PLANNER_STATUS status; const PLSR_SEGMENT_CONFIG *segment; uint8_t nextSegment = 0U; uint8_t currentPositive; uint8_t nextPositive; (void)memset(profile, 0, sizeof(*profile)); if ((PlsrActiveConfig.outputMode != PLSR_OUTPUT_PULSE_DIR) || (pulseCount == 0ULL) || (pulseCount > 0xFFFFFFFFULL) || (segmentNumber == 0U) || (segmentNumber > PlsrActiveConfig.segmentCount)) { return 0U; } block.entryHz = startFrequencyHz; block.cruiseHz = targetFrequencyHz; block.exitHz = PlsrEffectiveStopFrequency(targetFrequencyHz); block.pulseBudget = (uint32_t)pulseCount; block.referenceSpeedHz = PlsrActiveConfig.defaultSpeedHz; block.accelerationTimeMs = PlsrActiveConfig.accelerationTimeMs; block.decelerationTimeMs = PlsrActiveConfig.decelerationTimeMs; block.curveMode = PlsrActiveConfig.curveMode; block.pulseOutput = (uint8_t)PlsrActiveConfig.pulseOutput; block.boundary = PLSR_BOUNDARY_STOP; segment = &PlsrActiveConfig.segments[segmentNumber - 1U]; if (segment->waitType == PLSR_EXT_OR_COMPLETE) { nextSegment = (segment->jumpSegment != 0U) ? (uint8_t)segment->jumpSegment : ((segmentNumber < PlsrActiveConfig.segmentCount) ? (uint8_t)(segmentNumber + 1U) : 0U); } if (nextSegment != 0U) { currentPositive = PlsrCountPositive; if (PlsrActiveConfig.positionMode == PLSR_POSITION_RELATIVE) { currentPositive = (segment->pulses >= 0L) ? 1U : 0U; nextPositive = (PlsrActiveConfig.segments[nextSegment - 1U].pulses >= 0L) ? 1U : 0U; } else if ((segmentNumber == PlsrCurrentSegment) && (PlsrPredictNextDirection(nextSegment, &nextPositive) != 0U)) { /* direction resolved from the live absolute position */ } else { nextPositive = (uint8_t)(currentPositive ^ 1U); } if (nextPositive == currentPositive) { block.exitHz = targetFrequencyHz; block.boundary = PLSR_BOUNDARY_NEXT_CARRY; } } status = PlsrPlannerBegin(&profile->planner, &block, startFrequencyHz, 0ULL); if ((status != PLSR_PLANNER_OK) && (status != PLSR_PLANNER_CLIPPED)) { return 0U; } profile->startHz = profile->planner.startHz; profile->peakHz = profile->planner.peakHz; profile->endHz = profile->planner.endHz; profile->pulseCount = profile->planner.block.pulseBudget; profile->entryPulses = profile->planner.entryPulses; profile->steadyPulses = profile->planner.steadyPulses; profile->exitPulses = profile->planner.exitPulses; profile->active = 1U; return 1U; } static uint8_t PlsrShortProfileTakeRun(PLSR_SHORT_PROFILE *profile, PLSR_PROFILE_ENTRY *entry) { uint32_t repeatCount; if ((profile->nextPeriod >= profile->pulseCount) || (entry == NULL)) { return 0U; } if (profile->pendingRepeats == 0UL) { if (PlsrPlannerGenerate(&profile->planner, &profile->pendingItem, 1U) == 0U) { profile->active = 0U; return 0U; } profile->pendingRepeats = profile->pendingItem.repeatCount; } repeatCount = profile->pendingRepeats; if (repeatCount > profile->pulseCount - profile->nextPeriod) { repeatCount = profile->pulseCount - profile->nextPeriod; } if (repeatCount == 0UL) { profile->active = 0U; return 0U; } entry->setting = profile->pendingItem.setting; entry->requestedFrequencyHz = profile->pendingItem.requestedFrequencyHz; entry->repeatCount = repeatCount; entry->startsNextSegment = 0U; profile->pendingRepeats -= repeatCount; profile->nextPeriod += repeatCount; profile->lastRampFrequencyHz = entry->setting.actualFrequencyHz; if (profile->nextPeriod >= profile->pulseCount) { profile->active = 0U; } return 1U; } /* Build the replacement stream in the inactive queue bank while the IRQ keeps consuming the published bank. A bounded prefix from the old queue bridges the construction interval; the final bank flip is the only critical part. */ static uint8_t PlsrReplanPulseDir(uint32_t targetHz, uint32_t totalPulses) { PLSR_PROFILE_QUEUE *source; PLSR_PROFILE_QUEUE *destination; PLSR_SHORT_PROFILE replacement; PLSR_SHORT_PROFILE candidate; PLSR_PROFILE_ENTRY entry; uint64_t prefixTimeUs = 0ULL; uint64_t prefixPulses = 0ULL; uint32_t committedPulses; uint32_t tailFrequencyHz; uint32_t sourceGeneration; uint32_t snapshotReadIndex; uint32_t snapshotWriteIndex; uint32_t currentReadIndex; uint32_t availablePlanPulses; uint32_t replacementPulses; uint32_t prefixCount = 0UL; uint32_t consumedPrefix; uint32_t sourceIndex; uint32_t criticalState; uint32_t epoch; uint8_t segmentNumber; uint8_t handoffBank; uint8_t sourceBank; uint8_t destinationBank; uint8_t boundaryWriteIndex; uint8_t pipelineStartsNextSegment; uint8_t tailWasClipped = 0U; uint16_t fillBudget = PLSR_REPLAN_BUILD_ITEMS; criticalState = PlsrPlatformEnterCritical(); sourceBank = PlsrProfileQueueBank; destinationBank = (uint8_t)(sourceBank ^ 1U); source = &PlsrProfileQueues[sourceBank]; destination = &PlsrProfileQueues[destinationBank]; sourceGeneration = source->generation; snapshotReadIndex = source->readIndex; snapshotWriteIndex = source->writeIndex; epoch = PlsrSegmentEpoch; segmentNumber = PlsrCurrentSegment; handoffBank = PlsrPreparedHandoffBank; boundaryWriteIndex = PlsrCountedBoundaryWriteIndex; if ((PlsrPulseActive == 0U) || (segmentNumber == 0U) || (segmentNumber > PlsrActiveConfig.segmentCount) || (boundaryWriteIndex != PlsrCountedBoundaryReadIndex) || (PlsrCountedFaultEvent != 0U) || (PlsrPlatformFinitePipelineSnapshot( (uint8_t)PlsrActiveConfig.pulseOutput, &committedPulses, &tailFrequencyHz, &pipelineStartsNextSegment) == 0U) || (pipelineStartsNextSegment != 0U)) { PlsrPlatformExitCritical(criticalState); return 0U; } PlsrPlatformExitCritical(criticalState); if (totalPulses < committedPulses) { totalPulses = committedPulses; } availablePlanPulses = totalPulses - committedPulses; destination->readIndex = 0UL; destination->writeIndex = 0UL; destination->repeatRemaining = 0UL; destination->producerEpoch = epoch; destination->producerSegment = segmentNumber; destination->preparedHandoffBank = handoffBank; destination->generatorComplete = 0U; destination->active = 0U; destination->producerProfile.active = 0U; sourceIndex = snapshotReadIndex; while ((sourceIndex != snapshotWriteIndex) && (prefixCount < PLSR_REPLAN_PREFIX_ITEMS) && (prefixPulses < availablePlanPulses) && (prefixTimeUs < PLSR_REPLAN_PREFIX_TIME_US)) { uint32_t remainingPrefixPulses = availablePlanPulses - (uint32_t)prefixPulses; uint32_t frequencyHz; uint64_t remainingPrefixTimeUs; uint64_t timeLimitedPulses; entry = source->entries[sourceIndex & PLSR_PROFILE_QUEUE_MASK]; if ((entry.repeatCount == 0UL) || (entry.startsNextSegment != 0U)) { break; } frequencyHz = entry.setting.actualFrequencyHz; if (frequencyHz == 0UL) { return 0U; } remainingPrefixTimeUs = PLSR_REPLAN_PREFIX_TIME_US - prefixTimeUs; timeLimitedPulses = (remainingPrefixTimeUs * frequencyHz + 999999ULL) / 1000000ULL; if (timeLimitedPulses == 0ULL) { timeLimitedPulses = 1ULL; } if (entry.repeatCount > timeLimitedPulses) { entry.repeatCount = (uint32_t)timeLimitedPulses; tailWasClipped = 1U; } if (entry.repeatCount > remainingPrefixPulses) { entry.repeatCount = remainingPrefixPulses; tailWasClipped = 1U; } destination->entries[prefixCount] = entry; prefixCount++; sourceIndex++; prefixPulses += entry.repeatCount; prefixTimeUs += ((uint64_t)entry.repeatCount * 1000000ULL + frequencyHz - 1UL) / frequencyHz; tailFrequencyHz = frequencyHz; } replacementPulses = availablePlanPulses - (uint32_t)prefixPulses; if (replacementPulses != 0UL) { if ((tailFrequencyHz == 0UL) || (PlsrPrepareShortProfile(&replacement, segmentNumber, tailFrequencyHz, targetHz, replacementPulses) == 0U)) { return 0U; } PlsrCopyShortProfile(&destination->producerProfile, &replacement); destination->active = 1U; candidate = replacement; while ((fillBudget != 0U) && (prefixCount < PLSR_PROFILE_QUEUE_CAPACITY) && (candidate.active != 0U)) { fillBudget--; if (PlsrShortProfileTakeRun(&candidate, &entry) == 0U) { return 0U; } destination->entries[prefixCount++] = entry; } PlsrCopyShortProfile(&destination->producerProfile, &candidate); destination->generatorComplete = ((candidate.active == 0U) || (candidate.nextPeriod >= candidate.pulseCount)) ? 1U : 0U; } else { (void)memset(&replacement, 0, sizeof(replacement)); replacement.endHz = tailFrequencyHz; destination->generatorComplete = 1U; destination->active = (prefixCount != 0UL) ? 1U : 0U; } destination->writeIndex = prefixCount; criticalState = PlsrPlatformEnterCritical(); source = &PlsrProfileQueues[sourceBank]; currentReadIndex = source->readIndex; if ((PlsrProfileQueueBank != sourceBank) || (source->generation != sourceGeneration) || (source->writeIndex != snapshotWriteIndex) || (PlsrSegmentEpoch != epoch) || (PlsrCurrentSegment != segmentNumber) || (PlsrCountedBoundaryWriteIndex != boundaryWriteIndex) || (PlsrCountedFaultEvent != 0U) || (currentReadIndex < snapshotReadIndex) || ((currentReadIndex - snapshotReadIndex) > prefixCount) || ((tailWasClipped != 0U) && ((currentReadIndex - snapshotReadIndex) >= prefixCount))) { PlsrPlatformExitCritical(criticalState); return 0U; } consumedPrefix = currentReadIndex - snapshotReadIndex; destination->readIndex = consumedPrefix; destination->generation = sourceGeneration + 1UL; PlsrRamp.active = 0U; PlsrHandoffPlan.valid = 0U; PlsrCountedHandoffStaged = 0U; PlsrProfileQueueBank = destinationBank; PlsrCopyShortProfile(&PlsrShortProfile, &destination->producerProfile); PlsrRemainingPulses = totalPulses; PlsrProfileQueueMinimumDepth = (uint16_t)(prefixCount - consumedPrefix); PlsrProfileQueueLowWaterLatched = 0U; PlsrExecutor.generation++; PlsrExecutor.mode = (destination->generatorComplete != 0U) ? PLSR_EXEC_STEP_TABLE : PLSR_EXEC_STREAM; PlsrPlatformExitCritical(criticalState); PlsrProfileRecordPlannerStatus(&replacement); PlsrRefreshCurrentHandoffPlan(replacement.endHz); return 1U; } static void PlsrCopyShortProfile(PLSR_SHORT_PROFILE *destination, const PLSR_SHORT_PROFILE *source) { *destination = *source; } static uint16_t PlsrProfileQueueCount(void) { uint32_t writeIndex = PlsrProfileQueue.writeIndex; uint32_t readIndex = PlsrProfileQueue.readIndex; return (uint16_t)(writeIndex - readIndex); } static void PlsrProfileQueueRecordDepth(uint16_t depth) { if (depth < PlsrProfileQueueMinimumDepth) { PlsrProfileQueueMinimumDepth = depth; } if (depth <= PLSR_PROFILE_LOW_WATER) { PlsrProfileQueueDiagnosticFlags |= PLSR_QUEUE_DIAG_LOW_WATER; if (PlsrProfileQueueLowWaterLatched == 0U) { PlsrProfileQueueLowWaterLatched = 1U; if (PlsrProfileQueueLowWaterEvents != 0xFFFFFFFFUL) { PlsrProfileQueueLowWaterEvents++; } } } else if (depth >= PLSR_PROFILE_LOW_WATER_CLEAR) { PlsrProfileQueueLowWaterLatched = 0U; } } static void PlsrProfileQueueRecordUnderrun(void) { PlsrProfileQueueDiagnosticFlags |= PLSR_QUEUE_DIAG_UNDERRUN; if (PlsrProfileQueueUnderruns != 0xFFFFFFFFUL) { PlsrProfileQueueUnderruns++; } } static void PlsrProfileRecordPlannerStatus( const PLSR_SHORT_PROFILE *profile) { if ((profile != NULL) && (profile->planner.clipped != 0U)) { uint32_t criticalState = PlsrPlatformEnterCritical(); PlsrProfileQueueDiagnosticFlags |= PLSR_QUEUE_DIAG_PLANNER_CLIPPED; if (PlsrPlannerClippedCount != 0xFFFFFFFFUL) { PlsrPlannerClippedCount++; } PlsrPlatformExitCritical(criticalState); } } static void PlsrProfileQueueReset(void) { uint32_t criticalState = PlsrPlatformEnterCritical(); uint8_t bank; for (bank = 0U; bank < 2U; bank++) { PlsrProfileQueues[bank].generation++; PlsrProfileQueues[bank].readIndex = 0U; PlsrProfileQueues[bank].writeIndex = 0U; PlsrProfileQueues[bank].active = 0U; PlsrProfileQueues[bank].generatorComplete = 0U; PlsrProfileQueues[bank].producerEpoch = 0UL; PlsrProfileQueues[bank].repeatRemaining = 0UL; PlsrProfileQueues[bank].producerSegment = 0U; PlsrProfileQueues[bank].preparedHandoffBank = 0U; PlsrProfileQueues[bank].producerProfile.active = 0U; } PlsrProfileQueueBank = 0U; PlsrProfileQueueLowWaterLatched = 0U; PlsrPlatformExitCritical(criticalState); } static uint8_t PlsrProfileQueueBegin( const PLSR_SHORT_PROFILE *producerProfile, uint32_t producerEpoch, uint8_t producerSegment, uint8_t preparedHandoffBank) { uint32_t criticalState = PlsrPlatformEnterCritical(); PLSR_PROFILE_QUEUE *queue = &PlsrProfileQueues[PlsrProfileQueueBank]; queue->generation++; queue->readIndex = 0U; queue->writeIndex = 0U; queue->producerEpoch = producerEpoch; queue->producerSegment = producerSegment; queue->preparedHandoffBank = preparedHandoffBank; queue->generatorComplete = 0U; queue->repeatRemaining = 0UL; PlsrProfileQueueMinimumDepth = PLSR_PROFILE_QUEUE_CAPACITY; PlsrProfileQueueLowWaterLatched = 0U; PlsrCopyShortProfile(&queue->producerProfile, producerProfile); PlsrProfileRecordPlannerStatus(producerProfile); queue->active = ((producerProfile->active != 0U) && (producerProfile->nextPeriod < producerProfile->pulseCount)) ? 1U : 0U; if (queue->active == 0U) { queue->generatorComplete = 1U; } PlsrPlatformExitCritical(criticalState); return 1U; } static void PlsrProfileQueueInvalidateGeneration(void) { uint32_t criticalState = PlsrPlatformEnterCritical(); PLSR_PROFILE_QUEUE *queue = &PlsrProfileQueues[PlsrProfileQueueBank]; queue->generation++; queue->active = 0U; queue->generatorComplete = 0U; queue->readIndex = 0U; queue->writeIndex = 0U; queue->repeatRemaining = 0UL; PlsrPlatformExitCritical(criticalState); } #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) static uint32_t PlsrProfileTimingNow(void) { return *((volatile uint32_t *)0xE0001004UL); } static void PlsrProfileRecordProducerCycles(uint32_t startCycles) { uint32_t elapsed = PlsrProfileTimingNow() - startCycles; uint32_t total = PlsrProfileProducerTotalCycles; if (PlsrProfileProducerItemCount != 0xFFFFFFFFUL) { PlsrProfileProducerItemCount++; } PlsrProfileProducerTotalCycles = (elapsed > (0xFFFFFFFFUL - total)) ? 0xFFFFFFFFUL : total + elapsed; if (elapsed > PlsrProfileProducerMaxItemCycles) { PlsrProfileProducerMaxItemCycles = elapsed; } } #endif static uint8_t PlsrProfileQueueFill(uint16_t targetCount, uint16_t *itemBudget) { PLSR_SHORT_PROFILE candidate; PLSR_PROFILE_ENTRY entry; uint32_t generation; uint32_t producerEpoch; uint32_t criticalState; uint32_t writeIndex; #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) uint32_t startCycles; #endif if (itemBudget == NULL) { return 0U; } if ((targetCount == 0U) || (targetCount > PLSR_PROFILE_QUEUE_CAPACITY)) { targetCount = PLSR_PROFILE_QUEUE_CAPACITY; } while (1) { criticalState = PlsrPlatformEnterCritical(); if ((PlsrProfileQueue.active == 0U) || (PlsrProfileQueue.generatorComplete != 0U) || (PlsrProfileQueueCount() >= targetCount) || (*itemBudget == 0U)) { PlsrPlatformExitCritical(criticalState); return 1U; } (*itemBudget)--; generation = PlsrProfileQueue.generation; producerEpoch = PlsrProfileQueue.producerEpoch; PlsrCopyShortProfile(&candidate, &PlsrProfileQueue.producerProfile); PlsrPlatformExitCritical(criticalState); #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) startCycles = PlsrProfileTimingNow(); #endif if (PlsrShortProfileTakeRun(&candidate, &entry) == 0U) { return 0U; } criticalState = PlsrPlatformEnterCritical(); if ((PlsrProfileQueue.active == 0U) || (PlsrProfileQueue.generation != generation) || (PlsrProfileQueue.producerEpoch != producerEpoch) || (PlsrProfileQueueCount() >= PLSR_PROFILE_QUEUE_CAPACITY)) { PlsrPlatformExitCritical(criticalState); continue; } writeIndex = PlsrProfileQueue.writeIndex; PlsrProfileQueue.entries[writeIndex & PLSR_PROFILE_QUEUE_MASK] = entry; PlsrCopyShortProfile(&PlsrProfileQueue.producerProfile, &candidate); PlsrProfileQueue.writeIndex = writeIndex + 1UL; if (PlsrProfileQueueCount() >= PLSR_PROFILE_LOW_WATER_CLEAR) { PlsrProfileQueueLowWaterLatched = 0U; } if ((candidate.active == 0U) || (candidate.nextPeriod >= candidate.pulseCount)) { PlsrProfileQueue.generatorComplete = 1U; } PlsrPlatformExitCritical(criticalState); #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) PlsrProfileRecordProducerCycles(startCycles); #endif } } static PLSR_PLATFORM_QUEUE_RESULT PlsrProfileQueueCommitNext( uint8_t pulseOutput) { const PLSR_PROFILE_ENTRY *entry; uint32_t readIndex; uint32_t nextReadIndex; uint32_t writeIndex; uint32_t actualFrequencyHz; uint32_t repeatRemaining; PLSR_PLATFORM_QUEUE_RESULT result; repeatRemaining = PlsrProfileQueue.repeatRemaining; if (repeatRemaining != 0UL) { PlsrProfileQueue.repeatRemaining = repeatRemaining - 1UL; return PLSR_PLATFORM_QUEUE_APPLIED; } if (PlsrProfileQueue.active == 0U) { if (PlsrProfileQueue.generatorComplete != 0U) { PlsrShortProfile.active = 0U; } return PLSR_PLATFORM_QUEUE_APPLIED; } readIndex = PlsrProfileQueue.readIndex; if (readIndex == PlsrProfileQueue.writeIndex) { if (PlsrProfileQueue.generatorComplete != 0U) { PlsrProfileQueue.active = 0U; PlsrShortProfile.active = 0U; return PLSR_PLATFORM_QUEUE_APPLIED; } PlsrProfileQueueRecordUnderrun(); return PLSR_PLATFORM_QUEUE_FAILED; } entry = &PlsrProfileQueue.entries[readIndex & PLSR_PROFILE_QUEUE_MASK]; result = PlsrPlatformLoadPreparedFromIrq(pulseOutput, &entry->setting, &actualFrequencyHz); if (result != PLSR_PLATFORM_QUEUE_APPLIED) { return result; } nextReadIndex = readIndex + 1UL; PlsrProfileQueue.readIndex = nextReadIndex; PlsrProfileQueue.repeatRemaining = entry->repeatCount - 1UL; if (PlsrProfileQueue.generatorComplete == 0U) { PlsrProfileQueueRecordDepth( (uint16_t)(PlsrProfileQueue.writeIndex - nextReadIndex)); } PlsrDeferredFrequencyPending = 0U; PlsrQueuedFrequencyHz = actualFrequencyHz; PlsrDiagnosticRecordFrequency(entry->requestedFrequencyHz, entry->setting.actualFrequencyHz, actualFrequencyHz); PlsrDiagnosticQueuedExpectedHz = entry->setting.actualFrequencyHz; if (actualFrequencyHz > PlsrCurrentFrequencyHz) { PlsrRunStatus = PLSR_STATUS_ACCELERATING; } else if (actualFrequencyHz < PlsrCurrentFrequencyHz) { PlsrRunStatus = PLSR_STATUS_DECELERATING; } else { PlsrRunStatus = PLSR_STATUS_RUNNING; } writeIndex = PlsrProfileQueue.writeIndex; if ((nextReadIndex == writeIndex) && (PlsrProfileQueue.generatorComplete != 0U)) { PlsrProfileQueue.active = 0U; PlsrShortProfile.active = 0U; } return PLSR_PLATFORM_QUEUE_APPLIED; } static void PlsrInvalidateHandoffPlans(void) { uint8_t bank; uint8_t index; uint32_t criticalState = PlsrPlatformEnterCritical(); PlsrHandoffPlan.valid = 0U; PlsrCountedHandoffStaged = 0U; for (bank = 0U; bank < 2U; bank++) { for (index = 0U; index < PLSR_SEGMENT_COUNT_MAX; index++) { PlsrPreparedHandoffPlans[bank][index].valid = 0U; } } PlsrPlatformExitCritical(criticalState); } static void PlsrMaybePlanBoundaryRamp(uint32_t expectedEpoch) { const PLSR_SEGMENT_CONFIG *segment; uint8_t nextSegment; uint8_t nextPositive; uint8_t hasNext; uint32_t targetHz; uint32_t criticalState; uint32_t currentHz; uint32_t queuedHz; uint32_t deferredHz; uint32_t rateBoundHz; uint8_t deferredPending; uint64_t estimate; uint64_t candidateEstimate; uint64_t remaining; uint8_t rampActive; if ((PlsrPulseActive == 0U) || (PlsrBoundaryRampStarted != 0U) || (PlsrShortProfile.active != 0U) || (PlsrHandoffPlan.valid != 0U) || (PlsrStopRequested != 0U) || (PlsrCurrentSegment == 0U)) { return; } segment = &PlsrActiveConfig.segments[PlsrCurrentSegment - 1U]; hasNext = PlsrGetNextSegment(&nextSegment); targetHz = PlsrEffectiveStopFrequency(segment->frequencyHz); if ((hasNext != 0U) && (segment->waitType == PLSR_EXT_OR_COMPLETE)) { if ((PlsrPredictNextDirection(nextSegment, &nextPositive) != 0U) && (nextPositive == PlsrCountPositive)) { targetHz = PlsrActiveConfig.segments[nextSegment - 1U].frequencyHz; } } criticalState = PlsrPlatformEnterCritical(); currentHz = PlsrCurrentFrequencyHz; queuedHz = PlsrQueuedFrequencyHz; deferredHz = PlsrDeferredFrequencyHz; deferredPending = PlsrDeferredFrequencyPending; if ((PlsrSegmentEpoch != expectedEpoch) || (PlsrBoundaryPending != 0U) || (PlsrPulseActive == 0U) || (PlsrBoundaryRampStarted != 0U) || (PlsrShortProfile.active != 0U) || (PlsrHandoffPlan.valid != 0U) || (PlsrStopRequested != 0U) || (PlsrAbStopArmed != 0U)) { PlsrPlatformExitCritical(criticalState); return; } PlsrPlatformExitCritical(criticalState); rateBoundHz = (currentHz > queuedHz) ? currentHz : queuedHz; if ((deferredPending != 0U) && (deferredHz > rateBoundHz)) { rateBoundHz = deferredHz; } estimate = PlsrRampPulseEstimate(currentHz, targetHz, rateBoundHz); candidateEstimate = PlsrRampPulseEstimate(queuedHz, targetHz, rateBoundHz); if (candidateEstimate > estimate) { estimate = candidateEstimate; } if (deferredPending != 0U) { candidateEstimate = PlsrRampPulseEstimate(deferredHz, targetHz, rateBoundHz); if (candidateEstimate > estimate) { estimate = candidateEstimate; } } criticalState = PlsrPlatformEnterCritical(); remaining = PlsrRemainingPulses; if ((PlsrSegmentEpoch != expectedEpoch) || (PlsrBoundaryPending != 0U) || (PlsrPulseActive == 0U) || (PlsrBoundaryRampStarted != 0U) || (PlsrShortProfile.active != 0U) || (PlsrHandoffPlan.valid != 0U) || (PlsrStopRequested != 0U) || (PlsrAbStopArmed != 0U)) { PlsrPlatformExitCritical(criticalState); return; } if (remaining > estimate) { PlsrPlatformExitCritical(criticalState); return; } PlsrBoundaryRampStarted = 1U; PlsrRampStart(PlsrCurrentFrequencyHz, targetHz); rampActive = PlsrRamp.active; PlsrPlatformExitCritical(criticalState); if (rampActive != 0U) { if (PlsrRampAdvance(expectedEpoch) == 0U) { PlsrEnterErrorIfEpoch(PLSR_ERROR_TIMER, expectedEpoch); } } else { (void)PlsrApplyFrequency(targetHz, expectedEpoch); } } static uint8_t PlsrBeginSegmentOutput(uint32_t startFrequencyHz) { uint32_t targetFrequencyHz = PlsrActiveConfig.segments[PlsrCurrentSegment - 1U].frequencyHz; uint32_t firstFrequencyHz; uint32_t secondFrequencyHz; uint32_t actualFrequencyHz; uint32_t profileEpoch; uint8_t profileSegment; uint8_t profileHandoffBank; uint16_t fillBudget = PLSR_PROFILE_STARTUP_BUDGET; PLSR_PROFILE_ENTRY firstRun; PlsrSegmentClockStarted = 1U; PlsrSegmentElapsedMs = 0UL; if (PlsrPrepareShortProfile(&PlsrShortProfile, PlsrCurrentSegment, startFrequencyHz, targetFrequencyHz, PlsrRemainingSnapshot()) != 0U) { PlsrRamp.active = 0U; PlsrRefreshCurrentHandoffPlan(PlsrShortProfile.endHz); if (PlsrShortProfileTakeRun(&PlsrShortProfile, &firstRun) == 0U) { return 0U; } firstFrequencyHz = firstRun.setting.actualFrequencyHz; secondFrequencyHz = firstFrequencyHz; profileEpoch = PlsrSegmentEpoch; profileSegment = PlsrCurrentSegment; profileHandoffBank = PlsrPreparedHandoffBank; (void)PlsrProfileQueueBegin(&PlsrShortProfile, profileEpoch, profileSegment, profileHandoffBank); if (PlsrProfileQueueFill(PLSR_PROFILE_STARTUP_TARGET, &fillBudget) == 0U) { PlsrProfileQueueReset(); PlsrShortProfile.active = 0U; return 0U; } PlsrCountedHandoffStaged = 0U; if ((PlsrStageCountedHandoff() == 0U) || (PlsrProfileQueueFill(PLSR_PROFILE_STARTUP_TARGET, &fillBudget) == 0U)) { PlsrProfileQueueReset(); PlsrShortProfile.active = 0U; return 0U; } { uint32_t readIndex = PlsrProfileQueue.readIndex; uint32_t writeIndex = PlsrProfileQueue.writeIndex; if (readIndex != writeIndex) { secondFrequencyHz = PlsrProfileQueue.entries[ readIndex & PLSR_PROFILE_QUEUE_MASK] .setting.actualFrequencyHz; } } if (secondFrequencyHz > firstFrequencyHz) { PlsrRunStatus = PLSR_STATUS_ACCELERATING; } else if (secondFrequencyHz < firstFrequencyHz) { PlsrRunStatus = PLSR_STATUS_DECELERATING; } else { PlsrRunStatus = PLSR_STATUS_RUNNING; } PlsrCurrentFrequencyHz = firstFrequencyHz; PlsrQueuedFrequencyHz = secondFrequencyHz; PlsrDiagnosticQueuedExpectedHz = firstRun.setting.actualFrequencyHz; PlsrCountedObservedPublished = PlsrPlatformObservedPulses( (uint8_t)PlsrActiveConfig.pulseOutput); PlsrProfileQueueMinimumDepth = PlsrProfileQueueCount(); PlsrProfileQueueLowWaterLatched = 0U; PlsrPulseActive = 1U; PlsrExecutor.generation++; PlsrExecutor.mode = (PlsrProfileQueue.generatorComplete != 0U) ? PLSR_EXEC_STEP_TABLE : PLSR_EXEC_STREAM; if (PlsrPlatformStartCountedStreamPrepared( (uint8_t)PlsrActiveConfig.pulseOutput, &firstRun.setting, firstRun.repeatCount, &actualFrequencyHz) == 0U) { PlsrPulseActive = 0U; PlsrExecutor.mode = PLSR_EXEC_IDLE; PlsrProfileQueueReset(); PlsrShortProfile.active = 0U; return 0U; } PlsrCurrentFrequencyHz = actualFrequencyHz; PlsrDiagnosticRecordFrequency(firstRun.requestedFrequencyHz, firstRun.setting.actualFrequencyHz, actualFrequencyHz); return 1U; } if (PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR) { return 0U; } PlsrProfileQueueReset(); PlsrRampStart(startFrequencyHz, targetFrequencyHz); if (PlsrRamp.active == 0U) { if (PlsrApplyFrequency(targetFrequencyHz, PlsrSegmentEpoch) == 0U) { return 0U; } PlsrRunStatus = PLSR_STATUS_RUNNING; } else if ((startFrequencyHz != 0UL) && (PlsrApplyFrequency(startFrequencyHz, PlsrSegmentEpoch) == 0U)) { return 0U; } if (PlsrPrepareFutureHandoffQueue() == 0U) { return 0U; } PlsrExecutor.generation++; PlsrExecutor.mode = PLSR_EXEC_AB_LEGACY; return 1U; } static uint8_t PlsrStartSegment(uint8_t segmentNumber, uint8_t allowCarry, uint32_t carryFrequencyHz) { uint32_t criticalState; int32_t position; int64_t displacement; uint64_t magnitude; uint8_t positive; uint8_t directionLevel; uint8_t directionChanged; uint32_t startFrequencyHz; PLSR_PROFILE_QUEUE *queue; if ((segmentNumber == 0U) || (segmentNumber > PlsrActiveConfig.segmentCount)) { return 0U; } criticalState = PlsrPlatformEnterCritical(); position = PlsrPosition; PlsrPlatformExitCritical(criticalState); displacement = PlsrSegmentDisplacement(segmentNumber, position); positive = (displacement >= 0) ? 1U : 0U; magnitude = (displacement < 0) ? (uint64_t)(-displacement) : (uint64_t)displacement; PlsrSegmentEpoch++; PlsrCurrentSegment = segmentNumber; PlsrSegmentClockStarted = 0U; PlsrSegmentElapsedMs = 0UL; PlsrWaitElapsedMs = 0UL; PlsrBoundaryRampStarted = 0U; PlsrBoundaryPending = 0U; PlsrBoundaryWasCut = 0U; PlsrCutRequested = 0U; PlsrStopPulsesRemaining = 0U; PlsrAbStopArmed = 0U; PlsrFrequencyUpdatePending = 0U; PlsrDeferredFrequencyPending = 0U; PlsrCurrentFrequencyHz = 0UL; PlsrQueuedFrequencyHz = 0UL; PlsrHandoffPlan.valid = 0U; PlsrShortProfile.active = 0U; PlsrShortProfile.nextPeriod = 0U; queue = &PlsrProfileQueues[PlsrProfileQueueBank]; if ((allowCarry == 0U) || (queue->active == 0U) || (queue->producerEpoch != PlsrSegmentEpoch) || (queue->producerSegment != segmentNumber)) { PlsrProfileQueueReset(); } PlsrDirectionDelayActive = 0U; PlsrDirectionDelayRemainingMs = 0U; PlsrExtEdgePending = 0U; PlsrExtPreviousLevel = PlsrPlatformReadInput((uint8_t)PlsrActiveConfig.extInput); criticalState = PlsrPlatformEnterCritical(); PlsrRemainingPulses = magnitude; PlsrCountPositive = positive; PlsrPlatformExitCritical(criticalState); if ((PlsrActiveConfig.segments[segmentNumber - 1U].waitType == PLSR_ACT_TIME) && (PlsrActiveConfig.segments[segmentNumber - 1U].actTimeMs == 0U)) { PlsrSegmentClockStarted = 1U; PlsrRunStatus = PLSR_STATUS_RUNNING; PlsrBoundaryFrequencyHz = (allowCarry != 0U) ? carryFrequencyHz : 0UL; PlsrBoundaryWasCut = 1U; PlsrBoundaryPending = 1U; PlsrDiagnosticBeginSegment(segmentNumber, 0UL, positive); PlsrDiagnosticFinishSegment(1U); return 1U; } if (magnitude == 0UL) { PlsrSegmentClockStarted = 1U; PlsrRunStatus = PLSR_STATUS_RUNNING; PlsrBoundaryFrequencyHz = 0UL; PlsrBoundaryPending = 1U; PlsrDiagnosticBeginSegment(segmentNumber, 0UL, positive); PlsrDiagnosticFinishSegment(1U); return 1U; } directionLevel = positive; if ((PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR) && (PlsrActiveConfig.directionNegativeLogic != 0U)) { directionLevel ^= 1U; } directionChanged = ((PlsrLastDirectionValid == 0U) || (PlsrLastDirectionOutput != (uint8_t)PlsrActiveConfig.directionOutput) || (PlsrLastDirectionLevel != directionLevel)) ? 1U : 0U; if (PlsrPlatformPrepare((uint8_t)PlsrActiveConfig.pulseOutput, (uint8_t)PlsrActiveConfig.directionOutput, directionLevel, (uint8_t)PlsrActiveConfig.outputMode, positive) == 0U) { return 0U; } PlsrDiagnosticBeginSegment(segmentNumber, magnitude, positive); PlsrLastDirectionValid = 1U; PlsrLastDirectionOutput = (uint8_t)PlsrActiveConfig.directionOutput; PlsrLastDirectionLevel = directionLevel; startFrequencyHz = PlsrEffectiveStartFrequency( PlsrActiveConfig.segments[segmentNumber - 1U].frequencyHz, allowCarry, directionChanged, carryFrequencyHz); if ((PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR) && (directionChanged != 0U) && (PlsrActiveConfig.directionDelayMs != 0U)) { PlsrDirectionDelayActive = 1U; PlsrDirectionDelayRemainingMs = PlsrActiveConfig.directionDelayMs; PlsrRunStatus = PLSR_STATUS_ACCELERATING; return 1U; } PlsrDirectionDelayActive = 0U; return PlsrBeginSegmentOutput(startFrequencyHz); } static void PlsrMarkPersistenceDirty(uint16_t delayMs) { PlsrPersistenceDirty = 1U; PlsrPersistenceDelayMs = delayMs; } static void PlsrCheckpointPosition(uint8_t wasBusy) { uint32_t criticalState; int32_t position; uint8_t positionValid; criticalState = PlsrPlatformEnterCritical(); position = PlsrPosition; positionValid = PlsrPositionValid; PlsrPositionCheckpointDirty = 0U; PlsrPlatformExitCritical(criticalState); PlsrPlatformCheckpointPosition(position, positionValid, wasBusy); PlsrPositionCheckpointElapsedMs = 0U; } static void PlsrPollPositionCheckpoint(void) { if (PlsrPositionCheckpointDirty == 0U) { PlsrPositionCheckpointElapsedMs = 0U; return; } if (PlsrPositionCheckpointElapsedMs < PLSR_POSITION_CHECKPOINT_MS) { PlsrPositionCheckpointElapsedMs++; } if (PlsrPositionCheckpointElapsedMs >= PLSR_POSITION_CHECKPOINT_MS) { PlsrCheckpointPosition(1U); } } static void PlsrFinishCompleted(void) { PlsrPlatformStopPulse((uint8_t)PlsrActiveConfig.pulseOutput); PlsrRemainingPulses = 0UL; PlsrPulseActive = 0U; PlsrCutRequested = 0U; PlsrBoundaryPending = 0U; PlsrBoundaryWasCut = 0U; PlsrCurrentFrequencyHz = 0UL; PlsrQueuedFrequencyHz = 0UL; PlsrCurrentSegment = 0U; PlsrSegmentClockStarted = 0U; PlsrDirectionDelayActive = 0U; PlsrDirectionDelayRemainingMs = 0U; PlsrExtEdgePending = 0U; PlsrStopRequested = 0U; PlsrStopPulsesRemaining = 0U; PlsrAbStopArmed = 0U; PlsrSeamlessHandoffPending = 0U; PlsrDeferredFrequencyPending = 0U; PlsrShortProfile.active = 0U; PlsrProfileQueueReset(); PlsrHandoffPlan.valid = 0U; PlsrCountedBoundaryWriteIndex = 0U; PlsrCountedBoundaryReadIndex = 0U; PlsrCountedBoundaryBusy = 0U; PlsrCountedFaultEvent = 0U; PlsrTimerErrorPending = 0U; PlsrRamp.active = 0U; PlsrExecutor.mode = PLSR_EXEC_IDLE; PlsrInvalidateHandoffPlans(); PlsrRunStatus = PLSR_STATUS_COMPLETED; PlsrError = PLSR_ERROR_NONE; PlsrCheckpointPosition(0U); PlsrMarkPersistenceDirty(PLSR_CONFIG_SAVE_DELAY_MS); } static void PlsrFinishStopped(void) { PlsrDiagnosticFinishSegment(0U); PlsrPlatformStopPulse((uint8_t)PlsrActiveConfig.pulseOutput); PlsrRemainingPulses = 0UL; PlsrPulseActive = 0U; PlsrCutRequested = 0U; PlsrBoundaryPending = 0U; PlsrBoundaryWasCut = 0U; PlsrCurrentFrequencyHz = 0UL; PlsrQueuedFrequencyHz = 0UL; PlsrCurrentSegment = 0U; PlsrSegmentClockStarted = 0U; PlsrDirectionDelayActive = 0U; PlsrDirectionDelayRemainingMs = 0U; PlsrExtEdgePending = 0U; PlsrStopRequested = 0U; PlsrStopPulsesRemaining = 0U; PlsrAbStopArmed = 0U; PlsrSeamlessHandoffPending = 0U; PlsrDeferredFrequencyPending = 0U; PlsrShortProfile.active = 0U; PlsrProfileQueueReset(); PlsrHandoffPlan.valid = 0U; PlsrCountedBoundaryWriteIndex = 0U; PlsrCountedBoundaryReadIndex = 0U; PlsrCountedBoundaryBusy = 0U; PlsrCountedFaultEvent = 0U; PlsrTimerErrorPending = 0U; PlsrRamp.active = 0U; PlsrExecutor.mode = PLSR_EXEC_IDLE; PlsrInvalidateHandoffPlans(); PlsrRunStatus = PLSR_STATUS_STOPPED; PlsrError = PLSR_ERROR_NONE; PlsrCheckpointPosition(0U); PlsrMarkPersistenceDirty(PLSR_CONFIG_SAVE_DELAY_MS); } static void PlsrEnterError(PLSR_ERROR error) { PlsrDiagnosticFinishSegment(0U); PlsrPlatformStopPulse((uint8_t)PlsrActiveConfig.pulseOutput); PlsrRemainingPulses = 0UL; PlsrPulseActive = 0U; PlsrCutRequested = 0U; PlsrBoundaryPending = 0U; PlsrBoundaryWasCut = 0U; PlsrCurrentFrequencyHz = 0UL; PlsrQueuedFrequencyHz = 0UL; PlsrCurrentSegment = 0U; PlsrSegmentClockStarted = 0U; PlsrDirectionDelayActive = 0U; PlsrDirectionDelayRemainingMs = 0U; PlsrExtEdgePending = 0U; PlsrStopRequested = 0U; PlsrStopPulsesRemaining = 0U; PlsrAbStopArmed = 0U; PlsrSeamlessHandoffPending = 0U; PlsrDeferredFrequencyPending = 0U; PlsrShortProfile.active = 0U; PlsrProfileQueueReset(); PlsrHandoffPlan.valid = 0U; PlsrCountedBoundaryWriteIndex = 0U; PlsrCountedBoundaryReadIndex = 0U; PlsrCountedBoundaryBusy = 0U; PlsrCountedFaultEvent = 0U; PlsrTimerErrorPending = 0U; PlsrRamp.active = 0U; PlsrExecutor.mode = PLSR_EXEC_IDLE; PlsrInvalidateHandoffPlans(); PlsrRunStatus = PLSR_STATUS_ERROR; PlsrError = error; PlsrCheckpointPosition(0U); PlsrMarkPersistenceDirty(PLSR_CONFIG_SAVE_DELAY_MS); } static void PlsrTransitionToNext(uint8_t allowCarry) { uint8_t nextSegment; uint32_t carryFrequencyHz = PlsrBoundaryFrequencyHz; if (PlsrGetNextSegment(&nextSegment) == 0U) { PlsrFinishCompleted(); return; } if (PlsrStartSegment(nextSegment, allowCarry, carryFrequencyHz) == 0U) { PlsrEnterError(PLSR_ERROR_INVALID_RESOURCE); } } static uint8_t PlsrBuildHandoffPlan(uint8_t sourceSegment, const PLSR_CONFIG *frequencyConfig, uint32_t carryFrequencyHz, PLSR_HANDOFF_PLAN *plan) { const PLSR_SEGMENT_CONFIG *segment; uint8_t nextSegment; int64_t displacement; uint32_t nextFrequencyHz; uint8_t positive; uint8_t warmupIndex; plan->valid = 0U; if ((sourceSegment == 0U) || (sourceSegment > PlsrActiveConfig.segmentCount)) { return 0U; } segment = &PlsrActiveConfig.segments[sourceSegment - 1U]; if (segment->waitType != PLSR_EXT_OR_COMPLETE) { return 0U; } if (segment->jumpSegment != 0U) { nextSegment = (uint8_t)segment->jumpSegment; } else if (sourceSegment < PlsrActiveConfig.segmentCount) { nextSegment = (uint8_t)(sourceSegment + 1U); } else { return 0U; } if (PlsrActiveConfig.positionMode == PLSR_POSITION_ABSOLUTE) { displacement = (int64_t)PlsrActiveConfig.segments[nextSegment - 1U].pulses - (int64_t)segment->pulses; } else { displacement = PlsrActiveConfig.segments[nextSegment - 1U].pulses; } if (displacement == 0) { return 0U; } positive = (displacement > 0) ? 1U : 0U; plan->magnitude = (displacement < 0) ? (uint64_t)(-displacement) : (uint64_t)displacement; if (carryFrequencyHz == 0UL) { carryFrequencyHz = PlsrResolvedSegmentFrequency(frequencyConfig, sourceSegment - 1U); } nextFrequencyHz = PlsrResolvedSegmentFrequency(frequencyConfig, nextSegment - 1U); if (PlsrPrepareShortProfile(&plan->profile, nextSegment, carryFrequencyHz, nextFrequencyHz, plan->magnitude) == 0U) { return 0U; } { PLSR_PROFILE_ENTRY firstEntry; PLSR_PROFILE_ENTRY secondEntry; if (PlsrShortProfileTakeRun(&plan->profile, &firstEntry) == 0U) { return 0U; } plan->firstSetting = firstEntry.setting; plan->firstFrequencyHz = firstEntry.setting.actualFrequencyHz; plan->firstRequestedFrequencyHz = firstEntry.requestedFrequencyHz; plan->firstRepeatCount = firstEntry.repeatCount; if (PlsrShortProfileTakeRun(&plan->profile, &secondEntry) != 0U) { plan->secondSetting = secondEntry.setting; plan->secondFrequencyHz = secondEntry.setting.actualFrequencyHz; plan->secondRequestedFrequencyHz = secondEntry.requestedFrequencyHz; plan->secondRepeatCount = secondEntry.repeatCount; } else { plan->secondSetting = firstEntry.setting; plan->secondFrequencyHz = firstEntry.setting.actualFrequencyHz; plan->secondRequestedFrequencyHz = firstEntry.requestedFrequencyHz; plan->secondRepeatCount = 0UL; } } if ((plan->firstRepeatCount == 0UL) || (plan->firstFrequencyHz == 0UL)) { return 0U; } plan->warmupCount = 0U; for (warmupIndex = 0U; (warmupIndex < PLSR_HANDOFF_WARMUP_ITEMS) && (plan->profile.nextPeriod < plan->profile.pulseCount); warmupIndex++) { PLSR_PROFILE_ENTRY *entry = &plan->warmup[warmupIndex]; if (PlsrShortProfileTakeRun(&plan->profile, entry) == 0U) { return 0U; } plan->warmupCount++; } plan->firstFrequencyHz = plan->firstSetting.actualFrequencyHz; plan->secondFrequencyHz = plan->secondSetting.actualFrequencyHz; plan->nextSegment = nextSegment; plan->positive = positive; plan->valid = 1U; return 1U; } static uint8_t PlsrBuildHandoffPlanBank( const PLSR_CONFIG *frequencyConfig) { uint8_t buildBank = (uint8_t)(PlsrPreparedHandoffBank ^ 1U); uint8_t sourceSegment; PLSR_HANDOFF_PLAN *destination; for (sourceSegment = 0U; sourceSegment < PLSR_SEGMENT_COUNT_MAX; sourceSegment++) { PlsrPreparedHandoffPlans[buildBank][sourceSegment].valid = 0U; } for (sourceSegment = 1U; sourceSegment <= PlsrActiveConfig.segmentCount; sourceSegment++) { destination = &PlsrPreparedHandoffPlans[buildBank][sourceSegment - 1U]; (void)PlsrBuildHandoffPlan(sourceSegment, frequencyConfig, 0UL, destination); } return buildBank; } static void PlsrRefreshCurrentHandoffPlan(uint32_t terminalFrequencyHz) { PLSR_PLATFORM_TIMER_SETTING terminalSetting; PLSR_HANDOFF_PLAN *destination; uint8_t currentSegment = PlsrCurrentSegment; uint8_t preparedBank = PlsrPreparedHandoffBank; if ((currentSegment == 0U) || (currentSegment > PlsrActiveConfig.segmentCount) || (PlsrPlatformBuildTimerSetting( (uint8_t)PlsrActiveConfig.pulseOutput, (uint8_t)PlsrActiveConfig.outputMode, terminalFrequencyHz, &terminalSetting) == 0U)) { return; } destination = &PlsrPreparedHandoffPlans[preparedBank] [currentSegment - 1U]; (void)PlsrBuildHandoffPlan(currentSegment, &PlsrActiveConfig, terminalSetting.actualFrequencyHz, destination); } static uint8_t PlsrSelectPreparedHandoffPlan(PLSR_HANDOFF_PLAN *plan) { const PLSR_HANDOFF_PLAN *prepared; uint8_t preparedBank; uint8_t currentSegment; plan->valid = 0U; currentSegment = PlsrCurrentSegment; if ((PlsrRemainingPulses != 1UL) || (PlsrStopRequested != 0U) || (PlsrCountOverflowPending != 0U) || (PlsrCutRequested != 0U) || (PlsrAbStopArmed != 0U) || (currentSegment == 0U) || (currentSegment > PlsrActiveConfig.segmentCount)) { return 0U; } preparedBank = PlsrPreparedHandoffBank; prepared = &PlsrPreparedHandoffPlans[preparedBank][currentSegment - 1U]; if ((prepared->valid == 0U) || (prepared->positive != PlsrCountPositive)) { return 0U; } *plan = *prepared; return 1U; } static uint8_t PlsrStageCountedHandoff(void) { const PLSR_HANDOFF_PLAN *prepared; PLSR_HANDOFF_PLAN candidate; PLSR_PROFILE_ENTRY entry; uint32_t criticalState; uint32_t writeIndex; uint16_t required; uint8_t currentSegment; uint8_t index; PLSR_PROFILE_QUEUE *queue = &PlsrProfileQueues[PlsrProfileQueueBank]; if ((PlsrActiveConfig.outputMode != PLSR_OUTPUT_PULSE_DIR) || (PlsrCountedHandoffStaged != 0U) || (PlsrStopRequested != 0U) || (PlsrCutRequested != 0U) || (queue->generatorComplete == 0U)) { return 1U; } currentSegment = PlsrCurrentSegment; if ((currentSegment == 0U) || (currentSegment > PlsrActiveConfig.segmentCount)) { return 1U; } prepared = &PlsrPreparedHandoffPlans[PlsrPreparedHandoffBank] [currentSegment - 1U]; if ((prepared->valid == 0U) || (prepared->positive != PlsrCountPositive)) { return 1U; } candidate = *prepared; required = (uint16_t)(1U + candidate.warmupCount + ((candidate.secondRepeatCount != 0UL) ? 1U : 0U)); criticalState = PlsrPlatformEnterCritical(); if ((PlsrCountedHandoffStaged != 0U) || (queue->generatorComplete == 0U) || ((uint16_t)(PLSR_PROFILE_QUEUE_CAPACITY - PlsrProfileQueueCount()) < required)) { PlsrPlatformExitCritical(criticalState); return 1U; } writeIndex = queue->writeIndex; entry.setting = candidate.firstSetting; entry.requestedFrequencyHz = candidate.firstRequestedFrequencyHz; entry.repeatCount = candidate.firstRepeatCount; entry.startsNextSegment = 1U; queue->entries[writeIndex & PLSR_PROFILE_QUEUE_MASK] = entry; writeIndex++; if (candidate.secondRepeatCount != 0UL) { entry.setting = candidate.secondSetting; entry.requestedFrequencyHz = candidate.secondRequestedFrequencyHz; entry.repeatCount = candidate.secondRepeatCount; entry.startsNextSegment = 0U; queue->entries[writeIndex & PLSR_PROFILE_QUEUE_MASK] = entry; writeIndex++; } for (index = 0U; index < candidate.warmupCount; index++) { entry = candidate.warmup[index]; entry.startsNextSegment = 0U; queue->entries[writeIndex & PLSR_PROFILE_QUEUE_MASK] = entry; writeIndex++; } queue->writeIndex = writeIndex; PlsrCopyShortProfile(&queue->producerProfile, &candidate.profile); queue->producerEpoch = PlsrSegmentEpoch + 1UL; queue->producerSegment = candidate.nextSegment; queue->generatorComplete = ((candidate.profile.active == 0U) || (candidate.profile.nextPeriod >= candidate.profile.pulseCount)) ? 1U : 0U; queue->active = 1U; PlsrHandoffPlan = candidate; PlsrHandoffPlan.valid = 1U; PlsrCountedHandoffStaged = 1U; PlsrPlatformExitCritical(criticalState); return 1U; } static uint8_t PlsrSyncCountedProgress(void) { uint64_t observed; uint64_t delta; uint64_t remaining; int64_t nextPosition; observed = PlsrPlatformObservedPulses( (uint8_t)PlsrActiveConfig.pulseOutput); if (observed < PlsrCountedObservedPublished) { return 0U; } delta = observed - PlsrCountedObservedPublished; if (delta == 0ULL) { return 1U; } remaining = PlsrRemainingPulses; if (delta > remaining) { delta = remaining; #if PLSR_COUNT_ERROR_CHECK_ENABLED PlsrCountOverflowPending = 1U; #endif } if (PlsrCountPositive != 0U) { nextPosition = (int64_t)PlsrPosition + (int64_t)delta; } else { nextPosition = (int64_t)PlsrPosition - (int64_t)delta; } if ((nextPosition > (int64_t)INT32_MAX) || (nextPosition < (int64_t)INT32_MIN)) { PlsrPositionValid = 0U; #if PLSR_COUNT_ERROR_CHECK_ENABLED PlsrCountOverflowPending = 1U; #endif } else { PlsrPosition = (int32_t)nextPosition; } PlsrRemainingPulses = remaining - delta; PlsrCountedObservedPublished = observed; PlsrPositionCheckpointDirty = 1U; return 1U; } uint8_t PlsrExecTakeCountedRunIrq( uint8_t pulseOutput, PLSR_PLATFORM_TIMER_SETTING *setting, uint32_t *pulseCount, uint8_t *startsNextSegment) { const PLSR_PROFILE_ENTRY *entry; uint32_t readIndex; uint32_t nextReadIndex; if ((pulseOutput != (uint8_t)PlsrActiveConfig.pulseOutput) || (setting == NULL) || (pulseCount == NULL) || (startsNextSegment == NULL) || (PlsrPulseActive == 0U)) { return PLSR_EXEC_RUN_FAILED; } readIndex = PlsrProfileQueue.readIndex; if (readIndex == PlsrProfileQueue.writeIndex) { if (PlsrProfileQueue.generatorComplete != 0U) { return PLSR_EXEC_RUN_DONE; } PlsrProfileQueueRecordUnderrun(); return PLSR_EXEC_RUN_FAILED; } entry = &PlsrProfileQueue.entries[readIndex & PLSR_PROFILE_QUEUE_MASK]; if (entry->repeatCount == 0UL) { return PLSR_EXEC_RUN_FAILED; } /* 先校验 handoff 前置条件,再推进读索引:失败时该项不被消费。 不在此处更新任何执行状态(Pending/active 等),全部由任务消费事件时做。 */ if (entry->startsNextSegment != 0U) { if ((PlsrCountedHandoffStaged == 0U) || (PlsrHandoffPlan.valid == 0U)) { return PLSR_EXEC_RUN_FAILED; } } *setting = entry->setting; *pulseCount = entry->repeatCount; *startsNextSegment = entry->startsNextSegment; nextReadIndex = readIndex + 1UL; PlsrProfileQueue.readIndex = nextReadIndex; if (PlsrProfileQueue.generatorComplete == 0U) { PlsrProfileQueueRecordDepth( (uint16_t)(PlsrProfileQueue.writeIndex - nextReadIndex)); } /* 记录硬件即将生效的频率(事实镜像),不做任何状态转换。 */ PlsrQueuedFrequencyHz = entry->setting.actualFrequencyHz; return PLSR_EXEC_RUN_READY; } void PlsrExecCountedSegmentBoundaryIrq(uint8_t pulseOutput, uint32_t completedPulses) { PLSR_COUNTED_BOUNDARY_EVENT *event; uint8_t writeIndex; if (pulseOutput != (uint8_t)PlsrActiveConfig.pulseOutput) { return; } if (PlsrHandoffPlan.valid == 0U) { PlsrCountedFaultEvent = 1U; return; } writeIndex = PlsrCountedBoundaryWriteIndex; if ((PlsrCountedBoundaryBusy != 0U) || ((uint8_t)(writeIndex - PlsrCountedBoundaryReadIndex) >= PLSR_BOUNDARY_EVENT_CAPACITY)) { PlsrCountedFaultEvent = 1U; return; } event = &PlsrCountedBoundaryEvents[ writeIndex & (PLSR_BOUNDARY_EVENT_CAPACITY - 1U)]; event->magnitude = PlsrHandoffPlan.magnitude; event->firstFrequencyHz = PlsrHandoffPlan.firstFrequencyHz; event->completedPulses = completedPulses; event->nextSegment = PlsrHandoffPlan.nextSegment; event->positive = PlsrHandoffPlan.positive; #if defined(__ICCARM__) __DMB(); #endif PlsrCountedBoundaryWriteIndex = (uint8_t)(writeIndex + 1U); } static void PlsrExecServiceCountedBoundaryEvent(void); /* 任务上下文:消费 counted 流的全部 IRQ 事件(流故障 + 段边界)。 段切换簿记与错误标志写入均在此完成,IRQ 内不再做状态转换。 */ static void PlsrExecServiceCountedEvents(void) { if (PlsrCountedFaultEvent != 0U) { PlsrCountedFaultEvent = 0U; PlsrTimerErrorPending = 1U; return; } PlsrExecServiceCountedBoundaryEvent(); } /* 任务上下文:消费段边界事件,完成全部段切换簿记。 硬件连续性不依赖本函数(下一段首 run 已由队列预装载),晚 1ms 消费无影响。 */ static void PlsrExecServiceCountedBoundaryEvent(void) { PLSR_COUNTED_BOUNDARY_EVENT event; uint64_t observed; uint64_t delta; int64_t nextPosition; uint32_t currentFrequencyHz; uint32_t queuedFrequencyHz; uint8_t nextSegment; uint32_t criticalState; uint8_t readIndex; criticalState = PlsrPlatformEnterCritical(); readIndex = PlsrCountedBoundaryReadIndex; if (readIndex == PlsrCountedBoundaryWriteIndex) { PlsrPlatformExitCritical(criticalState); return; } event = PlsrCountedBoundaryEvents[ readIndex & (PLSR_BOUNDARY_EVENT_CAPACITY - 1U)]; PlsrCountedBoundaryReadIndex = (uint8_t)(readIndex + 1U); PlsrCountedBoundaryBusy = 1U; PlsrPlatformExitCritical(criticalState); observed = event.completedPulses; nextSegment = event.nextSegment; if ((nextSegment == 0U) || (nextSegment > PlsrActiveConfig.segmentCount)) { PlsrTimerErrorPending = 1U; return; } /* 用 IRQ 边界时刻的计数快照结算旧段:任务消费时下一 run 可能已发出 若干脉冲,直接读当前硬件计数会把下一段脉冲算进旧段。 */ if (observed < PlsrCountedObservedPublished) { PlsrTimerErrorPending = 1U; return; } delta = observed - PlsrCountedObservedPublished; if (delta > PlsrRemainingPulses) { delta = PlsrRemainingPulses; #if PLSR_COUNT_ERROR_CHECK_ENABLED PlsrCountOverflowPending = 1U; #endif } if (PlsrCountPositive != 0U) { nextPosition = (int64_t)PlsrPosition + (int64_t)delta; } else { nextPosition = (int64_t)PlsrPosition - (int64_t)delta; } if ((nextPosition > (int64_t)INT32_MAX) || (nextPosition < (int64_t)INT32_MIN)) { PlsrPositionValid = 0U; #if PLSR_COUNT_ERROR_CHECK_ENABLED PlsrCountOverflowPending = 1U; #endif } else { PlsrPosition = (int32_t)nextPosition; } PlsrRemainingPulses -= delta; PlsrCountedObservedPublished = observed; PlsrPositionCheckpointDirty = 1U; if (PlsrRemainingPulses != 0ULL) { PlsrTimerErrorPending = 1U; return; } PlsrDiagnosticFinishSegment(1U); PlsrSegmentEpoch++; PlsrCurrentSegment = nextSegment; PlsrRemainingPulses = event.magnitude; PlsrCountPositive = event.positive; PlsrCurrentFrequencyHz = event.firstFrequencyHz; PlsrBoundaryFrequencyHz = event.firstFrequencyHz; PlsrSegmentClockStarted = 1U; PlsrSegmentElapsedMs = 0UL; PlsrWaitElapsedMs = 0UL; PlsrBoundaryRampStarted = 0U; PlsrBoundaryPending = 0U; PlsrBoundaryWasCut = 0U; PlsrCutRequested = 0U; PlsrFrequencyUpdatePending = 0U; PlsrDeferredFrequencyPending = 0U; PlsrExtEdgePending = 0U; PlsrExtPreviousLevel = PlsrPlatformReadInput((uint8_t)PlsrActiveConfig.extInput); PlsrCopyShortProfile(&PlsrShortProfile, &PlsrProfileQueue.producerProfile); PlsrDiagnosticBeginSegment(nextSegment, event.magnitude, event.positive); PlsrSeamlessHandoffPending = 1U; PlsrCountedHandoffStaged = 0U; PlsrHandoffPlan.valid = 0U; PlsrExecutor.generation++; PlsrExecutor.mode = (PlsrProfileQueue.generatorComplete != 0U) ? PLSR_EXEC_STEP_TABLE : PLSR_EXEC_STREAM; currentFrequencyHz = PlsrCurrentFrequencyHz; queuedFrequencyHz = PlsrQueuedFrequencyHz; if (queuedFrequencyHz > currentFrequencyHz) { PlsrRunStatus = PLSR_STATUS_ACCELERATING; } else if (queuedFrequencyHz < currentFrequencyHz) { PlsrRunStatus = PLSR_STATUS_DECELERATING; } else { PlsrRunStatus = PLSR_STATUS_RUNNING; } criticalState = PlsrPlatformEnterCritical(); PlsrCountedBoundaryBusy = 0U; PlsrPlatformExitCritical(criticalState); } void PlsrExecCountedStreamFaultIrq(uint8_t pulseOutput) { if (pulseOutput == (uint8_t)PlsrActiveConfig.pulseOutput) { /* 只置事件:任务消费时统一置 PlsrTimerErrorPending。 */ PlsrCountedFaultEvent = 1U; } } static uint8_t PlsrPrepareFutureHandoffQueue(void) { const PLSR_HANDOFF_PLAN *prepared; uint8_t currentSegment = PlsrCurrentSegment; uint8_t preparedBank = PlsrPreparedHandoffBank; uint16_t fillBudget = PLSR_PROFILE_STARTUP_BUDGET; PlsrProfileQueueReset(); if ((currentSegment == 0U) || (currentSegment > PlsrActiveConfig.segmentCount)) { return 1U; } prepared = &PlsrPreparedHandoffPlans[preparedBank][currentSegment - 1U]; if ((prepared->valid == 0U) || (prepared->profile.active == 0U) || (prepared->profile.nextPeriod >= prepared->profile.pulseCount)) { return 1U; } (void)PlsrProfileQueueBegin(&prepared->profile, PlsrSegmentEpoch + 1UL, prepared->nextSegment, preparedBank); if (PlsrProfileQueueFill(PLSR_PROFILE_STARTUP_TARGET, &fillBudget) == 0U) { PlsrProfileQueueReset(); return 0U; } return 1U; } /* The first two periods are already in the timer pipeline. Seed a small precomputed tail here so the 1 ms producer has time to resume without doing planner math in the pulse IRQ. */ static void PlsrActivateHandoffQueueFromIrq( const PLSR_HANDOFF_PLAN *plan, uint32_t producerEpoch) { uint8_t index; PLSR_PROFILE_QUEUE *queue = &PlsrProfileQueues[PlsrProfileQueueBank]; uint32_t readIndex = queue->readIndex; uint32_t writeIndex = queue->writeIndex; uint32_t queuedEpoch = queue->producerEpoch; uint8_t queuedSegment = queue->producerSegment; uint8_t queueActive = queue->active; if ((queueActive != 0U) && (queuedEpoch == producerEpoch) && (queuedSegment == plan->nextSegment) && (readIndex != writeIndex)) { PlsrCopyShortProfile(&PlsrShortProfile, &queue->producerProfile); PlsrShortProfile.active = 1U; PlsrProfileQueueMinimumDepth = PlsrProfileQueueCount(); PlsrProfileQueueLowWaterLatched = 0U; return; } /* Fallback for a stale/missing prepared bank. This path is fault containment only; normal AB handoff uses the 400-item task-built queue. */ queue->generation++; queue->readIndex = 0U; queue->writeIndex = plan->warmupCount; for (index = 0U; index < plan->warmupCount; index++) { queue->entries[index] = plan->warmup[index]; } PlsrCopyShortProfile(&queue->producerProfile, &plan->profile); queue->producerEpoch = producerEpoch; queue->producerSegment = plan->nextSegment; queue->preparedHandoffBank = PlsrPreparedHandoffBank; queue->generatorComplete = (plan->profile.active == 0U) ? 1U : 0U; queue->active = ((plan->warmupCount != 0U) || (plan->profile.active != 0U)) ? 1U : 0U; PlsrCopyShortProfile(&PlsrShortProfile, &plan->profile); if (queue->active != 0U) { PlsrShortProfile.active = 1U; } } static PLSR_PLATFORM_QUEUE_RESULT PlsrPrimeHandoff(void) { PLSR_HANDOFF_PLAN candidatePlan; uint32_t actualFrequencyHz; PLSR_PLATFORM_QUEUE_RESULT result; PlsrHandoffPlan.valid = 0U; if (PlsrAbStopArmed != 0U) { return PLSR_PLATFORM_QUEUE_FAILED; } if (PlsrSelectPreparedHandoffPlan(&candidatePlan) == 0U) { return PLSR_PLATFORM_QUEUE_FAILED; } PlsrDeferredFrequencyPending = 0U; result = PlsrPlatformLoadPreparedFromIrq( (uint8_t)PlsrActiveConfig.pulseOutput, &candidatePlan.firstSetting, &actualFrequencyHz); if (result != PLSR_PLATFORM_QUEUE_APPLIED) { if (result == PLSR_PLATFORM_QUEUE_FAILED) { PlsrTimerErrorPending = 1U; } return result; } PlsrDiagnosticRecordFrequency(candidatePlan.firstRequestedFrequencyHz, candidatePlan.firstSetting.actualFrequencyHz, actualFrequencyHz); PlsrDiagnosticQueuedExpectedHz = candidatePlan.firstSetting.actualFrequencyHz; PlsrQueuedFrequencyHz = actualFrequencyHz; PlsrHandoffPlan = candidatePlan; PlsrHandoffPlan.firstFrequencyHz = actualFrequencyHz; PlsrHandoffPlan.valid = 1U; return PLSR_PLATFORM_QUEUE_APPLIED; } static PLSR_PLATFORM_QUEUE_RESULT PlsrTryContinuousHandoff(void) { uint32_t actualQueuedFrequencyHz; uint32_t currentFrequencyHz; uint32_t queuedFrequencyHz; uint8_t nextSegment = PlsrHandoffPlan.nextSegment; uint64_t magnitude = PlsrHandoffPlan.magnitude; uint8_t positive = PlsrHandoffPlan.positive; PLSR_PLATFORM_QUEUE_RESULT result; if ((PlsrHandoffPlan.valid == 0U) || (PlsrStopRequested != 0U) || (PlsrCountOverflowPending != 0U) || (PlsrCutRequested != 0U) || (PlsrAbStopArmed != 0U) || (PlsrCurrentFrequencyHz != PlsrHandoffPlan.firstFrequencyHz) || (nextSegment == 0U) || (nextSegment > PlsrActiveConfig.segmentCount)) { return PLSR_PLATFORM_QUEUE_FAILED; } PlsrDeferredFrequencyPending = 0U; result = PlsrPlatformLoadPreparedFromIrq( (uint8_t)PlsrActiveConfig.pulseOutput, &PlsrHandoffPlan.secondSetting, &actualQueuedFrequencyHz); if (result == PLSR_PLATFORM_QUEUE_STALE) { return result; } if (result == PLSR_PLATFORM_QUEUE_FAILED) { PlsrShortProfile.active = 0U; PlsrHandoffPlan.valid = 0U; PlsrTimerErrorPending = 1U; return result; } PlsrDiagnosticRecordFrequency( PlsrHandoffPlan.secondRequestedFrequencyHz, PlsrHandoffPlan.secondSetting.actualFrequencyHz, actualQueuedFrequencyHz); PlsrDiagnosticQueuedExpectedHz = PlsrHandoffPlan.secondSetting.actualFrequencyHz; PlsrQueuedFrequencyHz = actualQueuedFrequencyHz; PlsrDiagnosticFinishSegment(1U); PlsrSegmentEpoch++; PlsrActivateHandoffQueueFromIrq(&PlsrHandoffPlan, PlsrSegmentEpoch); PlsrCurrentSegment = nextSegment; PlsrRemainingPulses = magnitude; PlsrCountPositive = positive; PlsrBoundaryFrequencyHz = PlsrCurrentFrequencyHz; PlsrSegmentClockStarted = 1U; PlsrSegmentElapsedMs = 0UL; PlsrWaitElapsedMs = 0UL; PlsrBoundaryRampStarted = 0U; PlsrBoundaryPending = 0U; PlsrBoundaryWasCut = 0U; PlsrCutRequested = 0U; PlsrFrequencyUpdatePending = 0U; PlsrDeferredFrequencyPending = 0U; PlsrExtEdgePending = 0U; PlsrExtPreviousLevel = PlsrPlatformReadInput((uint8_t)PlsrActiveConfig.extInput); PlsrDiagnosticBeginSegment(nextSegment, magnitude, positive); PlsrSeamlessHandoffPending = 1U; PlsrHandoffPlan.valid = 0U; if (PlsrRemainingPulses == 1UL) { result = PlsrPrimeHandoff(); if (result == PLSR_PLATFORM_QUEUE_STALE) { return result; } } (void)PlsrQueueFinalAbBoundaryFromIrq(); queuedFrequencyHz = PlsrQueuedFrequencyHz; currentFrequencyHz = PlsrCurrentFrequencyHz; if (PlsrShortProfile.active != 0U) { if (queuedFrequencyHz > currentFrequencyHz) { PlsrRunStatus = PLSR_STATUS_ACCELERATING; } else if (queuedFrequencyHz < currentFrequencyHz) { PlsrRunStatus = PLSR_STATUS_DECELERATING; } else { PlsrRunStatus = PLSR_STATUS_RUNNING; } } else { PlsrRunStatus = PLSR_STATUS_RUNNING; } return PLSR_PLATFORM_QUEUE_APPLIED; } static void PlsrHandleBoundary(uint8_t extEdge) { const PLSR_SEGMENT_CONFIG *segment; uint8_t wasCut = PlsrBoundaryWasCut; PlsrPlatformStopPulse((uint8_t)PlsrActiveConfig.pulseOutput); PlsrBoundaryPending = 0U; PlsrBoundaryWasCut = 0U; PlsrPulseActive = 0U; PlsrCurrentFrequencyHz = 0UL; PlsrQueuedFrequencyHz = 0UL; PlsrShortProfile.active = 0U; PlsrProfileQueueReset(); PlsrHandoffPlan.valid = 0U; PlsrDeferredFrequencyPending = 0U; PlsrAbStopArmed = 0U; PlsrCheckpointPosition(1U); if (PlsrTimerErrorPending != 0U) { PlsrTimerErrorPending = 0U; PlsrEnterError(PLSR_ERROR_TIMER); return; } #if PLSR_COUNT_ERROR_CHECK_ENABLED if (PlsrCountOverflowPending != 0U) { PlsrCountOverflowPending = 0U; PlsrEnterError(PLSR_ERROR_COUNT); return; } #else /* 计数错误检查暂时关闭:偏差已由同步路径钳制,标志直接清除。 */ PlsrCountOverflowPending = 0U; #endif if (PlsrStopRequested != 0U) { PlsrFinishStopped(); return; } if ((PlsrCurrentSegment == 0U) || (PlsrCurrentSegment > PlsrActiveConfig.segmentCount)) { PlsrEnterError(PLSR_ERROR_INTERNAL); return; } segment = &PlsrActiveConfig.segments[PlsrCurrentSegment - 1U]; if (wasCut != 0U) { PlsrTransitionToNext( (PlsrActiveConfig.sendMode == PLSR_SEND_SUBSEQUENT) ? 1U : 0U); return; } switch (segment->waitType) { case PLSR_WAIT_TIME: PlsrWaitElapsedMs = 0UL; PlsrRunStatus = PLSR_STATUS_WAITING; break; case PLSR_WAIT_SIGNAL: if (PlsrPlatformReadInput((uint8_t)PlsrActiveConfig.waitInput) != 0U) { PlsrTransitionToNext(0U); } else { PlsrRunStatus = PLSR_STATUS_WAITING; } break; case PLSR_ACT_TIME: if (PlsrSegmentElapsedMs >= segment->actTimeMs) { PlsrTransitionToNext(0U); } else { PlsrRunStatus = PLSR_STATUS_WAITING; } break; case PLSR_EXT_SIGNAL: if (extEdge != 0U) { PlsrTransitionToNext(0U); } else { PlsrRunStatus = PLSR_STATUS_WAITING; } break; case PLSR_EXT_OR_COMPLETE: PlsrTransitionToNext(1U); break; default: PlsrEnterError(PLSR_ERROR_INTERNAL); break; } } static void PlsrRequestCut(uint32_t expectedEpoch) { uint32_t criticalState = PlsrPlatformEnterCritical(); if (PlsrSegmentEpoch != expectedEpoch) { PlsrPlatformExitCritical(criticalState); return; } if (PlsrBoundaryPending != 0U) { PlsrBoundaryWasCut = 1U; } else if (PlsrPulseActive != 0U) { PlsrCutRequested = 1U; (void)PlsrArmFinalAbBoundaryLocked(); } else { PlsrBoundaryFrequencyHz = PlsrCurrentFrequencyHz; PlsrBoundaryWasCut = 1U; PlsrBoundaryPending = 1U; } PlsrPlatformExitCritical(criticalState); } static void PlsrPollWaiting(uint8_t extEdge) { const PLSR_SEGMENT_CONFIG *segment = &PlsrActiveConfig.segments[PlsrCurrentSegment - 1U]; switch (segment->waitType) { case PLSR_WAIT_TIME: PlsrWaitElapsedMs++; if (PlsrWaitElapsedMs >= segment->waitTimeMs) { PlsrTransitionToNext(0U); } break; case PLSR_WAIT_SIGNAL: if (PlsrPlatformReadInput((uint8_t)PlsrActiveConfig.waitInput) != 0U) { PlsrTransitionToNext(0U); } break; case PLSR_ACT_TIME: if (PlsrSegmentElapsedMs >= segment->actTimeMs) { PlsrTransitionToNext(0U); } break; case PLSR_EXT_SIGNAL: if (extEdge != 0U) { PlsrTransitionToNext(0U); } break; default: PlsrEnterError(PLSR_ERROR_INTERNAL); break; } } static void PlsrPollPersistenceDelay(void) { if ((PlsrPersistenceDirty != 0U) && (PlsrIsBusy() == 0U) && (PlsrPersistenceDelayMs != 0U)) { PlsrPersistenceDelayMs--; } } void PlsrServicePersistence(void) { PLSR_PERSIST_PAYLOAD payload; PLSR_PLATFORM_SERVICE_RESULT serviceResult; uint32_t criticalState; if (PlsrIsBusy() != 0U) { return; } serviceResult = PlsrPlatformServicePersistence(); if (serviceResult == PLSR_PLATFORM_SERVICE_FAILED) { PlsrRunStatus = PLSR_STATUS_ERROR; PlsrError = PLSR_ERROR_INTERNAL; return; } if ((serviceResult == PLSR_PLATFORM_SERVICE_DEFERRED) || (PlsrPersistenceDirty == 0U)) { return; } if (PlsrPersistenceDelayMs != 0U) { return; } payload.config = PlsrShadowConfig; criticalState = PlsrPlatformEnterCritical(); payload.position = PlsrPosition; payload.positionValid = PlsrPositionValid; PlsrPlatformExitCritical(criticalState); payload.wasBusy = 0U; payload.reserved = 0U; if (PlsrPlatformSave(&payload) != 0U) { PlsrPersistenceDirty = 0U; } else { PlsrRunStatus = PLSR_STATUS_ERROR; PlsrError = PLSR_ERROR_INTERNAL; } } uint8_t PlsrInit(void) { PLSR_PERSIST_PAYLOAD payload; PlsrInitialized = 0U; PlsrRunStatus = PLSR_STATUS_UNINITIALIZED; if (PlsrPlatformInit() == 0U) { return 0U; } if ((PlsrPlatformLoad(&payload) == 0U) || (PlsrConfigIsValid(&payload.config, 0U) == 0U) || (payload.positionValid > 1U) || (payload.wasBusy > 1U)) { PlsrSetDefaults(&PlsrShadowConfig); PlsrPosition = 0L; PlsrPositionValid = 1U; PlsrPlatformCheckpointConfig(&PlsrShadowConfig); PlsrPlatformCheckpointPosition(0L, 1U, 0U); PlsrMarkPersistenceDirty(PLSR_CONFIG_SAVE_DELAY_MS); } else { PlsrShadowConfig = payload.config; PlsrPosition = payload.position; PlsrPositionValid = ((payload.positionValid != 0U) && (payload.wasBusy == 0U)) ? 1U : 0U; PlsrPersistenceDirty = 0U; PlsrPersistenceDelayMs = 0U; } (void)memset(&PlsrActiveConfig, 0, sizeof(PlsrActiveConfig)); (void)memset(&PlsrRamp, 0, sizeof(PlsrRamp)); (void)memset(&PlsrShortProfile, 0, sizeof(PlsrShortProfile)); (void)memset(PlsrProfileQueues, 0, sizeof(PlsrProfileQueues)); PlsrProfileQueueBank = 0U; (void)memset(&PlsrHandoffPlan, 0, sizeof(PlsrHandoffPlan)); (void)memset(&PlsrExecutor, 0, sizeof(PlsrExecutor)); (void)memset(PlsrPreparedHandoffPlans, 0, sizeof(PlsrPreparedHandoffPlans)); PlsrPreparedHandoffBank = 0U; PlsrRemainingPulses = 0UL; PlsrPulseActive = 0U; PlsrCutRequested = 0U; PlsrBoundaryPending = 0U; PlsrBoundaryWasCut = 0U; PlsrCountOverflowPending = 0U; PlsrCountedObservedPublished = 0ULL; PlsrCountedHandoffStaged = 0U; PlsrCountedBoundaryWriteIndex = 0U; PlsrCountedBoundaryReadIndex = 0U; PlsrCountedBoundaryBusy = 0U; (void)memset(PlsrCountedBoundaryEvents, 0, sizeof(PlsrCountedBoundaryEvents)); PlsrCountedFaultEvent = 0U; PlsrPositionCheckpointDirty = 0U; PlsrFrequencyUpdatePending = 0U; PlsrDeferredFrequencyPending = 0U; PlsrFrequencyUpdateSegment = 0U; PlsrSeamlessHandoffPending = 0U; PlsrTimerErrorPending = 0U; PlsrCurrentFrequencyHz = 0UL; PlsrQueuedFrequencyHz = 0UL; PlsrBoundaryFrequencyHz = 0UL; PlsrFrequencyUpdateTargetHz = 0UL; PlsrDeferredFrequencyHz = 0UL; PlsrSegmentEpoch = 0UL; PlsrCurrentSegment = 0U; PlsrDirectionDelayActive = 0U; PlsrDirectionDelayRemainingMs = 0U; PlsrSegmentClockStarted = 0U; PlsrExtPreviousLevel = 0U; PlsrExtEdgePending = 0U; PlsrStopRequested = 0U; PlsrStopPulsesRemaining = 0U; PlsrAbStopArmed = 0U; PlsrLastDirectionValid = 0U; PlsrPositionCheckpointElapsedMs = 0U; PlsrCommandMailbox.command = 0U; PlsrCommandMailbox.state = PLSR_COMMAND_MAILBOX_EMPTY; PlsrDiagnosticReset(); #ifdef PLSR_HOST_TEST PlsrHostTestEventSequence = 0UL; PlsrHostTestLastGateEvent = 0UL; PlsrHostTestLastDiagnosticFinishEvent = 0UL; #endif #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) PlsrProfileProducerItemCount = 0UL; PlsrProfileProducerTotalCycles = 0UL; PlsrProfileProducerMaxItemCycles = 0UL; #endif PlsrError = PLSR_ERROR_NONE; PlsrRunStatus = PLSR_STATUS_IDLE; PlsrInitialized = 1U; return 1U; } static PLSR_MB_RESULT PlsrQueueCommand(uint16_t command) { uint32_t criticalState; PLSR_MB_RESULT result = PLSR_MB_OK; if (PlsrInitialized == 0U) { return PLSR_MB_SERVER_FAILURE; } criticalState = PlsrPlatformEnterCritical(); /* Capacity one: repeats acknowledge the first command; conflicts wait. */ if (PlsrCommandMailbox.state != PLSR_COMMAND_MAILBOX_EMPTY) { result = (PlsrCommandMailbox.command == command) ? PLSR_MB_OK : PLSR_MB_DEVICE_BUSY; PlsrPlatformExitCritical(criticalState); return result; } if (command == PLSR_COMMAND_START) { if ((PlsrIsBusy() != 0U) || (PlsrRunStatus == PLSR_STATUS_ERROR)) { result = PLSR_MB_DEVICE_BUSY; } else if ((PlsrRunStatus != PLSR_STATUS_IDLE) && (PlsrRunStatus != PLSR_STATUS_COMPLETED) && (PlsrRunStatus != PLSR_STATUS_STOPPED)) { result = PLSR_MB_ILLEGAL_VALUE; } else if ((PlsrConfigIsValid(&PlsrShadowConfig, 1U) == 0U) || ((PlsrShadowConfig.positionMode == PLSR_POSITION_ABSOLUTE) && (PlsrPositionValid == 0U))) { result = PLSR_MB_ILLEGAL_VALUE; } else { PlsrCommandMailbox.startConfig = PlsrShadowConfig; } } else if ((command == PLSR_COMMAND_CLEAR) && (PlsrIsBusy() != 0U)) { result = PLSR_MB_DEVICE_BUSY; } if (result == PLSR_MB_OK) { PlsrCommandMailbox.command = command; PlsrCommandMailbox.state = PLSR_COMMAND_MAILBOX_PENDING; } PlsrPlatformExitCritical(criticalState); return result; } static void PlsrExecuteStart(void) { uint32_t criticalState; uint8_t handoffBank; uint8_t index; PlsrActiveConfig = PlsrCommandMailbox.startConfig; for (index = 0U; index < PlsrActiveConfig.segmentCount; index++) { PlsrActiveConfig.segments[index].frequencyHz = PlsrResolvedSegmentFrequency(&PlsrActiveConfig, index); } handoffBank = PlsrBuildHandoffPlanBank(&PlsrActiveConfig); criticalState = PlsrPlatformEnterCritical(); PlsrPreparedHandoffBank = handoffBank; PlsrHandoffPlan.valid = 0U; PlsrPlatformExitCritical(criticalState); PlsrStopRequested = 0U; PlsrTimerErrorPending = 0U; PlsrShortProfile.active = 0U; PlsrError = PLSR_ERROR_NONE; PlsrLastDirectionValid = 0U; PlsrCheckpointPosition(1U); if (PlsrStartSegment((uint8_t)PlsrActiveConfig.startSegment, 0U, 0UL) == 0U) { PlsrEnterError((PlsrTimerErrorPending != 0U) ? PLSR_ERROR_TIMER : PLSR_ERROR_INVALID_RESOURCE); } } static uint8_t PlsrExecuteStop(void) { uint32_t criticalState; uint32_t stopTargetHz; uint32_t appliedHz; uint64_t drainPulses; if (PlsrIsBusy() == 0U) { return 0U; } criticalState = PlsrPlatformEnterCritical(); if (PlsrStopRequested != 0U) { PlsrPlatformExitCritical(criticalState); return 0U; } PlsrStopRequested = 1U; PlsrStopPulsesRemaining = 0U; if (PlsrBoundaryPending != 0U) { PlsrPlatformExitCritical(criticalState); return 1U; } if (PlsrPulseActive == 0U) { PlsrPlatformExitCritical(criticalState); PlsrFinishStopped(); return 1U; } if (PlsrActiveConfig.outputMode != PLSR_OUTPUT_PULSE_DIR) { PlsrShortProfile.active = 0U; PlsrProfileQueueInvalidateGeneration(); PlsrHandoffPlan.valid = 0U; PlsrInvalidateHandoffPlans(); } stopTargetHz = PlsrEffectiveStopFrequency( PlsrActiveConfig.segments[PlsrCurrentSegment - 1U].frequencyHz); appliedHz = (PlsrQueuedFrequencyHz != 0UL) ? PlsrQueuedFrequencyHz : PlsrCurrentFrequencyHz; if (stopTargetHz > appliedHz) { stopTargetHz = appliedHz; } if (PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR) { drainPulses = PlsrRampPulseEstimate(appliedHz, stopTargetHz, appliedHz) + 1ULL; if (drainPulses > PlsrRemainingPulses) { drainPulses = PlsrRemainingPulses; } if (drainPulses == 0ULL) { drainPulses = 1ULL; } PlsrRemainingPulses = drainPulses; PlsrPlatformExitCritical(criticalState); if (PlsrReplanPulseDir(stopTargetHz, (uint32_t)drainPulses) == 0U) { PlsrEnterError(PLSR_ERROR_TIMER); return 1U; } PlsrInvalidateHandoffPlans(); criticalState = PlsrPlatformEnterCritical(); PlsrRunStatus = PLSR_STATUS_DECELERATING; PlsrPlatformExitCritical(criticalState); return 1U; } PlsrRampStart(PlsrCurrentFrequencyHz, stopTargetHz); PlsrRunStatus = PLSR_STATUS_DECELERATING; if (PlsrRamp.active == 0U) { if (PlsrApplyFrequency(stopTargetHz, PlsrSegmentEpoch) == 0U) { PlsrPlatformExitCritical(criticalState); PlsrEnterError(PLSR_ERROR_TIMER); return 1U; } (void)PlsrArmStopDrainLocked(); } PlsrPlatformExitCritical(criticalState); return 1U; } static void PlsrExecuteClear(void) { uint32_t criticalState; criticalState = PlsrPlatformEnterCritical(); PlsrPosition = 0L; PlsrPositionValid = 1U; PlsrRemainingPulses = 0UL; PlsrPlatformExitCritical(criticalState); PlsrPlatformCheckpointPosition(0L, 1U, 0U); PlsrCountOverflowPending = 0U; PlsrPositionCheckpointDirty = 0U; PlsrPositionCheckpointElapsedMs = 0U; PlsrCurrentSegment = 0U; PlsrCurrentFrequencyHz = 0UL; PlsrQueuedFrequencyHz = 0UL; PlsrCutRequested = 0U; PlsrBoundaryPending = 0U; PlsrBoundaryWasCut = 0U; PlsrDirectionDelayActive = 0U; PlsrDirectionDelayRemainingMs = 0U; PlsrExtEdgePending = 0U; PlsrShortProfile.active = 0U; PlsrProfileQueueReset(); PlsrHandoffPlan.valid = 0U; PlsrInvalidateHandoffPlans(); PlsrDeferredFrequencyPending = 0U; PlsrTimerErrorPending = 0U; PlsrRamp.active = 0U; PlsrExecutor.mode = PLSR_EXEC_IDLE; PlsrError = PLSR_ERROR_NONE; PlsrRunStatus = PLSR_STATUS_IDLE; PlsrMarkPersistenceDirty(PLSR_CONFIG_SAVE_DELAY_MS); } static uint8_t PlsrPollCommandMailbox(void) { uint16_t command; uint8_t endPoll = 1U; uint32_t criticalState; criticalState = PlsrPlatformEnterCritical(); if (PlsrCommandMailbox.state != PLSR_COMMAND_MAILBOX_PENDING) { PlsrPlatformExitCritical(criticalState); return 0U; } PlsrCommandMailbox.state = PLSR_COMMAND_MAILBOX_EXECUTING; command = PlsrCommandMailbox.command; PlsrPlatformExitCritical(criticalState); switch (command) { case PLSR_COMMAND_START: PlsrExecuteStart(); break; case PLSR_COMMAND_STOP: endPoll = PlsrExecuteStop(); break; case PLSR_COMMAND_CLEAR: PlsrExecuteClear(); break; default: PlsrEnterError(PLSR_ERROR_INTERNAL); break; } criticalState = PlsrPlatformEnterCritical(); PlsrCommandMailbox.command = 0U; PlsrCommandMailbox.state = PLSR_COMMAND_MAILBOX_EMPTY; PlsrPlatformExitCritical(criticalState); return endPoll; } static void PlsrEnterErrorIfEpoch(PLSR_ERROR error, uint32_t expectedEpoch) { uint32_t criticalState = PlsrPlatformEnterCritical(); if (PlsrSegmentEpoch == expectedEpoch) { PlsrEnterError(error); } PlsrPlatformExitCritical(criticalState); } static uint8_t PlsrServiceCountedExecutor(void) { uint32_t completedPulses; uint32_t completedFrequencyHz; uint32_t activeFrequencyHz; uint32_t queuedFrequencyHz; uint8_t completedNormally; if ((PlsrPulseActive == 0U) || ((PlsrExecutor.mode != PLSR_EXEC_STEP_TABLE) && (PlsrExecutor.mode != PLSR_EXEC_STREAM) && (PlsrExecutor.mode != PLSR_EXEC_STOPPING))) { return 0U; } if (PlsrSyncCountedProgress() == 0U) { #if PLSR_COUNT_ERROR_CHECK_ENABLED PlsrEnterError(PLSR_ERROR_COUNT); return 1U; #else /* 计数错误检查暂时关闭:observed 回退(平台计数器基准重置造成的 视图回退)时重新校准发布点,继续运行,不进错误。 */ PlsrCountedObservedPublished = PlsrPlatformObservedPulses( (uint8_t)PlsrActiveConfig.pulseOutput); return 0U; #endif } activeFrequencyHz = PlsrPlatformActiveFrequency( (uint8_t)PlsrActiveConfig.pulseOutput); if (activeFrequencyHz != 0UL) { PlsrCurrentFrequencyHz = activeFrequencyHz; PlsrDiagnosticCheckActiveFrequency(activeFrequencyHz); queuedFrequencyHz = PlsrQueuedFrequencyHz; if (queuedFrequencyHz > activeFrequencyHz) { PlsrRunStatus = PLSR_STATUS_ACCELERATING; } else if (queuedFrequencyHz < activeFrequencyHz) { PlsrRunStatus = PLSR_STATUS_DECELERATING; } else { PlsrRunStatus = PLSR_STATUS_RUNNING; } } if (PlsrPlatformTakeFiniteCompletion( (uint8_t)PlsrActiveConfig.pulseOutput, &completedPulses) == 0U) { return 0U; } (void)completedPulses; (void)PlsrSyncCountedProgress(); completedFrequencyHz = PlsrCurrentFrequencyHz; completedNormally = ((PlsrRemainingPulses == 0ULL) && (PlsrCutRequested == 0U) && (PlsrCountOverflowPending == 0U) && (PlsrTimerErrorPending == 0U)) ? 1U : 0U; PlsrDiagnosticFinishSegment(completedNormally); PlsrShortProfile.active = 0U; PlsrProfileQueue.active = 0U; PlsrHandoffPlan.valid = 0U; PlsrCountedHandoffStaged = 0U; PlsrBoundaryFrequencyHz = completedFrequencyHz; PlsrBoundaryWasCut = (PlsrCutRequested != 0U) ? 1U : 0U; PlsrCutRequested = 0U; PlsrPulseActive = 0U; PlsrCurrentFrequencyHz = 0UL; PlsrQueuedFrequencyHz = 0UL; PlsrBoundaryPending = 1U; PlsrExecutor.mode = PLSR_EXEC_IDLE; return 1U; } void PlsrPoll1ms(void) { uint8_t extLevel; uint8_t extEdge; uint8_t activeSegmentNumber; uint8_t applyDynamicFrequency = 0U; uint8_t pulseDirReplanRequested = 0U; uint8_t pulseDirReplanFailed = 0U; PLSR_SEGMENT_CONFIG *activeSegment; uint32_t criticalState; uint32_t newTargetHz; uint32_t pulseDirReplanPulses = 0UL; uint32_t pollEpoch; uint16_t fillBudget = PLSR_PROFILE_REFILL_BUDGET; if (PlsrInitialized == 0U) { return; } if (PlsrPollCommandMailbox() != 0U) { return; } /* 段边界事件必须在任何计数同步之前消费:快照是 IRQ 边界时刻的 硬件计数,晚消费会把下一 run 的脉冲算进旧段。 */ PlsrExecServiceCountedEvents(); if (PlsrTimerErrorPending != 0U) { PlsrEnterError(PLSR_ERROR_TIMER); return; } PlsrPollPositionCheckpoint(); if (PlsrServiceCountedExecutor() != 0U) { if (PlsrBoundaryPending != 0U) { PlsrHandleBoundary(0U); } PlsrPollPersistenceDelay(); return; } if (PlsrProfileQueueCount() <= PLSR_PROFILE_LOW_WATER) { fillBudget = PLSR_PROFILE_STARTUP_BUDGET; } if (((PlsrExecutor.mode == PLSR_EXEC_STEP_TABLE) || (PlsrExecutor.mode == PLSR_EXEC_STREAM) || (PlsrExecutor.mode == PLSR_EXEC_AB_LEGACY)) && (PlsrProfileQueue.active != 0U) && (PlsrProfileQueue.generatorComplete == 0U) && (PlsrProfileQueueFill(PLSR_PROFILE_REFILL_TARGET, &fillBudget) == 0U)) { PlsrEnterError(PLSR_ERROR_TIMER); return; } if (((PlsrExecutor.mode == PLSR_EXEC_STEP_TABLE) || (PlsrExecutor.mode == PLSR_EXEC_STREAM)) && ((PlsrStageCountedHandoff() == 0U) || (PlsrProfileQueueFill(PLSR_PROFILE_REFILL_TARGET, &fillBudget) == 0U))) { PlsrEnterError(PLSR_ERROR_TIMER); return; } criticalState = PlsrPlatformEnterCritical(); pollEpoch = PlsrSegmentEpoch; extLevel = PlsrPlatformReadInput((uint8_t)PlsrActiveConfig.extInput); extEdge = ((extLevel != 0U) && (PlsrExtPreviousLevel == 0U)) ? 1U : 0U; PlsrExtPreviousLevel = extLevel; if (PlsrExtEdgePending != 0U) { extEdge = 1U; } PlsrPlatformExitCritical(criticalState); if (PlsrBoundaryPending != 0U) { PlsrExtEdgePending = 0U; PlsrHandleBoundary(extEdge); PlsrPollPersistenceDelay(); return; } if (PlsrIsBusy() == 0U) { PlsrExtEdgePending = 0U; PlsrPollPersistenceDelay(); return; } if (PlsrSegmentEpoch != pollEpoch) { return; } if (PlsrDirectionDelayActive != 0U) { if (extEdge != 0U) { PlsrExtEdgePending = 1U; } if (PlsrDirectionDelayRemainingMs != 0U) { PlsrDirectionDelayRemainingMs--; } if (PlsrDirectionDelayRemainingMs == 0U) { PlsrDirectionDelayActive = 0U; if (PlsrBeginSegmentOutput(PlsrEffectiveStartFrequency( PlsrActiveConfig.segments[PlsrCurrentSegment - 1U] .frequencyHz, 0U, 1U, 0UL)) == 0U) { PlsrEnterError(PLSR_ERROR_TIMER); } } return; } criticalState = PlsrPlatformEnterCritical(); if (PlsrSegmentEpoch != pollEpoch) { PlsrPlatformExitCritical(criticalState); return; } PlsrExtEdgePending = 0U; if (PlsrSegmentClockStarted != 0U) { PlsrSegmentElapsedMs++; } if (PlsrRunStatus == PLSR_STATUS_WAITING) { PlsrPlatformExitCritical(criticalState); PlsrPollWaiting(extEdge); return; } activeSegmentNumber = PlsrCurrentSegment; if ((activeSegmentNumber == 0U) || (activeSegmentNumber > PlsrActiveConfig.segmentCount)) { PlsrPlatformExitCritical(criticalState); PlsrEnterError(PLSR_ERROR_INTERNAL); return; } activeSegment = &PlsrActiveConfig.segments[activeSegmentNumber - 1U]; if (PlsrSeamlessHandoffPending != 0U) { PlsrSeamlessHandoffPending = 0U; if ((PlsrStopRequested == 0U) && (PlsrShortProfile.active == 0U) && (PlsrFrequencyUpdatePending == 0U)) { PlsrRamp.active = 0U; PlsrRunStatus = PLSR_STATUS_RUNNING; } } newTargetHz = PlsrFrequencyUpdateTargetHz; if ((PlsrStopRequested == 0U) && (PlsrFrequencyUpdatePending != 0U) && (PlsrAbStopArmed == 0U) && (PlsrFrequencyUpdateSegment == activeSegmentNumber)) { PlsrFrequencyUpdatePending = 0U; activeSegment->frequencyHz = newTargetHz; PlsrBoundaryRampStarted = 0U; if (PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR) { uint64_t remaining = PlsrRemainingPulses; if ((remaining == 0ULL) || (remaining > 0xFFFFFFFFULL)) { pulseDirReplanFailed = 1U; } else { pulseDirReplanPulses = (uint32_t)remaining; pulseDirReplanRequested = 1U; } } else { PlsrShortProfile.active = 0U; PlsrProfileQueueInvalidateGeneration(); PlsrRampStart(PlsrCurrentFrequencyHz, newTargetHz); if (PlsrRamp.active == 0U) { applyDynamicFrequency = 1U; } } } PlsrPlatformExitCritical(criticalState); if ((pulseDirReplanRequested != 0U) && (PlsrReplanPulseDir(newTargetHz, pulseDirReplanPulses) == 0U)) { pulseDirReplanFailed = 1U; } if (pulseDirReplanFailed != 0U) { PlsrEnterErrorIfEpoch(PLSR_ERROR_TIMER, pollEpoch); return; } if (applyDynamicFrequency != 0U) { if (PlsrApplyFrequency(newTargetHz, pollEpoch) == 0U) { PlsrEnterErrorIfEpoch(PLSR_ERROR_TIMER, pollEpoch); return; } criticalState = PlsrPlatformEnterCritical(); if (PlsrSegmentEpoch == pollEpoch) { PlsrRunStatus = PLSR_STATUS_RUNNING; } PlsrPlatformExitCritical(criticalState); } if ((PlsrActiveConfig.outputMode == PLSR_OUTPUT_AB) && (PlsrShortProfile.active == 0U) && (PlsrRamp.active != 0U) && (PlsrRampAdvance(pollEpoch) == 0U)) { PlsrEnterErrorIfEpoch(PLSR_ERROR_TIMER, pollEpoch); return; } criticalState = PlsrPlatformEnterCritical(); if (PlsrSegmentEpoch != pollEpoch) { PlsrPlatformExitCritical(criticalState); return; } if ((PlsrActiveConfig.outputMode == PLSR_OUTPUT_AB) && (PlsrStopRequested != 0U) && (PlsrRamp.active == 0U)) { if (PlsrStopPulsesRemaining == 0U) { if (PlsrArmStopDrainLocked() == 0U) { PlsrPlatformExitCritical(criticalState); PlsrEnterError(PLSR_ERROR_TIMER); return; } } else { (void)PlsrArmFinalAbBoundaryLocked(); } PlsrPlatformExitCritical(criticalState); return; } PlsrPlatformExitCritical(criticalState); if ((activeSegment->waitType == PLSR_ACT_TIME) && (PlsrSegmentElapsedMs >= activeSegment->actTimeMs)) { PlsrRequestCut(pollEpoch); return; } if (((activeSegment->waitType == PLSR_EXT_SIGNAL) || (activeSegment->waitType == PLSR_EXT_OR_COMPLETE)) && (extEdge != 0U)) { PlsrRequestCut(pollEpoch); return; } if (PlsrActiveConfig.outputMode == PLSR_OUTPUT_AB) { PlsrMaybePlanBoundaryRamp(pollEpoch); } } void PlsrPulseTimerIrq(uint8_t pulseOutput) { uint32_t positionBits; uint32_t completedFrequencyHz; uint64_t remainingPulses; PLSR_PLATFORM_QUEUE_RESULT queueResult; if ((PlsrPulseActive == 0U) || (pulseOutput != (uint8_t)PlsrActiveConfig.pulseOutput)) { return; } if ((PlsrExecutor.mode == PLSR_EXEC_STEP_TABLE) || (PlsrExecutor.mode == PLSR_EXEC_STREAM) || (PlsrExecutor.mode == PLSR_EXEC_STOPPING)) { return; } completedFrequencyHz = PlsrCurrentFrequencyHz; PlsrDiagnosticCheckActiveFrequency(completedFrequencyHz); PlsrCurrentFrequencyHz = PlsrPlatformActiveFrequency(pulseOutput); if (PlsrCurrentFrequencyHz == 0UL) { PlsrCurrentFrequencyHz = PlsrQueuedFrequencyHz; } if ((PlsrStopRequested != 0U) && (PlsrRamp.active == 0U) && (PlsrStopPulsesRemaining != 0U)) { PlsrStopPulsesRemaining--; if (PlsrStopPulsesRemaining == 0U) { PlsrCutRequested = 1U; } } positionBits = (uint32_t)PlsrPosition; if (PlsrCountPositive != 0U) { if (positionBits == (uint32_t)INT32_MAX) { PlsrPositionValid = 0U; #if PLSR_COUNT_ERROR_CHECK_ENABLED PlsrCountOverflowPending = 1U; #endif } positionBits++; } else { if (positionBits == (uint32_t)INT32_MIN) { PlsrPositionValid = 0U; #if PLSR_COUNT_ERROR_CHECK_ENABLED PlsrCountOverflowPending = 1U; #endif } positionBits--; } PlsrPosition = (int32_t)positionBits; PlsrPositionCheckpointDirty = 1U; remainingPulses = PlsrRemainingPulses; if (remainingPulses != 0UL) { remainingPulses--; PlsrRemainingPulses = remainingPulses; } if ((remainingPulses == 0UL) || (PlsrCutRequested != 0U) || (PlsrCountOverflowPending != 0U)) { if (remainingPulses == 0UL) { queueResult = PlsrTryContinuousHandoff(); if (queueResult != PLSR_PLATFORM_QUEUE_FAILED) { return; } } PlsrPlatformGateFromIrq(pulseOutput); #ifdef PLSR_HOST_TEST PlsrHostTestEventSequence++; PlsrHostTestLastGateEvent = PlsrHostTestEventSequence; #endif PlsrDiagnosticFinishSegment( ((remainingPulses == 0UL) && (PlsrCutRequested == 0U) && (PlsrCountOverflowPending == 0U)) ? 1U : 0U); PlsrShortProfile.active = 0U; PlsrHandoffPlan.valid = 0U; PlsrBoundaryFrequencyHz = completedFrequencyHz; PlsrBoundaryWasCut = (PlsrCutRequested != 0U) ? 1U : 0U; PlsrCutRequested = 0U; PlsrAbStopArmed = 0U; PlsrPulseActive = 0U; PlsrCurrentFrequencyHz = 0UL; PlsrQueuedFrequencyHz = 0UL; PlsrBoundaryPending = 1U; return; } if (PlsrShortProfile.active != 0U) { queueResult = PlsrProfileQueueCommitNext(pulseOutput); if (queueResult == PLSR_PLATFORM_QUEUE_STALE) { return; } if (queueResult == PLSR_PLATFORM_QUEUE_FAILED) { PlsrDiagnosticFinishSegment(0U); PlsrTimerErrorPending = 1U; PlsrShortProfile.active = 0U; PlsrHandoffPlan.valid = 0U; PlsrBoundaryFrequencyHz = completedFrequencyHz; PlsrBoundaryWasCut = 0U; PlsrCutRequested = 0U; PlsrPlatformGateFromIrq(pulseOutput); PlsrAbStopArmed = 0U; PlsrPulseActive = 0U; PlsrCurrentFrequencyHz = 0UL; PlsrQueuedFrequencyHz = 0UL; PlsrBoundaryPending = 1U; return; } } if ((PlsrShortProfile.active == 0U) && (remainingPulses == 1UL)) { if (PlsrPrimeHandoff() == PLSR_PLATFORM_QUEUE_STALE) { return; } } if ((PlsrShortProfile.active == 0U) && (PlsrHandoffPlan.valid == 0U) && (PlsrDeferredFrequencyPending != 0U)) { queueResult = PlsrCommitDeferredFrequency(pulseOutput); if (queueResult == PLSR_PLATFORM_QUEUE_STALE) { return; } if (queueResult == PLSR_PLATFORM_QUEUE_FAILED) { PlsrDiagnosticFinishSegment(0U); PlsrTimerErrorPending = 1U; PlsrBoundaryFrequencyHz = completedFrequencyHz; PlsrBoundaryWasCut = 0U; PlsrCutRequested = 0U; PlsrPlatformGateFromIrq(pulseOutput); PlsrAbStopArmed = 0U; PlsrPulseActive = 0U; PlsrCurrentFrequencyHz = 0UL; PlsrQueuedFrequencyHz = 0UL; PlsrBoundaryPending = 1U; return; } } if ((PlsrActiveConfig.outputMode == PLSR_OUTPUT_AB) && (PlsrQueueFinalAbBoundaryFromIrq() == 0U)) { return; } } PLSR_MB_RESULT PlsrModbusReadHolding(uint16_t startAddress, uint16_t quantity, uint16_t *values) { PLSR_MB_RESULT classification; uint16_t index; uint32_t criticalState; int32_t position; uint32_t frequency; uint16_t statusWords[7]; uint16_t diagnosticWords[26]; uint16_t queueDiagnosticWords[9]; if (values == NULL) { return PLSR_MB_ILLEGAL_VALUE; } classification = PlsrClassifyRange(startAddress, quantity); if (classification != PLSR_MB_OK) { return classification; } criticalState = PlsrPlatformEnterCritical(); position = PlsrPosition; frequency = PlsrCurrentFrequencyHz; statusWords[0] = PlsrLowWord((uint32_t)position); statusWords[1] = PlsrHighWord((uint32_t)position); statusWords[2] = PlsrLowWord(frequency); statusWords[3] = PlsrHighWord(frequency); statusWords[4] = (uint16_t)PlsrRunStatus; statusWords[5] = PlsrCurrentSegment; statusWords[6] = (uint16_t)PlsrError; diagnosticWords[0] = PlsrDiagnostic.flags; diagnosticWords[1] = PlsrDiagnostic.reason; diagnosticWords[2] = PlsrDiagnostic.lastSegment; diagnosticWords[3] = PlsrDiagnostic.modeDirection; diagnosticWords[4] = PlsrLowWord(PlsrDiagnostic.expectedCycles); diagnosticWords[5] = PlsrHighWord(PlsrDiagnostic.expectedCycles); diagnosticWords[6] = PlsrLowWord(PlsrDiagnostic.observedCycles); diagnosticWords[7] = PlsrHighWord(PlsrDiagnostic.observedCycles); diagnosticWords[8] = PlsrLowWord((uint32_t)PlsrDiagnostic.countError); diagnosticWords[9] = PlsrHighWord((uint32_t)PlsrDiagnostic.countError); diagnosticWords[10] = PlsrLowWord(PlsrDiagnostic.requestedHz); diagnosticWords[11] = PlsrHighWord(PlsrDiagnostic.requestedHz); diagnosticWords[12] = PlsrLowWord(PlsrDiagnostic.expectedTimerHz); diagnosticWords[13] = PlsrHighWord(PlsrDiagnostic.expectedTimerHz); diagnosticWords[14] = PlsrLowWord(PlsrDiagnostic.activeTimerHz); diagnosticWords[15] = PlsrHighWord(PlsrDiagnostic.activeTimerHz); diagnosticWords[16] = PlsrLowWord((uint32_t)PlsrDiagnostic.requestErrorHz); diagnosticWords[17] = PlsrHighWord((uint32_t)PlsrDiagnostic.requestErrorHz); diagnosticWords[18] = PlsrLowWord(PlsrDiagnostic.curveSampleCount); diagnosticWords[19] = PlsrHighWord(PlsrDiagnostic.curveSampleCount); diagnosticWords[20] = PlsrLowWord(PlsrDiagnostic.curveMismatchCount); diagnosticWords[21] = PlsrHighWord(PlsrDiagnostic.curveMismatchCount); diagnosticWords[22] = PlsrLowWord(PlsrDiagnostic.curveMaxAbsErrorHz); diagnosticWords[23] = PlsrHighWord(PlsrDiagnostic.curveMaxAbsErrorHz); diagnosticWords[24] = PlsrLowWord(PlsrDiagnostic.firstMismatchSample); diagnosticWords[25] = PlsrHighWord(PlsrDiagnostic.firstMismatchSample); queueDiagnosticWords[0] = PlsrProfileQueueDiagnosticFlags; queueDiagnosticWords[1] = PlsrProfileQueueMinimumDepth; queueDiagnosticWords[2] = PlsrProfileQueueCount(); queueDiagnosticWords[3] = PlsrLowWord(PlsrProfileQueueLowWaterEvents); queueDiagnosticWords[4] = PlsrHighWord(PlsrProfileQueueLowWaterEvents); queueDiagnosticWords[5] = PlsrLowWord(PlsrProfileQueueUnderruns); queueDiagnosticWords[6] = PlsrHighWord(PlsrProfileQueueUnderruns); queueDiagnosticWords[7] = PlsrLowWord(PlsrPlannerClippedCount); queueDiagnosticWords[8] = PlsrHighWord(PlsrPlannerClippedCount); PlsrPlatformExitCritical(criticalState); for (index = 0U; index < quantity; index++) { uint16_t address = (uint16_t)(startAddress + index); if ((address >= PLSR_CONFIG_FIRST_ADDRESS) && (address <= PLSR_CONFIG_LAST_ADDRESS)) { values[index] = PlsrReadConfigWord(&PlsrShadowConfig, address); } else if (address == PLSR_OUTPUT_MODE_ADDRESS) { values[index] = PlsrShadowConfig.outputMode; } else if ((address >= PLSR_STATUS_FIRST_ADDRESS) && (address <= PLSR_STATUS_LAST_ADDRESS)) { values[index] = statusWords[address - PLSR_STATUS_FIRST_ADDRESS]; } else if ((address >= PLSR_DIAGNOSTIC_FIRST_ADDRESS) && (address <= PLSR_DIAGNOSTIC_LAST_ADDRESS)) { values[index] = diagnosticWords[address - PLSR_DIAGNOSTIC_FIRST_ADDRESS]; } else if ((address >= PLSR_QUEUE_DIAGNOSTIC_FIRST_ADDRESS) && (address <= PLSR_QUEUE_DIAGNOSTIC_LAST_ADDRESS)) { values[index] = queueDiagnosticWords[ address - PLSR_QUEUE_DIAGNOSTIC_FIRST_ADDRESS]; } else if (address == PLSR_CONTROL_ADDRESS) { values[index] = 0U; } else if (address == PLSR_DIAGNOSTIC_CONTROL_ADDRESS) { values[index] = 0U; } else { return PLSR_MB_ILLEGAL_ADDRESS; } } return PLSR_MB_OK; } PLSR_MB_RESULT PlsrModbusWriteHolding(uint16_t startAddress, uint16_t quantity, const uint16_t *values) { PLSR_MB_RESULT classification; PLSR_WORD_RESULT wordResult; uint16_t index; uint16_t pairedAddress; uint32_t requestEnd; uint32_t criticalState; uint8_t handoffBank = 0U; uint8_t updateActiveFrequencies = 0U; uint8_t drainedToDifferentSegment = 0U; uint8_t segmentBeforeDrain; uint32_t drainedSegmentTargetHz = 0UL; PLSR_PLATFORM_QUEUE_RESULT queueResult; if (values == NULL) { return PLSR_MB_ILLEGAL_VALUE; } classification = PlsrClassifyRange(startAddress, quantity); if (classification != PLSR_MB_OK) { return classification; } if ((startAddress >= PLSR_STATUS_FIRST_ADDRESS) && (startAddress <= PLSR_STATUS_LAST_ADDRESS)) { return PLSR_MB_ILLEGAL_ADDRESS; } if ((startAddress >= PLSR_DIAGNOSTIC_FIRST_ADDRESS) && (startAddress <= PLSR_DIAGNOSTIC_LAST_ADDRESS)) { return PLSR_MB_ILLEGAL_ADDRESS; } if ((startAddress >= PLSR_QUEUE_DIAGNOSTIC_FIRST_ADDRESS) && (startAddress <= PLSR_QUEUE_DIAGNOSTIC_LAST_ADDRESS)) { return PLSR_MB_ILLEGAL_ADDRESS; } if (startAddress == PLSR_DIAGNOSTIC_CONTROL_ADDRESS) { if (quantity != 1U) { return PLSR_MB_ILLEGAL_ADDRESS; } if ((values[0] != 0U) && (values[0] != 1U)) { return PLSR_MB_ILLEGAL_VALUE; } if ((values[0] != 0U) && (PlsrIsBusy() != 0U)) { return PLSR_MB_DEVICE_BUSY; } if (values[0] != 0U) { criticalState = PlsrPlatformEnterCritical(); PlsrDiagnosticReset(); PlsrPlatformExitCritical(criticalState); } return PLSR_MB_OK; } if (startAddress == PLSR_CONTROL_ADDRESS) { if (quantity != 1U) { return PLSR_MB_ILLEGAL_ADDRESS; } switch (values[0]) { case 0U: return PLSR_MB_OK; case PLSR_COMMAND_START: case PLSR_COMMAND_STOP: case PLSR_COMMAND_CLEAR: return PlsrQueueCommand(values[0]); default: return PLSR_MB_ILLEGAL_VALUE; } } if (startAddress == PLSR_OUTPUT_MODE_ADDRESS) { if (quantity != 1U) { return PLSR_MB_ILLEGAL_ADDRESS; } if (PlsrIsBusy() != 0U) { return PLSR_MB_DEVICE_BUSY; } PlsrCandidateConfig = PlsrShadowConfig; PlsrCandidateConfig.outputMode = values[0]; if (PlsrConfigIsValid(&PlsrCandidateConfig, 0U) == 0U) { return PLSR_MB_ILLEGAL_VALUE; } criticalState = PlsrPlatformEnterCritical(); PlsrShadowConfig = PlsrCandidateConfig; PlsrPlatformExitCritical(criticalState); PlsrPlatformCheckpointConfig(&PlsrShadowConfig); PlsrMarkPersistenceDirty(PLSR_CONFIG_SAVE_DELAY_MS); return PLSR_MB_OK; } if ((startAddress < PLSR_CONFIG_FIRST_ADDRESS) || ((uint32_t)startAddress + quantity - 1UL > PLSR_CONFIG_LAST_ADDRESS)) { return PLSR_MB_ILLEGAL_ADDRESS; } if ((PlsrIsBusy() != 0U) && (startAddress <= 0x1001U) && ((uint32_t)startAddress + quantity - 1UL >= 0x1000UL)) { return PLSR_MB_DEVICE_BUSY; } requestEnd = (uint32_t)startAddress + quantity; for (index = 0U; index < quantity; index++) { uint16_t address = (uint16_t)(startAddress + index); if (PlsrAddressIsDwordHalf(address, &pairedAddress) != 0U) { if (((uint32_t)pairedAddress < startAddress) || ((uint32_t)pairedAddress >= requestEnd)) { return PLSR_MB_ILLEGAL_ADDRESS; } } } PlsrCandidateConfig = PlsrShadowConfig; for (index = 0U; index < quantity; index++) { wordResult = PlsrWriteConfigWord(&PlsrCandidateConfig, (uint16_t)(startAddress + index), values[index]); if (wordResult == PLSR_WORD_ILLEGAL_ADDRESS) { return PLSR_MB_ILLEGAL_ADDRESS; } if (wordResult == PLSR_WORD_ILLEGAL_VALUE) { return PLSR_MB_ILLEGAL_VALUE; } } if (PlsrConfigIsValid(&PlsrCandidateConfig, 0U) == 0U) { return PLSR_MB_ILLEGAL_VALUE; } if (PlsrIsBusy() != 0U) { for (index = 0U; index < PlsrActiveConfig.segmentCount; index++) { if (PlsrCandidateConfig.segments[index].frequencyHz != PlsrShadowConfig.segments[index].frequencyHz) { updateActiveFrequencies = 1U; } } } if (updateActiveFrequencies != 0U) { handoffBank = PlsrBuildHandoffPlanBank(&PlsrCandidateConfig); } criticalState = PlsrPlatformEnterCritical(); segmentBeforeDrain = PlsrCurrentSegment; if ((updateActiveFrequencies != 0U) && (PlsrPulseActive != 0U)) { PlsrPlatformDrainPendingPulse( (uint8_t)PlsrActiveConfig.pulseOutput); drainedToDifferentSegment = (PlsrCurrentSegment != segmentBeforeDrain) ? 1U : 0U; if ((drainedToDifferentSegment != 0U) && (PlsrCurrentSegment != 0U) && (PlsrCurrentSegment <= PlsrCandidateConfig.segmentCount)) { drainedSegmentTargetHz = PlsrResolvedSegmentFrequency( &PlsrCandidateConfig, PlsrCurrentSegment - 1U); } } PlsrShadowConfig = PlsrCandidateConfig; if (updateActiveFrequencies != 0U) { for (index = 0U; index < PlsrActiveConfig.segmentCount; index++) { uint8_t frequencyChanged = (PlsrActiveConfig.segments[index].frequencyHz != PlsrResolvedSegmentFrequency(&PlsrCandidateConfig, index)) ? 1U : 0U; PlsrActiveConfig.segments[index].frequencyHz = PlsrResolvedSegmentFrequency(&PlsrCandidateConfig, index); if ((frequencyChanged != 0U) && (index + 1U == PlsrCurrentSegment) && (PlsrStopRequested == 0U) && (PlsrAbStopArmed == 0U) && (PlsrRunStatus != PLSR_STATUS_WAITING)) { PlsrFrequencyUpdateTargetHz = PlsrResolvedSegmentFrequency(&PlsrCandidateConfig, index); PlsrFrequencyUpdateSegment = (uint8_t)(index + 1U); PlsrFrequencyUpdatePending = 1U; } } PlsrPreparedHandoffBank = handoffBank; PlsrHandoffPlan.valid = 0U; PlsrProfileQueueInvalidateGeneration(); if ((drainedToDifferentSegment == 0U) && (PlsrPulseActive != 0U) && (PlsrAbStopArmed == 0U) && (PlsrShortProfile.active == 0U) && (PlsrRemainingPulses == 1UL)) { (void)PlsrPrimeHandoff(); } if ((drainedToDifferentSegment != 0U) && (PlsrPulseActive != 0U) && (PlsrCurrentSegment != 0U) && (PlsrCurrentSegment <= PlsrActiveConfig.segmentCount) && (PlsrStopRequested == 0U) && (PlsrAbStopArmed == 0U) && (PlsrRunStatus != PLSR_STATUS_WAITING) && (PlsrCurrentFrequencyHz != drainedSegmentTargetHz)) { PlsrFrequencyUpdateTargetHz = drainedSegmentTargetHz; PlsrFrequencyUpdateSegment = PlsrCurrentSegment; PlsrFrequencyUpdatePending = 1U; } } PlsrPlatformExitCritical(criticalState); if ((updateActiveFrequencies != 0U) && (PlsrPulseActive != 0U) && (PlsrAbStopArmed == 0U) && (PlsrFrequencyUpdatePending == 0U) && (PlsrPrepareFutureHandoffQueue() == 0U)) { PlsrTimerErrorPending = 1U; } if ((drainedToDifferentSegment != 0U) && (PlsrAbStopArmed == 0U) && (PlsrFrequencyUpdatePending != 0U)) { uint32_t drainEpoch = PlsrSegmentEpoch; uint32_t actualDrainFrequencyHz; queueResult = PlsrDiagnosticQueueFrequency( (uint8_t)PlsrActiveConfig.pulseOutput, drainedSegmentTargetHz, &actualDrainFrequencyHz); if (queueResult == PLSR_PLATFORM_QUEUE_FAILED) { PlsrTimerErrorPending = 1U; } else if (queueResult == PLSR_PLATFORM_QUEUE_APPLIED) { uint32_t currentEpoch; uint8_t updateSegment; uint8_t currentSegment; criticalState = PlsrPlatformEnterCritical(); currentEpoch = PlsrSegmentEpoch; updateSegment = PlsrFrequencyUpdateSegment; currentSegment = PlsrCurrentSegment; if ((currentEpoch == drainEpoch) && (updateSegment == currentSegment)) { PlsrQueuedFrequencyHz = actualDrainFrequencyHz; PlsrFrequencyUpdatePending = 0U; PlsrRamp.active = 0U; PlsrRunStatus = PLSR_STATUS_RUNNING; } PlsrPlatformExitCritical(criticalState); } } PlsrPlatformCheckpointConfig(&PlsrShadowConfig); PlsrMarkPersistenceDirty(PLSR_CONFIG_SAVE_DELAY_MS); return PLSR_MB_OK; } #ifdef PLSR_HOST_TEST void PlsrTestSetPosition(int32_t position, uint8_t positionValid) { uint32_t criticalState = PlsrPlatformEnterCritical(); PlsrPosition = position; PlsrPositionValid = (positionValid != 0U) ? 1U : 0U; PlsrRemainingPulses = 0UL; PlsrPositionCheckpointDirty = 0U; PlsrPlatformExitCritical(criticalState); PlsrPlatformCheckpointPosition(position, PlsrPositionValid, 0U); } void PlsrTestSetDiagnosticCurveCounts(uint32_t sampleCount, uint32_t mismatchCount) { PlsrDiagnostic.curveSampleCount = sampleCount; PlsrDiagnostic.curveMismatchCount = mismatchCount; } void PlsrTestForceProfileQueueEmpty(void) { PlsrProfileQueue.readIndex = PlsrProfileQueue.writeIndex; PlsrProfileQueue.generatorComplete = 0U; PlsrProfileQueue.active = 1U; } uint16_t PlsrTestProfileQueueCount(void) { return PlsrProfileQueueCount(); } uint32_t PlsrTestProfileQueueGeneration(void) { return PlsrProfileQueue.generation; } uint32_t PlsrTestProfileQueueReadIndex(void) { return PlsrProfileQueue.readIndex; } uint32_t PlsrTestProfileQueueWriteIndex(void) { return PlsrProfileQueue.writeIndex; } uint32_t PlsrTestProfileQueueHeadFrequency(void) { uint32_t readIndex = PlsrProfileQueue.readIndex; if (readIndex == PlsrProfileQueue.writeIndex) { return 0UL; } return PlsrProfileQueue .entries[readIndex & PLSR_PROFILE_QUEUE_MASK].requestedFrequencyHz; } uint16_t PlsrTestProfileProducerNextPeriod(void) { return PlsrProfileQueue.producerProfile.nextPeriod; } uint16_t PlsrTestActiveProfileNextPeriod(void) { return PlsrShortProfile.nextPeriod; } uint32_t PlsrTestLastGateEvent(void) { return PlsrHostTestLastGateEvent; } uint32_t PlsrTestLastDiagnosticFinishEvent(void) { return PlsrHostTestLastDiagnosticFinishEvent; } #endif