So, where to begin?

I tried to use this ROS2 tello_driver. Back then it seemed promising, but after some messing around, I've learned that this ROS2 tello_driver is intended for tello EDU, which uses SKD 2.0, while the regular tello uses SDK 1.3.

DJI tend to do so in their so-called open-API products, like Tello and RobomasterS1 (we have one of these in work, for fun and hacking purposes).

tl;dr

In the previous post, I realized that the ROS2 tello_driver's flight_data message was missing the POS data, which is relevant for making an Autonomous drone. Hence I hacked the sh**t out of the good old tello_driver (I worked with it before), and of this ROS2 tello_driver as well, and trying to figure out what makes them tick.

As part of learning and improving ROS2 & C++ skills, in this post, I will share the journey I went through, and what I've learned on the way.

Let's dive in!

The next step was figuring out which communication protocol does the ROS2 tello_driver uses, and what are the differences between this protocol and the one that TelloPy's driver uses. According to their explanation, the ROS2 tello_driver is implemented around the DJI's SDK. DJI released two types of SDK's: 1.3, and 2.0, therefore I needed to get to know them both, here is what I learned:

SDK 2.0

According to DJI sdk 2 documentation:

The user can send many HIGH-LEVEL commands through port: 8889, such as:

• takeoff
• land
• flip
• go x y z speed mid <--- 'mid' == Mission ID
• Many more high-level commands..

The Tello sends back its state (through port: 8990):

• mid
• x
• y
• z
• pith
• Many more parameters..

- Well, that's lovely.

If the drone knows it's position (Only god knows how it is being inferred - for me it is a BlackBox), then the goal of building an autonomous drone is more feasible then before!

In the previous post, showed that installing ROS2 tello_driver is easy as plug and play, also I posted a short video showing the /flight_data topic ( a ROS convention for message):

One can see the empty x y z fields of the message.

After some digging in the ROS2 tello_driver code, I found that it has a state_socket thread, which performs minimal parsing from the raw data:

// Process a state packet from the drone runs at 10Hz
  void StateSocket::process_packet(size_t r)
  {
    std::lock_guard<std::mutex> lock(mtx_);

    static std::map<uint8_t, std::string> enum_names{
      {tello_msgs::msg::FlightData::SDK_UNKNOWN, "unknown"},
      {tello_msgs::msg::FlightData::SDK_1_3,     "v1.3"},
      {tello_msgs::msg::FlightData::SDK_2_0,     "v2.0"}};

    receive_time_ = driver_->now();

    if (receiving_ && driver_->count_subscribers(driver_->flight_data_pub_->get_topic_name()) == 0) {
      // Nothing to do
      return;
    }

    // Split on ";" and ":" and generate a key:value map
    std::map<std::string, std::string> fields;
    std::string raw(buffer_.begin(), buffer_.begin() + r);
    std::regex re("([^:]+):([^;]+);");
    for (auto i = std::sregex_iterator(raw.begin(), raw.end(), re); i != std::sregex_iterator(); ++i) {
      auto match = *i;
      fields[match[1]] = match[2];
    }

    // First message?
    if (!receiving_) {
      receiving_ = true;

      // Hack to figure out the SDK version
      sdk_ = tello_msgs::msg::FlightData::SDK_1_3;
      auto i = fields.find("mid");
      if (i != fields.end() && i->second != "257") {
        sdk_ = tello_msgs::msg::FlightData::SDK_2_0;
      }
      RCLCPP_INFO(driver_->get_logger(), "Receiving state, SDK version %s", enum_names[sdk_].c_str());
    }

    // Only send ROS messages if there are subscribers
    if (driver_->count_subscribers(driver_->flight_data_pub_->get_topic_name()) > 0) {
      tello_msgs::msg::FlightData msg;
      msg.header.stamp = receive_time_;
      msg.raw = raw;
      msg.sdk = sdk_;

      try {

        if (sdk_ == tello_msgs::msg::FlightData::SDK_2_0) {
          msg.mid = std::stoi(fields["mid"]);
          msg.x = std::stoi(fields["x"]);
          msg.y = std::stoi(fields["y"]);
          msg.z = std::stoi(fields["z"]);
        }

        msg.pitch = std::stoi(fields["pitch"]);
        msg.roll = std::stoi(fields["roll"]);
        msg.yaw = std::stoi(fields["yaw"]);
        msg.vgx = std::stoi(fields["vgx"]);
        msg.vgy = std::stoi(fields["vgy"]);
        msg.vgz = std::stoi(fields["vgz"]);
        msg.templ = std::stoi(fields["templ"]);
        msg.temph = std::stoi(fields["temph"]);
        msg.tof = std::stoi(fields["tof"]);
        msg.h = std::stoi(fields["h"]);
        msg.bat = std::stoi(fields["bat"]);
        msg.baro = std::stof(fields["baro"]);
        msg.time = std::stoi(fields["time"]);
        msg.agx = std::stof(fields["agx"]);
        msg.agy = std::stof(fields["agy"]);
        msg.agz = std::stof(fields["agz"]);

      } catch (std::exception e) {
        RCLCPP_ERROR(driver_->get_logger(), "Can't parse flight data");
        return;
      }

      driver_->flight_data_pub_->publish(msg);
    }
  }

