1.六軸與九軸的區別

六軸包括：三軸加速度計、三軸陀螺儀

九軸包括：三軸加速度計、三軸陀螺儀、三軸磁強計

2.MPU6050簡單介紹

MPU6050 是 InvenSense 公司推出的全球首款整合性 6 軸運動處理元件，相較於多元件方案，免除了組合陀螺儀與加速器時之軸間差的問題，減少了安裝空間。MPU6050 內部整合了 3 軸陀螺儀和 3 軸加速度感測器，並且含有一個第二 IIC 介面，可用於連線外部磁力感測器即AUX_CL 和 AUX_DA，並利用自帶的數字運動處理器（DMP: DigitalMotion Processor）硬體加速引擎，通過主 IIC 介面，嚮應用端輸出完整的 9 軸融合演算資料。有了 DMP，我們可以使用 InvenSense 公司提供的運動處理資料庫，非常方便的實現姿態解算，降低了運動處理運算對作業系統的負荷，同時大大降低了開發難度。

3.模組原理圖

其中，SCL 和 SDA 是連線 MCU 的 IIC 介面，MCU 通過這個 IIC 介面來控制 MPU6050，
另外還有一個 IIC 介面：AUX_CL 和 AUX_DA，這個介面可用來連線外部從裝置，比如磁
感測器，這樣就可以組成一個九軸感測器。VLOGIC 是 IO 口電壓，該引腳最低可以到 1.8V，
我們一般直接接 VDD 即可。AD0 是從 IIC 介面（接 MCU）的地址控制引腳，該引腳控制
IIC 地址的最低位。如果接 GND，則 MPU6050 的 IIC 地址是：0X68，如果接 VDD，則是
0X69，注意：這裡的地址是不包含資料傳輸的最低位的（最低位用來表示讀寫）！！

4.初始化步驟及暫存器詳解

1 ）初始化 IIC  介面
MPU6050 採用 IIC 與 STM32F1 通訊，所以我們需要先初始化與 MPU6050 連線的 SDA
和 SCL 資料線。
2 ）復位 MPU6050
這一步讓 MPU6050 內部所有暫存器恢復預設值，通過對電源管理暫存器 1（0X6B）的
bit7 寫 1 實現。 復位後，電源管理暫存器 1 恢復預設值(0X40)，然後必須設定該暫存器為
0X00，以喚醒 MPU6050，進入正常工作狀態。
3 ）設定角速度感測器（陀螺儀）和加速度感測器的滿量程範圍
這一步，我們設定兩個感測器的滿量程範圍(FSR)，分別通過陀螺儀配置暫存器（0X1B）
和加速度感測器配置暫存器（0X1C）設定。我們一般設定陀螺儀的滿量程範圍為±2000dps，
加速度感測器的滿量程範圍為±2g。
4 ）設定其他引數
這裡，我們還需要配置的引數有：關閉中斷、關閉 AUX IIC 介面、禁止 FIFO、設定陀
螺儀取樣率和設定數字低通濾波器（DLPF）等。我們不用中斷方式讀取資料，所以關
閉中斷，然後也沒用到 AUX IIC 介面外接其他感測器，所以也關閉這個介面。分別通過中
斷使能暫存器（0X38）和使用者控制暫存器（0X6A）控制。MPU6050 可以使用 FIFO 儲存傳
感器資料，不過我們沒有用到，所以關閉所有 FIFO 通道，這個通過 FIFO 使能暫存器
（0X23）控制，預設都是 0（即禁止 FIFO），所以用預設值就可以了。陀螺儀取樣率通過採
樣率分頻暫存器（0X19）控制，這個取樣率我們一般設定為 50 即可。數字低通濾波器(DLPF)
則通過配置暫存器（0X1A）設定，一般設定 DLPF 為頻寬的 1/2 即可。
5 ）配置系統時鐘源並使能角速度感測器和加速度感測器
系統時鐘源同樣是通過電源管理暫存器 1（0X1B）來設定，該暫存器的最低三位用於
設定系統時鐘源選擇，預設值是 0（內部 8M RC 震盪），不過我們一般設定為 1，選擇 x 軸
陀螺 PLL 作為時鐘源，以獲得更高精度的時鐘。同時，使能角速度感測器和加速度感測器，
這兩個操作通過電源管理暫存器 2（0X6C）來設定，設定對應位為 0 即可開啟。
至此，MPU6050 的初始化就完成了，可以正常工作了（其他未設定的暫存器全部採用
預設值即可），接下來，我們就可以讀取相關暫存器，得到加速度感測器、角速度感測器和
溫度感測器的資料了。不過，我們先簡單介紹幾個重要的暫存器。
首先，我們介紹電源管理暫存器 1，該暫存器地址為 0X6B，各位描述如圖

