melvin_ob/mode_control/mode/
zo_prep_mode.rs

1use super::{
2    global_mode::{GlobalMode, OrbitalMode},
3    in_orbit_mode::InOrbitMode,
4    zo_retrieval_mode::ZORetrievalMode,
5};
6use crate::flight_control::{
7    FlightComputer, FlightState,
8    orbit::{BurnSequence, ExitBurnResult},
9};
10use crate::objective::KnownImgObjective;
11use crate::scheduling::{
12    EndCondition, TaskController,
13    task::{BaseTask, Task},
14};
15use crate::util::logger::JsonDump;
16use crate::mode_control::{
17    base_mode::BaseMode,
18    mode_context::ModeContext,
19    signal::{ExecExitSignal, OpExitSignal, OptOpExitSignal, WaitExitSignal},
20};
21use crate::{error, fatal, info, log, log_burn, obj};
22use async_trait::async_trait;
23use chrono::{DateTime, TimeDelta, Utc};
24use std::{
25    mem::discriminant,
26    sync::{
27        Arc,
28        atomic::{AtomicBool, Ordering},
29    },
30};
31use tokio_util::sync::CancellationToken;
32
33/// [`ZOPrepMode`] is a mission-critical mode responsible for preparing and scheduling
34/// orbital exit maneuvers to complete a given [`KnownImgObjective`]. It calculates optimal
35/// burn sequences, evaluates feasibility, and executes scheduled preparatory tasks.
36///
37/// This mode can re-prioritize based on new objectives, dynamically adapt to changing beacon
38/// conditions, and transition into a [`ZORetrievalMode`] once the exit burn is executed.
39pub(super) struct ZOPrepMode {
40    /// Underlying pre-exit behavior context (Mapping or Beacon Scanning).
41    base: BaseMode,
42    /// The precomputed exit burn sequence to leave the current orbit.
43    exit_burn: ExitBurnResult,
44    /// The currently targeted zoned objective.
45    target: KnownImgObjective,
46    /// Indicates whether the satellite has already left its orbit.
47    left_orbit: AtomicBool,
48}
49
50impl Clone for ZOPrepMode {
51    fn clone(&self) -> Self {
52        Self {
53            base: self.base,
54            exit_burn: self.exit_burn.clone(),
55            target: self.target.clone(),
56            left_orbit: AtomicBool::new(self.left_orbit.load(Ordering::Acquire)),
57        }
58    }
59}
60
61impl ZOPrepMode {
62    /// Internal name used for logging and identification.
63    const MODE_NAME: &'static str = "ZOPrepMode";
64    /// Minimum time before scheduled burn start during which re-planning is allowed.
65    const MIN_REPLANNING_DT: TimeDelta = TimeDelta::seconds(500);
66
67    /// Constructs a [`ZOPrepMode`] from a known zoned objective if a valid maneuver is found.
68    ///
69    /// # Arguments
70    /// * `context` – Shared mode context   .
71    /// * `zo` – The target zoned objective.
72    /// * `curr_base` – The current base mode (Mapping or Beacon).
73    ///
74    /// # Returns
75    /// * `Some(ZOPrepMode)` if a valid burn sequence can be computed.
76    /// * `None` if the objective is unreachable.
77    #[allow(clippy::cast_possible_wrap)]
78    pub(super) async fn from_obj(
79        context: &Arc<ModeContext>,
80        zo: KnownImgObjective,
81        curr_base: BaseMode,
82    ) -> Option<Self> {
83        log!("Trying ZOPrepMode for Zoned Objective: {}", zo.id());
84        let due = zo.end();
85        let (current_vel, fuel_left) = {
86            let f_cont_lock = context.k().f_cont();
87            let f_cont = f_cont_lock.read().await;
88            (f_cont.current_vel(), f_cont.fuel_left())
89        };
90        let start = zo.start();
91        if start > Utc::now() {
92            log!(
93                "Objective {} will be calculated as a short objective.",
94                zo.id()
95            );
96        }
97        let exit_burn = if zo.min_images() == 1 {
98            let target = zo.get_single_image_point();
99            TaskController::calculate_single_target_burn_sequence(
100                context.o_ch_clone().await.i_entry(),
101                current_vel,
102                target,
103                start,
104                due,
105                fuel_left,
106                zo.id(),
107            )
108        } else {
109            let entries = zo.get_corners();
110            TaskController::calculate_multi_target_burn_sequence(
111                context.o_ch_clone().await.i_entry(),
112                current_vel,
113                entries,
114                start,
115                due,
116                fuel_left,
117                zo.id(),
118            )
119        }?;
120        Self::log_burn(&exit_burn, &zo);
121        let base = Self::overthink_base(context, curr_base, exit_burn.sequence()).await;
122        exit_burn.dump_json();
123        Some(ZOPrepMode { base, exit_burn, target: zo, left_orbit: AtomicBool::new(false) })
124    }
125
126    /// Logs key information about the generated burn sequence.
127    ///
128    /// # Arguments
129    /// * `exit_burn` – The calculated burn data.
130    /// * `target` – The objective the burn aims to reach.
131    fn log_burn(exit_burn: &ExitBurnResult, target: &KnownImgObjective) {
132        let exit_burn_seq = exit_burn.sequence();
133        let entry_pos = exit_burn_seq.sequence_pos().first().unwrap();
134        let exit_pos = exit_burn_seq.sequence_pos().last().unwrap();
135        let entry_t = exit_burn_seq.start_i().t().format("%H:%M:%S").to_string();
136        let vel = exit_burn_seq.sequence_vel().last().unwrap();
137        let tar = exit_burn.target_pos();
138        let add_tar = exit_burn.add_target();
139        let det_dt = exit_burn_seq.detumble_dt();
140        let acq_dt = exit_burn_seq.acc_dt();
141        let tar_unwrap = exit_burn.unwrapped_target();
142        info!(
143            "Calculated Burn Sequence for Zoned Objective: {}",
144            target.id()
145        );
146        log_burn!("Entry at {entry_t}, Position will be {entry_pos}");
147        log_burn!("Exit after {acq_dt}s, Position will be {exit_pos}. Detumble time is {det_dt}s.");
148        log_burn!(
149            "Exit Velocity will be {vel} aiming for target at {tar} unwrapped to {tar_unwrap}."
150        );
151        if let Some(tar2) = add_tar {
152            log_burn!("Additional Target will be {tar2}");
153        }
154    }
155
156    /// Clones the current `ZOPrepMode` but with an updated base mode.
157    ///
158    /// # Arguments
159    /// * `base` – The new base mode.
160    ///
161    /// # Returns
162    /// * `Self` – A modified copy of the current mode.
163    fn new_base(&self, base: BaseMode) -> Self {
164        Self {
165            base,
166            exit_burn: self.exit_burn.clone(),
167            target: self.target.clone(),
168            left_orbit: AtomicBool::new(self.left_orbit.load(Ordering::Acquire)),
169        }
170    }
171
172    /// Determines whether the current base mode should change based on the burn timing
173    /// and worst-case beacon communication schedules.
174    ///
175    /// # Arguments
176    /// * `c` – Shared context.
177    /// * `base` – Proposed base mode.
178    /// * `burn` – Calculated burn sequence.
179    ///
180    /// # Returns
181    /// * `BaseMode` – The chosen base mode to continue with.
182    #[allow(clippy::cast_possible_wrap)]
183    async fn overthink_base(c: &Arc<ModeContext>, base: BaseMode, burn: &BurnSequence) -> BaseMode {
184        if matches!(base, BaseMode::MappingMode) {
185            return BaseMode::MappingMode;
186        }
187        let burn_start = burn.start_i().t();
188        let worst_case_first_comms_end = {
189            let to_dt = FlightComputer::get_to_comms_t_est(c.k().f_cont()).await;
190            let state_change = FlightState::Comms.td_dt_to(FlightState::Acquisition);
191            to_dt + TimeDelta::seconds(TaskController::IN_COMMS_SCHED_SECS as i64) + state_change
192        };
193        if worst_case_first_comms_end + TimeDelta::seconds(5) > burn_start {
194            let t = worst_case_first_comms_end.format("%d %H:%M:%S").to_string();
195            log!("Requested BOScanningMode not feasible, first comms end is {t}.");
196            BaseMode::MappingMode
197        } else {
198            BaseMode::BeaconObjectiveScanningMode
199        }
200    }
201}
202
203impl OrbitalMode for ZOPrepMode {
204    /// Returns the current base mode for delegation.
205    fn base(&self) -> &BaseMode { &self.base }
206}
207
208#[async_trait]
209impl GlobalMode for ZOPrepMode {
210    /// Returns the internal name of this mode.
211    fn type_name(&self) -> &'static str { Self::MODE_NAME }
212
213    /// Initializes scheduling and preparatory logic for the exit burn.
214    ///
215    /// If a base mode change is required due to beacon conflicts, the mode reinitializes.
216    /// Otherwise, a scheduler is launched and the burn is queued for execution.
217    ///
218    /// # Arguments
219    /// * `context` – Shared mode context.
220    ///
221    /// # Returns
222    /// * `OpExitSignal` – Indicates whether to continue or reinitialize.
223    async fn init_mode(&self, context: Arc<ModeContext>) -> OpExitSignal {
224        let cancel_task = CancellationToken::new();
225        let new_base = Self::overthink_base(&context, self.base, self.exit_burn.sequence()).await;
226        if discriminant(&self.base) != discriminant(&new_base) {
227            return OpExitSignal::ReInit(Box::new(self.new_base(new_base)));
228        }
229        let comms_end = self.base.handle_sched_preconditions(Arc::clone(&context)).await;
230        let end = EndCondition::from_burn(self.exit_burn.sequence());
231        let sched_handle = {
232            let cancel_clone = cancel_task.clone();
233            self.base
234                .get_schedule_handle(Arc::clone(&context), cancel_clone, comms_end, Some(end))
235                .await
236        };
237        tokio::pin!(sched_handle);
238        let safe_mon = context.super_v().safe_mon();
239        tokio::select!(
240            _ = &mut sched_handle => {
241                info!("Additionally scheduling Orbit Escape Burn Sequence!");
242                context.k().t_cont().schedule_vel_change(self.exit_burn.sequence().clone()).await;
243                context.k().con().send_tasklist().await;
244            },
245            () = safe_mon.notified() => {
246                cancel_task.cancel();
247                sched_handle.await.ok();
248                return self.safe_handler(context).await;
249            }
250        );
251        OpExitSignal::Continue
252    }
253
254    /// Waits until the next scheduled task using a default primitive, while monitoring safe mode and events.
255    async fn exec_task_wait(&self, c: Arc<ModeContext>, due: DateTime<Utc>) -> WaitExitSignal {
256        <Self as OrbitalMode>::exec_task_wait(self, c, due).await
257    }
258
259    /// Executes a scheduled task (only [`SwitchState`] or [`VelocityChange`] tasks are allowed).
260    ///
261    /// # Arguments
262    /// * `context` – Shared mode context.
263    /// * `task` – The task to execute.
264    ///
265    /// # Returns
266    /// * `ExecExitSignal::Continue` – Always continues unless an illegal task is found.
267    async fn exec_task(&self, context: Arc<ModeContext>, task: Task) -> ExecExitSignal {
268        match task.task_type() {
269            BaseTask::SwitchState(switch) => self.base.get_task(context, *switch).await,
270            BaseTask::ChangeVelocity(vel_change) => {
271                let pos = context.k().f_cont().read().await.current_pos();
272                log_burn!(
273                    "Burn started at Pos {pos}. Expected Position was: {}.",
274                    vel_change.burn().sequence_pos()[0]
275                );
276                FlightComputer::execute_burn(context.k().f_cont(), vel_change.burn()).await;
277                self.left_orbit.store(true, Ordering::Release);
278            }
279            BaseTask::TakeImage(_) => fatal!(
280                "Illegal task type {} for state {}!",
281                task.task_type(),
282                Self::MODE_NAME
283            ),
284        }
285        ExecExitSignal::Continue
286    }
287
288    /// Responds to a safe mode interrupt by escaping and attempting to reinitiate the mode.
289    async fn safe_handler(&self, context: Arc<ModeContext>) -> OpExitSignal {
290        FlightComputer::escape_safe(context.k().f_cont(), false).await;
291        context.o_ch_lock().write().await.finish(
292            context.k().f_cont().read().await.current_pos(),
293            self.safe_mode_rationale(),
294        );
295        let new = Self::from_obj(&context, self.target.clone(), self.base).await;
296        OpExitSignal::ReInit(new.map_or(Box::new(InOrbitMode::new(self.base)), |b| Box::new(b)))
297    }
298
299    /// Handles a newly received zoned objective.
300    /// Replaces the current target if the new one ends earlier and sufficient time remains.
301    ///
302    /// # Arguments
303    /// * `c` – Shared context.
304    /// * `obj` – The new zoned objective.
305    ///
306    /// # Returns
307    /// * `Some(OpExitSignal::ReInit)` if reprioritization occurs.
308    /// * `None` otherwise.
309    async fn zo_handler(&self, c: &Arc<ModeContext>, obj: KnownImgObjective) -> OptOpExitSignal {
310        let burn_dt_cond =
311            self.exit_burn.sequence().start_i().t() - Utc::now() > Self::MIN_REPLANNING_DT;
312        if obj.end() < self.target.end() && burn_dt_cond {
313            let new_obj_mode = Self::from_obj(c, obj.clone(), self.base).await;
314            if let Some(prep_mode) = new_obj_mode {
315                c.o_ch_lock().write().await.finish(
316                    c.k().f_cont().read().await.current_pos(),
317                    self.new_zo_rationale(),
318                );
319                obj!(
320                    "Objective {} is prioritized. Stashing current ZO {}!",
321                    obj.id(),
322                    self.target.id()
323                );
324                c.k_buffer().lock().await.push(self.target.clone());
325                return Some(OpExitSignal::ReInit(Box::new(prep_mode)));
326            }
327        }
328        obj!("Objective {} is not prioritized. Stashing!", obj.id());
329        c.k_buffer().lock().await.push(obj);
330        None
331    }
332
333    /// Reacts to a Beacon Objective state change by potentially switching the base mode.
334    ///
335    /// # Arguments
336    /// * `context` – Shared mode context.
337    ///
338    /// # Returns
339    /// * `Some(OpExitSignal::ReInit)` if a base mode change is needed.
340    /// * `None` if the current base mode is still valid.
341    async fn bo_event_handler(&self, context: &Arc<ModeContext>) -> OptOpExitSignal {
342        let prop_new_base = self.base.bo_event();
343        let new_base =
344            Self::overthink_base(context, prop_new_base, self.exit_burn.sequence()).await;
345        if discriminant(&self.base) == discriminant(&new_base) {
346            None
347        } else {
348            self.log_bo_event(context, new_base).await;
349            log!(
350                "Trying to change base mode from {} to {} due to BO Event!",
351                self.base,
352                new_base
353            );
354            Some(OpExitSignal::ReInit(Box::new(self.new_base(new_base))))
355        }
356    }
357
358    /// Finalizes the mode and transitions into a `ZORetrievalMode` if the satellite has left orbit.
359    ///
360    /// # Arguments
361    /// * `context` – Shared mode context.
362    ///
363    /// # Returns
364    /// * `Box<dyn GlobalMode>` – The next mode (retrieval or fallback).
365    async fn exit_mode(&self, context: Arc<ModeContext>) -> Box<dyn GlobalMode> {
366        context.o_ch_lock().write().await.finish(
367            context.k().f_cont().read().await.current_pos(),
368            self.tasks_done_exit_rationale(),
369        );
370        if self.left_orbit.load(Ordering::Acquire) {
371            Box::new(ZORetrievalMode::new(
372                self.target.clone(),
373                self.exit_burn.add_target(),
374                *self.exit_burn.unwrapped_target(),
375            ))
376        } else {
377            error!("ZOPrepMode::exit_mode called without left_orbit flag set!");
378            Box::new(InOrbitMode::new(self.base))
379        }
380    }
381}
melvin_ob/mode_control/mode/zo_prep_mode.rs

melvin_ob/mode_control/mode/
zo_prep_mode.rs
