summaryrefslogtreecommitdiff
path: root/digital/mimot/src/dirty/counter_ext.avr.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/mimot/src/dirty/counter_ext.avr.c')
-rw-r--r--digital/mimot/src/dirty/counter_ext.avr.c99
1 files changed, 55 insertions, 44 deletions
diff --git a/digital/mimot/src/dirty/counter_ext.avr.c b/digital/mimot/src/dirty/counter_ext.avr.c
index e99d9dc0..630f6cec 100644
--- a/digital/mimot/src/dirty/counter_ext.avr.c
+++ b/digital/mimot/src/dirty/counter_ext.avr.c
@@ -33,38 +33,44 @@
* 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 first auxiliary counter address. */
-#define COUNTER_AUX0 0
-/** Define the second auxiliary counter address. */
-#define COUNTER_AUX1 1
+/** Define the left counter address. */
+#define COUNTER_LEFT 0
+/** Define the right counter address. */
+#define COUNTER_RIGHT 1
-/** Define to 1 to reverse the first auxiliary counter. */
-#define COUNTER_AUX0_REVERSE 0
-/** Define to 1 to reverse the second auxiliary counter. */
-#define COUNTER_AUX1_REVERSE 0
+/** Define to 1 to reverse the left counter. */
+#define COUNTER_LEFT_REVERSE 0
+/** Define to 1 to reverse the right counter. */
+#define COUNTER_RIGHT_REVERSE 0
-/** First auxiliary counter shift. */
-#define COUNTER_AUX0_SHIFT 1
-/** Second auxiliary counter shift. */
-#define COUNTER_AUX1_SHIFT 1
+/** Left counter shift. */
+#define COUNTER_LEFT_SHIFT 0
+/** Right counter shift. */
+#define COUNTER_RIGHT_SHIFT 0
/** Define to 1 to use the AVR External Memory system, or 0 to use hand made
* signals. */
#define COUNTER_USE_XMEM 0
/** Last values. */
-static uint16_t counter_aux_old[AC_ASSERV_AUX_NB];
+static uint16_t counter_left_old, counter_right_old;
/** New values, being updated by step update. */
-static int16_t counter_aux_new_step[AC_ASSERV_AUX_NB];
+static int16_t counter_left_new_step, counter_right_new_step;
/** Last raw step values */
-static uint8_t counter_aux_old_step[AC_ASSERV_AUX_NB];
+static uint8_t counter_left_old_step, counter_right_old_step;
/** Overall counter values. */
-uint16_t counter_aux[AC_ASSERV_AUX_NB];
+uint16_t counter_left, counter_right;
+/** 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 9 significant bits, sign included. */
-int16_t counter_aux_diff[AC_ASSERV_AUX_NB];
+int16_t counter_left_diff, counter_right_diff;
#if !COUNTER_USE_XMEM
# define COUNTER_ALE_IO B, 4
@@ -119,8 +125,8 @@ counter_init (void)
DDRB |= 0x0f;
#endif
/* Begin with safe values. */
- counter_aux_old_step[0] = counter_read (COUNTER_AUX0);
- counter_aux_old_step[1] = counter_read (COUNTER_AUX1);
+ counter_left_old_step = counter_read (COUNTER_LEFT);
+ counter_right_old_step = counter_read (COUNTER_RIGHT);
}
/** Update one step. If counters are not read fast enough, they could
@@ -128,18 +134,18 @@ counter_init (void)
void
counter_update_step (void)
{
- uint8_t aux0, aux1;
+ uint8_t left, right;
int8_t diff;
/* Sample counters. */
- aux0 = counter_read (COUNTER_AUX0);
- aux1 = counter_read (COUNTER_AUX1);
+ left = counter_read (COUNTER_LEFT);
+ right = counter_read (COUNTER_RIGHT);
/* Update step counters. */
- diff = (int8_t) (aux0 - counter_aux_old_step[0]);
- counter_aux_old_step[0] = aux0;
- counter_aux_new_step[0] += diff;
- diff = (int8_t) (aux1 - counter_aux_old_step[1]);
- counter_aux_old_step[1] = aux1;
- counter_aux_new_step[1] += diff;
+ diff = (int8_t) (left - counter_left_old_step);
+ counter_left_old_step = left;
+ counter_left_new_step += diff;
+ diff = (int8_t) (right - counter_right_old_step);
+ counter_right_old_step = right;
+ counter_right_new_step += diff;
}
/** Update overall counter values and compute diffs. */
@@ -148,25 +154,30 @@ counter_update (void)
{
/* Wants fresh data. */
counter_update_step ();
- /* First auxiliary counter. */
- uint16_t aux0 = counter_aux_new_step[0];
-#if !COUNTER_AUX0_REVERSE
- counter_aux_diff[0] = (int16_t) (aux0 - counter_aux_old[0]);
+ /* Left counter. */
+ uint16_t left = counter_left_new_step;
+#if !COUNTER_LEFT_REVERSE
+ counter_left_diff = (int16_t) (left - counter_left_old);
#else
- counter_aux_diff[0] = (int16_t) (counter_aux_old[0] - aux0);
+ counter_left_diff = (int16_t) (counter_left_old - left);
#endif
- counter_aux_diff[0] >>= COUNTER_AUX0_SHIFT;
- counter_aux_old[0] = aux0;
- counter_aux[0] += counter_aux_diff[0];
- /* Second auxiliary counter. */
- uint16_t aux1 = counter_aux_new_step[1];
-#if !COUNTER_AUX1_REVERSE
- counter_aux_diff[1] = (int16_t) (aux1 - counter_aux_old[1]);
+ counter_left_diff >>= COUNTER_LEFT_SHIFT;
+ counter_left_old = left;
+ counter_left += counter_left_diff;
+ /* Right counter. */
+ uint16_t right = counter_right_new_step;
+#if !COUNTER_RIGHT_REVERSE
+ counter_right_diff = (int16_t) (right - counter_right_old);
#else
- counter_aux_diff[1] = (int16_t) (counter_aux_old[1] - aux1);
+ counter_right_diff = (int16_t) (counter_right_old - right);
#endif
- counter_aux_diff[1] >>= COUNTER_AUX1_SHIFT;
- counter_aux_old[1] = aux1;
- counter_aux[1] += counter_aux_diff[1];
+ counter_right_diff >>= COUNTER_RIGHT_SHIFT;
+ counter_right_old = right;
+ /* 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;
}