1#![cfg_attr(not(feature = "std"), no_std)]
2
3extern crate alloc;
4
5mod protocol;
6
7use alloc::collections::VecDeque;
8use alloc::format;
9use alloc::string::{String, ToString};
10use alloc::vec::Vec;
11use core::fmt;
12#[cfg(feature = "std")]
13use cu_linux_resources::LinuxSerialPort;
14use cu_sensor_payloads::{PeerRangeObservation, RangePeerId};
15use cu29::bincode::de::{Decode, Decoder};
16use cu29::bincode::enc::{Encode, Encoder};
17use cu29::bincode::error::{DecodeError, EncodeError};
18use cu29::clock::{CuTime, Tov};
19use cu29::prelude::*;
20use cu29::resource::{Owned, ResourceBindingMap, ResourceBindings, ResourceManager};
21use embedded_io::{ErrorKind, ErrorType, Read, Write};
22
23use crate::protocol::{ModemEvent, parse_line};
24
25const DEFAULT_READ_BUFFER_BYTES: usize = 512;
26const DEFAULT_MAX_PENDING_OBSERVATIONS: usize = 32;
27const DEFAULT_RESPONSE_TIMEOUT_MS: u64 = 250;
28const DEFAULT_POLL_PAYLOAD: &str = "PING";
29const DEFAULT_LINE_BUFFER_BYTES: usize = 256;
30const MODEM_ADDRESS_MAX_BYTES: usize = 8;
31const MODEM_PAYLOAD_MAX_BYTES: usize = 12;
32
33#[derive(Copy, Clone, Debug, Eq, PartialEq)]
34pub enum Binding {
35 Serial,
36}
37
38pub struct Ryuw122ResourcesT<S> {
39 pub serial: Owned<S>,
40}
41
42#[cfg(feature = "std")]
43pub type Ryuw122Resources = Ryuw122ResourcesT<LinuxSerialPort>;
44
45impl<'r, S: 'static + Send + Sync> ResourceBindings<'r> for Ryuw122ResourcesT<S> {
46 type Binding = Binding;
47
48 fn from_bindings(
49 manager: &'r mut ResourceManager,
50 mapping: Option<&ResourceBindingMap<Self::Binding>>,
51 ) -> CuResult<Self> {
52 let mapping = mapping.ok_or_else(|| {
53 CuError::from("Ryuw122InitiatorSourceTask requires a `serial` resource mapping")
54 })?;
55 let path = mapping.get(Binding::Serial).ok_or_else(|| {
56 CuError::from(
57 "Ryuw122InitiatorSourceTask resources must include `serial: <bundle.resource>`",
58 )
59 })?;
60
61 let serial = manager
62 .take::<S>(path.typed())
63 .map_err(|e| e.add_cause("Failed to fetch RYUW122 serial resource"))?;
64
65 Ok(Self { serial })
66 }
67}
68
69#[derive(Clone, Copy, Debug, PartialEq, Eq)]
70struct InFlightRequest {
71 anchor_index: usize,
72 sent_at_ns: u64,
73}
74
75#[derive(Clone, Copy, Debug, PartialEq)]
76struct PendingObservation {
77 observed_at: CuTime,
78 observation: PeerRangeObservation,
79}
80
81#[derive(Clone, Copy, Debug, PartialEq, Eq)]
82enum DecodedLine {
83 Observation {
84 anchor_id: RangePeerId,
85 distance_cm: u32,
86 rssi_dbm: Option<i16>,
87 },
88 Error,
89 Ignore,
90}
91
92#[derive(Reflect)]
93#[reflect(no_field_bounds, from_reflect = false, type_path = false)]
94pub struct Ryuw122InitiatorSourceTask<S> {
95 #[reflect(ignore)]
96 serial: S,
97 #[reflect(ignore)]
98 read_buffer: Vec<u8>,
99 #[reflect(ignore)]
100 line_buffer: Vec<u8>,
101 #[reflect(ignore)]
102 pending_observations: VecDeque<PendingObservation>,
103 max_pending_observations: usize,
104 #[reflect(ignore)]
105 anchor_ids: Vec<RangePeerId>,
106 #[reflect(ignore)]
107 anchor_commands: Vec<Vec<u8>>,
108 poll_payload: String,
109 response_timeout_ns: u64,
110 next_anchor_index: usize,
111 #[reflect(ignore)]
112 in_flight: Option<InFlightRequest>,
113}
114
115#[cfg(feature = "std")]
116pub type Ryuw122InitiatorSource = Ryuw122InitiatorSourceTask<LinuxSerialPort>;
117
118impl<S: 'static> TypePath for Ryuw122InitiatorSourceTask<S> {
119 fn type_path() -> &'static str {
120 "cu_ryuw122::Ryuw122InitiatorSourceTask"
121 }
122
123 fn short_type_path() -> &'static str {
124 "Ryuw122InitiatorSourceTask"
125 }
126
127 fn type_ident() -> Option<&'static str> {
128 Some("Ryuw122InitiatorSourceTask")
129 }
130
131 fn crate_name() -> Option<&'static str> {
132 Some("cu_ryuw122")
133 }
134
135 fn module_path() -> Option<&'static str> {
136 Some("cu_ryuw122")
137 }
138}
139
140impl<S> fmt::Debug for Ryuw122InitiatorSourceTask<S> {
141 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
142 f.debug_struct("Ryuw122InitiatorSourceTask")
143 .field("anchor_count", &self.anchor_ids.len())
144 .field("poll_payload", &self.poll_payload)
145 .field("response_timeout_ns", &self.response_timeout_ns)
146 .field("max_pending_observations", &self.max_pending_observations)
147 .field("next_anchor_index", &self.next_anchor_index)
148 .field("in_flight", &self.in_flight)
149 .finish()
150 }
151}
152
153impl<S> Freezable for Ryuw122InitiatorSourceTask<S> {
154 fn freeze<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
155 Encode::encode(&self.line_buffer, encoder)?;
156
157 Encode::encode(&(self.pending_observations.len() as u64), encoder)?;
158 for pending in &self.pending_observations {
159 Encode::encode(&pending.observed_at, encoder)?;
160 Encode::encode(&pending.observation, encoder)?;
161 }
162
163 Encode::encode(&(self.next_anchor_index as u64), encoder)?;
164 let in_flight = self
165 .in_flight
166 .map(|request| (request.anchor_index as u64, request.sent_at_ns));
167 Encode::encode(&in_flight, encoder)?;
168 Ok(())
169 }
170
171 fn thaw<D: Decoder>(&mut self, decoder: &mut D) -> Result<(), DecodeError> {
172 let line_buffer: Vec<u8> = Decode::decode(decoder)?;
173 if line_buffer.len() > self.line_buffer.capacity() {
174 return Err(DecodeError::ArrayLengthMismatch {
175 required: self.line_buffer.capacity(),
176 found: line_buffer.len(),
177 });
178 }
179 self.line_buffer.clear();
180 self.line_buffer.extend_from_slice(&line_buffer);
181
182 let pending_len: u64 = Decode::decode(decoder)?;
183 let pending_len = usize::try_from(pending_len).map_err(|_| {
184 DecodeError::OtherString(
185 "RYUW122 pending observation count overflows usize".to_string(),
186 )
187 })?;
188 if pending_len > self.max_pending_observations {
189 return Err(DecodeError::ArrayLengthMismatch {
190 required: self.max_pending_observations,
191 found: pending_len,
192 });
193 }
194 self.pending_observations.clear();
195 for _ in 0..pending_len {
196 self.pending_observations.push_back(PendingObservation {
197 observed_at: Decode::decode(decoder)?,
198 observation: Decode::decode(decoder)?,
199 });
200 }
201
202 let next_anchor_index: u64 = Decode::decode(decoder)?;
203 self.next_anchor_index = usize::try_from(next_anchor_index).map_err(|_| {
204 DecodeError::OtherString("RYUW122 next anchor index overflows usize".to_string())
205 })?;
206 if !self.anchor_ids.is_empty() && self.next_anchor_index >= self.anchor_ids.len() {
207 return Err(DecodeError::OtherString(
208 "RYUW122 keyframe next anchor index is out of range".to_string(),
209 ));
210 }
211
212 let in_flight: Option<(u64, u64)> = Decode::decode(decoder)?;
213 self.in_flight = in_flight
214 .map(|(anchor_index, sent_at_ns)| {
215 let anchor_index = usize::try_from(anchor_index).map_err(|_| {
216 DecodeError::OtherString(
217 "RYUW122 in-flight anchor index overflows usize".to_string(),
218 )
219 })?;
220 if anchor_index >= self.anchor_ids.len() {
221 return Err(DecodeError::OtherString(
222 "RYUW122 keyframe in-flight anchor index is out of range".to_string(),
223 ));
224 }
225 Ok(InFlightRequest {
226 anchor_index,
227 sent_at_ns,
228 })
229 })
230 .transpose()?;
231 Ok(())
232 }
233}
234
235impl<S> CuSrcTask for Ryuw122InitiatorSourceTask<S>
236where
237 S: Read + Write + ErrorType + Send + Sync + 'static,
238 <S as ErrorType>::Error: embedded_io::Error + fmt::Debug + 'static,
239{
240 type Resources<'r> = Ryuw122ResourcesT<S>;
241 type Output<'m> = output_msg!(PeerRangeObservation);
242
243 fn new(config: Option<&ComponentConfig>, resources: Self::Resources<'_>) -> CuResult<Self>
244 where
245 Self: Sized,
246 {
247 let anchor_ids = config_anchor_ids(config)?;
248 let poll_payload = config_string(config, "poll_payload", DEFAULT_POLL_PAYLOAD)?;
249 validate_poll_payload(&poll_payload)?;
250
251 let read_buffer_bytes = config_u32(
252 config,
253 "read_buffer_bytes",
254 DEFAULT_READ_BUFFER_BYTES as u32,
255 )? as usize;
256 let max_pending_observations = config_u32(
257 config,
258 "max_pending_observations",
259 DEFAULT_MAX_PENDING_OBSERVATIONS as u32,
260 )? as usize;
261 let response_timeout_ms =
262 config_u64(config, "response_timeout_ms", DEFAULT_RESPONSE_TIMEOUT_MS)?;
263
264 Ok(Self {
265 serial: resources.serial.0,
266 read_buffer: alloc::vec![0_u8; read_buffer_bytes.max(64)],
267 line_buffer: Vec::with_capacity(read_buffer_bytes.max(DEFAULT_LINE_BUFFER_BYTES)),
268 pending_observations: VecDeque::with_capacity(max_pending_observations.max(1)),
269 max_pending_observations: max_pending_observations.max(1),
270 anchor_commands: build_anchor_commands(&anchor_ids, &poll_payload),
271 anchor_ids,
272 poll_payload,
273 response_timeout_ns: response_timeout_ms.max(1).saturating_mul(1_000_000),
274 next_anchor_index: 0,
275 in_flight: None,
276 })
277 }
278
279 fn start(&mut self, ctx: &CuContext) -> CuResult<()> {
280 self.line_buffer.clear();
281 self.pending_observations.clear();
282 self.next_anchor_index = 0;
283 self.in_flight = None;
284 self.drive_request_cycle(ctx.now().as_nanos())
285 }
286
287 fn process(&mut self, ctx: &CuContext, output: &mut Self::Output<'_>) -> CuResult<()> {
288 let observed_at = ctx.now();
289 self.read_and_decode(observed_at)?;
290 self.drive_request_cycle(observed_at.as_nanos())?;
291
292 if let Some(pending) = self.pending_observations.pop_front() {
293 output.tov = Tov::Time(pending.observed_at);
294 output.set_payload(pending.observation);
295 }
296
297 Ok(())
298 }
299
300 fn stop(&mut self, _ctx: &CuContext) -> CuResult<()> {
301 self.in_flight = None;
302 self.pending_observations.clear();
303 self.line_buffer.clear();
304 Ok(())
305 }
306}
307
308impl<S> Ryuw122InitiatorSourceTask<S>
309where
310 S: Read + Write + ErrorType + Send + Sync + 'static,
311 <S as ErrorType>::Error: embedded_io::Error + fmt::Debug + 'static,
312{
313 fn drive_request_cycle(&mut self, now_ns: u64) -> CuResult<()> {
314 if self.anchor_ids.is_empty() {
315 return Ok(());
316 }
317
318 if let Some(in_flight) = self.in_flight {
319 if now_ns.saturating_sub(in_flight.sent_at_ns) < self.response_timeout_ns {
320 return Ok(());
321 }
322 self.next_anchor_index = (in_flight.anchor_index + 1) % self.anchor_ids.len();
323 self.in_flight = None;
324 }
325
326 self.send_request(self.next_anchor_index, now_ns)
327 }
328
329 fn send_request(&mut self, anchor_index: usize, now_ns: u64) -> CuResult<()> {
330 let serial = &mut self.serial;
331 let command = &self.anchor_commands[anchor_index];
332 write_all(serial, command)?;
333 serial
334 .flush()
335 .map_err(|e| CuError::from(format!("RYUW122 flush failed: {e:?}")))?;
336 self.in_flight = Some(InFlightRequest {
337 anchor_index,
338 sent_at_ns: now_ns,
339 });
340 Ok(())
341 }
342
343 fn read_and_decode(&mut self, observed_at: CuTime) -> CuResult<()> {
344 loop {
345 match self.serial.read(&mut self.read_buffer) {
346 Ok(0) => break,
347 Ok(n) => {
348 let (line_buffer, read_buffer) = (&mut self.line_buffer, &self.read_buffer);
349 append_read_bytes(line_buffer, &read_buffer[..n]);
350 self.decode_from_buffer(observed_at);
351
352 if n < self.read_buffer.len() {
353 break;
354 }
355 }
356 Err(e)
357 if matches!(
358 embedded_io::Error::kind(&e),
359 ErrorKind::TimedOut | ErrorKind::Interrupted
360 ) =>
361 {
362 break;
363 }
364 Err(e) => return Err(CuError::from(format!("RYUW122 serial read failed: {e:?}"))),
365 }
366 }
367
368 Ok(())
369 }
370
371 fn decode_from_buffer(&mut self, observed_at: CuTime) {
372 while let Some(newline_idx) = self.line_buffer.iter().position(|byte| *byte == b'\n') {
373 let decoded = {
374 let line_bytes = &self.line_buffer[..=newline_idx];
375 let line = match core::str::from_utf8(line_bytes) {
376 Ok(line) => line,
377 Err(_) => {
378 self.line_buffer.drain(..=newline_idx);
379 continue;
380 }
381 };
382
383 match parse_line(line) {
384 Ok(ModemEvent::RangeResponse(event)) => {
385 match RangePeerId::try_from(event.peer_id) {
386 Ok(anchor_id) => DecodedLine::Observation {
387 anchor_id,
388 distance_cm: event.distance_cm,
389 rssi_dbm: event.rssi_dbm,
390 },
391 Err(_) => DecodedLine::Ignore,
392 }
393 }
394 Ok(ModemEvent::Error) => DecodedLine::Error,
395 Ok(_) | Err(_) => DecodedLine::Ignore,
396 }
397 };
398
399 self.line_buffer.drain(..=newline_idx);
400
401 match decoded {
402 DecodedLine::Observation {
403 anchor_id,
404 distance_cm,
405 rssi_dbm,
406 } => self.handle_range_response(observed_at, anchor_id, distance_cm, rssi_dbm),
407 DecodedLine::Error => {
408 if let Some(in_flight) = self.in_flight.take() {
409 self.next_anchor_index =
410 (in_flight.anchor_index + 1) % self.anchor_ids.len();
411 }
412 }
413 DecodedLine::Ignore => {}
414 }
415 }
416 }
417
418 fn handle_range_response(
419 &mut self,
420 observed_at: CuTime,
421 anchor_id: RangePeerId,
422 distance_cm: u32,
423 rssi_dbm: Option<i16>,
424 ) {
425 self.push_pending_observation(PendingObservation {
426 observed_at,
427 observation: PeerRangeObservation::from_centimeters(anchor_id, distance_cm, rssi_dbm),
428 });
429
430 if let Some(in_flight) = self.in_flight
431 && self.anchor_ids[in_flight.anchor_index] == anchor_id
432 {
433 self.next_anchor_index = (in_flight.anchor_index + 1) % self.anchor_ids.len();
434 self.in_flight = None;
435 }
436 }
437
438 fn push_pending_observation(&mut self, observation: PendingObservation) {
439 if self.pending_observations.len() >= self.max_pending_observations {
440 self.pending_observations.pop_front();
441 }
442 self.pending_observations.push_back(observation);
443 }
444}
445
446fn config_u32(config: Option<&ComponentConfig>, key: &str, default: u32) -> CuResult<u32> {
447 if let Some(cfg) = config {
448 Ok(cfg.get::<u32>(key)?.unwrap_or(default))
449 } else {
450 Ok(default)
451 }
452}
453
454fn config_u64(config: Option<&ComponentConfig>, key: &str, default: u64) -> CuResult<u64> {
455 if let Some(cfg) = config {
456 Ok(cfg.get::<u64>(key)?.unwrap_or(default))
457 } else {
458 Ok(default)
459 }
460}
461
462fn config_string(config: Option<&ComponentConfig>, key: &str, default: &str) -> CuResult<String> {
463 if let Some(cfg) = config {
464 Ok(cfg
465 .get::<String>(key)?
466 .unwrap_or_else(|| default.to_string()))
467 } else {
468 Ok(default.to_string())
469 }
470}
471
472fn config_anchor_ids(config: Option<&ComponentConfig>) -> CuResult<Vec<RangePeerId>> {
473 let config = config.ok_or_else(|| {
474 CuError::from("Ryuw122InitiatorSourceTask requires a config with non-empty `anchor_ids`")
475 })?;
476
477 let raw_anchor_ids: Vec<String> = config.get_value("anchor_ids")?.ok_or_else(|| {
478 CuError::from("Ryuw122InitiatorSourceTask config must include `anchor_ids`")
479 })?;
480
481 if raw_anchor_ids.is_empty() {
482 return Err(CuError::from(
483 "Ryuw122InitiatorSourceTask config must include at least one anchor id",
484 ));
485 }
486
487 raw_anchor_ids
488 .into_iter()
489 .map(|anchor_id| {
490 validate_anchor_id(&anchor_id)?;
491 RangePeerId::try_from(anchor_id.as_str()).map_err(|err| {
492 CuError::from(format!("anchor id `{anchor_id}` is not valid: {err}"))
493 })
494 })
495 .collect()
496}
497
498fn build_anchor_commands(anchor_ids: &[RangePeerId], payload: &str) -> Vec<Vec<u8>> {
499 let payload_len = payload.len();
500 anchor_ids
501 .iter()
502 .map(|anchor_id| {
503 format!(
504 "AT+ANCHOR_SEND={},{},{}\r\n",
505 anchor_id.as_str(),
506 payload_len,
507 payload
508 )
509 .into_bytes()
510 })
511 .collect()
512}
513
514fn append_read_bytes(line_buffer: &mut Vec<u8>, bytes: &[u8]) {
515 if line_buffer.len() + bytes.len() > line_buffer.capacity() {
516 line_buffer.clear();
517 }
518
519 debug_assert!(bytes.len() <= line_buffer.capacity());
520 line_buffer.extend_from_slice(bytes);
521}
522
523fn write_all<S>(serial: &mut S, bytes: &[u8]) -> CuResult<()>
524where
525 S: Write + ErrorType,
526 <S as ErrorType>::Error: embedded_io::Error + fmt::Debug + 'static,
527{
528 let mut written = 0;
529 while written < bytes.len() {
530 let n = serial
531 .write(&bytes[written..])
532 .map_err(|e| CuError::from(format!("RYUW122 write failed: {e:?}")))?;
533 if n == 0 {
534 return Err(CuError::from("RYUW122 write failed: zero-byte write"));
535 }
536 written += n;
537 }
538 Ok(())
539}
540
541fn validate_anchor_id(anchor_id: &str) -> CuResult<()> {
542 if anchor_id.is_empty() {
543 return Err(CuError::from("RYUW122 anchor ids must not be empty"));
544 }
545 if !anchor_id.is_ascii() {
546 return Err(CuError::from("RYUW122 anchor ids must be ASCII"));
547 }
548 if anchor_id.len() > MODEM_ADDRESS_MAX_BYTES {
549 return Err(CuError::from(format!(
550 "RYUW122 anchor id `{anchor_id}` exceeds {MODEM_ADDRESS_MAX_BYTES} bytes"
551 )));
552 }
553 Ok(())
554}
555
556fn validate_poll_payload(payload: &str) -> CuResult<()> {
557 if !payload.is_ascii() {
558 return Err(CuError::from("RYUW122 poll payload must be ASCII"));
559 }
560 if payload.len() > MODEM_PAYLOAD_MAX_BYTES {
561 return Err(CuError::from(format!(
562 "RYUW122 poll payload exceeds {MODEM_PAYLOAD_MAX_BYTES} bytes"
563 )));
564 }
565 Ok(())
566}
567
568#[cfg(test)]
569mod tests {
570 use super::*;
571 use alloc::collections::VecDeque;
572 use cu29::clock::CuDuration;
573
574 #[derive(Default)]
575 struct FakeSerial {
576 reads: VecDeque<Result<Vec<u8>, std::io::Error>>,
577 writes: Vec<Vec<u8>>,
578 }
579
580 impl FakeSerial {
581 fn with_reads(reads: impl IntoIterator<Item = Result<Vec<u8>, std::io::Error>>) -> Self {
582 Self {
583 reads: reads.into_iter().collect(),
584 writes: Vec::new(),
585 }
586 }
587
588 fn writes_as_strings(&self) -> Vec<String> {
589 self.writes
590 .iter()
591 .map(|bytes| String::from_utf8(bytes.clone()).unwrap())
592 .collect()
593 }
594 }
595
596 impl ErrorType for FakeSerial {
597 type Error = std::io::Error;
598 }
599
600 impl Read for FakeSerial {
601 fn read(&mut self, buf: &mut [u8]) -> Result<usize, Self::Error> {
602 let Some(next) = self.reads.pop_front() else {
603 return Err(std::io::Error::from(std::io::ErrorKind::TimedOut));
604 };
605 match next {
606 Ok(chunk) => {
607 let len = chunk.len().min(buf.len());
608 buf[..len].copy_from_slice(&chunk[..len]);
609 Ok(len)
610 }
611 Err(err) => Err(err),
612 }
613 }
614 }
615
616 impl Write for FakeSerial {
617 fn write(&mut self, buf: &[u8]) -> Result<usize, Self::Error> {
618 self.writes.push(buf.to_vec());
619 Ok(buf.len())
620 }
621
622 fn flush(&mut self) -> Result<(), Self::Error> {
623 Ok(())
624 }
625 }
626
627 fn build_task(serial: FakeSerial) -> Ryuw122InitiatorSourceTask<FakeSerial> {
628 Ryuw122InitiatorSourceTask {
629 serial,
630 read_buffer: alloc::vec![0_u8; 64],
631 line_buffer: Vec::with_capacity(64),
632 pending_observations: VecDeque::new(),
633 max_pending_observations: 8,
634 anchor_ids: alloc::vec![
635 RangePeerId::new("ANCH0001").unwrap(),
636 RangePeerId::new("ANCH0002").unwrap(),
637 ],
638 anchor_commands: alloc::vec![
639 b"AT+ANCHOR_SEND=ANCH0001,4,PING\r\n".to_vec(),
640 b"AT+ANCHOR_SEND=ANCH0002,4,PING\r\n".to_vec(),
641 ],
642 poll_payload: "PING".to_string(),
643 response_timeout_ns: 50_000_000,
644 next_anchor_index: 0,
645 in_flight: None,
646 }
647 }
648
649 #[test]
650 fn start_sends_first_request_immediately() {
651 let mut task = build_task(FakeSerial::default());
652 let (ctx, _mock) = CuContext::new_mock_clock();
653
654 task.start(&ctx).unwrap();
655
656 assert_eq!(
657 task.serial.writes_as_strings(),
658 alloc::vec!["AT+ANCHOR_SEND=ANCH0001,4,PING\r\n".to_string()]
659 );
660 }
661
662 #[test]
663 fn advances_to_next_anchor_after_timeout() {
664 let mut task = build_task(FakeSerial::default());
665 let (ctx, mock) = CuContext::new_mock_clock();
666
667 task.start(&ctx).unwrap();
668 mock.increment(CuDuration::from_millis(60));
669
670 let mut output = <Ryuw122InitiatorSourceTask<FakeSerial> as CuSrcTask>::Output::default();
671 task.process(&ctx, &mut output).unwrap();
672
673 assert_eq!(
674 task.serial.writes_as_strings(),
675 alloc::vec![
676 "AT+ANCHOR_SEND=ANCH0001,4,PING\r\n".to_string(),
677 "AT+ANCHOR_SEND=ANCH0002,4,PING\r\n".to_string()
678 ]
679 );
680 }
681
682 #[test]
683 fn response_emits_observation_and_immediately_sends_next_request() {
684 let response = b"+ANCHOR_RCV=ANCH0001,5,HELLO,40 cm,-71 dBm\r\n".to_vec();
685 let mut task = build_task(FakeSerial::with_reads([Ok(response)]));
686 let (ctx, _mock) = CuContext::new_mock_clock();
687
688 task.start(&ctx).unwrap();
689 let mut output = <Ryuw122InitiatorSourceTask<FakeSerial> as CuSrcTask>::Output::default();
690 task.process(&ctx, &mut output).unwrap();
691
692 let payload = output.payload().expect("observation payload");
693 assert_eq!(payload.peer_id.as_str(), "ANCH0001");
694 assert_eq!(
695 payload.distance.get::<cu29::units::si::length::meter>(),
696 0.4
697 );
698 assert_eq!(payload.rssi_dbm, Some(-71));
699 assert_eq!(
700 task.serial.writes_as_strings(),
701 alloc::vec![
702 "AT+ANCHOR_SEND=ANCH0001,4,PING\r\n".to_string(),
703 "AT+ANCHOR_SEND=ANCH0002,4,PING\r\n".to_string()
704 ]
705 );
706 }
707}