summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorburg2004-09-21 16:41:08 +0000
committerburg2004-09-21 16:41:08 +0000
commit2f9746bd5c5dd83b66dbd832d34c22ab4afde10a (patch)
treec0604c71a20978a3543b9ab22022f1e2320f6ccc
parente13b9ba42de78ef57f11ad0a2f45f4f4648fce77 (diff)
Correction de la méthode de calcul de l'accélération.
-rw-r--r--n/accel/accel.c16
-rw-r--r--n/accel/calcul.c30
-rw-r--r--n/accel/calcul.h4
3 files changed, 28 insertions, 22 deletions
diff --git a/n/accel/accel.c b/n/accel/accel.c
index eaa848a..fb63fc9 100644
--- a/n/accel/accel.c
+++ b/n/accel/accel.c
@@ -130,6 +130,8 @@ test_callback (uint8_t c, uint8_t argc, proto_arg_t argv[])
break;
case 'y' : proto_send2('Y',Ty >> 8, Tx);
break;
+ case 'r' : proto_send2('R',T2 >> 8, T2);
+ break;
case 't' : proto_send2('T',Timer1 >> 8, Timer1);
break;
case 'g' : proto_send2('x',Gx >> 8, Gx);
@@ -142,7 +144,7 @@ test_callback (uint8_t c, uint8_t argc, proto_arg_t argv[])
rs232_putc('V');
proto_send2('y',Vy >> 8,Vy);
break;
- case 'z' : reset;
+ case 'z' : reset();
default : proto_send1('E',2); //erreur 2 Command incorrect
}
}
@@ -161,12 +163,12 @@ int
main (void)
{
//initialisation
- rs232_init();
+ rs232_init();
proto_init(test_callback,rs232_putc);
proto_send0('Z');
- //initialisation du timer prescaler 254
+ //initialisation du timer prescaler 256
TCCR1B = _BV(ICES1) | _BV(CS12);
// Input Capture Interrupt enable & Timer 1 overflow
TIMSK = _BV(TICIE1) | _BV(TOIE1);
@@ -189,10 +191,10 @@ main (void)
T2 = (Td + Tc - Tb) /2;
// remise de TCNT1 pour mesurer le temps
TCNT1 = 0;
- Gx = calculG(Tx,T2_cal,G0x);
- Gy = calculG(Ty,T2_cal,G0y);
- Vx += (Gx) >> 8;
- Vy += (Gy) >> 8;
+ Gx = calculGb(Tx,T2,T1x_cal);
+ Gy = calculGb(Ty,T2,T1y_cal);
+ Vx +=Gx;
+ Vy +=Gy;
Timer1 = TCNT1;
//remise dans l'état de capture
//Todo : remettre dans cet état plutôt
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;
+}
diff --git a/n/accel/calcul.h b/n/accel/calcul.h
index e474fa5..27d0698 100644
--- a/n/accel/calcul.h
+++ b/n/accel/calcul.h
@@ -32,7 +32,7 @@
*/
#include <inttypes.h>
-#define BIT_SCALE_FACTOR 60000
+#define BIT_SCALE_FACTOR 4096
enum T_etat {strt_at_Ta, rd_at_Tb, rd_at_Tc, rd_at_Td, calcul };
@@ -53,6 +53,6 @@ extern volatile enum T_etat etat;
*/
void calibration(void);
int16_t calculG(uint16_t T1, uint16_t T2,int16_t Zcal);
-
+int16_t calculGb(uint16_t T1, uint16_t T2,uint16_t T1cal);
#endif /* calcul_h */