Lecture des vitesses et intégration en angle

Des problème lors de l'envoi consécutif de valeur par SPI

gyro.c

@@ -7,9 +7,8 @@
 
 const uint PIN_CS = 1;
 
-static inline void cs_select();
-static inline void cs_deselect();
 int gyro_init_check();
+void gyro_config();
 int gyro_read_register_blocking(uint8_t registrer, uint8_t *tampon, uint8_t nb_a_lire);
 
 
@@ -24,32 +23,18 @@ void Gyro_Init(void){
     gpio_set_dir(PIN_CS, GPIO_OUT);
     cs_deselect();
 
-    spi_init(spi0, 100 * 1000); // SPI init @ 100 kHZ
+    spi_init(spi0, 100 * 1000); // SPI init @ 100 kHz
+    //spi_init(spi0, 2 * 1000 * 1000); // SPI init @ 2 MHz
 
     //Ça doit être les valeurs par défaut, mais ça marche !
     spi_set_format(spi0, 8, SPI_CPHA_1, SPI_CPOL_1, SPI_MSB_FIRST);
 
-    // Pour chaque SPI nous avons les registres suivants :
-
-    // SSPCR0 : Configuré par spi_init()
-    // SSPCR1 : Configuré par spi_init()
-    // SSPDR : Lecture ou écriture des données (avec FIFO)
-    // SSPSR : C'est les status qui permettent de savoir si on a des données à recevoir ou à envoyer.
-    // SSPCPSR : Configuré par spi_init() ou baudrate
-    // SSPIMSC : Activation ou désactivation des interruptions
-    // SSPRIS : Etat des drapeaux des interrutions - sans tenir compte des masquages
-    // SSPMIS : Etat des drapeaux des interrutions - en tenant compte des masquages
-    // SSPICR : Pour effacer les interruptions 
-    // SSPDMACR : DMA
-    // 
-    
-    
-
     // Test de la présence du gyroscope :
     if(gyro_init_check()){
         puts("Gyroscope non trouve");
     }else{
         puts("Gyroscope trouve");
+        gyro_config();
     }
 }
 
@@ -62,6 +47,33 @@ int gyro_init_check(){
         return 0;
     }
     return 1;
+}
+
+void gyro_config(){
+    // Registre CTRL1
+    // DR : 11
+    // BW : 10
+    // PD : 1
+    // Zen : 1
+    // Yen : 1
+    // Xen : 1
+
+    uint16_t tampon[2] = {0x20, 0b11101111};
+    while(spi_nb_busy(spi0) == SPI_BUSY);
+    cs_select();
+    int rep = spi_nb_write_data(spi0, tampon, 2);
+    while(spi_nb_busy(spi0) == SPI_BUSY);
+    cs_deselect();
+
+    if (rep == SPI_ERR_TRANSMIT_FIFO_FULL){
+        while(1){
+            printf("gyro_config: SPI_ERR_TRANSMIT_FIFO_FULL\n");
+        }
+    }else{
+        puts("gyro_config ok !");
+    }
+
+    // Registre 
 
 
 }
@@ -78,30 +90,43 @@ int gyro_read_register_blocking(uint8_t registrer, uint8_t *tampon, uint8_t nb_a
     
 }
 
-void Gyro_Read(void){
+void Gyro_Read(uint16_t step_ms){
-    uint8_t tampon[10]="";
+    uint8_t tampon[10]="123456789";
-    uint8_t reg[2] = {0x0F | 0x80 | 0x40, '\0'};
+    int16_t rot_x, rot_y, rot_z;
+    static double angle_x=0, angle_y=0, angle_z=0;
     int nb_recu;
-    // Lire l'adresse d'identification
+
-    // WHO_AM_I : 0x0F
+
-    puts("Lecture 0x0F");
+    //cs_select();
-    cs_select();
+    //while(spi_nb_read_register_8bits(spi0, 0x20, tampon, 1) == SPI_IN_PROGRESS);
-    while(spi_nb_read_register_8bits(spi0, 0x0F, tampon, 1) == SPI_IN_PROGRESS);
+    //cs_deselect();
-    cs_deselect();
+    gyro_read_register_blocking(0x20, tampon, 1);
-    tampon[1]='\0'; 
+    //printf ("Gyro CTRL1 : %#4x\n", tampon[0] );
-    puts(tampon);
+
+
+    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_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;
     
+    printf("rx : %f, ry : %f, rz: %f\n", angle_x, angle_y, angle_z);
 
 
-}
+}
-static inline void cs_select() {
-    asm volatile("nop \n nop \n nop");
-    gpio_put(PIN_CS, 0);  // Active low
-    asm volatile("nop \n nop \n nop");
-}
-
-static inline void cs_deselect() {
-    asm volatile("nop \n nop \n nop");
-    gpio_put(PIN_CS, 1);
-    asm volatile("nop \n nop \n nop");
-}

