知识库 / 开源飞控开发相关教程 / Ardupilot开发系列教程

SITL + pymavlink通讯库，获取飞控的俯仰和偏航角度

一、前言

1.1 开启SITL仿真

通过APM仿真环境搭建的文章，我们知道在没有飞控实物的情况下，可以开启Ardupilot代码中的SITL软件仿真系统进行仿真。

如果不清楚的朋友可参考《SITL仿真环境搭建》

当我们输入开启SITL仿真命令

../Tools/autotest/sim_vehicle.py --map --console

此开启MP地面站，会发现地面站自动以UDP的方式连接了SITL仿真，地面站也能读取到SITL仿真中的各类数据。

1.2 用python + Pymavlink库 ，进行Ardupilot二次开发入门。

1）Ardupilot的C/C++底层代码现在非常庞大，并且耦合性较大，对于不熟悉这个代码的新手基本上难以下手去修改。

2）新手小白建议使用语法简单的Python语言，封装好的Mavlink协议的工具包也就是Pymavlink通讯库，做二次开发的学习与验证。

3）对于我们新手小白而言，使用Mavlink通讯协议，基本上就可以实现对无人机各类遥测数据的获取及对飞控的各种控制。

4）因此涉及的主要工具有以下几个：

  • Ardupilot固件的SITL仿真环境。

  • MP地面站(实时观察飞控的各类数据)

  • pymavlink通信库(安装这个库以后，我们才可以通过python对无人机进行控制。)

  • Vscode(可有可无，有的话就可以直接打开编译器编写python的代码。未安装的话，就需要在linux环境下，使用vim编辑器对python代码进行编写与修改!)

1.3 开发需求(牛刀小试)

那么现在，我想写一个python脚本与飞控之间的通讯，通过Python脚本实现对无人机的俯仰、偏航、横滚角度进行获取。

后续就可在代码中拓展为当我的飞行模式处理XX模时或者当无人机处于某个高度时，就会执行某个特定的动作。

然后在将我们上述写的python仿真环境下进行测试，测试通过后，我们就可以挂一个树莓派与飞控连接， 在树莓派上部署着仿真环境里验证过的代码。

1.4 注意：Python如何连接仿真环境中的飞控？

在SITL仿真开启后，MP地面站也连上了。我自己写得Python脚本应该怎么连接到SITL仿真？实现对无人机的各种控制以及数据获取。

使用命令开启SILT仿真后，再开启MP地面站。此时MP地面站会自动占用默认的127.0.0.1：14550这个IP地址与端口！

前提：

需要在启动SITL仿真时，再广播一个IP地址和端口，启动SILT仿真的命令需要做一些简单的修改。

../Tools/autotest/sim_vehicle.py --map --console --out 127.0.0.1:14552

上述命令的意思是，我打开仿真的时，再将飞控的数据广播到127.0.0.1这个地址，端口是14552。（具体地址，大家可以按需修改）

然后python脚本中在写几句简单的程序，再去连接指定的这个IP地址和端口(程序如何写，会在后面提及)。

注意：多写几个--out，就多广播几个地址，理论上可以无线广播，然后大家可以接多个python测试脚本！

最终我们就可以实现MP地面站连接着仿真飞控固件，python脚本也能连接到仿真飞控的固件。

然后我们修改python的代码，在命令终端执行就可以调试我的代码了。

二、安装Pymavlink

打开wsl子系统，在ubuntu的任意目录下的命令行终端输入以下命令：

2.1 更新系统包列表

sudo apt-get update

2.2 安装Python 3和pip

sudo apt-get install python3 python3-pip

2.3 安装系统依赖

sudo apt-get install libxml2-dev libxslt-dev

2.4 安装Python依赖

sudo python3 -m pip install --upgrade lxml

2.5 安装pymavlink

sudo python3 -m pip install --upgrade pymavlink

2.6 简单验证是否安装成功

python3 -c "import pymavlink; print('安装成功')"

2.7 运行验证脚本

python3 -c "from pymavlink import mavutil; connection = mavutil.mavlink_connection('udp:localhost:14550'); print('连接已创建')"

三、启动SITL仿真并打开地面站软件

打开wsl子系统，输入以下命令，进入Ardupilot的多旋翼文件夹下。(此处以多旋翼仿真为例)

cd ~
cd ardupilot/ArduCopter/

注：

如果要开启固定翼的仿真，则先进入/ardupilot/ArduPlane文件夹下，

如果要开启无人车船的仿真，则先进入/ardupilot/Rover文件夹下

然后输入开启SITL仿真的指令，注意此命令后面增加一个地址：--out 127.0.0.1:14552

../Tools/autotest/sim_vehicle.py --map --console --out 127.0.0.1:14552

