crazyflie_lib/subsystems/param.rs
1//! # Parameter subsystem
2//!
3//! The Crazyflie exposes a param subsystem that allows to easily declare parameter
4//! variables in the Crazyflie and to discover, read and write them from the ground.
5//!
6//! Variables are defined in a table of content that is downloaded upon connection.
7//! Each param variable have a unique name composed from a group and a variable name.
8//! Functions that accesses variables, take a `name` parameter that accepts a string
9//! in the format "group.variable"
10//!
11//! During connection, the full param table of content is downloaded form the
12//! Crazyflie as well as the values of all the variable. If a variable value
13//! is modified by the Crazyflie during runtime, it sends a packet with the new
14//! value which updates the local value cache.
15
16use crate::{crtp_utils::WaitForPacket, Error, Result};
17use crate::{Value, ValueType};
18use crazyflie_link::Packet;
19use flume as channel;
20use futures::lock::Mutex;
21use std::{
22 collections::{BTreeMap, HashMap},
23 convert::{TryFrom, TryInto},
24 sync::Arc,
25};
26
27use crate::crazyflie::PARAM_PORT;
28
29#[derive(Debug)]
30struct ParamItemInfo {
31 item_type: ValueType,
32 writable: bool,
33}
34
35impl TryFrom<u8> for ParamItemInfo {
36 type Error = Error;
37
38 fn try_from(value: u8) -> Result<Self> {
39 Ok(Self {
40 item_type: match value & 0x0f {
41 0x08 => ValueType::U8,
42 0x09 => ValueType::U16,
43 0x0A => ValueType::U32,
44 0x0B => ValueType::U64,
45 0x00 => ValueType::I8,
46 0x01 => ValueType::I16,
47 0x02 => ValueType::I32,
48 0x03 => ValueType::I64,
49 0x05 => ValueType::F16,
50 0x06 => ValueType::F32,
51 0x07 => ValueType::F64,
52 _ => {
53 return Err(Error::ParamError(format!(
54 "Type error in TOC: type {} is unknown",
55 value & 0x0f
56 )))
57 }
58 },
59 writable: (value & (1 << 6)) == 0,
60 })
61 }
62}
63
64type ParamChangeWatchers =
65 Arc<Mutex<Vec<futures::channel::mpsc::UnboundedSender<(String, Value)>>>>;
66
67/// # Access to the Crazyflie Param Subsystem
68///
69/// This struct provide methods to interact with the parameter subsystem. See the
70/// [param module documentation](crate::subsystems::param) for more context and information.
71#[derive(Debug)]
72pub struct Param {
73 uplink: channel::Sender<Packet>,
74 read_downlink: channel::Receiver<Packet>,
75 write_downlink: Mutex<channel::Receiver<Packet>>,
76 toc: Arc<BTreeMap<String, (u16, ParamItemInfo)>>,
77 values: Arc<Mutex<HashMap<String, Value>>>,
78 watchers: ParamChangeWatchers,
79}
80
81fn not_found(name: &str) -> Error {
82 Error::ParamError(format!("Parameter {} not found", name))
83}
84
85const READ_CHANNEL: u8 = 1;
86const _WRITE_CHANNEL: u8 = 2;
87const _MISC_CHANNEL: u8 = 3;
88
89impl Param {
90 pub(crate) async fn new(
91 downlink: channel::Receiver<Packet>,
92 uplink: channel::Sender<Packet>,
93 ) -> Result<Self> {
94 let (toc_downlink, read_downlink, write_downlink, misc_downlink) =
95 crate::crtp_utils::crtp_channel_dispatcher(downlink);
96
97 let toc = crate::crtp_utils::fetch_toc(PARAM_PORT, uplink.clone(), toc_downlink).await?;
98
99 let mut param = Self {
100 uplink,
101 read_downlink,
102 write_downlink: Mutex::new(write_downlink),
103 toc: Arc::new(toc),
104 values: Arc::new(Mutex::new(HashMap::new())),
105 watchers: Arc::default(),
106 };
107
108 param.update_values().await?;
109
110 param.spawn_misc_loop(misc_downlink).await;
111
112 Ok(param)
113 }
114
115 async fn update_values(&mut self) -> Result<()> {
116 for (name, (param_id, info)) in self.toc.as_ref() {
117 let mut values = self.values.lock().await;
118 values.insert(
119 name.into(),
120 self.read_value(*param_id, info.item_type).await?,
121 );
122 }
123
124 Ok(())
125 }
126
127 async fn read_value(&self, param_id: u16, param_type: ValueType) -> Result<Value> {
128 let request = Packet::new(PARAM_PORT, READ_CHANNEL, param_id.to_le_bytes().into());
129 self.uplink
130 .send_async(request.clone())
131 .await
132 .map_err(|_| Error::Disconnected)?;
133
134 let response = self
135 .read_downlink
136 .wait_packet(
137 request.get_port(),
138 request.get_channel(),
139 request.get_data(),
140 )
141 .await?;
142
143 Value::from_le_bytes(&response.get_data()[3..], param_type)
144 }
145
146 async fn spawn_misc_loop(&self, misc_downlink: channel::Receiver<Packet>) {
147 let values = self.values.clone();
148 let toc = self.toc.clone();
149
150 tokio::spawn(async move {
151 while let Ok(pk) = misc_downlink.recv_async().await {
152 if pk.get_data()[0] != 1 {
153 continue;
154 }
155
156 // The range sets the buffer to 2 bytes long so this unwrap cannot fail
157 let param_id = u16::from_le_bytes(pk.get_data()[1..3].try_into().unwrap());
158 if let Some((param, (_, item_info))) = toc.iter().find(|v| v.1 .0 == param_id) {
159 if let Ok(value) =
160 Value::from_le_bytes(&pk.get_data()[3..], item_info.item_type)
161 {
162 // The param is tested as being in the toc so this unwrap cannot fail.
163 *values.lock().await.get_mut(param).unwrap() = value;
164 } else {
165 println!("Warning: Malformed param update");
166 }
167 } else {
168 println!("Warning: malformed param update");
169 }
170 }
171 });
172 }
173
174 /// Get the names of all the parameters
175 ///
176 /// The names contain group and name of the parameter variable formatted as
177 /// "group.name".
178 pub fn names(&self) -> Vec<String> {
179 self.toc.keys().cloned().collect()
180 }
181
182 /// Return the type of a parameter variable or an Error if the parameter does not exist.
183 pub fn get_type(&self, name: &str) -> Result<ValueType> {
184 Ok(self
185 .toc
186 .get(name)
187 .ok_or_else(|| not_found(name))?
188 .1
189 .item_type)
190 }
191
192 /// Return true if he parameter variable is writable. False otherwise.
193 ///
194 /// Return an error if the parameter does not exist.
195 pub fn is_writable(&self, name: &str) -> Result<bool> {
196 Ok(self
197 .toc
198 .get(name)
199 .ok_or_else(|| not_found(name))?
200 .1
201 .writable)
202 }
203
204 /// Set a parameter value.
205 ///
206 /// This function will set the variable value and wait for confirmation from the
207 /// Crazyflie. If the set is successful `Ok(())` is returned, otherwise the
208 /// error code reported by the Crazyflie is returned in the error.
209 ///
210 /// This function accepts any primitive type as well as the [Value](crate::Value) type. The
211 /// type of the param variable is checked at runtime and must match the type
212 /// given to the function, either the direct primitive type or the type
213 /// contained in the `Value` enum. For example, to write a u16 value, both lines are valid:
214 ///
215 /// ```no_run
216 /// # use crazyflie_lib::{Crazyflie, Value, Error};
217 /// # use crazyflie_link::LinkContext;
218 /// # async fn example() -> Result<(), Error> {
219 /// # let context = LinkContext::new();
220 /// # let cf = Crazyflie::connect_from_uri(&context, "radio://0/60/2M/E7E7E7E7E7").await?;
221 /// cf.param.set("example.param", 42u16).await?; // From primitive
222 /// cf.param.set("example.param", Value::U16(42)).await?; // From Value
223 /// # Ok(())
224 /// # };
225 /// ```
226 ///
227 /// Return an error in case of type mismatch or if the variable does not exist.
228 pub async fn set<T: Into<Value>>(&self, param: &str, value: T) -> Result<()> {
229 let value: Value = value.into();
230 let (param_id, param_info) = self.toc.get(param).ok_or_else(|| not_found(param))?;
231
232 if param_info.item_type != value.into() {
233 return Err(Error::ParamError(format!(
234 "Parameter {} is type {:?}, cannot set with value {:?}",
235 param, param_info.item_type, value
236 )));
237 }
238
239 let downlink = self.write_downlink.lock().await;
240
241 let mut request_data = Vec::from(param_id.to_le_bytes());
242 request_data.append(&mut value.into());
243 let request = Packet::new(PARAM_PORT, _WRITE_CHANNEL, request_data);
244 self.uplink
245 .send_async(request)
246 .await
247 .map_err(|_| Error::Disconnected)?;
248
249 let answer = downlink
250 .wait_packet(PARAM_PORT, _WRITE_CHANNEL, ¶m_id.to_le_bytes())
251 .await?;
252
253 if answer.get_data()[2] == 0 {
254 // The param is tested as being in the TOC so this unwrap cannot fail
255 *self.values.lock().await.get_mut(param).unwrap() = value;
256 self.notify_watchers(param, value).await;
257 Ok(())
258 } else {
259 Err(Error::ParamError(format!(
260 "Error setting the parameter: code {}",
261 answer.get_data()[2]
262 )))
263 }
264 }
265
266 /// Get param value
267 ///
268 /// Get value of a parameter. This function takes the value from a local
269 /// cache and so is quick.
270 ///
271 /// Similarly to the `set` function above, the type of the param must match
272 /// the return parameter. For example to get a u16 param:
273 /// ```no_run
274 /// # use crazyflie_lib::{Crazyflie, Value, Error};
275 /// # use crazyflie_link::LinkContext;
276 /// # async fn example() -> Result<(), Error> {
277 /// # let context = LinkContext::new();
278 /// # let cf = Crazyflie::connect_from_uri(&context, "radio://0/60/2M/E7E7E7E7E7").await?;
279 /// let example: u16 = cf.param.get("example.param").await?; // To primitive
280 /// dbg!(example); // 42
281 /// let example: Value = cf.param.get("example.param").await?; // To Value
282 /// dbg!(example); // Value::U16(42)
283 /// # Ok(())
284 /// # };
285 /// ```
286 ///
287 /// Return an error in case of type mismatch or if the variable does not exist.
288 pub async fn get<T: TryFrom<Value>>(&self, name: &str) -> Result<T>
289 where
290 <T as TryFrom<Value>>::Error: std::fmt::Debug,
291 {
292 let value = *self
293 .values
294 .lock()
295 .await
296 .get(name)
297 .ok_or_else(|| not_found(name))?;
298
299 Ok(value
300 .try_into()
301 .map_err(|e| Error::ParamError(format!("Type error reading param: {:?}", e)))?)
302 }
303
304 /// Set a parameter from a f64 potentially loosing data
305 ///
306 /// This function is a forgiving version of the `set` function. It allows
307 /// to set any parameter of any type from a `f64` value. This allows to set
308 /// parameters without caring about the type and risking a type mismatch
309 /// runtime error. Since there is no type or value check, loss of information
310 /// can happen when using this function.
311 ///
312 /// Loss of information can happen in the following cases:
313 /// - When setting an integer, the value is truncated to the number of bit of the parameter
314 /// - Example: Setting `257` to a `u8` variable will set it to the value `1`
315 /// - Similarly floating point precision will be truncated to the parameter precision. Rounding is undefined.
316 /// - Setting a floating point outside the range of the parameter is undefined.
317 /// - It is not possible to represent accurately a `u64` parameter in a `f64`.
318 ///
319 /// Returns an error if the param does not exists.
320 pub async fn set_lossy(&self, name: &str, value: f64) -> Result<()> {
321 let param_type = self
322 .toc
323 .get(name)
324 .ok_or_else(|| not_found(name))?
325 .1
326 .item_type;
327
328 let value = Value::from_f64_lossy(param_type, value);
329
330 self.set(name, value).await
331 }
332
333 /// Get a parameter as a `f64` independently of the parameter type
334 ///
335 /// This function is a forgiving version of the `get` function. It allows
336 /// to get any parameter of any type as a `f64` value. This allows to get
337 /// parameters without caring about the type and risking a type mismatch
338 /// runtime error. Since there is no type or value check, loss of information
339 /// can happen when using this function.
340 ///
341 /// Loss of information can happen in the following cases:
342 /// - It is not possible to represent accurately a `u64` parameter in a `f64`.
343 ///
344 /// Returns an error if the param does not exists.
345 pub async fn get_lossy(&self, name: &str) -> Result<f64> {
346 let value: Value = self.get(name).await?;
347
348 Ok(value.to_f64_lossy())
349 }
350
351 /// Get notified for all parameter value change
352 ///
353 /// This function returns an async stream that will generate a tuple containing
354 /// the name of the variable that has changed (in the form of group.name)
355 /// and its new value.
356 ///
357 /// There can be two reasons for a parameter to change:
358 /// - Either the parameter was changed by a call to [Param::set()]. The
359 /// notification will be generated when the Crazyflie confirms the parameter
360 /// has been set.
361 /// - Or it can be a parameter change in the Crazyflie itself. The Crazyflie
362 /// will send notification packet for every internal parameter change.
363 pub async fn watch_change(&self) -> impl futures::Stream<Item = (String, Value)> {
364 let (tx, rx) = futures::channel::mpsc::unbounded();
365
366 let mut watchers = self.watchers.lock().await;
367 watchers.push(tx);
368
369 rx
370 }
371
372 async fn notify_watchers(&self, name: &str, value: Value) {
373 let mut to_remove = Vec::new();
374 let mut watchers = self.watchers.lock().await;
375
376 for (i, watcher) in watchers.iter().enumerate() {
377 if watcher.unbounded_send((name.to_owned(), value)).is_err() {
378 to_remove.push(i);
379 }
380 }
381
382 // Remove watchers that have dropped
383 for i in to_remove.into_iter().rev() {
384 watchers.remove(i);
385 }
386 }
387}