Jarvis's Blog

白帽子、全栈、IoT安全专家、嵌入式系统专家

大疆DJI Robomaster S1机器人通过4G网络远程控制的方法（无需改硬件）

Jarvis ·

　　去年买了一台Robomaster S1机器人，组装完成之后玩了一会觉得只能在路由器的无线信号距离里面玩略有点局限性，有些乏味了，就在想能不能改成4G远程控制机器人呢。这样就可以开出去买个菜啥的？或者在外面控制家里的机器人？

　　网上参考了一些大疆协议的资料，有hack CAN总线相关的方法实现自定义控制的，这种方法的好处就是可以脱离手机app，而且可以实现完全控制机器人的任何一个部件。但这并不是我最想达到的效果。我希望改装完成后的效果是：

  1. 机器人可以通过4G网络被远程控制。
  2. 无需对机器人的硬件做修改。
  3. 控制端仍然可以使用大疆官方的Robomaster App来控制，而无需自己对控制协议进行逆向工程以后再重写一个控制界面。

　　明确了效果以后，那么首先来看看大疆的App是如何控制机器人的吧，最开始实验的时候，我在PC上虚拟了一个无线热点，然后在这个无线热点上抓包，看看App和机器人之间的流量，后来发现什么都没抓到。疑惑了很久，后来才醒悟，同一个WLAN局域网下两个STA之间的通信是直接通过AP中继转发了，而不会被转发到交换机上。所以换了一个可以抓802.11数据包的macbook直接抓无线流量，然后解密802.11数据包，最后才抓到了通信的流量。

如上图，通过观察可以发现，协议其实比较简单，机器人在连入网络之后，会用56789端口向.255广播地址的45678端口发送一个24个字节的UDP广播包，推测是用来告知机器人的一些信息的，手机App就是通过这个地址来探测局域网中的机器人，然后当手机App探测到机器人的存在后，就会通过UDP协议和机器人的10607端口通信。通过观察还可以发现，图传似乎也是通过这个端口实现的。实现并不复杂，所以其实有个最简单的思路那就是我不需要知道协议的具体细节，我只需要做一个UDP中继就好了。

那么思路很简单，这个UDP中继的实现大概分为三部分：

机器人部分：

  1. 机器人需要连接到一个4G路由器，同时最好这个4G路由器能够编程（比如Openwrt操作系统可以自己编译代码进去）
  2. 4G路由器既作为访问公网的网关，本身作为一个伪手机终端，监听到机器人发送的数据包后，打包发送给云端的服务器。
  3. 接收云端服务器发送的来自真实手机的控制指令，然后以自己的身份发送给机器人。

云端部分：

　　云端部分就比较简单了，只需要开启一个TCP Server（注意，由于公网有可能丢包，所以我这里选择了用TCP协议来转发），然后分别接受来自于伪手机和伪机器人发来的流量，转发给对侧，完成中继。

手机App部分：

  1. 手机App部分自由度就比较高了，只需要有WiFi可以上网即可，在这个WiFi网络里需要用另一台PC（其实用路由器本身也可以，但是用另一台PC更方便）虚拟一个伪机器人。
  2. 伪机器人将云端转发的机器人发送的流量以自己的身份发送给手机App。
  3. 同时，伪机器人接收手机发送过来的控制指令，打包转发给云端，由云端转发给真实的机器人，完成控制。

　　思路明确了之后，那么接下来代码其实不难写，不过机器人这边的话，理论上来说最方便的方法就是用另一个独立的终端来模拟伪手机，但是受限于机器人的装载能力，所以最佳方案还是把4G路由器和模拟手机App的功能合二为一。那么如何比较方便的完成这样的工作呢，这里给大家推荐一个产品：

gl.inet有一款4G路由器，里面是openwrt系统，可以自己编译固件，这样就比较方便开发了。

　　大小尺寸也还合适，续航也可以，绑在机器人的顶部正好，而且自带电池供电，还是比较完美的。所以就用他了。

最终，整个架构如下图所示：

代码实现

