优化了方向延时

PLSR/Src/plsr.c

@@ -47,6 +47,7 @@
 
 #define PLSR_CONFIG_SAVE_DELAY_MS      (1000U)
 #define PLSR_POSITION_CHECKPOINT_MS    (10U)
+#define PLSR_DIRECTION_DELAY_DEFAULT_MS (10U)
 #define PLSR_PROFILE_QUEUE_CAPACITY    (1024U)
 #define PLSR_PROFILE_QUEUE_MASK        (PLSR_PROFILE_QUEUE_CAPACITY - 1U)
 #if ((PLSR_PROFILE_QUEUE_CAPACITY == 0U) \
@@ -960,7 +961,7 @@ static void PlsrSetDefaults(PLSR_CONFIG *config)
     config->waitInput = 0U;
     config->extInput = 0U;
     config->sendMode = PLSR_SEND_COMPLETE;
-    config->directionDelayMs = 10U;
+    config->directionDelayMs = PLSR_DIRECTION_DELAY_DEFAULT_MS;
     config->directionNegativeLogic = 0U;
     config->curveMode = 0U;
     config->positionMode = PLSR_POSITION_RELATIVE;
@@ -1368,6 +1369,13 @@ static uint32_t PlsrEffectiveStartFrequency(uint32_t segmentFrequencyHz,
     return PlsrActiveConfig.startSpeedHz;
 }
 
+static uint16_t PlsrEffectiveDirectionDelayMs(void)
+{
+    return (PlsrActiveConfig.directionDelayMs != 0U)
+               ? PlsrActiveConfig.directionDelayMs
+               : PLSR_DIRECTION_DELAY_DEFAULT_MS;
+}
+
 static uint32_t PlsrEffectiveStopFrequency(uint32_t segmentFrequencyHz)
 {
     return (PlsrActiveConfig.decelerationTimeMs == 0U)
@@ -2448,6 +2456,7 @@ static void PlsrServiceTimedStartPreparation(void)
     uint64_t magnitude;
     int32_t position;
     uint16_t budget;
+    uint16_t sourceWaitType;
     uint8_t sourceSegment;
     uint8_t nextSegment;
     uint8_t positive;
@@ -2455,6 +2464,8 @@ static void PlsrServiceTimedStartPreparation(void)
     uint8_t directionChanged;
     uint8_t queueBank;
     uint8_t nextActTimedCut = 0U;
+    uint8_t sourceWaiting;
+    uint8_t coldStartBoundary;
 
     if ((PlsrActiveConfig.outputMode != PLSR_OUTPUT_PULSE_DIR)
         || (PlsrStopRequested != 0U)
@@ -2466,20 +2477,16 @@ static void PlsrServiceTimedStartPreparation(void)
     criticalState = PlsrPlatformEnterCritical();
     sourceEpoch = PlsrSegmentEpoch;
     sourceSegment = PlsrCurrentSegment;
+    sourceWaiting = (PlsrRunStatus == PLSR_STATUS_WAITING) ? 1U : 0U;
     if ((sourceSegment == 0U)
-        || (sourceSegment > PlsrActiveConfig.segmentCount)
-        || ((PlsrActiveConfig.segments[sourceSegment - 1U].waitType
-             != PLSR_WAIT_TIME)
-            && (PlsrActiveConfig.segments[sourceSegment - 1U].waitType
-                != PLSR_WAIT_SIGNAL)
-            && ((PlsrActiveConfig.segments[sourceSegment - 1U].waitType
-                 != PLSR_ACT_TIME)
-                || (PlsrRunStatus != PLSR_STATUS_WAITING))))
+        || (sourceSegment > PlsrActiveConfig.segmentCount))
     {
         PlsrInvalidateTimedStartLocked();
         PlsrPlatformExitCritical(criticalState);
         return;
     }
+    sourceWaitType =
+        PlsrActiveConfig.segments[sourceSegment - 1U].waitType;
     if ((PlsrTimedStart.building != 0U)
         || (PlsrTimedStart.valid != 0U))
     {
@@ -2541,6 +2548,17 @@ static void PlsrServiceTimedStartPreparation(void)
                         || (PlsrLastDirectionOutput
                             != (uint8_t)PlsrActiveConfig.directionOutput)
                         || (PlsrLastDirectionLevel != directionLevel)) ? 1U : 0U;
+    coldStartBoundary = ((sourceWaitType == PLSR_WAIT_TIME)
+                         || (sourceWaitType == PLSR_WAIT_SIGNAL)
+                         || ((sourceWaitType == PLSR_ACT_TIME)
+                             && (sourceWaiting != 0U))) ? 1U : 0U;
+    if ((coldStartBoundary == 0U) && (directionChanged == 0U))
+    {
+        criticalState = PlsrPlatformEnterCritical();
+        PlsrInvalidateTimedStartLocked();
+        PlsrPlatformExitCritical(criticalState);
+        return;
+    }
     targetFrequencyHz =
         PlsrActiveConfig.segments[nextSegment - 1U].frequencyHz;
     startFrequencyHz = PlsrEffectiveStartFrequency(
@@ -2687,6 +2705,166 @@ fill_timed_start:
     }
 }
 
