Rust Learning

从零开始的 Rust 学习笔记(12) —— 用 Rust CLI 来飞 Tello Drone

Leave a comment

试手了一下 DJI 和 Intel 出的 Tello 迷你四轴飞行器～

Tello 起飞之后基本上还是比较稳，但是在相对较小的室内还是会受到自身气流的干扰，在室外有风的时候也能看到 Tello 随风摆动。

在玩了一段时间之后，感觉 Tello，或者说所有四轴飞行器的硬伤还是电池。Tello 标准套装里只有 1 块电池，满电续航差不多10分钟，说实话这个时间真的还蛮短的。我的 Kit 里包含了 3 块电池，理论上「车轮战」的话似乎可行，但除非自己一直跟着飞行器，要不然实际可以飞的距离还是受限于单块电池。

此外，Kit 里包含的充电器虽然可以同时插 3 块电池，但其并不能同时为 3 块电池充电。因此就算是「车轮战」可能也坚持不了多少，带 3 块电池车轮战的话，最多玩 1 小时左右吧。

主要的缺点说完之后，来说说 Tello 比较好玩的地方。首先是有 SDK，可以自己编程上去，要想更灵活的话，也可以自己按照 API 来写 —— 也就是这篇 post 玩的，当然并没有实现全部功能。其次是非常轻，3 块电池加上飞行器本体背在包里几乎没有什么感觉。

先放几张拆箱图吧（＾Ｏ＾）

这个小盒子里面装的就是 Tello 了(((o(*ﾟ▽ﾟ*)o)))

在说明书下面还有一套备用替换的螺旋桨～♪(´ε｀ )

正面 45 度角拍一张～

背面是两个 IR，红外线距离传感器，在接收到降落的命令之后用来检测是否靠近了某个平面的（地面或者手掌之类的都行）

给 Tello 写代码的话，其实有蛮多方式，最简单的就是等 Tello 开机之后，连上它的无线热点，然后直接发送相应的格式的 UDP 包到 8889 端口上（Tello 上还有一些别的端口，对应了不同的功能 / 数据流）

我们这里就叫它 tellors 好了/ (●°u°●)​ 」

cargo new tellors
cd tellors

那么我们必然会用到 UDP 相关的功能，这里选择了 tokio 库作为我们 UDP client 的封装。于是在 Cargo.toml 中的 dependencies section 里写上～

futures = "0"
tokio = { version = "0.2", features = ["full"] }

我们这里要实现的控制功能有，起飞、降落、上升、下降、向左平飞、向右平飞、向前飞、向后飞、水平顺时针旋转、水平逆时针旋转 和 翻滚。

那么在 src/main.rs 里就先定义一个名为 TelloCommand 的枚举类型吧～

/// Basic tello drone commands
enum TelloCommand {
    Takeoff,
    Land,
    Up,
    Down,
    Left,
    Right,
    Forward,
    Backward,
    ClockWise,
    CounterClockWise,
    Flip,
    // An extra enum for quitting this cli
    TelloQuit,
}

Tello 的 IP 地址是 192.168.10.1，接受命令的端口是 8889。因此我们用 tokio::net::UdpSocket 去连接。

为了监听用户按键事件，我们需要另一个线程，Rust 中提供了 std::thread 来处理相关的操作～ 要 spawn 一个新的线程的话，一个简单的例子如下w

use std::{thread, time};

fn main() {
    // spawn a new thread
    let child = thread::spawn(move || {
        // sleep 1000ms to mimic handling some work
        thread::sleep(time::Duration::from_millis(1000));
        println!("[child] from another thread");
        // return some result
        return 233
    });
    
    println!("[main] waiting for `child`");
    let res = child.join();
    match res {
        Ok(result) => println!("[main] `child` has done its work with result {}", result),
        Err(e) => println!("[main] error happened in `child`: {:?}", e),
    };
}

