wifiboy

Tello 即時飛行控制器實作

31 Oct , 2018  

 

最近大受歡迎的迷你空拍機 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(); // 按鍵初始化
}



發佈留言

發佈留言必須填寫的電子郵件地址不會公開。 必填欄位標示為 *