summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--n/asserv/src/Makefile12
-rw-r--r--n/asserv/src/dsp.c74
-rw-r--r--n/asserv/src/dsp_check.pl97
-rw-r--r--n/asserv/src/dsp_check2.c90
-rw-r--r--n/asserv/src/test_dsp.c61
5 files changed, 311 insertions, 23 deletions
diff --git a/n/asserv/src/Makefile b/n/asserv/src/Makefile
index f1be6f7..1666979 100644
--- a/n/asserv/src/Makefile
+++ b/n/asserv/src/Makefile
@@ -15,8 +15,8 @@ OPTIMIZE = -O2
DEFS =
LDLIBS =
-EXTRA_TARGETS = cos_check
-EXTRA_CLEAN_FILES = cos_check
+EXTRA_TARGETS = cos_check dsp_check2
+EXTRA_CLEAN_FILES = cos_check dsp_check2
include Makefile.avr
@@ -26,7 +26,7 @@ test_pwm.elf: $(test_pwm_OBJECTS)
test_dsp.elf: $(test_dsp_OBJECTS)
-cos_check: CC = gcc
-cos_check: CPPFLAGS =
-cos_check: CFLAGS = -Wall -g -O2
-cos_check: LDLIBS = -lm
+cos_check dsp_check2: CC = gcc
+cos_check dsp_check2: CPPFLAGS =
+cos_check dsp_check2: CFLAGS = -Wall -g -O2
+cos_check dsp_check2: LDLIBS = -lm
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));
+}
diff --git a/n/asserv/src/dsp_check.pl b/n/asserv/src/dsp_check.pl
index 738d32e..b6e8f76 100644
--- a/n/asserv/src/dsp_check.pl
+++ b/n/asserv/src/dsp_check.pl
@@ -3,6 +3,10 @@ use strict;
use POSIX qw(floor);
my $fail = 0;
+my $incomplete = 0;
+my $bugged = 0;
+
+my $pi = 4 * atan2 1, 1;
sub check_mul
{
@@ -26,21 +30,60 @@ sub check_mul
sub check_mul_f824
{
+ use bignum;
my ($a, $b, $r) = @_;
+ my ($ha, $hb, $hr) = (sprintf ("%x", $a), sprintf ("%x", $b), sprintf ("%x", $r));
$a = $a / (1 << 24);
$b = $b / (1 << 24);
- my $m = floor ($a * $b * (1 << 24));
- if ($m > (1 << 31) - 1 || $m < -(1 << 31))
+ $r = $r / (1 << 24);
+ my $m = floor ($a * $b * (1 << 24)) / (1 << 24);
+ if ($m > (1 << 7) - 1 || $m < -(1 << 7))
{
- print "overflow $a * $b = $r ($m)\n";
+ print "overflow $a * $b = $r ($m) | $ha $hb $hr\n";
}
elsif ($m == $r)
{
- print "pass $a * $b = $r\n";
+ print "pass $a * $b = $r | $ha $hb $hr\n";
}
else
{
- print "fail $a * $b = $r ($m)\n";
+ print "fail $a * $b = $r ($m) | $ha $hb $hr\n";
+ $fail = 1;
+ }
+}
+
+sub check_cos
+{
+ my ($a, $r) = @_;
+ my ($ha, $hr) = (sprintf ("%x", $a), sprintf ("%x", $r));
+ $a = $a / (1 << 24) * 2 * $pi;
+ $r = $r / (1 << 24);
+ my $c = floor (cos ($a) * (1 << 24)) / (1 << 24);
+ if ($c == $r)
+ {
+ print "pass cos $a = $r | $ha $hr\n";
+ }
+ else
+ {
+ print "fail cos $a = $r ($c) | $ha $hr\n";
+ $fail = 1;
+ }
+}
+
+sub check_sin
+{
+ my ($a, $r) = @_;
+ my ($ha, $hr) = (sprintf ("%x", $a), sprintf ("%x", $r));
+ $a = $a / (1 << 24) * 2 * $pi;
+ $r = $r / (1 << 24);
+ my $s = floor (sin ($a) * (1 << 24)) / (1 << 24);
+ if ($s == $r)
+ {
+ print "pass sin $a = $r | $ha $hr\n";
+ }
+ else
+ {
+ print "fail sin $a = $r ($s) | $ha $hr\n";
$fail = 1;
}
}
@@ -50,6 +93,7 @@ while (<>)
chomp;
if (/^m (-?\d+) (\d+)$/)
{
+ $incomplete++;
my ($a, $b) = ($1, $2);
$_ = <>; chomp;
next unless (/^r (-?\d+)$/);
@@ -59,9 +103,12 @@ while (<>)
my $R = $1;
check_mul $a, $b, $r;
check_mul -$a, $b, $R;
+ $incomplete--;
}
if (/^A (-?\d+)$/)
{
+ $bugged = 1;
+ $incomplete++;
my $a = $1;
$_ = <>; chomp;
next unless (/^B (-?\d+)$/);
@@ -69,14 +116,45 @@ while (<>)
$_ = <>; chomp;
next unless (/^r (-?\d+)$/);
my $r = $1;
- $_ = <>; chomp;
- next unless (/^R (-?\d+)$/);
- my $R = $1;
check_mul_f824 $a, $b, $r;
- check_mul_f824 -$a, $b, $R;
+ $_ = <>; chomp;
+ next unless (/^r (-?\d+)$/);
+ $r = $1;
+ check_mul_f824 -$a, $b, $r;
+ $_ = <>; chomp;
+ next unless (/^r (-?\d+)$/);
+ $r = $1;
+ check_mul_f824 $a, -$b, $r;
+ $_ = <>; chomp;
+ next unless (/^r (-?\d+)$/);
+ $r = $1;
+ check_mul_f824 -$a, -$b, $r;
+ $incomplete--;
+ }
+ if (/^c (-?\d+)$/)
+ {
+ $bugged = 1;
+ $incomplete++;
+ my $c = $1;
+ $_ = <>; chomp;
+ next unless (/^r (-?\d+)$/);
+ my $r = $1;
+ check_cos $c, $r;
+ $_ = <>; chomp;
+ next unless (/^r (-?\d+)$/);
+ $r = $1;
+ check_sin $c, $r;
+ $incomplete--;
}
}
+print "WARNING: test bugged\n" if ($bugged);
+
+if ($incomplete)
+{
+ print "$incomplete incomplete tests\n";
+}
+
if ($fail)
{
print "test failled\n";
@@ -87,3 +165,4 @@ else
print "test passed\n";
exit 0;
}
+
diff --git a/n/asserv/src/dsp_check2.c b/n/asserv/src/dsp_check2.c
new file mode 100644
index 0000000..76d5cfe
--- /dev/null
+++ b/n/asserv/src/dsp_check2.c
@@ -0,0 +1,90 @@
+/* dsp_check2.c - check dsp algorithms. */
+/* asserv - Position & speed motor control on a ATmega128. {{{
+ *
+ * Copyright (C) 2004 Nicolas Schodet
+ *
+ * Robot APB Team/Efrei 2005.
+ * Web: http://assos.efrei.fr/robot/
+ * Email: robot AT efrei DOT fr
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+#include <stdio.h>
+
+/* +AutoDec */
+/* -AutoDec */
+
+static int failled = 0;
+
+void
+check_mul_f824 (int a, int b, int r)
+{
+ long long m = ((long long) a * b) >> 24;
+ if (m > (1LL << 31) - 1 || m < -(1LL << 31))
+ printf ("overflow %d * %d = %d (%Ld)", a, b, r, m);
+ else if (m == r)
+ printf ("pass %d * %d = %d", a, b, r);
+ else
+ {
+ printf ("fail %d * %d = %d (%Ld)", a, b, r, m);
+ failled = 1;
+ }
+ printf (" | %08x %08x %08x\n", a, b, r);
+}
+
+int
+get (int c, int *v)
+{
+ int ret;
+ char cs;
+ ret = scanf (" %c", &cs);
+ if (ret != 1 || cs != c)
+ return 0;
+ ret = scanf (" %d", v);
+ return ret == 1;
+}
+
+int
+main (void)
+{
+ int incomplete = 0;
+ int a, b, r1, r2, r3, r4;
+ while (!feof (stdin))
+ {
+ if (get ('A', &a) && get ('B', &b) && get ('r', &r1) && get ('r', &r2)
+ && get ('r', &r3) && get ('r', &r4))
+ {
+ check_mul_f824 (a, b, r1);
+ check_mul_f824 (-a, b, r2);
+ check_mul_f824 (a, -b, r3);
+ check_mul_f824 (-a, -b, r4);
+ }
+ else if (!feof (stdin))
+ incomplete++;
+ }
+ if (incomplete)
+ printf ("%d incomplete tests\n", incomplete);
+ if (failled)
+ {
+ printf ("test failled\n");
+ return 1;
+ }
+ else
+ {
+ printf ("test passed\n");
+ return 0;
+ }
+}
diff --git a/n/asserv/src/test_dsp.c b/n/asserv/src/test_dsp.c
index dace398..d8781f2 100644
--- a/n/asserv/src/test_dsp.c
+++ b/n/asserv/src/test_dsp.c
@@ -41,9 +41,24 @@ proto_callback (uint8_t c, uint8_t argc, proto_arg_t argv[])
int16_t i, j, k;
uint16_t w = 0xa66a;
int32_t al, bl, rl;
- uint32_t wl[] = { 0xa66a6aa6, 0xffffffff, 0xaff605be };
+ uint32_t wl[] = { 0xa66a6aa6, 0xffffffff };
+ static int32_t sa, sb;
switch (c | argc << 8)
{
+ case 'a' | 4 << 8:
+ sa = argv[0];
+ sa = sa << 8 | argv[1];
+ sa = sa << 8 | argv[2];
+ sa = sa << 8 | argv[3];
+ proto_send ('a', argc, argv);
+ break;
+ case 'b' | 4 << 8:
+ sb = argv[0];
+ sb = sb << 8 | argv[1];
+ sb = sb << 8 | argv[2];
+ sb = sb << 8 | argv[3];
+ proto_send ('b', argc, argv);
+ break;
case 'm' | 0 << 8:
for (i = 16; i > 0; i--)
{
@@ -63,8 +78,24 @@ proto_callback (uint8_t c, uint8_t argc, proto_arg_t argv[])
}
}
break;
+ case 'M' | 1 << 8:
+ rl = dsp_mul_f824 (sa, sb);
+ proto_send4 ('r',
+ (rl >> 24) & 0xff, (rl >> 16) & 0xff,
+ (rl >> 8) & 0xff, rl & 0xff);
+ break;
+ case 'c' | 1 << 8:
+ rl = dsp_cos (sa);
+ proto_send4 ('r',
+ (rl >> 24) & 0xff, (rl >> 16) & 0xff,
+ (rl >> 8) & 0xff, rl & 0xff);
+ rl = dsp_sin (sa);
+ proto_send4 ('r',
+ (rl >> 24) & 0xff, (rl >> 16) & 0xff,
+ (rl >> 8) & 0xff, rl & 0xff);
+ break;
case 'M' | 0 << 8:
- for (k = 0; k < 3; k++)
+ for (k = 0; k < 2; k++)
{
for (i = 32; i > 0; i--)
{
@@ -83,13 +114,37 @@ proto_callback (uint8_t c, uint8_t argc, proto_arg_t argv[])
(rl >> 24) & 0xff, (rl >> 16) & 0xff,
(rl >> 8) & 0xff, rl & 0xff);
rl = dsp_mul_f824 (-al, bl);
- proto_send4 ('R',
+ proto_send4 ('r',
+ (rl >> 24) & 0xff, (rl >> 16) & 0xff,
+ (rl >> 8) & 0xff, rl & 0xff);
+ rl = dsp_mul_f824 (al, -bl);
+ proto_send4 ('r',
+ (rl >> 24) & 0xff, (rl >> 16) & 0xff,
+ (rl >> 8) & 0xff, rl & 0xff);
+ rl = dsp_mul_f824 (-al, -bl);
+ proto_send4 ('r',
(rl >> 24) & 0xff, (rl >> 16) & 0xff,
(rl >> 8) & 0xff, rl & 0xff);
}
}
}
break;
+ case 'c' | 0 << 8:
+ for (al = 0; al < (1L << 24) + (1L << 21); al += 32 << 8)
+ {
+ proto_send4 ('c',
+ (al >> 24) & 0xff, (al >> 16) & 0xff,
+ (al >> 8) & 0xff, al & 0xff);
+ rl = dsp_cos (al);
+ proto_send4 ('r',
+ (rl >> 24) & 0xff, (rl >> 16) & 0xff,
+ (rl >> 8) & 0xff, rl & 0xff);
+ rl = dsp_sin (al);
+ proto_send4 ('r',
+ (rl >> 24) & 0xff, (rl >> 16) & 0xff,
+ (rl >> 8) & 0xff, rl & 0xff);
+ }
+ break;
case 'z' | 0 << 8:
reset ();
default: