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
#include "basicmaths.h"
#include <stdint.h>
#include <string.h>
#include "fastpow.h"
#include "fastlog.h"
#ifdef ARM_CORTEX
/* The realloc() function changes the size of the memory block pointed to */
/* by ptr to size bytes. The contents will be unchanged in the range from */
/* the start of the region up to the minimum of the old and new sizes. If */
/* the new size is larger than the old size, the added memory will not be */
/* initialized. If ptr is NULL, then the call is equivalent to mal‐ */
/* loc(size), for all values of size; if size is equal to zero, and ptr is */
/* not NULL, then the call is equivalent to free(ptr). Unless ptr is */
/* NULL, it must have been returned by an earlier call to malloc(), cal‐ */
/* loc(), or realloc(). If the area pointed to was moved, a free(ptr) is */
/* done. */
void* pvPortRealloc(void *ptr, size_t new_size) {
if(ptr == NULL)
return pvPortMalloc(new_size);
size_t old_size = vPortGetSizeBlock(ptr);
if(new_size == 0){
vPortFree(ptr);
return NULL;
}
if(new_size <= old_size)
return ptr;
void* p = pvPortMalloc(new_size);
if(p == NULL)
return p;
memcpy(p, ptr, old_size);
vPortFree(ptr);
return p;
}
/* The calloc() function allocates memory for an array of nmemb elements */
/* of size bytes each and returns a pointer to the allocated memory. */
/* The memory is set to zero. */
void *pvPortCalloc(size_t nmemb, size_t size){
size_t xWantedSize = nmemb*size;
void* ptr = pvPortMalloc(xWantedSize);
if(ptr != NULL)
memset(ptr, 0, xWantedSize);
return ptr;
}
#endif
// todo: see
// http://www.hxa.name/articles/content/fast-pow-adjustable_hxa7241_2007.html
// http://www.finesse.demon.co.uk/steven/sqrt.html
// http://www.keil.com/forum/7934/
// http://processors.wiki.ti.com/index.php/ARM_compiler_optimizations
/* void *_sbrk(intptr_t increment){} */
static uint32_t r32seed = 33641;
void arm_srand32(uint32_t s){
r32seed = s;
}
/**
* Generate an unsigned 32bit pseudo-random number using xorshifter algorithm. Aka xorshifter32.
* "Anyone who considers arithmetical methods of producing random digits is, of course, in a state of sin."
* -- John von Neumann.
*/
uint32_t arm_rand32(){
r32seed ^= r32seed << 13;
r32seed ^= r32seed >> 17;
r32seed ^= r32seed << 5;
return r32seed;
}
float randf(){
return arm_rand32()*(1/4294967296.0f);
}
float arm_sqrtf(float in){
float out;
#ifdef ARM_CORTEX
arm_sqrt_f32(in, &out);
#else
out=sqrtf(in);
#endif
return out;
}
/* Fast arctan2
* from http://dspguru.com/dsp/tricks/fixed-point-atan2-with-self-normalization
*/
float fast_atan2f(float y, float x){
const float coeff_1 = M_PI/4;
const float coeff_2 = 3*M_PI/4;
float abs_y = fabs(y)+1e-10; // kludge to prevent 0/0 condition
float r, angle;
if (x>=0){
r = (x - abs_y) / (x + abs_y);
angle = coeff_1 - coeff_1 * r;
}else{
r = (x + abs_y) / (abs_y - x);
angle = coeff_2 - coeff_1 * r;
}
if(y < 0)
return(-angle); // negate if in quad III or IV
else
return(angle);
}
/* static const float* log_table = fast_log_table; */
/* static uint32_t log_precision = fast_log_precision; */
/* static const uint32_t* pow_table = fast_pow_table; */
/* static uint32_t pow_precision = fast_pow_precision; */
static const float* log_table;
static uint32_t log_precision;
static const uint32_t* pow_table;
static uint32_t pow_precision;
#define M_LOG210 3.32192809488736
float fast_powf(float x, float y){
return powFastLookup(y, logf(x)*M_LOG2E, pow_table, pow_precision);
}
float fast_expf(float x){
return powFastLookup(x, M_LOG2E, pow_table, pow_precision);
}
float fast_exp2f(float x){
return powFastLookup(x, 1, pow_table, pow_precision);
}
float fast_exp10f(float x){
return powFastLookup(x, M_LOG210, pow_table, pow_precision);
}
float fast_logf(float x){
return icsi_log(x, log_table, log_precision);
}
float fast_log10f(float x){
/* log10 (x) equals log (x) / log (10). */
return icsi_log(x, log_table, log_precision) / M_LN10;
}
float fast_log2f(float x){
/* log2 (x) equals log (x) / log (2). */
return icsi_log(x, log_table, log_precision) / M_LN2;
}
void fast_pow_set_table(const uint32_t* table, int size){
pow_table = table;
pow_precision = fast_log2i(size);
}
void fast_log_set_table(const float* table, int size){
log_table = table;
log_precision = fast_log2i(size);
}
float fast_fmodf(float x, float y) {
float a = x/y;
return (a-(int)a)*y;
}
uint32_t fast_log2i(uint32_t x){
return 31 - __builtin_clz (x); /* clz returns the number of leading 0's */
}