-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathavrfix.h
297 lines (230 loc) · 9.17 KB
/
avrfix.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
/****************************************************************
* *
* __ ____ _________ *
* /_ \\\ \/ /| \______ *
* // \\\ /|| D /_ /. *
* // \\\_ /.|| \ ___/. *
* /___/\___\\__/. |__|\__\__.___ ___ *
* .... ....... ...|| _/_\ \////. *
* || |.| |\ ///. *
* |__|.|_|/// \ *
* .... ./__/\__\ *
* ........ *
* Fixed Point Library *
* according to *
* ISO/IEC DTR 18037 *
* *
* Version 1.0.1 *
* Maximilan Rosenblattl, Andreas Wolf 2007-02-07 *
****************************************************************/
#ifndef _AVRFIX_H
#define _AVRFIX_H
#ifndef TEST_ON_PC
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
#include <avr/pgmspace.h>
#endif
/* Only two datatypes are used from the ISO/IEC standard:
* short _Accum with s7.8 bit format
* _Accum with s15.16 bit format
* long _Accum with s7.24 bit format
*/
typedef signed short _sAccum;
typedef signed long _Accum;
typedef signed long _lAccum;
/* Pragmas for defining overflow behaviour */
#define DEFAULT 0
#define SAT 1
#ifndef FX_ACCUM_OVERFLOW
#define FX_ACCUM_OVERFLOW DEFAULT
#endif
/* Pragmas for internal use */
#define SACCUM_IBIT 7
#define SACCUM_FBIT 8
#define ACCUM_IBIT 15
#define ACCUM_FBIT 16
#define LACCUM_IBIT 7
#define LACCUM_FBIT 24
#define SACCUM_MIN -32767
#define SACCUM_MAX 32767
#define ACCUM_MIN -2147483647L
#define ACCUM_MAX 2147483647L
#define LACCUM_MIN -2147483647L
#define LACCUM_MAX 2147483647L
#define SACCUM_FACTOR ((short)1 << SACCUM_FBIT)
#define ACCUM_FACTOR ((long)1 << ACCUM_FBIT)
#define LACCUM_FACTOR ((long)1 << LACCUM_FBIT)
/* Mathematical constants */
#define PIsk 804
#define PIk 205887
#define PIlk 52707179
#define LOG2k 45426
#define LOG2lk 11629080
#define LOG10k 150902
#define LOG10lk 38630967
#ifndef NULL
#define NULL ((void*)0)
#endif
/* conversion Functions */
#define itosk(i) ((_sAccum)(i) << SACCUM_FBIT)
#define itok(i) ((_Accum)(i) << ACCUM_FBIT)
#define itolk(i) ((_lAccum)(i) << LACCUM_FBIT)
#define sktoi(k) ((int8_t)((k) >> SACCUM_FBIT))
#define ktoi(k) ((signed short)((k) >> ACCUM_FBIT))
#define lktoi(k) ((int8_t)((k) >> LACCUM_FBIT))
#define sktok(sk) ( (_Accum)(sk) << (ACCUM_FBIT-SACCUM_FBIT))
#define ktosk(k) ((_sAccum)((k) >> (ACCUM_FBIT-SACCUM_FBIT)))
#define sktolk(sk) ((_lAccum)(sk) << (LACCUM_FBIT-SACCUM_FBIT))
#define lktosk(lk) ((_sAccum)((lk) >> (LACCUM_FBIT-SACCUM_FBIT)))
#define ktolk(k) ((_Accum)(k) << (LACCUM_FBIT-ACCUM_FBIT))
#define lktok(lk) ((_lAccum)(lk) >> (LACCUM_FBIT-ACCUM_FBIT))
#define ftosk(f) ((_Accum)(f) * (1 << SACCUM_FBIT))
#define ftok(f) ((_Accum)(f) * (1 << ACCUM_FBIT))
#define ftolk(f) ((_lAccum)(f) * (1 << LACCUM_FBIT))
#define sktof(sk) ((float)((_sAccum)(sk) / (1 << SACCUM_FBIT)))
#define ktod(k) ((double)((_sAccum)(k) / (1 << SACCUM_FBIT)))
#define lktod(lk) ((double)((_sAccum)(lk) / (1 << SACCUM_FBIT)))
/* Main Functions */
extern _sAccum smulskD(_sAccum, _sAccum);
extern _Accum mulkD(_Accum, _Accum);
extern _lAccum lmullkD(_lAccum, _lAccum);
extern _sAccum sdivskD(_sAccum, _sAccum);
extern _Accum divkD(_Accum, _Accum);
extern _lAccum ldivlkD(_lAccum, _lAccum);
extern _sAccum smulskS(_sAccum, _sAccum);
extern _Accum mulkS(_Accum, _Accum);
extern _lAccum lmullkS(_lAccum, _lAccum);
extern _sAccum sdivskS(_sAccum, _sAccum);
extern _Accum divkS(_Accum, _Accum);
extern _lAccum ldivlkS(_lAccum, _lAccum);
#if FX_ACCUM_OVERFLOW == DEFAULT
#define smulsk(a,b) smulskD((a),(b))
#define mulk(a,b) mulkD((a),(b))
#define lmullk(a,b) lmullkD((a), (b))
#define sdivsk(a,b) sdivskD((a), (b))
#define divk(a,b) divkD((a), (b))
#define ldivlk(a,b) ldivlkD((a), (b))
#elif FX_ACCUM_OVERFLOW == SAT
#define smulsk(a,b) smulskS((a),(b))
#define mulk(a,b) mulkS((a),(b))
#define lmullk(a,b) lmullkS((a), (b))
#define sdivsk(a,b) sdivskS((a), (b))
#define divk(a,b) divkS((a), (b))
#define ldivlk(a,b) ldivlkS((a), (b))
#endif
/* Support Functions */
#define mulikD(i,k) ktoi((i) * (k))
#define mulilkD(i,lk) lktoi((i) * (lk))
#define divikD(i,k) ktoi(divkD(itok(i),(k)))
#define divilkD(i,lk) lktoi(ldivlkD(itolk(i),(lk)))
#define kdiviD(a,b) divkD(itok(a),itok(b))
#define lkdiviD(a,b) ldivlkD(itolk(a),itolk(b))
#define idivkD(a,b) ktoi(divkD((a),(b)))
#define idivlkD(a,b) lktoi(ldivlkD((a),(b)))
#define mulikS(i,k) ktoi(mulkS(itok(i),(k)))
#define mulilkS(i,lk) lktoi(lmullkS(itolk(i),(lk)))
#define divikS(i,k) ktoi(divkS(itok(i),(k)))
#define divilkS(i,lk) lktoi(ldivlkS(itolk(i),(lk)))
#define kdiviS(a,b) divkS(itok(a),itok(b))
#define lkdiviS(a,b) ldivlkS(itolk(a),itolk(b))
#define idivkS(a,b) ktoi(divkS((a),(b)))
#define idivlkS(a,b) lktoi(ldivlkS((a),(b)))
#if FX_ACCUM_OVERFLOW == DEFAULT
#define mulik(a,b) mulikD((a),(b))
#define mulilk(a,b) mulilkD((a),(b))
#define divik(a,b) divikD((a),(b))
#define divilk(a,b) divilkD((a),(b))
#define kdivi(a,b) kdiviD((a),(b))
#define lkdivi(a,b) lkdiviD((a),(b))
#define idivk(a,b) idivkD((a),(b))
#define idivlk(a,b) idivlkD((a),(b))
#elif FX_ACCUM_OVERFLOW == SAT
#define mulik(a,b) mulikS((a),(b))
#define mulilk(a,b) mulilkS((a),(b))
#define divik(a,b) divikS((a),(b))
#define divilk(a,b) divilkS((a),(b))
#define kdivi(a,b) kdiviS((a),(b))
#define lkdivi(a,b) lkdiviS((a),(b))
#define idivk(a,b) idivkS((a),(b))
#define idivlk(a,b) idivlkS((a),(b))
#endif
/* Abs Functions */
#define sabssk(f) ((f) < 0 ? (-(f)) : (f))
#define absk(f) ((f) < 0 ? (-(f)) : (f))
#define labslk(f) ((f) < 0 ? (-(f)) : (f))
/* Rounding Functions */
extern _sAccum roundskD(_sAccum f, uint8_t n);
extern _Accum roundkD(_Accum f, uint8_t n);
extern _lAccum roundlkD(_lAccum f, uint8_t n);
extern _sAccum roundskS(_sAccum f, uint8_t n);
extern _Accum roundkS(_Accum f, uint8_t n);
extern _lAccum roundlkS(_lAccum f, uint8_t n);
#if FX_ACCUM_OVERFLOW == DEFAULT
#define roundsk(f, n) roundskD((f), (n))
#define roundk(f, n) roundkD((f), (n))
#define roundlk(f, n) roundlkD((f), (n))
#elif FX_ACCUM_OVERFLOW == SAT
#define roundsk(f, n) roundskS((f), (n))
#define roundk(f, n) roundkS((f), (n))
#define roundlk(f, n) roundlkS((f), (n))
#endif
/* countls Functions */
extern uint8_t countlssk(_sAccum f);
extern uint8_t countlsk(_Accum f);
#define countlslk(f) countlsk((f))
/* Special Functions */
#define CORDICC_GAIN 10188012
#define CORDICH_GAIN 20258445
extern _Accum sqrtk_uncorrected(_Accum,int8_t,uint8_t);
#define sqrtkD(a) mulkD(sqrtk_uncorrected(a, -8, 17), CORDICH_GAIN/256)
#define lsqrtlkD(a) lmullkD(sqrtk_uncorrected(a, 0, 24), CORDICH_GAIN)
#define sqrtkS(a) mulkS(sqrtk_uncorrected(a, -8, 17), CORDICH_GAIN/256)
#define lsqrtlkS(a) lmullkS(sqrtk_uncorrected(a, 0, 24), CORDICH_GAIN)
#if FX_ACCUM_OVERFLOW == DEFAULT
#define sqrtk(a) sqrtkD(a)
#define lsqrtlk(a) lsqrtlkD(a)
#else
#define sqrtk(a) sqrtkS(a)
#define lsqrtlk(a) lsqrtlkS(a)
#endif
extern _Accum sincosk(_Accum, _Accum*);
extern _lAccum lsincoslk(_lAccum, _lAccum*);
extern _lAccum lsincosk(_Accum, _lAccum*);
extern _sAccum ssincossk(_sAccum, _sAccum*);
#define sink(a) sincosk((a), NULL)
#define lsinlk(a) lsincoslk((a), NULL)
#define lsink(a) lsincosk((a), NULL)
#define ssinsk(a) ssincossk((a), NULL)
#define cosk(a) sink((a) + PIk/2 + 1)
#define lcoslk(a) lsinlk((a) + PIlk/2)
#define lcosk(a) lsink((a) + PIk/2 + 1)
#define scossk(a) ssinsk((a) + PIsk/2)
extern _Accum tankD(_Accum);
extern _lAccum ltanlkD(_lAccum);
extern _lAccum ltankD(_Accum);
extern _Accum tankS(_Accum);
extern _lAccum ltanlkS(_lAccum);
extern _lAccum ltankS(_Accum);
#if FX_ACCUM_OVERFLOW == DEFAULT
#define tank(a) tankD((a))
#define ltanlk(a) ltanlkD((a))
#define ltank(a) ltankD((a))
#elif FX_ACCUM_OVERFLOW == SAT
#define tank(a) tankS((a))
#define ltanlk(a) ltanlkS((a))
#define ltank(a) ltankS((a))
#endif
extern _Accum atan2k(_Accum, _Accum);
extern _lAccum latan2lk(_lAccum, _lAccum);
#define atank(a) atan2k(itok(1), (a))
#define latanlk(a) latan2lk(itolk(1), (a))
extern _Accum logk(_Accum);
extern _lAccum lloglk(_lAccum);
#define log2k(x) (divk(logk((x)), LOG2k))
#define log10k(x) (divk(logk((x)), LOG10k))
#define logak(a, x) (divk(logk((x)), logk((a))))
#define llog2lk(x) (ldivlk(lloglk((x)), LOG2lk))
#define llog10lk(x) (ldivlk(lloglk((x)), LOG10lk))
#define llogalk(a, x) (ldivlk(lloglk((x)), lloglk((a))))
#endif /* _AVRFIX_H */