Distinction du cas où la localisation se fait sans gyroscope et celui où le gyroscope est en erreur

Localisation.c

@@ -45,7 +45,7 @@ void Localisation_gestion(){
     delta_x_ref_robot = (distance_roue_a_mm + distance_roue_b_mm - 2 * distance_roue_c_mm)  / 3.0;
     delta_y_ref_robot = (-distance_roue_a_mm + distance_roue_b_mm)  * RACINE_DE_3 / 3.0;
 
-    if(get_position_avec_gyroscope()){
+    if(get_position_avec_gyroscope() && !get_position_avec_gyroscope_error()){
         angle_gyro = gyro_get_angle_degres();
         position.angle_radian = angle_gyro.rot_z / 180. * M_PI ;
     }else{

Log.c

@@ -9,6 +9,7 @@ struct log_message_storage{
 } * log_message_envoi, log_message_storage_premier, *log_message_storage_courant;
 
 uint log_error = 0;
+enum Log_level log_level_reference;
 
 int store_new_message(struct Log_message_data message, struct log_message_storage * stockage);
 void envoi_message(struct Log_message_data message);
@@ -22,6 +23,7 @@ void Log_init(void){
     log_message_storage_courant = &log_message_storage_premier;
     log_message_envoi = &log_message_storage_premier;
 
+    log_level_reference = TELEPLOT;
 }
 
 
@@ -29,26 +31,29 @@ void Log_init(void){
 /// @param message : string, without '\n' at the end.
 /// @param log_level : can be in TELEPLOT, TRACE, DEBUG, INFO, WARN, ERROR, FATAL
 void Log_message(char * message, enum Log_level log_level){
-    /// Création de la structure de données
-    struct Log_message_data message_container;
-    if(strlen(message) > LOG_MAX_MESSAGE_SIZE){
-        strcpy(message_container.message, "MSG TOO LONG");
-    }else{
-        strcpy(message_container.message, message);
-    }
-    message_container.log_level = log_level;
-    message_container.timestamp = time_us_32() / 1000;
+    // On vérifie que le message a une urgence suffisante pour être enregistrée
+    if(log_level >= log_level_reference){
+        /// Création de la structure de données
+        struct Log_message_data message_container;
+        if(strlen(message) > LOG_MAX_MESSAGE_SIZE){
+            strcpy(message_container.message, "MSG TOO LONG");
+        }else{
+            strcpy(message_container.message, message);
+        }
+        message_container.log_level = log_level;
+        message_container.timestamp = time_us_32() / 1000;
 
-    /// Insertion de la structure dans la liste chaînée
-    struct log_message_storage* tmp_message_storage;
-    tmp_message_storage = (struct log_message_storage*) malloc(sizeof(struct log_message_storage));
-    if(tmp_message_storage != NULL){
-        tmp_message_storage->message = message_container;
-        tmp_message_storage->next = NULL;
-        log_message_storage_courant->next = tmp_message_storage;
-        log_message_storage_courant = log_message_storage_courant->next;
-    }else{
-        log_error |= LOG_ERROR_MEMORY_FULL;
+        /// Insertion de la structure dans la liste chaînée
+        struct log_message_storage* tmp_message_storage;
+        tmp_message_storage = (struct log_message_storage*) malloc(sizeof(struct log_message_storage));
+        if(tmp_message_storage != NULL){
+            tmp_message_storage->message = message_container;
+            tmp_message_storage->next = NULL;
+            log_message_storage_courant->next = tmp_message_storage;
+            log_message_storage_courant = log_message_storage_courant->next;
+        }else{
+            log_error |= LOG_ERROR_MEMORY_FULL;
+        }
     }
 }
 
@@ -72,6 +77,10 @@ void Log_get_full_log(){
     log_message_envoi = &log_message_storage_premier;
 }
 
-void envoi_message(struct Log_message_data message){
-    printf("%u ms:%s\n", message.timestamp, message.message);
+void envoi_message(struct Log_message_data message_data){
+    if(message_data.log_level == TELEPLOT){
+        printf("%s\n", message_data.message);
+    }else{
+        printf("%u ms:%s\n", message_data.timestamp, message_data.message);
+    }
 }

Robot_config.c

@@ -1,4 +1,5 @@
 int position_avec_gyroscope = 0;
+int position_avec_gyroscope_erreur = 0;
 
 int get_position_avec_gyroscope(void){
     return position_avec_gyroscope;
@@ -6,4 +7,16 @@ int get_position_avec_gyroscope(void){
 
 void set_position_avec_gyroscope(int _use_gyro){
     position_avec_gyroscope = _use_gyro;
-}
+}
+
+/// @brief Indique que le gyroscope est en erreur
+/// @param _erreur : 1 si en erreur
+void set_position_avec_gyroscope_error(int _erreur){
+    position_avec_gyroscope_erreur = _erreur;
+}
+
+/// @brief Indique si le gyroscope est en erreur
+/// @return 1 si en erreur, 0 sinon
+int get_position_avec_gyroscope_error(){
+    return position_avec_gyroscope_erreur;
+}

Robot_config.h

@@ -1,2 +1,4 @@
 int get_position_avec_gyroscope(void);
-void set_position_avec_gyroscope(int _use_gyro);
+void set_position_avec_gyroscope(int _use_gyro);
+void set_position_avec_gyroscope_error(int _erreur);
+int get_position_avec_gyroscope_error(void);

Test_gyro.c

@@ -1,4 +1,5 @@
 #include "pico/stdlib.h"
+#include "pico/multicore.h"
 #include "gyro.h"
 #include "gyro_ADXRS453.h"
 #include "Log.h"
@@ -49,15 +50,20 @@ int test_gyro_vitesse_brute(void){
     printf("Lecture vitesse brute\n");
     uint16_t tampon_envoi[4];
     uint8_t tampon_reception[4];
+    char message[LOG_MAX_MESSAGE_SIZE];
     Log_init();
     Gyro_init_spi();
     Gyro_init_config();
 
     struct t_angle_gyro angle_gyro;
+    multicore_launch_core1(affichage);
     printf("Debut acquisition\n");
     while(1){
         gyro_get_vitesse_brute(&angle_gyro, NULL);
-        printf("%2.2f rad\n", angle_gyro.rot_z);
+        if(time_us_32() % 100000){
+            sprintf(message,">angle:%d\n", angle_gyro.rot_z);
+            Log_message(message, TELEPLOT);
+        }
     }
 
 }

gyro.c

@@ -64,6 +64,8 @@ void Gyro_init_spi(){
     vitesse_angulaire = &_vitesse_angulaire;
 
     uint speed = spi_init(spi0, 2 * 1000 * 1000); // SPI init @ 2 MHz    
+    // Normalement, le capteur tient jusqu'à 8 MHz, à voir si c'est pertinent
+    //uint speed = spi_init(spi0, 8 * 1000 * 1000); // SPI init @ 8 MHz    
 
     spi_set_format(spi0, 8, SPI_CPHA_0, SPI_CPOL_0, SPI_MSB_FIRST);
 }

gyro_ADXRS453.c

@@ -49,7 +49,10 @@ void affiche_tampon_32bits(uint8_t *tampon){
 
 }
 
-
+/// @brief Lit les données du gyroscope
+/// @param tampon_envoi espace mémoire à fournir à la fonction 
+/// @param tampon_reception espace mémoire à fournir à la fonction 
+/// @return 1 en cas d'erreur, 0 sinon
 int gyro_get_sensor_data(uint16_t tampon_envoi[], uint8_t tampon_reception[]){
     tampon_envoi[0] = 0x30;
     tampon_envoi[1] = 0x00;
@@ -61,28 +64,29 @@ int gyro_get_sensor_data(uint16_t tampon_envoi[], uint8_t tampon_reception[]){
         
         Monitoring_Error("Gyro Failed - SQ bits != 0x4\n");
         if(tampon_reception[1] & 0x04){
+            set_position_avec_gyroscope_error(1);
             //printf("SPI ERROR\n");
             return 1;
         }else{
+            set_position_avec_gyroscope_error(1);
             Monitoring_set_erreur_critique();
             //while(1){
                 affiche_tampon_32bits(tampon_reception);
                 printf("Gyro Failed - SQ bits (%#3x)!= 0x4\n", Gyro_SensorData.SQ);
             //}
-            //set_position_avec_gyroscope(0);
         }
     }
     if(Gyro_SensorData.ST != 0x1){
         Monitoring_Error("Gyro Failed - Status != 0x1\n");
-        set_position_avec_gyroscope(0);
+        set_position_avec_gyroscope_error(1);
         /*while(1){
             printf("Gyro Failed - Status (%#3x)!= 0x1\n", Gyro_SensorData.ST);
             affiche_tampon_32bits(tampon_reception);
         }*/
         Monitoring_set_erreur_critique();
-        //set_position_avec_gyroscope(0);
         return 1;
     }
+    set_position_avec_gyroscope_error(0);
     return 0;
 }
 
@@ -175,7 +179,8 @@ void gyro_get_vitesse_brute(struct t_angle_gyro* angle_gyro, struct t_angle_gyro
     uint8_t tampon_reception[5]="\0\0\0\0\0";
     int16_t rot_z;
     
-    sleep_us(1); // A supprimer plus tard
+    // Supprimé le 11/7/2023, pas d'impacts visibles..
+    //sleep_us(1); // A supprimer plus tard
     if(gyro_get_sensor_data(tampon_envoi, tampon_reception)){
         return;
     }