diff --git a/PLSR/Src/plsr.c b/PLSR/Src/plsr.c index a28d1b4..9234c08 100644 --- a/PLSR/Src/plsr.c +++ b/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;