詳解MPU6050,用STM32讀取原始數(shù)據(jù),并相互融合算出俯仰角、翻滾角、偏航角

詳解MPU6050,用STM32讀取原始數(shù)據(jù)，并相互融合算出俯仰角、翻滾角、偏航角MPU6050是什么？MPU6050是一個6軸運動處理組件，包含了3軸加速度和3軸陀螺儀。MPU-6000為全球首例整合性6軸運動處理組件，相較于多組件方案，免除了組合陀螺儀與加速器時之軸間差的問題，減少了人量的包裝空間。MPU-6000整合了3軸陀螺儀、3軸加速器，并含可藉由第二個I2C端II連接其他廠牌之加速器、磁力傳感器、或其他傳感器的數(shù)位運動處理(DMP:DigitalMotionProcessor)硬件加速引擎，由主要I2C端II以單一數(shù)據(jù)流的形式，向應(yīng)用端輸出完整的9軸融合演算技術(shù)LivenSeiise的運動處理資料庫，可處理運動感測的復(fù)雜數(shù)據(jù)，降低了運動處理運算對操作系統(tǒng)的負(fù)荷，并為應(yīng)用開發(fā)提供架構(gòu)化的API。MPU-6000的角速度全格感測范I判為±250、±500、±1000與±2000%“(dps),町準(zhǔn)確追縱快速與慢速動作，并且，用戶可程式控制的加速器全格感測范I制為±2g、±4g±8g與±16g。產(chǎn)品傳輸可透過最高至400kHz的I2C或最高達(dá)20MHz的SPI。MPU-6000可在不同電壓卞工作，VDD供電電壓介為2.5V±5%、3.0V±5%或3.3V±5%,邏輯接IIWDIO供電為1.8V±5%。MPU-6000的包裝尺寸4x4x0.9mm(QFN),在業(yè)界是革命性的尺寸。其他的特征包含內(nèi)建的溫度感測器、包含在運作環(huán)境中僅有±1%變動的振蕩器。加速度傳感器是干嘛用的？總而言這，加速度傳感器，其實是力傳感器。用來檢查上下左右前后哪幾個面都受了多少力(包括重力)，然后計算角度。陀螺儀是干嘛用的？簡而言之，陀螺儀就是角速度檢測儀。比如，一塊板，以X軸為軸心，在一秒鐘的時間轉(zhuǎn)到了90度，那么它在X軸上的角速度就是90度/秒(DPS,角速度單DegreePerSecond的縮寫o/S,體現(xiàn)了轉(zhuǎn)動的快慢)MPU6050分辨率是多少？3軸加速度和3軸陀螺儀分別用了3個16位的ADC,也就是說，加速度有3個16位ADC,其中每個軸使用了一個。也是說，每個軸輸出的數(shù)據(jù)，是2T6也就是-32768--+32768。陀螺儀也是一樣。單位換算上面說的-32768-+32768,那么這個數(shù)字到底代表了什么呢？比如陀螺儀32768到底是指角速度達(dá)到多少度/秒？這個其實是根據(jù)MPU6050設(shè)置的量程來決定的，量程不一樣，32768代表的值就不一樣。MPU6050的量程設(shè)置，在MPU6050::uutialize()(MPU6050.cpp庫)初始化函數(shù)中進(jìn)行了設(shè)置：setFullScaleGvroRange(MPU6050_GYRO_FS_250);setFullScaleAccelRange(MPU6050_ACCEL_FS_2);分別設(shè)置為，250度/秒，2g按陀螺儀來說，MPU6050有?四個量程可選：±250,±500,±1000,±2000度/s比方說，設(shè)置了是±250,那么-32768—-+32768就代表了-250—-+250。此時它的LSE(拉傻B,最低有效位)是131LSB/(度/s)STM32自帶I2C,但一般有兩個I2C1(PB6.PB7)和I2C2(PB10.PB11),而且,I2C分為碩件、和模擬。軟件12c是程序員使用程序控制SCL,SDA線輸出高低電平，模擬12c協(xié)議的時序.喚件12c程序員只要調(diào)用12c的控制函數(shù)即可，不用直接的去控制SCL,SDA高低電平的輸出本模塊采用的是IIC通信方式，所以我們只需要連接四跟線就可以完成電路的連接，簡單方便！原始數(shù)據(jù)有：AX、AY、AZGX、GY、GZ簡單的算法之后可以得到Rolbpitch,yaw參考MPU-6050數(shù)據(jù)手冊引腳說明：引腳編號袖PU-6000MPU&060引腳名稱描述1YYCLKIN可選的外部時鐘輸入，如果不用則連到GND6YYAUXDAI2C主串行數(shù)據(jù)，用于外接傳感器7YYAUXCLI2C主串行時鐘，用于外接傳感器8Y/CSSPI片選(0=SPImode)8YVLOGIC數(shù)字I/O供電電壓9YADO/SDOI2CSlave地址LSB(AD0)；SPI串行數(shù)據(jù)輸岀(SDO)9YADOI2CSlave地址LSB(ADO)10YYREGOUT校準(zhǔn)濾波電容連線11YYFSYNC幀同步數(shù)字輸入12YYINT中斷數(shù)字輸出(推挽或開漏)13YYVDD電源電壓及數(shù)字I/O供電電壓18YYGND電源地19,21,22YYRESV預(yù)留，不接20YYCPOUT電荷泵電容連線23YSCL/SCLKI2C串行時鐘(SCL)；SPI串行時鐘(SCLK)23YSCLI2C串行時鐘(SCL)24YSDA/SDII2C串行數(shù)據(jù)(SDA)；SPI串行數(shù)據(jù)輸入(SDI)24YSDAI2C串行數(shù)據(jù)(SDA)2,3,4,5,14,15,16,17YYNC不接VDD供電電壓為2?5V±5%、3?OV±5%、3?3V±5%；VDDIO為1.8V±5%內(nèi)建振蕩器在工作溫度范用內(nèi)僅有±1%頻率變化?？蛇x外部時鐘輸入32.768kHz或19.2NIHZ找出幾個重要的寄存器：Register25一SampleRateDivider(SMPRT_DI\r)Type:Read/WriteRegister(Hex)Register(Decimal)Bh7BitGBh5Bit4Bit3Bit2BillBitO宀25SMPLRTDIV[70]SMPLRT.DIV8位無符號值，通過該值將陀螺儀輸出分頻，得到采樣頻率該寄存器指定陀螺儀輸出率的分頻，用來產(chǎn)生MPU-60X0的采樣率。傳感器寄存器的輸出、FIFO輸出、DMP采樣和運動檢測的都是基于該采樣率。采樣率的計算公式采樣率=陀螺儀的輸出率/(I+SMPLRT_DIV)當(dāng)數(shù)字低通濾波器沒有使能的時候，陀螺儀的輸出平路等于8KH乙反之等于1KH乙Register26一Configuration(CONFIG)Type:Read/WriteRegister(Hex)Register(Decimal)Bit7Bh6Bk5Bit4Bh3Bit2Bit1BitO1A26■■EXT-SYNCSET[20]DLPFCFG[20]EXT_SYNC_SET3位無符號值，配置幀同步引腳的采樣DLPF_CFG3位無符號值，配置數(shù)字低通濾波器該寄存器為陀螺儀和加速度計配置外部幀同步(FSYNC)引腳采樣和數(shù)字低通濾波器(DLPF)。通過配置EXT_SYNC_SET,可以對連接到FSYNC引腳的一個外部信號進(jìn)行采樣。FSYNC引腳上的信號變化會被鎖存，這樣就能捕獲到很短的頻閃信號。采樣結(jié)束后，鎖存器將復(fù)位到當(dāng)前的FSYNC信號狀態(tài)。根據(jù)卞面的表格定義的值，采集到的數(shù)據(jù)會替換掉數(shù)據(jù)寄存器中上次接收到的有效數(shù)據(jù)EXT-SYNCSETFSYNCBitLocation0Inputdisabled1TEMPOUTL[0]2GYROXOUTL[0]3GYROYOUTL[0]4GYROZOUTL[0]5ACCELXOUTL[0]6ACCELYOUTL[0]7ACCELZOUTL[0]數(shù)字低通濾波器是由DLPF_CFG來配置，根據(jù)卞表中DLPF.CFG的值對加速度傳感器和陀

