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