　　最终实现的代码其实比较简单，我这里为了方便，使用了Tornado框架来管理TCP和UDP连接。但是完成后实际调试的时候发现一个问题，那就是转发的广播包最终不能被App正常识别，也就是说伪造成机器人在另一个网络中发送由真实的机器人发送的广播包，并不能让App识别并发起连接。这一点让我疑惑了很久。最终对RoboMaster App做了一些逆向工程的分析，发现原来45678端口的24个字节的广播包实际上其中包含了机器人在局域网中的IP地址和MAC地址，以及是否处于配对状态的标志。机器人在第一次配对（也就是按下Connect键第一次扫描二维码连接网络的时候）时，会将pair标志位置1，当App在第一次和机器人配对之后，会使用IP地址和AppID来记住机器人，在此之后只有接收到这个IP地址发出的广播包才会识别到机器人，由于我们跨网络，IP地址是有变化的，所以就必须将其转换为伪机器人的IP和MAC地址才行。

所以最终的代码如下：

由于Tornado没有UDPServer的例子，所以我这里参考别人自己写了一个：

#!/usr/bin/env python3
# coding=utf-8
# File: udpserver.py

import socket
import os
import errno
from tornado.ioloop import IOLoop
from tornado import process
from tornado.platform.auto import set_close_exec


def bind_sockets(port, address=None, family=socket.AF_UNSPEC, backlog=25):
    sockets = []
    if address == "":
        address = None
    flags = socket.AI_PASSIVE
    if hasattr(socket, "AI_ADDRCONFIG"):
        flags |= socket.AI_ADDRCONFIG
    for res in set(socket.getaddrinfo(address, port, family, socket.SOCK_DGRAM, 0, flags)):
        af, socktype, proto, canonname, sockaddr = res
        sock = socket.socket(af, socktype, proto)
        set_close_exec(sock.fileno())
        if os.name != 'nt':
            sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        if af == socket.AF_INET6 and hasattr(sock, "IPPROTO_IPV6") and hasattr(sock, "IPV6_V6ONLY"):
            sock.setsockopt(socket.IPPROTO_IPV6, socket.IPV6_V6ONLY, 1)
        sock.setblocking(0)
        sock.bind(sockaddr)
        sockets.append(sock)
        print("Server Listen On:", sockaddr)
    return sockets


def add_accept_handler(sock, callback, io_loop=None):
    if io_loop is None:
        io_loop = IOLoop.instance()

    def accept_handler(fd, events):
        while True:
            try:
                data, address = sock.recvfrom(2500)
            # print("received_data")
            except socket.error as e:
                if e.args[0] in (errno.EWOULDBLOCK, errno.EAGAIN):
                    return
                raise
            callback(sock, data, address)

    io_loop.add_handler(sock.fileno(), accept_handler, IOLoop.READ)


class UDPServer(object):
    def __init__(self, io_loop=None):
        self.io_loop = io_loop
        self._sockets = {}
        self._pending_sockets = []
        self._started = False
        self._port = 0

    def add_sockets(self, sockets):
        if self.io_loop is None:
            self.io_loop = IOLoop.instance()

        for sock in sockets:
            self._sockets[sock.fileno()] = sock
            add_accept_handler(sock, self._on_receive, io_loop=self.io_loop)

    def bind(self, port, address=None, family=socket.AF_UNSPEC, backlog=25):
        self._port = port
        sockets = bind_sockets(port, address=address, family=family, backlog=backlog)

        if self._started:
            self.add_sockets(sockets)
        else:
            self._pending_sockets.extend(sockets)

    def start(self, num_processes=1):
        assert not self._started
        self._started = True
        if num_processes != 1:
            process.fork_processes(num_processes)
        sockets = self._pending_sockets
        self._pending_sockets = []
        self.add_sockets(sockets)

    def stop(self):
        for fd, sock in self._sockets.iteritems():
            self.io_loop.remove_handler(fd)
            sock.close()

    def _on_receive(self, sock, data, address):
        print("receive from address:", address, "data:", data.hex())

    def send_pkt_to_peer(self, data,address):
        for sock in self._sockets.values():
            sock.sendto(data,address)

# if address[1]==56789:
#	print(robotpkt_encdec(data).hex())

# sock.sendto(data, address)


if __name__ == "__main__":
    server = UDPServer()
    server.bind(9876)
    server.start()
    server2 = UDPServer()
    server2.bind(9877)
    server2.start()
    IOLoop.instance().start()

伪机器人的代码：

#!/usr/bin/env python3
#coding=utf-8
#File fake_robot.py


