melvin_ob/flight_control/orbit/
orbit_base.rs

1use crate::flight_control::FlightComputer;
2use crate::imaging::CameraAngle;
3use crate::util::{helpers::*, Vec2D, MapSize};
4use chrono::{DateTime, Utc};
5use fixed::types::I32F32;
6
7/// Struct representing the base properties of an orbit.
8#[derive(serde::Serialize, serde::Deserialize, Debug)]
9pub struct OrbitBase {
10    /// The timestamp of the orbit initialization.
11    init_timestamp: DateTime<Utc>,
12    /// The initial position (in fixed-point coordinates) of the object in orbit.
13    fp: Vec2D<I32F32>,
14    /// The velocity vector (in fixed-point coordinates) of the object in orbit.
15    vel: Vec2D<I32F32>,
16}
17
18impl OrbitBase {
19    /// Simulation time step used for calculations.
20    const SIM_TIMESTEP: I32F32 = I32F32::lit("0.5");
21
22    /// Creates a new [`OrbitBase`] instance based on the current state of the flight computer.
23    ///
24    /// # Arguments
25    /// - `cont`: Reference to the [`FlightComputer`] to initialize the orbit data.
26    ///
27    /// # Returns
28    /// - A new [`OrbitBase`] instance.
29    #[allow(clippy::cast_possible_truncation)]
30    pub fn new(cont: &FlightComputer) -> Self {
31        let vel = cont.current_vel();
32        Self {
33            init_timestamp: Utc::now(),
34            fp: cont.current_pos(),
35            vel,
36        }
37    }
38    
39    /// Test initialize an orbit base
40    #[cfg(test)]
41    pub fn test(pos: Vec2D<I32F32>, vel: Vec2D<I32F32>) -> Self {
42        Self {
43            init_timestamp: Utc::now(),
44            fp: pos,
45            vel
46        }
47    }
48
49    /// Returns the timestamp when the orbit was initialized.
50    ///
51    /// # Returns
52    /// - A `chrono::DateTime` with the UTC initialization timestamp.
53    pub fn start_timestamp(&self) -> DateTime<Utc> { self.init_timestamp }
54
55    /// Calculates the period of the orbit along with the individual periods in the x and y
56    /// directions.
57    ///
58    /// # Returns
59    /// - `Some((tts, t_x, t_y))`: The total orbit period (time to full repeat) and the x/y periods.
60    /// - `None`: If the orbit period cannot be determined.
61    #[allow(clippy::cast_possible_truncation)]
62    pub fn period(&self) -> Option<(I32F32, I32F32, I32F32)> {
63        let gcd_x = gcd_fixed64(self.vel.x(), Vec2D::map_size().x(), MAX_DEC);
64        let gcd_y = gcd_fixed64(self.vel.y(), Vec2D::map_size().y(), MAX_DEC);
65        let t_x = Vec2D::<I32F32>::map_size().x() / gcd_x;
66        let t_y = Vec2D::<I32F32>::map_size().y() / gcd_y;
67        let tts = lcm_fixed64(t_x, t_y);
68        let disp_x = self.vel.x() * tts;
69        let disp_y = self.vel.y() * tts;
70        let disp = Vec2D::new(disp_x, disp_y);
71
72        let mut resulting_point = self.fp + disp;
73        resulting_point = resulting_point.wrap_around_map();
74        let resulting_dist = (resulting_point - self.fp).abs();
75        let delta = I32F32::DELTA * tts;
76        if resulting_dist.round() <= delta {
77            Some((tts.round(), t_x.round(), t_y.round()))
78        } else {
79            None
80        }
81    }
82
83    /// Calculates the maximum time between image captures ensuring sufficient area overlap.
84    ///
85    /// # Arguments
86    /// - `used_lens`: The camera lens configuration (field of view).
87    /// - `periods`: A tuple containing the orbit periods `(tts, t_x, t_y)`.
88    ///
89    /// # Returns
90    /// - `Some(max_dt)`: Maximum allowable time interval for image captures.
91    /// - `None`: If the overlap conditions are not satisfied.
92    #[allow(clippy::cast_possible_truncation)]
93    pub fn max_image_dt(
94        &self,
95        used_lens: CameraAngle,
96        periods: (I32F32, I32F32, I32F32),
97    ) -> Option<I32F32> {
98        let wraps_x = periods.0 / periods.1;
99        let wraps_y = periods.0 / periods.2;
100        let img_side_length = I32F32::from_num(used_lens.get_square_side_length());
101        let ver_wrap_hor_dist = Vec2D::<I32F32>::map_size().y() / wraps_x;
102        let hor_wrap_ver_dist = Vec2D::<I32F32>::map_size().x() / wraps_y;
103
104        let dominant_vel = self.vel.x().max(self.vel.y());
105        let overlap_hor = ver_wrap_hor_dist - (img_side_length / I32F32::lit("2.0"));
106        let overlap_ver = hor_wrap_ver_dist - (img_side_length / I32F32::lit("2.0"));
107        if overlap_hor < img_side_length || overlap_ver < img_side_length {
108            if self.vel.x() / Vec2D::<I32F32>::map_size().x()
109                < self.vel.y() / Vec2D::<I32F32>::map_size().y()
110            {
111                Some(
112                    (overlap_hor / self.vel.x())
113                        .min((img_side_length / I32F32::lit("2.0")) / dominant_vel),
114                )
115            } else {
116                Some(
117                    (overlap_ver / self.vel.y())
118                        .min((img_side_length / I32F32::lit("2.0")) / dominant_vel),
119                )
120            }
121        } else {
122            None
123        }
124    }
125
126    /// Returns the footpoint of the orbit base
127    pub fn fp(&self) -> &Vec2D<I32F32> { &self.fp }
128    /// Returns the constant velocity of the orbit base
129    pub fn vel(&self) -> &Vec2D<I32F32> { &self.vel }
130}
melvin_ob/flight_control/orbit/orbit_base.rs

melvin_ob/flight_control/orbit/
orbit_base.rs
