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) α: {azimuth}\n\televation (out-of-plane) β: {elevation}"
105            ),
106        }
107    }
108}
109
110impl Maneuver {
111    /// Creates an impulsive maneuver whose vector is the deltaV.
112    /// TODO: This should use William's algorithm
113    pub fn from_impulsive(dt: Epoch, vector: Vector3<f64>, frame: LocalFrame) -> Self {
114        Self::from_time_invariant(dt, dt + Unit::Millisecond, 1.0, vector, frame)
115    }
116
117    /// Creates a maneuver from the provided time-invariant delta-v, in km/s
118    pub fn from_time_invariant(
119        start: Epoch,
120        end: Epoch,
121        thrust_lvl: f64,
122        vector: Vector3<f64>,
123        frame: LocalFrame,
124    ) -> Self {
125        Self {
126            start,
127            end,
128            thrust_prct: thrust_lvl,
129            representation: MnvrRepr::Vector(vector),
130            frame,
131        }
132    }
133
134    /// Return the thrust vector computed at the provided epoch
135    pub fn vector(&self, epoch: Epoch) -> Vector3<f64> {
136        match self.representation {
137            MnvrRepr::Vector(vector) => vector,
138            MnvrRepr::Angles { azimuth, elevation } => {
139                let t = (epoch - self.start).to_seconds();
140                let alpha = azimuth.eval(t);
141                let delta = elevation.eval(t);
142                unit_vector_from_ra_dec(alpha, delta)
143            }
144        }
145    }
146
147    /// Return the duration of this maneuver
148    pub fn duration(&self) -> Duration {
149        self.end - self.start
150    }
151
152    /// Return whether this is an antichronological maneuver
153    pub fn antichronological(&self) -> bool {
154        self.duration().abs() > 1.microseconds() && self.duration() < 1.microseconds()
155    }
156
157    /// Returns the direction of the burn at the start of the burn, useful for setting new angles
158    pub fn direction(&self) -> Vector3<f64> {
159        match self.representation {
160            MnvrRepr::Vector(vector) => vector / vector.norm(),
161            MnvrRepr::Angles { azimuth, elevation } => {
162                let alpha = azimuth.coeff_in_order(0).unwrap();
163                let delta = elevation.coeff_in_order(0).unwrap();
164                unit_vector_from_ra_dec(alpha, delta)
165            }
166        }
167    }
168
169    /// Set the time-invariant direction for this finite burn while keeping the other components as they are
170    pub fn set_direction(&mut self, vector: Vector3<f64>) -> Result<(), GuidanceError> {
171        self.set_direction_and_rates(vector, self.rate(), self.accel())
172    }
173
174    /// Returns the rate of direction of the burn at the start of the burn, useful for setting new angles
175    pub fn rate(&self) -> Vector3<f64> {
176        match self.representation {
177            MnvrRepr::Vector(_) => Vector3::zeros(),
178            MnvrRepr::Angles { azimuth, elevation } => match azimuth.coeff_in_order(1) {
179                Ok(alpha) => {
180                    let delta = elevation.coeff_in_order(1).unwrap();
181                    unit_vector_from_ra_dec(alpha, delta)
182                }
183                Err(_) => Vector3::zeros(),
184            },
185        }
186    }
187
188    /// Set the rate of direction for this finite burn while keeping the other components as they are
189    pub fn set_rate(&mut self, rate: Vector3<f64>) -> Result<(), GuidanceError> {
190        self.set_direction_and_rates(self.direction(), rate, self.accel())
191    }
192
193    /// Returns the acceleration of the burn at the start of the burn, useful for setting new angles
194    pub fn accel(&self) -> Vector3<f64> {
195        match self.representation {
196            MnvrRepr::Vector(_) => Vector3::zeros(),
197            MnvrRepr::Angles { azimuth, elevation } => match azimuth.coeff_in_order(2) {
198                Ok(alpha) => {
199                    let delta = elevation.coeff_in_order(2).unwrap();
200                    unit_vector_from_ra_dec(alpha, delta)
201                }
202                Err(_) => Vector3::zeros(),
203            },
204        }
205    }
206
207    /// Set the acceleration of the direction of this finite burn while keeping the other components as they are
208    pub fn set_accel(&mut self, accel: Vector3<f64>) -> Result<(), GuidanceError> {
209        self.set_direction_and_rates(self.direction(), self.rate(), accel)
210    }
211
212    /// Set the initial direction, direction rate, and direction acceleration for this finite burn
213    pub fn set_direction_and_rates(
214        &mut self,
215        dir: Vector3<f64>,
216        rate: Vector3<f64>,
217        accel: Vector3<f64>,
218    ) -> Result<(), GuidanceError> {
219        if rate.norm() < f64::EPSILON && accel.norm() < f64::EPSILON {
220            // Set as a vector
221            self.representation = MnvrRepr::Vector(dir)
222        } else {
223            let (alpha, delta) = ra_dec_from_unit_vector(dir);
224            if alpha.is_nan() || delta.is_nan() {
225                return Err(GuidanceError::InvalidDirection {
226                    x: dir[0],
227                    y: dir[1],
228                    z: dir[2],
229                    in_plane_deg: alpha.to_degrees(),
230                    out_of_plane_deg: delta.to_degrees(),
231                });
232            }
233            if rate.norm() < f64::EPSILON && accel.norm() < f64::EPSILON {
234                self.representation = MnvrRepr::Angles {
235                    azimuth: CommonPolynomial::Constant(alpha),
236                    elevation: CommonPolynomial::Constant(delta),
237                };
238            } else {
239                let (alpha_dt, delta_dt) = ra_dec_from_unit_vector(rate);
240                if alpha_dt.is_nan() || delta_dt.is_nan() {
241                    return Err(GuidanceError::InvalidRate {
242                        x: rate[0],
243                        y: rate[1],
244                        z: rate[2],
245                        in_plane_deg_s: alpha_dt.to_degrees(),
246                        out_of_plane_deg_s: delta_dt.to_degrees(),
247                    });
248                }
249                if accel.norm() < f64::EPSILON {
250                    self.representation = MnvrRepr::Angles {
251                        azimuth: CommonPolynomial::Linear(alpha_dt, alpha),
252                        elevation: CommonPolynomial::Linear(delta_dt, delta),
253                    };
254                } else {
255                    let (alpha_ddt, delta_ddt) = ra_dec_from_unit_vector(accel);
256                    if alpha_ddt.is_nan() || delta_ddt.is_nan() {
257                        return Err(GuidanceError::InvalidAcceleration {
258                            x: accel[0],
259                            y: accel[1],
260                            z: accel[2],
261                            in_plane_deg_s2: alpha_ddt.to_degrees(),
262                            out_of_plane_deg_s2: delta_ddt.to_degrees(),
263                        });
264                    }
265
266                    self.representation = MnvrRepr::Angles {
267                        azimuth: CommonPolynomial::Quadratic(alpha_ddt, alpha_dt, alpha),
268                        elevation: CommonPolynomial::Quadratic(delta_ddt, delta_dt, delta),
269                    };
270                }
271            }
272        }
273        Ok(())
274    }
275}
276
277impl GuidanceLaw for Maneuver {
278    fn direction(&self, osc: &Spacecraft) -> Result<Vector3<f64>, GuidanceError> {
279        match osc.mode() {
280            GuidanceMode::Thrust => match self.frame {
281                LocalFrame::Inertial => Ok(self.vector(osc.epoch())),
282                _ => Ok(self.frame.dcm_to_inertial(osc.orbit).context({
283                    GuidancePhysicsSnafu {
284                        action: "computing RCN frame",
285                    }
286                })? * self.vector(osc.epoch())),
287            },
288            _ => Ok(Vector3::zeros()),
289        }
290    }
291
292    fn throttle(&self, osc: &Spacecraft) -> Result<f64, GuidanceError> {
293        // match self.next(osc) {
294        match osc.mode() {
295            GuidanceMode::Thrust => Ok(self.thrust_prct),
296            _ => {
297                // We aren't in maneuver mode, so return 0% throttle
298                Ok(0.0)
299            }
300        }
301    }
302
303    fn next(&self, sc: &mut Spacecraft, _almanac: Arc<Almanac>) {
304        let next_mode = if sc.epoch() >= self.start && sc.epoch() < self.end {
305            GuidanceMode::Thrust
306        } else {
307            GuidanceMode::Coast
308        };
309        sc.mut_mode(next_mode);
310    }
311}
312
313#[cfg(test)]
314mod ut_mnvr {
315    use hifitime::Epoch;
316    use nalgebra::Vector3;
317
318    use crate::dynamics::guidance::LocalFrame;
319
320    use super::Maneuver;
321
322    #[test]
323    fn serde_mnvr() {
324        let epoch = Epoch::from_gregorian_utc_at_midnight(2012, 2, 29);
325        let mnvr = Maneuver::from_impulsive(epoch, Vector3::new(1.0, 1.0, 0.0), LocalFrame::RCN);
326
327        let mnvr_yml = serde_yml::to_string(&mnvr).unwrap();
328        println!("{mnvr_yml}");
329
330        let mnvr2 = serde_yml::from_str(&mnvr_yml).unwrap();
331        assert_eq!(mnvr, mnvr2);
332    }
333}
nyx_space/dynamics/guidance/mnvr.rs

nyx_space/dynamics/guidance/
mnvr.rs
