nyx_space/od/simulator/
arc.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 anise::almanac::Almanac;
20use hifitime::{Duration, Epoch, TimeSeries, TimeUnits};
21use num::integer::gcd;
22use rand::SeedableRng;
23use rand_pcg::Pcg64Mcg;
24
25use crate::dynamics::NyxError;
26use crate::io::ConfigError;
27use crate::md::trajectory::Interpolatable;
28use crate::od::msr::TrackingDataArc;
29use crate::od::prelude::Strand;
30use crate::od::simulator::Cadence;
31use crate::od::GroundStation;
32use crate::Spacecraft;
33use crate::State;
34use crate::{linalg::allocator::Allocator, od::TrackingDevice};
35use crate::{linalg::DefaultAllocator, md::prelude::Traj};
36use std::collections::BTreeMap;
37use std::fmt::Display;
38use std::marker::PhantomData;
39use std::sync::Arc;
40
41use super::{Handoff, TrkConfig};
42
43#[derive(Clone)]
44pub struct TrackingArcSim<MsrIn, D>
45where
46    D: TrackingDevice<MsrIn>,
47    MsrIn: State,
48    MsrIn: Interpolatable,
49    DefaultAllocator: Allocator<<MsrIn as State>::Size>
50        + Allocator<<MsrIn as State>::Size, <MsrIn as State>::Size>
51        + Allocator<<MsrIn as State>::VecLength>,
52{
53    /// Map of devices from their names.
54    pub devices: BTreeMap<String, D>,
55    /// Receiver trajectory
56    pub trajectory: Traj<MsrIn>,
57    /// Configuration of each device
58    pub configs: BTreeMap<String, TrkConfig>,
59    /// Random number generator used for this tracking arc, ensures repeatability
60    rng: Pcg64Mcg,
61    /// Greatest common denominator time series that allows this arc to meet all of the conditions.
62    time_series: TimeSeries,
63    _msr_in: PhantomData<MsrIn>,
64}
65
66impl<MsrIn, D> TrackingArcSim<MsrIn, D>
67where
68    D: TrackingDevice<MsrIn>,
69    MsrIn: State,
70    MsrIn: Interpolatable,
71    DefaultAllocator: Allocator<<MsrIn as State>::Size>
72        + Allocator<<MsrIn as State>::Size, <MsrIn as State>::Size>
73        + Allocator<<MsrIn as State>::VecLength>,
74{
75    /// Build a new tracking arc simulator using the provided seeded random number generator.
76    pub fn with_rng(
77        devices: BTreeMap<String, D>,
78        trajectory: Traj<MsrIn>,
79        configs: BTreeMap<String, TrkConfig>,
80        rng: Pcg64Mcg,
81    ) -> Result<Self, ConfigError> {
82        // Check that each device has an associated configurations.
83        // We don't care if there are more configurations than chosen devices.
84        let mut sampling_rates_ns = Vec::with_capacity(devices.len());
85        for name in devices.keys() {
86            if let Some(cfg) = configs.get(name) {
87                if let Err(e) = cfg.sanity_check() {
88                    warn!("Ignoring device {name}: {e}");
89                    continue;
90                }
91                sampling_rates_ns.push(cfg.sampling.truncated_nanoseconds());
92            } else {
93                warn!("Ignoring device {name}: no associated tracking configuration",);
94                continue;
95            }
96        }
97
98        if sampling_rates_ns.is_empty() {
99            return Err(ConfigError::InvalidConfig {
100                msg: "None of the devices are properly configured".to_string(),
101            });
102        }
103
104        let common_sampling_rate_ns = sampling_rates_ns
105            .iter()
106            .fold(sampling_rates_ns[0], |a, &b| gcd(a, b));
107
108        // The overall time series is the one going from the start to the end of the trajectory with the smallest time step
109        // of all the tracking configurations.
110        let time_series = TimeSeries::inclusive(
111            trajectory.first().epoch(),
112            trajectory.last().epoch(),
113            Duration::from_truncated_nanoseconds(common_sampling_rate_ns),
114        );
115
116        let me = Self {
117            devices,
118            trajectory,
119            configs,
120            rng,
121            time_series,
122            _msr_in: PhantomData,
123        };
124
125        info!("{me}");
126
127        Ok(me)
128    }
129
130    /// Build a new tracking arc simulator using the provided seed to initialize the random number generator.
131    pub fn with_seed(
132        devices: BTreeMap<String, D>,
133        trajectory: Traj<MsrIn>,
134        configs: BTreeMap<String, TrkConfig>,
135        seed: u64,
136    ) -> Result<Self, ConfigError> {
137        let rng = Pcg64Mcg::new(seed as u128);
138
139        Self::with_rng(devices, trajectory, configs, rng)
140    }
141
142    /// Build a new tracking arc simulator using the system entropy to seed the random number generator.
143    pub fn new(
144        devices: BTreeMap<String, D>,
145        trajectory: Traj<MsrIn>,
146        configs: BTreeMap<String, TrkConfig>,
147    ) -> Result<Self, ConfigError> {
148        let rng = Pcg64Mcg::from_os_rng();
149
150        Self::with_rng(devices, trajectory, configs, rng)
151    }
152
153    /// Generates measurements for the tracking arc using the defined strands
154    ///
155    /// # Warning
156    /// This function will return an error if any of the devices defines as a scheduler.
157    /// You must create the schedule first using `build_schedule` first.
158    ///
159    /// # Notes
160    /// Although mutable, this function may be called several times to generate different measurements.
161    ///
162    /// # Algorithm
163    /// For each tracking device, and for each strand within that device, sample the trajectory at the sample
164    /// rate of the tracking device, adding a measurement whenever the spacecraft is visible.
165    /// Build the measurements as a vector, ordered chronologically.
166    ///
167    pub fn generate_measurements(
168        &mut self,
169        almanac: Arc<Almanac>,
170    ) -> Result<TrackingDataArc, NyxError> {
171        let mut measurements = BTreeMap::new();
172
173        for (name, device) in self.devices.iter_mut() {
174            if let Some(cfg) = self.configs.get(name) {
175                if cfg.scheduler.is_some() {
176                    if cfg.strands.is_none() {
177                        return Err(NyxError::CustomError {
178                            msg: format!(
179                                "schedule for {name} must be built before generating measurements"
180                            ),
181                        });
182                    } else {
183                        warn!("scheduler for {name} is ignored, using the defined tracking strands instead")
184                    }
185                }
186
187                let init_msr_count = measurements.len();
188                let tick = Epoch::now().unwrap();
189
190                match cfg.strands.as_ref() {
191                    Some(strands) => {
192                        // Strands are defined at this point
193                        'strands: for (ii, strand) in strands.iter().enumerate() {
194                            // Build the time series for this strand, sampling at the correct rate
195                            for epoch in
196                                TimeSeries::inclusive(strand.start, strand.end, cfg.sampling)
197                            {
198                                match device.measure(
199                                    epoch,
200                                    &self.trajectory,
201                                    Some(&mut self.rng),
202                                    almanac.clone(),
203                                ) {
204                                    Ok(msr_opt) => {
205                                        if let Some(msr) = msr_opt {
206                                            measurements.insert(epoch, msr);
207                                        }
208                                    }
209                                    Err(e) => {
210                                        if epoch != strand.end {
211                                            warn!(
212                                            "Skipping the remaining strand #{ii} ending on {}: {e}",
213                                            strand.end
214                                        );
215                                        }
216                                        continue 'strands;
217                                    }
218                                }
219                            }
220                        }
221
222                        info!(
223                            "Simulated {} measurements for {name} for {} tracking strands in {}",
224                            measurements.len() - init_msr_count,
225                            strands.len(),
226                            (Epoch::now().unwrap() - tick).round(1.0_f64.milliseconds())
227                        );
228                    }
229                    None => {
230                        warn!("No tracking strands defined for {name}, skipping");
231                    }
232                }
233            }
234        }
235
236        // Build the tracking arc.
237        let trk_data = TrackingDataArc {
238            measurements,
239            source: None,
240            moduli: None,
241            force_reject: false,
242        };
243
244        Ok(trk_data)
245    }
246}
247
248impl<MsrIn, D> Display for TrackingArcSim<MsrIn, D>
249where
250    D: TrackingDevice<MsrIn>,
251    MsrIn: Interpolatable,
252    DefaultAllocator: Allocator<<MsrIn as State>::Size>
253        + Allocator<<MsrIn as State>::Size, <MsrIn as State>::Size>
254        + Allocator<<MsrIn as State>::VecLength>,
255{
256    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
257        write!(
258            f,
259            "Tracking Arc Simulator on {} with devices {:?} over {}",
260            self.trajectory,
261            self.devices.keys(),
262            self.time_series
263        )
264    }
265}
266
267// Literally the same as above, but can't make it generic =(
268impl TrackingArcSim<Spacecraft, GroundStation> {
269    /// Builds the schedule provided the config. Requires the tracker to be a ground station.
270    ///
271    /// # Algorithm
272    ///
273    /// 1. For each tracking device:
274    /// 2. Find when the vehicle trajectory has an elevation greater or equal to zero, and use that as the first start of the first tracking arc for this station
275    /// 3. Find when the vehicle trajectory has an elevation less than zero (i.e. disappears below the horizon), after that initial epoch
276    /// 4. Repeat 2, 3 until the end of the trajectory
277    /// 5. Build each of these as "tracking strands" for this tracking device.
278    /// 6. Organize all of the built tracking strands chronologically.
279    /// 7. Iterate through all of the strands:
280    ///    7.a. if that tracker is marked as `Greedy` and it ends after the start of the next strand, change the start date of the next strand.
281    ///    7.b. if that tracker is marked as `Eager` and it ends after the start of the next strand, change the end date of the current strand.
282    pub fn generate_schedule(
283        &self,
284        almanac: Arc<Almanac>,
285    ) -> Result<BTreeMap<String, TrkConfig>, NyxError> {
286        // Consider using find_all via the heuristic
287        let mut built_cfg = self.configs.clone();
288
289        for (name, device) in self.devices.iter() {
290            if let Some(cfg) = self.configs.get(name) {
291                if let Some(scheduler) = cfg.scheduler {
292                    info!("Building schedule for {name}");
293                    if scheduler.handoff == Handoff::Overlap {
294                        warn!("Overlapping measurements on {name} is no longer supported on identical epochs.");
295                    }
296                    built_cfg.get_mut(name).unwrap().scheduler = None;
297                    built_cfg.get_mut(name).unwrap().strands = Some(Vec::new());
298
299                    // Convert the trajectory into the ground station frame.
300                    let traj = self.trajectory.to_frame(device.frame, almanac.clone())?;
301
302                    match traj.find_arcs(&device, almanac.clone()) {
303                        Err(_) => info!("No measurements from {name}"),
304                        Ok(elevation_arcs) => {
305                            for arc in elevation_arcs {
306                                let strand_start = arc.rise.state.epoch();
307                                let strand_end = arc.fall.state.epoch();
308
309                                if strand_end - strand_start
310                                    < cfg.sampling * i64::from(scheduler.min_samples)
311                                {
312                                    info!(
313                                    "Too few samples from {name} opportunity from {strand_start} to {strand_end}, discarding strand",
314                                );
315                                    continue;
316                                }
317
318                                let mut strand_range = Strand {
319                                    start: strand_start,
320                                    end: strand_end,
321                                };
322
323                                // If there is an alignment, apply it
324                                if let Some(alignment) = scheduler.sample_alignment {
325                                    strand_range.start = strand_range.start.round(alignment);
326                                    strand_range.end = strand_range.end.round(alignment);
327                                }
328
329                                if let Cadence::Intermittent { on, off } = scheduler.cadence {
330                                    // Check that the next start time is within the allocated time
331                                    if let Some(prev_strand) =
332                                        built_cfg[name].strands.as_ref().unwrap().last()
333                                    {
334                                        if prev_strand.end + off > strand_range.start {
335                                            // We're turning on the tracking sooner than the schedule allows, so let's fix that.
336                                            strand_range.start = prev_strand.end + off;
337                                            // Check that we didn't eat into the whole tracking opportunity
338                                            if strand_range.start > strand_end {
339                                                // Lost this whole opportunity.
340                                                info!("Discarding {name} opportunity from {strand_start} to {strand_end} due to cadence {:?}", scheduler.cadence);
341                                                continue;
342                                            }
343                                        }
344                                    }
345                                    // Check that we aren't tracking for longer than configured
346                                    if strand_range.end - strand_range.start > on {
347                                        strand_range.end = strand_range.start + on;
348                                    }
349                                }
350
351                                // We've found when the spacecraft is below the horizon, so this is a new strand.
352                                built_cfg
353                                    .get_mut(name)
354                                    .unwrap()
355                                    .strands
356                                    .as_mut()
357                                    .unwrap()
358                                    .push(strand_range);
359                            }
360
361                            info!(
362                                "Built {} tracking strands for {name}",
363                                built_cfg[name].strands.as_ref().unwrap().len()
364                            );
365                        }
366                    }
367                }
368            }
369        }
370        // Build all of the strands, remembering which tracker they come from.
371        let mut cfg_as_vec = Vec::new();
372        for (name, cfg) in &built_cfg {
373            if let Some(strands) = &cfg.strands {
374                for (ii, strand) in strands.iter().enumerate() {
375                    cfg_as_vec.push((name.clone(), ii, *strand));
376                }
377            }
378        }
379        // Iterate through the strands by chronological order. Cannot use maps because we change types.
380        cfg_as_vec.sort_by_key(|(_, _, strand)| strand.start);
381        for (ii, (this_name, this_pos, this_strand)) in
382            cfg_as_vec.iter().take(cfg_as_vec.len() - 1).enumerate()
383        {
384            // Grab the config
385            if let Some(config) = self.configs[this_name].scheduler.as_ref() {
386                // Grab the next strand, chronologically
387                if let Some((next_name, next_pos, next_strand)) = cfg_as_vec.get(ii + 1) {
388                    if config.handoff == Handoff::Greedy && this_strand.end >= next_strand.start {
389                        // Modify the built configurations to change the start time of the next strand because the current one is greedy.
390                        let next_config = built_cfg.get_mut(next_name).unwrap();
391                        let new_start = this_strand.end + next_config.sampling;
392                        next_config.strands.as_mut().unwrap()[*next_pos].start = new_start;
393                        info!(
394                            "{this_name} configured as {:?}, so {next_name} now starts on {new_start}",
395                            config.handoff
396                        );
397                    } else if config.handoff == Handoff::Eager
398                        && this_strand.end >= next_strand.start
399                    {
400                        let this_config = built_cfg.get_mut(this_name).unwrap();
401                        let new_end = next_strand.start - this_config.sampling;
402                        this_config.strands.as_mut().unwrap()[*this_pos].end = new_end;
403                        info!(
404                            "{this_name} now hands off to {next_name} on {new_end} because it's configured as {:?}",
405                            config.handoff
406                        );
407                    }
408                } else {
409                    // Reached the end
410                    break;
411                }
412            }
413        }
414
415        Ok(built_cfg)
416    }
417
418    /// Sets the schedule to that built in `build_schedule`
419    pub fn build_schedule(&mut self, almanac: Arc<Almanac>) -> Result<(), NyxError> {
420        self.configs = self.generate_schedule(almanac)?;
421
422        Ok(())
423    }
424}