Let's break it down:

It seems the SDK is being inferred from the message:

      // Hack to figure out the SDK version
      sdk_ = tello_msgs::msg::FlightData::SDK_1_3;
      auto i = fields.find("mid");
      if (i != fields.end() && i->second != "257") {
        sdk_ = tello_msgs::msg::FlightData::SDK_2_0;
      }

Accordingly, x y z fields are being filled:

        if (sdk_ == tello_msgs::msg::FlightData::SDK_2_0) {
          msg.mid = std::stoi(fields["mid"]);
          msg.x = std::stoi(fields["x"]);
          msg.y = std::stoi(fields["y"]);
          msg.z = std::stoi(fields["z"]);
        }

According to the SDK 2.0; drone state is being published to port 8890.

Using Wireshark, I saw that the packet sent to port 8890 contains parsed data. That fits the sdk 2.0 description:

Packet's data section contains drone state in one line:

pitch:0;roll:0;yaw:-5;vgx:0;vgy:0;vgz:0;templ:60;temph:62;tof:10;h:0;bat:63;baro:149.54;time:0;agx:-15.00;agy:1.00;agz:-998.00;

This oneliner can be easily parsed using this parser:

    std::regex re("([^:]+):([^;]+);");

And with this parser, data is being inserted from the message into fields:

    for (auto i = std::sregex_iterator(raw.begin(), raw.end(), re); i != std::sregex_iterator(); ++i) {
      auto match = *i;
      fields[match[1]] = match[2];
    }

The final ROS2 message look likes that:

(using the following command, in a new terminal: ros2 topic echo /flight_data)

header:  
  stamp:
    sec: 1601124796
    nanosec: 806872198
  frame_id: ''
raw: "pitch:0;roll:0;yaw:-5;vgx:0;vgy:0;vgz:0;templ:60;temph:62;tof:10;h:0;bat:63;baro:149.54;time:0;agx:-15.00;agy:1.00;agz:-998.00;\r..."  
sdk: 1 #<---------------------------- MY DRONE's SDK (1.3)  
pitch: 0.0  
roll: 0.0  
yaw: -5.0  
vgx: 0  
vgy: 0  
vgz: 0  
templ: 60  
temph: 62  
tof: 10  
h: 0  
bat: 63  
baro: 149.5399932861328  
time: 0  
agx: -15.0  
agy: 1.0  
agz: -998.0  
mid: 0 #<---------------------------- EMPTY - MY DRONE IS USING SDK 1.3  
x: 0.0 #<---------------------------- EMPTY - MY DRONE IS USING SDK 1.3  
y: 0.0 #<---------------------------- EMPTY - MY DRONE IS USING SDK 1.3  
z: 0.0 #<---------------------------- EMPTY - MY DRONE IS USING SDK 1.3  
---

Using the good-old TelloPy driver

Surprisingly, after running the TelloPy's 'simpletakeoff.py' script and subscribing to 'drone.EVENTLOG_DATA', data was successfully received from the drone, including POS!!!

This is the simple_takeoff.py script I ran (with minor modifications):

from time import sleep  
import sys  
sys.path.append("/home/gal/sandbox/tello_py/TelloPy") # Simple hack I did to resolve PATH issues.  
import tellopy

