當前位置：首頁 > 编程资源 > 编程问答 >内容正文

编程问答

DMP解算

發布時間：2023/12/8 编程问答 48 豆豆

生活随笔收集整理的這篇文章主要介紹了 DMP解算小編覺得挺不錯的,現在分享給大家,幫大家做個參考.

1.硬件：mpu9250， stm32f103zet6。

2.inv_mpu.h文件，一種基于i2c的直覺陀螺儀的驅動器，所含函數有(移植官方MSP430的DMP驅動過來)：

1.static int set_int_enable(unsigned char enable) 模塊中斷使能函數
2.int mpu_reg_dump(void) 測試打印函數
3.int mpu_read_reg(unsigned char reg, unsigned char *data) 3.向芯片讀寄存器值，除了.MEMERY和FIFO
4.int mpu_init(void) MPU9250的初始化
5.int mpu_lp_accel_mode(unsigned char rate) 進入低功耗模式
6.int mpu_get_gyro_reg(short *data, unsigned long *timestamp) 獲取新的原始陀螺儀數據
7.int mpu_get_accel_reg(short *data, unsigned long *timestamp獲取新的原始加速度數據
8.int mpu_get_temperature(long *data, unsigned long *timestamp) 獲取新的溫度數據
9.int mpu_set_accel_bias(const long *accel_bias) 偏差配置函數
10.int mpu_reset_fifo(void) 重置FIFO函數
11.int mpu_get_gyro_fsr(unsigned short *fsr) 獲得陀螺儀全尺寸范圍函數
12.int mpu_set_gyro_fsr(unsigned short fsr) 設置陀螺儀全尺寸范圍函數
13.int mpu_get_accel_fsr(unsigned char *fsr) 獲得加速度全尺寸范圍函數
14.int mpu_set_accel_fsr(unsigned char fsr) 配置加速度全尺寸范圍函數
15.int mpu_get_lpf(unsigned short *lpf) .獲得DLPF范圍函數
16.int mpu_set_lpf(unsigned short lpf) 配置DLPF范圍函數
17.int mpu_get_sample_rate(unsigned short *rate) 獲得采樣頻率范圍函數
18.int mpu_set_sample_rate(unsigned short rate) 配置采樣頻率范圍函數
19.int mpu_get_compass_sample_rate(unsigned short *rate) 獲得羅盤采樣頻率范圍函數
20.int mpu_set_compass_sample_rate(unsigned short rate) 配置羅盤采樣頻率范圍函數
21.int?mpu_get_gyro_sens(float *sens) 獲得陀螺儀靈敏度比例因子函數
22.int mpu_get_accel_sens(unsigned short *sens) 獲得加速計靈敏度比例因子函數
23.int mpu_get_fifo_config(unsigned char *sensors) 獲得開啟的FIFO通道函數
24.int mpu_configure_fifo(unsigned char sensors) 配置開啟FIFO通道函數
25.int mpu_get_power_state(unsigned char *power_on) 獲得芯片工作狀態
26.int mpu_set_sensors(unsigned char sensors) 配置傳感器的時鐘和工作狀態函數
27.int mpu_get_int_status(short *status).獲得中斷狀態函數
28.int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp,unsigned char *sensors, unsigned char *more) 獲得FIFO數據函數
29.int mpu_read_fifo_stream(unsigned short length, unsigned char *data,unsigned char *more) 獲得FIFO數據長度函數
30.int mpu_set_bypass(unsigned char bypass_on) 設置旁路模式函數
31.int mpu_set_int_level(unsigned char active_low) 設置中斷優先級函數
32.int mpu_set_int_latched(unsigned char enable) 設置中斷鎖存函數-
設置自檢函數
33.static int get_st_biases(long *gyro, long *accel, unsigned char hw_test) 獲取所有的偏差值函數
34.int mpu_run_self_test(long *gyro, long *accel) 行自檢值函數
35.int mpu_write_mem(unsigned short mem_addr, unsigned short length,unsigned char *data) 向DMP寫記憶函數
36.int mpu_read_mem(unsigned short mem_addr, unsigned short length,unsigned char *data) 向DMP讀記憶函數
37.int mpu_load_firmware(unsigned short length, const unsigned char*firmware,unsigned short start_addr, unsigned short sample_rate) 加載并驗證DMP映像函數
38.int mpu_set_dmp_state(unsigned char enable) DMP狀態控制函數
39.int mpu_get_dmp_state(unsigned char *enabled) DMP狀態讀取函數

2.創建IIC協議。可實現包括 IIC 的初始化（IO 口）、IIC 開始、IIC 結束、ACK、IIC讀寫等功能即可，在其他函數里面，只需要調用相關的 IIC 函數就可以和外部 IIC 器件通信。部分代碼如下：

