From c23cec9776bd23638e316306906b644c29f1afdb Mon Sep 17 00:00:00 2001 From: Benjamin Valentin Date: Fri, 10 Mar 2023 13:27:36 +0100 Subject: [PATCH 1/4] sys/imath: add integer math module --- sys/include/imath.h | 101 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 101 insertions(+) create mode 100644 sys/include/imath.h diff --git a/sys/include/imath.h b/sys/include/imath.h new file mode 100644 index 0000000000..9e9de170e2 --- /dev/null +++ b/sys/include/imath.h @@ -0,0 +1,101 @@ +/* + * 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 ISIN_PERIOD 0x8000 /**< Period of the isin() function */ +#define ISIN_MAX 0x1000 /**< Max value of the isin() function */ +#define ISIN_MIN -0x1000 /**< Min value of the isin() function */ + +/** + * @brief Internal isin/icos 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 isin(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 icos(int32_t x) +{ + static const int32_t qN = 13; + + int32_t c = (x + (ISIN_PERIOD >> 2)) << (30 - qN); + int32_t y = _ihelp(x); + + return c >= 0 ? y :-y; +} + +#ifdef __cplusplus +} +#endif + +#endif /* IMATH_H */ +/** @} */ From 69a6b3cb1418e4df14d3eb24e6d3142edc02c0ed Mon Sep 17 00:00:00 2001 From: Benjamin Valentin Date: Fri, 10 Mar 2023 13:27:50 +0100 Subject: [PATCH 2/4] tests/driver_dac_dds: make use of imath --- tests/drivers/dac_dds/main.c | 41 +++++------------------------------- 1 file changed, 5 insertions(+), 36 deletions(-) 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; } From eefa1b86b56243be42f18f9ea5e6c2d96861e1fc Mon Sep 17 00:00:00 2001 From: Benjamin Valentin Date: Wed, 22 Mar 2023 14:08:52 +0100 Subject: [PATCH 3/4] imath: add sqrti() function --- sys/include/imath.h | 38 +++++++++++++++++++++++++++++++------- 1 file changed, 31 insertions(+), 7 deletions(-) diff --git a/sys/include/imath.h b/sys/include/imath.h index 9e9de170e2..b4175280dc 100644 --- a/sys/include/imath.h +++ b/sys/include/imath.h @@ -27,12 +27,12 @@ extern "C" { #endif -#define ISIN_PERIOD 0x8000 /**< Period of the isin() function */ -#define ISIN_MAX 0x1000 /**< Max value of the isin() function */ -#define ISIN_MIN -0x1000 /**< Min value of the isin() function */ +#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 isin/icos helper function + * @brief Internal fast_sini/fast_cosi helper function * @internal * * @param[in] x Input parameter @@ -66,7 +66,7 @@ int32_t _ihelp(int32_t x) * @param x angle (with 2^15 units/circle) * @return sine value (-2^12 ≤ y ≤ 2^12) */ -static inline int32_t isin(int32_t x) +static inline int32_t fast_sini(int32_t x) { static const int32_t qN = 13; @@ -83,16 +83,40 @@ static inline int32_t isin(int32_t x) * @param x angle (with 2^15 units/circle) * @return sine value (-2^12 ≤ y ≤ 2^12) */ -static inline int32_t icos(int32_t x) +static inline int32_t fast_cosi(int32_t x) { static const int32_t qN = 13; - int32_t c = (x + (ISIN_PERIOD >> 2)) << (30 - qN); + 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; +} + #ifdef __cplusplus } #endif From 34d002754b3ca78eae7c67b57d761cba5ee8da9d Mon Sep 17 00:00:00 2001 From: Benjamin Valentin Date: Wed, 22 Mar 2023 14:28:11 +0100 Subject: [PATCH 4/4] imath: add powi() function --- sys/include/imath.h | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/sys/include/imath.h b/sys/include/imath.h index b4175280dc..1f2ff5fcb1 100644 --- a/sys/include/imath.h +++ b/sys/include/imath.h @@ -117,6 +117,25 @@ static inline unsigned sqrti(unsigned x) 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