Tello 即時飛行控制器實作

derek

.

最近大受歡迎的迷你空拍機 Tello，起飛重量不到100克，非常輕巧卻極為安全穩定，很容易在小小的室內進行操控，相當適合初學者嘗試。

特別是 Tello 還提供了 Scratch 的積木，可以任意編寫飛行控制的邏輯程序，讓 Tello 在空中依照自己的指令翱翔飛舞，實在是很有趣。

今天我們要來挑戰以 WiFiBoy32 實做一個 Tello 遙控器，可設定 start/select 鍵為自動起飛、自動降落的按鈕，以四個方向鍵與AB鈕組合做出12個飛行指令進行即時操控，也可以按照我們預設的指令序列，進行一段精彩的自動飛行特技表演。

依據Tello SDK 的五頁簡要說明，WiFiBoy32 可用 WiFi 連上 Tello 開機就啟動的 Access Point，再以 WiFi 的 UDP 通訊機制，對 IP 地址 192.168.10.1 的 port#8889 傳送指令，就可以任意控制 Tello 了。

開啟 WiFi UDP 連接 Tello 的 WiFiBoy32 Arduino 範例程式如下：

#include <WiFi.h>
#include <WiFiUdp.h>
const char *networkName = "TELLO-ABB31E"; // 請記得改為你的 Tello AP 名稱
const char *networkPwd = "";
WiFiUDP udp;
void setup()
{
  Serial.begin(115200);
  connectToWiFi(networkName, networkPwd);
}
void loop()
{
}
void connectToWiFi(const char * ssid, const char * pwd)
{
  WiFi.disconnect(true);
  WiFi.begin(ssid, pwd);
  Serial.println("Connecting to Tello");
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.printf("\nConnected to %s\n", ssid);
  Serial.printf("\nIP address: %s", WiFi.localIP());
  udp.begin(udpPort);
}

讓 WiFiBoy32 連上 Tello 後，第一個要下的指令是字串 "command" 來啟動 Tello 的文字指令模式，成功之後就可以文字模式繼續傳送各種控制指令。

根據 Tello 愛好者們最近陸續 Hack 出來的資料顯示，Tello App 並不是以文字模式控制 Tello，而是以特殊的指令編碼封包，傳送更多 SDK 沒有公開的控制指令，來做到精密的飛行姿態控制、拍照與即時 streaming video 控制，這些沒公開的祕密指令我們已經做了些小小研究，將會陸續以 WiFiBoy32 進行一些實驗，在這個網站與各位朋友一起研究分享。今天我們就先玩玩文字指令模式，做出一個基本的入門版即時飛行控制器。

以 UDP 送出 "command" 字串後，Tello 應該要立即回應一個 "OK" 回來。

為了要能進行即時互動測試，我們今天還要來試用 WiFiBoy.Club 正在開發中的 Blockly Python WebIDE，接上內建 MicroPython 的 WiFiBoy（或 WiFiBoy32），可以用超級方便的即時互動控制台來試試這個 UDP 啟始指令：

這裡用 WiFiBoy 積木以 UDP 連上 Tello 的 TELLO-ABB31E 存取點，請掃描看看你的 TELLO，這個 AP 的名字應該是不一樣的。

接著，我們送出了一個 "command" 字串到 192.168.10.1 port 8889，成功後得到一個 b'OK' 字串。（Python 顯示 bytes 資料會加一個 b' 符號）

再來我們可以試試新的指令："battery?"，詢問 Tello 的電量還有多少百分比。這次測試收到的回應是 89%。

在這個即時互動控制台的左邊，是一個積木編輯器，隨時可以送出給 WiFiBoy 執行。右邊是一個 REPL 的 Terminal，可以用文字送出指令。（請參考後面的測試影片）

＊＊＊

我們知道如何利用 UDP 與 Tello 溝通後，就可以寫一個 Arduino 版本的測試程式如下：