四、写python脚本(实现获取飞控的飞行模式)

4.1 新建一个ceshi1.py文件，用于代码编写。

打开wsl子系统，在任意文件目录下，输入以下命令。

cd ~

然后新建一个名为“mypython”文件夹，用来存放后续编写的python代码，输入以下指令：

mkdir mypython

然后新建一个名为“ceshi1.py”的python文件，输入以下指令：

cd mypython
vim ceshi1.py

此时会弹出以下空白文件界面：

只需按住shift按键 + 回车按键左边的：冒号按键 ，然后按下键盘Q按键即可退出这个黑色框框。

4.2 复制以下代码

#函数功能：实时打印飞控偏航与俯仰角度

# 使用Pymavlink必须要包含这个
from pymavlink import mavutil

 #导入math的库，类似于C头文件，用于后面的数据计算
import math  


# 连接飞控（使用UDP方式，连接127.0.0.1:14552）
master = mavutil.mavlink_connection('udp:127.0.0.1:14552')

# 等待读取心跳包的反馈！
master.wait_heartbeat()

# 如果读到了心跳包，怎么打印已连接成功！
print(f"已成功连接飞控！")


# 程序主循环：实时读取俯仰角度、偏航角度
print("\n实时读取角度程序运行中！可按Ctrl+C结束程序！...")

try:
    while True:

        # 读取ATTITUDE消息（核心：只读取最新的姿态消息）
        msg = master.recv_match(type='ATTITUDE', blocking=True, timeout=1)
        
        if msg:
           # 从飞控Mavlink拿到的msg.pitch和msg.yaw，单位是弧度！所以hi用math.dagrees进行弧度转角度！

	   #将转换后的俯仰角度存到变量pitch中！
            pitch = round(math.degrees(msg.pitch), 2)   

           #将转换后的偏航角度存到变量yaw中！
            yaw = round(math.degrees(msg.yaw) % 360, 2)             

            # 实时打印更新的两个角度数值
            print(f"\r当前飞控俯仰角度为: {pitch}度     当前飞控偏航角度为: {yaw}度", end="", flush=True)

 # 按下按Ctrl+C，结束程序时，打印提示：程序退出
except KeyboardInterrupt:
    print("\n程序退出!")
    master.close()

4.3 粘贴代码，CTRL+S保存代码

粘贴代码，推荐新手朋友直接使用记事本进行编辑！

因为vim还需要按I插入代码，按：wq保存代码，而且无法使用鼠标光标。

(如果你是有一定经验的开发者，请原谅我写的如此傻瓜！因为有很多新手朋友，可能真的不熟悉vim操作或者Vscode都没安装。)

具体步骤如下：

4.4 运行代码

打开wsl子系统终端，进入到前面pyhon代码的存放目录下，输入以下命令

cd ~
cd mypython   #注意这个文件夹，是你前面命名的那个，如果不是这个名称，请自行修改！
ls #查看这个文件夹下的python代码是否存在

#运行代码
python3 ceshi1.py

执行成功后，会看到如下图所示：

4.5 打开mp地面站，确认俯仰与偏航角度解析是否一致！

五、结合Mavlink协议分析python源码。

通过上述python代码，我们成功获取了pitch俯仰角、yaw偏航角度。其核心代码如下：

  msg = master.recv_match(type='ATTITUDE', blocking=True, timeout=1)
       
        if msg:
           # 从飞控Mavlink拿到的msg.pitch和msg.yaw，单位是弧度！所以hi用math.dagrees进行弧度转角度！

	   #将转换后的俯仰角度存到变量pitch中！
            pitch = round(math.degrees(msg.pitch), 2)   

           #将转换后的偏航角度存到变量yaw中！
            yaw = round(math.degrees(msg.yaw) % 360, 2)

从上述代码中，我们可以看到pitch俯仰角和yaw偏航角度都存放在字段ATTITUDE

我们可以查询mavlink官网的通用消息字段表(https://mavlink.io/en/messages/common.html)，对ATTITUDE这个字段进行查看，看看里面都包含了什么？

如下，看完以后大家基本就非常清楚了， 俯仰角数据、偏航角度数据都藏在ID为30的ATTITUDE字段中。

后续要显示什么数据，就先去这里查表，看看我要显示的数据都在哪个特定的ID字段里面。

五、结尾

至此，大家就基本打通了如何使用python脚本与飞控进行通讯了，并且还可通过mavlink对飞控做一些简单的二次开发的验证。

后续，大家就可以通过AI问答，自行发挥了！

比如问AI：

如何通过pymavlink实现对飞行模式的切换？

如何获取返航高度的参数值？等等！

相关文档

MP地面站SITL仿真使用的一些小技巧如何编译Missionplanner1.3.83地面站？Ardupilot源码下载与编译