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/Makefile | 12 +++--- n/asserv/src/dsp.c | 74 +++++++++++++++++++++++++++++++++--- n/asserv/src/dsp_check.pl | 97 ++++++++++++++++++++++++++++++++++++++++++----- n/asserv/src/dsp_check2.c | 90 +++++++++++++++++++++++++++++++++++++++++++ n/asserv/src/test_dsp.c | 61 +++++++++++++++++++++++++++-- 5 files changed, 311 insertions(+), 23 deletions(-) create mode 100644 n/asserv/src/dsp_check2.c 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 + +/* +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: -- cgit v1.2.3