40 lines
1.1 KiB
C
40 lines
1.1 KiB
C
/*
|
|
* 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;
|
|
}
|