一直没看的无人机定高

除了定高的部分，其他没看的还有一些控制函数。

定高的原理的话，也是利用了两级pid，

按照原本的理解，从外环开始看的话，反馈高度比较容易获得，就是激光测距得到的高度。期望高度好像无法得到。内环的话，期望速度是外环的输出，实际速度不知道怎么测得的，应该是高度的微分，等下仔细看看。

说期望高度“无法得到”，是因为没有必要，或者说我们通常不这么做。

首先是因为我们通常不使用定高在某个高度这个功能（吧），而是通常利用一键起飞，飞起来后再控制无人机能够按照一定的速度，去上升和下降。

然后从控制效果的角度看，如果按照我们通常的两级pid的控制思路：期望高度是我们给定的高度，反馈高度是我们测得的高度，利用pid可以得到期望速度，再利用这个期望速度结合实际速度，可以得到我们应得的加速度，也就是电机输出量。这也许可以达到定高的目的（而且是在不考虑最初期望高度和反馈速度的巨大偏差可能带来的不稳定的情况下），但会使我们丧失对飞机运动的控制：因为我们常常需要飞机上升或者下降，通常这个需求是在控制中被作为期望速度输入。但看上述的控制过程，我们会发现期望速度会由外环给出，我们不能对它赋值以实现控制。
这种情况下想要实现飞机的上升和下降，就只能去改变飞机的期望高度，而且我们很难去控制它的速度，这当然是我们不想要看到的。

为什么角度环没有遇到这种问题？这是一个较为细微的概念，对于初学者对于控制的原理和过程不明确，就不太容易理解。原因在于，我们对无人机的控制需求大体只有转向、左右、前后、上下移动，这也是遥控器上的那两个东西所能控制的部分。

我们实现前后左右移动，也就是想要飞机达到我们期望的前后左右方向的速度，只要使飞机的pitch和roll保持固定的角度就可以，想要前进，就让pitch增大，而且想要前进的速度越大，pitch就要越大。因此在角度环，我们外环是角度，用期望速度换算成期望角度、内环是角速度，得到电机输出量，就可以实现稳定或者移动。

但上下就不同了。我们要想实现一定的上下速度，速度本身就是目标值。对应角度环的话，这个速度应该对应角速度。

那问题来了，为什么我们不直接使用一个环呢？只用速度环按道理来讲就可以实现以指定速度上下移动了吧。原因就在于。。。还不知道怎么去表述，但原因之一应该跟起飞降落有关。

接下来看代码：
对于高度控制的头文件，定义了如下函数，内容很简单：

高度环控制和pid初始化、高度速度环控制和pid初始化
以及自动上升下降任务

我们先看这个串级pid：
参数初始化就不说了，其实也不是很懂。

对于外环：

Auto_Take_Off_Land_Task(1000*dT_s);

先调用了自动起飞/降落任务，

fs.alt_ctrl_speed_set = fs.speed_set_h[Z] + auto_taking_off_speed;

这个 fs.alt_ctrl_speed_set速度，就是我们的实际期望速度，为什么叫实际期望速度？就是说它是由两个速度相加：
1、 fs.speed_set_h[Z]

这个速度是程序或者用户对飞机的控制指令，包括那三个东西相加。

2、auto_taking_off_speed

一键起飞功能给出的速度，等下详细看一键起飞这些功能。

接下来：

loc_ctrl_2.exp[Z] += fs.alt_ctrl_speed_set *dT_s;
loc_ctrl_2.exp[Z] = LIMIT(loc_ctrl_2.exp[Z],loc_ctrl_2.fb[Z]-200,loc_ctrl_2.fb[Z]+200);
loc_ctrl_2.fb[Z] = (s32)wcz_hei_fus.out;/

对期望高度和反馈高度赋值。

if(fs.alt_ctrl_speed_set != 0)
{flag.ct_alt_hold = 0;
}

如果此时的期望速度不为零，那么也就是说还在上升或者下降，那么这个标志为就置零，也即是说现在不是在定高状态。

else
{if(ABS(loc_ctrl_1.exp[Z] - loc_ctrl_1.fb[Z])<20){flag.ct_alt_hold = 1;}
}

否则，期望速度位为零了，也就是说需要高度不会再变了，同时对于位置环，期望速度与实际速度差别很小了，那么就置位这个标志位。

if(flag.taking_off == 1)
{PID_calculate( dT_s,            0,             loc_ctrl_2.exp[Z],          loc_ctrl_2.fb[Z],       &alt_arg_2, &alt_val_2, 100,0                               );
}
else
{loc_ctrl_2.exp[Z] = loc_ctrl_2.fb[Z];alt_val_2.out = 0;
}