//IIC 初始化 void IIC_Init(void) {GPIO_InitTypeDef GPIO_Initure;__HAL_RCC_GPIOB_CLK_ENABLE(); //使能 GPIOB 時鐘//PH4,5 初始化設置GPIO_Initure.Pin=GPIO_PIN_10|GPIO_PIN_11;GPIO_Initure.Mode=GPIO_MODE_OUTPUT_PP; //推挽輸出GPIO_Initure.Pull=GPIO_PULLUP; //上拉GPIO_Initure.Speed=GPIO_SPEED_FREQ_HIGH;//高速HAL_GPIO_Init(GPIOB,&GPIO_Initure);IIC_SDA=1;IIC_SCL=1; } //產生 IIC 起始信號 void IIC_Start(void) { SDA_OUT(); //sda 線輸出 IIC_SDA=1; IIC_SCL=1; delay_us(4); IIC_SDA=0;//START:when CLK is high,DATA change form high to low delay_us(4); IIC_SCL=0;//鉗住 I2C 總線，準備發送或接收數據 }//產生 IIC 停止信號 void IIC_Stop(void) { SDA_OUT();//sda 線輸出 IIC_SCL=0; IIC_SDA=0;//STOP:when CLK is high DATA change form low to high delay_us(4); IIC_SCL=1; IIC_SDA=1;//發送 I2C 總線結束信號 delay_us(4);} //等待應答信號到來 //返回值：1，接收應答失敗 // 0，接收應答成功 u8 IIC_Wait_Ack(void) { u8 ucErrTime=0; SDA_IN(); //SDA 設置為輸入 IIC_SDA=1;delay_us(1);IIC_SCL=1;delay_us(1); while(READ_SDA) { ucErrTime++; if(ucErrTime>250) { IIC_Stop(); return 1; } } IIC_SCL=0;//時鐘輸出 0 return 0; } //產生 ACK 應答 void IIC_Ack(void) { IIC_SCL=0; SDA_OUT(); IIC_SDA=0; delay_us(2); IIC_SCL=1; delay_us(2); IIC_SCL=0; } //不產生 ACK 應答void IIC_NAck(void) { IIC_SCL=0; SDA_OUT(); IIC_SDA=1; delay_us(2); IIC_SCL=1; delay_us(2); IIC_SCL=0; }//IIC 發送一個字節 //返回從機有無應答 //1，有應答 //0，無應答void IIC_Send_Byte(u8 txd) { u8 t; SDA_OUT(); IIC_SCL=0;//拉低時鐘開始數據傳輸for(t=0;t<8;t++){ IIC_SDA=(txd&0x80)>>7;txd<<=1; delay_us(2); IIC_SCL=1; delay_us(2); IIC_SCL=0; delay_us(2);} } //讀 1 個字節，ack=1 時，發送 ACK，ack=0，發送 nACK u8 IIC_Read_Byte(unsigned char ack) { unsigned char i,receive=0; SDA_IN();//SDA 設置為輸入for(i=0;i<8;i++ ) {IIC_SCL=0; delay_us(2); IIC_SCL=1;receive<<=1;if(READ_SDA)receive++; delay_us(1); }if (!ack)IIC_NAck();//發送 nACKelseIIC_Ack(); //發送 ACK return receive; }

可以運用stm32cubemx建立iic，這里可以借鑒別人的，不多贅述。

4.四元函數解算

short gryox,gyroy,gyroz;
short accx,accy,accz;
void IMU_GetData(void)
{
?? ?MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);
?? ?MPU_Get_Accelerometer(&accx,&accy,&accz);

}

//定義各變量

#define RtA ?? ??? ?57.295779f?? ?//弧度->角度?? ??? ??? ?
#define AtR ? ??? ??? ?0.0174533f?? ?//角度->弧度
#define Acc_G ?? ??? ?0.0011963f?? ?
#define Gyro_G ?? ??? ?0.0610351f?? ??? ??? ??? ?
#define Gyro_Gr?? ??? ?0.0010653f?? ?

#define Kp 18.0f ? ? ? ? ? ? ? ? ? ? ? ?
#define Ki 0.008f ? ? ? ? ? ? ? ? ? ? ? ??
#define halfT 0.008f ? ??
? ? ? ??
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; ??
float exInt = 0, eyInt = 0, ezInt = 0; ?
float Angle[3]={0};//最終角度