const char * udpAddress = "192.168.10.1";
const int udpPort = 8889;
void TelloCommand(char *cmd)
{
  uint8_t buffer[100];
  Serial.printf("Send [%s] to Tello.\n", cmd);
  udp.beginPacket(udpAddress, udpPort); 
  udp.printf(cmd);
  udp.endPacket();
  memset(buffer, 0, 100);
  while(udp.parsePacket()==0) { delay(1); } // check reply messages
  if(udp.read(buffer, 100) > 0){
    Serial.print("Message from Tello: ");
    Serial.println((char * )buffer);
  }
}

好，有了這個基礎，就容易多了。

我們設計了利用 A/B+上下左右的組合鍵形式，將 Tello 的飛行指令，選擇了其中 12 個，做成 WiFiBoy32 的飛行控制器。

經過測試數次，我們設定了左右旋一次轉 45 度，一次移動 20cm，移動速度 30cm/s。這樣飛起來還算輕鬆。

這個 WiFiBoy32 飛行控制器的按鍵設定如下：

START 鍵 / SELECT 鍵：自動起飛 / 自動降落
上 / 下鍵：前進 / 後退
左 / 右鍵：左旋（逆時針）/ 右旋（順時針）
B 鍵＋上 / 下鍵：上升 / 下降
B 鍵＋左 / 右鍵：左移 / 右移
A 鍵＋上 / 下鍵：前空翻 / 後空翻
A 鍵＋左 / 右鍵：左空翻 / 右空翻

＊＊＊

增加了畫面提示與按鍵聲音提示後，我們就完成了一個基本版的飛行控制器。

我們偷偷在背後的 PRG 鍵，增加了一個串列特技飛行動作指令，可以一鍵進行多個循序飛舞指令。有興趣試試的朋友可以自行編輯製作自己的飛舞指令序列。

如果你手上有個 Tello 四軸空拍機，不妨一試這個範例程式，真的很有趣喔！

＊＊＊

如果有朋友想要研究 Tello 更進一步的 undocumented 控制方式，歡迎與我們聯繫。

