Anafi mgmt with an example 🚀

Cargo.toml

@@ -3,5 +3,5 @@ members = [
     "arsdk-rs",
     "jumpingsumo-rs",
     "bebop2",
-    # "anafi-rs",
+    "anafi-rs",
 ]

anafi-rs/Cargo.toml

@@ -7,6 +7,9 @@ edition = "2018"
 # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
 [dependencies]
 arsdk-rs = { path = "../arsdk-rs" }
+glutin = "=0.27"
+rdev = {version = "0.5.3", features = ["unstable_grab"] }
+scroll = "0.10.1"
 
 [dev-dependencies]
 # Used for examples

anafi-rs/examples/liftoff.rs

@@ -4,24 +4,61 @@ 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())?;
 
-    info!("Takeoff!");
+    drone.take_off()?;
 
-    for i in 0..50 {
-        drone.take_off()?;
-    }
+    std::thread::sleep(Duration::from_secs(2));
+    log::warn!("UP!");
+    drone.up()?;
+    std::thread::sleep(Duration::from_secs(2));
 
-    info!("Wait 5 seconds and fly UP");
-    std::thread::sleep(Duration::from_secs(5));
+    log::warn!("forward!");
+    drone.forward()?;
+    std::thread::sleep(Duration::from_secs(1));
+    drone.stop()?;
+
+    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));
 
-    for i in 0..50 {
-        drone.landing()?;
+    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));
+
+    std::thread::sleep(Duration::from_secs(2));
+    log::warn!("LAND!");
+    drone.landing()?;
+
+    std::thread::sleep(Duration::from_secs(5));
+
     Ok(())
 }

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,
 }
 
@@ -31,41 +35,148 @@ impl Anafi {
     pub fn take_off(&self) -> Result<(), Error> {
         let feature = Feature::ArDrone3(Some(ArDrone3::Piloting(Piloting::TakeOff)));
 
-        let frame = Frame::for_drone(
-            &self.drone,
-            Type::DataWithAck,
-            BufferID::CDAck,
-            Some(feature),
-        );
+        let frame = Frame::for_drone(&self.drone, Type::Data, BufferID::CDNonAck, Some(feature));
 
         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));
@@ -76,12 +187,7 @@ impl Anafi {
     pub fn landing(&self) -> Result<(), Error> {
         let feature = Feature::ArDrone3(Some(ArDrone3::Piloting(Piloting::Landing)));
 
-        let frame = Frame::for_drone(
-            &self.drone,
-            Type::DataWithAck,
-            BufferID::CDAck,
-            Some(feature),
-        );
+        let frame = Frame::for_drone(&self.drone, Type::Data, BufferID::CDNonAck, Some(feature));
 
         self.drone.send_frame(frame)
     }

arsdk-rs/Cargo.toml

@@ -1,19 +1,22 @@
 [package]
 name = "arsdk-rs"
 version = "0.0.5"
-authors = ["o0Ignition0o <[email protected]>", "Lachezar Lechev <[email protected]>"]
+authors = [
+    "o0Ignition0o <[email protected]>",
+    "Lachezar Lechev <[email protected]>",
+]
 edition = "2018"
 description = "Parrot drones SDK in Rust (AeroRust)"
 license = "MIT/Apache-2.0"
 keywords = ["AeroRust", "drone", "parrot", "sdk"]
 
 [dependencies]
 thiserror = "1.0"
-pnet = "0.25"
-serde = {version = "1.0", features = ["derive"]}
+serde = { version = "1.0", features = ["derive"] }
 serde_json = "1.0"
 serde_with = "1"
 dashmap = "3.11"
 chrono = "0.4"
 scroll = "0.10"
 log = "0.4"
+pnet = "0.35.0"

arsdk-rs/src/ardrone3/piloting.rs

@@ -14,8 +14,8 @@
     /// ARCOMMANDS_Decoder_ARDrone3PilotingPCMDCb (_flag, _roll, _pitch, _yaw, _gaz, _timestampAndSeqNum, ARCOMMANDS_Decoder_ARDrone3PilotingPCMDCustom);
     /// ARCOMMANDS_Decoder_ARDrone3PilotingPCMDDecodeArgs (uint8_t *_flag, int8_t *_roll, int8_t *_pitch, int8_t *_yaw, int8_t *_gaz, uint32_t *_timestampAndSeqNum)
     /// * @param _timestampAndSeqNum Command timestamp in milliseconds (low 24 bits) + command sequence number (high 8 bits) [0;255].
     /// 1_588_771_372_921
     /// @see https://developer.parrot.com/docs/reference/bebop_2/index.html#move-the-drone
     PCMD(PCMD),
     /// ARCOMMANDS_ID_ARDRONE3_PILOTING_CMD_LANDING = 3
     Landing,
@@ -45,7 +45,7 @@
     StopPilotedPOI,
 }
 
 impl Into<u16> for &Piloting {
     fn into(self) -> u16 {
         use Piloting::*;
 
@@ -107,7 +107,7 @@
         }
     }
 
     impl<'a> ctx::TryIntoCtx<Endian> for Piloting {
         type Error = Error;
 
         fn try_into_ctx(self, this: &mut [u8], ctx: Endian) -> Result<usize, Self::Error> {
@@ -120,7 +120,7 @@
                 Piloting::PCMD(pcmd) => {
                     this.gwrite_with(pcmd, &mut offset, ctx)?;
                 }
-                // Piloting::Landing => {}
+                Piloting::Landing => {}
                 // Piloting::Emergency => {}
                 // Piloting::NavigateHome => {}
                 // Piloting::AutoTakeOffMode => {}

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

@@ -10,7 +10,6 @@
     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,
 }
 
