nyx_space/od/msr/trackingdata/
io_ccsds_tdm.rs

1/*
2    Nyx, blazing fast astrodynamics
3    Copyright (C) 2018-onwards Christopher Rabotin <christopher.rabotin@gmail.com>
4
5    This program is free software: you can redistribute it and/or modify
6    it under the terms of the GNU Affero General Public License as published
7    by the Free Software Foundation, either version 3 of the License, or
8    (at your option) any later version.
9
10    This program is distributed in the hope that it will be useful,
11    but WITHOUT ANY WARRANTY; without even the implied warranty of
12    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13    GNU Affero General Public License for more details.
14
15    You should have received a copy of the GNU Affero General Public License
16    along with this program.  If not, see <https://www.gnu.org/licenses/>.
17*/
18
19use crate::io::watermark::prj_name_ver;
20use crate::io::ExportCfg;
21use crate::io::{InputOutputError, StdIOSnafu};
22use crate::od::msr::{Measurement, MeasurementType};
23use anise::constants::SPEED_OF_LIGHT_KM_S;
24use hifitime::efmt::{Format, Formatter};
25use hifitime::prelude::Epoch;
26use hifitime::TimeScale;
27use indexmap::{IndexMap, IndexSet};
28use snafu::ResultExt;
29use std::collections::{BTreeMap, HashMap};
30use std::fs::File;
31use std::io::Write;
32use std::io::{BufRead, BufReader, BufWriter};
33use std::path::{Path, PathBuf};
34use std::str::FromStr;
35
36use super::TrackingDataArc;
37
38impl TrackingDataArc {
39    /// Loads a tracking arc from its serialization in CCSDS TDM.
40    ///
41    /// # Support level
42    ///
43    /// - Only the KVN format is supported.
44    /// - Support is limited to orbit determination in "xGEO", i.e. cislunar and deep space missions.
45    /// - Only one metadata and data section per file is tested.
46    ///
47    /// ## Data types
48    ///
49    /// Fully supported:
50    ///     - RANGE
51    ///     - DOPPLER_INSTANTANEOUS, DOPPLER_INTEGRATED
52    ///     - ANGLE_1 / ANGLE_2, as azimuth/elevation only
53    ///
54    /// Partially supported:
55    ///     - TRANSMIT_FREQ / RECEIVE_FREQ : these will be converted to Doppler measurements using the TURNAROUND_NUMERATOR and TURNAROUND_DENOMINATOR in the TDM. The freq rate is _not_ supported.
56    ///
57    /// ## Metadata support
58    ///
59    /// ### Mode
60    ///
61    /// Only the MODE = SEQUENTIAL is supported.
62    ///
63    /// ### Time systems / time scales
64    ///
65    /// All timescales supported by hifitime are supported here. This includes: UTC, TAI, GPS, TT, TDB, TAI, GST, QZSST.
66    ///
67    /// ### Path
68    ///
69    /// Only one way or two way data is supported, i.e. path must be either `PATH n,m,n` or `PATH n,m`.
70    ///
71    /// Note that the actual indexes of the path are ignored.
72    ///
73    /// ### Participants
74    ///
75    /// `PARTICIPANT_1` must be the ground station / tracker.
76    /// The second participant is ignored: the user must ensure that the Orbit Determination Process is properly configured and the proper arc is given.
77    ///
78    /// ### Turnaround ratio
79    ///
80    /// The turnaround ratio is only accounted for when the data contains RECEIVE_FREQ and TRANSMIT_FREQ data.
81    ///
82    /// ### Range and modulus
83    ///
84    /// Only kilometers are supported in range units. Range modulus is accounted for to compute range ambiguity.
85    ///
86    pub fn from_tdm<P: AsRef<Path>>(
87        path: P,
88        aliases: Option<HashMap<String, String>>,
89    ) -> Result<Self, InputOutputError> {
90        let file = File::open(&path).context(StdIOSnafu {
91            action: "opening CCSDS TDM file for tracking arc",
92        })?;
93
94        let source = path.as_ref().to_path_buf().display().to_string();
95        info!("parsing CCSDS TDM {source}");
96
97        let mut measurements = BTreeMap::new();
98        let mut metadata = HashMap::new();
99
100        let reader = BufReader::new(file);
101
102        let mut in_data_section = false;
103        let mut current_tracker = String::new();
104        let mut time_system = TimeScale::UTC;
105        let mut has_freq_data = false;
106        let mut msr_divider = 1.0;
107
108        for line in reader.lines() {
109            let line = line.context(StdIOSnafu {
110                action: "reading CCSDS TDM file",
111            })?;
112            let line = line.trim();
113
114            if line == "DATA_START" {
115                in_data_section = true;
116                continue;
117            } else if line == "DATA_STOP" {
118                in_data_section = false;
119            }
120
121            if !in_data_section {
122                if line.starts_with("PARTICIPANT_1") {
123                    current_tracker = line.split('=').nth(1).unwrap_or("").trim().to_string();
124                    // If aliases are provided, try to map them.
125                    if let Some(aliases) = &aliases {
126                        if let Some(alias) = aliases.get(&current_tracker) {
127                            current_tracker = alias.clone();
128                        }
129                    }
130                } else if line.starts_with("TIME_SYSTEM") {
131                    let ts = line.split('=').nth(1).unwrap_or("UTC").trim();
132                    // Support for all time scales of hifitime
133                    if let Ok(ts) = TimeScale::from_str(ts) {
134                        time_system = ts;
135                    } else {
136                        return Err(InputOutputError::UnsupportedData {
137                            which: format!("time scale `{ts}` not supported"),
138                        });
139                    }
140                } else if line.starts_with("PATH") {
141                    match line.split(",").count() {
142                        2 => msr_divider = 1.0,
143                        3 => msr_divider = 2.0,
144                        cnt => {
145                            return Err(InputOutputError::UnsupportedData {
146                                which: format!(
147                                    "found {cnt} paths in TDM, only 1 or 2 are supported"
148                                ),
149                            })
150                        }
151                    }
152                }
153
154                let mut splt = line.split('=');
155                if let Some(keyword) = splt.nth(0) {
156                    // Get the zeroth item again since we've consumed the first zeroth one.
157                    if let Some(value) = splt.nth(0) {
158                        metadata.insert(keyword.trim().to_string(), value.trim().to_string());
159                    }
160                }
161
162                continue;
163            }
164
165            if let Some((mtype, epoch, value)) = parse_measurement_line(line, time_system)? {
166                if [
167                    MeasurementType::ReceiveFrequency,
168                    MeasurementType::TransmitFrequency,
169                ]
170                .contains(&mtype)
171                {
172                    has_freq_data = true;
173                    // Don't modify the values.
174                    msr_divider = 1.0;
175                }
176                measurements
177                    .entry(epoch)
178                    .or_insert_with(|| Measurement {
179                        tracker: current_tracker.clone(),
180                        epoch,
181                        data: IndexMap::new(),
182                    })
183                    .data
184                    .insert(mtype, value / msr_divider);
185            }
186        }
187
188        let mut turnaround_ratio = None;
189        let drop_freq_data;
190        if has_freq_data {
191            // If there is any frequency measurement, compute the turn-around ratio.
192            if let Some(ta_num_str) = metadata.get("TURNAROUND_NUMERATOR") {
193                if let Some(ta_denom_str) = metadata.get("TURNAROUND_DENOMINATOR") {
194                    if let Ok(ta_num) = ta_num_str.parse::<i32>() {
195                        if let Ok(ta_denom) = ta_denom_str.parse::<i32>() {
196                            // turn-around ratio is set.
197                            turnaround_ratio = Some(f64::from(ta_num) / f64::from(ta_denom));
198                            info!("turn-around ratio is {ta_num}/{ta_denom}");
199                            drop_freq_data = false;
200                        } else {
201                            error!("turn-around denominator `{ta_denom_str}` is not a valid double precision float");
202                            drop_freq_data = true;
203                        }
204                    } else {
205                        error!("turn-around numerator `{ta_num_str}` is not a valid double precision float");
206                        drop_freq_data = true;
207                    }
208                } else {
209                    error!("required turn-around denominator missing from metadata -- dropping ALL RECEIVE/TRANSMIT data");
210                    drop_freq_data = true;
211                }
212            } else {
213                error!("required turn-around numerator missing from metadata -- dropping ALL RECEIVE/TRANSMIT data");
214                drop_freq_data = true;
215            }
216        } else {
217            drop_freq_data = true;
218        }
219
220        // Now, let's convert the receive and transmit frequencies to Doppler measurements in velocity units.
221        // We expect the transmit and receive frequencies to have the exact same timestamp.
222        let mut freq_types = IndexSet::new();
223        freq_types.insert(MeasurementType::ReceiveFrequency);
224        freq_types.insert(MeasurementType::TransmitFrequency);
225        let mut latest_transmit_freq = None;
226        let mut malformed_warning = 0;
227        for (epoch, measurement) in measurements.iter_mut() {
228            if drop_freq_data {
229                for freq in &freq_types {
230                    measurement.data.swap_remove(freq);
231                }
232                continue;
233            }
234
235            let avail = measurement.availability(&freq_types);
236            let use_prev_transmit_freq;
237            let num_freq_msr = avail
238                .iter()
239                .copied()
240                .map(|v| if v { 1 } else { 0 })
241                .sum::<u8>();
242            if num_freq_msr == 0 {
243                // No frequency measurements
244                continue;
245            } else if num_freq_msr == 1 {
246                // avail[0] means that Receive Freq is available
247                // avail[1] means that Transmit Freq is available
248                // We can only compute Doppler data from one data point if that data point
249                // if the receive frequency and the transmit frequency was previously set.
250                if latest_transmit_freq.is_some() && avail[0] {
251                    use_prev_transmit_freq = true;
252                    if malformed_warning == 0 {
253                        warn!(
254                            "no transmit frequency at {epoch}, using previous value of {} Hz",
255                            latest_transmit_freq.unwrap()
256                        );
257                    }
258                    malformed_warning += 1;
259                } else {
260                    warn!("only one of receive or transmit frequencies found at {epoch}, ignoring");
261                    for freq in &freq_types {
262                        measurement.data.swap_remove(freq);
263                    }
264                    continue;
265                }
266            } else {
267                use_prev_transmit_freq = false;
268            }
269
270            if !use_prev_transmit_freq {
271                // Update the latest transmit frequency since it's set.
272                latest_transmit_freq = Some(
273                    *measurement
274                        .data
275                        .get(&MeasurementType::TransmitFrequency)
276                        .unwrap(),
277                );
278            }
279
280            let transmit_freq_hz = latest_transmit_freq.unwrap();
281            let receive_freq_hz = *measurement
282                .data
283                .get(&MeasurementType::ReceiveFrequency)
284                .unwrap();
285
286            // Compute the Doppler shift, equation from section 3.5.2.8.2 of CCSDS TDM v2 specs
287            let doppler_shift_hz = transmit_freq_hz * turnaround_ratio.unwrap() - receive_freq_hz;
288            // Compute the expected Doppler measurement as range-rate.
289            let rho_dot_km_s = (doppler_shift_hz * SPEED_OF_LIGHT_KM_S)
290                / (2.0 * transmit_freq_hz * turnaround_ratio.unwrap());
291
292            // Finally, replace the frequency data with a Doppler measurement.
293            for freq in &freq_types {
294                measurement.data.swap_remove(freq);
295            }
296            measurement
297                .data
298                .insert(MeasurementType::Doppler, rho_dot_km_s);
299        }
300
301        if malformed_warning > 1 {
302            warn!("missing transmit frequency warning occured {malformed_warning} times",);
303        }
304
305        let moduli = if let Some(range_modulus) = metadata.get("RANGE_MODULUS") {
306            if let Ok(value) = range_modulus.parse::<f64>() {
307                if value > 0.0 {
308                    let mut modulos = IndexMap::new();
309                    modulos.insert(MeasurementType::Range, value);
310                    // Only range modulus exists in TDM files.
311                    Some(modulos)
312                } else {
313                    // Do not apply a modulus of zero.
314                    None
315                }
316            } else {
317                warn!("could not parse RANGE_MODULUS of `{range_modulus}` as a double");
318                None
319            }
320        } else {
321            None
322        };
323
324        let trk = Self {
325            measurements,
326            source: Some(source),
327            moduli,
328            force_reject: false,
329        };
330
331        if trk.unique_types().is_empty() {
332            Err(InputOutputError::EmptyDataset {
333                action: "CCSDS TDM file",
334            })
335        } else {
336            Ok(trk)
337        }
338    }
339
340    /// Store this tracking arc to a CCSDS TDM file, with optional metadata and a timestamp appended to the filename.
341    pub fn to_tdm_file<P: AsRef<Path>>(
342        mut self,
343        spacecraft_name: String,
344        aliases: Option<HashMap<String, String>>,
345        path: P,
346        cfg: ExportCfg,
347    ) -> Result<PathBuf, InputOutputError> {
348        if self.is_empty() {
349            return Err(InputOutputError::MissingData {
350                which: " - empty tracking data cannot be exported to TDM".to_string(),
351            });
352        }
353
354        // Filter epochs if needed.
355        if cfg.start_epoch.is_some() && cfg.end_epoch.is_some() {
356            self = self.filter_by_epoch(cfg.start_epoch.unwrap()..cfg.end_epoch.unwrap());
357        } else if cfg.start_epoch.is_some() {
358            self = self.filter_by_epoch(cfg.start_epoch.unwrap()..);
359        } else if cfg.end_epoch.is_some() {
360            self = self.filter_by_epoch(..cfg.end_epoch.unwrap());
361        }
362
363        let tick = Epoch::now().unwrap();
364        info!("Exporting tracking data to CCSDS TDM file...");
365
366        // Grab the path here before we move stuff.
367        let path_buf = cfg.actual_path(path);
368
369        let metadata = cfg.metadata.unwrap_or_default();
370
371        let file = File::create(&path_buf).context(StdIOSnafu {
372            action: "creating CCSDS TDM file for tracking arc",
373        })?;
374        let mut writer = BufWriter::new(file);
375
376        let err_hdlr = |source| InputOutputError::StdIOError {
377            source,
378            action: "writing data to TDM file",
379        };
380
381        // Epoch formmatter.
382        let iso8601_no_ts = Format::from_str("%Y-%m-%dT%H:%M:%S.%f").unwrap();
383
384        // Write mandatory metadata
385        writeln!(writer, "CCSDS_TDM_VERS = 2.0").map_err(err_hdlr)?;
386        writeln!(
387            writer,
388            "\nCOMMENT Build by {} -- https://nyxspace.com",
389            prj_name_ver()
390        )
391        .map_err(err_hdlr)?;
392        writeln!(
393            writer,
394            "COMMENT Nyx Space provided under the AGPL v3 open source license -- https://nyxspace.com/pricing\n"
395        )
396        .map_err(err_hdlr)?;
397        writeln!(
398            writer,
399            "CREATION_DATE = {}",
400            Formatter::new(Epoch::now().unwrap(), iso8601_no_ts)
401        )
402        .map_err(err_hdlr)?;
403        writeln!(
404            writer,
405            "ORIGINATOR = {}\n",
406            metadata
407                .get("originator")
408                .unwrap_or(&"Nyx Space".to_string())
409        )
410        .map_err(err_hdlr)?;
411
412        // Create a new meta section for each tracker and for each measurement type that is one or two way.
413        // Get unique trackers and process each one separately
414        let trackers = self.unique_aliases();
415
416        for tracker in trackers {
417            let tracker_data = self.clone().filter_by_tracker(tracker.clone());
418
419            let types = tracker_data.unique_types();
420
421            let two_way_types = types
422                .iter()
423                .filter(|msr_type| msr_type.may_be_two_way())
424                .copied()
425                .collect::<Vec<_>>();
426
427            let one_way_types = types
428                .iter()
429                .filter(|msr_type| !msr_type.may_be_two_way())
430                .copied()
431                .collect::<Vec<_>>();
432
433            // Add the two-way data first.
434            for (tno, types) in [two_way_types, one_way_types].iter().enumerate() {
435                writeln!(writer, "META_START").map_err(err_hdlr)?;
436                writeln!(writer, "\tTIME_SYSTEM = UTC").map_err(err_hdlr)?;
437                writeln!(
438                    writer,
439                    "\tSTART_TIME = {}",
440                    Formatter::new(tracker_data.start_epoch().unwrap(), iso8601_no_ts)
441                )
442                .map_err(err_hdlr)?;
443                writeln!(
444                    writer,
445                    "\tSTOP_TIME = {}",
446                    Formatter::new(tracker_data.end_epoch().unwrap(), iso8601_no_ts)
447                )
448                .map_err(err_hdlr)?;
449
450                let multiplier = if tno == 0 {
451                    writeln!(writer, "\tPATH = 1,2,1").map_err(err_hdlr)?;
452                    2.0
453                } else {
454                    writeln!(writer, "\tPATH = 1,2").map_err(err_hdlr)?;
455                    1.0
456                };
457
458                writeln!(
459                    writer,
460                    "\tPARTICIPANT_1 = {}",
461                    if let Some(aliases) = &aliases {
462                        if let Some(alias) = aliases.get(&tracker) {
463                            alias
464                        } else {
465                            &tracker
466                        }
467                    } else {
468                        &tracker
469                    }
470                )
471                .map_err(err_hdlr)?;
472
473                writeln!(writer, "\tPARTICIPANT_2 = {spacecraft_name}").map_err(err_hdlr)?;
474
475                writeln!(writer, "\tMODE = SEQUENTIAL").map_err(err_hdlr)?;
476
477                // Add additional metadata, could include timetag ref for example.
478                for (k, v) in &metadata {
479                    if k != "originator" {
480                        writeln!(writer, "\t{k} = {v}").map_err(err_hdlr)?;
481                    }
482                }
483
484                if types.contains(&MeasurementType::Range) {
485                    writeln!(writer, "\tRANGE_UNITS = km").map_err(err_hdlr)?;
486
487                    if let Some(moduli) = &self.moduli {
488                        if let Some(range_modulus) = moduli.get(&MeasurementType::Range) {
489                            writeln!(writer, "\tRANGE_MODULUS = {range_modulus:E}")
490                                .map_err(err_hdlr)?;
491                        }
492                    }
493                }
494
495                if types.contains(&MeasurementType::Azimuth)
496                    || types.contains(&MeasurementType::Elevation)
497                {
498                    writeln!(writer, "\tANGLE_TYPE = AZEL").map_err(err_hdlr)?;
499                }
500
501                writeln!(writer, "META_STOP\n").map_err(err_hdlr)?;
502
503                // Write the data section
504                writeln!(writer, "DATA_START").map_err(err_hdlr)?;
505
506                // Process measurements for this tracker
507                for (epoch, measurement) in &tracker_data.measurements {
508                    for (mtype, value) in &measurement.data {
509                        if !types.contains(mtype) {
510                            continue;
511                        }
512                        let type_str = match mtype {
513                            MeasurementType::Range => "RANGE",
514                            MeasurementType::Doppler => "DOPPLER_INTEGRATED",
515                            MeasurementType::Azimuth => "ANGLE_1",
516                            MeasurementType::Elevation => "ANGLE_2",
517                            MeasurementType::ReceiveFrequency => "RECEIVE_FREQ",
518                            MeasurementType::TransmitFrequency => "TRANSMIT_FREQ",
519                        };
520
521                        writeln!(
522                            writer,
523                            "\t{:<20} = {:<23}\t{:.12}",
524                            type_str,
525                            Formatter::new(*epoch, iso8601_no_ts),
526                            value * multiplier
527                        )
528                        .map_err(err_hdlr)?;
529                    }
530                }
531
532                writeln!(writer, "DATA_STOP\n").map_err(err_hdlr)?;
533            }
534        }
535
536        #[allow(clippy::writeln_empty_string)]
537        writeln!(writer, "").map_err(err_hdlr)?;
538
539        // Return the path this was written to
540        let tock_time = Epoch::now().unwrap() - tick;
541        info!("CCSDS TDM written to {} in {tock_time}", path_buf.display());
542        Ok(path_buf)
543    }
544}
545
546fn parse_measurement_line(
547    line: &str,
548    time_system: TimeScale,
549) -> Result<Option<(MeasurementType, Epoch, f64)>, InputOutputError> {
550    let parts: Vec<&str> = line.split('=').collect();
551    if parts.len() != 2 {
552        return Ok(None);
553    }
554
555    let (mtype_str, data) = (parts[0].trim(), parts[1].trim());
556    let mtype = match mtype_str {
557        "RANGE" => MeasurementType::Range,
558        "DOPPLER_INSTANTANEOUS" | "DOPPLER_INTEGRATED" => MeasurementType::Doppler,
559        "ANGLE_1" => MeasurementType::Azimuth,
560        "ANGLE_2" => MeasurementType::Elevation,
561        "RECEIVE_FREQ" | "RECEIVE_FREQ_1" | "RECEIVE_FREQ_2" | "RECEIVE_FREQ_3"
562        | "RECEIVE_FREQ_4" | "RECEIVE_FREQ_5" => MeasurementType::ReceiveFrequency,
563        "TRANSMIT_FREQ" | "TRANSMIT_FREQ_1" | "TRANSMIT_FREQ_2" | "TRANSMIT_FREQ_3"
564        | "TRANSMIT_FREQ_4" | "TRANSMIT_FREQ_5" => MeasurementType::TransmitFrequency,
565        _ => {
566            return Err(InputOutputError::UnsupportedData {
567                which: mtype_str.to_string(),
568            })
569        }
570    };
571
572    let data_parts: Vec<&str> = data.split_whitespace().collect();
573    if data_parts.len() != 2 {
574        return Ok(None);
575    }
576
577    let epoch =
578        Epoch::from_gregorian_str(&format!("{} {time_system}", data_parts[0])).map_err(|e| {
579            InputOutputError::Inconsistency {
580                msg: format!("{e} when parsing epoch"),
581            }
582        })?;
583
584    let value = data_parts[1]
585        .parse::<f64>()
586        .map_err(|e| InputOutputError::UnsupportedData {
587            which: format!("`{}` is not a float: {e}", data_parts[1]),
588        })?;
589
590    Ok(Some((mtype, epoch, value)))
591}