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

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（）函数里的计算。

fujianshu

希望继续提供更多，谢谢

wuhf520