修正了起始速度和终止速度语义和代码实现

PLSR/Src/plsr.c

@@ -211,6 +211,13 @@ typedef enum
     PLSR_COMMAND_MAILBOX_EXECUTING
 } PLSR_COMMAND_MAILBOX_STATE;
 
+typedef enum
+{
+    PLSR_SEGMENT_ENTRY_COMMAND_START = 0,
+    PLSR_SEGMENT_ENTRY_INTERNAL_RESTART,
+    PLSR_SEGMENT_ENTRY_CARRY
+} PLSR_SEGMENT_ENTRY;
+
 typedef struct
 {
     PLSR_CONFIG startConfig;
@@ -281,6 +288,8 @@ static volatile uint32_t PlsrBoundaryFrequencyHz;
 static volatile uint32_t PlsrFrequencyUpdateTargetHz;
 static volatile uint32_t PlsrDeferredFrequencyHz;
 static volatile uint32_t PlsrSegmentEpoch;
+static PLSR_SEGMENT_ENTRY PlsrDelayedEntryType;
+static uint8_t PlsrPulseDirMotionStarted;
 
 static volatile PLSR_STATUS PlsrRunStatus = PLSR_STATUS_UNINITIALIZED;
 static PLSR_ERROR PlsrError = PLSR_ERROR_NONE;
@@ -368,12 +377,17 @@ static void PlsrExecuteStart(void);
 static uint8_t PlsrExecuteStop(void);
 static void PlsrExecuteClear(void);
 static uint8_t PlsrStartSegment(uint8_t segmentNumber,
-                                uint8_t allowCarry,
+                                PLSR_SEGMENT_ENTRY entryType,
                                 uint32_t carryFrequencyHz);
 static uint32_t PlsrEffectiveStartFrequency(uint32_t segmentFrequencyHz,
                                             uint8_t allowCarry,
                                             uint8_t directionChanged,
                                             uint32_t carryFrequencyHz);
+static uint32_t PlsrPulseDirStartFrequency(
+    uint32_t segmentFrequencyHz,
+    PLSR_SEGMENT_ENTRY entryType,
+    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);
@@ -395,7 +409,8 @@ static uint8_t PlsrPrepareShortProfile(PLSR_SHORT_PROFILE *profile,
                                        uint32_t startFrequencyHz,
                                        uint32_t targetFrequencyHz,
                                        uint64_t pulseCount,
-                                       uint8_t currentPositive);
+                                       uint8_t currentPositive,
+                                       uint8_t forceTerminalExit);
 static uint8_t PlsrShortProfileTakeRun(PLSR_SHORT_PROFILE *profile,
                                        PLSR_PROFILE_ENTRY *entry);
 static uint8_t PlsrShortProfileTakeRunLimited(
@@ -410,7 +425,8 @@ static void PlsrProfileRecordPlannerStatus(
     const PLSR_SHORT_PROFILE *profile);
 static PLSR_PLATFORM_SERVICE_RESULT PlsrReplanPulseDir(
     uint32_t targetHz,
-    uint32_t totalPulses);
+    uint32_t totalPulses,
+    uint8_t forceTerminalExit);
 static void PlsrProfileQueueReset(void);
 static void PlsrProfileQueueResetBankLocked(uint8_t bank);
 static void PlsrInvalidateTimedStartLocked(void);
@@ -1369,6 +1385,29 @@ static uint32_t PlsrEffectiveStartFrequency(uint32_t segmentFrequencyHz,
     return PlsrActiveConfig.startSpeedHz;
 }
 
