From eb7c8ed6915ab369c331442e5aafd054b30d6ba4 Mon Sep 17 00:00:00 2001 From: stijncarelsbergh Date: Thu, 8 Oct 2026 00:04:40 +0200 Subject: [PATCH 1/2] 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. --- src/common/foc_utils.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 1d75d7ba7..905f641a8 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -1,5 +1,7 @@ #include "foc_utils.h" +#include + // function approximating the sine calculation by using fixed size array // uses a 65 element lookup table and interpolation @@ -56,7 +58,7 @@ __attribute__((weak)) float _atan2(float y, float x) { float abs_y = fabsf(y); float abs_x = fabsf(x); // inject FLT_MIN in denominator to avoid division by zero - float a = min(abs_x, abs_y) / (max(abs_x, abs_y)); + float a = min(abs_x, abs_y) / (max(abs_x, abs_y) + FLT_MIN); // s := a * a float s = a * a; // r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + a From f43a780db18399ced705dca3235b99ab3653bbef Mon Sep 17 00:00:00 2001 From: stijncarelsbergh Date: Sat, 10 Oct 2026 17:43:00 +0200 Subject: [PATCH 2/2] refactor(foc utils): use the comparison chain suggested in review, keep |x| == |y| correct @dekutree64 pointed out in #583 that the comparison chain is cheaper than min/max plus an epsilon in the denominator, and that is right: it drops one float add and one min/max pair, and on AVR every float compare and division is a library call. His snippet as written was `else return 0; // Avoid division by zero`, but that branch also catches |x| == |y| != 0 (where a == 1 and atan2 is +-PI/4). Measured in single precision: atan2(1,1) would become 0 instead of 0.7854, i.e. a 45 degree error, and up to 135 degrees over a full sweep, while the chain below keeps the 0.0116 degree accuracy of the polynomial. So: chain form, with the equal case split into (0,0) -> 0 and otherwise a = 1. This also drops the FLT_MIN include from the previous commit. --- src/common/foc_utils.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 905f641a8..17710c05a 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -1,7 +1,5 @@ #include "foc_utils.h" -#include - // function approximating the sine calculation by using fixed size array // uses a 65 element lookup table and interpolation @@ -57,8 +55,14 @@ __attribute__((weak)) float _atan2(float y, float x) { // a := min (|x|, |y|) / max (|x|, |y|) float abs_y = fabsf(y); float abs_x = fabsf(x); - // inject FLT_MIN in denominator to avoid division by zero - float a = min(abs_x, abs_y) / (max(abs_x, abs_y) + FLT_MIN); + // the single comparison chain avoids the division by zero without an epsilon in the + // denominator; the |x| == |y| case still needs to divide (a == 1), only the + // degenerate (0,0) input returns early - atan2(0,0) is 0 + 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; + else a = 1.0f; // s := a * a float s = a * a; // r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + a