Lecture des vitesses et intégration en angle
Des problème lors de l'envoi consécutif de valeur par SPI
This commit is contained in:
parent
969755f889
commit
16ee1109b4
107
gyro.c
107
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){
|
||||
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
2
gyro.h
@ -1,2 +1,2 @@
|
||||
void Gyro_Init(void);
|
||||
void Gyro_Read(void);
|
||||
void Gyro_Read(u_int16_t);
|
||||
|
||||
80
spi_nb.c
80
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
|
||||
|
||||
|
||||
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;
|
||||
@ -171,3 +188,26 @@ int spi_nb_write_byte(spi_inst_t * spi, uint16_t data){
|
||||
}
|
||||
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");
|
||||
}
|
||||
|
||||
}
|
||||
10
spi_nb.h
10
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_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);
|
||||
Loading…
Reference in New Issue
Block a user