+static uint32_t PlsrPulseDirStartFrequency(
+    uint32_t segmentFrequencyHz,
+    PLSR_SEGMENT_ENTRY entryType,
+    uint8_t directionChanged,
+    uint32_t carryFrequencyHz)
+{
+    if ((entryType == PLSR_SEGMENT_ENTRY_CARRY)
+        && (directionChanged == 0U) && (carryFrequencyHz != 0UL))
+    {
+        return carryFrequencyHz;
+    }
+    if (PlsrActiveConfig.accelerationTimeMs == 0U)
+    {
+        return segmentFrequencyHz;
+    }
+    if ((entryType == PLSR_SEGMENT_ENTRY_COMMAND_START)
+        || (PlsrPulseDirMotionStarted == 0U))
+    {
+        return PlsrActiveConfig.startSpeedHz;
+    }
+    return 0UL;
+}
+
 static uint16_t PlsrEffectiveDirectionDelayMs(void)
 {
     return (PlsrActiveConfig.directionDelayMs != 0U)
@@ -1817,7 +1856,8 @@ static uint8_t PlsrPrepareShortProfile(PLSR_SHORT_PROFILE *profile,
                                        uint32_t startFrequencyHz,
                                        uint32_t targetFrequencyHz,
                                        uint64_t pulseCount,
-                                       uint8_t currentPositive)
+                                       uint8_t currentPositive,
+                                       uint8_t forceTerminalExit)
 {
     PLSR_MOTION_BLOCK block;
     PLSR_PLANNER_STATUS status;
@@ -1838,7 +1878,10 @@ static uint8_t PlsrPrepareShortProfile(PLSR_SHORT_PROFILE *profile,
 
     block.entryHz = startFrequencyHz;
     block.cruiseHz = targetFrequencyHz;
-    block.exitHz = PlsrEffectiveStopFrequency(targetFrequencyHz);
+    /* A configured stop speed belongs to the command endpoint.  Internal
+       PUL/DIR boundaries stop at the planner's zero endpoint unless they can
+       carry continuously into the next motion segment. */
+    block.exitHz = 0UL;
     block.pulseBudget = (uint32_t)pulseCount;
     block.referenceSpeedHz = PlsrActiveConfig.defaultSpeedHz;
     block.accelerationTimeMs = PlsrActiveConfig.accelerationTimeMs;
@@ -1848,19 +1891,24 @@ static uint8_t PlsrPrepareShortProfile(PLSR_SHORT_PROFILE *profile,
     block.boundary = PLSR_BOUNDARY_STOP;
 
     segment = &PlsrActiveConfig.segments[segmentNumber - 1U];
-    if (segment->waitType == PLSR_EXT_OR_COMPLETE)
+    boundaryPosition = (PlsrActiveConfig.positionMode
+                        == PLSR_POSITION_ABSOLUTE)
+                           ? segment->pulses : 0L;
+    if (PlsrResolveNextMotionSegment(
+            segmentNumber, boundaryPosition,
+            &nextSegment, &nextDisplacement) == 0U)
     {
-        boundaryPosition = (PlsrActiveConfig.positionMode
-                            == PLSR_POSITION_ABSOLUTE)
-                               ? segment->pulses : 0L;
-        if (PlsrResolveNextMotionSegment(
-                segmentNumber, boundaryPosition,
-                &nextSegment, &nextDisplacement) == 0U)
-        {
-            nextSegment = 0U;
-        }
+        nextSegment = 0U;
+    }
+    if (forceTerminalExit != 0U)
+    {
+        block.exitHz = targetFrequencyHz;
+    }
+    else if (nextSegment == 0U)
+    {
+        block.exitHz = PlsrEffectiveStopFrequency(targetFrequencyHz);
     }
-    if (nextSegment != 0U)
+    else if (segment->waitType == PLSR_EXT_OR_COMPLETE)
     {
         nextPositive = (nextDisplacement > 0) ? 1U : 0U;
         if (nextPositive == currentPositive)
@@ -1992,7 +2040,8 @@ static uint8_t PlsrLimitProfileToActTime(
    the construction interval; the final bank flip is the only critical part. */
 static PLSR_PLATFORM_SERVICE_RESULT PlsrReplanPulseDir(
     uint32_t targetHz,
-    uint32_t totalPulses)
+    uint32_t totalPulses,
+    uint8_t forceTerminalExit)
 {
     PLSR_PROFILE_QUEUE *source;
     PLSR_PROFILE_QUEUE *destination;
@@ -2142,7 +2191,8 @@ static PLSR_PLATFORM_SERVICE_RESULT PlsrReplanPulseDir(
             || (PlsrPrepareShortProfile(&replacement, segmentNumber,
                                         tailFrequencyHz, targetHz,
                                         replacementPulses,
-                                        PlsrCountPositive) == 0U))
+                                        PlsrCountPositive,
+                                        forceTerminalExit) == 0U))
         {
             return PLSR_PLATFORM_SERVICE_FAILED;
         }
@@ -2561,11 +2611,12 @@ static void PlsrServiceTimedStartPreparation(void)
     }
     targetFrequencyHz =
         PlsrActiveConfig.segments[nextSegment - 1U].frequencyHz;
-    startFrequencyHz = PlsrEffectiveStartFrequency(
-        targetFrequencyHz, 0U, directionChanged, 0UL);
+    startFrequencyHz = PlsrPulseDirStartFrequency(
+        targetFrequencyHz, PLSR_SEGMENT_ENTRY_INTERNAL_RESTART,
+        directionChanged, 0UL);
     if (PlsrPrepareShortProfile(&profile, nextSegment,
                                 startFrequencyHz, targetFrequencyHz,
-                                magnitude, positive) == 0U)
+                                magnitude, positive, 0U) == 0U)
     {
         return;
     }
