temp mahony filter

main/fixedpoint.c

@@ -0,0 +1,156 @@
+#include "fixedpoint.h"
+
+#include <limits.h>
+#include <stddef.h>
+
+static const int32_t s_fixed_atan_table_q30[] = {
+    843314857, 497837829, 263043836, 133525159,
+    67021687,  33543516,  16775851,  8388437,
+    4194283,   2097149,   1048575,   524288,
+    262144,    131072,    65536,     32768,
+};
+
+static uint32_t fixed_isqrt_u64(uint64_t value)
+{
+    uint64_t bit = (uint64_t)1 << 62;
+    uint64_t result = 0;
+
+    while (bit > value)
+    {
+        bit >>= 2;
+    }
+
+    while (bit != 0)
+    {
+        if (value >= result + bit)
+        {
+            value -= result + bit;
+            result = (result >> 1) + bit;
+        }
+        else
+        {
+            result >>= 1;
+        }
+
+        bit >>= 2;
+    }
+
+    return (uint32_t)result;
+}
+
+int32_t fixed_sqrt(int32_t value, uint8_t frac_bits)
+{
+    uint64_t scaled_value;
+    uint32_t root;
+
+    if (value <= 0)
+    {
+        return 0;
+    }
+
+    scaled_value = ((uint64_t)(uint32_t)value) << frac_bits;
+    root = fixed_isqrt_u64(scaled_value);
+    if (root > (uint32_t)INT32_MAX)
+    {
+        return INT32_MAX;
+    }
+
+    return (int32_t)root;
+}
+
+int32_t fixed_inv_sqrt(int32_t value, uint8_t frac_bits)
+{
+    int32_t root;
+
+    if (value <= 0)
+    {
+        return 0;
+    }
+
+    root = fixed_sqrt(value, frac_bits);
+    return FIXED_DIV(FIXED_ONE(frac_bits), root, frac_bits);
+}
+
+int32_t fixed_atan2(int32_t y, int32_t x, uint8_t frac_bits)
+{
+    int64_t x_q30;
+    int64_t y_q30;
+    int64_t angle_q30 = 0;
+    int shift;
+
+    if (x == 0)
+    {
+        if (y > 0)
+        {
+            return FIXED_HALF_PI(frac_bits);
+        }
+        if (y < 0)
+        {
+            return -FIXED_HALF_PI(frac_bits);
+        }
+        return 0;
+    }
+
+    if (x < 0)
+    {
+        if (y >= 0)
+        {
+            return fixed_atan2(-y, -x, frac_bits) + FIXED_PI(frac_bits);
+        }
+        return fixed_atan2(-y, -x, frac_bits) - FIXED_PI(frac_bits);
+    }
+
+    shift = 30 - frac_bits;
+    if (shift >= 0)
+    {
+        x_q30 = (int64_t)x << shift;
+        y_q30 = (int64_t)y << shift;
+    }
+    else
+    {
+        x_q30 = (int64_t)x >> (-shift);
+        y_q30 = (int64_t)y >> (-shift);
+    }
+
+    for (size_t i = 0; i < sizeof(s_fixed_atan_table_q30) / sizeof(s_fixed_atan_table_q30[0]); ++i)
+    {
+        int64_t x_next;
+        int64_t y_next;
+
+        if (y_q30 > 0)
+        {
+            x_next = x_q30 + (y_q30 >> i);
+            y_next = y_q30 - (x_q30 >> i);
+            angle_q30 += s_fixed_atan_table_q30[i];
+        }
+        else
+        {
+            x_next = x_q30 - (y_q30 >> i);
+            y_next = y_q30 + (x_q30 >> i);
+            angle_q30 -= s_fixed_atan_table_q30[i];
+        }
+
+        x_q30 = x_next;
+        y_q30 = y_next;
+    }
+
+    if (shift >= 0)
+    {
+        return (int32_t)(angle_q30 >> shift);
+    }
+    return (int32_t)(angle_q30 << (-shift));
+}
+
+int32_t fixed_asin(int32_t value, uint8_t frac_bits)
+{
+    const int32_t one = FIXED_ONE(frac_bits);
+    const int32_t clamped = FIXED_CLAMP(value, -one, one);
+    const int32_t one_minus_square = one - FIXED_MUL(clamped, clamped, frac_bits);
+
+    if (one_minus_square <= 0)
+    {
+        return clamped >= 0 ? FIXED_HALF_PI(frac_bits) : -FIXED_HALF_PI(frac_bits);
+    }
+
+    return fixed_atan2(clamped, fixed_sqrt(one_minus_square, frac_bits), frac_bits);
+}