From 44daa40214681aaca1e5b28ab4ce8d4567cbe984 Mon Sep 17 00:00:00 2001 From: burg Date: Fri, 23 Jul 2004 23:15:31 +0000 Subject: Correction des algos. Debut d'implémentation de la vitesse. --- n/accel/calcul.c | 31 ++++++++++++++++++++++--------- 1 file changed, 22 insertions(+), 9 deletions(-) (limited to 'n/accel/calcul.c') diff --git a/n/accel/calcul.c b/n/accel/calcul.c index f8e02bb..cbaed7b 100644 --- a/n/accel/calcul.c +++ b/n/accel/calcul.c @@ -35,6 +35,9 @@ uint32_t K; volatile enum T_etat etat; +int16_t G0x; +int16_t G0y; + void calibration(void) { @@ -42,27 +45,37 @@ calibration(void) uint32_t T1x = 0; uint32_t T1y = 0; uint32_t T2 = 0; - while (i < 8 ) + while (i < 128 ) { if( etat == calcul) { T1x += Tb; T1y += Td - Tc; - T2 = (Td - (Td - Tc)/2) - Tb/2; + //T2 += (Td - (Td - Tc)/2) - Tb/2; + T2 += (Td + Tc - Tb) /2; i++; etat = strt_at_Ta; } } - T2_cal = T2 / 8; - T1x_cal = T1x / 8; - T1y_cal = T1y / 8; - K = ( 4 * ( T2_cal * BIT_SCALE_FACTOR) / T2_cal); + T2_cal = T2 / 128; + T1x_cal = T1x / 128; + T1y_cal = T1y / 128; + //K = ( 4 * ( T2_cal * BIT_SCALE_FACTOR) / T2_cal); + //K = 65336; + K = 8192; + G0x = K * T1x_cal / T2_cal; + G0y = K * T1y_cal / T2_cal; + Vx =0; + Vy = 0; + } int16_t -calculG ( uint16_t T1, uint16_t T2, uint16_t Zcal) +calculG ( uint16_t T1, uint16_t T2, int16_t Go) { - uint32_t Zactual; + /*uint32_t Zactual; Zactual = Zcal * T2 / T2_cal; - return ( K * ( T1 - Zactual) / T2); + return ( (int16_t) K * (int16_t)( T1 - Zactual) / (int16_t)T2); +*/ + return (K * T1) / T2 - Go ; } -- cgit v1.2.3