Mavlink地面站编写之七—发送控制指令

转载请注明出处:http://www.amovauto.com/?cat=19/;  阿木UAV社区

好久没更新MAVLINK系列文章了，最近事情比较多，中间去了趟深圳(见了老朋友顺便去了趟华强北溜了圈)。所以中间耽误更新的文章，不过从航点写入到MAVLINK指令发送，关于mavlink协议部分的文章也要写完了。后面只要能够用mavlink协议发送指令，基本就可以用指令，控制无人机做任何你想做的事情了，我想如果后面要扩展，比如加入三维激光壁障模块，视觉导航什么的，也可以利用mavlink的协议接口来控制无人机了。可以在ARM或者DSP等高级处理器上运行高级视觉和壁障的算法，计算出三维坐标参数，然后通过mavlink协议给下面的飞控来飞行到指定位置。这样飞控算法和导航壁障算法就是独立的，两者之间就是接口。当然这只是抛砖引玉，无人机开发有很多方向可以去做。
言归正传，下面讲述怎么实现给APM2.8飞控板发送控制指令。在MAVLINK中有发送指令的函数包装，我们只要用即可，但是我们要清楚，指令的发送数据格式，我在发送格式和发送参数上走了不少弯路，下面做一个记录，避免后来者遇到同样的问题，一起学习这个强大的开源项目。
我们知道APM/PIX 支持很多的飞行模式：
0:Stabilize自稳,1:Acro特技,2:AltHold定高,3:Auto自动
4:Guided引导,5:Loiter留待,6:RTL返航,7:Circle绕圈
9:Land降落,11:Drift飘移,13:Sport运动,14:Flip翻转
15:AutoTune自动调参,16:PosHold定点,17:Brake暂停
目前一共16种飞行模式。
默认上电的APM飞控是在自稳模式的，比如我们要在地面站实现 按键 一键起飞到10米高空，这个功能。那么我们一定是用地面站给飞控发送指令，同时我们必须获取当前飞控的处于什么模式下面。前面我们分析心跳包格式我们知道：读取心跳包就可以知道目前飞控是处于什么模式

其中有个base_mode 和 custom_mode ，对于base_mode 我们可以找到定义

 
public enum MAV_MODE_FLAG
{
   CUSTOM_MODE_ENABLED = 1,
   TEST_ENABLED = 2,
   AUTO_ENABLED = 4,
   GUIDED_ENABLED = 8,
   STABILIZE_ENABLED = 16,
   HIL_ENABLED = 32,
   MANUAL_INPUT_ENABLED = 64,
   SAFETY_ARMED = 128,
   ENUM_END = 129,
}

这是一些枚举，这是基本模式。下面看custom_mode用户模式定义:
0:Stabilize自稳,1:Acro特技,2:AltHold定高,3:Auto自动
4:Guided引导,5:Loiter留待,6:RTL返航,7:Circle绕圈
9:Land降落,11:Drift飘移,13:Sport运动,14:Flip翻转
15:AutoTune自动调参,16:PosHold定点,17:Brake暂停

我们读取base_mode 和 custom_mode，和对这两个参数设置，就可以控制飞控系统目前处于什么模式。
用户模式目前有这么多种，如果我们要实习一键起飞的模式，我们首先要把飞控设置到Guided引导模式，然后发送takeoff指令。在mavlink协议中，我们要使用custom_mode模式，必须要把base_mode模式设置为 CUSTOM_MODE_ENABLED = 1,也就是用户模式设置使能。这是实验得出的真理啊啊.....

 
   Form1.comPort.setMode(new MAVLink.mavlink_set_mode_t
                {
                    //使能了用户模式选择
 base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED,
                    //在指导模式下custom_mode = 4 标示指导模式
                    custom_mode = 4,
                    target_system = Form1.comPort.MAV.sysid
                });

这是MP源码的setMode函数我分别设置了基本模式和用户模式，我们跟踪可以知道 generatePacket((byte) (byte) MAVLink.MAVLINK_MSG_ID.SET_MODE, mode);这个函数发送了设置模式的指令，我们把设置好的mode传入了参数，通过这个函数发送给了飞控。也就是通过这个函数我们可以把飞控设置为任意飞行模式，但是要注意要符合逻辑。

 
   private void flybutton_Click(object sender, EventArgs e)
        {
            stabilizemode();
            System.Threading.Thread.Sleep(100);   
            if (Form1.comPort.doARM(true))
            {
                Form1.comPort.setMode(new MAVLink.mavlink_set_mode_t
                {
                    //使能了用户模式选择
base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED,
                    //在指导模式下custom_mode = 4 标示指导模式
                    custom_mode = 4,
                    target_system = Form1.comPort.MAV.sysid
                });
                System.Threading.Thread.Sleep(300);
                bool ans =Form1.comPort.doCommand(MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, 0, 0, 0,(float)(double.Parse(alttextBox.Text)));
                if (ans == true) { flymode.Text = "GUIDE"; }
            }
        }

这是完整的设置一键起飞的函数，我们在指导模式下，向飞控发送了MAVLink.MAV_CMD.TAKEOFF指令，用的doCommand函数。这个函数后面有很多参数:  https://pixhawk.ethz.ch/mavlink/;这里有解释那些参数代表的意思，

在mavlink协议我们看到后面的一些参数填写规则，一共有7个参数，我们可以找到起飞指令怎么填写参数：

其实最主要的是起飞高度和坐标值，我在doCommand只传送了高度值，坐标值应该是当前坐标，就默认了。里面还有很多其他指令，可以详细看看。发送任何指令都要填写7个参数，具体怎么填，协议里面说的很清楚。很多朋友想用指令来控制输出通道PMM值，这也是可以的，填入正确的参数即可。
阿木(UAV)社区！欢迎加入QQ群 526221258