306 lines
9.0 KiB
C
306 lines
9.0 KiB
C
/*
|
|
* MPU6000.c
|
|
*
|
|
* Created on: Nov 24, 2021
|
|
* Author: angoosh
|
|
*/
|
|
|
|
#include "MPU6000.h"
|
|
#include "main.h"
|
|
#include <math.h>
|
|
|
|
MPU6000_Typedef IMU;
|
|
|
|
uint32_t sysclock = 0;
|
|
|
|
void MPU6000_readGyro();
|
|
void MPU6000_readAccel();
|
|
int MPU6000_Init(I2C_HandleTypeDef *hi2c, uint32_t timeout);
|
|
uint8_t MPU6000_readRegister(uint8_t reg);
|
|
void MPU6000_Calibrate();
|
|
void MPU6000_Gyro();
|
|
void MPU6000_Raw_Data_Convert();
|
|
void MPU6000_readAll();
|
|
void MPU6000_Pedometer_Init();
|
|
void MPU6000_Pedometer();
|
|
void MPU6000_I2C_CallbackFunc(I2C_HandleTypeDef *hi2c);
|
|
void IMU_Check_State();
|
|
|
|
madgwick_handle_t madgwick_handle;
|
|
madgwick_quat_data_t quat_data;
|
|
|
|
/**
|
|
* @brief Converts raw sensor data to usable format
|
|
* @retval none
|
|
*/
|
|
void MPU6000_Raw_Data_Convert(){
|
|
for(int i = 0; i < 6; i++){
|
|
IMU.accel_data[i] = IMU.all_data[i];
|
|
IMU.gyro_data[i] = IMU.all_data[i+8];
|
|
}
|
|
|
|
IMU.g_x = (float)((int16_t)(IMU.gyro_data[0] * 256 + IMU.gyro_data[1]) - IMU.cal_g_x) / IMU.gyro_LSB;
|
|
IMU.g_y = (float)((int16_t)(IMU.gyro_data[2] * 256 + IMU.gyro_data[3]) - IMU.cal_g_y) / IMU.gyro_LSB;
|
|
IMU.g_z = (float)((int16_t)(IMU.gyro_data[4] * 256 + IMU.gyro_data[5]) - IMU.cal_g_z) / IMU.gyro_LSB;
|
|
|
|
IMU.a_x = (float)((int16_t)(IMU.accel_data[0] * 256 + IMU.accel_data[1]) - IMU.cal_a_x) / IMU.accel_LSB;
|
|
IMU.a_y = (float)((int16_t)(IMU.accel_data[2] * 256 + IMU.accel_data[3]) - IMU.cal_a_y) / IMU.accel_LSB;
|
|
IMU.a_z = (float)((int16_t)(IMU.accel_data[4] * 256 + IMU.accel_data[5]) - IMU.cal_a_z) / IMU.accel_LSB;
|
|
}
|
|
|
|
/**
|
|
* @brief Read all data on the sensor
|
|
* @retval none
|
|
*/
|
|
void MPU6000_readAll(){
|
|
HAL_I2C_Mem_Read_IT(IMU.peripheral,(MPU6000_ADDRESS << 1),ACCEL_XOUT_H,1,IMU.all_data,14);
|
|
}
|
|
|
|
/**
|
|
* @brief Read only gyroscope
|
|
* @retval none
|
|
*/
|
|
void MPU6000_readGyro(){
|
|
switch(GYRO_RANGE){
|
|
case 0:
|
|
IMU.gyro_LSB = 131;
|
|
case 1:
|
|
IMU.gyro_LSB = 65.5;
|
|
case 2:
|
|
IMU.gyro_LSB = 32.8;
|
|
case 3:
|
|
IMU.gyro_LSB = 16.4;
|
|
default:
|
|
IMU.gyro_LSB = 16.4;
|
|
}
|
|
HAL_I2C_Mem_Read_IT(IMU.peripheral,(MPU6000_ADDRESS << 1),GYRO_XOUT_H,1,IMU.gyro_data,6);
|
|
}
|
|
|
|
/**
|
|
* @brief Read only accelerometer
|
|
* @retval none
|
|
*/
|
|
void MPU6000_readAccel(){
|
|
switch(GYRO_RANGE){
|
|
case 0:
|
|
IMU.accel_LSB = 16384;
|
|
case 1:
|
|
IMU.accel_LSB = 8192;
|
|
case 2:
|
|
IMU.accel_LSB = 4096;
|
|
case 3:
|
|
IMU.accel_LSB = 2048;
|
|
default:
|
|
IMU.accel_LSB = 2048;
|
|
}
|
|
|
|
HAL_I2C_Mem_Read_IT(IMU.peripheral,(MPU6000_ADDRESS << 1),ACCEL_XOUT_H,1,IMU.accel_data,6);
|
|
}
|
|
|
|
/**
|
|
* @brief Converts accelerometer data to roll pitch and yaw
|
|
* @retval none
|
|
*/
|
|
void MPU6000_Gyro(){
|
|
IMU.roll = atan2(IMU.a_y, IMU.a_z) * 180.0 / M_PI;
|
|
IMU.pitch = atan2(IMU.a_x, IMU.a_z) * 180.0 / M_PI;
|
|
IMU.yaw = atan2(IMU.a_x, IMU.a_y) * 180.0 / M_PI;
|
|
}
|
|
|
|
/**
|
|
* @brief Initialization function of MPU6000 library
|
|
* @param i2c peripheral to be used
|
|
* @param i2c timeout
|
|
* @retval none
|
|
*/
|
|
int MPU6000_Init(I2C_HandleTypeDef *hi2c, uint32_t timeout){
|
|
uint8_t data;
|
|
|
|
IMU.accel_LSB = 16384;
|
|
IMU.gyro_LSB = 131;
|
|
IMU.peripheral = hi2c;
|
|
IMU.timeout = timeout;
|
|
|
|
IMU.dataReady = 0;
|
|
|
|
madgwick_cfg_t madgwick_cfg;
|
|
|
|
madgwick_cfg.beta = MADGWICK_BETA;
|
|
madgwick_cfg.sample_freq = MADGWICK_SAMPLE_RATE;
|
|
madgwick_handle = madgwick_init(&madgwick_cfg);
|
|
|
|
if(HAL_I2C_IsDeviceReady(hi2c, (uint16_t)(MPU6000_ADDRESS << 1), 3, IMU.timeout) != 0){
|
|
return 1;
|
|
}
|
|
|
|
data = 0x00;
|
|
HAL_I2C_Mem_Write(IMU.peripheral,(MPU6000_ADDRESS << 1),PWR_MGMT_1,1,&data,1,IMU.timeout);
|
|
HAL_Delay(200);
|
|
|
|
data = 0x07;
|
|
HAL_I2C_Mem_Write(IMU.peripheral,(MPU6000_ADDRESS << 1),SMPLRT_DIV,1,&data,1,IMU.timeout);
|
|
HAL_Delay(50);
|
|
data = 0x00;
|
|
HAL_I2C_Mem_Write(IMU.peripheral,(MPU6000_ADDRESS << 1),ACCEL_CONFIG,1,&data,1,IMU.timeout);
|
|
HAL_Delay(50);
|
|
data = 0x00;
|
|
HAL_I2C_Mem_Write(IMU.peripheral,(MPU6000_ADDRESS << 1),GYRO_CONFIG,1,&data,1,IMU.timeout);
|
|
HAL_Delay(50);
|
|
// data = 0;
|
|
// HAL_I2C_Mem_Write(IMU.peripheral,(MPU6000_ADDRESS << 1),PWR_MGMT_1,1,&data,1,IMU.timeout);
|
|
// HAL_Delay(50);
|
|
// data = 0x08;//0x28 pro lowpower rezim s frekvenci 1.25Hz
|
|
// HAL_I2C_Mem_Write(IMU.peripheral,(MPU6000_ADDRESS << 1),PWR_MGMT_1,1,&data,1,IMU.timeout);
|
|
// HAL_Delay(50);
|
|
data = 0x00;
|
|
HAL_I2C_Mem_Write(IMU.peripheral,(MPU6000_ADDRESS << 1),PWR_MGMT_2,1,&data,1,IMU.timeout);
|
|
HAL_Delay(50);
|
|
|
|
MPU6000_readAll();
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* @brief Read certain register of sensor
|
|
* @param register address
|
|
* @retval value of register
|
|
*/
|
|
uint8_t MPU6000_readRegister(uint8_t reg){
|
|
uint8_t reg_data[1];
|
|
|
|
HAL_I2C_Mem_Read(IMU.peripheral,(MPU6000_ADDRESS << 1),reg,1,reg_data,1,IMU.timeout);
|
|
HAL_Delay(50);
|
|
return reg_data[0];
|
|
}
|
|
|
|
/**
|
|
* @brief Calibrate the sensor
|
|
* @retval none
|
|
*/
|
|
void MPU6000_Calibrate(){
|
|
uint8_t data[1] = {GYRO_XOUT_H};
|
|
uint8_t data_out[6];
|
|
|
|
HAL_I2C_Master_Transmit(IMU.peripheral, (MPU6000_ADDRESS << 1), data, 1, IMU.timeout);
|
|
HAL_I2C_Master_Receive(IMU.peripheral, (MPU6000_ADDRESS << 1), data_out, 6, IMU.timeout);
|
|
|
|
IMU.cal_g_x = data_out[0] * 256 + data_out[1];
|
|
IMU.cal_g_y = data_out[2] * 256 + data_out[3];
|
|
IMU.cal_g_z = data_out[4] * 256 + data_out[5];
|
|
|
|
data[0] = ACCEL_XOUT_H;
|
|
HAL_I2C_Master_Transmit(IMU.peripheral, (MPU6000_ADDRESS << 1), data, 1, IMU.timeout);
|
|
HAL_I2C_Master_Receive(IMU.peripheral, (MPU6000_ADDRESS << 1), data_out, 6, IMU.timeout);
|
|
|
|
IMU.cal_a_x = data_out[0] * 256 + data_out[1];
|
|
IMU.cal_a_y = data_out[2] * 256 + data_out[3];
|
|
IMU.cal_a_z = data_out[4] * 256 + data_out[5];
|
|
}
|
|
|
|
/**
|
|
* @brief Initialize pedometer
|
|
* @retval none
|
|
*/
|
|
void MPU6000_Pedometer_Init(){
|
|
IMU.pedo_index = 1;
|
|
IMU.pedo_steps = 0;
|
|
IMU.pedo_vector[0] = 0;
|
|
IMU.pedo_flag = 0;
|
|
IMU.pedo_threshold = 2000;
|
|
IMU.pedo_vector_max = 0;
|
|
IMU.pedo_vector_avg = 0;
|
|
IMU.pedo_vector_samples = 0;
|
|
|
|
sysclock = HAL_RCC_GetSysClockFreq();
|
|
}
|
|
|
|
/**
|
|
* @brief Pedometer calculations
|
|
* @retval none
|
|
*/
|
|
void MPU6000_Pedometer(){
|
|
IMU.pedo_a_x[IMU.pedo_index] = IMU.a_x * IMU.accel_LSB;
|
|
IMU.pedo_a_y[IMU.pedo_index] = IMU.a_y * IMU.accel_LSB;
|
|
IMU.pedo_a_z[IMU.pedo_index] = IMU.a_z * IMU.accel_LSB;
|
|
|
|
IMU.pedo_work_vector[IMU.pedo_index] = sqrt(((IMU.pedo_a_x[IMU.pedo_index] - IMU.pedo_avg_a_x) * (IMU.pedo_a_x[IMU.pedo_index] - IMU.pedo_avg_a_x)) + ((IMU.pedo_a_y[IMU.pedo_index] - IMU.pedo_avg_a_y) * (IMU.pedo_a_y[IMU.pedo_index] - IMU.pedo_avg_a_y)) + ((IMU.pedo_a_z[IMU.pedo_index] - IMU.pedo_avg_a_z) * (IMU.pedo_a_z[IMU.pedo_index] - IMU.pedo_avg_a_z)));
|
|
|
|
IMU.pedo_vector[0] = (IMU.pedo_work_vector[IMU.pedo_index] + IMU.pedo_work_vector[IMU.pedo_index - 1]) / 2 ;
|
|
|
|
if(IMU.pedo_vector[0] > IMU.pedo_threshold && IMU.pedo_flag == 0){
|
|
IMU.pedo_steps = IMU.pedo_steps + 1;
|
|
IMU.pedo_last_step_timestamp = HAL_GetTick();
|
|
IMU.pedo_flag = 1;
|
|
}
|
|
else if (IMU.pedo_vector[0] > IMU.pedo_threshold && IMU.pedo_flag == 1){
|
|
// Don't Count
|
|
}
|
|
if (IMU.pedo_vector[0] < IMU.pedo_threshold && IMU.pedo_flag == 1){
|
|
IMU.pedo_flag = 0;
|
|
}
|
|
if (IMU.pedo_steps < 0) {
|
|
IMU.pedo_steps = 0;
|
|
}
|
|
IMU.pedo_index++;
|
|
|
|
if(IMU.pedo_work_vector[IMU.pedo_index] > IMU.pedo_vector_max){
|
|
IMU.pedo_vector_max = IMU.pedo_work_vector[IMU.pedo_index];
|
|
}
|
|
|
|
IMU.pedo_vector_avg = IMU.pedo_vector_avg + (IMU.pedo_vector[0] - IMU.pedo_vector_avg) / ((float)IMU.pedo_vector_samples + 1);
|
|
IMU.pedo_vector_samples ++;
|
|
|
|
if(IMU.pedo_index >= 100){
|
|
IMU.pedo_index = 1;
|
|
IMU.pedo_vector[0] = 0;
|
|
|
|
float sum[3] = {0};
|
|
for(int i = 0; i < 100; i++){
|
|
sum[0] = IMU.pedo_a_x[i] + sum[0];
|
|
sum[1] = IMU.pedo_a_y[i] + sum[1];
|
|
sum[2] = IMU.pedo_a_z[i] + sum[2];
|
|
}
|
|
IMU.pedo_avg_a_x = sum[0] / 100.0;
|
|
IMU.pedo_avg_a_y = sum[1] / 100.0;
|
|
IMU.pedo_avg_a_z = sum[2] / 100.0;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @brief Function to be put into i2c callback function
|
|
* @retval none
|
|
*/
|
|
void MPU6000_I2C_CallbackFunc(I2C_HandleTypeDef *hi2c){
|
|
if(hi2c -> Instance == IMU.peripheral->Instance){
|
|
IMU.peripheral->Instance->CR1 &= ~(1 << 1);
|
|
IMU.dataReady = 1;
|
|
// MPU6000_Raw_Data_Convert();
|
|
// madgwick_update_6dof(madgwick_handle, IMU.g_x * DEG2RAD, IMU.g_y * DEG2RAD, IMU.g_z * DEG2RAD, IMU.a_x, IMU.a_y, IMU.a_z);
|
|
// madgwick_get_quaternion(madgwick_handle, &quat_data);
|
|
|
|
// IMU.roll = 180.0 / 3.14 * atan2(2 * (quat_data.q0 * quat_data.q1 + quat_data.q2 * quat_data.q3), 1 - 2 * (quat_data.q1 * quat_data.q1 + quat_data.q2 * quat_data.q2));
|
|
// IMU.pitch = 180.0 / 3.14 * asin(2 * (quat_data.q0 * quat_data.q2 - quat_data.q3 * quat_data.q1));
|
|
// IMU.yaw = 180.0 / 3.14 * atan2f(quat_data.q0 * quat_data.q3 + quat_data.q1 * quat_data.q2, 0.5f - quat_data.q2 * quat_data.q2 - quat_data.q3 * quat_data.q3);
|
|
//
|
|
// IMU.dataReady = 0;
|
|
// MPU6000_readAll();
|
|
}
|
|
}
|
|
|
|
void IMU_Check_State(){
|
|
if(IMU.dataReady){
|
|
MPU6000_Raw_Data_Convert();
|
|
madgwick_update_6dof(madgwick_handle, IMU.g_x * DEG2RAD, IMU.g_y * DEG2RAD, IMU.g_z * DEG2RAD, IMU.a_x, IMU.a_y, IMU.a_z);
|
|
madgwick_get_quaternion(madgwick_handle, &quat_data);
|
|
|
|
IMU.roll = 180.0 / 3.14 * atan2(2 * (quat_data.q0 * quat_data.q1 + quat_data.q2 * quat_data.q3), 1 - 2 * (quat_data.q1 * quat_data.q1 + quat_data.q2 * quat_data.q2));
|
|
IMU.pitch = 180.0 / 3.14 * asin(2 * (quat_data.q0 * quat_data.q2 - quat_data.q3 * quat_data.q1));
|
|
IMU.yaw = 180.0 / 3.14 * atan2f(quat_data.q0 * quat_data.q3 + quat_data.q1 * quat_data.q2, 0.5f - quat_data.q2 * quat_data.q2 - quat_data.q3 * quat_data.q3);
|
|
|
|
IMU.dataReady = 0;
|
|
MPU6000_readAll();
|
|
}
|
|
}
|
|
|