Fork of the Ben Katz's motor controller firmware.
Diff: math_ops.h
- Revision:
- 59:8fb0145aa933
- Parent:
- 48:74a40481740c
diff -r 312f8ec71d74 -r 8fb0145aa933 math_ops.h --- a/math_ops.h Tue Dec 01 06:29:10 2020 +0000 +++ b/math_ops.h Fri Jan 22 12:44:34 2021 +0000 @@ -13,7 +13,61 @@ float roundf(float x); void limit_norm(float *x, float *y, float limit); void limit(float *x, float min, float max); -int float_to_uint(float x, float x_min, float x_max, int bits); -float uint_to_float(int x_int, float x_min, float x_max, int bits); + +inline float my_fmod(float a, float b) { + return (a - b * floor(a / b)); +} + +template<unsigned int bits> +int float_to_uint(float x, const float x_max, const bool symmetric=true) +{ + float span = x_max + symmetric*x_max; + float quantum = span/((1<<bits)-1); + + float offset = 0; + if (symmetric) { + offset = -x_max - quantum/2; + } + + return (int) ((x-offset)/quantum + 0.5); +} + +template<unsigned int bits> +float uint_to_float(int x, const float x_max, const bool symmetric=true) +{ + float span = x_max + symmetric*x_max; + float quantum = span/((1<<bits)-1); + + float offset = 0; + if (symmetric) { + offset = -x_max - quantum/2; + } + + return ((float)x)*quantum + offset; +} + +template<unsigned int bits> +int float_to_uint_symmetric(float x, const float x_lim) +{ + return float_to_uint<bits>(x, x_lim, true); +} + +template<unsigned int bits> +float uint_to_float_symmetric(int x, const float x_lim) +{ + return uint_to_float<bits>(x, x_lim, true); +} + +template<unsigned int bits> +int float_to_uint_positive(float x, const float x_lim) +{ + return float_to_uint<bits>(x, x_lim, false); +} + +template<unsigned int bits> +float uint_to_float_positive(int x, const float x_lim) +{ + return uint_to_float<bits>(x, x_lim, false); +} #endif