def handler(event, sender, data, **args):  
    drone = sender
    if event is drone.EVENT_LOG_DATA:
        print(data) # <------------------------------ THE FOLLOWING MESSAGE


def test():  
    drone = tellopy.Tello()
    try:
        drone.subscribe(drone.EVENT_LOG_DATA, handler)
        drone.connect()
        drone.wait_for_connection(60.0)
        while(1):
           inp =  input() 
        # drone.takeoff()
        # sleep(5)
        # drone.down(50)
        # sleep(5)
        # drone.land()
        # sleep(5)
    except Exception as ex:
        print(ex)
    finally:
        drone.quit()

if __name__ == '__main__':  
    test()

And this is how the drone.EVENT_LOG_DATA looks like:

MVO: VEL:  0.00  0.00  0.00 POS:  0.02  0.01 -0.01|IMU: ACC: -0.02 -0.00 -1.01 GYRO:  0.00  0.00 -0.00 QUATERNION:  1.00 -0.00 -0.00  0.00 VG: -0.27 -0.15 -0.06  

Can you see the POS: 0.02 0.01 -0.01?!

What is happening under the hood?

Well, it seems that hanyazou wrapped a go_lang project that hacked the Tello packets protocol.

Looking again at the simple_takeoff.py:

drone = tellopy.Tello() # First a Tello object is instantiated  

Tello.py module looks like that:

class Tello(object):  
    EVENT_CONNECTED = event.Event('connected')
    EVENT_WIFI = event.Event('wifi')
    EVENT_LIGHT = event.Event('light')
    # Bunch of CONSTANTS
    *
    *
    *
    *
    # Bunch of CONSTANTS

    def __init__(self, port=9000):
        self.tello_addr = ('192.168.10.1', 8889)
        self.debug = False
        self.pkt_seq_num = 0x01e4
        self.port = port
        self.udpsize = 2000
        # Bunch of initialisers
        *
        *
        *
        # Bunch of initializers

        # Create a UDP socket
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.sock.bind(('', self.port)) #<----------------------------- BIND THAT SOCKET TO THE GIVEN PORT
        self.sock.settimeout(2.0)

        dispatcher.connect(self.__state_machine, dispatcher.signal.All)
        threading.Thread(target=self.__recv_thread).start() # <-------- THREAD FOR RECEIVING DATA
        threading.Thread(target=self.__video_thread).start()

Wait what?! what is port 9000????

It appears that there's an undocumented port, through which all the drone's data is 'spilled' into.

After sniffing that port with Wireshark:

There's no human-readable data here.

And yet, the driver extrapolated data successfully.

So I looked even deeper...

This is what the __recv_thread looks like:

    def __recv_thread(self):
        sock = self.sock

        while self.state != self.STATE_QUIT:

            if self.state == self.STATE_CONNECTED:
                self.__send_stick_command()  # ignore errors <---- *KEEP ALIVE

            try:
                data, server = sock.recvfrom(self.udpsize)
                log.debug("recv: %s" % byte_to_hexstring(data))
                self.__process_packet(data) # <------------------ THIS IS WHAT I'M INTRESTED IN
            except socket.timeout as ex:
                if self.state == self.STATE_CONNECTED:
                    log.error('recv: timeout')
                self.__publish(event=self.__EVENT_TIMEOUT)
            except Exception as ex:
                log.error('recv: %s' % str(ex))
                show_exception(ex)

Side Note

*KEEP ALIVE - The Tello drone turns itself OFF if no command is received within 30 sec. Hence for every data the Tello drone sends to the TelloPy client, self.__send_stick_command() is called to simulate stick commands back to the Tello drone.

Before diving into self.__process_packet(data), Some preperation is needed.

Inside protocol.py, hanyazou mapped all possible commands to their HEX value:

