From 2f9746bd5c5dd83b66dbd832d34c22ab4afde10a Mon Sep 17 00:00:00 2001 From: burg Date: Tue, 21 Sep 2004 16:41:08 +0000 Subject: Correction de la méthode de calcul de l'accélération. --- n/accel/calcul.c | 30 +++++++++++++++++------------- 1 file changed, 17 insertions(+), 13 deletions(-) (limited to 'n/accel/calcul.c') diff --git a/n/accel/calcul.c b/n/accel/calcul.c index cbaed7b..aeff534 100644 --- a/n/accel/calcul.c +++ b/n/accel/calcul.c @@ -41,31 +41,27 @@ int16_t G0y; void calibration(void) { - uint8_t i = 0; + uint16_t i = 0; uint32_t T1x = 0; uint32_t T1y = 0; - uint32_t T2 = 0; - while (i < 128 ) + uint32_t T2c = 0; + while (i < 1024 ) { if( etat == calcul) { T1x += Tb; T1y += Td - Tc; //T2 += (Td - (Td - Tc)/2) - Tb/2; - T2 += (Td + Tc - Tb) /2; + T2c += (Td + Tc - Tb) /2; i++; etat = strt_at_Ta; } } - 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; + T2_cal = T2c / 1024; + T1x_cal = T1x / 1024; + T1y_cal = T1y / 1024; + K = (uint16_t)((uint32_t)( 4 * ( T2_cal * BIT_SCALE_FACTOR) / T2_cal)); + Vx = 0; Vy = 0; } @@ -79,3 +75,11 @@ calculG ( uint16_t T1, uint16_t T2, int16_t Go) */ return (K * T1) / T2 - Go ; } +int16_t +calculGb(uint16_t T1, uint16_t T2, uint16_t T1cal) +{ + int32_t Zactual = T1cal * T2 / T2_cal; + int32_t Gtmp = K * (T1 - Zactual); + int16_t G = Gtmp / T2; + return G; +} -- cgit v1.2.3