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 // Success response: firmware echoes back the written value
254 let expected_bytes: Vec<u8> = value.into();
255 let data = answer.get_data();
256 if data.len() < 2 {
257 return Err(Error::ProtocolError(
258 format!("Parameter write response too short: expected at least 2 bytes, got {}", data.len())
259 ));
260 }
261 let echoed_bytes = &data[2..];
262
263 if echoed_bytes == expected_bytes.as_slice() {
264 // The param is tested as being in the TOC so this unwrap cannot fail
265 *self.values.lock().await.get_mut(param).unwrap() = value;
266 self.notify_watchers(param, value).await;
267 Ok(())
268 } else {
269 // If echoed value doesn't match, it's likely a parameter error code
270 if echoed_bytes.is_empty() {
271 return Err(Error::ProtocolError(
272 "Parameter write response invalid: no error code or echoed value".to_string()
273 ));
274 }
275 let error_code = echoed_bytes[0]; // For u8 params, single byte error code
276 Err(Error::ParamError(format!(
277 "Error setting parameter: parameter error code {}",
278 error_code
279 )))
280 }
281 }
282
283 /// Get param value
284 ///
285 /// Get value of a parameter. This function takes the value from a local
286 /// cache and so is quick.
287 ///
288 /// Similarly to the `set` function above, the type of the param must match
289 /// the return parameter. For example to get a u16 param:
290 /// ```no_run
291 /// # use crazyflie_lib::{Crazyflie, Value, Error};
292 /// # use crazyflie_link::LinkContext;
293 /// # async fn example() -> Result<(), Error> {
294 /// # let context = LinkContext::new();
295 /// # let cf = Crazyflie::connect_from_uri(&context, "radio://0/60/2M/E7E7E7E7E7").await?;
296 /// let example: u16 = cf.param.get("example.param").await?; // To primitive
297 /// dbg!(example); // 42
298 /// let example: Value = cf.param.get("example.param").await?; // To Value
299 /// dbg!(example); // Value::U16(42)
300 /// # Ok(())
301 /// # };
302 /// ```
303 ///
304 /// Return an error in case of type mismatch or if the variable does not exist.
305 pub async fn get<T: TryFrom<Value>>(&self, name: &str) -> Result<T>
306 where
307 <T as TryFrom<Value>>::Error: std::fmt::Debug,
308 {
309 let value = *self
310 .values
311 .lock()
312 .await
313 .get(name)
314 .ok_or_else(|| not_found(name))?;
315
316 Ok(value
317 .try_into()
318 .map_err(|e| Error::ParamError(format!("Type error reading param: {:?}", e)))?)
319 }
320
321 /// Set a parameter from a f64 potentially loosing data
322 ///
323 /// This function is a forgiving version of the `set` function. It allows
324 /// to set any parameter of any type from a `f64` value. This allows to set
325 /// parameters without caring about the type and risking a type mismatch
326 /// runtime error. Since there is no type or value check, loss of information
327 /// can happen when using this function.
328 ///
329 /// Loss of information can happen in the following cases:
330 /// - When setting an integer, the value is truncated to the number of bit of the parameter
331 /// - Example: Setting `257` to a `u8` variable will set it to the value `1`
332 /// - Similarly floating point precision will be truncated to the parameter precision. Rounding is undefined.
333 /// - Setting a floating point outside the range of the parameter is undefined.
334 /// - It is not possible to represent accurately a `u64` parameter in a `f64`.
335 ///
336 /// Returns an error if the param does not exists.
337 pub async fn set_lossy(&self, name: &str, value: f64) -> Result<()> {
338 let param_type = self
339 .toc
340 .get(name)
341 .ok_or_else(|| not_found(name))?
342 .1
343 .item_type;
344
345 let value = Value::from_f64_lossy(param_type, value);
346
347 self.set(name, value).await
348 }
349
350 /// Get a parameter as a `f64` independently of the parameter type
351 ///
352 /// This function is a forgiving version of the `get` function. It allows
353 /// to get any parameter of any type as a `f64` value. This allows to get
354 /// parameters without caring about the type and risking a type mismatch
355 /// runtime error. Since there is no type or value check, loss of information
356 /// can happen when using this function.
357 ///
358 /// Loss of information can happen in the following cases:
359 /// - It is not possible to represent accurately a `u64` parameter in a `f64`.
360 ///
361 /// Returns an error if the param does not exists.
362 pub async fn get_lossy(&self, name: &str) -> Result<f64> {
363 let value: Value = self.get(name).await?;
364
365 Ok(value.to_f64_lossy())
366 }
367
368 /// Get notified for all parameter value change
369 ///
370 /// This function returns an async stream that will generate a tuple containing
371 /// the name of the variable that has changed (in the form of group.name)
372 /// and its new value.
373 ///
374 /// There can be two reasons for a parameter to change:
375 /// - Either the parameter was changed by a call to [Param::set()]. The
376 /// notification will be generated when the Crazyflie confirms the parameter
377 /// has been set.
378 /// - Or it can be a parameter change in the Crazyflie itself. The Crazyflie
379 /// will send notification packet for every internal parameter change.
380 pub async fn watch_change(&self) -> impl futures::Stream<Item = (String, Value)> {
381 let (tx, rx) = futures::channel::mpsc::unbounded();
382
383 let mut watchers = self.watchers.lock().await;
384 watchers.push(tx);
385
386 rx
387 }
388
389 async fn notify_watchers(&self, name: &str, value: Value) {
390 let mut to_remove = Vec::new();
391 let mut watchers = self.watchers.lock().await;
392
393 for (i, watcher) in watchers.iter().enumerate() {
394 if watcher.unbounded_send((name.to_owned(), value)).is_err() {
395 to_remove.push(i);
396 }
397 }
398
399 // Remove watchers that have dropped
400 for i in to_remove.into_iter().rev() {
401 watchers.remove(i);
402 }
403 }
404}