diff --git a/src/include/sof/audio/format.h b/src/include/sof/audio/format.h index 8174e53a4163..a468278531f3 100644 --- a/src/include/sof/audio/format.h +++ b/src/include/sof/audio/format.h @@ -70,6 +70,9 @@ /* Convert fractional Qnx.ny number x to float */ #define Q_CONVERT_QTOF(x, ny) ((float)(x) / ((int64_t)1 << (ny))) +/* Convert fractional Qnx.ny number x to double */ +#define Q_CONVERT_QTOD(x, ny) ((double)(x) / ((int64_t)1 << (ny))) + /* A more clever macro for Q-shifts */ #define Q_SHIFT(x, src_q, dst_q) ((x) >> ((src_q) - (dst_q))) #define Q_SHIFT_RND(x, src_q, dst_q) \ diff --git a/src/include/sof/math/trig.h b/src/include/sof/math/trig.h index 9f04c137a265..67428cac494a 100644 --- a/src/include/sof/math/trig.h +++ b/src/include/sof/math/trig.h @@ -383,4 +383,18 @@ static inline int16_t acos_fixed_16b(int32_t cdc_acos_th) return sat_int16(Q_SHIFT_RND(th_acos_fxp, 29, 13)); } +/** + * sofm_atan2_32b() - Four-quadrant arctangent using degree-9 Remez minimax polynomial + * @param y Imaginary component (sine) in Q1.31 format. + * @param x Real component (cosine) in Q1.31 format. + * @return Angle in Q3.29 radians, range [-pi, +pi]. + * + * Uses the Horner-form polynomial: + * atan(z) = z * (C0 + z^2 * (C1 + z^2 * (C2 + z^2 * (C3 + z^2 * C4)))) + * + * with Remez minimax coefficients on [0, 1]. + * Maximum error ~0.001 degrees (1.94e-5 radians). + */ +int32_t sofm_atan2_32b(int32_t y, int32_t x); + #endif /* __SOF_MATH_TRIG_H__ */ diff --git a/src/math/CMakeLists.txt b/src/math/CMakeLists.txt index c3dcfae0f070..a52263006295 100644 --- a/src/math/CMakeLists.txt +++ b/src/math/CMakeLists.txt @@ -11,6 +11,10 @@ if(CONFIG_CORDIC_FIXED) list(APPEND base_files trig.c) endif() +if(CONFIG_MATH_ATAN2) + list(APPEND base_files atan2.c) +endif() + if(CONFIG_MATH_LUT_SINE_FIXED) list(APPEND base_files lut_trig.c) endif() diff --git a/src/math/Kconfig b/src/math/Kconfig index 8d5647a159be..1feaab8f03d1 100644 --- a/src/math/Kconfig +++ b/src/math/Kconfig @@ -9,6 +9,16 @@ config CORDIC_FIXED Select this to enable sin(), cos(), asin(), acos(), and cexp() functions as 16 bit and 32 bit versions. +config MATH_ATAN2 + bool "Four-quadrant arctangent function" + default n + help + Select this to enable sofm_atan2_32b(y, x) function. It computes + the four-quadrant arctangent using a polynomial approximation. + Input arguments y and x are Q1.31 format and the output angle + is Q3.29 format with range [-pi, pi]. The maximum approximation + error is ~0.001 degrees. + config MATH_LUT_SINE_FIXED bool "Lookup table based sine function" default n @@ -98,6 +108,9 @@ config MATH_DECIBELS config MATH_COMPLEX bool "Operations for complex numbers" default n + select CORDIC_FIXED + select MATH_ATAN2 + select SQRT_FIXED help Select this to enable functions for complex numbers arithmetic such as conversions to/from polar format. diff --git a/src/math/atan2.c b/src/math/atan2.c new file mode 100644 index 000000000000..652f1d5c9cc7 --- /dev/null +++ b/src/math/atan2.c @@ -0,0 +1,114 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Intel Corporation. +// +// Author: Seppo Ingalsuo + +/** + * \file math/atan2.c + * \brief Four-quadrant arctangent function using polynomial approximation + */ + +#include +#include +#include +#include + +/* + * Degree-9 Remez minimax polynomial for atan(z) in first octant, z in [0, 1]: + * + * atan(z) = z * (C0 + z^2 * (C1 + z^2 * (C2 + z^2 * (C3 + z^2 * C4)))) + * + * Coefficients are in Q3.29 format, computed via Remez exchange algorithm + * to minimize the maximum approximation error over [0, 1]. + * Maximum approximation error is ~0.0011 degrees (1.94e-5 radians). + */ +#define SOFM_ATAN2_C0 536890772 /* Q3.29, +1.000036992319 */ +#define SOFM_ATAN2_C1 -178298711 /* Q3.29, -0.332107229582 */ +#define SOFM_ATAN2_C2 99794340 /* Q3.29, +0.185881443393 */ +#define SOFM_ATAN2_C3 -49499604 /* Q3.29, -0.092200197922 */ +#define SOFM_ATAN2_C4 12781063 /* Q3.29, +0.023806584771 */ + +/** + * sofm_atan2_32b() - Compute four-quadrant arctangent of y/x + * @param y Imaginary component in Q1.31 format + * @param x Real component in Q1.31 format + * @return Angle in Q3.29 radians, range [-pi, +pi] + * + * Uses degree-9 Remez minimax polynomial for the core atan(z) computation + * where z = min(|y|,|x|) / max(|y|,|x|) is reduced to [0, 1] range. + * Octant and quadrant corrections extend the result to full [-pi, +pi]. + */ +int32_t sofm_atan2_32b(int32_t y, int32_t x) +{ + int32_t abs_x; + int32_t abs_y; + int32_t num; + int32_t den; + int32_t z; + int32_t z2; + int32_t acc; + int32_t angle; + int swap; + + /* Return zero for origin */ + if (x == 0 && y == 0) + return 0; + + /* Take absolute values, handle INT32_MIN overflow */ + abs_x = (x > 0) ? x : ((x == INT32_MIN) ? INT32_MAX : -x); + abs_y = (y > 0) ? y : ((y == INT32_MIN) ? INT32_MAX : -y); + + /* Reduce to first octant: z = min/max ensures z in [0, 1] */ + if (abs_y <= abs_x) { + num = abs_y; + den = abs_x; + swap = 0; + } else { + num = abs_x; + den = abs_y; + swap = 1; + } + + /* z = num / den in Q1.31, den is always > 0 here */ + z = sat_int32(((int64_t)num << 31) / den); + + /* + * Horner evaluation of degree-9 Remez minimax polynomial: + * atan(z) = z * (C0 + z^2 * (C1 + z^2 * (C2 + z^2 * (C3 + z^2 * C4)))) + * + * z is Q1.31, coefficients are Q3.29. + * Multiplications: Q1.31 * Q3.29 = Q4.60, >> 31 -> Q3.29. + */ + + /* z^2: Q1.31 * Q1.31 = Q2.62, >> 31 -> Q1.31 */ + z2 = (int32_t)(((int64_t)z * z) >> 31); + + /* Innermost: C3 + z^2 * C4 */ + acc = (int32_t)(((int64_t)z2 * SOFM_ATAN2_C4) >> 31) + SOFM_ATAN2_C3; + + /* C2 + z^2 * (C3 + z^2 * C4) */ + acc = (int32_t)(((int64_t)z2 * acc) >> 31) + SOFM_ATAN2_C2; + + /* C1 + z^2 * (C2 + z^2 * (C3 + z^2 * C4)) */ + acc = (int32_t)(((int64_t)z2 * acc) >> 31) + SOFM_ATAN2_C1; + + /* C0 + z^2 * (C1 + z^2 * (C2 + z^2 * (C3 + z^2 * C4))) */ + acc = (int32_t)(((int64_t)z2 * acc) >> 31) + SOFM_ATAN2_C0; + + /* angle = z * acc: Q1.31 * Q3.29 = Q4.60, >> 31 -> Q3.29 */ + angle = (int32_t)(((int64_t)z * acc) >> 31); + + /* If |y| > |x|, use identity: atan(y/x) = pi/2 - atan(x/y) */ + if (swap) + angle = PI_DIV2_Q3_29 - angle; + + /* Map to correct quadrant based on signs of x and y */ + if (x < 0) + angle = (y >= 0) ? PI_Q3_29 - angle : -(PI_Q3_29 - angle); + else if (y < 0) + angle = -angle; + + return angle; +} +EXPORT_SYMBOL(sofm_atan2_32b); diff --git a/src/math/complex.c b/src/math/complex.c index cdc5f7fd1d58..ac2a59e91498 100644 --- a/src/math/complex.c +++ b/src/math/complex.c @@ -9,6 +9,7 @@ #include #include #include +#include #include /* sofm_icomplex32_to_polar() - Convert (re, im) complex number to polar. */ @@ -17,8 +18,6 @@ void sofm_icomplex32_to_polar(struct icomplex32 *complex, struct ipolar32 *polar struct icomplex32 c = *complex; int64_t squares_sum; int32_t sqrt_arg; - int32_t acos_arg; - int32_t acos_val; /* Calculate square of magnitudes Q1.31, result is Q2.62 */ squares_sum = (int64_t)c.real * c.real + (int64_t)c.imag * c.imag; @@ -27,16 +26,8 @@ void sofm_icomplex32_to_polar(struct icomplex32 *complex, struct ipolar32 *polar sqrt_arg = Q_SHIFT_RND(squares_sum, 62, 30); polar->magnitude = sofm_sqrt_int32(sqrt_arg); /* Q2.30 */ - /* Avoid divide by zero and ambiguous angle for a zero vector. */ - if (polar->magnitude == 0) { - polar->angle = 0; - return; - } - - /* Calculate phase angle with acos( complex->real / polar->magnitude) */ - acos_arg = sat_int32((((int64_t)c.real) << 29) / polar->magnitude); /* Q2.30 */ - acos_val = acos_fixed_32b(acos_arg); /* Q3.29 */ - polar->angle = (c.imag < 0) ? -acos_val : acos_val; + /* Angle */ + polar->angle = sofm_atan2_32b(c.imag, c.real); /* Q3.29 */ } EXPORT_SYMBOL(sofm_icomplex32_to_polar); diff --git a/test/ztest/unit/math/basic/complex/CMakeLists.txt b/test/ztest/unit/math/basic/complex/CMakeLists.txt index 8a31ce60d33a..db756e341b5b 100644 --- a/test/ztest/unit/math/basic/complex/CMakeLists.txt +++ b/test/ztest/unit/math/basic/complex/CMakeLists.txt @@ -22,6 +22,7 @@ target_sources(app PRIVATE ${SOF_ROOT}/src/math/complex.c ${SOF_ROOT}/src/math/sqrt_int32.c ${SOF_ROOT}/src/math/trig.c + ${SOF_ROOT}/src/math/atan2.c ) # Link math library for standard math functions diff --git a/test/ztest/unit/math/basic/complex/test_complex_polar.c b/test/ztest/unit/math/basic/complex/test_complex_polar.c index bf394aaf3615..22d5e1d3471b 100644 --- a/test/ztest/unit/math/basic/complex/test_complex_polar.c +++ b/test/ztest/unit/math/basic/complex/test_complex_polar.c @@ -15,7 +15,7 @@ LOG_MODULE_REGISTER(test_complex_polar, LOG_LEVEL_INF); #define COMPLEX_ABS_TOL 1.2e-8 #define MAGNITUDE_ABS_TOL 7.1e-8 -#define ANGLE_ABS_TOL 4.4e-5 +#define ANGLE_ABS_TOL 2.0e-5 /** * @brief Test complex to polar conversion function @@ -35,7 +35,7 @@ ZTEST(math_complex, test_icomplex32_to_polar) double magnitude, angle; double delta_mag, delta_ang; double magnitude_scale_q30 = 1.0 / 1073741824.0; /* 1.0 / 2^30 */ - double angle_scale_q29 = 1.0 / 536870912.0; /* 1.0 / 2^29 */ + double angle_scale_q29 = 1.0 / 536870912.0; /* 1.0 / 2^29 */ double delta_mag_max = 0; double delta_ang_max = 0; int i; @@ -118,11 +118,11 @@ ZTEST(math_complex, test_ipolar32_to_complex) /* Re-run worst cases to print info */ sofm_ipolar32_to_complex(&polar_real_max, &complex); - printf("delta_real_max = %g at (%d, %d) -> (%d, %d)\n", delta_real_max, + printf(" INFO - delta_real_max = %g at (%d, %d) -> (%d, %d)\n", delta_real_max, polar_real_max.magnitude, polar_real_max.angle, complex.real, complex.imag); sofm_ipolar32_to_complex(&polar_imag_max, &complex); - printf("delta_imag_max = %g at (%d, %d) -> (%d, %d)\n", delta_imag_max, + printf(" INFO - delta_imag_max = %g at (%d, %d) -> (%d, %d)\n", delta_imag_max, polar_imag_max.magnitude, polar_imag_max.angle, complex.real, complex.imag); } diff --git a/test/ztest/unit/math/basic/trigonometry/CMakeLists.txt b/test/ztest/unit/math/basic/trigonometry/CMakeLists.txt index 90b636fa940c..48d009abbee8 100644 --- a/test/ztest/unit/math/basic/trigonometry/CMakeLists.txt +++ b/test/ztest/unit/math/basic/trigonometry/CMakeLists.txt @@ -21,6 +21,7 @@ target_sources(app PRIVATE trig_test.c ${SOF_ROOT}/src/math/trig.c ${SOF_ROOT}/src/math/lut_trig.c + ${SOF_ROOT}/src/math/atan2.c ) # Link math library for standard math functions diff --git a/test/ztest/unit/math/basic/trigonometry/atan2_tables.h b/test/ztest/unit/math/basic/trigonometry/atan2_tables.h new file mode 100644 index 000000000000..f7ec1e6587fa --- /dev/null +++ b/test/ztest/unit/math/basic/trigonometry/atan2_tables.h @@ -0,0 +1,151 @@ +/* SPDX-License-Identifier: BSD-3-Clause + * + * Copyright(c) 2026 Intel Corporation. + */ + +#ifndef __ATAN2_TABLES_H__ +#define __ATAN2_TABLES_H__ + +#include + +#define ATAN2_TEST_TABLE_SIZE 256 + +static const int32_t atan2_test_y[ATAN2_TEST_TABLE_SIZE] = { + 0, 593678131, -532114458, 826669466, 85578803, 2092045542, + -1879009088, -1576694240, 1539058893, 1401024205, -1764925184, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, -1002953741, 562543872, 1322188032, + -1091606067, -400921101, 2147483647, -1690120538, 2050372122, -350646912, + -1880522864, 320130202, 1583472563, 1870898586, 1777558605, 1275437645, + 2147483647, 645138560, 1016824371, 1133248230, -1409419475, -65799642, + 751321472, -554752, -481011942, -1824835690, 1120701542, 1219868109, + 1276479565, -1402484230, 1094266906, 345378458, 1388947226, 1235927757, + -67347315, -113975488, 352132070, 646445466, -175333594, 1313648794, + 498544640, 689375667, 1101318170, -1891416384, 973726336, 260722458, + 2049027200, 1619159731, 659320781, 811575142, 2062582016, 1804117709, + -556148173, 2147483647, -1581181395, -1384873248, -2147483648, 1919463322, + -715849728, 1482341555, -2106111198, 2147483647, -1581934605, -2147483648, + 2147483647, -1977711542, -1219287520, -1913594461, -1213455936, 1750786893, + -420132698, -222367974, -435172250, -2087889971, -919525235, 834603827, + -1669661664, 2147483647, 1584016051, 2082185882, 212862848, 1926648627, + -1889370525, -1133371149, 2147483647, -125891533, 629086234, -580819290, + -814994048, -940839821, -2147483648, -11732723, -499195981, 2003249459, + -1797551043, 1543991962, 703381043, -200345024, 1618129075, 1901467674, + 2147483647, 747880038, 2017725389, 1628990106, -2147483648, 2147483647, + -451489280, 486361498, -2132152282, -58571955, 157509299, -834756314, + 1251017293, 1892762010, -1455901638, 90604237, -957637261, -2147483648, + 752198656, -1829826099, -1725792499, -569240883, 42635648, -1498449850, + -437911232, -23190746, -661963034, -324850381, -2134133197, 297851418, + 980486426, -357129907, 1688064717, -819664806, 1316280909, 13885414, + -1496825299, 18689510, 1180361037, -324324774, 370182912, 2146612582, + -652058458, -259263629, 277880346, -1507396774, -1420640883, 1603635123, + 1512286899, 925569638, 64723789, 2101077862, 2046962227, -1243819526, + 1021511040, -1659139469, 1906461286, 1168691814, 1900425472, 195734528, + -689034931, 510480538, -1956886624, 965214694, -1640061773, -2146964829, + -1588224, -1125154624, 34522470, 1535756570, 692508749, 292941158, + -143428173, 715886899, 1614379853, 33759053, -1082890266, 610783923, + 1470741325, 1262575846, -53151718, 2033863603, 844302131, 19598797, + 654380390, -411747354, -1558739706, -1846029434, 1652960461, -2147483648, + -587736512, -595937408, 1554227840, 562723533, 1592959667, -46749261, + 1489178803, 582155622, -2126878934, -1168122701, -2147483648, 1081324570, + 1864094003, 1138184397, -547145984, 772395853, -41048269, 1008931123, + -844305651, 1003180570, 2147483647, 2147483647, -715760602, -2035354781, + -1371158131, 1678904832, 494394701, -1407855398, 440926182, 2082209818, + -1291104390, 1887003290, 744169114, -1480452019, 2087889126, 23236224, + -1745024160, -72790784, 523330227, -115015155, 1655730278, 923114086, + 880698086, 1312830746, 132706534, -978800064, +}; + +static const int32_t atan2_test_x[ATAN2_TEST_TABLE_SIZE] = { + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, -1399816141, + -230097050, -1261376090, -2081157232, -440471258, -876176998, -1642130829, + -171532698, 104774067, -928553894, 803256115, 1159264128, -451441549, + 1477543373, -1407978810, -155422643, -1755391194, 1797471949, -2131587568, + -251177766, -2147483648, -868828787, 1321866445, -550897318, -288342067, + 158300877, 370411853, 1242460877, 560164634, 560658278, 1170291584, + -1531339462, 1861627750, 881152307, -76744870, 1266329293, -808628198, + 779498650, -1042297485, 29521254, -2147483648, -1831276832, 315334554, + 1677842637, -314975091, 1873312179, 1370099174, -853027507, 1308034816, + 1094235366, 2147483647, 268095872, 2147483647, -834663949, 838375578, + -574429504, -707173350, 1876573952, 874547379, 1613107021, -2076715221, + 1954203187, -1230503296, 1975324314, 1574426445, 150468173, -2147483648, + -2045228592, 102846515, -1659941536, 2147483647, 523171686, -919468352, + 1801804365, 25905792, -2147483648, 2100982400, -2147483648, -592549478, + 774778470, -794157056, -687915571, 1915112320, -1895780832, -651402752, + -1753523904, -2127834438, -1292520768, -72287987, 471997645, -1600389824, + 1050780826, 909749350, 1526121907, -480663181, 1690915917, 152633958, + 1151843968, -1504568947, -2135539402, 799874662, 1630831770, -643009382, + -1334604269, -2147483648, -1856544518, 1010088781, 1164046259, 914998093, + 2147483647, 560258406, 1061672550, 1741415526, 730181760, -1559917498, + 1348011034, 280000230, 247261696, 1805741133, -998184000, 2003341542, + 182103398, -343834726, -665013325, 681587456, -965692570, 773658266, + -854945766, -1014991155, -663706278, 200810086, -2147483648, 1065857690, + -1146915827, -2147483648, 1094864461, 1700262502, -1050449382, -713241830, + -1989792816, -1679438253, -2144952374, 1737513395, -76085786, -999872192, + 1268036915, 1857466829, 1334412570, 351298253, 1767924506, -206440256, + -2147483648, -1185212653, 2030329805, 2147483647, 1363327539, 848855885, + 1660833434, 1357533338, -1650400646, 991000525, 2106705357, 2147483647, + -1964598170, 1936573338, 799362995, 406677427, -395381466, 2147483647, + -1469184218, 204290944, 1538024576, 2093231642, 2147483647, -1031910246, + -2147483648, 2147483647, -172979558, 1774660941, 795639117, 1787846861, + 1848797773, 282311603, -379224384, -889636070, -1874511760, -1696152198, + -96912922, 1257014528, 1355642675, -28044262, -1442693542, -2147483648, + 586790195, 170231987, -474024038, 1662995277, -139357645, -447027750, + -774900826, 2147483647, 1843413581, 1234652954, -991492621, -2147483648, + -272780570, -1956468659, 1327163059, 1548914611, 1038220902, 2147483647, + -1708905722, 550796928, -1898930634, 1387736064, -1010688026, -1955962237, + 2147483647, 1687800013, -538678554, -2051015613, 1152012083, -1856525088, + -784337101, 361366016, 1829781043, -926315456, 584568653, -421916493, + 1094186931, 1752872986, -1590665542, -714551270, 1503289216, -2001796262, + -554735245, -1757686726, -904190426, -2083870624, -561217114, -885333222, + -314317274, 1836398643, -1568309389, 2094263142, +}; + +static const int32_t atan2_test_ref[ATAN2_TEST_TABLE_SIZE] = { + 0, 843314857, -843314857, 843314857, 843314857, 843314857, + -843314857, -843314857, 843314857, 843314857, -843314857, 1686629713, + 1686629713, 1686629713, 1686629713, 1686629713, 1686629713, 1686629713, + 1686629713, 0, 1686629713, -480774686, 242550644, 1019958505, + -341607303, -1537697774, 882102887, -1275141377, 456892678, -1599098184, + -914601725, 1607182223, 1112734724, 513084281, 1004661107, 962680342, + 803811086, 563496816, 368216934, 596849638, -640053917, -30153818, + 1441750136, -159984, -268264737, -865880079, 388944604, 1157587081, + 548989894, -1186440684, 828834606, 1601018220, 1338258195, 709198926, + -21538024, -1500229594, 99753206, 236681525, -1577795578, 422807062, + 229517388, 166764936, 715116877, -387665176, 1223768840, 161868893, + 990055806, 1064389455, 181392893, 401616028, 486983396, 1302621340, + -148853162, 1122663950, -362401539, -387315937, -805759192, 1295041388, + -1505874184, 806125749, -1201663508, 421657428, -671841804, -1060503213, + 468530021, -836282855, -1409396260, -396616142, -1410500034, 1018520658, + -266761868, -1540056775, -1383811666, -444815486, -1444175423, 1199116908, + -1278122081, 1262504863, 1210753289, 861946112, 227456622, 1215451470, + -570823003, -480185572, 511613552, -1549105769, 191218374, -705349581, + -330593881, -1386609757, -1263475094, -7874366, -159473626, 1010064033, + -1186190715, 1351971671, 1492198979, -105120786, 508515064, 602528322, + 421657428, 498137329, 583272798, 403755854, -667353222, 1180588320, + -173509607, 562873277, -781331817, -17408120, 1602606516, -211958872, + 765710697, 939789547, -1073348055, 70950927, -1267220800, -657671271, + 1299248585, -1115207171, -1040423529, -661241101, 1675972201, -511381404, + -1490815052, -1680832252, -291950578, -101352536, -1088879133, 1474249986, + 1440830194, -1574140544, 1328665659, -236647292, 874313438, 1679174565, + -465981743, 5401720, 388810509, -400234846, 110813576, 894787567, + -1528364782, -1571011137, 73024931, -328578059, -432708387, 581946141, + 396542676, 321267954, 1665586022, 606704001, 413936026, -281848594, + 1429206964, -380316068, 630163376, 663532294, 953439316, 48798797, + -1451193270, 638942546, -485695493, 231958476, -350157942, -1083853093, + -1686232657, -259102718, 1580872968, 382979880, 384511044, 87192252, + -41566736, 641651025, 967182272, 1666266823, -1405387221, 1501063583, + 878640321, 422842424, -21038732, 850717120, 1402364287, 1681730150, + 450864858, -632841657, -1001810229, -449635784, 888470538, -953498287, + -1338255336, -145327507, 376072239, 229590563, 1142214026, -1674944244, + 940578011, 1531360468, -543807078, -346902051, -601547020, 250433321, + 1241668689, 601343996, -1536018589, 272669402, -1664837116, 1430957794, + -201109884, 287908177, 975262018, 1252638885, -298470119, -1240320708, + -1122270737, 729495693, 141675968, -1155748018, 346943281, 950647349, + -465878468, 441432301, 1451702422, -1084733681, 508293499, 1680398164, + -1008559815, -1664409066, 1404948145, -1657028209, 1018764798, 1253757970, + 1027356695, 333210160, 1641308961, -234723315, +}; + +#endif /* __ATAN2_TABLES_H__ */ diff --git a/test/ztest/unit/math/basic/trigonometry/atan2_tables.m b/test/ztest/unit/math/basic/trigonometry/atan2_tables.m new file mode 100644 index 000000000000..37728a20e0d7 --- /dev/null +++ b/test/ztest/unit/math/basic/trigonometry/atan2_tables.m @@ -0,0 +1,95 @@ +% SPDX-License-Identifier: BSD-3-Clause +% +% Copyright(c) 2026 Intel Corporation. + +function atan2_tables() + + % Create random test points with bias to min/max values + q31_scale = 2^31; + q29_scale = 2^29; + num_points = 256; + rand('seed', 42); + x = max(min(2.2 * rand(num_points, 1) - 1.1, 1), -1); + y = max(min(2.2 * rand(num_points, 1) - 1.1, 1), -1); + + % Force 1st test point to be (0, 0) to ensure it is tested + x(1) = 0; + y(1) = 0; + + % Then force 10 test points to have x = 0, the reference values are 0 or pi + x(2:11) = 0; + + % And force 10 test points to have y = 0, the reference values are +/-pi + y(12:21) = 0; + + % Quantize input values + ref_x = int32(x * q31_scale); + ref_y = int32(y * q31_scale); + x = double(ref_x)/q31_scale; + y = double(ref_y)/q31_scale; + a = atan2(y, x); + ref_a = int32(a * q29_scale); + + figure(1) + subplot(2,1,1) + plot(x, y, 'o'); + grid on; + subplot(2,1,2) + plot(a, 'o'); + grid on; + + fn = 'atan2_tables.h'; + fh = export_headerfile(fn); + dn = 'ATAN2_TEST_TABLE_SIZE'; + vl = 6; + fu = export_ifndef(fh, fn); + export_define(fh, dn, num_points); + export_array(fh, 'atan2_test_y', dn, vl, ref_y); + export_array(fh, 'atan2_test_x', dn, vl, ref_x); + export_array(fh, 'atan2_test_ref', dn, vl, ref_a); + export_fclose(fh, fu); + +end + +function [fh] = export_headerfile(headerfn) + fh = fopen(headerfn, 'w'); + fprintf(fh, '/* SPDX-License-Identifier: BSD-3-Clause\n'); + fprintf(fh, ' *\n'); + fprintf(fh, ' * Copyright(c) %s Intel Corporation.\n', ... + datestr(now, 'yyyy')); + fprintf(fh, ' */\n\n'); +end + +function fu = export_ifndef(fh, fn) + fu = sprintf('__%s__', upper(strrep(fn, '.', '_'))); + fprintf(fh, '#ifndef %s\n', fu); + fprintf(fh, '#define %s\n\n', fu); + fprintf(fh, '#include \n\n'); +end + +function export_define(fh, dn, val) + fprintf(fh, '#define %s %d\n', dn, val); +end + +function export_array(fh, vn, size_str, vl, data) + fprintf(fh, '\n'); + fprintf(fh, 'static const int32_t %s[%s] = {\n', vn, size_str); + + n = length(data); + k = 0; + for i = 1:vl:n + fprintf(fh, '\t'); + for j = 1:min(vl, n-k) + k = k + 1; + fprintf(fh, '%12d,', data(k)); + end + fprintf(fh, '\n'); + end + + fprintf(fh, '};\n'); +end + +function export_fclose(fh, fu) + fprintf(fh, "\n#endif /* %s */\n", fu); + fclose(fh); +end diff --git a/test/ztest/unit/math/basic/trigonometry/trig_test.c b/test/ztest/unit/math/basic/trigonometry/trig_test.c index 4c033cc5f54b..0e9471712891 100644 --- a/test/ztest/unit/math/basic/trigonometry/trig_test.c +++ b/test/ztest/unit/math/basic/trigonometry/trig_test.c @@ -31,6 +31,7 @@ #include #include #include "trig_tables.h" +#include "atan2_tables.h" /* Define M_PI if not available */ #ifndef M_PI @@ -47,6 +48,7 @@ #define CMP_TOLERANCE_ASIN_16B 0.0001152158f #define CMP_TOLERANCE_ACOS_16B 0.0001196862f #define CMP_TOLERANCE_SIN 3.1e-5f +#define CMP_TOLERANCE_ATAN2 2.0e-5 /* ~0.001 degrees in radians */ /* * Helper function for rounding double values to nearest integer @@ -230,4 +232,38 @@ ZTEST(trigonometry, test_sin_lut_16b_fixed) } } +/* Test sofm_atan2_32b function */ +ZTEST(trigonometry, test_atan2) +{ + double reference; + double result; + double delta; + double delta_max = 0.0; + int32_t result_q29_max = 0; + int32_t result_q29; + int i_max = 0; + int i; + + /* Note that the first atan2_test_y[], atan2_test_x[] have forced to + * zero y or x, values to test edge cases where x == 0 or y == 0. See + * script atan2_tables.m for details. + */ + for (i = 0; i < ATAN2_TEST_TABLE_SIZE; ++i) { + result_q29 = sofm_atan2_32b(atan2_test_y[i], atan2_test_x[i]); + result = Q_CONVERT_QTOD(result_q29, 29); + reference = Q_CONVERT_QTOD(atan2_test_ref[i], 29); + delta = fabs(reference - result); + if (i == 0 || delta > delta_max) { + result_q29_max = result_q29; + delta_max = delta; + i_max = i; + } + zassert_true(delta <= CMP_TOLERANCE_ATAN2, + "sofm_atan2_32b failed for input %d: (%d, %d) result %d, expected %d", + i, atan2_test_y[i], atan2_test_x[i], result_q29, atan2_test_ref[i]); + } + printf(" INFO - Maximum delta for atan2 test %d: %.6e (%d, %d) result %d\n", + i_max, delta_max, atan2_test_y[i_max], atan2_test_x[i_max], result_q29_max); +} + ZTEST_SUITE(trigonometry, NULL, NULL, NULL, NULL, NULL);