nyx_space/dynamics/guidance/
replay.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 super::{GuidanceError, GuidanceLaw};
20use crate::cosmic::{GuidanceMode, Spacecraft};
21use crate::linalg::Vector3;
22use crate::md::Trajectory;
23use crate::State;
24use anise::prelude::Almanac;
25use std::fmt;
26use std::sync::Arc;
27
28/// Replays an inertial thrust-direction profile stored on a spacecraft trajectory.
29///
30/// The command is held piecewise-constant between stored trajectory samples.
31#[derive(Clone)]
32pub struct ThrustDirectionReplay {
33    pub profile: Trajectory,
34}
35
36impl ThrustDirectionReplay {
37    pub fn from_trajectory(profile: Trajectory) -> Arc<Self> {
38        Arc::new(Self { profile })
39    }
40
41    fn command_at_or_before(&self, state: &Spacecraft) -> Option<&Spacecraft> {
42        if self.profile.states.is_empty() {
43            return None;
44        }
45
46        let epoch = state.epoch();
47        if epoch < self.profile.first().epoch() || epoch > self.profile.last().epoch() {
48            return None;
49        }
50
51        let command = match self
52            .profile
53            .states
54            .binary_search_by(|sample| sample.epoch().cmp(&epoch))
55        {
56            Ok(idx) => self.profile.states.get(idx),
57            Err(0) => None,
58            Err(idx) => self.profile.states.get(idx - 1),
59        };
60
61        if let Some(command) = command {
62            if command.thrust_direction().is_some() || command.mode() != GuidanceMode::Thrust {
63                return Some(command);
64            }
65        }
66
67        if self.profile.first().mode() == GuidanceMode::Thrust {
68            return self
69                .profile
70                .states
71                .iter()
72                .find(|sample| sample.epoch() >= epoch && sample.thrust_direction().is_some());
73        }
74
75        command
76    }
77}
78
79impl fmt::Display for ThrustDirectionReplay {
80    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
81        write!(
82            f,
83            "Thrust direction replay from {} to {} ({} samples)",
84            self.profile.first().epoch(),
85            self.profile.last().epoch(),
86            self.profile.states.len()
87        )
88    }
89}
90
91impl GuidanceLaw for ThrustDirectionReplay {
92    fn direction(&self, osc_state: &Spacecraft) -> Result<Vector3<f64>, GuidanceError> {
93        Ok(self
94            .command_at_or_before(osc_state)
95            .and_then(|sample| sample.thrust_direction())
96            .unwrap_or_else(Vector3::zeros))
97    }
98
99    fn throttle(&self, osc_state: &Spacecraft) -> Result<f64, GuidanceError> {
100        Ok(
101            if self
102                .command_at_or_before(osc_state)
103                .and_then(|sample| sample.thrust_direction())
104                .is_some()
105            {
106                1.0
107            } else {
108                0.0
109            },
110        )
111    }
112
113    fn next(&self, next_state: &mut Spacecraft, _almanac: Arc<Almanac>) {
114        let thrust_direction = self
115            .command_at_or_before(next_state)
116            .and_then(|sample| sample.thrust_direction());
117        next_state.mut_thrust_direction(thrust_direction);
118        next_state.mut_mode(if thrust_direction.is_some() {
119            GuidanceMode::Thrust
120        } else {
121            GuidanceMode::Coast
122        });
123    }
124
125    fn achieved(&self, osc_state: &Spacecraft) -> Result<bool, GuidanceError> {
126        Ok(osc_state.epoch() > self.profile.last().epoch())
127    }
128}
nyx_space/dynamics/guidance/replay.rs

nyx_space/dynamics/guidance/
replay.rs