void IMU_Update(short gyrox,short gyroy,short gyroz,short accx,short accy,short accz)
{
?? ?float ax=accx,ay=accy,az=accz;
?? ?float gx=gyrox,gy=gyroy,gz=gyroz;
? ?? ?float norm;
? ?? ?float vx, vy, vz;
? ?? ?float ex, ey, ez;
? ?? ?float q0q0 = q0*q0;
? ?? ?float q0q1 = q0*q1;
? ?? ?float q0q2 = q0*q2;
? ?? ?float q1q1 = q1*q1;
? ?? ?float q1q3 = q1*q3;
? ?? ?float q2q2 = q2*q2;
? ?? ?float q2q3 = q2*q3;
? ?? ?float q3q3 = q3*q3;
?? ?if(ax*ay*az==0)//此時任意方向角加速度為0
??? ??? ?return;?
??? ?gx *= Gyro_Gr;
?? ?gy *= Gyro_Gr;
?? ?gz *= Gyro_Gr;
?? ?
? //對速度值歸一化 ? ?

norm = sqrt(ax*ax + ay*ay + az*az); ? ? ??
? ?? ?ax = ax /norm;
? ?? ?ay = ay / norm;
? ?? ?az = az / norm;

??? ? // 提取姿態矩陣的重力分量 ? ? ? ? ?
? ?? ?vx = 2*(q1q3 - q0q2);?? ??? ??? ??? ??? ??? ??? ??? ??? ??? ??? ??? ?
? ?? ?vy = 2*(q0q1 + q2q3);
? ?? ?vz = q0q0 - q1q1 - q2q2 + q3q3 ;

? ?? ?// 得到誤差向量，并進行積分而消除
? ?? ?ex = (ay*vz - az*vy) ; ? ? ? ? ? ? ? ? ? ? ? ? ? ?? ??? ??? ??? ??? ?
? ?? ?ey = (az*vx - ax*vz) ;
? ?? ?ez = (ax*vy - ay*vx) ;

? ?? ?exInt = exInt + ex * Ki;?? ??? ??? ??? ??? ??? ??? ??? ? ?
? ?? ?eyInt = eyInt + ey * Ki;
? ?? ?ezInt = ezInt + ez * Ki;

? // 互補濾波，將誤差輸入Pid控制器與本次姿態更新中陀螺儀測得的角速度相加，得到一個修正的角速度值，獲得的修正的角速度值去更新四元素，從而獲得準確的姿態角信息。
? ?? ?gx = gx + Kp*ex + exInt;?? ??? ??? ??? ??? ? ? ?? ??? ??? ??? ??? ??? ??? ?
? ?? ?gy = gy + Kp*ey + eyInt;
? ?? ?gz = gz + Kp*ez + ezInt;?? ??? ??? ??? ? ? ?? ??? ??? ??? ??? ??? ??? ?

? ?? ?// 解四元數? ? ? ? ? ? ? ? ? ? ? ? ??
? ?? ?q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
? ?? ?q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
? ?? ?q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
? ?? ?q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;

? ?? ?// 四元數歸一化
? ?? ?norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
? ?? ?
? ?? ?q0 = q0 / norm;
? ?? ?q1 = q1 / norm;
? ?? ?q2 = q2 / norm;
? ?? ?q3 = q3 / norm;
? ?? ?
? //求解姿態角（歐拉角）
ANgle[0]?? ?= asin(-2 * q1 * q3 + 2 * q0* q2)* RtA ; //pitch
ANgle[1]?? ?= atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* RtA; ??? ?//roll ? ? ? ? ? ? ? ? ? ??ANgle[2]??=atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* RtA; ?? ??? ??? ?// yaw
?? ?}

4.“inv_mpu.c”和“inv_mpu_dmp_motion_driver.c”的文件的調用

????????現移植mpu9250的官方DMP庫中eMpl。

關于inv_mpu.c：

1.因為所要移植的DMP庫原先是基于STM32F4 的，可將關于msp430的文件注釋掉。

2.加上#include mpu9250.h，#include usart.h（用于串口輸出）。

3.定義所用傳感器以及驅動（可用MSP430驅動）

4.添加DMP_讀取角度等

