nyx_space/dynamics/guidance/
mnvr.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::{
20    ra_dec_from_unit_vector, GuidanceError, GuidanceLaw, GuidancePhysicsSnafu, LocalFrame,
21};
22use crate::cosmic::{GuidanceMode, Spacecraft};
23use crate::dynamics::guidance::unit_vector_from_ra_dec;
24use crate::linalg::Vector3;
25use crate::polyfit::CommonPolynomial;
26use crate::time::{Epoch, Unit};
27use crate::State;
28use anise::prelude::Almanac;
29use hifitime::{Duration, TimeUnits};
30use serde::{Deserialize, Serialize};
31use snafu::ResultExt;
32use std::fmt;
33use std::sync::Arc;
34
35/// Mnvr defined a single maneuver. Direction MUST be in the VNC frame (Velocity / Normal / Cross).
36/// It may be used with a maneuver scheduler.
37#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
38pub struct Maneuver {
39    /// Start epoch of the maneuver
40    pub start: Epoch,
41    /// End epoch of the maneuver
42    pub end: Epoch,
43    /// TODO: Add a thruster group set to specify which set of thrusters to use for this maneuver, should be a key to a thruster (maybe change thruster to a hashmap actually now that I don't care about embedded stuff).
44    /// Thrust level, if 1.0 use all thruster available at full power
45    /// TODO: Convert this to a common polynomial as well to optimize throttle, throttle rate (and accel?)
46    pub thrust_prct: f64,
47    /// The representation of this maneuver.
48    pub representation: MnvrRepr,
49    /// The frame in which the maneuvers are defined.
50    pub frame: LocalFrame,
51}
52
53impl fmt::Display for Maneuver {
54    /// Prints the polynomial with the least significant coefficients first
55    #[allow(clippy::identity_op)]
56    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
57        if self.end != self.start {
58            let start_vec = self.vector(self.start);
59            let end_vec = self.vector(self.end);
60            write!(
61                f,
62                "Finite burn maneuver @ {:.2}% on {} for {} (ending on {})",
63                100.0 * self.thrust_prct,
64                self.start,
65                self.end - self.start,
66                self.end,
67            )?;
68            write!(f, "\n{}", self.representation)?;
69            write!(
70                f,
71                "\n\tinitial dir: [{:.6}, {:.6}, {:.6}]\n\tfinal dir  : [{:.6}, {:.6}, {:.6}]",
72                start_vec[0], start_vec[1], start_vec[2], end_vec[0], end_vec[1], end_vec[2]
73            )
74        } else {
75            write!(
76                f,
77                "Impulsive maneuver @ {}\n{}",
78                self.start, self.representation
79            )
80        }
81    }
82}
83
84/// Defines the available maneuver representations.
85#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
86pub enum MnvrRepr {
87    /// Represents the maneuver as a fixed vector in the local frame.
88    Vector(Vector3<f64>),
89    /// Represents the maneuver as a polynominal of azimuth (right ascension / in-plane) and elevation (declination / out of plane)
90    Angles {
91        azimuth: CommonPolynomial,
92        elevation: CommonPolynomial,
93    },
94}
95
96impl fmt::Display for MnvrRepr {
97    /// Prints the polynomial with the least significant coefficients first
98    #[allow(clippy::identity_op)]
99    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
100        match self {
101            MnvrRepr::Vector(vector) => write!(f, "{vector}"),
102            MnvrRepr::Angles { azimuth, elevation } => write!(
103                f,
104                "\tazimuth (in-plane) α: {}\n\televation (out-of-plane) β: {}",
105                azimuth, elevation
106            ),
107        }
108    }
109}
110
111impl Maneuver {
112    /// Creates an impulsive maneuver whose vector is the deltaV.
113    /// TODO: This should use William's algorithm
114    pub fn from_impulsive(dt: Epoch, vector: Vector3<f64>, frame: LocalFrame) -> Self {
115        Self::from_time_invariant(dt, dt + Unit::Millisecond, 1.0, vector, frame)
116    }
117
118    /// Creates a maneuver from the provided time-invariant delta-v, in km/s
119    pub fn from_time_invariant(
120        start: Epoch,
121        end: Epoch,
122        thrust_lvl: f64,
123        vector: Vector3<f64>,
124        frame: LocalFrame,
125    ) -> Self {
126        Self {
127            start,
128            end,
129            thrust_prct: thrust_lvl,
130            representation: MnvrRepr::Vector(vector),
131            frame,
132        }
133    }
134
135    /// Return the thrust vector computed at the provided epoch
136    pub fn vector(&self, epoch: Epoch) -> Vector3<f64> {
137        match self.representation {
138            MnvrRepr::Vector(vector) => vector,
139            MnvrRepr::Angles { azimuth, elevation } => {
140                let t = (epoch - self.start).to_seconds();
141                let alpha = azimuth.eval(t);
142                let delta = elevation.eval(t);
143                unit_vector_from_ra_dec(alpha, delta)
144            }
145        }
146    }
147
148    /// Return the duration of this maneuver
149    pub fn duration(&self) -> Duration {
150        self.end - self.start
151    }
152
153    /// Return whether this is an antichronological maneuver
154    pub fn antichronological(&self) -> bool {
155        self.duration().abs() > 1.microseconds() && self.duration() < 1.microseconds()
156    }
157
158    /// Returns the direction of the burn at the start of the burn, useful for setting new angles
159    pub fn direction(&self) -> Vector3<f64> {
160        match self.representation {
161            MnvrRepr::Vector(vector) => vector / vector.norm(),
162            MnvrRepr::Angles { azimuth, elevation } => {
163                let alpha = azimuth.coeff_in_order(0).unwrap();
164                let delta = elevation.coeff_in_order(0).unwrap();
165                unit_vector_from_ra_dec(alpha, delta)
166            }
167        }
168    }
169
170    /// Set the time-invariant direction for this finite burn while keeping the other components as they are
171    pub fn set_direction(&mut self, vector: Vector3<f64>) -> Result<(), GuidanceError> {
172        self.set_direction_and_rates(vector, self.rate(), self.accel())
173    }
174
175    /// Returns the rate of direction of the burn at the start of the burn, useful for setting new angles
176    pub fn rate(&self) -> Vector3<f64> {
177        match self.representation {
178            MnvrRepr::Vector(_) => Vector3::zeros(),
179            MnvrRepr::Angles { azimuth, elevation } => match azimuth.coeff_in_order(1) {
180                Ok(alpha) => {
181                    let delta = elevation.coeff_in_order(1).unwrap();
182                    unit_vector_from_ra_dec(alpha, delta)
183                }
184                Err(_) => Vector3::zeros(),
185            },
186        }
187    }
188
189    /// Set the rate of direction for this finite burn while keeping the other components as they are
190    pub fn set_rate(&mut self, rate: Vector3<f64>) -> Result<(), GuidanceError> {
191        self.set_direction_and_rates(self.direction(), rate, self.accel())
192    }
193
194    /// Returns the acceleration of the burn at the start of the burn, useful for setting new angles
195    pub fn accel(&self) -> Vector3<f64> {
196        match self.representation {
197            MnvrRepr::Vector(_) => Vector3::zeros(),
198            MnvrRepr::Angles { azimuth, elevation } => match azimuth.coeff_in_order(2) {
199                Ok(alpha) => {
200                    let delta = elevation.coeff_in_order(2).unwrap();
201                    unit_vector_from_ra_dec(alpha, delta)
202                }
203                Err(_) => Vector3::zeros(),
204            },
205        }
206    }
207
208    /// Set the acceleration of the direction of this finite burn while keeping the other components as they are
209    pub fn set_accel(&mut self, accel: Vector3<f64>) -> Result<(), GuidanceError> {
210        self.set_direction_and_rates(self.direction(), self.rate(), accel)
211    }
212
213    /// Set the initial direction, direction rate, and direction acceleration for this finite burn
214    pub fn set_direction_and_rates(
215        &mut self,
216        dir: Vector3<f64>,
217        rate: Vector3<f64>,
218        accel: Vector3<f64>,
219    ) -> Result<(), GuidanceError> {
220        if rate.norm() < f64::EPSILON && accel.norm() < f64::EPSILON {
221            // Set as a vector
222            self.representation = MnvrRepr::Vector(dir)
223        } else {
224            let (alpha, delta) = ra_dec_from_unit_vector(dir);
225            if alpha.is_nan() || delta.is_nan() {
226                return Err(GuidanceError::InvalidDirection {
227                    x: dir[0],
228                    y: dir[1],
229                    z: dir[2],
230                    in_plane_deg: alpha.to_degrees(),
231                    out_of_plane_deg: delta.to_degrees(),
232                });
233            }
234            if rate.norm() < f64::EPSILON && accel.norm() < f64::EPSILON {
235                self.representation = MnvrRepr::Angles {
236                    azimuth: CommonPolynomial::Constant(alpha),
237                    elevation: CommonPolynomial::Constant(delta),
238                };
239            } else {
240                let (alpha_dt, delta_dt) = ra_dec_from_unit_vector(rate);
241                if alpha_dt.is_nan() || delta_dt.is_nan() {
242                    return Err(GuidanceError::InvalidRate {
243                        x: rate[0],
244                        y: rate[1],
245                        z: rate[2],
246                        in_plane_deg_s: alpha_dt.to_degrees(),
247                        out_of_plane_deg_s: delta_dt.to_degrees(),
248                    });
249                }
250                if accel.norm() < f64::EPSILON {
251                    self.representation = MnvrRepr::Angles {
252                        azimuth: CommonPolynomial::Linear(alpha_dt, alpha),
253                        elevation: CommonPolynomial::Linear(delta_dt, delta),
254                    };
255                } else {
256                    let (alpha_ddt, delta_ddt) = ra_dec_from_unit_vector(accel);
257                    if alpha_ddt.is_nan() || delta_ddt.is_nan() {
258                        return Err(GuidanceError::InvalidAcceleration {
259                            x: accel[0],
260                            y: accel[1],
261                            z: accel[2],
262                            in_plane_deg_s2: alpha_ddt.to_degrees(),
263                            out_of_plane_deg_s2: delta_ddt.to_degrees(),
264                        });
265                    }
266
267                    self.representation = MnvrRepr::Angles {
268                        azimuth: CommonPolynomial::Quadratic(alpha_ddt, alpha_dt, alpha),
269                        elevation: CommonPolynomial::Quadratic(delta_ddt, delta_dt, delta),
270                    };
271                }
272            }
273        }
274        Ok(())
275    }
276}
277
278impl GuidanceLaw for Maneuver {
279    fn direction(&self, osc: &Spacecraft) -> Result<Vector3<f64>, GuidanceError> {
280        match osc.mode() {
281            GuidanceMode::Thrust => match self.frame {
282                LocalFrame::Inertial => Ok(self.vector(osc.epoch())),
283                _ => Ok(self.frame.dcm_to_inertial(osc.orbit).context({
284                    GuidancePhysicsSnafu {
285                        action: "computing RCN frame",
286                    }
287                })? * self.vector(osc.epoch())),
288            },
289            _ => Ok(Vector3::zeros()),
290        }
291    }
292
293    fn throttle(&self, osc: &Spacecraft) -> Result<f64, GuidanceError> {
294        // match self.next(osc) {
295        match osc.mode() {
296            GuidanceMode::Thrust => Ok(self.thrust_prct),
297            _ => {
298                // We aren't in maneuver mode, so return 0% throttle
299                Ok(0.0)
300            }
301        }
302    }
303
304    fn next(&self, sc: &mut Spacecraft, _almanac: Arc<Almanac>) {
305        let next_mode = if sc.epoch() >= self.start && sc.epoch() < self.end {
306            GuidanceMode::Thrust
307        } else {
308            GuidanceMode::Coast
309        };
310        sc.mut_mode(next_mode);
311    }
312}
313
314#[cfg(test)]
315mod ut_mnvr {
316    use hifitime::Epoch;
317    use nalgebra::Vector3;
318
319    use crate::dynamics::guidance::LocalFrame;
320
321    use super::Maneuver;
322
323    #[test]
324    fn serde_mnvr() {
325        let epoch = Epoch::from_gregorian_utc_at_midnight(2012, 2, 29);
326        let mnvr = Maneuver::from_impulsive(epoch, Vector3::new(1.0, 1.0, 0.0), LocalFrame::RCN);
327
328        let mnvr_yml = serde_yml::to_string(&mnvr).unwrap();
329        println!("{mnvr_yml}");
330
331        let mnvr2 = serde_yml::from_str(&mnvr_yml).unwrap();
332        assert_eq!(mnvr, mnvr2);
333    }
334}
nyx_space/dynamics/guidance/mnvr.rs

nyx_space/dynamics/guidance/
mnvr.rs
