diff --git a/Cargo.lock b/Cargo.lock index 0b8e330..cb3d9aa 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -32,6 +32,8 @@ dependencies = [ "chrono", "dashmap", "pnet", + "scroll", + "scroll_derive", "serde", "serde_json", ] @@ -373,6 +375,23 @@ version = "1.0.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ed3d612bc64430efeb3f7ee6ef26d590dce0c43249217bddc62112540c7941e1" +[[package]] +name = "scroll" +version = "0.10.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "abb2332cb595d33f7edd5700f4cbf94892e680c7f0ae56adab58a35190b66cb1" + +[[package]] +name = "scroll_derive" +version = "0.10.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f8584eea9b9ff42825b46faf46a8c24d2cff13ec152fa2a50df788b87c07ee28" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + [[package]] name = "serde" version = "1.0.106" diff --git a/arsdk-rs/Cargo.toml b/arsdk-rs/Cargo.toml index 8c0e742..c668557 100644 --- a/arsdk-rs/Cargo.toml +++ b/arsdk-rs/Cargo.toml @@ -14,3 +14,5 @@ serde = {version = "1.0", features = ["derive"]} serde_json = "1.0" dashmap = "3.11" chrono = "0.4" +scroll = "0.10" +scroll_derive = "0.10" diff --git a/arsdk-rs/src/ardrone3.rs b/arsdk-rs/src/ardrone3.rs index 661e3a5..39e9f59 100644 --- a/arsdk-rs/src/ardrone3.rs +++ b/arsdk-rs/src/ardrone3.rs @@ -1,7 +1,6 @@ use crate::frame::Data; /// eARCOMMANDS_ID_ARDRONE3_PILOTING_CMD -#[repr(u8)] #[derive(Debug, Clone, Copy, Eq, PartialEq)] pub enum ArDrone3 { /// ARCOMMANDS_ID_ARDRONE3_PILOTING_CMD_FLATTRIM = 0 @@ -29,17 +28,52 @@ pub enum ArDrone3 { /// ARCOMMANDS_ID_ARDRONE3_PILOTING_CMD_CANCELMOVETO = 11 CancelMoveTo = 11, /// ARCOMMANDS_ID_ARDRONE3_PILOTING_CMD_STARTPILOTEDPOI = 12 - StartPilotdPOI = 12, + StartPilotedPOI = 12, /// ARCOMMANDS_ID_ARDRONE3_PILOTING_CMD_STOPPILOTEDPOI = 13 StopPilotedPOI = 13, } impl Data for ArDrone3 { fn serialize(&self) -> Vec { - // todo: Fix this hardcoded value - let take_off: u16 = 1; take_off.to_le_bytes().to_vec() } } + +pub mod scroll_impl { + use super::*; + use scroll::{ctx, Endian, Pread}; + + impl<'a> ctx::TryFromCtx<'a, Endian> for ArDrone3 { + type Error = scroll::Error; + + // and the lifetime annotation on `&'a [u8]` here + fn try_from_ctx(src: &'a [u8], endian: Endian) -> Result<(Self, usize), Self::Error> { + use ArDrone3::*; + + let offset = &mut 0; + + let ardrone3 = match src.gread_with::(offset, endian)? { + 0 => FlatTrim, + 1 => TakeOff, + 2 => PCMD, + 3 => Landing, + 4 => Emergency, + 5 => NavigateHome, + 6 => AutoTakeOffMode, + 7 => MoveBy, + 8 => UserTakeOff, + 9 => Circle, + 10 => MoveTo, + 11 => CancelMoveTo, + 12 => StartPilotedPOI, + 13 => StopPilotedPOI, + _ => return Err(scroll::Error::Custom("Out of range".into())) + }; + + Ok((ardrone3, *offset)) + } + + } +} diff --git a/arsdk-rs/src/command.rs b/arsdk-rs/src/command.rs index 81cf1f4..39f1f8b 100644 --- a/arsdk-rs/src/command.rs +++ b/arsdk-rs/src/command.rs @@ -2,7 +2,7 @@ use crate::ardrone3::ArDrone3; use crate::common; use crate::frame::Data; use crate::jumping_sumo; -#[derive(Debug, PartialEq, Clone, Copy)] +#[derive(Debug, PartialEq, Eq, Clone, Copy)] pub enum Feature { Common(common::Class), // ARCOMMANDS_ID_FEATURE_COMMON = 0, @@ -72,6 +72,68 @@ impl Data for Feature { } } +pub mod scroll_impl { + use super::*; + use scroll::{ctx, Endian, Pread, Pwrite}; + + impl<'a> ctx::TryFromCtx<'a, Endian> for Feature { + type Error = scroll::Error; + + // and the lifetime annotation on `&'a [u8]` here + fn try_from_ctx(src: &'a [u8], endian: Endian) -> Result<(Self, usize), Self::Error> { + let mut offset = 0; + + let feature = match src.gread_with::(&mut offset, endian)? { + 0 => { + let common = src.gread_with(&mut offset, endian)?; + + Self::Common(common) + }, + 1 => { + let ardrone3 = src.gread_with(&mut offset, endian)?; + + Self::ArDrone3(ardrone3) + }, + 2 => Self::Minidrone, + 3 => { + let js_class = src.gread_with(&mut offset, endian)?; + + Self::JumpingSumo(js_class) + }, + 4 => Self::SkyController, + 8 => Self::PowerUp, + 133 => Self::Generic, + 134 => Self::FollowMe, + 135 => Self::Wifi, + 136 => Self::RC, + 137 => Self::DroneManager, + 138 => Self::Mapper, + 139 => Self::Debug, + 140 => Self::ControllerInfo, + 141 => Self::MapperMini, + 142 => Self::ThermalCam, + 144 => Self::Animation, + 147 => Self::SequoiaCam, + _ => return Err(scroll::Error::Custom("Out of range".into())) + }; + + Ok((feature, offset)) + } + } + + impl<'a> ctx::TryIntoCtx for Feature { + type Error = scroll::Error; + + fn try_into_ctx(self, this: &mut [u8], _ctx: Endian) -> Result { + let ser_feature = self.serialize(); + let written = this.pwrite_with(ser_feature.as_slice(), 0, ())?; + + + Ok(written) + } + } +} + // --------------------- Tests --------------------- // #[cfg(test)] diff --git a/arsdk-rs/src/common.rs b/arsdk-rs/src/common.rs index 3f24b21..a5ec272 100644 --- a/arsdk-rs/src/common.rs +++ b/arsdk-rs/src/common.rs @@ -1,7 +1,7 @@ use crate::frame::Data; use chrono::{offset::Utc, DateTime}; -#[derive(Debug, PartialEq, Clone, Copy)] +#[derive(Debug, PartialEq, Eq, Clone, Copy)] pub enum Class { Network, // ARCOMMANDS_ID_COMMON_CLASS_NETWORK = 0, NetworkEvent, // ARCOMMANDS_ID_COMMON_CLASS_NETWORKEVENT = 1, @@ -39,7 +39,7 @@ pub enum Class { Factory, // ARCOMMANDS_ID_COMMON_CLASS_FACTORY = 31, } -#[derive(Debug, PartialEq, Clone, Copy)] +#[derive(Debug, PartialEq, Eq, Clone, Copy)] pub enum Common { AllStates, // ARCOMMANDS_ID_COMMON_COMMON_CMD_ALLSTATES = 0, CurrentDate(DateTime), // ARCOMMANDS_ID_COMMON_COMMON_CMD_CURRENTDATE = 1, @@ -144,6 +144,106 @@ impl Into for Common { } } +pub mod scroll_impl { + use super::*; + use scroll::{ctx, Endian, Pread, Pwrite}; + + impl<'a> ctx::TryFromCtx<'a, Endian> for Class { + type Error = scroll::Error; + + // and the lifetime annotation on `&'a [u8]` here + fn try_from_ctx(src: &'a [u8], endian: Endian) -> Result<(Self, usize), Self::Error> { + let mut offset = 0; + + let class = match src.gread_with::(&mut offset, endian)? { + 0 => Self::Network, + 1 => Self::NetworkEvent, + 2 => Self::Settings, + 3 => Self::SettingsState, + 4 => { + let common = src.gread_with(&mut offset, endian)?; + + Self::Common(common) + } + 5 => Self::CommonState, + 6 => Self::Overheat, + 7 => Self::OverheatState, + 8 => Self::Controller, + 9 => Self::WifiSettings, + 10 => Self::WifiSettingsState, + 11 => Self::Mavlink, + 12 => Self::MavlinkState, + 13 => Self::Calibration, + 14 => Self::CalibrationState, + 15 => Self::CameraSettingsState, + 16 => Self::Gps, + 17 => Self::FlightPlanState, + 18 => Self::ArLibsVersionsState, + 19 => Self::FlightPlanEvent, + 20 => Self::Audio, + 21 => Self::AudioState, + 22 => Self::HeadLights, + 23 => Self::HeadLightsState, + 24 => Self::Animations, + 25 => Self::AnimationsState, + 26 => Self::Accessory, + 27 => Self::AccessoryState, + 28 => Self::Charger, + 29 => Self::ChargerState, + 30 => Self::Runstate, + 31 => Self::Factory, + 32 => Self::FlightPlanSettings, + 33 => Self::FlightPlanSettingsState, + _ => return Err(scroll::Error::Custom("Out of range".into())), + }; + + Ok((class, offset)) + } + } + + impl<'a> ctx::TryIntoCtx for Common { + type Error = scroll::Error; + + fn try_into_ctx(self, this: &mut [u8], _ctx: Endian) -> Result { + let ser_common = self.serialize(); + let written = this.pwrite_with(ser_common.as_slice(), 0, ())?; + + Ok(written) + } + } + + impl<'a> ctx::TryFromCtx<'a, Endian> for Common { + type Error = scroll::Error; + + fn try_from_ctx(src: &'a [u8], endian: Endian) -> Result<(Self, usize), Self::Error> { + use Common::*; + let mut offset = 0; + + let common = match src.gread_with::(&mut offset, endian)? { + 0 => AllStates, + // @TODO: FIX THIS! + 1 => CurrentDate(Utc::now()), + // @TODO: FIX THIS! + 2 => CurrentTime(Utc::now()), + 3 => Reboot, + _ => return Err(scroll::Error::Custom("Out of range".into())), + }; + + Ok((common, offset)) + } + } + + impl<'a> ctx::TryIntoCtx for Class { + type Error = scroll::Error; + + fn try_into_ctx(self, this: &mut [u8], _ctx: Endian) -> Result { + let ser_class = self.serialize(); + let written = this.pwrite_with(ser_class.as_slice(), 0, ())?; + + Ok(written) + } + } +} // --------------------- Tests --------------------- // #[cfg(test)] diff --git a/arsdk-rs/src/frame.rs b/arsdk-rs/src/frame.rs index a52c7e8..4050474 100644 --- a/arsdk-rs/src/frame.rs +++ b/arsdk-rs/src/frame.rs @@ -11,7 +11,7 @@ pub trait Data { fn serialize(&self) -> Vec; } -#[derive(Debug, Clone)] +#[derive(Debug, Clone, PartialEq, Eq)] pub struct Frame { frame_type: Type, buffer_id: BufferID, @@ -64,7 +64,7 @@ impl IntoRawFrame for Frame { // --------------------- Types --------------------- // -#[derive(Debug, PartialEq, Clone, Copy)] +#[derive(Debug, PartialEq, Eq, Clone, Copy)] pub enum Type { Uninitialized = 0, // ARNETWORKAL_FRAME_TYPE_UNINITIALIZED 0 Ack = 1, // ARNETWORKAL_FRAME_TYPE_ACK 1 @@ -191,6 +191,121 @@ impl Into for BufferID { } } +pub mod impl_scroll { + use super::*; + use crate::command::Feature; + + use scroll::{ctx, Endian, Pread, Pwrite}; + + impl<'a> ctx::TryFromCtx<'a, Endian> for Frame { + type Error = scroll::Error; + + // and the lifetime annotation on `&'a [u8]` here + fn try_from_ctx(src: &'a [u8], endian: Endian) -> Result<(Self, usize), Self::Error> { + let offset = &mut 0; + + let frame_type = src.gread_with(offset, endian)?; + let buffer_id = src.gread_with(offset, endian)?; + let sequence_id = src.gread_with(offset, endian)?; + let buf_len: u32 = src.gread_with(offset, endian)?; + let feature = src.gread_with(offset, endian)?; + + if buf_len != *offset as u32 { + // type u8 buffer_id: u8 sequence_id: u8 buf_len: u32 feature + let error = format!("Expected {} got {} bytes of whole Frame", buf_len, *offset - 1); + return Err(scroll::Error::Custom(error)); + } + + Ok((Frame { frame_type, buffer_id, sequence_id, feature }, *offset)) + } + } + + impl<'a> ctx::TryIntoCtx for Frame { + type Error = scroll::Error; + + fn try_into_ctx(self, this: &mut [u8], ctx: Endian) -> Result { + let offset = &mut 0; + + this.gwrite_with::(self.frame_type.into(), offset, ctx)?; + this.gwrite_with::(self.buffer_id.into(), offset, ctx)?; + this.gwrite_with::(self.sequence_id.into(), offset, ctx)?; + + let buf_length_offset = *offset; + // reserve bytes for the buffer length (u32) + this.gwrite_with::(0, offset, ctx)?; + let feature_length = this.gwrite_with::(self.feature, offset, ctx)?; + // 7 bytes + feature_length bytes = buf.length + let written = 7 + feature_length; + this.pwrite_with::(written as u32, buf_length_offset, ctx)?; + + Ok(written) + } + } + + impl<'a> ctx::TryFromCtx<'a, Endian> for Type { + type Error = scroll::Error; + + // and the lifetime annotation on `&'a [u8]` here + fn try_from_ctx(src: &'a [u8], _endian: Endian) -> Result<(Self, usize), Self::Error> { + let offset = &mut 0; + let frame_value = src.gread::(offset)?; + + Type::try_from(frame_value) + .map(|frame_type| (frame_type, *offset)) + .map_err(|err| scroll::Error::Custom(err.to_string())) + } + } + + impl<'a> ctx::TryFromCtx<'a, Endian> for BufferID { + type Error = scroll::Error; + + // and the lifetime annotation on `&'a [u8]` here + fn try_from_ctx(src: &'a [u8], _endian: Endian) -> Result<(Self, usize), Self::Error> { + let offset = &mut 0; + let id_value = src.gread::(offset)?; + + BufferID::try_from(id_value) + .map(|buffer_id| (buffer_id, *offset)) + .map_err(|err| scroll::Error::Custom(err.to_string())) + } + } + + #[cfg(test)] + mod test { + use super::*; + use crate::jumping_sumo::*; + use scroll::{LE, Pwrite}; + + #[test] + fn test_full_frame() { + let message: [u8; 14] = [0x2, 0xa, 0x67, 0xe, 0x0, 0x0, 0x0, 0x3, 0x0, 0x0, 0x0, 0x1, 0x0, 0x9c]; + + let pilot_state = PilotState { + flag: true, + speed: 0, + turn: -100, + }; + + let expected_frame = Frame { + frame_type: Type::Data, + buffer_id: BufferID::CDNonAck, + sequence_id: 103, + feature: command::Feature::JumpingSumo(Class::Piloting(PilotingID::Pilot(pilot_state))), + }; + + let actual_frame: Frame = message.pread_with(0, LE).unwrap(); + + assert_eq!(expected_frame, actual_frame); + + let mut actual_message: [u8; 14] = [0; 14]; + actual_message.pwrite_with(actual_frame, 0, LE).expect("whoopsy"); + + assert_eq!(message, actual_message) + } + + } +} + // --------------------- Tests --------------------- // #[cfg(test)] diff --git a/arsdk-rs/src/jumping_sumo.rs b/arsdk-rs/src/jumping_sumo.rs index 45e155a..a568828 100644 --- a/arsdk-rs/src/jumping_sumo.rs +++ b/arsdk-rs/src/jumping_sumo.rs @@ -1,13 +1,13 @@ use crate::frame::Data; -#[derive(Debug, PartialEq, Clone, Copy)] +#[derive(Debug, PartialEq, Eq, Clone, Copy)] pub enum JumpType { LONG, // ARCOMMANDS_ID_JUMPINGSUMO_CLASS_PILOTING = 0, HIGH, // ARCOMMANDS_ID_JUMPINGSUMO_CLASS_PILOTING = 0, DEFAULT, // ARCOMMANDS_ID_JUMPINGSUMO_CLASS_PILOTING = 0, } -#[derive(Debug, PartialEq, Clone, Copy)] +#[derive(Debug, PartialEq, Eq, Clone, Copy)] pub enum Class { Piloting(PilotingID), // ARCOMMANDS_ID_JUMPINGSUMO_CLASS_PILOTING = 0, PilotingState, // ARCOMMANDS_ID_JUMPINGSUMO_CLASS_PILOTINGSTATE = 1, @@ -33,7 +33,7 @@ pub enum Class { VideoSettingsState, // ARCOMMANDS_ID_JUMPINGSUMO_CLASS_VIDEOSETTINGSSTATE = 22, } -#[derive(Debug, PartialEq, Clone, Copy)] +#[derive(Debug, PartialEq, Eq, Clone, Copy)] pub enum Anim { JumpStop = 0, // ARCOMMANDS_ID_JUMPINGSUMO_ANIMATIONS_CMD_JUMPSTOP = 0, JumpCancel = 1, // ARCOMMANDS_ID_JUMPINGSUMO_ANIMATIONS_CMD_JUMPCANCEL = 1, @@ -42,14 +42,14 @@ pub enum Anim { SimpleAnimation = 4, // ARCOMMANDS_ID_JUMPINGSUMO_ANIMATIONS_CMD_SIMPLEANIMATION = 4, } -#[derive(Debug, PartialEq, Clone, Copy)] +#[derive(Debug, PartialEq, Eq, Clone, Copy)] pub enum PilotingID { Pilot(PilotState), // ARCOMMANDS_ID_JUMPINGSUMO_PILOTING_CMD_PCMD = 0, Posture, // ARCOMMANDS_ID_JUMPINGSUMO_PILOTING_CMD_POSTURE = 1, AddCapOffset, // ARCOMMANDS_ID_JUMPINGSUMO_PILOTING_CMD_ADDCAPOFFSET = 2, } -#[derive(Default, Debug, PartialEq, Clone, Copy)] +#[derive(Default, Debug, PartialEq, Eq, Clone, Copy)] pub struct PilotState { pub flag: bool, pub speed: i8, @@ -150,6 +150,132 @@ impl Into for PilotingID { } } } +pub mod scroll_impl { + use super::*; + use scroll::{ctx, Endian, Pread, Pwrite}; + + impl<'a> ctx::TryFromCtx<'a, Endian> for Class { + type Error = scroll::Error; + + // and the lifetime annotation on `&'a [u8]` here + fn try_from_ctx(src: &'a [u8], endian: Endian) -> Result<(Self, usize), Self::Error> { + let mut offset = 0; + + let class = match src.gread_with::(&mut offset, endian)? { + 0 => { + let pilot_state = src.gread_with(&mut offset, endian)?; + + Self::Piloting(pilot_state) + } + 1 => Self::PilotingState, + // TODO: Impl animcation `TryFromCtx` + 2 => Self::Animations(Anim::Jump), + 3 => Self::AnimationsState, + 5 => Self::SettingsState, + 6 => Self::MediaRecord, + 7 => Self::MediaRecordState, + 8 => Self::NetworkSettings, + 9 => Self::NetworkSettingsState, + 10 => Self::Network, + 11 => Self::NetworkState, + 12 => Self::AutioSettings, + 13 => Self::AudioSettingsState, + 14 => Self::Roadplan, + 15 => Self::RoadplanState, + 16 => Self::SpeedSettings, + 17 => Self::SpeedSettingsState, + 18 => Self::MediaStreaming, + 19 => Self::MediaStreamingState, + 20 => Self::MediaRecordEvent, + 21 => Self::VideoSettings, + 22 => Self::VideoSettingsState, + _ => return Err(scroll::Error::Custom("Out of range".into())), + }; + + Ok((class, offset)) + } + } + + impl<'a> ctx::TryIntoCtx for Class { + type Error = scroll::Error; + + fn try_into_ctx(self, this: &mut [u8], _ctx: Endian) -> Result { + let ser_class = self.serialize(); + let written = this.pwrite_with(ser_class.as_slice(), 0, ())?; + + Ok(written) + } + } + + impl<'a> ctx::TryFromCtx<'a, Endian> for PilotingID { + type Error = scroll::Error; + + // and the lifetime annotation on `&'a [u8]` here + fn try_from_ctx(src: &'a [u8], endian: Endian) -> Result<(Self, usize), Self::Error> { + let mut offset = 0; + + let piloting_id = match src.gread_with::(&mut offset, endian)? { + 0 => { + let pilot_state = src.gread_with(&mut offset, endian)?; + + Self::Pilot(pilot_state) + } + 1 => Self::Posture, + 2 => Self::AddCapOffset, + _ => return Err(scroll::Error::Custom("Out of range".into())), + }; + + Ok((piloting_id, offset)) + } + } + + impl<'a> ctx::TryIntoCtx for PilotingID { + type Error = scroll::Error; + + fn try_into_ctx(self, this: &mut [u8], _ctx: Endian) -> Result { + let ser_class = self.serialize(); + let written = this.pwrite_with(ser_class.as_slice(), 0, ())?; + + Ok(written) + } + } + + impl<'a> ctx::TryFromCtx<'a, Endian> for PilotState { + type Error = scroll::Error; + + // and the lifetime annotation on `&'a [u8]` here + fn try_from_ctx(src: &'a [u8], endian: Endian) -> Result<(Self, usize), Self::Error> { + let mut offset = 0; + + let flag = match src.gread_with::(&mut offset, endian)? { + 0 => false, + 1 => true, + _ => { + return Err(scroll::Error::Custom( + "`flag` is out of range it can only be true or false".into(), + )) + } + }; + let speed: i8 = src.gread_with(&mut offset, endian)?; + let turn: i8 = src.gread_with(&mut offset, endian)?; + + let pilot_state = PilotState { flag, speed, turn }; + + Ok((pilot_state, offset)) + } + } + + impl<'a> ctx::TryIntoCtx for PilotState { + type Error = scroll::Error; + + fn try_into_ctx(self, this: &mut [u8], _ctx: Endian) -> Result { + let ser_class = self.serialize(); + let written = this.pwrite_with(ser_class.as_slice(), 0, ())?; + + Ok(written) + } + } +} // --------------------- Tests --------------------- // diff --git a/bebop2/src/lib.rs b/bebop2/src/lib.rs index 5c3691c..a6b425c 100644 --- a/bebop2/src/lib.rs +++ b/bebop2/src/lib.rs @@ -22,6 +22,9 @@ impl Bebop2 { Ok(Self { drone }) } + // ARCOMMANDS_ID_ARDRONE3_PILOTING_CMD_NAVIGATEHOME + // ARCOMMANDS_ID_ARDRONE3_PILOTING_CMD_AUTOTAKEOFFMODE + pub fn take_off(&self) -> AnyResult<()> { // Ardrone3 // ARCOMMANDS_ID_ARDRONE3_CLASS_PILOTING