计算机控制apm飞控自动飞行

摘要

使用mavproxy(mavlink)控制ArduCopter飞控无人机自动起飞到1米高度然后前进1m然后降落.

关键信息

• 飞控固件:ArduCopter 4.2.0
• 飞控MAVProxy 1.8.70
• PyMavlink 2.4.41

原理简介

mavproxy简介

[https://ardupilot.org/dev/docs/copter-sitl-mavproxy-tutorial.html]
[https://github.com/ArduPilot/ardupilot_wiki/blob/master/mavproxy/source/docs/modules/cmdlong.rst]
[https://pypi.org/project/MAVProxy/]
[https://ardupilot.org/mavproxy/index.html]
A MAVLink protocol proxy and ground station. MAVProxy is oriented towards command line operation, and is suitable for embedding in small autonomous vehicles or for using on ground control stations. It also features a number of graphical tools such as a slipmap for satellite mapping view of the vehicles location, and status console and several useful vehicle control modules. MAVProxy is extensible via a modules system - see the modules subdirectory for some example modules. MAVProxy was developed by CanberraUAV for use in the 2012 Outback Challenge, and includes a module for the CanberraUAV search and rescue system.

MAVProxy is a fully-functioning GCS for UAV’s, designed as a minimalist, portable and extendable GCS for any autonomous system supporting the MAVLink protocol (such as one using ArduPilot). MAVProxy is a powerful command-line based “developer” ground station software. It can be extended via add-on modules, or complemented with another ground station, such as Mission Planner, APM Planner 2, QGroundControl etc, to provide a graphical user interface.

It has a number of key features, including the ability to forward the messages from your UAV over the network via UDP to multiple other ground station software on other devices.

MAVProxy is commonly used by developers (especially with SITL) for testing new builds.

MAVProxy was first developed by CanberraUAV, to enable the use of companion computing and multiple datalinks with ArduPilot. It has grown to be one of the most versatile tools in the ArduPilot ecosystem, and many of the features users now see in other GCS tools can trace their origins to MAVProxy.

MAVLink协议代理和地面站。MAVProxy面向命令行操作，适用于小型自主车辆或地面控制站的嵌入使用。它还提供了许多图形工具，例如slipmap卫星地图查看车辆位置，状态控制台和几个有用的车辆控制模块。MAVProxy可以通过模块系统进行扩展 - 在模块子目录中查看一些示例模块。 MAVProxy由CanberraUAV为2012 Outback Challenge开发，包括用于CanberraUAV搜索和救援系统的模块。
MAVProxy是一个全功能的UAV GCS，被设计为一个简约，便携且可扩展的GCS，适用于支持MAVLink协议的任何自主系统（例如使用ArduPilot的系统）。MAVProxy是一种功能强大的基于命令行的“开发人员”地面站软件。它可以通过附加模块进行扩展，或者与另一个地面站（如Mission Planner，APM Planner 2，QGroundControl等）结合使用，以提供图形用户界面。
它具有许多关键功能，包括能够通过网络上的UDP将您的UAV的消息转发到其他设备上的其他地面站软件。
MAVProxy通常被开发人员（尤其是在SITL中使用）用于测试新版本。
MAVProxy最初是由CanberraUAV开发的，用于启用伴随式计算和与ArduPilot的多个数据链路。它已发展成为ArduPilot生态系统中最多才多艺的工具之一，许多用户在其他GCS工具中看到的特性都可以追溯到MAVProxy。

pexpect简介

[https://www.cnblogs.com/bmjoker/p/10583809.html]
pexpect是一个通过启动子程序，使用正则表达式对程序输出做出特定响应，以此实现与其自动交互的python模块。
使用python来作自动化登陆，并执行命令

pexpect模块的使用偏向于偏交互式的使用，如：管理员需要登陆100台机器，每次都会手动执行不同的命令，如果通过ssh的命令进行登陆，这样每次输入不同的密码，用户，主机地址等信息会是一个不小的工作量，用pexpect这个模块，就可以解决问题。
pexpect可以理解成Linux下的expect的python封装，通过expect我们可以实现ssh，ftp，telnet等命令行进行自动交互，包括输入主机地址，用户名，密码，上传文件等，待出现异常我们还可以进行尝试自动处理。

实现

文件下载

我用夸克网盘分享了「pymavlink-2.4.41-py3-none-any.whl等文件」，点击链接即可保存。
链接：[https://pan.quark.cn/s/594c07c2ce0f]
提取码：fvsq

安装mavproxy

pip install -i https://pypi.tuna.tsinghua.edu.cn/simple future
pip config set global.index-url https://pypi.tuna.tsinghua.edu.cn/simple
pip install -i https://pypi.tuna.tsinghua.edu.cn/simple pexpect
pip3 install "./pymavlink-2.4.41-py3-none-any.whl" --user
pip3 install "./MAVProxy-1.8.70-py3-none-any.whl" --user
# 测试
mavproxy.py --master=/dev/ttyACM0 --baudrate=57600

核心命令

mavproxy.py --master=/dev/ttyACM0 --baudrate=57600

# 切换到guided模式
mode guided

# 解锁
arm throttle

# 起飞到1m并悬停
# 必须在解锁后15s内发送起飞命令,否则就会重新锁定电机
takeoff 1

# 锁定
disarm throttle

# 降落
mode loiter

# 重启
reboot

# 绕圆圈
param set circle_radius 0.5
mode circle

# 前进1
position 1 0 0
# 后退1
position -1 0 0

# 左1
position 0 -1 0
# 右1
position 0 1 0

# 升1
position 0 0 1
# 降1
position 0 0 -1

# 右转90度
# setyaw ANGLE ANGULAR_SPEED MODE
setyaw 90 30 1

核心代码

import subprocess # 异步相关
from mavproxy import RaicomMavproxy # mavproxy相关
import pexpect # shell-expect相关
from termcolor import colored # 终端相关
import time # 时间相关
class RaicomUtils:
    @classmethod
    def echoColor(cls,text,status):
        '''需要导入:from termcolor import colored'''
        if status == "ok":
            print(colored("<<<","blue"),colored(text, "white"),colored("[", "yellow"),colored(status, "green"),colored("]", "yellow"))
        elif status == "error":
            print(colored("<<<","red"),colored(text, "white"),colored("[", "yellow"),colored(status, "red"),colored("]", "yellow"))
        elif status == "info":
            print(colored("<<<","blue"),colored(text, "white"),colored("[", "yellow"),colored(status, "light_magenta"),colored("]", "yellow"))
        elif status == "start":
            print(colored(">>>","blue"),colored(text, "white"),colored("[", "yellow"),colored(status, "light_cyan"),colored("]", "yellow"))
        else:
            print(colored(text, "white"))
    @classmethod
    def waitKeyToExit(cls):
        '''等待用户输入特定字符的函数'''
        try:
            while True:
                key = input("按下 'q' 以退出: \n")
                if key.lower() == 'q':
                    print("\nExiting...")
                    exit(0)
        except KeyboardInterrupt:
            print("\nExiting due to KeyboardInterrupt...")
            exit(1)
class RaicomMission():
    @classmethod
    def test4(cls):
        '''纯mavproxy方式实现起飞后悬停5秒然后降落'''
        RaicomUtils.echoColor("自动执行Test#4 蚱蜢跳任务","start")
        # 获取当前脚本所在的目录
        script_dir = os.path.dirname(os.path.abspath(__file__))
        RaicomUtils.echoColor("脚本所在目录:{}".format(script_dir),"info")

        # 启动mavproxy
        child = pexpect.spawn('mavproxy.py --master=/dev/ttyACM0 --baudrate=57600')

        # 切换到guided模式
        child.sendline('mode guided')
        print(child.before)
        child.expect('MAV>')
        print(child.before)

        time.sleep(2)
        # 解锁
        child.sendline('arm throttle')
        print(child.before)
        child.expect('MAV>')
        print(child.before)

        # 起飞到1m并悬停
        child.sendline('takeoff 1')
        print(child.before)
        child.expect('MAV>')
        print(child.before)

        time.sleep(5)

        # 前进1
        child.sendline('position 1 0 0')
        print(child.before)
        child.expect('MAV>')
        print(child.before)

        time.sleep(2)

        # 降1
        child.sendline('position 0 0 -1')
        print(child.before)
        child.expect('MAV>')
        print(child.before)

        time.sleep(2)
        
        child.sendline('mode loiter')
        print(child.before)
        child.expect('MAV>')
        print(child.before)

        time.sleep(2)

        # 锁定
        child.sendline('disarm throttle')
        print(child.before)
        child.expect('MAV>')
        print(child.before)

        # 退出
        child.sendline('exit')
        print(child.before)
        child.expect(pexpect.EOF)
        print(child.before)

        RaicomUtils.echoColor("自动执行Test#4 蚱蜢跳任务","ok")
        pass

效果

无人机飞行