rtcm3 0.1.0

no_std, no_alloc parser and encoder for RTCM3 GNSS RTK correction messages
Documentation
//! GPS Full Pseudoranges and `PhaseRanges` plus CNR (high resolution)
//! (message type 1077): the actual GPS observation data (pseudorange,
//! carrier phase, Doppler, signal strength) an RTK rover needs, unlike
//! [`super::StationArp`] or [`super::GlonassBias`], which only carry
//! metadata about the correction stream itself.
//!
//! Unlike every other message in this crate, an MSM message's structure
//! depends on data read earlier in the same message: a 64-bit satellite
//! mask and 32-bit signal mask say which satellites/signal types are
//! present, then a variable-length cell mask (one bit per satellite/signal
//! *pair*) says which of those combinations actually have data - and only
//! after reading all three masks does it become known how many times the
//! per-satellite and per-cell field groups repeat. Bit widths for every
//! field below were cross-checked against two independent
//! implementations: `pyrtcm`'s `RTCM_DATA_FIELDS`/`RTCM_PAYLOADS_GET_MSM`
//! tables and RTKLIB's `decode_msm7` (`src/rtcm3.c`) - the two agreed
//! exactly, which is what caught that a real captured MSM7 frame can
//! (and did, in the fixture below) carry trailing padding bits beyond
//! what the header's satellite/cell counts strictly require: RTKLIB's own
//! length check is `>=`, not `==`, so this crate doesn't require an exact
//! match either - decoding simply stops once every named field is read.

use crate::bits::{BitReader, BitWriter};
use crate::error::BitError;

/// This message's type (DF002).
pub const TYPE: u16 = 1077;

/// Largest number of satellites a single MSM message can describe (the
/// mask is 64 bits, one per possible satellite slot).
pub const MAX_SATELLITES: usize = 64;
/// Largest number of satellite/signal cells a single MSM message can
/// describe (`num_satellites * num_signals` is capped at 64 by the
/// protocol itself, since the cell mask is read as a single 64-bit value).
pub const MAX_CELLS: usize = 64;

/// One satellite's data block (repeated `num_satellites` times). Only
/// entries at index `< num_satellites` are meaningful; the rest are
/// zeroed, unused padding.
#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
pub struct SatelliteData {
    /// Integer milliseconds in the satellite's rough range (DF397), `255`
    /// means "no data".
    pub rough_range_ms_raw: u8,
    /// Extended satellite information (DF419, reserved for GPS).
    pub extended_info_raw: u8,
    /// Rough range modulo 1 millisecond, raw units of 2^-10 ms (DF398).
    pub rough_range_mod_raw: u16,
    /// Rough phase range rate, m/s (DF399), `-8192` means "no data".
    pub rough_phase_range_rate_raw: i16,
}

/// One satellite/signal cell's data block (repeated `num_cells` times).
/// Only entries at index `< num_cells` are meaningful; the rest are
/// zeroed, unused padding.
#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
pub struct CellData {
    /// Fine pseudorange, raw units of 2^-29 ms (DF405), `-524288` means
    /// "no data".
    pub fine_pseudorange_raw: i32,
    /// Fine phase range, raw units of 2^-31 ms (DF406), `-8388608` means
    /// "no data".
    pub fine_phase_range_raw: i32,
    /// Phase range lock time indicator, extended range/resolution (DF407).
    pub lock_time_raw: u16,
    /// Half-cycle ambiguity indicator (DF420).
    pub half_cycle_ambiguity: bool,
    /// Carrier-to-noise ratio, raw units of 2^-4 dB-Hz (DF408).
    pub cnr_raw: u16,
    /// Fine phase range rate, raw units of 0.0001 m/s (DF404), `-16384`
    /// means "no data".
    pub fine_phase_range_rate_raw: i16,
}

