minifly轨迹飞行

最新推荐文章于 2024-04-30 17:56:28 发布

poetry&

最新推荐文章于 2024-04-30 17:56:28 发布

阅读量1.1k

点赞数 12

文章标签：机器学习动态规划嵌入式

本文链接：https://blog.csdn.net/time_will_tell_/article/details/117308653

版权

minifly轨迹飞行

校内赛

1. 首先来看看笔者的校内赛题目：

设计并制作一架四旋翼自主飞行器。飞行区域俯视图和立体图分别如下图 1和图 2 所示。

要求：

  （1）四旋翼自主飞行器（下简称飞行器）摆放在图 1 所示的 A 区，一键式启动

  飞行器起飞；飞向 B 区，在 B 区降落并停机；飞行时间不大于 45s。 

  （2）飞行器摆放在 B 区，一键式启动飞行器起飞；飞向 A 区，在 A 区降落并

  停机；飞行时间不大于 45s。

校内赛题目分析

首先分析一下校内赛题目，发现其并不困难，根本无需使用视觉，于是我选择了价格比较便宜的minifly来完成此项目。

此题我是这么想的，首先minifly给的代码中有一键起飞一键降落，那么我们只需要在一键起飞与一键降落中插入一个

横飞并将其写成闭环就行。至于飞行高度，在一键起飞中即可以设定。
```
setFastAdjustPosParam(0, 1, 80.f);	/*一键起飞高度 80cm*/COMMUNICATE commonder.c 260行
```

首先我们来实现一键起飞后到达指定高度自动降落，先看一下正点原子已给好的代码

