diff --git a/sys/include/imath.h b/sys/include/imath.h new file mode 100644 index 0000000000..1f2ff5fcb1 --- /dev/null +++ b/sys/include/imath.h @@ -0,0 +1,144 @@ +/* + * Copyright (C) 2023 ML!PA Consulting GmbH + * + * This file is subject to the terms and conditions of the GNU Lesser + * General Public License v2.1. See the file LICENSE in the top level + * directory for more details. + */ + +/** + * @defgroup sys_imath imath: Integer Math functions + * @ingroup sys + * + * @brief This modules provides some integer-only math functions. + * They can be used when no FPU is available or no exact + * precision is needed. + * + * @author Karl Fessel + * @{ + */ + +#ifndef IMATH_H +#define IMATH_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#define SINI_PERIOD 0x8000 /**< Period of the fast_sini() function */ +#define SINI_MAX 0x1000 /**< Max value of the fast_sini() function */ +#define SINI_MIN -0x1000 /**< Min value of the fast_sini() function */ + +/** + * @brief Internal fast_sini/fast_cosi helper function + * @internal + * + * @param[in] x Input parameter + * + * @return absolute value of cosine + */ +static inline __attribute__((always_inline)) +int32_t _ihelp(int32_t x) +{ + int32_t y; + static const int32_t qN = 13, + qA = 12, + B = 19900, + C = 3516; + + x = x << (31 - qN); /* Mask with PI */ + x = x >> (31 - qN); /* Note: SIGNED shift! (to qN) */ + x = x * x >> (2 * qN - 14); /* x=x^2 To Q14 */ + + y = B - (x * C >> 14); /* B - x^2*C */ + y = (1 << qA) /* A - x^2*(B-x^2*C) */ + - (x * y >> 16); + + return y; +} + +/** + * @brief A sine approximation via a fourth-order cosine approx. + * source: https://www.coranac.com/2009/07/sines/ + * + * @param x angle (with 2^15 units/circle) + * @return sine value (-2^12 ≤ y ≤ 2^12) + */ +static inline int32_t fast_sini(int32_t x) +{ + static const int32_t qN = 13; + + int32_t c = x << (30 - qN); + int32_t y = _ihelp(x - (1 << qN)); + + return c >= 0 ? y :-y; +} + +/** + * @brief A a fourth-order cosine approx. + * source: https://www.coranac.com/2009/07/sines/ + * + * @param x angle (with 2^15 units/circle) + * @return sine value (-2^12 ≤ y ≤ 2^12) + */ +static inline int32_t fast_cosi(int32_t x) +{ + static const int32_t qN = 13; + + int32_t c = (x + (SINI_PERIOD >> 2)) << (30 - qN); + int32_t y = _ihelp(x); + + return c >= 0 ? y :-y; +} + +/** + * @brief Square root of an integer + * + * @param x unsigned integer value + * @return square root of @p x + */ +static inline unsigned sqrti(unsigned x) +{ + if (x <= 1) { + return x; + } + + /* initial estimate */ + unsigned y0 = x >> 1; + unsigned y1 = (y0 + x / y0) >> 1; + + while (y1 < y0) { + y0 = y1; + y1 = (y0 + x / y0) >> 1; + } + + return y0; +} + +/** + * @brief Returns the value of x to the power of y + * + * @param x base + * @param y exponent + * + * @return x^y + */ +static inline uint32_t powi(unsigned x, unsigned y) +{ + uint32_t res = 1; + + while (y--) { + res *= x; + } + + return res; +} + +#ifdef __cplusplus +} +#endif + +#endif /* IMATH_H */ +/** @} */ diff --git a/tests/drivers/dac_dds/main.c b/tests/drivers/dac_dds/main.c index ac8adcb1db..f3cc83162f 100644 --- a/tests/drivers/dac_dds/main.c +++ b/tests/drivers/dac_dds/main.c @@ -31,6 +31,7 @@ #include "mutex.h" #include "dac_dds.h" #include "dac_dds_params.h" +#include "imath.h" #include "shell.h" #include "blob/hello.raw.h" @@ -58,38 +59,6 @@ static bool res_16b = 0; static unsigned sample_rate = 8000; -#define ISIN_PERIOD (0x7FFF) -#define ISIN_MAX (0x1000) - -/** - * @brief A sine approximation via a fourth-order cosine approx. - * source: https://www.coranac.com/2009/07/sines/ - * - * @param x angle (with 2^15 units/circle) - * @return sine value (Q12) - */ -static int32_t isin(int32_t x) -{ - int32_t c, y; - static const int32_t qN = 13, - qA = 12, - B = 19900, - C = 3516; - - c = x << (30 - qN); /* Semi-circle info into carry. */ - x -= 1 << qN; /* sine -> cosine calc */ - - x = x << (31 - qN); /* Mask with PI */ - x = x >> (31 - qN); /* Note: SIGNED shift! (to qN) */ - x = x * x >> (2 * qN - 14); /* x=x^2 To Q14 */ - - y = B - (x * C >> 14); /* B - x^2*C */ - y = (1 << qA) /* A - x^2*(B-x^2*C) */ - - (x * y >> 16); - - return c >= 0 ? y : -y; -} - /* simple function to fill buffer with samples * * It is up to the caller to ensure that len is always at least period. @@ -115,11 +84,11 @@ static void _fill_saw_samples_16(uint8_t *buf, size_t len, uint16_t period) static void _fill_sine_samples_8(uint8_t *buf, size_t len, uint16_t period) { uint16_t x = 0; - unsigned step = ISIN_PERIOD / period; + unsigned step = SINI_PERIOD / period; for (uint16_t i = 0; i < period; ++i) { x += step; - buf[i] = (isin(x) + 4096) >> 6; + buf[i] = (fast_sini(x) - SINI_MIN) >> 6; } for (uint16_t i = period; i < len; i += period) { @@ -130,14 +99,14 @@ static void _fill_sine_samples_8(uint8_t *buf, size_t len, uint16_t period) static void _fill_sine_samples_16(uint8_t *buf, size_t len, uint16_t period) { uint16_t x = 0; - unsigned step = ISIN_PERIOD / period; + unsigned step = SINI_PERIOD / period; period *= 2; for (uint16_t i = 0; i < period; ++i) { x += step; - uint16_t y = (isin(x) + 4096) << 2; + uint16_t y = (fast_sini(x) - SINI_MIN) << 2; buf[i] = y; buf[++i] = y >> 8; }