其中，DEVICE_RESET 位用來控制復位，設定為 1，復位 MPU6050，復位結束後，MPU
硬體自動清零該位。SLEEEP 位用於控制 MPU6050 的工作模式，復位後，該位為 1，即進
入了睡眠模式（低功耗），所以我們要清零該位，以進入正常工作模式。TEMP_DIS 用於設
置是否使能溫度感測器，設定為 0，則使能。最後 CLKSEL[2:0]用於選擇系統時鐘源，選擇
關係如表

預設是使用內部 8M RC 晶振的，精度不高，所以我們一般選擇 X/Y/Z 軸陀螺作為參考
的 PLL 作為時鐘源，一般設定 CLKSEL=001 即可。
接著，我們看陀螺儀配置暫存器，該暫存器地址為：0X1B，各位描述如圖

該暫存器我們只關心 FS_SEL[1:0]這兩個位，用於設定陀螺儀的滿量程範圍：0，±250°
/S；1，±500°/S；2，±1000°/S；3，±2000°/S；我們一般設定為 3，即±2000°/S，因
為陀螺儀的 ADC 為 16 位解析度，所以得到靈敏度為：65536/4000=16.4LSB/(°/S)。
接下來，我們看加速度感測器配置暫存器，暫存器地址為：0X1C，各位描述如圖

該暫存器我們只關心 AFS_SEL[1:0]這兩個位，用於設定加速度感測器的滿量程範圍：0，
±2g；1，±4g；2，±8g；3，±16g；我們一般設定為 0，即±2g，因為加速度感測器的
ADC 也是 16 位，所以得到靈敏度為：65536/4=16384LSB/g。
接下來，我看看 FIFO 使能暫存器，暫存器地址為：0X1C，各位描述如圖

該暫存器用於控制 FIFO 使能，在簡單讀取感測器資料的時候，可以不用 FIFO，設定
對應位為 0 即可禁止 FIFO，設定為 1，則使能 FIFO。注意加速度感測器的 3 個軸，全由 1
個位（ACCEL_FIFO_EN）控制，只要該位置 1，則加速度感測器的三個通道都開啟 FIFO
了。
接下來，我們看陀螺儀取樣率分頻暫存器，暫存器地址為：0X19，各位描述如圖

該暫存器用於設定 MPU6050 的陀螺儀取樣頻率，計算公式為：
取樣頻率 =  陀螺儀輸出頻率 / (1+SMPLRT_DIV)
這裡陀螺儀的輸出頻率，是 1Khz 或者 8Khz，與數字低通濾波器（DLPF）的設定有關，
當 DLPF_CFG=0/7 的時候，頻率為 8Khz，其他情況是 1Khz。而且 DLPF 濾波頻率一般設定
為取樣率的一半。取樣率，我們假定設定為 50Hz，那麼 SMPLRT_DIV=1000/50-1=19。
接下來，我們看配置暫存器，暫存器地址為：0X1A，各位描述如圖

