diff --git a/gyro.c b/gyro.c index 1c0cbaa..5e734b9 100644 --- a/gyro.c +++ b/gyro.c @@ -73,7 +73,7 @@ void Gyro_Init(void){ if(!gyro_config()){ puts("gyro_config ok !"); }else{ - printf("gyro_config FAILED !"); + puts("gyro_config FAILED !"); } } @@ -84,13 +84,8 @@ void Gyro_Init(void){ } void Gyro_Read(uint16_t step_ms){ - uint8_t tampon[10]="\0\0\0\0\0\0\0\0\0"; - uint8_t tampon2[10]="ABCDEFGHI"; - int16_t rot_x, rot_y, rot_z; - static double angle_x=0, angle_y=0, angle_z=0; struct t_angle_gyro * _vitesse_angulaire_brute; struct t_angle_gyro m_vitesse_angulaire_brute; - int nb_recu; _vitesse_angulaire_brute = &m_vitesse_angulaire_brute; @@ -98,7 +93,7 @@ void Gyro_Read(uint16_t step_ms){ gyro_get_vitesse_brute(_vitesse_angulaire_brute, vitesse_calibration); //gyro_get_angles(&vitesse_angulaire, NULL); - + /* // conversion de la vitesse angulaire en degré/seconde gyro_get_vitesse_normalisee(_vitesse_angulaire_brute, vitesse_angulaire); @@ -108,6 +103,7 @@ void Gyro_Read(uint16_t step_ms){ angle_gyro.rot_x = angle_gyro.rot_x + vitesse_angulaire->rot_x * step_ms * 0.001; angle_gyro.rot_y = angle_gyro.rot_y + vitesse_angulaire->rot_y * step_ms * 0.001; angle_gyro.rot_z = angle_gyro.rot_z + vitesse_angulaire->rot_z * step_ms * 0.001; + */ } int16_t gyro_get_temp(void){ diff --git a/gyro_ADXRS453.c b/gyro_ADXRS453.c index 409736c..5487ad0 100644 --- a/gyro_ADXRS453.c +++ b/gyro_ADXRS453.c @@ -40,28 +40,6 @@ int gyro_spi_wr_32bits(uint8_t *transmit_buffer, uint8_t *recieve_buffer){ cs_deselect(); } -int gyro_read_register_blocking(uint8_t registrer, uint8_t *tampon, uint8_t nb_a_lire){ - uint8_t tampon_envoi[4]="\0\0\0\0"; - int nb_recu; - tampon_envoi[0] = registrer; - - // Envoie commande N - cs_select(); - spi_write_blocking(spi0, tampon_envoi, 4); - nb_recu = spi_read_blocking(spi0, 0, tampon, nb_a_lire); - cs_deselect(); - - // A faire passer à 0,1 µs - sleep_us(1); - - // lire reponse N - cs_select(); - spi_write_blocking(spi0, tampon_envoi, 4); - nb_recu = spi_read_blocking(spi0, 0, tampon, nb_a_lire); - cs_deselect(); - -} - void affiche_tampon_32bits(uint8_t *tampon){ uint32_t valeur; valeur = (tampon[0] << 24) + (tampon[1] << 16) + (tampon[2]<<8) + tampon[3]; @@ -70,10 +48,7 @@ void affiche_tampon_32bits(uint8_t *tampon){ } -int gyro_get_sensor_data(){ - uint8_t tampon_envoi[5]="\0\0\0\0\0"; - uint8_t tampon_reception[5]="\0\0\0\0\0"; - +int gyro_get_sensor_data(uint8_t tampon_envoi[], uint8_t tampon_reception[]){ tampon_envoi[0] = 0x30; tampon_envoi[1] = 0x00; tampon_envoi[2] = 0x00; @@ -81,12 +56,12 @@ int gyro_get_sensor_data(){ gyro_spi_wr_32bits(tampon_envoi, tampon_reception); Gyro_traitementDonnees(tampon_reception); if(Gyro_SensorData.SQ != 0x4){ - printf("Gyro_Data - SQ bits (%#01x)!= 0x4\n", Gyro_SensorData.SQ); + printf("Gyro Failed - SQ bits (%#3x)!= 0x4\n", Gyro_SensorData.SQ); affiche_tampon_32bits(tampon_reception); return 1; } if(Gyro_SensorData.ST != 0x1){ - printf("Gyro_Data - Status (%#01x)!= 0x1\n", Gyro_SensorData.ST); + printf("Gyro Failed - Status (%#3x)!= 0x1\n", Gyro_SensorData.ST); affiche_tampon_32bits(tampon_reception); return 1; } @@ -171,25 +146,41 @@ int gyro_init_check(){ sleep_us(1); // t=200ms + 2TD printf("T=200ms+2TD\n"); - tampon_envoi[0] = 0x30; + tampon_envoi[0] = 0x00; tampon_envoi[1] = 0x00; tampon_envoi[2] = 0x00; - tampon_envoi[3] = 0x01; - gyro_spi_wr_32bits(tampon_envoi, tampon_reception); - Gyro_traitementDonnees(tampon_reception); - if(Gyro_SensorData.SQ != 0x4){ - printf("Gyro_Init - SQ bits (%#01x)!= 0x4\n", Gyro_SensorData.SQ); - affiche_tampon_32bits(tampon_reception); + tampon_envoi[3] = 0x00; + if(gyro_get_sensor_data(tampon_envoi, tampon_reception)){ return 1; } - if(Gyro_SensorData.ST != 0x1){ - printf("Gyro_Init - Status (%#01x)!= 0x1\n", Gyro_SensorData.ST); - affiche_tampon_32bits(tampon_reception); - return 1; + + + for(int i=3; i<10; i++){ + sleep_us(1); // t=200ms + 3TD + printf("T=200ms+%dTD\n", i); + if(gyro_get_sensor_data(tampon_envoi, tampon_reception)){ + return 1; + } } - affiche_tampon_32bits(tampon_reception); + + for(int i=10; i<20; i++){ + gyro_get_vitesse_brute(NULL, NULL); + } + + + for(int i=20; i<30; i++){ + sleep_us(1); // t=200ms + 3TD + printf("T=200ms+%dTD\n", i); + if(gyro_get_sensor_data(tampon_envoi, tampon_reception)){ + //return 1; + } + } + //DEBUG + return 1; + return 0; + } @@ -200,16 +191,16 @@ int gyro_config(){ void gyro_get_vitesse_brute(struct t_angle_gyro* angle_gyro, struct t_angle_gyro* angle_gyro_moy){ - uint8_t tampon[10]="\0\0\0\0\0\0\0\0\0"; - int16_t rot_x, rot_y, rot_z; + uint8_t tampon_envoi[5]="\0\0\0\0\0"; + uint8_t tampon_reception[5]="\0\0\0\0\0"; + int16_t rot_z; - - if(gyro_get_sensor_data()){ - printf("GYRO : Erreur d'acquisition !\n"); + sleep_us(1); // A supprimer plus tard + printf("T=READ\n"); + if(gyro_get_sensor_data(tampon_envoi, tampon_reception)){ + return; } - - rot_x = 0; - rot_y = 0; + /* rot_z = Gyro_SensorData.rateData; if(angle_gyro_moy == NULL){ @@ -220,7 +211,7 @@ void gyro_get_vitesse_brute(struct t_angle_gyro* angle_gyro, struct t_angle_gyro angle_gyro->rot_x = 0; angle_gyro->rot_y = 0; angle_gyro->rot_z = (int32_t) rot_z * 32 - angle_gyro_moy->rot_z; - } + }*/ } void gyro_get_vitesse_normalisee(struct t_angle_gyro* _vitesse_angulaire, diff --git a/test.c b/test.c index 9ef51fc..c0a9a9d 100644 --- a/test.c +++ b/test.c @@ -46,6 +46,7 @@ int main() { // Tous les pas de step_ms if(Temps_get_temps_ms() % step_ms){ Gyro_Read(step_ms); + /* //gyro_affiche(gyro_get_vitesse(), "Angle :"); // Filtre angle_gyro = gyro_get_vitesse(); @@ -61,6 +62,7 @@ int main() { printf("%f, %f, %f, %f\n", (double)temps_ms_old / 1000, vitesse_filtre_x, vitesse_filtre_y, vitesse_filtre_z); //gyro_affiche(angle_gyro, "Vitesse (°/s),"); + */ } // Toutes les 50 ms