sidereon_core/sbas/
format.rs

1use crate::astro::time::gnss::{seconds_of_week_from_calendar, week_from_calendar};
2use crate::astro::time::model::{GnssWeekTow, TimeScale};
3use crate::error::{Error, Result};
4use crate::id::GnssSatelliteId;
5
6use super::message::SbasWireForm;
7use super::store::sbas_prn_to_sat;
8
9#[derive(Clone, Debug, PartialEq)]
10pub struct SbasLogBlock {
11    pub satellite_id: GnssSatelliteId,
12    pub epoch: GnssWeekTow,
13    pub form: SbasWireForm,
14    pub bytes: Vec<u8>,
15}
16
17pub fn parse_ems_lines(text: &str) -> Result<Vec<SbasLogBlock>> {
18    let mut out = Vec::new();
19    for line in text.lines() {
20        if let Some(block) = parse_ems_line(line)? {
21            out.push(block);
22        }
23    }
24    Ok(out)
25}
26
27pub fn parse_rtklib_lines(text: &str) -> Result<Vec<SbasLogBlock>> {
28    let mut out = Vec::new();
29    for line in text.lines() {
30        if let Some(block) = parse_rtklib_line(line)? {
31            out.push(block);
32        }
33    }
34    Ok(out)
35}
36
37fn parse_ems_line(line: &str) -> Result<Option<SbasLogBlock>> {
38    let parts: Vec<&str> = line
39        .split(',')
40        .map(str::trim)
41        .filter(|s| !s.is_empty())
42        .collect();
43    if parts.len() < 8 {
44        return Ok(None);
45    }
46    let Some(hex) = parts.last().copied().filter(|s| looks_hex(s)) else {
47        return Ok(None);
48    };
49    let Some(prn) = parse_u16(parts[0]) else {
50        return Ok(None);
51    };
52    let Some(satellite_id) = sbas_prn_to_sat(prn) else {
53        return Ok(None);
54    };
55    let Some(year) = parse_i64(parts[1]) else {
56        return Ok(None);
57    };
58    let Some(month) = parse_i64(parts[2]) else {
59        return Ok(None);
60    };
61    let Some(day) = parse_i64(parts[3]) else {
62        return Ok(None);
63    };
64    let Some(hour) = parse_i64(parts[4]) else {
65        return Ok(None);
66    };
67    let Some(minute) = parse_i64(parts[5]) else {
68        return Ok(None);
69    };
70    let Some(second) = parse_i64(parts[6]) else {
71        return Ok(None);
72    };
73    let year = if year < 100 { 2000 + year } else { year };
74    let Some(week) = week_from_calendar(TimeScale::Gpst, year, month, day) else {
75        return Ok(None);
76    };
77    let tow_s = seconds_of_week_from_calendar(year, month, day, hour, minute, second);
78    let epoch = GnssWeekTow::new(TimeScale::Gpst, week, tow_s)
79        .map_err(|e| Error::Parse(format!("invalid SBAS EMS epoch: {e}")))?;
80    let (form, bytes) = decode_hex_block(hex)?;
81    Ok(Some(SbasLogBlock {
82        satellite_id,
83        epoch,
84        form,
85        bytes,
86    }))
87}
88
89fn parse_rtklib_line(line: &str) -> Result<Option<SbasLogBlock>> {
90    let Some((head, hex)) = line.split_once(':') else {
91        return Ok(None);
92    };
93    if !looks_hex(hex.trim()) {
94        return Ok(None);
95    }
96    let fields: Vec<&str> = head.split_whitespace().collect();
97    if fields.len() < 4 {
98        return Ok(None);
99    }
100    let Some(week) = parse_u32(fields[0]) else {
101        return Ok(None);
102    };
103    let Some(tow_s) = parse_f64(fields[1]) else {
104        return Ok(None);
105    };
106    let Some(prn) = parse_u16(fields[2]) else {
107        return Ok(None);
108    };
109    let Some(satellite_id) = sbas_prn_to_sat(prn) else {
110        return Ok(None);
111    };
112    let epoch = GnssWeekTow::new(TimeScale::Gpst, week, tow_s)
113        .map_err(|e| Error::Parse(format!("invalid SBAS RTKLIB epoch: {e}")))?;
114    let (_, bytes) = decode_hex_block(hex.trim())?;
115    Ok(Some(SbasLogBlock {
116        satellite_id,
117        epoch,
118        form: SbasWireForm::Body226,
119        bytes,
120    }))
121}
122
123fn decode_hex_block(hex: &str) -> Result<(SbasWireForm, Vec<u8>)> {
124    let mut clean: String = hex.chars().filter(|c| !c.is_whitespace()).collect();
125    if !clean.len().is_multiple_of(2) {
126        clean.push('0');
127    }
128    let mut bytes = Vec::with_capacity(clean.len() / 2);
129    for idx in (0..clean.len()).step_by(2) {
130        let byte = u8::from_str_radix(&clean[idx..idx + 2], 16)
131            .map_err(|e| Error::Parse(format!("invalid SBAS hex block: {e}")))?;
132        bytes.push(byte);
133    }
134    let form = match bytes.len() {
135        32 => SbasWireForm::Framed250,
136        29 => SbasWireForm::Body226,
137        _ => return Err(Error::Parse("invalid SBAS hex block length".to_string())),
138    };
139    Ok((form, bytes))
140}
141
142fn looks_hex(value: &str) -> bool {
143    let trimmed = value.trim();
144    !trimmed.is_empty()
145        && trimmed
146            .chars()
147            .all(|c| c.is_ascii_hexdigit() || c.is_whitespace())
148}
149
150fn parse_u16(value: &str) -> Option<u16> {
151    value.trim().parse().ok()
152}
153
154fn parse_u32(value: &str) -> Option<u32> {
155    value.trim().parse().ok()
156}
157
158fn parse_i64(value: &str) -> Option<i64> {
159    value.trim().parse().ok()
160}
161
162fn parse_f64(value: &str) -> Option<f64> {
163    value.trim().parse().ok()
164}
165
166#[cfg(test)]
167mod tests {
168    use super::*;
169    use crate::sbas::message::{SbasBlock, SbasMessage, SbasPrnMask, SpareBits};
170
171    fn sample_body_hex() -> String {
172        let mut mask = [false; 210];
173        mask[0] = true;
174        let bytes = SbasBlock {
175            form: SbasWireForm::Body226,
176            message: SbasMessage::PrnMask(SbasPrnMask {
177                preamble: 0x53,
178                iodp: 1,
179                mask,
180                reserved: SpareBits::new(),
181            }),
182        }
183        .encode();
184        bytes.iter().map(|b| format!("{b:02X}")).collect()
185    }
186
187    #[test]
188    fn rtklib_lines_parse_body_blocks_and_skip_malformed_lines() {
189        let hex = sample_body_hex();
190        let text = format!("bad line\n2360 259200 120 1 : {hex}\n");
191        let parsed = parse_rtklib_lines(&text).expect("parse RTKLIB lines");
192        assert_eq!(parsed.len(), 1);
193        assert_eq!(parsed[0].satellite_id.to_string(), "S20");
194        assert_eq!(parsed[0].form, SbasWireForm::Body226);
195        assert_eq!(parsed[0].bytes.len(), 29);
196    }
197
198    #[test]
199    fn ems_lines_parse_calendar_epochs() {
200        let hex = sample_body_hex();
201        let text = format!("120,26,7,1,0,0,1,1,{hex}\nnot,enough\n");
202        let parsed = parse_ems_lines(&text).expect("parse EMS lines");
203        assert_eq!(parsed.len(), 1);
204        assert_eq!(parsed[0].satellite_id.to_string(), "S20");
205        assert_eq!(parsed[0].form, SbasWireForm::Body226);
206    }
207}
sidereon_core/sbas/format.rs

sidereon_core/sbas/
format.rs