from udpserver import UDPServer
from tornado import gen, iostream
from tornado.ioloop import IOLoop
from tornado.tcpclient import TCPClient
import socket
import os

center_server = ('47.116.100.159',7777)   #改成你自己的中继服务器地址
robot_register_message = b'\xdd\xcc\xbb\xaa\x00\x00\x00\x00\x00\x01\x01'
MyIp = '192.168.0.173'  #改成作为伪机器人的PC在局域网中的IP
Broadcast_addr = '192.168.0.255' #广播地址，自己看着改
MyMac = '00:0c:29:f5:ad:d1'   #机器在局域网中的MAC地址
robot_server = None
Phone_Ip = None

def mac_to_bytes(macaddr):
	macaddr_bytes = b''
	bts = macaddr.split(":")
	for s in bts:
		macaddr_bytes += int(s,16).to_bytes(1,'little')
	return macaddr_bytes

def ip_to_bytes(ipaddr):
	ipaddr_bytes = b''
	bts = ipaddr.split(".")
	for s in bts:
		ipaddr_bytes += int(s).to_bytes(1,'little')
	return ipaddr_bytes

def pkt_encdec(data):
	b = 7
	orig_data = b''
	for ch in data:
		orig_data += bytes([ch ^ b])
		b = ((b + 7) ^ 178) & 0xff
	return orig_data

@gen.coroutine
def RobotClient():
	stream = yield TCPClient().connect(center_server[0], center_server[1])
	bcastsock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
	bcastsock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
	bcastsock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
	bcastsock.bind((MyIp,56789))
	try:
		while True:
			if (RobotUDPServer.center_server_stream == None):
				RobotUDPServer.center_server_stream = stream
				print("Center Server Connected.",RobotUDPServer.center_server_stream)
				yield RobotUDPServer.center_server_stream.write(robot_register_message)
			data = yield stream.read_bytes(4)
			if (data == b'\x44\x33\x22\x11'):
				src_port = yield stream.read_bytes(2)
				dst_port = yield stream.read_bytes(2)
				len_bytes = yield stream.read_bytes(2)
				sport = int.from_bytes(src_port,byteorder = 'big',signed = False)
				dport = int.from_bytes(dst_port,byteorder = 'big',signed = False)
				payload_len = int.from_bytes(len_bytes,byteorder = 'big',signed = False)
				payload = yield stream.read_bytes(payload_len)
				if (dport == 45678):
					orig_data = pkt_encdec(payload)
					#print(orig_data.hex())
					new_pkt = orig_data[:2] + b'\x01' + orig_data[3:6] + ip_to_bytes(MyIp) + mac_to_bytes(MyMac) + orig_data[16:]
					pkt2send = pkt_encdec(new_pkt)
					#print(pkt2send.hex())
					#print(payload.hex())
					#print(pkt2send == payload)
					bcastsock.sendto(pkt2send,(Broadcast_addr,dport))
				if (sport == 10607):
					global Phone_Ip,robot_server
					if (robot_server != None and Phone_Ip != None):
						robot_server.send_pkt_to_peer(payload, (Phone_Ip, dport))
					
	except iostream.StreamClosedError:
		bcastsock.close()
		os._exit(1)


def pack_robot_data(src_port,dst_port,data):
	return b'\xdd\xcc\xbb\xaa' + src_port.to_bytes(2,'big') + dst_port.to_bytes(2,'big') + len(data).to_bytes(2,'big') + data

class RobotUDPServer(UDPServer):
	center_server_stream = None
	@gen.coroutine
	def _on_receive(self, sock, data, address):
		#print("receive from address:", address, "data:", data.hex())
		if (self._port == 10607):
			global Phone_Ip
			if Phone_Ip == None:
				Phone_Ip = address[0]
				print("Got Real Phone Ip:",Phone_Ip)
			upload_pkt = pack_robot_data(address[1],self._port,data)
			#print(upload_pkt.hex())
			#print(RobotUDPServer.center_server_stream)
			if (RobotUDPServer.center_server_stream != None):
				RobotUDPServer.center_server_stream.write(upload_pkt)

if __name__=="__main__":
	robot_server = RobotUDPServer()
	robot_server.bind(10607,MyIp)
	robot_server.start()
	IOLoop.current().run_sync(RobotClient)
	IOLoop.instance().start(2)

伪手机App的代码：