+/* Boundary-triggered reversals are normally prepared incrementally while the
+   source segment is running.  If a very short segment reaches its boundary
+   first, finish the bounded remainder before changing the direction pin so
+   planner time is never included in the configured direction setup time. */
+static void PlsrCompleteTimedStartPreparation(void)
+{
+    uint16_t pass;
+    uint32_t criticalState;
+    uint8_t building;
+    uint8_t valid;
+
+    for (pass = 0U;
+         pass < (uint16_t)(PLSR_TIMED_START_READY_ITEMS
+                           / PLSR_TIMED_START_BUILD_BUDGET + 2U);
+         pass++)
+    {
+        criticalState = PlsrPlatformEnterCritical();
+        building = PlsrTimedStart.building;
+        valid = PlsrTimedStart.valid;
+        PlsrPlatformExitCritical(criticalState);
+        if (valid != 0U)
+        {
+            return;
+        }
+        PlsrServiceTimedStartPreparation();
+        criticalState = PlsrPlatformEnterCritical();
+        if ((PlsrTimedStart.building == 0U)
+            && (PlsrTimedStart.valid == 0U)
+            && (building == 0U))
+        {
+            PlsrPlatformExitCritical(criticalState);
+            return;
+        }
+        PlsrPlatformExitCritical(criticalState);
+    }
+}
+
+/* Prepare the current segment before PlsrPlatformPrepare changes the
+   direction pin.  The delay interval then contains no bulk startup-profile
+   generation or queue fill; expiry publishes this inactive bank and starts
+   the timer. */
+static PLSR_PLATFORM_SERVICE_RESULT PlsrPrepareDelayedCurrentOutput(
+    uint32_t startFrequencyHz,
+    uint8_t directionLevel,
+    uint8_t directionChanged)
+{
+    PLSR_SHORT_PROFILE profile;
+    PLSR_PROFILE_ENTRY firstRun;
+    PLSR_PROFILE_ENTRY entry;
+    PLSR_PROFILE_QUEUE *queue;
+    uint64_t remainingPulses = PlsrRemainingSnapshot();
+    uint32_t targetFrequencyHz =
+        PlsrActiveConfig.segments[PlsrCurrentSegment - 1U].frequencyHz;
+    uint32_t criticalState;
+    uint32_t queueGeneration;
+    uint16_t generatedItems = 0U;
+    uint8_t countPositive;
+    uint8_t queueBank;
+    uint8_t timedCutPlanned = 0U;
+
+    criticalState = PlsrPlatformEnterCritical();
+    countPositive = PlsrCountPositive;
+    PlsrPlatformExitCritical(criticalState);
+    if (PlsrPrepareShortProfile(&profile, PlsrCurrentSegment,
+                                startFrequencyHz, targetFrequencyHz,
+                                remainingPulses,
+                                countPositive) == 0U)
+    {
+        return PLSR_PLATFORM_SERVICE_FAILED;
+    }
+    if ((PlsrActiveConfig.segments[PlsrCurrentSegment - 1U].waitType
+         == PLSR_ACT_TIME)
+        && (PlsrLimitProfileToActTime(
+                &profile,
+                PlsrActiveConfig.segments[PlsrCurrentSegment - 1U].actTimeMs,
+                &timedCutPlanned) == 0U))
+    {
+        return PLSR_PLATFORM_SERVICE_FAILED;
+    }
+    if (profile.pulseCount == 0UL)
+    {
+        return PLSR_PLATFORM_SERVICE_DEFERRED;
+    }
+    if (PlsrShortProfileTakeRun(&profile, &firstRun) == 0U)
+    {
+        return PLSR_PLATFORM_SERVICE_FAILED;
+    }
+
+    criticalState = PlsrPlatformEnterCritical();
+    queueBank = (uint8_t)(PlsrProfileQueueBank ^ 1U);
+    PlsrProfileQueueResetBankLocked(queueBank);
+    queue = &PlsrProfileQueues[queueBank];
+    queueGeneration = queue->generation;
+    queue->producerEpoch = PlsrSegmentEpoch;
+    queue->producerSegment = PlsrCurrentSegment;
+    queue->preparedHandoffBank = PlsrPreparedHandoffBank;
+    queue->repeatRemaining = 0UL;
+    queue->underrunDebtPulses = 0UL;
+    queue->haveLastSetting = 0U;
+    PlsrCopyShortProfile(&queue->producerProfile, &profile);
+    queue->active = 1U;
+    queue->generatorComplete = (profile.active == 0U) ? 1U : 0U;
+    PlsrPlatformExitCritical(criticalState);
+
+    while ((generatedItems < PLSR_TIMED_START_READY_ITEMS)
+           && (profile.active != 0U))
+    {
+        if (PlsrShortProfileTakeRun(&profile, &entry) == 0U)
+        {
+            return PLSR_PLATFORM_SERVICE_FAILED;
+        }
+        criticalState = PlsrPlatformEnterCritical();
+        queue = &PlsrProfileQueues[queueBank];
+        if ((queue->generation != queueGeneration)
+            || (queueBank == PlsrProfileQueueBank)
+            || (PlsrProfileQueueAppendLocked(queue, &entry) == 0U))
+        {
+            PlsrPlatformExitCritical(criticalState);
+            return PLSR_PLATFORM_SERVICE_FAILED;
+        }
+        PlsrCopyShortProfile(&queue->producerProfile, &profile);
+        if ((profile.active == 0U)
+            || (profile.nextPeriod >= profile.pulseCount))
+        {
+            queue->generatorComplete = 1U;
+        }
+        PlsrPlatformExitCritical(criticalState);
+        generatedItems++;
+    }
+
+    criticalState = PlsrPlatformEnterCritical();
+    queue = &PlsrProfileQueues[queueBank];
+    if ((queue->generation != queueGeneration)
+        || (queueBank == PlsrProfileQueueBank))
+    {
+        PlsrPlatformExitCritical(criticalState);
+        return PLSR_PLATFORM_SERVICE_FAILED;
+    }
+    PlsrTimedStart.firstRun = firstRun;
+    PlsrTimedStart.magnitude = profile.pulseCount;
+    PlsrTimedStart.sourceEpoch = PlsrSegmentEpoch - 1UL;
+    PlsrTimedStart.queueGeneration = queueGeneration;
+    PlsrTimedStart.generatedItems = generatedItems;
+    PlsrTimedStart.sourceSegment = PlsrCurrentSegment;
+    PlsrTimedStart.nextSegment = PlsrCurrentSegment;
+    PlsrTimedStart.positive = countPositive;
+    PlsrTimedStart.directionLevel = directionLevel;
+    PlsrTimedStart.directionChanged = directionChanged;
+    PlsrTimedStart.queueBank = queueBank;
+    PlsrTimedStart.nextActTimedCut = timedCutPlanned;
+    PlsrTimedStart.building = 0U;
+    PlsrTimedStart.valid = 1U;
+    PlsrTimedStart.pendingActivation = 1U;
+    PlsrRemainingPulses = profile.pulseCount;
+    PlsrActTimedCutPlanned = timedCutPlanned;
+    PlsrPlatformExitCritical(criticalState);
+    PlsrProfileRecordPlannerStatus(&profile);
+    return PLSR_PLATFORM_SERVICE_READY;
+}
+
 #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
     && !defined(PLSR_HOST_TEST)
 static uint32_t PlsrProfileTimingNow(void)
