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

nyx_space/dynamics/guidance/
mod.rs
