crazyflie_lib/subsystems/memory/
loco2.rs

1//! Loco Positioning System v2 memory for anchor position data
2//!
3//! This module provides types and functionality for reading Loco Positioning
4//! System anchor data from the Crazyflie. This includes anchor IDs, active
5//! anchor IDs, and anchor position data.
6
7use crate::{Error, Result, subsystems::memory::{MemoryBackend, memory_types}};
8use memory_types::{FromMemoryBackend, MemoryType};
9use std::collections::HashMap;
10
11const SIZE_FLOAT: usize = std::mem::size_of::<f32>();
12
13const MAX_NR_OF_ANCHORS: usize = 16;
14const ID_LIST_LEN: usize = 1 + MAX_NR_OF_ANCHORS;
15
16const ADR_ID_LIST: usize = 0x0000;
17const ADR_ACTIVE_ID_LIST: usize = 0x1000;
18const ADR_ANCHOR_BASE: usize = 0x2000;
19
20const ANCHOR_PAGE_SIZE: usize = 0x0100;
21const ANCHOR_DATA_LEN: usize = 3 * SIZE_FLOAT + 1;
22
23/// Data for a single Loco Positioning anchor
24#[derive(Debug, Clone, Copy, Default, PartialEq)]
25pub struct LocoAnchorData {
26    /// 3D position (x, y, z) in meters
27    pub position: [f32; 3],
28    /// Whether this anchor has valid data
29    pub is_valid: bool,
30}
31
32impl LocoAnchorData {
33    fn from_bytes(data: &[u8]) -> Result<Self> {
34        if data.len() < ANCHOR_DATA_LEN {
35            return Err(Error::MemoryError(format!(
36                "Insufficient data for anchor: expected {} bytes, got {}",
37                ANCHOR_DATA_LEN, data.len()
38            )));
39        }
40
41        let x = f32::from_le_bytes(data[0..4].try_into().unwrap());
42        let y = f32::from_le_bytes(data[4..8].try_into().unwrap());
43        let z = f32::from_le_bytes(data[8..12].try_into().unwrap());
44        let is_valid = data[12] != 0;
45
46        Ok(Self {
47            position: [x, y, z],
48            is_valid,
49        })
50    }
51}
52
53/// Memory interface for Loco Positioning System v2 data
54///
55/// Provides methods to read anchor IDs, active anchor IDs, and anchor
56/// position data from the Crazyflie's LPS memory.
57#[derive(Debug)]
58pub struct LocoMemory2 {
59    memory: MemoryBackend,
60}
61
62impl LocoMemory2 {
63    fn from_backend(memory: MemoryBackend) -> Result<Self> {
64        if memory.memory_type == MemoryType::Loco2 {
65            Ok(Self { memory })
66        } else {
67            Err(Error::MemoryError(format!(
68                "Expected Loco2 memory type, got {:?}",
69                memory.memory_type
70            )))
71        }
72    }
73
74    /// Read the list of configured anchor IDs
75    ///
76    /// Returns a vector of anchor IDs that are configured in the system.
77    pub async fn read_id_list(&self) -> Result<Vec<u8>> {
78        let data = self.memory.read::<fn(usize, usize)>(ADR_ID_LIST, ID_LIST_LEN, None).await?;
79        let count = data[0] as usize;
80        if count > MAX_NR_OF_ANCHORS {
81            return Err(Error::MemoryError(format!(
82                "Anchor count {} exceeds maximum {}", count, MAX_NR_OF_ANCHORS
83            )));
84        }
85        Ok(data[1..1 + count].to_vec())
86    }
87
88    /// Read the list of currently active anchor IDs
89    ///
90    /// Returns a vector of anchor IDs that are currently active.
91    pub async fn read_active_id_list(&self) -> Result<Vec<u8>> {
92        let data = self.memory.read::<fn(usize, usize)>(ADR_ACTIVE_ID_LIST, ID_LIST_LEN, None).await?;
93        let count = data[0] as usize;
94        if count > MAX_NR_OF_ANCHORS {
95            return Err(Error::MemoryError(format!(
96                "Active anchor count {} exceeds maximum {}", count, MAX_NR_OF_ANCHORS
97            )));
98        }
99        Ok(data[1..1 + count].to_vec())
100    }
101
102    /// Read position data for a single anchor
103    ///
104    /// # Arguments
105    /// * `anchor_id` - The anchor ID (0-15, as stored in the ID list)
106    pub async fn read_anchor_data(&self, anchor_id: u8) -> Result<LocoAnchorData> {
107        if anchor_id as usize >= MAX_NR_OF_ANCHORS {
108            return Err(Error::MemoryError(format!(
109                "Anchor ID {} out of range (0-{})",
110                anchor_id,
111                MAX_NR_OF_ANCHORS - 1
112            )));
113        }
114        let addr = ADR_ANCHOR_BASE + ANCHOR_PAGE_SIZE * anchor_id as usize;
115        let data = self.memory.read::<fn(usize, usize)>(addr, ANCHOR_DATA_LEN, None).await?;
116        LocoAnchorData::from_bytes(&data)
117    }
118
119    /// Read all anchor data
120    ///
121    /// Reads the ID list, active ID list, then fetches position data for each
122    /// configured anchor. Returns a struct containing all the information.
123    pub async fn read_all(&self) -> Result<LocoSystemData> {
124        let anchor_ids = self.read_id_list().await?;
125        let active_ids = self.read_active_id_list().await?;
126
127        let mut anchors = HashMap::new();
128        for &id in &anchor_ids {
129            let data = self.read_anchor_data(id).await?;
130            anchors.insert(id, data);
131        }
132
133        Ok(LocoSystemData {
134            anchor_ids,
135            active_anchor_ids: active_ids,
136            anchors,
137        })
138    }
139}
140
141/// Complete snapshot of the Loco Positioning System state
142#[derive(Debug, Clone)]
143pub struct LocoSystemData {
144    /// List of configured anchor IDs
145    pub anchor_ids: Vec<u8>,
146    /// List of currently active anchor IDs
147    pub active_anchor_ids: Vec<u8>,
148    /// Anchor position data, keyed by anchor ID
149    pub anchors: HashMap<u8, LocoAnchorData>,
150}
151
152impl FromMemoryBackend for LocoMemory2 {
153    async fn from_memory_backend(memory: MemoryBackend) -> Result<Self> {
154        Self::from_backend(memory)
155    }
156
157    async fn initialize_memory_backend(memory: MemoryBackend) -> Result<Self> {
158        Self::from_backend(memory)
159    }
160
161    fn close_memory(self) -> MemoryBackend {
162        self.memory
163    }
164}
crazyflie_lib/subsystems/memory/loco2.rs

crazyflie_lib/subsystems/memory/
loco2.rs