這裡，我們主要關心數字低通濾波器（DLPF）的設定位，即：DLPF_CFG[2:0]，加速
度計和陀螺儀，都是根據這三個位的配置進行過濾的。DLPF_CFG 不同配置對應的過濾情
況如表

這裡的加速度感測器，輸出速率（Fs）固定是 1Khz，而角速度感測器的輸出速率（Fs），
則根據 DLPF_CFG 的配置有所不同。一般我們設定角速度感測器的頻寬為其取樣率的一半，
如前面所說的，如果設定取樣率為 50Hz，那麼頻寬就應該設定為 25Hz，取近似值 20Hz，
就應該設定 DLPF_CFG=100。
接下來，我們看電源管理暫存器 2，暫存器地址為：0X6C，各位描述如圖 

該暫存器的 LP_WAKE_CTRL 用於控制低功耗時的喚醒頻率，本章用不到。剩下的 6
位，分別控制加速度和陀螺儀的 x/y/z 軸是否進入待機模式，這裡我們全部都不進入待機模
式，所以全部設定為 0 即可。
接下來，我們看看陀螺儀資料輸出暫存器，總共有 8 個暫存器組成，地址為：0X43~0X48，
通過讀取這 8 個暫存器，就可以讀到陀螺儀 x/y/z 軸的值，比如 x 軸的資料，可以通過讀取
0X43（高 8 位）和 0X44（低 8 位）暫存器得到，其他軸以此類推。
同樣，加速度感測器資料輸出暫存器，也有 8 個，地址為：0X3B~0X40，通過讀取這 8
個暫存器，就可以讀到加速度感測器 x/y/z 軸的值，比如讀 x 軸的資料，可以通過讀取 0X3B
（高 8 位）和 0X3C（低 8 位）暫存器得到，其他軸以此類推。
最後，溫度感測器的值，可以通過讀取 0X41（高 8 位）和 0X42（低 8 位）暫存器得到，
溫度換算公式為：
Temperature = 36.53 + regval/340
其中，Temperature 為計算得到的溫度值，單位為℃，regval 為從 0X41 和 0X42 讀到的
溫度感測器值。

5.DMP使用

我們可以讀出 MPU6050 的加速度感測器和角速度感測器的原始數
據。不過這些原始資料，對想搞四軸之類的初學者來說，用處不大，我們期望得到的是姿態
資料，也就是尤拉角：航向角（yaw）、橫滾角（roll）和俯仰角（pitch）。有了這三個角，我
們就可以得到當前四軸的姿態，這才是我們想要的結果。
要得到尤拉角資料，就得利用我們的原始資料，進行姿態融合解算，這個比較複雜，知
識點比較多，初學者 不易掌握。而 MPU6050 自帶了數字運動處理器，即 DMP，並且，
InvenSense 提供了一個 MPU6050 的嵌入式運動驅動庫，結合 MPU6050 的 DMP，可以將我
們的原始資料，直接轉換成四元數輸出，而得到四元數之後，就可以很方便的計算出尤拉角，
從而得到 yaw、roll 和 pitch。
使用內建的 DMP，大大簡化了四軸的程式碼設計，且 MCU 不用進行姿態解算過程，大
大降低了 MCU 的負擔，從而有更多的時間去處理其他事件，提高系統實時性。
使用 MPU6050 的 DMP 輸出的四元數是 q30 格式的，也就是浮點數放大了 2 的 30 次方
倍。在換算成尤拉角之前，必須先將其轉換為浮點數，也就是除以 2 的 30 次方，然後再進
行計算，計算公式為：
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; //俯仰角
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; //航向角
其中 quat[0]~ quat[3]是 MPU6050 的 DMP 解算後的四元數，q30 格式，所以要除以一個
2 的 30 次方，其中 q30 是一個常量：1073741824，即 2 的 30 次方，然後帶入公式，計算出
尤拉角。上述計算公式的 57.3 是弧度轉換為角度，即 180/π，這樣得到的結果就是以度（°）
為單位的。

