crazyflie_lib/
crazyflie.rs

1use crate::subsystems::commander::Commander;
2use crate::subsystems::console::Console;
3use crate::subsystems::log::Log;
4use crate::subsystems::param::Param;
5
6use crate::crtp_utils::CrtpDispatch;
7use crate::subsystems::platform::Platform;
8use crate::{Error, Result};
9use crate::SUPPORTED_PROTOCOL_VERSION;
10use flume as channel;
11use futures::lock::Mutex;
12use tokio::task::JoinHandle;
13use std::sync::atomic::AtomicBool;
14use std::sync::atomic::Ordering::Relaxed;
15use std::sync::Arc;
16use std::time::Duration;
17
18// CRTP ports
19pub(crate) const CONSOLE_PORT: u8 = 0;
20pub(crate) const PARAM_PORT: u8 = 2;
21pub(crate) const COMMANDER_PORT: u8 = 3;
22pub(crate) const _MEMORY_PORT: u8 = 4;
23pub(crate) const LOG_PORT: u8 = 5;
24pub(crate) const _LOCALIZATION_PORT: u8 = 6;
25pub(crate) const _GENERIC_SETPOINT_PORT: u8 = 7;
26pub(crate) const PLATFORM_PORT: u8 = 13;
27pub(crate) const _LINK_PORT: u8 = 15;
28
29/// # The Crazyflie
30///
31/// This struct is one-time use: Creating it will connect to a Crazyflie and once disconnected, either as requested
32/// by the lib user or as a result of a connection loss, the object cannot be reconnected. A new one need to be created
33/// to connect again.
34///
35/// See the [crazyflie-lib crate root documentation](crate) for more context and information.
36pub struct Crazyflie {
37    /// Log subsystem access
38    pub log: Log,
39    /// Parameter subsystem access
40    pub param: Param,
41    /// Commander/setpoint subsystem access
42    pub commander: Commander,
43    /// Console subsystem access
44    pub console: Console,
45    /// Platform services
46    pub platform: Platform,
47    uplink_task: Mutex<Option<JoinHandle<()>>>,
48    dispatch_task: Mutex<Option<JoinHandle<()>>>,
49    disconnect: Arc<AtomicBool>,
50    link: Arc<crazyflie_link::Connection>,
51}
52
53impl Crazyflie {
54    /// Open a Crazyflie connection to a given URI
55    ///
56    /// This function opens a link to the given URI and calls [Crazyflie::connect_from_link()] to connect the Crazyflie.
57    ///
58    /// The executor argument should be an async executor from the crate `async_executors`. See example in the
59    /// [crate root documentation](crate).
60    ///
61    /// An error is returned either if the link cannot be opened or if the Crazyflie connection fails.
62    pub async fn connect_from_uri(
63        link_context: &crazyflie_link::LinkContext,
64        uri: &str,
65    ) -> Result<Self> {
66        let link = link_context.open_link(uri).await?;
67
68        Self::connect_from_link(link).await
69    }
70
71    /// Connect a Crazyflie using an existing link
72    ///
73    /// Connect a Crazyflie using an existing connected link.
74    ///
75    /// The executor argument should be an async executor from the crate `async_executors`. See example in the
76    /// [crate root documentation](crate).
77    ///
78    /// This function will return an error if anything goes wrong in the connection process.
79    pub async fn connect_from_link(
80        link: crazyflie_link::Connection,
81    ) -> Result<Self> {
82        let disconnect = Arc::new(AtomicBool::new(false));
83
84        // Downlink dispatcher
85        let link = Arc::new(link);
86        let mut dispatcher = CrtpDispatch::new(link.clone(), disconnect.clone());
87
88        // Uplink queue
89        let disconnect_uplink = disconnect.clone();
90        let (uplink, rx) = channel::unbounded();
91        let link_uplink = link.clone();
92        let uplink_task = tokio::spawn(async move {
93                while !disconnect_uplink.load(Relaxed) {
94                    match tokio::time::timeout(
95                          Duration::from_millis(100), rx.recv_async()
96                        ).await
97                    {
98                        Ok(Ok(pk)) => {
99                            if link_uplink.send_packet(pk).await.is_err() {
100                                return;
101                            }
102                        }
103                        Err(_) => (),
104                        Ok(Err(flume::RecvError::Disconnected)) => return,
105                    }
106                }
107            });
108
109        // Downlink dispatch
110        let platform_downlink = dispatcher.get_port_receiver(PLATFORM_PORT).unwrap();
111        let log_downlink = dispatcher.get_port_receiver(LOG_PORT).unwrap();
112        let param_downlink = dispatcher.get_port_receiver(PARAM_PORT).unwrap();
113        let console_downlink = dispatcher.get_port_receiver(CONSOLE_PORT).unwrap();
114
115        // Start the downlink packet dispatcher
116        let dispatch_task = dispatcher.run().await?;
117
118        // Start with the platform subsystem to get and test the Crazyflie's protocol version
119        let platform = Platform::new(uplink.clone(), platform_downlink);
120
121        let protocol_version = platform.protocol_version().await?;
122
123        if !(SUPPORTED_PROTOCOL_VERSION..=(SUPPORTED_PROTOCOL_VERSION + 1))
124            .contains(&protocol_version)
125        {
126            return Err(Error::ProtocolVersionNotSupported);
127        }
128
129        // Create subsystems one by one
130        // The future is passed to join!() later down so that all modules initializes at the same time
131        // The get_port_receiver calls are guaranteed to work if the same port is not used twice (any way to express that at compile time?)
132        let log_future = Log::new(log_downlink, uplink.clone());
133        let param_future = Param::new(param_downlink, uplink.clone());
134
135        let commander = Commander::new(uplink.clone());
136        let console = Console::new(console_downlink).await?;
137
138        // Initialize async modules in parallel
139        let (log, param) = futures::join!(log_future, param_future);
140
141        Ok(Crazyflie {
142            log: log?,
143            param: param?,
144            commander,
145            console,
146            platform,
147            uplink_task: Mutex::new(Some(uplink_task)),
148            dispatch_task: Mutex::new(Some(dispatch_task)),
149            disconnect,
150            link,
151        })
152    }
153
154    /// Disconnect the Crazyflie
155    ///
156    /// The Connection can be ended in two ways: either by dropping the [Crazyflie] object or by calling this
157    /// disconnect() function. Once this function return, the Crazyflie is fully disconnected.
158    ///
159    /// Once disconnected, any methods that uses the communication to the Crazyflie will return the error
160    /// [Error::Disconnected]
161    pub async fn disconnect(&self) {
162        // Set disconnect to true, will make both uplink and dispatcher task quit
163        self.disconnect.store(true, Relaxed);
164
165        // Wait for both task to finish
166        if let Some(uplink_task) = self.uplink_task.lock().await.take() {
167            uplink_task.await.expect("Uplink task failed");
168        }
169        if let Some(dispatch_task) = self.dispatch_task.lock().await.take() {
170            dispatch_task.await.expect("Dispatcher task failed");
171        }
172
173        self.link.close().await;
174    }
175
176    /// Wait for the Crazyflie to be disconnected
177    ///
178    /// This function waits for the Crazyflie link to close and for the Crazyflie to fully disconnect. It returns
179    /// a string describing the reason for the disconnection.
180    ///
181    /// One intended use if to call and block on this function from an async task to detect a disconnection and, for
182    /// example, update the state of a GUI.
183    pub async fn wait_disconnect(&self) -> String {
184        let reason = self.link.wait_close().await;
185
186        self.disconnect().await;
187
188        reason
189    }
190}
191
192impl Drop for Crazyflie {
193    fn drop(&mut self) {
194        self.disconnect.store(true, Relaxed);
195    }
196}
crazyflie_lib/crazyflie.rs

crazyflie_lib/
crazyflie.rs