#!/usr/bin/env python3
#coding=utf-8
# File: fake_phone.py

from udpserver import UDPServer
from tornado import gen, iostream
from tornado.ioloop import IOLoop
from tornado.tcpclient import TCPClient
import os

center_server = ('47.116.100.159',7777)  #改成你自己的中继服务器地址
MyIp =  '192.168.8.1'    #伪App在局域网中的IP，在这里就是4G路由器本身的LAN地址了
Robot_Ip = None
phone_register_message = b'\x44\x33\x22\x11\x00\x00\x00\x00\x00\x01\x01'
fake_phone_server = None

@gen.coroutine
def PhoneClient():
	stream = yield TCPClient().connect(center_server[0], center_server[1])
	try:
		while True:
			if (PhoneUDPServer.center_server_stream == None):
				PhoneUDPServer.center_server_stream = stream
				yield stream.write(phone_register_message)
			data = yield stream.read_bytes(4)
			if (data == b'\xdd\xcc\xbb\xaa'):
				src_port = yield stream.read_bytes(2)
				dst_port = yield stream.read_bytes(2)
				len_bytes = yield stream.read_bytes(2)
				sport = int.from_bytes(src_port, byteorder='big', signed=False)
				dport = int.from_bytes(dst_port, byteorder='big', signed=False)
				payload_len = int.from_bytes(len_bytes, byteorder='big', signed=False)
				payload = yield stream.read_bytes(payload_len)
				#print(payload.hex())
				global Robot_Ip,fake_phone_server
				if (fake_phone_server == None):
					fake_phone_server = PhoneUDPServer()
					fake_phone_server.bind(sport,MyIp)
					fake_phone_server.start()
				elif (Robot_Ip != None):
					#print("Payload sent...")
					fake_phone_server.send_pkt_to_peer(payload,(Robot_Ip, dport))
			#print(data.hex())
	except iostream.StreamClosedError:
		os._exit(1)
def pack_phone_data(src_port,dst_port,data):
	return b'\x44\x33\x22\x11' + src_port.to_bytes(2,'big') + dst_port.to_bytes(2,'big') + len(data).to_bytes(2,'big') + data
class PhoneUDPServer(UDPServer):
	center_server_stream = None
	@gen.coroutine
	def _on_receive(self, sock, data, address):
		#print("receive from address:", address, "data:", data.hex())
		if (address[1] == 56789):
			upload_pkt = pack_phone_data(address[1],self._port,data)
			global Robot_Ip
			if (Robot_Ip == None):
				Robot_Ip = address[0]
				print("Got Real Robot Ip address:",Robot_Ip)
			#print(upload_pkt.hex())
			if (PhoneUDPServer.center_server_stream != None):
				PhoneUDPServer.center_server_stream.write(upload_pkt)
		if (address[1] == 10607):
			upload_pkt = pack_phone_data(address[1], self._port, data)
			if (PhoneUDPServer.center_server_stream != None):
				PhoneUDPServer.center_server_stream.write(upload_pkt)
if __name__=="__main__":
	server = PhoneUDPServer()
	server.bind(45678, MyIp)
	server.start()
	IOLoop.current().run_sync(PhoneClient)
	IOLoop.instance().start(2)

中继服务器的代码：

#!/usr/bin/env python3
#File: centerserver.py


from tornado.tcpserver import TCPServer
from tornado.iostream import StreamClosedError
from tornado import gen
from tornado.ioloop import IOLoop

'''
Server Communication Protocol
Data From Robot Magic: 0xaabbccdd
Data From Phone Magic: 0x11223344
Format:
[Magic]  [Src Port]  [Dst Port]  [Payload len]  [Payload]
(4bytes)  (2bytes)    (2bytes)      (2bytes)     (...)
'''

