#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
}

float MyRoulis_Angle(){
	//Calcul de l'angle alpha
	//int x = x1 + (x2 << 8);
	int y = y1 + (y2 << 8);
	int z = z1 + (z2 << 8);
	float alpha = atan(y / z);  //sqrtf(x*x + y*y)
	return alpha;
}