crazyflie-lib 0.8.0

Crazyflie quadcopter control lib
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110

//! LED driver memory for the Crazyflie LED ring
//!
//! This module provides types and functionality for controlling the LEDs in
//! the Crazyflie LED ring deck by writing RGB values to the LED driver memory.

use crate::{Error, Result, subsystems::memory::{MemoryBackend, memory_types}};
use memory_types::{FromMemoryBackend, MemoryType};

const NUM_LEDS: usize = 12;
const LED_DATA_SIZE: usize = NUM_LEDS * 2; // RGB565, 2 bytes per LED

/// Represents a single LED with RGB color and intensity
#[derive(Debug, Clone, Copy)]
pub struct Led {
    /// Red component (0-255)
    pub r: u8,
    /// Green component (0-255)
    pub g: u8,
    /// Blue component (0-255)
    pub b: u8,
    /// Intensity percentage (0-100). Values above 100 are clamped to 100 at write time.
    pub intensity: u8,
}

impl Default for Led {
    fn default() -> Self {
        Self {
            r: 0,
            g: 0,
            b: 0,
            intensity: 100,
        }
    }
}

impl Led {
    /// Set the R/G/B and optionally intensity in one call
    pub fn set(&mut self, r: u8, g: u8, b: u8, intensity: Option<u8>) {
        self.r = r;
        self.g = g;
        self.b = b;
        if let Some(i) = intensity {
            self.intensity = i;
        }
    }

    fn to_rgb565(&self) -> u16 {
        let intensity = self.intensity.min(100) as u32;
        let r5 = ((((self.r as u32) * 249 + 1014) >> 11) & 0x1F) * intensity / 100;
        let g6 = ((((self.g as u32) * 253 + 505) >> 10) & 0x3F) * intensity / 100;
        let b5 = ((((self.b as u32) * 249 + 1014) >> 11) & 0x1F) * intensity / 100;
        ((r5 << 11) | (g6 << 5) | b5) as u16
    }
}

/// Memory interface for the Crazyflie LED ring
///
/// Provides methods to control the 12 LEDs in the Crazyflie LED ring by writing
/// RGB values to the LED driver memory. Colors are compressed to RGB565 format
/// with intensity applied before writing.
#[derive(Debug)]
pub struct LedDriverMemory {
    /// The 12 LEDs in the ring
    pub leds: [Led; NUM_LEDS],
    memory: MemoryBackend,
}

impl LedDriverMemory {
    fn from_backend(memory: MemoryBackend) -> Result<Self> {
        if memory.memory_type == MemoryType::DriverLed {
            Ok(Self {
                leds: [Led::default(); NUM_LEDS],
                memory,
            })
        } else {
            Err(Error::MemoryError(format!(
                "Expected DriverLed memory type, got {:?}",
                memory.memory_type
            )))
        }
    }

    /// Write the current LED values to the Crazyflie LED ring
    ///
    /// Converts each LED's RGB values to RGB565 format with intensity applied,
    /// and writes the 24-byte result to address 0x00 of the LED driver memory.
    pub async fn write_leds(&self) -> Result<()> {
        let mut data = Vec::with_capacity(LED_DATA_SIZE);
        for led in &self.leds {
            let rgb565 = led.to_rgb565();
            data.push((rgb565 >> 8) as u8);
            data.push((rgb565 & 0xFF) as u8);
        }
        self.memory.write::<fn(usize, usize)>(0x00, &data, None).await
    }
}

impl FromMemoryBackend for LedDriverMemory {
    async fn from_memory_backend(memory: MemoryBackend) -> Result<Self> {
        Self::from_backend(memory)
    }

    async fn initialize_memory_backend(memory: MemoryBackend) -> Result<Self> {
        Self::from_backend(memory)
    }

    fn close_memory(self) -> MemoryBackend {
        self.memory
    }
}