自动飞行所需主要参数的获取

wuhf520

    要实现自动飞行，飞控CPU必须获取飞机飞行的位置、高度、地速、空速、姿态等传感器信息。下面介绍如何从飞控CPU连接的传感器获取并使用这些信息。

    首先最重要的是飞机的位置，即经纬度信息。CY2007采用的是ublox公司的LEA-4S芯片，支持4HZ数据更新。事实上MP2028等优秀飞控也采用的ublox公司的LEA-4系列GPS芯片。

    Ublox公司自定义的ubx协议简短易用，飞控程序里的AT91F_UART0_HANDLER(void)函数完成了GPS信息的提取，包括了经纬度信息、GPS高度、搜星数、PDOP值、当前航向角、当前地速等，具体请参见源代码里的注释。

    姿态根据所用AHRS的不同而进行不同的协议解析，CY2007飞控程序里提供了某公司的小型AHRS姿态提取的代码，请参考AT91F_UART2_HANDLER(void)。

    气压高度和空速的提取，需由飞控程序执行相应代码，通过FPGA去访问MS5534A，才能获得SPI总线的数据。具体方式如下：

    根据MS5534A的手册，每次测量要从SPI总线读出D1、D2两个测量结果，在CY2007程序里记做swordD1、swordD2，最后的压力结果要经过公式计算：

ut1 = 8*c5+20224;
dt = swordD2 - ut1;
spitemp = 200 + dt*(c6+50)/1024;
off = c2*4+((c4-512)*dt)/4096;
sens = c1+(c3*dt)/1024+24576;
x = (sens*(swordD1-7168))/16384-off;
spip = x*100/32+250*100;

    其中c1、c2、c3、c4、c5、c6是传感器固有的参数，并且每个传感器的参数各不相同。因此我们要先读出这些参数。在main主程序里，有这样的代码：

reset5534();
swordw1 = rdSPIw1() & 0xffff;
reset5534();
swordw2 = rdSPIw2() & 0xffff;
reset5534();
swordw3 = rdSPIw3() & 0xffff;
reset5534();
swordw4 = rdSPIw4() & 0xffff;
c1 = swordw1 /2;
c5 = (swordw1 & 0x01)*1024 + (swordw2 >>6);
c6 = swordw2 & 0x3f;
c4 = swordw3 /64;
c3 = swordw4 /64;
c2 = (swordw3 & 0x3f)*64 + (swordw4 & 0x3f);

    有兴趣的朋友可以详细解读rdSPIw1等函数，其代码作用是通过连接到MS5534A的DIN管脚的FPGA脚，输出SPI总线的相应时序信号，通过与MS5534A的DOUT管脚连的FPGA脚，将各参数读取出来，具体可参见MS5534A的DATASHEET。

    同样，rdSPID1和rdSPID2，也是通过FPGA输出SPI总线的相应时序信号，将测量的结果D1、D2读取出来。最后在rdSPIpara（）函数里经过公式的计算，得出两个压力传感器测量的压力数据。

    我们把两个压力传感器分别定义为高度压力传感器和空速压力传感器。在rdSPIpara（）函数里，spip是高度气压传感器测量出的压力，spipnew是空速压力传感器测量出的压力。再定义并记录刚开机时即飞机在地面时测量的压力值为groundp，则groundp与spip的差就是飞机所在高度与地面气压的差，通过与GPS高度的比较和查表可获知当前与地面的相对高度，由于5534A的精度为0.1mba，对应高度精度大约为0.8m。而飞机在天上时的空速可以由两个压力传感器的差来计算，传感器压力之差与空速的平方是成正比的，因此另dongya = spipnew-spip，于是sqrt(dongya * XS)与空速成正比，选择适当的XS（用地速来校准）可使sqrt(dongya * XS)即为空速。具体请参见rdSPIpara（）函数里的计算。

TMAA

