summaryrefslogtreecommitdiff
path: root/n/accel/calcul.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/accel/calcul.c')
-rw-r--r--n/accel/calcul.c31
1 files changed, 22 insertions, 9 deletions
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 ;
}