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

Utilisation de plusieurs gyroscopes

Browse Source
This commit is contained in:
Samuel 2022-10-16 10:26:44 +02:00
parent c9cb35aa33
commit 3137dc5583
11 changed files with 468 additions and 135 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -10,6 +10,7 @@ spi_nb.c
gyro.c
Temps.c
Servomoteur.c
gyro_L3GD20H.c
)
pico_enable_stdio_usb(test 1)
pico_enable_stdio_uart(test 1)

2
graph_enr01.gp
View File

@ -1,4 +1,4 @@
set title "Observation des 3 axes\nT_{acq} : 5 ms"
set title "Observation des 3 axes\nT_{acq} : 5 ms - filtre 15 secondes"
set xlabel "Temps (s)"
set ylabel "Vitesse (°/s)"

2
graph_enr02.gp
View File

@ -1,4 +1,4 @@
set title "Observation des 3 axes\nT_{acq} : 5 ms"
set title "Observation des 3 axes\nT_{acq} : 5 ms - filtre 15 secondes"
set xlabel "Temps (s)"
set ylabel "Vitesse (°/s)"

179
gyro.c
View File

@ -7,17 +7,26 @@
#include "Temps.h"
#include "gyro.h"
struct t_angle_gyro{
int32_t rot_x, rot_y, rot_z, temp;
};
#define GYRO_L3GD20H
#ifdef GYRO_L3GD20H
#include "gyro_L3GD20H.h"
#else
#ifdef GYRO_ADXRS453
#include "gyro_ADXRS453.h"
#else
#error "Choissisez un gyroscope"
#endif
#endif
/// @brief structure d'échange des angles du gyrocope
struct t_angle_gyro vitesse_angulaire, vitesse_calibration;
struct t_angle_gyro _vitesse_calibration;
struct t_angle_gyro *vitesse_calibration;
struct t_angle_gyro_double _vitesse_angulaire;
struct t_angle_gyro_double *vitesse_angulaire;
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, struct t_angle_gyro* angle_gyro_moy);
void gyro_calibration(void);
uint32_t rot_x_zero, rot_y_zero, rot_z_zero;
@ -45,16 +54,14 @@ void Gyro_Init(void){
gpio_set_dir(PIN_CS, GPIO_OUT);
cs_deselect();
vitesse_calibration.rot_x = 0;
vitesse_calibration.rot_y = 0;
vitesse_calibration.rot_z = 0;
vitesse_calibration.temp = 0;
vitesse_calibration = NULL;
vitesse_angulaire = &_vitesse_angulaire;
//spi_init(spi0, 100 * 1000); // SPI init @ 100 kHz
uint speed = spi_init(spi0, 2 * 1000 * 1000); // SPI init @ 2 MHz
printf("vitesse SPI : %d\n", speed);
//Ç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);
// Test de la présence du gyroscope :
@ -62,7 +69,12 @@ void Gyro_Init(void){
puts("Gyroscope non trouve");
}else{
puts("Gyroscope trouve");
gyro_config();
if(!gyro_config()){
puts("gyro_config ok !");
}else{
printf("gyro_config FAILED !");
}
}
sleep_ms(150); // Temps d'init du gyroscope
/*while(1){
@ -70,102 +82,31 @@ void Gyro_Init(void){
}*/
}
int gyro_init_check(){
// Renvoi 0 si l'initialisation s'est bien passée
// Renvoi 1 si le gyroscope n'a pas répondu
uint8_t tampon[2]="";
gyro_read_register_blocking(0x0F, tampon, 1);
if(tampon[0] == 0xd7){
return 0;
}
return 1;
}
void gyro_config(){
// Registre CTRL1
// DR : 11
// BW : 10
// PD : 1
// Zen : 1
// Yen : 1
// Xen : 1
uint8_t config = 0b11101111;
uint16_t tampon[2] = {0x20, config};
uint8_t tampon2[10]="\0\0\0\0\0\0\0\0\0";
int statu, nb_read;
//while(spi_nb_busy(spi0) == SPI_BUSY);
cs_select();
int rep = spi_nb_write_data(spi0, tampon, 2);
if(rep == SPI_ERR_TRANSMIT_FIFO_FULL){
printf("Erreur: spi_read_register: SPI_ERR_TRANSMIT_FIFO_FULL\n");
//return statu;
}
while(spi_nb_busy(spi0));
cs_deselect();
int nb_lu = spi_read_register(spi0, 0x20, tampon2, 1);
printf("Nb lu: %d\n", nb_lu);
if(tampon2[1] == config){
puts("gyro_config ok !");
}else{
printf("gyro_config FAILED ! :%#4x\n", tampon2[1]);
}
// Registre
}
int gyro_read_register_blocking(uint8_t registrer, uint8_t *tampon, uint8_t nb_a_lire){
uint8_t reg = registrer | 0xC0 ;
int nb_recu;
cs_select();
spi_write_blocking(spi0, &reg, 1);
sleep_ms(10);
nb_recu = spi_read_blocking(spi0, 0, tampon, nb_a_lire);
cs_deselect();
}
void Gyro_Read(uint16_t step_ms){
uint8_t tampon[10]="\0\0\0\0\0\0\0\0\0";
uint8_t tampon2[10]="ABCDEFGHI";
int16_t rot_x, rot_y, rot_z;
static double angle_x=0, angle_y=0, angle_z=0;
struct t_angle_gyro * _vitesse_angulaire_brute;
struct t_angle_gyro m_vitesse_angulaire_brute;
int nb_recu;
_vitesse_angulaire_brute = &m_vitesse_angulaire_brute;
//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(&vitesse_angulaire, &vitesse_calibration);
// Acquisition des valeurs
gyro_get_vitesse_brute(_vitesse_angulaire_brute, vitesse_calibration);
//gyro_get_angles(&vitesse_angulaire, NULL);
// Angle en degré
vitesse_gyro.rot_x = (double)vitesse_angulaire.rot_x * 0.00875 / 32.0;
vitesse_gyro.rot_y = (double)vitesse_angulaire.rot_y * 0.00875 / 32.0;
vitesse_gyro.rot_z = (double)vitesse_angulaire.rot_z * 0.00875 / 32.0;
angle_gyro.rot_x = angle_gyro.rot_x + vitesse_gyro.rot_x * step_ms * 0.001;
angle_gyro.rot_y = angle_gyro.rot_y + vitesse_gyro.rot_y * step_ms * 0.001;
angle_gyro.rot_z = angle_gyro.rot_z + vitesse_gyro.rot_z * step_ms * 0.001;
// conversion de la vitesse angulaire en degré/seconde
gyro_get_vitesse_normalisee(_vitesse_angulaire_brute, vitesse_angulaire);
//printf("%d, %#4x, %#4x, %#4x\n", step_ms, vitesse_angulaire.rot_x, vitesse_angulaire.rot_y, vitesse_angulaire.rot_z);
vitesse_gyro = *vitesse_angulaire;
//printf("rx : %f, ry : %f, rz: %f\n", angle_gyro.rot_x, angle_gyro.rot_y, angle_gyro.rot_z);
//while(spi_nb_busy(spi0));
//spi_nb_read_data_8bits(spi0,tampon);
//printf("tampon : %s\n", tampon);
// Intégration en fonction du pas de temps
angle_gyro.rot_x = angle_gyro.rot_x + vitesse_angulaire->rot_x * step_ms * 0.001;
angle_gyro.rot_y = angle_gyro.rot_y + vitesse_angulaire->rot_y * step_ms * 0.001;
angle_gyro.rot_z = angle_gyro.rot_z + vitesse_angulaire->rot_z * step_ms * 0.001;
}
int16_t gyro_get_temp(void){
@ -177,28 +118,6 @@ int16_t gyro_get_temp(void){
}
void gyro_get_angles(struct t_angle_gyro* angle_gyro, struct t_angle_gyro* angle_gyro_moy){
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));
if(angle_gyro_moy == NULL){
angle_gyro->rot_x = (int32_t) rot_x * 32;
angle_gyro->rot_y = (int32_t) rot_y * 32;
angle_gyro->rot_z = (int32_t) rot_z * 32;
}else{
angle_gyro->rot_x = (int32_t) rot_x * 32 - angle_gyro_moy->rot_x;
angle_gyro->rot_y = (int32_t) rot_y * 32 - angle_gyro_moy->rot_y;
angle_gyro->rot_z = (int32_t) rot_z * 32 - angle_gyro_moy->rot_z;
}
}
void gyro_affiche(struct t_angle_gyro_double angle_gyro, char * titre){
if(titre != NULL){
printf("%s ",titre);
@ -212,21 +131,23 @@ void gyro_calibration(void){
uint32_t m_temps_ms = Temps_get_temps_ms();
uint32_t temps_500ms = m_temps_ms;
int16_t temperature;
struct t_angle_gyro vitesse_grute_gyro;
printf("Calibration...\n");
vitesse_calibration = &_vitesse_calibration;
vitesse_calibration.rot_x = 0;
vitesse_calibration.rot_y = 0;
vitesse_calibration.rot_z = 0;
vitesse_calibration->rot_x = 0;
vitesse_calibration->rot_y = 0;
vitesse_calibration->rot_z = 0;
// Acquisition des échantillons, 1 par milliseconde (1 ms, c'est trop court on dirait !)
for(uint32_t i=0; i<nb_ech; i++){
while(m_temps_ms == Temps_get_temps_ms());
m_temps_ms = Temps_get_temps_ms();
gyro_get_angles(&vitesse_angulaire, NULL);
vitesse_calibration.rot_x += vitesse_angulaire.rot_x;
vitesse_calibration.rot_y += vitesse_angulaire.rot_y;
vitesse_calibration.rot_z += vitesse_angulaire.rot_z;
gyro_get_vitesse_brute(&vitesse_grute_gyro, NULL);
vitesse_calibration->rot_x += vitesse_grute_gyro.rot_x;
vitesse_calibration->rot_y += vitesse_grute_gyro.rot_y;
vitesse_calibration->rot_z += vitesse_grute_gyro.rot_z;
if(m_temps_ms > temps_500ms){
printf(".");
gyro_get_temp();
@ -234,12 +155,12 @@ void gyro_calibration(void){
}
sleep_ms(5);
}
vitesse_calibration.rot_x = vitesse_calibration.rot_x / (int32_t)nb_ech;
vitesse_calibration.rot_y = vitesse_calibration.rot_y / (int32_t)nb_ech;
vitesse_calibration.rot_z = vitesse_calibration.rot_z / (int32_t)nb_ech;
vitesse_calibration->rot_x = vitesse_calibration->rot_x / (int32_t)nb_ech;
vitesse_calibration->rot_y = vitesse_calibration->rot_y / (int32_t)nb_ech;
vitesse_calibration->rot_z = vitesse_calibration->rot_z / (int32_t)nb_ech;
temperature = gyro_get_temp();
printf("fin calibration, %d, %d, %d, %d\n", vitesse_calibration.rot_x, vitesse_calibration.rot_y ,vitesse_calibration.rot_z, temperature);
printf("fin calibration, %d, %d, %d, %d\n", vitesse_calibration->rot_x, vitesse_calibration->rot_y ,vitesse_calibration->rot_z, temperature);
}

4
gyro.h
View File

@ -1,6 +1,4 @@
struct t_angle_gyro_double{
double rot_x, rot_y, rot_z;
};
#include "gyro_data.h"
void Gyro_Init(void);
void Gyro_Read(u_int16_t);

291
gyro_ADXRS473.c Normal file
View File

@ -0,0 +1,291 @@
#include "gyro_ADXRS473.h"
#include "spi_nb.h"
#include <stdio.h>
#define NB_MAX_CHAR_GYRO 4
struct {
unsigned short SQ:3;
unsigned short ST:2;
unsigned short P0:1;
unsigned short P1:1;
unsigned short PLL:1;
unsigned short Q:1;
unsigned short NVM:1;
unsigned short POR:1;
unsigned short PWR:1;
unsigned short CST:1;
unsigned short CHK:1;
signed int rateData;
} Gyro_SensorData;
void Gyro_traitementDonnees(unsigned char * tamponRecu);
unsigned char pariteOctet(unsigned char octet);
int gyro_read_register_blocking(uint8_t registrer, uint8_t *tampon, uint8_t nb_a_lire){
uint8_t tampon_envoi[4]="\0\0\0\0";
int nb_recu;
tampon_envoi[0] = registrer;
// Envoie commande N
cs_select();
spi_write_blocking(spi0, tampon_envoi, 4);
nb_recu = spi_read_blocking(spi0, 0, tampon, nb_a_lire);
cs_deselect();
// lire reponse N
cs_select();
spi_write_blocking(spi0, tampon_envoi, 4);
nb_recu = spi_read_blocking(spi0, 0, tampon, nb_a_lire);
cs_deselect();
}
int gyro_init_check(){
// Renvoi 0 si l'initialisation s'est bien passée
// Renvoi 1 si le gyroscope n'a pas répondu
uint8_t tampon[5]="\0\0\0\0\0";
gyro_read_register_blocking(0x0C, tampon, 1);
Gyro_traitementDonnees(tampon);
printf("Init check : %#06x\n", Gyro_SensorData.rateData);
/* if(tampon[0] == 0xd7){
return 0;
}*/
return 1;
}
int gyro_config(){
// Registre CTRL1
// DR : 11
// BW : 10
// PD : 1
// Zen : 1
// Yen : 1
// Xen : 1
uint8_t config = 0b11101111;
uint16_t tampon[2] = {0x20, config};
uint8_t tampon2[10]="\0\0\0\0\0\0\0\0\0";
int statu, nb_read;
//while(spi_nb_busy(spi0) == SPI_BUSY);
cs_select();
int rep = spi_nb_write_data(spi0, tampon, 2);
if(rep == SPI_ERR_TRANSMIT_FIFO_FULL){
printf("Erreur: spi_read_register: SPI_ERR_TRANSMIT_FIFO_FULL\n");
//return statu;
}
while(spi_nb_busy(spi0));
cs_deselect();
int nb_lu = spi_read_register(spi0, 0x20, tampon2, 1);
printf("Nb lu: %d\n", nb_lu);
if(tampon2[1] == config){
//puts("gyro_config ok !");
return 0;
}else{
//printf("gyro_config FAILED ! :%#4x\n", tampon2[1]);
return 1;
}
// Registre
}
void gyro_get_vitesse_brute(struct t_angle_gyro* angle_gyro, struct t_angle_gyro* angle_gyro_moy){
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));
if(angle_gyro_moy == NULL){
angle_gyro->rot_x = (int32_t) rot_x * 32;
angle_gyro->rot_y = (int32_t) rot_y * 32;
angle_gyro->rot_z = (int32_t) rot_z * 32;
}else{
angle_gyro->rot_x = (int32_t) rot_x * 32 - angle_gyro_moy->rot_x;
angle_gyro->rot_y = (int32_t) rot_y * 32 - angle_gyro_moy->rot_y;
angle_gyro->rot_z = (int32_t) rot_z * 32 - angle_gyro_moy->rot_z;
}
}
void gyro_get_vitesse_normalisee(struct t_angle_gyro* _vitesse_angulaire,
struct t_angle_gyro_double * _vitesse_gyro){
_vitesse_gyro->rot_x = (double)_vitesse_angulaire->rot_x * 0.00875 / 32.0;
_vitesse_gyro->rot_y = (double)_vitesse_angulaire->rot_y * 0.00875 / 32.0;
_vitesse_gyro->rot_z = (double)_vitesse_angulaire->rot_z * 0.00875 / 32.0;
}
inline unsigned char Gyro_commande_SensorData(unsigned char autotest){
// On met SQ2 à 1 afin de différencier facilement une erreur et des données
uint8_t tamponGyroscopeEnvoi[4];
tamponGyroscopeEnvoi[0] = 0x30;
tamponGyroscopeEnvoi[1] = 0x00;
tamponGyroscopeEnvoi[2] = 0x00;
if (autotest){
tamponGyroscopeEnvoi[3] = 0x03;
}else{
tamponGyroscopeEnvoi[3] = 0x01;
}
// La parité, dans ce cas est triviale, autant prévoir tous les cas
//Gyro_commande_PariteData(tamponGyroscopeEnvoi);
//return SPI_envData(tamponGyroscopeEnvoi);
}
void Gyro_commande_PariteData(unsigned char* tampon){
unsigned char parite=0,i;
// Obtention de la parité actuelle
for(i=0 ; i< NB_MAX_CHAR_GYRO ; i++){
parite ^= pariteOctet(tampon[i]);
}
// On veut une parité impaire
parite ^= 0x01;
// On insere ce bit dans le message, au bon endroit
tampon[NB_MAX_CHAR_GYRO-1] = tampon[NB_MAX_CHAR_GYRO-1] | parite;
}
unsigned char pariteOctet(unsigned char octet){
unsigned char parite=0,i;
for (i=0 ; i<8 ; i++){
parite ^= octet & 0x01;
octet = octet >> 1;
}
return parite;
}
void Gyro_traitementDonnees(unsigned char * tamponRecu){
Gyro_SensorData.SQ = (tamponRecu[0]>>5) & 0x07;
Gyro_SensorData.P0 = (tamponRecu[0]>>4) & 0x01;
Gyro_SensorData.ST = (tamponRecu[0]>>2) & 0x03;
Gyro_SensorData.rateData = (int)
( (0xC000 &((unsigned int) (tamponRecu[0] & 0x03)<<14)) |
( 0x3FC0 & ((unsigned int) tamponRecu[1] << 6)) |
( 0x003F & (unsigned int) ( tamponRecu[2] >> 2)));
Gyro_SensorData.PLL = (tamponRecu[3] & 0x80) >> 7;
Gyro_SensorData.Q = (tamponRecu[3] & 0x40) >> 6;
Gyro_SensorData.NVM = (tamponRecu[3] & 0x20) >> 5;
Gyro_SensorData.POR = (tamponRecu[3] & 0x10) >> 4;
Gyro_SensorData.PWR = (tamponRecu[3] & 0x08) >> 3;
Gyro_SensorData.CST = (tamponRecu[3] & 0x04) >> 2;
Gyro_SensorData.CHK = (tamponRecu[3] & 0x02) >> 1;
Gyro_SensorData.P1 = (tamponRecu[3] & 0x01);
}
/*
unsigned char Gyro_gestion(void){
Gyro_commande_SensorData(0);
while(!SPI_recData(GyroscopeReception));
Gyro_traitementDonnees(GyroscopeReception);
if (Gyro_SensorData.SQ & 0x04){
Gyro_Angle +=(long) (Gyro_SensorData.rateData - angle0);
//Gyro_Angle = angle0;
}else{
Gyro_Angle = (long)0x3333;
}
return 0;
}
inline unsigned char Gyro_gestion_nb(){
if(SPI_recData(GyroscopeReception)){
Gyro_traitementDonnees(GyroscopeReception);
if (Gyro_SensorData.SQ & 0x04)
// calcul du nouvel angle
// TODO : Améliorer la stabilitée en augmentant la précision
Gyro_Angle +=(long) ((long)Gyro_SensorData.rateData - (long)angle0);
return 0;
}
return 1;
}
int Gyro_getAngle(void){
// 80° par secondes
// 5 kHz => 200 µs
// Gyro_Angle en 1,6 e-2 degré
return (int)(-Gyro_Angle / 5000 / 80);
}
long Gyro_getRawAngle(void){
// 80° par secondes
// 5 kHz => 200 µs
// Gyro_Angle en 1,6 e-2 degré
return Gyro_Angle ;
}
long double Gyro_getAngleRadian(void){
// 80° par secondes
// 5 kHz => 200 µs
// Gyro_Angle en 1,6 e-2 degré
return -Gyro_Angle * GYRO_COEF_RADIAN_5kHz;
}
unsigned char * Gyro_getRawData(){
return GyroscopeReception;
}
int Gyro_init(){
long long calcul_angle0;
int i, erreur_gyro;
Gyro_Timer_ms=100;
while(Gyro_Timer_ms);
// Envoie message auto-test des test
while(!Gyro_commande_SensorData(1));
while(!SPI_recData(GyroscopeReception));
// Attente 50 ms - les tests doivent indiquer des erreur
Gyro_Timer_ms=50;
while(Gyro_Timer_ms);
while(!Gyro_commande_SensorData(0));
while(!SPI_recData(GyroscopeReception));
// Attente 50 ms - les erreurs doivent s'être effacées
Gyro_Timer_ms=50;
while(Gyro_Timer_ms);
while(!Gyro_commande_SensorData(0));
while(!SPI_recData(GyroscopeReception));
// Calibration du gyroscope
calcul_angle0 = 0;
i=0;
erreur_gyro = 0;
while((i<NB_ACQ_CALIBRATION) && (erreur_gyro < NB_MAX_ERREUR_GYRO)){
while(!Gyro_commande_SensorData(0));
while(!SPI_recData(GyroscopeReception));
Gyro_traitementDonnees(GyroscopeReception);
if (Gyro_SensorData.SQ & 0x04){
calcul_angle0 += Gyro_SensorData.rateData;
erreur_gyro = 0;
i++;
}else{
erreur_gyro++;
}
__delay32(2000); // 50 µs
}
if (erreur_gyro < NB_MAX_ERREUR_GYRO){
erreur_gyro = 0;
// TODO : Améliorer la stabilitée en augmentant la précision
angle0 = (long)(calcul_angle0 / NB_ACQ_CALIBRATION);
Gyro_Pret=1;
}else{
erreur_gyro = 1;
}
return erreur_gyro;
}
*/

6
gyro_ADXRS473.h Normal file
View File

@ -0,0 +1,6 @@
#include "gyro_data.h"
int gyro_init_check();
int gyro_config();
void gyro_get_vitesse_brute(struct t_angle_gyro* angle_gyro, struct t_angle_gyro* angle_gyro_moy);
void gyro_get_vitesse_normalisee(struct t_angle_gyro* _vitesse_angulaire, struct t_angle_gyro_double * vitesse_gyro);

96
gyro_L3GD20H.c Normal file
View File

@ -0,0 +1,96 @@
#include "gyro_L3GD20H.h"
#include "spi_nb.h"
#include <stdio.h>
int gyro_read_register_blocking(uint8_t registrer, uint8_t *tampon, uint8_t nb_a_lire){
uint8_t reg = registrer | 0xC0 ;
int nb_recu;
cs_select();
spi_write_blocking(spi0, &reg, 1);
sleep_ms(10);
nb_recu = spi_read_blocking(spi0, 0, tampon, nb_a_lire);
cs_deselect();
}
int gyro_init_check(){
// Renvoi 0 si l'initialisation s'est bien passée
// Renvoi 1 si le gyroscope n'a pas répondu
uint8_t tampon[2]="";
gyro_read_register_blocking(0x0F, tampon, 1);
if(tampon[0] == 0xd7){
return 0;
}
return 1;
}
int gyro_config(){
// Registre CTRL1
// DR : 11
// BW : 10
// PD : 1
// Zen : 1
// Yen : 1
// Xen : 1
uint8_t config = 0b11101111;
uint16_t tampon[2] = {0x20, config};
uint8_t tampon2[10]="\0\0\0\0\0\0\0\0\0";
int statu, nb_read;
//while(spi_nb_busy(spi0) == SPI_BUSY);
cs_select();
int rep = spi_nb_write_data(spi0, tampon, 2);
if(rep == SPI_ERR_TRANSMIT_FIFO_FULL){
printf("Erreur: spi_read_register: SPI_ERR_TRANSMIT_FIFO_FULL\n");
//return statu;
}
while(spi_nb_busy(spi0));
cs_deselect();
int nb_lu = spi_read_register(spi0, 0x20, tampon2, 1);
printf("Nb lu: %d\n", nb_lu);
if(tampon2[1] == config){
//puts("gyro_config ok !");
return 0;
}else{
//printf("gyro_config FAILED ! :%#4x\n", tampon2[1]);
return 1;
}
// Registre
}
void gyro_get_vitesse_brute(struct t_angle_gyro* angle_gyro, struct t_angle_gyro* angle_gyro_moy){
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));
if(angle_gyro_moy == NULL){
angle_gyro->rot_x = (int32_t) rot_x * 32;
angle_gyro->rot_y = (int32_t) rot_y * 32;
angle_gyro->rot_z = (int32_t) rot_z * 32;
}else{
angle_gyro->rot_x = (int32_t) rot_x * 32 - angle_gyro_moy->rot_x;
angle_gyro->rot_y = (int32_t) rot_y * 32 - angle_gyro_moy->rot_y;
angle_gyro->rot_z = (int32_t) rot_z * 32 - angle_gyro_moy->rot_z;
}
}
void gyro_get_vitesse_normalisee(struct t_angle_gyro* _vitesse_angulaire,
struct t_angle_gyro_double * _vitesse_gyro){
_vitesse_gyro->rot_x = (double)_vitesse_angulaire->rot_x * 0.00875 / 32.0;
_vitesse_gyro->rot_y = (double)_vitesse_angulaire->rot_y * 0.00875 / 32.0;
_vitesse_gyro->rot_z = (double)_vitesse_angulaire->rot_z * 0.00875 / 32.0;
}

6
gyro_L3GD20H.h Normal file
View File

@ -0,0 +1,6 @@
#include "gyro_data.h"
int gyro_init_check();
int gyro_config();
void gyro_get_vitesse_brute(struct t_angle_gyro* angle_gyro, struct t_angle_gyro* angle_gyro_moy);
void gyro_get_vitesse_normalisee(struct t_angle_gyro* _vitesse_angulaire, struct t_angle_gyro_double * vitesse_gyro);

14
gyro_data.h Normal file
View File

@ -0,0 +1,14 @@
#include "pico/stdlib.h"
#ifndef GYRO_DATA_H
#define GYRO_DATA_H
struct t_angle_gyro_double{
double rot_x, rot_y, rot_z;
};
struct t_angle_gyro{
int32_t rot_x, rot_y, rot_z, temp;
};
#endif

2
test.c
View File

@ -38,7 +38,7 @@ int main() {
temps_ms_old = temps_ms;
while (1) {
u_int16_t step_ms = 5;
float coef_filtre = 0.001;
float coef_filtre = 0.977;
while(temps_ms_old == Temps_get_temps_ms());
temps_ms_old = Temps_get_temps_ms();