Skip to content

M
MPU6050

DevelopLink / MPU6050
与 Fork 源项目一致

从无法访问的项目Fork

通知 1
Star 0
Fork 0
M
MPU6050

前往新版Gitcode，体验更适合开发者的 AI 搜索 >>

未验证 提交 4e380d1e 编写于 作者: H Heimerdingerzzz 提交者： GitHub
浏览文件
操作

Update inv_mpu.c

上级 eb0e58aa
隐藏空白更改
内联 并排
Showing with 42 addition and 51 deletion
inv_mpu.c
浏览文件 @ 4e380d1e
......@@ -28,8 +28,8 @@
#include "stm32f1xx_hal.h"
#define MPU6050 //定义我们使用的传感器为MPU6050
#define MOTION_DRIVER_TARGET_MSP430 //定义驱动部分,采用MSP430的驱动(移植到STM32F4)
#define MPU6050 //定义我们使用的传感器为MPU6050
#define MOTION_DRIVER_TARGET_MSP430 //定义驱动部分,采用MSP430的驱动(移植到STM32F4)
/* The following functions must be defined for this platform:
* i2c_write(unsigned char slave_addr, unsigned char reg_addr,
......@@ -58,8 +58,8 @@
// return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit,
// int_param->active_low);
//}
#define log_i printf //打印信息
#define log_e printf //打印信息
#define log_i printf //打印信息
#define log_e printf //打印信息
/* labs is already defined by TI's toolchain. */
/* fabs is for doubles. fabsf is for floats. */
#define fabs fabsf
......@@ -2877,28 +2877,19 @@ lp_int_restore:
return 0;
}
//////////////////////////////////////////////////////////////////////////////////
//添加的代码部分
//添加的代码部分
//////////////////////////////////////////////////////////////////////////////////
//ALIENTEK STM32F407开发板
//MPU6050 DMP 驱动代码
//正点原子@ALIENTEK
//技术论坛:www.openedv.com
//创建日期:2014/5/9
//版本：V1.0
//Copyright(C) 广州市星翼电子科技有限公司 2014-2024
//All rights reserved
//////////////////////////////////////////////////////////////////////////////////
//q30格式,long转float时的除数.
//q30格式,long转float时的除数.
#define q30 1073741824.0f
//陀螺仪方向设置
//陀螺仪方向设置
static signed char gyro_orientation[9] = { 1, 0, 0,
0, 1, 0,
0, 0, 1};
//MPU6050自测试
//返回值:0,正常
// 其他,失败
//MPU6050自测试
//返回值:0,正常
// 其他,失败
uint8_t run_self_test(void)
{
int result;
......@@ -2926,7 +2917,7 @@ uint8_t run_self_test(void)
return 0;
}else return 1;
}
//陀螺仪方向控制
//陀螺仪方向控制
unsigned short inv_orientation_matrix_to_scalar(
const signed char *mtx)
{
......@@ -2947,7 +2938,7 @@ unsigned short inv_orientation_matrix_to_scalar(
return scalar;
}
//方向转换
//方向转换
unsigned short inv_row_2_scale(const signed char *row)
{
unsigned short b;
......@@ -2968,58 +2959,58 @@ unsigned short inv_row_2_scale(const signed char *row)
b = 7; // error
return b;
}
//空函数,未用到.
//空函数,未用到.
void mget_ms(unsigned long *time)
{
}
//mpu6050,dmp初始化
//返回值:0,正常
// 其他,失败
//mpu6050,dmp初始化
//返回值:0,正常
// 其他,失败
uint8_t mpu_dmp_init(void)
{
extern I2C_HandleTypeDef hi2c1;
uint8_t res=0;
HAL_I2C_Init(&hi2c1); //初始化IIC总线
HAL_I2C_Init(&hi2c1); //初始化IIC总线
HAL_Delay(100);
if(mpu_init()==0) //初始化MPU6050
if(mpu_init()==0) //初始化MPU6050
{
res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器
res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器
if(res)
{
printf("set sensors failed \n");
return 1;
}
else printf("set sensors successful \n");
res=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);//设置FIFO
res=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);//设置FIFO
if(res)
{
printf("set FIFO failed \n");
return 2;
}
else printf("set FIFO successful \n");
res=mpu_set_sample_rate(DEFAULT_MPU_HZ); //设置采样率
res=mpu_set_sample_rate(DEFAULT_MPU_HZ); //设置采样率
if(res)
{
printf("set sample rate failed \n");
return 3;
}
else printf("set sample rate successful \n");
res=dmp_load_motion_driver_firmware(); //加载dmp固件
res=dmp_load_motion_driver_firmware(); //加载dmp固件
if(res)
{
printf("load DMP failed \n");
return 4;
}
else printf("load DMP successful \n");
res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
if(res)
{
printf("set orientation failed \n");
return 5;
}
else printf("set orientation successful \n");
res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| //设置dmp功能
res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| //设置dmp功能
DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
DMP_FEATURE_GYRO_CAL);
if(res)
......@@ -3028,19 +3019,19 @@ uint8_t mpu_dmp_init(void)
return 6;
}
else printf("set DMP successful \n");
res=dmp_set_fifo_rate(20); //设置DMP输出速率(最大不超过200Hz)
res=dmp_set_fifo_rate(20); //设置DMP输出速率(最大不超过200Hz)
if(res)
{
return 7;
}
// res=run_self_test(); //自检
// if(res)
// {
// printf("self test failed \n");
// return 8;
// }
// else printf("self test successful \n");
res=mpu_set_dmp_state(1); //使能DMP
res=run_self_test(); //自检
if(res)
{
printf("self test failed \n");
return 8;
}
else printf("self test successful \n");
res=mpu_set_dmp_state(1); //使能DMP
if(res)
{
printf("enable DMP failed \n");
......@@ -3051,12 +3042,12 @@ uint8_t mpu_dmp_init(void)
printf("initialize MPU6050 successful \n");
return 0;
}
//得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
//pitch:俯仰角 精度:0.1° 范围:-90.0° <---> +90.0°
//roll:横滚角 精度:0.1° 范围:-180.0°<---> +180.0°
//yaw:航向角 精度:0.1° 范围:-180.0°<---> +180.0°
//返回值:0,正常
// 其他,失败
//得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
//pitch:俯仰角 精度:0.1° 范围:-90.0° <---> +90.0°
//roll:横滚角 精度:0.1° 范围:-180.0°<---> +180.0°
//yaw:航向角 精度:0.1° 范围:-180.0°<---> +180.0°
//返回值:0,正常
// 其他,失败
uint8_t mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
{
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
......@@ -3077,11 +3068,11 @@ uint8_t mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
**/
if(sensors&INV_WXYZ_QUAT)
{
q0 = quat[0] / q30; //q30格式转换为浮点数
q0 = quat[0] / q30; //q30格式转换为浮点数
q1 = quat[1] / q30;
q2 = quat[2] / q30;
q3 = quat[3] / q30;
//计算得到俯仰角/横滚角/航向角
//计算得到俯仰角/横滚角/航向角
*pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
*roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
*yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
......
Markdown is supported
0% 或 .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑！
想要评论请 注册 或