crazyflie_lib/subsystems/
platform.rs

1//! # Platform services
2//!
3//! The platform CRTP port hosts a couple of utility services. This range from fetching the version of the firmware
4//! and CRTP protocol, communication with apps using the App layer to setting the continuous wave radio mode for
5//! radio testing.
6
7use std::convert::TryFrom;
8
9use crate::{crtp_utils::crtp_channel_dispatcher, Error, Result};
10use crazyflie_link::Packet;
11use flume::{Receiver, Sender};
12use futures::{lock::Mutex, stream, Sink, SinkExt, Stream, StreamExt};
13
14use crate::crazyflie::PLATFORM_PORT;
15use crate::subsystems::supervisor::{CMD_ARM_SYSTEM, CMD_RECOVER_SYSTEM, SUPERVISOR_CH_COMMAND};
16
17const PLATFORM_COMMAND: u8 = 0;
18const VERSION_CHANNEL: u8 = 1;
19const APP_CHANNEL: u8 = 2;
20
21const PLATFORM_SET_CONT_WAVE: u8 = 0;
22
23const VERSION_GET_PROTOCOL: u8 = 0;
24const VERSION_GET_FIRMWARE: u8 = 1;
25const VERSION_GET_DEVICE_TYPE: u8 = 2;
26
27/// Maximum packet size that can be transmitted in an app channel packet.
28pub const APPCHANNEL_MTU: usize = 31;
29
30/// Access to platform services
31///
32/// See the [platform module documentation](crate::subsystems::platform) for more context and information.
33pub struct Platform {
34    version_comm: Mutex<(Sender<Packet>, Receiver<Packet>)>,
35    appchannel_comm: Mutex<Option<(Sender<Packet>, Receiver<Packet>)>>,
36    uplink: Sender<Packet>,
37}
38/// Access to the platform services
39impl Platform {
40    pub(crate) fn new(uplink: Sender<Packet>, downlink: Receiver<Packet>) -> Self {
41        let (_, version_downlink, appchannel_downlink, _) = crtp_channel_dispatcher(downlink);
42
43        Self {
44            version_comm: Mutex::new((uplink.clone(), version_downlink)),
45            appchannel_comm: Mutex::new(Some((uplink.clone(), appchannel_downlink))),
46            uplink,
47        }
48    }
49
50    /// Fetch the protocol version from Crazyflie
51    ///
52    /// The protocol version is updated when new message or breaking change are
53    /// implemented in the protocol.
54    /// see [the crate documentation](crate#compatibility) for more information.
55    ///
56    /// Compatibility is checked at connection time.
57    pub async fn protocol_version(&self) -> Result<u8> {
58        let (uplink, downlink) = &*self.version_comm.lock().await;
59
60        let pk = Packet::new(PLATFORM_PORT, VERSION_CHANNEL, vec![VERSION_GET_PROTOCOL]);
61        uplink.send_async(pk).await?;
62
63        let pk = downlink.recv_async().await?;
64
65        if pk.get_data()[0] != VERSION_GET_PROTOCOL {
66            return Err(Error::ProtocolError("Wrong version answer".to_owned()));
67        }
68
69        Ok(pk.get_data()[1])
70    }
71
72    /// Fetch the firmware version
73    ///
74    /// If this firmware is a stable release, the release name will be returned for example ```2021.06```.
75    /// If this firmware is a git build, between releases, the number of commit since the last release will be added
76    /// for example ```2021.06 +128```.
77    pub async fn firmware_version(&self) -> Result<String> {
78        let (uplink, downlink) = &*self.version_comm.lock().await;
79
80        let pk = Packet::new(PLATFORM_PORT, VERSION_CHANNEL, vec![VERSION_GET_FIRMWARE]);
81        uplink.send_async(pk).await?;
82
83        let pk = downlink.recv_async().await?;
84
85        if pk.get_data()[0] != VERSION_GET_FIRMWARE {
86            return Err(Error::ProtocolError("Wrong version answer".to_owned()));
87        }
88
89        let version = String::from_utf8_lossy(&pk.get_data()[1..]);
90
91        Ok(version.to_string())
92    }
93
94    /// Fetch the device type.
95    ///
96    /// The Crazyflie firmware can run on multiple device. This function returns the name of the device. For example
97    /// ```Crazyflie 2.1``` is returned in the case of a Crazyflie 2.1.
98    pub async fn device_type_name(&self) -> Result<String> {
99        let (uplink, downlink) = &*self.version_comm.lock().await;
100
101        let pk = Packet::new(
102            PLATFORM_PORT,
103            VERSION_CHANNEL,
104            vec![VERSION_GET_DEVICE_TYPE],
105        );
106        uplink.send_async(pk).await?;
107
108        let pk = downlink.recv_async().await?;
109
110        if pk.get_data()[0] != VERSION_GET_DEVICE_TYPE {
111            return Err(Error::ProtocolError("Wrong device type answer".to_owned()));
112        }
113
114        let version = String::from_utf8_lossy(&pk.get_data()[1..]);
115
116        Ok(version.to_string())
117    }
118
119    /// Get sender and receiver to the app channel
120    ///
121    /// This function returns the transmit and receive channel to and from
122    /// the app channel. The channel accepts and generates [AppChannelPacket]
123    /// which guarantees that the packet length is correct. the From trait is
124    /// implemented to all possible ```[u8; n]``` and TryFrom to `Vec<u8>` for
125    /// [AppChannelPacket].
126    pub async fn get_app_channel(
127        &self,
128    ) -> Option<(
129        impl Sink<AppChannelPacket> + use<>,
130        impl Stream<Item = AppChannelPacket> + use<>,
131    )> {
132        match self.appchannel_comm.lock().await.take() { Some((tx, rx)) => {
133            // let all_rx = ;
134
135            let app_tx = Box::pin(tx.into_sink().with_flat_map(|app_pk: AppChannelPacket| {
136                stream::once(async { Ok(Packet::new(PLATFORM_PORT, APP_CHANNEL, app_pk.0)) })
137            }));
138
139            let app_rx = rx
140                .into_stream()
141                .map(|pk: Packet| AppChannelPacket(pk.get_data().to_vec()))
142                .boxed();
143
144            Some((app_tx, app_rx))
145        } _ => {
146            None
147        }}
148    }
149
150    /// Set radio in continious wave mode
151    ///
152    /// If activate is set to true, the Crazyflie's radio will transmit a continious wave at the current channel
153    /// frequency. This will be active until the Crazyflie is reset or this function is called with activate to false.
154    ///
155    /// Setting continious wave will:
156    ///  - Disconnect the radio link. So this function should practically only be used when connected over USB
157    ///  - Jam any radio running on the same frequency, this includes Wifi and Bluetooth
158    ///
159    /// As such, this shall only be used for test purpose in a controlled environment.
160    pub async fn set_cont_wave(&self, activate: bool) -> Result<()> {
161        let command = if activate { 1 } else { 0 };
162        self.uplink
163            .send_async(Packet::new(
164                PLATFORM_PORT,
165                PLATFORM_COMMAND,
166                vec![PLATFORM_SET_CONT_WAVE, command],
167            ))
168            .await?;
169        Ok(())
170    }
171
172    /// Send system arm/disarm request
173    ///
174    /// Arms or disarms the Crazyflie's safety systems. When disarmed, the motors
175    /// will not spin even if thrust commands are sent.
176    ///
177    /// # Arguments
178    /// * `do_arm` - true to arm, false to disarm
179    #[deprecated(since = "0.8.1", note = "Use [`Supervisor::send_arming_request`](crate::subsystems::supervisor::Supervisor::send_arming_request) instead")]
180    pub async fn send_arming_request(&self, do_arm: bool) -> Result<()> {
181        // Route to supervisor port for compatibility
182        let command = if do_arm { 1u8 } else { 0u8 };
183        self.uplink
184            .send_async(Packet::new(
185                crate::crazyflie::SUPERVISOR_PORT,
186                SUPERVISOR_CH_COMMAND,
187                vec![CMD_ARM_SYSTEM, command],
188            ))
189            .await?;
190        Ok(())
191    }
192
193    /// Send crash recovery request
194    ///
195    /// Requests recovery from a crash state detected by the Crazyflie.
196    #[deprecated(since = "0.8.1", note = "Use [`Supervisor::send_crash_recovery_request`](crate::subsystems::supervisor::Supervisor::send_crash_recovery_request) instead")]
197    pub async fn send_crash_recovery_request(&self) -> Result<()> {
198        // Route to supervisor port for compatibility
199        self.uplink
200            .send_async(Packet::new(
201                crate::crazyflie::SUPERVISOR_PORT,
202                SUPERVISOR_CH_COMMAND,
203                vec![CMD_RECOVER_SYSTEM],
204            ))
205            .await?;
206        Ok(())
207    }
208}
209
210/// # App channel packet
211///
212/// This object wraps a `Vec<u8>` but can only be created for byte array of length
213/// <= [APPCHANNEL_MTU].
214///
215/// The [TryFrom] trait is implemented for ```Vec<u8>``` and ```&[u8]```. The
216/// From trait is implemented for fixed size array with compatible length. These
217/// traits are teh expected way to build a packet:
218///
219/// ```
220/// # use std::convert::TryInto;
221/// # use crazyflie_lib::subsystems::platform::AppChannelPacket;
222/// let a: AppChannelPacket = [1,2,3].into();
223/// let b: AppChannelPacket = vec![1,2,3].try_into().unwrap();
224/// ```
225///
226/// And it protects agains building bad packets:
227/// ``` should_panic
228/// # use std::convert::TryInto;
229/// # use crazyflie_lib::subsystems::platform::AppChannelPacket;
230/// // This will panic!
231/// let bad: AppChannelPacket = vec![0; 64].try_into().unwrap();
232/// ```
233///
234/// The traits also allows to go the other way:
235/// ```
236/// # use crazyflie_lib::subsystems::platform::AppChannelPacket;
237/// let pk: AppChannelPacket = [1,2,3].into();
238/// let data: Vec<u8> = pk.into();
239/// assert_eq!(data, vec![1,2,3]);
240/// ```
241#[derive(Debug, PartialEq, Eq)]
242pub struct AppChannelPacket(Vec<u8>);
243
244impl TryFrom<Vec<u8>> for AppChannelPacket {
245    type Error = Error;
246
247    fn try_from(value: Vec<u8>) -> Result<Self> {
248        if value.len() <= APPCHANNEL_MTU {
249            Ok(AppChannelPacket(value))
250        } else {
251            Err(Error::AppchannelPacketTooLarge)
252        }
253    }
254}
255
256impl TryFrom<&[u8]> for AppChannelPacket {
257    type Error = Error;
258
259    fn try_from(value: &[u8]) -> Result<Self> {
260        if value.len() <= APPCHANNEL_MTU {
261            Ok(AppChannelPacket(value.to_vec()))
262        } else {
263            Err(Error::AppchannelPacketTooLarge)
264        }
265    }
266}
267
268impl From<AppChannelPacket> for Vec<u8> {
269    fn from(pk: AppChannelPacket) -> Self {
270        pk.0
271    }
272}
273
274// Implement useful From<> for fixed size array
275// This would be much better as a contrained const generic but
276// it does not seems to be possible at the moment
277macro_rules! from_impl {
278    ($n:expr_2021) => {
279        impl From<[u8; $n]> for AppChannelPacket {
280            fn from(v: [u8; $n]) -> Self {
281                AppChannelPacket(v.to_vec())
282            }
283        }
284    };
285}
286
287from_impl!(0);
288from_impl!(1);
289from_impl!(2);
290from_impl!(3);
291from_impl!(4);
292from_impl!(5);
293from_impl!(6);
294from_impl!(7);
295from_impl!(8);
296from_impl!(9);
297from_impl!(10);
298from_impl!(11);
299from_impl!(12);
300from_impl!(13);
301from_impl!(14);
302from_impl!(15);
303from_impl!(16);
304from_impl!(17);
305from_impl!(18);
306from_impl!(19);
307from_impl!(20);
308from_impl!(21);
309from_impl!(22);
310from_impl!(23);
311from_impl!(24);
312from_impl!(25);
313from_impl!(26);
314from_impl!(27);
315from_impl!(28);
316from_impl!(29);
317from_impl!(30);
crazyflie_lib/subsystems/platform.rs

crazyflie_lib/subsystems/
platform.rs
