crazyflie_lib/subsystems/memory/
trajectory.rs

1//! Trajectory memory for the Crazyflie high level commander
2//!
3//! This module provides types and functionality for defining and uploading
4//! trajectories to the Crazyflie's trajectory memory. Trajectories can be
5//! either uncompressed (using `Poly4D`) or compressed (using `CompressedStart`
6//! and `CompressedSegment`).
7
8use crate::{Error, Result, subsystems::memory::{MemoryBackend, memory_types}};
9use memory_types::{FromMemoryBackend, MemoryType};
10
11/// Encode a spatial coordinate (meters) to millimeters as i16
12///
13/// Valid range: approximately -32.767 to +32.767 meters
14fn encode_spatial(coordinate: f32) -> Result<i16> {
15    let scaled = coordinate * 1000.0;
16    if scaled < i16::MIN as f32 || scaled > i16::MAX as f32 {
17        return Err(Error::InvalidArgument(
18            format!("Spatial coordinate {:.3}m out of representable range ({:.3}m to {:.3}m)",
19                coordinate, i16::MIN as f32 / 1000.0, i16::MAX as f32 / 1000.0)
20        ));
21    }
22    Ok(scaled as i16)
23}
24
25/// Encode a yaw angle (radians) to 1/10th degrees as i16
26///
27/// Valid range: approximately -573.2 to +573.2 degrees (-10.0 to +10.0 radians)
28fn encode_yaw(angle_rad: f32) -> Result<i16> {
29    let scaled = angle_rad.to_degrees() * 10.0;
30    if scaled < i16::MIN as f32 || scaled > i16::MAX as f32 {
31        return Err(Error::InvalidArgument(
32            format!("Yaw angle {:.3} rad out of representable range", angle_rad)
33        ));
34    }
35    Ok(scaled as i16)
36}
37
38/// A polynomial with up to 8 coefficients
39#[derive(Debug, Clone)]
40pub struct Poly {
41    /// The polynomial coefficients (up to 8)
42    pub values: [f32; 8],
43}
44
45impl Default for Poly {
46    fn default() -> Self {
47        Self { values: [0.0; 8] }
48    }
49}
50
51impl Poly {
52    /// Create a new polynomial with the given coefficients
53    pub fn new(values: [f32; 8]) -> Self {
54        Self { values }
55    }
56
57    /// Create a polynomial from a slice of values
58    ///
59    /// If the slice has fewer than 8 values, the remaining coefficients are set to 0.
60    /// If the slice has more than 8 values, only the first 8 are used.
61    pub fn from_slice(values: &[f32]) -> Self {
62        let mut poly = Self::default();
63        let len = values.len().min(8);
64        poly.values[..len].copy_from_slice(&values[..len]);
65        poly
66    }
67}
68
69/// A 4D polynomial trajectory segment (uncompressed format)
70///
71/// This represents a single segment of a trajectory defined by polynomials
72/// for x, y, z, and yaw coordinates over a duration of time.
73#[derive(Debug, Clone)]
74pub struct Poly4D {
75    /// Duration of this segment in seconds
76    pub duration: f32,
77    /// Polynomial for x coordinate
78    pub x: Poly,
79    /// Polynomial for y coordinate
80    pub y: Poly,
81    /// Polynomial for z coordinate
82    pub z: Poly,
83    /// Polynomial for yaw angle
84    pub yaw: Poly,
85}
86
87impl Poly4D {
88    /// Create a new Poly4D trajectory segment
89    pub fn new(duration: f32, x: Poly, y: Poly, z: Poly, yaw: Poly) -> Self {
90        Self { duration, x, y, z, yaw }
91    }
92
93    /// Pack this segment into bytes for transmission
94    pub fn pack(&self) -> Vec<u8> {
95        let mut data = Vec::with_capacity(132); // 8*4*4 + 4 = 132 bytes
96
97        // Pack x coefficients (8 * f32)
98        for &v in &self.x.values {
99            data.extend_from_slice(&v.to_le_bytes());
100        }
101        // Pack y coefficients (8 * f32)
102        for &v in &self.y.values {
103            data.extend_from_slice(&v.to_le_bytes());
104        }
105        // Pack z coefficients (8 * f32)
106        for &v in &self.z.values {
107            data.extend_from_slice(&v.to_le_bytes());
108        }
109        // Pack yaw coefficients (8 * f32)
110        for &v in &self.yaw.values {
111            data.extend_from_slice(&v.to_le_bytes());
112        }
113        // Pack duration (f32)
114        data.extend_from_slice(&self.duration.to_le_bytes());
115
116        data
117    }
118}
119
120/// Starting point for a compressed trajectory
121///
122/// Compressed trajectories begin with a `CompressedStart` that defines
123/// the initial position and yaw, followed by `CompressedSegment`s.
124#[derive(Debug, Clone)]
125pub struct CompressedStart {
126    /// X coordinate in meters
127    pub x: f32,
128    /// Y coordinate in meters
129    pub y: f32,
130    /// Z coordinate in meters
131    pub z: f32,
132    /// Yaw angle in radians
133    pub yaw: f32,
134}
135
136impl CompressedStart {
137    /// Create a new compressed start point
138    pub fn new(x: f32, y: f32, z: f32, yaw: f32) -> Self {
139        Self { x, y, z, yaw }
140    }
141
142    /// Pack this start point into bytes for transmission
143    ///
144    /// Returns an error if any coordinate is out of the representable range.
145    pub fn pack(&self) -> Result<Vec<u8>> {
146        let mut data = Vec::with_capacity(8);
147
148        data.extend_from_slice(&encode_spatial(self.x)?.to_le_bytes());
149        data.extend_from_slice(&encode_spatial(self.y)?.to_le_bytes());
150        data.extend_from_slice(&encode_spatial(self.z)?.to_le_bytes());
151        data.extend_from_slice(&encode_yaw(self.yaw)?.to_le_bytes());
152
153        Ok(data)
154    }
155}
156
157/// Type of polynomial element in a compressed segment
158#[derive(Debug, Clone, Copy, PartialEq, Eq)]
159enum ElementType {
160    /// No movement (0 coefficients)
161    Constant = 0,
162    /// Linear (1 coefficient)
163    Linear = 1,
164    /// Quadratic (3 coefficients)
165    Quadratic = 2,
166    /// Full (7 coefficients)
167    Full = 3,
168}
169
170impl ElementType {
171    fn from_len(len: usize) -> Option<Self> {
172        match len {
173            0 => Some(ElementType::Constant),
174            1 => Some(ElementType::Linear),
175            3 => Some(ElementType::Quadratic),
176            7 => Some(ElementType::Full),
177            _ => None,
178        }
179    }
180}
181
182/// A segment in a compressed trajectory
183///
184/// Compressed segments use variable-length encoding where each axis
185/// can have 0, 1, 3, or 7 coefficients depending on the complexity
186/// of motion along that axis.
187#[derive(Debug, Clone)]
188pub struct CompressedSegment {
189    duration: f32,
190    x: Vec<f32>,
191    y: Vec<f32>,
192    z: Vec<f32>,
193    yaw: Vec<f32>,
194}
195
196impl CompressedSegment {
197    /// Create a new compressed segment
198    ///
199    /// # Arguments
200    /// * `duration` - Duration of this segment in seconds
201    /// * `x` - X polynomial coefficients (must be 0, 1, 3, or 7 elements)
202    /// * `y` - Y polynomial coefficients (must be 0, 1, 3, or 7 elements)
203    /// * `z` - Z polynomial coefficients (must be 0, 1, 3, or 7 elements)
204    /// * `yaw` - Yaw polynomial coefficients (must be 0, 1, 3, or 7 elements)
205    ///
206    /// # Errors
207    /// Returns an error if any element vector has an invalid length
208    pub fn new(duration: f32, x: Vec<f32>, y: Vec<f32>, z: Vec<f32>, yaw: Vec<f32>) -> Result<Self> {
209        Self::validate(&x)?;
210        Self::validate(&y)?;
211        Self::validate(&z)?;
212        Self::validate(&yaw)?;
213
214        Ok(Self { duration, x, y, z, yaw })
215    }
216
217    fn validate(element: &[f32]) -> Result<()> {
218        let len = element.len();
219        if len != 0 && len != 1 && len != 3 && len != 7 {
220            return Err(Error::InvalidArgument(
221                "Element length must be 0, 1, 3, or 7".to_owned()
222            ));
223        }
224        Ok(())
225    }
226
227    fn encode_type(element: &[f32]) -> u8 {
228        // Safe: fields are validated in new()
229        ElementType::from_len(element.len()).unwrap() as u8
230    }
231
232    fn pack_spatial_element(element: &[f32]) -> Result<Vec<u8>> {
233        let mut data = Vec::new();
234        for &v in element {
235            data.extend_from_slice(&encode_spatial(v)?.to_le_bytes());
236        }
237        Ok(data)
238    }
239
240    fn pack_yaw_element(element: &[f32]) -> Result<Vec<u8>> {
241        let mut data = Vec::new();
242        for &v in element {
243            data.extend_from_slice(&encode_yaw(v)?.to_le_bytes());
244        }
245        Ok(data)
246    }
247
248    /// Pack this segment into bytes for transmission
249    ///
250    /// Returns an error if any coordinate is out of the representable range.
251    pub fn pack(&self) -> Result<Vec<u8>> {
252        let element_types = Self::encode_type(&self.x)
253            | (Self::encode_type(&self.y) << 2)
254            | (Self::encode_type(&self.z) << 4)
255            | (Self::encode_type(&self.yaw) << 6);
256        let duration_ms = (self.duration * 1000.0) as u16;
257
258        let mut data = Vec::new();
259
260        data.push(element_types);
261        data.extend_from_slice(&duration_ms.to_le_bytes());
262        data.extend(Self::pack_spatial_element(&self.x)?);
263        data.extend(Self::pack_spatial_element(&self.y)?);
264        data.extend(Self::pack_spatial_element(&self.z)?);
265        data.extend(Self::pack_yaw_element(&self.yaw)?);
266
267        Ok(data)
268    }
269}
270
271/// Memory interface for trajectories used by the high level commander
272///
273/// Trajectories can be either uncompressed (using `Poly4D` segments) or
274/// compressed (using `CompressedStart` followed by `CompressedSegment`s).
275/// Use `write_uncompressed` for Poly4D trajectories and `write_compressed`
276/// for compressed trajectories.
277#[derive(Debug)]
278pub struct TrajectoryMemory {
279    memory: MemoryBackend,
280}
281
282impl FromMemoryBackend for TrajectoryMemory {
283    async fn from_memory_backend(memory: MemoryBackend) -> Result<Self> {
284        if memory.memory_type == MemoryType::Trajectory {
285            Ok(Self { memory })
286        } else {
287            Err(Error::MemoryError("Wrong type of memory!".to_owned()))
288        }
289    }
290
291    async fn initialize_memory_backend(memory: MemoryBackend) -> Result<Self> {
292        if memory.memory_type == MemoryType::Trajectory {
293            Ok(Self { memory })
294        } else {
295            Err(Error::MemoryError("Wrong type of memory!".to_owned()))
296        }
297    }
298
299    fn close_memory(self) -> MemoryBackend {
300        self.memory
301    }
302}
303
304impl TrajectoryMemory {
305    /// Write an uncompressed trajectory (Poly4D segments) to the Crazyflie
306    ///
307    /// # Arguments
308    /// * `segments` - A slice of Poly4D trajectory segments
309    /// * `start_addr` - The address in trajectory memory to upload to (0 by default)
310    ///
311    /// # Returns
312    /// The number of bytes written
313    pub async fn write_uncompressed(
314        &self,
315        segments: &[Poly4D],
316        start_addr: usize,
317    ) -> Result<usize> {
318        let mut data = Vec::new();
319        for segment in segments {
320            data.extend(segment.pack());
321        }
322
323        self.memory.write::<fn(usize, usize)>(start_addr, &data, None).await?;
324        Ok(data.len())
325    }
326
327    /// Write an uncompressed trajectory with progress reporting
328    ///
329    /// # Arguments
330    /// * `segments` - A slice of Poly4D trajectory segments
331    /// * `start_addr` - The address in trajectory memory to upload to (0 by default)
332    /// * `progress_callback` - Called with (bytes_written, total_bytes)
333    ///
334    /// # Returns
335    /// The number of bytes written
336    pub async fn write_uncompressed_with_progress<F>(
337        &self,
338        segments: &[Poly4D],
339        start_addr: usize,
340        progress_callback: F,
341    ) -> Result<usize>
342    where
343        F: FnMut(usize, usize),
344    {
345        let mut data = Vec::new();
346        for segment in segments {
347            data.extend(segment.pack());
348        }
349
350        self.memory.write(start_addr, &data, Some(progress_callback)).await?;
351        Ok(data.len())
352    }
353
354    /// Write a compressed trajectory to the Crazyflie
355    ///
356    /// Compressed trajectories must start with a `CompressedStart` followed
357    /// by zero or more `CompressedSegment`s.
358    ///
359    /// # Arguments
360    /// * `start` - The starting point of the trajectory
361    /// * `segments` - A slice of compressed trajectory segments
362    /// * `start_addr` - The address in trajectory memory to upload to (0 by default)
363    ///
364    /// # Returns
365    /// The number of bytes written
366    pub async fn write_compressed(
367        &self,
368        start: &CompressedStart,
369        segments: &[CompressedSegment],
370        start_addr: usize,
371    ) -> Result<usize> {
372        let mut data = start.pack()?;
373        for segment in segments {
374            data.extend(segment.pack()?);
375        }
376
377        self.memory.write::<fn(usize, usize)>(start_addr, &data, None).await?;
378        Ok(data.len())
379    }
380
381    /// Write a compressed trajectory with progress reporting
382    ///
383    /// # Arguments
384    /// * `start` - The starting point of the trajectory
385    /// * `segments` - A slice of compressed trajectory segments
386    /// * `start_addr` - The address in trajectory memory to upload to (0 by default)
387    /// * `progress_callback` - Called with (bytes_written, total_bytes)
388    ///
389    /// # Returns
390    /// The number of bytes written
391    pub async fn write_compressed_with_progress<F>(
392        &self,
393        start: &CompressedStart,
394        segments: &[CompressedSegment],
395        start_addr: usize,
396        progress_callback: F,
397    ) -> Result<usize>
398    where
399        F: FnMut(usize, usize),
400    {
401        let mut data = start.pack()?;
402        for segment in segments {
403            data.extend(segment.pack()?);
404        }
405
406        self.memory.write(start_addr, &data, Some(progress_callback)).await?;
407        Ok(data.len())
408    }
409
410}
crazyflie_lib/subsystems/memory/trajectory.rs

crazyflie_lib/subsystems/memory/
trajectory.rs
