FTM: fix printer reset with resonance test for MCU without FPU

[narno2202]

Description

@plampix reports in #28259, SKR mini E3 V3 reset when a FT_MOTION resonance test is launched. The cause is probably a math error related to the absence of FPU in the STM32G0. I adapted the math of resonance generator. @plampix reports success with the patch.
@dbuezas , could you have a look at the code to be sure it's ok.

Requirements

Benefits

FT_MOTION resonance test success on MCU without FPU

Configurations

Related Issues

#28259

[dbuezas]

Getting rid of trigonometry functions is great. Mod and exp are also expensive, so it would be great to do without them too.

[narno2202]

Don't forget, it's just a test to be run infrequently; I don't think, as said @thinkyhead before, that it's time-critical.

[dbuezas]

That doesn't relevant, if the mcu can't keep up the resonance results will be skewed

[thinkyhead]

If we could do all the expensive calculations in advance and cache them in a not-too-large buffer that would be a good optimization. I suggest taking 25 samples of one quarter of a sine wave and caching them, and then doing a simple linear interpolation between the nearest values in the cache, mirroring in XY for the other three quarters of the sine wave.

[thinkyhead]

Here is the general idea as outlined by gpt-oss:20b for us…

import math

#
#  Build a 25‑point lookup table for the first quarter wave.
#  Index  0 corresponds to sin(0)   = 0
#  Index 24 corresponds to sin(π/2) = 1
#
QUARTER_POINTS = 25
TABLE = [math.sin(i * (math.pi / 2) / (QUARTER_POINTS - 1))
         for i in range(QUARTER_POINTS)]

#
# Linear interpolation helper.
# Given a value 't' in [0, 1], return an interpolated
# sine value between TABLE[0] and TABLE[24].
#
def _lerp_sin(t: float) -> float:
    """
    Linear interpolation of sine over the first quarter.
    t = 0 -> sin(0) , t = 1 -> sin(π/2)
    """
    if t <= 0.0:
        return TABLE[0]
    if t >= 1.0:
        return TABLE[-1]

    # Position in the table: 0 … 24
    pos = t * (QUARTER_POINTS - 1)
    idx = int(math.floor(pos))
    frac = pos - idx

    # Clamp to avoid index overflow
    if idx >= QUARTER_POINTS - 1:
        return TABLE[-1]

    return TABLE[idx] + (TABLE[idx + 1] - TABLE[idx]) * frac

#
# Fast sine approximation for any angle (radians).
#
def sin_fast(x: float) -> float:
    """
    Approximate sin(x) using a 25‑point quarter‑wave table and
    linear interpolation. Works for all real x.
    """
    # Wrap x into [0, 2π)
    two_pi = 2 * math.pi
    x = x % two_pi

    # Determine quadrant (0 to 3)
    quadrant = int(x / (math.pi / 2))
    # Normalized angle within the quadrant [0, π/2)
    t = (x - quadrant * (math.pi / 2)) / (math.pi / 2)

    # Map quadrants to base value and sign
    if quadrant == 0:          # 0 to π/2  → sin(t)
        base = _lerp_sin(t)
        sign = 1.0
    elif quadrant == 1:        # π/2 to π  → sin(π - x) = sin(t)
        base = _lerp_sin(1.0 - t)
        sign = 1.0
    elif quadrant == 2:        # π to 3π/2 → sin(x) = -sin(t)
        base = _lerp_sin(t)
        sign = -1.0
    else:                      # 3π/2 to 2π → sin(2π - x) = -sin(t)
        base = _lerp_sin(1.0 - t)
        sign = -1.0

    return sign * base

#
# Example: compute a few values
#
if __name__ == "__main__":
    test_angles = [0, math.pi/6, math.pi/4, math.pi/3, math.pi/2,
                   2*math.pi/3, 5*math.pi/6, math.pi, 7*math.pi/6,
                   4*math.pi/3, 3*math.pi/2, 5*math.pi/3, 11*math.pi/6, 2*math.pi]

    print("Angle (rad) | sin_exact | sin_fast | error")
    for a in test_angles:
        exact = math.sin(a)
        fast = sin_fast(a)
        err = abs(exact - fast)
        print(f"{a:12.6f} | {exact:10.6f} | {fast:10.6f} | {err:.6e}")

[narno2202]

Perhaps we could wait for more reports before doing other changes for computation if the last code is ok (the code with the last @thinkyhead changes).

[thinkyhead]

I was able to move a number of multiply operations out of the inner loop and reduce the amount of data copied for each trajectory point, so these should help somewhat. The value of amplitude_precalc could be calculated completely outside of the resonance test, e.g., whenever amplitude_correction or accel_per_hz are modified, saving some cycles upon entry to fill_stepper_plan_buffer. If the linear interpolation approach to fast_sin turns out to be faster (and if it's not too sloppy) then we can look at that also.

[narno2202]

@plampix reports success with my initial code on his MCU (Cortex-M0+) so it should also be ok for STM32 F1 and F2 and LPC1768/69 and it's even better with your changes. I am in favor of waiting for more reports.

[thinkyhead]

I am in favor of waiting for more reports.

If you have a tester with a pending report, we can wait. If it's uncertain that a report is coming, well, we are guaranteed to get some testing and feedback after this is merged. So I'm happy to merge this whenever you are.

[dbuezas]

The expensive ones are exp and mod which are still there. Multiplies are fine, there are lots in the normal ftmotion core. The fast sin was imo a lot clearer before 😁

We can eliminate the mod by tracking the delta of the phase and only adding that delta to the phase and subtracting 2*pi from it when it goes beyond it
The exp can be removed by doing a linear or quadratic frequency sweep instead of exponential (which will also behave better since the exponential currently stays so long on low frequencies)

[dbuezas]

On an m0, i believe that:

mulf = 1×
modf = ~20×
exp2f = ~100×
sinf = ~150×

Compared to those, caching a couple of multiplications doesn't make much difference unfortunately

[narno2202]

When I say more reports, I mean after merging current code.
For sure, we can choose another way of generating vibrations. The frequency range to test is probably 20 to 70 Hz; if the generator is ok in this range, there are probably more important things to fix before an optimization.

[thinkyhead]

Sounds good! Here are the alternatives mentioned…

float getFrequencyFromTimelineLinear() {
  const float k = rt_params.min_freq / rt_params.octave_duration;
  return rt_params.min_freq + k * rt_time;
}

float getFrequencyFromTimelineQuadratic() {
  const float k = rt_params.min_freq / sq(rt_params.octave_duration);
  return rt_params.min_freq + k * sq(rt_time);
}