#![cfg_attr(docrs, feature(doc_cfg))]
extern crate gnss_rs as gnss;
use gnss::prelude::{Constellation, SV};
use hifitime::{Duration, Epoch, TimeScale};
use std::collections::BTreeMap;
use std::str::FromStr;
use thiserror::Error;
#[cfg(test)]
mod tests;
mod header;
mod merge;
mod position;
mod reader;
mod velocity;
mod version;
#[cfg(doc_cfg)]
mod bibliography;
use header::{
line1::{is_header_line1, Line1},
line2::{is_header_line2, Line2},
};
use position::{position_entry, ClockRecord, PositionEntry, PositionRecord};
use velocity::{velocity_entry, ClockRateRecord, VelocityEntry, VelocityRecord};
use reader::BufferedReader;
use std::io::BufRead;
use version::Version;
#[cfg(feature = "serde")]
use serde::{Deserialize, Serialize};
use std::path::Path;
type Vector3D = (f64, f64, f64);
pub mod prelude {
pub use crate::version::Version;
pub use crate::{DataType, OrbitType, SP3};
pub use gnss::prelude::{Constellation, SV};
pub use hifitime::{Duration, Epoch, TimeScale};
}
pub use merge::{Merge, MergeError};
fn file_descriptor(content: &str) -> bool {
content.starts_with("%c")
}
fn sp3_comment(content: &str) -> bool {
content.starts_with("/*")
}
fn end_of_file(content: &str) -> bool {
content.eq("EOF")
}
fn new_epoch(content: &str) -> bool {
content.starts_with("* ")
}
#[derive(Default, Copy, Clone, Debug, PartialEq, Eq, Hash)]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
pub enum DataType {
#[default]
Position,
Velocity,
}
impl std::fmt::Display for DataType {
fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
match self {
Self::Position => f.write_str("P"),
Self::Velocity => f.write_str("V"),
}
}
}
impl std::str::FromStr for DataType {
type Err = ParsingError;
fn from_str(s: &str) -> Result<Self, Self::Err> {
if s.eq("P") {
Ok(Self::Position)
} else if s.eq("V") {
Ok(Self::Velocity)
} else {
Err(ParsingError::UnknownDataType(s.to_string()))
}
}
}
#[derive(Default, Copy, Clone, Debug, PartialEq, Eq, Hash)]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
pub enum OrbitType {
#[default]
FIT,
EXT,
BCT,
BHN,
HLM,
}
impl std::fmt::Display for OrbitType {
fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
match self {
Self::FIT => f.write_str("FIT"),
Self::EXT => f.write_str("EXT"),
Self::BCT => f.write_str("BCT"),
Self::BHN => f.write_str("BHN"),
Self::HLM => f.write_str("HLM"),
}
}
}
impl std::str::FromStr for OrbitType {
type Err = ParsingError;
fn from_str(s: &str) -> Result<Self, Self::Err> {
if s.eq("FIT") {
Ok(Self::FIT)
} else if s.eq("EXT") {
Ok(Self::EXT)
} else if s.eq("BCT") {
Ok(Self::BCT)
} else if s.eq("BHN") {
Ok(Self::BHN)
} else if s.eq("HLM") {
Ok(Self::HLM)
} else {
Err(ParsingError::UnknownOrbitType(s.to_string()))
}
}
}
type Comments = Vec<String>;
#[derive(Default, Clone, Debug)]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
pub struct SP3 {
pub version: Version,
pub data_type: DataType,
pub coord_system: String,
pub orbit_type: OrbitType,
pub agency: String,
pub constellation: Constellation,
pub time_system: TimeScale,
pub week_counter: (u32, f64),
pub mjd_start: (u32, f64),
pub epoch: Vec<Epoch>,
pub epoch_interval: Duration,
pub sv: Vec<SV>,
pub position: PositionRecord,
pub clock: ClockRecord,
pub velocities: VelocityRecord,
pub clock_rate: ClockRateRecord,
pub comments: Comments,
}
#[derive(Debug, Error)]
pub enum Errors {
#[error("parsing error")]
ParsingError(#[from] ParsingError),
#[error("hifitime parsing error")]
HifitimeParsingError(#[from] hifitime::Errors),
#[error("constellation parsing error")]
ConstellationParsingError(#[from] gnss::constellation::ParsingError),
#[error("unknown or non supported revision \"{0}\"")]
UnknownVersion(String),
#[error("unknown data type \"{0}\"")]
UnknownDataType(String),
#[error("unknown orbit type \"{0}\"")]
UnknownOrbitType(String),
#[error("file i/o error")]
DataParsingError(#[from] std::io::Error),
}
#[derive(Debug, Error)]
pub enum ParsingError {
#[error("unknown or non supported revision \"{0}\"")]
UnknownVersion(String),
#[error("unknown data type \"{0}\"")]
UnknownDataType(String),
#[error("unknown orbit type \"{0}\"")]
UnknownOrbitType(String),
#[error("malformed header line #1")]
MalformedH1,
#[error("malformed header line #2")]
MalformedH2,
#[error("malformed %c line \"{0}\"")]
MalformedDescriptor(String),
#[error("failed to parse epoch year from \"{0}\"")]
EpochYear(String),
#[error("failed to parse epoch month from \"{0}\"")]
EpochMonth(String),
#[error("failed to parse epoch day from \"{0}\"")]
EpochDay(String),
#[error("failed to parse epoch hours from \"{0}\"")]
EpochHours(String),
#[error("failed to parse epoch minutes from \"{0}\"")]
EpochMinutes(String),
#[error("failed to parse epoch seconds from \"{0}\"")]
EpochSeconds(String),
#[error("failed to parse epoch milliseconds from \"{0}\"")]
EpochMilliSeconds(String),
#[error("failed to parse number of epochs \"{0}\"")]
NumberEpoch(String),
#[error("failed to parse week counter")]
WeekCounter(String),
#[error("failed to parse hifitime::Epoch")]
Epoch,
#[error("failed to parse sample rate from \"{0}\"")]
EpochInterval(String),
#[error("failed to parse mjd start \"{0}\"")]
Mjd(String),
#[error("failed to parse sv from \"{0}\"")]
SV(String),
#[error("failed to parse (x, y, or z) coordinates from \"{0}\"")]
Coordinates(String),
#[error("failed to parse clock data from \"{0}\"")]
Clock(String),
}
fn parse_epoch(content: &str, time_scale: TimeScale) -> Result<Epoch, ParsingError> {
let y = u32::from_str(content[0..4].trim())
.or(Err(ParsingError::EpochYear(content[0..4].to_string())))?;
let m = u32::from_str(content[4..7].trim())
.or(Err(ParsingError::EpochMonth(content[4..7].to_string())))?;
let d = u32::from_str(content[7..10].trim())
.or(Err(ParsingError::EpochDay(content[7..10].to_string())))?;
let hh = u32::from_str(content[10..13].trim())
.or(Err(ParsingError::EpochHours(content[10..13].to_string())))?;
let mm = u32::from_str(content[13..16].trim())
.or(Err(ParsingError::EpochMinutes(content[13..16].to_string())))?;
let ss = u32::from_str(content[16..19].trim())
.or(Err(ParsingError::EpochSeconds(content[16..19].to_string())))?;
let _ss_fract = f64::from_str(content[20..27].trim()).or(Err(
ParsingError::EpochMilliSeconds(content[20..27].to_string()),
))?;
Epoch::from_str(&format!(
"{:04}-{:02}-{:02}T{:02}:{:02}:{:02} {}",
y, m, d, hh, mm, ss, time_scale,
))
.or(Err(ParsingError::Epoch))
}
impl SP3 {
pub fn from_path(path: &Path) -> Result<Self, Errors> {
let fullpath = path.to_string_lossy().to_string();
Self::from_file(&fullpath)
}
pub fn from_file(path: &str) -> Result<Self, Errors> {
let reader = BufferedReader::new(path)?;
let mut version = Version::default();
let mut data_type = DataType::default();
let mut time_system = TimeScale::default();
let mut constellation = Constellation::default();
let mut pc_count = 0_u8;
let mut coord_system = String::from("Unknown");
let mut orbit_type = OrbitType::default();
let mut agency = String::from("Unknown");
let mut week_counter = (0_u32, 0_f64);
let mut epoch_interval = Duration::default();
let mut mjd_start = (0_u32, 0_f64);
let mut vehicles: Vec<SV> = Vec::new();
let mut position = PositionRecord::default();
let mut velocities = VelocityRecord::default();
let mut clock = ClockRecord::default();
let mut clock_rate = ClockRateRecord::default();
let mut comments = Comments::new();
let mut epoch = Epoch::default();
let mut epochs: Vec<Epoch> = Vec::new();
for line in reader.lines() {
let line = line.unwrap();
let line = line.trim();
if sp3_comment(line) {
if line.len() > 4 {
comments.push(line[3..].to_string());
}
continue;
}
if end_of_file(line) {
break;
}
if is_header_line1(line) && !is_header_line2(line) {
let l1 = Line1::from_str(line)?;
(version, data_type, coord_system, orbit_type, agency) = l1.to_parts();
}
if is_header_line2(line) {
let l2 = Line2::from_str(line)?;
(week_counter, epoch_interval, mjd_start) = l2.to_parts();
}
if file_descriptor(line) {
if line.len() < 60 {
return Err(Errors::ParsingError(ParsingError::MalformedDescriptor(
line.to_string(),
)));
}
if pc_count == 0 {
constellation = Constellation::from_str(line[3..5].trim())?;
time_system = TimeScale::from_str(line[9..12].trim())?;
}
pc_count += 1;
}
if new_epoch(line) {
epoch = parse_epoch(&line[3..], time_system)?;
epochs.push(epoch);
}
if position_entry(line) {
if line.len() < 60 {
continue;
}
let entry = PositionEntry::from_str(line)?;
let (sv, (pos_x, pos_y, pos_z), clk) = entry.to_parts();
if !vehicles.contains(&sv) {
vehicles.push(sv);
}
if pos_x != 0.0_f64 && pos_y != 0.0_f64 && pos_z != 0.0_f64 {
if let Some(e) = position.get_mut(&epoch) {
e.insert(sv, (pos_x, pos_y, pos_z));
} else {
let mut map: BTreeMap<SV, Vector3D> = BTreeMap::new();
map.insert(sv, (pos_x, pos_y, pos_z));
position.insert(epoch, map);
}
}
if let Some(clk) = clk {
if let Some(e) = clock.get_mut(&epoch) {
e.insert(sv, clk);
} else {
let mut map: BTreeMap<SV, f64> = BTreeMap::new();
map.insert(sv, clk);
clock.insert(epoch, map);
}
}
}
if velocity_entry(line) {
if line.len() < 60 {
continue;
}
let entry = VelocityEntry::from_str(line)?;
let (sv, (vel_x, vel_y, vel_z), clk) = entry.to_parts();
if !vehicles.contains(&sv) {
vehicles.push(sv);
}
if vel_x != 0.0_f64 && vel_y != 0.0_f64 && vel_z != 0.0_f64 {
if let Some(e) = velocities.get_mut(&epoch) {
e.insert(sv, (vel_x, vel_y, vel_z));
} else {
let mut map: BTreeMap<SV, Vector3D> = BTreeMap::new();
map.insert(sv, (vel_x, vel_y, vel_z));
velocities.insert(epoch, map);
}
}
if let Some(clk) = clk {
if let Some(e) = clock_rate.get_mut(&epoch) {
e.insert(sv, clk);
} else {
let mut map: BTreeMap<SV, f64> = BTreeMap::new();
map.insert(sv, clk);
clock_rate.insert(epoch, map);
}
}
}
}
Ok(Self {
version,
data_type,
epoch: epochs,
time_system,
constellation,
coord_system,
orbit_type,
agency,
week_counter,
epoch_interval,
mjd_start,
sv: vehicles,
position,
velocities,
clock,
clock_rate,
comments,
})
}
pub fn epoch(&self) -> impl Iterator<Item = Epoch> + '_ {
self.epoch.iter().copied()
}
pub fn nb_epochs(&self) -> usize {
self.epoch.len()
}
pub fn first_epoch(&self) -> Option<Epoch> {
self.epoch.first().copied()
}
pub fn last_epoch(&self) -> Option<Epoch> {
self.epoch.last().copied()
}
pub fn sv(&self) -> impl Iterator<Item = SV> + '_ {
self.sv.iter().copied()
}
pub fn sv_position(&self) -> impl Iterator<Item = (Epoch, SV, Vector3D)> + '_ {
self.position
.iter()
.flat_map(|(e, sv)| sv.iter().map(|(sv, pos)| (*e, *sv, *pos)))
}
pub fn sv_elevation_azimuth(
&self,
ref_geo: Vector3D,
) -> impl Iterator<Item = (Epoch, SV, (f64, f64))> + '_ {
self.sv_position().map(move |(t, sv, (x_km, y_km, z_km))| {
let (azim, elev, _) = map_3d::ecef2aer(
x_km * 1.0E3,
y_km * 1.0E3,
z_km * 1.0E3,
ref_geo.0,
ref_geo.1,
ref_geo.2,
map_3d::Ellipsoid::WGS84,
);
(t, sv, (map_3d::rad2deg(elev), map_3d::rad2deg(azim)))
})
}
pub fn sv_velocities(&self) -> impl Iterator<Item = (Epoch, SV, Vector3D)> + '_ {
self.velocities
.iter()
.flat_map(|(e, sv)| sv.iter().map(|(sv, vel)| (*e, *sv, *vel)))
}
pub fn sv_clock(&self) -> impl Iterator<Item = (Epoch, SV, f64)> + '_ {
self.clock
.iter()
.flat_map(|(e, sv)| sv.iter().map(|(sv, clk)| (*e, *sv, *clk)))
}
pub fn sv_clock_change(&self) -> impl Iterator<Item = (Epoch, SV, f64)> + '_ {
self.clock_rate
.iter()
.flat_map(|(e, sv)| sv.iter().map(|(sv, clk)| (*e, *sv, *clk)))
}
pub fn sv_clock_interpolate(&self, t: Epoch, sv: SV) -> Option<f64> {
let before = self
.sv_clock()
.filter_map(|(clk_t, clk_sv, value)| {
if clk_t <= t && clk_sv == sv {
Some((clk_t, value))
} else {
None
}
})
.last()?;
let after = self
.sv_clock()
.filter_map(|(clk_t, clk_sv, value)| {
if clk_t > t && clk_sv == sv {
Some((clk_t, value))
} else {
None
}
})
.reduce(|k, _| k)?;
let (before_t, before_clk) = before;
let (after_t, after_clk) = after;
let dt = (after_t - before_t).to_seconds();
let mut bias = (after_t - t).to_seconds() / dt * before_clk;
bias += (t - before_t).to_seconds() / dt * after_clk;
Some(bias)
}
pub fn comments(&self) -> impl Iterator<Item = &String> + '_ {
self.comments.iter()
}
pub fn sv_position_interpolate(&self, sv: SV, t: Epoch, order: usize) -> Option<Vector3D> {
let odd_order = order % 2 > 0;
let sv_position: Vec<_> = self
.sv_position()
.filter_map(|(e, svnn, (x, y, z))| {
if sv == svnn {
Some((e, (x, y, z)))
} else {
None
}
})
.collect();
let center = match sv_position
.iter()
.find(|(e, _)| (*e - t).abs() < self.epoch_interval)
{
Some(center) => center,
None => {
return None;
},
};
let center_pos = match sv_position.iter().position(|(e, _)| *e == center.0) {
Some(center) => center,
None => {
return None;
},
};
let (min_before, min_after): (usize, usize) = match odd_order {
true => ((order + 1) / 2, (order + 1) / 2),
false => (order / 2, order / 2 + 1),
};
if center_pos < min_before || sv_position.len() - center_pos < min_after {
return None;
}
let mut polynomials = Vector3D::default();
let offset = center_pos - min_before;
for i in 0..order + 1 {
let mut li = 1.0_f64;
let (e_i, (x_i, y_i, z_i)) = sv_position[offset + i];
for j in 0..order + 1 {
let (e_j, _) = sv_position[offset + j];
if j != i {
li *= (t - e_j).to_seconds();
li /= (e_i - e_j).to_seconds();
}
}
polynomials.0 += x_i * li;
polynomials.1 += y_i * li;
polynomials.2 += z_i * li;
}
Some(polynomials)
}
}
impl Merge for SP3 {
fn merge(&self, rhs: &Self) -> Result<Self, MergeError> {
let mut s = self.clone();
s.merge_mut(rhs)?;
Ok(s)
}
fn merge_mut(&mut self, rhs: &Self) -> Result<(), MergeError> {
if self.time_system != rhs.time_system {
return Err(MergeError::TimeScale);
}
if self.coord_system != rhs.coord_system {
return Err(MergeError::CoordSystem);
}
if self.constellation != rhs.constellation {
self.constellation = Constellation::Mixed;
}
if rhs.version > self.version {
self.version = rhs.version;
}
if rhs.mjd_start.0 < self.mjd_start.0 {
self.mjd_start.0 = rhs.mjd_start.0;
}
if rhs.mjd_start.1 < self.mjd_start.1 {
self.mjd_start.1 = rhs.mjd_start.1;
}
if rhs.week_counter.0 < self.week_counter.0 {
self.week_counter.0 = rhs.week_counter.0;
}
if rhs.week_counter.1 < self.week_counter.1 {
self.week_counter.1 = rhs.week_counter.1;
}
for sv in &rhs.sv {
if !self.sv.contains(sv) {
self.sv.push(*sv);
}
}
self.epoch_interval = std::cmp::max(self.epoch_interval, rhs.epoch_interval);
for (epoch, svnn) in &rhs.position {
if let Some(lhs_sv) = self.position.get_mut(epoch) {
for (sv, position) in svnn {
lhs_sv.insert(*sv, *position);
}
} else {
self.epoch.push(*epoch);
self.position.insert(*epoch, svnn.clone());
}
}
for (epoch, svnn) in &rhs.clock {
if let Some(lhs_sv) = self.clock.get_mut(epoch) {
for (sv, clock) in svnn {
lhs_sv.insert(*sv, *clock);
}
} else {
self.clock.insert(*epoch, svnn.clone());
let mut found = false;
for e in &self.epoch {
found |= *e == *epoch;
if found {
break;
}
}
if !found {
self.epoch.push(*epoch);
}
}
}
for (epoch, svnn) in &rhs.velocities {
if let Some(lhs_sv) = self.velocities.get_mut(epoch) {
for (sv, position) in svnn {
lhs_sv.insert(*sv, *position);
}
} else {
self.velocities.insert(*epoch, svnn.clone());
let mut found = false;
for e in &self.epoch {
found |= *e == *epoch;
if found {
break;
}
}
if !found {
self.epoch.push(*epoch);
}
}
}
self.epoch.sort();
Ok(())
}
}