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,7 @@ edition = "2018"
 # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
 [dependencies]
 arsdk-rs = { path = "../arsdk-rs" }
+scroll = "0.10.1"
 
 [dev-dependencies]
 # Used for examples

anafi-rs/examples/liftoff.rs

@@ -1,27 +1,64 @@
 use log::info;
 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,16 +1,19 @@
 [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"]}
+pnet = "0.26"
+serde = { version = "1.0", features = ["derive"] }
 serde_json = "1.0"
 serde_with = "1"
 dashmap = "3.11"

arsdk-rs/src/ardrone3/piloting.rs

@@ -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 @@
     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,
@@ -80,7 +80,7 @@
     },
 }
 
 impl Into<u16> for &PilotingState {
     fn into(self) -> u16 {
         use PilotingState::*;
 
@@ -91,7 +91,7 @@
             NavigateHomeStateChanged => 3,
             PositionChanged => 4,
             SpeedChanged => 5,
-            AttitudeChanged => 6,
+            AttitudeChanged(_) => 6,
             AutoTakeOffModeChanged => 7,
             AltitudeChanged => 8,
             GpsLocationChanged => 9,
@@ -106,6 +106,11 @@
     }
 }
 
+// ------------------------------------------------------------
+
+#[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 @@
         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)?,
@@ -130,7 +135,7 @@
         }
     }
 
     impl<'a> ctx::TryIntoCtx<Endian> for PilotingState {
         type Error = Error;
 
         fn try_into_ctx(self, this: &mut [u8], ctx: Endian) -> Result<usize, Self::Error> {
@@ -148,4 +153,21 @@
             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 @@
     /// 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
@@ -53,7 +53,7 @@
 
 // --------------------- Conversion impls --------------------- //
 
 impl Into<u8> for &Feature {
     fn into(self) -> u8 {
         use Feature::*;