63 lines
No EOL
1.8 KiB
C
63 lines
No EOL
1.8 KiB
C
#include <stm32f10x.h>
|
|
#include <math.h>
|
|
#include <roulis.h>
|
|
#include <MySPI.h>
|
|
#include <MyTimer.h>
|
|
|
|
//char x1,x2
|
|
char y1,y2,z1,z2;
|
|
|
|
void MyRoulis_Init(void){
|
|
MySPI_Init(SPI1);
|
|
|
|
// Initialisation des registres externes
|
|
MySPI_Clear_NSS(); // Déput transmission
|
|
MySPI_Send((0x2D|(1<<7))); // On veut écrire à l'adresse de POWER_CTL : 0x2D
|
|
MySPI_Send(1<<3); // On veut mettre le bit "measure" à 1;
|
|
MySPI_Set_NSS(); // Fin transmission
|
|
|
|
MySPI_Clear_NSS();
|
|
MySPI_Send((0x2C)|(1<<7)); // On veut écrire à l'adresse de BW_RATE
|
|
MySPI_Send(0x0A);
|
|
MySPI_Set_NSS();
|
|
|
|
MySPI_Clear_NSS();
|
|
MySPI_Send((0x31)|(1<<7)); // On veut écrire à l'adresse de DATA_FORMAT
|
|
MySPI_Send(0x03);
|
|
MySPI_Set_NSS();
|
|
|
|
MyTimer_Init(TIM2,3600,200); // Configurer le timer2 a 100Hz
|
|
MyTimer_ActiveIT(TIM2,0,&MyRoulis_Data);// Configurer interruption sur timer2
|
|
MyTimer_Base_Start(TIM2);
|
|
}
|
|
|
|
void MyRoulis_Data(void){
|
|
|
|
// Récupération des registred DATAX0..DATAZ1 (6 registres)
|
|
// MySPI_Clear_NSS(); // Début transmission X
|
|
// MySPI_Send((0x32)|(0<<7)|(1<<6)); // On veut lire les 2 bytes DATAX
|
|
// x1 = MySPI_Read(); // premier byte
|
|
// x2 = MySPI_Read(); // deuxième byte
|
|
// MySPI_Set_NSS(); // Fin transmission X
|
|
|
|
MySPI_Clear_NSS(); // Début transmission Y
|
|
MySPI_Send((0x34)|(0<<7)|(1<<6)); // On veut lire les 2 bytes DATAY
|
|
y1 = MySPI_Read(); // premier byte
|
|
y2 = MySPI_Read(); // deuxième byte
|
|
MySPI_Set_NSS(); // Fin transmission Y
|
|
|
|
MySPI_Clear_NSS(); // Début transmission Z
|
|
MySPI_Send((0x36)|(0<<7)|(1<<6)); // On veut lire les 2 bytes DATAZ
|
|
z1 = MySPI_Read(); // premier byte
|
|
z2 = MySPI_Read(); // deuxième byte
|
|
MySPI_Set_NSS(); // Fin transmission Z
|
|
}
|
|
|
|
int MyRoulis_Angle(){
|
|
//Calcul de l'angle alpha
|
|
//int x = x1 + (x2 << 8);
|
|
int y = y1 + (y2 << 8);
|
|
int z = z1 + (z2 << 8);
|
|
int alpha = atan(y / z); //sqrtf(x*x + y*y)
|
|
return alpha;
|
|
} |