crazyflie_lib/subsystems/
log.rs

1//! # Data logging subsystem
2//!
3//! The Crazyflie log subsystem allows to asynchronously log the value of exposed Crazyflie variables from the ground.
4//!
5//! At connection time, a Table Of Content (TOC) of the log variable is fetched from the Crazyflie which allows to
6//! log variables using their names. To log variable a [LogBlock] needs to be created. The variable to be logged are
7//! added to the LogBlock and then the LogBlock can be started returning a LogStream that will yield the log data.
8//!
9//! ```no_run
10//! # use crazyflie_lib::{Crazyflie, Value, Error, subsystems::log::LogPeriod};
11//! # use crazyflie_link::LinkContext;
12//! # async fn example() -> Result<(), Error> {
13//! # let context = LinkContext::new();
14//! # let cf = Crazyflie::connect_from_uri(
15//! #   &context,
16//! #   "radio://0/60/2M/E7E7E7E7E7",
17//! #   crazyflie_lib::NoTocCache
18//! # ).await?;
19//! // Create the log block
20//! let mut block = cf.log.create_block().await?;
21//!
22//! // Append Variables
23//! block.add_variable("stateEstimate.roll").await?;
24//! block.add_variable("stateEstimate.pitch").await?;
25//! block.add_variable("stateEstimate.yaw").await?;
26//!
27//! // Start the block
28//! let period = LogPeriod::from_millis(100)?;
29//! let stream = block.start(period).await?;
30//!
31//! // Get Data!
32//! while let Ok(data) = stream.next().await {
33//!     println!("Yaw is {:?}", data.data["stateEstimate.yaw"]);
34//! }
35//! # Ok(())
36//! # };
37//! ```
38
39use crate::crtp_utils::{TocCache, WaitForPacket};
40use crate::{Error, Result, Value, ValueType};
41use crazyflie_link::Packet;
42use flume as channel;
43use futures::lock::Mutex;
44use serde::{Deserialize, Serialize};
45use std::collections::HashMap;
46use std::convert::TryInto;
47use std::sync::Weak;
48use std::{collections::BTreeMap, convert::TryFrom, sync::Arc, time::Duration};
49
50use crate::crazyflie::LOG_PORT;
51
52/// # Access to the Crazyflie Log Subsystem
53///
54/// This struct provide functions to interact with the Crazyflie Log subsystem.
55///
56/// See the [log module documentation](crate::subsystems::log) for more context and information.
57#[derive(Debug)]
58pub struct Log {
59    uplink: channel::Sender<Packet>,
60    control_downlink: Arc<Mutex<channel::Receiver<Packet>>>,
61    toc: Arc<BTreeMap<String, (u16, LogItemInfo)>>,
62    next_block_id: Mutex<u8>,
63    data_channels: Arc<Mutex<BTreeMap<u8, flume::Sender<Packet>>>>,
64    active_blocks: Mutex<BTreeMap<u8, Weak<()>>>,
65}
66
67fn not_found(name: &str) -> Error {
68    Error::ParamError(format!("Log variable {} not found", name))
69}
70
71const CONTROL_CHANNEL: u8 = 1;
72
73const DELETE_BLOCK: u8 = 2;
74const START_BLOCK: u8 = 3;
75const STOP_BLOCK: u8 = 4;
76const RESET: u8 = 5;
77const CREATE_BLOCK_V2: u8 = 6;
78const APPEND_BLOCK_V2: u8 = 7;
79
80impl Log {
81    pub(crate) async fn new<T>(
82        downlink: channel::Receiver<Packet>,
83        uplink: channel::Sender<Packet>,
84        toc_cache: T,
85    ) -> Result<Self>
86    where
87        T: TocCache,
88    {
89        let (toc_downlink, control_downlink, data_downlink, _) =
90            crate::crtp_utils::crtp_channel_dispatcher(downlink);
91
92        let toc = crate::crtp_utils::fetch_toc(LOG_PORT, uplink.clone(), toc_downlink, toc_cache).await?;
93        let toc = Arc::new(toc);
94
95        let control_downlink = Arc::new(Mutex::new(control_downlink));
96
97        let next_block_id = Mutex::new(0);
98
99        let data_channels = Arc::new(Mutex::new(BTreeMap::new()));
100
101        let active_blocks = Mutex::new(BTreeMap::new());
102
103        let log = Self {
104            uplink,
105            control_downlink,
106            toc,
107            next_block_id,
108            data_channels,
109            active_blocks,
110        };
111        log.reset().await?;
112        log.spawn_data_dispatcher(data_downlink).await;
113
114        Ok(log)
115    }
116
117    async fn reset(&self) -> Result<()> {
118        let downlink = self.control_downlink.lock().await;
119
120        let pk = Packet::new(LOG_PORT, CONTROL_CHANNEL, vec![RESET]);
121        self.uplink
122            .send_async(pk)
123            .await
124            .map_err(|_| Error::Disconnected)?;
125
126        let pk = downlink
127            .wait_packet(LOG_PORT, CONTROL_CHANNEL, &[RESET])
128            .await?;
129        assert_eq!(pk.get_data()[2], 0);
130
131        Ok(())
132    }
133
134    async fn spawn_data_dispatcher(&self, data_downlink: flume::Receiver<Packet>) {
135        let data_channels = self.data_channels.clone();
136        tokio::spawn(async move {
137            while let Ok(packet) = data_downlink.recv_async().await {
138                if packet.get_data().len() > 1 {
139                    let block_id = packet.get_data()[0];
140                    let data_channels = data_channels.lock().await;
141                    if data_channels.contains_key(&block_id)
142                        && data_channels
143                            .get(&block_id)
144                            .unwrap()
145                            .send_async(packet)
146                            .await
147                            .is_err()
148                    {
149                        break;
150                    }
151                }
152            }
153            data_channels.lock().await.clear();
154        });
155    }
156
157    /// Get the names of all the log variables
158    ///
159    /// The names contain group and name of the log variable formatted as
160    /// "group.name".
161    pub fn names(&self) -> Vec<String> {
162        self.toc.keys().cloned().collect()
163    }
164
165    /// Return the type of a log variable or an Error if the parameter does not exist.
166    pub fn get_type(&self, name: &str) -> Result<ValueType> {
167        Ok(self
168            .toc
169            .get(name)
170            .ok_or_else(|| not_found(name))?
171            .1
172            .item_type)
173    }
174
175    async fn generate_next_block_id(&self) -> Result<u8> {
176        let mut next_block_id = self.next_block_id.lock().await;
177        if *next_block_id == u8::MAX {
178            return Err(Error::LogError("No more block ID available!".into()));
179        }
180        let id = *next_block_id;
181        *next_block_id += 1;
182        Ok(id)
183    }
184
185    /// Cleanup dropped LogBlocks
186    async fn cleanup_blocks(&self) -> Result<()> {
187        let mut active_blocks = self.active_blocks.lock().await;
188
189        for (block_id, canary) in active_blocks.clone().into_iter() {
190            if canary.upgrade() == None {
191                // Delete the block!
192                let control_downlink = self.control_downlink.lock().await;
193
194                let pk = Packet::new(LOG_PORT, CONTROL_CHANNEL, vec![DELETE_BLOCK, block_id]);
195                self.uplink
196                    .send_async(pk)
197                    .await
198                    .map_err(|_| Error::Disconnected)?;
199
200                let pk = control_downlink
201                    .wait_packet(LOG_PORT, CONTROL_CHANNEL, &[DELETE_BLOCK, block_id])
202                    .await?;
203                let error = pk.get_data()[2];
204
205                if error != 0 {
206                    return Err(Error::LogError(format!(
207                        "Protocol error when deleting block: {}",
208                        error
209                    )));
210                }
211
212                active_blocks.remove_entry(&block_id);
213            }
214        }
215
216        Ok(())
217    }
218
219    /// Create a Log block
220    ///
221    /// This will create a log block in the Crazyflie firmware and return a
222    /// [LogBlock] object that can be used to add variable to the block and start
223    /// logging
224    ///
225    /// This function can fail if there is no more log block ID available: each
226    /// log block is assigned a 8 bit ID by the lib and so far they are not
227    /// re-used. So during a Crazyflie connection lifetime, up to 256 log
228    /// blocks can be created. If this becomes a problem for any use-case, it
229    /// can be solved by a more clever ID generation algorithm.
230    ///
231    /// The Crazyflie firmware also has a limit in number of active log block,
232    /// this function will fail if this limit is reached. Unlike for the ID, the
233    /// active log blocks in the Crazyflie are cleaned-up when the [LogBlock]
234    /// object is dropped.
235    pub async fn create_block(&self) -> Result<LogBlock> {
236        self.cleanup_blocks().await?;
237
238        let block_id = self.generate_next_block_id().await?;
239        let control_downlink = self.control_downlink.lock().await;
240
241        let pk = Packet::new(LOG_PORT, CONTROL_CHANNEL, vec![CREATE_BLOCK_V2, block_id]);
242        self.uplink
243            .send_async(pk)
244            .await
245            .map_err(|_| Error::Disconnected)?;
246
247        let pk = control_downlink
248            .wait_packet(LOG_PORT, CONTROL_CHANNEL, &[CREATE_BLOCK_V2, block_id])
249            .await?;
250        let error = pk.get_data()[2];
251
252        if error != 0 {
253            return Err(Error::LogError(format!(
254                "Protocol error when creating block: {}",
255                error
256            )));
257        }
258
259        // Todo: Create data channel for the block
260        let (tx, rx) = flume::unbounded();
261        self.data_channels.lock().await.insert(block_id, tx);
262
263        let canary = Arc::new(());
264        self.active_blocks
265            .lock()
266            .await
267            .insert(block_id, Arc::downgrade(&canary));
268
269        Ok(LogBlock {
270            _canary: canary,
271            toc: Arc::downgrade(&self.toc),
272            uplink: self.uplink.clone(),
273            control_downlink: Arc::downgrade(&self.control_downlink),
274            block_id,
275            variables: Vec::new(),
276            data_channel: rx,
277        })
278    }
279}
280
281#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
282struct LogItemInfo {
283    item_type: ValueType,
284}
285
286impl TryFrom<u8> for LogItemInfo {
287    type Error = Error;
288
289    fn try_from(log_type: u8) -> Result<Self> {
290        let item_type = match log_type {
291            1 => ValueType::U8,
292            2 => ValueType::U16,
293            3 => ValueType::U32,
294            4 => ValueType::I8,
295            5 => ValueType::I16,
296            6 => ValueType::I32,
297            7 => ValueType::F32,
298            8 => ValueType::F16,
299            _ => {
300                return Err(Error::ProtocolError(format!(
301                    "Invalid log item type: {}",
302                    log_type
303                )))
304            }
305        };
306
307        Ok(LogItemInfo { item_type })
308    }
309}
310
311impl TryInto<u8> for LogItemInfo {
312    type Error = Error;
313
314    fn try_into(self) -> Result<u8> {
315        let value = match self.item_type {
316            ValueType::U8 => 1,
317            ValueType::U16 => 2,
318            ValueType::U32 => 3,
319            ValueType::I8 => 4,
320            ValueType::I16 => 5,
321            ValueType::I32 => 6,
322            ValueType::F32 => 7,
323            ValueType::F16 => 8,
324            _ => {
325                return Err(Error::LogError(format!(
326                    "Value type {:?} not handled by log",
327                    self.item_type
328                )))
329            }
330        };
331        Ok(value)
332    }
333}
334
335/// # Log Block
336///
337/// This object represent an IDLE LogBlock in the Crazyflie.
338///
339/// If the [LogBlock] object is dropped or its associated [LogStream], the
340/// Log Block will be deleted in the Crazyflie freeing resources.
341///
342/// See the [log module documentation](crate::subsystems::log) for more context and information.
343pub struct LogBlock {
344    _canary: Arc<()>,
345    toc: Weak<BTreeMap<String, (u16, LogItemInfo)>>,
346    uplink: channel::Sender<Packet>,
347    control_downlink: Weak<Mutex<channel::Receiver<Packet>>>,
348    block_id: u8,
349    variables: Vec<(String, ValueType)>,
350    data_channel: flume::Receiver<Packet>,
351}
352
353impl LogBlock {
354    /// Start log block and return a stream to read  the value
355    ///
356    /// Since a log-block cannot be modified after being started, this function
357    /// consumes the [LogBlock] object and return a [LogStream]. The function
358    /// [LogStream::stop()] can be called on the LogStream to get back the [LogBlock] object.
359    ///
360    /// This function can fail if there is a protocol error or an error
361    /// reported by the Crazyflie. In such case, the LogBlock object will be
362    /// dropped and the block will be deleted in the Crazyflie
363    pub async fn start(self, period: LogPeriod) -> Result<LogStream> {
364        let control_uplink = self.control_downlink.upgrade().ok_or(Error::Disconnected)?;
365        let control_uplink = control_uplink.lock().await;
366
367        let pk = Packet::new(
368            LOG_PORT,
369            CONTROL_CHANNEL,
370            vec![START_BLOCK, self.block_id, period.0],
371        );
372        self.uplink
373            .send_async(pk)
374            .await
375            .map_err(|_| Error::Disconnected)?;
376
377        let answer = control_uplink
378            .wait_packet(LOG_PORT, CONTROL_CHANNEL, &[START_BLOCK, self.block_id])
379            .await?;
380        if answer.get_data().len() != 3 {
381            return Err(Error::ProtocolError(
382                "Malformed Log control packet".to_owned(),
383            ));
384        }
385        let error_code = answer.get_data()[2];
386        if error_code != 0 {
387            return Err(Error::LogError(format!(
388                "Error starting lock: {}",
389                error_code
390            )));
391        }
392
393        Ok(LogStream { log_block: self })
394    }
395
396    /// Add a variable to the log block
397    ///
398    /// A packet will be sent to the Crazyflie to add the variable. The variable is logged in the same format as
399    /// it is stored in the Crazyflie (ie. there is no conversion done)
400    ///
401    /// This function can fail if the variable is not found in the toc or of the Crazyflie returns an error
402    /// The most common error reported by the Crazyflie would be if the log block is already too full.
403    pub async fn add_variable(&mut self, name: &str) -> Result<()> {
404        let toc = self.toc.upgrade().ok_or(Error::Disconnected)?;
405        let (variable_id, info) = toc.get(name).ok_or(Error::VariableNotFound)?;
406
407        // Add variable to Crazyflie
408        let control_uplink = self.control_downlink.upgrade().ok_or(Error::Disconnected)?;
409        let control_uplink = control_uplink.lock().await;
410
411        let mut payload = vec![APPEND_BLOCK_V2, self.block_id, (*info).try_into()?];
412        payload.extend_from_slice(&variable_id.to_le_bytes());
413        let pk = Packet::new(LOG_PORT, CONTROL_CHANNEL, payload);
414        self.uplink
415            .send_async(pk)
416            .await
417            .map_err(|_| Error::Disconnected)?;
418
419        let answer = control_uplink
420            .wait_packet(LOG_PORT, CONTROL_CHANNEL, &[APPEND_BLOCK_V2, self.block_id])
421            .await?;
422        if answer.get_data().len() != 3 {
423            return Err(Error::ProtocolError(
424                "Malformed Log control packet".to_owned(),
425            ));
426        }
427        let error_code = answer.get_data()[2];
428        if error_code != 0 {
429            return Err(Error::LogError(format!(
430                "Error appending variable to block: {}",
431                error_code
432            )));
433        }
434
435        // Add variable to local list
436        self.variables.push((name.to_owned(), info.item_type));
437
438        Ok(())
439    }
440}
441
442/// # Log Steam
443///
444/// This object represents a started log block that is currently returning data
445/// at regular intervals.
446///
447/// Dropping this object or the associated [LogBlock] will delete the log block
448/// in the Crazyflie.
449///
450/// See the [log module documentation](crate::subsystems::log) for more context and information.
451pub struct LogStream {
452    log_block: LogBlock,
453}
454
455impl LogStream {
456    /// Stops the log block from streaming
457    ///
458    /// This method consumes the stream and returns back the log block object so that it can be started again later
459    /// with a different period.
460    ///
461    /// This function can only fail on unexpected protocol error. If it does, the log block is dropped and will be
462    /// cleaned-up next time a log block is created.
463    pub async fn stop(self) -> Result<LogBlock> {
464        let control_uplink = self
465            .log_block
466            .control_downlink
467            .upgrade()
468            .ok_or(Error::Disconnected)?;
469        let control_uplink = control_uplink.lock().await;
470
471        let pk = Packet::new(
472            LOG_PORT,
473            CONTROL_CHANNEL,
474            vec![STOP_BLOCK, self.log_block.block_id],
475        );
476        self.log_block
477            .uplink
478            .send_async(pk)
479            .await
480            .map_err(|_| Error::Disconnected)?;
481
482        let answer = control_uplink
483            .wait_packet(
484                LOG_PORT,
485                CONTROL_CHANNEL,
486                &[STOP_BLOCK, self.log_block.block_id],
487            )
488            .await?;
489        if answer.get_data().len() != 3 {
490            return Err(Error::ProtocolError(
491                "Malformed Log control packet".to_owned(),
492            ));
493        }
494        let error_code = answer.get_data()[2];
495        if error_code != 0 {
496            return Err(Error::LogError(format!(
497                "Error starting lock: {}",
498                error_code
499            )));
500        }
501
502        Ok(self.log_block)
503    }
504
505    /// Get the next log data from the log block stream
506    ///
507    /// This function will wait for the data and only return a value when the
508    /// next data is available.
509    ///
510    /// This function will return an error if the Crazyflie gets disconnected.
511    pub async fn next(&self) -> Result<LogData> {
512        let packet = self
513            .log_block
514            .data_channel
515            .recv_async()
516            .await
517            .map_err(|_| Error::Disconnected)?;
518
519        self.decode_packet(&packet.get_data()[1..])
520    }
521
522    fn decode_packet(&self, data: &[u8]) -> Result<LogData> {
523        let mut timestamp = data[0..=2].to_vec();
524        timestamp.push(0);
525        // The timestamp is 3 bytes long, padded to 4 for u32 conversion
526        let timestamp = u32::from_le_bytes(timestamp.try_into().unwrap());
527
528        let mut index = 3;
529        let mut log_data = HashMap::new();
530        for (name, value_type) in &self.log_block.variables {
531            let byte_length = value_type.byte_length();
532            log_data.insert(
533                name.clone(),
534                Value::from_le_bytes(&data[index..(index + byte_length)], *value_type)?,
535            );
536            index += byte_length;
537        }
538
539        Ok(LogData {
540            timestamp,
541            data: log_data,
542        })
543    }
544}
545
546/// # Log data sample
547///
548/// This object represents a data sample coming from a started log block. It
549/// provides the Crazyflie timestamp in milliseconds when the data was sampled
550/// and a hash-table of variable name and value.
551///
552/// See the [log module documentation](crate::subsystems::log) for more context and information.
553#[derive(Debug)]
554pub struct LogData {
555    /// Timestamp in milliseconds of when the data sample was taken
556    pub timestamp: u32,
557    /// HashMap of the name of the variable vs sampled value
558    pub data: HashMap<String, Value>,
559}
560
561/// # Log block period
562///
563/// This object represent a valid log period. It implements the [`TryFrom<Duration>`]
564/// trait so it can be constructed from a [Duration] object. a [LogPeriod::from_millis()]
565/// function is provided for convenience.
566///
567/// A valid period for a Log block is between 10ms and 2550ms.
568///
569/// See the [log module documentation](crate::subsystems::log) for more context and information.
570pub struct LogPeriod(u8);
571
572impl LogPeriod {
573    /// Create a LogPeriod object from milliseconds
574    ///
575    /// Return an error if the millis is not valid.
576    /// A valid period for a Log block is between 10ms and 2550ms.
577    pub fn from_millis(millis: u64) -> Result<Self> {
578        Duration::from_millis(millis).try_into()
579    }
580}
581
582impl TryFrom<Duration> for LogPeriod {
583    type Error = Error;
584
585    fn try_from(value: Duration) -> Result<Self> {
586        let period_ms = value.as_millis();
587        let period_arg = period_ms / 10;
588        if period_arg == 0 || period_arg > 255 {
589            return Err(Error::LogError(
590                "Invalid log period, should be between 10ms and 2550ms".to_owned(),
591            ));
592        }
593        Ok(LogPeriod(period_arg as u8))
594    }
595}
596
597#[cfg(test)]
598mod tests {
599    use super::*;
600
601    #[test]
602    fn log_toc_cache_format_stability() {
603        // This test pins the serialization format of LogItemInfo.
604        // If it fails, the TOC cache format has changed. Bump TOC_CACHE_VERSION
605        // and update this test.
606        let info = LogItemInfo { item_type: ValueType::U8 };
607        let json = serde_json::to_string(&info).unwrap();
608        assert_eq!(json, r#"{"item_type":"U8"}"#);
609    }
610}
crazyflie_lib/subsystems/log.rs

crazyflie_lib/subsystems/
log.rs
