owow it s working now

anafi-rs/examples/liftoff.rs

@@ -4,28 +4,59 @@ use std::error::Error;
 use anafi_rs::prelude::*;
 use std::time::Duration;
 
+// https://www.dema.ch/media/catalog/product/pdf/1976008063/pdf_file_3/en_US/white-paper-anafi-usa-v1.5.2_en.pdf
+// https://github.com/RIAEvangelist/node-parrot-drone/blob/master/docs/ardrone3.md
+
 fn main() -> Result<(), Box<dyn Error>> {
     env_logger::init();
 
     let drone_ip: std::net::IpAddr = "192.168.42.1".parse()?;
     let drone = Anafi::connect(drone_ip.into())?;
 
-    std::thread::sleep(Duration::from_secs(10));
+    drone.take_off()?;
 
-    log::warn!("Takeoff!");
+    std::thread::sleep(Duration::from_secs(2));
+    log::warn!("UP!");
+    drone.up()?;
+    std::thread::sleep(Duration::from_secs(2));
 
-    for _ in 1..50 {
-        std::thread::sleep(std::time::Duration::from_millis(200));
-        drone.take_off()?;
-    }
+    // log::warn!("forward!");
+    // drone.forward()?;
+    // std::thread::sleep(Duration::from_secs(1));
+    // drone.stop()?;
 
-    log::warn!("Wait 5 seconds and get down");
-    std::thread::sleep(Duration::from_secs(5));
+    // log::warn!("backward!");
+    // drone.backward()?;
+    // std::thread::sleep(Duration::from_secs(1));
+    // drone.stop()?;
+
+    log::warn!("left!");
+    drone.strafe_left()?;
+    std::thread::sleep(Duration::from_secs(1));
+
+    log::warn!("right!");
+    drone.strafe_right()?;
+    std::thread::sleep(Duration::from_secs(1));
+
+    // log::warn!("turn left!");
+    // for _ in 0..30 {
+    //     drone.turn_left()?;
+    //     std::thread::sleep(Duration::from_millis(300));
+    // }
+
+    // log::warn!("turn right!");
+    // for _ in 0..30 {
+    //     drone.turn_right()?;
+    //     std::thread::sleep(Duration::from_millis(300));
+    // }
+
+    log::warn!("DOWN!");
+    drone.down()?;
+    std::thread::sleep(Duration::from_secs(2));
 
-    for _ in 1..50 {
-        std::thread::sleep(std::time::Duration::from_millis(200));
-        drone.landing()?;
-    }
+    std::thread::sleep(Duration::from_secs(2));
+    log::warn!("LAND!");
+    drone.landing()?;
 
     std::thread::sleep(Duration::from_secs(5));
 

anafi-rs/src/lib.rs

@@ -17,6 +17,10 @@ pub mod prelude {
 }
 
 pub struct Anafi {
+    // roll: 0,
+    // pitch: forward / backward,
+    // yaw: strafe left / right,
+    // gaz: up / down
     drone: Drone,
 }
 
@@ -36,31 +40,143 @@ impl Anafi {
         self.drone.send_frame(frame)
     }
 
-    pub fn up(&self, sequence_id: u8) -> Result<(), Error> {
+    pub fn up(&self) -> Result<(), Error> {
         let feature = Feature::ArDrone3(Some(ArDrone3::Piloting(Piloting::PCMD(PCMD {
             flag: true,
             roll: 0,
             pitch: 0,
             yaw: 0,
             gaz: 100,
             timestamp: Utc::now(),
-            sequence_id,
+            sequence_id: self.drone.piloting_id(),
         }))));
 
         let frame = Frame::for_drone(&self.drone, Type::Data, BufferID::CDNonAck, Some(feature));
 
         self.drone.send_frame(frame)
     }
 
-    pub fn down(&self, sequence_id: u8) -> Result<(), Error> {
+    pub fn down(&self) -> Result<(), Error> {
         let feature = Feature::ArDrone3(Some(ArDrone3::Piloting(Piloting::PCMD(PCMD {
             flag: true,
             roll: 0,
             pitch: 0,
             yaw: 0,
             gaz: -100,
             timestamp: Utc::now(),
-            sequence_id,
+            sequence_id: self.drone.piloting_id(),
+        }))));
+
+        let frame = Frame::for_drone(&self.drone, Type::Data, BufferID::CDNonAck, Some(feature));
+
+        self.drone.send_frame(frame)
+    }
+
+    pub fn backward(&self) -> Result<(), Error> {
+        let feature = Feature::ArDrone3(Some(ArDrone3::Piloting(Piloting::PCMD(PCMD {
+            flag: true,
+            roll: 0,
+            pitch: -100,
+            yaw: 0,
+            gaz: 0,
+            timestamp: Utc::now(),
+            sequence_id: self.drone.piloting_id(),
+        }))));
+
+        let frame = Frame::for_drone(&self.drone, Type::Data, BufferID::CDNonAck, Some(feature));
+
+        self.drone.send_frame(frame)
+    }
+
+    pub fn forward(&self) -> Result<(), Error> {
+        let feature = Feature::ArDrone3(Some(ArDrone3::Piloting(Piloting::PCMD(PCMD {
+            flag: true,
+            roll: 0,
+            pitch: 100,
+            yaw: 0,
+            gaz: 0,
+            timestamp: Utc::now(),
+            sequence_id: self.drone.piloting_id(),
+        }))));
+
+        let frame = Frame::for_drone(&self.drone, Type::Data, BufferID::CDNonAck, Some(feature));
+
+        self.drone.send_frame(frame)
+    }
+
+    pub fn strafe_left(&self) -> Result<(), Error> {
+        let feature = Feature::ArDrone3(Some(ArDrone3::Piloting(Piloting::PCMD(PCMD {
+            flag: true,
+            roll: -100,
+            pitch: 0,
+            yaw: 0,
+            gaz: 0,
+            timestamp: Utc::now(),
+            sequence_id: self.drone.piloting_id(),
+        }))));
+
+        let frame = Frame::for_drone(&self.drone, Type::Data, BufferID::CDNonAck, Some(feature));
+
+        self.drone.send_frame(frame)
+    }
+
+    pub fn strafe_right(&self) -> Result<(), Error> {
+        let feature = Feature::ArDrone3(Some(ArDrone3::Piloting(Piloting::PCMD(PCMD {
+            flag: true,
+            roll: 100,
+            pitch: 0,
+            yaw: 0,
+            gaz: 0,
+            timestamp: Utc::now(),
+            sequence_id: self.drone.piloting_id(),
+        }))));
+
+        let frame = Frame::for_drone(&self.drone, Type::Data, BufferID::CDNonAck, Some(feature));
+
+        self.drone.send_frame(frame)
+    }
+
+    pub fn turn_left(&self) -> Result<(), Error> {
+        let feature = Feature::ArDrone3(Some(ArDrone3::Piloting(Piloting::PCMD(PCMD {
+            flag: false,
+            roll: 0,
+            pitch: 0,
+            yaw: -128,
+            gaz: 0,
+            timestamp: Utc::now(),
+            sequence_id: self.drone.piloting_id(),
+        }))));
+
+        let frame = Frame::for_drone(&self.drone, Type::Data, BufferID::CDNonAck, Some(feature));
+
+        self.drone.send_frame(frame)
+    }
+
+    pub fn turn_right(&self) -> Result<(), Error> {
+        let feature = Feature::ArDrone3(Some(ArDrone3::Piloting(Piloting::PCMD(PCMD {
+            flag: false,
+            roll: 0,
+            pitch: 0,
+            yaw: 127,
+            gaz: 0,
+            timestamp: Utc::now(),
+            sequence_id: self.drone.piloting_id(),
+        }))));
+
+        let frame = Frame::for_drone(&self.drone, Type::Data, BufferID::CDNonAck, Some(feature));
+
+        self.drone.send_frame(frame)
+    }
+
+    pub fn stop(&self) -> Result<(), Error> {
+        let feature = Feature::ArDrone3(Some(ArDrone3::Piloting(Piloting::PCMD(PCMD {
+            flag: true,
+            roll: 0,
+            pitch: 0,
+            yaw: 0,
+            gaz: 0,
+            timestamp: Utc::now(),
+            sequence_id: self.drone.piloting_id(),
         }))));
 
         let frame = Frame::for_drone(&self.drone, Type::Data, BufferID::CDNonAck, Some(feature));

arsdk-rs/src/ardrone3/piloting/pcmd.rs

@@ -10,7 +10,6 @@ pub struct PCMD {
     pub yaw: i8,
     pub gaz: i8,
     pub timestamp: DateTime<Utc>,
-    // TODO: How should we handle the `sequence_id` in order not to show it to the user?
     pub sequence_id: u8,
 }
 

arsdk-rs/src/handshake.rs

@@ -1,4 +1,4 @@
-use log::{error, info};
+use log::{debug, error, info};
 use serde::{Deserialize, Serialize};
 use serde_with::with_prefix;
 use std::{
@@ -92,18 +92,18 @@ pub(crate) fn perform_handshake(
 
     let mut handshake_stream = retry(10, init_address)?;
 
-    info!("Request: {}", serde_json::to_string(&request)?);
+    debug!("Request: {}", serde_json::to_string(&request)?);
     let request_string = serde_json::to_vec(&request)?;
 
     handshake_stream.write_all(&request_string)?;
 
     let mut buf = [0_u8; 1024];
     let read = handshake_stream.read(&mut buf)?;
-    info!("Read {} bytes!", read);
+    debug!("Read {} bytes!", read);
 
     let response_string = String::from_utf8(buf[..read].to_vec())?;
 
-    info!("Response: {}", response_string);
+    debug!("Response: {}", response_string);
 
     handshake_stream.shutdown(Shutdown::Both)?;
 

arsdk-rs/src/lib.rs

@@ -15,7 +15,6 @@ use thiserror::Error;
 pub use chrono;
 
 pub const INIT_PORT: u16 = 44444;
-pub const LISTEN_PORT: u16 = 43210;
 pub const PARROT_SPHINX_IP: IpAddr = IpAddr::V4(Ipv4Addr::new(10, 202, 0, 1));
 pub const PARROT_SPHINX_CONFIG: Config = Config {
     drone_addr: PARROT_SPHINX_IP,
@@ -94,6 +93,13 @@ where
 #[derive(Clone, Debug)]
 pub struct Drone {
     inner: Arc<DroneInner>,
+    shutdown_sender: SyncSender<()>,
+}
+
+impl Drop for Drone {
+    fn drop(&mut self) {
+        let _ = self.shutdown_sender.send(());
+    }
 }
 
 #[derive(Debug)]
@@ -116,17 +122,22 @@ impl Drone {
         // @TODO: Check if we're going to miss any messages between spawning the listener and the receiver of commands
         let (tx_cmd, rx_cmd) = sync_channel(200);
 
+        let (shutdown_sender, shutdown_receiver) = sync_channel(1);
+
         let drone = Self {
             inner: Arc::new(DroneInner {
                 sequence_ids: DashMap::new(),
                 sender: tx_cmd,
             }),
+            shutdown_sender,
         };
 
-        let local_listener = SocketAddr::new(local_ip, LISTEN_PORT);
-        info!("{}: Spawning Listener", &&local_listener);
-
-        spawn_listener(drone.clone(), local_listener)?;
+        let local_listener = spawn_listener(
+            drone.clone(),
+            SocketAddr::new(local_ip, 0), // let kernel decide which port is available
+            shutdown_receiver,
+        )?;
+        info!("{}: Spawned Listener", &local_listener);
 
         let init_addr = SocketAddr::new(config.drone_addr, INIT_PORT);
 
@@ -198,6 +209,10 @@ impl Drone {
 
         self.send_frame(pong)
     }
+
+    pub fn piloting_id(&self) -> u8 {
+        self.inner.sequence_id(frame::BufferID::CDNonAck)
+    }
 }
 
 impl DroneInner {
@@ -225,20 +240,28 @@ fn local_ip(target: IpAddr) -> Option<IpAddr> {
         .next()
 }
 
-fn spawn_listener(drone: Drone, addr: SocketAddr) -> Result<(), ConnectionError> {
+fn spawn_listener(
+    drone: Drone,
+    addr: SocketAddr,
+    shutdown_receiver: Receiver<()>,
+) -> Result<SocketAddr, ConnectionError> {
     let listener_socket =
         UdpSocket::bind(addr).map_err(|error| ConnectionError::Io { error, addr })?;
 
+    let addr = listener_socket
+        .local_addr()
+        .map_err(|error| ConnectionError::Io { error, addr })?;
+
     std::thread::spawn(move || {
         let listener = Listener {
             drone: drone.clone(),
             socket: listener_socket,
         };
 
-        listener.listen();
+        listener.listen(shutdown_receiver);
     });
 
-    Ok(())
+    Ok(addr)
 }
 
 pub(crate) fn print_buf(buf: &[u8]) -> String {
@@ -273,7 +296,7 @@ fn spawn_cmd_sender(
         if frame_to_send[0] == 1 {
             trace!("Frame to send: {:?}", &frame_to_send);
         } else {
-            log::warn!(
+            log::debug!(
                 "Frame to send: {}: {:?}",
                 &frame_to_send
                     .iter()

arsdk-rs/src/listener.rs

@@ -1,6 +1,7 @@
 use crate::{parse::handle_bytes, print_buf, Drone};
 use log::debug;
 use std::net::UdpSocket;
+use std::sync::mpsc::{Receiver, TryRecvError};
 
 pub struct Listener {
     pub drone: Drone,
@@ -9,8 +10,8 @@ pub struct Listener {
 
 impl Listener {
     /// Blocking listener in a infinite loop
-    pub fn listen(&self) {
-        loop {
+    pub fn listen(&self, shutdown_receiver: Receiver<()>) {
+        while shutdown_receiver.try_recv() == Err(TryRecvError::Empty) {
             let mut buf = [0_u8; 40960];
             if let Ok((bytes_read, origin)) = self.socket.recv_from(&mut buf) {
                 debug!("Received: {} bytes from {}", bytes_read, origin);
@@ -19,5 +20,7 @@ impl Listener {
                 handle_bytes(&self.drone, &buf[..bytes_read]);
             }
         }
+
+        log::error!("listener shutting down");
     }
 }