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

crazyflie_lib/subsystems/
platform.rs