/// A decoded GPS MSM7 message.
///
/// `num_satellites`/`num_signals`/`num_cells` aren't stored fields: they're
/// always the popcount of `satellite_mask`/`signal_mask`/`cell_mask`, so
/// there's no way for a stored count to disagree with the mask that
/// actually determines it.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct GpsMsm7 {
    /// Reference station ID (DF003).
    pub reference_station_id: u16,
    /// GPS epoch time (time of week), raw milliseconds (DF004).
    pub gps_epoch_time_tow_ms: u32,
    /// Multiple message bit: more MSM messages follow for this epoch (DF393).
    pub multiple_message_bit: bool,
    /// Issue of data station (DF409).
    pub issue_of_data_station: u8,
    /// Clock steering indicator (DF411).
    pub clock_steering_indicator: u8,
    /// External clock indicator (DF412).
    pub external_clock_indicator: u8,
    /// GNSS divergence-free smoothing indicator (DF417).
    pub smoothing_indicator: bool,
    /// GNSS smoothing interval, raw encoding (DF418).
    pub smoothing_interval_raw: u8,
    /// Satellite mask (DF394): bit `n` (0-indexed) set means satellite
    /// PRN `n + 1` is present.
    pub satellite_mask: u64,
    /// Signal mask (DF395): bit `n` (0-indexed) set means signal type
    /// `n + 1` is present.
    pub signal_mask: u32,
    /// Cell mask (DF396): bit `i * num_signals + j` set means the `i`-th
    /// present satellite has data for the `j`-th present signal type.
    pub cell_mask: u64,
    /// Per-satellite data blocks, indexed in mask-bit order (only the
    /// first `num_satellites()` entries are meaningful).
    pub satellites: [SatelliteData; MAX_SATELLITES],
    /// Per-cell data blocks, indexed in mask-bit order (only the first
    /// `num_cells()` entries are meaningful).
    pub cells: [CellData; MAX_CELLS],
}

impl GpsMsm7 {
    /// Number of satellites present (popcount of `satellite_mask`).
    #[must_use]
    pub const fn num_satellites(&self) -> u32 {
        self.satellite_mask.count_ones()
    }

    /// Number of signal types present (popcount of `signal_mask`).
    #[must_use]
    pub const fn num_signals(&self) -> u32 {
        self.signal_mask.count_ones()
    }

    /// Number of satellite/signal cells present (popcount of `cell_mask`).
    #[must_use]
    pub const fn num_cells(&self) -> u32 {
        self.cell_mask.count_ones()
    }

