From f5711b7a03b6b494fa660e5da38a9c4c44279e54 Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Thu, 18 May 2023 22:26:34 +0200
Subject: [PATCH] Ajout information init Gyro + robustesse Gyro

---
 gyro.c          | 12 +++++++++---
 gyro_ADXRS453.c | 23 ++++++++++++++++-------
 2 files changed, 25 insertions(+), 10 deletions(-)

diff --git a/gyro.c b/gyro.c
index a31d5c0..659f7c0 100644
--- a/gyro.c
+++ b/gyro.c
@@ -4,6 +4,7 @@
 #include "hardware/spi.h"
 #include "hardware/structs/spi.h"
 #include "Geometrie.h"
+#include "Monitoring.h"
 #include "spi_nb.h"
 #include "Temps.h"
 #include "gyro.h"
@@ -73,14 +74,19 @@ void Gyro_Init(void){
 
     // Test de la présence du gyroscope :
     if(gyro_init_check()){
-        puts("Gyroscope non trouve");
-        while(1); // On s'arrête là !
+        Monitoring_Error("Gyroscope non trouve");
+        while(1){
+            puts("Gyroscope non trouve");
+        }; // On s'arrête là !
     }else{
         //puts("Gyroscope trouve");
         if(!gyro_config()){
             //puts("gyro_config ok !");
         }else{
-            puts("gyro_config FAILED !");
+            Monitoring_Error("gyro_config FAILED !");
+            while(1){
+                puts("gyro_config FAILED !");
+            }; // On s'arrête là !
 
         }
     }
diff --git a/gyro_ADXRS453.c b/gyro_ADXRS453.c
index abca285..d73def9 100644
--- a/gyro_ADXRS453.c
+++ b/gyro_ADXRS453.c
@@ -58,17 +58,26 @@ int gyro_get_sensor_data(uint16_t tampon_envoi[], uint8_t tampon_reception[]){
     gyro_spi_wr_32bits(tampon_envoi, tampon_reception);
     Gyro_traitementDonnees(tampon_reception);
     if(Gyro_SensorData.SQ != 0x4){
-        printf("Gyro Failed - SQ bits (%#3x)!= 0x4\n", Gyro_SensorData.SQ);
-        affiche_tampon_32bits(tampon_reception);
+        
         Monitoring_Error("Gyro Failed - SQ bits != 0x4\n");
-        set_position_avec_gyroscope(0);
-        return 1;
+        if(tampon_reception[1] & 0x04){
+            printf("SPI ERROR\n");
+            return 1;
+        }else{
+            /*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){
-        printf("Gyro Failed - Status (%#3x)!= 0x1\n", Gyro_SensorData.ST);
         Monitoring_Error("Gyro Failed - Status != 0x1\n");
-        affiche_tampon_32bits(tampon_reception);
-        set_position_avec_gyroscope(0);
+        /*while(1){
+            printf("Gyro Failed - Status (%#3x)!= 0x1\n", Gyro_SensorData.ST);
+            affiche_tampon_32bits(tampon_reception);
+        }*/
+        //set_position_avec_gyroscope(0);
         return 1;
     }
     return 0;