lego_powered_up/iodevice/
definition.rs

1//! Models the information available about a device from
2//! the AttachedIo, PortInformation and PortModeInformation
3//! message types.
4
5use std::collections::BTreeMap;
6use std::fmt;
7
8use crate::notifications::*;
9use crate::IoTypeId;
10
11type ModeId = u8;
12
13#[derive(Debug, Default, Clone)]
14pub struct Definition {
15    kind: IoTypeId,
16    port: u8,
17    capabilities: Vec<Capability>,
18    mode_count: u8,
19    modes: std::collections::BTreeMap<ModeId, PortMode>,
20    valid_combos: Vec<Vec<u8>>,
21}
22impl fmt::Display for Definition {
23    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
24        write!(
25            f,
26            "{:#?} on port {} ({:#x}) with {} modes: {:#?}",
27            self.kind,
28            self.port,
29            self.port,
30            self.mode_count,
31            self.modes
32                .values()
33                .map(|mode| &mode.name[..])
34                .collect::<Vec<_>>()
35        )
36    }
37}
38
39impl Definition {
40    pub fn kind(&self) -> &IoTypeId {
41        &self.kind
42    }
43    pub fn port(&self) -> u8 {
44        self.port
45    }
46    pub fn capabilities(&self) -> &Vec<Capability> {
47        &self.capabilities
48    }
49    pub fn mode_count(&self) -> &u8 {
50        &self.mode_count
51    }
52    pub fn modes(&self) -> &BTreeMap<ModeId, PortMode> {
53        &self.modes
54    }
55    pub fn valid_combos(&self) -> &Vec<Vec<u8>> {
56        &self.valid_combos
57    }
58
59    pub fn new(kind: IoTypeId, port: u8) -> Self {
60        Self {
61            kind,
62            port,
63            mode_count: Default::default(),
64            capabilities: Default::default(),
65            valid_combos: Default::default(),
66            modes: Default::default(),
67        }
68    }
69    pub fn set_mode_count(&mut self, mode_count: u8) {
70        self.mode_count = mode_count;
71    }
72    pub fn set_modes(&mut self, input_modes: u16, output_modes: u16) {
73        let mut r: BTreeMap<ModeId, PortMode> = BTreeMap::new();
74        for mode in 0..15 {
75            if (input_modes >> mode as u16) & 1 == 1 {
76                r.insert(mode as u8, PortMode::new(ModeKind::Sensor));
77            }
78        }
79        for mode in 0..15 {
80            if (output_modes >> mode as u16) & 1 == 1 {
81                r.insert(mode as u8, PortMode::new(ModeKind::Output));
82            }
83        }
84
85        // Add hidden modes
86        while r.len() < self.mode_count as usize {
87            let mut empty_key = 0;
88            while r.contains_key(&empty_key) {
89                empty_key += 1
90            }
91            r.insert(empty_key, PortMode::new(ModeKind::Hidden));
92        }
93        self.modes = r;
94    }
95    // pub fn get_modes(&self) -> &BTreeMap<ModeId, PortMode> {
96    //     &self.modes
97    // }
98
99    pub fn set_capabilities(&mut self, capabilities: u8) {
100        let mut r: Vec<Capability> = Vec::new();
101        if (capabilities >> 3) & 1 == 1 {
102            r.push(Capability::LogicalSynchronizable)
103        }
104        if (capabilities >> 2) & 1 == 1 {
105            r.push(Capability::LogicalCombinable)
106        }
107        if (capabilities >> 1) & 1 == 1 {
108            r.push(Capability::ProvideData)
109        }
110        if capabilities & 1 == 1 {
111            r.push(Capability::AcceptData)
112        }
113        self.capabilities = r;
114    }
115
116    pub fn set_valid_combos(&mut self, valid: Vec<u8>) {
117        for combo in valid {
118            let mut v: Vec<u8> = Vec::new();
119            for mode in 0..7 {
120                if (combo >> mode as u8) & 1 == 1 {
121                    v.push(mode as u8);
122                }
123            }
124            self.valid_combos.push(v);
125        }
126        self.valid_combos.pop(); // Last one is empty end-marker
127    }
128
129    pub fn set_mode_name(&mut self, mode_id: u8, chars_as_bytes: Vec<u8>) {
130        // let mut truncated = vec![chars_as_bytes.into_iter)]; // iter with closure..?E
131        let mut truncated: Vec<u8> = Vec::new();
132        for c in chars_as_bytes {
133            if c == 0 {
134                break;
135            } else {
136                truncated.push(c)
137            }
138        }
139
140        let name = String::from_utf8(truncated).expect("Found invalid UTF-8");
141        let mode = self.modes.get_mut(&mode_id);
142        match mode {
143            Some(m) => m.name = name,
144            None => {
145                error!(
146                    "Found name without matching mode. Port:{} Mode:{} Name:{}",
147                    self.port, &mode_id, &name
148                );
149                println!(
150                    "Found name without matching mode. Port:{} Mode:{} Name:{}",
151                    self.port, &mode_id, &name
152                );
153
154                // Some devices have modes that  count towards mode_count but are not listed in available modes.
155                // For example the TecnhicLargeLinearMotor has the "hidden" modes CALIB and STATS in addition to
156                // normal modes POWER, SPEED, POS and APOS. The Vision Sensor has a few as well. They might be
157                // useful for something, so we'll get their info as well and list them with ModeKind::Hidden.
158            }
159        }
160    }
161    pub fn set_mode_raw(&mut self, mode_id: u8, min: f32, max: f32) {
162        // let mut mode =
163        self.modes.get_mut(&mode_id).unwrap().raw = (min, max);
164    }
165    pub fn set_mode_pct(&mut self, mode_id: u8, min: f32, max: f32) {
166        // let mut mode =
167        self.modes.get_mut(&mode_id).unwrap().pct = (min, max);
168    }
169    pub fn set_mode_si(&mut self, mode_id: u8, min: f32, max: f32) {
170        // let mut mode =
171        self.modes.get_mut(&mode_id).unwrap().si = (min, max);
172    }
173    pub fn set_mode_symbol(&mut self, mode_id: u8, chars_as_bytes: Vec<u8>) {
174        let mut truncated: Vec<u8> = Vec::new();
175        for c in chars_as_bytes {
176            if c == 0 {
177                break;
178            } else {
179                truncated.push(c)
180            }
181        }
182        let symbol = String::from_utf8(truncated).expect("Found invalid UTF-8");
183        let mode = self.modes.get_mut(&mode_id);
184        match mode {
185            Some(m) => m.symbol = symbol,
186            None => {
187                error!("Found symbol without matching mode. Port:{} Mode:{} Symbol:{}", self.port, &mode_id, &symbol);
188                println!("Found symbol without matching mode. Port:{} Mode:{} Symbol:{}", self.port, &mode_id, &symbol);
189            }
190        }
191    }
192
193    // Input mapping info from docs:
194    // The roles are: The host of the sensor (even a simple and dumb black box)
195    // can then decide, what to do with the sensor without any setup (default
196    // mode 0 (zero). Using the LSB first (highest priority).
197    pub fn set_mode_mapping(
198        &mut self,
199        mode_id: u8,
200        input: MappingValue,
201        output: MappingValue,
202    ) {
203        let mode = self.modes.get_mut(&mode_id).unwrap();
204        let mut r: Vec<Mapping> = Vec::new();
205        if (input.0 >> 7) & 1 == 1 {
206            r.push(Mapping::SupportsNull)
207        }
208        if (input.0 >> 6) & 1 == 1 {
209            r.push(Mapping::SupportsFunctional)
210        }
211        // if (input.0 >> 5) & 1 == 1 {}    // Not used
212        if (input.0 >> 4) & 1 == 1 {
213            r.push(Mapping::Absolute)
214        }
215        if (input.0 >> 3) & 1 == 1 {
216            r.push(Mapping::Relative)
217        }
218        if (input.0 >> 2) & 1 == 1 {
219            r.push(Mapping::Discrete)
220        }
221        // if (input.0 >> 1) & 1 == 1 {}    // Not used
222        // if (input.0 >> 0) & 1 == 1 {}    // Not used
223        mode.input_mapping = r;
224
225        let mut r: Vec<Mapping> = Vec::new();
226        if (output.0 >> 7) & 1 == 1 {
227            r.push(Mapping::SupportsNull)
228        }
229        if (output.0 >> 6) & 1 == 1 {
230            r.push(Mapping::SupportsFunctional)
231        }
232        // if (output.0 >> 5) & 1 == 1 {}   // Not used
233        if (output.0 >> 4) & 1 == 1 {
234            r.push(Mapping::Absolute)
235        }
236        if (output.0 >> 3) & 1 == 1 {
237            r.push(Mapping::Relative)
238        }
239        if (output.0 >> 2) & 1 == 1 {
240            r.push(Mapping::Discrete)
241        }
242        // if (output.0 >> 1) & 1 == 1 {}   // Not used
243        // if (output.0 >> 0) & 1 == 1 {}   // Not used
244        mode.output_mapping = r;
245    }
246    pub fn set_mode_valueformat(
247        &mut self,
248        mode_id: u8,
249        format: ValueFormatType,
250    ) {
251        self.modes.get_mut(&mode_id).unwrap().value_format = format;
252    }
253
254    pub fn set_mode_motor_bias(&mut self, mode_id: u8, bias: u8) {
255        // let mut mode =
256        self.modes.get_mut(&mode_id).unwrap().motor_bias = bias;
257    }
258}
259
260#[derive(Debug, Default, Clone)]
261pub struct PortMode {
262    pub kind: ModeKind,
263    pub name: String,    // Transmitted from hub as [u8; 11]
264    pub raw: (f32, f32), // (min, max) The range for the raw (transmitted) signal, remember other ranges are used for scaling the value.
265    pub pct: (f32, f32), // (min, max) % scaling. Ex: RAW == 0-200 PCT == 0-100 => 100 RAW == 50%
266    pub si: (f32, f32),  // (min, max) SI-unit scaling (probably?)
267    pub symbol: String,  // Transmitted from hub as [u8; 5]
268    pub input_mapping: Vec<Mapping>, // Cf. info below. Can more than 1 mapping be enabled? Yes.
269    pub output_mapping: Vec<Mapping>,
270    pub motor_bias: u8, // 0..100
271    // pub sensor_cabability: [u8; 6],  // Sensor capabilities as bits. No help from docs how to interpret, just ignore it for now.
272    pub value_format: ValueFormatType,
273}
274impl fmt::Display for PortMode {
275    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
276        write!(
277            f,
278            "{:<10?} {:<10} {:<10} {:}",
279            self.kind, self.name, self.symbol, self.value_format
280        )
281    }
282}
283
284impl PortMode {
285    pub fn new(mode_kind: ModeKind) -> Self {
286        Self {
287            kind: mode_kind,
288            name: Default::default(),
289            raw: Default::default(),
290            pct: Default::default(),
291            si: Default::default(),
292            symbol: Default::default(),
293            input_mapping: Default::default(),
294            output_mapping: Default::default(),
295            motor_bias: Default::default(),
296            // sensor_cabability: Default::default(),
297            value_format: Default::default(),
298        }
299    }
300    pub fn name(&self) -> &str {
301        &self.name
302    }
303}
304
305#[derive(Debug, Default, Copy, Clone, PartialEq, Eq)]
306pub enum ModeKind {
307    #[default]
308    Unknown,
309    Sensor,
310    Output,
311    Hidden,
312}
313
314#[derive(Debug, Default, Copy, Clone)]
315pub enum Capability {
316    // Transmitted as u8, upper nibble not used
317    #[default]
318    None,
319    LogicalSynchronizable = 0b1000,
320    LogicalCombinable = 0b0100,
321    ProvideData = 0b0010, // Input (seen from Hub)
322    AcceptData = 0b0001,  // Output (seen from Hub)
323}
324
325#[derive(Debug, Default, Copy, Clone)]
326pub enum Mapping {
327    #[default]
328    Unknown,
329    SupportsNull = 0b1000_0000,
330    SupportsFunctional = 0b0100_0000,
331    // bit 5 not used
332    Absolute = 0b0001_0000, // ABS (Absolute [min..max])
333    Relative = 0b0000_1000, // REL (Relative [-1..1])
334    Discrete = 0b0000_0100, // DIS (Discrete [0, 1, 2, 3])
335                            // bit 1 not used
336                            // bit 0 not used
337}
lego_powered_up/iodevice/definition.rs

lego_powered_up/iodevice/
definition.rs
