crazyflie_lib/subsystems/memory/
lighthouse.rs

1//! Lighthouse memory for base station geometry and calibration data
2//!
3//! This module provides types and functionality for reading and writing
4//! Lighthouse positioning system configuration to the Crazyflie. This includes
5//! base station geometry (position and orientation) and calibration data.
6
7use crate::{Error, Result, subsystems::memory::{MemoryBackend, memory_types}};
8use memory_types::{FromMemoryBackend, MemoryType};
9use std::collections::HashMap;
10
11// Binary format constants
12const SIZE_FLOAT: usize = std::mem::size_of::<f32>();
13const SIZE_U32: usize = std::mem::size_of::<u32>();
14const SIZE_BOOL: usize = std::mem::size_of::<u8>();
15const SIZE_VECTOR: usize = 3 * SIZE_FLOAT;
16const NUM_SWEEP_PARAMS: usize = 7;
17const NUM_SWEEPS: usize = 2;
18const NUM_ROTATION_ROWS: usize = 3;
19
20/// Helper to read a little-endian f32 from a byte slice at a given offset
21fn read_f32(data: &[u8], offset: usize) -> Result<f32> {
22    data.get(offset..offset + SIZE_FLOAT)
23        .and_then(|slice| slice.try_into().ok())
24        .map(f32::from_le_bytes)
25        .ok_or_else(|| Error::MemoryError(format!(
26            "Failed to read f32 at offset {}", offset
27        )))
28}
29
30/// Helper to read a little-endian u32 from a byte slice at a given offset
31fn read_u32(data: &[u8], offset: usize) -> Result<u32> {
32    data.get(offset..offset + SIZE_U32)
33        .and_then(|slice| slice.try_into().ok())
34        .map(u32::from_le_bytes)
35        .ok_or_else(|| Error::MemoryError(format!(
36            "Failed to read u32 at offset {}", offset
37        )))
38}
39
40/// Calibration data for one sweep of a lighthouse base station
41#[derive(Debug, Clone, Copy, Default, PartialEq)]
42pub struct LighthouseCalibrationSweep {
43    /// Phase offset
44    pub phase: f32,
45    /// Tilt angle
46    pub tilt: f32,
47    /// Curve compensation
48    pub curve: f32,
49    /// Gibbs magnitude
50    pub gibmag: f32,
51    /// Gibbs phase
52    pub gibphase: f32,
53    /// OGEE magnitude
54    pub ogeemag: f32,
55    /// OGEE phase
56    pub ogeephase: f32,
57}
58
59impl LighthouseCalibrationSweep {
60    /// Size in bytes when serialized
61    pub const SIZE: usize = NUM_SWEEP_PARAMS * SIZE_FLOAT;
62
63    /// Parse sweep calibration data from bytes
64    pub fn from_bytes(data: &[u8]) -> Result<Self> {
65        if data.len() < Self::SIZE {
66            return Err(Error::MemoryError(format!(
67                "Insufficient data for calibration sweep: expected {} bytes, got {}",
68                Self::SIZE, data.len()
69            )));
70        }
71
72        Ok(Self {
73            phase: read_f32(data, 0 * SIZE_FLOAT)?,
74            tilt: read_f32(data, 1 * SIZE_FLOAT)?,
75            curve: read_f32(data, 2 * SIZE_FLOAT)?,
76            gibmag: read_f32(data, 3 * SIZE_FLOAT)?,
77            gibphase: read_f32(data, 4 * SIZE_FLOAT)?,
78            ogeemag: read_f32(data, 5 * SIZE_FLOAT)?,
79            ogeephase: read_f32(data, 6 * SIZE_FLOAT)?,
80        })
81    }
82
83    /// Serialize sweep calibration data to bytes
84    #[must_use]
85    pub fn to_bytes(&self) -> Vec<u8> {
86        let mut data = Vec::with_capacity(Self::SIZE);
87        data.extend_from_slice(&self.phase.to_le_bytes());
88        data.extend_from_slice(&self.tilt.to_le_bytes());
89        data.extend_from_slice(&self.curve.to_le_bytes());
90        data.extend_from_slice(&self.gibmag.to_le_bytes());
91        data.extend_from_slice(&self.gibphase.to_le_bytes());
92        data.extend_from_slice(&self.ogeemag.to_le_bytes());
93        data.extend_from_slice(&self.ogeephase.to_le_bytes());
94        data
95    }
96}
97
98/// Calibration data for one lighthouse base station
99#[derive(Debug, Clone, Default, PartialEq)]
100pub struct LighthouseBsCalibration {
101    /// Calibration data for both sweeps
102    pub sweeps: [LighthouseCalibrationSweep; NUM_SWEEPS],
103    /// Unique identifier for this base station
104    pub uid: u32,
105    /// Whether this calibration data is valid
106    pub valid: bool,
107}
108
109impl LighthouseBsCalibration {
110    /// Size in bytes when serialized
111    pub const SIZE: usize = NUM_SWEEPS * LighthouseCalibrationSweep::SIZE + SIZE_U32 + SIZE_BOOL;
112
113    // Offset constants for parsing
114    const SWEEP0_OFFSET: usize = 0;
115    const SWEEP1_OFFSET: usize = LighthouseCalibrationSweep::SIZE;
116    const UID_OFFSET: usize = NUM_SWEEPS * LighthouseCalibrationSweep::SIZE;
117    const VALID_OFFSET: usize = Self::UID_OFFSET + SIZE_U32;
118
119    /// Parse calibration data from bytes
120    pub fn from_bytes(data: &[u8]) -> Result<Self> {
121        if data.len() < Self::SIZE {
122            return Err(Error::MemoryError(format!(
123                "Insufficient data for calibration: expected {} bytes, got {}",
124                Self::SIZE, data.len()
125            )));
126        }
127
128        let sweep0 = LighthouseCalibrationSweep::from_bytes(&data[Self::SWEEP0_OFFSET..Self::SWEEP1_OFFSET])?;
129        let sweep1 = LighthouseCalibrationSweep::from_bytes(&data[Self::SWEEP1_OFFSET..Self::UID_OFFSET])?;
130        let uid = read_u32(data, Self::UID_OFFSET)?;
131        let valid = data.get(Self::VALID_OFFSET).map_or(false, |&b| b != 0);
132
133        Ok(Self {
134            sweeps: [sweep0, sweep1],
135            uid,
136            valid,
137        })
138    }
139
140    /// Serialize calibration data to bytes
141    #[must_use]
142    pub fn to_bytes(&self) -> Vec<u8> {
143        let mut data = Vec::with_capacity(Self::SIZE);
144        data.extend(self.sweeps[0].to_bytes());
145        data.extend(self.sweeps[1].to_bytes());
146        data.extend_from_slice(&self.uid.to_le_bytes());
147        data.push(u8::from(self.valid));
148        data
149    }
150}
151
152/// Geometry data for one lighthouse base station
153#[derive(Debug, Clone, PartialEq)]
154pub struct LighthouseBsGeometry {
155    /// Origin position of the base station [x, y, z] in meters
156    pub origin: [f32; 3],
157    /// Rotation matrix of the base station (3x3)
158    pub rotation_matrix: [[f32; 3]; 3],
159    /// Whether this geometry data is valid
160    pub valid: bool,
161}
162
163impl Default for LighthouseBsGeometry {
164    fn default() -> Self {
165        Self {
166            origin: [0.0, 0.0, 0.0],
167            rotation_matrix: [[0.0; 3]; 3],
168            valid: false,
169        }
170    }
171}
172
173impl LighthouseBsGeometry {
174    /// Size in bytes when serialized (origin vector + 3 rotation vectors + valid flag)
175    pub const SIZE: usize = (1 + NUM_ROTATION_ROWS) * SIZE_VECTOR + SIZE_BOOL;
176
177    // Offset constants for parsing
178    const ORIGIN_OFFSET: usize = 0;
179    const ROTATION_OFFSET: usize = SIZE_VECTOR;
180    const VALID_OFFSET: usize = (1 + NUM_ROTATION_ROWS) * SIZE_VECTOR;
181
182    /// Parse geometry data from bytes
183    pub fn from_bytes(data: &[u8]) -> Result<Self> {
184        if data.len() < Self::SIZE {
185            return Err(Error::MemoryError(format!(
186                "Insufficient data for geometry: expected {} bytes, got {}",
187                Self::SIZE, data.len()
188            )));
189        }
190
191        let read_vector = |offset: usize| -> Result<[f32; 3]> {
192            Ok([
193                read_f32(data, offset)?,
194                read_f32(data, offset + SIZE_FLOAT)?,
195                read_f32(data, offset + 2 * SIZE_FLOAT)?,
196            ])
197        };
198
199        let origin = read_vector(Self::ORIGIN_OFFSET)?;
200        let rotation_matrix = [
201            read_vector(Self::ROTATION_OFFSET)?,
202            read_vector(Self::ROTATION_OFFSET + SIZE_VECTOR)?,
203            read_vector(Self::ROTATION_OFFSET + 2 * SIZE_VECTOR)?,
204        ];
205        let valid = data.get(Self::VALID_OFFSET).map_or(false, |&b| b != 0);
206
207        Ok(Self {
208            origin,
209            rotation_matrix,
210            valid,
211        })
212    }
213
214    /// Serialize geometry data to bytes
215    #[must_use]
216    pub fn to_bytes(&self) -> Vec<u8> {
217        let mut data = Vec::with_capacity(Self::SIZE);
218
219        // Write origin
220        for &v in &self.origin {
221            data.extend_from_slice(&v.to_le_bytes());
222        }
223
224        // Write rotation matrix rows
225        for row in &self.rotation_matrix {
226            for &v in row {
227                data.extend_from_slice(&v.to_le_bytes());
228            }
229        }
230
231        // Write valid flag
232        data.push(u8::from(self.valid));
233
234        data
235    }
236}
237
238/// Memory interface for lighthouse configuration data
239///
240/// This provides methods to read and write lighthouse base station
241/// geometry and calibration data to the Crazyflie.
242#[derive(Debug)]
243pub struct LighthouseMemory {
244    memory: MemoryBackend,
245}
246
247impl LighthouseMemory {
248    /// Start address for geometry data
249    pub const GEO_START_ADDR: usize = 0x00;
250    /// Start address for calibration data
251    pub const CALIB_START_ADDR: usize = 0x1000;
252    /// Size of one page (each base station uses one page)
253    pub const PAGE_SIZE: usize = 0x100;
254    /// Maximum number of base stations supported
255    pub const MAX_BASE_STATIONS: usize = 16;
256
257    /// Validate that a base station ID is within the valid range
258    fn validate_bs_id(bs_id: u8) -> Result<()> {
259        if bs_id as usize >= Self::MAX_BASE_STATIONS {
260            return Err(Error::InvalidArgument(format!(
261                "Base station ID {} out of range (0-{})",
262                bs_id, Self::MAX_BASE_STATIONS - 1
263            )));
264        }
265        Ok(())
266    }
267
268    /// Create a LighthouseMemory from a MemoryBackend, validating the memory type
269    fn from_backend(memory: MemoryBackend) -> Result<Self> {
270        if memory.memory_type == MemoryType::Lighthouse {
271            Ok(Self { memory })
272        } else {
273            Err(Error::MemoryError(format!(
274                "Expected Lighthouse memory type, got {:?}",
275                memory.memory_type
276            )))
277        }
278    }
279
280    /// Read geometry data for a specific base station
281    ///
282    /// # Arguments
283    /// * `bs_id` - Base station ID (0-15)
284    ///
285    /// # Returns
286    /// The geometry data, or an error if the read failed
287    pub async fn read_geometry(&self, bs_id: u8) -> Result<LighthouseBsGeometry> {
288        Self::validate_bs_id(bs_id)?;
289
290        let addr = Self::GEO_START_ADDR + (bs_id as usize) * Self::PAGE_SIZE;
291        let data = self.memory.read::<fn(usize, usize)>(addr, LighthouseBsGeometry::SIZE, None).await?;
292        LighthouseBsGeometry::from_bytes(&data)
293    }
294
295    /// Write geometry data for a specific base station
296    ///
297    /// # Arguments
298    /// * `bs_id` - Base station ID (0-15)
299    /// * `geometry` - The geometry data to write
300    pub async fn write_geometry(&self, bs_id: u8, geometry: &LighthouseBsGeometry) -> Result<()> {
301        Self::validate_bs_id(bs_id)?;
302
303        let addr = Self::GEO_START_ADDR + (bs_id as usize) * Self::PAGE_SIZE;
304        let data = geometry.to_bytes();
305        self.memory.write::<fn(usize, usize)>(addr, &data, None).await
306    }
307
308    /// Read calibration data for a specific base station
309    ///
310    /// # Arguments
311    /// * `bs_id` - Base station ID (0-15)
312    ///
313    /// # Returns
314    /// The calibration data, or an error if the read failed
315    pub async fn read_calibration(&self, bs_id: u8) -> Result<LighthouseBsCalibration> {
316        Self::validate_bs_id(bs_id)?;
317
318        let addr = Self::CALIB_START_ADDR + (bs_id as usize) * Self::PAGE_SIZE;
319        let data = self.memory.read::<fn(usize, usize)>(addr, LighthouseBsCalibration::SIZE, None).await?;
320        LighthouseBsCalibration::from_bytes(&data)
321    }
322
323    /// Write calibration data for a specific base station
324    ///
325    /// # Arguments
326    /// * `bs_id` - Base station ID (0-15)
327    /// * `calibration` - The calibration data to write
328    pub async fn write_calibration(&self, bs_id: u8, calibration: &LighthouseBsCalibration) -> Result<()> {
329        Self::validate_bs_id(bs_id)?;
330
331        let addr = Self::CALIB_START_ADDR + (bs_id as usize) * Self::PAGE_SIZE;
332        let data = calibration.to_bytes();
333        self.memory.write::<fn(usize, usize)>(addr, &data, None).await
334    }
335
336    /// Read all geometry data from the Crazyflie
337    ///
338    /// Attempts to read geometry for all base stations (0-15). Only base stations
339    /// with valid data are included in the result.
340    ///
341    /// # Returns
342    /// A HashMap mapping base station ID to geometry data
343    pub async fn read_all_geometries(&self) -> Result<HashMap<u8, LighthouseBsGeometry>> {
344        self.read_all_geometries_with_progress(|_, _| {}).await
345    }
346
347    /// Read all geometry data with progress reporting
348    ///
349    /// # Arguments
350    /// * `progress_callback` - Called with (completed_count, total_count) after each read
351    pub async fn read_all_geometries_with_progress<F>(&self, mut progress_callback: F) -> Result<HashMap<u8, LighthouseBsGeometry>>
352    where
353        F: FnMut(usize, usize),
354    {
355        let mut result = HashMap::new();
356
357        for bs_id in 0..Self::MAX_BASE_STATIONS as u8 {
358            match self.read_geometry(bs_id).await {
359                Ok(geo) => {
360                    if geo.valid {
361                        result.insert(bs_id, geo);
362                    }
363                }
364                Err(Error::MemoryError(_)) => {
365                    // Base station not supported by firmware, skip it
366                }
367                Err(e) => return Err(e),
368            }
369            progress_callback(bs_id as usize + 1, Self::MAX_BASE_STATIONS);
370        }
371
372        Ok(result)
373    }
374
375    /// Read all calibration data from the Crazyflie
376    ///
377    /// Attempts to read calibration for all base stations (0-15). Only base stations
378    /// with valid data are included in the result.
379    ///
380    /// # Returns
381    /// A HashMap mapping base station ID to calibration data
382    pub async fn read_all_calibrations(&self) -> Result<HashMap<u8, LighthouseBsCalibration>> {
383        self.read_all_calibrations_with_progress(|_, _| {}).await
384    }
385
386    /// Read all calibration data with progress reporting
387    ///
388    /// # Arguments
389    /// * `progress_callback` - Called with (completed_count, total_count) after each read
390    pub async fn read_all_calibrations_with_progress<F>(&self, mut progress_callback: F) -> Result<HashMap<u8, LighthouseBsCalibration>>
391    where
392        F: FnMut(usize, usize),
393    {
394        let mut result = HashMap::new();
395
396        for bs_id in 0..Self::MAX_BASE_STATIONS as u8 {
397            match self.read_calibration(bs_id).await {
398                Ok(calib) => {
399                    if calib.valid {
400                        result.insert(bs_id, calib);
401                    }
402                }
403                Err(Error::MemoryError(_)) => {
404                    // Base station not supported by firmware, skip it
405                }
406                Err(e) => return Err(e),
407            }
408            progress_callback(bs_id as usize + 1, Self::MAX_BASE_STATIONS);
409        }
410
411        Ok(result)
412    }
413
414    /// Write geometry data for multiple base stations
415    ///
416    /// # Arguments
417    /// * `geometries` - A HashMap mapping base station ID to geometry data
418    pub async fn write_geometries(&self, geometries: &HashMap<u8, LighthouseBsGeometry>) -> Result<()> {
419        self.write_geometries_with_progress(geometries, |_, _| {}).await
420    }
421
422    /// Write geometry data for multiple base stations with progress reporting
423    ///
424    /// # Arguments
425    /// * `geometries` - A HashMap mapping base station ID to geometry data
426    /// * `progress_callback` - Called with (completed_count, total_count) after each write
427    pub async fn write_geometries_with_progress<F>(
428        &self,
429        geometries: &HashMap<u8, LighthouseBsGeometry>,
430        mut progress_callback: F,
431    ) -> Result<()>
432    where
433        F: FnMut(usize, usize),
434    {
435        let total = geometries.len();
436        let mut completed = 0;
437
438        for (&bs_id, geometry) in geometries {
439            self.write_geometry(bs_id, geometry).await?;
440            completed += 1;
441            progress_callback(completed, total);
442        }
443
444        Ok(())
445    }
446
447    /// Write calibration data for multiple base stations
448    ///
449    /// # Arguments
450    /// * `calibrations` - A HashMap mapping base station ID to calibration data
451    pub async fn write_calibrations(&self, calibrations: &HashMap<u8, LighthouseBsCalibration>) -> Result<()> {
452        self.write_calibrations_with_progress(calibrations, |_, _| {}).await
453    }
454
455    /// Write calibration data for multiple base stations with progress reporting
456    ///
457    /// # Arguments
458    /// * `calibrations` - A HashMap mapping base station ID to calibration data
459    /// * `progress_callback` - Called with (completed_count, total_count) after each write
460    pub async fn write_calibrations_with_progress<F>(
461        &self,
462        calibrations: &HashMap<u8, LighthouseBsCalibration>,
463        mut progress_callback: F,
464    ) -> Result<()>
465    where
466        F: FnMut(usize, usize),
467    {
468        let total = calibrations.len();
469        let mut completed = 0;
470
471        for (&bs_id, calibration) in calibrations {
472            self.write_calibration(bs_id, calibration).await?;
473            completed += 1;
474            progress_callback(completed, total);
475        }
476
477        Ok(())
478    }
479}
480
481impl FromMemoryBackend for LighthouseMemory {
482    async fn from_memory_backend(memory: MemoryBackend) -> Result<Self> {
483        Self::from_backend(memory)
484    }
485
486    async fn initialize_memory_backend(memory: MemoryBackend) -> Result<Self> {
487        Self::from_backend(memory)
488    }
489
490    fn close_memory(self) -> MemoryBackend {
491        self.memory
492    }
493}
crazyflie_lib/subsystems/memory/lighthouse.rs

crazyflie_lib/subsystems/memory/
lighthouse.rs