class CenterServer(TCPServer):
	FakeRobot_Stream = None
	FakePhone_Stream = None
	@gen.coroutine
	def handle_stream(self,stream,address):
		while True:
			try:
				data = yield stream.read_bytes(4)
				#print data.encode('hex')
				if data == b'\xdd\xcc\xbb\xaa':
					if (CenterServer.FakeRobot_Stream == None):
						CenterServer.FakeRobot_Stream = stream
						print("Fake Robot Connected!\n",address,CenterServer.FakeRobot_Stream)
					port_pair_data = yield stream.read_bytes(4)
					len_bytes = yield stream.read_bytes(2)
					payload_len = int.from_bytes(len_bytes,byteorder = 'big',signed = False)
					#print "payload_len =",payload_len
					payload = yield stream.read_bytes(payload_len)
					#print(CenterServer.FakePhone_Stream)
					if (CenterServer.FakePhone_Stream != None):
						if (port_pair_data != b'\x00\x00\x00\x00'):
							payload_relayed = data + port_pair_data + payload_len.to_bytes(2,'big') + payload
							#print("Received from Robot:",payload_relayed.hex())
							CenterServer.FakePhone_Stream.write(payload_relayed)
				elif data == b'\x44\x33\x22\x11': 
					if (CenterServer.FakePhone_Stream == None):
						CenterServer.FakePhone_Stream = stream
						print("Fake Phone Connected!\n",address,CenterServer.FakeRobot_Stream)
					port_pair_data = yield stream.read_bytes(4)
					len_bytes = yield stream.read_bytes(2)
					payload_len = int.from_bytes(len_bytes,byteorder = 'big',signed = False)
					payload = yield stream.read_bytes(payload_len)
					if (CenterServer.FakeRobot_Stream != None):
						if (port_pair_data != b'\x00\x00\x00\x00'):
							payload_relayed = data + port_pair_data + payload_len.to_bytes(2,'big') + payload
							#print("Received from Phone:",payload_relayed.hex())
							CenterServer.FakeRobot_Stream.write(payload_relayed)
				else:
					pass
			except StreamClosedError:
				break
		
if __name__=="__main__":
	server = CenterServer()
	server.bind(7777)
	print("Server Lisen on tcp:7777")
	server.start()
	IOLoop.current().start()

完成后按照前面的架构图，部署好代码，依赖库就只有一个——tornado，是不是特别简单？

然后启动fake_phone和fake_robot，然后打开手机App，就可以听到机器人熟悉的被连接上的声音了，从此可以愉快的远程控制它做各种事情了。

关于延迟：

　　根据测试来看，延迟还是有一些的，不过总的来说还可以接受，不算太离谱，所以尽量要选择ping值小的服务器，能够获得更好的控制体验。

补充：理论上新出的EP也能用这种方法来实现，不过EP提供的SDK和文档更强大，相对来说难度会更低一些，可玩性会更高。

给linux内核的mips malta board添加MIPS 64 Release 2架构支持

上一篇

博客搬迁啦(附踩坑记录)

下一篇
相关文章

  • 终于决定开始写点什么了

  • CC2640/2650 IAR环境编译报错Error while running "D:\ti\xdctools_3_31_01_33_core\xs"原因分析

  • TI DSP TMS320F28335上的TCP/IP协议栈

  • CAN Omega 1.1版本开发笔记（一）

  • CAN Omega 1.1版本开发笔记（一）

  • CAN Omega 1.1版本开发笔记（二）——CAN总线120Ω端电阻的正确理解

  • CAN Omega 1.1版本开发笔记（二）——CAN总线120Ω端电阻的正确理解

  • 关于Jarvis OJ的一切，我一定要写下来

评论
发表评论 说点什么

  • 毒丸

    4G 路由器可以由安卓手机替代吗？ 淘汰的安卓手机倒是有不少

  • 毒丸

    广播数据里包含了ip和mac信息，我们需要进行替换，能够仔细说一下这个广播数据的具体格式吗，以及pkt_encdec 的作用是什么？谢谢

    • Jarvis

      机器人通信协议是大疆自己写的编解码方式，这个是基于逆向工程得出来的算法。

  • 毒丸

    参考你的代码运行后，App提示：机器人当前固件版本过低，请在设置中升级后使用。 是不是版本升级后，你这个代码不能运行了？而且我注意到robot多了一个40297的端口广播，是不是因为这个原因？

  • 毒丸

    请教一下，反编译apk包，classes.dev里面找不到入口类com.dji.robomaster, 这些dji的代码是存在哪里的？

  • 哈啦熊宝

    2022/07/29前来考古 一切都setup好了，但是fake phone和fake robo一直timeout就中断了 继续努力吧

  • 陈伟健

    大佬，想问问能不能利用通信协议给机器人发送信息，然后利用这个信息来编程？

浙公网安备 33011002014706号