开源无人机MAVProxy代码赏析以及改进

作为olivia项目的集成部分负责人，说白了就是需要每部分的代码都懂一些，为了折腾我们这套数传/3G/卫星的之前写的前后台链接，之前用twisted+tornado搭了套异步服务器＋web panel，又写了部分unity3d前端代码。

本来该把整个通信服务全部扔给学弟自己去专注下直升机飞控算法以及ros什么的集成还有机械设计什么的（对，我就是除了画电路板什么都做的那个），还是觉得自己之前写的代码太渣。加之小伙伴去美帝iGEM了。最近自己仍然处于一点不变的失恋状况，睡不着起来研究了下mavproxy，发现这个玩意存在一些问题。

MAVProxy简介

MAVProxy是无人机系统下MAVLink协议的一种代理实现，其中包含了一定量的GCS的实现，以MAVProxy为参考并复用部分代码来搭建我们的server无疑是一个不错的主意。

根据笔者的一些探索，MAVProxy确实可以算的上粗制滥造的作品，重点在于核心部分等。

MAVProxy的代码结构简析

MAVProxy由一堆模块和核心组成，MAVProxy的核心本质是通信，所有对于MAVProxy的分析应该从通信的角度入手

MAVProxy的通信部分很神奇，其调用了pymavlink，

#MAVProxy/mavproxy_link.py
def link_add(self, device):
    '''add new link'''
    try:
        conn = mavutil.mavlink_connection(device, autoreconnect=True, baud=self.settings.baudrate)
    except Exception as msg:
        print("Failed to connect to %s : %s" % (device, msg))
        return False
    if self.settings.rtscts:
        conn.set_rtscts(True)
    conn.linknum = len(self.mpstate.mav_master)
    conn.mav.set_callback(self.master_callback, conn)
    if hasattr(conn.mav, 'set_send_callback'):
        conn.mav.set_send_callback(self.master_send_callback, conn)
        ...
        self.mpstate.mav_master.append(conn)
        ...

其中mavutil.mavlink_connection来源于pymavlink

用于产生一个到无人机系统的链接，那么在pymavlink里面这段代码是怎么写的呢

#pymavlink/mavutil.py
def mavlink_connection(device, baud=115200, source_system=255,
                   planner_format=None, write=False, append=False,
                   robust_parsing=True, notimestamps=False, input=True,
                   dialect=None, autoreconnect=False, zero_time_base=False):
'''open a serial, UDP, TCP or file mavlink connection'''
    if dialect is not None:
    set_dialect(dialect)
    if device.startswith('tcp:'):
    return mavtcp(device[4:], source_system=source_system)
    if device.startswith('udp:'):
    return mavudp(device[4:], input=input, source_system=source_system)

简介明了，其实就是识别下我们用的是什么device，然后适配到协议上去。

我们以最简单的udp部分为例子看下其代码

class mavudp(mavfile):
'''a UDP mavlink socket'''
def __init__(self, device, input=True, broadcast=False, source_system=255):
    a = device.split(':')
    if len(a) != 2:
        print("UDP ports must be specified as host:port")
        sys.exit(1)
    self.port = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    self.udp_server = input
    if input:
        self.port.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        self.port.bind((a[0], int(a[1])))
    else:
        self.destination_addr = (a[0], int(a[1]))
        if broadcast:
            self.port.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
    set_close_on_exec(self.port.fileno())
    self.port.setblocking(0)
    self.last_address = None
    mavfile.__init__(self, self.port.fileno(), device, source_system=source_system, input=input)

def close(self):
    self.port.close()

def recv(self,n=None):
    try:
        data, self.last_address = self.port.recvfrom(300)
    except socket.error as e:
        if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK, errno.ECONNREFUSED ]:
            return ""
        raise
    return data

def write(self, buf):
    try:
        if self.udp_server:
            if self.last_address:
                self.port.sendto(buf, self.last_address)
        else:
            self.port.sendto(buf, self.destination_addr)
    except socket.error:
        pass

def recv_msg(self):
    '''message receive routine for UDP link'''
    self.pre_message()
    s = self.recv()
    if len(s) == 0:
        return None
    if self.first_byte:
        self.auto_mavlink_version(s)
    msg = self.mav.parse_buffer(s)
    if msg is not None:
        for m in msg:
            self.post_message(m)
        return msg[0]
    return None


   def post_message(self, msg):
    '''default post message call'''
    msg._timestamp = time.time()
    type = msg.get_type()
    self.messages[type] = msg
    self.timestamp = msg._timestamp
    if type == 'HEARTBEAT':
        self.target_system = msg.get_srcSystem()
        self.target_component = msg.get_srcComponent()
        if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
            self.flightmode = mode_string_v10(msg)
    elif type == 'PARAM_VALUE':
        self.params[str(msg.param_id)] = msg.param_value
        if msg.param_index+1 == msg.param_count:
            self.param_fetch_in_progress = False
            self.param_fetch_complete = True
    elif type == 'SYS_STATUS' and mavlink.WIRE_PROTOCOL_VERSION == '0.9':
        self.flightmode = mode_string_v09(msg)
    elif type == 'GPS_RAW':
        if self.messages['HOME'].fix_type < 2:
            self.messages['HOME'] = msg
    for hook in self.message_hooks:
        hook(self, msg)

