Utilisation de plusieurs gyroscopes

CMakeLists.txt

@@ -10,6 +10,7 @@ spi_nb.c
 gyro.c
 Temps.c
 Servomoteur.c
+gyro_L3GD20H.c
 )
 pico_enable_stdio_usb(test 1)
 pico_enable_stdio_uart(test 1)

graph_enr01.gp

@@ -1,4 +1,4 @@
-set title "Observation des 3 axes\nT_{acq} : 5 ms"
+set title "Observation des 3 axes\nT_{acq} : 5 ms - filtre 15 secondes"
 
 set xlabel "Temps (s)"
 set ylabel "Vitesse (°/s)"

graph_enr02.gp

@@ -1,4 +1,4 @@
-set title "Observation des 3 axes\nT_{acq} : 5 ms"
+set title "Observation des 3 axes\nT_{acq} : 5 ms - filtre 15 secondes"
 
 set xlabel "Temps (s)"
 set ylabel "Vitesse (°/s)"

gyro.c

@@ -7,17 +7,26 @@
 #include "Temps.h"
 #include "gyro.h"
 
-struct t_angle_gyro{
+#define GYRO_L3GD20H
-    int32_t rot_x, rot_y, rot_z, temp;
+
-};
+#ifdef GYRO_L3GD20H
+    #include "gyro_L3GD20H.h"
+#else
+    #ifdef GYRO_ADXRS453
+        #include "gyro_ADXRS453.h"
+    #else
+        #error "Choissisez un gyroscope"
+    #endif
+#endif
+
 
 /// @brief structure d'échange des angles du gyrocope
-struct t_angle_gyro vitesse_angulaire, vitesse_calibration;
+struct t_angle_gyro _vitesse_calibration;
+struct t_angle_gyro *vitesse_calibration;
+struct t_angle_gyro_double _vitesse_angulaire;
+struct t_angle_gyro_double *vitesse_angulaire;
 
-int gyro_init_check();
-void gyro_config();
 int gyro_read_register_blocking(uint8_t registrer, uint8_t *tampon, uint8_t nb_a_lire);
-void gyro_get_angles(struct t_angle_gyro* angle_gyro, struct t_angle_gyro* angle_gyro_moy);
 void gyro_calibration(void);
 
 uint32_t rot_x_zero, rot_y_zero, rot_z_zero;
@@ -45,16 +54,14 @@ void Gyro_Init(void){
     gpio_set_dir(PIN_CS, GPIO_OUT);
     cs_deselect();
 
-    vitesse_calibration.rot_x = 0;
+    vitesse_calibration = NULL;
-    vitesse_calibration.rot_y = 0;
+    vitesse_angulaire = &_vitesse_angulaire;
-    vitesse_calibration.rot_z = 0;
-    vitesse_calibration.temp = 0;
 
     //spi_init(spi0, 100 * 1000); // SPI init @ 100 kHz
     uint speed = spi_init(spi0, 2 * 1000 * 1000); // SPI init @ 2 MHz
     printf("vitesse SPI : %d\n", speed);
 
-    //Ça doit être les valeurs par défaut, mais ça marche !
+
     spi_set_format(spi0, 8, SPI_CPHA_1, SPI_CPOL_1, SPI_MSB_FIRST);
 
     // Test de la présence du gyroscope :
@@ -62,7 +69,12 @@ void Gyro_Init(void){
         puts("Gyroscope non trouve");
     }else{
         puts("Gyroscope trouve");
-        gyro_config();        
+        if(!gyro_config()){
+            puts("gyro_config ok !");
+        }else{
+            printf("gyro_config FAILED !");
+
+        }
     }
     sleep_ms(150); // Temps d'init du gyroscope
     /*while(1){
@@ -70,102 +82,31 @@ void Gyro_Init(void){
     }*/
 }
 
