nyx_space/dynamics/guidance/
mod.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::cosmic::{GuidanceMode, Orbit, STD_GRAVITY, Spacecraft};
20use crate::errors::{NyxError, StateError};
21use crate::linalg::Vector3;
22use anise::astro::PhysicsResult;
23use anise::errors::PhysicsError;
24use anise::math::rotation::DCM;
25use anise::prelude::Almanac;
26use der::{Decode, Encode, Reader};
27use serde::{Deserialize, Serialize};
28use serde_dhall::StaticType;
29
30pub mod mnvr;
31pub use mnvr::{Maneuver, MnvrRepr};
32
33mod replay;
34pub use replay::ThrustDirectionReplay;
35
36mod ruggiero;
37pub use ruggiero::{Objective, Ruggiero, StateParameter};
38
39mod kluever;
40pub use kluever::Kluever;
41use snafu::Snafu;
42
43use std::fmt;
44use std::sync::Arc;
45
46#[cfg(feature = "python")]
47use pyo3::prelude::*;
48
49/// Defines a thruster with a maximum isp and a maximum thrust.
50#[allow(non_snake_case)]
51#[cfg_attr(feature = "python", pyclass(from_py_object, get_all, set_all))]
52#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize, StaticType)]
53pub struct Thruster {
54    /// The thrust is to be provided in Newtons
55    pub thrust_N: f64,
56    /// The Isp is to be provided in seconds
57    pub isp_s: f64,
58}
59
60#[cfg_attr(feature = "python", pymethods)]
61impl Thruster {
62    /// Returns the exhaust velocity v_e in meters per second
63    pub fn exhaust_velocity_m_s(&self) -> f64 {
64        self.isp_s * STD_GRAVITY
65    }
66}
67
68#[cfg(feature = "python")]
69#[cfg_attr(feature = "python", pymethods)]
70impl Thruster {
71    #[allow(non_snake_case)]
72    #[new]
73    fn py_new(thrust_N: f64, isp_s: f64) -> Self {
74        Self { thrust_N, isp_s }
75    }
76}
77
78impl Encode for Thruster {
79    fn encoded_len(&self) -> der::Result<der::Length> {
80        self.thrust_N.encoded_len()? + self.isp_s.encoded_len()?
81    }
82
83    fn encode(&self, encoder: &mut impl der::Writer) -> der::Result<()> {
84        self.thrust_N.encode(encoder)?;
85        self.isp_s.encode(encoder)
86    }
87}
88
89impl<'a> Decode<'a> for Thruster {
90    fn decode<R: Reader<'a>>(decoder: &mut R) -> der::Result<Self> {
91        Ok(Self {
92            thrust_N: decoder.decode()?,
93            isp_s: decoder.decode()?,
94        })
95    }
96}
97
98#[derive(Clone, Debug, Serialize, Deserialize, StaticType)]
99pub struct ObjectiveEfficiency {
100    pub objective: Objective,
101    pub efficiency: f64,
102}
103
104#[derive(Clone, Debug, Serialize, Deserialize, StaticType)]
105pub struct ObjectiveWeight {
106    pub objective: Objective,
107    pub weight: f64,
108}
109
110/// The `GuidanceLaw` trait handles guidance laws, optimizations, and other such methods for
111/// controlling the overall thrust direction when tied to a `Spacecraft`. For delta V control,
112/// tie the DeltaVctrl to a MissionArc.
113pub trait GuidanceLaw: fmt::Display + Send + Sync {
114    /// Returns a unit vector corresponding to the thrust direction in the inertial frame.
115    fn direction(&self, osc_state: &Spacecraft) -> Result<Vector3<f64>, GuidanceError>;
116
117    /// Returns a number between [0;1] corresponding to the engine throttle level.
118    /// For example, 0 means coasting, i.e. no thrusting, and 1 means maximum thrusting.
119    fn throttle(&self, osc_state: &Spacecraft) -> Result<f64, GuidanceError>;
120
121    /// Updates the state of the BaseSpacecraft for the next maneuver, e.g. prepares the controller for the next maneuver
122    fn next(&self, next_state: &mut Spacecraft, almanac: Arc<Almanac>);
123
124    /// Returns whether this thrust control has been achieved, if it has an objective
125    fn achieved(&self, _osc_state: &Spacecraft) -> Result<bool, GuidanceError> {
126        Err(GuidanceError::NoGuidanceObjectiveDefined)
127    }
128}
129
130/// Converts the alpha (in-plane) and beta (out-of-plane) angles in the RCN frame to the unit vector in the RCN frame
131fn unit_vector_from_plane_angles(alpha: f64, beta: f64) -> Vector3<f64> {
132    Vector3::new(
133        alpha.sin() * beta.cos(),
134        alpha.cos() * beta.cos(),
135        beta.sin(),
136    )
137}
138
139/// Converts the provided unit vector into in-plane and out-of-plane angles, returned in radians
140pub fn plane_angles_from_unit_vector(vhat: Vector3<f64>) -> (f64, f64) {
141    (vhat[1].atan2(vhat[0]), vhat[2].asin())
142}
143
144/// Converts the alpha (in-plane) and beta (out-of-plane) angles in the RCN frame to the unit vector
145pub(crate) fn unit_vector_from_ra_dec(alpha: f64, delta: f64) -> Vector3<f64> {
146    Vector3::new(
147        delta.cos() * alpha.cos(),
148        delta.cos() * alpha.sin(),
149        delta.sin(),
150    )
151}
152
153/// Converts the provided unit vector into in-plane and out-of-plane angles, returned in radians
154pub(crate) fn ra_dec_from_unit_vector(vhat: Vector3<f64>) -> (f64, f64) {
155    let alpha = vhat[1].atan2(vhat[0]);
156    let delta = vhat[2].asin();
157    (alpha, delta)
158}
159
160#[derive(Debug, PartialEq, Snafu)]
161pub enum GuidanceError {
162    #[snafu(display("No thruster attached to spacecraft"))]
163    NoThrustersDefined,
164    #[snafu(display("Throttle is not between 0.0 and 1.0: {ratio}"))]
165    ThrottleRatio { ratio: f64 },
166    #[snafu(display(
167        "Invalid finite burn control direction u = [{x}, {y}, {z}] => i-plane = {in_plane_deg} deg, Delta = {out_of_plane_deg} deg",
168    ))]
169    InvalidDirection {
170        x: f64,
171        y: f64,
172        z: f64,
173        in_plane_deg: f64,
174        out_of_plane_deg: f64,
175    },
176    #[snafu(display(
177        "Invalid finite burn control rate u = [{x}, {y}, {z}] => in-plane = {in_plane_deg_s} deg/s, out of plane = {out_of_plane_deg_s} deg/s",
178    ))]
179    InvalidRate {
180        x: f64,
181        y: f64,
182        z: f64,
183        in_plane_deg_s: f64,
184        out_of_plane_deg_s: f64,
185    },
186    #[snafu(display(
187        "Invalid finite burn control acceleration u = [{x}, {y}, {z}] => in-plane = {in_plane_deg_s2} deg/s^2, out of plane = {out_of_plane_deg_s2} deg/s^2",
188    ))]
189    InvalidAcceleration {
190        x: f64,
191        y: f64,
192        z: f64,
193        in_plane_deg_s2: f64,
194        out_of_plane_deg_s2: f64,
195    },
196    #[snafu(display("when {action} encountered {source}"))]
197    GuidancePhysicsError {
198        action: &'static str,
199        source: PhysicsError,
200    },
201    #[snafu(display(
202        "An objective based analysis or control was attempted, but no objective was defined"
203    ))]
204    NoGuidanceObjectiveDefined,
205    #[snafu(display("{param} is not a control variable in this guidance law"))]
206    InvalidControl { param: StateParameter },
207    #[snafu(display("guidance encountered {source}"))]
208    GuidState { source: StateError },
209}
210
211/// Local frame options, used notably for guidance laws.
212/// TODO: Replace with ANISE enum, which is identical, but needs Serialize/Deserialize implemented.
213/// https://github.com/nyx-space/anise/issues/725
214#[derive(Copy, Clone, Debug, PartialEq, Eq, Serialize, Deserialize, StaticType)]
215#[cfg_attr(feature = "python", pyclass(from_py_object))]
216pub enum LocalFrame {
217    Inertial,
218    RIC,
219    VNC,
220    RCN,
221}
222
223impl LocalFrame {
224    pub fn dcm_to_inertial(&self, state: Orbit) -> PhysicsResult<DCM> {
225        match self {
226            LocalFrame::Inertial => Ok(DCM::identity(
227                state.frame.orientation_id,
228                state.frame.orientation_id,
229            )),
230            LocalFrame::RIC => state.dcm_from_ric_to_inertial(),
231            LocalFrame::VNC => state.dcm_from_vnc_to_inertial(),
232            LocalFrame::RCN => state.dcm_from_rcn_to_inertial(),
233        }
234    }
235}
236
237#[test]
238fn ra_dec_from_vec() {
239    use std::f64::consts::{FRAC_PI_2, PI, TAU};
240    let mut delta = -FRAC_PI_2;
241    let mut alpha = 0.0;
242    loop {
243        loop {
244            let unit_v = unit_vector_from_ra_dec(alpha, delta);
245            let (alpha2, delta2) = ra_dec_from_unit_vector(unit_v);
246            assert!((alpha - alpha2).abs() < f64::EPSILON);
247            assert!((delta - delta2).abs() < f64::EPSILON);
248            alpha += TAU * 0.1; // Increment right ascension by one tenth of a circle
249            if alpha > PI {
250                alpha = 0.0;
251                break;
252            }
253        }
254        delta += TAU * 0.1; // Increment declination by one tenth of a circle
255        if delta > FRAC_PI_2 {
256            break;
257        }
258    }
259}
nyx_space/dynamics/guidance/mod.rs

nyx_space/dynamics/guidance/
mod.rs