short gyro[3], accel[3], sensors;unsigned char more;long quat[4];float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;float pitch,roll,yaw;u8 DMP_DATA_UPDATA(void){?? ??unsigned long sensor_timestamp;gyro_data_ready_cb(); ? ? ? ? ? ? ? ? ? //數據采集結束標志位if (hal.new_gyro && hal.dmp_on) ? ? ? ? //用于計算四元數函數{if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more)){return 1;}?? ?if (!more)hal.new_gyro = 0;if(sensors & INV_WXYZ_QUAT){q0=quat[0]/q30;q1=quat[1]/q30;q2=quat[2]/q30;q3=quat[3]/q30;pitch=asin((-2)*q1*q3+2*q0*q2)*57.3;roll=atan2(2*q2*q3+2*q0*q1,(-2)*q1*q1-2*q2*q2+1)*57.3;yaw=atan2(2*(q1*q2+q0*q3),q0*q0+q1*q1-q2*q2-q3*q3)*57.3;}}

關于inv_mpu_dmp_motion_driver.c：

1.添加dmpKey.h和dmpmap.h文件以及串口通信文件。

2.將相關msp430的文件注釋掉。

3.定義目標版采用MSP430，對f103無影響。

5.添加MPU9250的初始化程序

可根據正點原子MPU6050驅動程序進行修改：

5.1.初始化MPU9250（時鐘、端口、引腳、iic總線、傳感器）

5.2.設置各傳感器量程

5.3.獲取數字濾波器、溫度以及陀螺儀原始值等功能

5.4.實現IIC連續寫讀，添加函數MPU_Write/Read_Len, MPU_IIC_Start/Stop等，即跟上面所訴一樣進行調用iic即可

6.main.c主函數進行補充

主要完成系統初始化以及主循環功能（可對正點原子MPU6050工程main.c文件的基礎上進行修改）：

while(1) {error = mpu_dmp_init(); //初始化mpu_dmp庫printf("ERROR:%d ",error); //串口顯示初始化錯誤值switch (error) //對不同錯誤值類型判斷{case 0:printf("DMP庫初始化正常 ");break;case 1:printf("設置傳感器失敗 ");break;case 2:printf("設置FIFO失敗 ");break;case 3:printf("設置采樣率失敗 ");break;case 4:printf("加載dmp固件失敗 ");break;case 5:printf("設置陀螺儀方向失敗 ");break;case 6:printf("設置dmp功能失敗 ");break;case 7:printf("設置DMP輸出速率失敗 ");break;case 8:printf("自檢失敗 ");break;case 9:printf("使能DMP失敗 ");break;case 10:printf("初始化MPU6050失敗 ");break;default :printf("未知錯誤 ");break;}if(error == 0)break; //如果沒有錯誤直接退出循環void MPU_Read(void) {if(mpu_dmp_get_data(&yaw,&pitch,&roll)==0) //dmp處理得到數據，對返回值進行判斷{ printf("Pitch:%f Roll:%f Yaw:%f ",pitch,roll,yaw);//串口輸出三軸角度mpu9250.speed++; //顯示速度自加if(mpu9250.speed == 1) //顯示速度閾值設置{mpu9250.flag = 1; //采集成功標志位設置為有效mpu9250.speed = 0; //顯示速度歸零} }else //采集不成功 {mpu9250.flag = 0; //采集成功標志位設置為無效} } /*** @brief MPU6050數據上報* @param 無* @retval 無*/ void DATA_Report(void) {if(mpu9250.flag == 1) //采集成功時{ temp=pitch*100; //賦temp為pitchif(temp<0) //對數據正負判斷，判斷為負時{temp=-temp; //對負數據取反 sprintf((char *)tmp_buf," Pitch:-%.3d.%.2d",temp/100,temp%100);//字符串格式化命令}else //判斷為正時 {sprintf((char *)tmp_buf," Pitch: %.3d.%.2d",temp/100,temp%100);//字符串格式化命令}temp=roll*100; //賦temp為pitchif(temp<0) //對數據正負判斷，判斷為負時{temp=-temp; //對負數據取反 sprintf((char *)tmp_buf," Roll :-%.3d.%.2d",temp/100,temp%100);//字符串格式化命令}else //判斷為正時 {sprintf((char *)tmp_buf," Roll : %.3d.%.2d",temp/100,temp%100);//字符串格式化命令}temp=yaw*100; //賦temp為pitchif(temp<0) //對數據正負判斷，判斷為負時{temp=-temp; //對負數據取反 sprintf((char *)tmp_buf," Yaw :-%.3d.%.2d",temp/100,temp%100);//字符串格式化命令}else //判斷為正時 {sprintf((char *)tmp_buf," Yaw : %.3d.%.2d",temp/100,temp%100);//字符串格式化命令}mpu9250.flag = 0; //采集成功標志位設置為無效}else ; //防卡死 }

DMP相關文件工程在下面鏈接：

鏈接：https://pan.baidu.com/s/1BJWJsD-4n0pl7FQ_z2Du0Q?pwd=zhr6 提取碼：zhr6

總結

以上是生活随笔為你收集整理的DMP解算的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。

dmp
解算

上一篇：基于遗传算法的卷积神经网络架构搜索
下一篇：卡尔曼滤波（Kalman filter)