1use std::sync::Arc;
2
3use crate::constants::C_M_S;
4use crate::id::{GnssSatelliteId, GnssSystem};
5use crate::observables::{ObservableEphemerisSource, ObservableState, ObservablesError};
6use crate::spp::EphemerisSource;
7
8use super::store::{SbasCorrectionStore, SbasIonoGrid};
9
10pub trait IssueAwareBroadcast: EphemerisSource {
11 fn state_by_iode_at(
12 &self,
13 sat: GnssSatelliteId,
14 iode: u8,
15 t_j2000_s: f64,
16 ) -> Option<([f64; 3], f64)>;
17}
18
19#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
20pub enum SbasSolveMode {
21 #[default]
22 MixedAugmentation,
23 SbasOnly,
24}
25
26pub struct SbasCorrectedEphemeris<'a> {
27 broadcast: &'a dyn IssueAwareBroadcast,
28 store: &'a SbasCorrectionStore,
29 geo: GnssSatelliteId,
30 mode: SbasSolveMode,
31}
32
33impl<'a> SbasCorrectedEphemeris<'a> {
34 pub fn new(
35 broadcast: &'a dyn IssueAwareBroadcast,
36 store: &'a SbasCorrectionStore,
37 geo: GnssSatelliteId,
38 ) -> Self {
39 Self {
40 broadcast,
41 store,
42 geo,
43 mode: SbasSolveMode::MixedAugmentation,
44 }
45 }
46
47 pub fn with_preferred_geo(
48 broadcast: &'a dyn IssueAwareBroadcast,
49 store: &'a SbasCorrectionStore,
50 t_j2000_s: f64,
51 ) -> Option<Self> {
52 let geo = store.ready_geos(t_j2000_s).first().copied()?;
53 Some(Self::new(broadcast, store, geo))
54 }
55
56 pub fn with_mode(mut self, mode: SbasSolveMode) -> Self {
57 self.mode = mode;
58 self
59 }
60
61 pub fn iono_grid(&self) -> Option<&SbasIonoGrid> {
62 self.store.iono_grid(self.geo)
63 }
64
65 fn corrected_state(&self, sat: GnssSatelliteId, t_j2000_s: f64) -> Option<([f64; 3], f64)> {
66 if self.store.is_disabled(self.geo, t_j2000_s) || self.store.is_withdrawn(self.geo, sat) {
67 return None;
68 }
69
70 if sat == self.geo {
71 let geo_state = self.store.fresh_geo_nav(self.geo, t_j2000_s)?;
72 let (position, clock) = geo_state.state_at(t_j2000_s);
73 let clock = clock + self.fast_clock_delta_s(sat, t_j2000_s).unwrap_or(0.0);
74 return Some((position, clock));
75 }
76
77 let fast = self.store.fresh_fast(self.geo, sat, t_j2000_s);
78 let long = (sat.system == GnssSystem::Gps)
79 .then(|| self.store.fresh_long_term(self.geo, sat, t_j2000_s))
80 .flatten();
81
82 match (fast, long) {
83 (Some(fast), Some(long)) => {
84 let (mut position, mut clock) =
85 self.broadcast.state_by_iode_at(sat, long.iode, t_j2000_s)?;
86 let dt = t_j2000_s - long.t0_j2000_s;
87 for (i, component) in position.iter_mut().enumerate() {
88 *component += long.delta_ecef_m[i] + long.delta_ecef_rate_m_s[i] * dt;
89 }
90 clock += long.delta_af0_s + long.delta_af1_s_s * dt;
91 clock += (fast.prc_m + fast.rrc_m_s * (t_j2000_s - fast.t_of_j2000_s)) / C_M_S;
92 Some((position, clock))
93 }
94 (Some(fast), None) if self.store.allow_partial_corrections() => {
95 let (position, mut clock) =
96 self.broadcast.position_clock_at_j2000_s(sat, t_j2000_s)?;
97 clock += (fast.prc_m + fast.rrc_m_s * (t_j2000_s - fast.t_of_j2000_s)) / C_M_S;
98 Some((position, clock))
99 }
100 (None, Some(long)) if self.store.allow_partial_corrections() => {
101 let (mut position, mut clock) =
102 self.broadcast.state_by_iode_at(sat, long.iode, t_j2000_s)?;
103 let dt = t_j2000_s - long.t0_j2000_s;
104 for (i, component) in position.iter_mut().enumerate() {
105 *component += long.delta_ecef_m[i] + long.delta_ecef_rate_m_s[i] * dt;
106 }
107 clock += long.delta_af0_s + long.delta_af1_s_s * dt;
108 Some((position, clock))
109 }
110 _ => match self.mode {
111 SbasSolveMode::MixedAugmentation => {
112 self.broadcast.position_clock_at_j2000_s(sat, t_j2000_s)
113 }
114 SbasSolveMode::SbasOnly => None,
115 },
116 }
117 }
118
119 fn fast_clock_delta_s(&self, sat: GnssSatelliteId, t_j2000_s: f64) -> Option<f64> {
120 let fast = self.store.fresh_fast(self.geo, sat, t_j2000_s)?;
121 Some((fast.prc_m + fast.rrc_m_s * (t_j2000_s - fast.t_of_j2000_s)) / C_M_S)
122 }
123}
124
125impl EphemerisSource for SbasCorrectedEphemeris<'_> {
126 fn position_clock_at_j2000_s(
127 &self,
128 sat: GnssSatelliteId,
129 t_j2000_s: f64,
130 ) -> Option<([f64; 3], f64)> {
131 self.corrected_state(sat, t_j2000_s)
132 }
133}
134
135impl ObservableEphemerisSource for SbasCorrectedEphemeris<'_> {
136 fn observable_state_at_j2000_s(
137 &self,
138 sat: GnssSatelliteId,
139 t_j2000_s: f64,
140 ) -> Result<ObservableState, ObservablesError> {
141 let Some((position_ecef_m, clock_s)) = self.corrected_state(sat, t_j2000_s) else {
142 return Err(ObservablesError::NoEphemeris);
143 };
144 Ok(ObservableState {
145 position_ecef_m,
146 clock_s: Some(clock_s),
147 })
148 }
149}
150
151pub struct SbasCorrectedEphemerisOwned {
152 broadcast: Arc<dyn IssueAwareBroadcast + Send + Sync>,
153 store: Arc<SbasCorrectionStore>,
154 geo: GnssSatelliteId,
155 mode: SbasSolveMode,
156}
157
158impl SbasCorrectedEphemerisOwned {
159 pub fn new(
160 broadcast: Arc<dyn IssueAwareBroadcast + Send + Sync>,
161 store: Arc<SbasCorrectionStore>,
162 geo: GnssSatelliteId,
163 ) -> Self {
164 Self {
165 broadcast,
166 store,
167 geo,
168 mode: SbasSolveMode::MixedAugmentation,
169 }
170 }
171
172 pub fn with_mode(mut self, mode: SbasSolveMode) -> Self {
173 self.mode = mode;
174 self
175 }
176
177 pub fn iono_grid(&self) -> Option<&SbasIonoGrid> {
178 self.store.iono_grid(self.geo)
179 }
180
181 fn borrowed(&self) -> SbasCorrectedEphemeris<'_> {
182 SbasCorrectedEphemeris::new(self.broadcast.as_ref(), self.store.as_ref(), self.geo)
183 .with_mode(self.mode)
184 }
185}
186
187impl EphemerisSource for SbasCorrectedEphemerisOwned {
188 fn position_clock_at_j2000_s(
189 &self,
190 sat: GnssSatelliteId,
191 t_j2000_s: f64,
192 ) -> Option<([f64; 3], f64)> {
193 self.borrowed().position_clock_at_j2000_s(sat, t_j2000_s)
194 }
195}
196
197impl ObservableEphemerisSource for SbasCorrectedEphemerisOwned {
198 fn observable_state_at_j2000_s(
199 &self,
200 sat: GnssSatelliteId,
201 t_j2000_s: f64,
202 ) -> Result<ObservableState, ObservablesError> {
203 self.borrowed().observable_state_at_j2000_s(sat, t_j2000_s)
204 }
205}
206
207impl IssueAwareBroadcast for crate::rinex_nav::BroadcastStore {
208 fn state_by_iode_at(
209 &self,
210 sat: GnssSatelliteId,
211 iode: u8,
212 t_j2000_s: f64,
213 ) -> Option<([f64; 3], f64)> {
214 crate::rinex_nav::BroadcastStore::state_by_iode_at(self, sat, iode, t_j2000_s)
215 }
216}
217
218#[cfg(test)]
219mod tests {
220 use super::*;
221 use crate::astro::time::model::{GnssWeekTow, TimeScale};
222 use crate::sbas::message::{
223 SbasDoNotUse, SbasFastCorrections, SbasIgpDelay, SbasIgpMask, SbasIonoDelays, SbasMessage,
224 SbasPrnMask, SpareBits,
225 };
226 use crate::sbas::store::{sbas_prn_to_sat, SbasLongTermCorrection};
227
228 struct StaticBroadcast {
229 sat: GnssSatelliteId,
230 state: ([f64; 3], f64),
231 iode: u8,
232 }
233
234 impl EphemerisSource for StaticBroadcast {
235 fn position_clock_at_j2000_s(
236 &self,
237 sat: GnssSatelliteId,
238 _t_j2000_s: f64,
239 ) -> Option<([f64; 3], f64)> {
240 (sat == self.sat).then_some(self.state)
241 }
242 }
243
244 impl IssueAwareBroadcast for StaticBroadcast {
245 fn state_by_iode_at(
246 &self,
247 sat: GnssSatelliteId,
248 iode: u8,
249 t_j2000_s: f64,
250 ) -> Option<([f64; 3], f64)> {
251 (iode == self.iode)
252 .then(|| self.position_clock_at_j2000_s(sat, t_j2000_s))
253 .flatten()
254 }
255 }
256
257 fn epoch(tow_s: f64) -> GnssWeekTow {
258 GnssWeekTow::new(TimeScale::Gpst, 2400, tow_s).expect("valid epoch")
259 }
260
261 #[test]
262 fn mixed_mode_falls_back_to_broadcast_without_complete_correction() {
263 let sat = GnssSatelliteId::new(GnssSystem::Gps, 1).expect("valid GPS PRN");
264 let geo = sbas_prn_to_sat(120).unwrap();
265 let broadcast = StaticBroadcast {
266 sat,
267 state: ([1.0, 2.0, 3.0], 4.0),
268 iode: 7,
269 };
270 let store = SbasCorrectionStore::new();
271 let source = SbasCorrectedEphemeris::new(&broadcast, &store, geo);
272 assert_eq!(
273 source.position_clock_at_j2000_s(sat, 0.0),
274 Some(([1.0, 2.0, 3.0], 4.0))
275 );
276 assert_eq!(
277 source
278 .with_mode(SbasSolveMode::SbasOnly)
279 .position_clock_at_j2000_s(sat, 0.0),
280 None
281 );
282 }
283
284 #[test]
285 fn withdrawn_satellite_returns_no_state() {
286 let sat = GnssSatelliteId::new(GnssSystem::Gps, 1).expect("valid GPS PRN");
287 let geo = sbas_prn_to_sat(120).unwrap();
288 let mut store = SbasCorrectionStore::new();
289 let mut mask = [false; 210];
290 mask[0] = true;
291 store
292 .ingest(
293 &SbasMessage::PrnMask(SbasPrnMask {
294 preamble: 0x53,
295 iodp: 1,
296 mask,
297 reserved: SpareBits::new(),
298 }),
299 geo,
300 epoch(10.0),
301 )
302 .unwrap();
303 let mut udrei = [0u8; 13];
304 udrei[0] = 15;
305 store
306 .ingest(
307 &SbasMessage::FastCorrections(SbasFastCorrections {
308 preamble: 0x53,
309 message_type: 2,
310 iodf: 1,
311 iodp: 1,
312 prc: [0; 13],
313 udrei,
314 reserved: SpareBits::new(),
315 }),
316 geo,
317 epoch(20.0),
318 )
319 .unwrap();
320 let broadcast = StaticBroadcast {
321 sat,
322 state: ([1.0, 2.0, 3.0], 4.0),
323 iode: 7,
324 };
325 let source = SbasCorrectedEphemeris::new(&broadcast, &store, geo);
326 assert_eq!(source.position_clock_at_j2000_s(sat, 0.0), None);
327 }
328
329 #[test]
330 fn complete_correction_adds_position_and_clock_terms() {
331 let sat = GnssSatelliteId::new(GnssSystem::Gps, 1).expect("valid GPS PRN");
332 let geo = sbas_prn_to_sat(120).unwrap();
333 let broadcast = StaticBroadcast {
334 sat,
335 state: ([1.0, 2.0, 3.0], 4.0),
336 iode: 9,
337 };
338 let mut store = SbasCorrectionStore::new();
339 let mut mask = [false; 210];
340 mask[0] = true;
341 store
342 .ingest(
343 &SbasMessage::PrnMask(SbasPrnMask {
344 preamble: 0x53,
345 iodp: 1,
346 mask,
347 reserved: SpareBits::new(),
348 }),
349 geo,
350 epoch(10.0),
351 )
352 .unwrap();
353 let mut prc = [0i16; 13];
354 prc[0] = 8;
355 store
356 .ingest(
357 &SbasMessage::FastCorrections(SbasFastCorrections {
358 preamble: 0x53,
359 message_type: 2,
360 iodf: 1,
361 iodp: 1,
362 prc,
363 udrei: [0; 13],
364 reserved: SpareBits::new(),
365 }),
366 geo,
367 epoch(20.0),
368 )
369 .unwrap();
370 store.insert_long_term_for_test(
371 geo,
372 sat,
373 SbasLongTermCorrection {
374 iode: 9,
375 delta_ecef_m: [10.0, 20.0, 30.0],
376 delta_ecef_rate_m_s: [1.0, 0.0, 0.0],
377 delta_af0_s: 0.5,
378 delta_af1_s_s: 0.0,
379 t0_j2000_s: epoch_to_j2000_s_for_test(epoch(20.0)),
380 },
381 epoch_to_j2000_s_for_test(epoch(20.0)),
382 );
383 let source = SbasCorrectedEphemeris::new(&broadcast, &store, geo);
384 let t = epoch_to_j2000_s_for_test(epoch(21.0));
385 let (pos, clock) = source.position_clock_at_j2000_s(sat, t).unwrap();
386 assert_eq!(pos, [12.0, 22.0, 33.0]);
387 assert!((clock - (4.5 + 1.0 / C_M_S)).abs() < 1.0e-15);
388 }
389
390 #[test]
391 fn fresh_mt0_withholds_iono_grid_from_corrected_source() {
392 let sat = GnssSatelliteId::new(GnssSystem::Gps, 1).expect("valid GPS PRN");
393 let geo = sbas_prn_to_sat(120).unwrap();
394 let broadcast = StaticBroadcast {
395 sat,
396 state: ([1.0, 2.0, 3.0], 4.0),
397 iode: 7,
398 };
399 let mut store = SbasCorrectionStore::new();
400 let mut mask = [false; 201];
401 mask[0] = true;
402 store
403 .ingest(
404 &SbasMessage::IgpMask(SbasIgpMask {
405 preamble: 0x53,
406 band_number: 0,
407 iodi: 1,
408 mask,
409 reserved: SpareBits::new(),
410 }),
411 geo,
412 epoch(1.0),
413 )
414 .unwrap();
415 let mut entries: [SbasIgpDelay; 15] = core::array::from_fn(|_| SbasIgpDelay::default());
416 entries[0] = SbasIgpDelay {
417 vertical_delay: 8,
418 givei: 0,
419 };
420 store
421 .ingest(
422 &SbasMessage::IonoDelays(SbasIonoDelays {
423 preamble: 0x53,
424 band_number: 0,
425 block_id: 0,
426 iodi: 1,
427 entries,
428 reserved: SpareBits::new(),
429 }),
430 geo,
431 epoch(2.0),
432 )
433 .unwrap();
434 let source = SbasCorrectedEphemeris::new(&broadcast, &store, geo);
435 assert!(source.iono_grid().is_some());
436
437 store
438 .ingest(
439 &SbasMessage::DoNotUse(SbasDoNotUse {
440 preamble: 0x53,
441 data: Vec::new(),
442 }),
443 geo,
444 epoch(3.0),
445 )
446 .unwrap();
447 let source = SbasCorrectedEphemeris::new(&broadcast, &store, geo);
448 assert!(source.iono_grid().is_none());
449 }
450
451 fn epoch_to_j2000_s_for_test(epoch: GnssWeekTow) -> f64 {
452 f64::from(epoch.week) * crate::constants::SECONDS_PER_WEEK + epoch.tow_s
453 - crate::constants::GPS_EPOCH_TO_J2000_S
454 }
455}