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

crazyflie_lib/subsystems/
platform.rs
