melvin_ob/flight_control/orbit/
burn_sequence.rs

1use super::index::IndexedOrbitPosition;
2use crate::util::{Vec2D, helpers};
3use crate::flight_control::{FlightComputer,
4    flight_computer::TurnsClockCClockTup, FlightState,
5};
6use crate::scheduling::TaskController;
7use chrono::{TimeDelta, Utc};
8use fixed::types::I32F32;
9use num::Zero;
10use crate::util::logger::JsonDump;
11
12/// Represents a sequence of corrective burns for orbital adjustments.
13///
14/// The [`BurnSequence`] contains position and velocity sequences, along with
15/// timing and cost information, for controlling orbit behavior.
16#[derive(Debug, Clone, serde::Serialize)]
17pub struct BurnSequence {
18    /// The orbital position where the sequence starts.
19    start_i: IndexedOrbitPosition,
20    /// The sequence of positional corrections.
21    sequence_pos: Box<[Vec2D<I32F32>]>,
22    /// The sequence of velocity corrections.
23    sequence_vel: Box<[Vec2D<I32F32>]>,
24    /// Acceleration time in seconds.
25    acc_dt: usize,
26    /// Time duration for detumbling after acceleration, in seconds.
27    detumble_dt: usize,
28    /// Remaining angular deviation after the sequence.
29    rem_angle_dev: I32F32,
30    /// Minimum battery needed to initiate the sequence.
31    min_charge: I32F32,
32    /// Minimum needed fuel to initiate the sequence.
33    min_fuel: I32F32,
34}
35
36impl BurnSequence {
37    /// Additional approximate detumble + return fuel need per exit maneuver
38    const ADD_FUEL_CONST: I32F32 = I32F32::lit("10.0");
39    /// Additional approximate fuel cost for secondary maneuvers
40    const ADD_SECOND_MANEUVER_FUEL_CONST: I32F32 = I32F32::lit("5.0");
41
42    /// Creates a new [`BurnSequence`] with the provided parameters.
43    ///
44    /// # Arguments
45    /// * `start_i` - The initial orbital position for the sequence.
46    /// * `sequence_pos` - A boxed slice of positional corrections.
47    /// * `sequence_vel` - A boxed slice of velocity corrections.
48    /// * `acc_dt` - Acceleration time duration, in seconds.
49    /// * `detumble_dt` - Detumbling time duration, in seconds.
50    /// * `cost_factor` - The predetermined cost factor for the sequence.
51    /// * `rem_angle_dev` - The remaining angular deviation
52    /// 
53    /// # Returns
54    /// A newly constructed [`BurnSequence`]
55    pub fn new(
56        start_i: IndexedOrbitPosition,
57        sequence_pos: Box<[Vec2D<I32F32>]>,
58        sequence_vel: Box<[Vec2D<I32F32>]>,
59        acc_dt: usize,
60        detumble_dt: usize,
61        rem_angle_dev: I32F32,
62        second_target_add_dt: usize,
63    ) -> Self {
64        let acq_db = FlightState::Acquisition.get_charge_rate();
65        let acq_acc_db = acq_db + FlightState::ACQ_ACC_ADDITION;
66        let trunc_detumble_time = detumble_dt.saturating_sub(TaskController::MANEUVER_MIN_DETUMBLE_DT);
67        let acq_charge_dt =
68            i32::try_from(FlightState::Acquisition.dt_to(FlightState::Charge).as_secs())
69                .unwrap_or(i32::MAX);
70        let charge_acq_dt =
71            i32::try_from(FlightState::Acquisition.dt_to(FlightState::Charge).as_secs())
72                .unwrap_or(i32::MAX);
73        let poss_charge_dt =
74            i32::try_from(trunc_detumble_time).unwrap_or(i32::MAX) - acq_charge_dt - charge_acq_dt;
75        let acq_time = {
76            if poss_charge_dt > 0 {
77                0
78            } else {
79                trunc_detumble_time
80            }
81        };
82
83        let poss_charge = (I32F32::from_num(poss_charge_dt)
84            * FlightState::Charge.get_charge_rate())
85        .clamp(I32F32::zero(), TaskController::MAX_BATTERY_THRESHOLD);
86        let acq_acc_time = I32F32::from_num(acc_dt + TaskController::MANEUVER_MIN_DETUMBLE_DT);
87
88        let mut min_fuel = acq_acc_time * FlightComputer::ACC_CONST + Self::ADD_FUEL_CONST;
89
90        let min_acc_acq_batt = (I32F32::from_num(acq_acc_time) * acq_acc_db).abs();
91        let min_acq_batt = (I32F32::from_num(acq_time) * acq_db).abs();
92        let mut add_acq_secs =
93            2 * usize::try_from(TaskController::ZO_IMAGE_FIRST_DEL.num_seconds()).unwrap_or(0);
94
95        if second_target_add_dt > 0 {
96            add_acq_secs += second_target_add_dt;
97            min_fuel += Self::ADD_SECOND_MANEUVER_FUEL_CONST;
98        }
99
100        let second_need = (I32F32::from_num(add_acq_secs) * acq_acc_db).abs();
101        let add_charge = (second_need - poss_charge).max(I32F32::zero());
102        let min_charge = TaskController::MIN_BATTERY_THRESHOLD + min_acc_acq_batt + min_acq_batt + add_charge;
103
104        Self {
105            start_i,
106            sequence_pos,
107            sequence_vel,
108            acc_dt,
109            detumble_dt,
110            rem_angle_dev,
111            min_charge,
112            min_fuel,
113        }
114    }
115
116    /// Returns the starting orbital position as [`IndexedOrbitPosition`] for the sequence.
117    pub fn start_i(&self) -> IndexedOrbitPosition { self.start_i }
118
119    /// Returns the sequence of positional corrections.
120    pub fn sequence_pos(&self) -> &[Vec2D<I32F32>] { &self.sequence_pos }
121
122    /// Returns the sequence of velocity corrections.
123    pub fn sequence_vel(&self) -> &[Vec2D<I32F32>] { &self.sequence_vel }
124
125    /// Returns the detumbling time duration, in seconds.
126    pub fn detumble_dt(&self) -> usize { self.detumble_dt }
127
128    /// Returns the acceleration time duration, in seconds.
129    pub fn acc_dt(&self) -> usize { self.acc_dt }
130
131    /// Returns the remaining angular deviation after the sequence.
132    pub fn rem_angle_dev(&self) -> I32F32 { self.rem_angle_dev }
133
134    /// Returns the minimum charge to initiate the burn.
135    pub fn min_charge(&self) -> I32F32 { self.min_charge }
136
137    /// Returns the minimum fuel to initiate the burn
138    pub fn min_fuel(&self) -> I32F32 { self.min_fuel }
139}
140
141/// Represents the result of a completed evaluation of a potential burn sequence.
142///
143/// This includes the final sequence, associated cost, primary and (optional) secondary targets,
144/// and target metadata for logging/export.
145#[derive(Debug, Clone, serde::Serialize)]
146pub struct ExitBurnResult {
147    sequence: BurnSequence,
148    cost: I32F32,
149    target_pos: Vec2D<I32F32>,
150    add_target: Option<Vec2D<I32F32>>,
151    unwrapped_target: Vec2D<I32F32>,
152    target_id: usize,
153}
154
155impl JsonDump for ExitBurnResult {
156    /// Returns a unique filename based on the zoned objective target ID.
157    fn file_name(&self) -> String {format!("zo_burn_{}", self.target_id)  }
158
159    /// Specifies the output directory for dumped zoned objective results.
160    fn dir_name(&self) -> &'static str { "zoned_objectives"  }
161}
162
163impl ExitBurnResult {
164    /// Creates a new [`ExitBurnResult`] with the given parameters.
165    ///
166    /// # Arguments
167    /// * `sequence` - The completed [`BurnSequence`] for this burn.
168    /// * `target` - A tuple where the first element is the primary target position and the second is the secondary offset vector.
169    /// * `unwrapped_target` - The unwrapped target position in the orbital map.
170    /// * `cost` - The total cost of the burn sequence.
171    /// * `target_id` - An identifier for the target.
172    ///
173    /// # Returns
174    /// A new instance of [`ExitBurnResult`].
175    pub fn new(
176        sequence: BurnSequence,
177        target: (Vec2D<I32F32>, Vec2D<I32F32>),
178        unwrapped_target: Vec2D<I32F32>,
179        cost: I32F32,
180        target_id: usize,
181    ) -> Self {
182        let target_pos = target.0;
183        let add_target = if target.1 == Vec2D::zero() {
184            None
185        } else {
186            Some((target.0 + target.1).wrap_around_map())
187        };
188        Self { sequence, cost, target_pos, add_target, unwrapped_target, target_id }
189    }
190
191    /// Returns the total cost of the burn sequence.
192    pub fn cost(&self) -> I32F32 { self.cost }
193
194    /// Returns a reference to the `BurnSequence` associated with this result.
195    pub fn sequence(&self) -> &BurnSequence { &self.sequence }
196
197    /// Returns the position of the primary target.
198    pub fn target_pos(&self) -> &Vec2D<I32F32> { &self.target_pos }
199
200    /// Returns the additional target position, if it exists.
201    pub fn add_target(&self) -> Option<Vec2D<I32F32>> { self.add_target }
202
203    /// Returns the unwrapped target position.
204    pub fn unwrapped_target(&self) -> &Vec2D<I32F32> { &self.unwrapped_target }
205}
206
207/// A struct responsible for evaluating potential burn sequences for an orbit.
208///
209/// [`BurnSequenceEvaluator`] processes orbital positions, velocities, and 
210/// target data to determine optimal burn sequences based on constraints such
211/// as time, fuel, and battery consumption.
212pub struct BurnSequenceEvaluator<'a> {
213    /// The current indexed orbital position.
214    i: IndexedOrbitPosition,
215    /// The current velocity vector in 2D space.
216    vel: Vec2D<I32F32>,
217    /// A slice of target positions and their secondary offsets.
218    targets: &'a [(Vec2D<I32F32>, Vec2D<I32F32>)],
219    /// The maximum allowable delta time for a burn sequence.
220    max_dt: usize,
221    /// The minimum allowable delta time for a burn sequence.
222    min_dt: usize,
223    /// The maximum allowable time spent off-orbit during a sequence.
224    max_off_orbit_dt: usize,
225    /// The maximum angular deviation for the burn sequence.
226    max_angle_dev: I32F32,
227    /// Precomputed tuples of clockwise and counterclockwise turns for the sequence.
228    turns: TurnsClockCClockTup,
229    /// The current best computed burn result, if one exists.
230    best_burn: Option<ExitBurnResult>,
231    /// The available fuel for the evaluator to use.
232    fuel_left: I32F32,
233    /// The dynamic weight assigned to fuel usage during scoring.
234    dynamic_fuel_w: I32F32,
235    /// The identifier for the current target being evaluated.
236    target_id: usize,
237}
238
239impl<'a> BurnSequenceEvaluator<'a> {
240    /// A constant representing a 90-degree angle, in fixed-point format.
241    const NINETY_DEG: I32F32 = I32F32::lit("90.0");
242    /// Weight assigned to off-orbit delta time in optimization calculations.
243    const OFF_ORBIT_W: I32F32 = I32F32::lit("2.0");
244    /// Maximum Weight assigned to fuel consumption in optimization calculations.
245    const MAX_FUEL_W: I32F32 = I32F32::lit("3.0");
246    /// Minimum Weight assigned to fuel consumption in optimization calculations.
247    const MIN_FUEL_W: I32F32 = I32F32::lit("1.0");
248    /// Weight assigned to angle deviation in optimization calculations.
249    const ANGLE_DEV_W: I32F32 = I32F32::lit("1.5");
250    /// Weight assigned to additional target angle deviation.
251    const ADD_ANGLE_DEV_W: I32F32 = I32F32::lit("3.0");
252
253    /// Constructs a new `BurnSequenceEvaluator` object
254    #[allow(clippy::too_many_arguments)]
255    pub fn new(
256        i: IndexedOrbitPosition,
257        vel: Vec2D<I32F32>,
258        targets: &'a [(Vec2D<I32F32>, Vec2D<I32F32>)],
259        min_dt: usize,
260        max_dt: usize,
261        max_off_orbit_dt: usize,
262        turns: TurnsClockCClockTup,
263        fuel_left: I32F32,
264        target_id: usize,
265    ) -> Self {
266        let max_angle_dev = {
267            let vel_perp = vel.perp_unit(true) * FlightComputer::ACC_CONST;
268            vel.angle_to(&vel_perp).abs()
269        };
270        let dynamic_fuel_w = helpers::interpolate(
271            FlightComputer::MIN_0,
272            FlightComputer::MAX_100,
273            Self::MIN_FUEL_W,
274            Self::MAX_FUEL_W,
275            fuel_left,
276        );
277        Self {
278            i,
279            vel,
280            targets,
281            max_dt,
282            min_dt,
283            max_off_orbit_dt,
284            max_angle_dev,
285            turns,
286            fuel_left,
287            dynamic_fuel_w,
288            target_id,
289            best_burn: None,
290        }
291    }
292
293    /// Evaluates whether a burn sequence at a specific `dt` is viable and better than existing sequences.
294    ///
295    /// # Arguments
296    /// - `dt`: Time offset in seconds from current position.
297    /// - `max_needed_batt`: Upper bound for acceptable battery consumption.
298    ///
299    /// # Behavior
300    /// Builds and scores a candidate burn. Updates `best_burn` if it's better
301    /// and satisfies fuel/charge constraints.
302    #[allow(clippy::cast_possible_wrap)]
303    pub fn process_dt(&mut self, dt: usize, max_needed_batt: I32F32) {
304        let pos = (self.i.pos() + self.vel * I32F32::from_num(dt)).wrap_around_map().round();
305        let bs_i = self.i.new_from_future_pos(pos, self.i.t() + TimeDelta::seconds(dt as i64));
306
307        let n_target = *self.targets.iter().min_by_key(|t| pos.unwrapped_to(&t.0).abs()).unwrap();
308        let shortest_dir = pos.unwrapped_to(&n_target.0);
309
310        if self.vel.angle_to(&shortest_dir).abs() > Self::NINETY_DEG {
311            return;
312        }
313        let (turns_in_dir, break_cond) = {
314            if shortest_dir.is_clockwise_to(&self.vel).unwrap_or(false) {
315                (&self.turns.0, false)
316            } else {
317                (&self.turns.1, true)
318            }
319        };
320        if let Some(b) = self.build_burn_sequence(bs_i, turns_in_dir, break_cond, &n_target) {
321            let cost = self.get_bs_cost(&b);
322            let add_cost = Self::get_add_target_cost(&b, &n_target);
323            let curr_cost = self.best_burn.as_ref().map_or(I32F32::MAX, ExitBurnResult::cost);
324            if curr_cost > cost.saturating_add(add_cost)
325                && b.min_charge() <= max_needed_batt
326                && b.min_fuel() <= self.fuel_left
327            {
328                let unwrapped_target = Self::get_unwrapped_target(&b, &n_target.0);
329                self.best_burn = Some(ExitBurnResult::new(b, n_target, unwrapped_target, cost, self.target_id));
330            }
331        }
332    }
333
334    /// Returns the unwrapped target position
335    pub fn get_unwrapped_target(b: &BurnSequence, tar: &Vec2D<I32F32>) -> Vec2D<I32F32> {
336        let impact_pos = *b.sequence_pos().last().unwrap()
337            + *b.sequence_vel().last().unwrap() * I32F32::from_num(b.detumble_dt());
338        let wrapped_impact_pos = impact_pos.wrap_around_map();
339        let offset = wrapped_impact_pos.to(tar);
340        impact_pos + offset
341    }
342
343    /// Returns the (heuristically) optimal [`ExitBurnResult`] if present
344    pub fn get_best_burn(self) -> Option<ExitBurnResult> { self.best_burn }
345
346    /// Attempts to build a complete burn sequence using directional turns and
347    /// evaluating if the final orientation and arrival meet objective constraints.
348    ///
349    /// # Arguments
350    /// - `burn_i`: Starting orbit index for this sequence.
351    /// - `turns_in_dir`: Precomputed valid turn maneuvers in chosen direction.
352    /// - `break_cond`: Whether to break based on clockwise/anticlockwise matching.
353    /// - `best_target`: Target location and secondary offset.
354    ///
355    /// # Returns
356    /// A viable `BurnSequence` if one exists, otherwise `None`.
357    #[allow(clippy::cast_possible_wrap, clippy::cast_sign_loss, clippy::cast_possible_truncation)]
358    fn build_burn_sequence(
359        &self,
360        burn_i: IndexedOrbitPosition,
361        turns_in_dir: &[(Vec2D<I32F32>, Vec2D<I32F32>)],
362        break_cond: bool,
363        best_target: &(Vec2D<I32F32>, Vec2D<I32F32>),
364    ) -> Option<BurnSequence> {
365        let mut add_dt = 0;
366        let mut fin_sequence_pos: Vec<Vec2D<I32F32>> = vec![burn_i.pos()];
367        let mut fin_sequence_vel: Vec<Vec2D<I32F32>> = vec![self.vel];
368
369        for atomic_turn in turns_in_dir {
370            // Update position and velocity based on turns
371            let next_seq_pos = (burn_i.pos() + atomic_turn.0).wrap_around_map();
372            let next_vel = atomic_turn.1;
373
374            let next_to_target = next_seq_pos.unwrapped_to(&best_target.0);
375            let min_dt = (next_to_target.abs() / next_vel.abs()).round().to_num::<usize>();
376            let min_add_target_dt =
377                (best_target.1.abs() / next_vel.abs()).abs().round().to_num::<usize>();
378            let dt = (burn_i.t() - Utc::now()).num_seconds() as usize;
379            // Check if the maneuver exceeds the maximum allowed time
380            add_dt += 1;
381            let obj_finish_dt = min_dt + dt + add_dt + min_add_target_dt;
382            let obj_arrival_dt = min_dt + dt + add_dt;
383            if obj_finish_dt > self.max_dt || obj_arrival_dt < self.min_dt || min_dt < TaskController::MANEUVER_MIN_DETUMBLE_DT
384            {
385                return None;
386            }
387
388            // Break and finalize the burn sequence if close enough to the target
389            if next_to_target.is_clockwise_to(&next_vel).unwrap_or(break_cond) == break_cond {
390                let last_pos = fin_sequence_pos.last().unwrap();
391                let last_vel = fin_sequence_vel.last().unwrap();
392                let (fin_dt, fin_angle_dev) = {
393                    let last_to_target = last_pos.unwrapped_to(&best_target.0);
394                    let last_angle_deviation = -last_vel.angle_to(&last_to_target);
395                    let this_angle_deviation = next_vel.angle_to(&next_to_target);
396
397                    let corr_burn_perc = helpers::interpolate(
398                        last_angle_deviation,
399                        this_angle_deviation,
400                        I32F32::zero(),
401                        I32F32::lit("1.0"),
402                        I32F32::zero(),
403                    );
404
405                    let acc = (next_vel - *last_vel) * corr_burn_perc;
406                    let (corr_vel, _) = FlightComputer::trunc_vel(next_vel + acc);
407                    let corr_pos = (*last_pos + corr_vel).wrap_around_map();
408                    let corr_to_target = corr_pos.unwrapped_to(&best_target.0);
409                    let corr_angle_dev = corr_vel.angle_to(&corr_to_target);
410                    fin_sequence_pos.push(corr_pos.round());
411                    fin_sequence_vel.push(corr_vel);
412                    add_dt += 1;
413                    (min_dt + dt + add_dt, corr_angle_dev)
414                };
415                let last_vel = fin_sequence_vel.last().unwrap();
416                let add_target_traversal_time =
417                    (best_target.1.abs() / last_vel.abs()).to_num::<usize>();
418                return Some(BurnSequence::new(
419                    burn_i,
420                    Box::from(fin_sequence_pos),
421                    Box::from(fin_sequence_vel),
422                    add_dt,
423                    fin_dt - dt - add_dt,
424                    fin_angle_dev,
425                    add_target_traversal_time,
426                ));
427            }
428            fin_sequence_pos.push(next_seq_pos);
429            fin_sequence_vel.push(next_vel);
430        }
431        None
432    }
433
434    /// Calculates the additional cost for a secondary target
435    ///
436    /// # Arguments
437    /// `bs`: corresponding burn sequence
438    /// `target`: A tuple of the primary and secondary target position
439    ///
440    /// # Returns
441    /// `I32F32` additional target cost factor
442    fn get_add_target_cost(bs: &BurnSequence, target: &(Vec2D<I32F32>, Vec2D<I32F32>)) -> I32F32 {
443        let last_pos = bs.sequence_pos().last().unwrap();
444        let last_to_target = last_pos.unwrapped_to(&target.0);
445
446        let angle_deviation = target.1.angle_to(&last_to_target);
447
448        let add_angle_dev =
449            helpers::normalize_fixed32(angle_deviation, I32F32::zero(), Self::NINETY_DEG)
450                .unwrap_or(I32F32::zero());
451        add_angle_dev * Self::ADD_ANGLE_DEV_W
452    }
453
454    /// Calculates the normalized cost factor for a burn sequence
455    ///
456    /// # Arguments
457    /// * `bs`: the burn sequence to be evaluated
458    ///
459    /// # Returns
460    /// The `I32F32`-cost factor.
461    fn get_bs_cost(&self, bs: &BurnSequence) -> I32F32 {
462        let max_add_dt = self.turns.0.len().max(self.turns.1.len());
463        // Normalize the factors contributing to burn sequence cost
464        let norm_fuel = helpers::normalize_fixed32(
465            I32F32::from_num(bs.acc_dt()) * FlightComputer::FUEL_CONST,
466            I32F32::zero(),
467            I32F32::from_num(max_add_dt) * FlightComputer::FUEL_CONST,
468        )
469        .unwrap_or(I32F32::zero());
470
471        let norm_off_orbit_dt = helpers::normalize_fixed32(
472            I32F32::from_num(bs.acc_dt() + bs.detumble_dt()),
473            I32F32::zero(),
474            I32F32::from_num(self.max_off_orbit_dt),
475        )
476        .unwrap_or(I32F32::zero());
477
478        let norm_angle_dev =
479            helpers::normalize_fixed32(bs.rem_angle_dev().abs(), I32F32::zero(), self.max_angle_dev)
480                .unwrap_or(I32F32::zero());
481
482        // Compute the total cost of the burn sequence
483        Self::OFF_ORBIT_W * norm_off_orbit_dt
484            + self.dynamic_fuel_w * norm_fuel
485            + Self::ANGLE_DEV_W * norm_angle_dev
486    }
487}
melvin_ob/flight_control/orbit/burn_sequence.rs

melvin_ob/flight_control/orbit/
burn_sequence.rs