如果taking_off标志位有效，那么说明飞机已经起来了，那么就执行pid，否则，认为飞机在地上，也就没有速度。期望值等于反馈值，所需速度也就为0.

对于内环：

loc_ctrl_1.exp[Z] = 0.6f *fs.alt_ctrl_speed_set + alt_val_2.out;w_acc_z_lpf += 0.1f *(imu_data.w_acc[Z] - w_acc_z_lpf); loc_ctrl_1.fb[Z] = wcz_spe_fus.out + Ano_Parame.set.pid_alt_1level[KD] *w_acc_z_lpf;

反馈和期望速度赋值，注意这里的期望速度，两个值相加，应该是用于修正。

PID_calculate( dT_s,          0,             loc_ctrl_1.exp[Z],              loc_ctrl_1.fb[Z] ,          &alt_arg_1, &alt_val_1, 100,(THR_INTE_LIM *10 - err_i_comp )*out_en                                 );

pid就略了。

if(flag.taking_off == 1)
{LPF_1_(1.0f,dT_s,THR_START *10,err_i_comp);//err_i_comp = THR_START *10;
}
else
{err_i_comp = 0;
}

如果起飞了，那么 err_i_comp就赋值这个，否则，飞机在地上，err_i_comp就为0。
这个err_i_comp应该也是修正用的。

loc_ctrl_1.out[Z] = out_en *(alt_val_1.out + err_i_comp);loc_ctrl_1.out[Z] = LIMIT(loc_ctrl_1.out[Z],0,MAX_THR_SET *10);  mc.ct_val_thr = loc_ctrl_1.out[Z];

飞行时out_en才为1，地上时为0.
也就是说是否把油门值输出，取决于当前taking_off状态，因为即使在地上时，没有alt_val_2.out，但仍然有fs.alt_ctrl_speed_set

接下来就是看看非常混乱的一键起飞/降落了。。。。
非常混乱，是因为这个相关的代码有好几个，不知道怎么控制的，尝试分析一下。。

先是注意到了高度控制的那个文件里，有一个自动起飞降落任务：

Auto_Take_Off_Land_Task

进去之后还有一个一键起飞降落任务：

one_key_take_off_task

先看这个一键起飞任务：