# low-level Protocol (https://tellopilots.com/wiki/protocol/#MessageIDs)
START_OF_PACKET                     = 0xcc  
SSID_MSG                            = 0x0011  
SSID_CMD                            = 0x0012  
SSID_PASSWORD_MSG                   = 0x0013  
SSID_PASSWORD_CMD                   = 0x0014  
WIFI_REGION_MSG                     = 0x0015  
WIFI_REGION_CMD                     = 0x0016  
WIFI_MSG                            = 0x001a  
VIDEO_ENCODER_RATE_CMD              = 0x0020  
VIDEO_DYN_ADJ_RATE_CMD              = 0x0021  
EIS_CMD                             = 0x0024  
VIDEO_START_CMD                     = 0x0025  
VIDEO_RATE_QUERY                    = 0x0028  
TAKE_PICTURE_COMMAND                = 0x0030  
VIDEO_MODE_CMD                      = 0x0031  
VIDEO_RECORD_CMD                    = 0x0032  
EXPOSURE_CMD                        = 0x0034  
LIGHT_MSG                           = 0x0035  
JPEG_QUALITY_MSG                    = 0x0037  
ERROR_1_MSG                         = 0x0043  
ERROR_2_MSG                         = 0x0044  
VERSION_MSG                         = 0x0045  
TIME_CMD                            = 0x0046  
ACTIVATION_TIME_MSG                 = 0x0047  
LOADER_VERSION_MSG                  = 0x0049  
STICK_CMD                           = 0x0050  
TAKEOFF_CMD                         = 0x0054  
LAND_CMD                            = 0x0055  
FLIGHT_MSG                          = 0x0056  
SET_ALT_LIMIT_CMD                   = 0x0058  
FLIP_CMD                            = 0x005c  
THROW_AND_GO_CMD                    = 0x005d  
PALM_LAND_CMD                       = 0x005e  
TELLO_CMD_FILE_SIZE                 = 0x0062  # pt50  
TELLO_CMD_FILE_DATA                 = 0x0063  # pt50  
TELLO_CMD_FILE_COMPLETE             = 0x0064  # pt48  
SMART_VIDEO_CMD                     = 0x0080  
SMART_VIDEO_STATUS_MSG              = 0x0081  
LOG_HEADER_MSG                      = 0x1050  
LOG_DATA_MSG                        = 0x1051  
LOG_CONFIG_MSG                      = 0x1052  
BOUNCE_CMD                          = 0x1053  
CALIBRATE_CMD                       = 0x1054  
LOW_BAT_THRESHOLD_CMD               = 0x1055  
ALT_LIMIT_MSG                       = 0x1056  
LOW_BAT_THRESHOLD_MSG               = 0x1057  
ATT_LIMIT_CMD                       = 0x1058 # Stated incorrectly by Wiki (checked from raw packets)  
ATT_LIMIT_MSG                       = 0x1059

EMERGENCY_CMD                       = 'emergency'  

Also, I've learned from the go_lang project the structure of the Tello drone packets:

So for example: 0xcc 0x58 0x00 0x7c 0x68 0x54 0x00 0xe4 0x01 0xc2 0x16

Knowing the packet structure, and using the messages mapping described above, I can now understand the sniffed package from port 9000:

0xcc 0xa0 0x11 0x50 0x88 0x51 0x10 0x14 0x02 0x00 0x55 0x43 0x00 0x15 .....

As for processing a packet:

