Skip to content

Commit

Permalink
Improve support for BDRC ephemeris (#249)
Browse files Browse the repository at this point in the history 
* Fix bugs in kepler orbit calculation
* Correct calculation of GPS,BDS,GALIEO satellite position and 
* QZSS support has not been tested.
* Add missing "log" to Cargo.toml
* Rename toe_gpst and emphasize that we work in GPST
* Update to latest Hifitime 4 and RTK
* Fix "velocity" typo 
*Fix toe_gpst for Galileo SV [validated]
* Rebase pending work: preliminary to SBAS support
* remove old kepler2ecef test

---------

Signed-off-by: Guillaume W. Bres <guillaume.bressaix@gmail.com>
Co-authored-by: gwbres <guillaume.bressaix@gmail.com>
  • Loading branch information
@FlowLoveV @gwbres
FlowLoveV and gwbres authored Jun 8, 2024
1 parent 4f959d1 commit 01143ef
Show file tree
Hide file tree
Showing 10 changed files with 674 additions and 493 deletions.
152 changes: 76 additions & 76 deletions rinex-cli/src/positioning/cggtts/mod.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,8 @@ use rtk::prelude::{
InterpolationResult,
IonosphereBias,
Method,
Observation,
PhaseRange,
PseudoRange,
Solver,
TroposphereBias, //TimeScale
};
Expand Down Expand Up @@ -128,7 +129,7 @@ where
let clock_corr = match time.next_at(*t, *sv) {
Some(dt) => dt,
None => {
error!("{:?} ({}) - failed to determine clock correction", *t, *sv);
error!("{} ({}) - failed to determine clock correction", *t, *sv);
continue;
},
};
Expand All @@ -155,63 +156,24 @@ where
let carrier = carrier.unwrap();
let rtk_carrier = cast_rtk_carrier(carrier);

let mut code = Option::<Observation>::None;
let phase = Option::<Observation>::None;
let mut doppler = Option::<Observation>::None;

if observable.is_pseudorange_observable() {
code = Some(Observation {
carrier: rtk_carrier,
snr: { data.snr.map(|snr| snr.into()) },
value: data.obs,
});

// attach one phase, if need be
match solver.cfg.method {
Method::SPP => {}, // nothing to do
Method::CPP => {}, // nothing to do
Method::PPP => {
// try to attach phase data
// let to_match =
// Observable::from_str(&format!("L{}", &observable.to_string()[1..])).unwrap();
// for (observable, data) in observations {
// if *observable == phase_to_match {
// phase = Some(Observation {
// carrier: rtk_carrier,
// snr: { data.snr.map(|snr| snr.into()) },
// value: data.obs,
// });
// }
// }
},
}

// try to attach doppler
let doppler_to_match =
Observable::from_str(&format!("D{}", &observable.to_string()[1..]))
.unwrap();
for (observable, data) in observations {
if *observable == doppler_to_match {
doppler = Some(Observation {
carrier: rtk_carrier,
snr: { data.snr.map(|snr| snr.into()) },
value: data.obs,
});
break;
}
}
}

if code.is_none() {
// We consider a reference Pseudo Range
// and possibly gather other signals later on
if !observable.is_pseudorange_observable() {
continue;
}

let mut codes = vec![code.unwrap()];
let mut codes = vec![PseudoRange {
carrier: rtk_carrier,
snr: { data.snr.map(|snr| snr.into()) },
value: data.obs,
}];

// complete if need be
//let mut doppler = Option::<Observation>::None;
let mut phases = Vec::<PhaseRange>::with_capacity(4);

// Subsidary Pseudo Range (if needed)
match solver.cfg.method {
Method::SPP => {}, // nothing to do
Method::CPP => {
Method::CPP | Method::PPP => {
// Attach secondary PR
for (second_obs, second_data) in observations {
let rhs_carrier =
Expand All @@ -223,28 +185,54 @@ where
let rtk_carrier = cast_rtk_carrier(rhs_carrier);

if second_obs.is_pseudorange_observable() && rhs_carrier != carrier {
codes.push(Observation {
value: second_data.obs,
codes.push(PseudoRange {
carrier: rtk_carrier,
value: second_data.obs,
snr: { data.snr.map(|snr| snr.into()) },
});
break;
}
}
},
Method::PPP => {}, //TODO
_ => {}, // not needed
};

let dopplers = match doppler {
Some(doppler) => vec![doppler],
None => vec![],
};
let phases = match phase {
Some(phase) => vec![phase],
None => vec![],
// Subsidary Phase Range (if needed)
if solver.cfg.method == Method::PPP {
for (second_obs, second_data) in observations {
if second_obs.is_phase_observable() {
let rhs_carrier =
Carrier::from_observable(sv.constellation, second_obs);
if rhs_carrier.is_err() {
continue;
}
let rhs_carrier = rhs_carrier.unwrap();
let rtk_carrier = cast_rtk_carrier(rhs_carrier);
phases.push(PhaseRange {
ambiguity: None,
carrier: rtk_carrier,
value: second_data.obs,
snr: { data.snr.map(|snr| snr.into()) },
});
}
}
};
let candidate =
Candidate::new(*sv, *t, clock_corr, sv_eph.tgd(), codes, phases, dopplers);

let candidate = Candidate::new(*sv, *t, clock_corr, sv_eph.tgd(), codes, phases);

let ref_observable = Carrier::L1;
//TODO
//match solver.cfg.method {
// Method::SPP => candidate
// .prefered_pseudorange()
// .and_then(|sig| Some(sig.carrier)),
// Method::CPP => candidate
// .code_if_combination()
// .and_then(|cmb| Some(cmb.reference)),
// Method::PPP => candidate
// .phase_if_combination()
// .and_then(|cmb| Some(cmb.reference)),
//};

match solver.resolve(*t, &vec![candidate], &iono_bias, &tropo_bias) {
Ok((t, pvt_solution)) => {
let pvt_data = pvt_solution.sv.get(sv).unwrap(); // infaillible
Expand All @@ -267,7 +255,7 @@ where
let mdio = pvt_data.iono_bias.modeled;
let msio = pvt_data.iono_bias.measured;
debug!(
"{:?} : new {}:{} PVT solution (elev={:.2}°, azi={:.2}°, REFSV={:.3E}, REFSYS={:.3E})",
"{:?} : new {}:{} solution (elev={:.2}°, azi={:.2}°, refsv={:.3E}, refsys={:.3E})",
t, sv, observable, elevation, azimuth, refsv, refsys
);

Expand Down Expand Up @@ -322,7 +310,7 @@ where
) {
Ok(((trk_elev, trk_azi), trk_data, _iono_data)) => {
info!(
"{:?} - new {} track: elev {:.2}° - azi {:.2}° - REFSV {:.3E} REFSYS {:.3E}",
"{:?} : new {} cggtts solution (elev={:.2}°, azi={:.2}°, refsv={:.3E}, refsys={:.3E})",
t,
sv,
trk_elev,
Expand All @@ -341,10 +329,16 @@ where
trk_elev,
trk_azi,
trk_data,
None, //TODO "iono": once L2/L5 unlocked,
0, // TODO "rcvr_channel" > 0 if known
match solver.cfg.method {
Method::CPP | Method::PPP => {
// TODO: grab IONOD from PVTSol
None
},
_ => None,
},
0, // TODO "rcvr_channel" > 0 if known
GlonassChannel::default(), //TODO
&observable.to_string(),
&ref_observable.to_string(),
)
},
_ => {
Expand All @@ -356,16 +350,22 @@ where
trk_elev,
trk_azi,
trk_data,
None, //TODO "iono": once L2/L5 unlocked,
0, // TODO "rcvr_channel" > 0 if known
&observable.to_string(),
match solver.cfg.method {
Method::CPP | Method::PPP => {
// TODO: grab IONOD from PVTSol
None
},
_ => None,
},
0, // TODO "rcvr_channel" > 0 if known
&ref_observable.to_string(),
)
},
}; // match constellation
tracks.push(track);
},
Err(e) => {
warn!("{:?} - track fitting error: \"{}\"", t, e);
warn!("{} - track fitting error: \"{}\"", t, e);
// TODO: most likely we should reset the SV signal tracker here
},
} //.fit()
Expand Down
2 changes: 1 addition & 1 deletion rinex-cli/src/positioning/orbit/nav.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ impl<'a> Orbit<'a> {
let toe_i = eph_i.toe_gpst(sv_ts).unwrap();
t - toe_i
})?;
let (x_km, y_km, z_km) = eph_i.kepler2ecef(sv, t)?;
let (x_km, y_km, z_km) = eph_i.kepler2position(sv, t)?;
let (x, y, z) = (x_km * 1.0E3, y_km * 1.0E3, z_km * 1.0E3);
Some(RTKInterpolationResult::from_position((x, y, z)))
},
Expand Down
47 changes: 27 additions & 20 deletions rinex-cli/src/positioning/ppp/mod.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,14 +11,14 @@ use crate::{
};
use std::collections::BTreeMap;

use rinex::{carrier::Carrier, prelude::SV};
use rinex::{carrier::Carrier, observation::LliFlags, prelude::SV};

mod post_process;
pub use post_process::{post_process, Error as PostProcessingError};

use rtk::prelude::{
Candidate, Epoch, InterpolationResult, IonosphereBias, Observation, PVTSolution, Solver,
TroposphereBias,
Candidate, Epoch, InterpolationResult, IonosphereBias, PVTSolution, PhaseRange, PseudoRange,
Solver, TroposphereBias,
};

pub fn resolve<I>(
Expand All @@ -42,7 +42,8 @@ where
let mut candidates = Vec::<Candidate>::with_capacity(4);

if !flag.is_ok() {
/* we only consider _valid_ epochs" */
// TODO: handle these invalid Epochs
warn!("{}: (unhandled) rx event: {}", t, flag);
continue;
}

Expand All @@ -57,7 +58,7 @@ where
for (sv, observations) in vehicles {
let sv_eph = nav_data.sv_ephemeris(*sv, *t);
if sv_eph.is_none() {
error!("{:?} ({}) : undetermined ephemeris", t, sv);
error!("{} ({}) : undetermined ephemeris", t, sv);
continue; // can't proceed further
}

Expand All @@ -66,38 +67,45 @@ where
let clock_corr = match time.next_at(*t, *sv) {
Some(dt) => dt,
None => {
error!("{:?} ({}) - failed to determine clock correction", *t, *sv);
error!("{} ({}) - failed to determine clock correction", *t, *sv);
continue;
},
};

let mut codes = Vec::<Observation>::new();
let mut phases = Vec::<Observation>::new();
let mut dopplers = Vec::<Observation>::new();
let mut codes = Vec::<PseudoRange>::new();
let mut phases = Vec::<PhaseRange>::new();
// let mut dopplers = Vec::<Observation>::new();

for (observable, data) in observations {
if let Some(lli) = data.lli {
if lli != LliFlags::OK_OR_UNKNOWN {
// TODO: manage those events
warn!("lli not_ok: {}({}): {:?}", t, sv, lli);
}
}
if let Ok(carrier) = Carrier::from_observable(sv.constellation, observable) {
let rtk_carrier: gnss_rtk::prelude::Carrier = cast_rtk_carrier(carrier);
let rtk_carrier = cast_rtk_carrier(carrier);

if observable.is_pseudorange_observable() {
codes.push(Observation {
codes.push(PseudoRange {
value: data.obs,
carrier: rtk_carrier,
snr: { data.snr.map(|snr| snr.into()) },
});
} else if observable.is_phase_observable() {
let lambda = carrier.wavelength();
phases.push(Observation {
phases.push(PhaseRange {
ambiguity: None,
carrier: rtk_carrier,
value: data.obs * lambda,
snr: { data.snr.map(|snr| snr.into()) },
});
} else if observable.is_doppler_observable() {
dopplers.push(Observation {
value: data.obs,
carrier: rtk_carrier,
snr: { data.snr.map(|snr| snr.into()) },
});
//dopplers.push(Observation {
// value: data.obs,
// carrier: rtk_carrier,
// snr: { data.snr.map(|snr| snr.into()) },
//});
}
}
}
Expand All @@ -108,7 +116,6 @@ where
sv_eph.tgd(),
codes.clone(),
phases.clone(),
dopplers.clone(),
);
candidates.push(candidate);
}
Expand All @@ -130,10 +137,10 @@ where

match solver.resolve(*t, &candidates, &iono_bias, &tropo_bias) {
Ok((t, pvt)) => {
debug!("{:?} : {:?}", t, pvt);
debug!("{} : {:?}", t, pvt);
solutions.insert(t, pvt);
},
Err(e) => warn!("{:?} : pvt solver error \"{}\"", t, e),
Err(e) => warn!("{} : pvt solver error \"{}\"", t, e),
}
}

Expand Down
2 changes: 1 addition & 1 deletion rinex-cli/src/positioning/time/mod.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ impl<'a> Time<'a> {
Self::Interp(Interpolator::from_iter(iter))
} else {
let brdc = ctx.data.brdc_navigation().unwrap(); // infaillible
let iter = brdc.ephemeris().map(|(_, (_, sv, eph))| (sv, eph));
let iter = brdc.ephemeris().map(|(toc, (_, sv, eph))| (sv, toc, eph));
Self::NAV(NAVTime::from_iter(iter))
}
}
Expand Down
Loading

0 comments on commit 01143ef

Please sign in to comment.