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}