diff --git a/PLSR/Src/plsr.c b/PLSR/Src/plsr.c index 9234c08..cbb2812 100644 --- a/PLSR/Src/plsr.c +++ b/PLSR/Src/plsr.c @@ -405,6 +405,7 @@ static void PlsrDisarmWaitSignal(void); static uint8_t PlsrWaitSignalDetected(uint8_t inputSelection); static PLSR_PLATFORM_QUEUE_RESULT PlsrTryContinuousHandoff(void); static uint8_t PlsrPrepareShortProfile(PLSR_SHORT_PROFILE *profile, + const PLSR_CONFIG *frequencyConfig, uint8_t segmentNumber, uint32_t startFrequencyHz, uint32_t targetFrequencyHz, @@ -1852,6 +1853,7 @@ static uint64_t PlsrRampPulseEstimate(uint32_t fromHz, } static uint8_t PlsrPrepareShortProfile(PLSR_SHORT_PROFILE *profile, + const PLSR_CONFIG *frequencyConfig, uint8_t segmentNumber, uint32_t startFrequencyHz, uint32_t targetFrequencyHz, @@ -1868,7 +1870,8 @@ static uint8_t PlsrPrepareShortProfile(PLSR_SHORT_PROFILE *profile, uint8_t nextPositive; (void)memset(profile, 0, sizeof(*profile)); - if ((PlsrActiveConfig.outputMode != PLSR_OUTPUT_PULSE_DIR) + if ((frequencyConfig == NULL) + || (PlsrActiveConfig.outputMode != PLSR_OUTPUT_PULSE_DIR) || (pulseCount == 0ULL) || (pulseCount > 0xFFFFFFFFULL) || (segmentNumber == 0U) || (segmentNumber > PlsrActiveConfig.segmentCount)) @@ -1879,8 +1882,8 @@ static uint8_t PlsrPrepareShortProfile(PLSR_SHORT_PROFILE *profile, block.entryHz = startFrequencyHz; block.cruiseHz = 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. */ + discontinuities use the zero endpoint; continuous boundaries override + it according to the configured send mode below. */ block.exitHz = 0UL; block.pulseBudget = (uint32_t)pulseCount; block.referenceSpeedHz = PlsrActiveConfig.defaultSpeedHz; @@ -1913,7 +1916,11 @@ static uint8_t PlsrPrepareShortProfile(PLSR_SHORT_PROFILE *profile, nextPositive = (nextDisplacement > 0) ? 1U : 0U; if (nextPositive == currentPositive) { - block.exitHz = targetFrequencyHz; + block.exitHz = + (PlsrActiveConfig.sendMode == PLSR_SEND_SUBSEQUENT) + ? PlsrResolvedSegmentFrequency( + frequencyConfig, nextSegment - 1U) + : targetFrequencyHz; block.boundary = PLSR_BOUNDARY_NEXT_CARRY; } } @@ -2188,7 +2195,8 @@ static PLSR_PLATFORM_SERVICE_RESULT PlsrReplanPulseDir( if (replacementPulses != 0UL) { if ((tailFrequencyHz == 0UL) - || (PlsrPrepareShortProfile(&replacement, segmentNumber, + || (PlsrPrepareShortProfile(&replacement, &PlsrActiveConfig, + segmentNumber, tailFrequencyHz, targetHz, replacementPulses, PlsrCountPositive, @@ -2614,7 +2622,7 @@ static void PlsrServiceTimedStartPreparation(void) startFrequencyHz = PlsrPulseDirStartFrequency( targetFrequencyHz, PLSR_SEGMENT_ENTRY_INTERNAL_RESTART, directionChanged, 0UL); - if (PlsrPrepareShortProfile(&profile, nextSegment, + if (PlsrPrepareShortProfile(&profile, &PlsrActiveConfig, nextSegment, startFrequencyHz, targetFrequencyHz, magnitude, positive, 0U) == 0U) { @@ -2819,7 +2827,8 @@ static PLSR_PLATFORM_SERVICE_RESULT PlsrPrepareDelayedCurrentOutput( criticalState = PlsrPlatformEnterCritical(); countPositive = PlsrCountPositive; PlsrPlatformExitCritical(criticalState); - if (PlsrPrepareShortProfile(&profile, PlsrCurrentSegment, + if (PlsrPrepareShortProfile(&profile, &PlsrActiveConfig, + PlsrCurrentSegment, startFrequencyHz, targetFrequencyHz, remainingPulses, countPositive, 0U) == 0U) @@ -3502,7 +3511,7 @@ static uint8_t PlsrBeginSegmentOutput(uint32_t startFrequencyHz) profileSegment = PlsrCurrentSegment; countPositive = PlsrCountPositive; remainingPulses = PlsrRemainingSnapshot(); - if (PlsrPrepareShortProfile(&PlsrShortProfile, + if (PlsrPrepareShortProfile(&PlsrShortProfile, &PlsrActiveConfig, profileSegment, startFrequencyHz, targetFrequencyHz, remainingPulses, @@ -4089,7 +4098,7 @@ static uint8_t PlsrBuildHandoffPlan(uint8_t sourceSegment, } nextFrequencyHz = PlsrResolvedSegmentFrequency(frequencyConfig, nextSegment - 1U); - if (PlsrPrepareShortProfile(&plan->profile, nextSegment, + if (PlsrPrepareShortProfile(&plan->profile, frequencyConfig, nextSegment, carryFrequencyHz, nextFrequencyHz, plan->magnitude, positive, 0U) == 0U) {