-int gyro_init_check(){
-    // Renvoi 0 si l'initialisation s'est bien passée
-    // Renvoi 1 si le gyroscope n'a pas répondu
-    uint8_t tampon[2]="";
-    gyro_read_register_blocking(0x0F, tampon, 1);
-    if(tampon[0] == 0xd7){
-        return 0;
-    }
-    return 1;
-}
-
-void gyro_config(){
-    // Registre CTRL1
-    // DR : 11
-    // BW : 10
-    // PD : 1
-    // Zen : 1
-    // Yen : 1
-    // Xen : 1
-
-    uint8_t config = 0b11101111;
-    uint16_t tampon[2] = {0x20, config};
-    uint8_t tampon2[10]="\0\0\0\0\0\0\0\0\0";
-    int statu, nb_read;
-
-    //while(spi_nb_busy(spi0) == SPI_BUSY);
-    cs_select();
-    int rep = spi_nb_write_data(spi0, tampon, 2);
-    if(rep == SPI_ERR_TRANSMIT_FIFO_FULL){
-        printf("Erreur: spi_read_register: SPI_ERR_TRANSMIT_FIFO_FULL\n");
-        //return statu;
-    }
-    while(spi_nb_busy(spi0));
-    cs_deselect();
-
-    int nb_lu = spi_read_register(spi0, 0x20, tampon2, 1);
-    
-
-
-    printf("Nb lu: %d\n", nb_lu);
-    
-    if(tampon2[1] == config){
-        puts("gyro_config ok !");
-    }else{
-        printf("gyro_config FAILED ! :%#4x\n", tampon2[1]);
-    }
-    // Registre 
-
-
-}
-
-
-int gyro_read_register_blocking(uint8_t registrer, uint8_t *tampon, uint8_t nb_a_lire){
-    uint8_t reg = registrer | 0xC0 ;
-    int nb_recu;
-    cs_select();
-    spi_write_blocking(spi0, &reg, 1);
-    sleep_ms(10);
-    nb_recu = spi_read_blocking(spi0, 0, tampon, nb_a_lire);
-    cs_deselect();
-    
-}
-
 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;
 
-    //spi_read_register(spi0, 0x20, tampon, 1);
+    // Acquisition des valeurs
-    //printf ("Gyro CTRL1 (bis) : %#4x\n", tampon[1] );
+    gyro_get_vitesse_brute(_vitesse_angulaire_brute, vitesse_calibration);
-
-    //printf ("RPI SSPCPSR : %#4x\n", spi_get_hw(spi0)->cpsr );
-    //printf ("RPI SSPCR0 : %#4x\n", spi_get_hw(spi0)->cr0 );
-
-    gyro_get_angles(&vitesse_angulaire, &vitesse_calibration);
     //gyro_get_angles(&vitesse_angulaire, NULL);
 
-    // Angle en degré
-    vitesse_gyro.rot_x = (double)vitesse_angulaire.rot_x * 0.00875 / 32.0;
-    vitesse_gyro.rot_y = (double)vitesse_angulaire.rot_y * 0.00875 / 32.0;
-    vitesse_gyro.rot_z = (double)vitesse_angulaire.rot_z * 0.00875 / 32.0;
 
-    angle_gyro.rot_x = angle_gyro.rot_x + vitesse_gyro.rot_x * step_ms * 0.001;
+    // conversion de la vitesse angulaire en degré/seconde
-    angle_gyro.rot_y = angle_gyro.rot_y + vitesse_gyro.rot_y * step_ms * 0.001;
+    gyro_get_vitesse_normalisee(_vitesse_angulaire_brute, vitesse_angulaire);
-    angle_gyro.rot_z = angle_gyro.rot_z + vitesse_gyro.rot_z * step_ms * 0.001;
 
-    //printf("%d, %#4x, %#4x, %#4x\n", step_ms, vitesse_angulaire.rot_x, vitesse_angulaire.rot_y, vitesse_angulaire.rot_z);
+    vitesse_gyro = *vitesse_angulaire;
 
