use std::collections::BTreeMap;
use astrodynamics::time::model::{Instant, InstantRepr, JulianDateSplit, TimeScale};
use crate::frame::{ItrfPositionM, ItrfVelocityMS};
use crate::id::{GnssSatelliteId, GnssSystem};
use crate::parse::{raw_field as field, raw_field_from as field_from};
use crate::{Error, Result};
const MISSING_POSITION_KM: f64 = 0.0;
const BAD_CLOCK_US: f64 = 999_999.999_999;
const KM_TO_M: f64 = 1_000.0;
const US_TO_S: f64 = 1.0e-6;
const DM_S_TO_M_S: f64 = 1.0e-1;
const CLOCK_RATE_TO_S_PER_S: f64 = 1.0e-10;
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum Sp3Version {
A,
B,
C,
D,
}
impl Sp3Version {
fn from_char(c: char) -> Result<Self> {
match c {
'a' | 'A' => Ok(Sp3Version::A),
'b' | 'B' => Ok(Sp3Version::B),
'c' | 'C' => Ok(Sp3Version::C),
'd' | 'D' => Ok(Sp3Version::D),
other => Err(Error::Parse(format!("unknown SP3 version '{other}'"))),
}
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum Sp3DataType {
Position,
Velocity,
}
impl Sp3DataType {
fn from_char(c: char) -> Result<Self> {
match c {
'P' => Ok(Sp3DataType::Position),
'V' => Ok(Sp3DataType::Velocity),
other => Err(Error::Parse(format!("unknown SP3 data type '{other}'"))),
}
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
pub struct Sp3Flags {
pub clock_event: bool,
pub clock_predicted: bool,
pub maneuver: bool,
pub orbit_predicted: bool,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Sp3State {
pub position: ItrfPositionM,
pub clock_s: Option<f64>,
pub velocity: Option<ItrfVelocityMS>,
pub clock_rate_s_s: Option<f64>,
pub flags: Sp3Flags,
}
#[derive(Debug, Clone, PartialEq)]
pub struct Sp3Header {
pub version: Sp3Version,
pub data_type: Sp3DataType,
pub num_epochs: u64,
pub coordinate_system: String,
pub orbit_type: String,
pub agency: String,
pub gnss_week: u32,
pub seconds_of_week: f64,
pub epoch_interval_s: f64,
pub mjd: u32,
pub mjd_fraction: f64,
pub time_scale: TimeScale,
pub satellites: Vec<GnssSatelliteId>,
}
#[derive(Debug, Clone, PartialEq)]
pub struct Sp3 {
pub header: Sp3Header,
pub epochs: Vec<Instant>,
states: Vec<BTreeMap<GnssSatelliteId, Sp3State>>,
interp_raw: Vec<BTreeMap<GnssSatelliteId, RawNode>>,
pub comments: Vec<String>,
}
#[derive(Debug, Clone, Copy, PartialEq)]
struct RawNode {
km: [f64; 3],
clock_us: Option<f64>,
clock_event: bool,
}
impl Sp3 {
pub fn parse(bytes: &[u8]) -> Result<Self> {
let text = std::str::from_utf8(bytes)
.map_err(|e| Error::Parse(format!("SP3 is not valid UTF-8: {e}")))?;
Self::parse_str(text)
}
pub fn parse_str(text: &str) -> Result<Self> {
let mut parser = Parser::new();
for raw in text.lines() {
parser.feed(raw)?;
}
parser.finish()
}
pub fn satellites(&self) -> &[GnssSatelliteId] {
&self.header.satellites
}
pub fn epoch_count(&self) -> usize {
self.epochs.len()
}
pub fn state(&self, sat: GnssSatelliteId, epoch_index: usize) -> Result<Sp3State> {
let per_epoch = self.states.get(epoch_index).ok_or(Error::EpochOutOfRange)?;
per_epoch
.get(&sat)
.copied()
.ok_or(Error::UnknownSatellite(sat))
}
pub fn states_at(&self, epoch_index: usize) -> Result<&BTreeMap<GnssSatelliteId, Sp3State>> {
self.states.get(epoch_index).ok_or(Error::EpochOutOfRange)
}
}
impl core::str::FromStr for Sp3 {
type Err = Error;
fn from_str(s: &str) -> Result<Self> {
Self::parse_str(s)
}
}
#[cfg(test)]
impl Sp3 {
pub(crate) fn interp_node_for_test(
&self,
sat: GnssSatelliteId,
epoch_index: usize,
) -> Option<([f64; 3], Option<f64>)> {
self.interp_raw
.get(epoch_index)
.and_then(|m| m.get(&sat))
.map(|r| (r.km, r.clock_us))
}
}
fn time_scale_from_label(label: &str) -> Result<TimeScale> {
match label.trim() {
"GPS" => Ok(TimeScale::Gpst),
"GAL" => Ok(TimeScale::Gst),
"BDT" | "BDS" => Ok(TimeScale::Bdt),
"TAI" => Ok(TimeScale::Tai),
"UTC" => Ok(TimeScale::Utc),
other => Err(Error::Parse(format!(
"unsupported SP3 time system '{other}'"
))),
}
}
fn civil_to_julian_split(
year: i64,
month: i64,
day: i64,
hour: i64,
minute: i64,
seconds: f64,
) -> JulianDateSplit {
let a = (14 - month) / 12;
let y = year + 4800 - a;
let m = month + 12 * a - 3;
let jdn = day + (153 * m + 2) / 5 + 365 * y + y / 4 - y / 100 + y / 400 - 32_045;
let jd_whole = jdn as f64 - 0.5;
let day_seconds = hour as f64 * 3600.0 + minute as f64 * 60.0 + seconds;
let fraction = day_seconds / 86_400.0;
JulianDateSplit::new(jd_whole, fraction)
}
struct Parser {
version: Option<Sp3Version>,
data_type: Option<Sp3DataType>,
num_epochs: u64,
coordinate_system: String,
orbit_type: String,
agency: String,
gnss_week: u32,
seconds_of_week: f64,
epoch_interval_s: f64,
mjd: u32,
mjd_fraction: f64,
time_scale: Option<TimeScale>,
sat_list: Vec<GnssSatelliteId>,
pc_count: u32,
have_line1: bool,
have_line2: bool,
current_epoch: Option<Instant>,
epochs: Vec<Instant>,
states: Vec<BTreeMap<GnssSatelliteId, Sp3State>>,
interp_raw: Vec<BTreeMap<GnssSatelliteId, RawNode>>,
comments: Vec<String>,
done: bool,
}
impl Parser {
fn new() -> Self {
Self {
version: None,
data_type: None,
num_epochs: 0,
coordinate_system: String::new(),
orbit_type: String::new(),
agency: String::new(),
gnss_week: 0,
seconds_of_week: 0.0,
epoch_interval_s: 0.0,
mjd: 0,
mjd_fraction: 0.0,
time_scale: None,
sat_list: Vec::new(),
pc_count: 0,
have_line1: false,
have_line2: false,
current_epoch: None,
epochs: Vec::new(),
states: Vec::new(),
interp_raw: Vec::new(),
comments: Vec::new(),
done: false,
}
}
fn feed(&mut self, raw: &str) -> Result<()> {
if self.done {
return Ok(());
}
let line = raw.trim_end_matches(['\r', '\n']);
if line == "EOF" {
self.done = true;
return Ok(());
}
if line.starts_with("/*") {
if line.len() > 3 {
self.comments.push(line[3..].trim_end().to_string());
} else {
self.comments.push(String::new());
}
return Ok(());
}
if line.starts_with("##") {
self.parse_line2(line)?;
return Ok(());
}
if line.starts_with('#') {
self.parse_line1(line)?;
return Ok(());
}
if line.starts_with('+') {
self.parse_plus_line(line)?;
return Ok(());
}
if line.starts_with("%c") {
self.parse_pc_line(line)?;
return Ok(());
}
if line.starts_with("%f") || line.starts_with("%i") {
return Ok(());
}
if line.starts_with('*') {
self.parse_epoch_line(line)?;
return Ok(());
}
if line.starts_with('P') {
self.parse_position_line(line)?;
return Ok(());
}
if line.starts_with('V') {
self.parse_velocity_line(line)?;
return Ok(());
}
Ok(())
}
fn parse_line1(&mut self, line: &str) -> Result<()> {
if line.len() < 55 {
return Err(Error::Parse(format!(
"SP3 header line 1 too short: {line:?}"
)));
}
let chars: Vec<char> = line.chars().collect();
let version = Sp3Version::from_char(chars[1])?;
self.version = Some(version);
self.data_type = Some(Sp3DataType::from_char(chars[2])?);
if matches!(version, Sp3Version::A) {
self.time_scale = Some(TimeScale::Gpst);
}
self.num_epochs = field(line, 32, 40)
.trim()
.parse::<u64>()
.map_err(|_| Error::Parse(format!("SP3 num_epochs unparsable in {line:?}")))?;
self.coordinate_system = field(line, 45, 51).trim().to_string();
self.orbit_type = field(line, 51, 55).trim().to_string();
self.agency = field_from(line, 55).trim().to_string();
self.have_line1 = true;
Ok(())
}
fn parse_line2(&mut self, line: &str) -> Result<()> {
self.gnss_week = field(line, 3, 7)
.trim()
.parse::<u32>()
.map_err(|_| Error::Parse(format!("SP3 GNSS week unparsable in {line:?}")))?;
self.seconds_of_week = field(line, 8, 23)
.trim()
.parse::<f64>()
.map_err(|_| Error::Parse(format!("SP3 seconds-of-week unparsable in {line:?}")))?;
self.epoch_interval_s = field(line, 24, 38)
.trim()
.parse::<f64>()
.map_err(|_| Error::Parse(format!("SP3 epoch interval unparsable in {line:?}")))?;
self.mjd = field(line, 39, 44)
.trim()
.parse::<u32>()
.map_err(|_| Error::Parse(format!("SP3 MJD unparsable in {line:?}")))?;
self.mjd_fraction = field_from(line, 45).trim().parse::<f64>().unwrap_or(0.0);
self.have_line2 = true;
Ok(())
}
fn parse_plus_line(&mut self, line: &str) -> Result<()> {
if line.starts_with("++") {
return Ok(());
}
let chars: Vec<char> = line.chars().collect();
let mut col = 9;
while col + 3 <= chars.len() {
let token: String = chars[col..col + 3].iter().collect();
let trimmed = token.trim();
if trimmed.is_empty() || trimmed == "0" {
col += 3;
continue;
}
if let Some(id) = parse_sv_token(&token, self.version) {
if !self.sat_list.contains(&id) {
self.sat_list.push(id);
}
}
col += 3;
}
Ok(())
}
fn parse_pc_line(&mut self, line: &str) -> Result<()> {
if self.pc_count == 0 {
if matches!(self.version, Some(Sp3Version::A)) {
self.time_scale = Some(TimeScale::Gpst);
} else if line.len() >= 12 {
let label = field(line, 9, 12);
let trimmed = label.trim();
if trimmed.is_empty() {
return Err(Error::Parse(format!(
"SP3 %c time system is blank in {line:?}"
)));
}
self.time_scale = Some(time_scale_from_label(label)?);
} else {
return Err(Error::Parse(format!(
"SP3 %c descriptor too short to carry a time system: {line:?}"
)));
}
}
self.pc_count += 1;
Ok(())
}
fn parse_epoch_line(&mut self, line: &str) -> Result<()> {
let scale = self.time_scale.ok_or_else(|| {
Error::Parse("SP3 epoch encountered with no time system (missing %c descriptor)".into())
})?;
let body = &line[1..];
let mut it = body.split_whitespace();
let year: i64 = next_field(&mut it, "epoch year")?;
let month: i64 = next_field(&mut it, "epoch month")?;
let day: i64 = next_field(&mut it, "epoch day")?;
let hour: i64 = next_field(&mut it, "epoch hour")?;
let minute: i64 = next_field(&mut it, "epoch minute")?;
let seconds: f64 = next_field(&mut it, "epoch seconds")?;
let split = civil_to_julian_split(year, month, day, hour, minute, seconds);
let epoch = Instant {
scale,
repr: InstantRepr::JulianDate(split),
};
self.epochs.push(epoch);
self.states.push(BTreeMap::new());
self.interp_raw.push(BTreeMap::new());
self.current_epoch = Some(epoch);
Ok(())
}
fn parse_position_line(&mut self, line: &str) -> Result<()> {
if self.current_epoch.is_none() {
return Err(Error::Parse(
"SP3 position record before any epoch line".into(),
));
}
if line.len() < 46 {
return Ok(());
}
let token = field(line, 1, 4);
let sat = parse_sv_token(token, self.version)
.ok_or_else(|| Error::Parse(format!("SP3 unparsable satellite token {token:?}")))?;
let x_km = parse_coord(line, 4, 18)?;
let y_km = parse_coord(line, 18, 32)?;
let z_km = parse_coord(line, 32, 46)?;
if x_km == MISSING_POSITION_KM && y_km == MISSING_POSITION_KM && z_km == MISSING_POSITION_KM
{
return Ok(());
}
let clock_us = parse_clock_us(line)?;
let clock_s = clock_us.map(|us| us * US_TO_S);
let flags = parse_flags(line);
let position = ItrfPositionM::new(x_km * KM_TO_M, y_km * KM_TO_M, z_km * KM_TO_M);
let state = Sp3State {
position,
clock_s,
velocity: None,
clock_rate_s_s: None,
flags,
};
let idx = self.states.len() - 1;
self.states[idx].insert(sat, state);
self.interp_raw[idx].insert(
sat,
RawNode {
km: [x_km, y_km, z_km],
clock_us,
clock_event: flags.clock_event,
},
);
Ok(())
}
fn parse_velocity_line(&mut self, line: &str) -> Result<()> {
if self.current_epoch.is_none() {
return Err(Error::Parse(
"SP3 velocity record before any epoch line".into(),
));
}
if line.len() < 46 {
return Ok(());
}
let token = field(line, 1, 4);
let sat = parse_sv_token(token, self.version)
.ok_or_else(|| Error::Parse(format!("SP3 unparsable satellite token {token:?}")))?;
let vx_dm_s = parse_coord(line, 4, 18)?;
let vy_dm_s = parse_coord(line, 18, 32)?;
let vz_dm_s = parse_coord(line, 32, 46)?;
let velocity = ItrfVelocityMS::new(
vx_dm_s * DM_S_TO_M_S,
vy_dm_s * DM_S_TO_M_S,
vz_dm_s * DM_S_TO_M_S,
);
let clock_rate_s_s = parse_clock_us(line)?.map(|rate| rate * CLOCK_RATE_TO_S_PER_S);
let idx = self.states.len() - 1;
match self.states[idx].get_mut(&sat) {
Some(state) => {
state.velocity = Some(velocity);
state.clock_rate_s_s = clock_rate_s_s;
}
None => {
}
}
Ok(())
}
fn finish(self) -> Result<Sp3> {
if !self.have_line1 {
return Err(Error::Parse("SP3 missing header line 1".into()));
}
if !self.have_line2 {
return Err(Error::Parse("SP3 missing header line 2".into()));
}
let version = self
.version
.ok_or_else(|| Error::Parse("SP3 version not determined".into()))?;
let data_type = self
.data_type
.ok_or_else(|| Error::Parse("SP3 data type not determined".into()))?;
let time_scale = self.time_scale.ok_or_else(|| {
Error::Parse(
"SP3 time system not determined (missing/short/blank %c descriptor)".into(),
)
})?;
let header = Sp3Header {
version,
data_type,
num_epochs: self.num_epochs,
coordinate_system: self.coordinate_system,
orbit_type: self.orbit_type,
agency: self.agency,
gnss_week: self.gnss_week,
seconds_of_week: self.seconds_of_week,
epoch_interval_s: self.epoch_interval_s,
mjd: self.mjd,
mjd_fraction: self.mjd_fraction,
time_scale,
satellites: self.sat_list,
};
Ok(Sp3 {
header,
epochs: self.epochs,
states: self.states,
interp_raw: self.interp_raw,
comments: self.comments,
})
}
}
fn parse_coord(line: &str, start: usize, end: usize) -> Result<f64> {
let raw = field(line, start, end).trim();
raw.parse::<f64>()
.map_err(|_| Error::Parse(format!("SP3 coordinate {raw:?} unparsable in {line:?}")))
}
fn parse_clock_us(line: &str) -> Result<Option<f64>> {
if line.len() <= 46 {
return Ok(None);
}
let raw = field(line, 46, 60).trim();
if raw.is_empty() {
return Ok(None);
}
let value = raw
.parse::<f64>()
.map_err(|_| Error::Parse(format!("SP3 clock {raw:?} unparsable in {line:?}")))?;
if value.abs() >= BAD_CLOCK_US {
return Ok(None);
}
Ok(Some(value))
}
fn parse_flags(line: &str) -> Sp3Flags {
let at = |col: usize, want: char| -> bool {
line.as_bytes().get(col).map(|&b| b as char) == Some(want)
};
Sp3Flags {
clock_event: at(74, 'E'),
clock_predicted: at(75, 'P'),
maneuver: at(78, 'M'),
orbit_predicted: at(79, 'P'),
}
}
fn parse_sv_token(token: &str, version: Option<Sp3Version>) -> Option<GnssSatelliteId> {
let token = token.trim();
if token.is_empty() {
return None;
}
let first = token.chars().next()?;
if first.is_ascii_digit() {
if matches!(version, Some(Sp3Version::A)) || version.is_none() {
let prn = token.parse::<u8>().ok()?;
return Some(GnssSatelliteId::new(GnssSystem::Gps, prn));
}
return None;
}
let system = GnssSystem::from_letter(first)?;
let prn = token[first.len_utf8()..].trim().parse::<u8>().ok()?;
Some(GnssSatelliteId::new(system, prn))
}
fn next_field<T: std::str::FromStr>(
it: &mut std::str::SplitWhitespace<'_>,
what: &str,
) -> Result<T> {
let tok = it
.next()
.ok_or_else(|| Error::Parse(format!("SP3 missing {what}")))?;
tok.parse::<T>()
.map_err(|_| Error::Parse(format!("SP3 {what} {tok:?} unparsable")))
}
mod combine;
mod interp;
mod write;
pub use combine::{
align_clock_reference, clock_reference_offset, merge, ClockReferenceOffset, MergeCombine,
MergeFlag, MergeOptions, MergeReport,
};
#[cfg(test)]
mod tests;