/*
 * sensor_interface.c
 *
 *  Created on: 7 déc. 2019
 *      Author: JackCarterSmith
 */

#include "sensor_interface.h"
#include "i2c.h"

#include "math.h"


void initGyro() {
	uint8_t MEMS_ODR_CONFIG []= {0x88};

	//Configure all configurations datas for sensor chip
	HAL_I2C_Mem_Write(&hi2c2,MEMS_ADD_WR,CTRL1_XL_REGISTER,1,MEMS_ODR_CONFIG,1,HAL_TIMEOUT);
}

/**
  * @brief  function to retrieve values from sensors and calculate angles from it
  */
void getAnglesFromSensor(PP_GyroAnglesTypeDef *anglesVar) {
	int16_t OUTX_XL[1],OUTY_XL[1],OUTZ_XL[1];
	float tempX, tempY, tempZ;

	HAL_I2C_Mem_Read(&hi2c2, MEMS_ADD_RD, 0x28, 1, (uint8_t*)&OUTX_XL, 2, HAL_TIMEOUT);
	HAL_I2C_Mem_Read(&hi2c2, MEMS_ADD_RD, 0x2A, 1, (uint8_t*)&OUTY_XL, 2, HAL_TIMEOUT);
	HAL_I2C_Mem_Read(&hi2c2, MEMS_ADD_RD, 0x2C, 1, (uint8_t*)&OUTZ_XL, 2, HAL_TIMEOUT);

	tempX = (float)OUTX_XL[0];
	tempY = (float)OUTY_XL[0];
	tempZ = (float)OUTZ_XL[0];

	anglesVar->Pitch = (int16_t)atanf(-tempY / tempZ) * 60;
	anglesVar->Yaw = (int16_t)atanf(-tempX / tempZ) * 60;
	anglesVar->Roll = (int16_t)atanf(-tempX / tempY) * 60;
}