From 1ed85d788e1a0be407a0ca52a4e86705ca2d3784 Mon Sep 17 00:00:00 2001 From: Seppo Ingalsuo Date: Mon, 16 Mar 2026 12:08:08 +0200 Subject: [PATCH 1/5] Math: Add function sofm_atan2_32b() to SOF math library This patch adds the four quadrant arctan function version. It is useful for fast computation of phase angle -pi to +pi in radians from a complex number (real, imaginary) value. The approximation uses a 9th order polynomial that is implemented with Horner's rule evaluation. Signed-off-by: Seppo Ingalsuo --- src/include/sof/math/trig.h | 13 ++++ src/math/CMakeLists.txt | 4 ++ src/math/Kconfig | 10 ++++ src/math/atan2.c | 114 ++++++++++++++++++++++++++++++++++++ 4 files changed, 141 insertions(+) create mode 100644 src/math/atan2.c diff --git a/src/include/sof/math/trig.h b/src/include/sof/math/trig.h index 9f04c137a265..036324553640 100644 --- a/src/include/sof/math/trig.h +++ b/src/include/sof/math/trig.h @@ -383,4 +383,17 @@ 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))) + * 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..d691df20a5ec 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 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); From 6b2ad7bbe6541ad409d39384e0cdfd57e37f9511 Mon Sep 17 00:00:00 2001 From: Seppo Ingalsuo Date: Wed, 18 Mar 2026 11:46:09 +0200 Subject: [PATCH 2/5] Audio: Format: Add macro to convert fraction to double The usage of Q_CONVERT_QTOF() causes build issue with llvm toolchain when assigned to a double type in ztest code: error: implicit conversion increases floating-point precision: 'float' to 'double' [-Werror,-Wdouble-promotion] Since a double is needed to fully match the accuracy of 32 bit fraction, a new macro Q_CONVERT_QTOD() is added for the purpose. Signed-off-by: Seppo Ingalsuo --- src/include/sof/audio/format.h | 3 +++ 1 file changed, 3 insertions(+) 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) \ From a50ba54966485ed8a3755cf4e606ada56cd78117 Mon Sep 17 00:00:00 2001 From: Seppo Ingalsuo Date: Mon, 16 Mar 2026 13:41:32 +0200 Subject: [PATCH 3/5] Test: ztest: Add test for function sofm_atan2_32b() This patch adds the test for the function. The input values table for atan are Q1.31 (x, y) pair numbers and reference angle values in Q3.29 radians. The table is computed with the double precision atan2() function in Octave and converted to fractional integers. See script atan2_tables.m. Signed-off-by: Seppo Ingalsuo --- .../math/basic/trigonometry/CMakeLists.txt | 1 + .../math/basic/trigonometry/atan2_tables.h | 151 ++++++++++++++++++ .../math/basic/trigonometry/atan2_tables.m | 82 ++++++++++ .../unit/math/basic/trigonometry/trig_test.c | 32 ++++ 4 files changed, 266 insertions(+) create mode 100644 test/ztest/unit/math/basic/trigonometry/atan2_tables.h create mode 100644 test/ztest/unit/math/basic/trigonometry/atan2_tables.m 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..029aed8b8df7 --- /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] = { + -1383874342, 593678131, -532114458, 826669466, 85578803, 2092045542, + -1879009088, -1576694240, 1539058893, 1401024205, -1764925184, -1902937942, + -326983219, -576722150, -2052493203, -2123326885, -1395301741, 2147483647, + -751773018, 1738382694, 564864819, -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] = { + -888290445, -1872849862, 299813325, -1294156653, -27268877, 2102403072, + -605305254, 1561399347, -1933088326, 854312166, 1554108442, -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] = { + -1149680491, 1521825465, -567845606, 1381450449, 1008923044, 420331713, + -1010627222, -424274086, 1325638429, 549336487, -455712381, -1183812531, + -1172525572, -1456398494, -1268695056, -953127949, -1144347207, 1193799356, + -963751354, 810996204, 1393218951, -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..2c2948ecc347 --- /dev/null +++ b/test/ztest/unit/math/basic/trigonometry/atan2_tables.m @@ -0,0 +1,82 @@ +# SPDX-License-Identifier: BSD-3-Clause +# +# Copyright(c) 2026 Intel Corporation. + +function atan2_tables() + + 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); + 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..2dd099a2df83 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,34 @@ ZTEST(trigonometry, test_sin_lut_16b_fixed) } } +/* Test sofm_atan2 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 = -1; + int i; + + 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 (delta > delta_max) { + result_q29_max = result_q29; + delta_max = delta; + i_max = i; + } + zassert_true(delta <= CMP_TOLERANCE_ATAN2, + "sofm_atan2 failed for input %d: (%d, %d) result %d", + i, atan2_test_y[i], atan2_test_x[i], result_q29); + } + 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); From 0b7c7cb0fa0e03ab781f30bc04e463f041708d0f Mon Sep 17 00:00:00 2001 From: Seppo Ingalsuo Date: Mon, 16 Mar 2026 15:46:04 +0200 Subject: [PATCH 4/5] Math: Complex: Calculate complex phase angle with sofm_atan2() This patch updates the phase angle calculation in function sofm_icomple32_to_polar() with more efficient sofm_atan2() function. Calculate of angle with arctan function avoids the 64 bit divide in the previous arccos function based calculation. In 48 kHz stereo STFT processing with 1024 size FFT and hop of 256 this change saves about 14 MCPS in MTL platform (204 MCPS to 190 MCPS). Signed-off-by: Seppo Ingalsuo --- src/math/Kconfig | 3 +++ src/math/complex.c | 15 +++------------ test/ztest/unit/math/basic/complex/CMakeLists.txt | 1 + 3 files changed, 7 insertions(+), 12 deletions(-) diff --git a/src/math/Kconfig b/src/math/Kconfig index d691df20a5ec..1feaab8f03d1 100644 --- a/src/math/Kconfig +++ b/src/math/Kconfig @@ -108,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/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 From aecc9d3f89242b7ac1cab9dd819b2198cb26e74c Mon Sep 17 00:00:00 2001 From: Seppo Ingalsuo Date: Tue, 17 Mar 2026 18:15:53 +0200 Subject: [PATCH 5/5] Test: ztest: Tighten the tolerance for test_icomplex32_to_polar The atan2() function in sofm_ipolar32_to_complex() helped to increase accuracy, so the tolerance for error could be decreased to about 0.001 degrees (2.0e-5 radians). Also the print format for achieved accuracy is improved for nicer appearance. Signed-off-by: Seppo Ingalsuo --- test/ztest/unit/math/basic/complex/test_complex_polar.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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); }