├── .gitignore ├── examples ├── DejaVuSans.ttf ├── command_mode_state │ └── main.rs ├── command_mode_tokio │ └── main.rs ├── command_mode │ └── main.rs ├── command_mode_keyboard │ └── main.rs ├── fly_gamepad │ └── main.rs └── fly │ └── main.rs ├── .editorconfig ├── LICENCE ├── Cargo.toml ├── src ├── meta_data.rs ├── odometry.rs ├── crc.rs ├── rc_state.rs ├── drone_state.rs ├── command_mode.rs └── lib.rs ├── README.md └── Cargo.lock /.gitignore: -------------------------------------------------------------------------------- 1 | **/target 2 | **/*.rs.bk 3 | -------------------------------------------------------------------------------- /examples/DejaVuSans.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexander89/rust-tello/HEAD/examples/DejaVuSans.ttf -------------------------------------------------------------------------------- /.editorconfig: -------------------------------------------------------------------------------- 1 | root = true 2 | 3 | [*] 4 | indent_style = space 5 | indent_size = 4 6 | charset = utf-8 7 | trim_trailing_whitespace = true 8 | insert_final_newline = true 9 | -------------------------------------------------------------------------------- /examples/command_mode_state/main.rs: -------------------------------------------------------------------------------- 1 | use tello::Drone; 2 | 3 | #[tokio::main] 4 | async fn main() -> Result<(), String> { 5 | let mut drone = Drone::new("192.168.10.1:8889").command_mode(); 6 | // let mut drone = Drone::new("127.0.0.1:8880").command_mode(); 7 | drone.enable().await?; 8 | let mut state = drone.state_receiver().unwrap(); 9 | 10 | loop { 11 | if let Ok(_) = state.changed().await { 12 | if let Some(s) = state.borrow_and_update().clone() { 13 | println!( 14 | "Battery {}% Height {}dm | pitch {}° roll {}° yaw {}° | baro {}", 15 | s.bat, s.h, s.pitch, s.roll, s.yaw, s.baro 16 | ); 17 | } 18 | } 19 | } 20 | } 21 | -------------------------------------------------------------------------------- /examples/command_mode_tokio/main.rs: -------------------------------------------------------------------------------- 1 | use futures::StreamExt; 2 | use tello::Drone; 3 | use tokio_stream::wrappers::WatchStream; 4 | 5 | #[tokio::main] 6 | async fn main() -> Result<(), String> { 7 | let mut drone = Drone::new("192.168.10.1:8889").command_mode(); 8 | drone.enable().await?; 9 | let state = WatchStream::new(drone.state_receiver().unwrap()); 10 | 11 | let path = async { 12 | println!("take off {:?}", drone.take_off().await); 13 | for _ in 0..6 { 14 | println!("forward {:?}", drone.forward(30).await); 15 | println!("cw {:?}", drone.cw(60).await); 16 | } 17 | println!("land {:?}", drone.land().await); 18 | }; 19 | 20 | let mut s = Box::pin(state.take_until(path)); 21 | while let Some(s) = s.next().await { 22 | if let Some(state) = s { 23 | println!("Battery {}% Height {}", state.bat, state.h); 24 | } 25 | } 26 | Ok(()) 27 | } 28 | -------------------------------------------------------------------------------- /LICENCE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Alexander Halemba 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /examples/command_mode/main.rs: -------------------------------------------------------------------------------- 1 | use futures::executor::block_on; 2 | use std::{string::String, thread::sleep, time::Duration}; 3 | use tello::Drone; 4 | 5 | fn main() -> Result<(), String> { 6 | block_on(async { 7 | let mut drone = Drone::new("192.168.10.1:8889").command_mode(); 8 | let state = drone.state_receiver().unwrap(); 9 | drone.enable().await?; 10 | 11 | match state.recv_timeout(Duration::from_secs(5)) { 12 | Ok(message) => println!( 13 | "Battery {}% Height {}dm POS {:?}", 14 | message.bat, message.h, drone.odometry 15 | ), 16 | _ => println!("No state package received"), 17 | } 18 | 19 | println!("take_off {:?}", drone.take_off().await); 20 | sleep(Duration::from_secs(7)); 21 | 22 | for _ in 0..6 { 23 | println!("forward {:?}", drone.forward(30).await); 24 | sleep(Duration::from_secs(5)); 25 | println!("cw {:?}", drone.cw(60).await); 26 | sleep(Duration::from_secs(4)); 27 | } 28 | 29 | println!("land {:?}", drone.land().await); 30 | sleep(Duration::from_secs(3)); 31 | Ok(()) 32 | }) 33 | } 34 | -------------------------------------------------------------------------------- /Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "tello" 3 | description = "SDK for intel DJI Tello drone using the native api" 4 | version = "0.6.3" 5 | authors = ["Alex Halemba "] 6 | edition = "2018" 7 | repository = "https://github.com/Alexander89/rust-tello" 8 | license-file = "./LICENCE" 9 | license = "MIT" 10 | readme = "./README.md" 11 | keywords = ["Drone", "Tello", "Ryze", "DJI", "RyzeRobotics"] 12 | documentation = "https://docs.rs/tello" 13 | 14 | [lib] 15 | name = "tello" 16 | path = "src/lib.rs" 17 | 18 | [[example]] 19 | name = "fly" 20 | path = "examples/fly/main.rs" 21 | 22 | [[example]] 23 | name = "fly_gamepad" 24 | path = "examples/fly_gamepad/main.rs" 25 | 26 | [[example]] 27 | name = "command_mode" 28 | path = "examples/command_mode/main.rs" 29 | 30 | [[example]] 31 | name = "command_mode_tokio" 32 | path = "examples/command_mode_tokio/main.rs" 33 | required-features = ["tokio_async"] 34 | 35 | [[example]] 36 | name = "command_mode_state" 37 | path = "examples/command_mode_state/main.rs" 38 | required-features = ["tokio_async"] 39 | 40 | [[example]] 41 | name = "command_mode_keyboard" 42 | path = "examples/command_mode_keyboard/main.rs" 43 | 44 | [dependencies] 45 | byteorder = "1.4" 46 | chrono = "0.4.19" 47 | tokio = { version = "1.11.0", features = ["net", "rt", "sync", "macros", "rt-multi-thread"], optional = true } 48 | tokio-stream = { version = "0.1.7", features = ["sync"], optional = true } 49 | 50 | [dev-dependencies] 51 | sdl2 = {version = "0.34.5", features = ["ttf"]} 52 | # glib = "0.9" 53 | # gstreamer = "0.15" 54 | # gstreamer-video = "0.15" 55 | gilrs = "0.7.4" 56 | futures = "0.3.16" 57 | 58 | [features] 59 | default = ["tokio_async"] 60 | tokio_async = ["tokio", "tokio-stream"] 61 | -------------------------------------------------------------------------------- /src/meta_data.rs: -------------------------------------------------------------------------------- 1 | use std::convert::TryInto; 2 | use std::ops::Deref; 3 | use std::string::String; 4 | 5 | #[derive(Debug, Clone, Copy)] 6 | pub struct MetaData { 7 | pitch: i16, 8 | roll: i16, 9 | yaw: i16, 10 | vgx: i16, 11 | vgy: i16, 12 | vgz: i16, 13 | templ: u16, 14 | temph: u16, 15 | tof: u16, 16 | h: u16, 17 | bat: u16, 18 | baro: f32, 19 | time: u16, 20 | agx: f32, 21 | agy: f32, 22 | agz: f32, 23 | } 24 | 25 | impl MetaData { 26 | fn new() -> MetaData { 27 | MetaData { 28 | pitch: 0, 29 | roll: 0, 30 | yaw: 0, 31 | vgx: 0, 32 | vgy: 0, 33 | vgz: 0, 34 | templ: 0, 35 | temph: 0, 36 | tof: 0, 37 | h: 0, 38 | bat: 0, 39 | baro: 0.0, 40 | time: 0, 41 | agx: 0.0, 42 | agy: 0.0, 43 | agz: 0.0, 44 | } 45 | } 46 | } 47 | 48 | macro_rules! extractValue { 49 | ($d:ident, $e:ident, $v:ident) => { 50 | if $d.starts_with(stringify!($e)) { 51 | let db = $d.get(stringify!($e).len() + 1..).unwrap(); 52 | match db.parse() { 53 | Ok(value) => $v.$e = value, 54 | Err(_) => (), 55 | } 56 | $v 57 | } else { 58 | $v 59 | } 60 | }; 61 | } 62 | 63 | impl TryInto for String { 64 | type Error = (); 65 | 66 | fn try_into(self: String) -> Result { 67 | let meta_data: Vec<&str> = self.deref().split(';').collect(); 68 | let mut data = MetaData::new(); 69 | for d in meta_data.iter() { 70 | data = extractValue!(d, pitch, data); 71 | data = extractValue!(d, roll, data); 72 | data = extractValue!(d, yaw, data); 73 | data = extractValue!(d, vgx, data); 74 | data = extractValue!(d, vgy, data); 75 | data = extractValue!(d, vgz, data); 76 | data = extractValue!(d, templ, data); 77 | data = extractValue!(d, temph, data); 78 | data = extractValue!(d, tof, data); 79 | data = extractValue!(d, h, data); 80 | data = extractValue!(d, bat, data); 81 | data = extractValue!(d, baro, data); 82 | data = extractValue!(d, time, data); 83 | data = extractValue!(d, agx, data); 84 | data = extractValue!(d, agy, data); 85 | data = extractValue!(d, agz, data); 86 | } 87 | Ok(data) 88 | } 89 | } 90 | -------------------------------------------------------------------------------- /examples/command_mode_keyboard/main.rs: -------------------------------------------------------------------------------- 1 | use futures::executor::block_on; 2 | use std::string::String; 3 | use std::sync::mpsc::{self, Receiver, TryRecvError}; 4 | use std::thread::sleep; 5 | use std::time::Duration; 6 | use std::{io, thread}; 7 | 8 | use tello::Drone; 9 | 10 | fn main() -> Result<(), String> { 11 | block_on(async { 12 | let mut drone = Drone::new("192.168.10.1:8889").command_mode(); 13 | let state = drone.state_receiver().unwrap(); 14 | 15 | let stdin_channel = create_stdin_channel(); 16 | let _failed_sometimes_but_works = drone.enable().await; 17 | 'mainLoop: loop { 18 | match stdin_channel.try_recv() { 19 | Ok(input) => { 20 | let commands: Vec<&str> = input.split(' ').collect(); 21 | let send = match commands[0] { 22 | "exit" => break 'mainLoop, 23 | "streamon" => drone.video_on().await, 24 | "streamoff" => drone.video_off().await, 25 | "enable" => drone.enable().await, 26 | "start" => drone.take_off().await, 27 | "land" => drone.land().await, 28 | "down" => drone.down(commands[1].parse().unwrap_or(0)).await, 29 | "up" => drone.up(commands[1].parse().unwrap_or(0)).await, 30 | "forward" => drone.forward(commands[1].parse().unwrap_or(0)).await, 31 | "back" => drone.back(commands[1].parse().unwrap_or(0)).await, 32 | "left" => drone.left(commands[1].parse().unwrap_or(0)).await, 33 | "right" => drone.right(commands[1].parse().unwrap_or(0)).await, 34 | "cw" => drone.cw(commands[1].parse().unwrap_or(0)).await, 35 | "ccw" => drone.ccw(commands[1].parse().unwrap_or(0)).await, 36 | //"home" => drone.go_home(0, 0, 0, 20), 37 | _ => Ok(()), 38 | }; 39 | if send.is_err() { 40 | println!("{}", send.err().unwrap()) 41 | } 42 | } 43 | Err(TryRecvError::Disconnected) => panic!("Channel disconnected"), 44 | _ => (), 45 | } 46 | match state.try_recv() { 47 | Ok(message) => println!( 48 | "battery {}% height {}dm POS {:?}", 49 | message.bat, message.h, drone.odometry 50 | ), 51 | _ => (), 52 | } 53 | sleep(Duration::from_millis(100)); 54 | } 55 | Ok(()) 56 | }) 57 | } 58 | 59 | fn create_stdin_channel() -> Receiver { 60 | let (tx, rx) = mpsc::channel::(); 61 | thread::spawn(move || loop { 62 | let mut buffer = String::new(); 63 | io::stdin().read_line(&mut buffer).unwrap(); 64 | tx.send(buffer.trim().to_string()).unwrap(); 65 | }); 66 | rx 67 | } 68 | -------------------------------------------------------------------------------- /src/odometry.rs: -------------------------------------------------------------------------------- 1 | #[derive(Default, Debug, PartialEq, Clone)] 2 | pub struct Odometry { 3 | pub x: f64, 4 | pub y: f64, 5 | pub z: f64, 6 | pub rot: f64, 7 | } 8 | 9 | impl Odometry { 10 | fn translate(&mut self, x: f64, y: f64) -> () { 11 | self.x += x * self.rot.cos() - y * self.rot.sin(); 12 | self.y += x * self.rot.sin() + y * self.rot.cos(); 13 | } 14 | 15 | pub fn reset(&mut self) -> () { 16 | self.x = 0.0; 17 | self.y = 0.0; 18 | self.z = 0.0; 19 | self.rot = 0.0; 20 | } 21 | 22 | pub fn up(&mut self, z: u32) -> () { 23 | self.z += z.max(20).min(500) as f64; 24 | } 25 | pub fn down(&mut self, z: u32) -> () { 26 | self.z -= z.max(20).min(500) as f64; 27 | } 28 | pub fn right(&mut self, x: u32) -> () { 29 | let x = x.max(20).min(500) as f64; 30 | self.translate(x as f64, 0.0); 31 | } 32 | pub fn left(&mut self, x: u32) -> () { 33 | let x = x.max(20).min(500) as f64; 34 | self.translate(-x, 0.0); 35 | } 36 | pub fn forward(&mut self, y: u32) -> () { 37 | let y = y.max(20).min(500) as f64; 38 | self.translate(0.0, y); 39 | } 40 | pub fn back(&mut self, y: u32) -> () { 41 | let y = y.max(20).min(500) as f64; 42 | self.translate(0.0, -y); 43 | } 44 | pub fn cw(&mut self, rot: u32) -> () { 45 | let mut rot: f64 = rot.max(1).min(3600).into(); 46 | rot = rot / 180.0 * std::f64::consts::PI; 47 | self.rot -= rot 48 | } 49 | pub fn ccw(&mut self, rot: u32) -> () { 50 | let mut rot: f64 = rot.max(1).min(3600).into(); 51 | rot = rot / 180.0 * std::f64::consts::PI; 52 | self.rot += rot 53 | } 54 | } 55 | 56 | #[test] 57 | pub fn test_go_back_again() { 58 | let mut p = Odometry::default(); 59 | p.forward(100); 60 | p.cw(45); 61 | p.forward(100); 62 | p.cw(180); 63 | p.forward(100); 64 | p.ccw(45); 65 | p.forward(100); 66 | let rx = p.x.round(); 67 | let ry = p.y.round(); 68 | assert_eq!(rx, 0.0f64); 69 | assert_eq!(ry, 0.0f64); 70 | } 71 | #[test] 72 | pub fn test_go_right() { 73 | let mut p = Odometry::default(); 74 | p.forward(100); 75 | assert_eq!(p.x, 0.0f64); 76 | assert_eq!(p.y, 100.0f64); 77 | p.cw(90); 78 | p.forward(100); 79 | assert_eq!(p.x, 100.0f64); 80 | assert_eq!(p.y, 100.0f64); 81 | } 82 | #[test] 83 | pub fn test_go_left() { 84 | let mut p = Odometry::default(); 85 | p.forward(100); 86 | assert_eq!(p.x, 0.0f64); 87 | assert_eq!(p.y, 100.0f64); 88 | p.ccw(90); 89 | p.forward(100); 90 | assert_eq!(p.x, -100.0f64); 91 | assert_eq!(p.y, 100.0f64); 92 | } 93 | #[test] 94 | pub fn test_go_square() { 95 | let mut p = Odometry::default(); 96 | p.forward(100); 97 | assert_eq!(p.x, 0.0f64); 98 | assert_eq!(p.y, 100.0f64); 99 | p.cw(90); 100 | p.forward(100); 101 | assert_eq!(p.x.round(), 100.0f64); 102 | assert_eq!(p.y.round(), 100.0f64); 103 | p.cw(90); 104 | p.forward(200); 105 | assert_eq!(p.x.round(), 100.0f64); 106 | assert_eq!(p.y.round(), -100.0f64); 107 | p.cw(90); 108 | p.forward(200); 109 | assert_eq!(p.x.round(), -100.0f64); 110 | assert_eq!(p.y.round(), -100.0f64); 111 | } 112 | -------------------------------------------------------------------------------- /src/crc.rs: -------------------------------------------------------------------------------- 1 | const CRC8_TABLE: [u8; 256] = [ 2 | 0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20, 0xa3, 0xfd, 0x1f, 0x41, 3 | 0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e, 0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, 4 | 0x23, 0x7d, 0x9f, 0xc1, 0x42, 0x1c, 0xfe, 0xa0, 0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62, 5 | 0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d, 0x7c, 0x22, 0xc0, 0x9e, 0x1d, 0x43, 0xa1, 0xff, 6 | 0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5, 0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07, 7 | 0xdb, 0x85, 0x67, 0x39, 0xba, 0xe4, 0x06, 0x58, 0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a, 8 | 0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, 0xa7, 0xf9, 0x1b, 0x45, 0xc6, 0x98, 0x7a, 0x24, 9 | 0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b, 0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9, 10 | 0x8c, 0xd2, 0x30, 0x6e, 0xed, 0xb3, 0x51, 0x0f, 0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd, 11 | 0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, 0xd3, 0x8d, 0x6f, 0x31, 0xb2, 0xec, 0x0e, 0x50, 12 | 0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c, 0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee, 13 | 0x32, 0x6c, 0x8e, 0xd0, 0x53, 0x0d, 0xef, 0xb1, 0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73, 14 | 0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49, 0x08, 0x56, 0xb4, 0xea, 0x69, 0x37, 0xd5, 0x8b, 15 | 0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4, 0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16, 16 | 0xe9, 0xb7, 0x55, 0x0b, 0x88, 0xd6, 0x34, 0x6a, 0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8, 17 | 0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 0xb6, 0xe8, 0x0a, 0x54, 0xd7, 0x89, 0x6b, 0x35, 18 | ]; 19 | 20 | /// calculates the starting CRC8 byte for packet. 21 | pub fn crc8(pkt: impl IntoIterator) -> u8 { 22 | pkt.into_iter() 23 | .fold(0x77u8, |crc, x| CRC8_TABLE[(crc ^ x) as usize]) 24 | } 25 | 26 | const CRC16_TABLE: [u16; 256] = [ 27 | 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 28 | 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, 29 | 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399, 30 | 0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, 31 | 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 32 | 0xfbef, 0xea66, 0xd8fd, 0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, 33 | 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285, 0x430c, 0x7197, 0x601e, 34 | 0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, 35 | 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 36 | 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, 37 | 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693, 38 | 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, 39 | 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1, 0x0948, 0x3bd3, 0x2a5a, 40 | 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, 41 | 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710, 42 | 0xf3af, 0xe226, 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, 43 | 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df, 44 | 0x0c60, 0x1de9, 0x2f72, 0x3efb, 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, 45 | 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, 0xe70e, 0xf687, 0xc41c, 0xd595, 46 | 0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, 47 | 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 48 | 0x3de3, 0x2c6a, 0x1ef1, 0x0f78, 49 | ]; 50 | 51 | /// calculates the ending CRC16 bytes for packet. 52 | pub fn crc16(pkt: impl IntoIterator) -> u16 { 53 | pkt.into_iter().fold(0x3692u16, |crc, x| { 54 | CRC16_TABLE[((crc ^ x as u16) & 0xff) as usize] ^ (crc >> 8) 55 | }) 56 | } 57 | -------------------------------------------------------------------------------- /src/rc_state.rs: -------------------------------------------------------------------------------- 1 | use std::time::SystemTime; 2 | 3 | /// represent the current input to remote control the drone. 4 | #[derive(Clone, Debug, Default)] 5 | pub struct RCState { 6 | left_right: f32, 7 | forward_back: f32, 8 | turn: f32, 9 | up_down: f32, 10 | 11 | start_engines: bool, 12 | start_engines_set_time: Option, 13 | } 14 | 15 | impl RCState { 16 | /// set the rc-controller to the mode to hold down the key-combination to do an manual take_off. 17 | /// 18 | pub fn start_engines(&mut self) { 19 | self.start_engines = true; 20 | self.start_engines_set_time = Some(SystemTime::now()); 21 | } 22 | 23 | /// returns the current stick parameter to send them to the drone 24 | /// 25 | /// Actually, this is an workaround to keep the start_engines in this struct and 26 | /// don't move them to the Drone it self 27 | pub fn get_stick_parameter(&mut self) -> (f32, f32, f32, f32, bool) { 28 | if self.start_engines { 29 | if let Some(start) = self.start_engines_set_time { 30 | let elapsed = SystemTime::now().duration_since(start); 31 | if let Ok(time) = elapsed { 32 | if time.as_millis() > 350 { 33 | self.start_engines = false; 34 | } 35 | } else { 36 | self.start_engines = false; 37 | } 38 | } else { 39 | self.start_engines = false; 40 | } 41 | (-1.0, -1.0, -1.0, 1.0, true) 42 | } else { 43 | ( 44 | self.up_down, 45 | self.forward_back, 46 | self.left_right, 47 | self.turn, 48 | true, 49 | ) 50 | } 51 | } 52 | 53 | /// stop moving left or right by setting the axis to 0.0 54 | pub fn stop_left_right(&mut self) { 55 | self.left_right = 0.0; 56 | } 57 | 58 | /// set a fixed value of -1.0 to the left right axis to fly to the left 59 | pub fn go_left(&mut self) { 60 | if self.left_right > 0.0 { 61 | self.stop_left_right() 62 | } else { 63 | self.left_right = -1.0; 64 | } 65 | } 66 | 67 | /// set a fixed value of 1.0 to the left right axis to fly to the right 68 | pub fn go_right(&mut self) { 69 | if self.left_right < 0.0 { 70 | self.stop_left_right() 71 | } else { 72 | self.left_right = 1.0; 73 | } 74 | } 75 | 76 | /// set a analog value to the left right axis 77 | /// this value has to be between -1 and 1 (including), where -1 is left and 1 is right 78 | pub fn go_left_right(&mut self, value: f32) { 79 | assert!(value <= 1.0); 80 | assert!(value >= -1.0); 81 | 82 | self.left_right = value; 83 | } 84 | 85 | /// stop moving forward or back by setting the axis to 0.0 86 | pub fn stop_forward_back(&mut self) { 87 | self.forward_back = 0.0; 88 | } 89 | 90 | /// set a fixed value of -1.0 to the forward and back axis to fly back 91 | pub fn go_back(&mut self) { 92 | if self.forward_back > 0.0 { 93 | self.stop_forward_back() 94 | } else { 95 | self.forward_back = -1.0; 96 | } 97 | } 98 | 99 | /// set a fixed value of 1.0 to the forward and back axis to fly forward 100 | pub fn go_forward(&mut self) { 101 | if self.forward_back < 0.0 { 102 | self.stop_forward_back() 103 | } else { 104 | self.forward_back = 1.0; 105 | } 106 | } 107 | 108 | /// set a analog value to the forward or back axis 109 | /// this value has to be between -1 and 1 (including), where -1 is back and 1 is forward 110 | pub fn go_forward_back(&mut self, value: f32) { 111 | assert!(value <= 1.0); 112 | assert!(value >= -1.0); 113 | 114 | self.forward_back = value; 115 | } 116 | 117 | /// stop moving up or down by setting the axis to 0.0 118 | pub fn stop_up_down(&mut self) { 119 | self.up_down = 0.0; 120 | } 121 | 122 | /// set a fixed value of -1.0 to the up and down axis to raise up 123 | pub fn go_down(&mut self) { 124 | if self.up_down > 0.0 { 125 | self.stop_up_down() 126 | } else { 127 | self.up_down = -1.0; 128 | } 129 | } 130 | 131 | /// set a fixed value of 1.0 to the up and down axis to go down 132 | pub fn go_up(&mut self) { 133 | if self.up_down < 0.0 { 134 | self.stop_up_down() 135 | } else { 136 | self.up_down = 1.0; 137 | } 138 | } 139 | 140 | /// set a analog value to the up or down axis 141 | /// this value has to be between -1 and 1 (including), where -1 is going down and 1 is flying up 142 | pub fn go_up_down(&mut self, value: f32) { 143 | assert!(value <= 1.0); 144 | assert!(value >= -1.0); 145 | 146 | self.up_down = value; 147 | } 148 | 149 | /// stop turning by setting it to 0.0 150 | pub fn stop_turn(&mut self) { 151 | self.turn = 0.0; 152 | } 153 | 154 | /// set a fixed value of -1.0 to the turn axis to turn counter clock wise 155 | pub fn go_ccw(&mut self) { 156 | if self.turn > 0.0 { 157 | self.stop_turn() 158 | } else { 159 | self.turn = -1.0; 160 | } 161 | } 162 | 163 | /// set a fixed value of 1.0 to the forward and back axis to turn clock wise 164 | pub fn go_cw(&mut self) { 165 | if self.turn < 0.0 { 166 | self.stop_turn() 167 | } else { 168 | self.turn = 1.0; 169 | } 170 | } 171 | 172 | /// Set a analog value to turn the drone 173 | /// This value has to be between -1 and 1 (including), where -1 is turning ccw and 1 is turning cw 174 | pub fn turn(&mut self, value: f32) { 175 | assert!(value <= 1.0); 176 | assert!(value >= -1.0); 177 | 178 | self.turn = value; 179 | } 180 | } 181 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Tello drone 2 | 3 | There are two interfaces for the tello drone. The text based and a 4 | non-public interface, used by the native app. The guys from the 5 | [tellopilots forum](https://tellopilots.com/) did an awesome job by 6 | reverse engineer this interface and support other public repositories 7 | for go, python... 8 | 9 | This library combines the network protocol to communicate with the drone and get 10 | available meta data additionally and a remote-control framework is available to 11 | simplify the wiring to the keyboard or an joystick. 12 | 13 | In the sources you will find an example, how to create a SDL-Ui and use 14 | the keyboard to control the drone. You can run it with `cargo run --example fly` 15 | 16 | **Please keep in mind, advanced maneuvers require a bright environment. (Flip, Bounce, ...)** 17 | 18 | ## Communication 19 | 20 | When the drone gets an enable package (`drone.connect(11111);`), the Tello drone 21 | send data on two UDP channels. A the command channel (port: 8889) and B (WIP) the 22 | video channel (default: port: 11111). In the AP mode, the drone will appear with 23 | the default ip 192.168.10.1. All send calls are done synchronously. 24 | To receive the data, you have to poll the drone. Here is an example: 25 | 26 | ### Example 27 | 28 | ```rust 29 | use tello::{Drone, Message, Package, PackageData, ResponseMsg}; 30 | use std::time::Duration; 31 | 32 | fn main() -> Result<(), String> { 33 | let mut drone = Drone::new("192.168.10.1:8889"); 34 | drone.connect(11111); 35 | loop { 36 | if let Some(msg) = drone.poll() { 37 | match msg { 38 | Message::Data(Package {data: PackageData::FlightData(d), ..}) => { 39 | println!("battery {}", d.battery_percentage); 40 | } 41 | Message::Response(ResponseMsg::Connected(_)) => { 42 | println!("connected"); 43 | drone.throw_and_go().unwrap(); 44 | } 45 | _ => () 46 | } 47 | } 48 | ::std::thread::sleep(Duration::new(0, 1_000_000_000u32 / 20)); 49 | } 50 | } 51 | ``` 52 | 53 | ## Command mode 54 | 55 | You can switch the drone to the command mode. to get back to the "Free-Flight-Mode" you have to reboot the drone. 56 | The CommandMode provides following information to you: 57 | 58 | - `state_receiver(): Option>`: parsed incoming state packages from the drone. You will take the ownership, you could do this only once. 59 | - `video_receiver(): Option>>`: Video frames (h264) from the drone. You will take the ownership, you could do this only once. 60 | - `odometry: Odometry` odometer data for your movements. 61 | 62 | ### Example 63 | 64 | ```rust 65 | use futures::executor::block_on; 66 | use std::{string::String, thread::sleep, time::Duration}; 67 | use tello::Drone; 68 | 69 | fn main() -> Result<(), String> { 70 | block_on(async { 71 | let mut drone = Drone::new("192.168.10.1:8889").command_mode(); 72 | let state = drone.state_receiver().unwrap(); 73 | drone.enable().await?; 74 | 75 | // if you use "tokio_async" this will be a `tokio::sync::watch::Receiver` 76 | match state.recv_timeout(Duration::from_secs(5)) { 77 | Ok(message) => println!( 78 | "Battery {}% Height {}dm POS {:?}", 79 | message.bat, message.h, drone.odometry 80 | ), 81 | _ => println!("No state package received"), 82 | } 83 | 84 | println!("take_off {:?}", drone.take_off().await); 85 | 86 | for _ in 0..6 { 87 | println!("forward {:?}", drone.forward(30).await); 88 | println!("cw {:?}", drone.cw(60).await); 89 | } 90 | 91 | println!("land {:?}", drone.land().await); 92 | Ok(()) 93 | }) 94 | } 95 | ``` 96 | 97 | ## Remote control 98 | 99 | the poll is not only receiving messages from the drone, it will also send some default-settings, replies with acknowledgements, triggers the key frames or send the remote-control state for the live move commands. 100 | 101 | The Drone contains a rc_state to manipulate the movement. e.g.: `drone.rc_state.go_down()`, `drone.rc_state.go_forward_back(-0.7)` 102 | 103 | The following example is opening a window with SDL, handles the keyboard inputs and shows how to connect a game pad or joystick. 104 | 105 | ### Example 106 | 107 | ```rust 108 | use sdl2::event::Event; 109 | use sdl2::keyboard::Keycode; 110 | use tello::{Drone, Message, Package, PackageData, ResponseMsg}; 111 | use std::time::Duration; 112 | 113 | fn main() -> Result<(), String> { 114 | let mut drone = Drone::new("192.168.10.1:8889"); 115 | drone.connect(11111); 116 | 117 | let sdl_context = sdl2::init()?; 118 | let video_subsystem = sdl_context.video()?; 119 | let window = video_subsystem.window("TELLO drone", 1280, 720).build().unwrap(); 120 | let mut canvas = window.into_canvas().build().unwrap(); 121 | 122 | let mut event_pump = sdl_context.event_pump()?; 123 | 'running: loop { 124 | // draw some stuff 125 | canvas.clear(); 126 | // [...] 127 | 128 | // handle input from a keyboard or something like a game-pad 129 | // ue the keyboard events 130 | for event in event_pump.poll_iter() { 131 | match event { 132 | Event::Quit { .. } 133 | | Event::KeyDown { keycode: Some(Keycode::Escape), .. } => 134 | break 'running, 135 | Event::KeyDown { keycode: Some(Keycode::K), .. } => 136 | drone.take_off().unwrap(), 137 | Event::KeyDown { keycode: Some(Keycode::L), .. } => 138 | drone.land().unwrap(), 139 | Event::KeyDown { keycode: Some(Keycode::A), .. } => 140 | drone.rc_state.go_left(), 141 | Event::KeyDown { keycode: Some(Keycode::D), .. } => 142 | drone.rc_state.go_right(), 143 | Event::KeyUp { keycode: Some(Keycode::A), .. } 144 | | Event::KeyUp { keycode: Some(Keycode::D), .. } => 145 | drone.rc_state.stop_left_right(), 146 | //... 147 | } 148 | } 149 | 150 | // or use a game pad (range from -1 to 1) 151 | // drone.rc_state.go_left_right(dummy_joystick.axis.1); 152 | // drone.rc_state.go_forward_back(dummy_joystick.axis.2); 153 | // drone.rc_state.go_up_down(dummy_joystick.axis.3); 154 | // drone.rc_state.turn(dummy_joystick.axis.4); 155 | 156 | // the poll will send the move command to the drone 157 | drone.poll(); 158 | 159 | canvas.present(); 160 | ::std::thread::sleep(Duration::new(0, 1_000_000_000u32 / 20)); 161 | } 162 | } 163 | ``` 164 | -------------------------------------------------------------------------------- /src/drone_state.rs: -------------------------------------------------------------------------------- 1 | use super::PackageData; 2 | use byteorder::{LittleEndian, ReadBytesExt}; 3 | use std::io::{BufRead, Cursor, Seek, SeekFrom}; 4 | 5 | /// Represents the last received meta data from the drone 6 | /// 7 | #[derive(Debug, Clone, Default)] 8 | pub struct DroneMeta { 9 | flight: Option, 10 | wifi: Option, 11 | light: Option, 12 | } 13 | 14 | impl DroneMeta { 15 | /// returns an option of the FlightData. 16 | /// 17 | /// this will always represent the last state event if the network connection is dropped 18 | pub fn get_flight_data(&self) -> Option { 19 | self.flight.clone() 20 | } 21 | /// returns an option of the WifiInfo. 22 | /// stang 90% is max in the AP mode 23 | /// 24 | /// this will always represent the last state event if the network connection is dropped 25 | pub fn get_wifi_info(&self) -> Option { 26 | self.wifi.clone() 27 | } 28 | /// returns an option of the LightInfo. 29 | /// 30 | /// this will always represent the last state event if the network connection is dropped 31 | pub fn get_light_info(&self) -> Option { 32 | self.light.clone() 33 | } 34 | /// applies the package to the current data. 35 | /// It ignore non Meta package data and just overwrite the current metadata 36 | pub fn update(&mut self, package: &PackageData) { 37 | match package { 38 | PackageData::FlightData(fd) => self.flight = Some(fd.clone()), 39 | PackageData::WifiInfo(wifi) => self.wifi = Some(wifi.clone()), 40 | PackageData::LightInfo(li) => self.light = Some(li.clone()), 41 | _ => (), 42 | }; 43 | } 44 | } 45 | 46 | fn int16(val0: u8, val1: u8) -> i16 { 47 | if val1 != 0 { 48 | (((val0 as i32) | ((val1 as i32) << 8)) - 0x10000) as i16 49 | } else { 50 | (val0 as i16) | ((val1 as i16) << 8) 51 | } 52 | } 53 | 54 | #[derive(Clone)] 55 | pub struct FlightData { 56 | pub height: i16, 57 | pub north_speed: i16, 58 | pub east_speed: i16, 59 | pub ground_speed: i16, 60 | pub fly_time: i16, 61 | pub imu_state: bool, 62 | pub pressure_state: bool, 63 | pub down_visual_state: bool, 64 | pub power_state: bool, 65 | pub battery_state: bool, 66 | pub gravity_state: bool, 67 | pub wind_state: bool, 68 | pub imu_calibration_state: u8, 69 | pub battery_percentage: u8, 70 | pub drone_battery_left: i16, 71 | pub drone_fly_time_left: i16, 72 | 73 | pub em_sky: bool, 74 | pub em_ground: bool, 75 | pub em_open: bool, 76 | pub drone_hover: bool, 77 | pub outage_recording: bool, 78 | pub battery_low: bool, 79 | pub battery_lower: bool, 80 | pub factory_mode: bool, 81 | 82 | pub fly_mode: u8, 83 | pub throw_fly_timer: u8, 84 | pub camera_state: u8, 85 | pub electrical_machinery_state: u8, 86 | pub front_in: bool, 87 | pub front_out: bool, 88 | pub front_lsc: bool, 89 | pub temperature_height: bool, 90 | } 91 | 92 | impl std::fmt::Debug for FlightData { 93 | fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { 94 | write!( 95 | f, 96 | "FlightData {{ alt: {}, north_sp: {}, east_sp: {}, ground_sp: {}, fly_time: {}, imu_cal: {}, battery: {}, battery_left: {}, fly_time_left: {}, fly_mode: {}, throw_fly_timer: {}, camera: {}, em: {}}}", 97 | self.height, self.north_speed, self.east_speed, self.ground_speed, self.fly_time, self.imu_calibration_state, self.battery_percentage, 98 | self.drone_battery_left, self.drone_fly_time_left, self.fly_mode, self.throw_fly_timer, self.camera_state, self.electrical_machinery_state 99 | ) 100 | } 101 | } 102 | 103 | impl From> for FlightData { 104 | fn from(data: Vec) -> FlightData { 105 | FlightData { 106 | height: int16(data[0], data[1]), 107 | north_speed: int16(data[2], data[3]), 108 | east_speed: int16(data[4], data[5]), 109 | ground_speed: int16(data[6], data[7]), 110 | fly_time: int16(data[8], data[9]), 111 | 112 | imu_state: ((data[10]) & 0x1) != 0, 113 | pressure_state: ((data[10] >> 1) & 0x1) != 0, 114 | down_visual_state: ((data[10] >> 2) & 0x1) != 0, 115 | power_state: ((data[10] >> 3) & 0x1) != 0, 116 | battery_state: ((data[10] >> 4) & 0x1) != 0, 117 | gravity_state: ((data[10] >> 5) & 0x1) != 0, 118 | wind_state: ((data[10] >> 7) & 0x1) != 0, 119 | 120 | imu_calibration_state: data[11], 121 | battery_percentage: data[12], 122 | drone_battery_left: int16(data[13], data[14]), 123 | drone_fly_time_left: int16(data[15], data[16]), 124 | 125 | em_sky: ((data[17]) & 0x1) != 0, 126 | em_ground: ((data[17] >> 1) & 0x1) != 0, 127 | em_open: ((data[17] >> 2) & 0x1) != 0, 128 | drone_hover: ((data[17] >> 3) & 0x1) != 0, 129 | outage_recording: ((data[17] >> 4) & 0x1) != 0, 130 | battery_low: ((data[17] >> 5) & 0x1) != 0, 131 | battery_lower: ((data[17] >> 6) & 0x1) != 0, 132 | factory_mode: ((data[17] >> 7) & 0x1) != 0, 133 | 134 | fly_mode: data[18], 135 | throw_fly_timer: data[19], 136 | camera_state: data[20], 137 | electrical_machinery_state: data[21], 138 | 139 | front_in: ((data[22]) & 0x1) != 0, 140 | front_out: ((data[22] >> 1) & 0x1) != 0, 141 | front_lsc: ((data[22] >> 2) & 0x1) != 0, 142 | 143 | temperature_height: ((data[23]) & 0x1) != 0, 144 | } 145 | } 146 | } 147 | 148 | /// current strength of the wifi signal and distortion. 149 | /// When the drone is in the AP mode, the max strength value is 90 150 | #[derive(Debug, Clone)] 151 | pub struct WifiInfo { 152 | strength: u8, 153 | disturb: u8, 154 | } 155 | impl From> for WifiInfo { 156 | /// parse the incoming network package 157 | fn from(data: Vec) -> WifiInfo { 158 | WifiInfo { 159 | strength: data[0], 160 | disturb: data[1], 161 | } 162 | } 163 | } 164 | 165 | /// some features like a flip or bouncing is only available when the battery is charged and there is enough light 166 | /// check the FlightData for the battery state 167 | #[derive(Debug, Clone)] 168 | pub struct LightInfo { 169 | good: bool, 170 | } 171 | impl From> for LightInfo { 172 | /// parse the incoming network package 173 | fn from(data: Vec) -> LightInfo { 174 | LightInfo { good: data[0] == 0 } 175 | } 176 | } 177 | 178 | /// not complete parse log message. This message is send frequently from the drone 179 | #[derive(Debug, Clone)] 180 | pub struct LogMessage { 181 | pub id: u16, 182 | pub message: String, 183 | } 184 | impl From> for LogMessage { 185 | /// parse the incoming network package 186 | fn from(data: Vec) -> LogMessage { 187 | let mut cur = Cursor::new(data); 188 | let id: u16 = cur.read_u16::().unwrap(); 189 | cur.seek(SeekFrom::Start(19)).unwrap(); 190 | let mut msg: Vec = Vec::new(); 191 | cur.read_until(0, &mut msg).unwrap(); 192 | LogMessage { 193 | id, 194 | message: String::from_utf8(msg) 195 | .unwrap() 196 | .trim_matches('\u{0}') 197 | .to_string(), 198 | } 199 | } 200 | } 201 | -------------------------------------------------------------------------------- /examples/fly_gamepad/main.rs: -------------------------------------------------------------------------------- 1 | use sdl2::pixels::Color; 2 | use sdl2::rect::Rect; 3 | use std::path::Path; 4 | use std::string::String; 5 | use std::time::Duration; 6 | use std::ops::Deref; 7 | use gilrs::{Gilrs, Axis, Event, Button, EventType}; 8 | use std::net::UdpSocket; 9 | 10 | use tello::{Drone, Flip, Message, Package, PackageData, ResponseMsg}; 11 | 12 | const WINDOW_WIDTH: u32 = 1280; 13 | const WINDOW_HEIGHT: u32 = 720; 14 | 15 | fn main() -> Result<(), String> { 16 | 17 | let mut gilrs = Gilrs::new().map_err(|_| "gamepad not valid")?; 18 | 19 | // Iterate over all connected gamepads 20 | for (_id, gamepad) in gilrs.gamepads() { 21 | println!("{} is {:?}", gamepad.name(), gamepad.power_info()); 22 | } 23 | 24 | let mut drone = Drone::new("192.168.10.1:8889"); 25 | 26 | let sdl_context = sdl2::init()?; 27 | let video_subsystem = sdl_context.video()?; 28 | 29 | let window = video_subsystem 30 | .window("TELLO drone", WINDOW_WIDTH, WINDOW_HEIGHT) 31 | .position_centered() 32 | .opengl() 33 | .build() 34 | .expect("could not initialize video subsystem"); 35 | 36 | let mut canvas = window 37 | .into_canvas() 38 | .build() 39 | .expect("could not make a canvas"); 40 | 41 | let ttf_context = sdl2::ttf::init().expect("could not initialize font subsystem"); 42 | let texture_creator = canvas.texture_creator(); 43 | let font_path: &Path = Path::new("./examples/DejaVuSans.ttf"); 44 | let font = ttf_context.load_font(font_path, 24).expect("load font"); 45 | let keys_target = Rect::new((WINDOW_WIDTH - 250) as i32, 0, 250, 196); 46 | let key_texture = texture_creator.create_texture_from_surface( 47 | &font 48 | .render("i: connect\nk: take off\no: manual take off\np: throw 'n go\nl: land/cancel\nv: start video\nESC: Exit") 49 | .blended_wrapped(Color::RGB(0, 0, 0), 250) 50 | .unwrap() 51 | ).unwrap(); 52 | let stats_target = Rect::new(50, WINDOW_HEIGHT as i32 - 40, WINDOW_WIDTH - 100, 40); 53 | 54 | let mut land = false; 55 | let mut bounce_on = false; 56 | 57 | let socket = UdpSocket::bind("127.0.0.1:34254").expect("couldn't bind to address"); 58 | 'running: loop { 59 | canvas.set_draw_color(Color::RGB(80, 64, 255 - 80)); 60 | canvas.clear(); 61 | canvas.copy(&key_texture, None, Some(keys_target))?; 62 | 63 | // render drone state to the screen 64 | if let Some(data) = drone.drone_meta.get_flight_data() { 65 | let d = format!("{:?}", data); 66 | let surface_stats = font.render(d.deref()).blended(Color::RGB(0, 0, 0)).unwrap(); 67 | let texture_stats = texture_creator 68 | .create_texture_from_surface(&surface_stats) 69 | .unwrap(); 70 | canvas.copy(&texture_stats, None, Some(stats_target))?; 71 | } 72 | 73 | // map GamePad events to drone 74 | while let Some(Event { event, .. }) = gilrs.next_event() { 75 | match event { 76 | EventType::ButtonReleased(Button::Mode, _) => { 77 | break 'running; 78 | } 79 | EventType::ButtonPressed(Button::Start, _) => { 80 | drone.connect(11111); 81 | } 82 | EventType::ButtonPressed(Button::North, _) => { 83 | if bounce_on == false { 84 | bounce_on = true; 85 | drone.bounce().map_err(|_| "bounce failed")?; 86 | } else { 87 | bounce_on = false; 88 | drone.bounce_stop().map_err(|_| "bounce_stop failed")?; 89 | } 90 | } 91 | EventType::ButtonPressed(Button::West, _) => { 92 | drone.start_video().map_err(|_| "start_video failed")?; 93 | } 94 | EventType::ButtonPressed(Button::East, _) => { 95 | drone.throw_and_go().map_err(|_| "throw_and_go failed")?; 96 | } 97 | EventType::ButtonPressed(Button::South, _) => { 98 | drone.rc_state.start_engines(); 99 | } 100 | EventType::ButtonPressed(Button::LeftTrigger, _) => { 101 | land = false; 102 | drone.take_off().map_err(|_| "take_off failed")?; 103 | } 104 | EventType::ButtonPressed(Button::RightTrigger, _) => { 105 | if land { 106 | land = false; 107 | drone.stop_land().map_err(|_| "stop_land failed")?; 108 | } else { 109 | land = true; 110 | drone.land().map_err(|_| "land failed")?; 111 | } 112 | } 113 | // EventType::AxisChanged(Axis::LeftStickX, value, _) => { 114 | // drone.rc_state.go_left_right(value) 115 | // } 116 | // EventType::AxisChanged(Axis::LeftStickY, value, _) => { 117 | // drone.rc_state.go_forward_back(value) 118 | // } 119 | // EventType::AxisChanged(Axis::RightStickX, value, _) => { 120 | // drone.rc_state.turn(value) 121 | // } 122 | // EventType::AxisChanged(Axis::RightStickY, value, _) => { 123 | // drone.rc_state.go_up_down(value) 124 | // } 125 | EventType::AxisChanged(Axis::LeftStickX, value, _) => { 126 | drone.rc_state.turn(value) 127 | } 128 | EventType::AxisChanged(Axis::LeftStickY, value, _) => { 129 | drone.rc_state.go_up_down(value) 130 | } 131 | EventType::AxisChanged(Axis::RightStickX, value, _) => { 132 | drone.rc_state.go_left_right(value) 133 | } 134 | EventType::AxisChanged(Axis::RightStickY, value, _) => { 135 | drone.rc_state.go_forward_back(value) 136 | } 137 | EventType::ButtonPressed(Button::DPadDown, _) => { 138 | drone.flip(Flip::Back).map_err(|_| "Flip failed")?; 139 | } 140 | EventType::ButtonPressed(Button::DPadUp, _) => { 141 | drone.flip(Flip::Forward).map_err(|_| "Flip failed")?; 142 | } 143 | EventType::ButtonPressed(Button::DPadLeft, _) => { 144 | drone.flip(Flip::Left).map_err(|_| "Flip failed")?; 145 | } 146 | EventType::ButtonPressed(Button::DPadRight, _) => { 147 | drone.flip(Flip::Right).map_err(|_| "Flip failed")?; 148 | } 149 | _ => {} 150 | } 151 | }; 152 | 153 | // poll drone state and react to it 154 | while let Some(msg) = drone.poll() { 155 | match msg { 156 | Message::Data(Package {data: PackageData::FlightData(_), ..}) => { 157 | //println!("battery {}", d.battery_percentage); 158 | } 159 | Message::Data(d) /*if d.cmd != CommandIds::LogHeaderMsg*/ => { 160 | println!("msg {:?}", d.clone()); 161 | } 162 | Message::Frame(frame_id, d)=> { 163 | 164 | socket.send_to(&d, "127.0.0.1:11110").expect("couldn't send data"); 165 | println!("send frame {} {:?}", frame_id, &d[..15]); 166 | } 167 | Message::Response(ResponseMsg::Connected(_)) => { 168 | println!("connected"); 169 | } 170 | _ => () 171 | } 172 | } 173 | 174 | canvas.present(); 175 | ::std::thread::sleep(Duration::from_millis(10)); 176 | } 177 | 178 | Ok(()) 179 | } 180 | -------------------------------------------------------------------------------- /examples/fly/main.rs: -------------------------------------------------------------------------------- 1 | use sdl2::event::Event; 2 | use sdl2::keyboard::Keycode; 3 | use sdl2::pixels::Color; 4 | use sdl2::rect::Rect; 5 | use std::path::Path; 6 | use std::string::String; 7 | use std::time::Duration; 8 | 9 | use std::ops::Deref; 10 | use tello::{Drone, Flip, Message, Package, PackageData, RCState, ResponseMsg}; 11 | 12 | // extern crate glib; 13 | #[derive(Debug)] 14 | struct MissingElement(&'static str); 15 | 16 | const WINDOW_WIDTH: u32 = 1280; 17 | const WINDOW_HEIGHT: u32 = 720; 18 | 19 | fn main() -> Result<(), String> { 20 | let mut drone = Drone::new("192.168.10.1:8889"); 21 | 22 | let sdl_context = sdl2::init()?; 23 | let video_subsystem = sdl_context.video()?; 24 | 25 | let window = video_subsystem 26 | .window("TELLO drone", WINDOW_WIDTH, WINDOW_HEIGHT) 27 | .position_centered() 28 | .opengl() 29 | .build() 30 | .expect("could not initialize video subsystem"); 31 | 32 | let mut canvas = window 33 | .into_canvas() 34 | .build() 35 | .expect("could not make a canvas"); 36 | 37 | let ttf_context = sdl2::ttf::init().expect("could not initialize font subsystem"); 38 | 39 | let texture_creator = canvas.texture_creator(); 40 | let font_path: &Path = Path::new("./examples/DejaVuSans.ttf"); 41 | let font = ttf_context.load_font(font_path, 24).expect("load font"); 42 | let keys_target = Rect::new((WINDOW_WIDTH - 250) as i32, 0, 250, 196); 43 | let key_texture = texture_creator.create_texture_from_surface( 44 | &font 45 | .render("i: connect\nk: take off\no: manual take off\np: throw 'n go\nl: land/cancel\nv: start video\nESC: Exit") 46 | .blended_wrapped(Color::RGB(0, 0, 0), 250) 47 | .unwrap() 48 | ).unwrap(); 49 | let control_target = Rect::new(10, 0, 240, 112); 50 | let control_texture = texture_creator 51 | .create_texture_from_surface( 52 | &font 53 | .render("w/s: forward/back\na/d: left/right\nup/down: up/down\nleft/right: turn") 54 | .blended_wrapped(Color::RGB(0, 0, 0), 240) 55 | .unwrap(), 56 | ) 57 | .unwrap(); 58 | let stats_target = Rect::new(50, WINDOW_HEIGHT as i32 - 40, WINDOW_WIDTH - 100, 40); 59 | 60 | let mut event_pump = sdl_context.event_pump()?; 61 | 62 | let mut land = false; 63 | let mut video_on = false; 64 | let mut bounce_on = false; 65 | let mut keyboard = ControllerState::default(); 66 | 67 | 'running: loop { 68 | canvas.set_draw_color(Color::RGB(80, 64, 255 - 80)); 69 | canvas.clear(); 70 | canvas.copy(&key_texture, None, Some(keys_target))?; 71 | canvas.copy(&control_texture, None, Some(control_target))?; 72 | 73 | if let Some(data) = drone.drone_meta.get_flight_data() { 74 | let d = format!("{:?}", data); 75 | let surface_stats = font.render(d.deref()).blended(Color::RGB(0, 0, 0)).unwrap(); 76 | let texture_stats = texture_creator 77 | .create_texture_from_surface(&surface_stats) 78 | .unwrap(); 79 | canvas.copy(&texture_stats, None, Some(stats_target))?; 80 | } 81 | 82 | for event in event_pump.poll_iter() { 83 | match event { 84 | Event::Quit { .. } 85 | | Event::KeyDown { 86 | keycode: Some(Keycode::Escape), 87 | .. 88 | } => { 89 | break 'running; 90 | } 91 | Event::KeyDown { 92 | keycode: Some(Keycode::I), 93 | .. 94 | } => { 95 | drone.connect(11111); 96 | } 97 | Event::KeyDown { 98 | keycode: Some(Keycode::K), 99 | .. 100 | } => { 101 | land = false; 102 | drone.take_off().unwrap(); 103 | } 104 | Event::KeyDown { 105 | keycode: Some(Keycode::O), 106 | .. 107 | } => { 108 | drone.rc_state.start_engines(); 109 | } 110 | Event::KeyDown { 111 | keycode: Some(Keycode::P), 112 | .. 113 | } => { 114 | drone.throw_and_go().unwrap(); 115 | } 116 | Event::KeyDown { 117 | keycode: Some(Keycode::L), 118 | .. 119 | } => { 120 | if land == false { 121 | land = true; 122 | drone.land().unwrap(); 123 | } else { 124 | land = false; 125 | drone.stop_land().unwrap(); 126 | } 127 | } 128 | Event::KeyDown { 129 | keycode: Some(Keycode::V), 130 | .. 131 | } => { 132 | if video_on == false { 133 | video_on = true; 134 | drone.start_video().unwrap(); 135 | } else { 136 | video_on = false; 137 | // @TODO unknown command for stop_video 138 | drone.start_video().unwrap(); 139 | } 140 | } 141 | Event::KeyDown { 142 | keycode: Some(Keycode::H), 143 | .. 144 | } => { 145 | if bounce_on == false { 146 | bounce_on = true; 147 | drone.bounce().unwrap(); 148 | } else { 149 | bounce_on = false; 150 | drone.bounce_stop().unwrap(); 151 | } 152 | } 153 | 154 | Event::KeyDown { 155 | keycode: Some(Keycode::T), 156 | .. 157 | } => drone.flip(Flip::ForwardLeft).unwrap(), 158 | Event::KeyDown { 159 | keycode: Some(Keycode::Z), 160 | .. 161 | } => drone.flip(Flip::Forward).unwrap(), 162 | Event::KeyDown { 163 | keycode: Some(Keycode::U), 164 | .. 165 | } => drone.flip(Flip::ForwardRight).unwrap(), 166 | Event::KeyDown { 167 | keycode: Some(Keycode::G), 168 | .. 169 | } => drone.flip(Flip::Left).unwrap(), 170 | Event::KeyDown { 171 | keycode: Some(Keycode::J), 172 | .. 173 | } => drone.flip(Flip::Right).unwrap(), 174 | Event::KeyDown { 175 | keycode: Some(Keycode::B), 176 | .. 177 | } => drone.flip(Flip::BackLeft).unwrap(), 178 | Event::KeyDown { 179 | keycode: Some(Keycode::N), 180 | .. 181 | } => drone.flip(Flip::Back).unwrap(), 182 | Event::KeyDown { 183 | keycode: Some(Keycode::M), 184 | .. 185 | } => drone.flip(Flip::BackRight).unwrap(), 186 | 187 | Event::KeyDown { 188 | keycode: Some(keycode), 189 | .. 190 | } => keyboard.handle_key_down(keycode), 191 | Event::KeyUp { 192 | keycode: Some(keycode), 193 | .. 194 | } => keyboard.handle_key_up(keycode), 195 | _ => {} 196 | } 197 | } 198 | 199 | keyboard.update_rc_state(&mut drone.rc_state); 200 | 201 | if let Some(msg) = drone.poll() { 202 | match msg { 203 | Message::Data(Package {data: PackageData::FlightData(d), ..}) => { 204 | println!("battery {}", d.battery_percentage); 205 | } 206 | Message::Data(d) /*if d.cmd != CommandIds::LogHeaderMsg*/ => { 207 | println!("msg {:?}", d.clone()); 208 | } 209 | Message::Frame(frame_id, d)=> { 210 | println!("frame {} {:?}", frame_id, &d[..15]); 211 | } 212 | Message::Response(ResponseMsg::Connected(_)) => { 213 | println!("connected"); 214 | } 215 | _ => () 216 | } 217 | } 218 | 219 | canvas.present(); 220 | ::std::thread::sleep(Duration::from_millis(10)); 221 | } 222 | 223 | Ok(()) 224 | } 225 | 226 | // represent the keyboard state. 227 | // witch key is currently pressed 228 | #[derive(Clone, Debug, Default)] 229 | pub struct ControllerState { 230 | pub a_down: bool, 231 | pub d_down: bool, 232 | pub w_down: bool, 233 | pub s_down: bool, 234 | pub up_down: bool, 235 | pub down_down: bool, 236 | pub left_down: bool, 237 | pub right_down: bool, 238 | } 239 | 240 | impl ControllerState { 241 | /// handle the SDL key down event 242 | pub fn handle_key_down(&mut self, keycode: Keycode) { 243 | match keycode { 244 | Keycode::A => { 245 | self.d_down = false; 246 | self.a_down = true; 247 | } 248 | Keycode::D => { 249 | self.a_down = false; 250 | self.d_down = true; 251 | } 252 | Keycode::W => { 253 | self.s_down = false; 254 | self.w_down = true; 255 | } 256 | Keycode::S => { 257 | self.w_down = false; 258 | self.s_down = true; 259 | } 260 | Keycode::Up => { 261 | self.down_down = false; 262 | self.up_down = true; 263 | } 264 | Keycode::Down => { 265 | self.up_down = false; 266 | self.down_down = true; 267 | } 268 | Keycode::Left => { 269 | self.right_down = false; 270 | self.left_down = true; 271 | } 272 | Keycode::Right => { 273 | self.left_down = false; 274 | self.right_down = true; 275 | } 276 | _ => (), 277 | } 278 | } 279 | /// handle the SDL key up event 280 | pub fn handle_key_up(&mut self, keycode: Keycode) { 281 | match keycode { 282 | Keycode::A => self.a_down = false, 283 | Keycode::D => self.d_down = false, 284 | Keycode::W => self.w_down = false, 285 | Keycode::S => self.s_down = false, 286 | Keycode::Up => self.up_down = false, 287 | Keycode::Down => self.down_down = false, 288 | Keycode::Left => self.left_down = false, 289 | Keycode::Right => self.right_down = false, 290 | _ => (), 291 | } 292 | } 293 | 294 | pub fn update_rc_state(&self, rc_state: &mut RCState) { 295 | if self.a_down { 296 | rc_state.go_left() 297 | } else if self.d_down { 298 | rc_state.go_right() 299 | } else { 300 | rc_state.stop_left_right() 301 | } 302 | 303 | if self.w_down { 304 | rc_state.go_forward() 305 | } else if self.s_down { 306 | rc_state.go_back() 307 | } else { 308 | rc_state.stop_forward_back() 309 | } 310 | 311 | if self.up_down { 312 | rc_state.go_up() 313 | } else if self.down_down { 314 | rc_state.go_down() 315 | } else { 316 | rc_state.stop_up_down() 317 | } 318 | 319 | if self.left_down { 320 | rc_state.go_ccw() 321 | } else if self.right_down { 322 | rc_state.go_cw() 323 | } else { 324 | rc_state.stop_turn() 325 | } 326 | } 327 | } 328 | -------------------------------------------------------------------------------- /Cargo.lock: -------------------------------------------------------------------------------- 1 | # This file is automatically @generated by Cargo. 2 | # It is not intended for manual editing. 3 | version = 3 4 | 5 | [[package]] 6 | name = "autocfg" 7 | version = "0.1.7" 8 | source = "registry+https://github.com/rust-lang/crates.io-index" 9 | checksum = "1d49d90015b3c36167a20fe2810c5cd875ad504b39cff3d4eae7977e6b7c1cb2" 10 | 11 | [[package]] 12 | name = "autocfg" 13 | version = "1.0.1" 14 | source = "registry+https://github.com/rust-lang/crates.io-index" 15 | checksum = "cdb031dd78e28731d87d56cc8ffef4a8f36ca26c38fe2de700543e627f8a464a" 16 | 17 | [[package]] 18 | name = "base-x" 19 | version = "0.2.6" 20 | source = "registry+https://github.com/rust-lang/crates.io-index" 21 | checksum = "1b20b618342cf9891c292c4f5ac2cde7287cc5c87e87e9c769d617793607dec1" 22 | 23 | [[package]] 24 | name = "bitflags" 25 | version = "1.2.1" 26 | source = "registry+https://github.com/rust-lang/crates.io-index" 27 | checksum = "cf1de2fe8c75bc145a2f577add951f8134889b4795d47466a54a5c846d691693" 28 | 29 | [[package]] 30 | name = "bumpalo" 31 | version = "3.4.0" 32 | source = "registry+https://github.com/rust-lang/crates.io-index" 33 | checksum = "2e8c087f005730276d1096a652e92a8bacee2e2472bcc9715a74d2bec38b5820" 34 | 35 | [[package]] 36 | name = "byteorder" 37 | version = "1.4.3" 38 | source = "registry+https://github.com/rust-lang/crates.io-index" 39 | checksum = "14c189c53d098945499cdfa7ecc63567cf3886b3332b312a5b4585d8d3a6a610" 40 | 41 | [[package]] 42 | name = "bytes" 43 | version = "1.1.0" 44 | source = "registry+https://github.com/rust-lang/crates.io-index" 45 | checksum = "c4872d67bab6358e59559027aa3b9157c53d9358c51423c17554809a8858e0f8" 46 | 47 | [[package]] 48 | name = "cc" 49 | version = "1.0.57" 50 | source = "registry+https://github.com/rust-lang/crates.io-index" 51 | checksum = "0fde55d2a2bfaa4c9668bbc63f531fbdeee3ffe188f4662511ce2c22b3eedebe" 52 | 53 | [[package]] 54 | name = "cfg-if" 55 | version = "0.1.10" 56 | source = "registry+https://github.com/rust-lang/crates.io-index" 57 | checksum = "4785bdd1c96b2a846b2bd7cc02e86b6b3dbf14e7e53446c4f54c92a361040822" 58 | 59 | [[package]] 60 | name = "chrono" 61 | version = "0.4.19" 62 | source = "registry+https://github.com/rust-lang/crates.io-index" 63 | checksum = "670ad68c9088c2a963aaa298cb369688cf3f9465ce5e2d4ca10e6e0098a1ce73" 64 | dependencies = [ 65 | "libc", 66 | "num-integer", 67 | "num-traits", 68 | "time", 69 | "winapi", 70 | ] 71 | 72 | [[package]] 73 | name = "core-foundation" 74 | version = "0.6.4" 75 | source = "registry+https://github.com/rust-lang/crates.io-index" 76 | checksum = "25b9e03f145fd4f2bf705e07b900cd41fc636598fe5dc452fd0db1441c3f496d" 77 | dependencies = [ 78 | "core-foundation-sys", 79 | "libc", 80 | ] 81 | 82 | [[package]] 83 | name = "core-foundation-sys" 84 | version = "0.6.2" 85 | source = "registry+https://github.com/rust-lang/crates.io-index" 86 | checksum = "e7ca8a5221364ef15ce201e8ed2f609fc312682a8f4e0e3d4aa5879764e0fa3b" 87 | 88 | [[package]] 89 | name = "discard" 90 | version = "1.0.4" 91 | source = "registry+https://github.com/rust-lang/crates.io-index" 92 | checksum = "212d0f5754cb6769937f4501cc0e67f4f4483c8d2c3e1e922ee9edbe4ab4c7c0" 93 | 94 | [[package]] 95 | name = "fnv" 96 | version = "1.0.7" 97 | source = "registry+https://github.com/rust-lang/crates.io-index" 98 | checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" 99 | 100 | [[package]] 101 | name = "futures" 102 | version = "0.3.16" 103 | source = "registry+https://github.com/rust-lang/crates.io-index" 104 | checksum = "1adc00f486adfc9ce99f77d717836f0c5aa84965eb0b4f051f4e83f7cab53f8b" 105 | dependencies = [ 106 | "futures-channel", 107 | "futures-core", 108 | "futures-executor", 109 | "futures-io", 110 | "futures-sink", 111 | "futures-task", 112 | "futures-util", 113 | ] 114 | 115 | [[package]] 116 | name = "futures-channel" 117 | version = "0.3.16" 118 | source = "registry+https://github.com/rust-lang/crates.io-index" 119 | checksum = "74ed2411805f6e4e3d9bc904c95d5d423b89b3b25dc0250aa74729de20629ff9" 120 | dependencies = [ 121 | "futures-core", 122 | "futures-sink", 123 | ] 124 | 125 | [[package]] 126 | name = "futures-core" 127 | version = "0.3.16" 128 | source = "registry+https://github.com/rust-lang/crates.io-index" 129 | checksum = "af51b1b4a7fdff033703db39de8802c673eb91855f2e0d47dcf3bf2c0ef01f99" 130 | 131 | [[package]] 132 | name = "futures-executor" 133 | version = "0.3.16" 134 | source = "registry+https://github.com/rust-lang/crates.io-index" 135 | checksum = "4d0d535a57b87e1ae31437b892713aee90cd2d7b0ee48727cd11fc72ef54761c" 136 | dependencies = [ 137 | "futures-core", 138 | "futures-task", 139 | "futures-util", 140 | ] 141 | 142 | [[package]] 143 | name = "futures-io" 144 | version = "0.3.16" 145 | source = "registry+https://github.com/rust-lang/crates.io-index" 146 | checksum = "0b0e06c393068f3a6ef246c75cdca793d6a46347e75286933e5e75fd2fd11582" 147 | 148 | [[package]] 149 | name = "futures-macro" 150 | version = "0.3.16" 151 | source = "registry+https://github.com/rust-lang/crates.io-index" 152 | checksum = "c54913bae956fb8df7f4dc6fc90362aa72e69148e3f39041fbe8742d21e0ac57" 153 | dependencies = [ 154 | "autocfg 1.0.1", 155 | "proc-macro-hack", 156 | "proc-macro2", 157 | "quote", 158 | "syn", 159 | ] 160 | 161 | [[package]] 162 | name = "futures-sink" 163 | version = "0.3.16" 164 | source = "registry+https://github.com/rust-lang/crates.io-index" 165 | checksum = "c0f30aaa67363d119812743aa5f33c201a7a66329f97d1a887022971feea4b53" 166 | 167 | [[package]] 168 | name = "futures-task" 169 | version = "0.3.16" 170 | source = "registry+https://github.com/rust-lang/crates.io-index" 171 | checksum = "bbe54a98670017f3be909561f6ad13e810d9a51f3f061b902062ca3da80799f2" 172 | 173 | [[package]] 174 | name = "futures-util" 175 | version = "0.3.16" 176 | source = "registry+https://github.com/rust-lang/crates.io-index" 177 | checksum = "67eb846bfd58e44a8481a00049e82c43e0ccb5d61f8dc071057cb19249dd4d78" 178 | dependencies = [ 179 | "autocfg 1.0.1", 180 | "futures-channel", 181 | "futures-core", 182 | "futures-io", 183 | "futures-macro", 184 | "futures-sink", 185 | "futures-task", 186 | "memchr", 187 | "pin-project-lite", 188 | "pin-utils", 189 | "proc-macro-hack", 190 | "proc-macro-nested", 191 | "slab", 192 | ] 193 | 194 | [[package]] 195 | name = "gilrs" 196 | version = "0.7.4" 197 | source = "registry+https://github.com/rust-lang/crates.io-index" 198 | checksum = "122bb249f904e5f4ac73fc514b9b2ce6cce3af511f5df00ffc8000e47de6b290" 199 | dependencies = [ 200 | "fnv", 201 | "gilrs-core", 202 | "log", 203 | "stdweb", 204 | "uuid", 205 | "vec_map", 206 | ] 207 | 208 | [[package]] 209 | name = "gilrs-core" 210 | version = "0.2.6" 211 | source = "registry+https://github.com/rust-lang/crates.io-index" 212 | checksum = "43c758daf46af26d6872fe55507e3b2339779a160a06ad7a9b2a082f221209cd" 213 | dependencies = [ 214 | "core-foundation", 215 | "io-kit-sys", 216 | "libc", 217 | "libudev-sys", 218 | "log", 219 | "nix", 220 | "rusty-xinput", 221 | "stdweb", 222 | "uuid", 223 | "vec_map", 224 | "winapi", 225 | ] 226 | 227 | [[package]] 228 | name = "hermit-abi" 229 | version = "0.1.19" 230 | source = "registry+https://github.com/rust-lang/crates.io-index" 231 | checksum = "62b467343b94ba476dcb2500d242dadbb39557df889310ac77c5d99100aaac33" 232 | dependencies = [ 233 | "libc", 234 | ] 235 | 236 | [[package]] 237 | name = "io-kit-sys" 238 | version = "0.1.0" 239 | source = "registry+https://github.com/rust-lang/crates.io-index" 240 | checksum = "f21dcc74995dd4cd090b147e79789f8d65959cbfb5f0b118002db869ea3bd0a0" 241 | dependencies = [ 242 | "core-foundation-sys", 243 | "mach", 244 | ] 245 | 246 | [[package]] 247 | name = "itoa" 248 | version = "0.4.6" 249 | source = "registry+https://github.com/rust-lang/crates.io-index" 250 | checksum = "dc6f3ad7b9d11a0c00842ff8de1b60ee58661048eb8049ed33c73594f359d7e6" 251 | 252 | [[package]] 253 | name = "lazy_static" 254 | version = "1.4.0" 255 | source = "registry+https://github.com/rust-lang/crates.io-index" 256 | checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646" 257 | 258 | [[package]] 259 | name = "libc" 260 | version = "0.2.99" 261 | source = "registry+https://github.com/rust-lang/crates.io-index" 262 | checksum = "a7f823d141fe0a24df1e23b4af4e3c7ba9e5966ec514ea068c93024aa7deb765" 263 | 264 | [[package]] 265 | name = "libudev-sys" 266 | version = "0.1.4" 267 | source = "registry+https://github.com/rust-lang/crates.io-index" 268 | checksum = "3c8469b4a23b962c1396b9b451dda50ef5b283e8dd309d69033475fa9b334324" 269 | dependencies = [ 270 | "libc", 271 | "pkg-config", 272 | ] 273 | 274 | [[package]] 275 | name = "log" 276 | version = "0.4.8" 277 | source = "registry+https://github.com/rust-lang/crates.io-index" 278 | checksum = "14b6052be84e6b71ab17edffc2eeabf5c2c3ae1fdb464aae35ac50c67a44e1f7" 279 | dependencies = [ 280 | "cfg-if", 281 | ] 282 | 283 | [[package]] 284 | name = "mach" 285 | version = "0.2.3" 286 | source = "registry+https://github.com/rust-lang/crates.io-index" 287 | checksum = "86dd2487cdfea56def77b88438a2c915fb45113c5319bfe7e14306ca4cd0b0e1" 288 | dependencies = [ 289 | "libc", 290 | ] 291 | 292 | [[package]] 293 | name = "memchr" 294 | version = "2.4.0" 295 | source = "registry+https://github.com/rust-lang/crates.io-index" 296 | checksum = "b16bd47d9e329435e309c58469fe0791c2d0d1ba96ec0954152a5ae2b04387dc" 297 | 298 | [[package]] 299 | name = "mio" 300 | version = "0.7.13" 301 | source = "registry+https://github.com/rust-lang/crates.io-index" 302 | checksum = "8c2bdb6314ec10835cd3293dd268473a835c02b7b352e788be788b3c6ca6bb16" 303 | dependencies = [ 304 | "libc", 305 | "log", 306 | "miow", 307 | "ntapi", 308 | "winapi", 309 | ] 310 | 311 | [[package]] 312 | name = "miow" 313 | version = "0.3.7" 314 | source = "registry+https://github.com/rust-lang/crates.io-index" 315 | checksum = "b9f1c5b025cda876f66ef43a113f91ebc9f4ccef34843000e0adf6ebbab84e21" 316 | dependencies = [ 317 | "winapi", 318 | ] 319 | 320 | [[package]] 321 | name = "nix" 322 | version = "0.15.0" 323 | source = "registry+https://github.com/rust-lang/crates.io-index" 324 | checksum = "3b2e0b4f3320ed72aaedb9a5ac838690a8047c7b275da22711fddff4f8a14229" 325 | dependencies = [ 326 | "bitflags", 327 | "cc", 328 | "cfg-if", 329 | "libc", 330 | "void", 331 | ] 332 | 333 | [[package]] 334 | name = "ntapi" 335 | version = "0.3.4" 336 | source = "registry+https://github.com/rust-lang/crates.io-index" 337 | checksum = "7a31937dea023539c72ddae0e3571deadc1414b300483fa7aaec176168cfa9d2" 338 | dependencies = [ 339 | "winapi", 340 | ] 341 | 342 | [[package]] 343 | name = "num-integer" 344 | version = "0.1.41" 345 | source = "registry+https://github.com/rust-lang/crates.io-index" 346 | checksum = "b85e541ef8255f6cf42bbfe4ef361305c6c135d10919ecc26126c4e5ae94bc09" 347 | dependencies = [ 348 | "autocfg 0.1.7", 349 | "num-traits", 350 | ] 351 | 352 | [[package]] 353 | name = "num-traits" 354 | version = "0.2.10" 355 | source = "registry+https://github.com/rust-lang/crates.io-index" 356 | checksum = "d4c81ffc11c212fa327657cb19dd85eb7419e163b5b076bede2bdb5c974c07e4" 357 | dependencies = [ 358 | "autocfg 0.1.7", 359 | ] 360 | 361 | [[package]] 362 | name = "num_cpus" 363 | version = "1.13.0" 364 | source = "registry+https://github.com/rust-lang/crates.io-index" 365 | checksum = "05499f3756671c15885fee9034446956fff3f243d6077b91e5767df161f766b3" 366 | dependencies = [ 367 | "hermit-abi", 368 | "libc", 369 | ] 370 | 371 | [[package]] 372 | name = "pin-project-lite" 373 | version = "0.2.7" 374 | source = "registry+https://github.com/rust-lang/crates.io-index" 375 | checksum = "8d31d11c69a6b52a174b42bdc0c30e5e11670f90788b2c471c31c1d17d449443" 376 | 377 | [[package]] 378 | name = "pin-utils" 379 | version = "0.1.0" 380 | source = "registry+https://github.com/rust-lang/crates.io-index" 381 | checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184" 382 | 383 | [[package]] 384 | name = "pkg-config" 385 | version = "0.3.17" 386 | source = "registry+https://github.com/rust-lang/crates.io-index" 387 | checksum = "05da548ad6865900e60eaba7f589cc0783590a92e940c26953ff81ddbab2d677" 388 | 389 | [[package]] 390 | name = "proc-macro-hack" 391 | version = "0.5.19" 392 | source = "registry+https://github.com/rust-lang/crates.io-index" 393 | checksum = "dbf0c48bc1d91375ae5c3cd81e3722dff1abcf81a30960240640d223f59fe0e5" 394 | 395 | [[package]] 396 | name = "proc-macro-nested" 397 | version = "0.1.7" 398 | source = "registry+https://github.com/rust-lang/crates.io-index" 399 | checksum = "bc881b2c22681370c6a780e47af9840ef841837bc98118431d4e1868bd0c1086" 400 | 401 | [[package]] 402 | name = "proc-macro2" 403 | version = "1.0.28" 404 | source = "registry+https://github.com/rust-lang/crates.io-index" 405 | checksum = "5c7ed8b8c7b886ea3ed7dde405212185f423ab44682667c8c6dd14aa1d9f6612" 406 | dependencies = [ 407 | "unicode-xid", 408 | ] 409 | 410 | [[package]] 411 | name = "quote" 412 | version = "1.0.7" 413 | source = "registry+https://github.com/rust-lang/crates.io-index" 414 | checksum = "aa563d17ecb180e500da1cfd2b028310ac758de548efdd203e18f283af693f37" 415 | dependencies = [ 416 | "proc-macro2", 417 | ] 418 | 419 | [[package]] 420 | name = "rustc_version" 421 | version = "0.2.3" 422 | source = "registry+https://github.com/rust-lang/crates.io-index" 423 | checksum = "138e3e0acb6c9fb258b19b67cb8abd63c00679d2851805ea151465464fe9030a" 424 | dependencies = [ 425 | "semver", 426 | ] 427 | 428 | [[package]] 429 | name = "rusty-xinput" 430 | version = "1.2.0" 431 | source = "registry+https://github.com/rust-lang/crates.io-index" 432 | checksum = "d2aa654bc32eb9ca14cce1a084abc9dfe43949a4547c35269a094c39272db3bb" 433 | dependencies = [ 434 | "lazy_static", 435 | "log", 436 | "winapi", 437 | ] 438 | 439 | [[package]] 440 | name = "ryu" 441 | version = "1.0.5" 442 | source = "registry+https://github.com/rust-lang/crates.io-index" 443 | checksum = "71d301d4193d031abdd79ff7e3dd721168a9572ef3fe51a1517aba235bd8f86e" 444 | 445 | [[package]] 446 | name = "sdl2" 447 | version = "0.34.5" 448 | source = "registry+https://github.com/rust-lang/crates.io-index" 449 | checksum = "deecbc3fa9460acff5a1e563e05cb5f31bba0aa0c214bb49a43db8159176d54b" 450 | dependencies = [ 451 | "bitflags", 452 | "lazy_static", 453 | "libc", 454 | "sdl2-sys", 455 | ] 456 | 457 | [[package]] 458 | name = "sdl2-sys" 459 | version = "0.34.5" 460 | source = "registry+https://github.com/rust-lang/crates.io-index" 461 | checksum = "41a29aa21f175b5a41a6e26da572d5e5d1ee5660d35f9f9d0913e8a802098f74" 462 | dependencies = [ 463 | "cfg-if", 464 | "libc", 465 | "version-compare", 466 | ] 467 | 468 | [[package]] 469 | name = "semver" 470 | version = "0.9.0" 471 | source = "registry+https://github.com/rust-lang/crates.io-index" 472 | checksum = "1d7eb9ef2c18661902cc47e535f9bc51b78acd254da71d375c2f6720d9a40403" 473 | dependencies = [ 474 | "semver-parser", 475 | ] 476 | 477 | [[package]] 478 | name = "semver-parser" 479 | version = "0.7.0" 480 | source = "registry+https://github.com/rust-lang/crates.io-index" 481 | checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3" 482 | 483 | [[package]] 484 | name = "serde" 485 | version = "1.0.114" 486 | source = "registry+https://github.com/rust-lang/crates.io-index" 487 | checksum = "5317f7588f0a5078ee60ef675ef96735a1442132dc645eb1d12c018620ed8cd3" 488 | 489 | [[package]] 490 | name = "serde_derive" 491 | version = "1.0.114" 492 | source = "registry+https://github.com/rust-lang/crates.io-index" 493 | checksum = "2a0be94b04690fbaed37cddffc5c134bf537c8e3329d53e982fe04c374978f8e" 494 | dependencies = [ 495 | "proc-macro2", 496 | "quote", 497 | "syn", 498 | ] 499 | 500 | [[package]] 501 | name = "serde_json" 502 | version = "1.0.56" 503 | source = "registry+https://github.com/rust-lang/crates.io-index" 504 | checksum = "3433e879a558dde8b5e8feb2a04899cf34fdde1fafb894687e52105fc1162ac3" 505 | dependencies = [ 506 | "itoa", 507 | "ryu", 508 | "serde", 509 | ] 510 | 511 | [[package]] 512 | name = "sha1" 513 | version = "0.6.0" 514 | source = "registry+https://github.com/rust-lang/crates.io-index" 515 | checksum = "2579985fda508104f7587689507983eadd6a6e84dd35d6d115361f530916fa0d" 516 | 517 | [[package]] 518 | name = "slab" 519 | version = "0.4.4" 520 | source = "registry+https://github.com/rust-lang/crates.io-index" 521 | checksum = "c307a32c1c5c437f38c7fd45d753050587732ba8628319fbdf12a7e289ccc590" 522 | 523 | [[package]] 524 | name = "stdweb" 525 | version = "0.4.20" 526 | source = "registry+https://github.com/rust-lang/crates.io-index" 527 | checksum = "d022496b16281348b52d0e30ae99e01a73d737b2f45d38fed4edf79f9325a1d5" 528 | dependencies = [ 529 | "discard", 530 | "rustc_version", 531 | "serde", 532 | "serde_json", 533 | "stdweb-derive", 534 | "stdweb-internal-macros", 535 | "stdweb-internal-runtime", 536 | "wasm-bindgen", 537 | ] 538 | 539 | [[package]] 540 | name = "stdweb-derive" 541 | version = "0.5.3" 542 | source = "registry+https://github.com/rust-lang/crates.io-index" 543 | checksum = "c87a60a40fccc84bef0652345bbbbbe20a605bf5d0ce81719fc476f5c03b50ef" 544 | dependencies = [ 545 | "proc-macro2", 546 | "quote", 547 | "serde", 548 | "serde_derive", 549 | "syn", 550 | ] 551 | 552 | [[package]] 553 | name = "stdweb-internal-macros" 554 | version = "0.2.9" 555 | source = "registry+https://github.com/rust-lang/crates.io-index" 556 | checksum = "58fa5ff6ad0d98d1ffa8cb115892b6e69d67799f6763e162a1c9db421dc22e11" 557 | dependencies = [ 558 | "base-x", 559 | "proc-macro2", 560 | "quote", 561 | "serde", 562 | "serde_derive", 563 | "serde_json", 564 | "sha1", 565 | "syn", 566 | ] 567 | 568 | [[package]] 569 | name = "stdweb-internal-runtime" 570 | version = "0.1.5" 571 | source = "registry+https://github.com/rust-lang/crates.io-index" 572 | checksum = "213701ba3370744dcd1a12960caa4843b3d68b4d1c0a5d575e0d65b2ee9d16c0" 573 | 574 | [[package]] 575 | name = "syn" 576 | version = "1.0.74" 577 | source = "registry+https://github.com/rust-lang/crates.io-index" 578 | checksum = "1873d832550d4588c3dbc20f01361ab00bfe741048f71e3fecf145a7cc18b29c" 579 | dependencies = [ 580 | "proc-macro2", 581 | "quote", 582 | "unicode-xid", 583 | ] 584 | 585 | [[package]] 586 | name = "tello" 587 | version = "0.6.3" 588 | dependencies = [ 589 | "byteorder", 590 | "chrono", 591 | "futures", 592 | "gilrs", 593 | "sdl2", 594 | "tokio", 595 | "tokio-stream", 596 | ] 597 | 598 | [[package]] 599 | name = "time" 600 | version = "0.1.44" 601 | source = "registry+https://github.com/rust-lang/crates.io-index" 602 | checksum = "6db9e6914ab8b1ae1c260a4ae7a49b6c5611b40328a735b21862567685e73255" 603 | dependencies = [ 604 | "libc", 605 | "wasi", 606 | "winapi", 607 | ] 608 | 609 | [[package]] 610 | name = "tokio" 611 | version = "1.11.0" 612 | source = "registry+https://github.com/rust-lang/crates.io-index" 613 | checksum = "b4efe6fc2395938c8155973d7be49fe8d03a843726e285e100a8a383cc0154ce" 614 | dependencies = [ 615 | "autocfg 1.0.1", 616 | "libc", 617 | "mio", 618 | "num_cpus", 619 | "pin-project-lite", 620 | "tokio-macros", 621 | "winapi", 622 | ] 623 | 624 | [[package]] 625 | name = "tokio-macros" 626 | version = "1.3.0" 627 | source = "registry+https://github.com/rust-lang/crates.io-index" 628 | checksum = "54473be61f4ebe4efd09cec9bd5d16fa51d70ea0192213d754d2d500457db110" 629 | dependencies = [ 630 | "proc-macro2", 631 | "quote", 632 | "syn", 633 | ] 634 | 635 | [[package]] 636 | name = "tokio-stream" 637 | version = "0.1.7" 638 | source = "registry+https://github.com/rust-lang/crates.io-index" 639 | checksum = "7b2f3f698253f03119ac0102beaa64f67a67e08074d03a22d18784104543727f" 640 | dependencies = [ 641 | "futures-core", 642 | "pin-project-lite", 643 | "tokio", 644 | "tokio-util", 645 | ] 646 | 647 | [[package]] 648 | name = "tokio-util" 649 | version = "0.6.8" 650 | source = "registry+https://github.com/rust-lang/crates.io-index" 651 | checksum = "08d3725d3efa29485e87311c5b699de63cde14b00ed4d256b8318aa30ca452cd" 652 | dependencies = [ 653 | "bytes", 654 | "futures-core", 655 | "futures-sink", 656 | "log", 657 | "pin-project-lite", 658 | "tokio", 659 | ] 660 | 661 | [[package]] 662 | name = "unicode-xid" 663 | version = "0.2.1" 664 | source = "registry+https://github.com/rust-lang/crates.io-index" 665 | checksum = "f7fe0bb3479651439c9112f72b6c505038574c9fbb575ed1bf3b797fa39dd564" 666 | 667 | [[package]] 668 | name = "uuid" 669 | version = "0.8.1" 670 | source = "registry+https://github.com/rust-lang/crates.io-index" 671 | checksum = "9fde2f6a4bea1d6e007c4ad38c6839fa71cbb63b6dbf5b595aa38dc9b1093c11" 672 | 673 | [[package]] 674 | name = "vec_map" 675 | version = "0.8.2" 676 | source = "registry+https://github.com/rust-lang/crates.io-index" 677 | checksum = "f1bddf1187be692e79c5ffeab891132dfb0f236ed36a43c7ed39f1165ee20191" 678 | 679 | [[package]] 680 | name = "version-compare" 681 | version = "0.0.10" 682 | source = "registry+https://github.com/rust-lang/crates.io-index" 683 | checksum = "d63556a25bae6ea31b52e640d7c41d1ab27faba4ccb600013837a3d0b3994ca1" 684 | 685 | [[package]] 686 | name = "void" 687 | version = "1.0.2" 688 | source = "registry+https://github.com/rust-lang/crates.io-index" 689 | checksum = "6a02e4885ed3bc0f2de90ea6dd45ebcbb66dacffe03547fadbb0eeae2770887d" 690 | 691 | [[package]] 692 | name = "wasi" 693 | version = "0.10.0+wasi-snapshot-preview1" 694 | source = "registry+https://github.com/rust-lang/crates.io-index" 695 | checksum = "1a143597ca7c7793eff794def352d41792a93c481eb1042423ff7ff72ba2c31f" 696 | 697 | [[package]] 698 | name = "wasm-bindgen" 699 | version = "0.2.64" 700 | source = "registry+https://github.com/rust-lang/crates.io-index" 701 | checksum = "6a634620115e4a229108b71bde263bb4220c483b3f07f5ba514ee8d15064c4c2" 702 | dependencies = [ 703 | "cfg-if", 704 | "wasm-bindgen-macro", 705 | ] 706 | 707 | [[package]] 708 | name = "wasm-bindgen-backend" 709 | version = "0.2.64" 710 | source = "registry+https://github.com/rust-lang/crates.io-index" 711 | checksum = "3e53963b583d18a5aa3aaae4b4c1cb535218246131ba22a71f05b518098571df" 712 | dependencies = [ 713 | "bumpalo", 714 | "lazy_static", 715 | "log", 716 | "proc-macro2", 717 | "quote", 718 | "syn", 719 | "wasm-bindgen-shared", 720 | ] 721 | 722 | [[package]] 723 | name = "wasm-bindgen-macro" 724 | version = "0.2.64" 725 | source = "registry+https://github.com/rust-lang/crates.io-index" 726 | checksum = "3fcfd5ef6eec85623b4c6e844293d4516470d8f19cd72d0d12246017eb9060b8" 727 | dependencies = [ 728 | "quote", 729 | "wasm-bindgen-macro-support", 730 | ] 731 | 732 | [[package]] 733 | name = "wasm-bindgen-macro-support" 734 | version = "0.2.64" 735 | source = "registry+https://github.com/rust-lang/crates.io-index" 736 | checksum = "9adff9ee0e94b926ca81b57f57f86d5545cdcb1d259e21ec9bdd95b901754c75" 737 | dependencies = [ 738 | "proc-macro2", 739 | "quote", 740 | "syn", 741 | "wasm-bindgen-backend", 742 | "wasm-bindgen-shared", 743 | ] 744 | 745 | [[package]] 746 | name = "wasm-bindgen-shared" 747 | version = "0.2.64" 748 | source = "registry+https://github.com/rust-lang/crates.io-index" 749 | checksum = "7f7b90ea6c632dd06fd765d44542e234d5e63d9bb917ecd64d79778a13bd79ae" 750 | 751 | [[package]] 752 | name = "winapi" 753 | version = "0.3.8" 754 | source = "registry+https://github.com/rust-lang/crates.io-index" 755 | checksum = "8093091eeb260906a183e6ae1abdba2ef5ef2257a21801128899c3fc699229c6" 756 | dependencies = [ 757 | "winapi-i686-pc-windows-gnu", 758 | "winapi-x86_64-pc-windows-gnu", 759 | ] 760 | 761 | [[package]] 762 | name = "winapi-i686-pc-windows-gnu" 763 | version = "0.4.0" 764 | source = "registry+https://github.com/rust-lang/crates.io-index" 765 | checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" 766 | 767 | [[package]] 768 | name = "winapi-x86_64-pc-windows-gnu" 769 | version = "0.4.0" 770 | source = "registry+https://github.com/rust-lang/crates.io-index" 771 | checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" 772 | -------------------------------------------------------------------------------- /src/command_mode.rs: -------------------------------------------------------------------------------- 1 | use std::{convert::TryFrom, net::SocketAddr, string::FromUtf8Error, time::Duration}; 2 | 3 | #[cfg(not(feature = "tokio_async"))] 4 | use std::net::UdpSocket; 5 | 6 | #[cfg(not(feature = "tokio_async"))] 7 | use std::time::Instant; 8 | #[cfg(feature = "tokio_async")] 9 | use tokio::net::UdpSocket; 10 | #[cfg(feature = "tokio_async")] 11 | use tokio::time::{sleep, timeout}; 12 | 13 | #[cfg(not(feature = "tokio_async"))] 14 | use std::sync::mpsc; 15 | #[cfg(feature = "tokio_async")] 16 | use tokio::sync::{mpsc, watch}; 17 | 18 | #[cfg(not(feature = "tokio_async"))] 19 | type StateReceiver = mpsc::Receiver; 20 | #[cfg(feature = "tokio_async")] 21 | type StateReceiver = watch::Receiver>; 22 | 23 | use crate::odometry::Odometry; 24 | 25 | /// Command mode for your tello drone. to leave the command mode, you have to reboot the drone. 26 | /// 27 | /// The CommandMode provides following information to you: 28 | /// 29 | /// - `state_receiver(): Option>`: parsed incoming state packages from the drone. You will take the ownership, you could do this only once. 30 | /// - `video_receiver(): Option>>`: Video frames (h264) from the drone. You will take the ownership, you could do this only once. 31 | /// - `odometry: Odometry` odometer data for your movements. 32 | #[derive(Debug)] 33 | pub struct CommandMode { 34 | peer_addr: SocketAddr, 35 | state_receiver: Option>, 36 | video_receiver: Option>>, 37 | pub odometry: Odometry, 38 | } 39 | #[derive(Default, Debug, Clone)] 40 | pub struct CommandModeState { 41 | pub pitch: i16, // 0 42 | pub roll: i16, // 0 43 | pub yaw: i16, // -45 44 | pub vgx: i16, // 0 45 | pub vgy: i16, // 0 46 | pub vgz: i16, // 0 47 | pub templ: i8, // 69 48 | pub temph: i8, // 70 49 | pub tof: i16, // 10 50 | pub h: i16, // 0 51 | pub bat: u8, // 92 52 | pub baro: f32, // 548.55 53 | pub time: f32, // 0 54 | pub agx: f32, // -5.00 55 | pub agy: f32, // 0.00 56 | pub agz: f32, // -998.00 57 | } 58 | 59 | impl TryFrom<&[u8; 150]> for CommandModeState { 60 | type Error = FromUtf8Error; 61 | fn try_from(error: &[u8; 150]) -> Result { 62 | String::from_utf8(error.to_vec()).and_then(|str| { 63 | Ok(str 64 | .split(';') 65 | .fold(CommandModeState::default(), |mut acc, v| { 66 | let param: Vec<&str> = v.split(':').collect(); 67 | match (param.get(0).and_then(|v| Some(v.clone())), param.get(1)) { 68 | (Some("pitch"), Some(value)) => acc.pitch = value.parse().unwrap(), 69 | (Some("roll"), Some(value)) => acc.roll = value.parse().unwrap(), 70 | (Some("yaw"), Some(value)) => acc.yaw = value.parse().unwrap(), 71 | (Some("vgx"), Some(value)) => acc.vgx = value.parse().unwrap(), 72 | (Some("vgy"), Some(value)) => acc.vgy = value.parse().unwrap(), 73 | (Some("vgz"), Some(value)) => acc.vgz = value.parse().unwrap(), 74 | (Some("templ"), Some(value)) => acc.templ = value.parse().unwrap(), 75 | (Some("temph"), Some(value)) => acc.temph = value.parse().unwrap(), 76 | (Some("tof"), Some(value)) => acc.tof = value.parse().unwrap(), 77 | (Some("h"), Some(value)) => acc.h = value.parse().unwrap(), 78 | (Some("bat"), Some(value)) => acc.bat = value.parse().unwrap(), 79 | (Some("baro"), Some(value)) => acc.baro = value.parse().unwrap(), 80 | (Some("time"), Some(value)) => acc.time = value.parse().unwrap(), 81 | (Some("agx"), Some(value)) => acc.agx = value.parse().unwrap(), 82 | (Some("agy"), Some(value)) => acc.agy = value.parse().unwrap(), 83 | (Some("agz"), Some(value)) => acc.agz = value.parse().unwrap(), 84 | _ => (), 85 | } 86 | acc 87 | })) 88 | }) 89 | } 90 | } 91 | 92 | #[cfg(not(feature = "tokio_async"))] 93 | impl CommandMode { 94 | fn create_state_receiver() -> mpsc::Receiver { 95 | let (tx, state_receiver) = mpsc::channel::(); 96 | std::thread::spawn(move || { 97 | let state_socket = UdpSocket::bind(&SocketAddr::from(([0, 0, 0, 0], 8890))) 98 | .expect("couldn't bind to command address"); 99 | state_socket.set_nonblocking(true).unwrap(); 100 | 'udpReceiverLoop: loop { 101 | let mut buf = [0u8; 150]; 102 | match state_socket.recv(&mut buf) { 103 | Ok(_) => { 104 | if let Ok(state) = CommandModeState::try_from(&buf) { 105 | tx.send(state).unwrap() 106 | } 107 | } 108 | Err(e) => { 109 | if e.raw_os_error().unwrap_or(0) == 11 { 110 | std::thread::sleep(Duration::from_millis(500)); 111 | } else { 112 | println!("BOOM: {:?}", e.to_string()); 113 | break 'udpReceiverLoop; 114 | } 115 | } 116 | } 117 | } 118 | }); 119 | state_receiver 120 | } 121 | 122 | fn create_video_receiver(port: u16) -> mpsc::Receiver> { 123 | let (video_sender, video_receiver) = mpsc::channel::>(); 124 | std::thread::spawn(move || { 125 | let video_socket = UdpSocket::bind(&SocketAddr::from(([0, 0, 0, 0], port))) 126 | .expect("couldn't bind to command address"); 127 | video_socket.set_nonblocking(true).unwrap(); 128 | let mut res_buffer = [0u8; 20000]; 129 | let mut ptr = 0; 130 | let mut buf = [0u8; 1460]; 131 | loop { 132 | match video_socket.recv(&mut buf) { 133 | Ok(size) => { 134 | for v in 0..size { 135 | res_buffer[ptr] = buf[v]; 136 | ptr += 1; 137 | } 138 | if size < 1460 { 139 | //println!("got frame: size {}", ptr); 140 | video_sender.send(res_buffer[0..ptr].to_owned()).unwrap(); 141 | ptr = 0; 142 | res_buffer = [0u8; 20000]; 143 | } 144 | } 145 | Err(_) => { 146 | std::thread::sleep(Duration::from_millis(100)); 147 | } 148 | } 149 | } 150 | }); 151 | video_receiver 152 | } 153 | } 154 | #[cfg(feature = "tokio_async")] 155 | impl CommandMode { 156 | fn create_state_receiver() -> StateReceiver { 157 | let (tx, state_receiver) = watch::channel::>(None); 158 | tokio::spawn(async move { 159 | let state_socket = UdpSocket::bind(&SocketAddr::from(([0, 0, 0, 0], 8890))) 160 | .await 161 | .expect("couldn't bind to command address"); 162 | 163 | let mut buf = [0u8; 150]; 164 | while let Ok(_) = state_socket.recv_from(&mut buf).await { 165 | // println!("{:?} bytes received from {:?}", len, addr); 166 | if let Ok(data) = CommandModeState::try_from(&buf) { 167 | let _ = tx.send(Some(data)); 168 | } 169 | } 170 | }); 171 | state_receiver 172 | } 173 | 174 | fn create_video_receiver(port: u16) -> mpsc::Receiver> { 175 | let (video_sender, video_receiver) = mpsc::channel::>(50); 176 | tokio::spawn(async move { 177 | let video_socket = UdpSocket::bind(&SocketAddr::from(([0, 0, 0, 0], port))) 178 | .await 179 | .expect("couldn't bind to command address"); 180 | 181 | let mut res_buffer = [0u8; 20000]; 182 | let mut ptr = 0; 183 | let mut buf = [0u8; 1460]; 184 | loop { 185 | while let Ok((size, _)) = video_socket.recv_from(&mut buf).await { 186 | for v in 0..size { 187 | res_buffer[ptr] = buf[v]; 188 | ptr += 1; 189 | } 190 | if size < 1460 { 191 | // println!("got frame: size {}", ptr); 192 | let _ = video_sender.send(res_buffer[0..ptr].to_owned()); 193 | ptr = 0; 194 | res_buffer = [0u8; 20000]; 195 | } 196 | } 197 | } 198 | }); 199 | video_receiver 200 | } 201 | } 202 | 203 | impl From for CommandMode { 204 | /// Constructs a new CommandMode from a SocketAddr. 205 | /// 206 | /// The state and the video frames receivers are spawned and provide those information 207 | /// if the drone already sends them. Otherwise you have to `enable()` the drone fist. 208 | fn from(peer_addr: SocketAddr) -> CommandMode { 209 | Self { 210 | peer_addr, 211 | odometry: Odometry::default(), 212 | state_receiver: Some(Self::create_state_receiver()), 213 | video_receiver: Some(Self::create_video_receiver(11111)), 214 | } 215 | } 216 | } 217 | 218 | impl CommandMode { 219 | /// Constructs a new CommandMode from a ip address `:`. 220 | /// 221 | /// The state and the video frames receivers are spawned and provide those information 222 | /// if the drone already sends them. Otherwise you have to `enable()` the drone fist. 223 | pub async fn new(ip: &str) -> Result { 224 | Ok(Self::from(ip.parse::().unwrap())) 225 | } 226 | /// Take over the ownership of the state receiver. This method returns once the receiver and 227 | /// returns `None` afterwards 228 | /// 229 | /// If you using `tokio_async` you will always get the last known value. otherwise, you will 230 | /// get a channel of the incoming data. 231 | pub fn state_receiver(&mut self) -> Option> { 232 | let mut recv = None; 233 | std::mem::swap(&mut recv, &mut self.state_receiver); 234 | recv 235 | } 236 | 237 | /// Take over the ownership of the video receiver. This method returns once the receiver and 238 | /// returns `None` afterwards 239 | pub fn video_receiver(&mut self) -> Option>> { 240 | let mut recv = None; 241 | std::mem::swap(&mut recv, &mut self.video_receiver); 242 | recv 243 | } 244 | } 245 | 246 | #[cfg(feature = "tokio_async")] 247 | impl CommandMode { 248 | async fn send_command(&self, command: Vec) -> Result<(), String> { 249 | let peer = self.peer_addr.clone(); 250 | let l = tokio::spawn(async move { 251 | let socket = UdpSocket::bind("0.0.0.0:8889") 252 | .await 253 | .map_err(|e| format!("can't create socket: {:?}", e))?; 254 | 255 | socket 256 | .send_to(&command, peer) 257 | .await 258 | .map_err(|e| format!("Failed to send command to drone: {:?}", e))?; 259 | 260 | let mut buf = [0u8; 64]; 261 | let res = timeout(Duration::new(30, 0), socket.recv(&mut buf)).await; 262 | 263 | match res { 264 | Err(_) => Err(format!("timeout")), 265 | Ok(Err(e)) => { 266 | // 11 = Resource temporarily unavailable 267 | if let Some(11) = e.raw_os_error() { 268 | sleep(Duration::from_millis(300)).await; 269 | // println!("I should restart the thing !?"); 270 | Err(format!("retry?")) 271 | } else { 272 | return Err(format!("socket error {:?}", e)); 273 | } 274 | } 275 | Ok(Ok(bytes)) => { 276 | // println!("got data {}, {:?}", bytes, buf[..bytes].to_vec()); 277 | return String::from_utf8(buf[..bytes].to_vec()) 278 | .map_err(|_| format!("Failed to read data {:?}", buf)) 279 | .and_then(|res| { 280 | if res.starts_with("ok") { 281 | // println!( 282 | // "got OK for {:?}!", 283 | // String::from_utf8(command.to_vec()).unwrap() 284 | // ); 285 | Ok(()) 286 | } else if res.starts_with("error") { 287 | Err(res) 288 | } else { 289 | Err("Unknown response".to_string()) 290 | } 291 | }); 292 | } 293 | } 294 | }); 295 | l.await.unwrap() 296 | } 297 | } 298 | 299 | #[cfg(not(feature = "tokio_async"))] 300 | impl CommandMode { 301 | async fn send_command(&self, command: Vec) -> Result<(), String> { 302 | let timeout = Instant::now(); 303 | async move { 304 | let socket = UdpSocket::bind("0.0.0.0:8889") 305 | .map_err(|e| format!("can't create socket: {:?}", e))?; 306 | socket 307 | .set_nonblocking(true) 308 | .map_err(|e| format!("set to non-Blocking failed: {:?}", e))?; 309 | { 310 | // clear socket if something is left in there 311 | let mut buf = [0u8; 4192]; 312 | let _ignore = socket.recv(&mut buf); 313 | } 314 | socket 315 | .send_to(&command, self.peer_addr) 316 | .map_err(|e| format!("Failed to send command to drone: {:?}", e))?; 317 | 318 | let mut buf = [0u8; 64]; 319 | loop { 320 | let res = socket.recv(&mut buf); 321 | match res { 322 | Err(e) => { 323 | // 11 = Resource temporarily unavailable 324 | if let Some(11) = e.raw_os_error() { 325 | if timeout.elapsed() > Duration::new(30, 0) { 326 | break Err("timeout".to_string()); 327 | } 328 | std::thread::sleep(Duration::from_millis(300)); 329 | } else { 330 | break Err(format!("socket error {:?}", e)); 331 | } 332 | } 333 | Ok(bytes) => { 334 | break String::from_utf8(buf[..bytes].to_vec()) 335 | .map_err(|_| format!("Failed to read data {:?}", buf)) 336 | .and_then(|res| { 337 | if res.starts_with("ok") { 338 | // println!( 339 | // "got OK for {:?}!", 340 | // String::from_utf8(command.to_vec()).unwrap() 341 | // ); 342 | Ok(()) 343 | } else if res.starts_with("error") { 344 | Err(res) 345 | } else { 346 | Err("Unknown response".to_string()) 347 | } 348 | }); 349 | } 350 | } 351 | } 352 | } 353 | .await 354 | } 355 | } 356 | 357 | impl CommandMode { 358 | /// enables the drone. This command should be the first one you send. 359 | /// 360 | /// Note: There is no disable(). you have to power-cycle the drone to get it 361 | /// back to the normal mode. 362 | pub async fn enable(&self) -> Result<(), String> { 363 | self.send_command("command".into()).await 364 | } 365 | /// Emergency will stop the motors immediately without landing 366 | pub async fn emergency(&self) -> Result<(), String> { 367 | self.send_command("emergency".into()).await 368 | } 369 | /// starts the drone to 1 meter above the ground 370 | pub async fn take_off(&mut self) -> Result<(), String> { 371 | let r = self.send_command("takeoff".into()).await; 372 | self.odometry.reset(); 373 | self.odometry.up(100); 374 | r 375 | } 376 | /// Land the drone 377 | pub async fn land(&self) -> Result<(), String> { 378 | self.send_command("land".into()).await 379 | } 380 | /// Enable the drone to send video frames to the 11111 port of the command sender IP 381 | pub async fn video_on(&self) -> Result<(), String> { 382 | self.send_command("streamon".into()).await 383 | } 384 | /// Disable the video stream 385 | pub async fn video_off(&self) -> Result<(), String> { 386 | self.send_command("streamoff".into()).await 387 | } 388 | /// move upwards for 20-500 cm 389 | pub async fn up(&mut self, step: u32) -> Result<(), String> { 390 | let step_norm = step.min(500).max(20); 391 | let command = format!("up {}", step_norm); 392 | self.send_command(command.into()) 393 | .await 394 | .and_then(|_| Ok(self.odometry.up(step_norm))) 395 | } 396 | /// move downwards for 20-500 cm (if possible) 397 | pub async fn down(&mut self, step: u32) -> Result<(), String> { 398 | let step_norm = step.min(500).max(20); 399 | let command = format!("down {}", step_norm); 400 | self.send_command(command.into()) 401 | .await 402 | .and_then(|_| Ok(self.odometry.down(step_norm))) 403 | } 404 | /// move to the left for 20-500 cm 405 | pub async fn left(&mut self, step: u32) -> Result<(), String> { 406 | let step_norm = step.min(500).max(20); 407 | let command = format!("left {}", step_norm); 408 | self.send_command(command.into()) 409 | .await 410 | .and_then(|_| Ok(self.odometry.left(step_norm))) 411 | } 412 | /// move to the right for 20-500 cm 413 | pub async fn right(&mut self, step: u32) -> Result<(), String> { 414 | let step_norm = step.min(500).max(20); 415 | let command = format!("right {}", step_norm); 416 | self.send_command(command.into()) 417 | .await 418 | .and_then(|_| Ok(self.odometry.right(step_norm))) 419 | } 420 | /// move forwards for 20-200 cm 421 | pub async fn forward(&mut self, step: u32) -> Result<(), String> { 422 | let step_norm = step.min(500).max(20); 423 | let command = format!("forward {}", step_norm); 424 | self.send_command(command.into()) 425 | .await 426 | .and_then(|_| Ok(self.odometry.forward(step_norm))) 427 | } 428 | /// move backwards for 20 - 500 cm 429 | pub async fn back(&mut self, step: u32) -> Result<(), String> { 430 | let step_norm = step.min(500).max(20); 431 | let command = format!("back {}", step_norm); 432 | self.send_command(command.into()) 433 | .await 434 | .and_then(|_| Ok(self.odometry.back(step_norm))) 435 | } 436 | /// turn clockwise for 0 - 3600 degrees (10 times 360) 437 | pub async fn cw(&mut self, step: u32) -> Result<(), String> { 438 | let command = format!("cw {}", step); 439 | let step_norm = step.min(3600).max(1); 440 | self.send_command(command.into()) 441 | .await 442 | .and_then(|_| Ok(self.odometry.cw(step_norm))) 443 | } 444 | /// turn counter clockwise for 0 - 3600 degrees (10 times 360) 445 | pub async fn ccw(&mut self, step: u32) -> Result<(), String> { 446 | let step_norm = step.min(3600).max(1); 447 | let command = format!("ccw {}", step); 448 | self.send_command(command.into()) 449 | .await 450 | .and_then(|_| Ok(self.odometry.ccw(step_norm))) 451 | } 452 | 453 | /// Go to a given position in the 3D space. 454 | /// 455 | /// - `x`, `y`, `z` 0 or (-)20 - (-)500 cm 456 | /// - `speed` speed in centimeter per second 457 | pub async fn go_to(&mut self, x: i32, y: i32, z: i32, speed: u8) -> Result<(), String> { 458 | let x_dir = x > 0; 459 | let y_dir = y > 0; 460 | let z_dir = z > 0; 461 | 462 | let mut x_norm = (x == 0).then(|| 0).unwrap_or(x.abs().min(500).max(20)); 463 | x_norm = x_dir.then(|| x_norm).unwrap_or(x_norm * -1); 464 | 465 | let mut y_norm = (y == 0).then(|| 0).unwrap_or(y.abs().min(500).max(20)); 466 | y_norm = y_dir.then(|| y_norm).unwrap_or(y_norm * -1); 467 | 468 | let mut z_norm = (z == 0).then(|| 0).unwrap_or(z.abs().min(500).max(20)); 469 | z_norm = z_dir.then(|| z_norm).unwrap_or(z_norm * -1); 470 | 471 | let speed_norm = speed.min(100).max(10); 472 | let command = format!("go {} {} {} {}", x_norm, y_norm, z_norm, speed_norm); 473 | // println!("{}", command); 474 | self.send_command(command.into()).await.and_then(|_| { 475 | if x_dir { 476 | self.odometry.forward(x_norm.abs() as u32); 477 | } else { 478 | self.odometry.back(x_norm.abs() as u32); 479 | } 480 | if y_dir { 481 | self.odometry.right(y_norm.abs() as u32); 482 | } else { 483 | self.odometry.left(y_norm.abs() as u32); 484 | } 485 | if z_dir { 486 | self.odometry.up(z_norm.abs() as u32); 487 | } else { 488 | self.odometry.down(z_norm.abs() as u32); 489 | } 490 | Ok(()) 491 | }) 492 | } 493 | 494 | /// Moves in a curve parsing the first point to the second point in the shortest path. 495 | /// 496 | /// The radius could not be to large and the distance cold not exceed the 500 cm 497 | /// the minimal distance to go is 0 or 20cm on `x`,`y`,`z` 498 | pub async fn curve( 499 | &mut self, 500 | x1: i32, 501 | y1: i32, 502 | z1: i32, 503 | x2: i32, 504 | y2: i32, 505 | z2: i32, 506 | speed: u8, 507 | ) -> Result<(), String> { 508 | let x1_dir = x1 > 0; 509 | let y1_dir = y1 > 0; 510 | let z1_dir = z1 > 0; 511 | let x2_dir = x2 > 0; 512 | let y2_dir = y2 > 0; 513 | let z2_dir = z2 > 0; 514 | 515 | let mut x1_norm = (x1 == 0).then(|| 0).unwrap_or(x1.min(500).max(20)); 516 | x1_norm = x1_dir.then(|| x1_norm).unwrap_or(x1_norm * -1); 517 | let mut y1_norm = (y1 == 0).then(|| 0).unwrap_or(y1.min(500).max(20)); 518 | y1_norm = y1_dir.then(|| y1_norm).unwrap_or(y1_norm * -1); 519 | let mut z1_norm = (z1 == 0).then(|| 0).unwrap_or(z1.min(500).max(20)); 520 | z1_norm = z1_dir.then(|| z1_norm).unwrap_or(z1_norm * -1); 521 | let mut x2_norm = (x2 == 0).then(|| 0).unwrap_or(x2.min(500).max(20)); 522 | x2_norm = x2_dir.then(|| x2_norm).unwrap_or(x2_norm * -1); 523 | let mut y2_norm = (y2 == 0).then(|| 0).unwrap_or(y2.min(500).max(20)); 524 | y2_norm = y2_dir.then(|| y2_norm).unwrap_or(y2_norm * -1); 525 | let mut z2_norm = (z2 == 0).then(|| 0).unwrap_or(z2.min(500).max(20)); 526 | z2_norm = z2_dir.then(|| z2_norm).unwrap_or(z2_norm * -1); 527 | let speed_norm = speed.min(100).max(10); 528 | let command = format!( 529 | "curve {} {} {} {} {} {} {}", 530 | x1_norm, y1_norm, z1_norm, x2_norm, y2_norm, z2_norm, speed_norm 531 | ); 532 | self.send_command(command.into()).await.and_then(|_| { 533 | if x2_dir { 534 | self.odometry.forward(x2_norm.abs() as u32); 535 | } else { 536 | self.odometry.back(x2_norm.abs() as u32); 537 | } 538 | if y2_dir { 539 | self.odometry.right(y2_norm.abs() as u32); 540 | } else { 541 | self.odometry.left(y2_norm.abs() as u32); 542 | } 543 | if z2_dir { 544 | self.odometry.up(z2_norm.abs() as u32); 545 | } else { 546 | self.odometry.down(z2_norm.abs() as u32); 547 | } 548 | Ok(()) 549 | }) 550 | } 551 | 552 | /// set the speed for the forward, backward, right, left, up, down motion 553 | pub async fn speed(&self, speed: u8) -> Result<(), String> { 554 | // println!("speed"); 555 | let normalized_speed = speed.min(100).max(10); 556 | let command = format!("speed {}", normalized_speed); 557 | self.send_command(command.into()).await 558 | } 559 | } 560 | -------------------------------------------------------------------------------- /src/lib.rs: -------------------------------------------------------------------------------- 1 | //! # Tello drone 2 | //! 3 | //! There are two interfaces for the tello drone. The text based and a 4 | //! non-public interface, used by the native app. The guys from the 5 | //! [tellopilots forum](https://tellopilots.com/) did an awesome job by 6 | //! reverse engineer this interface and support other public repositories 7 | //! for go, python... 8 | //! 9 | //! This library combines the network protocol to communicate with the drone and get 10 | //! available meta data additionally and a remote-control framework is available to 11 | //! simplify the wiring to the keyboard or an joystick. 12 | //! 13 | //! In the sources you will find an example, how to create a SDL-Ui and use 14 | //! the keyboard to control the drone. You can run it with `cargo run --example fly` 15 | //! 16 | //! **Please keep in mind, advanced maneuvers require a bright environment. (Flip, Bounce, ...)** 17 | //! 18 | //! ## Communication 19 | //! 20 | //! When the drone gets an enable package (`drone.connect(11111);`), the Tello drone 21 | //! send data on two UDP channels. A the command channel (port: 8889) and B the 22 | //! video channel (default: port: 11111). In the AP mode, the drone will appear with 23 | //! the default ip 192.168.10.1. All send calls are done synchronously. 24 | //! To receive the data, you have to poll the drone. Here is an example: 25 | //! 26 | //! 27 | //! ### Example 28 | //! 29 | //! ``` 30 | //! use tello::{Drone, Message, Package, PackageData, ResponseMsg}; 31 | //! use std::time::Duration; 32 | //! 33 | //! fn main() -> Result<(), String> { 34 | //! let mut drone = Drone::new("192.168.10.1:8889"); 35 | //! drone.connect(11111); 36 | //! loop { 37 | //! if let Some(msg) = drone.poll() { 38 | //! match msg { 39 | //! Message::Data(Package {data: PackageData::FlightData(d), ..}) => { 40 | //! println!("battery {}", d.battery_percentage); 41 | //! } 42 | //! Message::Frame(frame_id, data) => { 43 | //! println!("frame {} {:?}", frame_id, data); 44 | //! } 45 | //! Message::Response(ResponseMsg::Connected(_)) => { 46 | //! println!("connected"); 47 | //! drone.throw_and_go().unwrap(); 48 | //! } 49 | //! _ => () 50 | //! } 51 | //! } 52 | //! ::std::thread::sleep(Duration::new(0, 1_000_000_000u32 / 20)); 53 | //! } 54 | //! } 55 | //! ``` 56 | //! 57 | //! ## Remote control 58 | //! 59 | //! The poll is not only receiving messages from the drone, it will also send some default-settings, 60 | //! replies with acknowledgements, triggers the key frames or send the remote-control state for the 61 | //! live move commands. 62 | //! 63 | //! The Drone contains a rc_state to manipulate the movement. e.g.: `drone.rc_state.go_down()`, 64 | //! `drone.rc_state.go_forward_back(-0.7)` 65 | //! 66 | //! The following example is opening a window with SDL, handles the keyboard inputs and shows how to connect a 67 | //! game pad or joystick. 68 | //! 69 | //! 70 | //! ### Examples 71 | //! 72 | //! ``` 73 | //! use sdl2::event::Event; 74 | //! use sdl2::keyboard::Keycode; 75 | //! use tello::{Drone, Message, Package, PackageData, ResponseMsg}; 76 | //! use std::time::Duration; 77 | //! 78 | //! fn main() -> Result<(), String> { 79 | //! let mut drone = Drone::new("192.168.10.1:8889"); 80 | //! drone.connect(11111); 81 | //! 82 | //! let sdl_context = sdl2::init()?; 83 | //! let video_subsystem = sdl_context.video()?; 84 | //! let window = video_subsystem.window("TELLO drone", 1280, 720).build().unwrap(); 85 | //! let mut canvas = window.into_canvas().build().unwrap(); 86 | //! 87 | //! let mut event_pump = sdl_context.event_pump()?; 88 | //! 'running: loop { 89 | //! // draw some stuff 90 | //! canvas.clear(); 91 | //! // [...] 92 | //! 93 | //! // handle input from a keyboard or something like a game-pad 94 | //! // ue the keyboard events 95 | //! for event in event_pump.poll_iter() { 96 | //! match event { 97 | //! Event::Quit { .. } 98 | //! | Event::KeyDown { keycode: Some(Keycode::Escape), .. } => 99 | //! break 'running, 100 | //! Event::KeyDown { keycode: Some(Keycode::K), .. } => 101 | //! drone.take_off().unwrap(), 102 | //! Event::KeyDown { keycode: Some(Keycode::L), .. } => 103 | //! drone.land().unwrap(), 104 | //! Event::KeyDown { keycode: Some(Keycode::A), .. } => 105 | //! drone.rc_state.go_left(), 106 | //! Event::KeyDown { keycode: Some(Keycode::D), .. } => 107 | //! drone.rc_state.go_right(), 108 | //! Event::KeyUp { keycode: Some(Keycode::A), .. } 109 | //! | Event::KeyUp { keycode: Some(Keycode::D), .. } => 110 | //! drone.rc_state.stop_left_right(), 111 | //! //... 112 | //! } 113 | //! } 114 | //! 115 | //! // or use a game pad (range from -1 to 1) 116 | //! // drone.rc_state.go_left_right(dummy_joystick.axis.1); 117 | //! // drone.rc_state.go_forward_back(dummy_joystick.axis.2); 118 | //! // drone.rc_state.go_up_down(dummy_joystick.axis.3); 119 | //! // drone.rc_state.turn(dummy_joystick.axis.4); 120 | //! 121 | //! // the poll will send the move command to the drone 122 | //! drone.poll(); 123 | //! 124 | //! canvas.present(); 125 | //! ::std::thread::sleep(Duration::new(0, 1_000_000_000u32 / 20)); 126 | //! } 127 | //! } 128 | //! ``` 129 | 130 | use byteorder::{LittleEndian, ReadBytesExt, WriteBytesExt}; 131 | use chrono::prelude::*; 132 | use crc::{crc16, crc8}; 133 | use drone_state::{FlightData, LightInfo, LogMessage, WifiInfo}; 134 | use std::convert::TryFrom; 135 | use std::io::{Cursor, Read, Seek, SeekFrom, Write}; 136 | use std::net::{SocketAddr, UdpSocket}; 137 | use std::sync::atomic::{AtomicU16, Ordering}; 138 | use std::time::SystemTime; 139 | 140 | pub mod command_mode; 141 | mod crc; 142 | pub mod drone_state; 143 | pub mod odometry; 144 | mod rc_state; 145 | 146 | pub use command_mode::CommandMode; 147 | pub use drone_state::DroneMeta; 148 | pub use rc_state::RCState; 149 | 150 | static SEQ_NO: AtomicU16 = AtomicU16::new(1); 151 | 152 | type Result = std::result::Result<(), ()>; 153 | 154 | /// The video data itself is just H264 encoded YUV420p 155 | #[derive(Debug, Clone)] 156 | struct VideoSettings { 157 | pub port: u16, 158 | pub enabled: bool, 159 | pub mode: VideoMode, 160 | pub level: u8, 161 | pub encoding_rate: u8, 162 | pub last_video_poll: SystemTime, 163 | } 164 | 165 | /// Main connection and controller for the drone 166 | #[derive(Debug)] 167 | pub struct Drone { 168 | peer_ip: String, 169 | 170 | socket: UdpSocket, 171 | video_socket: Option, 172 | video: VideoSettings, 173 | last_stick_command: SystemTime, 174 | 175 | /// remote control values to control the drone 176 | pub rc_state: RCState, 177 | 178 | /// current meta data from the drone 179 | pub drone_meta: DroneMeta, 180 | 181 | /// used to query some metadata delayed after connecting 182 | status_counter: u32, 183 | } 184 | 185 | const START_OF_PACKET: u8 = 0xcc; 186 | 187 | /// known Command ids. Not all of them are implemented. 188 | #[derive(Debug, Clone, Copy, PartialEq)] 189 | #[repr(u16)] 190 | pub enum CommandIds { 191 | Undefined = 0x0000, 192 | SsidMsg = 0x0011, 193 | SsidCmd = 0x0012, 194 | SsidPasswordMsg = 0x0013, 195 | SsidPasswordCmd = 0x0014, 196 | WifiRegionMsg = 0x0015, 197 | WifiRegionCmd = 0x0016, 198 | WifiMsg = 0x001a, 199 | VideoEncoderRateCmd = 0x0020, 200 | VideoDynAdjRateCmd = 0x0021, 201 | EisCmd = 0x0024, 202 | VideoStartCmd = 0x0025, 203 | VideoRateQuery = 0x0028, 204 | TakePictureCommand = 0x0030, 205 | VideoModeCmd = 0x0031, 206 | VideoRecordCmd = 0x0032, 207 | ExposureCmd = 0x0034, 208 | LightMsg = 0x0035, 209 | JpegQualityMsg = 0x0037, 210 | Error1Msg = 0x0043, 211 | Error2Msg = 0x0044, 212 | VersionMsg = 0x0045, 213 | TimeCmd = 0x0046, 214 | ActivationTimeMsg = 0x0047, 215 | LoaderVersionMsg = 0x0049, 216 | StickCmd = 0x0050, 217 | TakeoffCmd = 0x0054, 218 | LandCmd = 0x0055, 219 | FlightMsg = 0x0056, 220 | AltLimitCmd = 0x0058, 221 | FlipCmd = 0x005c, 222 | ThrowAndGoCmd = 0x005d, 223 | PalmLandCmd = 0x005e, 224 | TelloCmdFileSize = 0x0062, 225 | TelloCmdFileData = 0x0063, 226 | TelloCmdFileComplete = 0x0064, 227 | SmartVideoCmd = 0x0080, 228 | SmartVideoStatusMsg = 0x0081, 229 | LogHeaderMsg = 0x1050, 230 | LogDataMsg = 0x1051, 231 | LogConfigMsg = 0x1052, 232 | BounceCmd = 0x1053, 233 | CalibrateCmd = 0x1054, 234 | LowBatThresholdCmd = 0x1055, 235 | AltLimitMsg = 0x1056, 236 | LowBatThresholdMsg = 0x1057, 237 | AttLimitCmd = 0x1058, 238 | AttLimitMsg = 0x1059, 239 | } 240 | 241 | impl From for CommandIds { 242 | fn from(value: u16) -> CommandIds { 243 | match value { 244 | 0x0011 => CommandIds::SsidMsg, 245 | 0x0012 => CommandIds::SsidCmd, 246 | 0x0013 => CommandIds::SsidPasswordMsg, 247 | 0x0014 => CommandIds::SsidPasswordCmd, 248 | 0x0015 => CommandIds::WifiRegionMsg, 249 | 0x0016 => CommandIds::WifiRegionCmd, 250 | 0x001a => CommandIds::WifiMsg, 251 | 0x0020 => CommandIds::VideoEncoderRateCmd, 252 | 0x0021 => CommandIds::VideoDynAdjRateCmd, 253 | 0x0024 => CommandIds::EisCmd, 254 | 0x0025 => CommandIds::VideoStartCmd, 255 | 0x0028 => CommandIds::VideoRateQuery, 256 | 0x0030 => CommandIds::TakePictureCommand, 257 | 0x0031 => CommandIds::VideoModeCmd, 258 | 0x0032 => CommandIds::VideoRecordCmd, 259 | 0x0034 => CommandIds::ExposureCmd, 260 | 0x0035 => CommandIds::LightMsg, 261 | 0x0037 => CommandIds::JpegQualityMsg, 262 | 0x0043 => CommandIds::Error1Msg, 263 | 0x0044 => CommandIds::Error2Msg, 264 | 0x0045 => CommandIds::VersionMsg, 265 | 0x0046 => CommandIds::TimeCmd, 266 | 0x0047 => CommandIds::ActivationTimeMsg, 267 | 0x0049 => CommandIds::LoaderVersionMsg, 268 | 0x0050 => CommandIds::StickCmd, 269 | 0x0054 => CommandIds::TakeoffCmd, 270 | 0x0055 => CommandIds::LandCmd, 271 | 0x0056 => CommandIds::FlightMsg, 272 | 0x0058 => CommandIds::AltLimitCmd, 273 | 0x005c => CommandIds::FlipCmd, 274 | 0x005d => CommandIds::ThrowAndGoCmd, 275 | 0x005e => CommandIds::PalmLandCmd, 276 | 0x0062 => CommandIds::TelloCmdFileSize, 277 | 0x0063 => CommandIds::TelloCmdFileData, 278 | 0x0064 => CommandIds::TelloCmdFileComplete, 279 | 0x0080 => CommandIds::SmartVideoCmd, 280 | 0x0081 => CommandIds::SmartVideoStatusMsg, 281 | 0x1050 => CommandIds::LogHeaderMsg, 282 | 0x1051 => CommandIds::LogDataMsg, 283 | 0x1052 => CommandIds::LogConfigMsg, 284 | 0x1053 => CommandIds::BounceCmd, 285 | 0x1054 => CommandIds::CalibrateCmd, 286 | 0x1055 => CommandIds::LowBatThresholdCmd, 287 | 0x1056 => CommandIds::AltLimitMsg, 288 | 0x1057 => CommandIds::LowBatThresholdMsg, 289 | 0x1058 => CommandIds::AttLimitCmd, 290 | 0x1059 => CommandIds::AttLimitMsg, 291 | _ => CommandIds::Undefined, 292 | } 293 | } 294 | } 295 | /// unformatted response from the drone. 296 | #[derive(Debug, Clone)] 297 | pub enum ResponseMsg { 298 | Connected(String), 299 | UnknownCommand(CommandIds), 300 | } 301 | 302 | /// The package type bitmask discripe the payload and how the drone should behave. 303 | /// More info are available on the https://tellopilots.com webpage 304 | #[derive(Debug, Clone, Copy)] 305 | #[repr(u8)] 306 | pub enum PackageTypes { 307 | X48 = 0x48, 308 | X50 = 0x50, 309 | X60 = 0x60, 310 | X70 = 0x70, 311 | X68 = 0x68, 312 | } 313 | 314 | /// Flip commands taken from Go version of code 315 | pub enum Flip { 316 | /// flips forward. 317 | Forward = 0, 318 | /// flips left. 319 | Left = 1, 320 | /// flips backwards. 321 | Back = 2, 322 | /// flips to the right. 323 | Right = 3, 324 | /// flips forwards and to the left. 325 | ForwardLeft = 4, 326 | /// flips backwards and to the left. 327 | BackLeft = 5, 328 | /// flips backwards and to the right. 329 | BackRight = 6, 330 | /// flips forwards and to the right. 331 | ForwardRight = 7, 332 | } 333 | 334 | /// available modes for the tello drone 335 | #[derive(Debug, Clone)] 336 | pub enum VideoMode { 337 | M960x720 = 0, 338 | M1280x720 = 1, 339 | } 340 | 341 | impl Drone { 342 | /// create a new drone and and listen to the Response port 8889 343 | /// this struct implements a number of commands to control the drone 344 | /// 345 | /// After connection to the drone it is very important to poll the drone at least with 20Hz. 346 | /// This will acknowledge some messages and parse the state of the drone. 347 | /// 348 | /// # Example 349 | /// 350 | /// ``` 351 | /// let mut drone = Drone::new("192.168.10.1:8889"); 352 | /// drone.connect(11111); 353 | /// // wait for the connection 354 | /// drone.take_off(); 355 | /// ``` 356 | pub fn new(ip: &str) -> Drone { 357 | let peer_ip = ip.to_string(); 358 | let socket = UdpSocket::bind(&SocketAddr::from(([0, 0, 0, 0], 8889))) 359 | .expect("couldn't bind to command address"); 360 | socket.set_nonblocking(true).unwrap(); 361 | socket.connect(ip).expect("connect command socket failed"); 362 | 363 | let video = VideoSettings { 364 | port: 0, 365 | enabled: false, 366 | mode: VideoMode::M960x720, 367 | level: 1, 368 | encoding_rate: 4, 369 | last_video_poll: SystemTime::now(), 370 | }; 371 | 372 | let rc_state = RCState::default(); 373 | let drone_meta = DroneMeta::default(); 374 | 375 | Drone { 376 | peer_ip, 377 | socket, 378 | video_socket: None, 379 | video, 380 | status_counter: 0, 381 | last_stick_command: SystemTime::now(), 382 | rc_state, 383 | drone_meta, 384 | } 385 | } 386 | 387 | /// Connect to the drone and inform the drone on with port you are ready to receive the video-stream 388 | /// 389 | /// The Video stream do not start automatically. You have to start it with 390 | /// `drone.start_video()` and pool every key-frame with an additional `drone.start_video()` call. 391 | pub fn connect(&mut self, video_port: u16) -> usize { 392 | let mut data = b"conn_req: ".to_vec(); 393 | let mut cur = Cursor::new(&mut data); 394 | cur.set_position(9); 395 | cur.write_u16::(video_port).unwrap(); 396 | self.video.port = video_port; 397 | self.start_video().unwrap(); 398 | 399 | let video_socket = UdpSocket::bind(&SocketAddr::from(([0, 0, 0, 0], self.video.port))) 400 | .expect("couldn't bind to video address"); 401 | video_socket.set_nonblocking(true).unwrap(); 402 | self.video_socket = Some(video_socket); 403 | 404 | self.socket.send(&data).expect("network should be usable") 405 | } 406 | 407 | /// convert the command into a Vec and send it to the drone. 408 | /// this is mostly for internal purposes, but you can implement missing commands your self 409 | pub fn send(&self, command: UdpCommand) -> Result { 410 | let data: Vec = command.into(); 411 | 412 | if self.socket.send(&data).is_ok() { 413 | Ok(()) 414 | } else { 415 | Err(()) 416 | } 417 | } 418 | 419 | /// when the drone send the current log stats, it is required to ack this. 420 | /// The logic is implemented in the poll function. 421 | fn send_ack_log(&self, id: u16) -> Result { 422 | let mut cmd = UdpCommand::new_with_zero_sqn(CommandIds::LogHeaderMsg, PackageTypes::X50); 423 | cmd.write_u16(id); 424 | self.send(cmd) 425 | } 426 | 427 | /// if there are some data in the udp-socket, all of one frame are collected and returned as UDP-Package 428 | fn receive_video_frame(&self, socket: &UdpSocket) -> Option { 429 | let mut read_buf = [0; 1440]; 430 | 431 | socket.set_nonblocking(true).unwrap(); 432 | if let Ok(received) = socket.recv(&mut read_buf) { 433 | let active_frame_id = read_buf[0]; 434 | let mut sqn = read_buf[1]; 435 | let mut frame_buffer = read_buf[2..received].to_owned(); 436 | 437 | // should start with 0. otherwise delete frame package 438 | if sqn != 0 { 439 | return None; 440 | } 441 | 442 | socket.set_nonblocking(false).unwrap(); 443 | 'recVideo: loop { 444 | if sqn >= 120 { 445 | break 'recVideo Some(Message::Frame(active_frame_id, frame_buffer)); 446 | } 447 | if let Ok(received) = socket.recv(&mut read_buf) { 448 | let frame_id = read_buf[0]; 449 | if frame_id != active_frame_id { 450 | // drop frame to stop data mess 451 | break 'recVideo None; 452 | } 453 | 454 | sqn = read_buf[1]; 455 | let mut data = read_buf[2..received].to_owned(); 456 | 457 | frame_buffer.append(&mut data); 458 | } else { 459 | break 'recVideo None; 460 | } 461 | } 462 | } else { 463 | None 464 | } 465 | } 466 | 467 | /// poll data from drone and send common data to the drone 468 | /// - every 33 millis, the sick command is send to the drone 469 | /// - every 1 sec, a key-frame is requested from the drone 470 | /// - logMessage packages are replied immediately with an ack package 471 | /// - dateTime packages are replied immediately with the local SystemTime 472 | /// - after the third status message some default data are send to the drone 473 | /// 474 | /// To receive a smooth video stream, you should poll at least 35 times per second 475 | pub fn poll(&mut self) -> Option { 476 | let now = SystemTime::now(); 477 | 478 | let delta = now.duration_since(self.last_stick_command).unwrap(); 479 | if delta.as_millis() > 1000 / 30 { 480 | let (pitch, nick, roll, yaw, fast) = self.rc_state.get_stick_parameter(); 481 | self.send_stick(pitch, nick, roll, yaw, fast).unwrap(); 482 | self.last_stick_command = now.clone(); 483 | } 484 | 485 | // poll I-Frame every second and receive udp frame data 486 | if self.video.enabled { 487 | let delta = now.duration_since(self.video.last_video_poll).unwrap(); 488 | if delta.as_secs() > 1 { 489 | self.video.last_video_poll = now; 490 | self.poll_key_frame().unwrap(); 491 | } 492 | if let Some(socket) = self.video_socket.as_ref() { 493 | let frame = self.receive_video_frame(&socket); 494 | if frame.is_some() { 495 | return frame; 496 | } 497 | } 498 | } 499 | 500 | // receive and process data on command socket 501 | let mut read_buf = [0; 1440]; 502 | if let Ok(received) = self.socket.recv(&mut read_buf) { 503 | let data = read_buf[..received].to_vec(); 504 | match Message::try_from(data) { 505 | Ok(msg) => { 506 | match &msg { 507 | Message::Response(ResponseMsg::Connected(_)) => self.status_counter = 0, 508 | Message::Data(Package { 509 | data: PackageData::LogMessage(log), 510 | .. 511 | }) => self.send_ack_log(log.id).unwrap(), 512 | Message::Data(Package { cmd, .. }) if *cmd == CommandIds::TimeCmd => { 513 | self.send_date_time().unwrap() 514 | } 515 | Message::Data(Package { cmd, data, .. }) 516 | if *cmd == CommandIds::FlightMsg => 517 | { 518 | self.drone_meta.update(&data); 519 | 520 | self.status_counter += 1; 521 | if self.status_counter == 3 { 522 | self.get_version().unwrap(); 523 | self.set_video_bitrate(4).unwrap(); 524 | self.get_alt_limit().unwrap(); 525 | self.get_battery_threshold().unwrap(); 526 | self.get_att_angle().unwrap(); 527 | self.get_region().unwrap(); 528 | self.set_exposure(2).unwrap(); 529 | }; 530 | } 531 | Message::Data(Package { data, .. }) => { 532 | self.drone_meta.update(&data); 533 | } 534 | _ => (), 535 | }; 536 | 537 | Some(msg) 538 | } 539 | Err(_e) => None, 540 | } 541 | } else { 542 | None 543 | } 544 | } 545 | } 546 | 547 | impl Drone { 548 | /// You can switch the drone to the command mode. 549 | /// To get back to the "Free-Flight-Mode" you have to reboot the drone. 550 | /// 551 | /// In the CommandMode you can move the drone by cm in the 3D space 552 | /// If you are using tokio as executer, use the `tokio_async` feature 553 | /// to prevent the executer from being blocked. 554 | pub fn command_mode(self) -> CommandMode { 555 | CommandMode::from(self.peer_ip.parse::().unwrap()) 556 | } 557 | } 558 | 559 | impl Drone { 560 | pub fn take_off(&self) -> Result { 561 | self.send(UdpCommand::new(CommandIds::TakeoffCmd, PackageTypes::X68)) 562 | } 563 | pub fn throw_and_go(&self) -> Result { 564 | let mut cmd = UdpCommand::new(CommandIds::ThrowAndGoCmd, PackageTypes::X48); 565 | cmd.write_u8(0); 566 | self.send(cmd) 567 | } 568 | pub fn land(&self) -> Result { 569 | let mut command = UdpCommand::new(CommandIds::LandCmd, PackageTypes::X68); 570 | command.write_u8(0x00); 571 | self.send(command) 572 | } 573 | pub fn stop_land(&self) -> Result { 574 | let mut command = UdpCommand::new(CommandIds::LandCmd, PackageTypes::X68); 575 | command.write_u8(0x00); 576 | self.send(command) 577 | } 578 | pub fn palm_land(&self) -> Result { 579 | let mut cmd = UdpCommand::new(CommandIds::PalmLandCmd, PackageTypes::X68); 580 | cmd.write_u8(0); 581 | self.send(cmd) 582 | } 583 | 584 | pub fn flip(&self, direction: Flip) -> Result { 585 | let mut cmd = UdpCommand::new_with_zero_sqn(CommandIds::FlipCmd, PackageTypes::X70); 586 | cmd.write_u8(direction as u8); 587 | self.send(cmd) 588 | } 589 | pub fn bounce(&self) -> Result { 590 | let mut cmd = UdpCommand::new(CommandIds::BounceCmd, PackageTypes::X68); 591 | cmd.write_u8(0x30); 592 | self.send(cmd) 593 | } 594 | pub fn bounce_stop(&self) -> Result { 595 | let mut cmd = UdpCommand::new(CommandIds::BounceCmd, PackageTypes::X68); 596 | cmd.write_u8(0x31); 597 | self.send(cmd) 598 | } 599 | 600 | pub fn get_version(&self) -> Result { 601 | self.send(UdpCommand::new(CommandIds::VersionMsg, PackageTypes::X48)) 602 | } 603 | pub fn get_alt_limit(&self) -> Result { 604 | self.send(UdpCommand::new(CommandIds::AltLimitMsg, PackageTypes::X68)) 605 | } 606 | pub fn set_alt_limit(&self, limit: u8) -> Result { 607 | let mut cmd = UdpCommand::new(CommandIds::AltLimitCmd, PackageTypes::X68); 608 | cmd.write_u8(limit); 609 | cmd.write_u8(0); 610 | self.send(cmd) 611 | } 612 | pub fn get_att_angle(&self) -> Result { 613 | self.send(UdpCommand::new(CommandIds::AttLimitMsg, PackageTypes::X68)) 614 | } 615 | pub fn set_att_angle(&self) -> Result { 616 | let mut cmd = UdpCommand::new(CommandIds::AttLimitCmd, PackageTypes::X68); 617 | cmd.write_u8(0); 618 | cmd.write_u8(0); 619 | // TODO set angle correct 620 | // pkt.add_byte( int(float_to_hex(float(limit))[4:6], 16) ) # 'attitude limit' formatted in float of 4 bytes 621 | cmd.write_u8(10); 622 | cmd.write_u8(0x41); 623 | self.send(cmd) 624 | } 625 | 626 | pub fn get_battery_threshold(&self) -> Result { 627 | self.send(UdpCommand::new( 628 | CommandIds::LowBatThresholdMsg, 629 | PackageTypes::X68, 630 | )) 631 | } 632 | pub fn set_battery_threshold(&self, threshold: u8) -> Result { 633 | let mut cmd = UdpCommand::new(CommandIds::LowBatThresholdCmd, PackageTypes::X68); 634 | cmd.write_u8(threshold); 635 | self.send(cmd) 636 | } 637 | 638 | pub fn get_region(&self) -> Result { 639 | self.send(UdpCommand::new( 640 | CommandIds::WifiRegionCmd, 641 | PackageTypes::X48, 642 | )) 643 | } 644 | 645 | /// send the stick command via udp to the drone 646 | /// 647 | /// pitch up/down -1 -> 1 648 | /// nick forward/backward -1 -> 1 649 | /// roll right/left -1 -> 1 650 | /// yaw cw/ccw -1 -> 1 651 | pub fn send_stick(&self, pitch: f32, nick: f32, roll: f32, yaw: f32, fast: bool) -> Result { 652 | let mut cmd = UdpCommand::new_with_zero_sqn(CommandIds::StickCmd, PackageTypes::X60); 653 | 654 | // RightX center=1024 left =364 right =-364 655 | let pitch_u = (1024.0 + 660.0 * pitch) as i64; 656 | 657 | // RightY down =364 up =-364 658 | let nick_u = (1024.0 + 660.0 * nick) as i64; 659 | 660 | // LeftY down =364 up =-364 661 | let roll_u = (1024.0 + 660.0 * roll) as i64; 662 | 663 | // LeftX left =364 right =-364 664 | let yaw_u = (1024.0 + 660.0 * yaw) as i64; 665 | 666 | // speed control 667 | let throttle_u = if fast { 1i64 } else { 0i64 }; 668 | 669 | // create axis package 670 | let packed_axis: i64 = (roll_u & 0x7FF) 671 | | (nick_u & 0x7FF) << 11 672 | | (pitch_u & 0x7FF) << 22 673 | | (yaw_u & 0x7FF) << 33 674 | | throttle_u << 44; 675 | 676 | // println!("p {:} n {:} r {:} y {:} t {:} => {:x}", pitch_u & 0x7FF, nick_u& 0x7FF, roll_u& 0x7FF, yaw_u& 0x7FF, throttle_u, packed_axis); 677 | 678 | cmd.write_u8(((packed_axis) & 0xFF) as u8); 679 | cmd.write_u8(((packed_axis >> 8) & 0xFF) as u8); 680 | cmd.write_u8(((packed_axis >> 16) & 0xFF) as u8); 681 | cmd.write_u8(((packed_axis >> 24) & 0xFF) as u8); 682 | cmd.write_u8(((packed_axis >> 32) & 0xFF) as u8); 683 | cmd.write_u8(((packed_axis >> 40) & 0xFF) as u8); 684 | 685 | self.send(Drone::add_time(cmd)) 686 | } 687 | 688 | /// SendDateTime sends the current date/time to the drone. 689 | pub fn send_date_time(&self) -> Result { 690 | let command = UdpCommand::new(CommandIds::TimeCmd, PackageTypes::X50); 691 | self.send(Drone::add_date_time(command)) 692 | } 693 | 694 | pub fn add_time(mut command: UdpCommand) -> UdpCommand { 695 | let now = Local::now(); 696 | let millis = now.nanosecond() / 1_000_000; 697 | command.write_u8(now.hour() as u8); 698 | command.write_u8(now.minute() as u8); 699 | command.write_u8(now.second() as u8); 700 | command.write_u16(millis as u16); 701 | command 702 | } 703 | 704 | pub fn add_date_time(mut command: UdpCommand) -> UdpCommand { 705 | let now = Local::now(); 706 | let millis = now.nanosecond() / 1_000_000; 707 | command.write_u8(0); 708 | command.write_u16(now.year() as u16); 709 | command.write_u16(now.month() as u16); 710 | command.write_u16(now.day() as u16); 711 | command.write_u16(now.hour() as u16); 712 | command.write_u16(now.minute() as u16); 713 | command.write_u16(now.second() as u16); 714 | command.write_u16(millis as u16); 715 | command 716 | } 717 | } 718 | 719 | impl Drone { 720 | /// start_video starts the streaming and requests the info (SPS/PPS) for the video stream. 721 | /// 722 | /// Video-metadata: 723 | /// e.g.: caps = video/x-h264, stream-format=(string)avc, width=(int)960, height=(int)720, framerate=(fraction)0/1, interlace-mode=(string)progressive, chroma-format=(string)4:2:0, bit-depth-luma=(uint)8, bit-depth-chroma=(uint)8, parsed=(boolean)true, alignment=(string)au, profile=(string)main, level=(string)4, codec_data=(buffer)014d4028ffe10009674d402895a03c05b901000468ee3880 724 | /// 725 | /// # Examples 726 | /// ```no_run 727 | /// let mut drone = Drone::new("192.168.10.1:8889"); 728 | /// drone.connect(11111); 729 | /// // ... 730 | /// drone.start_video().unwrap(); 731 | /// ``` 732 | pub fn start_video(&mut self) -> Result { 733 | self.video.enabled = true; 734 | self.video.last_video_poll = SystemTime::now(); 735 | self.send(UdpCommand::new_with_zero_sqn( 736 | CommandIds::VideoStartCmd, 737 | PackageTypes::X60, 738 | )) 739 | } 740 | 741 | /// Same as start_video(), but a better name to poll the (SPS/PPS) for the video stream. 742 | /// 743 | /// This is automatically called in the poll function every second. 744 | pub fn poll_key_frame(&mut self) -> Result { 745 | self.start_video() 746 | } 747 | 748 | /// Set the video mode to 960x720 4:3 video, or 1280x720 16:9 zoomed video. 749 | /// 4:3 has a wider field of view (both vertically and horizontally), 16:9 is crisper. 750 | /// 751 | /// # Examples 752 | /// ```no_run 753 | /// let mut drone = Drone::new("192.168.10.1:8889"); 754 | /// drone.connect(11111); 755 | /// // ... 756 | /// drone.set_video_mode(VideoMode::M960x720).unwrap(); 757 | /// ``` 758 | pub fn set_video_mode(&mut self, mode: VideoMode) -> Result { 759 | self.video.mode = mode.clone(); 760 | let mut cmd = UdpCommand::new_with_zero_sqn(CommandIds::VideoStartCmd, PackageTypes::X68); 761 | cmd.write_u8(mode as u8); 762 | self.send(cmd) 763 | } 764 | 765 | /// Set the camera exposure level. 766 | /// param level: it can be 0, 1 or 2 767 | /// 768 | /// # Examples 769 | /// ```no_run 770 | /// let mut drone = Drone::new("192.168.10.1:8889"); 771 | /// drone.connect(11111); 772 | /// // ... 773 | /// drone.set_exposure(2).unwrap(); 774 | /// ``` 775 | pub fn set_exposure(&mut self, level: u8) -> Result { 776 | let mut cmd = UdpCommand::new(CommandIds::ExposureCmd, PackageTypes::X48); 777 | cmd.write_u8(level); 778 | self.send(cmd) 779 | } 780 | 781 | /// set the video encoder rate for the camera. 782 | /// param rate: TODO: unknown 783 | /// 784 | /// # Examples 785 | /// ```no_run 786 | /// let mut drone = Drone::new("192.168.10.1:8889"); 787 | /// drone.connect(11111); 788 | /// // ... 789 | /// drone.set_video_bitrate(3).unwrap(); 790 | /// ``` 791 | pub fn set_video_bitrate(&mut self, rate: u8) -> Result { 792 | self.video.encoding_rate = rate; 793 | let mut cmd = UdpCommand::new(CommandIds::VideoEncoderRateCmd, PackageTypes::X68); 794 | cmd.write_u8(rate); 795 | self.send(cmd) 796 | } 797 | 798 | /// take a single picture and provide it to download it. 799 | /// 800 | /// # Examples 801 | /// ```no_run 802 | /// let mut drone = Drone::new("192.168.10.1:8889"); 803 | /// drone.connect(11111); 804 | /// // ... 805 | /// drone.take_picture(3).unwrap(); 806 | /// 807 | /// @TODO: download image 808 | /// ``` 809 | pub fn take_picture(&self) -> Result { 810 | self.send(UdpCommand::new( 811 | CommandIds::TakePictureCommand, 812 | PackageTypes::X68, 813 | )) 814 | } 815 | } 816 | 817 | /// wrapper to generate Udp Commands to send them to the drone. 818 | /// 819 | /// It is public, to enable users to implement missing commands 820 | #[derive(Debug, Clone)] 821 | pub struct UdpCommand { 822 | cmd: CommandIds, 823 | pkt_type: PackageTypes, 824 | zero_sqn: bool, 825 | inner: Vec, 826 | } 827 | 828 | impl UdpCommand { 829 | /// create a new command, prepare the header to send out the command 830 | pub fn new(cmd: CommandIds, pkt_type: PackageTypes) -> UdpCommand { 831 | UdpCommand { 832 | cmd, 833 | pkt_type, 834 | zero_sqn: false, 835 | inner: Vec::new(), 836 | } 837 | } 838 | pub fn new_with_zero_sqn(cmd: CommandIds, pkt_type: PackageTypes) -> UdpCommand { 839 | UdpCommand { 840 | cmd, 841 | pkt_type, 842 | zero_sqn: true, 843 | inner: Vec::new(), 844 | } 845 | } 846 | } 847 | 848 | impl UdpCommand { 849 | pub fn write(&mut self, bytes: &[u8]) { 850 | self.inner.append(&mut bytes.to_owned()) 851 | } 852 | pub fn write_u8(&mut self, byte: u8) { 853 | self.inner.push(byte) 854 | } 855 | pub fn write_u16(&mut self, value: u16) { 856 | let mut cur = Cursor::new(&mut self.inner); 857 | cur.seek(SeekFrom::End(0)).expect(""); 858 | cur.write_u16::(value).expect(""); 859 | } 860 | pub fn write_u64(&mut self, value: u64) { 861 | let mut cur = Cursor::new(&mut self.inner); 862 | cur.seek(SeekFrom::End(0)).expect(""); 863 | cur.write_u64::(value).expect(""); 864 | } 865 | } 866 | 867 | impl Into> for UdpCommand { 868 | fn into(self) -> Vec { 869 | let mut data = { 870 | let lng = self.inner.len(); 871 | let data: &[u8] = &self.inner; 872 | 873 | let mut cur = Cursor::new(Vec::new()); 874 | cur.write_u8(START_OF_PACKET).expect(""); 875 | cur.write_u16::((lng as u16 + 11) << 3) 876 | .expect(""); 877 | cur.write_u8(crc8(cur.clone().into_inner())).expect(""); 878 | cur.write_u8(self.pkt_type as u8).expect(""); 879 | cur.write_u16::(self.cmd as u16).expect(""); 880 | 881 | if self.zero_sqn { 882 | cur.write_u16::(0).expect(""); 883 | } else { 884 | let nr = SEQ_NO.fetch_add(1, Ordering::SeqCst); 885 | cur.write_u16::(nr).expect(""); 886 | } 887 | 888 | if lng > 0 { 889 | cur.write_all(&data).unwrap(); 890 | } 891 | 892 | cur.into_inner() 893 | }; 894 | 895 | data.write_u16::(crc16(data.clone())) 896 | .expect(""); 897 | 898 | data 899 | } 900 | } 901 | 902 | /// Data / command package received from the drone with parsed data (if supported and known) 903 | #[derive(Debug, Clone)] 904 | pub struct Package { 905 | pub cmd: CommandIds, 906 | pub size: u16, 907 | pub sq_nr: u16, 908 | pub data: PackageData, 909 | } 910 | 911 | /// Incoming message can be Data, a response from the drone or a VideoFrame 912 | #[derive(Debug, Clone)] 913 | pub enum Message { 914 | Data(Package), 915 | Response(ResponseMsg), 916 | Frame(u8, Vec), 917 | } 918 | 919 | impl TryFrom> for Message { 920 | type Error = String; 921 | 922 | fn try_from(data: Vec) -> std::result::Result { 923 | let mut cur = Cursor::new(data); 924 | if let Ok(START_OF_PACKET) = cur.read_u8() { 925 | let size = (cur.read_u16::().unwrap() >> 3) - 11; 926 | let _crc8 = cur.read_u8().unwrap(); 927 | let _pkt_type = cur.read_u8().unwrap(); 928 | let cmd = CommandIds::from(cur.read_u16::().unwrap()); 929 | let sq_nr = cur.read_u16::().unwrap(); 930 | let data = if size > 0 { 931 | let mut data: Vec = Vec::with_capacity(size as usize); 932 | cur.read_to_end(&mut data).unwrap(); 933 | if data.len() >= 2 { 934 | let _crc16: u16 = 935 | (data.pop().unwrap() as u16) + ((data.pop().unwrap() as u16) << 8); 936 | } 937 | match cmd { 938 | CommandIds::FlightMsg => PackageData::FlightData(FlightData::from(data)), 939 | CommandIds::WifiMsg => PackageData::WifiInfo(WifiInfo::from(data)), 940 | CommandIds::LightMsg => PackageData::LightInfo(LightInfo::from(data)), 941 | CommandIds::VersionMsg => PackageData::Version( 942 | String::from_utf8(data[1..].to_vec()) 943 | .expect("version is not valid") 944 | .trim_matches(char::from(0)) 945 | .to_string(), 946 | ), 947 | CommandIds::AltLimitMsg => { 948 | let mut c = Cursor::new(data); 949 | let _ = c.read_u8().unwrap(); 950 | let h = c.read_u16::().unwrap(); 951 | PackageData::AtlInfo(h) 952 | } 953 | 954 | CommandIds::LogHeaderMsg => PackageData::LogMessage(LogMessage::from(data)), 955 | _ => PackageData::Unknown(data), 956 | } 957 | } else { 958 | PackageData::NoData() 959 | }; 960 | 961 | Ok(Message::Data(Package { 962 | cmd, 963 | size, 964 | sq_nr, 965 | data, 966 | })) 967 | } else { 968 | let data = cur.into_inner(); 969 | if data[0..9].to_vec() == b"conn_ack:" { 970 | return Ok(Message::Response(ResponseMsg::Connected( 971 | String::from_utf8(data).unwrap(), 972 | ))); 973 | } else if data[0..16].to_vec() == b"unknown command:" { 974 | let mut cur = Cursor::new(data[17..].to_owned()); 975 | let command = CommandIds::from(cur.read_u16::().unwrap()); 976 | return Ok(Message::Response(ResponseMsg::UnknownCommand(command))); 977 | } 978 | 979 | let msg = String::from_utf8(data.clone()[0..5].to_vec()).unwrap_or_default(); 980 | Err(format!("invalid package {:x?}", msg)) 981 | } 982 | } 983 | } 984 | 985 | /// Parsed data from the drone. 986 | #[derive(Debug, Clone)] 987 | pub enum PackageData { 988 | NoData(), 989 | AtlInfo(u16), 990 | FlightData(FlightData), 991 | LightInfo(LightInfo), 992 | LogMessage(LogMessage), 993 | Version(String), 994 | WifiInfo(WifiInfo), 995 | Unknown(Vec), 996 | } 997 | --------------------------------------------------------------------------------