陀螺仪mpu6050的使用附带HAL的使用
我们日常见的陀螺仪模块的使用就是在平衡小车和控制小车的移动上,那么陀螺仪是怎么使用的呢,首先就是能很好的使用I2C,而看到这里,说的一切都是虚的,首先陀螺仪的配置和数据手册大家也是没少看的,但是还是跟我再了解一遍.
MPU6050内部整合了三轴MEMS陀螺仪、三轴MEMS加速度计以及一个可扩展的数字运动处理器DMP(Digital Motion Processor),而且还可以连接一个第三方数字传感器(如磁力计),这样的话,就可以通过IIC接口输出一个9轴信号(链接第三方数字传感器才可以输出九轴信号,否则只有六轴信号)。更加方便的是,有了DMP(同时这个可以在官方库去移植),可以结合InvenSense公司提供的运动处理资料库,实现姿态解算。通过自带的DMP,可以通过IIC接口输出9轴融合演算的数据,大大降低了运动处理运算对操作系统的负荷,同时也降低了开发难度。其实,简单一句话说,陀螺仪就是测角速度的,加速度传感器就是测角加速度的,二者数据通过算法就可以得到PITCH、YAW、ROLL角了。
而对陀螺仪的算法精确就是低通滤波和卡尔曼滤波(在低通滤波上我已学会,在卡尔曼滤波我已学废,但是我还是鼓励大家学卡尔曼滤波,毕竟对数据可以很精准)
这张图片大家也看得很多了吧,但是我们还是应该要知道这个图片就对应这mpu6050的方向,wo我也希望在使用这个模块的时候可以看清它的方向,毕竟这种事我也经常干。
这个图咋们也能看出来这个就是mpu6050的内部框图,其中就是测温度加速度和姿态的。
这个大家可以参考一下,HAL库的玩家更要看掌握知识:链接
那么简单的来了,上实践操作部分,而且不要怕不会用,我会慢慢讲解的。
-
/*
-
*mpu6050.c
-
*
-
*Creation date: September 11th, 2021
-
*Author: Xiaodong
-
*
-
*Kalman filter algorithm used from https://github.com/TKJElectronics/KalmanFilter
-
*/
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
// Setup MPU6050
-
-
const uint16_t i2c_timeout = 100;
-
const double Accel_Z_corrector = 14418.0;
-
-
uint32_t timer;
-
-
Kalman_t KalmanX = {
-
.Q_angle = 0.001f,
-
.Q_bias = 0.003f,
-
.R_measure = 0.03f};
-
-
Kalman_t KalmanY = {
-
.Q_angle = 0.001f,
-
.Q_bias = 0.003f,
-
.R_measure = 0.03f,
-
};
-
-
uint8_t MPU6050_Init(I2C_HandleTypeDef *I2Cx)
-
{
-
uint8_t check;
-
uint8_t Data; // data buffer
-
-
// check device ID WHO_AM_I
-
-
HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, WHO_AM_I_REG, 1, &check, 1, i2c_timeout);
-
-
if (check == 104) // 0x68 will be returned by the sensor if everything goes well
-
{
-
// power management register 0X6B we should write all 0's to wake the sensor up
-
Data = 0;
-
HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, PWR_MGMT_1_REG, 1, &Data, 1, i2c_timeout);
-
-
// Set DATA RATE of 1KHz by writing SMPLRT_DIV register
-
// Sample Rate = Gyroscope Output Rate / (1 SMPLRT_DIV)
-
// Gyroscope Output Rate = 8kHz when the DLPF is disabled, and 1kHz when the DLPF is enabled.
-
Data = 0x07;
-
HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, SMPLRT_DIV_REG, 1, &Data, 1, i2c_timeout);
-
-
// Set accelerometer configuration in ACCEL_CONFIG Register
-
// XA_ST=0,YA_ST=0,ZA_ST=0, FS_SEL=0 -> - 2g
-
Data = 0x00;
-
HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, ACCEL_CONFIG_REG, 1, &Data, 1, i2c_timeout);
-
-
// Set Gyroscopic configuration in GYRO_CONFIG Register
-
// XG_ST=0,YG_ST=0,ZG_ST=0, FS_SEL=0 -> - 250 degree/s
-
Data = 0x00;
-
HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, GYRO_CONFIG_REG, 1, &Data, 1, i2c_timeout);
-
return 0;
-
}
-
return 1;
-
}
-
-
void MPU6050_Read_Accel(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
-
{
-
uint8_t Rec_Data[6];
-
-
// Read 6 BYTES of data starting from ACCEL_XOUT_H register
-
-
HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, Rec_Data, 6, i2c_timeout);
-
-
DataStruct->Accel_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
-
DataStruct->Accel_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);
-
DataStruct->Accel_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);
-
-
/*** convert the RAW values into acceleration in 'g'
-
we have to divide according to the Full scale value set in FS_SEL
-
I have configured FS_SEL = 0. So I am dividing by 16384.0
-
for more details check ACCEL_CONFIG Register ****/
-
-
DataStruct->Ax = DataStruct->Accel_X_RAW / 16384.0;
-
DataStruct->Ay = DataStruct->Accel_Y_RAW / 16384.0;
-
DataStruct->Az = DataStruct->Accel_Z_RAW / Accel_Z_corrector;
-
// DataStruct->Az = DataStruct->Accel_Z_RAW / 16384.0;
-
}
-
-
void MPU6050_Read_Gyro(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
-
{
-
uint8_t Rec_Data[6];
-
-
// Read 6 BYTES of data starting from GYRO_XOUT_H register
-
-
HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, GYRO_XOUT_H_REG, 1, Rec_Data, 6, i2c_timeout);
-
-
DataStruct->Gyro_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
-
DataStruct->Gyro_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);
-
DataStruct->Gyro_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);
-
-
/*** convert the RAW values into dps (�/s)
-
we have to divide according to the Full scale value set in FS_SEL
-
I have configured FS_SEL = 0. So I am dividing by 131.0
-
for more details check GYRO_CONFIG Register ****/
-
-
DataStruct->Gx = DataStruct->Gyro_X_RAW / 131.0;
-
DataStruct->Gy = DataStruct->Gyro_Y_RAW / 131.0;
-
DataStruct->Gz = DataStruct->Gyro_Z_RAW / 131.0;
-
}
-
-
void MPU6050_Read_Temp(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
-
{
-
uint8_t Rec_Data[2];
-
int16_t temp;
-
-
// Read 2 BYTES of data starting from TEMP_OUT_H_REG register
-
-
HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, TEMP_OUT_H_REG, 1, Rec_Data, 2, i2c_timeout);
-
-
temp = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
-
DataStruct->Temperature = (float)((int16_t)temp / (float)340.0 (float)36.53);
-
}
-
-
void MPU6050_Read_All(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
-
{
-
uint8_t Rec_Data[14];
-
int16_t temp;
-
-
// Read 14 BYTES of data starting from ACCEL_XOUT_H register
-
// Accel and gyro's x, y and z data is seriate, so just read 14 bytes for the first register
-
-
HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, Rec_Data, 14, i2c_timeout);
-
-
DataStruct->Accel_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
-
DataStruct->Accel_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);
-
DataStruct->Accel_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);
-
temp = (int16_t)(Rec_Data[6] << 8 | Rec_Data[7]);
-
DataStruct->Gyro_X_RAW = (int16_t)(Rec_Data[8] << 8 | Rec_Data[9]);
-
DataStruct->Gyro_Y_RAW = (int16_t)(Rec_Data[10] << 8 | Rec_Data[11]);
-
DataStruct->Gyro_Z_RAW = (int16_t)(Rec_Data[12] << 8 | Rec_Data[13]);
-
-
DataStruct->Ax = DataStruct->Accel_X_RAW / 16384.0; // unit: g
-
DataStruct->Ay = DataStruct->Accel_Y_RAW / 16384.0;
-
DataStruct->Az = DataStruct->Accel_Z_RAW / Accel_Z_corrector;
-
DataStruct->Temperature = (float)((int16_t)temp / (float)340.0 (float)36.53);
-
DataStruct->Gx = DataStruct->Gyro_X_RAW / 131.0;
-
DataStruct->Gy = DataStruct->Gyro_Y_RAW / 131.0;
-
DataStruct->Gz = DataStruct->Gyro_Z_RAW / 131.0;
-
-
// Kalman angle solve
-
double dt = (double)(HAL_GetTick() - timer) / 1000;
-
timer = HAL_GetTick();
-
double roll;
-
double roll_sqrt = sqrt(
-
DataStruct->Accel_X_RAW * DataStruct->Accel_X_RAW DataStruct->Accel_Z_RAW * DataStruct->Accel_Z_RAW);
-
if (roll_sqrt != 0.0)
-
{
-
roll = atan(DataStruct->Accel_Y_RAW / roll_sqrt) * RAD_TO_DEG;
-
}
-
else
-
{
-
roll = 0.0;
-
}
-
double pitch = atan2(-DataStruct->Accel_X_RAW, DataStruct->Accel_Z_RAW) * RAD_TO_DEG;
-
if ((pitch < -90 && DataStruct->KalmanAngleY > 90) || (pitch > 90 && DataStruct->KalmanAngleY < -90))
-
{
-
KalmanY.angle = pitch;
-
DataStruct->KalmanAngleY = pitch;
-
}
-
else
-
{
-
DataStruct->KalmanAngleY = Kalman_getAngle(&KalmanY, pitch, DataStruct->Gy, dt);
-
}
-
if (fabs(DataStruct->KalmanAngleY) > 90)
-
DataStruct->Gx = -DataStruct->Gx;
-
DataStruct->KalmanAngleX = Kalman_getAngle(&KalmanX, roll, DataStruct->Gx, dt);
-
}
-
-
double Kalman_getAngle(Kalman_t *Kalman, double newAngle, double newRate, double dt)
-
{
-
double rate = newRate - Kalman->bias;
-
Kalman->angle = dt * rate;
-
-
Kalman->P[0][0] = dt * (dt * Kalman->P[1][1] - Kalman->P[0][1] - Kalman->P[1][0] Kalman->Q_angle);
-
Kalman->P[0][1] -= dt * Kalman->P[1][1];
-
Kalman->P[1][0] -= dt * Kalman->P[1][1];
-
Kalman->P[1][1] = Kalman->Q_bias * dt;
-
-
double S = Kalman->P[0][0] Kalman->R_measure;
-
double K[2];
-
K[0] = Kalman->P[0][0] / S;
-
K[1] = Kalman->P[1][0] / S;
-
-
double y = newAngle - Kalman->angle;
-
Kalman->angle = K[0] * y;
-
Kalman->bias = K[1] * y;
-
-
double P00_temp = Kalman->P[0][0];
-
double P01_temp = Kalman->P[0][1];
-
-
Kalman->P[0][0] -= K[0] * P00_temp;
-
Kalman->P[0][1] -= K[0] * P01_temp;
-
Kalman->P[1][0] -= K[1] * P00_temp;
-
Kalman->P[1][1] -= K[1] * P01_temp;
-
-
return Kalman->angle;
-
};
不要管这个代码有多长就是不要怕,这个可以直接使用,DataStruct->KalmanAngleX 就是结构指针变量,指针大家都学过吧,这也是一个意思,就是个变量其他的就是算法而已,其中,我也做了卡尔曼滤波的算法,将数据做到更精准,这个就是福利了。
-
/*
-
*mpu6050.h
-
*
-
*Creation date: September 11th, 2021
-
*Author: Xiaodong
-
*
-
*Kalman filter algorithm used from https://github.com/TKJElectronics/KalmanFilter
-
*/
-
-
-
-
-
-
-
-
-
-
// MPU6050 structure
-
typedef struct
-
{
-
-
int16_t Accel_X_RAW;
-
int16_t Accel_Y_RAW;
-
int16_t Accel_Z_RAW;
-
double Ax;
-
double Ay;
-
double Az;
-
-
int16_t Gyro_X_RAW;
-
int16_t Gyro_Y_RAW;
-
int16_t Gyro_Z_RAW;
-
double Gx;
-
double Gy;
-
double Gz;
-
-
float Temperature;
-
-
double KalmanAngleX;
-
double KalmanAngleY;
-
} MPU6050_t;
-
-
// Kalman structure
-
typedef struct
-
{
-
double Q_angle;
-
double Q_bias;
-
double R_measure;
-
double angle;
-
double bias;
-
double P[2][2];
-
} Kalman_t;
-
-
uint8_t MPU6050_Init(I2C_HandleTypeDef *I2Cx);
-
-
void MPU6050_Read_Accel(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct);
-
-
void MPU6050_Read_Gyro(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct);
-
-
void MPU6050_Read_Temp(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct);
-
-
void MPU6050_Read_All(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct);
-
-
double Kalman_getAngle(Kalman_t *Kalman, double newAngle, double newRate, double dt);
这估计大家都会看懵,并且使用上会有困难,毕竟我这都是指针和域的规划,mpu6050_t,写HAL库的大家应该不少见了吧,那么给他给个定义应该也是不难。
其中用freertos操作系统也是一样的使用
这篇好文章是转载于:学新通技术网
- 版权申明: 本站部分内容来自互联网,仅供学习及演示用,请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系,请提供相关证据及您的身份证明,我们将在收到邮件后48小时内删除。
- 本站站名: 学新通技术网
- 本文地址: /boutique/detail/tanhgabkhf
-
photoshop保存的图片太大微信发不了怎么办
PHP中文网 06-15 -
Android 11 保存文件到外部存储,并分享文件
Luke 10-12 -
word里面弄一个表格后上面的标题会跑到下面怎么办
PHP中文网 06-20 -
《学习通》视频自动暂停处理方法
HelloWorld317 07-05 -
photoshop扩展功能面板显示灰色怎么办
PHP中文网 06-14 -
微信公众号没有声音提示怎么办
PHP中文网 03-31 -
怎样阻止微信小程序自动打开
PHP中文网 06-13 -
excel下划线不显示怎么办
PHP中文网 06-23 -
excel打印预览压线压字怎么办
PHP中文网 06-22 -
photoshop蒙版画笔没反应怎么办
PHP中文网 06-24