DIY 飞控探讨，主角FY-20A，全部公开代码。

ly4328887

强贴:em26:

bdblue

GPS定位 得到偏差 一

好了现在我们已经得到当前飞机的所有参数了，在飞机未起飞之前，先执行一次HOME 设定，下面的代码里我们也以HOME为目标举例，也就是我们经常用到的返航回家功能。
飞机起飞后，我们把当前参数与HOME的参数进行比对计算，就可以得到返航所需要的各目标值，这些参数包括：
离HOME的水平距离
离HOME的直线距离
当前飞行方向
当前飞行速度
与HOME的直线夹角
以HOME点为原点，东西向为X轴，北南向为Y轴，天顶到原点为Z轴，得到一个3D坐标系，与HOMEDE夹角有两个：
一个是飞机与HOME连线和X轴的水平夹角
另一个是飞机与HOME连线和水平面的夹角
这两个夹角决定了飞机该如何偏航。
请注意，飞机在四个象限内飞行时，与X轴的水平夹角有几种情况。
至于这些东西怎么算的，请找个地球仪，自己摸索。
下面是代码：

bdblue

GPS 定位 得到偏差 二
void POSITION_CALCU(int long XP,int long YP,int HP)
{

int long LATI_DELTA;


int long LONGI_DELTA;


double DISTANCE_DELTA;


double DIREC_HORI_DOUB;


double DIREC_HORI_PRE;


double DIREC_VERT_DOUB;


double DISTANCE_HORI_METER;

double DISTANCE_STRA_METER;


LATI_DELTA = ACT.LATITUDE-XP;





LONGI_DELTA = (long) (ACT.LONGITUDE-YP)* cos(ACT.LATITUDE_LOCAL);











DISTANCE_DELTA = sqrt( LATI_DELTA * LATI_DELTA + LONGI_DELTA * LONGI_DELTA );


DISTANCE_HORI_METER = DISTANCE_DELTA * 1.85;


DISTANCE_STRA_METER = sqrt( DISTANCE_HORI_METER * DISTANCE_HORI_METER + HIGH_DIFF*HIGH_DIFF );


DIREC_HORI_PRE = atan2 ( LONGI_DELTA , LATI_DELTA );


DIREC_VERT_DOUB = atan2 ( DISTANCE_HORI_METER , HIGH_DIFF);


if (LATI_DELTA >0)


{


if(LONGI_DELTA>0)


{


DIREC_HORI_DOUB = DIREC_HORI_PRE;


}


else


DIREC_HORI_DOUB = PI - DIREC_HORI_PRE;


}


else


{


if(LONGI_DELTA>0)


{


DIREC_HORI_DOUB = 2*PI - DIREC_HORI_PRE;


}


else


DIREC_HORI_DOUB = PI + DIREC_HORI_PRE;


}


DIRECTION_HORI = (int) (DIREC_HORI_DOUB*57.3);


DIRECTION_VERT = (int) (DIREC_VERT_DOUB*57.3);


DISTANCE_HORI = (int) DISTANCE_HORI_METER;


DISTANCE_STRA = (int) DISTANCE_STRA_METER;


DIRECTION_DIFF = ACT.DIRECTION - DIRECTION_HORI;


HIGH_DIFF = ACT.HIGH-HP;
}

通过调用这个函数，我们可以进行返航飞行，或者其他航点飞行。你可以通过串口电台输入航点，也可以当飞机飞过某点时纪录下当前经纬度，超过2个纪录后可以在各航点间自动巡航，简单的讲，它能重复你飞过的航线。如果开发PC相应程序，可以进行鼠标指点巡航。
但是，以上这些功能必须依赖另一个重要的程序，下一节我们讲PID 航点程序。

bdblue

具体返航功能思路是这样的：
当设置HOME经纬度时，以HOME为原点，50米为半径，100米高度的空中水平画一个圆，然后在圆周上平均取6个点，存储下这六个点的经纬度，我称之为定位点。启动返航功能后，首先作一条飞机所在点与圆圈的切线，然后开始飞向离切点最近的定位点，到达此定位点后再按照顺序飞向下一个定位点，直到飞完六个定位点后，再重复飞向第一个定位点。由于固定翼特有的转弯特性，我们肉眼看来飞机似乎在HOME顶上100米的高度作圆圈飞行，定位点越多飞行轨迹越趋向于正圆。
这样处理的好处是：飞机返航后会在HOME顶上作圆周飞行，而且这个圆周飞行不受风的影响，只要GPS信号够稳定，飞机就永不会逃离这个无形的圈。如果是直飞HOME点然后再以一定的姿势盘旋，首先这个盘旋容易受风的影响，第二，盘旋的圆心不会是精确的HOME点。
首先飞向切点是为了让飞机平滑地切入正圆，尽量消除震荡。

yiny0730

:em26:

gale

有技术含量的要顶。

364156613

LZ加油！说实话这个应该属于一套单独的自动回家系统。
对于GPS这一块我还真的恼火。。看LZ的了。
可行性很好！

364156613

这个算法很好！我也要加紧对于MWC固定翼功能的探索了。
这样一来，完全可以把FPV成本降下来。
另外LZ有没有想过，利用1HZ GPS。隔一段时间读出一个坐标，并计算，确定飞机的大致飞行方向。这样可以很大程度降低成本。

fujianshu

:em26: 很不错的

bdblue

欢迎一起探讨，另外MWC是什么意思？

364156613

MWC是老外基于arduino为四轴飞行器开发的一套飞控系统。
基本功能有自稳、电子罗盘、气压定高。
实际上几乎所有的四轴飞控，代码稍加修改即可用于固定翼。
而且这些飞控的传感器配置堪称豪华，用于固定翼可以说是纹丝不动。
之前我把这套程序修改过一个固定翼版本，已经上到论坛了。

LZ大可不必考虑姿态部分，那本来就是平衡仪该做的。
另外就是LZ如果M128，可以不必引入其他通道，也就是接收机直接接到FY20A。而回家功能只控制方向舵就OK了。
交流不敢，我的技术也仅限照葫芦画瓢，对NMEA更可以说是一窍不通，只有学习的份了。
祝LZ早日成功！

bdblue

谢谢LS朋友的讲解，但是如果想要定高飞行的话，水平尾舵也是必须接入姿态控制的。
还有我用串口电台写遥控信号到飞机时，必须通过M128然后再传到FY20A。
NMEA其实并不复杂，是死的协议，我把数据严格按照顺序存放到数组里，然后直接到相应地方取需要的信息，非常方便。代码我已经帖出来。

bdblue

还有个问题想请教，自主导航时的转弯一般通过垂直尾翼还是副翼。目前我用的副翼。
我的代码里用到了两次PID 运算，一个是方位，通过副翼，一个是高度，通过水平尾翼。

364156613

如果要进行定高控制，确实需要引入升降舵。LZ是要搞大工程啊，还要加入数传，是要多航点飞行？
方向控制，副翼和升降舵皆可。只是平衡仪如果没有电子罗盘，是不接管方向通道的。这样直接接方向就会很简单。
另一个就是出于安全考虑。如果用副翼控制的话，因为遥控信号优先，所以平衡仪的平衡功能受到限制。倘若GPS指引有误，造成副翼转到极限。会使飞机滚转。
LZ的代码，我还一直没来得及看，因为这几天上课很忙。之后一定会好好研究学习的。

bdblue

考虑了下，决定返航靠副翼控制，方向舵效率太低，同时对副翼姿态做限幅，防止滚转发生。
加入数传是需要GPS信号进行天线自动跟踪，图传距离太不给力了。