From b320f6cd36a748cd4a83ba44fb125c0e20c84806 Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 7 Nov 2004 22:12:28 +0000 Subject: Correction des tests, ajout d'un test en C pour mul_f824, ajout de cos et sin. --- n/asserv/src/dsp.c | 74 ++++++++++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 69 insertions(+), 5 deletions(-) (limited to 'n/asserv/src/dsp.c') diff --git a/n/asserv/src/dsp.c b/n/asserv/src/dsp.c index bb81fdb..576040b 100644 --- a/n/asserv/src/dsp.c +++ b/n/asserv/src/dsp.c @@ -27,7 +27,19 @@ /* +AutoDec */ /* -AutoDec */ -/** Add two signed words and saturate. UNTESTED. */ +/** Numbers notation: + * [u]{i|f}x[.y] + * u: unsigned + * i: integer + * f: fixed point + * x: integral part size in bits + * y: fractionnal part size in bits + * + * Ex: i16: signed 16 bit word, uf8.8: unsigned fixed 8.8. + * + * Angles are mapped from [0, 2pi) to [0,1). */ + +/** Add two signed words (i16) and saturate. UNTESTED. */ extern inline int16_t dsp_add_sat_i16i16 (int16_t a, int16_t b) { @@ -48,7 +60,7 @@ dsp_add_sat_i16i16 (int16_t a, int16_t b) return a; } -/** Multiply signed 16 by unsigned 8.8, return signed 16. */ +/** Multiply i16 by uf8.8, return i16. */ extern inline int16_t dsp_mul_i16f88 (int16_t a, uint16_t b) { @@ -68,7 +80,7 @@ dsp_mul_i16f88 (int16_t a, uint16_t b) return r; } -/** Multiply signed 8.24 by signed 8.24, return signed 8.24. */ +/** Multiply f8.24 by f8.24, return f8.24. */ int32_t dsp_mul_f824 (int32_t a, int32_t b) { @@ -153,9 +165,61 @@ dsp_mul_f824 (int32_t a, int32_t b) "muls %D2, %D3\n\t" "add %D0, r0\n\t" "clr r1\n\t" - : "=&r" (r), "=r" (z) : "a" (ar), "a" (br) : "r0"); + : "=&r" (r), "=&r" (z) : "a" (ar), "a" (br) : "r0"); + return r; +} + +/** Compute cosinus for angles between [0,pi/2]. */ +int32_t +dsp_cos_dli (int32_t a) +{ + static const int32_t f[] = { + (1L << 24) * -26.42625678337439745096, + (1L << 24) * 60.24464137187666035919, + (1L << 24) * -85.45681720669372773226, + (1L << 24) * 64.93939402266829148905, + (1L << 24) * -19.73920880217871723738, + (1L << 24) * 1 + }; + int32_t r; + int32_t a2; + uint8_t i; + a2 = dsp_mul_f824 (a, a); + r = f[0]; + for (i = 1; i < sizeof (f) / sizeof (f[0]); i++) + r = dsp_mul_f824 (r, a2) + f[i]; return r; } -/** Compute cosinus. */ +/** Compute cosinus, angle f8.24, result f8.24. */ +int32_t +dsp_cos (int32_t a) +{ + a &= (1L << 24) - 1; + uint8_t z = ((uint32_t) a >> 16) & 0xc0; + if (z == 0) + return dsp_cos_dli (a); + else if (z == 1 << 6) + return -dsp_cos_dli ((1L << 23) - a); + else if (z == 2 << 6) + return -dsp_cos_dli (a & 0xff7fffff); + else + return dsp_cos_dli ((1L << 24) - a); +} + +/** Compute sinus, angle f8.24, result f8.24. */ +int32_t +dsp_sin (int32_t a) +{ + a &= (1L << 24) - 1; + uint8_t z = ((uint32_t) a >> 16) & 0xc0; + if (z == 0) + return dsp_cos_dli ((1L << 22) - a); + else if (z == 1 << 6) + return dsp_cos_dli (a - (1L << 22)); + else if (z == 2 << 6) + return -dsp_cos_dli ((1L << 23) + (1L << 22) - a); + else + return -dsp_cos_dli (a - (1L << 23) - (1L << 22)); +} -- cgit v1.2.3