螺儀濾波DLPF^CFGAccelerometer(Fs=1kHz)GyroscopeBandwidth(Hz)Delay(ms)Bandwidth(Hz)Delay(ms)Fs(kHz)026002560.98811842.01881.912943.0982.813444.9424.814218.5208.3151013.81013.416519.0518617RESERVEDRESERVED8Register27-GyroscopeConfiguration(GYRO_CONFIG)Type:Read/WriteRegister(Hex)RegisterJDecimal)Bh7Bh6BK5BK4Bh3BH2Bit1BitOIB27XGSTYGSTZG.STFSSIELg■■-XG_ST設(shè)置此位，X軸陀螺儀進(jìn)行自我測試。YG_ST設(shè)置此位，Y軸陀螺儀進(jìn)行自我測試。ZG_ST設(shè)置此位，Z軸陀螺儀進(jìn)行自我測試。FS_SEL2位無符號值。選擇陀螺儀的量程。這個寄存器是用來觸發(fā)陀螺儀自檢和配置陀螺儀的滿量程范I制。陀螺儀自檢允許用戶測試陀螺儀的機械和電氣部分,通過設(shè)置該寄存器的XG_ST、YG_ST和ZG_STbits可以激活陀螺儀對應(yīng)軸的自檢。每個軸的檢測可以獨立進(jìn)行或同時進(jìn)行。自檢的響應(yīng)=打開自檢功能時的傳感器輸出-未啟用自檢功能時傳感器的輸出在NIPU-6000/MPU-6050數(shù)據(jù)手冊的電氣特性表中已經(jīng)給出了每個軸的限制范I制。當(dāng)自檢的響應(yīng)值在規(guī)定的范圍內(nèi)，就能夠通過自檢；反之，就不能通過自檢。根據(jù)下表，F(xiàn)S_SEL選擇陀螺儀輸出的量程：FS-SELFullScaleRange0±250%1±5002±1000°/S3±2000°/sRegister28-AccelerometerConfiguiation(ACCEL_CONFIG)Type:Read/WriteRegister(Hex)Register(Decimal)Bh7Bit6Bit5BH4Bit3Bit2Bit1BitO1C28XASTYASTZASTAFSSEL[10]-XA_ST設(shè)置為1時，X軸加速度感應(yīng)器進(jìn)行自檢。YA_ST設(shè)置為1時，Y軸加速度感應(yīng)器進(jìn)行自檢。ZA_ST設(shè)置為1時，Z軸加速度感應(yīng)器進(jìn)行自檢。AFS_SEL2位無符號值。選擇加速度計的量程。具體細(xì)節(jié)和上面陀螺儀的相似。根據(jù)下表，AFS.SEL選擇加速度傳感器輸出的量程。Registers59to64-AccelerometerMeasurements(ACCEL_XOUT_H,ACCEL_XOUT_L,ACCEL_YOUT_H,ACCEL_YOUT_L,ACCEL_ZOUT_H,andACCEL_ZOUT_L)Type:ReadOnlyRegister(Hex)Register(Decimal)Bit7Bit6BM5Bit4Bit3Bh2Bit1BitO3659ACCEL.XOUT[15:8]3C60ACCELXOUT[70]3061ACCEL.YOUT[15:8j3E62ACCEL^YOUT[70]3F63ACCEL.ZOUT(15:8]4064ACCEL.ZOUT"]ACCEL_XOUT16位2，s補碼值。存儲最近的X軸加速度感應(yīng)器的測量值。ACCEL_YOUT16位2，s補碼值。存儲最近的Y軸加速度感應(yīng)器的測量值。3)ACCEL_ZOUT16位2兮補碼值。存儲最近的Z軸加速度感應(yīng)器的測量值。這些寄存器存儲加速感應(yīng)器最近的測量值。加速度傳感器寄存器，連同溫度傳感器寄存器、陀螺儀傳感器寄存器和外部感應(yīng)數(shù)據(jù)寄存器，都由兩部分寄存器組成(類似于STM32F10X系列中的影子寄存器)：一個內(nèi)部寄存器，用戶不可見。另一個用戶可讀的寄存器。內(nèi)部寄存器中數(shù)據(jù)在采樣的時候及時的到更新，僅在串行通信接II不忙碌時，才將內(nèi)部寄存器中的值復(fù)制到用戶可讀的寄存器中去，避免了直接對感應(yīng)測量值的突發(fā)訪問。在寄存器28中定義了每個16位的加速度測量值的最大范圍，對于設(shè)置的每個最人范圍,都對應(yīng)一個加速度的靈敏度ACCEL_xOUT,如下面的表中所示:AFSSELFullScaleRangeLSBSensitivity0±2g16384LSB/g1±4g8192LSB/g2士8g4096LSB/g3±l6g2048LSB?Registers65and66一TemperatuieMeasuiement(TEMP_OUT_HandTEMP_OUT_L)Type:ReadOnlyRegistec(Hex)Register(Decimal)Bi(7Bit6Bit5Bit4Bit3BH2Bit1BitO4165TEMP.OUT[15:8J4266TEMP.OUT卩幼1)TEMP_OUT16位有符號值。存儲的最近溫度傳感器的測量值。Registers67to72一GyroscopeMeasuiements(GYRO_XOUT_H,GYRO_XOUT_L,GYRO_YOUT_H.GYRO_YOUT丄GYRO_ZOUT_H?andGYRO_ZOUT_L)Type:ReadOnlyRegister(Hex)Register(Decimal)Bit7 Bh6 Bit5 Bit4Bh3 Bit2 Bit1 BitO4367GYRO.XOU11158!4468GYROXOUT[70]4569GYRO.YOUT(15:8j4670GYROYOUT(70]4771GYRO^ZOUT(15:8]4872GYRO.ZOUT[70]這個和加速度感應(yīng)器的寄存器相似對應(yīng)的靈敏度:FSSELFullScaleRangeLSBSensitivity0±2507S131LSB/7S1±500°/S65.5LSB/2/S2土10007S32.8LSB/°/S3±20007s164LSB/°/sType:Read/WriteRegister(Hex)Register(Decimal)Bit7Bh6Bit5Bit4Bh3Bit2Bit1BitO68107DEVICE.RESETSLEEPCYCLE-TEMPOSCLKSCLRO]該寄存器允許用戶配置電源模式和時鐘源。它還提供了一個復(fù)位整個器件的位，和一個關(guān)閉溫度傳感器的位DEVICE_RESET置1后所有的寄存器復(fù)位，隨后DE\1CE_RESET自動置0.SLEEP置1后進(jìn)入睡眠模式3)CYCLE當(dāng)CYCLE被設(shè)置為1,且SLEEP沒有設(shè)置，MPU-60X0進(jìn)入循壞模式，為了從速度傳感器中獲得采樣值，在睡眠模式和正常數(shù)據(jù)采集模式之間切換，每次獲得一個采樣數(shù)據(jù)。在LP_WAKE_CTRL(108)寄存器中，可以設(shè)置喚醒后的采樣率和被喚醒的頻率。TEMP_DIS置1后關(guān)閉溫度傳感器CLKSEL指定設(shè)備的時鐘源時鐘源的選擇：9)Register117-WhoAmI (WHO_AM_I)Type:ReadOnlyRegister(Hex)Register(Decimal)BH7.Bit6B4t5Bit4Bh3Bh2Brt1BitO75117?WHO.AM川6:1]■WHO_AM_I中的內(nèi)容是MPU-60X0的6位I2C地址上電復(fù)位的第6位到第1位值為：110100為了讓兩個MPU-6050能夠連接在一個I2C總線上，當(dāng)AD0引腳邏輯低電平時，設(shè)備的地址是bl101000,當(dāng)ADO引腳邏輯高電平時，設(shè)備的地址是bllOlOOl(2013.01.24)淘寶買的貨終于到了，學(xué)習(xí)用所以沒買好的，這個模塊只要18塊錢。MPU-6000可以使用SPI和I2C接口,而MPU-6050只能使用I2C,其中I2C的地址由ADO引腳決定;寄存器共117個，挺多的，卞面的是精簡常用的，根據(jù)具體的要求，適當(dāng)?shù)奶砑印?defineSNIPLRT_DI\70x19〃采樣率分頻，典型值：0x07（125Hz）*/#defineCONFIGOxlA 〃低通濾波頻率，肌型值：0x06（5Hz）*/^defineGYRO_CONFIGOxlB//陀螺儀自檢及測量范吊1，典型值：0x18（不自檢，2000deg/s）*/defineACCEL_CONF