nvenSense 提供的 MPU6050 運動驅動庫是基於 MSP430 ，官方DMP驅動庫移植起來，還是比較簡單的，主要是實現這4個函式：i2c_write，i2c_read，delay_ms 和 get_ms，主要移植六個檔案：

該驅動庫，重點就是兩個 c 檔案：inv_mpu.c 和 inv_mpu_dmp_motion_driver.c。其中我
們在 inv_mpu.c 添加了幾個函式，方便我們使用，重點是兩個函式：mpu_dmp_init 和
mpu_dmp_get_data 這兩個函式，這裡我們簡單介紹下這兩個函式。
mpu_dmp_init，是 MPU6050 DMP 初始化函式，該函式程式碼如下：
//mpu6050,dmp 初始化
//返回值:0,正常
// 其他,失敗
u8 mpu_dmp_init(void)
{
u8 res=0;
IIC_Init(); //初始化 IIC 匯流排

if(mpu_init()==0) //初始化 MPU6050
{ 
res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//設需要的感測器
if(res)return 1;
res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);//設定 FIFO
if(res)return 2;
res=mpu_set_sample_rate(DEFAULT_MPU_HZ);  //設定取樣率
if(res)return 3;
res=dmp_load_motion_driver_firmware(); //載入 dmp 韌體
if(res)return 4;
res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));
//設定陀螺儀方向
if(res)return 5;
res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP
|DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL
|DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL);
//設定 dmp 功能
if(res)return 6;
res=dmp_set_fifo_rate(DEFAULT_MPU_HZ);//設定 DMP 輸出速率(最大 200Hz)
if(res)return 7;
res=run_self_test(); //自檢
if(res)return 8;
res=mpu_set_dmp_state(1); //使能 DMP
if(res)return 9;
}
return 0;
}
此函式首先通過 IIC_Init(需外部提供)初始化與 MPU6050 連線的 IIC 介面，然後呼叫
mpu_init 函式，初始化 MPU6050，之後就是設定 DMP 所用感測器、FIFO、取樣率和載入
韌體等一系列操作，在所有操作都正常之後，最後通過 mpu_set_dmp_state(1)使能 DMP 功
能，在使能成功以後，我們便可以通過 mpu_dmp_get_data 來讀取姿態解算後的資料了。
mpu_dmp_get_data 函式程式碼如下：
//得到 dmp 處理後的資料(注意,本函式需要比較多堆疊,區域性變數有點多)
//pitch:俯仰角 精度:0.1° 範圍:-90.0° <---> +90.0°
//roll:橫滾角 精度:0.1° 範圍:-180.0°<---> +180.0°
//yaw:航向角 精度:0.1° 範圍:-180.0°<---> +180.0°
//返回值:0,正常
// 其他,失敗
u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
{
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
unsigned long sensor_timestamp;
short gyro[3], accel[3], sensors;
unsigned char more;

long quat[4];
if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;
if(sensors&INV_WXYZ_QUAT)
{
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
}else return 2;
return 0;
}
此函式用於得到 DMP 姿態解算後的俯仰角、橫滾角和航向角。這裡就用到了我們前面
介紹的四元數轉尤拉角公式，將 dmp_read_fifo 函式讀到的 q30 格式四元數轉換成尤拉角。

6.實現部分原始碼

//初始化 MPU6050
//返回值:0,成功
// 其他,錯誤程式碼
u8 MPU_Init(void)
{
u8 res;
MPU_IIC_Init();  //初始化 IIC 匯流排
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80);//復位 MPU6050
delay_ms(100);
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00);//喚醒 MPU6050
MPU_Set_Gyro_Fsr(3); //陀螺儀感測器,±2000dps
MPU_Set_Accel_Fsr(0);  //加速度感測器,±2g
MPU_Set_Rate(50);  //設定取樣率 50Hz
MPU_Write_Byte(MPU_INT_EN_REG,0X00); //關閉所有中斷
MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //I2C 主模式關閉
MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); //關閉 FIFO
MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); //INT 引腳低電平有效
res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
if(res==MPU_ADDR)//器件 ID 正確
{
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01); 
//設定 CLKSEL,PLL X 軸為參考
MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00);//加速度與陀螺儀都工作
MPU_Set_Rate(50);  //設定取樣率為 50Hz
}else return 1;
return 0;
}