if(commander.ctrlMode & 0x01)/*定高模式*/
	{
		if(commander.keyLand)/*一键降落*/
		{
			flyerAutoLand(setpoint, state);
		}
		else if(commander.keyFlight)/*一键起飞*/ 
		{	
			setpoint->thrust = 0;
			setpoint->mode.z = modeAbs;		
			
			if (initHigh == false)
			{
				initHigh = true;	
				isAdjustingPosXY = true;
				errorPosX = 0.f;
				errorPosY = 0.f;
				errorPosZ = 0.f;

				setFastAdjustPosParam(0, 1, 80.f);	/*一键起飞高度80cm*/															
			}//COMMUNICATE commonder.c 241~261行

此代码很明确，在定高模式下，按一键起飞就可以飞到80cm高度，加了光流就可以实现定点。那么如果想当它飞到指定高度就降落，我们可以在代码中插入如下代码即可实现。

bool flag=0;//先在上面bool一个标志位

if(commander.ctrlMode & 0x01)/*定高模式*/
	{
		if(commander.keyLand)/*一键降落*/
		{
			flyerAutoLand(setpoint, state);
		}
    //插入当高度高于70cm时自动降落，state->position.z是实时高度，笔者花了好久找到的
         else if(state->position.z > 70.f)//当高度达到70cm时
         {
             flag=1;
			flyerAutoLand(setpoint, state);/*一键降落*/
		}
    //上面为插入内容
		else if(commander.keyFlight&&flag==0)/*一键起飞*/ 
		{	
			setpoint->thrust = 0;
			setpoint->mode.z = modeAbs;		
			
			if (initHigh == false)
			{
				initHigh = true;	
				isAdjustingPosXY = true;
				errorPosX = 0.f;
				errorPosY = 0.f;
				errorPosZ = 0.f;

				setFastAdjustPosParam(0, 1, 80.f);	/*一键起飞高度80cm*/															
			}//COMMUNICATE commonder.c 241~261行

这样可以实现当飞机高度超过70cm时，自动降落，但是它不会缓缓降落而是掉下来，因为当高度低于70cm时他就不会进入

else if(state->position.z > 70.f)这个语句。切记不能时else if(state->position.z == 70.f)，因为飞机飞行时会有误差，很难说高度就刚好等于70cm。

然后我们要实现让穿越机一键起飞后在空中悬停指定时间然后自动缓慢降落而非掉落，这就要在函数中加入定时功能了。通过查看代码发现飞机运行时10ms进入一次commonder.c。那么我们可以在原来bool flag=0;处引入一个静态整型变量count。

static int count=0;

现在比如我们要让它飞到120cm高度并在此高度悬停5s然后自动降落，代码如下：

if(commander.ctrlMode & 0x01)/*定高模式*/
	{
		if(commander.keyLand)/*一键降落*/
		{
			flyerAutoLand(setpoint, state);
		}
    //插入当count==500时降落，因为10ms进入此任务一次，所以500也就是5s
		else if(count==500)
		{
			   
				 flyerAutoLand(setpoint, state);
		}
    //以上为插入代码
		else if(commander.keyFlight&&count<500)
		{	
			count++;//每进入此任务count+1
			setpoint->thrust = 0;
			setpoint->mode.z = modeAbs;		
			
			if (initHigh == false)
			{
				initHigh = true;	
				isAdjustingPosXY = true;
				errorPosX = 0.f;
				errorPosY = 0.f;
				errorPosZ = 0.f;

				setFastAdjustPosParam(0, 1, 120.f);	/*一键起飞高度120cm*/															
			}

最后我们在起飞和降落中间插入横飞，如果你看懂上面内容，这个就很简单了，代码如下：

if(commander.ctrlMode & 0x01)/*定高模式*/
	{
		if(commander.keyLand)/*一键降落*/
		{
			flyerAutoLand(setpoint, state);
		}
		else if(count==500)
		{
			flyerAutoLand(setpoint, state);
		}
		else if(state->position.z >120.f&&count<500)/*当高度大于120cm并且在5s内时*/
		{ 
            /*此处为关键，这个时位置，如果想定点这里应该为0，如果想让它横飞则给他数值，不同的数
            值对应不同的速度，距离除以速度就等于时间，而时间通过count来控制也就完成了轨迹飞行*/
			  ctrlValLpf.roll = 15;
			  count++;
		}
		else if(commander.keyFlight&&count<500)/*按下飞行按键并且时间在5秒内起飞*/ 
		{	
			setpoint->thrust = 0;
			setpoint->mode.z = modeAbs;		
			
			if (initHigh == false)
			{
				initHigh = true;	
				isAdjustingPosXY = true;
				errorPosX = 0.f;
				errorPosY = 0.f;
				errorPosZ = 0.f;

				setFastAdjustPosParam(0, 1, 125.f);	/*一键起飞125cm*/															
			}

这样笔者校内赛基本要求就算完成了，只需要测量A、B两点间距离，测试一下ctrlValLpf.roll给相应值的速度，算出时间给count，就完成了闭环从A飞到B并且自动降落。

注意事项

1.要完成此任务必须搭载光流模块并且开启激光，并且要在定点模式下运行。

2.由于飞行器会受到各种因素影响，比如灯光，风，所以会有一定误差，建议到无风并且采光良好的场地调试。

3.由于加入的count无法自动清零，因此每次飞行都需要关机重开（因为没有复位键）。（此处可以通过修改代码实现count的自动清0并且不是很难，有兴趣的小伙伴不妨试一下）

拓展
通过笔者给的思路，想让minifly到达指定的几个位置然后降落就非常容易了，只需要多加几个else if，

大体如下:
```
else if (高度大于xxcm&&count<xx)
{
    往哪飞；
    count++；
}
else if (高度大于xxcm&&count<xx)
{
    往哪飞；
    count++；
}
else if (高度大于xxcm&&count<xx)
{
    往哪飞；
    count++；
}
```
- 调整横滚角，俯仰角，偏航角的三个变量如下：
```
static void commanderLevelRPY(void)//稳定飞行时
{
	ctrlValLpf.roll = 0;//改变其值可实现横滚
	ctrlValLpf.pitch = 0;//改变其值可实现俯仰
	ctrlValLpf.yaw = 0;//有头模式改变其值可实现偏航
}
```
- 另外我发现，其实直接更改位置环的位置误差比较大，如果能够找到遥控相关代码将其写死会更加稳当，很遗憾我没有成功，如果你有兴趣，给你两个我的思路。1. 在飞控代码中找到遥控给飞机发数据的地方，然后将其遥控给飞机的数据直接写进去，封装成函数，调用即可。2.在上位机中修改。
再拓展

那么，有小伙伴不经要问了，你这个只能直线走啊，我要绕柱子，走的不是直线，那不就实现不了了吗？

其实，也还是可以实现的，方法如下：
```
else if (高度大于xxcm&&count<xx)
{
    ctrlValLpf.roll = xx;//改变其值可实现横滚
	ctrlValLpf.pitch = xx//改变其值可实现俯仰
    count++；
}
```
这就用到微积分啦，10ms进入任务一次，每次既横滚又俯仰，通过积分理论上可以算出其轨迹，这对高数和建模有点要求的哦，而且会有误差，误差通过积分会放大，不过这种误差也可以消除，这就要用到PID，滤波等方面的知识啦，就不详细说了。拓展与再拓展只是笔者的理论分析，我也没有尝试，可能有很多漏洞，但是我就是提供一个思路，就是不用视觉也可以实现轨迹飞行，但是会有很大误差，如果用上所谓的PID，滤波去处理那么说不定就没有视觉简单了，所以我去学视觉了哈哈哈。

笔者的话

最后还是说一点吧，如果本文对你有用的话，那么希望你以后也能在完成一个任务后写篇博客吧。在没完成这道题之前，我曾查过很多资料，问过很多人也去过论坛寻找解决方案，但是获得的只是碎片化的知识，很浪费时间，有时候也满绝望的，但是当自己解决了之后，又会觉得很简单。所以人总是在超越自己的过程中成长的，不要放弃。话说回来，如果没有人写博客，没有CSDN给我查，我想我也完不成此项目。所以即使我现在很忙，但是我接受过别人的帮助，现在就是我帮助你们的时候了，希望大家都能养成做完项目写博客的习惯，帮助他人，总结自己。最后送大家一句话：深自缄默，如云漂泊。