@@ -3235,11 +3413,10 @@ static PLSR_PLATFORM_SERVICE_RESULT PlsrTryStartPreparedTimedSegment(
     PlsrLastDirectionOutput = (uint8_t)PlsrActiveConfig.directionOutput;
     PlsrLastDirectionLevel = directionLevel;
 
-    if ((directionChanged != 0U)
-        && (PlsrActiveConfig.directionDelayMs != 0U))
+    if (directionChanged != 0U)
     {
         PlsrDirectionDelayActive = 1U;
-        PlsrDirectionDelayRemainingMs = PlsrActiveConfig.directionDelayMs;
+        PlsrDirectionDelayRemainingMs = PlsrEffectiveDirectionDelayMs();
         PlsrRunStatus = PLSR_STATUS_ACCELERATING;
         return PLSR_PLATFORM_SERVICE_READY;
     }
@@ -3424,6 +3601,8 @@ static uint8_t PlsrStartSegment(uint8_t segmentNumber,
                                 uint8_t allowCarry,
                                 uint32_t carryFrequencyHz)
 {
+    PLSR_PLATFORM_SERVICE_RESULT delayedPrepareResult =
+        PLSR_PLATFORM_SERVICE_DEFERRED;
     uint32_t criticalState;
     int32_t position;
     int64_t displacement;
@@ -3519,12 +3698,33 @@ static uint8_t PlsrStartSegment(uint8_t segmentNumber,
         return 1U;
     }
 
+    if ((PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR)
+        && (directionChanged != 0U))
+    {
+        delayedPrepareResult = PlsrPrepareDelayedCurrentOutput(
+            startFrequencyHz, directionLevel, directionChanged);
+        if (delayedPrepareResult == PLSR_PLATFORM_SERVICE_FAILED)
+        {
+            return 0U;
+        }
+        if (delayedPrepareResult == PLSR_PLATFORM_SERVICE_READY)
+        {
+            magnitude = PlsrRemainingSnapshot();
+        }
+    }
+
     if (PlsrPlatformPrepare((uint8_t)PlsrActiveConfig.pulseOutput,
                             (uint8_t)PlsrActiveConfig.directionOutput,
                             directionLevel,
                             (uint8_t)PlsrActiveConfig.outputMode,
                             positive) == 0U)
     {
+        if (delayedPrepareResult == PLSR_PLATFORM_SERVICE_READY)
+        {
+            criticalState = PlsrPlatformEnterCritical();
+            PlsrInvalidateTimedStartLocked();
+            PlsrPlatformExitCritical(criticalState);
+        }
         return 0U;
     }
     PlsrDiagnosticBeginSegment(segmentNumber, magnitude, positive);
@@ -3533,11 +3733,10 @@ static uint8_t PlsrStartSegment(uint8_t segmentNumber,
     PlsrLastDirectionLevel = directionLevel;
 
     if ((PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR)
-        && (directionChanged != 0U)
-        && (PlsrActiveConfig.directionDelayMs != 0U))
+        && (directionChanged != 0U))
     {
         PlsrDirectionDelayActive = 1U;
-        PlsrDirectionDelayRemainingMs = PlsrActiveConfig.directionDelayMs;
+        PlsrDirectionDelayRemainingMs = PlsrEffectiveDirectionDelayMs();
         PlsrRunStatus = PLSR_STATUS_ACCELERATING;
         return 1U;
     }
@@ -3723,7 +3922,13 @@ static void PlsrTransitionToNext(uint8_t allowCarry)
         return;
     }
 
-    if (allowCarry == 0U)
+    if (PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR)
+    {
+        PlsrCompleteTimedStartPreparation();
+    }
+
+    if ((allowCarry == 0U)
+        || (PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR))
     {
         preparedResult = PlsrTryStartPreparedTimedSegment(nextSegment);
         if (preparedResult == PLSR_PLATFORM_SERVICE_READY)