Tello 進階指令：(這些是 SDK 沒有揭露的破解資料，如有不正確請幫忙修正，謝謝！）
--
搖桿姿態指令 stick command [cmdID=80 (0x50)]
搖桿軸值介於 363(left, down) 與 1684(right, up) 之間，中間值為 1024。
利用搖桿姿態指令，才可以進行即時流暢的運動飛行，做出同時「自旋+前進+上升」之類的動作。
--
axis1 : aileron (RIGHT_X)
axis2 : elevator (RIGHT_Y)
axis3 : throttle (LEFT_Y)
axis4 : rudder (LEFT_X)
axis5 : 0 = slow mode, 1 = fast mode (這個軸只用到 4 bits)
--
影像數據啟動 "conn_req" + port
影像數據回應 "conn_ack" + port
（影像數據 UDP port = 8890）
影像數據：H.264 920x720, mp4 stream
--
控制指令的封包格式如下：
--
位址    用法
0       0xcc indicates the start of a packet
1-2     Packet size, 13 bit encoded ([2] << 8 ) | ([1] >> 3)
3       CRC8 of bytes 0-2
4       Packet type ID
5-6     Command ID, little endian
7-8     Sequence number of the packet, little endian
9-(n-2)    Data (if any)
(n-1)-n    CRC16 of bytes 0 to n-2
--
舉例「自動起飛」的封包如下：
--
0xcc 0x58 0x00 0x7c 0x68 0x54 0x00 0xe4 0x01 0xc2 0x16
Value        Usage
0xcc         start of packet
0x58 0x00    Packet size of 11
0x7c         CRC8 of bytes 0-2
0x68         Packet type ID
0x54 0x00    "Takeoff" command ID, little endian
0xe4 0x01    Sequence number of the packet, little endian
0xc2 0x16    CRC16 of bytes 0 to 8
==========================================================
以下是網友 Hack 與猜測發現的指令表：（尚不完整）
--
cmdId:4176//Write file header
cmdId:4177//Write data
cmdId:4178//Write configuration
cmdId:86//FlyData
cmdId:69//Answer - see the version number
cmdId:73//answer-loader version
cmdId:40//Answer-Get Rate Rate Parameters
cmdId:4182//Acknowledge - Get Height Limit Parameters
cmdId:4183//Answer - Get low-power parameter
cmdId:4185//Answer - Get attitude angle
cmdId:71//Answer - Aircraft Activation Time
cmdId:17//Answer-ssid Get
cmdId:18//answer-ssid settings
cmdId:19//answer-wifi password retrieval
cmdId:20//answer-wifi password settings
cmdId:22//Answer - Set Country Code
cmdId:84//Answer - One Click Off
cmdId:85//Answer-OneKeyDown
cmdId:82//l
cmdId:83//m
cmdId:88//g
cmdId:4184//Answer-set attitude angle
cmdId:89//f
cmdId:92//Answer-turnover
cmdId:93//Answer-throw fly
cmdId:94//answer-palm landing
cmdId:4180//Answer-center-of-gravity/calibration plane calibration
cmdId:55//Answer-toggle JPEG photo quality
cmdId:90//handleIMUStart
cmdId:32//Answer - set coding rate
rcmdId:36//Answer-EIS settings
cmdId:4179//Response - Xiao Huangren
cmdId:128//Answer - Wisdom Start/End
cmdId:129//Answer - Wisdom Entry/Exit

Tello 即時飛行控制器 WiFiBoy32 Arduino 全部的原始程式在這裡：下載。（wifiboy32 library 下載）

/*
 *  Tello Controller for WiFiBoy32 
 *  
 *  April 21, 2018 Created. (derek@wifiboy.org)
 *  (C)2018 WiFiBoy.Org, All rights reserved.
 *  
 */
#include <WiFi.h>
#include <WiFiUdp.h>
#include <wifiboy32.h>
#define PRG_KEY     0
#define L_KEY       17
#define D_KEY       27
#define R_KEY       32
#define U_KEY       33
#define A_KEY       35
#define B_KEY       34
#define START_KEY   23
#define SELECT_KEY  39
const char *networkName = "TELLO-ABB31E"; //  請記得改成你的 Tello AP 名稱
const char *networkPswd = "";
const char *udpAddress = "192.168.10.1"; // 標準的 Tello IP Address
const int udpPort = 8889; // Tello 的 UDP 通訊埠
WiFiUDP udp;
void setup()
{
  wb32_splash();
  Serial.begin(115200);
  connectToWiFi(networkName, networkPswd);
  wb32_drawString("<Connected>", 60, 270, 2, 2);
  
  TelloCommand("command"); // 送出一個字串指令給 Tello
  TelloCommand("battery?");
  TelloCommand("speed 30"); // initial speed = 30 cm/s
}
void loop()
{
  if (key_pressed(START_KEY)) { TelloCommand("takeoff"); wait_released(START_KEY); }
  if (key_pressed(SELECT_KEY)) { TelloCommand("land"); wait_released(SELECT_KEY); }
  if (key_pressed(B_KEY)) { // 組合鍵 B + L/R/U/D
    if (key_pressed(L_KEY)) { TelloCommand("left 20"); wait_released(L_KEY); }
    if (key_pressed(R_KEY)) { TelloCommand("right 20"); wait_released(R_KEY); }
    if (key_pressed(U_KEY)) { TelloCommand("up 20"); wait_released(U_KEY); }
    if (key_pressed(D_KEY)) { TelloCommand("down 20"); wait_released(D_KEY); }
    if (key_pressed(START_KEY)) { TelloCommand("speed 30"); wait_released(START_KEY); }
    if (key_pressed(SELECT_KEY)) { TelloCommand("speed 50"); wait_released(SELECT_KEY); }
  } else if (key_pressed(A_KEY)) { // 組合鍵 A + L/R/U/D
    if (key_pressed(L_KEY)) { TelloCommand("flip l"); wait_released(L_KEY); }
    if (key_pressed(R_KEY)) { TelloCommand("flip r"); wait_released(R_KEY); }
    if (key_pressed(U_KEY)) { TelloCommand("flip u"); wait_released(U_KEY); }
    if (key_pressed(D_KEY)) { TelloCommand("flip d"); wait_released(D_KEY); }
    if (key_pressed(START_KEY)) { TelloCommand("speed 30"); wait_released(START_KEY); }
    if (key_pressed(SELECT_KEY)) { TelloCommand("speed 50"); wait_released(SELECT_KEY); }
  } else { // 沒有組合鍵的 L/R/U/D
    if (key_pressed(L_KEY)) { TelloCommand("ccw 45"); wait_released(L_KEY); }
    if (key_pressed(R_KEY)) { TelloCommand("cw 45"); wait_released(R_KEY); }
    if (key_pressed(U_KEY)) { TelloCommand("forward 20"); wait_released(U_KEY); }
    if (key_pressed(D_KEY)) { TelloCommand("back 20"); wait_released(D_KEY); }
  }
  if (key_pressed(PRG_KEY)) { // WiFiBoy32 背後的 PRG 鍵，可以自行編輯飛舞指令序列
    TelloCommand("battery?");
    TelloCommand("time?");
    TelloCommand("speed?");
    wait_released(PRG_KEY); 
  }
  delay(5);
}
void connectToWiFi(const char * ssid, const char * pwd)
{
  WiFi.disconnect(true);
  WiFi.begin(ssid, pwd); // 啟動 WiFi
  Serial.println("Connecting to Tello");
  while (WiFi.status() != WL_CONNECTED) { // 檢查是否已經連線成功
    delay(500);
    Serial.print(".");
  }
  Serial.print("\nConnected to ");
  Serial.println(ssid);
  Serial.print("\nIP address: ");
  Serial.println(WiFi.localIP());
  udp.begin(udpPort); // 啟動 UDP
}
void TelloCommand(char *cmd)
{
  uint8_t buffer[100];
  ledcWriteTone(0, 880); ledcWrite(0, 100); // 發出 PWM 聲音
  Serial.printf("Send [%s] to Tello.\n", cmd);
  udp.beginPacket(udpAddress,udpPort);
  udp.printf(cmd);
  udp.endPacket(); // 送出 UDP 指令
  memset(buffer, 0, 100); // 把緩衝區清除
  while(udp.parsePacket()==0) { delay(1); } // 等待回應
  ledcWriteTone(0, 440); 
  if(udp.read(buffer, 100) > 0){ // 從 UDP 緩衝區取得回應內容
    Serial.print("Message from Tello: ");
    Serial.println((char * )buffer);
  }
  ledcWrite(0, 0); // 關閉聲音
}
void wb32_initkey()
{
  pinMode(PRG_KEY, INPUT); // 設定按鍵 IO 為輸入模式
  pinMode(L_KEY, INPUT);
  pinMode(D_KEY, INPUT);
  pinMode(R_KEY, INPUT);
  pinMode(U_KEY, INPUT);
  pinMode(A_KEY, INPUT);
  pinMode(B_KEY, INPUT);
  pinMode(START_KEY, INPUT);
  pinMode(SELECT_KEY, INPUT);
  ledcSetup(0, 400, 8 ); // 設定 PWM 通道 #0
  ledcAttachPin(25, 0); // PWM 設在 GPIO#25
}
int key_pressed(int key)
{
  if (digitalRead(key)==0) return 1; else return 0; // 檢查按鍵是否按下
}
int wait_released(int key)
{
  while(digitalRead(key)==0) delay(1); // 等待按鍵放開
}
void wb32_splash()
{
  wb32_init(); // WiFiBoy32 初始化
  wb32_setTextColor(wbCYAN, wbCYAN); // 設定文字顏色
  wb32_drawString("WiFiBoy", 24, 30, 1, 6); // 顯示 WiFiBoy Logo
  wb32_setTextColor(wbRED, wbRED); // 設定文字顏色
  wb32_drawString("32", 185, 44, 1, 4);
  wb32_drawFastHLine(10, 80, 220, wbYELLOW);
  wb32_drawFastHLine(10, 81, 220, wbYELLOW);
  wb32_setTextColor(wbWHITE, wbWHITE); // 設定文字顏色
  wb32_drawString("Tello Controller", 40, 108, 2, 2);
  wb32_setTextColor(wbGREEN, wbGREEN); // 設定文字顏色
  wb32_drawString("(C)2018 WiFiBoy.Org, All Rights Reserved", 20, 300, 2, 1);
  wb32_setTextColor(wbYELLOW, wbYELLOW); // 設定文字顏色
  wb32_drawString("Forward", 103, 148, 2, 1);
  wb32_setTextColor(wbGREEN, wbGREEN); // 設定文字顏色
  wb32_drawString("LTurn", 93, 168, 2, 1);
  wb32_setTextColor(wbRED, wbRED); // 設定文字顏色
  wb32_drawString("RTurn", 133, 168, 2, 1);
  wb32_setTextColor(wbCYAN, wbCYAN); // 設定文字顏色
  wb32_drawString("Back", 113, 188, 2, 1);
  wb32_setTextColor(wbCYAN, wbCYAN); // 設定文字顏色
  wb32_drawString("B", 56, 223, 2, 2);
  wb32_drawString("Up", 55, 208, 2, 1);
  wb32_drawString("Left", 25, 228, 2, 1);
  wb32_drawString("Right", 75, 228, 2, 1);
  wb32_drawString("Down", 52, 248, 2, 1);
  wb32_setTextColor(wbYELLOW, wbYELLOW); // 設定文字顏色
  wb32_drawString("A", 175, 223, 2, 2);
  wb32_drawString("FFlip", 170, 208, 2, 1);
  wb32_drawString("LFlip", 145, 228, 2, 1);
  wb32_drawString("RFlip", 195, 228, 2, 1);
  wb32_drawString("BFlip", 170, 248, 2, 1);
  wb32_initkey(); // 按鍵初始化
}

plusjason

@derek 說：

@hypnos-ma

好的，最近有 WiFiBoy 2019 改版，我會用新版 WiFiBoy 做一個 Stick commnad 範例！不過，不管哪一個版本的 WiFiBoy 都會兼容，可以用相同的 WiFi 控制方式玩 Tello 的 un-document mode。；-）

可否提供classic版的操控程式範例?

hypnos.ma

@derek

期待

derek

@hypnos-ma

好的，最近有 WiFiBoy 2019 改版，我會用新版 WiFiBoy 做一個 Stick commnad 範例！不過，不管哪一個版本的 WiFiBoy 都會兼容，可以用相同的 WiFi 控制方式玩 Tello 的 un-document mode。；-）

hypnos.ma

@derek 說：

stick command

Derek 你好, 可否提供一下使用 stick command 的簡單程序? 我的編程能力程度有限, 寫不出來呢, 有一點例子的話或許還可以

希望能用 WIFIBOY 來代替手機來控制 Tello

ricky

@thaiacepilot 說：

Is it maximum speed is 1m/s? It possible to increase speed like speed mode?

I guess it's not possible to speed up.

thaiacepilot

Is it maximum speed is 1m/s? It possible to increase speed like speed mode?

derek

@sambonnard 說：

@derek Hi ! Thanks you for your reply ! I've check on the Tello SDK, I see "Set command (xxx a)" what is the difference between this and "Control Commands" ? I want to control the drone like the DJI TELLO application and when I want to go to left when the drone go to right (it's and exemple) the ESP32 wait for the "OK" reply, can we modify it ? Thanks you

Please check this site for more info: https://tellopilots.com/forums...

Or check this pytello source code to find how to use packet mode to send realtime commands to Tello.

https://bitbucket.org/PingguSo...

Enjoy!

SamBonnard

@derek Hi ! Thanks you for your reply ! I've check on the Tello SDK, I see "Set command (xxx a)" what is the difference between this and "Control Commands" ? I want to control the drone like the DJI TELLO application and when I want to go to left when the drone go to right (it's and exemple) the ESP32 wait for the "OK" reply, can we modify it ? Thanks you

derek

@sambonnard 說：

Hello!

It's possible to control the Tello with an ESP8266 ? Thanks you

Yes. It’s no problem to control Tello with an ESP8266.

SamBonnard

Hello!

It's possible to control the Tello with an ESP8266 ? Thanks you

derek

@mendjo

It seems some power problem occurred when system init. Try to change another usb power source or change a usb cable.

mendjo

Hi Derek great tutorial.

I just tried this last code with my wifiboy on Tello and I'm geating this error on serial monitor..

http://prntscr.com/jawdk0

Do you know what I'm doing wrong?