diff --git a/PLSR/Src/plsr.c b/PLSR/Src/plsr.c index 4978efc..a28d1b4 100644 --- a/PLSR/Src/plsr.c +++ b/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)