Keuronde/RPiPico-Holonome2023
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Lecture des vitesses et intégration en angle

Browse Source 
Des problème lors de l'envoi consécutif de valeur par SPI
This commit is contained in:
Samuel 2022-09-24 18:55:09 +02:00
parent 969755f889
commit 16ee1109b4
5 changed files with 142 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

109
gyro.c
View File

@ -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){
uint8_t tampon[10]="";
uint8_t reg[2] = {0x0F | 0x80 | 0x40, '\0'};
void Gyro_Read(uint16_t step_ms){
uint8_t tampon[10]="123456789";
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();
while(spi_nb_read_register_8bits(spi0, 0x0F, tampon, 1) == SPI_IN_PROGRESS);
cs_deselect();
tampon[1]='\0';
puts(tampon);
//cs_select();
//while(spi_nb_read_register_8bits(spi0, 0x20, tampon, 1) == SPI_IN_PROGRESS);
//cs_deselect();
gyro_read_register_blocking(0x20, tampon, 1);
//printf ("Gyro CTRL1 : %#4x\n", tampon[0] );
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");
}
}

2
gyro.h
View File

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

80
spi_nb.c
View File

@ -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
static inline void cs_select() {
void cs_select(void) {
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");
}
}

12
spi_nb.h
View File

@ -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);

5
test.c
View File

@ -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);
Gyro_Read();
sleep_ms(step_ms);
Gyro_Read(step_ms);
}
}