|
14
|
1 /* Project: Q-Number (Q16.16, signed) library
|
|
|
2 * Author: Richard James Howe
|
|
|
3 * License: The Unlicense
|
|
|
4 * Email: howe.r.j.89@gmail.com
|
|
|
5 * Repo: <https://github.com/q> */
|
|
|
6
|
|
|
7 #ifndef Q_H
|
|
|
8 #define Q_H
|
|
|
9
|
|
|
10 #ifdef __cplusplus
|
|
|
11 extern "C" {
|
|
|
12 #endif
|
|
|
13
|
|
|
14 #include <stdint.h>
|
|
|
15 #include <stddef.h>
|
|
|
16
|
|
|
17 #define QMAX_ID (32)
|
|
|
18 #define QMAX_ERROR (256)
|
|
|
19
|
|
|
20 #define QBITS (12)
|
|
|
21 #define QMASK ((1ULL << QBITS) - 1ULL)
|
|
|
22 #define QHIGH (1ULL << (QBITS - 1ULL))
|
|
|
23
|
|
|
24 #define QMK(QHIGH, LOW, SF) ((ld_t)((((lu_t)QHIGH) << QBITS) | (QMASK & ((((lu_t)LOW) << QBITS) >> (SF)))))
|
|
|
25 #define QINT(INT) ((q_t)((u_t)(INT) << QBITS))
|
|
|
26 #define QPI (QMK(0x3, 0x243F, 16))
|
|
|
27
|
|
|
28 #ifndef PREPACK
|
|
|
29 #define PREPACK
|
|
|
30 #endif
|
|
|
31
|
|
|
32 #ifndef POSTPACK
|
|
|
33 #define POSTPACK
|
|
|
34 #endif
|
|
|
35
|
|
|
36 #ifndef RESTRICT
|
|
|
37 #ifdef __cplusplus
|
|
|
38 #define RESTRICT
|
|
|
39 #else
|
|
|
40 #define RESTRICT restrict
|
|
|
41 #endif
|
|
|
42 #endif
|
|
|
43
|
|
|
44 typedef int32_t q_t; /* Q Fixed Point Number, (Q16.16, Signed) */
|
|
|
45 typedef int64_t ld_t; /* Double width of Q, signed, for internal calculations, not in Q format */
|
|
|
46 typedef int32_t d_t; /* same width as Q, signed, but not in Q format */
|
|
|
47 typedef uint32_t u_t; /* same width as Q, unsigned, but not in Q format */
|
|
|
48
|
|
|
49 typedef PREPACK struct {
|
|
|
50 size_t whole, /* number of bits for whole, or integer, part of Q number */
|
|
|
51 fractional; /* number of bits for fractional part of Q number */
|
|
|
52 const q_t zero, /* the constant '0' */
|
|
|
53 bit, /* smallest 'q' number representable */
|
|
|
54 one, /* the constant '1' */
|
|
|
55 pi, /* the constant 'pi' */
|
|
|
56 e, /* the constant 'e' */
|
|
|
57 sqrt2, /* the square root of 2 */
|
|
|
58 sqrt3, /* the square root of 3 */
|
|
|
59 ln2, /* the natural logarithm of 2 */
|
|
|
60 ln10, /* the natural logarithm of 10 */
|
|
|
61 min, /* most negative 'q' number */
|
|
|
62 max; /* most positive 'q' number */
|
|
|
63 const uint32_t version; /* version in X.Y.Z format (Z = lowest 8 bits) */
|
|
|
64 } POSTPACK qinfo_t;
|
|
|
65
|
|
|
66 typedef PREPACK struct {
|
|
|
67 q_t rc, /* time constant */
|
|
|
68 time, /* time of previous measurement */
|
|
|
69 raw, /* previous raw value */
|
|
|
70 filtered; /* filtered value */
|
|
|
71 } POSTPACK qfilter_t; /* High/Low Pass Filter */
|
|
|
72
|
|
|
73 typedef PREPACK struct {
|
|
|
74 q_t d_gain, d_state; /* differentiator; gain, state */
|
|
|
75 q_t i_gain, i_state, i_min, i_max; /* integrator; gain, state, minimum and maximum */
|
|
|
76 q_t p_gain; /* proportional gain */
|
|
|
77 } POSTPACK qpid_t; /* PID Controller <https://en.wikipedia.org/wiki/PID_controller> */
|
|
|
78
|
|
|
79 struct qexpr;
|
|
|
80 typedef struct qexpr qexpr_t;
|
|
|
81
|
|
|
82 typedef PREPACK struct {
|
|
|
83 char *name;
|
|
|
84 union {
|
|
|
85 q_t (*unary) (q_t a);
|
|
|
86 q_t (*binary) (q_t a1, q_t a2);
|
|
|
87 } eval;
|
|
|
88 union {
|
|
|
89 q_t (*unary) (qexpr_t *e, q_t a);
|
|
|
90 q_t (*binary) (qexpr_t *e, q_t a1, q_t a2);
|
|
|
91 } check;
|
|
|
92 int precedence, arity, assocativity, hidden;
|
|
|
93 } POSTPACK qoperations_t; /* use in the expression evaluator */
|
|
|
94
|
|
|
95 typedef PREPACK struct {
|
|
|
96 char *name;
|
|
|
97 q_t value;
|
|
|
98 } POSTPACK qvariable_t; /* Variable which can be used with the expression evaluator */
|
|
|
99
|
|
|
100 struct PREPACK qexpr {
|
|
|
101 const qoperations_t **ops, *lpar, *rpar, *negate, *minus;
|
|
|
102 qvariable_t **vars;
|
|
|
103 char id[QMAX_ID];
|
|
|
104 char error_string[QMAX_ERROR];
|
|
|
105 q_t number;
|
|
|
106 const qoperations_t *op;
|
|
|
107 q_t *numbers;
|
|
|
108 size_t ops_count, ops_max;
|
|
|
109 size_t numbers_count, numbers_max;
|
|
|
110 size_t id_count;
|
|
|
111 size_t vars_max;
|
|
|
112 int error;
|
|
|
113 int initialized;
|
|
|
114 } POSTPACK; /* An expression evaluator for the Q library */
|
|
|
115
|
|
|
116 typedef q_t (*qbounds_t)(ld_t s);
|
|
|
117
|
|
|
118 q_t qbound_saturate(ld_t s); /* default over/underflow behavior, saturation */
|
|
|
119 q_t qbound_wrap(ld_t s); /* over/underflow behavior, wrap around */
|
|
|
120
|
|
|
121 typedef PREPACK struct {
|
|
|
122 qbounds_t bound; /* handles saturation when a number over or underflows */
|
|
|
123 int dp; /* decimal points to print, negative specifies maximum precision */
|
|
|
124 unsigned base; /* base to use for numeric number conversion */
|
|
|
125 } POSTPACK qconf_t; /* Q format configuration options */
|
|
|
126
|
|
|
127 extern const qinfo_t qinfo; /* information about the format and constants */
|
|
|
128 extern qconf_t qconf; /* @warning GLOBAL Q configuration options */
|
|
|
129
|
|
|
130 int qtoi(q_t toi);
|
|
|
131 q_t qint(int toq);
|
|
|
132 signed char qtoc(const q_t q);
|
|
|
133 q_t qchar(signed char c);
|
|
|
134 short qtoh(const q_t q);
|
|
|
135 q_t qshort(short s);
|
|
|
136 long qtol(const q_t q);
|
|
|
137 q_t qlong(long l);
|
|
|
138 long long qtoll(const q_t q);
|
|
|
139 q_t qvlong(long long ll);
|
|
|
140
|
|
|
141 q_t qisnegative(q_t a);
|
|
|
142 q_t qispositive(q_t a);
|
|
|
143 q_t qisinteger(q_t a);
|
|
|
144 q_t qisodd(q_t a);
|
|
|
145 q_t qiseven(q_t a);
|
|
|
146
|
|
|
147 q_t qless(q_t a, q_t b);
|
|
|
148 q_t qmore(q_t a, q_t b);
|
|
|
149 q_t qeqless(q_t a, q_t b);
|
|
|
150 q_t qeqmore(q_t a, q_t b);
|
|
|
151 q_t qequal(q_t a, q_t b);
|
|
|
152 q_t qunequal(q_t a, q_t b);
|
|
|
153 q_t qapproxequal(q_t a, q_t b, q_t epsilon);
|
|
|
154 q_t qapproxunequal(q_t a, q_t b, q_t epsilon);
|
|
|
155 q_t qwithin(q_t v, q_t b1, q_t b2);
|
|
|
156 q_t qwithin_interval(q_t v, q_t expected, q_t allowance);
|
|
|
157
|
|
|
158 q_t qnegate(q_t a);
|
|
|
159 q_t qmin(q_t a, q_t b);
|
|
|
160 q_t qmax(q_t a, q_t b);
|
|
|
161 q_t qabs(q_t a);
|
|
|
162 q_t qcopysign(q_t a, q_t b);
|
|
|
163 q_t qsign(q_t a);
|
|
|
164 q_t qsignum(q_t a);
|
|
|
165
|
|
|
166 q_t qadd(q_t a, q_t b);
|
|
|
167 q_t qsub(q_t a, q_t b);
|
|
|
168 q_t qmul(q_t a, q_t b);
|
|
|
169 q_t qdiv(q_t a, q_t b);
|
|
|
170 q_t qrem(q_t a, q_t b);
|
|
|
171 q_t qmod(q_t a, q_t b);
|
|
|
172 q_t qfma(q_t a, q_t b, q_t c);
|
|
|
173 q_t qsqr(q_t x);
|
|
|
174 q_t qexp(q_t e);
|
|
|
175 q_t qlog(q_t n);
|
|
|
176 q_t qsqrt(q_t x);
|
|
|
177
|
|
|
178 q_t qround(q_t q);
|
|
|
179 q_t qceil(q_t q);
|
|
|
180 q_t qtrunc(q_t q);
|
|
|
181 q_t qfloor(q_t q);
|
|
|
182
|
|
|
183 q_t qand(q_t a, q_t b);
|
|
|
184 q_t qxor(q_t a, q_t b);
|
|
|
185 q_t qor(q_t a, q_t b);
|
|
|
186 q_t qinvert(q_t a);
|
|
|
187 q_t qnot(q_t a);
|
|
|
188 q_t qlogical(q_t a);
|
|
|
189
|
|
|
190 q_t qlls(q_t a, q_t b);
|
|
|
191 q_t qlrs(q_t a, q_t b);
|
|
|
192 q_t qals(q_t a, q_t b);
|
|
|
193 q_t qars(q_t a, q_t b);
|
|
|
194
|
|
|
195 q_t qpow(q_t n, q_t exp);
|
|
|
196
|
|
|
197 int qsprint(q_t p, char *s, size_t length);
|
|
|
198 int qsprintb(q_t p, char *s, size_t length, u_t base);
|
|
|
199 int qsprintbdp(q_t p, char *s, size_t length, u_t base, d_t idp);
|
|
|
200 int qnconv(q_t *q, const char *s, size_t length);
|
|
|
201 int qnconvb(q_t *q, const char *s, size_t length, d_t base);
|
|
|
202 int qconv(q_t *q, const char *s);
|
|
|
203 int qconvb(q_t *q, const char * const s, d_t base);
|
|
|
204 int qnconvbdp(q_t *q, const char *s, size_t length, d_t base, u_t idp);
|
|
|
205
|
|
|
206 void qsincos(q_t theta, q_t *sine, q_t *cosine);
|
|
|
207 q_t qsin(q_t theta);
|
|
|
208 q_t qcos(q_t theta);
|
|
|
209 q_t qtan(q_t theta);
|
|
|
210 q_t qcot(q_t theta);
|
|
|
211 q_t qhypot(q_t a, q_t b);
|
|
|
212
|
|
|
213 q_t qatan(q_t t);
|
|
|
214 q_t qatan2(q_t x, q_t y);
|
|
|
215 q_t qasin(q_t t);
|
|
|
216 q_t qacos(q_t t);
|
|
|
217 q_t qsinh(q_t a);
|
|
|
218 q_t qcosh(q_t a);
|
|
|
219 q_t qtanh(q_t a);
|
|
|
220
|
|
|
221 q_t qatanh(q_t t);
|
|
|
222 q_t qasinh(q_t t);
|
|
|
223 q_t qacosh(q_t t);
|
|
|
224
|
|
|
225 void qsincosh(q_t a, q_t *sinh, q_t *cosh);
|
|
|
226
|
|
|
227 q_t qcordic_ln(q_t d); /* CORDIC testing only */
|
|
|
228 q_t qcordic_exp(q_t e); /* CORDIC testing only; useless for large values */
|
|
|
229 q_t qcordic_sqrt(q_t a); /* CORDIC testing only; do not use, a <= 2, a >= 0 */
|
|
|
230 q_t qcordic_mul(q_t a, q_t b); /* CORDIC testing only; do not use */
|
|
|
231 q_t qcordic_div(q_t a, q_t b); /* CORDIC testing only; do not use */
|
|
|
232 q_t qcordic_circular_gain(int n);
|
|
|
233 q_t qcordic_hyperbolic_gain(int n);
|
|
|
234
|
|
|
235 void qpol2rec(q_t magnitude, q_t theta, q_t *i, q_t *j);
|
|
|
236 void qrec2pol(q_t i, q_t j, q_t *magnitude, q_t *theta);
|
|
|
237
|
|
|
238 q_t qrad2deg(q_t rad);
|
|
|
239 q_t qdeg2rad(q_t deg);
|
|
|
240
|
|
|
241 d_t dpower(d_t b, unsigned e);
|
|
|
242 d_t dlog(d_t n, unsigned base);
|
|
|
243 d_t arshift(d_t v, unsigned p);
|
|
|
244 int qpack(const q_t *q, char *buffer, size_t length);
|
|
|
245 int qunpack(q_t *q, const char *buffer, size_t length);
|
|
|
246
|
|
|
247 q_t qsimpson(q_t (*f)(q_t), q_t x1, q_t x2, unsigned n); /* numerical integrator of f, between x1, x2, for n steps */
|
|
|
248
|
|
|
249 void qfilter_init(qfilter_t *f, q_t time, q_t rc, q_t seed);
|
|
|
250 q_t qfilter_low_pass(qfilter_t *f, q_t time, q_t data);
|
|
|
251 q_t qfilter_high_pass(qfilter_t *f, q_t time, q_t data);
|
|
|
252 q_t qfilter_value(const qfilter_t *f);
|
|
|
253
|
|
|
254 q_t qpid_update(qpid_t *pid, const q_t error, const q_t position);
|
|
|
255
|
|
|
256 /* A matrix consists of at least four elements, a meta data field,
|
|
|
257 * the length of the array (which must be big enough to store
|
|
|
258 * row*column, but may be * larger) and a row and a column count
|
|
|
259 * in unsigned integer format, and the array elements in Q format.
|
|
|
260 * This simplifies storage and declaration of matrices.
|
|
|
261 *
|
|
|
262 * An example, the 2x3 matrix:
|
|
|
263 *
|
|
|
264 * [ 1, 2, 3; 4, 5, 6 ]
|
|
|
265 *
|
|
|
266 * Should be defined as:
|
|
|
267 *
|
|
|
268 * q_t m[] = { 0, 2*3, 2, 3, QINT(1), QINT(2), QINT(3), QINT(4), QINT(5), QINT(6) };
|
|
|
269 *
|
|
|
270 */
|
|
|
271 int qmatrix_apply_unary(q_t *r, const q_t *a, q_t (*func)(q_t));
|
|
|
272 int qmatrix_apply_scalar(q_t *r, const q_t *a, q_t (*func)(q_t, q_t), const q_t c);
|
|
|
273 int qmatrix_apply_binary(q_t * RESTRICT r, const q_t *a, const q_t *b, q_t (*func)(q_t, q_t));
|
|
|
274 int qmatrix_sprintb(const q_t *m, char *str, size_t length, unsigned base);
|
|
|
275 int qmatrix_resize(q_t *m, const size_t row, const size_t column);
|
|
|
276 int qmatrix_copy(q_t *r, const q_t *a);
|
|
|
277 size_t qmatrix_string_length(const q_t *m);
|
|
|
278
|
|
|
279 q_t qmatrix_trace(const q_t *m);
|
|
|
280 q_t qmatrix_determinant(const q_t *m);
|
|
|
281 q_t qmatrix_equal(const q_t *a, const q_t *b);
|
|
|
282
|
|
|
283 int qmatrix_zero(q_t *r);
|
|
|
284 int qmatrix_one(q_t *r);
|
|
|
285 int qmatrix_identity(q_t *r); /* turn into identity matrix, r must be square */
|
|
|
286
|
|
|
287 int qmatrix_logical(q_t *r, const q_t *a);
|
|
|
288 int qmatrix_not(q_t *r, const q_t *a);
|
|
|
289 int qmatrix_signum(q_t *r, const q_t *a);
|
|
|
290 int qmatrix_invert(q_t *r, const q_t *a);
|
|
|
291
|
|
|
292 int qmatrix_is_valid(const q_t *m);
|
|
|
293 int qmatrix_is_square(const q_t *m);
|
|
|
294
|
|
|
295 int qmatrix_transpose(q_t * RESTRICT r, const q_t * RESTRICT m);
|
|
|
296 int qmatrix_add(q_t * RESTRICT r, const q_t *a, const q_t *b);
|
|
|
297 int qmatrix_sub(q_t * RESTRICT r, const q_t *a, const q_t *b);
|
|
|
298 int qmatrix_mul(q_t * RESTRICT r, const q_t *a, const q_t *b);
|
|
|
299 int qmatrix_and(q_t *r, const q_t *a, const q_t *b);
|
|
|
300 int qmatrix_or (q_t *r, const q_t *a, const q_t *b);
|
|
|
301 int qmatrix_xor(q_t *r, const q_t *a, const q_t *b);
|
|
|
302
|
|
|
303 int qmatrix_scalar_add(q_t *r, const q_t *a, const q_t scalar);
|
|
|
304 int qmatrix_scalar_sub(q_t *r, const q_t *a, const q_t scalar);
|
|
|
305 int qmatrix_scalar_mul(q_t *r, const q_t *a, const q_t scalar);
|
|
|
306 int qmatrix_scalar_div(q_t *r, const q_t *a, const q_t scalar);
|
|
|
307 int qmatrix_scalar_mod(q_t *r, const q_t *a, const q_t scalar);
|
|
|
308 int qmatrix_scalar_rem(q_t *r, const q_t *a, const q_t scalar);
|
|
|
309 int qmatrix_scalar_and(q_t *r, const q_t *a, const q_t scalar);
|
|
|
310 int qmatrix_scalar_or (q_t *r, const q_t *a, const q_t scalar);
|
|
|
311 int qmatrix_scalar_xor(q_t *r, const q_t *a, const q_t scalar);
|
|
|
312
|
|
|
313 /* Expression evaluator */
|
|
|
314
|
|
|
315 int qexpr(qexpr_t *e, const char *expr);
|
|
|
316 int qexpr_init(qexpr_t *e);
|
|
|
317 int qexpr_error(qexpr_t *e);
|
|
|
318 q_t qexpr_result(qexpr_t *e);
|
|
|
319 const qoperations_t *qop(const char *op);
|
|
|
320
|
|
|
321 /* A better cosine/sine, not in Q format */
|
|
|
322
|
|
|
323 int16_t furman_sin(int16_t x); /* SINE: 1 Furman = 1/65536 of a circle */
|
|
|
324 int16_t furman_cos(int16_t x); /* COSINE: 1 Furman = 1/65536 of a circle */
|
|
|
325
|
|
|
326 #ifdef __cplusplus
|
|
|
327 }
|
|
|
328 #endif
|
|
|
329 #endif
|