def __process_packet(self, data):  
        if isinstance(data, str):
            data = bytearray([x for x in data])

        if str(data[0:9]) == 'conn_ack:' or data[0:9] == b'conn_ack:':
            log.info('connected. (port=%2x%2x)' % (data[9], data[10]))
            log.debug('    %s' % byte_to_hexstring(data))
            if self.video_enabled:
                self.__send_exposure()
                self.__send_video_encoder_rate()
                self.__send_start_video()
            self.__publish(self.__EVENT_CONN_ACK, data)

            return True

        if data[0] != START_OF_PACKET:
            log.info('start of packet != %02x (%02x) (ignored)' % (START_OF_PACKET, data[0]))
            log.info('    %s' % byte_to_hexstring(data))
            log.info('    %s' % str(map(chr, data))[1:-1])
            return False

        pkt = Packet(data)
        cmd = uint16(data[5], data[6])
        if cmd == LOG_HEADER_MSG:
            id = uint16(data[9], data[10])
            log.info("recv: log_header: id=%04x, '%s'" % (id, str(data[28:54])))
            log.debug("recv: log_header: %s" % byte_to_hexstring(data[9:]))
            self.__send_ack_log(id)
            self.__publish(event=self.EVENT_LOG_HEADER, data=data[9:])
            if self.log_data_file and not self.log_data_header_recorded:
                self.log_data_file.write(data[12:-2])
                self.log_data_header_recorded = True
        elif cmd == LOG_DATA_MSG:
            log.debug("recv: log_data: length=%d, %s" % (len(data[9:]), byte_to_hexstring(data[9:])))
            self.__publish(event=self.EVENT_LOG_RAWDATA, data=data[9:])
            try:
                self.log_data.update(data[10:])
                if self.log_data_file:
                    self.log_data_file.write(data[10:-2])
            except Exception as ex:
                log.error('%s' % str(ex))
            self.__publish(event=self.EVENT_LOG_DATA, data=self.log_data)

        elif cmd == LOG_CONFIG_MSG:
            log.debug("recv: log_config: length=%d, %s" % (len(data[9:]), byte_to_hexstring(data[9:])))
            self.__publish(event=self.EVENT_LOG_CONFIG, data=data[9:])
        elif cmd == WIFI_MSG:
            log.debug("recv: wifi: %s" % byte_to_hexstring(data[9:]))
            self.wifi_strength = data[9]
            self.__publish(event=self.EVENT_WIFI, data=data[9:])
        elif cmd == ALT_LIMIT_MSG:
            log.info("recv: altitude limit: %s" % byte_to_hexstring(data[9:-2]))
        elif cmd == ATT_LIMIT_MSG:
            log.info("recv: attitude limit: %s" % byte_to_hexstring(data[9:-2]))
        elif cmd == LOW_BAT_THRESHOLD_MSG:
            log.info("recv: low battery threshold: %s" % byte_to_hexstring(data[9:-2]))
        elif cmd == LIGHT_MSG:
            log.debug("recv: light: %s" % byte_to_hexstring(data[9:-2]))
            self.__publish(event=self.EVENT_LIGHT, data=data[9:])
        elif cmd == FLIGHT_MSG:
            flight_data = FlightData(data[9:])
            flight_data.wifi_strength = self.wifi_strength
            log.debug("recv: flight data: %s" % str(flight_data))
            self.__publish(event=self.EVENT_FLIGHT_DATA, data=flight_data)
        elif cmd == TIME_CMD:
            log.debug("recv: time data: %s" % byte_to_hexstring(data))
            self.__publish(event=self.EVENT_TIME, data=data[7:9])
        elif cmd in (SET_ALT_LIMIT_CMD, ATT_LIMIT_CMD, LOW_BAT_THRESHOLD_CMD, TAKEOFF_CMD, LAND_CMD, VIDEO_START_CMD, VIDEO_ENCODER_RATE_CMD, PALM_LAND_CMD,
                     EXPOSURE_CMD, THROW_AND_GO_CMD, EMERGENCY_CMD):
            log.debug("recv: ack: cmd=0x%02x seq=0x%04x %s" %
                     (uint16(data[5], data[6]), uint16(data[7], data[8]), byte_to_hexstring(data)))
        elif cmd == TELLO_CMD_FILE_SIZE:
            # Drone is about to send us a file. Get ready.
            # N.b. one of the fields in the packet is a file ID; by demuxing
            # based on file ID we can receive multiple files at once. This
            # code doesn't support that yet, though, so don't take one photo
            # while another is still being received.
            log.info("recv: file size: %s" % byte_to_hexstring(data))
            if len(pkt.get_data()) >= 7:
                (size, filenum) = struct.unpack('<xLH', pkt.get_data())
                log.info('      file size: num=%d bytes=%d' % (filenum, size))
                # Initialize file download state.
                self.file_recv[filenum] = DownloadedFile(filenum, size)
            else:
                # We always seem to get two files, one with most of the payload missing.
                # Not sure what the second one is for.
                log.warn('      file size: payload too small: %s' % byte_to_hexstring(pkt.get_data()))
            # Ack the packet.
            self.send_packet(pkt)
        elif cmd == TELLO_CMD_FILE_DATA:
            # log.info("recv: file data: %s" % byte_to_hexstring(data[9:21]))
            # Drone is sending us a fragment of a file it told us to prepare
            # for earlier.
            self.recv_file_data(pkt.get_data())
        else:
            log.info('unknown packet: %04x %s' % (cmd, byte_to_hexstring(data)))
            return False

        return True

While Breaking down this packet processor. Although it seemed scary with all that long code, it all went down to this line::

