diff --git a/gyro.c b/gyro.c index 9d00172..2ae176d 100644 --- a/gyro.c +++ b/gyro.c @@ -78,9 +78,9 @@ void Gyro_Init(void){ } } sleep_ms(150); // Temps d'init du gyroscope - /*while(1){ - gyro_calibration(); - }*/ + + gyro_calibration(); + } void Gyro_Read(uint16_t step_ms){ @@ -123,8 +123,8 @@ void gyro_affiche(struct t_angle_gyro_double angle_gyro, char * titre){ } void gyro_calibration(void){ - uint32_t t_calibration_ms = 40000; - uint32_t nb_ech = t_calibration_ms/5; + uint32_t t_calibration_ms = 10000; + uint32_t nb_ech = t_calibration_ms/2; uint32_t m_temps_ms = Temps_get_temps_ms(); uint32_t temps_500ms = m_temps_ms; int16_t temperature; @@ -145,19 +145,20 @@ void gyro_calibration(void){ vitesse_calibration->rot_x += vitesse_grute_gyro.rot_x; vitesse_calibration->rot_y += vitesse_grute_gyro.rot_y; vitesse_calibration->rot_z += vitesse_grute_gyro.rot_z; - if(m_temps_ms > temps_500ms){ + /*if(m_temps_ms > temps_500ms){ printf("."); gyro_get_temp(); temps_500ms += 500; - } - sleep_ms(5); + }*/ + sleep_ms(2); } vitesse_calibration->rot_x = vitesse_calibration->rot_x / (int32_t)nb_ech; vitesse_calibration->rot_y = vitesse_calibration->rot_y / (int32_t)nb_ech; vitesse_calibration->rot_z = vitesse_calibration->rot_z / (int32_t)nb_ech; - temperature = gyro_get_temp(); + //temperature = gyro_get_temp(); - printf("fin calibration, %d, %d, %d, %d\n", vitesse_calibration->rot_x, vitesse_calibration->rot_y ,vitesse_calibration->rot_z, temperature); + //printf("fin calibration, %d, %d, %d, %d\n", vitesse_calibration->rot_x, vitesse_calibration->rot_y ,vitesse_calibration->rot_z, temperature); + printf("fin calibration, %d\n", vitesse_calibration->rot_z); } diff --git a/test.c b/test.c index c863f41..5121ffb 100644 --- a/test.c +++ b/test.c @@ -46,7 +46,11 @@ int main() { // Tous les pas de step_ms if(!(temps_ms % step_ms)){ + uint64_t _time; + uint32_t time_after_read, time_after_filter, time_after_printf; + _time = time_us_64(); Gyro_Read(step_ms); + time_after_read = time_us_64() - _time; //gyro_affiche(gyro_get_vitesse(), "Angle :"); // Filtre @@ -60,15 +64,21 @@ int main() { vitesse_filtre_y = vitesse_filtre_y * (1-coef_filtre) + angle_gyro.rot_y * coef_filtre; vitesse_filtre_z = vitesse_filtre_z * (1-coef_filtre) + angle_gyro.rot_z * coef_filtre; } + time_after_filter = time_us_64() - _time; - printf("%f, %f\n", (double)temps_ms_old / 1000, vitesse_filtre_z); + //printf("%#x, %#x\n", (double)temps_ms_old / 1000, vitesse_filtre_z); + + //printf("%d, %d\n", temps_ms, (int32_t) (vitesse_filtre_z * 1000)); + time_after_printf = time_us_64() - _time; //gyro_affiche(angle_gyro, "Vitesse (°/s),"); } // Toutes les 50 ms if((Temps_get_temps_ms() % 50) == 0){ - + struct t_angle_gyro_double m_gyro; + m_gyro = gyro_get_angle(); + printf("%f, %f\n", (double)temps_ms / 1000, m_gyro.rot_z); } // Toutes les 500 ms