From 7be2abd03605f27ba62103b25fefb1612e5d030e Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 23 Jan 2005 17:03:18 +0000 Subject: Correction des overflow et de la position. --- n/asserv/src/postrack.c | 32 ++++++++++++++++++++------------ 1 file changed, 20 insertions(+), 12 deletions(-) (limited to 'n/asserv') diff --git a/n/asserv/src/postrack.c b/n/asserv/src/postrack.c index 646a67b..5adcb37 100644 --- a/n/asserv/src/postrack.c +++ b/n/asserv/src/postrack.c @@ -63,34 +63,42 @@ postrack_init (void) postrack_set_footing (5142); } +#define M_PI 3.14159265358979323846 /* pi */ + /** Update the current position. */ static inline void postrack_update (void) { - int32_t d, dd, da, r, na; + int32_t d, dd, da, na, dsc; d = counter_right_diff + counter_left_diff; /* 10b */ d <<= 16; /* 10.16b */ d >>= 1; /* 9.16b */ if (counter_right_diff == counter_left_diff) { + /* Line. */ postrack_x += dsp_mul_f824 (d, dsp_cos (postrack_a)) >> 8; postrack_y += dsp_mul_f824 (d, dsp_sin (postrack_a)) >> 8; } else { - dd = counter_right_diff - counter_left_diff; /* 11b */ - dd <<= 16; /* 11.16b */ + /* Arc. */ + dd = counter_right_diff - counter_left_diff; /* 10b */ + dd <<= 16; /* 10.16b */ da = dsp_mul_f824 (dd, postrack_footing_factor);/* 8.24b */ - // XXX: WARNING ! could r overflow ? Yes :,-( - // Should try too divide by da later, but this means two divisions - // instead of one. - // Also, this division is false because da is not radians ! - r = dsp_div_f824 (d, da); /* 16.16b */ + // XXX: this division is false because da is not radians ! + /* New angle. */ na = postrack_a + da; - postrack_x += - dsp_mul_f824 (r, dsp_sin (na) - dsp_sin (postrack_a)) >> 8; - postrack_y += - dsp_mul_f824 (r, dsp_cos (postrack_a) - dsp_cos (na)) >> 8; + /* da in radians. */ + da = dsp_mul_f824 (da, 2 * M_PI * (1L << 24)); + /* X increment. */ + dsc = dsp_sin (na) - dsp_sin (postrack_a); /* 8.24b */ + dsc = dsp_div_f824 (dsc, da); /* 8.24b < 1 */ + postrack_x += dsp_mul_f824 (d, dsc) >> 8; /* 24.8b */ + /* Y increment. */ + dsc = dsp_cos (postrack_a) - dsp_cos (na); /* 8.24b */ + dsc = dsp_div_f824 (dsc, da); /* 8.24b < 1 */ + postrack_y += dsp_mul_f824 (d, dsc) >> 8; /* 24.8b */ + /* Angle update. */ postrack_a = na; postrack_a &= 0x00ffffff; } -- cgit v1.2.3