From 4e380d1e3dd68db4d9f4c8db4c0867901b8010af Mon Sep 17 00:00:00 2001 From: Heimerdingerzzz Date: Sun, 17 Mar 2019 20:57:20 +0800 Subject: [PATCH] Update inv_mpu.c --- inv_mpu.c | 93 +++++++++++++++++++++++++------------------------------ 1 file changed, 42 insertions(+), 51 deletions(-) diff --git a/inv_mpu.c b/inv_mpu.c index c7f4606..30ed017 100644 --- a/inv_mpu.c +++ b/inv_mpu.c @@ -28,8 +28,8 @@ #include "stm32f1xx_hal.h" -#define MPU6050 //定义我们使用的传感器为MPU6050 -#define MOTION_DRIVER_TARGET_MSP430 //定义驱动部分,采用MSP430的驱动(移植到STM32F4) +#define MPU6050 //瀹氫箟鎴戜滑浣跨敤鐨勪紶鎰熷櫒涓篗PU6050 +#define MOTION_DRIVER_TARGET_MSP430 //瀹氫箟椹卞姩閮ㄥ垎,閲囩敤MSP430鐨勯┍鍔(绉绘鍒癝TM32F4) /* 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杞琭loat鏃剁殑闄ゆ暟. #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); //鍒濆鍖朓IC鎬荤嚎 HAL_Delay(100); - if(mpu_init()==0) //初始化MPU6050 + if(mpu_init()==0) //鍒濆鍖朚PU6050 { - 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 -- GitLab