[發(fā)明專利]一種基于硬件語言的航向角指示儀

有效

• 申請?zhí)枺?/td>: 201410546130.5
• 申請日: 2014-10-16
• 公開（公告）號: CN104406584B
• 公開（公告）日: 2017-10-31
• 發(fā)明（設(shè)計）人: 孫華;吳宗凱;張巖;薛晶晶;李昊俊
• 申請（專利權(quán)）人: 哈爾濱工程大學(xué)
• 主分類號: G01C21/08
• 分類號: G01C21/08
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于硬件語言的航向角指示儀，包括：地磁傳感器、FPGA控制器、數(shù)碼管，其特征是：使用FPGA作為控制器，地磁傳感器接收到載體的地磁數(shù)據(jù)，通過I2C總線把接收數(shù)據(jù)傳給FPGA，經(jīng)過FPGA進(jìn)行數(shù)據(jù)處理，經(jīng)數(shù)據(jù)處理后實現(xiàn)對地磁傳感器所采集信號的計算、校正，最后將載體的航向角輸出顯示；

所述的地磁傳感器為MAG3110型三軸地磁傳感器模塊；

所述的FPGA與地磁傳感器的通信方式采用的是I2C方式，根據(jù)I2C總線的時序?qū)Φ卮判畔⑦M(jìn)行采集；

所述的數(shù)碼管為三位共陽數(shù)碼管，作為航向角指示儀的顯示器采用靜態(tài)顯示的方式；

(1)地磁傳感器與FPGA的數(shù)據(jù)傳輸

根據(jù)I2C總線協(xié)議時序圖以及地磁傳感器的操作時序圖，在FPGA中使用硬件描述語言建立一個I2C通行模塊，完成地磁傳感器與FPGA磁場測量信息的傳輸，地磁信號采集程序具體步驟如下：

第一步：設(shè)置一個計數(shù)器，輸出周期為10us的方波作為SCL信號；并設(shè)置每個周期的10us為下降沿，2.5us為低電平，5us為上升沿，7.5us為高電平；

第二步：當(dāng)SCL第一個高電平時，SDA給低電平；

第三步：在之后的八個低電平時，給SDA賦MAG3110寫地址0x1C；

第四步：在接著的下降沿時，讓SDA高阻態(tài)輸出，結(jié)束一次賦值；

第五步：在之后的八個低電平時，給SDA賦目標(biāo)寄存器地址；

第六步：在接著的下降沿時，讓SDA高阻態(tài)輸出，結(jié)束一次賦值；

第七步：判斷是寫操作還是讀操作，寫操作：在之后的八個低電平時，給SDA賦數(shù)據(jù)，結(jié)束；讀操作：重復(fù)起始位，并進(jìn)入下一步；

第八步：當(dāng)SCL第一個高電平時，SDA給低電平；

第九步：在之后的八個低電平時，給SDA賦MAG3110讀地址(0x1D)；然后，在接著的下降沿時，讓SDA高阻態(tài)輸出，結(jié)束一次賦值；

第十步：在之后的八個高電平時，從SDA讀取數(shù)據(jù)，結(jié)束；

第十一步：輸出結(jié)束信號；

FPGA與地磁傳感器的通信方式采用的是I2C方式，根據(jù)I2C總線的時序?qū)Φ卮判畔⑦M(jìn)行采集；初始化部分，先將MAG3110設(shè)為STANDBY模式配置OSR，ODR將MAG3110設(shè)為ACTIVE模式，傳感器開始轉(zhuǎn)化數(shù)據(jù)具體程序步驟如下：

第一步：寫操作允許，對寄存器CTRL_REG1進(jìn)行賦值0x00；

第二步：上一步結(jié)束后，再對寄存器CTRL_REG1進(jìn)行賦值0x30；

第三步：上一步結(jié)束后，再對寄存器CTRL_REG1進(jìn)行賦值0x31；

數(shù)據(jù)讀取部分，讀DR_STATUS狀態(tài)寄存器，如果DR_STATUS&0x08＝1，一次轉(zhuǎn)換已經(jīng)完成，此時讀取數(shù)據(jù)；讀OUT_X_MSB(0x01)、OUT_X_LSB(0x02)、OUT_Y_MSB(0x03)、OUT_Y_LSB(0x04)、OUT_Z_MSB(0x05)、OUT_Z_LSB(0x06)寄存器，分別得到X、Y、Z三軸十六位的數(shù)據(jù)；此時轉(zhuǎn)動MAG3110，三軸數(shù)據(jù)會發(fā)生變化；具體程序步驟如下：

第一步：讀操作允許，讀取寄存器DR_STATUS的值；

