GY-85九軸傳感器資料
模塊的原理圖：
ADXL345是一款小而薄的超低功耗3軸加速度計，分辨率高(13位)，測量范圍達± 16g。數字輸出數據為16位二進制補碼格式，可通過SPI(3線或4線)或I2C數字接口訪問。
ADXL345非常適合移動設備應用。它可以在傾斜檢測應用中測量靜態重力加速度，還可以測量運動或沖擊導致的動態加速度。其高分辨率(3.9mg/LSB)，能夠測量不到1.0°的傾斜角度變化。
該器件提供多種特殊檢測功能。活動和非活動檢測功能通過比較任意軸上的加速度與用戶設置的閾值來檢測有無運動發生。敲擊檢測功能可以檢測任意方向的單振和雙振動作。自由落體檢測功能可以檢測器件是否正在掉落。這些功能可以獨立映射到兩個中斷輸出引腳中的一個。正在申請專利的集成式存儲器管理系統采用一個32級先進先出(FIFO)緩沖器，可用于存儲數據，從而將主機處理器負荷降至最低，并降低整體系統功耗。
ADXL345引腳圖與中文資料PDF：
HMC5883L中文規格書.pdf：
ITG3205引腳圖：
ITG3205應用電路：
HMC5883L資料：
單片機源程序如下:
- #include "IMU.h"
- #include "ADXL345.h"
- #include "ITG3205.h"
- #include "arm_math.h"
- #include <math.h>
- struct IMU_Data MyIMU={0,0,0};
- float ADXL_X=0,ADXL_Y=0,ADXL_Z=0;
- float ITG_X=0,ITG_Y=0,ITG_Z=0;
- short GetX,GetY,GetZ;
- void TIM4_IRQHandler(void)
- {
- if(TIM_GetITStatus(TIM4,TIM_IT_Update)==SET)
- {
- TIM_ClearFlag(TIM4,TIM_FLAG_Update);
- IMU_Update();
- }
- }
- ////////////////////////////////////////////////////////////////////////////////
- #define Kp 10.0f // proportional gain governs rate of convergence to accelerometer/magnetometer
- #define Ki 0.008f // integral gain governs rate of convergence of gyroscope biases
- #define halfT 0.0025f // half the sample period采樣周期的一半
- float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation
- float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error
- void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
- {
- float norm;
- float vx, vy, vz;// wx, wy, wz;
- float ex, ey, ez;
- float q0q0 = q0*q0;
- float q0q1 = q0*q1;
- float q0q2 = q0*q2;
- // float q0q3 = q0*q3;
- float q1q1 = q1*q1;
- // float q1q2 = q1*q2;
- float q1q3 = q1*q3;
- float q2q2 = q2*q2;
- float q2q3 = q2*q3;
- float q3q3 = q3*q3;
- if(ax*ay*az==0)
- return;
- norm = __sqrtf(ax*ax + ay*ay + az*az);//acc數據歸一化
- ax = ax /norm;
- ay = ay / norm;
- az = az / norm;
- // estimated direction of gravity and flux (v and w) 估計重力方向和流量/變遷
- vx = 2*(q1q3 - q0q2);//四元素中xyz的表示
- vy = 2*(q0q1 + q2q3);
- vz = q0q0 - q1q1 - q2q2 + q3q3 ;
- // error is sum of cross product between reference direction of fields and direction measured by sensors
- 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;
- // adjusted gyroscope measurements
- gx = gx + Kp*ex + exInt;//將誤差PI后補償到陀螺儀，即補償零點漂移
- gy = gy + Kp*ey + eyInt;
- gz = gz + Kp*ez + ezInt;//這里的gz由于沒有觀測者進行矯正會產生漂移，表現出來的就是積分自增或自減
- // integrate quaternion rate and normalise//四元素的微分方程
- 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;
- // normalise quaternion
- norm = __sqrtf(q0*q0 + q1*q1 + q2*q2 + q3*q3);
- q0 = q0 / norm;
- q1 = q1 / norm;
- q2 = q2 / norm;
- q3 = q3 / norm;
- MyIMU.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw
- MyIMU.Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
- MyIMU.Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
-
- /*if(GYRO_I.Z>=360)GYRO_I.Z=0;
- Q_ANGLE.Z = GYRO_I.Z;*/
- }
- //#define NUMOFALL 8
- //short ADXL_X_Buf[NUMOFALL];
- //short ADXL_Y_Buf[NUMOFALL];
- //short ADXL_Z_Buf[NUMOFALL];
- //int ADXL_SumX=0,ADXL_SumY=0,ADXL_SumZ=0;
- //short ADXL_CurNum=0;
- void IMU_Update(void)
- {
- ITG3205_ReadXYZ(&GetX,&GetY,&GetZ);//-27 13 5
- GetX-=-27;GetY-=13;GetZ-=5;
- ITG_X = (float)GetX/14.375/57.325;//14.375是轉換為度每秒(來自與手冊資料) 57.325是轉換為弧度每秒(來源于180/π)
- ITG_Y = (float)GetY/14.375/57.325;
- ITG_Z = (float)GetZ/14.375/57.325;
-
- ADXL345_ReadXYZ(&GetX,&GetY,&GetZ);
-
- // ADXL_SumX-=ADXL_X_Buf[ADXL_CurNum];
- // ADXL_SumY-=ADXL_Y_Buf[ADXL_CurNum];
- // ADXL_SumZ-=ADXL_Z_Buf[ADXL_CurNum];
- //
- ……………………
- …………限于本文篇幅 余下代碼請從51黑下載附件…………
所有資料51hei提供下載:
GY-85發送資料.rar
(12.95 MB, 下載次數: 72)
2017-11-28 00:39 上傳
點擊文件名下載附件
|
