use sdl2::pixels::Color;
use sdl2::rect::Rect;
use std::path::Path;
use std::string::String;
use std::time::Duration;
use std::ops::Deref;
use gilrs::{Gilrs, Axis, Event, Button, EventType};
use std::net::UdpSocket;
use tello::{Drone, Flip, Message, Package, PackageData, ResponseMsg};
const WINDOW_WIDTH: u32 = 1280;
const WINDOW_HEIGHT: u32 = 720;
fn main() -> Result<(), String> {
let mut gilrs = Gilrs::new().map_err(|_| "gamepad not valid")?;
for (_id, gamepad) in gilrs.gamepads() {
println!("{} is {:?}", gamepad.name(), gamepad.power_info());
}
let mut drone = Drone::new("192.168.10.1:8889");
let sdl_context = sdl2::init()?;
let video_subsystem = sdl_context.video()?;
let window = video_subsystem
.window("TELLO drone", WINDOW_WIDTH, WINDOW_HEIGHT)
.position_centered()
.opengl()
.build()
.expect("could not initialize video subsystem");
let mut canvas = window
.into_canvas()
.build()
.expect("could not make a canvas");
let ttf_context = sdl2::ttf::init().expect("could not initialize font subsystem");
let texture_creator = canvas.texture_creator();
let font_path: &Path = Path::new("./examples/DejaVuSans.ttf");
let font = ttf_context.load_font(font_path, 24).expect("load font");
let keys_target = Rect::new((WINDOW_WIDTH - 250) as i32, 0, 250, 196);
let key_texture = texture_creator.create_texture_from_surface(
&font
.render("i: connect\nk: take off\no: manual take off\np: throw 'n go\nl: land/cancel\nv: start video\nESC: Exit")
.blended_wrapped(Color::RGB(0, 0, 0), 250)
.unwrap()
).unwrap();
let stats_target = Rect::new(50, WINDOW_HEIGHT as i32 - 40, WINDOW_WIDTH - 100, 40);
let mut land = false;
let mut bounce_on = false;
let socket = UdpSocket::bind("127.0.0.1:34254").expect("couldn't bind to address");
'running: loop {
canvas.set_draw_color(Color::RGB(80, 64, 255 - 80));
canvas.clear();
canvas.copy(&key_texture, None, Some(keys_target))?;
if let Some(data) = drone.drone_meta.get_flight_data() {
let d = format!("{:?}", data);
let surface_stats = font.render(d.deref()).blended(Color::RGB(0, 0, 0)).unwrap();
let texture_stats = texture_creator
.create_texture_from_surface(&surface_stats)
.unwrap();
canvas.copy(&texture_stats, None, Some(stats_target))?;
}
while let Some(Event { event, .. }) = gilrs.next_event() {
match event {
EventType::ButtonReleased(Button::Mode, _) => {
break 'running;
}
EventType::ButtonPressed(Button::Start, _) => {
drone.connect(11111);
}
EventType::ButtonPressed(Button::North, _) => {
if bounce_on == false {
bounce_on = true;
drone.bounce().map_err(|_| "bounce failed")?;
} else {
bounce_on = false;
drone.bounce_stop().map_err(|_| "bounce_stop failed")?;
}
}
EventType::ButtonPressed(Button::West, _) => {
drone.start_video().map_err(|_| "start_video failed")?;
}
EventType::ButtonPressed(Button::East, _) => {
drone.throw_and_go().map_err(|_| "throw_and_go failed")?;
}
EventType::ButtonPressed(Button::South, _) => {
drone.rc_state.start_engines();
}
EventType::ButtonPressed(Button::LeftTrigger, _) => {
land = false;
drone.take_off().map_err(|_| "take_off failed")?;
}
EventType::ButtonPressed(Button::RightTrigger, _) => {
if land {
land = false;
drone.stop_land().map_err(|_| "stop_land failed")?;
} else {
land = true;
drone.land().map_err(|_| "land failed")?;
}
}
EventType::AxisChanged(Axis::LeftStickX, value, _) => {
drone.rc_state.turn(value)
}
EventType::AxisChanged(Axis::LeftStickY, value, _) => {
drone.rc_state.go_up_down(value)
}
EventType::AxisChanged(Axis::RightStickX, value, _) => {
drone.rc_state.go_left_right(value)
}
EventType::AxisChanged(Axis::RightStickY, value, _) => {
drone.rc_state.go_forward_back(value)
}
EventType::ButtonPressed(Button::DPadDown, _) => {
drone.flip(Flip::Back).map_err(|_| "Flip failed")?;
}
EventType::ButtonPressed(Button::DPadUp, _) => {
drone.flip(Flip::Forward).map_err(|_| "Flip failed")?;
}
EventType::ButtonPressed(Button::DPadLeft, _) => {
drone.flip(Flip::Left).map_err(|_| "Flip failed")?;
}
EventType::ButtonPressed(Button::DPadRight, _) => {
drone.flip(Flip::Right).map_err(|_| "Flip failed")?;
}
_ => {}
}
};
while let Some(msg) = drone.poll() {
match msg {
Message::Data(Package {data: PackageData::FlightData(_), ..}) => {
}
Message::Data(d) => {
println!("msg {:?}", d.clone());
}
Message::Frame(frame_id, d)=> {
socket.send_to(&d, "127.0.0.1:11110").expect("couldn't send data");
println!("send frame {} {:?}", frame_id, &d[..15]);
}
Message::Response(ResponseMsg::Connected(_)) => {
println!("connected");
}
_ => ()
}
}
canvas.present();
::std::thread::sleep(Duration::from_millis(10));
}
Ok(())
}