/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; Copyright (c) 2020 STMicroelectronics.
  * All rights reserved.</center></h2>
  *
  * This software component is licensed by ST under BSD 3-Clause license,
  * the "License"; You may not use this file except in compliance with the
  * License. You may obtain a copy of the License at:
  *                        opensource.org/licenses/BSD-3-Clause
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "crc.h"
#include "i2c.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
/* Initialize mems driver interface */
uint8_t UartDataOut[MAX_LEN_UART_TX_BUFFER];
uint8_t whoamI, rst;

stmdev_ctx_t dev_ctx_mg;
stmdev_ctx_t dev_ctx_xl;

static axis3bit16_t data_raw_acceleration;
static axis3bit16_t data_raw_magnetic;

/* Extern variables ----------------------------------------------------------*/
uint32_t SensorsEnabled;
SENSOR_Axes_t AccValue;
SENSOR_Axes_t MagValue;
SENSOR_Axes_t MagValueComp;

uint8_t MCLibVersion[25];
uint8_t ECLibVersion[25];

volatile uint32_t TimeStamp = 0;
volatile uint32_t iteration = 0;

MEC_output_t 	Ecompas_data;
MMC_Input_t 	mag;
MMC_Output_t 	params;

uint8_t door_open_flag = 0;
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */
//void I2C_Debug(void);
static void Init_Sensors(void);
static void Accelero_Sensor_Handler(void);
static void Magneto_Sensor_Handler(void);
static int32_t platform_write(void *handle, uint8_t Reg, uint8_t *Bufp, uint16_t len);
static int32_t platform_read(void *handle, uint8_t Reg, uint8_t *Bufp, uint16_t len);
int16_t Abs_Degree(float a);
/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */
	/* Initialize mems driver interface */
	dev_ctx_xl.write_reg = platform_write;
	dev_ctx_xl.read_reg = platform_read;
	dev_ctx_xl.handle = (void*)LSM303AGR_I2C_ADD_XL;

	dev_ctx_mg.write_reg = platform_write;
	dev_ctx_mg.read_reg = platform_read;
	dev_ctx_mg.handle = (void*)LSM303AGR_I2C_ADD_MG;

	uint32_t delay5s, delay10ms;
	uint16_t step = 0;
	float SumY = 0, SumP = 0, SumR = 0;

	//Angles_t AnglesMin;
	//Angles_t AnglesMax;
	Angles_t AnglesRef;
	Angles_t Angles;
  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */
  /*AnglesMin.yaw 	= 0;
  AnglesMin.pitch 	= 0;
  AnglesMin.roll 	= 0;
  AnglesMax.yaw 	= 0;
  AnglesMax.pitch 	= 0;
  AnglesMax.roll 	= 0;*/
  AnglesRef.yaw 	= 0;
  AnglesRef.pitch 	= 0;
  AnglesRef.roll 	= 0;
  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_I2C1_Init();
  MX_USART2_UART_Init();
  MX_CRC_Init();
  /* USER CODE BEGIN 2 */
  sprintf((char *)UartDataOut, "[INIT] Initialization begin\r\n");
  HAL_UART_Transmit(&huart2, UartDataOut, sizeof(UartDataOut), UART_TIMEOUT);
  for(int i = 0; i < MAX_LEN_UART_TX_BUFFER; i ++) UartDataOut[i] = '\0';

  /* Sensor init */
  Init_Sensors();

  /* Librairy initialization */
  MotionMC_Initialize(ALGO_PERIOD, 1);
  MotionEC_manager_init(ALGO_FREQ);

  delay10ms = HAL_GetTick();

  /* Get lib version */
  MotionMC_GetLibVersion((char *)MCLibVersion);
  MotionEC_GetLibVersion((char *)ECLibVersion);

  sprintf((char *)UartDataOut, "[INIT] MC lib Version : %s\r\n[INIT] EC lib Version : %s\r\n", (char *)MCLibVersion, (char *)ECLibVersion);
  HAL_UART_Transmit(&huart2, UartDataOut, sizeof(UartDataOut), UART_TIMEOUT);
  for(int i = 0; i < MAX_LEN_UART_TX_BUFFER; i ++) UartDataOut[i] = '\0';

  /* Ref value set up */
  while(step < 100)
  {
	  if(HAL_GetTick() - delay10ms >= ALGO_PERIOD)
	  {
		  delay10ms = HAL_GetTick();

		  /* Get data */
	  	  Accelero_Sensor_Handler();
	  	  Magneto_Sensor_Handler();

	  	  /* Run E-compas function */
	  	  MotionEC_manager_run(&Ecompas_data);

	  	  step += 1;

	  	  SumY += (Ecompas_data.euler[0]);
	  	  SumP += (Ecompas_data.euler[1]);
	  	  SumR += (Ecompas_data.euler[2]);

	  	  /* Debug */
	  	  sprintf((char *)UartDataOut, "[ANGLE CAL] \tYaw = %.3d   \tPitch = %.3d   \tRoll = %.3d  \t(degree)\r\n", (int16_t)(SumY/step), (int16_t)(SumP/step), (int16_t)(SumR/step));
	  	  HAL_UART_Transmit(&huart2, UartDataOut, sizeof(UartDataOut), UART_TIMEOUT);
	  	  for(int i = 0; i < MAX_LEN_UART_TX_BUFFER; i ++) UartDataOut[i] = '\0';
	  }
  }

  AnglesRef.yaw 	= (int16_t)SumY / step;
  AnglesRef.pitch 	= (int16_t)SumP / step;
  AnglesRef.roll 	= (int16_t)SumR / step;

  sprintf((char *)UartDataOut, "[ANGLE REF] \tYaw = %.3d   \tPitch = %.3d   \tRoll = %.3d  \t(degree)\r\n", AnglesRef.yaw, AnglesRef.pitch, AnglesRef.roll);
  HAL_UART_Transmit(&huart2, UartDataOut, sizeof(UartDataOut), UART_TIMEOUT);
  for(int i = 0; i < MAX_LEN_UART_TX_BUFFER; i ++) UartDataOut[i] = '\0';

  /* Sensor deinit */
  lsm303agr_xl_data_rate_set(&dev_ctx_xl, LSM303AGR_XL_POWER_DOWN);
  lsm303agr_mag_operating_mode_set(&dev_ctx_xl, LSM303AGR_POWER_DOWN);

  /* Librairy de-initialization */
  MotionMC_Initialize(ALGO_PERIOD, 0);

  sprintf((char *)UartDataOut, "[INIT] Initialization end\r\n\n");
  HAL_UART_Transmit(&huart2, UartDataOut, sizeof(UartDataOut), UART_TIMEOUT);
  for(int i = 0; i < MAX_LEN_UART_TX_BUFFER; i ++) UartDataOut[i] = '\0';

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {

	HAL_Delay(20000);		//wait for 20 seconds

	/* Librairy initialization */
	MotionMC_manager_init(ALGO_PERIOD, 1);
	MotionEC_manager_init(ALGO_FREQ);

	delay10ms = HAL_GetTick();

	/* Get lib version */
	MotionMC_GetLibVersion((char *)MCLibVersion);
	MotionEC_GetLibVersion((char *)ECLibVersion);

	/* Sensor init */
	Init_Sensors();

	delay5s = HAL_GetTick();

	while(HAL_GetTick() - delay5s < 5000)
	{
		if(HAL_GetTick() - delay10ms >= ALGO_PERIOD)
		{
			delay10ms = HAL_GetTick();

			/* Get data */
			Accelero_Sensor_Handler();
			Magneto_Sensor_Handler();

			/* Run E-compas function */
			MotionEC_manager_run(&Ecompas_data);

			Angles.yaw 	= (int16_t)(Ecompas_data.euler[0]);
			Angles.pitch 	= (int16_t)(Ecompas_data.euler[1]);
			Angles.roll 	= (int16_t)(Ecompas_data.euler[2]);

/*		    AnglesMin.yaw 	= MIN(AnglesMin.yaw, 	Angles.yaw 		- AnglesRef.yaw		);
		    AnglesMin.pitch = MIN(AnglesMin.pitch, 	Angles.pitch 	- AnglesRef.pitch	);
		    AnglesMin.roll 	= MIN(AnglesMin.roll, 	Angles.roll 	- AnglesRef.roll	);

		    AnglesMax.yaw	= MAX(AnglesMax.yaw, 	Angles.yaw		- AnglesRef.yaw		);
		    AnglesMax.pitch	= MAX(AnglesMax.pitch, 	Angles.pitch	- AnglesRef.pitch	);
		    AnglesMax.roll 	= MAX(AnglesMax.roll, 	Angles.roll 	- AnglesRef.roll	);	*/

		    /*sprintf((char *)UartDataOut, "[ANGLE] yaw : [%.3d, %.3d], \tPitch : [%.3d, %.3d], \tRoll : [%.3d, %.3d]\r\n"
		  	  	  	  	  	 	 	   , AnglesMin.yaw, AnglesMax.yaw, AnglesMin.pitch, AnglesMax.pitch , AnglesMin.roll , AnglesMax.roll);
		    HAL_UART_Transmit(&huart2, UartDataOut, sizeof(UartDataOut), UART_TIMEOUT);
		    for(int i = 0; i < MAX_LEN_UART_TX_BUFFER; i ++) UartDataOut[i] = '\0';*/

			sprintf((char *)UartDataOut, "[ANGLE]     \tYaw = %.3d   \tPitch = %.3d   \tRoll = %.3d  \t(degree)\r\n", Angles.yaw - AnglesRef.yaw, Angles.pitch - AnglesRef.pitch, Angles.roll - AnglesRef.roll);
			HAL_UART_Transmit(&huart2, UartDataOut, sizeof(UartDataOut), UART_TIMEOUT);
			for(int i = 0; i < MAX_LEN_UART_TX_BUFFER; i ++) UartDataOut[i] = '\0';
		}

	}

	/* Sensor deinit */
	lsm303agr_xl_data_rate_set(&dev_ctx_xl, LSM303AGR_XL_POWER_DOWN);
	lsm303agr_mag_operating_mode_set(&dev_ctx_xl, LSM303AGR_POWER_DOWN);

	/* Librairy de-initialization */
	MotionMC_Initialize(ALGO_PERIOD, 0);

    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
  RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};

  /** Initializes the CPU, AHB and APB busses clocks 
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
  RCC_OscInitStruct.PLL.PLLM = 1;
  RCC_OscInitStruct.PLL.PLLN = 10;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV7;
  RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2;
  RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }
  /** Initializes the CPU, AHB and APB busses clocks 
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK)
  {
    Error_Handler();
  }
  PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_USART2|RCC_PERIPHCLK_I2C1;
  PeriphClkInit.Usart2ClockSelection = RCC_USART2CLKSOURCE_PCLK1;
  PeriphClkInit.I2c1ClockSelection = RCC_I2C1CLKSOURCE_PCLK1;
  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
  {
    Error_Handler();
  }
  /** Configure the main internal regulator output voltage 
  */
  if (HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */
static int32_t platform_write(void *handle, uint8_t Reg, uint8_t *Bufp, uint16_t len)
{
  uint32_t i2c_add = (uint32_t)handle;
	if (i2c_add == LSM303AGR_I2C_ADD_XL)
  {
    /* enable auto incremented in multiple read/write commands */
    Reg |= 0x80;
  }

	#ifdef DEBUG
	snprintf((char *)UartDataOut, MAX_LEN_UART_TX_BUFFER, "[DEBUG] Trying to write 0x%.2X in the register 0x%.2X at the address 0x%.2X\r\n", *Bufp, Reg, i2c_add);
	HAL_UART_Transmit(&huart2, UartDataOut, sizeof(UartDataOut), UART_TIMEOUT);
	for(int i = 0; i < MAX_LEN_UART_TX_BUFFER; i ++) UartDataOut[i] = '\0';

	uint8_t errorCode;
	errorCode =
	#endif
	HAL_I2C_Mem_Write(&hi2c1, i2c_add, Reg, I2C_MEMADD_SIZE_8BIT, Bufp, len, 1000);
	HAL_Delay(10);

	#ifdef DEBUG_I2C
	if(errorCode == HAL_OK)
		snprintf((char *)UartDataOut, MAX_LEN_UART_TX_BUFFER, "[DEBUG I2C] I2C comunication OK\r\n");
	else if(errorCode == HAL_TIMEOUT)
		snprintf((char *)UartDataOut, MAX_LEN_UART_TX_BUFFER, "[DEBUG I2C] I2C comunication timeout\r\n");
	else if(errorCode == HAL_BUSY)
		snprintf((char *)UartDataOut, MAX_LEN_UART_TX_BUFFER, "[DEBUG I2C] I2C comunication busy\r\n");
	else
		snprintf((char *)UartDataOut, MAX_LEN_UART_TX_BUFFER, "[DEBUG I2C] I2C comunication error\r\n");

	HAL_UART_Transmit(&huart2, UartDataOut, sizeof(UartDataOut), UART_TIMEOUT);
	for(int i = 0; i < MAX_LEN_UART_TX_BUFFER; i ++) UartDataOut[i] = '\0';
	#endif

  return 0;
}

static int32_t platform_read(void *handle, uint8_t Reg, uint8_t *Bufp, uint16_t len)
{
  uint32_t i2c_add = (uint32_t)handle;
	if (i2c_add == LSM303AGR_I2C_ADD_XL)
  {
    /* enable auto incremented in multiple read/write commands */
    Reg |= 0x80;
  }

	#ifdef DEBUG
	snprintf((char *)UartDataOut, MAX_LEN_UART_TX_BUFFER, "[DEBUG] Trying to read the register 0x%.2X at the address 0x%.2X\r\n", Reg, i2c_add);
	HAL_UART_Transmit(&huart2, UartDataOut, sizeof(UartDataOut), UART_TIMEOUT);
	for(int i = 0; i < MAX_LEN_UART_TX_BUFFER; i ++) UartDataOut[i] = '\0';

	uint8_t errorCode;
	errorCode =
	#endif
	HAL_I2C_Mem_Read(&hi2c1, (uint8_t) i2c_add, Reg, I2C_MEMADD_SIZE_8BIT, Bufp, len, 1000);
	HAL_Delay(10);

	#ifdef DEBUG
	#ifdef DEBUG_I2C
	if(errorCode == HAL_OK)
		snprintf((char *)UartDataOut, MAX_LEN_UART_TX_BUFFER, "[DEBUG I2C] I2C comunication OK\r\n");
	else if(errorCode == HAL_TIMEOUT)
		snprintf((char *)UartDataOut, MAX_LEN_UART_TX_BUFFER, "[DEBUG I2C] I2C comunication timeout\r\n");
	else if(errorCode == HAL_BUSY)
		snprintf((char *)UartDataOut, MAX_LEN_UART_TX_BUFFER, "[DEBUG I2C] I2C comunication busy\r\n");
	else
		snprintf((char *)UartDataOut, MAX_LEN_UART_TX_BUFFER, "[DEBUG I2C] I2C comunication error\r\n");

	HAL_UART_Transmit(&huart2, UartDataOut, sizeof(UartDataOut), UART_TIMEOUT);
	for(int i = 0; i < MAX_LEN_UART_TX_BUFFER; i ++) UartDataOut[i] = '\0';
	#endif

	snprintf((char *)UartDataOut, MAX_LEN_UART_TX_BUFFER, "[DEBUG] The value of the register 0x%.2X is 0x%.2X\r\n", Reg, *Bufp);
	HAL_UART_Transmit(&huart2, UartDataOut, sizeof(UartDataOut), UART_TIMEOUT);
	for(int i = 0; i < MAX_LEN_UART_TX_BUFFER; i ++) UartDataOut[i] = '\0';
	#endif

  return 0;
}
/**
 * @brief  Handles the ACC axes data getting/sending
 * @param  Msg the ACC part of the stream
 * @param  Instance the device instance
 * @retval None
 */
static void Accelero_Sensor_Handler(void)
{
  /* Read accelerometer data */
  memset(data_raw_acceleration.u8bit, 0x00, 3*sizeof(int16_t));
  lsm303agr_acceleration_raw_get(&dev_ctx_xl, data_raw_acceleration.u8bit);
  AccValue.x = lsm303agr_from_fs_2g_hr_to_mg( data_raw_acceleration.i16bit[0] );
  AccValue.y = lsm303agr_from_fs_2g_hr_to_mg( data_raw_acceleration.i16bit[1] );
  AccValue.z = lsm303agr_from_fs_2g_hr_to_mg( data_raw_acceleration.i16bit[2] );
}
/**
 * @brief  Handles the MAG axes data getting/sending
 * @param  Msg the MAG part of the stream
 * @param  Instance the device instance
 * @retval None
 */
static void Magneto_Sensor_Handler(void)
{
	/* Read magnetic field data */
	memset(data_raw_magnetic.u8bit, 0x00, 3*sizeof(int16_t));
	lsm303agr_magnetic_raw_get(&dev_ctx_mg, data_raw_magnetic.u8bit);
	MagValue.x = lsm303agr_from_lsb_to_mgauss( data_raw_magnetic.i16bit[0]);
	MagValue.y = lsm303agr_from_lsb_to_mgauss( data_raw_magnetic.i16bit[1]);
	MagValue.z = lsm303agr_from_lsb_to_mgauss( data_raw_magnetic.i16bit[2]);

	/* Convert magnetometer data from [mGauss] to [uT] */
	mag.Mag[0] = (float)MagValue.x / 10.0f;
	mag.Mag[1] = (float)MagValue.y / 10.0f;
	mag.Mag[2] = (float)MagValue.z / 10.0f;

	/* Set time stamp */
	mag.TimeStamp = TimeStamp;

	/* Magnetometer update */
	MotionMC_Update(&mag);

	/* Get magnetometer calibration parameters */
	MotionMC_GetCalParams(&params);

	/* Calibrate data */
	MagValueComp.x = 0;
	MagValueComp.y = 0;
	MagValueComp.z = 0;
	for (int j = 0; j < 3; j++)
	{
		MagValueComp.x += (MagValue.x - params.HI_Bias[j] * 10.0f) * params.SF_Matrix[0][j];
		MagValueComp.y += (MagValue.y - params.HI_Bias[j] * 10.0f) * params.SF_Matrix[1][j];
		MagValueComp.z += (MagValue.z - params.HI_Bias[j] * 10.0f) * params.SF_Matrix[2][j];
	}

	/* Debug */
	//sprintf((char *)UartDataOut, "\n[MAG] \t\tMagX : %.4ld,\t%.4ld,\t%.4ld\t(mG)\r\n", MagValue.x, MagValue.y, MagValue.z);
	//HAL_UART_Transmit(&huart2, UartDataOut, sizeof(UartDataOut), UART_TIMEOUT);
	//for(int i = 0; i < MAX_LEN_UART_TX_BUFFER; i ++) UartDataOut[i] = '\0';

	//sprintf((char *)UartDataOut, "[MAG_CMP] \tMagX : %.4ld,\t%.4ld,\t%.4ld\t(mG)\r\n", MagValueComp.x, MagValueComp.y, MagValueComp.z);
	//HAL_UART_Transmit(&huart2, UartDataOut, sizeof(UartDataOut), UART_TIMEOUT);
	//for(int i = 0; i < MAX_LEN_UART_TX_BUFFER; i ++) UartDataOut[i] = '\0';
}
/**
 * @brief  Initialize all sensors
 * @param  None
 * @retval None
 */
static void Init_Sensors(void)
{
  /* Check device ID */
  whoamI = 0;
  lsm303agr_xl_device_id_get(&dev_ctx_xl, &whoamI);
  if ( whoamI != LSM303AGR_ID_XL )
  {
	  sprintf((char *)UartDataOut, "[DEBUG] Who am I error : get 0x%.2X instead of 0x%.2X", whoamI, LSM303AGR_ID_XL);
	  HAL_UART_Transmit(&huart2, UartDataOut, sizeof(UartDataOut), UART_TIMEOUT);
	  for(int i = 0; i < MAX_LEN_UART_TX_BUFFER; i ++) UartDataOut[i] = '\0';

	  while(1);
  }

  whoamI = 0;
  lsm303agr_mag_device_id_get(&dev_ctx_mg, &whoamI);
  if ( whoamI != LSM303AGR_ID_MG )
  {
  	  sprintf((char *)UartDataOut, "[DEBUG] Who am I error : get 0x%.2X instead of 0x%.2X", whoamI, LSM303AGR_ID_MG);
  	  HAL_UART_Transmit(&huart2, UartDataOut, sizeof(UartDataOut), UART_TIMEOUT);
  	  for(int i = 0; i < MAX_LEN_UART_TX_BUFFER; i ++) UartDataOut[i] = '\0';

  	  while(1);
  }

  /* Restore default configuration for magnetometer */
  lsm303agr_mag_reset_set(&dev_ctx_mg, PROPERTY_ENABLE);
  do {
     lsm303agr_mag_reset_get(&dev_ctx_mg, &rst);
  } while (rst);

  /* Enable Block Data Update */
  lsm303agr_xl_block_data_update_set(&dev_ctx_xl, PROPERTY_ENABLE);
  lsm303agr_mag_block_data_update_set(&dev_ctx_mg, PROPERTY_ENABLE);
  /* Set Output Data Rate */
  lsm303agr_xl_data_rate_set(&dev_ctx_xl, LSM303AGR_XL_ODR_10Hz);
  lsm303agr_mag_data_rate_set(&dev_ctx_mg, LSM303AGR_MG_ODR_10Hz);
  /* Set accelerometer full scale */
  lsm303agr_xl_full_scale_set(&dev_ctx_xl, LSM303AGR_2g);
  /* Set / Reset magnetic sensor mode */
  lsm303agr_mag_set_rst_mode_set(&dev_ctx_mg, LSM303AGR_SENS_OFF_CANC_EVERY_ODR);
  /* Enable temperature compensation on mag sensor */
  lsm303agr_mag_offset_temp_comp_set(&dev_ctx_mg, PROPERTY_ENABLE);
  /* Enable temperature sensor */
  lsm303agr_temperature_meas_set(&dev_ctx_xl, LSM303AGR_TEMP_ENABLE);
  /* Set device in continuos mode */
  lsm303agr_xl_operating_mode_set(&dev_ctx_xl, LSM303AGR_HR_12bit);
  /* Set magnetometer in continuos mode */
  lsm303agr_mag_operating_mode_set(&dev_ctx_mg, LSM303AGR_CONTINUOUS_MODE);
}
/*
 *
 */
int16_t Abs_Degree(float a)
{
	while(a < -180)
		a += 360;

	while(a > 180)
		a -= 360;

	return (uint16_t)a;
}
/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
	sprintf((char *)UartDataOut, "[Error] Error_Handler somewhere");
	HAL_UART_Transmit(&huart2, UartDataOut, sizeof(UartDataOut), UART_TIMEOUT);
	for(int i = 0; i < MAX_LEN_UART_TX_BUFFER; i ++) UartDataOut[i] = '\0';
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{ 
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/