# Extract the command type from the packet and process it accordingly.
cmd = uint16(data[5], data[6])  

Earlier, in simplte_takeoff.py, I subscribed to drone.subscribe(drone.EVENT_LOG_DATA, handler). This is the type of packet I looked for in this packet_processor:

        elif cmd == LOG_DATA_MSG:
            log.debug("recv: log_data: length=%d, %s" % (len(data[9:]), byte_to_hexstring(data[9:])))
            self.__publish(event=self.EVENT_LOG_RAWDATA, data=data[9:])
            try:
                self.log_data.update(data[10:])
                if self.log_data_file:
                    self.log_data_file.write(data[10:-2])
            except Exception as ex:
                log.error('%s' % str(ex))
            self.__publish(event=self.EVENT_LOG_DATA, data=self.log_data) # <------------- MY SUBSCRIBER BEING NOTIFIED

This line is the actual data extraction. This is what I looked for:

self.log_data.update(data[10:])  

Bare with me, Its almost over :)

Peeking inside log_data.update():

    def update(self, data):
        if isinstance(data, bytearray):
            data = str(data)

        self.log.debug('LogData: data length=%d' % len(data))
        self.count += 1
        pos = 0
        while (pos < len(data) - 2):
            if (struct.unpack_from('B', data, pos+0)[0] != 0x55):
                raise Exception('LogData: corrupted data at pos=%d, data=%s'
                               % (pos, byte_to_hexstring(data[pos:])))
            length = struct.unpack_from('<h', data, pos+1)[0]
            checksum = data[pos+3]
            id = struct.unpack_from('<H', data, pos+4)[0]
            # 4bytes data[6:9] is tick
            # last 2 bytes are CRC
            # length-12 is the byte length of payload
            xorval = data[pos+6]
            if isinstance(data, str):
                payload = bytearray([ord(x) ^ ord(xorval) for x in data[pos+10:pos+10+length-12]])
            else:
                payload = bytearray([x ^ xorval for x in data[pos+10:pos+10+length-12]])
            if id == self.ID_NEW_MVO_FEEDBACK:
                self.mvo.update(payload, self.count) # <--------- THIS IS WHERE 'POS' DATA CAN BE FOUND
            elif id == self.ID_IMU_ATTI:
                self.imu.update(payload, self.count) 
            else:
                if not id in self.unknowns:
                    self.log.info('LogData: UNHANDLED LOG DATA: id=%5d, length=%4d' % (id, length-12))
                    self.unknowns.append(id)

            pos += length
        if pos != len(data) - 2:
            raise Exception('LogData: corrupted data at pos=%d, data=%s'
                            % (pos, byte_to_hexstring(data[pos:])))

Jackpot!

This line:

payload = bytearray([x ^ xorval for x in data[pos+10:pos+10+length-12]])  

And this one:

self.mvo.update(payload, self.count)  

Now that I've found a way to extract POS from the packet that comes from port 9000, I need to implement a similar packet_processor in CPP, and add it to the ROS2 tello_driver.

So, what is the difference?

ROS2 tello_driver:

• SUPPORTs ROS2 (discussed in the previous post).
• Uses a documented SDK, supplied by DJI.
• Listens to port 8890, which according to the SDK, supplies drone state:
  • human-readable data, that can be easily parsed and published.
    
    It has known serialized structure: pitch:0;roll:0;yaw:-5;vgx:0;vgy:0;vgz:0...
• CPP
  • Fast.
  • Relevant for me.

TelloPy driver:

• NO ROS2 SUPPORT
• Listens to port 9000 - An undocumented open port at the drone, this port appears to send RAW_LOG_DATA that contains EVERYTHING! It has been discovered by the community - with reverse engineering (and probably lots of hours using Wireshark)
• PYTHON only
  • Easy to understand & maintain & hack.

Final thoughts

In this post, I've focused on the journey I took.

It seems that I'm ought to implement a CPP packet_processor, for the ROS2 tello_driver. Thus adding support for RAW_LOG_DATA logging capabilities.

This is an excellent opportunity to support the development of ROS2. This mini-series is all about supporting the ROS2 community. I do have thoughts about POS data accuracy/availability though, will it be enough for a position estimator? is it solid enough to be considered in the VSLAM algorithm?

While the questions arise, the quest must go on.

Cheers, Gal.