@@ -2771,7 +2822,7 @@ static PLSR_PLATFORM_SERVICE_RESULT PlsrPrepareDelayedCurrentOutput(
     if (PlsrPrepareShortProfile(&profile, PlsrCurrentSegment,
                                 startFrequencyHz, targetFrequencyHz,
                                 remainingPulses,
-                                countPositive) == 0U)
+                                countPositive, 0U) == 0U)
     {
         return PLSR_PLATFORM_SERVICE_FAILED;
     }
@@ -3333,6 +3384,7 @@ static uint8_t PlsrStartPreparedTimedOutput(void)
         PlsrShortProfile.active = 0U;
         return 0U;
     }
+    PlsrPulseDirMotionStarted = 1U;
     PlsrCurrentFrequencyHz = actualFrequencyHz;
     PlsrDiagnosticRecordFrequency(firstRun.requestedFrequencyHz,
                                   firstRun.setting.actualFrequencyHz,
@@ -3415,6 +3467,7 @@ static PLSR_PLATFORM_SERVICE_RESULT PlsrTryStartPreparedTimedSegment(
 
     if (directionChanged != 0U)
     {
+        PlsrDelayedEntryType = PLSR_SEGMENT_ENTRY_INTERNAL_RESTART;
         PlsrDirectionDelayActive = 1U;
         PlsrDirectionDelayRemainingMs = PlsrEffectiveDirectionDelayMs();
         PlsrRunStatus = PLSR_STATUS_ACCELERATING;
@@ -3453,7 +3506,7 @@ static uint8_t PlsrBeginSegmentOutput(uint32_t startFrequencyHz)
                                 profileSegment, startFrequencyHz,
                                 targetFrequencyHz,
                                 remainingPulses,
-                                countPositive) != 0U)
+                                countPositive, 0U) != 0U)
     {
         PlsrRamp.active = 0U;
         if ((PlsrActiveConfig.segments[profileSegment - 1U].waitType
@@ -3558,6 +3611,7 @@ static uint8_t PlsrBeginSegmentOutput(uint32_t startFrequencyHz)
             PlsrShortProfile.active = 0U;
             return 0U;
         }
+        PlsrPulseDirMotionStarted = 1U;
         PlsrCurrentFrequencyHz = actualFrequencyHz;
         PlsrDiagnosticRecordFrequency(firstRun.requestedFrequencyHz,
                                       firstRun.setting.actualFrequencyHz,
@@ -3598,7 +3652,7 @@ static uint8_t PlsrBeginSegmentOutput(uint32_t startFrequencyHz)
 }
 
 static uint8_t PlsrStartSegment(uint8_t segmentNumber,
-                                uint8_t allowCarry,
+                                PLSR_SEGMENT_ENTRY entryType,
                                 uint32_t carryFrequencyHz)
 {
     PLSR_PLATFORM_SERVICE_RESULT delayedPrepareResult =
@@ -3612,6 +3666,7 @@ static uint8_t PlsrStartSegment(uint8_t segmentNumber,
     uint8_t directionChanged;
     uint32_t startFrequencyHz;
     PLSR_PROFILE_QUEUE *queue;
+    uint8_t allowCarry = (entryType == PLSR_SEGMENT_ENTRY_CARRY) ? 1U : 0U;
 
     if ((segmentNumber == 0U)
         || (segmentNumber > PlsrActiveConfig.segmentCount))
@@ -3675,9 +3730,18 @@ static uint8_t PlsrStartSegment(uint8_t segmentNumber,
                         || (PlsrLastDirectionOutput
                             != (uint8_t)PlsrActiveConfig.directionOutput)
                         || (PlsrLastDirectionLevel != directionLevel)) ? 1U : 0U;
-    startFrequencyHz = PlsrEffectiveStartFrequency(
-        PlsrActiveConfig.segments[segmentNumber - 1U].frequencyHz,
-        allowCarry, directionChanged, carryFrequencyHz);
+    if (PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR)
+    {
+        startFrequencyHz = PlsrPulseDirStartFrequency(
+            PlsrActiveConfig.segments[segmentNumber - 1U].frequencyHz,
+            entryType, directionChanged, carryFrequencyHz);
+    }
+    else
+    {
+        startFrequencyHz = PlsrEffectiveStartFrequency(
+            PlsrActiveConfig.segments[segmentNumber - 1U].frequencyHz,
+            allowCarry, directionChanged, carryFrequencyHz);
+    }
 
     criticalState = PlsrPlatformEnterCritical();
     PlsrRemainingPulses = magnitude;
@@ -3735,6 +3799,7 @@ static uint8_t PlsrStartSegment(uint8_t segmentNumber,
     if ((PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR)
         && (directionChanged != 0U))
     {
+        PlsrDelayedEntryType = entryType;
         PlsrDirectionDelayActive = 1U;
         PlsrDirectionDelayRemainingMs = PlsrEffectiveDirectionDelayMs();
         PlsrRunStatus = PLSR_STATUS_ACCELERATING;
@@ -3942,7 +4007,11 @@ static void PlsrTransitionToNext(uint8_t allowCarry)
         }
     }
 
-    if (PlsrStartSegment(nextSegment, allowCarry, carryFrequencyHz) == 0U)
+    if (PlsrStartSegment(
+            nextSegment,
+            (allowCarry != 0U) ? PLSR_SEGMENT_ENTRY_CARRY
+                               : PLSR_SEGMENT_ENTRY_INTERNAL_RESTART,
+            carryFrequencyHz) == 0U)
     {
         PlsrEnterError(PLSR_ERROR_INVALID_RESOURCE);
     }
@@ -4022,7 +4091,7 @@ static uint8_t PlsrBuildHandoffPlan(uint8_t sourceSegment,
         PlsrResolvedSegmentFrequency(frequencyConfig, nextSegment - 1U);
     if (PlsrPrepareShortProfile(&plan->profile, nextSegment,
                                 carryFrequencyHz, nextFrequencyHz,
-                                plan->magnitude, positive) == 0U)
+                                plan->magnitude, positive, 0U) == 0U)
     {
         return 0U;
     }
@@ -5267,6 +5336,8 @@ uint8_t PlsrInit(void)
     PlsrFrequencyUpdateTargetHz = 0UL;
     PlsrDeferredFrequencyHz = 0UL;
     PlsrSegmentEpoch = 0UL;
+    PlsrDelayedEntryType = PLSR_SEGMENT_ENTRY_COMMAND_START;
+    PlsrPulseDirMotionStarted = 0U;
     PlsrCurrentSegment = 0U;
     PlsrDirectionDelayActive = 0U;
     PlsrDirectionDelayRemainingMs = 0U;
@@ -5380,8 +5451,11 @@ static void PlsrExecuteStart(void)
     PlsrShortProfile.active = 0U;
     PlsrError = PLSR_ERROR_NONE;
     PlsrLastDirectionValid = 0U;
+    PlsrDelayedEntryType = PLSR_SEGMENT_ENTRY_COMMAND_START;
+    PlsrPulseDirMotionStarted = 0U;
     PlsrCheckpointPosition(1U);
-    if (PlsrStartSegment((uint8_t)PlsrActiveConfig.startSegment, 0U, 0UL)
+    if (PlsrStartSegment((uint8_t)PlsrActiveConfig.startSegment,
+                         PLSR_SEGMENT_ENTRY_COMMAND_START, 0UL)
         == 0U)
     {
         PlsrEnterError((PlsrTimerErrorPending != 0U)
@@ -5452,7 +5526,7 @@ static uint8_t PlsrExecuteStop(void)
         PlsrRemainingPulses = drainPulses;
         PlsrPlatformExitCritical(criticalState);
         if (PlsrReplanPulseDir(stopTargetHz,
-                              (uint32_t)drainPulses)
+                              (uint32_t)drainPulses, 1U)
             != PLSR_PLATFORM_SERVICE_READY)
         {
             PlsrEnterError(PLSR_ERROR_TIMER);
@@ -5767,10 +5841,11 @@ void PlsrPoll1ms(void)
                     PlsrEnterError(PLSR_ERROR_TIMER);
                 }
             }
-            else if (PlsrBeginSegmentOutput(PlsrEffectiveStartFrequency(
-                         PlsrActiveConfig.segments[PlsrCurrentSegment - 1U]
-                             .frequencyHz,
-                         0U, 1U, 0UL)) == 0U)
+            else if (PlsrBeginSegmentOutput(
+                         PlsrPulseDirStartFrequency(
+                             PlsrActiveConfig.segments[
+                                 PlsrCurrentSegment - 1U].frequencyHz,
+                             PlsrDelayedEntryType, 1U, 0UL)) == 0U)
             {
                 PlsrEnterError(PLSR_ERROR_TIMER);
             }
@@ -5863,7 +5938,7 @@ void PlsrPoll1ms(void)
     if (pulseDirReplanRequested != 0U)
     {
         pulseDirReplanResult = PlsrReplanPulseDir(
-            newTargetHz, pulseDirReplanPulses);
+            newTargetHz, pulseDirReplanPulses, 0U);
         if (pulseDirReplanResult == PLSR_PLATFORM_SERVICE_FAILED)
         {
             pulseDirReplanFailed = 1U;