Modbus-rtu/PLSR/Src/plsr_position.c

#include "plsr_position.h"
#include <limits.h>
#include <stddef.h>

uint8_t PlsrPositionUnitCodeIsValid(uint8_t unitCode)
{
    return ((unitCode == 0U) || (unitCode == 1U) || (unitCode == 3U)
            || (unitCode == 5U) || (unitCode == 7U))
               ? 1U
               : 0U;
}

uint8_t PlsrPositionUsesEquivalent(const PLSR_EQUIVALENT_CONFIG *config)
{
    return ((config != NULL) && (config->unitCode != 0U)) ? 1U : 0U;
}

PLSR_RESULT PlsrPositionValidateEquivalent(
    const PLSR_EQUIVALENT_CONFIG *config)
{
    if ((config == NULL)
        || (PlsrPositionUnitCodeIsValid(config->unitCode) == 0U))
    {
        return PLSR_RESULT_INVALID_S2;
    }
    if ((config->unitCode != 0U)
        && ((config->pulsesPerRevolution == 0UL)
            || (config->movementPerRevolution == 0UL)))
    {
        return PLSR_RESULT_INVALID_S2;
    }
    return PLSR_RESULT_OK;
}

PLSR_RESULT PlsrPositionUnitsToPulses(
    const PLSR_EQUIVALENT_CONFIG *config,
    int32_t units,
    int64_t remainder,
    int64_t *pulses,
    int64_t *newRemainder)
{
    int64_t numerator;
    int64_t pulseValue;
    uint32_t denominator;

    if ((pulses == NULL) || (newRemainder == NULL)
        || (PlsrPositionValidateEquivalent(config) != PLSR_RESULT_OK))
    {
        return PLSR_RESULT_INVALID_ARGUMENT;
    }
    if (config->unitCode == 0U)
    {
        *pulses = units;
        *newRemainder = 0;
        return PLSR_RESULT_OK;
    }
    denominator = config->movementPerRevolution;
    if ((remainder <= -(int64_t)denominator)
        || (remainder >= (int64_t)denominator))
    {
        return PLSR_RESULT_INVALID_ARGUMENT;
    }

    /* int32 * uint32 本身落在 int64 范围内；仅余数相加需要边界检查。 */
    numerator = (int64_t)units
                * (int64_t)config->pulsesPerRevolution;
    if (((remainder > 0) && (numerator > INT64_MAX - remainder))
        || ((remainder < 0) && (numerator < INT64_MIN - remainder)))
    {
        return PLSR_RESULT_POSITION_OVERFLOW;
    }
    numerator += remainder;
    pulseValue = numerator / (int64_t)denominator;
    *newRemainder = numerator % (int64_t)denominator;
    *pulses = pulseValue;
    return PLSR_RESULT_OK;
}

PLSR_RESULT PlsrPositionAbsoluteUnitsToPulses(
    const PLSR_EQUIVALENT_CONFIG *config,
    int32_t units,
    int64_t *pulses)
{
    int64_t ignoredRemainder;

    return PlsrPositionUnitsToPulses(config,
                                     units,
                                     0,
                                     pulses,
                                     &ignoredRemainder);
}

static uint64_t PlsrPositionMagnitude(int64_t value)
{
    return (value < 0) ? (uint64_t)(-(value + 1)) + 1ULL
                       : (uint64_t)value;
}

