83 lines
1.7 KiB
C++
83 lines
1.7 KiB
C++
#include "control_loop_math_implementation.h"
|
|
|
|
#define BITMASK(n) (((V)1 << (n)) - 1)
|
|
|
|
/* only works on 64 bit GCC/Clang, can use boost (eww boost) */
|
|
|
|
static V sat(__int128_t r, unsigned siz, unsigned discard) {
|
|
r >>= discard;
|
|
/* Since this is signed numbers, the actual number of bits of
|
|
* the largest number is one less than the bit size. */
|
|
siz -= 1;
|
|
|
|
if (r >= BITMASK(siz)) {
|
|
return BITMASK(siz);
|
|
} else if (r <= -BITMASK(siz)) {
|
|
V allzero = ~((V) 0);
|
|
// make (siz - 1) zero bits
|
|
return allzero & (allzero << (siz - 1));
|
|
} else {
|
|
return r;
|
|
}
|
|
}
|
|
|
|
V mulsat(V x, V y, unsigned siz, unsigned discard) {
|
|
__int128_t v = (__int128_t)x * (__int128_t)y;
|
|
|
|
return sat(v, siz, discard);
|
|
}
|
|
|
|
static int d2c(unsigned d) {
|
|
switch (d) {
|
|
case 0: return '0';
|
|
case 1: return '1';
|
|
case 2: return '2';
|
|
case 3: return '3';
|
|
case 4: return '4';
|
|
case 5: return '5';
|
|
case 6: return '6';
|
|
case 7: return '7';
|
|
case 8: return '8';
|
|
case 9: return '9';
|
|
default: return '?';
|
|
}
|
|
}
|
|
std::string fxp_to_string(const struct fixed_point &fxp) {
|
|
std::string r = std::to_string((fxp.val >> fxp.frac_len) & BITMASK(fxp.whole_len));
|
|
V frac = fxp.val & BITMASK(fxp.frac_len);
|
|
|
|
r.push_back('.');
|
|
|
|
for (unsigned i = 0; i < fxp.frac_len; i++) {
|
|
frac *= 10;
|
|
r.push_back(d2c(frac >> fxp.frac_len));
|
|
frac &= BITMASK(fxp.frac_len);
|
|
}
|
|
|
|
return r;
|
|
}
|
|
|
|
#if 0
|
|
V asr (V x, unsigned len) {
|
|
if (x >= 0)
|
|
return x >> len;
|
|
x >>= len;
|
|
|
|
/* x is shifted-right by N bits. This makes a mask of
|
|
* N bits, and shifts it to the highest position.
|
|
*/
|
|
V mask = ((1 << len) - 1) << (sizeof(x) * CHAR_BITS - len);
|
|
return mask | x;
|
|
}
|
|
#endif
|
|
|
|
V sign_extend(V x, unsigned len) {
|
|
/* if high bit is 1 */
|
|
if (x >> (len - 1) & 1) {
|
|
V mask = (1 << len) - 1;
|
|
return ~mask | x;
|
|
} else {
|
|
return x;
|
|
}
|
|
}
|