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))
+        }
+    }
 }