第二步：上一步結(jié)束后，判斷讀取的數(shù)據(jù)的第四位數(shù)據(jù)是否為1，是：則進(jìn)入下一步；不是：則返回上一步；

第三步：設(shè)置寄存器OUT_X_MSB(0x01)；把數(shù)據(jù)存入xData數(shù)組的高八位；

第四步：設(shè)置寄存器OUT_X_LSB(0x02)；把數(shù)據(jù)存入xData數(shù)組的低八位；

第五步：設(shè)置寄存器OUT_Y_MSB(0x03)；把數(shù)據(jù)存入yData數(shù)組的高八位；

第六步：設(shè)置寄存器OUT_Y_LSB(0x04)；把數(shù)據(jù)存入yData數(shù)組的低八位；

第七步：設(shè)置寄存器OUT_Z_MSB(0x05)；把數(shù)據(jù)存入zData數(shù)組的高八位；

第八步：設(shè)置寄存器OUT_Z_LSB(0x06)；把數(shù)據(jù)存入zData數(shù)組的低八位；

第九步：設(shè)置x坐標(biāo)值為xData，y坐標(biāo)值為yData，z坐標(biāo)值為zData，輸出結(jié)束信號；

(2)地磁場修正

在程序中對于受到干擾時地磁場形狀進(jìn)行修正，使之經(jīng)過拉伸、平移變換成原點位于圓心的規(guī)則圓；具體程序步驟如下：

第一步：把x坐標(biāo)值賦給XMAX和XMIN數(shù)組；

第二步：判斷新的x坐標(biāo)值與XMAX和XMIN的大小關(guān)系，如果大于XMAX，則賦給XMAX；如果小于XMIN，則賦給XMIN；并輸出X0為XMAX和XMIN的平均值；

則XO、YO為X、Y平面的原點，原受干擾的橢圓被修正成了一個圓；通過測量出載體在X、Y軸的磁場強度，再應(yīng)用反三角函數(shù)，使用泰勒級數(shù)展開即可求出載體的航向角；

根據(jù)泰勒級數(shù)在0處展開的公式：

arctan(x)=x-x33+x55+...]]>

選擇保留到5次方項；先求出然后分三步求出接下去的三項；最后一步輸出acrtan(x)的值，即角度值，然后再輸出一個結(jié)束信號；

(3)載體的航向與真北的夾角

數(shù)據(jù)處理程序流程包括：

等待角度計算完成后；

第一步：判斷X坐標(biāo)值是否等于均值X0；如果是，那么再判斷Y坐標(biāo)值是否大于均值Y0，是則角度等于90度并且需加上地區(qū)的地磁北與真北夾角度；如果Y坐標(biāo)值不大于均值Y0；則角度等于270度再加上地區(qū)的地磁北與真北夾角度；

第二步：判斷Y坐標(biāo)值是否等于均值Y0；如果是，那么再判斷X坐標(biāo)值是否大于均值X0，是則角度等于0度并且需加上地區(qū)的地磁北與真北夾角度；如果X坐標(biāo)值不大于均值X0；則角度等于180度再加上地區(qū)的地磁北與真北夾角度；

第三步：判斷X坐標(biāo)值和Y坐標(biāo)值是否都大于均值？是則角度等于計算得出的角度值再加上地區(qū)的地磁北與真北夾角度；不是，進(jìn)入下一步；

第四步：判斷是否是X小于均值而Y大于均值？是則角度等于180度減計算得到的角度再加上地區(qū)的地磁北與真北夾角度；不是則進(jìn)入下一步；

第五步：判斷是否是X小于均值而Y小于均值？是則角度等于180度加計算得到的角度再加上地區(qū)的地磁北與真北夾角度；不是則進(jìn)入下一步；

第六步：判斷是否X大于均值而Y小于均值且計算得到的角度是否大于地區(qū)的地磁北與真北夾角度？是則角度等于360度減計算得到的角度再加上地區(qū)的地磁北與真北夾角度；不是則進(jìn)入下一步；

第七步：判斷是否X大于均值而Y小于均值且計算得到的角度是否小于地區(qū)的地磁北與真北夾角度？是則角度等于地區(qū)的地磁北與真北夾角度減計算得到的角度；

第八步：最后輸出處理后的角度值；

(4)數(shù)碼管顯示模塊

航向角指示儀的顯示器使用的是三位共陽數(shù)碼管，采用靜態(tài)顯示的方式，具體顯示程序步驟如下：

第一步：初始化顯示數(shù)組display[]，因為使用的是共陽的數(shù)碼管，所以給display[]賦的是共陽數(shù)碼管的0～9顯示碼；

第二步：得到處理后的角度值后，計算每位數(shù)碼管顯示的數(shù)字，通過取整取余計算得到角度值的各位數(shù)字；并按順序始終顯示出來。