Modbus-rtu/PLSR/Test/test_plsr_profile.c

#include "plsr_profile.h"
#include <stdio.h>
#include <string.h>

static int TestFailures;
static int TestChecks;

#define CHECK(condition)                                                     \
    do                                                                       \
    {                                                                        \
        TestChecks++;                                                        \
        if (!(condition))                                                    \
        {                                                                    \
            TestFailures++;                                                  \
            (void)printf("FAIL line %d: %s\n", __LINE__, #condition);       \
        }                                                                    \
    } while (0)

/* 跑完一段曲线，返回实际脉冲数与峰值频率。 */
static void RunProfile(const PLSR_PROFILE_REQUEST *request,
                       int64_t pulses,
                       uint32_t refreshHz,
                       uint32_t retargetHz,
                       uint32_t retargetAfterSteps,
                       int64_t *actualPulses,
                       uint32_t *peakHz,
                       int64_t *steps)
{
    PLSR_PROFILE_STATE state;
    uint32_t frequency = 0U;
    uint8_t completed = 0U;
    int64_t stepCount = 0;
    uint32_t peak = 0U;

    CHECK(PlsrProfileStart(&state, request, pulses, refreshHz)
          == PLSR_RESULT_OK);
    while ((completed == 0U) && (stepCount < 5000000))
    {
        (void)PlsrProfileStep(&state, &frequency, &completed);
        if (frequency > peak)
        {
            peak = frequency;
        }
        stepCount++;
        if ((retargetHz != 0U) && (stepCount == retargetAfterSteps))
        {
            CHECK(PlsrProfileRetarget(&state, retargetHz) == PLSR_RESULT_OK);
        }
    }
    CHECK(completed != 0U);
    *actualPulses = (int64_t)(state.emittedPulsesQ32 >> 32U);
    *peakHz = peak;
    *steps = stepCount;
}

static PLSR_PROFILE_REQUEST MakeRequest(uint32_t targetHz,
                                        uint32_t startHz,
                                        uint32_t stopHz,
                                        uint32_t accelSlope,
                                        uint32_t decelSlope,
                                        uint8_t curveMode)
{
    PLSR_PROFILE_REQUEST request;

    (void)memset(&request, 0, sizeof(request));
    request.targetFrequencyHz = targetHz;
    request.startFrequencyHz = startHz;
    request.stopFrequencyHz = stopHz;
    request.maxFrequencyHz = 100000UL;
    request.accelSlopeHzPerMs = accelSlope;
    request.decelSlopeHzPerMs = decelSlope;
    request.curveMode = curveMode;
    return request;
}

static void TestTrapezoid(void)
{
    PLSR_PROFILE_REQUEST request = MakeRequest(10000U, 0U, 0U, 100U, 100U, 0U);
    int64_t pulses;
    uint32_t peak;
    int64_t steps;

    RunProfile(&request, 20000, 1000U, 0U, 0, &pulses, &peak, &steps);
    CHECK(pulses == 20000);
    CHECK(peak == 10000U);
    CHECK(steps > 2000);
}

static void TestTriangle(void)
{
    PLSR_PROFILE_REQUEST request = MakeRequest(10000U, 0U, 0U, 100U, 100U, 0U);
    int64_t pulses;
    uint32_t peak;
    int64_t steps;

    /* 脉冲太少：永远到不了目标频率，三角曲线。 */
    RunProfile(&request, 100, 1000U, 0U, 0, &pulses, &peak, &steps);
    CHECK(pulses == 100);
    CHECK(peak < 10000U);
    CHECK(peak > 0U);
}

static void TestStartStopSpeed(void)
{
    PLSR_PROFILE_REQUEST request = MakeRequest(10000U, 1000U, 500U,
                                               100U, 80U, 0U);
    int64_t pulses;
    uint32_t peak;
    int64_t steps;

    RunProfile(&request, 50000, 1000U, 0U, 0, &pulses, &peak, &steps);
    CHECK(pulses == 50000);
    CHECK(peak == 10000U);
}

static void TestSCurve(void)
{
    PLSR_PROFILE_REQUEST request = MakeRequest(10000U, 0U, 0U, 100U, 100U, 1U);
    int64_t pulses;
    uint32_t peak;
    int64_t steps;

    RunProfile(&request, 20000, 1000U, 0U, 0, &pulses, &peak, &steps);
    CHECK(pulses == 20000);
    CHECK(peak == 10000U);
    CHECK(steps > 2000);
}

static void TestSine(void)
{
    PLSR_PROFILE_REQUEST request = MakeRequest(10000U, 0U, 0U, 100U, 100U, 2U);
    int64_t pulses;
    uint32_t peak;
    int64_t steps;

    RunProfile(&request, 20000, 1000U, 0U, 0, &pulses, &peak, &steps);
    CHECK(pulses == 20000);
    CHECK(peak == 10000U);
}

static void TestRetargetUp(void)
{
    PLSR_PROFILE_REQUEST request = MakeRequest(5000U, 0U, 0U, 100U, 100U, 0U);
    int64_t pulses;
    uint32_t peak;
    int64_t steps;

    /* 运行中升频到 10000：平滑过渡且总脉冲精确。 */
    RunProfile(&request, 30000, 1000U, 10000U, 500, &pulses, &peak, &steps);
    CHECK(pulses == 30000);
    CHECK(peak == 10000U);
}

static void TestRetargetDown(void)
{
    PLSR_PROFILE_REQUEST request = MakeRequest(10000U, 0U, 0U, 100U, 100U, 0U);
    int64_t pulses;
    uint32_t peak;
    int64_t steps;

    /* 运行中降频到 3000：先减速到新目标再匀速，总脉冲精确。 */
    RunProfile(&request, 30000, 1000U, 3000U, 500, &pulses, &peak, &steps);
    CHECK(pulses == 30000);
    CHECK(peak == 10000U);
    CHECK(steps > 600);
}

static void TestNoAccel(void)
{
    PLSR_PROFILE_REQUEST request = MakeRequest(10000U, 0U, 0U, 0U, 0U, 0U);
    int64_t pulses;
    uint32_t peak;
    int64_t steps;

    RunProfile(&request, 5000, 1000U, 0U, 0, &pulses, &peak, &steps);
    CHECK(pulses == 5000);
    CHECK(peak == 10000U);
    CHECK(steps == 500);
}

static void TestZeroRampRetarget(void)
{
    PLSR_PROFILE_REQUEST request = MakeRequest(100000U, 100000U, 0U,
                                                0U, 0U, 0U);
    PLSR_PROFILE_STATE state;
    uint32_t frequency = 0U;
    uint8_t completed = 0U;

    CHECK(PlsrProfileStart(&state, &request, 1000000, 10000U)
          == PLSR_RESULT_OK);
    CHECK(state.phase == PLSR_PROFILE_PHASE_CRUISE);
    CHECK(PlsrProfileRetarget(&state, 50000U) == PLSR_RESULT_OK);
    CHECK(state.phase == PLSR_PROFILE_PHASE_CRUISE);
    CHECK((state.frequencyQ32 >> 32U) == 50000U);
    CHECK(PlsrProfileStep(&state, &frequency, &completed) == PLSR_RESULT_OK);
    CHECK(frequency == 50000U);
    CHECK(completed == 0U);

    CHECK(PlsrProfileRetarget(&state, 100000U) == PLSR_RESULT_OK);
    CHECK(state.phase == PLSR_PROFILE_PHASE_CRUISE);
    CHECK((state.frequencyQ32 >> 32U) == 100000U);
    CHECK(PlsrProfileStep(&state, &frequency, &completed) == PLSR_RESULT_OK);
    CHECK(frequency == 100000U);
    CHECK(completed == 0U);
}

static void TestSubMillisecondRefresh(void)
{
    PLSR_PROFILE_REQUEST request = MakeRequest(10000U, 0U, 0U, 100U, 100U, 0U);
    int64_t pulses;
    uint32_t peak;
    int64_t steps;

    /* 0.1ms 刷新：精度更高，步数是 1ms 的约 10 倍。 */
    RunProfile(&request, 20000, 10000U, 0U, 0, &pulses, &peak, &steps);
    CHECK(pulses == 20000);
    CHECK(peak == 10000U);
    CHECK(steps > 20000);
}

static void TestSinglePulse(void)
{
    PLSR_PROFILE_REQUEST request = MakeRequest(1000U, 0U, 0U, 100U, 100U, 0U);
    int64_t pulses;
    uint32_t peak;
    int64_t steps;

    RunProfile(&request, 1, 1000U, 0U, 0, &pulses, &peak, &steps);
    CHECK(pulses == 1);
    CHECK(peak <= 1000U);
}

static void TestHighFrequency(void)
{
    PLSR_PROFILE_REQUEST request = MakeRequest(100000U, 0U, 0U,
                                               10000U, 10000U, 0U);
    int64_t pulses;
    uint32_t peak;
    int64_t steps;

    RunProfile(&request, 1000000, 1000U, 0U, 0, &pulses, &peak, &steps);
    CHECK(pulses == 1000000);
    CHECK(peak == 100000U);
}

static void TestArgumentValidation(void)
{
    PLSR_PROFILE_STATE state;
    PLSR_PROFILE_REQUEST request = MakeRequest(1000U, 0U, 0U, 100U, 100U, 0U);
    uint32_t frequency;
    uint8_t completed;

    CHECK(PlsrProfileStart(NULL, &request, 100, 1000U)
          == PLSR_RESULT_INVALID_ARGUMENT);
    CHECK(PlsrProfileStart(&state, NULL, 100, 1000U)
          == PLSR_RESULT_INVALID_ARGUMENT);
    CHECK(PlsrProfileStart(&state, &request, 0, 1000U)
          == PLSR_RESULT_INVALID_ARGUMENT);
    CHECK(PlsrProfileStart(&state, &request, 100, 0U)
          == PLSR_RESULT_INVALID_ARGUMENT);
    request.targetFrequencyHz = 0U;
    CHECK(PlsrProfileStart(&state, &request, 100, 1000U)
          == PLSR_RESULT_INVALID_ARGUMENT);
    request.targetFrequencyHz = 1000U;

    CHECK(PlsrProfileStep(NULL, &frequency, &completed)
          == PLSR_RESULT_INVALID_ARGUMENT);
    CHECK(PlsrProfileStep(&state, NULL, &completed)
          == PLSR_RESULT_INVALID_ARGUMENT);
    CHECK(PlsrProfileStep(&state, &frequency, NULL)
          == PLSR_RESULT_INVALID_ARGUMENT);
    CHECK(PlsrProfileRetarget(NULL, 2000U) == PLSR_RESULT_INVALID_ARGUMENT);
    CHECK(PlsrProfileRetarget(&state, 0U) == PLSR_RESULT_INVALID_ARGUMENT);
}

static void TestSyncPulses(void)
{
    /* 校准虚拟计数到硬件计数：模拟硬件滞后 38 个脉冲（预装载滞后），
     * 校准后 DONE 判定与硬件同步，段尾不再冻结频率补发。 */
    PLSR_PROFILE_REQUEST request = MakeRequest(1000U, 0U, 0U, 100U, 100U, 0U);
    PLSR_PROFILE_STATE state;
    uint32_t frequency;
    uint8_t completed;
    int i;

    CHECK(PlsrProfileStart(&state, &request, 500, 1000U) == PLSR_RESULT_OK);

    /* 自然推进 100 步：虚拟计数已超前（未校准）。 */
    for (i = 0; i < 100; i++)
    {
        CHECK(PlsrProfileStep(&state, &frequency, &completed)
              == PLSR_RESULT_OK);
    }
    CHECK(completed == 0U);

    /* 校准到硬件计数（硬件只发了 400 个）——虚拟被拉回。 */
    PlsrProfileSyncPulses(&state, 400U);
    CHECK(PlsrProfileStep(&state, &frequency, &completed)
          == PLSR_RESULT_OK);
    CHECK(completed == 0U);   /* 400+本步增量 < 500，不提前 DONE */

    /* 校准到 499（还差 1 个）：下一步 DONE。 */
    PlsrProfileSyncPulses(&state, 499U);
    CHECK(PlsrProfileStep(&state, &frequency, &completed)
          == PLSR_RESULT_OK);
    CHECK(completed != 0U);
    CHECK(frequency == 0U);
}

static void TestInitialOutputFrequency(void)
{
    PLSR_PROFILE_REQUEST request = MakeRequest(1000U, 0U, 0U,
                                                10U, 10U, 0U);
    PLSR_PROFILE_STATE state;
    uint32_t frequency;
    uint8_t completed;

    CHECK(PlsrProfileStart(&state, &request, 500, 1000U)
          == PLSR_RESULT_OK);
    CHECK(PlsrProfileStep(&state, &frequency, &completed)
          == PLSR_RESULT_OK);
    CHECK(frequency == 10U);
    /* 10Hz 以10Hz/ms连续加速，首脉冲末约141Hz，
     * 首周期等效频率约75Hz（约13.3ms），不再等100ms。 */
    CHECK(PlsrProfileGetInitialOutputFrequency(&state) == 75U);

    request.targetFrequencyHz = 20U;
    request.maxFrequencyHz = 20U;
    CHECK(PlsrProfileStart(&state, &request, 50, 1000U)
          == PLSR_RESULT_OK);
    CHECK(PlsrProfileStep(&state, &frequency, &completed)
          == PLSR_RESULT_OK);
    CHECK(PlsrProfileGetInitialOutputFrequency(&state) == 19U);

    state.curveMode = PLSR_PROFILE_CURVE_S;
    CHECK(PlsrProfileGetInitialOutputFrequency(&state) == frequency);
    CHECK(PlsrProfileGetInitialOutputFrequency(NULL) == 0U);
}

static void TestBrakingOutputFrequency(void)
{
    PLSR_PROFILE_REQUEST request = MakeRequest(2000U, 0U, 0U,
                                                10U, 10U, 0U);
    PLSR_PROFILE_STATE state;

    CHECK(PlsrProfileStart(&state, &request, 1000, 1000U)
          == PLSR_RESULT_OK);
    CHECK(PlsrProfileGetBrakingOutputFrequency(&state, 201U) == 2000U);
    CHECK(PlsrProfileGetBrakingOutputFrequency(&state, UINT64_MAX)
          == 2000U);
    CHECK(PlsrProfileGetBrakingOutputFrequency(&state, 5U) == 299U);
    CHECK(PlsrProfileGetBrakingOutputFrequency(&state, 4U) == 263U);
    CHECK(PlsrProfileGetBrakingOutputFrequency(&state, 3U) == 222U);
    CHECK(PlsrProfileGetBrakingOutputFrequency(&state, 2U) == 170U);
    CHECK(PlsrProfileGetBrakingOutputFrequency(&state, 1U) == 70U);
    CHECK(PlsrProfileGetBrakingOutputFrequency(&state, 0U) == 0U);
    CHECK(PlsrProfileGetBrakingOutputFrequency(NULL, 1U) == 0U);

    state.curveMode = PLSR_PROFILE_CURVE_S;
    CHECK(PlsrProfileGetBrakingOutputFrequency(&state, 1U) == 2000U);

    /* 极端参数也不得让制动距离乘法溢出。 */
    state.curveMode = PLSR_PROFILE_CURVE_LINEAR;
    state.targetFrequencyHz = UINT32_MAX;
    state.stopFrequencyHz = 0U;
    state.decelSlopeHzPerMs = UINT32_MAX;
    CHECK(PlsrProfileGetBrakingOutputFrequency(&state, UINT64_MAX)
          == UINT32_MAX);
}

static void TestControlledStop(void)
{
    PLSR_PROFILE_REQUEST request = MakeRequest(1000U, 1000U, 0U,
                                                10U, 10U, 0U);
    PLSR_PROFILE_STATE state;
    uint32_t frequency = 0U;
    uint64_t pausedPulsesQ32;
    uint8_t completed = 0U;
    int step;

    CHECK(PlsrProfileStart(&state, &request, 10000, 1000U)
          == PLSR_RESULT_OK);
    CHECK(state.phase == PLSR_PROFILE_PHASE_CRUISE);
    CHECK(PlsrProfileRequestStop(&state) == PLSR_RESULT_OK);
    CHECK(state.phase == PLSR_PROFILE_PHASE_DECEL);
    for (step = 0; (step < 110) && (completed == 0U); step++)
    {
        CHECK(PlsrProfileStep(&state, &frequency, &completed)
              == PLSR_RESULT_OK);
    }
    CHECK(completed != 0U);
    CHECK(frequency == 0U);
    CHECK(state.phase == PLSR_PROFILE_PHASE_DONE);

    pausedPulsesQ32 = state.emittedPulsesQ32;
    CHECK(PlsrProfileResume(&state, 500U, 2000U, 0U)
          == PLSR_RESULT_OK);
    CHECK(state.phase == PLSR_PROFILE_PHASE_ACCEL);
    CHECK(state.emittedPulsesQ32 == pausedPulsesQ32);
    CHECK(PlsrProfileStep(&state, &frequency, &completed)
          == PLSR_RESULT_OK);
    CHECK(completed == 0U);
    CHECK(frequency > 500U);

    request.decelSlopeHzPerMs = 0U;
    CHECK(PlsrProfileStart(&state, &request, 10000, 1000U)
          == PLSR_RESULT_OK);
    CHECK(PlsrProfileRequestStop(&state) == PLSR_RESULT_OK);
    CHECK(state.phase == PLSR_PROFILE_PHASE_DONE);
    CHECK(PlsrProfileRequestStop(NULL) == PLSR_RESULT_INVALID_ARGUMENT);
    CHECK(PlsrProfileResume(NULL, 500U, 2000U, 0U)
          == PLSR_RESULT_INVALID_ARGUMENT);
    state.emittedPulsesQ32 = (uint64_t)state.totalPulses
                             * PLSR_PROFILE_Q32_ONE;
    CHECK(PlsrProfileResume(&state, 500U, 2000U, 0U)
          == PLSR_RESULT_INVALID_STATE);
}

static void TestPlan(void)
{
    PLSR_PROFILE_REQUEST request = MakeRequest(10000U, 0U, 0U, 100U, 100U, 0U);
    PLSR_PROFILE_PLAN plan;
    PLSR_PROFILE_REQUEST triangleRequest =
        MakeRequest(10000U, 0U, 0U, 100U, 100U, 0U);

    CHECK(PlsrProfilePlan(&request, 20000, &plan) == PLSR_RESULT_OK);
    CHECK(plan.accelTimeMs == 100U);
    CHECK(plan.decelTimeMs == 100U);
    CHECK(plan.triangular == 0U);
    CHECK(plan.peakFrequencyHz == 10000U);
    CHECK(plan.cruiseTimeMs > 0U);

    CHECK(PlsrProfilePlan(&triangleRequest, 100, &plan) == PLSR_RESULT_OK);
    CHECK(plan.triangular != 0U);
    CHECK(plan.peakFrequencyHz < 10000U);
    CHECK(plan.accelTimeMs > 0U);

    CHECK(PlsrProfilePlan(NULL, 100, &plan) == PLSR_RESULT_INVALID_ARGUMENT);
    CHECK(PlsrProfilePlan(&request, 0, &plan) == PLSR_RESULT_INVALID_ARGUMENT);
    CHECK(PlsrProfilePlan(&request, 100, NULL) == PLSR_RESULT_INVALID_ARGUMENT);
}

int main(void)
{
    TestTrapezoid();
    TestTriangle();
    TestStartStopSpeed();
    TestSCurve();
    TestSine();
    TestRetargetUp();
    TestRetargetDown();
    TestNoAccel();
    TestZeroRampRetarget();
    TestSubMillisecondRefresh();
    TestSinglePulse();
    TestHighFrequency();
    TestArgumentValidation();
    TestSyncPulses();
    TestInitialOutputFrequency();
    TestBrakingOutputFrequency();
    TestControlledStop();
    TestPlan();

    if (TestFailures != 0)
    {
        (void)printf("FAIL: %d of %d PLSR profile checks failed\n",
                     TestFailures,
                     TestChecks);
        return 1;
    }
    (void)printf("PASS: %d PLSR profile checks\n", TestChecks);
    return 0;
}