有趣的在其初始化代码里面，init函数没有另开线程的可能，查阅了其父类的代码同样如此。

我们可以看到recv这个函数是检查是否有数据－有数据则处理输入，这个玩意显然应该被轮询调用，而其中post_message是用来把包内容更新到内存的。两个函数互相没有调用就很奇怪，这和笔者对于pymavlink具有独立运行认知不符（没有独立运行能力干嘛要做udp适配器）。

可是在pymavlink里面居然完全没有call过这两个函数，回头翻下mavproxy的代码

有了惊人的发现

def process_master(m):
    '''process packets from the MAVLink master'''
    try:
     s = m.recv(16*1024)
        #s = ""

    except Exception:
        time.sleep(0.1)
        return
    ...
    if msgs:
        for msg in msgs:
            if getattr(m, '_timestamp', None) is None:
                m.post_message(msg)

代码里面的注释是笔者加的，看起来这个处理函数完成喊pymavlink跑去要数据的任务，而且这个time.sleep(0.1)是在逗我么。笔者的预判一直是pymavlink至少得有独自的线程吧，敢情就是一个解析器加了对于几种信号很烂的适配啊。

果然这个函数在主循环里面被调用

def main_loop():
    '''main processing loop'''
    ...
    for master in mpstate.mav_master:
        if master.fd is None:
            if master.port.inWaiting() > 0:
                process_master(master)
    ...

看到这里，笔者觉得卧槽这个让我觉得神秘了半年不敢碰的代码原来逼格这么低。。居然是阻塞的和无人机通信。测了下如果把 s=m.recv()注释掉，果然提示为link down

代码翻到这里结合下下面的main函数内容，

if __name__ == '__main__':
    from optparse import OptionParser
    parser = OptionParser("mavproxy.py [options]")

    parser.add_option("--master", dest="master", action='append',
                  metavar="DEVICE[,BAUD]", help="MAVLink master port and optional baud rate",
...(此处一堆opt)
    #加载链接的socket到内存
    load_module('link', quiet=True)
    #扔一个线程来处理UAVs
    mpstate.status.thread =threading.Thread(target=main_loop)
    mpstate.status.thread.daemon = True
    mpstate.status.thread.start()

    #下面是一个文字控制台
    while (mpstate.status.exit != True):
    try:
        input_loop()
    except KeyboardInterrupt:
        if mpstate.settings.requireexit:
            print("Interrupt caught.  Use 'exit' to quit MAVProxy.")

大致如此。

一些吐槽

数据融合

之前对于mavproxy的数据融合报以厚望，毕竟名字叫代理嘛！然后我看了下代码。这个是对于从一路无人机发射给多个控制台收到的控制的“融合”代码

def main_loop():
...
    for master in mpstate.mav_master:
        if master.fd is None:
            if master.port.inWaiting() > 0:
                process_master(master)
...


def process_mavlink(slave):
'''process packets from MAVLink slaves, forwarding to the master'''
    try:
        buf = slave.recv()
    except socket.error:
        return
    try:
        if slave.first_byte and opts.auto_protocol:
            slave.auto_mavlink_version(buf)
        msgs = slave.mav.parse_buffer(buf)
    except mavutil.mavlink.MAVError as e:
        mpstate.console.error("Bad MAVLink slave message from %s: %s" % (slave.address, e.message))
        return
    if msgs is None:
        return
    if mpstate.settings.mavfwd and not mpstate.status.setup_mode:
        for m in msgs:
            mpstate.master().write(m.get_msgbuf())
mpstate.status.counters['Slave'] += 1

原来就是循环了下几个输出，拿到能用得到的包直接写给无人机啊！线程锁哭晕在厕所，

改进

下面我们要把pymavlink挂到我的基于twisted 的oliviaserver无人机服务器中，之前采用的是mavproxy挂模块socket的方法未免不够优雅，现在直接用twisted重写pymavlink的底层通信是一个不错的思路，但是这样mavproxy写好的种种福利还要自己手写一遍，令人十分的心痛。

当然更加简单的思路是直接复用mavproxy的代码，重写其通信部分，再用websocket做个web命令行，可是mavproxy对于多无人机的管理对于我来说是冗余的。

思前想后，还是直接在mavproxy上面改吧，省了许多麻烦事儿。

主要要做的工作应该是

首先使用一个twisted udp协议来异步化下我们可爱的数据输入，即m.recv()。需要写个pymavlink.mavutil.mavfile的子类，
- 将mavproxy的飞行器姿态等数据结合到我们的可爱的olivia无人机obj中，前后台需要改一堆数据
- 让美工妹子给我好好画一个控制台 web panel

开源无人机MAVProxy代码赏析以及改进

MAVProxy简介

MAVProxy的代码结构简析

一些吐槽

数据融合

改进