Fork of the Ben Katz's motor controller firmware.
math_ops.h@60:8399756e1ba1, 2021-01-22 (annotated)
- Committer:
- MartinGurtner
- Date:
- Fri Jan 22 13:10:37 2021 +0000
- Revision:
- 60:8399756e1ba1
- Parent:
- 59:8fb0145aa933
.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
benkatz | 20:bf9ea5125d52 | 1 | #ifndef MATH_OPS_H |
benkatz | 20:bf9ea5125d52 | 2 | #define MATH_OPS_H |
benkatz | 20:bf9ea5125d52 | 3 | |
benkatz | 20:bf9ea5125d52 | 4 | #define PI 3.14159265359f |
benkatz | 48:74a40481740c | 5 | #define SQRT3 1.73205080757f |
benkatz | 20:bf9ea5125d52 | 6 | |
benkatz | 20:bf9ea5125d52 | 7 | #include "math.h" |
benkatz | 20:bf9ea5125d52 | 8 | |
benkatz | 20:bf9ea5125d52 | 9 | float fmaxf(float x, float y); |
benkatz | 20:bf9ea5125d52 | 10 | float fminf(float x, float y); |
benkatz | 20:bf9ea5125d52 | 11 | float fmaxf3(float x, float y, float z); |
benkatz | 20:bf9ea5125d52 | 12 | float fminf3(float x, float y, float z); |
benkatz | 34:51647c6c500d | 13 | float roundf(float x); |
benkatz | 20:bf9ea5125d52 | 14 | void limit_norm(float *x, float *y, float limit); |
benkatz | 47:e1196a851f76 | 15 | void limit(float *x, float min, float max); |
MartinGurtner | 59:8fb0145aa933 | 16 | |
MartinGurtner | 59:8fb0145aa933 | 17 | inline float my_fmod(float a, float b) { |
MartinGurtner | 59:8fb0145aa933 | 18 | return (a - b * floor(a / b)); |
MartinGurtner | 59:8fb0145aa933 | 19 | } |
MartinGurtner | 59:8fb0145aa933 | 20 | |
MartinGurtner | 59:8fb0145aa933 | 21 | template<unsigned int bits> |
MartinGurtner | 59:8fb0145aa933 | 22 | int float_to_uint(float x, const float x_max, const bool symmetric=true) |
MartinGurtner | 59:8fb0145aa933 | 23 | { |
MartinGurtner | 59:8fb0145aa933 | 24 | float span = x_max + symmetric*x_max; |
MartinGurtner | 59:8fb0145aa933 | 25 | float quantum = span/((1<<bits)-1); |
MartinGurtner | 59:8fb0145aa933 | 26 | |
MartinGurtner | 59:8fb0145aa933 | 27 | float offset = 0; |
MartinGurtner | 59:8fb0145aa933 | 28 | if (symmetric) { |
MartinGurtner | 59:8fb0145aa933 | 29 | offset = -x_max - quantum/2; |
MartinGurtner | 59:8fb0145aa933 | 30 | } |
MartinGurtner | 59:8fb0145aa933 | 31 | |
MartinGurtner | 59:8fb0145aa933 | 32 | return (int) ((x-offset)/quantum + 0.5); |
MartinGurtner | 59:8fb0145aa933 | 33 | } |
MartinGurtner | 59:8fb0145aa933 | 34 | |
MartinGurtner | 59:8fb0145aa933 | 35 | template<unsigned int bits> |
MartinGurtner | 59:8fb0145aa933 | 36 | float uint_to_float(int x, const float x_max, const bool symmetric=true) |
MartinGurtner | 59:8fb0145aa933 | 37 | { |
MartinGurtner | 59:8fb0145aa933 | 38 | float span = x_max + symmetric*x_max; |
MartinGurtner | 59:8fb0145aa933 | 39 | float quantum = span/((1<<bits)-1); |
MartinGurtner | 59:8fb0145aa933 | 40 | |
MartinGurtner | 59:8fb0145aa933 | 41 | float offset = 0; |
MartinGurtner | 59:8fb0145aa933 | 42 | if (symmetric) { |
MartinGurtner | 59:8fb0145aa933 | 43 | offset = -x_max - quantum/2; |
MartinGurtner | 59:8fb0145aa933 | 44 | } |
MartinGurtner | 59:8fb0145aa933 | 45 | |
MartinGurtner | 59:8fb0145aa933 | 46 | return ((float)x)*quantum + offset; |
MartinGurtner | 59:8fb0145aa933 | 47 | } |
MartinGurtner | 59:8fb0145aa933 | 48 | |
MartinGurtner | 59:8fb0145aa933 | 49 | template<unsigned int bits> |
MartinGurtner | 59:8fb0145aa933 | 50 | int float_to_uint_symmetric(float x, const float x_lim) |
MartinGurtner | 59:8fb0145aa933 | 51 | { |
MartinGurtner | 59:8fb0145aa933 | 52 | return float_to_uint<bits>(x, x_lim, true); |
MartinGurtner | 59:8fb0145aa933 | 53 | } |
MartinGurtner | 59:8fb0145aa933 | 54 | |
MartinGurtner | 59:8fb0145aa933 | 55 | template<unsigned int bits> |
MartinGurtner | 59:8fb0145aa933 | 56 | float uint_to_float_symmetric(int x, const float x_lim) |
MartinGurtner | 59:8fb0145aa933 | 57 | { |
MartinGurtner | 59:8fb0145aa933 | 58 | return uint_to_float<bits>(x, x_lim, true); |
MartinGurtner | 59:8fb0145aa933 | 59 | } |
MartinGurtner | 59:8fb0145aa933 | 60 | |
MartinGurtner | 59:8fb0145aa933 | 61 | template<unsigned int bits> |
MartinGurtner | 59:8fb0145aa933 | 62 | int float_to_uint_positive(float x, const float x_lim) |
MartinGurtner | 59:8fb0145aa933 | 63 | { |
MartinGurtner | 59:8fb0145aa933 | 64 | return float_to_uint<bits>(x, x_lim, false); |
MartinGurtner | 59:8fb0145aa933 | 65 | } |
MartinGurtner | 59:8fb0145aa933 | 66 | |
MartinGurtner | 59:8fb0145aa933 | 67 | template<unsigned int bits> |
MartinGurtner | 59:8fb0145aa933 | 68 | float uint_to_float_positive(int x, const float x_lim) |
MartinGurtner | 59:8fb0145aa933 | 69 | { |
MartinGurtner | 59:8fb0145aa933 | 70 | return uint_to_float<bits>(x, x_lim, false); |
MartinGurtner | 59:8fb0145aa933 | 71 | } |
benkatz | 20:bf9ea5125d52 | 72 | |
benkatz | 20:bf9ea5125d52 | 73 | #endif |