sidereon_core/navigation/
lnav.rs

1//! GPS L1 C/A LNAV navigation message synthesis and decoding (subframes 1-3).
2//!
3//! The legacy navigation (LNAV) message is the data stream modulated onto the
4//! GPS L1 C/A signal at 50 bits per second. Its structure is defined in
5//! IS-GPS-200 (Section 20.3): the message is organized into 1500-bit *frames*,
6//! each frame being five 300-bit *subframes*, and each subframe being ten 30-bit
7//! *words*. Every word carries 24 source data bits (most significant first)
8//! followed by 6 parity bits.
9//!
10//! This module covers the clock and ephemeris subframes:
11//!
12//!   * Subframe 1 - SV clock correction and health (IS-GPS-200 Table 20-I).
13//!   * Subframe 2 - first half of the ephemeris (IS-GPS-200 Table 20-II).
14//!   * Subframe 3 - second half of the ephemeris (IS-GPS-200 Table 20-III).
15//!
16//! The first word of every subframe is the telemetry (TLM) word; the second is
17//! the hand-over word (HOW). Both are described in IS-GPS-200 Section 20.3.3.
18//!
19//! [`encode`] and [`decode`] exchange engineering-unit parameter values (the
20//! products of the transmitted integers and their IS-GPS-200 scale factors).
21//! Angular ephemeris quantities are in semicircles (and semicircles/second),
22//! the harmonic correction terms are in radians, distances are in meters, and
23//! clock/time quantities are in seconds, exactly as tabulated in IS-GPS-200.
24//!
25//! The codec is integer / exact-power-of-two arithmetic throughout, so it is a
26//! 0-ULP target: a given set of parameters encodes to one exact bit pattern. The
27//! authoritative golden is the `lnav` section of
28//! `tests/fixtures/orbis_gnss_application_golden.json` (the Python reference
29//! generator), asserted bit-for-bit in `tests/lnav.rs`.
30
31use crate::validate;
32
33/// Bit length of a single LNAV word (IS-GPS-200 Section 20.3.2).
34pub const WORD_LENGTH: usize = 30;
35/// Bit length of a single LNAV subframe (IS-GPS-200 Section 20.3.2).
36pub const SUBFRAME_LENGTH: usize = 300;
37/// The 8-bit TLM preamble `1000 1011` as an integer (IS-GPS-200 Section 20.3.3.1).
38pub const PREAMBLE: u32 = 0b1000_1011;
39
40// IS-GPS-200 per-field LSB scale factors (Tables 20-I/II/III). Each is an exact
41// power of two; `1.0 / 2^n` is the exact f64 value and matches the reference
42// generator's `2 ** -n` bit-for-bit, so `round(value / scale)` is deterministic.
43const TWO_POW_4: f64 = 16.0;
44const TWO_POW_M5: f64 = 1.0 / 32.0;
45const TWO_POW_M19: f64 = 1.0 / 524_288.0;
46const TWO_POW_M29: f64 = 1.0 / 536_870_912.0;
47const TWO_POW_M31: f64 = 1.0 / 2_147_483_648.0;
48const TWO_POW_M33: f64 = 1.0 / 8_589_934_592.0;
49const TWO_POW_M43: f64 = 1.0 / 8_796_093_022_208.0;
50const TWO_POW_M55: f64 = 1.0 / 36_028_797_018_963_968.0;
51
52/// A numeric parameter value preserving the integer-vs-float distinction of the
53/// caller's input, so range-validation failures can echo the value back in its
54/// original type (an integer field reports an integer, a scaled field a float).
55#[derive(Clone, Copy, Debug, PartialEq)]
56pub enum LnavNumber {
57    /// An integer-typed value.
58    Int(i64),
59    /// A floating-point value.
60    Float(f64),
61}
62
63impl LnavNumber {
64    fn as_f64(self) -> f64 {
65        match self {
66            LnavNumber::Int(i) => i as f64,
67            LnavNumber::Float(f) => f,
68        }
69    }
70
71    fn as_i64_truncated(self) -> i64 {
72        match self {
73            LnavNumber::Int(i) => i,
74            LnavNumber::Float(f) => f as i64,
75        }
76    }
77
78    fn is_int(self) -> bool {
79        matches!(self, LnavNumber::Int(_))
80    }
81}
82
83/// An LNAV parameter field, used to tag a range-validation failure.
84#[derive(Clone, Copy, Debug, PartialEq, Eq)]
85pub enum LnavField {
86    Tow,
87    Alert,
88    AntiSpoof,
89    Integrity,
90    TlmMessage,
91    WeekNumber,
92    L2Code,
93    L2PDataFlag,
94    UraIndex,
95    SvHealth,
96    Iodc,
97    Tgd,
98    Toc,
99    Af2,
100    Af1,
101    Af0,
102    Iode,
103    Crs,
104    DeltaN,
105    M0,
106    Cuc,
107    Eccentricity,
108    Cus,
109    SqrtA,
110    Toe,
111    FitIntervalFlag,
112    Aodo,
113    Cic,
114    Omega0,
115    Cis,
116    I0,
117    Crc,
118    Omega,
119    OmegaDot,
120    Idot,
121}
122
123impl LnavField {
124    /// The snake_case field name, matching the Elixir error-tuple atom.
125    pub fn name(self) -> &'static str {
126        match self {
127            LnavField::Tow => "tow",
128            LnavField::Alert => "alert",
129            LnavField::AntiSpoof => "anti_spoof",
130            LnavField::Integrity => "integrity",
131            LnavField::TlmMessage => "tlm_message",
132            LnavField::WeekNumber => "week_number",
133            LnavField::L2Code => "l2_code",
134            LnavField::L2PDataFlag => "l2_p_data_flag",
135            LnavField::UraIndex => "ura_index",
136            LnavField::SvHealth => "sv_health",
137            LnavField::Iodc => "iodc",
138            LnavField::Tgd => "tgd",
139            LnavField::Toc => "toc",
140            LnavField::Af2 => "af2",
141            LnavField::Af1 => "af1",
142            LnavField::Af0 => "af0",
143            LnavField::Iode => "iode",
144            LnavField::Crs => "crs",
145            LnavField::DeltaN => "delta_n",
146            LnavField::M0 => "m0",
147            LnavField::Cuc => "cuc",
148            LnavField::Eccentricity => "eccentricity",
149            LnavField::Cus => "cus",
150            LnavField::SqrtA => "sqrt_a",
151            LnavField::Toe => "toe",
152            LnavField::FitIntervalFlag => "fit_interval_flag",
153            LnavField::Aodo => "aodo",
154            LnavField::Cic => "cic",
155            LnavField::Omega0 => "omega0",
156            LnavField::Cis => "cis",
157            LnavField::I0 => "i0",
158            LnavField::Crc => "crc",
159            LnavField::Omega => "omega",
160            LnavField::OmegaDot => "omega_dot",
161            LnavField::Idot => "idot",
162        }
163    }
164}
165
166/// An LNAV codec failure.
167#[derive(Clone, Copy, Debug, PartialEq)]
168pub enum LnavError {
169    /// A parameter does not fit its transmitted field; echoes the offending
170    /// value in its original numeric type.
171    OutOfRange { field: LnavField, value: LnavNumber },
172    /// A word's recomputed parity did not match (1-based subframe and word).
173    ParityFailed { subframe: u8, word: u8 },
174    /// A parity source word was not exactly 24 data bits.
175    BadWordLength { expected: usize, actual: usize },
176    /// A subframe was not exactly [`SUBFRAME_LENGTH`] bits.
177    BadSubframeLength { subframe: u8 },
178}
179
180/// Clock and ephemeris parameters in engineering units (the per-field input to
181/// [`encode`]). Values preserve their numeric type for faithful error echoing.
182#[derive(Clone, Copy, Debug)]
183pub struct LnavParams {
184    pub week_number: LnavNumber,
185    pub l2_code: LnavNumber,
186    pub l2_p_data_flag: LnavNumber,
187    pub ura_index: LnavNumber,
188    pub sv_health: LnavNumber,
189    pub iodc: LnavNumber,
190    pub tgd: LnavNumber,
191    pub toc: LnavNumber,
192    pub af0: LnavNumber,
193    pub af1: LnavNumber,
194    pub af2: LnavNumber,
195    pub iode: LnavNumber,
196    pub crs: LnavNumber,
197    pub delta_n: LnavNumber,
198    pub m0: LnavNumber,
199    pub cuc: LnavNumber,
200    pub eccentricity: LnavNumber,
201    pub cus: LnavNumber,
202    pub sqrt_a: LnavNumber,
203    pub toe: LnavNumber,
204    pub fit_interval_flag: LnavNumber,
205    pub aodo: LnavNumber,
206    pub cic: LnavNumber,
207    pub omega0: LnavNumber,
208    pub cis: LnavNumber,
209    pub i0: LnavNumber,
210    pub crc: LnavNumber,
211    pub omega: LnavNumber,
212    pub omega_dot: LnavNumber,
213    pub idot: LnavNumber,
214}
215
216/// TLM/HOW options accompanying an [`encode`] (defaults applied by the caller).
217#[derive(Clone, Copy, Debug)]
218pub struct LnavOptions {
219    pub tow: LnavNumber,
220    pub alert: LnavNumber,
221    pub anti_spoof: LnavNumber,
222    pub integrity: LnavNumber,
223    pub tlm_message: LnavNumber,
224}
225
226/// Decoded clock and ephemeris parameters (the typed output of [`decode`]). The
227/// integer-typed fields are recovered exactly; the scaled fields are the
228/// transmitted integer times the IS-GPS-200 LSB. (`l2_p_data_flag` is an
229/// encode-only flag in word 4 and is not recovered.)
230#[derive(Clone, Copy, Debug, PartialEq)]
231pub struct LnavDecoded {
232    pub week_number: i64,
233    pub l2_code: i64,
234    pub ura_index: i64,
235    pub sv_health: i64,
236    pub iodc: i64,
237    pub tgd: f64,
238    pub toc: i64,
239    pub af0: f64,
240    pub af1: f64,
241    pub af2: f64,
242    pub iode: i64,
243    pub crs: f64,
244    pub delta_n: f64,
245    pub m0: f64,
246    pub cuc: f64,
247    pub eccentricity: f64,
248    pub cus: f64,
249    pub sqrt_a: f64,
250    pub toe: i64,
251    pub fit_interval_flag: i64,
252    pub aodo: i64,
253    pub cic: f64,
254    pub omega0: f64,
255    pub cis: f64,
256    pub i0: f64,
257    pub crc: f64,
258    pub omega: f64,
259    pub omega_dot: f64,
260    pub idot: f64,
261}
262
263// --- field descriptors for range validation ---------------------------------
264
265#[derive(Clone, Copy)]
266enum FieldKind {
267    /// Pure unsigned integer field (must be a non-negative integer).
268    Uint { bits: u32 },
269    /// Scaled unsigned field (`round(value / scale)` must fit, value >= 0).
270    UintScaled { bits: u32, scale: f64 },
271    /// Scaled signed field (`round(value / scale)` in two's-complement range).
272    SintScaled { bits: u32, scale: f64 },
273}
274
275/// Validates one field exactly as IS-GPS-200 / the Elixir reference does, with
276/// the same first-failure semantics.
277fn validate_field(field: LnavField, value: LnavNumber, kind: FieldKind) -> Result<(), LnavError> {
278    let in_range = match kind {
279        FieldKind::Uint { bits } => {
280            value.is_int() && {
281                let v = value.as_i64_truncated();
282                v >= 0 && v < (1i64 << bits)
283            }
284        }
285        FieldKind::UintScaled { bits, scale } => {
286            value.as_f64() >= 0.0 && {
287                let n = round_half_away(value.as_f64() / scale);
288                n >= 0 && n < (1i64 << bits)
289            }
290        }
291        FieldKind::SintScaled { bits, scale } => {
292            // `is_finite` reproduces the Elixir `is_number/1` guard: a non-number
293            // (e.g. a missing `nil` field) is out of range, not a silent zero.
294            value.as_f64().is_finite() && {
295                let n = round_half_away(value.as_f64() / scale);
296                let limit = 1i64 << (bits - 1);
297                n >= -limit && n < limit
298            }
299        }
300    };
301
302    if in_range {
303        Ok(())
304    } else {
305        Err(LnavError::OutOfRange { field, value })
306    }
307}
308
309/// IEEE round-half-away-from-zero, matching Elixir `round/1`.
310fn round_half_away(x: f64) -> i64 {
311    x.round() as i64
312}
313
314// --- public API -------------------------------------------------------------
315
316/// Extracts the 17-bit time-of-week count from a hand-over word.
317///
318/// Accepts either a 30-bit HOW word or a full 300-bit subframe (whose word 2 is
319/// the HOW). Returns `None` on any other length.
320pub fn tow(bits: &[u8]) -> Option<u64> {
321    how_word(bits).map(|how| bits_to_uint(&how[0..17]))
322}
323
324/// Extracts the 3-bit subframe ID from a hand-over word.
325///
326/// Accepts a 30-bit HOW word or a full 300-bit subframe. Returns `None` on any
327/// other length.
328pub fn subframe_id(bits: &[u8]) -> Option<u64> {
329    how_word(bits).map(|how| bits_to_uint(&how[19..22]))
330}
331
332fn how_word(bits: &[u8]) -> Option<Vec<u8>> {
333    match bits.len() {
334        WORD_LENGTH => Some(bits.to_vec()),
335        SUBFRAME_LENGTH => Some(bits[WORD_LENGTH..2 * WORD_LENGTH].to_vec()),
336        _ => None,
337    }
338}
339
340/// Computes the 6 parity bits of a word (IS-GPS-200 Table 20-XIV).
341///
342/// `data24` is the 24 *source* data bits (most significant first, before the
343/// `D30*` complementation). `d29_prev`/`d30_prev` are the two trailing parity
344/// bits of the previous word. Returns `[D25, D26, D27, D28, D29, D30]`.
345pub fn parity(data24: &[u8], d29_prev: u8, d30_prev: u8) -> Result<[u8; 6], LnavError> {
346    validate::exact_len(data24, 24, "lnav parity data bits").map_err(|error| {
347        LnavError::BadWordLength {
348            expected: error.expected,
349            actual: error.actual,
350        }
351    })?;
352
353    // 1-based indexing of the source bits, matching IS-GPS-200 Table 20-XIV.
354    let d = |n: usize| data24[n - 1];
355
356    let d25 = xor(&[
357        d29_prev,
358        d(1),
359        d(2),
360        d(3),
361        d(5),
362        d(6),
363        d(10),
364        d(11),
365        d(12),
366        d(13),
367        d(14),
368        d(17),
369        d(18),
370        d(20),
371        d(23),
372    ]);
373    let d26 = xor(&[
374        d30_prev,
375        d(2),
376        d(3),
377        d(4),
378        d(6),
379        d(7),
380        d(11),
381        d(12),
382        d(13),
383        d(14),
384        d(15),
385        d(18),
386        d(19),
387        d(21),
388        d(24),
389    ]);
390    let d27 = xor(&[
391        d29_prev,
392        d(1),
393        d(3),
394        d(4),
395        d(5),
396        d(7),
397        d(8),
398        d(12),
399        d(13),
400        d(14),
401        d(15),
402        d(16),
403        d(19),
404        d(20),
405        d(22),
406    ]);
407    let d28 = xor(&[
408        d30_prev,
409        d(2),
410        d(4),
411        d(5),
412        d(6),
413        d(8),
414        d(9),
415        d(13),
416        d(14),
417        d(15),
418        d(16),
419        d(17),
420        d(20),
421        d(21),
422        d(23),
423    ]);
424    let d29 = xor(&[
425        d30_prev,
426        d(1),
427        d(3),
428        d(5),
429        d(6),
430        d(7),
431        d(9),
432        d(10),
433        d(14),
434        d(15),
435        d(16),
436        d(17),
437        d(18),
438        d(21),
439        d(22),
440        d(24),
441    ]);
442    let d30 = xor(&[
443        d29_prev,
444        d(3),
445        d(5),
446        d(6),
447        d(8),
448        d(9),
449        d(10),
450        d(11),
451        d(13),
452        d(15),
453        d(19),
454        d(22),
455        d(23),
456        d(24),
457    ]);
458
459    Ok([d25, d26, d27, d28, d29, d30])
460}
461
462/// Verifies the parity of a single 30-bit word.
463///
464/// `word30` is the 30-bit word as transmitted (data bits possibly complemented
465/// by `D30*`, followed by 6 received parity bits). `d29_prev`/`d30_prev` are the
466/// previous word's trailing parity bits.
467pub fn parity_valid(word30: &[u8], d29_prev: u8, d30_prev: u8) -> bool {
468    if word30.len() != WORD_LENGTH {
469        return false;
470    }
471    let source: Vec<u8> = word30[0..24].iter().map(|b| b ^ d30_prev).collect();
472    let received = &word30[24..30];
473    parity(&source, d29_prev, d30_prev).is_ok_and(|par| par.as_slice() == received)
474}
475
476/// Encodes clock and ephemeris parameters into LNAV subframes 1-3.
477///
478/// Returns the three 300-bit subframes (most significant first) keyed 1/2/3.
479/// Out-of-range parameters yield [`LnavError::OutOfRange`].
480pub fn encode(params: &LnavParams, opts: &LnavOptions) -> Result<[Vec<u8>; 3], LnavError> {
481    validate_field(LnavField::Tow, opts.tow, FieldKind::Uint { bits: 17 })?;
482    validate_field(LnavField::Alert, opts.alert, FieldKind::Uint { bits: 1 })?;
483    validate_field(
484        LnavField::AntiSpoof,
485        opts.anti_spoof,
486        FieldKind::Uint { bits: 1 },
487    )?;
488    validate_field(
489        LnavField::Integrity,
490        opts.integrity,
491        FieldKind::Uint { bits: 1 },
492    )?;
493    validate_field(
494        LnavField::TlmMessage,
495        opts.tlm_message,
496        FieldKind::Uint { bits: 14 },
497    )?;
498
499    let w1 = subframe1_words(params)?;
500    let w2 = subframe2_words(params)?;
501    let w3 = subframe3_words(params)?;
502
503    let tlm = tlm_data(
504        opts.tlm_message.as_i64_truncated(),
505        opts.integrity.as_i64_truncated(),
506    );
507
508    let sf1 = assemble_subframe(&prepend_headers(&tlm, opts, 1, w1))?;
509    let sf2 = assemble_subframe(&prepend_headers(&tlm, opts, 2, w2))?;
510    let sf3 = assemble_subframe(&prepend_headers(&tlm, opts, 3, w3))?;
511
512    Ok([sf1, sf2, sf3])
513}
514
515/// Decodes LNAV subframes 1-3 back into the engineering-unit parameter struct.
516///
517/// Parity is verified on all 30 words first; a failure returns
518/// [`LnavError::ParityFailed`] (1-based word).
519pub fn decode(sf1: &[u8], sf2: &[u8], sf3: &[u8]) -> Result<LnavDecoded, LnavError> {
520    verify_subframe(sf1, 1)?;
521    verify_subframe(sf2, 2)?;
522    verify_subframe(sf3, 3)?;
523
524    let w1 = source_words(sf1);
525    let w2 = source_words(sf2);
526    let w3 = source_words(sf3);
527
528    let mut d = LnavDecoded {
529        week_number: 0,
530        l2_code: 0,
531        ura_index: 0,
532        sv_health: 0,
533        iodc: 0,
534        tgd: 0.0,
535        toc: 0,
536        af0: 0.0,
537        af1: 0.0,
538        af2: 0.0,
539        iode: 0,
540        crs: 0.0,
541        delta_n: 0.0,
542        m0: 0.0,
543        cuc: 0.0,
544        eccentricity: 0.0,
545        cus: 0.0,
546        sqrt_a: 0.0,
547        toe: 0,
548        fit_interval_flag: 0,
549        aodo: 0,
550        cic: 0.0,
551        omega0: 0.0,
552        cis: 0.0,
553        i0: 0.0,
554        crc: 0.0,
555        omega: 0.0,
556        omega_dot: 0.0,
557        idot: 0.0,
558    };
559
560    decode_subframe1(&mut d, &w1);
561    decode_subframe2(&mut d, &w2);
562    decode_subframe3(&mut d, &w3);
563
564    Ok(d)
565}
566
567// --- Subframe 1 (clock/health), IS-GPS-200 Table 20-I -----------------------
568
569/// One word entry awaiting parity: its 24 source data bits, and whether the two
570/// trailing data bits must be solved so the word's `D29`/`D30` parity is zero.
571struct WordEntry {
572    data: Vec<u8>,
573    solve: bool,
574}
575
576impl WordEntry {
577    fn raw(data: Vec<u8>) -> Self {
578        WordEntry { data, solve: false }
579    }
580    fn solved(data: Vec<u8>) -> Self {
581        WordEntry { data, solve: true }
582    }
583}
584
585fn subframe1_words(p: &LnavParams) -> Result<Vec<WordEntry>, LnavError> {
586    validate_field(
587        LnavField::WeekNumber,
588        p.week_number,
589        FieldKind::Uint { bits: 10 },
590    )?;
591    validate_field(LnavField::L2Code, p.l2_code, FieldKind::Uint { bits: 2 })?;
592    validate_field(
593        LnavField::L2PDataFlag,
594        p.l2_p_data_flag,
595        FieldKind::Uint { bits: 1 },
596    )?;
597    validate_field(
598        LnavField::UraIndex,
599        p.ura_index,
600        FieldKind::Uint { bits: 4 },
601    )?;
602    validate_field(
603        LnavField::SvHealth,
604        p.sv_health,
605        FieldKind::Uint { bits: 6 },
606    )?;
607    validate_field(LnavField::Iodc, p.iodc, FieldKind::Uint { bits: 10 })?;
608    validate_field(
609        LnavField::Tgd,
610        p.tgd,
611        FieldKind::SintScaled {
612            bits: 8,
613            scale: TWO_POW_M31,
614        },
615    )?;
616    validate_field(
617        LnavField::Toc,
618        p.toc,
619        FieldKind::UintScaled {
620            bits: 16,
621            scale: TWO_POW_4,
622        },
623    )?;
624    validate_field(
625        LnavField::Af2,
626        p.af2,
627        FieldKind::SintScaled {
628            bits: 8,
629            scale: TWO_POW_M55,
630        },
631    )?;
632    validate_field(
633        LnavField::Af1,
634        p.af1,
635        FieldKind::SintScaled {
636            bits: 16,
637            scale: TWO_POW_M43,
638        },
639    )?;
640    validate_field(
641        LnavField::Af0,
642        p.af0,
643        FieldKind::SintScaled {
644            bits: 22,
645            scale: TWO_POW_M31,
646        },
647    )?;
648
649    let iodc = p.iodc.as_i64_truncated();
650    let iodc_msb = (iodc >> 8) & 0x3;
651    let iodc_lsb = iodc & 0xFF;
652    let l2_p_data_flag = p.l2_p_data_flag.as_i64_truncated();
653
654    // Word 3: WN(10) L2(2) URA(4) health(6) IODC-MSB(2).
655    let mut word3 = pack_uint(p.week_number.as_i64_truncated(), 10);
656    word3.extend(pack_uint(p.l2_code.as_i64_truncated(), 2));
657    word3.extend(pack_uint(p.ura_index.as_i64_truncated(), 4));
658    word3.extend(pack_uint(p.sv_health.as_i64_truncated(), 6));
659    word3.extend(pack_uint(iodc_msb, 2));
660
661    // Word 4: L2-P data flag (bit 1) + 23 reserved bits.
662    let mut word4 = pack_uint(l2_p_data_flag, 1);
663    word4.extend(zeros(23));
664    // Words 5, 6: reserved.
665    let word5 = zeros(24);
666    let word6 = zeros(24);
667    // Word 7: 16 reserved bits then TGD(8).
668    let mut word7 = zeros(16);
669    word7.extend(pack_sint(p.tgd.as_f64(), 8, TWO_POW_M31));
670    // Word 8: IODC-LSB(8) toc(16).
671    let mut word8 = pack_uint(iodc_lsb, 8);
672    word8.extend(pack_uint_scaled(p.toc.as_f64(), 16, TWO_POW_4));
673    // Word 9: af2(8) af1(16).
674    let mut word9 = pack_sint(p.af2.as_f64(), 8, TWO_POW_M55);
675    word9.extend(pack_sint(p.af1.as_f64(), 16, TWO_POW_M43));
676    // Word 10: af0(22) + 2 solved bits.
677    let word10 = pack_sint(p.af0.as_f64(), 22, TWO_POW_M31);
678
679    Ok(vec![
680        WordEntry::raw(word3),
681        WordEntry::raw(word4),
682        WordEntry::raw(word5),
683        WordEntry::raw(word6),
684        WordEntry::raw(word7),
685        WordEntry::raw(word8),
686        WordEntry::raw(word9),
687        WordEntry::solved(word10),
688    ])
689}
690
691fn decode_subframe1(p: &mut LnavDecoded, w: &[Vec<u8>]) {
692    let word3 = &w[0];
693    let word7 = &w[4];
694    let word8 = &w[5];
695    let word9 = &w[6];
696    let word10 = &w[7];
697
698    p.week_number = bits_to_uint(slice(word3, 1, 10)) as i64;
699    p.l2_code = bits_to_uint(slice(word3, 11, 2)) as i64;
700    p.ura_index = bits_to_uint(slice(word3, 13, 4)) as i64;
701    p.sv_health = bits_to_uint(slice(word3, 17, 6)) as i64;
702    let iodc_msb = bits_to_uint(slice(word3, 23, 2)) as i64;
703
704    p.tgd = unpack_sint(slice(word7, 17, 8), TWO_POW_M31);
705    let iodc_lsb = bits_to_uint(slice(word8, 1, 8)) as i64;
706    p.toc = unpack_uint_scaled_int(slice(word8, 9, 16), TWO_POW_4);
707    p.af2 = unpack_sint(slice(word9, 1, 8), TWO_POW_M55);
708    p.af1 = unpack_sint(slice(word9, 9, 16), TWO_POW_M43);
709    p.af0 = unpack_sint(slice(word10, 1, 22), TWO_POW_M31);
710
711    p.iodc = (iodc_msb << 8) | iodc_lsb;
712}
713
714// --- Subframe 2 (ephemeris part 1), IS-GPS-200 Table 20-II ------------------
715
716fn subframe2_words(p: &LnavParams) -> Result<Vec<WordEntry>, LnavError> {
717    validate_field(LnavField::Iode, p.iode, FieldKind::Uint { bits: 8 })?;
718    validate_field(
719        LnavField::Crs,
720        p.crs,
721        FieldKind::SintScaled {
722            bits: 16,
723            scale: TWO_POW_M5,
724        },
725    )?;
726    validate_field(
727        LnavField::DeltaN,
728        p.delta_n,
729        FieldKind::SintScaled {
730            bits: 16,
731            scale: TWO_POW_M43,
732        },
733    )?;
734    validate_field(
735        LnavField::M0,
736        p.m0,
737        FieldKind::SintScaled {
738            bits: 32,
739            scale: TWO_POW_M31,
740        },
741    )?;
742    validate_field(
743        LnavField::Cuc,
744        p.cuc,
745        FieldKind::SintScaled {
746            bits: 16,
747            scale: TWO_POW_M29,
748        },
749    )?;
750    validate_field(
751        LnavField::Eccentricity,
752        p.eccentricity,
753        FieldKind::UintScaled {
754            bits: 32,
755            scale: TWO_POW_M33,
756        },
757    )?;
758    validate_field(
759        LnavField::Cus,
760        p.cus,
761        FieldKind::SintScaled {
762            bits: 16,
763            scale: TWO_POW_M29,
764        },
765    )?;
766    validate_field(
767        LnavField::SqrtA,
768        p.sqrt_a,
769        FieldKind::UintScaled {
770            bits: 32,
771            scale: TWO_POW_M19,
772        },
773    )?;
774    validate_field(
775        LnavField::Toe,
776        p.toe,
777        FieldKind::UintScaled {
778            bits: 16,
779            scale: TWO_POW_4,
780        },
781    )?;
782    validate_field(
783        LnavField::FitIntervalFlag,
784        p.fit_interval_flag,
785        FieldKind::Uint { bits: 1 },
786    )?;
787    validate_field(LnavField::Aodo, p.aodo, FieldKind::Uint { bits: 5 })?;
788
789    let m0 = pack_sint(p.m0.as_f64(), 32, TWO_POW_M31);
790    let ecc = pack_uint_scaled(p.eccentricity.as_f64(), 32, TWO_POW_M33);
791    let sqrt_a = pack_uint_scaled(p.sqrt_a.as_f64(), 32, TWO_POW_M19);
792
793    // Word 3: IODE(8) Crs(16).
794    let mut word3 = pack_uint(p.iode.as_i64_truncated(), 8);
795    word3.extend(pack_sint(p.crs.as_f64(), 16, TWO_POW_M5));
796    // Word 4: Delta-n(16) M0-MSB(8).
797    let mut word4 = pack_sint(p.delta_n.as_f64(), 16, TWO_POW_M43);
798    word4.extend_from_slice(&m0[0..8]);
799    // Word 5: M0-LSB(24).
800    let word5 = m0[8..32].to_vec();
801    // Word 6: Cuc(16) e-MSB(8).
802    let mut word6 = pack_sint(p.cuc.as_f64(), 16, TWO_POW_M29);
803    word6.extend_from_slice(&ecc[0..8]);
804    // Word 7: e-LSB(24).
805    let word7 = ecc[8..32].to_vec();
806    // Word 8: Cus(16) sqrtA-MSB(8).
807    let mut word8 = pack_sint(p.cus.as_f64(), 16, TWO_POW_M29);
808    word8.extend_from_slice(&sqrt_a[0..8]);
809    // Word 9: sqrtA-LSB(24).
810    let word9 = sqrt_a[8..32].to_vec();
811    // Word 10: toe(16) fit(1) AODO(5) + 2 solved bits.
812    let mut word10 = pack_uint_scaled(p.toe.as_f64(), 16, TWO_POW_4);
813    word10.extend(pack_uint(p.fit_interval_flag.as_i64_truncated(), 1));
814    word10.extend(pack_uint(p.aodo.as_i64_truncated(), 5));
815
816    Ok(vec![
817        WordEntry::raw(word3),
818        WordEntry::raw(word4),
819        WordEntry::raw(word5),
820        WordEntry::raw(word6),
821        WordEntry::raw(word7),
822        WordEntry::raw(word8),
823        WordEntry::raw(word9),
824        WordEntry::solved(word10),
825    ])
826}
827
828fn decode_subframe2(p: &mut LnavDecoded, w: &[Vec<u8>]) {
829    let (word3, word4, word5, word6, word7, word8, word9, word10) =
830        (&w[0], &w[1], &w[2], &w[3], &w[4], &w[5], &w[6], &w[7]);
831
832    p.iode = bits_to_uint(slice(word3, 1, 8)) as i64;
833    p.crs = unpack_sint(slice(word3, 9, 16), TWO_POW_M5);
834    p.delta_n = unpack_sint(slice(word4, 1, 16), TWO_POW_M43);
835    let mut m0_bits = slice(word4, 17, 8).to_vec();
836    m0_bits.extend_from_slice(slice(word5, 1, 24));
837    p.m0 = unpack_sint(&m0_bits, TWO_POW_M31);
838    p.cuc = unpack_sint(slice(word6, 1, 16), TWO_POW_M29);
839    let mut ecc_bits = slice(word6, 17, 8).to_vec();
840    ecc_bits.extend_from_slice(slice(word7, 1, 24));
841    p.eccentricity = unpack_uint_scaled(&ecc_bits, TWO_POW_M33);
842    p.cus = unpack_sint(slice(word8, 1, 16), TWO_POW_M29);
843    let mut sqrt_a_bits = slice(word8, 17, 8).to_vec();
844    sqrt_a_bits.extend_from_slice(slice(word9, 1, 24));
845    p.sqrt_a = unpack_uint_scaled(&sqrt_a_bits, TWO_POW_M19);
846    p.toe = unpack_uint_scaled_int(slice(word10, 1, 16), TWO_POW_4);
847    p.fit_interval_flag = bits_to_uint(slice(word10, 17, 1)) as i64;
848    p.aodo = bits_to_uint(slice(word10, 18, 5)) as i64;
849}
850
851// --- Subframe 3 (ephemeris part 2), IS-GPS-200 Table 20-III -----------------
852
853fn subframe3_words(p: &LnavParams) -> Result<Vec<WordEntry>, LnavError> {
854    validate_field(
855        LnavField::Cic,
856        p.cic,
857        FieldKind::SintScaled {
858            bits: 16,
859            scale: TWO_POW_M29,
860        },
861    )?;
862    validate_field(
863        LnavField::Omega0,
864        p.omega0,
865        FieldKind::SintScaled {
866            bits: 32,
867            scale: TWO_POW_M31,
868        },
869    )?;
870    validate_field(
871        LnavField::Cis,
872        p.cis,
873        FieldKind::SintScaled {
874            bits: 16,
875            scale: TWO_POW_M29,
876        },
877    )?;
878    validate_field(
879        LnavField::I0,
880        p.i0,
881        FieldKind::SintScaled {
882            bits: 32,
883            scale: TWO_POW_M31,
884        },
885    )?;
886    validate_field(
887        LnavField::Crc,
888        p.crc,
889        FieldKind::SintScaled {
890            bits: 16,
891            scale: TWO_POW_M5,
892        },
893    )?;
894    validate_field(
895        LnavField::Omega,
896        p.omega,
897        FieldKind::SintScaled {
898            bits: 32,
899            scale: TWO_POW_M31,
900        },
901    )?;
902    validate_field(
903        LnavField::OmegaDot,
904        p.omega_dot,
905        FieldKind::SintScaled {
906            bits: 24,
907            scale: TWO_POW_M43,
908        },
909    )?;
910    validate_field(LnavField::Iode, p.iode, FieldKind::Uint { bits: 8 })?;
911    validate_field(
912        LnavField::Idot,
913        p.idot,
914        FieldKind::SintScaled {
915            bits: 14,
916            scale: TWO_POW_M43,
917        },
918    )?;
919
920    let omega0 = pack_sint(p.omega0.as_f64(), 32, TWO_POW_M31);
921    let i0 = pack_sint(p.i0.as_f64(), 32, TWO_POW_M31);
922    let omega = pack_sint(p.omega.as_f64(), 32, TWO_POW_M31);
923
924    // Word 3: Cic(16) OMEGA0-MSB(8).
925    let mut word3 = pack_sint(p.cic.as_f64(), 16, TWO_POW_M29);
926    word3.extend_from_slice(&omega0[0..8]);
927    // Word 4: OMEGA0-LSB(24).
928    let word4 = omega0[8..32].to_vec();
929    // Word 5: Cis(16) i0-MSB(8).
930    let mut word5 = pack_sint(p.cis.as_f64(), 16, TWO_POW_M29);
931    word5.extend_from_slice(&i0[0..8]);
932    // Word 6: i0-LSB(24).
933    let word6 = i0[8..32].to_vec();
934    // Word 7: Crc(16) omega-MSB(8).
935    let mut word7 = pack_sint(p.crc.as_f64(), 16, TWO_POW_M5);
936    word7.extend_from_slice(&omega[0..8]);
937    // Word 8: omega-LSB(24).
938    let word8 = omega[8..32].to_vec();
939    // Word 9: OMEGADOT(24).
940    let word9 = pack_sint(p.omega_dot.as_f64(), 24, TWO_POW_M43);
941    // Word 10: IODE(8) IDOT(14) + 2 solved bits.
942    let mut word10 = pack_uint(p.iode.as_i64_truncated(), 8);
943    word10.extend(pack_sint(p.idot.as_f64(), 14, TWO_POW_M43));
944
945    Ok(vec![
946        WordEntry::raw(word3),
947        WordEntry::raw(word4),
948        WordEntry::raw(word5),
949        WordEntry::raw(word6),
950        WordEntry::raw(word7),
951        WordEntry::raw(word8),
952        WordEntry::raw(word9),
953        WordEntry::solved(word10),
954    ])
955}
956
957fn decode_subframe3(p: &mut LnavDecoded, w: &[Vec<u8>]) {
958    let (word3, word4, word5, word6, word7, word8, word9, word10) =
959        (&w[0], &w[1], &w[2], &w[3], &w[4], &w[5], &w[6], &w[7]);
960
961    p.cic = unpack_sint(slice(word3, 1, 16), TWO_POW_M29);
962    let mut omega0_bits = slice(word3, 17, 8).to_vec();
963    omega0_bits.extend_from_slice(slice(word4, 1, 24));
964    p.omega0 = unpack_sint(&omega0_bits, TWO_POW_M31);
965    p.cis = unpack_sint(slice(word5, 1, 16), TWO_POW_M29);
966    let mut i0_bits = slice(word5, 17, 8).to_vec();
967    i0_bits.extend_from_slice(slice(word6, 1, 24));
968    p.i0 = unpack_sint(&i0_bits, TWO_POW_M31);
969    p.crc = unpack_sint(slice(word7, 1, 16), TWO_POW_M5);
970    let mut omega_bits = slice(word7, 17, 8).to_vec();
971    omega_bits.extend_from_slice(slice(word8, 1, 24));
972    p.omega = unpack_sint(&omega_bits, TWO_POW_M31);
973    p.omega_dot = unpack_sint(slice(word9, 1, 24), TWO_POW_M43);
974    p.idot = unpack_sint(slice(word10, 9, 14), TWO_POW_M43);
975}
976
977// --- TLM / HOW --------------------------------------------------------------
978
979fn tlm_data(tlm_message: i64, integrity: i64) -> Vec<u8> {
980    // IS-GPS-200 Section 20.3.3.1: preamble(8) message(14) integrity(1) reserved(1).
981    let mut bits = pack_uint(PREAMBLE as i64, 8);
982    bits.extend(pack_uint(tlm_message, 14));
983    bits.extend(pack_uint(integrity, 1));
984    bits.push(0);
985    bits
986}
987
988fn how_data(tow: i64, alert: i64, anti_spoof: i64, sf_id: i64) -> WordEntry {
989    // IS-GPS-200 Section 20.3.3.2: TOW(17) alert(1) A-S(1) SF-ID(3) + 2 solved.
990    let mut base = pack_uint(tow, 17);
991    base.extend(pack_uint(alert, 1));
992    base.extend(pack_uint(anti_spoof, 1));
993    base.extend(pack_uint(sf_id, 3));
994    base.extend(zeros(2));
995    WordEntry::solved(base)
996}
997
998/// Prepends the TLM and HOW header words to the eight data words of a subframe.
999fn prepend_headers(
1000    tlm: &[u8],
1001    opts: &LnavOptions,
1002    sf_id: i64,
1003    words: Vec<WordEntry>,
1004) -> Vec<WordEntry> {
1005    let how = how_data(
1006        opts.tow.as_i64_truncated(),
1007        opts.alert.as_i64_truncated(),
1008        opts.anti_spoof.as_i64_truncated(),
1009        sf_id,
1010    );
1011    let mut entries = Vec::with_capacity(words.len() + 2);
1012    entries.push(WordEntry::raw(tlm.to_vec()));
1013    entries.push(how);
1014    entries.extend(words);
1015    entries
1016}
1017
1018// --- word/subframe assembly with parity -------------------------------------
1019
1020/// Builds the 300-bit subframe from ten word entries (TLM, HOW, then words
1021/// 3..10), chaining parity through all ten words seeded with `D29* = D30* = 0`.
1022fn assemble_subframe(entries: &[WordEntry]) -> Result<Vec<u8>, LnavError> {
1023    let mut bits = Vec::with_capacity(SUBFRAME_LENGTH);
1024    let (mut d29_prev, mut d30_prev) = (0u8, 0u8);
1025
1026    for entry in entries {
1027        let data = if entry.solve {
1028            solve_tbits(&entry.data, d29_prev, d30_prev)?
1029        } else {
1030            pad24(&entry.data)
1031        };
1032        let source = pad24(&data);
1033        let par = parity(&source, d29_prev, d30_prev)?;
1034        for b in &source {
1035            bits.push(b ^ d30_prev);
1036        }
1037        bits.extend_from_slice(&par);
1038        d29_prev = par[4];
1039        d30_prev = par[5];
1040    }
1041
1042    Ok(bits)
1043}
1044
1045/// Solve the two trailing data bits (positions 23, 24) so `D29 = D30 = 0`.
1046fn solve_tbits(data24: &[u8], d29_prev: u8, d30_prev: u8) -> Result<Vec<u8>, LnavError> {
1047    let mut base = pad24(data24);
1048    base[22] = 0;
1049    base[23] = 0;
1050    let par = parity(&base, d29_prev, d30_prev)?;
1051    let d24 = par[4];
1052    let d23 = par[5] ^ d24;
1053    base[22] = d23;
1054    base[23] = d24;
1055    Ok(base)
1056}
1057
1058fn pad24(bits: &[u8]) -> Vec<u8> {
1059    let mut out = bits.to_vec();
1060    out.resize(24, 0);
1061    out
1062}
1063
1064fn verify_subframe(bits: &[u8], sf: u8) -> Result<(), LnavError> {
1065    if bits.len() != SUBFRAME_LENGTH {
1066        return Err(LnavError::BadSubframeLength { subframe: sf });
1067    }
1068
1069    let (mut d29_prev, mut d30_prev) = (0u8, 0u8);
1070    for (idx, word) in bits.chunks(WORD_LENGTH).enumerate() {
1071        if parity_valid(word, d29_prev, d30_prev) {
1072            d29_prev = word[28];
1073            d30_prev = word[29];
1074        } else {
1075            return Err(LnavError::ParityFailed {
1076                subframe: sf,
1077                word: (idx + 1) as u8,
1078            });
1079        }
1080    }
1081    Ok(())
1082}
1083
1084/// Returns words 3..10 as 24-bit source words (with `D30*` uncomplemented).
1085fn source_words(bits: &[u8]) -> Vec<Vec<u8>> {
1086    let mut decoded = Vec::with_capacity(10);
1087    let mut d30_prev = 0u8;
1088    for word in bits.chunks(WORD_LENGTH) {
1089        let source: Vec<u8> = word[0..24].iter().map(|b| b ^ d30_prev).collect();
1090        d30_prev = word[29];
1091        decoded.push(source);
1092    }
1093    // Drop TLM (word 1) and HOW (word 2); keep words 3..10.
1094    decoded.split_off(2)
1095}
1096
1097// --- packing helpers --------------------------------------------------------
1098
1099fn pack_uint(value: i64, bits: u32) -> Vec<u8> {
1100    (0..bits).rev().map(|i| ((value >> i) & 1) as u8).collect()
1101}
1102
1103fn pack_uint_scaled(value: f64, bits: u32, scale: f64) -> Vec<u8> {
1104    pack_uint(round_half_away(value / scale), bits)
1105}
1106
1107fn pack_sint(value: f64, bits: u32, scale: f64) -> Vec<u8> {
1108    let int = round_half_away(value / scale);
1109    pack_twos_complement(int, bits)
1110}
1111
1112fn pack_twos_complement(int: i64, bits: u32) -> Vec<u8> {
1113    let mask = (1i64 << bits) - 1;
1114    pack_uint(int & mask, bits)
1115}
1116
1117fn bits_to_uint(bits: &[u8]) -> u64 {
1118    bits.iter().fold(0u64, |acc, &b| (acc << 1) | u64::from(b))
1119}
1120
1121fn unpack_uint_scaled(bits: &[u8], scale: f64) -> f64 {
1122    bits_to_uint(bits) as f64 * scale
1123}
1124
1125fn unpack_uint_scaled_int(bits: &[u8], scale: f64) -> i64 {
1126    // The scale-16 fields (toc, toe) recover an exact integer.
1127    round_half_away(bits_to_uint(bits) as f64 * scale)
1128}
1129
1130fn unpack_sint(bits: &[u8], scale: f64) -> f64 {
1131    bits_to_sint(bits) as f64 * scale
1132}
1133
1134fn bits_to_sint(bits: &[u8]) -> i64 {
1135    let n = bits.len() as u32;
1136    let raw = bits_to_uint(bits) as i64;
1137    if raw & (1i64 << (n - 1)) == 0 {
1138        raw
1139    } else {
1140        raw - (1i64 << n)
1141    }
1142}
1143
1144fn zeros(n: usize) -> Vec<u8> {
1145    vec![0u8; n]
1146}
1147
1148/// 1-based slice (`start` is the IS-GPS-200 bit position within the 24-bit word).
1149fn slice(word: &[u8], start_1based: usize, len: usize) -> &[u8] {
1150    &word[start_1based - 1..start_1based - 1 + len]
1151}
1152
1153fn xor(bits: &[u8]) -> u8 {
1154    bits.iter().fold(0u8, |acc, &b| acc ^ b)
1155}
1156
1157#[cfg(test)]
1158mod tests;
sidereon_core/navigation/lnav.rs

sidereon_core/navigation/
lnav.rs
