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

nyx_space/dynamics/guidance/
mod.rs
