From 0db1380594157e8b15c62b46bdd75ebd010dc9dc Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 17 Apr 2008 00:32:45 +0200 Subject: * digital/asserv/src/asserv: - added counter correction to handle wheels with different size. --- digital/asserv/src/asserv/counter_ext.avr.c | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) (limited to 'digital/asserv/src/asserv/counter_ext.avr.c') diff --git a/digital/asserv/src/asserv/counter_ext.avr.c b/digital/asserv/src/asserv/counter_ext.avr.c index f0d1622d..4f8cad3b 100644 --- a/digital/asserv/src/asserv/counter_ext.avr.c +++ b/digital/asserv/src/asserv/counter_ext.avr.c @@ -26,12 +26,15 @@ #include "counter.h" #include "modules/utils/utils.h" +#include "modules/math/fixed/fixed.h" #include "io.h" /** * This file add support for an external counter like the hdlcounter or * avrcounter project. This can be better in order not to loose steps and * support more counters. + * + * There is additionnal support for error correction on the right counter. */ /** Define the left counter address. */ @@ -56,8 +59,12 @@ static uint8_t counter_left_old, counter_right_old, counter_aux0_old; /** Overall counter values. */ uint16_t counter_left, counter_right, counter_aux0; +/** Overall uncorrected counter values. */ +static int32_t counter_right_raw; +/** Correction factor (f8.24). */ +uint32_t counter_right_correction = 1L << 24; /** Counter differences since last update. - * Maximum of 7 significant bits, sign included. */ + * Maximum of 9 significant bits, sign included. */ int16_t counter_left_diff, counter_right_diff, counter_aux0_diff; #if !COUNTER_USE_XMEM @@ -137,7 +144,12 @@ counter_update (void) counter_right_diff = (int8_t) (counter_right_old - right); #endif counter_right_old = right; - counter_right += counter_right_diff; + /* Fix right counter. */ + counter_right_raw += counter_right_diff; + uint16_t right_new = fixed_mul_f824 (counter_right_raw, + counter_right_correction); + counter_right_diff = (int16_t) (right_new - counter_right); + counter_right = right_new; /* First auxiliary counter. */ #if !COUNTER_AUX0_REVERSE counter_aux0_diff = (int8_t) (aux0 - counter_aux0_old); -- cgit v1.2.3