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 (form, bytes) = decode_hex_block(hex.trim())?;
115    Ok(Some(SbasLogBlock {
116        satellite_id,
117        epoch,
118        form,
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::astro::time::model::{GnssWeekTow, TimeScale};
170    use crate::sbas::message::{SbasBlock, SbasFastCorrections, SbasMessage, SpareBits};
171
172    // The body bytes are the MT2 capture in tests/sbas_real_vectors.rs. The
173    // Framed250 counterpart is derived once from those 226 body bits with the
174    // public framing convention: CRC-24Q followed by six zero pad bits.
175    const RTKLIB_MT2_BODY: [u8; 29] = [
176        0x53, 0x08, 0xDF, 0xFC, 0x01, 0x00, 0x05, 0xFF, 0xC0, 0x0D, 0xFF, 0xC0, 0x09, 0xFF, 0xDF,
177        0xFC, 0x00, 0x1F, 0xFD, 0xFF, 0xDF, 0xFF, 0xBA, 0xBB, 0xBB, 0xBB, 0x9B, 0xBB, 0x80,
178    ];
179    const RTKLIB_MT2_FRAMED: [u8; 32] = [
180        0x53, 0x08, 0xDF, 0xFC, 0x01, 0x00, 0x05, 0xFF, 0xC0, 0x0D, 0xFF, 0xC0, 0x09, 0xFF, 0xDF,
181        0xFC, 0x00, 0x1F, 0xFD, 0xFF, 0xDF, 0xFF, 0xBA, 0xBB, 0xBB, 0xBB, 0x9B, 0xBB, 0x83, 0xA9,
182        0xCE, 0x00,
183    ];
184
185    fn block_hex(bytes: &[u8]) -> String {
186        bytes.iter().map(|byte| format!("{byte:02X}")).collect()
187    }
188
189    #[test]
190    fn rtklib_lines_parse_public_body_and_framed_blocks() {
191        let expected_message = SbasMessage::FastCorrections(SbasFastCorrections {
192            preamble: 0x53,
193            message_type: 2,
194            iodf: 0,
195            iodp: 3,
196            prc: [
197                2047, 4, 1, 2047, 3, 2047, 2, 2047, 2047, 0, 2047, 2047, 2047,
198            ],
199            udrei: [14, 14, 10, 14, 14, 14, 14, 14, 14, 6, 14, 14, 14],
200            reserved: SpareBits::new(),
201        });
202        let expected_epoch =
203            GnssWeekTow::new(TimeScale::Gpst, 2360, 259_200.0).expect("valid RTKLIB epoch");
204
205        for (form, expected_bytes) in [
206            (SbasWireForm::Body226, RTKLIB_MT2_BODY.as_slice()),
207            (SbasWireForm::Framed250, RTKLIB_MT2_FRAMED.as_slice()),
208        ] {
209            let text = format!(
210                "bad line\n2360 259200 120 1 : {}\n",
211                block_hex(expected_bytes)
212            );
213            let parsed = parse_rtklib_lines(&text).expect("parse RTKLIB lines");
214            assert_eq!(parsed.len(), 1);
215            assert_eq!(parsed[0].satellite_id.to_string(), "S20");
216            assert_eq!(parsed[0].epoch, expected_epoch);
217            assert_eq!(parsed[0].form, form);
218            assert_eq!(parsed[0].bytes.len(), expected_bytes.len());
219            assert_eq!(parsed[0].bytes, expected_bytes);
220
221            let decoded = SbasBlock::decode(&parsed[0].bytes, parsed[0].form)
222                .expect("public SBAS block decoder accepts parsed form");
223            assert_eq!(decoded.form, form);
224            assert_eq!(decoded.message, expected_message);
225        }
226    }
227
228    #[test]
229    fn ems_lines_parse_calendar_epochs() {
230        let hex = block_hex(&RTKLIB_MT2_BODY);
231        let text = format!("120,26,7,1,0,0,1,1,{hex}\nnot,enough\n");
232        let parsed = parse_ems_lines(&text).expect("parse EMS lines");
233        assert_eq!(parsed.len(), 1);
234        assert_eq!(parsed[0].satellite_id.to_string(), "S20");
235        assert_eq!(parsed[0].form, SbasWireForm::Body226);
236    }
237}
sidereon_core/sbas/format.rs

sidereon_core/sbas/
format.rs
