From feed3bce2544591c218a4be83848e23cc755230a Mon Sep 17 00:00:00 2001 From: Samuel Date: Tue, 18 Jul 2023 14:57:50 +0200 Subject: [PATCH] =?UTF-8?q?Distinction=20du=20cas=20o=C3=B9=20la=20localis?= =?UTF-8?q?ation=20se=20fait=20sans=20gyroscope=20et=20celui=20o=C3=B9=20l?= =?UTF-8?q?e=20gyroscope=20est=20en=20erreur?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Localisation.c | 2 +- Log.c | 51 +++++++++++++++++++++++++++++-------------------- Robot_config.c | 15 ++++++++++++++- Robot_config.h | 4 +++- Test_gyro.c | 8 +++++++- gyro.c | 2 ++ gyro_ADXRS453.c | 15 ++++++++++----- 7 files changed, 67 insertions(+), 30 deletions(-) diff --git a/Localisation.c b/Localisation.c index c1137f4..b7b6c32 100644 --- a/Localisation.c +++ b/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{ diff --git a/Log.c b/Log.c index ca1e251..9c8ef04 100644 --- a/Log.c +++ b/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); + } } diff --git a/Robot_config.c b/Robot_config.c index 873d199..698b06a 100644 --- a/Robot_config.c +++ b/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; -} \ No newline at end of file +} + +/// @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; +} diff --git a/Robot_config.h b/Robot_config.h index e1675fd..43e341d 100644 --- a/Robot_config.h +++ b/Robot_config.h @@ -1,2 +1,4 @@ int get_position_avec_gyroscope(void); -void set_position_avec_gyroscope(int _use_gyro); \ No newline at end of file +void set_position_avec_gyroscope(int _use_gyro); +void set_position_avec_gyroscope_error(int _erreur); +int get_position_avec_gyroscope_error(void); diff --git a/Test_gyro.c b/Test_gyro.c index 9204f7a..7651c61 100644 --- a/Test_gyro.c +++ b/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); + } } } \ No newline at end of file diff --git a/gyro.c b/gyro.c index 733a205..b4ed321 100644 --- a/gyro.c +++ b/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); } diff --git a/gyro_ADXRS453.c b/gyro_ADXRS453.c index b96fdc0..4507ee8 100644 --- a/gyro_ADXRS453.c +++ b/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; }