那么在另一个线程里，我们用了 ncurses 库来当作用户按键事件的监听（顺便以后可以同步在 Terminal 里显示一些 Tello 的信息，而不阻塞主线程）～那么在 Cargo.toml 的 dependencies 下再加上

ncurses = "5"

同时，我们假设有如下对应的按键规则

Keyncurses 中的常量 / Keycode对应的功能UDP 包内容
↑ncurses::KEY_UP飞行高度上升up 30
↓ncurses::KEY_DOWN飞行高度下降down 30
←ncurses::KEY_LEFT向左平飞left 30
→ncurses::KEY_RIGHT向右平飞right 30
Wconst KEY_W: i32 = 119;向前飞forward 30
Sconst KEY_S: i32 = 115;向后飞back 30
Aconst KEY_A: i32 = 97;水平逆时针旋转ccw 30
Dconst KEY_D: i32 = 100;水平顺时针旋转cw 30
Fconst KEY_F: i32 = 102;向前翻滚flip f
␣const KEY_SPACE: i32 = 32;起飞 / 降落takeoff / land
ESCconst KEY_ESC: i32 = 27;降落并退出 CLIland
Qconst KEY_Q: i32 = 113;降落并退出 CLIland

上面表格里面，UDP 包的内容的部分，除了 A 和 D 所对应的数值单位是「度」以外，其余的数值单位都是「厘米」～

假设上面新的线程叫做 user event，那么它跟主线程之间就需要一个异步 channel 用于通信。在 Rust 中我们可以直接使用 std::sync::mpsc 来实现这个功能～

std::sync::mpsc 的用法非常简单～给一个小?～

use std::thread;
use std::sync::mpsc;

fn main() {
    // create a simple streaming channel
    let (sender, receiver) = mpsc::channel();
    
    // spawn a new thread
    thread::spawn(move|| {
        // and send 10 to its corresponding receiver
        sender.send(10).unwrap();
    });
    
    // receive data from receiver
    // and match the result
    match receiver.recv() {
        Ok(data) => println!("{}", data),
        Err(e) => println!("[ERROR] {}", e),
    }
}

把上面我们列出来的整合一下，就写好了 Rust 版的 Tello 简单控制～Cargo.toml 的内容如下～

[package]
name = "tellors"
version = "0.1.0"
authors = ["Ryza<[data deleted]>"]
edition = "2018"

[dependencies]
futures = "0"
tokio = { version = "0.2", features = ["full"] }
ncurses = "5"

本体的 src/main.rs 如下～

extern crate futures;
extern crate ncurses;
extern crate tokio;

use ncurses::*;
use std::error::Error;
use std::net::SocketAddr;
use std::sync::mpsc;
use std::{io, thread};
use tokio::net::UdpSocket;

/// Basic tello drone commands
enum TelloCommand {
    Takeoff,
    Land,
    Up,
    Down,
    Left,
    Right,
    Forward,
    Backward,
    ClockWise,
    CounterClockWise,
    Flip,
    // An extra enum for quitting this cli
    TelloQuit,
}

/// Send command to tello drone
///
/// # Examples
/// 
/// ```
/// send_cmd(socket, "land");
/// ```
async fn send_cmd(tello: &mut UdpSocket, cmd: &str) -> Result<usize, io::Error> {
    mvaddstr(0, 0, format!("{}!\n", cmd).as_ref());
    tello.send(cmd.as_bytes()).await
}

