cu_sen0682/
lib.rs

1#![cfg_attr(not(feature = "std"), no_std)]
2
3extern crate alloc;
4
5mod protocol;
6
7use alloc::format;
8use core::fmt;
9
10#[cfg(feature = "std")]
11use cu_linux_resources::LinuxSerialPort;
12use cu_sensor_payloads::PointCloudSoa;
13use cu29::bincode::de::{Decode, Decoder};
14use cu29::bincode::enc::{Encode, Encoder};
15use cu29::bincode::error::{DecodeError, EncodeError};
16use cu29::prelude::*;
17use cu29::resource::{Owned, ResourceBindingMap, ResourceBindings, ResourceManager};
18use embedded_io::{ErrorKind, ErrorType, Read, Write};
19
20pub use protocol::{MAX_FRAME_BYTES, MAX_POINTS};
21
22const SERIAL_BUFFER_BYTES: usize = protocol::MAX_FRAME_BYTES * 2;
23const DEFAULT_MIN_RANGE_M: f32 = 0.05;
24const DEFAULT_ROW_ID: u8 = 0;
25const DEFAULT_START_COLUMN: u8 = 1;
26const DEFAULT_END_COLUMN: u8 = 64;
27
28#[derive(Clone, Copy, Debug, PartialEq)]
29pub struct Sen0682ReadoutConfig {
30    pub configure_device: bool,
31    pub row_id: u8,
32    pub start_column: u8,
33    pub end_column: u8,
34}
35
36impl Sen0682ReadoutConfig {
37    fn from_component_config(config: Option<&ComponentConfig>) -> CuResult<Self> {
38        let configure_device = cfg_bool(config, "configure_device", true)?;
39        let row_id = cfg_u8(config, "row_id", DEFAULT_ROW_ID)?;
40        let start_column = cfg_u8(config, "start_column", DEFAULT_START_COLUMN)?;
41        let end_column = cfg_u8(config, "end_column", DEFAULT_END_COLUMN)?;
42
43        if row_id > 8 {
44            return Err(CuError::from(format!(
45                "sen0682 row_id must be between 0 and 8, got {row_id}"
46            )));
47        }
48        if !(1..=64).contains(&start_column) {
49            return Err(CuError::from(format!(
50                "sen0682 start_column must be between 1 and 64, got {start_column}"
51            )));
52        }
53        if !(1..=64).contains(&end_column) {
54            return Err(CuError::from(format!(
55                "sen0682 end_column must be between 1 and 64, got {end_column}"
56            )));
57        }
58        if start_column > end_column {
59            return Err(CuError::from(format!(
60                "sen0682 start_column ({start_column}) must be <= end_column ({end_column})"
61            )));
62        }
63
64        Ok(Self {
65            configure_device,
66            row_id,
67            start_column,
68            end_column,
69        })
70    }
71}
72
73fn cfg_bool(config: Option<&ComponentConfig>, key: &str, default: bool) -> CuResult<bool> {
74    Ok(match config {
75        Some(cfg) => cfg.get::<bool>(key)?.unwrap_or(default),
76        None => default,
77    })
78}
79
80fn cfg_u8(config: Option<&ComponentConfig>, key: &str, default: u8) -> CuResult<u8> {
81    let raw = match config {
82        Some(cfg) => cfg.get::<u64>(key)?.unwrap_or(default as u64),
83        None => default as u64,
84    };
85    u8::try_from(raw).map_err(|_| {
86        CuError::from(format!(
87            "sen0682 config key `{key}` must fit in u8, got {raw}"
88        ))
89    })
90}
91
92fn cfg_f32(config: Option<&ComponentConfig>, key: &str, default: f32) -> CuResult<f32> {
93    let raw = match config {
94        Some(cfg) => cfg.get::<f64>(key)?.unwrap_or(default as f64),
95        None => default as f64,
96    };
97    Ok(raw as f32)
98}
99
100trait FrameTransport {
101    fn start(&mut self, readout: &Sen0682ReadoutConfig) -> CuResult<()>;
102    fn read_frame(&mut self, out: &mut [u8]) -> CuResult<Option<usize>>;
103    fn stop(&mut self) -> CuResult<()> {
104        Ok(())
105    }
106}
107
108struct Sen0682SourceCore<T> {
109    transport: T,
110    // The sensor publishes a small, fixed-size frame, so keeping the scratch
111    // buffers inline is simpler and more deterministic than pulling a pool into
112    // a 15 Hz source.
113    frame_buffer: [u8; protocol::MAX_FRAME_BYTES],
114    min_range_m: f32,
115}
116
117impl<T> Sen0682SourceCore<T>
118where
119    T: FrameTransport,
120{
121    fn new(transport: T, min_range_m: f32) -> Self {
122        Self {
123            transport,
124            frame_buffer: [0u8; protocol::MAX_FRAME_BYTES],
125            min_range_m,
126        }
127    }
128
129    fn start(&mut self, readout: &Sen0682ReadoutConfig) -> CuResult<()> {
130        self.transport.start(readout)
131    }
132
133    fn process(
134        &mut self,
135        ctx: &CuContext,
136        output: &mut CuMsg<PointCloudSoa<MAX_POINTS>>,
137    ) -> CuResult<()> {
138        let Some(frame_len) = self.transport.read_frame(&mut self.frame_buffer)? else {
139            output.metadata.set_status("waiting");
140            output.clear_payload();
141            return Ok(());
142        };
143
144        // Reusing the message-owned storage keeps the hot path from re-zeroing a
145        // 512-point SoA every cycle just because the transport delivered another frame.
146        let payload = output
147            .payload_mut()
148            .get_or_insert_with(PointCloudSoa::<MAX_POINTS>::default);
149
150        let stats = protocol::decode_frame_into(
151            &self.frame_buffer[..frame_len],
152            ctx.now(),
153            self.min_range_m,
154            payload,
155        )
156        .map_err(|err| CuError::from(format!("sen0682 frame decode failed: {err}")))?;
157
158        if payload.len == 0 {
159            output.metadata.set_status("filtered");
160            output.clear_payload();
161            return Ok(());
162        }
163
164        output.metadata.set_status("streaming");
165        output.tov = Tov::Time(ctx.now());
166
167        if stats.frame_idx == 0 {
168            debug!(
169                ctx,
170                "sen0682: first frame width={} height={} points={} device_index={}",
171                stats.width,
172                stats.height,
173                stats.valid_points,
174                stats.device_index
175            );
176        }
177
178        Ok(())
179    }
180
181    fn stop(&mut self) -> CuResult<()> {
182        self.transport.stop()
183    }
184}
185
186struct SerialTransport<S> {
187    serial: S,
188    buffer: [u8; SERIAL_BUFFER_BYTES],
189    buffered: usize,
190    configured_by_driver: bool,
191}
192
193impl<S> SerialTransport<S> {
194    fn new(serial: S) -> Self {
195        Self {
196            serial,
197            buffer: [0u8; SERIAL_BUFFER_BYTES],
198            buffered: 0,
199            configured_by_driver: false,
200        }
201    }
202
203    fn discard_prefix(&mut self, count: usize) {
204        if count >= self.buffered {
205            self.buffered = 0;
206            return;
207        }
208        self.buffer.copy_within(count..self.buffered, 0);
209        self.buffered -= count;
210    }
211
212    fn keep_tail(&mut self, count: usize) {
213        if count >= self.buffered {
214            return;
215        }
216        let start = self.buffered - count;
217        self.buffer.copy_within(start..self.buffered, 0);
218        self.buffered = count;
219    }
220
221    fn try_extract_frame(&mut self, out: &mut [u8]) -> CuResult<Option<usize>> {
222        loop {
223            if self.buffered < protocol::TAG.len() {
224                return Ok(None);
225            }
226
227            let Some(tag_pos) = protocol::find_tag(&self.buffer[..self.buffered]) else {
228                self.keep_tail(self.buffered.min(protocol::TAG.len() - 1));
229                return Ok(None);
230            };
231
232            if tag_pos > 0 {
233                self.discard_prefix(tag_pos);
234            }
235
236            if self.buffered < protocol::HEADER_BYTES {
237                return Ok(None);
238            }
239
240            match protocol::frame_total_bytes_from_prefix(&self.buffer[..protocol::HEADER_BYTES]) {
241                Ok(Some(total_bytes)) => {
242                    if total_bytes > out.len() {
243                        return Err(CuError::from(format!(
244                            "sen0682 frame length {total_bytes} exceeds parser buffer {}",
245                            out.len()
246                        )));
247                    }
248                    if self.buffered < total_bytes {
249                        return Ok(None);
250                    }
251                    out[..total_bytes].copy_from_slice(&self.buffer[..total_bytes]);
252                    self.discard_prefix(total_bytes);
253                    return Ok(Some(total_bytes));
254                }
255                Ok(None) => return Ok(None),
256                Err(_) => {
257                    self.discard_prefix(1);
258                }
259            }
260        }
261    }
262
263    fn freeze_state<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
264        Encode::encode(&(self.buffered as u64), encoder)?;
265        for byte in &self.buffer[..self.buffered] {
266            Encode::encode(byte, encoder)?;
267        }
268        Encode::encode(&self.configured_by_driver, encoder)?;
269        Ok(())
270    }
271
272    fn thaw_state<D: Decoder>(&mut self, decoder: &mut D) -> Result<(), DecodeError> {
273        let buffered: u64 = Decode::decode(decoder)?;
274        let buffered = usize::try_from(buffered).map_err(|_| {
275            DecodeError::OtherString("sen0682 buffered byte count overflows usize".to_string())
276        })?;
277        if buffered > self.buffer.len() {
278            return Err(DecodeError::ArrayLengthMismatch {
279                required: self.buffer.len(),
280                found: buffered,
281            });
282        }
283        self.buffered = buffered;
284        for slot in &mut self.buffer[..self.buffered] {
285            *slot = Decode::decode(decoder)?;
286        }
287        self.configured_by_driver = Decode::decode(decoder)?;
288        Ok(())
289    }
290}
291
292impl<S> SerialTransport<S>
293where
294    S: Read + Write + ErrorType,
295    <S as ErrorType>::Error: embedded_io::Error + fmt::Debug,
296{
297    fn send_command(&mut self, command: &str) -> CuResult<()> {
298        write_all(&mut self.serial, command.as_bytes())?;
299        self.serial
300            .flush()
301            .map_err(|err| CuError::from(format!("sen0682 command flush failed: {err:?}")))?;
302
303        let mut saw_error = false;
304        let mut scratch = [0u8; 256];
305        loop {
306            match self.serial.read(&mut scratch) {
307                Ok(0) => break,
308                Ok(n) => {
309                    saw_error |= contains_ascii_token(&scratch[..n], b"ERROR");
310                }
311                Err(err) if is_idle_io_error(&err) => break,
312                Err(err) => {
313                    return Err(CuError::from(format!(
314                        "sen0682 response read failed after `{}`: {err:?}",
315                        command.trim()
316                    )));
317                }
318            }
319        }
320
321        if saw_error {
322            return Err(CuError::from(format!(
323                "sen0682 rejected command `{}`",
324                command.trim()
325            )));
326        }
327
328        Ok(())
329    }
330
331    fn configure_streaming(&mut self, readout: &Sen0682ReadoutConfig) -> CuResult<()> {
332        if !readout.configure_device {
333            self.configured_by_driver = false;
334            return Ok(());
335        }
336
337        self.send_command("AT+STREAM_CONTROL=0\n")?;
338        self.send_command("AT+STREAM_DATA_TYPE=3\n")?;
339        self.send_command("AT+SPAD_FRAME_MODE=0\n")?;
340        self.send_command(
341            format!(
342                "AT+SPAD_OUTPUT_LINE_DATA={},{},{}\n",
343                readout.row_id, readout.start_column, readout.end_column
344            )
345            .as_str(),
346        )?;
347        // A Copper source should not rewrite bench hardware state as a startup
348        // side effect, so we intentionally stay away from AT+SAVE_CONFIG here.
349        self.send_command("AT+STREAM_CONTROL=1\n")?;
350        self.buffered = 0;
351        self.configured_by_driver = true;
352        Ok(())
353    }
354}
355
356impl<S> FrameTransport for SerialTransport<S>
357where
358    S: Read + Write + ErrorType + Send + Sync + 'static,
359    <S as ErrorType>::Error: embedded_io::Error + fmt::Debug + 'static,
360{
361    fn start(&mut self, readout: &Sen0682ReadoutConfig) -> CuResult<()> {
362        self.configure_streaming(readout)
363    }
364
365    fn read_frame(&mut self, out: &mut [u8]) -> CuResult<Option<usize>> {
366        if let Some(frame) = self.try_extract_frame(out)? {
367            return Ok(Some(frame));
368        }
369
370        if self.buffered == self.buffer.len() {
371            return Err(CuError::from(
372                "sen0682 serial framing buffer saturated before a valid frame was found",
373            ));
374        }
375
376        match self.serial.read(&mut self.buffer[self.buffered..]) {
377            Ok(0) => Ok(None),
378            Ok(n) => {
379                self.buffered += n;
380                self.try_extract_frame(out)
381            }
382            Err(err) if is_idle_io_error(&err) => Ok(None),
383            Err(err) => Err(CuError::from(format!(
384                "sen0682 serial read failed: {err:?}"
385            ))),
386        }
387    }
388
389    fn stop(&mut self) -> CuResult<()> {
390        if self.configured_by_driver {
391            let _ = self.send_command("AT+STREAM_CONTROL=0\n");
392            self.configured_by_driver = false;
393        }
394        Ok(())
395    }
396}
397
398fn write_all<S>(serial: &mut S, bytes: &[u8]) -> CuResult<()>
399where
400    S: Write + ErrorType,
401    <S as ErrorType>::Error: fmt::Debug,
402{
403    let mut written = 0;
404    while written < bytes.len() {
405        let n = serial
406            .write(&bytes[written..])
407            .map_err(|err| CuError::from(format!("sen0682 command write failed: {err:?}")))?;
408        if n == 0 {
409            return Err(CuError::from(
410                "sen0682 command write returned zero bytes before completion",
411            ));
412        }
413        written += n;
414    }
415    Ok(())
416}
417
418fn contains_ascii_token(haystack: &[u8], token: &[u8]) -> bool {
419    haystack.windows(token.len()).any(|window| window == token)
420}
421
422fn is_idle_io_error<E>(err: &E) -> bool
423where
424    E: embedded_io::Error,
425{
426    matches!(err.kind(), ErrorKind::TimedOut | ErrorKind::Interrupted)
427}
428
429/// Address-scoped I2C integration point for board bundles.
430///
431/// The driver expects the resource layer to own addressing and any burst-read
432/// quirks so the task config stays about lidar behavior instead of hardware
433/// wiring details.
434pub trait Sen0682I2cBus: Send + Sync + 'static {
435    type Error: fmt::Debug + Send + 'static;
436
437    fn configure_stream(&mut self, readout: &Sen0682ReadoutConfig) -> Result<(), Self::Error>;
438    fn read_frame(&mut self, out: &mut [u8]) -> Result<Option<usize>, Self::Error>;
439    fn stop_stream(&mut self) -> Result<(), Self::Error> {
440        Ok(())
441    }
442}
443
444struct I2cTransport<B> {
445    bus: B,
446}
447
448impl<B> I2cTransport<B> {
449    fn new(bus: B) -> Self {
450        Self { bus }
451    }
452}
453
454impl<B> FrameTransport for I2cTransport<B>
455where
456    B: Sen0682I2cBus,
457{
458    fn start(&mut self, readout: &Sen0682ReadoutConfig) -> CuResult<()> {
459        self.bus
460            .configure_stream(readout)
461            .map_err(|err| CuError::from(format!("sen0682 i2c configure failed: {err:?}")))
462    }
463
464    fn read_frame(&mut self, out: &mut [u8]) -> CuResult<Option<usize>> {
465        self.bus
466            .read_frame(out)
467            .map_err(|err| CuError::from(format!("sen0682 i2c read failed: {err:?}")))
468    }
469
470    fn stop(&mut self) -> CuResult<()> {
471        self.bus
472            .stop_stream()
473            .map_err(|err| CuError::from(format!("sen0682 i2c stop failed: {err:?}")))
474    }
475}
476
477#[derive(Copy, Clone, Debug, Eq, PartialEq)]
478pub enum SerialBinding {
479    Serial,
480}
481
482pub struct Sen0682SerialResourcesT<S> {
483    pub serial: Owned<S>,
484}
485
486#[cfg(feature = "std")]
487pub type Sen0682SerialResources = Sen0682SerialResourcesT<LinuxSerialPort>;
488
489impl<'r, S: 'static + Send + Sync> ResourceBindings<'r> for Sen0682SerialResourcesT<S> {
490    type Binding = SerialBinding;
491
492    fn from_bindings(
493        manager: &'r mut ResourceManager,
494        mapping: Option<&ResourceBindingMap<Self::Binding>>,
495    ) -> CuResult<Self> {
496        let mapping = mapping.ok_or_else(|| {
497            CuError::from("Sen0682SerialSourceTask requires a `serial` resource mapping")
498        })?;
499        let path = mapping.get(SerialBinding::Serial).ok_or_else(|| {
500            CuError::from(
501                "Sen0682SerialSourceTask resources must include `serial: <bundle.resource>`",
502            )
503        })?;
504        let serial = manager
505            .take::<S>(path.typed())
506            .map_err(|e| e.add_cause("Failed to fetch SEN0682 serial resource"))?;
507        Ok(Self { serial })
508    }
509}
510
511#[derive(Copy, Clone, Debug, Eq, PartialEq)]
512pub enum I2cBinding {
513    I2c,
514}
515
516pub struct Sen0682I2cResourcesT<B> {
517    pub i2c: Owned<B>,
518}
519
520impl<'r, B: 'static + Send + Sync> ResourceBindings<'r> for Sen0682I2cResourcesT<B> {
521    type Binding = I2cBinding;
522
523    fn from_bindings(
524        manager: &'r mut ResourceManager,
525        mapping: Option<&ResourceBindingMap<Self::Binding>>,
526    ) -> CuResult<Self> {
527        let mapping = mapping.ok_or_else(|| {
528            CuError::from("Sen0682I2cSourceTask requires an `i2c` resource mapping")
529        })?;
530        let path = mapping.get(I2cBinding::I2c).ok_or_else(|| {
531            CuError::from("Sen0682I2cSourceTask resources must include `i2c: <bundle.resource>`")
532        })?;
533        let i2c = manager
534            .take::<B>(path.typed())
535            .map_err(|e| e.add_cause("Failed to fetch SEN0682 I2C resource"))?;
536        Ok(Self { i2c })
537    }
538}
539
540#[derive(Reflect)]
541#[reflect(no_field_bounds, from_reflect = false, type_path = false)]
542pub struct Sen0682SerialSourceTask<S> {
543    #[reflect(ignore)]
544    core: Sen0682SourceCore<SerialTransport<S>>,
545    configure_device: bool,
546    row_id: u8,
547    start_column: u8,
548    end_column: u8,
549    min_range_m: f32,
550}
551
552#[cfg(feature = "std")]
553pub type Sen0682SerialSource = Sen0682SerialSourceTask<LinuxSerialPort>;
554
555impl<S: 'static> TypePath for Sen0682SerialSourceTask<S> {
556    fn type_path() -> &'static str {
557        "cu_sen0682::Sen0682SerialSourceTask"
558    }
559
560    fn short_type_path() -> &'static str {
561        "Sen0682SerialSourceTask"
562    }
563
564    fn type_ident() -> Option<&'static str> {
565        Some("Sen0682SerialSourceTask")
566    }
567
568    fn crate_name() -> Option<&'static str> {
569        Some("cu_sen0682")
570    }
571
572    fn module_path() -> Option<&'static str> {
573        Some("cu_sen0682")
574    }
575}
576
577impl<S> fmt::Debug for Sen0682SerialSourceTask<S> {
578    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
579        f.debug_struct("Sen0682SerialSourceTask")
580            .field("configure_device", &self.configure_device)
581            .field("row_id", &self.row_id)
582            .field("start_column", &self.start_column)
583            .field("end_column", &self.end_column)
584            .field("min_range_m", &self.min_range_m)
585            .finish()
586    }
587}
588
589impl<S> Freezable for Sen0682SerialSourceTask<S> {
590    fn freeze<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
591        self.core.transport.freeze_state(encoder)
592    }
593
594    fn thaw<D: Decoder>(&mut self, decoder: &mut D) -> Result<(), DecodeError> {
595        self.core.transport.thaw_state(decoder)
596    }
597}
598
599impl<S> Sen0682SerialSourceTask<S> {
600    fn readout_config(&self) -> Sen0682ReadoutConfig {
601        Sen0682ReadoutConfig {
602            configure_device: self.configure_device,
603            row_id: self.row_id,
604            start_column: self.start_column,
605            end_column: self.end_column,
606        }
607    }
608}
609
610impl<S> CuSrcTask for Sen0682SerialSourceTask<S>
611where
612    S: Read + Write + ErrorType + Send + Sync + 'static,
613    <S as ErrorType>::Error: embedded_io::Error + fmt::Debug + 'static,
614{
615    type Resources<'r> = Sen0682SerialResourcesT<S>;
616    type Output<'m> = output_msg!(PointCloudSoa<MAX_POINTS>);
617
618    fn new(config: Option<&ComponentConfig>, resources: Self::Resources<'_>) -> CuResult<Self>
619    where
620        Self: Sized,
621    {
622        let readout = Sen0682ReadoutConfig::from_component_config(config)?;
623        let min_range_m = cfg_f32(config, "min_range_m", DEFAULT_MIN_RANGE_M)?;
624
625        Ok(Self {
626            core: Sen0682SourceCore::new(SerialTransport::new(resources.serial.0), min_range_m),
627            configure_device: readout.configure_device,
628            row_id: readout.row_id,
629            start_column: readout.start_column,
630            end_column: readout.end_column,
631            min_range_m,
632        })
633    }
634
635    fn start(&mut self, _ctx: &CuContext) -> CuResult<()> {
636        self.core.start(&self.readout_config())
637    }
638
639    fn process(&mut self, ctx: &CuContext, output: &mut Self::Output<'_>) -> CuResult<()> {
640        self.core.process(ctx, output)
641    }
642
643    fn stop(&mut self, _ctx: &CuContext) -> CuResult<()> {
644        self.core.stop()
645    }
646}
647
648#[derive(Reflect)]
649#[reflect(no_field_bounds, from_reflect = false, type_path = false)]
650pub struct Sen0682I2cSourceTask<B> {
651    #[reflect(ignore)]
652    core: Sen0682SourceCore<I2cTransport<B>>,
653    configure_device: bool,
654    row_id: u8,
655    start_column: u8,
656    end_column: u8,
657    min_range_m: f32,
658}
659
660impl<B: 'static> TypePath for Sen0682I2cSourceTask<B> {
661    fn type_path() -> &'static str {
662        "cu_sen0682::Sen0682I2cSourceTask"
663    }
664
665    fn short_type_path() -> &'static str {
666        "Sen0682I2cSourceTask"
667    }
668
669    fn type_ident() -> Option<&'static str> {
670        Some("Sen0682I2cSourceTask")
671    }
672
673    fn crate_name() -> Option<&'static str> {
674        Some("cu_sen0682")
675    }
676
677    fn module_path() -> Option<&'static str> {
678        Some("cu_sen0682")
679    }
680}
681
682impl<B> fmt::Debug for Sen0682I2cSourceTask<B> {
683    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
684        f.debug_struct("Sen0682I2cSourceTask")
685            .field("configure_device", &self.configure_device)
686            .field("row_id", &self.row_id)
687            .field("start_column", &self.start_column)
688            .field("end_column", &self.end_column)
689            .field("min_range_m", &self.min_range_m)
690            .finish()
691    }
692}
693
694impl<B> Freezable for Sen0682I2cSourceTask<B> {}
695
696impl<B> Sen0682I2cSourceTask<B> {
697    fn readout_config(&self) -> Sen0682ReadoutConfig {
698        Sen0682ReadoutConfig {
699            configure_device: self.configure_device,
700            row_id: self.row_id,
701            start_column: self.start_column,
702            end_column: self.end_column,
703        }
704    }
705}
706
707impl<B> CuSrcTask for Sen0682I2cSourceTask<B>
708where
709    B: Sen0682I2cBus,
710{
711    type Resources<'r> = Sen0682I2cResourcesT<B>;
712    type Output<'m> = output_msg!(PointCloudSoa<MAX_POINTS>);
713
714    fn new(config: Option<&ComponentConfig>, resources: Self::Resources<'_>) -> CuResult<Self>
715    where
716        Self: Sized,
717    {
718        let readout = Sen0682ReadoutConfig::from_component_config(config)?;
719        let min_range_m = cfg_f32(config, "min_range_m", DEFAULT_MIN_RANGE_M)?;
720
721        Ok(Self {
722            core: Sen0682SourceCore::new(I2cTransport::new(resources.i2c.0), min_range_m),
723            configure_device: readout.configure_device,
724            row_id: readout.row_id,
725            start_column: readout.start_column,
726            end_column: readout.end_column,
727            min_range_m,
728        })
729    }
730
731    fn start(&mut self, _ctx: &CuContext) -> CuResult<()> {
732        self.core.start(&self.readout_config())
733    }
734
735    fn process(&mut self, ctx: &CuContext, output: &mut Self::Output<'_>) -> CuResult<()> {
736        self.core.process(ctx, output)
737    }
738
739    fn stop(&mut self, _ctx: &CuContext) -> CuResult<()> {
740        self.core.stop()
741    }
742}
743
744#[cfg(test)]
745mod tests {
746    use super::*;
747    use alloc::collections::VecDeque;
748    use alloc::vec::Vec;
749
750    fn build_test_frame() -> Vec<u8> {
751        let mut bytes = Vec::new();
752        bytes.extend_from_slice(b"wyld");
753        bytes.extend_from_slice(&2u16.to_le_bytes());
754        bytes.extend_from_slice(&1u16.to_le_bytes());
755        bytes.extend_from_slice(&16u32.to_le_bytes());
756        bytes.extend_from_slice(&16u16.to_le_bytes());
757        bytes.extend_from_slice(&3u16.to_le_bytes());
758        bytes.extend_from_slice(&7u32.to_le_bytes());
759        bytes.extend_from_slice(&0u32.to_le_bytes());
760        bytes.extend_from_slice(&0u32.to_le_bytes());
761        bytes.extend_from_slice(&0u32.to_le_bytes());
762        bytes.extend_from_slice(&1000i16.to_le_bytes());
763        bytes.extend_from_slice(&0i16.to_le_bytes());
764        bytes.extend_from_slice(&2000i16.to_le_bytes());
765        bytes.extend_from_slice(&123u16.to_le_bytes());
766        bytes.extend_from_slice(&1500i16.to_le_bytes());
767        bytes.extend_from_slice(&0i16.to_le_bytes());
768        bytes.extend_from_slice(&2500i16.to_le_bytes());
769        bytes.extend_from_slice(&456u16.to_le_bytes());
770        bytes
771    }
772
773    #[derive(Default)]
774    struct FakeSerial {
775        reads: VecDeque<Result<Vec<u8>, std::io::Error>>,
776        writes: Vec<Vec<u8>>,
777    }
778
779    impl FakeSerial {
780        fn with_reads(reads: impl IntoIterator<Item = Result<Vec<u8>, std::io::Error>>) -> Self {
781            Self {
782                reads: reads.into_iter().collect(),
783                writes: Vec::new(),
784            }
785        }
786    }
787
788    impl ErrorType for FakeSerial {
789        type Error = std::io::Error;
790    }
791
792    impl Read for FakeSerial {
793        fn read(&mut self, buf: &mut [u8]) -> Result<usize, Self::Error> {
794            let Some(next) = self.reads.pop_front() else {
795                return Err(std::io::Error::from(std::io::ErrorKind::TimedOut));
796            };
797            match next {
798                Ok(chunk) => {
799                    let len = chunk.len().min(buf.len());
800                    buf[..len].copy_from_slice(&chunk[..len]);
801                    Ok(len)
802                }
803                Err(err) => Err(err),
804            }
805        }
806    }
807
808    impl Write for FakeSerial {
809        fn write(&mut self, buf: &[u8]) -> Result<usize, Self::Error> {
810            self.writes.push(buf.to_vec());
811            Ok(buf.len())
812        }
813
814        fn flush(&mut self) -> Result<(), Self::Error> {
815            Ok(())
816        }
817    }
818
819    #[test]
820    fn serial_transport_resynchronizes_after_text_noise() {
821        let frame = build_test_frame();
822        let serial = FakeSerial::with_reads([
823            Ok(b"OK\r\n".to_vec()),
824            Ok(frame[..11].to_vec()),
825            Ok(frame[11..].to_vec()),
826        ]);
827        let mut transport = SerialTransport::new(serial);
828        let mut out = [0u8; protocol::MAX_FRAME_BYTES];
829
830        let mut extracted = None;
831        for _ in 0..3 {
832            extracted = transport.read_frame(&mut out).expect("read should succeed");
833            if extracted.is_some() {
834                break;
835            }
836        }
837
838        assert!(extracted.is_some());
839        assert_eq!(extracted.unwrap(), frame.len());
840        assert_eq!(&out[..frame.len()], frame.as_slice());
841    }
842}
cu_sen0682/lib.rs

cu_sen0682/
lib.rs
