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::ToString;
10use alloc::vec;
11use alloc::vec::Vec;
12use core::fmt;
13use cu_gnss_payloads::{
14 GnssAccuracy, GnssCommandAck, GnssEpochTime, GnssEvent, GnssFixSolution, GnssInfoText,
15 GnssRawUbxFrame, GnssRfStatus, GnssSatelliteState, GnssSatsInView, GnssSignalState,
16};
17#[cfg(feature = "std")]
18use cu_linux_resources::LinuxSerialPort;
19use cu29::bincode::de::{Decode, Decoder};
20use cu29::bincode::enc::{Encode, Encoder};
21use cu29::bincode::error::{DecodeError, EncodeError};
22use cu29::prelude::*;
23use cu29::resource::{Owned, ResourceBindingMap, ResourceBindings, ResourceManager};
24use embedded_io::{ErrorKind, ErrorType, Read, Write};
25
26use crate::protocol::{UbxFrame, decode_frame_to_event, extract_next_ubx_frame};
27
28const CLASS_NAV: u8 = 0x01;
29const ID_NAV_PVT: u8 = 0x07;
30const ID_NAV_SAT: u8 = 0x35;
31const ID_NAV_SIG: u8 = 0x43;
32const CLASS_MON: u8 = 0x0A;
33const ID_MON_RF: u8 = 0x38;
34
35const DEFAULT_READ_BUFFER_BYTES: usize = 4096;
36const DEFAULT_MAX_PENDING_EVENTS: usize = 256;
37
38const DEFAULT_POLL_NAV_PVT_MS: u64 = 200;
39const DEFAULT_POLL_NAV_SAT_MS: u64 = 1000;
40const DEFAULT_POLL_NAV_SIG_MS: u64 = 1000;
41const DEFAULT_POLL_MON_RF_MS: u64 = 2000;
42
43#[derive(Copy, Clone, Debug, Eq, PartialEq)]
44pub enum Binding {
45 Serial,
46}
47
48pub struct UbloxResourcesT<S> {
49 pub serial: Owned<S>,
50}
51
52#[cfg(feature = "std")]
53pub type UbloxResources = UbloxResourcesT<LinuxSerialPort>;
54
55impl<'r, S: 'static + Send + Sync> ResourceBindings<'r> for UbloxResourcesT<S> {
56 type Binding = Binding;
57
58 fn from_bindings(
59 manager: &'r mut ResourceManager,
60 mapping: Option<&ResourceBindingMap<Self::Binding>>,
61 ) -> CuResult<Self> {
62 let mapping = mapping.ok_or_else(|| {
63 CuError::from("UbxSource requires a `serial` resource mapping in copperconfig")
64 })?;
65 let path = mapping.get(Binding::Serial).ok_or_else(|| {
66 CuError::from("UbxSource resources must include `serial: <bundle.resource>`")
67 })?;
68
69 let serial = manager
70 .take::<S>(path.typed())
71 .map_err(|e| e.add_cause("Failed to fetch UBX serial resource"))?;
72
73 Ok(Self { serial })
74 }
75}
76
77#[derive(Reflect)]
78#[reflect(no_field_bounds, from_reflect = false, type_path = false)]
79pub struct UbxSourceTask<S> {
80 #[reflect(ignore)]
81 serial: S,
82 #[reflect(ignore)]
83 read_buffer: Vec<u8>,
84 #[reflect(ignore)]
85 frame_buffer: Vec<u8>,
86 #[reflect(ignore)]
87 pending_events: VecDeque<GnssEvent>,
88 max_pending_events: usize,
89 emit_raw_unknown: bool,
90 poll_nav_pvt_every_ms: u64,
91 poll_nav_sat_every_ms: u64,
92 poll_nav_sig_every_ms: u64,
93 poll_mon_rf_every_ms: u64,
94 #[reflect(ignore)]
95 last_poll_nav_pvt_ns: Option<u64>,
96 #[reflect(ignore)]
97 last_poll_nav_sat_ns: Option<u64>,
98 #[reflect(ignore)]
99 last_poll_nav_sig_ns: Option<u64>,
100 #[reflect(ignore)]
101 last_poll_mon_rf_ns: Option<u64>,
102}
103
104#[cfg(feature = "std")]
105pub type UbxSource = UbxSourceTask<LinuxSerialPort>;
106
107impl<S: 'static> TypePath for UbxSourceTask<S> {
108 fn type_path() -> &'static str {
109 "cu_gnss_ublox::UbxSourceTask"
110 }
111
112 fn short_type_path() -> &'static str {
113 "UbxSourceTask"
114 }
115
116 fn type_ident() -> Option<&'static str> {
117 Some("UbxSourceTask")
118 }
119
120 fn crate_name() -> Option<&'static str> {
121 Some("cu_gnss_ublox")
122 }
123
124 fn module_path() -> Option<&'static str> {
125 Some("cu_gnss_ublox")
126 }
127}
128
129impl<S> fmt::Debug for UbxSourceTask<S> {
130 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
131 f.debug_struct("UbxSource")
132 .field("emit_raw_unknown", &self.emit_raw_unknown)
133 .field("poll_nav_pvt_every_ms", &self.poll_nav_pvt_every_ms)
134 .field("poll_nav_sat_every_ms", &self.poll_nav_sat_every_ms)
135 .field("poll_nav_sig_every_ms", &self.poll_nav_sig_every_ms)
136 .field("poll_mon_rf_every_ms", &self.poll_mon_rf_every_ms)
137 .field("pending_events", &self.pending_events.len())
138 .finish()
139 }
140}
141
142impl<S> Freezable for UbxSourceTask<S> {
143 fn freeze<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
144 Encode::encode(&self.frame_buffer, encoder)?;
145
146 Encode::encode(&(self.pending_events.len() as u64), encoder)?;
147 for event in &self.pending_events {
148 Encode::encode(event, encoder)?;
149 }
150
151 Encode::encode(&self.last_poll_nav_pvt_ns, encoder)?;
152 Encode::encode(&self.last_poll_nav_sat_ns, encoder)?;
153 Encode::encode(&self.last_poll_nav_sig_ns, encoder)?;
154 Encode::encode(&self.last_poll_mon_rf_ns, encoder)?;
155 Ok(())
156 }
157
158 fn thaw<D: Decoder>(&mut self, decoder: &mut D) -> Result<(), DecodeError> {
159 let frame_buffer: Vec<u8> = Decode::decode(decoder)?;
160 if frame_buffer.len() > self.frame_buffer.capacity() {
161 return Err(DecodeError::ArrayLengthMismatch {
162 required: self.frame_buffer.capacity(),
163 found: frame_buffer.len(),
164 });
165 }
166 self.frame_buffer.clear();
167 self.frame_buffer.extend_from_slice(&frame_buffer);
168
169 let pending_len: u64 = Decode::decode(decoder)?;
170 let pending_len = usize::try_from(pending_len).map_err(|_| {
171 DecodeError::OtherString("UBX pending event count overflows usize".to_string())
172 })?;
173 if pending_len > self.max_pending_events {
174 return Err(DecodeError::ArrayLengthMismatch {
175 required: self.max_pending_events,
176 found: pending_len,
177 });
178 }
179 self.pending_events.clear();
180 for _ in 0..pending_len {
181 self.pending_events.push_back(Decode::decode(decoder)?);
182 }
183
184 self.last_poll_nav_pvt_ns = Decode::decode(decoder)?;
185 self.last_poll_nav_sat_ns = Decode::decode(decoder)?;
186 self.last_poll_nav_sig_ns = Decode::decode(decoder)?;
187 self.last_poll_mon_rf_ns = Decode::decode(decoder)?;
188 Ok(())
189 }
190}
191
192impl<S> CuSrcTask for UbxSourceTask<S>
193where
194 S: Read + Write + ErrorType + Send + Sync + 'static,
195 <S as ErrorType>::Error: embedded_io::Error + fmt::Debug + 'static,
196{
197 type Resources<'r> = UbloxResourcesT<S>;
198 type Output<'m> = output_msg!(
199 GnssEpochTime,
200 GnssFixSolution,
201 GnssAccuracy,
202 GnssSatsInView,
203 GnssSatelliteState,
204 GnssSignalState,
205 GnssRfStatus,
206 GnssInfoText,
207 GnssCommandAck,
208 GnssRawUbxFrame
209 );
210
211 fn new(config: Option<&ComponentConfig>, resources: Self::Resources<'_>) -> CuResult<Self>
212 where
213 Self: Sized,
214 {
215 let read_buffer_bytes = config_u32(
216 config,
217 "read_buffer_bytes",
218 DEFAULT_READ_BUFFER_BYTES as u32,
219 )? as usize;
220 let max_pending_events = config_u32(
221 config,
222 "max_pending_events",
223 DEFAULT_MAX_PENDING_EVENTS as u32,
224 )? as usize;
225 let max_pending_events = max_pending_events.max(1);
226
227 let emit_raw_unknown = config_bool(config, "emit_raw_unknown", true)?;
228
229 let poll_nav_pvt_every_ms = config_u64(config, "poll_nav_pvt_ms", DEFAULT_POLL_NAV_PVT_MS)?;
230 let poll_nav_sat_every_ms = config_u64(config, "poll_nav_sat_ms", DEFAULT_POLL_NAV_SAT_MS)?;
231 let poll_nav_sig_every_ms = config_u64(config, "poll_nav_sig_ms", DEFAULT_POLL_NAV_SIG_MS)?;
232 let poll_mon_rf_every_ms = config_u64(config, "poll_mon_rf_ms", DEFAULT_POLL_MON_RF_MS)?;
233
234 Ok(Self {
235 serial: resources.serial.0,
236 read_buffer: vec![0_u8; read_buffer_bytes.max(64)],
237 frame_buffer: Vec::with_capacity(4096),
238 pending_events: VecDeque::with_capacity(max_pending_events),
239 max_pending_events,
240 emit_raw_unknown,
241 poll_nav_pvt_every_ms,
242 poll_nav_sat_every_ms,
243 poll_nav_sig_every_ms,
244 poll_mon_rf_every_ms,
245 last_poll_nav_pvt_ns: None,
246 last_poll_nav_sat_ns: None,
247 last_poll_nav_sig_ns: None,
248 last_poll_mon_rf_ns: None,
249 })
250 }
251
252 fn start(&mut self, _ctx: &CuContext) -> CuResult<()> {
253 self.send_poll(CLASS_NAV, ID_NAV_PVT)?;
254 self.send_poll(CLASS_NAV, ID_NAV_SAT)?;
255 self.send_poll(CLASS_NAV, ID_NAV_SIG)?;
256 self.send_poll(CLASS_MON, ID_MON_RF)?;
257 Ok(())
258 }
259
260 fn process(&mut self, ctx: &CuContext, new_msg: &mut Self::Output<'_>) -> CuResult<()> {
261 let now_ns = ctx.now().as_nanos();
262
263 if Self::is_poll_due(
264 now_ns,
265 self.poll_nav_pvt_every_ms,
266 self.last_poll_nav_pvt_ns,
267 ) {
268 self.send_poll(CLASS_NAV, ID_NAV_PVT)?;
269 self.last_poll_nav_pvt_ns = Some(now_ns);
270 }
271 if Self::is_poll_due(
272 now_ns,
273 self.poll_nav_sat_every_ms,
274 self.last_poll_nav_sat_ns,
275 ) {
276 self.send_poll(CLASS_NAV, ID_NAV_SAT)?;
277 self.last_poll_nav_sat_ns = Some(now_ns);
278 }
279 if Self::is_poll_due(
280 now_ns,
281 self.poll_nav_sig_every_ms,
282 self.last_poll_nav_sig_ns,
283 ) {
284 self.send_poll(CLASS_NAV, ID_NAV_SIG)?;
285 self.last_poll_nav_sig_ns = Some(now_ns);
286 }
287 if Self::is_poll_due(now_ns, self.poll_mon_rf_every_ms, self.last_poll_mon_rf_ns) {
288 self.send_poll(CLASS_MON, ID_MON_RF)?;
289 self.last_poll_mon_rf_ns = Some(now_ns);
290 }
291
292 if let Some(event) = self.pending_events.pop_front() {
293 self.emit_event(ctx, new_msg, event);
294 return Ok(());
295 }
296
297 self.read_and_decode()?;
298
299 if let Some(event) = self.pending_events.pop_front() {
300 self.emit_event(ctx, new_msg, event);
301 }
302
303 Ok(())
304 }
305}
306
307impl<S> UbxSourceTask<S>
308where
309 S: Read + Write + ErrorType + Send + Sync + 'static,
310 <S as ErrorType>::Error: embedded_io::Error + fmt::Debug + 'static,
311{
312 fn send_poll(&mut self, class_id: u8, msg_id: u8) -> CuResult<()> {
313 let frame = UbxFrame::from_message(class_id, msg_id, &[]);
314 let bytes = frame.to_wire_bytes();
315 let mut written = 0;
316 while written < bytes.len() {
317 let n = self
318 .serial
319 .write(&bytes[written..])
320 .map_err(|e| CuError::from(format!("UBX poll write failed: {e:?}")))?;
321 if n == 0 {
322 return Err(CuError::from("UBX poll write failed: zero-byte write"));
323 }
324 written += n;
325 }
326 self.serial
327 .flush()
328 .map_err(|e| CuError::from(format!("UBX poll flush failed: {e:?}")))
329 }
330
331 fn is_poll_due(now_ns: u64, period_ms: u64, last_ns: Option<u64>) -> bool {
332 if period_ms == 0 {
333 return false;
334 }
335
336 let period_ns = period_ms.saturating_mul(1_000_000);
337 match last_ns {
338 Some(last) => now_ns.saturating_sub(last) >= period_ns,
339 None => true,
340 }
341 }
342
343 fn read_and_decode(&mut self) -> CuResult<()> {
344 loop {
345 match self.serial.read(&mut self.read_buffer) {
346 Ok(0) => break,
347 Ok(n) => {
348 self.frame_buffer.extend_from_slice(&self.read_buffer[..n]);
349 self.decode_from_buffer();
350
351 if n < self.read_buffer.len() {
352 break;
353 }
354 }
355 Err(e)
356 if embedded_io::Error::kind(&e) == ErrorKind::TimedOut
357 || embedded_io::Error::kind(&e) == ErrorKind::Interrupted =>
358 {
359 break;
360 }
361 Err(e) => return Err(CuError::from(format!("UBX serial read failed: {e:?}"))),
362 }
363 }
364
365 Ok(())
366 }
367
368 fn decode_from_buffer(&mut self) {
369 while let Some(frame) = extract_next_ubx_frame(&mut self.frame_buffer) {
370 if let Some(event) = decode_frame_to_event(&frame, self.emit_raw_unknown) {
371 self.push_pending_event(event);
372 }
373 }
374 }
375
376 fn push_pending_event(&mut self, event: GnssEvent) {
377 if self.pending_events.len() >= self.max_pending_events {
378 self.pending_events.pop_front();
379 }
380 self.pending_events.push_back(event);
381 }
382
383 fn emit_event(
384 &self,
385 ctx: &CuContext,
386 out: &mut <UbxSourceTask<S> as CuSrcTask>::Output<'_>,
387 event: GnssEvent,
388 ) {
389 let tov = Tov::Time(ctx.now());
390 match event {
391 GnssEvent::None => {}
392 GnssEvent::NavEpoch(nav) => {
393 out.0.tov = tov;
394 out.0.set_payload(nav.time);
395 out.1.tov = tov;
396 out.1.set_payload(nav.fix);
397 out.2.tov = tov;
398 out.2.set_payload(nav.accuracy);
399 }
400 GnssEvent::SatelliteState(sat) => {
401 out.3.tov = tov;
402 out.3.set_payload(GnssSatsInView {
403 itow_ms: sat.itow_ms,
404 count: sat.satellites.len() as u16,
405 });
406 out.4.tov = tov;
407 out.4.set_payload(sat);
408 }
409 GnssEvent::SignalState(sig) => {
410 out.5.tov = tov;
411 out.5.set_payload(sig);
412 }
413 GnssEvent::RfStatus(rf) => {
414 out.6.tov = tov;
415 out.6.set_payload(rf);
416 }
417 GnssEvent::InfoText(info) => {
418 out.7.tov = tov;
419 out.7.set_payload(info);
420 }
421 GnssEvent::CommandAck(ack) => {
422 out.8.tov = tov;
423 out.8.set_payload(ack);
424 }
425 GnssEvent::RawUbx(raw) => {
426 out.9.tov = tov;
427 out.9.set_payload(raw);
428 }
429 }
430 }
431}
432
433fn config_u32(config: Option<&ComponentConfig>, key: &str, default: u32) -> CuResult<u32> {
434 if let Some(cfg) = config {
435 Ok(cfg.get::<u32>(key)?.unwrap_or(default))
436 } else {
437 Ok(default)
438 }
439}
440
441fn config_u64(config: Option<&ComponentConfig>, key: &str, default: u64) -> CuResult<u64> {
442 if let Some(cfg) = config {
443 Ok(cfg.get::<u64>(key)?.unwrap_or(default))
444 } else {
445 Ok(default)
446 }
447}
448
449fn config_bool(config: Option<&ComponentConfig>, key: &str, default: bool) -> CuResult<bool> {
450 if let Some(cfg) = config {
451 Ok(cfg.get::<bool>(key)?.unwrap_or(default))
452 } else {
453 Ok(default)
454 }
455}