PLSR_RESULT PlsrPositionPulsesToUnits(
    const PLSR_EQUIVALENT_CONFIG *config,
    int64_t pulses,
    int64_t *units)
{
    uint64_t magnitude;
    uint64_t quotient;
    uint64_t remainder;
    uint64_t whole;
    uint64_t fraction;
    uint64_t scaled;
    uint64_t numerator;
    uint64_t denominator;

    if ((units == NULL)
        || (PlsrPositionValidateEquivalent(config) != PLSR_RESULT_OK))
    {
        return PLSR_RESULT_INVALID_ARGUMENT;
    }
    if (config->unitCode == 0U)
    {
        *units = pulses;
        return PLSR_RESULT_OK;
    }

    magnitude = PlsrPositionMagnitude(pulses);
    numerator = config->movementPerRevolution;
    denominator = config->pulsesPerRevolution;
    quotient = magnitude / denominator;
    remainder = magnitude % denominator;
    if ((quotient != 0ULL) && (numerator > UINT64_MAX / quotient))
    {
        return PLSR_RESULT_POSITION_OVERFLOW;
    }
    whole = quotient * numerator;
    /* remainder 与 numerator 均为 uint32 范围，乘积不会超过 uint64。 */
    fraction = (remainder * numerator) / denominator;
    if (whole > UINT64_MAX - fraction)
    {
        return PLSR_RESULT_POSITION_OVERFLOW;
    }
    scaled = whole + fraction;
    if ((pulses >= 0) && (scaled > (uint64_t)INT64_MAX))
    {
        return PLSR_RESULT_POSITION_OVERFLOW;
    }
    if ((pulses < 0) && (scaled > (UINT64_C(1) << 63U)))
    {
        return PLSR_RESULT_POSITION_OVERFLOW;
    }
    if ((pulses < 0) && (scaled == (UINT64_C(1) << 63U)))
    {
        *units = INT64_MIN;
    }
    else
    {
        *units = (pulses < 0) ? -(int64_t)scaled : (int64_t)scaled;
    }
    return PLSR_RESULT_OK;
}

static PLSR_RESULT PlsrPositionScaleUnsigned(uint32_t value,
                                             uint32_t numerator,
                                             uint32_t denominator,
                                             uint32_t *result)
{
    uint64_t product;
    uint64_t scaled;

    if ((result == NULL) || (denominator == 0UL))
    {
        return PLSR_RESULT_INVALID_ARGUMENT;
    }
    product = (uint64_t)value * (uint64_t)numerator;
    /* 速度采用四舍五入；累计位置仍采用截断并由绝对累计避免漂移。 */
    scaled = product / denominator;
    if ((product % denominator) >= ((uint64_t)denominator + 1ULL) / 2ULL)
    {
        scaled++;
    }
    if (scaled > UINT32_MAX)
    {
        return PLSR_RESULT_POSITION_OVERFLOW;
    }
    *result = (uint32_t)scaled;
    return PLSR_RESULT_OK;
}

PLSR_RESULT PlsrPositionSpeedToPulseFrequency(
    const PLSR_EQUIVALENT_CONFIG *config,
    uint32_t speed,
    uint32_t *frequencyHz)
{
    if ((frequencyHz == NULL)
        || (PlsrPositionValidateEquivalent(config) != PLSR_RESULT_OK))
    {
        return PLSR_RESULT_INVALID_ARGUMENT;
    }
    if (config->unitCode == 0U)
    {
        *frequencyHz = speed;
        return PLSR_RESULT_OK;
    }
    return PlsrPositionScaleUnsigned(speed,
                                     config->pulsesPerRevolution,
                                     config->movementPerRevolution,
                                     frequencyHz);
}

PLSR_RESULT PlsrPositionPulseFrequencyToSpeed(
    const PLSR_EQUIVALENT_CONFIG *config,
    uint32_t frequencyHz,
    uint32_t *speed)
{
    if ((speed == NULL)
        || (PlsrPositionValidateEquivalent(config) != PLSR_RESULT_OK))
    {
        return PLSR_RESULT_INVALID_ARGUMENT;
    }
    if (config->unitCode == 0U)
    {
        *speed = frequencyHz;
        return PLSR_RESULT_OK;
    }
    return PlsrPositionScaleUnsigned(frequencyHz,
                                     config->movementPerRevolution,
                                     config->pulsesPerRevolution,
                                     speed);
}