fix(foc utils): restore the FLT_MIN guard in _atan2()

[stijncarelsbergh]

fix(foc utils): restore the FLT_MIN guard in _atan2()

The comment says 'inject FLT_MIN in denominator to avoid division by zero' but the
term is missing (it was dropped from the ODrive original), so _atan2(0,0) is 0/0
and returns NaN instead of 0. Restores the original guard, which only affects the
degenerate (0,0) case.

---

Split out of #571 at your request: one fix per PR, against dev. The branch contains
nothing else, so it can be reviewed, amended or dropped on its own.

The CI board matrix runs automatically; I did not run any hardware test, so the behavioural
claims are from reading the code plus the compiler. Happy to adjust the wording, split it
differently or drop it - no attachment to this one.

[Copilot]

Copilot was unable to review this pull request because the user who requested the review has reached their quota limit.

[dekutree64]

Won't this still return a very large number? It seems more logical to return 0 for that case. I've been using this code for a long time, and meant to include it last time I updated LinearHall (the primary user of atan2), but forgot to do the PR for it due to being in different repositories. It should also be slightly faster, doing a single branch chain instead of min and max, and no extra addition.

    float a;
    if (abs_x < abs_y) a = abs_x / abs_y;
    else if (abs_x > abs_y) a = abs_y / abs_x;
    else return 0; // Avoid division by zero

[stijncarelsbergh]

Thanks @dekutree64 - you are right that the epsilon is the wrong tool here, and I have
switched to your chain. One detail worth flagging though, because it changes the snippet
slightly:

The epsilon version does not return a large number for (0,0). The numerator is exactly
0, so 0 / FLT_MIN == 0 - the function already returned 0 for that case. The epsilon was
hiding the 0/0, not producing a bogus angle. (Without it the result is NaN.) So the outcome
you wanted was already there, just by accident instead of by intent - and I agree an explicit
guard is better than relying on that.

Your snippet has one case that isn't (0,0): else return 0; also fires whenever
|x| == |y|, which includes (1,1), (5,5), (-3,3), ... For those a == 1 and atan2 is
±PI/4. Emulating the exact single-precision operation order:

y, x	else return 0	correct	error
0, 0	0.00000	0	-
1, 1	0.00000	0.78540	45 deg
5, 5	0.00000	0.78540	45 deg
−3, 3	0.00000	−0.78540	45 deg
2, 1	1.10713	1.10715	0.001 deg

Over a full 360 deg sweep the worst error becomes 135 deg, against 0.0116 deg for the
polynomial when a is right. So I kept your chain and split the equal case:

    float a;
    if (abs_x < abs_y)        a = abs_x / abs_y;
    else if (abs_x > abs_y)   a = abs_y / abs_x;
    else if (abs_x == 0.0f)   return 0.0f;   // atan2(0,0) == 0
    else                      a = 1.0f;      // |x| == |y| != 0  ->  a == 1

That returns 0 for (0,0), keeps a == 1 for the equal non-zero cases, and drops both the
extra add and one min/max pair - so the speed argument survives intact (on AVR each float
compare and division is a library call). It also removes the #include <float.h> I had added,
so the change is now just this block plus the three line comment above it.

Pushed as f43a780 on this branch, superseding the FLT_MIN commit. One extra data point in
favour of your instinct: the epsilon also distorts subnormal inputs - equal values of 1e-38
give 0.4309 instead of 0.7854 with it, because max + FLT_MIN is comparable to the inputs.
Irrelevant at sensor scale, but another reason to prefer the explicit guard.

If I misread your snippet (e.g. you expect callers to never hit |x| == |y|), say so and I'll
adjust - I'm happy with either form as long as that case stays correct.

[dekutree64]

Wow, I'm glad I forgot the PR then :) I suspect it's been silently failing for me, causing imperceptible twitches, whereas the div by 0 permanently wrecks the PID so it's really obvious when it happens.

I would probably implement it like this, but I don't think it really matters since in the vast majority of cases with yours, one of the first two if statements should fire and skip past the zero check anyway.

    if (abs_x < abs_y)        a = abs_x / abs_y;
    else if (abs_x != 0.0f)   a = abs_y / abs_x;
    else  return 0.0f; // atan2(0,0) == 0

EDIT: On second thought, yours is better since the < and > results can be extracted from a single compare operation (at least on ARM with FPU), whereas mine would do a separate compare with 0 in half of the common cases.