-    //printf("rx : %f, ry : %f, rz: %f\n", angle_gyro.rot_x, angle_gyro.rot_y, angle_gyro.rot_z);
+    // Intégration en fonction du pas de temps
-
+    angle_gyro.rot_x = angle_gyro.rot_x + vitesse_angulaire->rot_x * step_ms * 0.001;
-    //while(spi_nb_busy(spi0));
+    angle_gyro.rot_y = angle_gyro.rot_y + vitesse_angulaire->rot_y * step_ms * 0.001;
-    //spi_nb_read_data_8bits(spi0,tampon);
+    angle_gyro.rot_z = angle_gyro.rot_z + vitesse_angulaire->rot_z * step_ms * 0.001;
-    //printf("tampon : %s\n", tampon);
 }
 
 int16_t gyro_get_temp(void){
@@ -177,28 +118,6 @@ int16_t gyro_get_temp(void){
 
 }
 
-void gyro_get_angles(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;
-    spi_read_register(spi0, 0x28, tampon, 6);
-    
-    rot_x = -(tampon[1] + (tampon[2] << 8));
-    rot_y = -(tampon[3] + (tampon[4] << 8));
-    rot_z = -(tampon[5] + (tampon[6] << 8));
-
-    if(angle_gyro_moy == NULL){
-        angle_gyro->rot_x = (int32_t) rot_x * 32;
-        angle_gyro->rot_y = (int32_t) rot_y * 32;
-        angle_gyro->rot_z = (int32_t) rot_z * 32;
-    }else{
-        angle_gyro->rot_x = (int32_t) rot_x * 32 - angle_gyro_moy->rot_x;
-        angle_gyro->rot_y = (int32_t) rot_y * 32 - angle_gyro_moy->rot_y;
-        angle_gyro->rot_z = (int32_t) rot_z * 32 - angle_gyro_moy->rot_z;
-    }
-}
-
-
-
 void gyro_affiche(struct t_angle_gyro_double angle_gyro, char * titre){
     if(titre != NULL){
         printf("%s ",titre);
@@ -212,21 +131,23 @@ void gyro_calibration(void){
     uint32_t m_temps_ms = Temps_get_temps_ms();
     uint32_t temps_500ms = m_temps_ms;
     int16_t temperature;
+    struct t_angle_gyro vitesse_grute_gyro;
 
     printf("Calibration...\n");
+    vitesse_calibration = &_vitesse_calibration;
 
-    vitesse_calibration.rot_x = 0;
+    vitesse_calibration->rot_x = 0;
-    vitesse_calibration.rot_y = 0;
+    vitesse_calibration->rot_y = 0;
-    vitesse_calibration.rot_z = 0;
+    vitesse_calibration->rot_z = 0;
     
     // Acquisition des échantillons, 1 par milliseconde (1 ms, c'est trop court on dirait !)
     for(uint32_t i=0; i<nb_ech; i++){
         while(m_temps_ms == Temps_get_temps_ms());
         m_temps_ms = Temps_get_temps_ms();
-        gyro_get_angles(&vitesse_angulaire, NULL);
+        gyro_get_vitesse_brute(&vitesse_grute_gyro, NULL);
-        vitesse_calibration.rot_x += vitesse_angulaire.rot_x;
+        vitesse_calibration->rot_x += vitesse_grute_gyro.rot_x;
-        vitesse_calibration.rot_y += vitesse_angulaire.rot_y;
+        vitesse_calibration->rot_y += vitesse_grute_gyro.rot_y;
-        vitesse_calibration.rot_z += vitesse_angulaire.rot_z;
+        vitesse_calibration->rot_z += vitesse_grute_gyro.rot_z;
         if(m_temps_ms > temps_500ms){
             printf(".");
             gyro_get_temp();
@@ -234,12 +155,12 @@ void gyro_calibration(void){
         }
         sleep_ms(5);
     }
-    vitesse_calibration.rot_x = vitesse_calibration.rot_x / (int32_t)nb_ech;
+    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_y = vitesse_calibration->rot_y / (int32_t)nb_ech;
-    vitesse_calibration.rot_z = vitesse_calibration.rot_z / (int32_t)nb_ech;
+    vitesse_calibration->rot_z = vitesse_calibration->rot_z / (int32_t)nb_ech;
     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);
 
 }
 

gyro.h

@@ -1,6 +1,4 @@
-struct t_angle_gyro_double{
+#include "gyro_data.h"
-    double rot_x, rot_y, rot_z;
-};
 
 void Gyro_Init(void);
 void Gyro_Read(u_int16_t);

gyro_ADXRS473.c

@@ -0,0 +1,291 @@
+#include "gyro_ADXRS473.h"
+#include "spi_nb.h"
+#include <stdio.h>
+
+#define NB_MAX_CHAR_GYRO 4
+
+struct {
+    unsigned short SQ:3;
+    unsigned short ST:2;
+    unsigned short P0:1;
+    unsigned short P1:1;
+    unsigned short PLL:1;
+    unsigned short Q:1;
+    unsigned short NVM:1;
+    unsigned short POR:1;
+    unsigned short PWR:1;
+    unsigned short CST:1;
+    unsigned short CHK:1;
+    signed int rateData;
+} Gyro_SensorData;
+
+void Gyro_traitementDonnees(unsigned char * tamponRecu);
+unsigned char pariteOctet(unsigned char octet);
+
+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();
+
+    // 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();
+    
+}
+
+int gyro_init_check(){
+    // Renvoi 0 si l'initialisation s'est bien passée
+    // Renvoi 1 si le gyroscope n'a pas répondu
+    uint8_t tampon[5]="\0\0\0\0\0";
+    gyro_read_register_blocking(0x0C, tampon, 1);
+    Gyro_traitementDonnees(tampon);
+
+    printf("Init check : %#06x\n", Gyro_SensorData.rateData);
+    
+/*    if(tampon[0] == 0xd7){
+        return 0;
+    }*/
+    return 1;
+}
+
+
+int gyro_config(){
+    // Registre CTRL1
+    // DR : 11
+    // BW : 10
+    // PD : 1
+    // Zen : 1
+    // Yen : 1
+    // Xen : 1
+
+    uint8_t config = 0b11101111;
+    uint16_t tampon[2] = {0x20, config};
+    uint8_t tampon2[10]="\0\0\0\0\0\0\0\0\0";
+    int statu, nb_read;
+
+    //while(spi_nb_busy(spi0) == SPI_BUSY);
+    cs_select();
+    int rep = spi_nb_write_data(spi0, tampon, 2);
+    if(rep == SPI_ERR_TRANSMIT_FIFO_FULL){
+        printf("Erreur: spi_read_register: SPI_ERR_TRANSMIT_FIFO_FULL\n");
+        //return statu;
+    }
+    while(spi_nb_busy(spi0));
+    cs_deselect();
+
+    int nb_lu = spi_read_register(spi0, 0x20, tampon2, 1);
+    
+
+
+    printf("Nb lu: %d\n", nb_lu);
+    
+    if(tampon2[1] == config){
+        //puts("gyro_config ok !");
+        return 0;
+    }else{
+        //printf("gyro_config FAILED ! :%#4x\n", tampon2[1]);
+        return 1;
+    }
+    // Registre 
+
+
+}
+
+
+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;
+    spi_read_register(spi0, 0x28, tampon, 6);
+    
+    rot_x = -(tampon[1] + (tampon[2] << 8));
+    rot_y = -(tampon[3] + (tampon[4] << 8));
+    rot_z = -(tampon[5] + (tampon[6] << 8));
+
+    if(angle_gyro_moy == NULL){
+        angle_gyro->rot_x = (int32_t) rot_x * 32;
+        angle_gyro->rot_y = (int32_t) rot_y * 32;
+        angle_gyro->rot_z = (int32_t) rot_z * 32;
+    }else{
+        angle_gyro->rot_x = (int32_t) rot_x * 32 - angle_gyro_moy->rot_x;
+        angle_gyro->rot_y = (int32_t) rot_y * 32 - angle_gyro_moy->rot_y;
+        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,
+        struct t_angle_gyro_double * _vitesse_gyro){
+    _vitesse_gyro->rot_x = (double)_vitesse_angulaire->rot_x * 0.00875 / 32.0;
+    _vitesse_gyro->rot_y = (double)_vitesse_angulaire->rot_y * 0.00875 / 32.0;
+    _vitesse_gyro->rot_z = (double)_vitesse_angulaire->rot_z * 0.00875 / 32.0;
+}
+
+
+
+
+inline unsigned char Gyro_commande_SensorData(unsigned char autotest){
+    // On met SQ2 à 1 afin de différencier facilement une erreur et des données
+    uint8_t tamponGyroscopeEnvoi[4];
+    tamponGyroscopeEnvoi[0] = 0x30;
+    tamponGyroscopeEnvoi[1] = 0x00;
+    tamponGyroscopeEnvoi[2] = 0x00;
+    if (autotest){
+        tamponGyroscopeEnvoi[3] = 0x03;
+    }else{
+        tamponGyroscopeEnvoi[3] = 0x01;
+    }
+    // La parité, dans ce cas est triviale, autant prévoir tous les cas
+    //Gyro_commande_PariteData(tamponGyroscopeEnvoi);
+    //return SPI_envData(tamponGyroscopeEnvoi);
+}
+
+void Gyro_commande_PariteData(unsigned char* tampon){
+    unsigned char parite=0,i;
+    // Obtention de la parité actuelle
+    for(i=0 ; i< NB_MAX_CHAR_GYRO ; i++){
+        parite ^= pariteOctet(tampon[i]);
+    }
+    // On veut une parité impaire
+    parite ^= 0x01;
+
+    // On insere ce bit dans le message, au bon endroit
+    tampon[NB_MAX_CHAR_GYRO-1] = tampon[NB_MAX_CHAR_GYRO-1] | parite;
+}
+
+unsigned char pariteOctet(unsigned char octet){
+    unsigned char parite=0,i;
+    for (i=0 ; i<8 ; i++){
+        parite ^= octet & 0x01;
+        octet = octet >> 1;
+    }
+    return parite;
+}
+
+void Gyro_traitementDonnees(unsigned char * tamponRecu){
+    Gyro_SensorData.SQ = (tamponRecu[0]>>5) & 0x07;
+    Gyro_SensorData.P0 = (tamponRecu[0]>>4) & 0x01;
+    Gyro_SensorData.ST = (tamponRecu[0]>>2) & 0x03;
+    Gyro_SensorData.rateData = (int) 
+            ( (0xC000 &((unsigned int) (tamponRecu[0] & 0x03)<<14)) |
+            ( 0x3FC0 & ((unsigned int) tamponRecu[1] << 6)) |
+           ( 0x003F & (unsigned int) ( tamponRecu[2] >> 2)));
+    Gyro_SensorData.PLL = (tamponRecu[3] & 0x80) >> 7;
+    Gyro_SensorData.Q   = (tamponRecu[3] & 0x40) >> 6;
+    Gyro_SensorData.NVM = (tamponRecu[3] & 0x20) >> 5;
+    Gyro_SensorData.POR = (tamponRecu[3] & 0x10) >> 4;
+    Gyro_SensorData.PWR = (tamponRecu[3] & 0x08) >> 3;
+    Gyro_SensorData.CST = (tamponRecu[3] & 0x04) >> 2;
+    Gyro_SensorData.CHK = (tamponRecu[3] & 0x02) >> 1;
+    Gyro_SensorData.P1  = (tamponRecu[3] & 0x01);
+
+}
+/*
+unsigned char Gyro_gestion(void){
+    Gyro_commande_SensorData(0);
+    while(!SPI_recData(GyroscopeReception));
+    
+    Gyro_traitementDonnees(GyroscopeReception);
+    if (Gyro_SensorData.SQ & 0x04){
+        Gyro_Angle +=(long) (Gyro_SensorData.rateData - angle0);
+        //Gyro_Angle = angle0;
+    }else{
+        Gyro_Angle = (long)0x3333;
+    }
+    return 0;
+}
+inline unsigned char Gyro_gestion_nb(){
+    if(SPI_recData(GyroscopeReception)){
+        Gyro_traitementDonnees(GyroscopeReception);
+        if (Gyro_SensorData.SQ & 0x04)
+            // calcul du nouvel angle
+            // TODO : Améliorer la stabilitée en augmentant la précision
+            Gyro_Angle +=(long) ((long)Gyro_SensorData.rateData - (long)angle0);
+        return 0;
+    }
+    return 1;
+
+}
+
+int Gyro_getAngle(void){
+    // 80° par secondes
+    // 5 kHz => 200 µs
+    // Gyro_Angle en 1,6 e-2 degré
+    return (int)(-Gyro_Angle / 5000 / 80);
+}
+long Gyro_getRawAngle(void){
+    // 80° par secondes
+    // 5 kHz => 200 µs
+    // Gyro_Angle en 1,6 e-2 degré
+    return Gyro_Angle ;
+}
+long double Gyro_getAngleRadian(void){
+    // 80° par secondes
+    // 5 kHz => 200 µs
+    // Gyro_Angle en 1,6 e-2 degré
+    return -Gyro_Angle * GYRO_COEF_RADIAN_5kHz;
+}
+unsigned char * Gyro_getRawData(){
+    return GyroscopeReception;
+}
+
+int Gyro_init(){
+    
+    long long calcul_angle0;
+    
+    int i, erreur_gyro;
+    
+    Gyro_Timer_ms=100;
+    while(Gyro_Timer_ms);
+    // Envoie message auto-test des test
+    while(!Gyro_commande_SensorData(1));
+    while(!SPI_recData(GyroscopeReception));
+    
+    // Attente 50 ms - les tests doivent indiquer des erreur
+    Gyro_Timer_ms=50;
+    while(Gyro_Timer_ms);
+    while(!Gyro_commande_SensorData(0));
+    while(!SPI_recData(GyroscopeReception));
+
+    // Attente 50 ms - les erreurs doivent s'être effacées
+    Gyro_Timer_ms=50;
+    while(Gyro_Timer_ms);
+    while(!Gyro_commande_SensorData(0));
+    while(!SPI_recData(GyroscopeReception));
+
+    // Calibration du gyroscope
+    calcul_angle0 = 0;
+    i=0;
+    erreur_gyro = 0;
+    while((i<NB_ACQ_CALIBRATION) && (erreur_gyro < NB_MAX_ERREUR_GYRO)){
+        while(!Gyro_commande_SensorData(0));
+        while(!SPI_recData(GyroscopeReception));
+        Gyro_traitementDonnees(GyroscopeReception);
+        if (Gyro_SensorData.SQ & 0x04){
+            calcul_angle0 += Gyro_SensorData.rateData;
+            erreur_gyro = 0;
+            i++;
+        }else{
+            erreur_gyro++;
+        }
+        __delay32(2000); // 50 µs
+    }
+    if (erreur_gyro < NB_MAX_ERREUR_GYRO){
+        erreur_gyro = 0;
+        // TODO : Améliorer la stabilitée en augmentant la précision
+        angle0 = (long)(calcul_angle0 / NB_ACQ_CALIBRATION);
+        Gyro_Pret=1;
+    }else{
+        erreur_gyro = 1;
+    }
+    
+
+    return erreur_gyro;
+}
+*/

gyro_ADXRS473.h

@@ -0,0 +1,6 @@
+#include "gyro_data.h"
+
+int gyro_init_check();
+int gyro_config();
+void gyro_get_vitesse_brute(struct t_angle_gyro* angle_gyro, struct t_angle_gyro* angle_gyro_moy);
+void gyro_get_vitesse_normalisee(struct t_angle_gyro* _vitesse_angulaire, struct t_angle_gyro_double * vitesse_gyro);

gyro_L3GD20H.c

@@ -0,0 +1,96 @@
+#include "gyro_L3GD20H.h"
+#include "spi_nb.h"
+#include <stdio.h>
+
+int gyro_read_register_blocking(uint8_t registrer, uint8_t *tampon, uint8_t nb_a_lire){
+    uint8_t reg = registrer | 0xC0 ;
+    int nb_recu;
+    cs_select();
+    spi_write_blocking(spi0, &reg, 1);
+    sleep_ms(10);
+    nb_recu = spi_read_blocking(spi0, 0, tampon, nb_a_lire);
+    cs_deselect();
+    
+}
+
+int gyro_init_check(){
+    // Renvoi 0 si l'initialisation s'est bien passée
+    // Renvoi 1 si le gyroscope n'a pas répondu
+    uint8_t tampon[2]="";
+    gyro_read_register_blocking(0x0F, tampon, 1);
+    if(tampon[0] == 0xd7){
+        return 0;
+    }
+    return 1;
+}
+
+
+int gyro_config(){
+    // Registre CTRL1
+    // DR : 11
+    // BW : 10
+    // PD : 1
+    // Zen : 1
+    // Yen : 1
+    // Xen : 1
+
+    uint8_t config = 0b11101111;
+    uint16_t tampon[2] = {0x20, config};
+    uint8_t tampon2[10]="\0\0\0\0\0\0\0\0\0";
+    int statu, nb_read;
+
+    //while(spi_nb_busy(spi0) == SPI_BUSY);
+    cs_select();
+    int rep = spi_nb_write_data(spi0, tampon, 2);
+    if(rep == SPI_ERR_TRANSMIT_FIFO_FULL){
+        printf("Erreur: spi_read_register: SPI_ERR_TRANSMIT_FIFO_FULL\n");
+        //return statu;
+    }
+    while(spi_nb_busy(spi0));
+    cs_deselect();
+
+    int nb_lu = spi_read_register(spi0, 0x20, tampon2, 1);
+    
+
+
+    printf("Nb lu: %d\n", nb_lu);
+    
+    if(tampon2[1] == config){
+        //puts("gyro_config ok !");
+        return 0;
+    }else{
+        //printf("gyro_config FAILED ! :%#4x\n", tampon2[1]);
+        return 1;
+    }
+    // Registre 
+
+
+}
+
+
+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;
+    spi_read_register(spi0, 0x28, tampon, 6);
+    
+    rot_x = -(tampon[1] + (tampon[2] << 8));
+    rot_y = -(tampon[3] + (tampon[4] << 8));
+    rot_z = -(tampon[5] + (tampon[6] << 8));
+
+    if(angle_gyro_moy == NULL){
+        angle_gyro->rot_x = (int32_t) rot_x * 32;
+        angle_gyro->rot_y = (int32_t) rot_y * 32;
+        angle_gyro->rot_z = (int32_t) rot_z * 32;
+    }else{
+        angle_gyro->rot_x = (int32_t) rot_x * 32 - angle_gyro_moy->rot_x;
+        angle_gyro->rot_y = (int32_t) rot_y * 32 - angle_gyro_moy->rot_y;
+        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,
+        struct t_angle_gyro_double * _vitesse_gyro){
+    _vitesse_gyro->rot_x = (double)_vitesse_angulaire->rot_x * 0.00875 / 32.0;
+    _vitesse_gyro->rot_y = (double)_vitesse_angulaire->rot_y * 0.00875 / 32.0;
+    _vitesse_gyro->rot_z = (double)_vitesse_angulaire->rot_z * 0.00875 / 32.0;
+}

gyro_L3GD20H.h

@@ -0,0 +1,6 @@
+#include "gyro_data.h"
+
+int gyro_init_check();
+int gyro_config();
+void gyro_get_vitesse_brute(struct t_angle_gyro* angle_gyro, struct t_angle_gyro* angle_gyro_moy);
+void gyro_get_vitesse_normalisee(struct t_angle_gyro* _vitesse_angulaire, struct t_angle_gyro_double * vitesse_gyro);

gyro_data.h

@@ -0,0 +1,14 @@
+#include "pico/stdlib.h"
+
+#ifndef GYRO_DATA_H
+#define GYRO_DATA_H
+
+struct t_angle_gyro_double{
+    double rot_x, rot_y, rot_z;
+};
+
+struct t_angle_gyro{
+    int32_t rot_x, rot_y, rot_z, temp;
+};
+
+#endif

test.c

@@ -38,7 +38,7 @@ int main() {
     temps_ms_old = temps_ms;
     while (1) { 
         u_int16_t step_ms = 5;
-        float coef_filtre = 0.001;
+        float coef_filtre = 0.977;
         
         while(temps_ms_old == Temps_get_temps_ms());
         temps_ms_old = Temps_get_temps_ms();