summaryrefslogtreecommitdiff
path: root/n/asserv/src/dsp.c
diff options
context:
space:
mode:
authorschodet2004-11-07 22:12:28 +0000
committerschodet2004-11-07 22:12:28 +0000
commitb320f6cd36a748cd4a83ba44fb125c0e20c84806 (patch)
tree5ad1368af4e31f5773212e8fbba1025939f13e99 /n/asserv/src/dsp.c
parent5b4e5290c4c840f79722691c584cafc5c4223db0 (diff)
Correction des tests, ajout d'un test en C pour mul_f824, ajout de cos et sin.
Diffstat (limited to 'n/asserv/src/dsp.c')
-rw-r--r--n/asserv/src/dsp.c74
1 files changed, 69 insertions, 5 deletions
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));
+}