//得到溫度值
//返回值:溫度值(擴大了 100 倍)
short MPU_Get_Temperature(void)
{
u8 buf[2];
short raw; float temp; 
MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf);
raw=((u16)buf[0]<<8)|buf[1];
temp=36.53+((double)raw)/340;
return temp*100;

}
//得到陀螺儀值(原始值)
//gx,gy,gz:陀螺儀 x,y,z 軸的原始讀數(帶符號)
//返回值:0,成功
// 其他,錯誤程式碼
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
u8 buf[6],res;
res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
if(res==0)
{
*gx=((u16)buf[0]<<8)|buf[1];
*gy=((u16)buf[2]<<8)|buf[3];
*gz=((u16)buf[4]<<8)|buf[5];
}
return res;;
}
//得到加速度值(原始值)
//gx,gy,gz:陀螺儀 x,y,z 軸的原始讀數(帶符號)
//返回值:0,成功
// 其他,錯誤程式碼
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
u8 buf[6],res;
res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
if(res==0)
{
*ax=((u16)buf[0]<<8)|buf[1];
*ay=((u16)buf[2]<<8)|buf[3];
*az=((u16)buf[4]<<8)|buf[5];
}
return res;;
}

/IIC 連續寫
//addr:器件地址
//reg:暫存器地址
//len:寫入長度
//buf:資料區
//返回值:0,正常
// 其他,錯誤程式碼

u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
u8 i;
IIC_Start();
IIC_Send_Byte((addr<<1)|0);//傳送器件地址+寫命令 
if(IIC_Wait_Ack()){IIC_Stop();return 1;}//等待應答
IIC_Send_Byte(reg);  //寫暫存器地址
IIC_Wait_Ack();  //等待應答
for(i=0;i<len;i++)
{
IIC_Send_Byte(buf[i]); //傳送資料
if(IIC_Wait_Ack()) {IIC_Stop();return 1;}//等待 ACK
}
IIC_Stop(); 
return 0;
}
//IIC 連續讀
//addr:器件地址
//reg:要讀取的暫存器地址
//len:要讀取的長度
//buf:讀取到的資料儲存區
//返回值:0,正常
// 其他,錯誤程式碼
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
IIC_Start();
IIC_Send_Byte((addr<<1)|0); //傳送器件地址+寫命令 
if(IIC_Wait_Ack()){ IIC_Stop();return 1; } //等待應答
IIC_Send_Byte(reg);  //寫暫存器地址
IIC_Wait_Ack();  //等待應答
IIC_Start();
IIC_Send_Byte((addr<<1)|1);//傳送器件地址+讀命令 
IIC_Wait_Ack();  //等待應答
while(len)
{
if(len==1)*buf=IIC_Read_Byte(0);//讀資料,傳送 nACK
else *buf=IIC_Read_Byte(1); //讀資料,傳送 ACK
len--; buf++;
}
IIC_Stop();  //產生一個停止條件
return 0;
}