自動飛行所需主要參數的獲取


    要實現自動飛行，飛控CPU必須獲取飛機飛行的位置、高度、地速、空速、姿態等感測器資訊。下面介紹如何從飛控CPU連接的感測器獲取並使用這些資訊。

    首先最重要的是飛機的位置，即經緯度資訊。CY2007採用的是ublox公司的LEA-4S晶片，支援4HZ資料更新。事實上MP2028等優秀飛控也採用的ublox公司的LEA-4系列GPS晶片。

    Ublox公司自定義的ubx協議簡短易用，飛控程式裏的AT91F_UART0_HANDLER(void)函數完成了GPS資訊的提取，包括了經緯度資訊、GPS高度、搜星數、PDOP值、當前航向角、當前地速等，具體請參見源代碼裏的注釋。

    姿態根據所用AHRS的不同而進行不同的協議解析，CY2007飛控程式裏提供了某公司的小型AHRS姿態提取的代碼，請參考AT91F_UART2_HANDLER(void)。

    氣壓高度和空速的提取，需由飛控程式執行相應代碼，通過FPGA去訪問MS5534A，才能獲得SPI匯流排的資料。具體方式如下：

    根據MS5534A的手冊，每次測量要從SPI匯流排讀出D1、D2兩個測量結果，在CY2007程式裏記做swordD1、swordD2，最後的壓力結果要經過公式計算：

ut1 = 8*c5+20224;
dt = swordD2 - ut1;
spitemp = 200 + dt*(c6+50)/1024;
off = c2*4+((c4-512)*dt)/4096;
sens = c1+(c3*dt)/1024+24576;
x = (sens*(swordD1-7168))/16384-off;
spip = x*100/32+250*100;

    其中c1、c2、c3、c4、c5、c6是感測器固有的參數，並且每個感測器的參數各不相同。因此我們要先讀出這些參數。在main主程序裏，有這樣的代碼：

reset5534();
swordw1 = rdSPIw1() & 0xffff;
reset5534();
swordw2 = rdSPIw2() & 0xffff;
reset5534();
swordw3 = rdSPIw3() & 0xffff;
reset5534();
swordw4 = rdSPIw4() & 0xffff;
c1 = swordw1 /2;
c5 = (swordw1 & 0x01)*1024 + (swordw2 >>6);
c6 = swordw2 & 0x3f;
c4 = swordw3 /64;
c3 = swordw4 /64;
c2 = (swordw3 & 0x3f)*64 + (swordw4 & 0x3f);

    有興趣的朋友可以詳細解讀rdSPIw1等函數，其代碼作用是通過連接到MS5534A的DIN管腳的FPGA腳，輸出SPI匯流排的相應時序信號，通過與MS5534A的DOUT管腳連的FPGA腳，將各參數讀取出來，具體可參見MS5534A的DATASHEET。

    同樣，rdSPID1和rdSPID2，也是通過FPGA輸出SPI匯流排的相應時序信號，將測量的結果D1、D2讀取出來。最後在rdSPIpara（）函數裏經過公式的計算，得出兩個壓力感測器測量的壓力資料。

    我們把兩個壓力感測器分別定義為高度壓力感測器和空速壓力感測器。在rdSPIpara（）函數裏，spip是高度氣壓感測器測量出的壓力，spipnew是空速壓力感測器測量出的壓力。再定義並記錄剛開機時即飛機在地面時測量的壓力值為groundp，則groundp與spip的差就是飛機所在高度與地面氣壓的差，通過與GPS高度的比較和查表可獲知當前與地面的相對高度，由於5534A的精度為0.1mba，對應高度精度大約為0.8m。而飛機在天上時的空速可以由兩個壓力感測器的差來計算，感測器壓力之差與空速的平方是成正比的，因此另dongya = spipnew-spip，於是sqrt(dongya * XS)與空速成正比，選擇適當的XS（用地速來校準）可使sqrt(dongya * XS)即為空速。具體請參見rdSPIpara（）函數裏的計算。