Tello 即時飛行控制器實作


  • wbo


    .

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


  • @derek 說:

    @hypnos-ma

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

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


  • @derek 

    期待😀


  • wbo

    @hypnos-ma


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



  • @derek 說:

    stick command

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

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


  • wbo

    @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. :-(



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


  • wbo

    @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!



  • @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 


  • wbo

    @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. 



  • Hello! 

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


  • wbo

    @mendjo

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



  • 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?


Log in to reply
 

Looks like your connection to WiFiBoy.Club was lost, please wait while we try to reconnect.