@@ -59,7 +58,7 @@
         }
     }
 
     impl<'a> ctx::TryIntoCtx<Endian> for PCMD {
         type Error = Error;
 
         fn try_into_ctx(self, this: &mut [u8], ctx: Endian) -> Result<usize, Self::Error> {
@@ -100,7 +99,7 @@
                 0,
                 0,
             ]);
             let timestamp = Utc.timestamp_millis(timestamp_i64);
             // 8 bits
             let sequence_id = timestamp_and_seq[3];
 
@@ -114,7 +113,7 @@
         }
     }
 
     impl<'a> ctx::TryIntoCtx<Endian> for TimestampAndSeq {
         type Error = Error;
 
         fn try_into_ctx(self, this: &mut [u8], ctx: Endian) -> Result<usize, Self::Error> {

arsdk-rs/src/ardrone3/piloting_state.rs

@@ -43,7 +43,7 @@ pub enum PilotingState {
     SpeedChanged,
     /// ARCOMMANDS_ID_ARDRONE3_PILOTINGSTATE_CMD_ATTITUDECHANGED = 6,
     /// Frame { frame_type: Data, buffer_id: DCNavdata, sequence_id: 40, feature: Some(ArDrone3(Some(PilotingState { data: [6, 0, 44, 49, 49, 55, 153, 38, 7, 185, 107, 25, 201, 63] }))) }
-    AttitudeChanged,
+    AttitudeChanged(AttitudeChanged),
     /// ARCOMMANDS_ID_ARDRONE3_PILOTINGSTATE_CMD_AUTOTAKEOFFMODECHANGED = 7,
     AutoTakeOffModeChanged,
     /// ARCOMMANDS_ID_ARDRONE3_PILOTINGSTATE_CMD_ALTITUDECHANGED = 8,
@@ -91,7 +91,7 @@ impl Into<u16> for &PilotingState {
             NavigateHomeStateChanged => 3,
             PositionChanged => 4,
             SpeedChanged => 5,
-            AttitudeChanged => 6,
+            AttitudeChanged(_) => 6,
             AutoTakeOffModeChanged => 7,
             AltitudeChanged => 8,
             GpsLocationChanged => 9,
@@ -106,6 +106,11 @@ impl Into<u16> for &PilotingState {
     }
 }
 
+// ------------------------------------------------------------
+
+#[derive(Debug, Clone, Eq, PartialEq)]
+pub struct AttitudeChanged(Vec<u8>);
+
 pub mod scroll_impl {
     use super::*;
     use crate::{frame::Error, parse::read_unknown};
@@ -118,8 +123,8 @@ pub mod scroll_impl {
         fn try_from_ctx(src: &'a [u8], ctx: Endian) -> Result<(Self, usize), Self::Error> {
             let mut offset = 0;
 
-            #[allow(clippy::match_single_binding)]
             let piloting_state = match src.gread_with::<u16>(&mut offset, ctx)? {
+                6 => Self::AttitudeChanged(AttitudeChanged::try_from_ctx(&src[offset..], ctx)?.0),
                 unknown => Self::Unknown {
                     piloting_state: unknown,
                     data: read_unknown(src, &mut offset)?,
@@ -148,4 +153,21 @@ pub mod scroll_impl {
             Ok(offset)
         }
     }
+
+    impl<'a> ctx::TryFromCtx<'a, Endian> for AttitudeChanged {
+        type Error = Error;
+
+        // and the lifetime annotation on `&'a [u8]` here
+        fn try_from_ctx(src: &'a [u8], ctx: Endian) -> Result<(Self, usize), Self::Error> {
+            let mut offset = 0;
+
+            let mut buffer = Vec::new();
+
+            while let Ok(b) = src.gread_with::<u8>(&mut offset, ctx) {
+                buffer.push(b);
+            }
+
+            Ok((Self(buffer), offset))
+        }
+    }
 }

arsdk-rs/src/command.rs

@@ -19,17 +19,17 @@ pub enum Feature {
     /// 3. `ARCOMMANDS_Generic_DroneSettings_t` in `libARCommands/libARCommands/ARCOMMANDS_Types.h`
     /// 4. `ARCOMMANDS_Generic_DroneSettingsChanged_t` in `libARCommands/libARCommands/ARCOMMANDS_Types.h`
     Generic,
-    FollowMe,                         // ARCOMMANDS_ID_FEATURE_FOLLOW_ME = 134,
-    Wifi,                             // ARCOMMANDS_ID_FEATURE_WIFI = 135,
-    RC,                               // ARCOMMANDS_ID_FEATURE_RC = 136,
-    DroneManager,                     // ARCOMMANDS_ID_FEATURE_DRONE_MANAGER = 137,
-    Mapper,                           // ARCOMMANDS_ID_FEATURE_MAPPER = 138,
-    Debug,                            // ARCOMMANDS_ID_FEATURE_DEBUG = 139,
-    ControllerInfo,                   // ARCOMMANDS_ID_FEATURE_CONTROLLER_INFO = 140,
-    MapperMini,                       // ARCOMMANDS_ID_FEATURE_MAPPER_MINI = 141,
-    ThermalCam,                       // ARCOMMANDS_ID_FEATURE_THERMAL_CAM = 142,
-    Animation,                        // ARCOMMANDS_ID_FEATURE_ANIMATION = 144,
-    SequoiaCam,                       // ARCOMMANDS_ID_FEATURE_SEQUOIA_CAM = 147,
+    FollowMe,       // ARCOMMANDS_ID_FEATURE_FOLLOW_ME = 134,
+    Wifi,           // ARCOMMANDS_ID_FEATURE_WIFI = 135,
+    RC,             // ARCOMMANDS_ID_FEATURE_RC = 136,
+    DroneManager,   // ARCOMMANDS_ID_FEATURE_DRONE_MANAGER = 137,
+    Mapper,         // ARCOMMANDS_ID_FEATURE_MAPPER = 138,
+    Debug,          // ARCOMMANDS_ID_FEATURE_DEBUG = 139,
+    ControllerInfo, // ARCOMMANDS_ID_FEATURE_CONTROLLER_INFO = 140,
+    MapperMini,     // ARCOMMANDS_ID_FEATURE_MAPPER_MINI = 141,
+    ThermalCam,     // ARCOMMANDS_ID_FEATURE_THERMAL_CAM = 142,
+    Animation,      // ARCOMMANDS_ID_FEATURE_ANIMATION = 144,
+    SequoiaCam,     // ARCOMMANDS_ID_FEATURE_SEQUOIA_CAM = 147,
     /// Unknown 149 from anafi4k
     /// Frame { frame_type: Data, buffer_id: DCNavdata, sequence_id: 14, feature: Some(Unknown { feature: 149, data: [0, 3, 0, 91, 33] }) }
     /// Unknown 148 from anafi4k

arsdk-rs/src/frame.rs

@@ -359,19 +359,26 @@ pub mod impl_scroll {
         fn try_into_ctx(self, this: &mut [u8], ctx: Endian) -> Result<usize, Self::Error> {
             let mut offset = 0;
 
+            // A frame contains the following information:
+
+            // Data type (1 byte)
             this.gwrite_with::<u8>(self.frame_type.into(), &mut offset, ctx)?;
+            // Target buffer ID (1 byte)
             this.gwrite_with::<u8>(self.buffer_id.into(), &mut offset, ctx)?;
+            // Sequence number (1 byte)
             this.gwrite_with::<u8>(self.sequence_id, &mut offset, ctx)?;
-
-            let buf_length_offset = offset;
+            // Total size of the frame (4 bytes, Little endian)
             // reserve bytes for the buffer length (u32)
+            let buf_length_offset = offset;
+
             this.gwrite_with::<u32>(0, &mut offset, ctx)?;
 
+            // Actual data
             if let Some(feature) = self.feature {
                 this.gwrite_with::<Feature>(feature, &mut offset, ctx)?;
             };
 
-            // 7 bytes + feature_length bytes = buf.length
+            // we can now write the actual length
             this.pwrite_with::<u32>(offset as u32, buf_length_offset, ctx)?;
 
             Ok(offset)

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::{
@@ -17,7 +17,7 @@ pub(crate) struct Request {
     pub arstream2: Option<ArStream2>,
 }
 
-#[derive(Deserialize, Debug)]
+#[derive(Deserialize, Serialize, Debug)]
 /// Request: "{\"controller_name\":\"arsdk-rs\",\"controller_type\":\"computer\",\"d2c_port\":43210}"
 /// Response: "{ \"status\": 0, \"c2d_port\": 54321, \"c2d_update_port\": 51, \"c2d_user_port\": 21, \"qos_mode\": 0, \"arstream2_server_stream_port\": 5004, \"arstream2_server_control_port\": 5005 }\u{0}"
 /// `\u{0}` causes issues, but for now we `trim_end_matches`
@@ -72,7 +72,7 @@ pub(crate) fn perform_handshake(
     d2c_port: u16,
 ) -> Result<Response, Error> {
     let request = Request {
-        controller_name: "arsdk-rs".to_string(),
+        controller_name: "desktop".to_string(),
         controller_type: "computer".to_string(),
         d2c_port,
         // Anafi4k:
@@ -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; 256];
+    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

@@ -1,7 +1,7 @@
 use crate::frame::{Frame, FrameType};
 use chrono::{DateTime, Utc};
 use dashmap::DashMap;
-use log::{error, info};
+use log::{debug, error, info, trace};
 use pnet::datalink;
 use scroll::{ctx::TryIntoCtx, Pread, LE};
 use std::net::{IpAddr, Ipv4Addr, SocketAddr, UdpSocket};
@@ -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,26 +122,36 @@ 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);
 
         info!("Init address {}", &init_addr);
 
         let handshake_response = perform_handshake(init_addr, local_listener.port())?;
+
+        log::debug!(
+            "handshake_response: \n{}",
+            serde_json::to_string_pretty(&handshake_response).unwrap()
+        );
         let cmd_sender_target = SocketAddr::new(config.drone_addr, handshake_response.c2d_port);
 
-        info!("{}: Spawning CMD Sender", cmd_sender_target);
+        debug!("{}: Spawning CMD Sender", cmd_sender_target);
 
         spawn_cmd_sender(rx_cmd, local_ip, cmd_sender_target)?;
 
@@ -147,7 +163,7 @@ impl Drone {
     }
 
     pub fn send_frame(&self, frame: frame::Frame) -> Result<(), Error> {
-        let mut raw_message = [0_u8; 2048];
+        let mut raw_message = [0_u8; 128];
         let written = frame.try_into_ctx(&mut raw_message, LE)?;
 
         self.send_raw_message(&raw_message[0..written])
@@ -193,10 +209,16 @@ impl Drone {
 
         self.send_frame(pong)
     }
+
+    pub fn piloting_id(&self) -> u8 {
+        self.inner.sequence_id(frame::BufferID::CDNonAck)
+    }
 }
 
 impl DroneInner {
     pub(crate) fn sequence_id(&self, buffer_id: frame::BufferID) -> u8 {
+        // each buffer has its own independant sequence number,
+        // which should be increased on new data send, but not on retries
         if let Some(mut sequence_id) = self.sequence_ids.get_mut(&buffer_id) {
             let command_id = *sequence_id;
             *sequence_id = sequence_id.overflowing_add(1).0;
@@ -218,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 {
@@ -262,11 +292,24 @@ fn spawn_cmd_sender(
             }
         };
 
-        info!("Frame to sent: {:?}", &frame_to_send);
+        // ACK
+        if frame_to_send[0] == 1 {
+            trace!("Frame to send: {:?}", &frame_to_send);
+        } else {
+            log::debug!(
+                "Frame to send: {}: {:?}",
+                &frame_to_send
+                    .iter()
+                    .map(|n| format!("{:#b}", n))
+                    .collect::<Vec<_>>()
+                    .join(" "),
+                &frame_to_send
+            );
+        }
 
         let frame = frame_to_send.pread_with::<Frame>(0, LE);
 
-        info!(
+        debug!(
             "Sent Frame (length: {}) => {:?}",
             frame_to_send.len(),
             &frame
@@ -287,7 +330,7 @@ fn spawn_cmd_sender(
 #[cfg(test)]
 mod test {
     use super::*;
-    use crate::parse::parse_message_frames;
+    use crate::{ardrone3::Piloting, parse::parse_message_frames};
     use ardrone3::ArDrone3;
     use command::Feature;
     use frame::{BufferID, Type};
@@ -365,4 +408,26 @@ mod test {
             185,
         ];
     }
+
+    #[test]
+    fn send_takeoff_command() {
+        let feature = Feature::ArDrone3(Some(ArDrone3::Piloting(Piloting::TakeOff)));
+
+        let frame = Frame::new(Type::Data, BufferID::CDNonAck, 42, Some(feature));
+
+        let mut raw_message = [0_u8; 128];
+        let written = frame.try_into_ctx(&mut raw_message, LE).unwrap();
+
+        let expected: [u8; 11] = [
+            2,  // frametype data
+            10, // buffer id cdnonack
+            42, // sequence number,
+            11, 0, 0, 0, // size, (4 bytes, little endian)
+            1, // feature Ardrone3
+            0, // piloting
+            1, 0, // takeoff, two bytes
+        ];
+
+        assert_eq!(&expected, &raw_message[..written]);
+    }
 }

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,15 +10,17 @@ pub struct Listener {
 
 impl Listener {
     /// Blocking listener in a infinite loop
-    pub fn listen(&self) {
-        loop {
-            let mut buf = [0_u8; 256];
+    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);
                 debug!("Bytes: {}", print_buf(&buf[..bytes_read]));
 
                 handle_bytes(&self.drone, &buf[..bytes_read]);
             }
         }
+
+        log::error!("listener shutting down");
     }
 }

arsdk-rs/src/parse.rs

@@ -3,7 +3,7 @@ use crate::{
     frame::{BufferID, Error, Frame, Type},
     print_buf, Drone, FrameType,
 };
-use log::{error, info};
+use log::{debug, error, info};
 use scroll::{Pread, Pwrite, LE};
 
 /// - Parses Frames
@@ -14,12 +14,16 @@ pub(crate) fn handle_bytes(drone: &Drone, raw_frames: &[u8]) {
 
     for result in frames.iter() {
         match result {
-            Ok(FrameType::Known(frame)) => info!("Frame: {:?}", frame),
+            Ok(FrameType::Known(Frame {
+                feature: Some(Feature::ArDrone3(Some(ardrone_3))),
+                ..
+            })) => info!("Ardrone3 Frame: {:?}", ardrone_3),
+            Ok(FrameType::Known(frame)) => debug!("Frame: {:?}", frame),
             Ok(FrameType::Unknown(unknown)) => {
-                info!("Unknown Frame: {:?}", unknown);
-                info!("Bytes: {}", print_buf(raw_frames));
+                error!("Unknown Frame: {:?}", unknown);
+                error!("Bytes: {}", print_buf(raw_frames));
             }
-            Err(err) => error!("Receiving Frame: {:?}", err),
+            Err(err) => debug!("Receiving Frame: {:?}", err),
         }
     }
 
@@ -66,7 +70,7 @@ pub(crate) fn parse_message_frames(buf: &[u8]) -> Vec<Result<FrameType, Error>>
     let mut offset = 0;
     // TODO: Check how many frames can we receive at once
     // reasonable given that we receive at most (MAYBE?!) 2 frames
-    let mut frames = Vec::with_capacity(3);
+    let mut frames = Vec::with_capacity(10);
 
     let mut tried = 1;
     // try to read all the buf length & limit to 3 Frames of read