{"id":533,"date":"2014-10-29T06:08:55","date_gmt":"2014-10-28T22:08:55","guid":{"rendered":"http:\/\/blog.xuhao1.me\/?p=533"},"modified":"2014-12-13T14:09:13","modified_gmt":"2014-12-13T06:09:13","slug":"mavproxy%e7%a8%8b%e5%ba%8f%e7%bb%93%e6%9e%84%e4%bb%a5%e5%8f%8a%e6%94%b9%e8%bf%9b","status":"publish","type":"post","link":"http:\/\/blog.xuhao1.me\/?p=533","title":{"rendered":"\u5f00\u6e90\u65e0\u4eba\u673aMAVProxy\u4ee3\u7801\u8d4f\u6790\u4ee5\u53ca\u6539\u8fdb"},"content":{"rendered":"

\u4f5c\u4e3aolivia\u9879\u76ee\u7684\u96c6\u6210\u90e8\u5206\u8d1f\u8d23\u4eba\uff0c\u8bf4\u767d\u4e86\u5c31\u662f\u9700\u8981\u6bcf\u90e8\u5206\u7684\u4ee3\u7801\u90fd\u61c2\u4e00\u4e9b\uff0c\u4e3a\u4e86\u6298\u817e\u6211\u4eec\u8fd9\u5957\u6570\u4f20\/3G\/\u536b\u661f\u7684\u4e4b\u524d\u5199\u7684\u524d\u540e\u53f0\u94fe\u63a5\uff0c\u4e4b\u524d\u7528twisted+tornado\u642d\u4e86\u5957\u5f02\u6b65\u670d\u52a1\u5668\uff0bweb panel\uff0c\u53c8\u5199\u4e86\u90e8\u5206unity3d\u524d\u7aef\u4ee3\u7801\u3002
\n<\/p>\n

\u672c\u6765\u8be5\u628a\u6574\u4e2a\u901a\u4fe1\u670d\u52a1\u5168\u90e8\u6254\u7ed9\u5b66\u5f1f\u81ea\u5df1\u53bb\u4e13\u6ce8\u4e0b\u76f4\u5347\u673a\u98de\u63a7\u7b97\u6cd5\u4ee5\u53caros\u4ec0\u4e48\u7684\u96c6\u6210\u8fd8\u6709\u673a\u68b0\u8bbe\u8ba1\u4ec0\u4e48\u7684\uff08\u5bf9\uff0c\u6211\u5c31\u662f\u9664\u4e86\u753b\u7535\u8def\u677f\u4ec0\u4e48\u90fd\u505a\u7684\u90a3\u4e2a\uff09\uff0c\u8fd8\u662f\u89c9\u5f97\u81ea\u5df1\u4e4b\u524d\u5199\u7684\u4ee3\u7801\u592a\u6e23\u3002\u52a0\u4e4b\u5c0f\u4f19\u4f34\u53bb\u7f8e\u5e1diGEM\u4e86\u3002\u6700\u8fd1\u81ea\u5df1\u4ecd\u7136\u5904\u4e8e\u4e00\u70b9\u4e0d\u53d8\u7684\u5931\u604b\u72b6\u51b5\uff0c\u7761\u4e0d\u7740\u8d77\u6765\u7814\u7a76\u4e86\u4e0bmavproxy\uff0c\u53d1\u73b0\u8fd9\u4e2a\u73a9\u610f\u5b58\u5728\u4e00\u4e9b\u95ee\u9898\u3002<\/p>\n

MAVProxy\u7b80\u4ecb<\/h2>\n

MAVProxy<\/a>\u662f\u65e0\u4eba\u673a\u7cfb\u7edf\u4e0bMAVLink\u534f\u8bae\u7684\u4e00\u79cd\u4ee3\u7406\u5b9e\u73b0\uff0c\u5176\u4e2d\u5305\u542b\u4e86\u4e00\u5b9a\u91cf\u7684GCS\u7684\u5b9e\u73b0\uff0c\u4ee5MAVProxy\u4e3a\u53c2\u8003\u5e76\u590d\u7528\u90e8\u5206\u4ee3\u7801\u6765\u642d\u5efa\u6211\u4eec\u7684server\u65e0\u7591\u662f\u4e00\u4e2a\u4e0d\u9519\u7684\u4e3b\u610f\u3002<\/p>\n

\u6839\u636e\u7b14\u8005\u7684\u4e00\u4e9b\u63a2\u7d22\uff0cMAVProxy\u786e\u5b9e\u53ef\u4ee5\u7b97\u7684\u4e0a\u7c97\u5236\u6ee5\u9020\u7684\u4f5c\u54c1\uff0c\u91cd\u70b9\u5728\u4e8e\u6838\u5fc3\u90e8\u5206\u7b49\u3002<\/p>\n

MAVProxy\u7684\u4ee3\u7801\u7ed3\u6784\u7b80\u6790<\/h2>\n

MAVProxy\u7531\u4e00\u5806\u6a21\u5757\u548c\u6838\u5fc3\u7ec4\u6210\uff0cMAVProxy\u7684\u6838\u5fc3\u672c\u8d28\u662f\u901a\u4fe1\uff0c\u6240\u6709\u5bf9\u4e8eMAVProxy\u7684\u5206\u6790\u5e94\u8be5\u4ece\u901a\u4fe1\u7684\u89d2\u5ea6\u5165\u624b<\/p>\n

MAVProxy\u7684\u901a\u4fe1\u90e8\u5206\u5f88\u795e\u5947\uff0c\u5176\u8c03\u7528\u4e86pymavlink\uff0c<\/p>\n

#MAVProxy\/mavproxy_link.py\ndef link_add(self, device):\n    '''add new link'''\n    try:\n        conn = mavutil.mavlink_connection(device, autoreconnect=True, baud=self.settings.baudrate)\n    except Exception as msg:\n        print(\"Failed to connect to %s : %s\" % (device, msg))\n        return False\n    if self.settings.rtscts:\n        conn.set_rtscts(True)\n    conn.linknum = len(self.mpstate.mav_master)\n    conn.mav.set_callback(self.master_callback, conn)\n    if hasattr(conn.mav, 'set_send_callback'):\n        conn.mav.set_send_callback(self.master_send_callback, conn)\n        ...\n        self.mpstate.mav_master.append(conn)\n        ...\n<\/code><\/pre>\n
\u5176\u4e2dmavutil.mavlink_connection<\/strong>\u6765\u6e90\u4e8epymavlink<\/p>\n
\u7528\u4e8e\u4ea7\u751f\u4e00\u4e2a\u5230\u65e0\u4eba\u673a\u7cfb\u7edf\u7684\u94fe\u63a5\uff0c\u90a3\u4e48\u5728pymavlink\u91cc\u9762\u8fd9\u6bb5\u4ee3\u7801\u662f\u600e\u4e48\u5199\u7684\u5462<\/p>\n
#pymavlink\/mavutil.py\ndef mavlink_connection(device, baud=115200, source_system=255,\n                   planner_format=None, write=False, append=False,\n                   robust_parsing=True, notimestamps=False, input=True,\n                   dialect=None, autoreconnect=False, zero_time_base=False):\n'''open a serial, UDP, TCP or file mavlink connection'''\n    if dialect is not None:\n    set_dialect(dialect)\n    if device.startswith('tcp:'):\n    return mavtcp(device[4:], source_system=source_system)\n    if device.startswith('udp:'):\n    return mavudp(device[4:], input=input, source_system=source_system)\n<\/code><\/pre>\n
\u7b80\u4ecb\u660e\u4e86\uff0c\u5176\u5b9e\u5c31\u662f\u8bc6\u522b\u4e0b\u6211\u4eec\u7528\u7684\u662f\u4ec0\u4e48device\uff0c\u7136\u540e\u9002\u914d\u5230\u534f\u8bae\u4e0a\u53bb\u3002<\/p>\n
\u6211\u4eec\u4ee5\u6700\u7b80\u5355\u7684udp\u90e8\u5206\u4e3a\u4f8b\u5b50\u770b\u4e0b\u5176\u4ee3\u7801<\/p>\n
class mavudp(mavfile):\n'''a UDP mavlink socket'''\ndef __init__(self, device, input=True, broadcast=False, source_system=255):\n    a = device.split(':')\n    if len(a) != 2:\n        print(\"UDP ports must be specified as host:port\")\n        sys.exit(1)\n    self.port = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)\n    self.udp_server = input\n    if input:\n        self.port.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)\n        self.port.bind((a[0], int(a[1])))\n    else:\n        self.destination_addr = (a[0], int(a[1]))\n        if broadcast:\n            self.port.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)\n    set_close_on_exec(self.port.fileno())\n    self.port.setblocking(0)\n    self.last_address = None\n    mavfile.__init__(self, self.port.fileno(), device, source_system=source_system, input=input)\n\ndef close(self):\n    self.port.close()\n\ndef recv(self,n=None):\n    try:\n        data, self.last_address = self.port.recvfrom(300)\n    except socket.error as e:\n        if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK, errno.ECONNREFUSED ]:\n            return \"\"\n        raise\n    return data\n\ndef write(self, buf):\n    try:\n        if self.udp_server:\n            if self.last_address:\n                self.port.sendto(buf, self.last_address)\n        else:\n            self.port.sendto(buf, self.destination_addr)\n    except socket.error:\n        pass\n\ndef recv_msg(self):\n    '''message receive routine for UDP link'''\n    self.pre_message()\n    s = self.recv()\n    if len(s) == 0:\n        return None\n    if self.first_byte:\n        self.auto_mavlink_version(s)\n    msg = self.mav.parse_buffer(s)\n    if msg is not None:\n        for m in msg:\n            self.post_message(m)\n        return msg[0]\n    return None\n\n\n   def post_message(self, msg):\n    '''default post message call'''\n    msg._timestamp = time.time()\n    type = msg.get_type()\n    self.messages[type] = msg\n    self.timestamp = msg._timestamp\n    if type == 'HEARTBEAT':\n        self.target_system = msg.get_srcSystem()\n        self.target_component = msg.get_srcComponent()\n        if mavlink.WIRE_PROTOCOL_VERSION == '1.0':\n            self.flightmode = mode_string_v10(msg)\n    elif type == 'PARAM_VALUE':\n        self.params[str(msg.param_id)] = msg.param_value\n        if msg.param_index+1 == msg.param_count:\n            self.param_fetch_in_progress = False\n            self.param_fetch_complete = True\n    elif type == 'SYS_STATUS' and mavlink.WIRE_PROTOCOL_VERSION == '0.9':\n        self.flightmode = mode_string_v09(msg)\n    elif type == 'GPS_RAW':\n        if self.messages['HOME'].fix_type < 2:\n            self.messages['HOME'] = msg\n    for hook in self.message_hooks:\n        hook(self, msg)\n<\/code><\/pre>\n
\u6709\u8da3\u7684\u5728\u5176\u521d\u59cb\u5316\u4ee3\u7801\u91cc\u9762\uff0cinit\u51fd\u6570\u6ca1\u6709\u53e6\u5f00\u7ebf\u7a0b\u7684\u53ef\u80fd\uff0c\u67e5\u9605\u4e86\u5176\u7236\u7c7b\u7684\u4ee3\u7801\u540c\u6837\u5982\u6b64\u3002<\/p>\n
\u6211\u4eec\u53ef\u4ee5\u770b\u5230recv\u8fd9\u4e2a\u51fd\u6570\u662f\u68c0\u67e5\u662f\u5426\u6709\u6570\u636e\uff0d\u6709\u6570\u636e\u5219\u5904\u7406\u8f93\u5165\uff0c\u8fd9\u4e2a\u73a9\u610f\u663e\u7136\u5e94\u8be5\u88ab\u8f6e\u8be2\u8c03\u7528\uff0c\u800c\u5176\u4e2dpost_message<\/em>\u662f\u7528\u6765\u628a\u5305\u5185\u5bb9\u66f4\u65b0\u5230\u5185\u5b58\u7684\u3002\u4e24\u4e2a\u51fd\u6570\u4e92\u76f8\u6ca1\u6709\u8c03\u7528\u5c31\u5f88\u5947\u602a\uff0c\u8fd9\u548c\u7b14\u8005\u5bf9\u4e8epymavlink\u5177\u6709\u72ec\u7acb\u8fd0\u884c\u8ba4\u77e5\u4e0d\u7b26\uff08\u6ca1\u6709\u72ec\u7acb\u8fd0\u884c\u80fd\u529b\u5e72\u561b\u8981\u505audp\u9002\u914d\u5668\uff09\u3002<\/p>\n
\u53ef\u662f\u5728pymavlink\u91cc\u9762\u5c45\u7136\u5b8c\u5168\u6ca1\u6709call\u8fc7\u8fd9\u4e24\u4e2a\u51fd\u6570\uff0c\u56de\u5934\u7ffb\u4e0bmavproxy\u7684\u4ee3\u7801<\/p>\n
\u6709\u4e86\u60ca\u4eba\u7684\u53d1\u73b0<\/p>\n
def process_master(m):\n    '''process packets from the MAVLink master'''\n    try:\n     s = m.recv(16*1024)\n        #s = \"\"\n\n    except Exception:\n        time.sleep(0.1)\n        return\n    ...\n    if msgs:\n        for msg in msgs:\n            if getattr(m, '_timestamp', None) is None:\n                m.post_message(msg)\n<\/code><\/pre>\n
\u4ee3\u7801\u91cc\u9762\u7684\u6ce8\u91ca\u662f\u7b14\u8005\u52a0\u7684\uff0c\u770b\u8d77\u6765\u8fd9\u4e2a\u5904\u7406\u51fd\u6570\u5b8c\u6210\u558apymavlink\u8dd1\u53bb\u8981\u6570\u636e\u7684\u4efb\u52a1\uff0c\u800c\u4e14\u8fd9\u4e2atime.sleep(0.1)<\/strong>\u662f\u5728\u9017\u6211\u4e48\u3002\u7b14\u8005\u7684\u9884\u5224\u4e00\u76f4\u662fpymavlink\u81f3\u5c11\u5f97\u6709\u72ec\u81ea\u7684\u7ebf\u7a0b\u5427\uff0c\u6562\u60c5\u5c31\u662f\u4e00\u4e2a\u89e3\u6790\u5668\u52a0\u4e86\u5bf9\u4e8e\u51e0\u79cd\u4fe1\u53f7\u5f88\u70c2\u7684\u9002\u914d\u554a\u3002<\/p>\n
\u679c\u7136\u8fd9\u4e2a\u51fd\u6570\u5728\u4e3b\u5faa\u73af\u91cc\u9762\u88ab\u8c03\u7528<\/p>\n
def main_loop():\n    '''main processing loop'''\n    ...\n    for master in mpstate.mav_master:\n        if master.fd is None:\n            if master.port.inWaiting() > 0:\n                process_master(master)\n    ...\n<\/code><\/pre>\n
\u770b\u5230\u8fd9\u91cc\uff0c\u7b14\u8005\u89c9\u5f97\u5367\u69fd\u8fd9\u4e2a\u8ba9\u6211\u89c9\u5f97\u795e\u79d8\u4e86\u534a\u5e74\u4e0d\u6562\u78b0\u7684\u4ee3\u7801\u539f\u6765\u903c\u683c\u8fd9\u4e48\u4f4e\u3002\u3002\u5c45\u7136\u662f\u963b\u585e\u7684\u548c\u65e0\u4eba\u673a\u901a\u4fe1\u3002\u6d4b\u4e86\u4e0b\u5982\u679c\u628a s=m.recv()<\/em>\u6ce8\u91ca\u6389\uff0c\u679c\u7136\u63d0\u793a\u4e3alink down<\/p>\n
\u4ee3\u7801\u7ffb\u5230\u8fd9\u91cc\u7ed3\u5408\u4e0b\u4e0b\u9762\u7684main\u51fd\u6570\u5185\u5bb9\uff0c<\/p>\n
if __name__ == '__main__':\n    from optparse import OptionParser\n    parser = OptionParser(\"mavproxy.py [options]\")\n\n    parser.add_option(\"--master\", dest=\"master\", action='append',\n                  metavar=\"DEVICE[,BAUD]\", help=\"MAVLink master port and optional baud rate\",\n...(\u6b64\u5904\u4e00\u5806opt)\n    #\u52a0\u8f7d\u94fe\u63a5\u7684socket\u5230\u5185\u5b58\n    load_module('link', quiet=True)\n    #\u6254\u4e00\u4e2a\u7ebf\u7a0b\u6765\u5904\u7406UAVs\n    mpstate.status.thread =threading.Thread(target=main_loop)\n    mpstate.status.thread.daemon = True\n    mpstate.status.thread.start()\n\n    #\u4e0b\u9762\u662f\u4e00\u4e2a\u6587\u5b57\u63a7\u5236\u53f0\n    while (mpstate.status.exit != True):\n    try:\n        input_loop()\n    except KeyboardInterrupt:\n        if mpstate.settings.requireexit:\n            print(\"Interrupt caught.  Use 'exit' to quit MAVProxy.\")\n<\/code><\/pre>\n
\u5927\u81f4\u5982\u6b64\u3002<\/p>\n
\u4e00\u4e9b\u5410\u69fd<\/h2>\n
\u6570\u636e\u878d\u5408<\/h3>\n
\u4e4b\u524d\u5bf9\u4e8emavproxy\u7684\u6570\u636e\u878d\u5408\u62a5\u4ee5\u539a\u671b\uff0c\u6bd5\u7adf\u540d\u5b57\u53eb\u4ee3\u7406\u561b\uff01\u7136\u540e\u6211\u770b\u4e86\u4e0b\u4ee3\u7801\u3002\u8fd9\u4e2a\u662f\u5bf9\u4e8e\u4ece\u4e00\u8def\u65e0\u4eba\u673a\u53d1\u5c04\u7ed9\u591a\u4e2a\u63a7\u5236\u53f0\u6536\u5230\u7684\u63a7\u5236\u7684\u201c\u878d\u5408\u201d\u4ee3\u7801<\/p>\n
def main_loop():\n...\n    for master in mpstate.mav_master:\n        if master.fd is None:\n            if master.port.inWaiting() > 0:\n                process_master(master)\n...\n\n\ndef process_mavlink(slave):\n'''process packets from MAVLink slaves, forwarding to the master'''\n    try:\n        buf = slave.recv()\n    except socket.error:\n        return\n    try:\n        if slave.first_byte and opts.auto_protocol:\n            slave.auto_mavlink_version(buf)\n        msgs = slave.mav.parse_buffer(buf)\n    except mavutil.mavlink.MAVError as e:\n        mpstate.console.error(\"Bad MAVLink slave message from %s: %s\" % (slave.address, e.message))\n        return\n    if msgs is None:\n        return\n    if mpstate.settings.mavfwd and not mpstate.status.setup_mode:\n        for m in msgs:\n            mpstate.master().write(m.get_msgbuf())\nmpstate.status.counters['Slave'] += 1\n<\/code><\/pre>\n
\u539f\u6765\u5c31\u662f\u5faa\u73af\u4e86\u4e0b\u51e0\u4e2a\u8f93\u51fa\uff0c\u62ff\u5230\u80fd\u7528\u5f97\u5230\u7684\u5305\u76f4\u63a5\u5199\u7ed9\u65e0\u4eba\u673a\u554a\uff01\u7ebf\u7a0b\u9501\u54ed\u6655\u5728\u5395\u6240\uff0c<\/p>\n
\u6539\u8fdb<\/h2>\n
\u4e0b\u9762\u6211\u4eec\u8981\u628apymavlink\u6302\u5230\u6211\u7684\u57fa\u4e8etwisted \u7684oliviaserver\u65e0\u4eba\u673a\u670d\u52a1\u5668\u4e2d\uff0c\u4e4b\u524d\u91c7\u7528\u7684\u662fmavproxy\u6302\u6a21\u5757socket\u7684\u65b9\u6cd5\u672a\u514d\u4e0d\u591f\u4f18\u96c5\uff0c\u73b0\u5728\u76f4\u63a5\u7528twisted\u91cd\u5199pymavlink\u7684\u5e95\u5c42\u901a\u4fe1\u662f\u4e00\u4e2a\u4e0d\u9519\u7684\u601d\u8def\uff0c\u4f46\u662f\u8fd9\u6837mavproxy\u5199\u597d\u7684\u79cd\u79cd\u798f\u5229\u8fd8\u8981\u81ea\u5df1\u624b\u5199\u4e00\u904d\uff0c\u4ee4\u4eba\u5341\u5206\u7684\u5fc3\u75db\u3002<\/p>\n
\u5f53\u7136\u66f4\u52a0\u7b80\u5355\u7684\u601d\u8def\u662f\u76f4\u63a5\u590d\u7528mavproxy\u7684\u4ee3\u7801\uff0c\u91cd\u5199\u5176\u901a\u4fe1\u90e8\u5206\uff0c\u518d\u7528websocket\u505a\u4e2aweb\u547d\u4ee4\u884c\uff0c\u53ef\u662fmavproxy\u5bf9\u4e8e\u591a\u65e0\u4eba\u673a\u7684\u7ba1\u7406\u5bf9\u4e8e\u6211\u6765\u8bf4\u662f\u5197\u4f59\u7684\u3002<\/p>\n
\u601d\u524d\u60f3\u540e\uff0c\u8fd8\u662f\u76f4\u63a5\u5728mavproxy\u4e0a\u9762\u6539\u5427\uff0c\u7701\u4e86\u8bb8\u591a\u9ebb\u70e6\u4e8b\u513f\u3002<\/p>\n
\u4e3b\u8981\u8981\u505a\u7684\u5de5\u4f5c\u5e94\u8be5\u662f<\/p>\n