#[tokio::main]
async fn main() -> Result<(), Box<dyn Error>> {
    // create an asynchronous channel
    let (sender, receiver) = mpsc::channel();
    
    // IP address of tello drone
    let tello_addr: SocketAddr = "192.168.10.1:8889".parse().unwrap();
    // bind to 0.0.0.0 with system chosen port
    let local_addr: SocketAddr = "0.0.0.0:0".parse().unwrap();
    let mut socket = UdpSocket::bind(local_addr).await?;
    
    // connect to tello drone
    socket.connect(&tello_addr).await?;
    
    // spwan another thread for listen user event
    thread::spawn(move || {
        // init ncurse
        initscr();
        raw();
        // enable support for more keys
        keypad(stdscr(), true);
        // disable echo
        noecho();
        
        // more predefined keys
        const KEY_W: i32 = 119;
        const KEY_A: i32 = 97;
        const KEY_S: i32 = 115;
        const KEY_D: i32 = 100;
        const KEY_F: i32 = 102;
        const KEY_SPACE: i32 = 32;
        const KEY_ESC: i32 = 27;
        const KEY_Q: i32 = 113;
        
        // assuming the drone is on the ground
        let mut taken_off: bool = false;
        // continuously fetch pressed key
        loop {
            let keypressed = getch();
            // if key matches to any of the keys below
            // then send corresponding message
            match keypressed {
                KEY_UP => sender.send(TelloCommand::Up).unwrap(),
                KEY_DOWN => sender.send(TelloCommand::Down).unwrap(),
                KEY_LEFT => sender.send(TelloCommand::Left).unwrap(),
                KEY_RIGHT => sender.send(TelloCommand::Right).unwrap(),
                KEY_W => sender.send(TelloCommand::Forward).unwrap(),
                KEY_S => sender.send(TelloCommand::Backward).unwrap(),
                KEY_A => sender.send(TelloCommand::CounterClockWise).unwrap(),
                KEY_D => sender.send(TelloCommand::ClockWise).unwrap(),
                KEY_F => sender.send(TelloCommand::Flip).unwrap(),
                KEY_SPACE => {
                    taken_off = !taken_off;
                    if taken_off {
                        sender.send(TelloCommand::Takeoff).unwrap()
                    } else {
                        sender.send(TelloCommand::Land).unwrap()
                    }
                },
                KEY_ESC | KEY_Q => {
                    // exit if `ESC` or Q is pressed
                    sender.send(TelloCommand::Land).unwrap();
                    sender.send(TelloCommand::TelloQuit).unwrap();
                    break;
                },
                _ => (),
            }
        }

        // restore terminal
        endwin();
    });
    
    // loop on the main thread
    loop {
        // wait for message from async channel
        match receiver.recv() {
            // if the nothing goes wrong
            Ok(cmd) => {
                // match the command we sent in another thread
                match cmd {
                    TelloCommand::Takeoff => {
                        // issue `command` and then `takeoff`
                        send_cmd(&mut socket, "command").await?;
                        send_cmd(&mut socket, "takeoff").await?
                    },
                    // land
                    TelloCommand::Land => send_cmd(&mut socket, "land").await?,
                    // go up 30 centimeters
                    TelloCommand::Up => send_cmd(&mut socket, "up 30").await?,
                    // go down 30 centimeters
                    TelloCommand::Down => send_cmd(&mut socket, "down 30").await?,
                    // go left 30 centimeters
                    TelloCommand::Left => send_cmd(&mut socket, "left 30").await?,
                    // go right 30 centimeters
                    TelloCommand::Right => send_cmd(&mut socket, "right 30").await?,
                    // go forward 30 centimeters
                    TelloCommand::Forward => send_cmd(&mut socket, "forward 30").await?,
                    // go back 30 centimeters
                    TelloCommand::Backward => send_cmd(&mut socket, "back 30").await?,
                    // do a front flip
                    TelloCommand::Flip => send_cmd(&mut socket, "flip f").await?,
                    // turn 30 degrees counter clockwise 
                    TelloCommand::CounterClockWise => send_cmd(&mut socket, "ccw 30").await?,
                    // turn 30 degrees clockwise 
                    TelloCommand::ClockWise => send_cmd(&mut socket, "cw 30").await?,
                    // user pressed `ECS` or Q
                    TelloCommand::TelloQuit => break,
                };
                refresh();
            }
            Err(e) => {
                // if something goes wrong with async channel
                // then end this window
                endwin();
                // and print error message
                println!("[ERROR] {}", e);
            }
        }
    }

    Ok(())
}

Leave a Reply

Your email address will not be published. Required fields are marked *

3 × 4 =