void one_key_take_off_task(u16 dt_ms)
{if(one_key_taof_start != 0){one_key_taof_start += dt_ms;if(one_key_taof_start > 1400 && flag.motor_preparation == 1){one_key_taof_start = 0;if(flag.auto_take_off_land == AUTO_TAKE_OFF_NULL){flag.auto_take_off_land = AUTO_TAKE_OFF;flag.taking_off = 1;}}}//resetif(flag.unlock_sta == 0){one_key_taof_start = 0;}}

对one_key_taof_start进行判断，如果不是0，那就++，加到1400了，同时电机准备好了，就把one_key_taof_start清零。如果此时起飞降落标志位为起飞无效，那么就把状态改为起飞，taking _off=1。
如果没解锁，那么one_key_taof_start永远是0，无法累计。

也就是说，解锁之后等待1.4s，且状态为起飞无效，那么就把状态设置为起飞。

再看上一个任务：自动起飞降落任务
先执行了一键起飞任务，然后判断：

if(flag.unlock_sta)
{if(flag.taking_off){   if(flag.auto_take_off_land == AUTO_TAKE_OFF_NULL){flag.auto_take_off_land = AUTO_TAKE_OFF;       }}}
else
{auto_taking_off_speed = 0;    flag.auto_take_off_land = AUTO_TAKE_OFF_NULL;
}

如果解锁，且taking _off有效，那么如果起飞无效，就改成有效。
否则，没有解锁，auto_taking_off_speed恒为0，起飞设置为无效。

if(flag.auto_take_off_land ==AUTO_TAKE_OFF)
{s16 max_take_off_vel = LIMIT(Ano_Parame.set.auto_take_off_speed,20,200);take_off_ok_cnt += dT_ms;auto_taking_off_speed = AUTO_TAKE_OFF_KP *(Ano_Parame.set.auto_take_off_height - wcz_hei_fus.out);auto_taking_off_speed = LIMIT(auto_taking_off_speed,0,max_take_off_vel);if(take_off_ok_cnt>=5000 || (Ano_Parame.set.auto_take_off_height - loc_ctrl_2.exp[Z] <2)){flag.auto_take_off_land = AUTO_TAKE_OFF_FINISH;          }if(take_off_ok_cnt >2000 && ABS(fs.speed_set_h_norm[Z])>0.1f){flag.auto_take_off_land = AUTO_TAKE_OFF_FINISH;}}

如果自动起飞有效了，那么设置自动起飞速度。自动起飞流程有两个退出条件：
1、自动起飞流程运行的时间大于5s，或者满足高度。
2、自动起飞流程运行的时间大于2s，且用户在控制油门。
满足这两个条件之一，都认为自动起飞已经完成。

else
{take_off_ok_cnt = 0;      if(flag.auto_take_off_land ==AUTO_TAKE_OFF_FINISH){auto_taking_off_speed = 0;            }
}

否则，自动起飞无效（还没飞或者已经会玩），就不计数。
如果已经飞完，那么把自动速度设为零。

如果状态是自动下降，就给一个自动下降速度。

if(flag.auto_take_off_land == AUTO_LAND)
{auto_taking_off_speed = -(s16)LIMIT(Ano_Parame.set.auto_landing_speed,20,200);
}

除此之外还有一个一键起飞函数：

one_key_take_off（）

内容比较短：

void one_key_take_off()
{if(flag.unlock_err == 0){    if(flag.auto_take_off_land == AUTO_TAKE_OFF_NULL && one_key_taof_start == 0){one_key_taof_start = 1;flag.unlock_cmd = 1;}}
}

上述这三个有控制关系，一般来讲，只要调用函数one_key_take_off（），且已经解锁了，一键就可以起飞。

嗯，飞行就讲到这里，还有一个问题就是，期望高度如何去设定，等看看遥控器程序再回来解答。接下来的任务就是如何编写控制函数了。。。orz会不会太难了啊唉

一直没看的无人机定高相关推荐

小马哥robofly四轴气压计定高的不完全解读
我看过正点原子的定高算法,好像是直接用气压计测量得到的高度作为反馈进行,而小马哥的robofly是结合气压计获得的高度和运动情况下获得Z轴的位移2者结合来获取高度,但是2者结合的过程看得一知半解,下面 ...
零基础DIY四轴飞行器超级详细保姆级教程（STM32F407ZGT6主控、WIFI图传、陀螺仪平衡、气压计/超声波定高、手机蓝牙控制等功能）
前言大四毕业后暑假没事做就花了一点DIY了一个四轴飞行器,是比较大的那种F450机架,不是那种PCB板做机架的小四轴,因为我也是从零基础开始做的四轴,现在就想把自己的过程写成博客分享在网上.下面我会 ...
评论，转自于《低端没出路，请接触高端！》
评论,转自于<低端没出路,请接触高端!>,稍作修改和整理. [16楼] bisheng.hu 2009-08-17 13:05:00 看到这里,,我很无奈..你这个想法其实代表了 ...
超声波定高--过滤突然出现的障碍物
超声波测到的数据会有一点毛刺,先用中值滤波,可以很好去掉突变太大的数据.为什么不用均值滤波呢,均值滤波对数据的实时性影响比较大. 对中值滤波后的数据做一定的统计,计算出数据的变化量和离散程度.我能想到 ...
有量子计算机的山西高能小说,五本大神级高能热血小说，没看过也必定听说过，加入书架告别书荒！...
哈喽,大家好,我是云海,点击[关注],每天给您推好书!五本大神级劲爆小说,没看过的也必定听说过 ! 一.<杀神> 逆苍天简介: 在这个人吃人的疯狂世界,神已无力回天,就让我踏着漫天诸神的 ...
32岁了学python 来的及吗-32岁的程序员去了国企工作, 晒出年薪, 网友: 我没看错吧...
原标题:32岁的程序员去了国企工作, 晒出年薪, 网友: 我没看错吧现如今,相信很多朋友都非常羡慕那些互联网大公司的从业人员,因为他们动不动就年薪三四十万以上,但是这些互联网行业的从业人员也有自己的 ...
葫芦视频动漫排行榜前十名，没看过的可以补上
新的一年已经到来,去年到现在为止,你最喜欢的日漫番剧是哪一部呢?相信漫迷都有自己的选择.日漫的影响力早已遍布全球,世界各地的漫迷都非常喜欢日漫,认可他们创造的优秀作品.近期欧美那边就公布了一则非常有趣 ...
有些朋友没看清楚，我在这里再特意强调一下
没想到这么多朋友关注,针对评论里的回复内容,有不少是重复的,无法一一回复,在这里统一回答一下. 有些朋友没看清楚,我在这里再特意强调一下,她跟那个男的在一起比跟我在一起早差不多两年,所以到底是我被绿了 ...
新海诚没有参与制作的作品_由新海诚创作，却成为冷门的一部动漫，至今都没看懂...
原标题:由新海诚创作,却成为冷门的一部动漫,至今都没看懂由新海诚创作,却成为冷门的一部动漫,至今都没看懂.新海诚我们都很熟悉,他的作品<你的名字><天气之子>等等都给我们留下 ...

一直没看的无人机定高

一直没看的无人机定高相关推荐

最新文章

热门文章