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

nyx_space/dynamics/guidance/
mnvr.rs
