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;
}