//串列埠 1 傳送 1 個字元
//c:要傳送的字元
void usart1_send_char(u8 c)
{
while((USART1->SR&0X40)==0);//等待上一次傳送完畢
USART1->DR=c;
}
//傳送資料給匿名四軸上位機軟體(V2.6 版本)
//fun:功能字. 0XA0~0XAF
//data:資料快取區,最多 28 位元組!!
//len:data 區有效資料個數
void usart1_niming_report(u8 fun,u8*data,u8 len)
{
u8 send_buf[32]; u8 i;
if(len>28)return;  //最多 28 位元組資料
send_buf[len+3]=0; //校驗數置零
send_buf[0]=0X88; //幀頭
send_buf[1]=fun; //功能字
send_buf[2]=len;  //資料長度
for(i=0;i<len;i++)send_buf[3+i]=data[i];  //複製資料
for(i=0;i<len+3;i++)send_buf[len+3]+=send_buf[i];  //計算校驗和
for(i=0;i<len+4;i++)usart1_send_char(send_buf[i]);  //傳送資料到串列埠 1
}
//傳送加速度感測器資料和陀螺儀資料
//aacx,aacy,aacz:x,y,z 三個方向上面的加速度值
//gyrox,gyroy,gyroz:x,y,z 三個方向上面的陀螺儀值
void mpu6050_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short
gyroz)
{
u8 tbuf[12];
tbuf[0]=(aacx>>8)&0XFF;
tbuf[1]=aacx&0XFF;
tbuf[2]=(aacy>>8)&0XFF;
tbuf[3]=aacy&0XFF;
tbuf[4]=(aacz>>8)&0XFF;
tbuf[5]=aacz&0XFF;
tbuf[6]=(gyrox>>8)&0XFF;
tbuf[7]=gyrox&0XFF;
tbuf[8]=(gyroy>>8)&0XFF;

tbuf[9]=gyroy&0XFF;
tbuf[10]=(gyroz>>8)&0XFF;
tbuf[11]=gyroz&0XFF;
usart1_niming_report(0XA1,tbuf,12);//自定義幀,0XA1
} 
//通過串列埠 1 上報結算後的姿態資料給電腦
//aacx,aacy,aacz:x,y,z 三個方向上面的加速度值
//gyrox,gyroy,gyroz:x,y,z 三個方向上面的陀螺儀值
//roll:橫滾角.單位 0.01 度。 -18000 -> 18000 對應 -180.00 -> 180.00 度
//pitch:俯仰角.單位 0.01 度。-9000 - 9000 對應 -90.00 -> 90.00 度
//yaw:航向角.單位為 0.1 度 0 -> 3600 對應 0 -> 360.0 度
void usart1_report_imu(short aacx,short aacy,short aacz,short gyrox,short gyroy,short
gyroz,short roll,short pitch,short yaw)
{
u8 tbuf[28]; u8 i;
for(i=0;i<28;i++)tbuf[i]=0;//清 0
tbuf[0]=(aacx>>8)&0XFF;
tbuf[1]=aacx&0XFF;
tbuf[2]=(aacy>>8)&0XFF;
tbuf[3]=aacy&0XFF;
tbuf[4]=(aacz>>8)&0XFF;
tbuf[5]=aacz&0XFF;
tbuf[6]=(gyrox>>8)&0XFF;
tbuf[7]=gyrox&0XFF;
tbuf[8]=(gyroy>>8)&0XFF;
tbuf[9]=gyroy&0XFF;
tbuf[10]=(gyroz>>8)&0XFF;
tbuf[11]=gyroz&0XFF;
tbuf[18]=(roll>>8)&0XFF;
tbuf[19]=roll&0XFF;
tbuf[20]=(pitch>>8)&0XFF;
tbuf[21]=pitch&0XFF;
tbuf[22]=(yaw>>8)&0XFF;
tbuf[23]=yaw&0XFF;
usart1_niming_report(0XAF,tbuf,28);//飛控顯示幀,0XAF
}
int main(void)
{ 
u8 t=0,report=1;  //預設開啟上報
u8 key;
float pitch,roll,yaw; //尤拉角
short aacx,aacy,aacz;  //加速度感測器原始資料
short gyrox,gyroy,gyroz; //陀螺儀原始資料
short temp;  //溫度 

Stm32_Clock_Init(9);  //系統時鐘設定
uart_init(72,500000);  //串列埠初始化為 500000
delay_init(72); //延時初始化
usmart_dev.init(72);  //初始化 USMART
LED_Init();  //初始化與 LED 連線的硬體介面
KEY_Init();  //初始化按鍵
LCD_Init();  //初始化 LCD
MPU_Init(); //初始化 MPU6050
POINT_COLOR=RED; //設定字型為紅色
LCD_ShowString(30,50,200,16,16,"Mini STM32");
LCD_ShowString(30,70,200,16,16,"MPU6050 TEST"); 
LCD_ShowString(30,90,200,16,16,"[email protected]");
LCD_ShowString(30,110,200,16,16,"2015/4/18");
while(mpu_dmp_init())
{
LCD_ShowString(30,130,200,16,16,"MPU6050 Error"); delay_ms(200);
LCD_Fill(30,130,239,130+16,WHITE); delay_ms(200);
}
LCD_ShowString(30,130,200,16,16,"MPU6050 OK");
LCD_ShowString(30,150,200,16,16,"KEY0:UPLOAD ON/OFF");
POINT_COLOR=BLUE;//設定字型為藍色
LCD_ShowString(30,170,200,16,16,"UPLOAD ON "); 
LCD_ShowString(30,200,200,16,16," Temp: . C"); 
LCD_ShowString(30,220,200,16,16,"Pitch: . C");
LCD_ShowString(30,240,200,16,16," Roll: . C");
LCD_ShowString(30,260,200,16,16," Yaw : . C");
while(1)
{
key=KEY_Scan(0);
if(key==KEY0_PRES)
{
report=!report;
if(report)LCD_ShowString(30,170,200,16,16,"UPLOAD ON ");
else LCD_ShowString(30,170,200,16,16,"UPLOAD OFF");
}
if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)
{
temp=MPU_Get_Temperature(); //得到溫度值
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //得到加速度感測器資料
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺儀資料
if(report)mpu6050_send_data(aacx,aacy,aacz,gyrox,gyroy,gyroz);
//用自定義幀傳送加速度和陀螺儀原始資料
if(report)usart1_report_imu(aacx,aacy,aacz,gyrox,gyroy,gyroz,(int)(roll*100),
(int)(pitch*100),(int)(yaw*10));

if((t%10)==0)
{
if(temp<0)
{
LCD_ShowChar(30+48,200,'-',16,0);  //顯示負號
temp=-temp; //轉為正數
}else LCD_ShowChar(30+48,200,' ',16,0); //去掉負號
LCD_ShowNum(30+48+8,200,temp/100,3,16); //顯示整數部分
LCD_ShowNum(30+48+40,200,temp%10,1,16); //顯示小數部分
temp=pitch*10;
if(temp<0)
{
LCD_ShowChar(30+48,220,'-',16,0);  //顯示負號
temp=-temp; //轉為正數
}else LCD_ShowChar(30+48,220,' ',16,0); //去掉負號
LCD_ShowNum(30+48+8,220,temp/10,3,16);  //顯示整數部分 
LCD_ShowNum(30+48+40,220,temp%10,1,16); //顯示小數部分
temp=roll*10;
if(temp<0)
{
LCD_ShowChar(30+48,240,'-',16,0);  //顯示負號
temp=-temp; //轉為正數
}else LCD_ShowChar(30+48,240,' ',16,0); //去掉負號
LCD_ShowNum(30+48+8,240,temp/10,3,16);  //顯示整數部分 
LCD_ShowNum(30+48+40,240,temp%10,1,16); //顯示小數部分
temp=yaw*10;
if(temp<0)
{
LCD_ShowChar(30+48,260,'-',16,0);  //顯示負號
temp=-temp; //轉為正數
}else LCD_ShowChar(30+48,260,' ',16,0); //去掉負號
LCD_ShowNum(30+48+8,260,temp/10,3,16);  //顯示整數部分 
LCD_ShowNum(30+48+40,260,temp%10,1,16); //顯示小數部分
t=0;
LED0=!LED0;//LED 閃爍
}
}
t++;
}
}