#include "MySpi.h"

const char POWER_CTL = 0x2D;	//Power Control Register
const char BW_RATE = 0x2C; 
const char DATA_FORMAT = 0x31;

/*fonction écriture registre IMU*/
void driver_IMU_write_register(char registerAddress, char value)
{
	// On s'assure que le Chip Select est bien au niveau bas
	MySPI_Clear_NSS();
  // Envoi de l'adresse du registre par SPI
  MySPI_Send(registerAddress);
	// Envoi de la valeur désirée du registre par SPI
  MySPI_Send(value);
	// On active le chip select
	MySPI_Set_NSS();
}

/* Fonction d'initialisation*/
void driver_IMU_init(void)
{
	/* FSCK = 281kHz, Repos SCK = '1', Front actif = up
					 Gestion /CS logicielle à part, configure les 4 IO
					 - SCK, MOSI : Out Alt push pull
					 - MISO : floating input
					 - /NSS (/CS) : Out push pull */
	MySPI_Init(SPI1);
	//Paramètrage du registre POWER_CTL
	driver_IMU_write_register(POWER_CTL, 0x08);
	//Paramètrage du registe BW_RATE
	driver_IMU_write_register(BW_RATE, 0x0A);
	//Paramètrage du registre DATA_FORMAT
	driver_IMU_write_register(BW_RATE, 0x08); //Full resolution, et alignement à droite
	
}

/* Fonction de lecture des données*/
/*
@param *values : Les valeurs x, y, z sont chacune stockée sur 2 octets dans le tableau values
	x = values[1]<<8 & value[0];
	y = values[3]<<8 & value[2];
	z = values[5]<<8 & value[4];
@param numBytes : Le nombre de registre à lire à partir de registerAddress
@param registerAddress : Adresse du premier registre à lire*/
void driver_IMU_read(char registerAddress, int numBytes, unsigned char * values)
{
	int i;
	// Pour effectuer une lecture des registre, on doit mettre les bits R/W et MB à 1
	char trame = registerAddress | 0x80;
	trame = trame | 0x40;
	// On active le chip select (niveau bas)
	MySPI_Clear_NSS();
	// Envoie de la config
	MySPI_Send(trame);
	// On effectue la lecture
	for(i=0; i<numBytes; i++)
	{
		values[i] = MySPI_Read();
	}
	// On met le Chip Select au niveau haut
	MySPI_Set_NSS();
}