    pub(crate) fn decode(r: &mut BitReader<'_>) -> Result<Self, BitError> {
        let reference_station_id = r.read_u16(12)?;
        let gps_epoch_time_tow_ms = r.read_u32(30)?;
        let multiple_message_bit = r.read_bool()?;
        let issue_of_data_station = r.read_u8(3)?;
        r.skip(7)?; // reserved
        let clock_steering_indicator = r.read_u8(2)?;
        let external_clock_indicator = r.read_u8(2)?;
        let smoothing_indicator = r.read_bool()?;
        let smoothing_interval_raw = r.read_u8(3)?;
        let satellite_mask = r.read_u64(64)?;
        let signal_mask = r.read_u32(32)?;

        let num_satellites = satellite_mask.count_ones();
        let num_signals = signal_mask.count_ones();
        let cell_mask_bits = num_satellites * num_signals;
        let cell_mask = if cell_mask_bits == 0 {
            0
        } else {
            r.read_u64(cell_mask_bits)?
        };
        let num_cells = cell_mask.count_ones();

        let mut satellites = [SatelliteData::default(); MAX_SATELLITES];
        for sat in satellites.iter_mut().take(num_satellites as usize) {
            sat.rough_range_ms_raw = r.read_u8(8)?;
        }
        for sat in satellites.iter_mut().take(num_satellites as usize) {
            sat.extended_info_raw = r.read_u8(4)?;
        }
        for sat in satellites.iter_mut().take(num_satellites as usize) {
            sat.rough_range_mod_raw = r.read_u16(10)?;
        }
        #[allow(
            clippy::cast_possible_truncation,
            reason = "DF399 is defined as a 14-bit field, so read_i32(14) always fits in i16"
        )]
        for sat in satellites.iter_mut().take(num_satellites as usize) {
            sat.rough_phase_range_rate_raw = r.read_i32(14)? as i16;
        }

        let mut cells = [CellData::default(); MAX_CELLS];
        for cell in cells.iter_mut().take(num_cells as usize) {
            cell.fine_pseudorange_raw = r.read_i32(20)?;
        }
        for cell in cells.iter_mut().take(num_cells as usize) {
            cell.fine_phase_range_raw = r.read_i32(24)?;
        }
        for cell in cells.iter_mut().take(num_cells as usize) {
            cell.lock_time_raw = r.read_u16(10)?;
        }
        for cell in cells.iter_mut().take(num_cells as usize) {
            cell.half_cycle_ambiguity = r.read_bool()?;
        }
        for cell in cells.iter_mut().take(num_cells as usize) {
            cell.cnr_raw = r.read_u16(10)?;
        }
        #[allow(
            clippy::cast_possible_truncation,
            reason = "DF404 is defined as a 15-bit field, so read_i32(15) always fits in i16"
        )]
        for cell in cells.iter_mut().take(num_cells as usize) {
            cell.fine_phase_range_rate_raw = r.read_i32(15)? as i16;
        }

        Ok(Self {
            reference_station_id,
            gps_epoch_time_tow_ms,
            multiple_message_bit,
            issue_of_data_station,
            clock_steering_indicator,
            external_clock_indicator,
            smoothing_indicator,
            smoothing_interval_raw,
            satellite_mask,
            signal_mask,
            cell_mask,
            satellites,
            cells,
        })
    }

    pub(crate) fn encode(&self, w: &mut BitWriter<'_>) -> Result<(), BitError> {
        w.write_u32(u32::from(self.reference_station_id), 12)?;
        w.write_u32(self.gps_epoch_time_tow_ms, 30)?;
        w.write_bool(self.multiple_message_bit)?;
        w.write_u32(u32::from(self.issue_of_data_station), 3)?;
        w.skip(7)?; // reserved
        w.write_u32(u32::from(self.clock_steering_indicator), 2)?;
        w.write_u32(u32::from(self.external_clock_indicator), 2)?;
        w.write_bool(self.smoothing_indicator)?;
        w.write_u32(u32::from(self.smoothing_interval_raw), 3)?;
        w.write_u64(self.satellite_mask, 64)?;
        w.write_u32(self.signal_mask, 32)?;

        let num_satellites = self.num_satellites();
        let num_cells = self.num_cells();
        let cell_mask_bits = num_satellites * self.num_signals();
        if cell_mask_bits > 0 {
            w.write_u64(self.cell_mask, cell_mask_bits)?;
        }

        for sat in self.satellites.iter().take(num_satellites as usize) {
            w.write_u32(u32::from(sat.rough_range_ms_raw), 8)?;
        }
        for sat in self.satellites.iter().take(num_satellites as usize) {
            w.write_u32(u32::from(sat.extended_info_raw), 4)?;
        }
        for sat in self.satellites.iter().take(num_satellites as usize) {
            w.write_u32(u32::from(sat.rough_range_mod_raw), 10)?;
        }
        for sat in self.satellites.iter().take(num_satellites as usize) {
            w.write_i32(i32::from(sat.rough_phase_range_rate_raw), 14)?;
        }

        for cell in self.cells.iter().take(num_cells as usize) {
            w.write_i32(cell.fine_pseudorange_raw, 20)?;
        }
        for cell in self.cells.iter().take(num_cells as usize) {
            w.write_i32(cell.fine_phase_range_raw, 24)?;
        }
        for cell in self.cells.iter().take(num_cells as usize) {
            w.write_u32(u32::from(cell.lock_time_raw), 10)?;
        }
        for cell in self.cells.iter().take(num_cells as usize) {
            w.write_bool(cell.half_cycle_ambiguity)?;
        }
        for cell in self.cells.iter().take(num_cells as usize) {
            w.write_u32(u32::from(cell.cnr_raw), 10)?;
        }
        for cell in self.cells.iter().take(num_cells as usize) {
            w.write_i32(i32::from(cell.fine_phase_range_rate_raw), 15)?;
        }

        Ok(())
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    fn hex_decode<const N: usize>(hex: &str) -> [u8; N] {
        let mut out = [0u8; N];
        for (i, chunk) in hex.as_bytes().chunks(2).enumerate() {
            let s = core::str::from_utf8(chunk).unwrap();
            out[i] = u8::from_str_radix(s, 16).unwrap();
        }
        out
    }

    // Real MT1077 payload (269 bytes) from a captured stream
    // (pygpsdata-RTCM3.log in pyrtcm's own test corpus), extracted
    // programmatically (not hand-sliced) from the frame and cross-checked
    // field-by-field against pyrtcm's own decode, plus independently
    // against RTKLIB's decode_msm7 (src/rtcm3.c) for every field's bit
    // width. 10 satellites, 2 signals, 17 cells.
    const REAL_MT1077_PAYLOAD_HEX: &str = "43500030ab88a600000547580200000000200080007f7f5a5a5a8a421a825a925a3800000000000d11e1a474663a66e34c2cb17e9df687afa0eeff981428422141fca9fa58960a894b9197311963b604a9e146396cc38e65d8e1aa73a51f3fe979639798c61f6029c9dc6ba58ebc5a02535082597506657806590078104ef8540005b0fa8390a28389dcfcf16c7cfe577e5cdb7e681c06c382072307fae6707af003aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa93aaaaaaaa0000b4260ac2709c250b42e0b8270882d0981f0b42e0a42df8dc16beff7deb7faf0181327f52feaa24ae499225404b819ecb559de73bc10000000000000000000000000000000000000000000000000000000000000";

    #[test]
    fn decodes_a_real_captured_frame() {
        let payload: [u8; 269] = hex_decode(REAL_MT1077_PAYLOAD_HEX);
        let mut r = BitReader::new(&payload);
        assert_eq!(r.read_u16(12).unwrap(), 1077);
        let m = GpsMsm7::decode(&mut r).unwrap();

        assert_eq!(m.reference_station_id, 0);
        assert_eq!(m.gps_epoch_time_tow_ms, 204_137_001);
        assert!(m.multiple_message_bit);
        assert_eq!(m.satellite_mask, 0x0A8E_B004_0000_0000);
        assert_eq!(m.signal_mask, 0x4001_0000);
        assert_eq!(m.cell_mask, 0x000F_EFEB);
        assert_eq!(m.num_satellites(), 10);
        assert_eq!(m.num_signals(), 2);
        assert_eq!(m.num_cells(), 17);

        let sat0 = m.satellites[0];
        assert_eq!(sat0.rough_range_ms_raw, 75);
        assert_eq!(sat0.extended_info_raw, 0);
        assert_eq!(sat0.rough_range_mod_raw, 6);
        assert_eq!(sat0.rough_phase_range_rate_raw, -178);

        let cell0 = m.cells[0];
        assert_eq!(cell0.fine_pseudorange_raw, 76_821);
        assert_eq!(cell0.fine_phase_range_raw, 304_801);
        assert_eq!(cell0.lock_time_raw, 341);
        assert!(!cell0.half_cycle_ambiguity);
        assert_eq!(cell0.cnr_raw, 720);
        assert_eq!(cell0.fine_phase_range_rate_raw, -9231);
    }

    #[test]
    fn encode_then_decode_round_trips_the_real_frame() {
        let payload: [u8; 269] = hex_decode(REAL_MT1077_PAYLOAD_HEX);
        let mut r = BitReader::new(&payload);
        r.read_u16(12).unwrap();
        let original = GpsMsm7::decode(&mut r).unwrap();

        // Only re-encode the bits this message actually needs (header +
        // mask-driven repeat groups), not the real frame's full 269 bytes
        // (which, per the module docs, carries some trailing padding this
        // crate doesn't reproduce).
        let mut buf = [0u8; 269];
        let mut w = BitWriter::new(&mut buf);
        original.encode(&mut w).unwrap();

        let mut r2 = BitReader::new(&buf);
        let decoded = GpsMsm7::decode(&mut r2).unwrap();
        assert_eq!(decoded, original);
    }

    #[test]
    fn round_trips_a_minimal_single_satellite_single_cell_message() {
        let mut satellites = [SatelliteData::default(); MAX_SATELLITES];
        satellites[0] = SatelliteData {
            rough_range_ms_raw: 80,
            extended_info_raw: 3,
            rough_range_mod_raw: 512,
            rough_phase_range_rate_raw: -100,
        };
        let mut cells = [CellData::default(); MAX_CELLS];
        cells[0] = CellData {
            fine_pseudorange_raw: -1000,
            fine_phase_range_raw: 5000,
            lock_time_raw: 99,
            half_cycle_ambiguity: true,
            cnr_raw: 400,
            fine_phase_range_rate_raw: 123,
        };
        let original = GpsMsm7 {
            reference_station_id: 7,
            gps_epoch_time_tow_ms: 12345,
            multiple_message_bit: false,
            issue_of_data_station: 2,
            clock_steering_indicator: 1,
            external_clock_indicator: 0,
            smoothing_indicator: true,
            smoothing_interval_raw: 5,
            satellite_mask: 1, // satellite 1 only
            signal_mask: 1,    // signal 1 only
            cell_mask: 1,      // the one possible cell
            satellites,
            cells,
        };
        let mut buf = [0u8; 40];
        let mut w = BitWriter::new(&mut buf);
        original.encode(&mut w).unwrap();

        let mut r = BitReader::new(&buf);
        let decoded = GpsMsm7::decode(&mut r).unwrap();
        assert_eq!(decoded, original);
    }
}