From e98c500801d49eed6fddfdf7d67eb7baf7eb371f Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Mon, 26 Sep 2022 19:28:45 +0200
Subject: [PATCH] Structuration du code pour le gyroscope - calcul de l'angle

---
 CMakeLists.txt |  1 +
 gyro.c         | 78 ++++++++++++++++++++++++++++++++++++++------------
 spi_nb.c       | 12 ++++----
 3 files changed, 67 insertions(+), 24 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 96bce14..f5e9a02 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -8,6 +8,7 @@ add_executable(test
 test.c
 spi_nb.c
 gyro.c
+Temps.c
 )
 pico_enable_stdio_usb(test 1)
 pico_enable_stdio_uart(test 1)
diff --git a/gyro.c b/gyro.c
index 5b1f6a7..8577570 100644
--- a/gyro.c
+++ b/gyro.c
@@ -4,12 +4,25 @@
 #include "hardware/spi.h"
 #include "hardware/structs/spi.h"
 #include "spi_nb.h"
+#include "Temps.h"
 
 const uint PIN_CS = 1;
 
+/// @brief structure d'échange des angles du gyrocope
+struct t_angle_gyro{
+    int32_t rot_x, rot_y, rot_z;
+} angle_gyro, angle_gyro_moy;
+
 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);
+void gyro_calibration(void);
+
+uint32_t rot_x_zero, rot_y_zero, rot_z_zero;
+
+
+
 
 
 void Gyro_Init(void){
@@ -32,11 +45,11 @@ void Gyro_Init(void){
     // Test de la présence du gyroscope :
     if(gyro_init_check()){
         puts("Gyroscope non trouve");
-        gyro_config();
     }else{
         puts("Gyroscope trouve");
         gyro_config();
     }
+    //gyro_calibration();
 }
 
 int gyro_init_check(){
@@ -110,35 +123,64 @@ void Gyro_Read(uint16_t step_ms){
     int nb_recu;
 
 
-
     spi_read_register(spi0, 0x20, tampon, 1);
     //printf ("Gyro CTRL1 (bis) : %#4x\n", tampon[1] );
 
     //printf ("RPI SSPCPSR : %#4x\n", spi_get_hw(spi0)->cpsr );
     //printf ("RPI SSPCR0 : %#4x\n", spi_get_hw(spi0)->cr0 );
 
+    gyro_get_angles(&angle_gyro);
 
-    //gyro_read_register_blocking(0x28, tampon, 6);
-    spi_read_register(spi0, 0x28, tampon, 6);
-
-
-    for(int i=0; i<10; i++){
-        printf("%#4x ", tampon[i]);
-    }
-
-    rot_x = -(tampon[1] + (tampon[2] << 8));
-    rot_y = -(tampon[3] + (tampon[4] << 8));
-    rot_z = -(tampon[5] + (tampon[6] << 8));
-
-    angle_x = angle_x + (double)rot_x * step_ms * 0.001 * 0.00875;
-    angle_y = angle_y + (double)rot_y * step_ms * 0.001 * 0.00875;
-    angle_z = angle_z + (double)rot_z * step_ms * 0.001 * 0.00875;
+    angle_x = angle_x + (double)angle_gyro.rot_x * step_ms * 0.001 * 0.00875 * 0.125;
+    angle_y = angle_y + (double)angle_gyro.rot_y * step_ms * 0.001 * 0.00875 * 0.125;
+    angle_z = angle_z + (double)angle_gyro.rot_z * step_ms * 0.001 * 0.00875 * 0.125;
     
     printf("rx : %f, ry : %f, rz: %f\n", angle_x, angle_y, angle_z);
 
     //while(spi_nb_busy(spi0));
     //spi_nb_read_data_8bits(spi0,tampon);
     //printf("tampon : %s\n", tampon);
+}
 
+void gyro_get_angles(struct t_angle_gyro* angle_gyro){
+    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));
 
-}
\ No newline at end of file
+    angle_gyro->rot_x = (int32_t) rot_x * 8;
+    angle_gyro->rot_y = (int32_t) rot_y * 8;
+    angle_gyro->rot_z = (int32_t) rot_z * 8;
+
+    
+
+}
+
+void gyro_calibration(void){
+    uint16_t nb_ech = 3000;
+    uint32_t m_temps_ms = Temps_get_temps_ms();
+
+    printf("Calibration...\n");
+
+    angle_gyro_moy.rot_x = 0;
+    angle_gyro_moy.rot_y = 0;
+    angle_gyro_moy.rot_z = 0;
+    
+    for(uint16_t i=0; i<nb_ech; i++){
+        while(m_temps_ms == Temps_get_temps_ms());
+        gyro_get_angles(&angle_gyro);
+        angle_gyro_moy.rot_x += angle_gyro.rot_x;
+        angle_gyro_moy.rot_y += angle_gyro.rot_y;
+        angle_gyro_moy.rot_z += angle_gyro.rot_z;
+    }
+    angle_gyro_moy.rot_x = angle_gyro_moy.rot_x / nb_ech;
+    angle_gyro_moy.rot_y = angle_gyro_moy.rot_y / nb_ech;
+    angle_gyro_moy.rot_z = angle_gyro_moy.rot_z / nb_ech;
+
+    printf("Calibration : rx : %f, ry : %f, rz: %f\n", angle_gyro_moy.rot_x, 
+    angle_gyro_moy.rot_y, angle_gyro_moy.rot_z);
+
+}
diff --git a/spi_nb.c b/spi_nb.c
index 0b4f5b3..8552f1d 100644
--- a/spi_nb.c
+++ b/spi_nb.c
@@ -158,23 +158,23 @@ uint8_t spi_nb_read_data_8bits(spi_inst_t * spi, uint8_t * buffer){
 /// @param size size of the data to transmit
 /// @return SPI_OK or SPI_ERR_TRANSMIT_FIFO_FULL
 inline int spi_nb_write_data(spi_inst_t * spi, uint16_t * buffer, uint8_t size){
-    int statu_spi = SPI_OK;
+    int status_spi = SPI_OK;
     uint8_t index=0;
     do
     {
         if(spi_get_hw(spi)->sr & SPI_SSPSR_TNF_BITS){
             spi_get_hw(spi)->dr = buffer[index];
-            statu_spi = SPI_OK;
+            status_spi = SPI_OK;
         }else{
-            statu_spi = SPI_ERR_TRANSMIT_FIFO_FULL;
+            status_spi = SPI_ERR_TRANSMIT_FIFO_FULL;
         }
-        while (spi_is_busy(spi));
+        while (spi_nb_busy(spi));
         //statu_spi = spi_nb_write_byte(spi, buffer[index]);
         //printf("envoi : %x\n", buffer[index]);
         //sleep_ms(1);
         index++;
-    } while ( (statu_spi == SPI_OK) && (index < size));
-    return statu_spi;
+    } while ( (status_spi == SPI_OK) && (index < size));
+    return status_spi;
 }
 
 /// @brief Write one "byte", 4 to 16 bits to the SPI Transmit FIFO.