|
|
用了6050，我得說明幾點，，一 ，關于姿態角解算，你可以卡爾曼，也可以直接算i四元數再結算，其中區別請看源程序，
二、dmp、dmp很簡單、但是、需要具有iic的 接口 、能懂嗎？ 需要有推挽輸出能力的iic接口、不是模擬iic、我被這個氣死了 ！！！！！！！！！！！！！！
三、我懶得去整理 、直接打包上傳的三個文件 有點亂、時間緊迫也沒有去美化、你們可以看到我的這三種用法、如果自己用時哪里有問題 、請注意分析源代碼 的計算原理、強調一點、iic 沒成功是因為 149沒有iic 、對于這個我也是日了狗、我的設置是全部可以參考的
0.png (51.62 KB, 下載次數: 175)
下載附件
2017-4-4 04:07 上傳
這個是別人的資料：
平衡車全套資料.zip
(15.25 MB, 下載次數: 223)
2017-4-4 04:05 上傳
點擊文件名下載附件
下載積分: 黑幣 -5
mpu6050DMP.zip
(1.24 MB, 下載次數: 177)
2017-4-4 04:04 上傳
點擊文件名下載附件
下載積分: 黑幣 -5
這個是430的MPU6050-DMP.zip
(1.33 MB, 下載次數: 154)
2017-4-4 04:05 上傳
點擊文件名下載附件
下載積分: 黑幣 -5
0.png (61.87 KB, 下載次數: 185)
下載附件
2017-4-4 04:08 上傳
MSP430單片機主程序：
- #include <msp430f149.h>
- #include <math.h>
- #include "Config.h"
- #include "1602.c"
- #include "mpu6050.c"
- #include "mpu60500.h"
- #include "mpu60500.c"
- #include "mpuiic.h"
- #include "mpuiic.c"
- #include "IOI2C.h"
- #include "IOI2C.c"
- #include "dmpKey.h"
- #include "dmpmap.h"
- #include "inv_mpu.h"
- #include "inv_mpu.c"
- #include "inv_mpu_dmp_motion_driver.h"
- #include "inv_mpu_dmp_motion_driver.c"
- #include "kaerman.c"
- struct quaternion{
- float w;
- float x;
- float y;
- float z;
- }quaternion;
- float gyr[3],acc[3];
- float Pitch=1.0,Roll,Yaw ;
- //q30格式,long轉float時的除數.
- #define q30 1073741824.0f
- void mix_gyrAcc_crossMethod(struct quaternion *attitude,const float gyr[3],const float acc[3],float interval);
- void quaternion_normalize(struct quaternion*q);
- void main()
- {
- WDT_Init(); //看門狗設置
- Clock_Init(); //系統時鐘設置
- Port_init(); //系統初始化，設置IO口屬性
- delay_ms(100); //延時100ms
- LCD_init(); //液晶參數初始化設置
- LCD_clear(); //清屏
- //InitMPU6050();
- uchar e=mpu_dmp_init();
- float yy=e*1.0;
- delay_ms(300);
- /*
- P1DIR|=0x01;
- P1OUT&=0XFE;
- TA0CCTL1=OUTMOD_7+CCIE;
- TACTL=TASSEL_2+MC_1+TAIE;
- TA0CCR0=655;
- TA0CCR1=654;
-
- */
- //float aaa=0.0,bbb=0.0,ccc=0.0;
- quaternion.w=1;
- quaternion.x=0;
- quaternion.y=0;
- quaternion.z=0;
-
-
- _EINT();
- while(1)
- {
-
- Disp(yy,0,0);
-
- /*
- gyr[0]=-GetData(GYRO_XOUT_H)/16.4;
- gyr[1]=-GetData(GYRO_YOUT_H)/16.4;
- gyr[2]=-GetData(GYRO_ZOUT_H)/16.4;
- acc[0]=GetData(ACCEL_XOUT_H)/16384;
- acc[1]=GetData(ACCEL_YOUT_H)/16384;
- acc[2]=GetData(ACCEL_ZOUT_H)/16384;
-
- Disp(Pitch,0,0);
- Disp(Roll,7,0);
- Disp(Yaw,0,1);
- Disp(quaternion.z,7,1);
- mpu_dmp_get_data(&aaa,&bbb,&ccc);
- Disp(aaa,0,0);
- Display10BitData(GetData(ACCEL_XOUT_H),0,0); //顯示X軸加速度
- Display10BitData(GetData(ACCEL_YOUT_H),5,0); //顯示Y軸加速度
- Display10BitData(GetData(ACCEL_ZOUT_H),10,0); //顯示Z軸加速度
- Display10BitData(GetData(GYRO_XOUT_H),0,1); //顯示X軸角速度
- Display10BitData(GetData(GYRO_YOUT_H),5,1); //顯示Y軸角速度
- Display10BitData(GetData(GYRO_ZOUT_H),10,1); //顯示Z軸角速度 */
- }
- }
- /*
- #pragma vector=TIMERA1_VECTOR
- __interrupt void P1(void)
- {
- P1OUT|=0X01;
- if(TAIV==2)
- {_NOP();}
-
-
- mix_gyrAcc_crossMethod(&quaternion,gyr,acc,0.01);
- }
- */
- void mix_gyrAcc_crossMethod(struct quaternion *attitude,const float gyr[3],const float acc[3],float interval)
- {
- const static float FACTOR = 0.001;//取接近0的數
- //
- float w_q = attitude->w;
- float x_q = attitude->x;
- float y_q = attitude->y;
- float z_q = attitude->z;
- float x_q_2 = x_q * 2;
- float y_q_2 = y_q * 2;
- float z_q_2 = z_q * 2;
- //
- // 加速度計的讀數，單位化。
- float a_rsqrt = sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
- float x_aa = acc[0] * a_rsqrt;
- float y_aa = acc[1] * a_rsqrt;
- float z_aa = acc[2] * a_rsqrt; //加速度計測量出的加速度向量(載體坐標系下)
- //
- // 載體坐標下的重力加速度向量，單位化。
- float x_ac = x_q*z_q_2 - w_q*y_q_2;
- float y_ac = y_q*z_q_2 + w_q*x_q_2; //通過四元數旋轉矩陣與地理坐標系下的重力加速度向量[0 0 0 1]叉乘得到載體坐標系下的重力加速度向量
- float z_ac = 1 - x_q*x_q_2 - y_q*y_q_2;//（主要）角速度計測出的四元數表示的載體坐標系下的重力加速度向量（這里已轉換成載體坐標系下）
- //
- // 測量值與常量的叉積。
- float x_ca = y_aa * z_ac - z_aa * y_ac;
- float y_ca = z_aa * x_ac - x_aa * z_ac;
- float z_ca = x_aa * y_ac - y_aa * x_ac;//角速度計測出的角度誤差，疊加的FACTOR大小可以實驗試湊
- //
- // 構造增量旋轉。
- float delta_x = gyr[0] * interval / 2 + x_ca * FACTOR;
- float delta_y = gyr[1] * interval / 2 + y_ca * FACTOR;
- float delta_z = gyr[2] * interval / 2 + z_ca * FACTOR;
- //
- // 融合，四元數乘法。
- attitude->w = w_q - x_q*delta_x - y_q*delta_y - z_q*delta_z;
- attitude->x = w_q*delta_x + x_q + y_q*delta_z - z_q*delta_y;
- attitude->y = w_q*delta_y - x_q*delta_z + y_q + z_q*delta_x;
- attitude->z = w_q*delta_z + x_q*delta_y - y_q*delta_x + z_q;
- quaternion_normalize(attitude);//歸一化
- float q0=0.0,q1=0.0,q2=0.0,q3=0.0;
- q0=attitude->w;
- q1=attitude->x;
- q2=attitude->y;
- q3=attitude->z;
- 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;
- }
- void quaternion_normalize(struct quaternion*q)
- {
- float qlength_inv = 1.0/(sqrt(q->w*q->w + q->x*q->x + q->y*q->y + q->z*q->z));
- //這里只應該開根號x*x+y*y+z*z
- // now normalize
- q->w=q->w*qlength_inv;
- q->x=q->x*qlength_inv;
- q->y=q->y*qlength_inv;
- q->z=q->z*qlength_inv;
-
- }
自己做的程序：
卡爾曼重要設置已保留.zip
(3.17 KB, 下載次數: 80)
2017-4-4 04:06 上傳
點擊文件名下載附件
下載積分: 黑幣 -5
四元數算.zip
(493.84 KB, 下載次數: 85)
2017-4-4 04:06 上傳
點擊文件名下載附件
下載積分: 黑幣 -5
dmp需要iic里面149錯誤的其他設置完全是對的.zip
(143.94 KB, 下載次數: 78)
2017-4-4 04:06 上傳
點擊文件名下載附件
下載積分: 黑幣 -5
|
|
[復制鏈接]
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