gyro.h

@@ -1,2 +1,2 @@
 void Gyro_Init(void);
-void Gyro_Read(void);
+void Gyro_Read(u_int16_t);

spi_nb.c

@@ -3,29 +3,33 @@
 #include "hardware/gpio.h"
 #include "hardware/structs/spi.h"
 #include "hardware/spi.h"
+#include "hardware/irq.h"
 #include "hardware/regs/dreq.h"
 
-#define SPI_ERR_TRANSMIT_FIFO_FULL 1
-#define SPI_OK 0
-#define SPI_BUSY 1
-#define SPI_IDLE 0
 
 
+
+
+uint16_t spi0_slave_register;
+uint8_t* spi0_buffer;
+uint8_t  spi0_nb_data_to_read;
+
 #define PIN_CS 1
 
-
+void cs_select(void) {
-static inline void cs_select() {
     asm volatile("nop \n nop \n nop");
     gpio_put(PIN_CS, 0);  // Active low
     asm volatile("nop \n nop \n nop");
 }
 
-static inline void cs_deselect() {
+void  cs_deselect(void) {
     asm volatile("nop \n nop \n nop");
     gpio_put(PIN_CS, 1);
     asm volatile("nop \n nop \n nop");
 }
 
+
+
 int spi_nb_read_register_8bits(spi_inst_t * spi, uint16_t spi_slave_register, uint8_t *buffer, uint8_t nb_data_to_read){
     uint16_t dummy_buffer[8]={0, 0, 0, 0,0, 0, 0, 0};
     uint8_t nb_data_read;
@@ -43,11 +47,16 @@ int spi_nb_read_register_8bits(spi_inst_t * spi, uint16_t spi_slave_register, ui
 
     switch(status){
         case INIT:
+            if(spi == spi0){
+                spi0_slave_register = spi_slave_register;
+                spi0_buffer = buffer;
+                spi0_nb_data_to_read = nb_data_to_read;
+            }
             cs_select();
         case WAIT_SPI_IDLE:
             if(spi_nb_busy(spi) == SPI_IDLE){
                 status = SEND_REGISTER_ADRESS;
-                puts("SEND_REGISTER_ADRESS");
+                //puts("SEND_REGISTER_ADRESS");
             }
             break;
         
@@ -55,24 +64,27 @@ int spi_nb_read_register_8bits(spi_inst_t * spi, uint16_t spi_slave_register, ui
             spi_slave_register = spi_slave_register | 0x80 | 0X40;
             if(spi_nb_write_data(spi, &spi_slave_register, 1) == SPI_OK){
                 status = WAIT_SENDING_DATA;
-                puts("WAIT_SENDING_DATA");
+                // Ici on veut tester une interruption
+                // Armement de l'interruption
+                //puts("WAIT_SENDING_DATA");
             }else{
                 status = SPI_IN_ERROR;
             }
             break;
         
         case WAIT_SENDING_DATA:
+        
             if(!spi_nb_busy(spi)){
                 spi_nb_flush_recieve_fifo(spi);
                 status = SEND_DUMMY_DATA;
-                puts("SEND_DUMMY_DATA");
+                //puts("SEND_DUMMY_DATA");
             }
             break;
 
         case SEND_DUMMY_DATA:
             if(spi_nb_write_data(spi, dummy_buffer, nb_data_to_read) == SPI_OK){
                 status = WAIT_RECIEVING_DATA;
-                puts("WAIT_RECIEVING_DATA");
+                //puts("WAIT_RECIEVING_DATA");
             }else{
                 status = SPI_IN_ERROR;
             }
@@ -81,7 +93,7 @@ int spi_nb_read_register_8bits(spi_inst_t * spi, uint16_t spi_slave_register, ui
         case WAIT_RECIEVING_DATA:
             if(!spi_nb_busy(spi)){
                 status = READ_DATA;
-                puts("READ_DATA");
+                //puts("READ_DATA");
             }
             break;
 
@@ -89,17 +101,17 @@ int spi_nb_read_register_8bits(spi_inst_t * spi, uint16_t spi_slave_register, ui
             cs_deselect();
             nb_data_read = spi_nb_read_data_8bits(spi, buffer);
             if(nb_data_read == nb_data_to_read){
-                puts("SPI_SUCCESS");
+                //puts("SPI_SUCCESS");
                 status = INIT;
                 return SPI_SUCCESS;
             }
-            puts("SPI_FAILED");
+            //puts("SPI_FAILED");
             status = SPI_IN_ERROR;
             return SPI_FAILED;
             break;
         
         case SPI_IN_ERROR:
-            puts("SPI_IN_ERROR");
+            //puts("SPI_IN_ERROR");
             spi_nb_flush_recieve_fifo(spi);
             cs_deselect();
             status = INIT;
@@ -133,15 +145,11 @@ void spi_nb_flush_recieve_fifo(spi_inst_t * spi){
 /// @return Number of byte read
 uint8_t spi_nb_read_data_8bits(spi_inst_t * spi, uint8_t * buffer){
     uint8_t index = 0;
-    char debug[2]="x";
     while(spi_get_hw(spi)->sr & SPI_SSPSR_RNE_BITS){
         buffer[index] = (uint8_t)spi_get_hw(spi)->dr & SPI_SSPDR_DATA_BITS;
-        debug[0] = buffer[index];
-        puts(debug);
         index++;
     }
     return index;
-
 }
 
 /// @brief Write severals byte to the SPI Transmit FIFO
@@ -154,7 +162,16 @@ inline int spi_nb_write_data(spi_inst_t * spi, uint16_t * buffer, uint8_t size){
     uint8_t index=0;
     do
     {
-        statu_spi = spi_nb_write_byte(spi, buffer[index]);
+        if(spi_get_hw(spi)->sr & SPI_SSPSR_TNF_BITS){
+            spi_get_hw(spi)->dr = buffer[index];
+            statu_spi = SPI_OK;
+        }else{
+            statu_spi = SPI_ERR_TRANSMIT_FIFO_FULL;
+        }
+        while (spi_is_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;
@@ -170,4 +187,27 @@ int spi_nb_write_byte(spi_inst_t * spi, uint16_t data){
         return SPI_OK;
     }
     return SPI_ERR_TRANSMIT_FIFO_FULL;
+}
+
+int spi_read_register(spi_inst_t * spi, uint16_t spi_slave_register, uint8_t *buffer, uint8_t nb_to_read){
+    int statu;
+    uint8_t nb_read;
+    uint16_t tampon[15]={0,0,0,0,0,0,0,0,0,0,0,0};
+
+    spi_slave_register = spi_slave_register | 0x80 | 0X40;
+    tampon[0]= spi_slave_register;
+    spi_nb_flush_recieve_fifo(spi0);
+    cs_select();
+    statu = spi_nb_write_data(spi, tampon, 1 + nb_to_read);
+    if(statu == SPI_ERR_TRANSMIT_FIFO_FULL){
+        printf("Erreur: spi_read_register: SPI_ERR_TRANSMIT_FIFO_FULL");
+        return statu;
+    }
+    while(spi_nb_busy(spi0));
+    cs_deselect();
+    nb_read = spi_nb_read_data_8bits(spi0, buffer);
+    if(nb_read != nb_to_read+1){
+        printf("Erreur: spi_read_register, nb de valeurs lues incoherentes");
+    }
+
 }

spi_nb.h

@@ -1,12 +1,22 @@
 #include "hardware/spi.h"
 
+#define SPI_BUSY 1
+#define SPI_IDLE 0
+
 #define SPI_SUCCESS 0
 #define SPI_IN_PROGRESS 1
 #define SPI_FAILED 2
 
+#define SPI_ERR_TRANSMIT_FIFO_FULL 1
+#define SPI_OK 0
+
 int spi_nb_busy(spi_inst_t * spi);
 void spi_nb_flush_recieve_fifo(spi_inst_t * spi);
 int spi_nb_write_byte(spi_inst_t * spi, uint16_t data);
 int spi_nb_write_data(spi_inst_t * spi, uint16_t * buffer, uint8_t size);
 uint8_t spi_nb_read_data_8bits(spi_inst_t * spi, uint8_t * buffer);
-int spi_nb_read_register_8bits(spi_inst_t * spi, uint16_t spi_slave_register, uint8_t *buffer, uint8_t nb_data_to_read);
+int spi_nb_read_register_8bits(spi_inst_t * spi, uint16_t spi_slave_register, uint8_t *buffer, uint8_t nb_data_to_read);
+int spi_read_register(spi_inst_t * spi, uint16_t spi_slave_register, uint8_t *buffer, uint8_t nb_to_read);
+
+void cs_select(void);
+void cs_deselect(void);

test.c

@@ -22,12 +22,13 @@ int main() {
     Gyro_Init();
 
     while (1) { 
+        u_int16_t step_ms = 100;
         /*gpio_put(LED_PIN, 0);
         sleep_ms(251);
         gpio_put(LED_PIN, 1);
         puts("Bonjour");
         sleep_ms(1000);*/
-        sleep_ms(1000);
+        sleep_ms(step_ms);
-        Gyro_Read();
+        Gyro_Read(step_ms);
     }
 }