From 9af000d87179992bc944eb8cc85c77d051f136e5 Mon Sep 17 00:00:00 2001 From: ishrakj Date: Sat, 4 Oct 2025 15:07:14 -0500 Subject: [PATCH 1/6] created MIDAS_Events struct and corresponding cpp file --- .../finite-state-machines/MIDAS_Events.cpp | 53 +++++++++++++++++++ MIDAS/src/rocket_state.h | 46 ++++++++++++++++ 2 files changed, 99 insertions(+) create mode 100644 MIDAS/src/finite-state-machines/MIDAS_Events.cpp diff --git a/MIDAS/src/finite-state-machines/MIDAS_Events.cpp b/MIDAS/src/finite-state-machines/MIDAS_Events.cpp new file mode 100644 index 00000000..bccc436d --- /dev/null +++ b/MIDAS/src/finite-state-machines/MIDAS_Events.cpp @@ -0,0 +1,53 @@ +#include "rocket_state.h" + + +// SAFE +void safe_to_pyroTest(); +void safe_to_stateIdle(); + +// PYRO TEST +void pyroTest_to_safe_forced(); +void pyroTest_to_safe_timed(); +void pyroTest_to_firstBoost(); + +// FIRST BOOST +void firstBoost_to_idle(); +void firstBoost_to_burnout(); + +// BURNOUT +void burnout_to_firstBoost(); +void burnout_to_sustainerIgnition(); + +// SUSTAINER IGNITION +void sustainerIgnition_to_coast(); +void sustainerIgnition_to_secondBoost(); + +// SECOND BOOST +void secondBoost_to_sustainerIgnition(); +void secondBoost_to_coast(); + +// COAST +void coast_to_secondBoost(); +void coast_to_apogee(); + +// APOGEE +void apogee_to_coast(); +void apogee_to_drogueDeploy(); + +// DROUGE DEPLOY +void drogueDeploy_to_drogue_jerk(); +void drogueDeploy_to_drouge_timed(); + +// DROGUE +void drogue_to_mainDeploy(); + +// MAIN DEPLOY +void mainDeploy_to_main_jerk(); +void mainDeploy_to_main_timed(); + +// MAIN +void main_to_landed(); + +// LANDED +void landed_to_safe(); +void landed_to_main(); \ No newline at end of file diff --git a/MIDAS/src/rocket_state.h b/MIDAS/src/rocket_state.h index 19ecef13..da68ee78 100644 --- a/MIDAS/src/rocket_state.h +++ b/MIDAS/src/rocket_state.h @@ -172,6 +172,52 @@ struct CommandFlags { bool FSM_should_set_cam_feed_cam1 = false; // Triggered at launch (IDLE --> FIRST_BOOST) bool FSM_should_swap_camera_feed = false; // Triggered COAST --> APOGEE }; + +/** + * @struct MIDAS_Events + * + * @brief Abstracts changing command flags during state changes away from fsm.cpp. + * The functions are defined in MIDAS_Events.cpp + */ +struct MIDAS_Events { + void safe_to_pyroTest(); + void safe_to_stateIdle(); + + void pyroTest_to_safe_forced(); + void pyroTest_to_safe_timed(); + void pyroTest_to_firstBoost(); + + void firstBoost_to_idle(); + void firstBoost_to_burnout(); + + void burnout_to_firstBoost(); + void burnout_to_sustainerIgnition(); + + void sustainerIgnition_to_coast(); + void sustainerIgnition_to_secondBoost(); + + void secondBoost_to_sustainerIgnition(); + void secondBoost_to_coast(); + + void coast_to_secondBoost(); + void coast_to_apogee(); + + void apogee_to_coast(); + void apogee_to_drogueDeploy(); + + void drogueDeploy_to_drogue_jerk(); + void drogueDeploy_to_drouge_timed(); + + void drogue_to_mainDeploy(); + + void mainDeploy_to_main_jerk(); + void mainDeploy_to_main_timed(); + + void main_to_landed(); + + void landed_to_safe(); + void landed_to_main(); +}; /** * @struct RocketData * From f07f2592f31b87ded0a9956fd7b4a1be8432b3ed Mon Sep 17 00:00:00 2001 From: ishrakj Date: Sat, 18 Oct 2025 13:09:54 -0500 Subject: [PATCH 2/6] help ;-; --- .../finite-state-machines/MIDAS_Events.cpp | 174 +++++++++++++++--- MIDAS/src/finite-state-machines/fsm.cpp | 64 ++++--- MIDAS/src/finite-state-machines/fsm.h | 1 + MIDAS/src/rocket_state.h | 53 +++--- 4 files changed, 217 insertions(+), 75 deletions(-) diff --git a/MIDAS/src/finite-state-machines/MIDAS_Events.cpp b/MIDAS/src/finite-state-machines/MIDAS_Events.cpp index bccc436d..c647f1b1 100644 --- a/MIDAS/src/finite-state-machines/MIDAS_Events.cpp +++ b/MIDAS/src/finite-state-machines/MIDAS_Events.cpp @@ -1,53 +1,177 @@ #include "rocket_state.h" +#ifdef IS_SUSTAINER // SAFE -void safe_to_pyroTest(); -void safe_to_stateIdle(); +void safe_to_pyroTest(CommandFlags& commands) { + commands.should_transition_pyro_test = false; +} +void safe_to_stateIdle(CommandFlags& commands) { + commands.should_transition_idle = false; +} // PYRO TEST -void pyroTest_to_safe_forced(); -void pyroTest_to_safe_timed(); -void pyroTest_to_firstBoost(); +void pyroTest_to_safe_forced(CommandFlags& commands) { + commands.should_transition_pyro_test = false; + commands.should_transition_idle = false; + commands.should_transition_safe = false; +} +void pyroTest_to_safe_timed(CommandFlags& commands) { + commands.should_transition_pyro_test = false; +} +void pyroTest_to_firstBoost(CommandFlags& commands) { + +} + +// IDLE +void idle_to_safe_forced(CommandFlags& commands) { + commands.should_transition_pyro_test = false; + commands.should_transition_idle = false; + commands.should_transition_safe = false; +} + +void idle_to_firstBoost(CommandFlags& commands) { + commands.FSM_should_set_cam_feed_cam1 = true; +} + +// FIRST BOOST +void firstBoost_to_idle(CommandFlags& commands) {} +void firstBoost_to_burnout(CommandFlags& commands) {} + +// BURNOUT +void burnout_to_firstBoost(CommandFlags& commands) {} +void burnout_to_sustainerIgnition(CommandFlags& commands) {} +void burnout_to_firstSeparation(CommandFlags& commands) {} + +// SUSTAINER IGNITION +void sustainerIgnition_to_coast(CommandFlags& commands) {} +void sustainerIgnition_to_secondBoost(CommandFlags& commands) {} + +// FIRST SEPARATION +void firstSeparation_to_coast_jerk(CommandFlags& commands) {} +void firstSeparation_to_coast_timed(CommandFlags& commands) {} + +// SECOND BOOST +void secondBoost_to_sustainerIgnition(CommandFlags& commands); +void secondBoost_to_coast(CommandFlags& commands); + +// COAST +void coast_to_secondBoost(CommandFlags& commands); +void coast_to_apogee(CommandFlags& commands) { + commands.FSM_should_swap_camera_feed = true; +} + +// APOGEE +void apogee_to_coast(CommandFlags& commands); +void apogee_to_drogueDeploy(CommandFlags& commands); + +// DROUGE DEPLOY +void drogueDeploy_to_drogue_jerk(CommandFlags& commands); +void drogueDeploy_to_drouge_timed(CommandFlags& commands); + +// DROGUE +void drogue_to_mainDeploy(CommandFlags& commands); + +// MAIN DEPLOY +void mainDeploy_to_main_jerk(CommandFlags& commands); +void mainDeploy_to_main_timed(CommandFlags& commands); + +// MAIN +void main_to_landed(CommandFlags& commands); + +// LANDED +void landed_to_safe(CommandFlags& commands) { + commands.should_transition_pyro_test = false; + commands.should_transition_idle = false; + commands.should_transition_safe = false; +} +void landed_to_main(CommandFlags& commands); + +#else + +// SAFE +void safe_to_pyroTest(CommandFlags& commands) { + commands.should_transition_pyro_test = false; +} +void safe_to_stateIdle(CommandFlags& commands) { + commands.should_transition_idle = false; +} + +// PYRO TEST +void pyroTest_to_safe_forced(CommandFlags& commands) { + commands.should_transition_pyro_test = false; + commands.should_transition_idle = false; + commands.should_transition_safe = false; +} +void pyroTest_to_safe_timed(CommandFlags& commands) { + commands.should_transition_pyro_test = false; +} +void pyroTest_to_firstBoost(CommandFlags& commands) { + +} + +// IDLE +void idle_to_safe_forced(CommandFlags& commands) { + commands.should_transition_pyro_test = false; + commands.should_transition_idle = false; + commands.should_transition_safe = false; +} + +void idle_to_firstBoost(CommandFlags& commands) { + commands.FSM_should_set_cam_feed_cam1 = true; +} // FIRST BOOST -void firstBoost_to_idle(); -void firstBoost_to_burnout(); +void firstBoost_to_idle(CommandFlags& commands) {} +void firstBoost_to_burnout(CommandFlags& commands) {} // BURNOUT -void burnout_to_firstBoost(); -void burnout_to_sustainerIgnition(); +void burnout_to_firstBoost(CommandFlags& commands) {} +void burnout_to_sustainerIgnition(CommandFlags& commands) {} +void burnout_to_firstSeparation(CommandFlags& commands) {} // SUSTAINER IGNITION -void sustainerIgnition_to_coast(); -void sustainerIgnition_to_secondBoost(); +void sustainerIgnition_to_coast(CommandFlags& commands) {} +void sustainerIgnition_to_secondBoost(CommandFlags& commands) {} + +// FIRST SEPARATION +void firstSeparation_to_coast_jerk(CommandFlags& commands) {} +void firstSeparation_to_coast_timed(CommandFlags& commands) {} // SECOND BOOST -void secondBoost_to_sustainerIgnition(); -void secondBoost_to_coast(); +void secondBoost_to_sustainerIgnition(CommandFlags& commands); +void secondBoost_to_coast(CommandFlags& commands); // COAST -void coast_to_secondBoost(); -void coast_to_apogee(); +void coast_to_secondBoost(CommandFlags& commands); +void coast_to_apogee(CommandFlags& commands) { + commands.FSM_should_swap_camera_feed = true; +} // APOGEE -void apogee_to_coast(); -void apogee_to_drogueDeploy(); +void apogee_to_coast(CommandFlags& commands); +void apogee_to_drogueDeploy(CommandFlags& commands); // DROUGE DEPLOY -void drogueDeploy_to_drogue_jerk(); -void drogueDeploy_to_drouge_timed(); +void drogueDeploy_to_drogue_jerk(CommandFlags& commands); +void drogueDeploy_to_drouge_timed(CommandFlags& commands); // DROGUE -void drogue_to_mainDeploy(); +void drogue_to_mainDeploy(CommandFlags& commands); // MAIN DEPLOY -void mainDeploy_to_main_jerk(); -void mainDeploy_to_main_timed(); +void mainDeploy_to_main_jerk(CommandFlags& commands); +void mainDeploy_to_main_timed(CommandFlags& commands); // MAIN -void main_to_landed(); +void main_to_landed(CommandFlags& commands); // LANDED -void landed_to_safe(); -void landed_to_main(); \ No newline at end of file +void landed_to_safe(CommandFlags& commands) { + commands.should_transition_pyro_test = false; + commands.should_transition_idle = false; + commands.should_transition_safe = false; +} +void landed_to_main(CommandFlags& commands); + +#endif \ No newline at end of file diff --git a/MIDAS/src/finite-state-machines/fsm.cpp b/MIDAS/src/finite-state-machines/fsm.cpp index c434abed..6887314e 100644 --- a/MIDAS/src/finite-state-machines/fsm.cpp +++ b/MIDAS/src/finite-state-machines/fsm.cpp @@ -87,7 +87,7 @@ StateEstimate::StateEstimate(RocketData& state) { FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFlags& commands) { //get current time double current_time = pdTICKS_TO_MS(xTaskGetTickCount()); - + MIDAS_Events dispatch; switch (state) { case FSMState::STATE_SAFE: @@ -103,13 +103,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla if(commands.should_transition_pyro_test) { state = FSMState::STATE_PYRO_TEST; pyro_test_entry_time = current_time; - commands.should_transition_pyro_test = false; + safe_to_pyroTest(commands); } // Only switch to STATE_IDLE if triggered wirelessly. if(commands.should_transition_idle) { state = FSMState::STATE_IDLE; - commands.should_transition_idle = false; + safe_to_stateIdle(commands); } break; @@ -118,15 +118,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // Force transtion to safe if requested + clear all transition flags. if(commands.should_transition_safe) { state = FSMState::STATE_SAFE; - commands.should_transition_pyro_test = false; - commands.should_transition_idle = false; - commands.should_transition_safe = false; + pyroTest_to_safe_forced(commands); break; } // Switch back to STATE_SAFE after a certain amount of time passes if((current_time - pyro_test_entry_time) > safety_pyro_test_disarm_time) { - commands.should_transition_pyro_test = false; + pyroTest_to_safe_timed(commands); state = FSMState::STATE_SAFE; } @@ -137,16 +135,14 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // Force transtion to safe if requested + clear all transition flags. if(commands.should_transition_safe) { state = FSMState::STATE_SAFE; - commands.should_transition_pyro_test = false; - commands.should_transition_idle = false; - commands.should_transition_safe = false; + idle_to_safe_forced(commands); break; } // once a significant amount of acceleration is detected change states if (state_estimate.acceleration > sustainer_idle_to_first_boost_acceleration_threshold) { launch_time = current_time; - commands.FSM_should_set_cam_feed_cam1 = true; + idle_to_firstBoost(commands); state = FSMState::STATE_FIRST_BOOST; } @@ -156,12 +152,14 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if acceleration spike was too brief then go back to idle if ((state_estimate.acceleration < sustainer_idle_to_first_boost_acceleration_threshold) && ((current_time - launch_time) < sustainer_idle_to_first_boost_time_threshold)) { state = FSMState::STATE_IDLE; + firstBoost_to_idle(commands); break; } // once acceleartion decreases to a the threshold go on the next state if (state_estimate.acceleration < sustainer_coast_detection_acceleration_threshold) { burnout_time = current_time; + firstBoost_to_burnout(commands); state = FSMState::STATE_BURNOUT; } break; @@ -170,12 +168,14 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if low acceleration is too brief than go on to the previous state if ((state_estimate.acceleration >= sustainer_coast_detection_acceleration_threshold) && ((current_time - burnout_time) < sustainer_coast_time)) { state = FSMState::STATE_FIRST_BOOST; + burnout_to_firstBoost(commands); break; } // if in burnout for long enough then go on to the next state (time transition) if ((current_time - burnout_time) > sustainer_coast_time) { sustainer_ignition_time = current_time; + burnout_to_sustainerIgnition(commands); state = FSMState::STATE_SUSTAINER_IGNITION; } break; @@ -185,6 +185,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // another time transition into coast after a certain amount of time if ((current_time - sustainer_ignition_time) > sustainer_ignition_to_coast_timer_threshold) { coast_time = current_time; + sustainerIgnition_to_coast(commands); state = FSMState::STATE_COAST; break; } @@ -192,6 +193,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // once a high enough acceleration is detected then go to next state if (state_estimate.acceleration > sustainer_ignition_to_second_boost_acceleration_threshold) { second_boost_time = current_time; + sustainerIgnition_to_secondBoost(commands); state = FSMState::STATE_SECOND_BOOST; } @@ -201,12 +203,14 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if high accleration is too brief then return to previous state if ((state_estimate.acceleration < sustainer_ignition_to_second_boost_acceleration_threshold) && ((current_time - second_boost_time) < sustainer_ignition_to_second_boost_time_threshold)) { state = FSMState::STATE_SUSTAINER_IGNITION; + secondBoost_to_sustainerIgnition(commands); break; } // if low acceleration detected go to next state if (state_estimate.acceleration < sustainer_coast_detection_acceleration_threshold) { coast_time = current_time; + secondBoost_to_coast(commands); state = FSMState::STATE_COAST; } break; @@ -215,6 +219,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if the low acceleration detected was too brief then return to previous state if ((state_estimate.acceleration > sustainer_coast_detection_acceleration_threshold) && ((current_time - coast_time) < sustainer_second_boost_to_coast_time_threshold)) { state = FSMState::STATE_SECOND_BOOST; + coast_to_secondBoost(commands); break; } @@ -222,7 +227,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla if (state_estimate.vertical_speed <= sustainer_coast_to_apogee_vertical_speed_threshold) { apogee_time = current_time; state = FSMState::STATE_APOGEE; - commands.FSM_should_swap_camera_feed = true; + coast_to_apogee(commands); } break; @@ -230,12 +235,14 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if the slow speed was too brief then return to previous state if ((state_estimate.vertical_speed) > sustainer_apogee_backto_coast_vertical_speed_threshold && ((current_time - apogee_time) < sustainer_apogee_check_threshold)) { state = FSMState::STATE_COAST; + apogee_to_coast(commands); break; } // transition to next state after a certain amount of time if ((current_time - apogee_time) > sustainer_apogee_timer_threshold) { drogue_time = current_time; + apogee_to_drogueDeploy(commands); state = FSMState::STATE_DROGUE_DEPLOY; } break; @@ -248,12 +255,14 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla } if (abs(state_estimate.jerk) > sustainer_drogue_jerk_threshold) { + drogueDeploy_to_drogue_jerk(commands); state = FSMState::STATE_DROGUE; break; } // if no transtion after a certain amount of time then just move on to next state if ((current_time - drogue_time) > sustainer_drogue_timer_threshold) { + drogueDeploy_to_drouge_timed(commands); state = FSMState::STATE_DROGUE; } @@ -263,6 +272,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if altitude low enough then next state // Also, wait at least 1 second after drogue deploy to deploy main. if (state_estimate.altitude <= sustainer_main_deploy_altitude_threshold && (current_time - drogue_time) > sustainer_main_deploy_delay_after_drogue) { + drogue_to_mainDeploy(commands); state = FSMState::STATE_MAIN_DEPLOY; main_time = current_time; } @@ -277,6 +287,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if detected a sharp change in jerk then go to the next state if (abs(state_estimate.jerk) > sustainer_main_jerk_threshold) { state = FSMState::STATE_MAIN; + mainDeploy_to_main_jerk(commands); main_deployed_time = current_time; break; } @@ -284,6 +295,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if no transtion after a certain amount of time then just move on to next state if ((current_time - main_time) > sustainer_main_to_main_deploy_timer_threshold) { state = FSMState::STATE_MAIN; + mainDeploy_to_main_timed(commands); } break; @@ -291,6 +303,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if slowed down enough then go on to the next state if ((abs(state_estimate.vertical_speed) <= sustainer_landed_vertical_speed_threshold) && (current_time - main_deployed_time) > sustainer_main_to_landed_lockout) { landed_time = current_time; + main_to_landed(commands); state = FSMState::STATE_LANDED; } break; @@ -303,9 +316,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // Force transtion to safe if requested + clear all transition flags. if(commands.should_transition_safe) { state = FSMState::STATE_SAFE; - commands.should_transition_pyro_test = false; - commands.should_transition_idle = false; - commands.should_transition_safe = false; + landed_to_safe(commands); } break; @@ -314,6 +325,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if the slow speed was too brief then return to previous state if ((abs(state_estimate.vertical_speed) > sustainer_landed_to_main_vertical_speed_threshold) && ((current_time - landed_time) > sustainer_landed_timer_threshold)) { state = FSMState::STATE_MAIN; + landed_to_main(commands); } break; @@ -350,13 +362,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla if(commands.should_transition_pyro_test) { state = FSMState::STATE_PYRO_TEST; pyro_test_entry_time = current_time; - commands.should_transition_pyro_test = false; + safe_to_pyroTest(commands); } // Only switch to STATE_IDLE if triggered wirelessly. if(commands.should_transition_idle) { state = FSMState::STATE_IDLE; - commands.should_transition_idle = false; + safe_to_stateIdle(commands); } break; @@ -365,15 +377,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // Force transtion to safe if requested + clear all transition flags. if(commands.should_transition_safe) { state = FSMState::STATE_SAFE; - commands.should_transition_pyro_test = false; - commands.should_transition_idle = false; - commands.should_transition_safe = false; + pyroTest_to_safe_forced(commands); break; } // Switch back to STATE_SAFE after a certain amount of time passes if((current_time - pyro_test_entry_time) > safety_pyro_test_disarm_time) { - commands.should_transition_pyro_test = false; + pyroTest_to_safe_timed(commands); state = FSMState::STATE_SAFE; } @@ -383,16 +393,14 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // Force transtion to safe if requested + clear all transition flags. if(commands.should_transition_safe) { state = FSMState::STATE_SAFE; - commands.should_transition_pyro_test = false; - commands.should_transition_idle = false; - commands.should_transition_safe = false; + idle_to_safe_forced(commands); break; } // once a significant amount of acceleration is detected change states if (state_estimate.acceleration > booster_idle_to_first_boost_acceleration_threshold) { launch_time = current_time; - commands.FSM_should_set_cam_feed_cam1 = true; + idle_to_firstBoost(commands); state = FSMState::STATE_FIRST_BOOST; } @@ -400,11 +408,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla case FSMState::STATE_FIRST_BOOST: if ((state_estimate.acceleration < booster_idle_to_first_boost_acceleration_threshold) && ((current_time - launch_time) < booster_idle_to_first_boost_time_threshold)) { state = FSMState::STATE_IDLE; + firstBoost_to_idle(commands); break; } if (state_estimate.acceleration < booster_coast_detection_acceleration_threshold) { burnout_time = current_time; state = FSMState::STATE_BURNOUT; + firstBoost_to_burnout(commands); } break; @@ -412,11 +422,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla case FSMState::STATE_BURNOUT: if ((state_estimate.acceleration >= booster_coast_detection_acceleration_threshold) && ((current_time - burnout_time) < booster_first_boost_to_burnout_time_threshold)) { state = FSMState::STATE_FIRST_BOOST; + burnout_to_firstBoost(commands); break; } if ((current_time - burnout_time) > booster_first_boost_to_burnout_time_threshold) { first_separation_time = current_time; + burnout_to_firstSeparation(commands); state = FSMState::STATE_FIRST_SEPARATION; } break; @@ -424,11 +436,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla case FSMState::STATE_FIRST_SEPARATION: if (abs(state_estimate.jerk) < booster_first_separation_jerk_threshold) { state = FSMState::STATE_COAST; + firstSeparation_to_coast_jerk(commands); break; } if ((current_time - first_separation_time) > booster_first_seperation_time_threshold) { state = FSMState::STATE_COAST; + firstSeparation_to_coast_timed(commands); } break; diff --git a/MIDAS/src/finite-state-machines/fsm.h b/MIDAS/src/finite-state-machines/fsm.h index 5d1b94ea..20ae11f6 100644 --- a/MIDAS/src/finite-state-machines/fsm.h +++ b/MIDAS/src/finite-state-machines/fsm.h @@ -8,6 +8,7 @@ #include "sensor_data.h" #include "Buffer.h" #include "rocket_state.h" +#include "MIDAS_Events.cpp" /** * @struct StateEstimate diff --git a/MIDAS/src/rocket_state.h b/MIDAS/src/rocket_state.h index da68ee78..89fa5674 100644 --- a/MIDAS/src/rocket_state.h +++ b/MIDAS/src/rocket_state.h @@ -180,43 +180,46 @@ struct CommandFlags { * The functions are defined in MIDAS_Events.cpp */ struct MIDAS_Events { - void safe_to_pyroTest(); - void safe_to_stateIdle(); + void safe_to_pyroTest(CommandFlags& commands); + void safe_to_stateIdle(CommandFlags& commands); - void pyroTest_to_safe_forced(); - void pyroTest_to_safe_timed(); - void pyroTest_to_firstBoost(); + void pyroTest_to_safe_forced(CommandFlags& commands); + void pyroTest_to_safe_timed(CommandFlags& commands); - void firstBoost_to_idle(); - void firstBoost_to_burnout(); + void firstBoost_to_idle(CommandFlags& commands); + void firstBoost_to_burnout(CommandFlags& commands); - void burnout_to_firstBoost(); - void burnout_to_sustainerIgnition(); + void burnout_to_firstBoost(CommandFlags& commands); + void burnout_to_sustainerIgnition(CommandFlags& commands); + void burnout_to_firstSeparation(CommandFlags& commands); - void sustainerIgnition_to_coast(); - void sustainerIgnition_to_secondBoost(); + void sustainerIgnition_to_coast(CommandFlags& commands); + void sustainerIgnition_to_secondBoost(CommandFlags& commands); - void secondBoost_to_sustainerIgnition(); - void secondBoost_to_coast(); + void firstSeparation_to_coast_jerk(CommandFlags& commands); + void firstSeparation_to_coast_timed(CommandFlags& commands); - void coast_to_secondBoost(); - void coast_to_apogee(); + void secondBoost_to_sustainerIgnition(CommandFlags& commands); + void secondBoost_to_coast(CommandFlags& commands); - void apogee_to_coast(); - void apogee_to_drogueDeploy(); + void coast_to_secondBoost(CommandFlags& commands); + void coast_to_apogee(CommandFlags& commands); - void drogueDeploy_to_drogue_jerk(); - void drogueDeploy_to_drouge_timed(); + void apogee_to_coast(CommandFlags& commands); + void apogee_to_drogueDeploy(CommandFlags& commands); - void drogue_to_mainDeploy(); + void drogueDeploy_to_drogue_jerk(CommandFlags& commands); + void drogueDeploy_to_drouge_timed(CommandFlags& commands); - void mainDeploy_to_main_jerk(); - void mainDeploy_to_main_timed(); + void drogue_to_mainDeploy(CommandFlags& commands); - void main_to_landed(); + void mainDeploy_to_main_jerk(CommandFlags& commands); + void mainDeploy_to_main_timed(CommandFlags& commands); - void landed_to_safe(); - void landed_to_main(); + void main_to_landed(CommandFlags& commands); + + void landed_to_safe(CommandFlags& commands); + void landed_to_main(CommandFlags& commands); }; /** * @struct RocketData From c661caee5eda6f10b1b8b19e5ffbe01c19048b9f Mon Sep 17 00:00:00 2001 From: ishrakj Date: Sat, 18 Oct 2025 13:32:56 -0500 Subject: [PATCH 3/6] created MIDAS_Events object in fsm --- MIDAS/src/finite-state-machines/fsm.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/MIDAS/src/finite-state-machines/fsm.cpp b/MIDAS/src/finite-state-machines/fsm.cpp index 6887314e..a7af6fae 100644 --- a/MIDAS/src/finite-state-machines/fsm.cpp +++ b/MIDAS/src/finite-state-machines/fsm.cpp @@ -103,13 +103,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla if(commands.should_transition_pyro_test) { state = FSMState::STATE_PYRO_TEST; pyro_test_entry_time = current_time; - safe_to_pyroTest(commands); + dispatch.safe_to_pyroTest(commands); } // Only switch to STATE_IDLE if triggered wirelessly. if(commands.should_transition_idle) { state = FSMState::STATE_IDLE; - safe_to_stateIdle(commands); + dispatch.safe_to_stateIdle(commands); } break; @@ -118,7 +118,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // Force transtion to safe if requested + clear all transition flags. if(commands.should_transition_safe) { state = FSMState::STATE_SAFE; - pyroTest_to_safe_forced(commands); + dispatch.pyroTest_to_safe_forced(commands); break; } From 6b471ebaeeefb3640c1ed72339d19cb6af5d630e Mon Sep 17 00:00:00 2001 From: ishrakj Date: Sat, 18 Oct 2025 13:37:24 -0500 Subject: [PATCH 4/6] updated struct header in rocket_state.h --- MIDAS/src/finite-state-machines/fsm.cpp | 2 +- MIDAS/src/rocket_state.h | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/MIDAS/src/finite-state-machines/fsm.cpp b/MIDAS/src/finite-state-machines/fsm.cpp index a7af6fae..1c490c11 100644 --- a/MIDAS/src/finite-state-machines/fsm.cpp +++ b/MIDAS/src/finite-state-machines/fsm.cpp @@ -347,7 +347,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla */ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFlags& commands) { double current_time = pdTICKS_TO_MS(xTaskGetTickCount()); - + MIDAS_Events dispatch switch (state) { case FSMState::STATE_SAFE: // Deconflict if multip commands are processed diff --git a/MIDAS/src/rocket_state.h b/MIDAS/src/rocket_state.h index 89fa5674..f49413f5 100644 --- a/MIDAS/src/rocket_state.h +++ b/MIDAS/src/rocket_state.h @@ -185,7 +185,11 @@ struct MIDAS_Events { void pyroTest_to_safe_forced(CommandFlags& commands); void pyroTest_to_safe_timed(CommandFlags& commands); + void pyroTest_to_firstBoost(CommandFlags& commands); + void idle_to_safe_forced(CommandFlags& commands); + void idle_to_firstBoost(CommandFlags& commands); + void firstBoost_to_idle(CommandFlags& commands); void firstBoost_to_burnout(CommandFlags& commands); From 545e389e1c592853c6586bf1191d7c042360e576 Mon Sep 17 00:00:00 2001 From: ishrakj Date: Sat, 18 Oct 2025 14:20:12 -0500 Subject: [PATCH 5/6] fixed linking issue --- .../finite-state-machines/MIDAS_Events.cpp | 124 +++++++++--------- .../src/finite-state-machines/MIDAS_Events.h | 54 ++++++++ MIDAS/src/finite-state-machines/fsm.cpp | 86 ++++++------ MIDAS/src/finite-state-machines/fsm.h | 2 +- MIDAS/src/rocket_state.h | 51 ------- 5 files changed, 163 insertions(+), 154 deletions(-) create mode 100644 MIDAS/src/finite-state-machines/MIDAS_Events.h diff --git a/MIDAS/src/finite-state-machines/MIDAS_Events.cpp b/MIDAS/src/finite-state-machines/MIDAS_Events.cpp index c647f1b1..df65e089 100644 --- a/MIDAS/src/finite-state-machines/MIDAS_Events.cpp +++ b/MIDAS/src/finite-state-machines/MIDAS_Events.cpp @@ -1,177 +1,175 @@ -#include "rocket_state.h" +#include "MIDAS_Events.h" #ifdef IS_SUSTAINER // SAFE -void safe_to_pyroTest(CommandFlags& commands) { +void MIDAS_Events::safe_to_pyroTest(CommandFlags& commands) { commands.should_transition_pyro_test = false; } -void safe_to_stateIdle(CommandFlags& commands) { +void MIDAS_Events::safe_to_stateIdle(CommandFlags& commands) { commands.should_transition_idle = false; } // PYRO TEST -void pyroTest_to_safe_forced(CommandFlags& commands) { +void MIDAS_Events::pyroTest_to_safe_forced(CommandFlags& commands) { commands.should_transition_pyro_test = false; commands.should_transition_idle = false; commands.should_transition_safe = false; } -void pyroTest_to_safe_timed(CommandFlags& commands) { +void MIDAS_Events::pyroTest_to_safe_timed(CommandFlags& commands) { commands.should_transition_pyro_test = false; } -void pyroTest_to_firstBoost(CommandFlags& commands) { +void MIDAS_Events::pyroTest_to_firstBoost(CommandFlags& commands) { } // IDLE -void idle_to_safe_forced(CommandFlags& commands) { +void MIDAS_Events::idle_to_safe_forced(CommandFlags& commands) { commands.should_transition_pyro_test = false; commands.should_transition_idle = false; commands.should_transition_safe = false; } -void idle_to_firstBoost(CommandFlags& commands) { +void MIDAS_Events::idle_to_firstBoost(CommandFlags& commands) { commands.FSM_should_set_cam_feed_cam1 = true; } // FIRST BOOST -void firstBoost_to_idle(CommandFlags& commands) {} -void firstBoost_to_burnout(CommandFlags& commands) {} +void MIDAS_Events::firstBoost_to_idle(CommandFlags& commands) {} +void MIDAS_Events::firstBoost_to_burnout(CommandFlags& commands) {} // BURNOUT -void burnout_to_firstBoost(CommandFlags& commands) {} -void burnout_to_sustainerIgnition(CommandFlags& commands) {} -void burnout_to_firstSeparation(CommandFlags& commands) {} +void MIDAS_Events::burnout_to_firstBoost(CommandFlags& commands) {} +void MIDAS_Events::burnout_to_sustainerIgnition(CommandFlags& commands) {} +void MIDAS_Events::burnout_to_firstSeparation(CommandFlags& commands) {} // SUSTAINER IGNITION -void sustainerIgnition_to_coast(CommandFlags& commands) {} -void sustainerIgnition_to_secondBoost(CommandFlags& commands) {} +void MIDAS_Events::sustainerIgnition_to_coast(CommandFlags& commands) {} +void MIDAS_Events::sustainerIgnition_to_secondBoost(CommandFlags& commands) {} // FIRST SEPARATION -void firstSeparation_to_coast_jerk(CommandFlags& commands) {} -void firstSeparation_to_coast_timed(CommandFlags& commands) {} +void MIDAS_Events::firstSeparation_to_coast_jerk(CommandFlags& commands) {} +void MIDAS_Events::firstSeparation_to_coast_timed(CommandFlags& commands) {} // SECOND BOOST -void secondBoost_to_sustainerIgnition(CommandFlags& commands); -void secondBoost_to_coast(CommandFlags& commands); +void MIDAS_Events::secondBoost_to_sustainerIgnition(CommandFlags& commands) {} +void MIDAS_Events::secondBoost_to_coast(CommandFlags& commands) {} // COAST -void coast_to_secondBoost(CommandFlags& commands); -void coast_to_apogee(CommandFlags& commands) { +void MIDAS_Events::coast_to_secondBoost(CommandFlags& commands) {} +void MIDAS_Events::coast_to_apogee(CommandFlags& commands) { commands.FSM_should_swap_camera_feed = true; } // APOGEE -void apogee_to_coast(CommandFlags& commands); -void apogee_to_drogueDeploy(CommandFlags& commands); +void MIDAS_Events::apogee_to_coast(CommandFlags& commands) {} +void MIDAS_Events::apogee_to_drogueDeploy(CommandFlags& commands) {} // DROUGE DEPLOY -void drogueDeploy_to_drogue_jerk(CommandFlags& commands); -void drogueDeploy_to_drouge_timed(CommandFlags& commands); +void MIDAS_Events::drogueDeploy_to_drogue_jerk(CommandFlags& commands) {} +void MIDAS_Events::drogueDeploy_to_drouge_timed(CommandFlags& commands) {} // DROGUE -void drogue_to_mainDeploy(CommandFlags& commands); +void MIDAS_Events::drogue_to_mainDeploy(CommandFlags& commands) {} // MAIN DEPLOY -void mainDeploy_to_main_jerk(CommandFlags& commands); -void mainDeploy_to_main_timed(CommandFlags& commands); +void MIDAS_Events::mainDeploy_to_main_jerk(CommandFlags& commands) {} +void MIDAS_Events::mainDeploy_to_main_timed(CommandFlags& commands) {} // MAIN -void main_to_landed(CommandFlags& commands); +void MIDAS_Events::main_to_landed(CommandFlags& commands) {} // LANDED -void landed_to_safe(CommandFlags& commands) { +void MIDAS_Events::landed_to_safe(CommandFlags& commands) { commands.should_transition_pyro_test = false; commands.should_transition_idle = false; commands.should_transition_safe = false; } -void landed_to_main(CommandFlags& commands); +void MIDAS_Events::landed_to_main(CommandFlags& commands) {} #else // SAFE -void safe_to_pyroTest(CommandFlags& commands) { +void MIDAS_Events::safe_to_pyroTest(CommandFlags& commands) { commands.should_transition_pyro_test = false; } -void safe_to_stateIdle(CommandFlags& commands) { +void MIDAS_Events::safe_to_stateIdle(CommandFlags& commands) { commands.should_transition_idle = false; } // PYRO TEST -void pyroTest_to_safe_forced(CommandFlags& commands) { +void MIDAS_Events::pyroTest_to_safe_forced(CommandFlags& commands) { commands.should_transition_pyro_test = false; commands.should_transition_idle = false; commands.should_transition_safe = false; } -void pyroTest_to_safe_timed(CommandFlags& commands) { +void MIDAS_Events::pyroTest_to_safe_timed(CommandFlags& commands) { commands.should_transition_pyro_test = false; } -void pyroTest_to_firstBoost(CommandFlags& commands) { +void MIDAS_Events::pyroTest_to_firstBoost(CommandFlags& commands) { } // IDLE -void idle_to_safe_forced(CommandFlags& commands) { +void MIDAS_Events::idle_to_safe_forced(CommandFlags& commands) { commands.should_transition_pyro_test = false; commands.should_transition_idle = false; commands.should_transition_safe = false; } -void idle_to_firstBoost(CommandFlags& commands) { +void MIDAS_Events::idle_to_firstBoost(CommandFlags& commands) { commands.FSM_should_set_cam_feed_cam1 = true; } // FIRST BOOST -void firstBoost_to_idle(CommandFlags& commands) {} -void firstBoost_to_burnout(CommandFlags& commands) {} +void MIDAS_Events::firstBoost_to_idle(CommandFlags& commands) {} +void MIDAS_Events::firstBoost_to_burnout(CommandFlags& commands) {} // BURNOUT -void burnout_to_firstBoost(CommandFlags& commands) {} -void burnout_to_sustainerIgnition(CommandFlags& commands) {} -void burnout_to_firstSeparation(CommandFlags& commands) {} +void MIDAS_Events::burnout_to_firstBoost(CommandFlags& commands) {} +void MIDAS_Events::burnout_to_sustainerIgnition(CommandFlags& commands) {} +void MIDAS_Events::burnout_to_firstSeparation(CommandFlags& commands) {} // SUSTAINER IGNITION -void sustainerIgnition_to_coast(CommandFlags& commands) {} -void sustainerIgnition_to_secondBoost(CommandFlags& commands) {} +void MIDAS_Events::sustainerIgnition_to_coast(CommandFlags& commands) {} +void MIDAS_Events::sustainerIgnition_to_secondBoost(CommandFlags& commands) {} // FIRST SEPARATION -void firstSeparation_to_coast_jerk(CommandFlags& commands) {} -void firstSeparation_to_coast_timed(CommandFlags& commands) {} +void MIDAS_Events::firstSeparation_to_coast_jerk(CommandFlags& commands) {} +void MIDAS_Events::firstSeparation_to_coast_timed(CommandFlags& commands) {} // SECOND BOOST -void secondBoost_to_sustainerIgnition(CommandFlags& commands); -void secondBoost_to_coast(CommandFlags& commands); +void MIDAS_Events::secondBoost_to_sustainerIgnition(CommandFlags& commands) {} +void MIDAS_Events::secondBoost_to_coast(CommandFlags& commands) {} // COAST -void coast_to_secondBoost(CommandFlags& commands); -void coast_to_apogee(CommandFlags& commands) { - commands.FSM_should_swap_camera_feed = true; -} +void MIDAS_Events::coast_to_secondBoost(CommandFlags& commands) {} +void MIDAS_Events::coast_to_apogee(CommandFlags& commands) {} // APOGEE -void apogee_to_coast(CommandFlags& commands); -void apogee_to_drogueDeploy(CommandFlags& commands); +void MIDAS_Events::apogee_to_coast(CommandFlags& commands) {} +void MIDAS_Events::apogee_to_drogueDeploy(CommandFlags& commands) {} // DROUGE DEPLOY -void drogueDeploy_to_drogue_jerk(CommandFlags& commands); -void drogueDeploy_to_drouge_timed(CommandFlags& commands); +void MIDAS_Events::drogueDeploy_to_drogue_jerk(CommandFlags& commands) {} +void MIDAS_Events::drogueDeploy_to_drouge_timed(CommandFlags& commands) {} // DROGUE -void drogue_to_mainDeploy(CommandFlags& commands); +void MIDAS_Events::drogue_to_mainDeploy(CommandFlags& commands) {} // MAIN DEPLOY -void mainDeploy_to_main_jerk(CommandFlags& commands); -void mainDeploy_to_main_timed(CommandFlags& commands); +void MIDAS_Events::mainDeploy_to_main_jerk(CommandFlags& commands) {} +void MIDAS_Events::mainDeploy_to_main_timed(CommandFlags& commands) {} // MAIN -void main_to_landed(CommandFlags& commands); +void MIDAS_Events::main_to_landed(CommandFlags& commands) {} // LANDED -void landed_to_safe(CommandFlags& commands) { +void MIDAS_Events::landed_to_safe(CommandFlags& commands) { commands.should_transition_pyro_test = false; commands.should_transition_idle = false; commands.should_transition_safe = false; } -void landed_to_main(CommandFlags& commands); +void MIDAS_Events::landed_to_main(CommandFlags& commands) {} #endif \ No newline at end of file diff --git a/MIDAS/src/finite-state-machines/MIDAS_Events.h b/MIDAS/src/finite-state-machines/MIDAS_Events.h new file mode 100644 index 00000000..4a3d7ada --- /dev/null +++ b/MIDAS/src/finite-state-machines/MIDAS_Events.h @@ -0,0 +1,54 @@ +#include "rocket_state.h" + +/** + * @struct MIDAS_Events + * + * @brief Abstracts changing command flags during state changes away from fsm.cpp. + * The functions are defined in MIDAS_Events.cpp + */ +struct MIDAS_Events { + void safe_to_pyroTest(CommandFlags& commands); + void safe_to_stateIdle(CommandFlags& commands); + + void pyroTest_to_safe_forced(CommandFlags& commands); + void pyroTest_to_safe_timed(CommandFlags& commands); + void pyroTest_to_firstBoost(CommandFlags& commands); + + void idle_to_safe_forced(CommandFlags& commands); + void idle_to_firstBoost(CommandFlags& commands); + + void firstBoost_to_idle(CommandFlags& commands); + void firstBoost_to_burnout(CommandFlags& commands); + + void burnout_to_firstBoost(CommandFlags& commands); + void burnout_to_sustainerIgnition(CommandFlags& commands); + void burnout_to_firstSeparation(CommandFlags& commands); + + void sustainerIgnition_to_coast(CommandFlags& commands); + void sustainerIgnition_to_secondBoost(CommandFlags& commands); + + void firstSeparation_to_coast_jerk(CommandFlags& commands); + void firstSeparation_to_coast_timed(CommandFlags& commands); + + void secondBoost_to_sustainerIgnition(CommandFlags& commands); + void secondBoost_to_coast(CommandFlags& commands); + + void coast_to_secondBoost(CommandFlags& commands); + void coast_to_apogee(CommandFlags& commands); + + void apogee_to_coast(CommandFlags& commands); + void apogee_to_drogueDeploy(CommandFlags& commands); + + void drogueDeploy_to_drogue_jerk(CommandFlags& commands); + void drogueDeploy_to_drouge_timed(CommandFlags& commands); + + void drogue_to_mainDeploy(CommandFlags& commands); + + void mainDeploy_to_main_jerk(CommandFlags& commands); + void mainDeploy_to_main_timed(CommandFlags& commands); + + void main_to_landed(CommandFlags& commands); + + void landed_to_safe(CommandFlags& commands); + void landed_to_main(CommandFlags& commands); +}; \ No newline at end of file diff --git a/MIDAS/src/finite-state-machines/fsm.cpp b/MIDAS/src/finite-state-machines/fsm.cpp index 1c490c11..61043f36 100644 --- a/MIDAS/src/finite-state-machines/fsm.cpp +++ b/MIDAS/src/finite-state-machines/fsm.cpp @@ -124,7 +124,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // Switch back to STATE_SAFE after a certain amount of time passes if((current_time - pyro_test_entry_time) > safety_pyro_test_disarm_time) { - pyroTest_to_safe_timed(commands); + dispatch.pyroTest_to_safe_timed(commands); state = FSMState::STATE_SAFE; } @@ -135,14 +135,14 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // Force transtion to safe if requested + clear all transition flags. if(commands.should_transition_safe) { state = FSMState::STATE_SAFE; - idle_to_safe_forced(commands); + dispatch.idle_to_safe_forced(commands); break; } // once a significant amount of acceleration is detected change states if (state_estimate.acceleration > sustainer_idle_to_first_boost_acceleration_threshold) { launch_time = current_time; - idle_to_firstBoost(commands); + dispatch.idle_to_firstBoost(commands); state = FSMState::STATE_FIRST_BOOST; } @@ -152,14 +152,14 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if acceleration spike was too brief then go back to idle if ((state_estimate.acceleration < sustainer_idle_to_first_boost_acceleration_threshold) && ((current_time - launch_time) < sustainer_idle_to_first_boost_time_threshold)) { state = FSMState::STATE_IDLE; - firstBoost_to_idle(commands); + dispatch.firstBoost_to_idle(commands); break; } // once acceleartion decreases to a the threshold go on the next state if (state_estimate.acceleration < sustainer_coast_detection_acceleration_threshold) { burnout_time = current_time; - firstBoost_to_burnout(commands); + dispatch.firstBoost_to_burnout(commands); state = FSMState::STATE_BURNOUT; } break; @@ -168,14 +168,14 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if low acceleration is too brief than go on to the previous state if ((state_estimate.acceleration >= sustainer_coast_detection_acceleration_threshold) && ((current_time - burnout_time) < sustainer_coast_time)) { state = FSMState::STATE_FIRST_BOOST; - burnout_to_firstBoost(commands); + dispatch.burnout_to_firstBoost(commands); break; } // if in burnout for long enough then go on to the next state (time transition) if ((current_time - burnout_time) > sustainer_coast_time) { sustainer_ignition_time = current_time; - burnout_to_sustainerIgnition(commands); + dispatch.burnout_to_sustainerIgnition(commands); state = FSMState::STATE_SUSTAINER_IGNITION; } break; @@ -185,7 +185,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // another time transition into coast after a certain amount of time if ((current_time - sustainer_ignition_time) > sustainer_ignition_to_coast_timer_threshold) { coast_time = current_time; - sustainerIgnition_to_coast(commands); + dispatch.sustainerIgnition_to_coast(commands); state = FSMState::STATE_COAST; break; } @@ -193,7 +193,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // once a high enough acceleration is detected then go to next state if (state_estimate.acceleration > sustainer_ignition_to_second_boost_acceleration_threshold) { second_boost_time = current_time; - sustainerIgnition_to_secondBoost(commands); + dispatch.sustainerIgnition_to_secondBoost(commands); state = FSMState::STATE_SECOND_BOOST; } @@ -203,14 +203,14 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if high accleration is too brief then return to previous state if ((state_estimate.acceleration < sustainer_ignition_to_second_boost_acceleration_threshold) && ((current_time - second_boost_time) < sustainer_ignition_to_second_boost_time_threshold)) { state = FSMState::STATE_SUSTAINER_IGNITION; - secondBoost_to_sustainerIgnition(commands); + dispatch.secondBoost_to_sustainerIgnition(commands); break; } // if low acceleration detected go to next state if (state_estimate.acceleration < sustainer_coast_detection_acceleration_threshold) { coast_time = current_time; - secondBoost_to_coast(commands); + dispatch.secondBoost_to_coast(commands); state = FSMState::STATE_COAST; } break; @@ -219,7 +219,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if the low acceleration detected was too brief then return to previous state if ((state_estimate.acceleration > sustainer_coast_detection_acceleration_threshold) && ((current_time - coast_time) < sustainer_second_boost_to_coast_time_threshold)) { state = FSMState::STATE_SECOND_BOOST; - coast_to_secondBoost(commands); + dispatch.coast_to_secondBoost(commands); break; } @@ -227,7 +227,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla if (state_estimate.vertical_speed <= sustainer_coast_to_apogee_vertical_speed_threshold) { apogee_time = current_time; state = FSMState::STATE_APOGEE; - coast_to_apogee(commands); + dispatch.coast_to_apogee(commands); } break; @@ -235,14 +235,14 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if the slow speed was too brief then return to previous state if ((state_estimate.vertical_speed) > sustainer_apogee_backto_coast_vertical_speed_threshold && ((current_time - apogee_time) < sustainer_apogee_check_threshold)) { state = FSMState::STATE_COAST; - apogee_to_coast(commands); + dispatch.apogee_to_coast(commands); break; } // transition to next state after a certain amount of time if ((current_time - apogee_time) > sustainer_apogee_timer_threshold) { drogue_time = current_time; - apogee_to_drogueDeploy(commands); + dispatch.apogee_to_drogueDeploy(commands); state = FSMState::STATE_DROGUE_DEPLOY; } break; @@ -255,14 +255,14 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla } if (abs(state_estimate.jerk) > sustainer_drogue_jerk_threshold) { - drogueDeploy_to_drogue_jerk(commands); + dispatch.drogueDeploy_to_drogue_jerk(commands); state = FSMState::STATE_DROGUE; break; } // if no transtion after a certain amount of time then just move on to next state if ((current_time - drogue_time) > sustainer_drogue_timer_threshold) { - drogueDeploy_to_drouge_timed(commands); + dispatch.drogueDeploy_to_drouge_timed(commands); state = FSMState::STATE_DROGUE; } @@ -272,7 +272,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if altitude low enough then next state // Also, wait at least 1 second after drogue deploy to deploy main. if (state_estimate.altitude <= sustainer_main_deploy_altitude_threshold && (current_time - drogue_time) > sustainer_main_deploy_delay_after_drogue) { - drogue_to_mainDeploy(commands); + dispatch.drogue_to_mainDeploy(commands); state = FSMState::STATE_MAIN_DEPLOY; main_time = current_time; } @@ -287,7 +287,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if detected a sharp change in jerk then go to the next state if (abs(state_estimate.jerk) > sustainer_main_jerk_threshold) { state = FSMState::STATE_MAIN; - mainDeploy_to_main_jerk(commands); + dispatch.mainDeploy_to_main_jerk(commands); main_deployed_time = current_time; break; } @@ -295,7 +295,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if no transtion after a certain amount of time then just move on to next state if ((current_time - main_time) > sustainer_main_to_main_deploy_timer_threshold) { state = FSMState::STATE_MAIN; - mainDeploy_to_main_timed(commands); + dispatch.mainDeploy_to_main_timed(commands); } break; @@ -303,7 +303,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if slowed down enough then go on to the next state if ((abs(state_estimate.vertical_speed) <= sustainer_landed_vertical_speed_threshold) && (current_time - main_deployed_time) > sustainer_main_to_landed_lockout) { landed_time = current_time; - main_to_landed(commands); + dispatch.main_to_landed(commands); state = FSMState::STATE_LANDED; } break; @@ -316,7 +316,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // Force transtion to safe if requested + clear all transition flags. if(commands.should_transition_safe) { state = FSMState::STATE_SAFE; - landed_to_safe(commands); + dispatch.landed_to_safe(commands); } break; @@ -325,7 +325,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if the slow speed was too brief then return to previous state if ((abs(state_estimate.vertical_speed) > sustainer_landed_to_main_vertical_speed_threshold) && ((current_time - landed_time) > sustainer_landed_timer_threshold)) { state = FSMState::STATE_MAIN; - landed_to_main(commands); + dispatch.landed_to_main(commands); } break; @@ -347,7 +347,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla */ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFlags& commands) { double current_time = pdTICKS_TO_MS(xTaskGetTickCount()); - MIDAS_Events dispatch + MIDAS_Events dispatch; switch (state) { case FSMState::STATE_SAFE: // Deconflict if multip commands are processed @@ -362,13 +362,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla if(commands.should_transition_pyro_test) { state = FSMState::STATE_PYRO_TEST; pyro_test_entry_time = current_time; - safe_to_pyroTest(commands); + dispatch.safe_to_pyroTest(commands); } // Only switch to STATE_IDLE if triggered wirelessly. if(commands.should_transition_idle) { state = FSMState::STATE_IDLE; - safe_to_stateIdle(commands); + dispatch.safe_to_stateIdle(commands); } break; @@ -377,13 +377,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // Force transtion to safe if requested + clear all transition flags. if(commands.should_transition_safe) { state = FSMState::STATE_SAFE; - pyroTest_to_safe_forced(commands); + dispatch.pyroTest_to_safe_forced(commands); break; } // Switch back to STATE_SAFE after a certain amount of time passes if((current_time - pyro_test_entry_time) > safety_pyro_test_disarm_time) { - pyroTest_to_safe_timed(commands); + dispatch.pyroTest_to_safe_timed(commands); state = FSMState::STATE_SAFE; } @@ -393,14 +393,14 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // Force transtion to safe if requested + clear all transition flags. if(commands.should_transition_safe) { state = FSMState::STATE_SAFE; - idle_to_safe_forced(commands); + dispatch.idle_to_safe_forced(commands); break; } // once a significant amount of acceleration is detected change states if (state_estimate.acceleration > booster_idle_to_first_boost_acceleration_threshold) { launch_time = current_time; - idle_to_firstBoost(commands); + dispatch.idle_to_firstBoost(commands); state = FSMState::STATE_FIRST_BOOST; } @@ -408,13 +408,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla case FSMState::STATE_FIRST_BOOST: if ((state_estimate.acceleration < booster_idle_to_first_boost_acceleration_threshold) && ((current_time - launch_time) < booster_idle_to_first_boost_time_threshold)) { state = FSMState::STATE_IDLE; - firstBoost_to_idle(commands); + dispatch.firstBoost_to_idle(commands); break; } if (state_estimate.acceleration < booster_coast_detection_acceleration_threshold) { burnout_time = current_time; state = FSMState::STATE_BURNOUT; - firstBoost_to_burnout(commands); + dispatch.firstBoost_to_burnout(commands); } break; @@ -422,13 +422,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla case FSMState::STATE_BURNOUT: if ((state_estimate.acceleration >= booster_coast_detection_acceleration_threshold) && ((current_time - burnout_time) < booster_first_boost_to_burnout_time_threshold)) { state = FSMState::STATE_FIRST_BOOST; - burnout_to_firstBoost(commands); + dispatch.burnout_to_firstBoost(commands); break; } if ((current_time - burnout_time) > booster_first_boost_to_burnout_time_threshold) { first_separation_time = current_time; - burnout_to_firstSeparation(commands); + dispatch.burnout_to_firstSeparation(commands); state = FSMState::STATE_FIRST_SEPARATION; } break; @@ -436,13 +436,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla case FSMState::STATE_FIRST_SEPARATION: if (abs(state_estimate.jerk) < booster_first_separation_jerk_threshold) { state = FSMState::STATE_COAST; - firstSeparation_to_coast_jerk(commands); + dispatch.firstSeparation_to_coast_jerk(commands); break; } if ((current_time - first_separation_time) > booster_first_seperation_time_threshold) { state = FSMState::STATE_COAST; - firstSeparation_to_coast_timed(commands); + dispatch.firstSeparation_to_coast_timed(commands); } break; @@ -451,18 +451,21 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla if (state_estimate.vertical_speed <= booster_coast_to_apogee_vertical_speed_threshold) { apogee_time = current_time; state = FSMState::STATE_APOGEE; + dispatch.coast_to_apogee(commands); } break; case FSMState::STATE_APOGEE: if (state_estimate.vertical_speed > booster_coast_to_apogee_vertical_speed_threshold && ((current_time - apogee_time) < booster_apogee_check_threshold)) { state = FSMState::STATE_COAST; + dispatch.apogee_to_coast(commands); break; } if ((current_time - apogee_time) > booster_apogee_timer_threshold) { drogue_time = current_time; state = FSMState::STATE_DROGUE_DEPLOY; + dispatch.apogee_to_drogueDeploy(commands); } break; @@ -475,10 +478,12 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla if (abs(state_estimate.jerk) > booster_drogue_jerk_threshold) { state = FSMState::STATE_DROGUE; + dispatch.drogueDeploy_to_drogue_jerk(commands); break; } if ((current_time - drogue_time) > booster_drogue_timer_threshold) { state = FSMState::STATE_DROGUE; + dispatch.drogueDeploy_to_drouge_timed(commands); } break; @@ -489,6 +494,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla if (state_estimate.altitude <= booster_main_deploy_altitude_threshold && (current_time - drogue_time) > booster_main_deploy_delay_after_drogue) { state = FSMState::STATE_MAIN_DEPLOY; main_time = current_time; + dispatch.drogue_to_mainDeploy(commands); } break; @@ -502,11 +508,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla if (abs(state_estimate.jerk) > booster_main_jerk_threshold) { state = FSMState::STATE_MAIN; main_deployed_time = current_time; + dispatch.mainDeploy_to_main_jerk(commands); break; } if ((current_time - main_time) > booster_main_to_main_deploy_timer_threshold) { state = FSMState::STATE_MAIN; + dispatch.mainDeploy_to_main_timed(commands); } break; @@ -514,6 +522,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla if (abs(state_estimate.vertical_speed) <= booster_landed_vertical_speed_threshold && (current_time - main_deployed_time) > booster_main_to_landed_lockout) { landed_time = current_time; state = FSMState::STATE_LANDED; + dispatch.main_to_landed(commands); } break; @@ -525,9 +534,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // Force transtion to safe if requested + clear all transition flags. if(commands.should_transition_safe) { state = FSMState::STATE_SAFE; - commands.should_transition_pyro_test = false; - commands.should_transition_idle = false; - commands.should_transition_safe = false; + dispatch.landed_to_safe(commands); } break; @@ -535,6 +542,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla if ((abs(state_estimate.vertical_speed) > booster_landed_to_main_vertical_speed_threshold) && ((current_time - landed_time) > booster_landed_timer_threshold)) { state = FSMState::STATE_MAIN; + dispatch.landed_to_main(commands); } break; diff --git a/MIDAS/src/finite-state-machines/fsm.h b/MIDAS/src/finite-state-machines/fsm.h index 20ae11f6..23dbe87f 100644 --- a/MIDAS/src/finite-state-machines/fsm.h +++ b/MIDAS/src/finite-state-machines/fsm.h @@ -8,7 +8,7 @@ #include "sensor_data.h" #include "Buffer.h" #include "rocket_state.h" -#include "MIDAS_Events.cpp" +#include "MIDAS_Events.h" /** * @struct StateEstimate diff --git a/MIDAS/src/rocket_state.h b/MIDAS/src/rocket_state.h index f49413f5..e4d1308f 100644 --- a/MIDAS/src/rocket_state.h +++ b/MIDAS/src/rocket_state.h @@ -173,58 +173,7 @@ struct CommandFlags { bool FSM_should_swap_camera_feed = false; // Triggered COAST --> APOGEE }; -/** - * @struct MIDAS_Events - * - * @brief Abstracts changing command flags during state changes away from fsm.cpp. - * The functions are defined in MIDAS_Events.cpp - */ -struct MIDAS_Events { - void safe_to_pyroTest(CommandFlags& commands); - void safe_to_stateIdle(CommandFlags& commands); - - void pyroTest_to_safe_forced(CommandFlags& commands); - void pyroTest_to_safe_timed(CommandFlags& commands); - void pyroTest_to_firstBoost(CommandFlags& commands); - - void idle_to_safe_forced(CommandFlags& commands); - void idle_to_firstBoost(CommandFlags& commands); - - void firstBoost_to_idle(CommandFlags& commands); - void firstBoost_to_burnout(CommandFlags& commands); - - void burnout_to_firstBoost(CommandFlags& commands); - void burnout_to_sustainerIgnition(CommandFlags& commands); - void burnout_to_firstSeparation(CommandFlags& commands); - - void sustainerIgnition_to_coast(CommandFlags& commands); - void sustainerIgnition_to_secondBoost(CommandFlags& commands); - - void firstSeparation_to_coast_jerk(CommandFlags& commands); - void firstSeparation_to_coast_timed(CommandFlags& commands); - void secondBoost_to_sustainerIgnition(CommandFlags& commands); - void secondBoost_to_coast(CommandFlags& commands); - - void coast_to_secondBoost(CommandFlags& commands); - void coast_to_apogee(CommandFlags& commands); - - void apogee_to_coast(CommandFlags& commands); - void apogee_to_drogueDeploy(CommandFlags& commands); - - void drogueDeploy_to_drogue_jerk(CommandFlags& commands); - void drogueDeploy_to_drouge_timed(CommandFlags& commands); - - void drogue_to_mainDeploy(CommandFlags& commands); - - void mainDeploy_to_main_jerk(CommandFlags& commands); - void mainDeploy_to_main_timed(CommandFlags& commands); - - void main_to_landed(CommandFlags& commands); - - void landed_to_safe(CommandFlags& commands); - void landed_to_main(CommandFlags& commands); -}; /** * @struct RocketData * From 4a56cd0835fdc0c88fa3cb244e0a1c7d32a908b5 Mon Sep 17 00:00:00 2001 From: ishrakj Date: Sat, 8 Nov 2025 13:47:41 -0600 Subject: [PATCH 6/6] changed MIDAS_Events functions to take in RocketData struct and dereference command_flags, updated tick_fsm to take in rocket_data instead of commandflags --- .../finite-state-machines/MIDAS_Events.cpp | 174 +++++++++--------- .../src/finite-state-machines/MIDAS_Events.h | 60 +++--- MIDAS/src/finite-state-machines/fsm.cpp | 138 +++++++------- MIDAS/src/finite-state-machines/fsm.h | 2 +- MIDAS/src/systems.cpp | 2 +- 5 files changed, 188 insertions(+), 188 deletions(-) diff --git a/MIDAS/src/finite-state-machines/MIDAS_Events.cpp b/MIDAS/src/finite-state-machines/MIDAS_Events.cpp index df65e089..36b46e2c 100644 --- a/MIDAS/src/finite-state-machines/MIDAS_Events.cpp +++ b/MIDAS/src/finite-state-machines/MIDAS_Events.cpp @@ -3,173 +3,173 @@ #ifdef IS_SUSTAINER // SAFE -void MIDAS_Events::safe_to_pyroTest(CommandFlags& commands) { - commands.should_transition_pyro_test = false; +void MIDAS_Events::safe_to_pyroTest(RocketData& rocket_data) { + rocket_data.command_flags.should_transition_pyro_test = false; } -void MIDAS_Events::safe_to_stateIdle(CommandFlags& commands) { - commands.should_transition_idle = false; +void MIDAS_Events::safe_to_stateIdle(RocketData& rocket_data) { + rocket_data.command_flags.should_transition_idle = false; } // PYRO TEST -void MIDAS_Events::pyroTest_to_safe_forced(CommandFlags& commands) { - commands.should_transition_pyro_test = false; - commands.should_transition_idle = false; - commands.should_transition_safe = false; +void MIDAS_Events::pyroTest_to_safe_forced(RocketData& rocket_data) { + rocket_data.command_flags.should_transition_pyro_test = false; + rocket_data.command_flags.should_transition_idle = false; + rocket_data.command_flags.should_transition_safe = false; } -void MIDAS_Events::pyroTest_to_safe_timed(CommandFlags& commands) { - commands.should_transition_pyro_test = false; +void MIDAS_Events::pyroTest_to_safe_timed(RocketData& rocket_data) { + rocket_data.command_flags.should_transition_pyro_test = false; } -void MIDAS_Events::pyroTest_to_firstBoost(CommandFlags& commands) { +void MIDAS_Events::pyroTest_to_firstBoost(RocketData& rocket_data) { } // IDLE -void MIDAS_Events::idle_to_safe_forced(CommandFlags& commands) { - commands.should_transition_pyro_test = false; - commands.should_transition_idle = false; - commands.should_transition_safe = false; +void MIDAS_Events::idle_to_safe_forced(RocketData& rocket_data) { + rocket_data.command_flags.should_transition_pyro_test = false; + rocket_data.command_flags.should_transition_idle = false; + rocket_data.command_flags.should_transition_safe = false; } -void MIDAS_Events::idle_to_firstBoost(CommandFlags& commands) { - commands.FSM_should_set_cam_feed_cam1 = true; +void MIDAS_Events::idle_to_firstBoost(RocketData& rocket_data) { + rocket_data.command_flags.FSM_should_set_cam_feed_cam1 = true; } // FIRST BOOST -void MIDAS_Events::firstBoost_to_idle(CommandFlags& commands) {} -void MIDAS_Events::firstBoost_to_burnout(CommandFlags& commands) {} +void MIDAS_Events::firstBoost_to_idle(RocketData& rocket_data) {} +void MIDAS_Events::firstBoost_to_burnout(RocketData& rocket_data) {} // BURNOUT -void MIDAS_Events::burnout_to_firstBoost(CommandFlags& commands) {} -void MIDAS_Events::burnout_to_sustainerIgnition(CommandFlags& commands) {} -void MIDAS_Events::burnout_to_firstSeparation(CommandFlags& commands) {} +void MIDAS_Events::burnout_to_firstBoost(RocketData& rocket_data) {} +void MIDAS_Events::burnout_to_sustainerIgnition(RocketData& rocket_data) {} +void MIDAS_Events::burnout_to_firstSeparation(RocketData& rocket_data) {} // SUSTAINER IGNITION -void MIDAS_Events::sustainerIgnition_to_coast(CommandFlags& commands) {} -void MIDAS_Events::sustainerIgnition_to_secondBoost(CommandFlags& commands) {} +void MIDAS_Events::sustainerIgnition_to_coast(RocketData& rocket_data) {} +void MIDAS_Events::sustainerIgnition_to_secondBoost(RocketData& rocket_data) {} // FIRST SEPARATION -void MIDAS_Events::firstSeparation_to_coast_jerk(CommandFlags& commands) {} -void MIDAS_Events::firstSeparation_to_coast_timed(CommandFlags& commands) {} +void MIDAS_Events::firstSeparation_to_coast_jerk(RocketData& rocket_data) {} +void MIDAS_Events::firstSeparation_to_coast_timed(RocketData& rocket_data) {} // SECOND BOOST -void MIDAS_Events::secondBoost_to_sustainerIgnition(CommandFlags& commands) {} -void MIDAS_Events::secondBoost_to_coast(CommandFlags& commands) {} +void MIDAS_Events::secondBoost_to_sustainerIgnition(RocketData& rocket_data) {} +void MIDAS_Events::secondBoost_to_coast(RocketData& rocket_data) {} // COAST -void MIDAS_Events::coast_to_secondBoost(CommandFlags& commands) {} -void MIDAS_Events::coast_to_apogee(CommandFlags& commands) { - commands.FSM_should_swap_camera_feed = true; +void MIDAS_Events::coast_to_secondBoost(RocketData& rocket_data) {} +void MIDAS_Events::coast_to_apogee(RocketData& rocket_data) { + rocket_data.command_flags.FSM_should_swap_camera_feed = true; } // APOGEE -void MIDAS_Events::apogee_to_coast(CommandFlags& commands) {} -void MIDAS_Events::apogee_to_drogueDeploy(CommandFlags& commands) {} +void MIDAS_Events::apogee_to_coast(RocketData& rocket_data) {} +void MIDAS_Events::apogee_to_drogueDeploy(RocketData& rocket_data) {} // DROUGE DEPLOY -void MIDAS_Events::drogueDeploy_to_drogue_jerk(CommandFlags& commands) {} -void MIDAS_Events::drogueDeploy_to_drouge_timed(CommandFlags& commands) {} +void MIDAS_Events::drogueDeploy_to_drogue_jerk(RocketData& rocket_data) {} +void MIDAS_Events::drogueDeploy_to_drouge_timed(RocketData& rocket_data) {} // DROGUE -void MIDAS_Events::drogue_to_mainDeploy(CommandFlags& commands) {} +void MIDAS_Events::drogue_to_mainDeploy(RocketData& rocket_data) {} // MAIN DEPLOY -void MIDAS_Events::mainDeploy_to_main_jerk(CommandFlags& commands) {} -void MIDAS_Events::mainDeploy_to_main_timed(CommandFlags& commands) {} +void MIDAS_Events::mainDeploy_to_main_jerk(RocketData& rocket_data) {} +void MIDAS_Events::mainDeploy_to_main_timed(RocketData& rocket_data) {} // MAIN -void MIDAS_Events::main_to_landed(CommandFlags& commands) {} +void MIDAS_Events::main_to_landed(RocketData& rocket_data) {} // LANDED -void MIDAS_Events::landed_to_safe(CommandFlags& commands) { - commands.should_transition_pyro_test = false; - commands.should_transition_idle = false; - commands.should_transition_safe = false; +void MIDAS_Events::landed_to_safe(RocketData& rocket_data) { + rocket_data.command_flags.should_transition_pyro_test = false; + rocket_data.command_flags.should_transition_idle = false; + rocket_data.command_flags.should_transition_safe = false; } -void MIDAS_Events::landed_to_main(CommandFlags& commands) {} +void MIDAS_Events::landed_to_main(RocketData& rocket_data) {} #else // SAFE -void MIDAS_Events::safe_to_pyroTest(CommandFlags& commands) { - commands.should_transition_pyro_test = false; +void MIDAS_Events::safe_to_pyroTest(RocketData& rocket_data) { + rocket_data.command_flags.should_transition_pyro_test = false; } -void MIDAS_Events::safe_to_stateIdle(CommandFlags& commands) { - commands.should_transition_idle = false; +void MIDAS_Events::safe_to_stateIdle(RocketData& rocket_data) { + rocket_data.command_flags.should_transition_idle = false; } // PYRO TEST -void MIDAS_Events::pyroTest_to_safe_forced(CommandFlags& commands) { - commands.should_transition_pyro_test = false; - commands.should_transition_idle = false; - commands.should_transition_safe = false; +void MIDAS_Events::pyroTest_to_safe_forced(RocketData& rocket_data) { + rocket_data.command_flags.should_transition_pyro_test = false; + rocket_data.command_flags.should_transition_idle = false; + rocket_data.command_flags.should_transition_safe = false; } -void MIDAS_Events::pyroTest_to_safe_timed(CommandFlags& commands) { - commands.should_transition_pyro_test = false; +void MIDAS_Events::pyroTest_to_safe_timed(RocketData& rocket_data) { + rocket_data.command_flags.should_transition_pyro_test = false; } -void MIDAS_Events::pyroTest_to_firstBoost(CommandFlags& commands) { +void MIDAS_Events::pyroTest_to_firstBoost(RocketData& rocket_data) { } // IDLE -void MIDAS_Events::idle_to_safe_forced(CommandFlags& commands) { - commands.should_transition_pyro_test = false; - commands.should_transition_idle = false; - commands.should_transition_safe = false; +void MIDAS_Events::idle_to_safe_forced(RocketData& rocket_data) { + rocket_data.command_flags.should_transition_pyro_test = false; + rocket_data.command_flags.should_transition_idle = false; + rocket_data.command_flags.should_transition_safe = false; } -void MIDAS_Events::idle_to_firstBoost(CommandFlags& commands) { - commands.FSM_should_set_cam_feed_cam1 = true; +void MIDAS_Events::idle_to_firstBoost(RocketData& rocket_data) { + rocket_data.command_flags.FSM_should_set_cam_feed_cam1 = true; } // FIRST BOOST -void MIDAS_Events::firstBoost_to_idle(CommandFlags& commands) {} -void MIDAS_Events::firstBoost_to_burnout(CommandFlags& commands) {} +void MIDAS_Events::firstBoost_to_idle(RocketData& rocket_data) {} +void MIDAS_Events::firstBoost_to_burnout(RocketData& rocket_data) {} // BURNOUT -void MIDAS_Events::burnout_to_firstBoost(CommandFlags& commands) {} -void MIDAS_Events::burnout_to_sustainerIgnition(CommandFlags& commands) {} -void MIDAS_Events::burnout_to_firstSeparation(CommandFlags& commands) {} +void MIDAS_Events::burnout_to_firstBoost(RocketData& rocket_data) {} +void MIDAS_Events::burnout_to_sustainerIgnition(RocketData& rocket_data) {} +void MIDAS_Events::burnout_to_firstSeparation(RocketData& rocket_data) {} // SUSTAINER IGNITION -void MIDAS_Events::sustainerIgnition_to_coast(CommandFlags& commands) {} -void MIDAS_Events::sustainerIgnition_to_secondBoost(CommandFlags& commands) {} +void MIDAS_Events::sustainerIgnition_to_coast(RocketData& rocket_data) {} +void MIDAS_Events::sustainerIgnition_to_secondBoost(RocketData& rocket_data) {} // FIRST SEPARATION -void MIDAS_Events::firstSeparation_to_coast_jerk(CommandFlags& commands) {} -void MIDAS_Events::firstSeparation_to_coast_timed(CommandFlags& commands) {} +void MIDAS_Events::firstSeparation_to_coast_jerk(RocketData& rocket_data) {} +void MIDAS_Events::firstSeparation_to_coast_timed(RocketData& rocket_data) {} // SECOND BOOST -void MIDAS_Events::secondBoost_to_sustainerIgnition(CommandFlags& commands) {} -void MIDAS_Events::secondBoost_to_coast(CommandFlags& commands) {} +void MIDAS_Events::secondBoost_to_sustainerIgnition(RocketData& rocket_data) {} +void MIDAS_Events::secondBoost_to_coast(RocketData& rocket_data) {} // COAST -void MIDAS_Events::coast_to_secondBoost(CommandFlags& commands) {} -void MIDAS_Events::coast_to_apogee(CommandFlags& commands) {} +void MIDAS_Events::coast_to_secondBoost(RocketData& rocket_data) {} +void MIDAS_Events::coast_to_apogee(RocketData& rocket_data) {} // APOGEE -void MIDAS_Events::apogee_to_coast(CommandFlags& commands) {} -void MIDAS_Events::apogee_to_drogueDeploy(CommandFlags& commands) {} +void MIDAS_Events::apogee_to_coast(RocketData& rocket_data) {} +void MIDAS_Events::apogee_to_drogueDeploy(RocketData& rocket_data) {} // DROUGE DEPLOY -void MIDAS_Events::drogueDeploy_to_drogue_jerk(CommandFlags& commands) {} -void MIDAS_Events::drogueDeploy_to_drouge_timed(CommandFlags& commands) {} +void MIDAS_Events::drogueDeploy_to_drogue_jerk(RocketData& rocket_data) {} +void MIDAS_Events::drogueDeploy_to_drouge_timed(RocketData& rocket_data) {} // DROGUE -void MIDAS_Events::drogue_to_mainDeploy(CommandFlags& commands) {} +void MIDAS_Events::drogue_to_mainDeploy(RocketData& rocket_data) {} // MAIN DEPLOY -void MIDAS_Events::mainDeploy_to_main_jerk(CommandFlags& commands) {} -void MIDAS_Events::mainDeploy_to_main_timed(CommandFlags& commands) {} +void MIDAS_Events::mainDeploy_to_main_jerk(RocketData& rocket_data) {} +void MIDAS_Events::mainDeploy_to_main_timed(RocketData& rocket_data) {} // MAIN -void MIDAS_Events::main_to_landed(CommandFlags& commands) {} +void MIDAS_Events::main_to_landed(RocketData& rocket_data) {} // LANDED -void MIDAS_Events::landed_to_safe(CommandFlags& commands) { - commands.should_transition_pyro_test = false; - commands.should_transition_idle = false; - commands.should_transition_safe = false; +void MIDAS_Events::landed_to_safe(RocketData& rocket_data) { + rocket_data.command_flags.should_transition_pyro_test = false; + rocket_data.command_flags.should_transition_idle = false; + rocket_data.command_flags.should_transition_safe = false; } -void MIDAS_Events::landed_to_main(CommandFlags& commands) {} +void MIDAS_Events::landed_to_main(RocketData& rocket_data) {} #endif \ No newline at end of file diff --git a/MIDAS/src/finite-state-machines/MIDAS_Events.h b/MIDAS/src/finite-state-machines/MIDAS_Events.h index 4a3d7ada..24029ac5 100644 --- a/MIDAS/src/finite-state-machines/MIDAS_Events.h +++ b/MIDAS/src/finite-state-machines/MIDAS_Events.h @@ -7,48 +7,48 @@ * The functions are defined in MIDAS_Events.cpp */ struct MIDAS_Events { - void safe_to_pyroTest(CommandFlags& commands); - void safe_to_stateIdle(CommandFlags& commands); + void safe_to_pyroTest(RocketData& rocket_data); + void safe_to_stateIdle(RocketData& rocket_data); - void pyroTest_to_safe_forced(CommandFlags& commands); - void pyroTest_to_safe_timed(CommandFlags& commands); - void pyroTest_to_firstBoost(CommandFlags& commands); + void pyroTest_to_safe_forced(RocketData& rocket_data); + void pyroTest_to_safe_timed(RocketData& rocket_data); + void pyroTest_to_firstBoost(RocketData& rocket_data); - void idle_to_safe_forced(CommandFlags& commands); - void idle_to_firstBoost(CommandFlags& commands); + void idle_to_safe_forced(RocketData& rocket_data); + void idle_to_firstBoost(RocketData& rocket_data); - void firstBoost_to_idle(CommandFlags& commands); - void firstBoost_to_burnout(CommandFlags& commands); + void firstBoost_to_idle(RocketData& rocket_data); + void firstBoost_to_burnout(RocketData& rocket_data); - void burnout_to_firstBoost(CommandFlags& commands); - void burnout_to_sustainerIgnition(CommandFlags& commands); - void burnout_to_firstSeparation(CommandFlags& commands); + void burnout_to_firstBoost(RocketData& rocket_data); + void burnout_to_sustainerIgnition(RocketData& rocket_data); + void burnout_to_firstSeparation(RocketData& rocket_data); - void sustainerIgnition_to_coast(CommandFlags& commands); - void sustainerIgnition_to_secondBoost(CommandFlags& commands); + void sustainerIgnition_to_coast(RocketData& rocket_data); + void sustainerIgnition_to_secondBoost(RocketData& rocket_data); - void firstSeparation_to_coast_jerk(CommandFlags& commands); - void firstSeparation_to_coast_timed(CommandFlags& commands); + void firstSeparation_to_coast_jerk(RocketData& rocket_data); + void firstSeparation_to_coast_timed(RocketData& rocket_data); - void secondBoost_to_sustainerIgnition(CommandFlags& commands); - void secondBoost_to_coast(CommandFlags& commands); + void secondBoost_to_sustainerIgnition(RocketData& rocket_data); + void secondBoost_to_coast(RocketData& rocket_data); - void coast_to_secondBoost(CommandFlags& commands); - void coast_to_apogee(CommandFlags& commands); + void coast_to_secondBoost(RocketData& rocket_data); + void coast_to_apogee(RocketData& rocket_data); - void apogee_to_coast(CommandFlags& commands); - void apogee_to_drogueDeploy(CommandFlags& commands); + void apogee_to_coast(RocketData& rocket_data); + void apogee_to_drogueDeploy(RocketData& rocket_data); - void drogueDeploy_to_drogue_jerk(CommandFlags& commands); - void drogueDeploy_to_drouge_timed(CommandFlags& commands); + void drogueDeploy_to_drogue_jerk(RocketData& rocket_data); + void drogueDeploy_to_drouge_timed(RocketData& rocket_data); - void drogue_to_mainDeploy(CommandFlags& commands); + void drogue_to_mainDeploy(RocketData& rocket_data); - void mainDeploy_to_main_jerk(CommandFlags& commands); - void mainDeploy_to_main_timed(CommandFlags& commands); + void mainDeploy_to_main_jerk(RocketData& rocket_data); + void mainDeploy_to_main_timed(RocketData& rocket_data); - void main_to_landed(CommandFlags& commands); + void main_to_landed(RocketData& rocket_data); - void landed_to_safe(CommandFlags& commands); - void landed_to_main(CommandFlags& commands); + void landed_to_safe(RocketData& rocket_data); + void landed_to_main(RocketData& rocket_data); }; \ No newline at end of file diff --git a/MIDAS/src/finite-state-machines/fsm.cpp b/MIDAS/src/finite-state-machines/fsm.cpp index 61043f36..16209d86 100644 --- a/MIDAS/src/finite-state-machines/fsm.cpp +++ b/MIDAS/src/finite-state-machines/fsm.cpp @@ -84,7 +84,7 @@ StateEstimate::StateEstimate(RocketData& state) { * * @return New FSM State */ -FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFlags& commands) { +FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, RocketData& rocket_data) { //get current time double current_time = pdTICKS_TO_MS(xTaskGetTickCount()); MIDAS_Events dispatch; @@ -92,39 +92,39 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla case FSMState::STATE_SAFE: // Deconflict if multip commands are processed - if(commands.should_transition_safe) { - commands.should_transition_pyro_test = false; - commands.should_transition_idle = false; - commands.should_transition_safe = false; + if(rocket_data.command_flags.should_transition_safe) { + rocket_data.command_flags.should_transition_pyro_test = false; + rocket_data.command_flags.should_transition_idle = false; + rocket_data.command_flags.should_transition_safe = false; break; } // Only switch to STATE_PYRO_TEST if triggered wirelessly - if(commands.should_transition_pyro_test) { + if(rocket_data.command_flags.should_transition_pyro_test) { state = FSMState::STATE_PYRO_TEST; pyro_test_entry_time = current_time; - dispatch.safe_to_pyroTest(commands); + dispatch.safe_to_pyroTest(rocket_data); } // Only switch to STATE_IDLE if triggered wirelessly. - if(commands.should_transition_idle) { + if(rocket_data.command_flags.should_transition_idle) { state = FSMState::STATE_IDLE; - dispatch.safe_to_stateIdle(commands); + dispatch.safe_to_stateIdle(rocket_data); } break; case FSMState::STATE_PYRO_TEST: // Force transtion to safe if requested + clear all transition flags. - if(commands.should_transition_safe) { + if(rocket_data.command_flags.should_transition_safe) { state = FSMState::STATE_SAFE; - dispatch.pyroTest_to_safe_forced(commands); + dispatch.pyroTest_to_safe_forced(rocket_data); break; } // Switch back to STATE_SAFE after a certain amount of time passes if((current_time - pyro_test_entry_time) > safety_pyro_test_disarm_time) { - dispatch.pyroTest_to_safe_timed(commands); + dispatch.pyroTest_to_safe_timed(rocket_data); state = FSMState::STATE_SAFE; } @@ -133,16 +133,16 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla case FSMState::STATE_IDLE: // Force transtion to safe if requested + clear all transition flags. - if(commands.should_transition_safe) { + if(rocket_data.command_flags.should_transition_safe) { state = FSMState::STATE_SAFE; - dispatch.idle_to_safe_forced(commands); + dispatch.idle_to_safe_forced(rocket_data); break; } // once a significant amount of acceleration is detected change states if (state_estimate.acceleration > sustainer_idle_to_first_boost_acceleration_threshold) { launch_time = current_time; - dispatch.idle_to_firstBoost(commands); + dispatch.idle_to_firstBoost(rocket_data); state = FSMState::STATE_FIRST_BOOST; } @@ -152,14 +152,14 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if acceleration spike was too brief then go back to idle if ((state_estimate.acceleration < sustainer_idle_to_first_boost_acceleration_threshold) && ((current_time - launch_time) < sustainer_idle_to_first_boost_time_threshold)) { state = FSMState::STATE_IDLE; - dispatch.firstBoost_to_idle(commands); + dispatch.firstBoost_to_idle(rocket_data); break; } // once acceleartion decreases to a the threshold go on the next state if (state_estimate.acceleration < sustainer_coast_detection_acceleration_threshold) { burnout_time = current_time; - dispatch.firstBoost_to_burnout(commands); + dispatch.firstBoost_to_burnout(rocket_data); state = FSMState::STATE_BURNOUT; } break; @@ -168,14 +168,14 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if low acceleration is too brief than go on to the previous state if ((state_estimate.acceleration >= sustainer_coast_detection_acceleration_threshold) && ((current_time - burnout_time) < sustainer_coast_time)) { state = FSMState::STATE_FIRST_BOOST; - dispatch.burnout_to_firstBoost(commands); + dispatch.burnout_to_firstBoost(rocket_data); break; } // if in burnout for long enough then go on to the next state (time transition) if ((current_time - burnout_time) > sustainer_coast_time) { sustainer_ignition_time = current_time; - dispatch.burnout_to_sustainerIgnition(commands); + dispatch.burnout_to_sustainerIgnition(rocket_data); state = FSMState::STATE_SUSTAINER_IGNITION; } break; @@ -185,7 +185,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // another time transition into coast after a certain amount of time if ((current_time - sustainer_ignition_time) > sustainer_ignition_to_coast_timer_threshold) { coast_time = current_time; - dispatch.sustainerIgnition_to_coast(commands); + dispatch.sustainerIgnition_to_coast(rocket_data); state = FSMState::STATE_COAST; break; } @@ -193,7 +193,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // once a high enough acceleration is detected then go to next state if (state_estimate.acceleration > sustainer_ignition_to_second_boost_acceleration_threshold) { second_boost_time = current_time; - dispatch.sustainerIgnition_to_secondBoost(commands); + dispatch.sustainerIgnition_to_secondBoost(rocket_data); state = FSMState::STATE_SECOND_BOOST; } @@ -203,14 +203,14 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if high accleration is too brief then return to previous state if ((state_estimate.acceleration < sustainer_ignition_to_second_boost_acceleration_threshold) && ((current_time - second_boost_time) < sustainer_ignition_to_second_boost_time_threshold)) { state = FSMState::STATE_SUSTAINER_IGNITION; - dispatch.secondBoost_to_sustainerIgnition(commands); + dispatch.secondBoost_to_sustainerIgnition(rocket_data); break; } // if low acceleration detected go to next state if (state_estimate.acceleration < sustainer_coast_detection_acceleration_threshold) { coast_time = current_time; - dispatch.secondBoost_to_coast(commands); + dispatch.secondBoost_to_coast(rocket_data); state = FSMState::STATE_COAST; } break; @@ -219,7 +219,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if the low acceleration detected was too brief then return to previous state if ((state_estimate.acceleration > sustainer_coast_detection_acceleration_threshold) && ((current_time - coast_time) < sustainer_second_boost_to_coast_time_threshold)) { state = FSMState::STATE_SECOND_BOOST; - dispatch.coast_to_secondBoost(commands); + dispatch.coast_to_secondBoost(rocket_data); break; } @@ -227,7 +227,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla if (state_estimate.vertical_speed <= sustainer_coast_to_apogee_vertical_speed_threshold) { apogee_time = current_time; state = FSMState::STATE_APOGEE; - dispatch.coast_to_apogee(commands); + dispatch.coast_to_apogee(rocket_data); } break; @@ -235,14 +235,14 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if the slow speed was too brief then return to previous state if ((state_estimate.vertical_speed) > sustainer_apogee_backto_coast_vertical_speed_threshold && ((current_time - apogee_time) < sustainer_apogee_check_threshold)) { state = FSMState::STATE_COAST; - dispatch.apogee_to_coast(commands); + dispatch.apogee_to_coast(rocket_data); break; } // transition to next state after a certain amount of time if ((current_time - apogee_time) > sustainer_apogee_timer_threshold) { drogue_time = current_time; - dispatch.apogee_to_drogueDeploy(commands); + dispatch.apogee_to_drogueDeploy(rocket_data); state = FSMState::STATE_DROGUE_DEPLOY; } break; @@ -255,14 +255,14 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla } if (abs(state_estimate.jerk) > sustainer_drogue_jerk_threshold) { - dispatch.drogueDeploy_to_drogue_jerk(commands); + dispatch.drogueDeploy_to_drogue_jerk(rocket_data); state = FSMState::STATE_DROGUE; break; } // if no transtion after a certain amount of time then just move on to next state if ((current_time - drogue_time) > sustainer_drogue_timer_threshold) { - dispatch.drogueDeploy_to_drouge_timed(commands); + dispatch.drogueDeploy_to_drouge_timed(rocket_data); state = FSMState::STATE_DROGUE; } @@ -272,7 +272,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if altitude low enough then next state // Also, wait at least 1 second after drogue deploy to deploy main. if (state_estimate.altitude <= sustainer_main_deploy_altitude_threshold && (current_time - drogue_time) > sustainer_main_deploy_delay_after_drogue) { - dispatch.drogue_to_mainDeploy(commands); + dispatch.drogue_to_mainDeploy(rocket_data); state = FSMState::STATE_MAIN_DEPLOY; main_time = current_time; } @@ -287,7 +287,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if detected a sharp change in jerk then go to the next state if (abs(state_estimate.jerk) > sustainer_main_jerk_threshold) { state = FSMState::STATE_MAIN; - dispatch.mainDeploy_to_main_jerk(commands); + dispatch.mainDeploy_to_main_jerk(rocket_data); main_deployed_time = current_time; break; } @@ -295,7 +295,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if no transtion after a certain amount of time then just move on to next state if ((current_time - main_time) > sustainer_main_to_main_deploy_timer_threshold) { state = FSMState::STATE_MAIN; - dispatch.mainDeploy_to_main_timed(commands); + dispatch.mainDeploy_to_main_timed(rocket_data); } break; @@ -303,7 +303,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if slowed down enough then go on to the next state if ((abs(state_estimate.vertical_speed) <= sustainer_landed_vertical_speed_threshold) && (current_time - main_deployed_time) > sustainer_main_to_landed_lockout) { landed_time = current_time; - dispatch.main_to_landed(commands); + dispatch.main_to_landed(rocket_data); state = FSMState::STATE_LANDED; } break; @@ -314,9 +314,9 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // Check for any telem transitions // Force transtion to safe if requested + clear all transition flags. - if(commands.should_transition_safe) { + if(rocket_data.command_flags.should_transition_safe) { state = FSMState::STATE_SAFE; - dispatch.landed_to_safe(commands); + dispatch.landed_to_safe(rocket_data); } break; @@ -325,7 +325,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // if the slow speed was too brief then return to previous state if ((abs(state_estimate.vertical_speed) > sustainer_landed_to_main_vertical_speed_threshold) && ((current_time - landed_time) > sustainer_landed_timer_threshold)) { state = FSMState::STATE_MAIN; - dispatch.landed_to_main(commands); + dispatch.landed_to_main(rocket_data); } break; @@ -345,45 +345,45 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla * * @return New FSM State */ -FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFlags& commands) { +FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, RocketData& rocket_data) { double current_time = pdTICKS_TO_MS(xTaskGetTickCount()); MIDAS_Events dispatch; switch (state) { case FSMState::STATE_SAFE: // Deconflict if multip commands are processed - if(commands.should_transition_safe) { - commands.should_transition_pyro_test = false; - commands.should_transition_idle = false; - commands.should_transition_safe = false; + if(rocket_data.command_flags.should_transition_safe) { + rocket_data.command_flags.should_transition_pyro_test = false; + rocket_data.command_flags.should_transition_idle = false; + rocket_data.command_flags.should_transition_safe = false; break; } // Only switch to STATE_PYRO_TEST if triggered wirelessly - if(commands.should_transition_pyro_test) { + if(rocket_data.command_flags.should_transition_pyro_test) { state = FSMState::STATE_PYRO_TEST; pyro_test_entry_time = current_time; - dispatch.safe_to_pyroTest(commands); + dispatch.safe_to_pyroTest(rocket_data); } // Only switch to STATE_IDLE if triggered wirelessly. - if(commands.should_transition_idle) { + if(rocket_data.command_flags.should_transition_idle) { state = FSMState::STATE_IDLE; - dispatch.safe_to_stateIdle(commands); + dispatch.safe_to_stateIdle(rocket_data); } break; case FSMState::STATE_PYRO_TEST: // Force transtion to safe if requested + clear all transition flags. - if(commands.should_transition_safe) { + if(rocket_data.command_flags.should_transition_safe) { state = FSMState::STATE_SAFE; - dispatch.pyroTest_to_safe_forced(commands); + dispatch.pyroTest_to_safe_forced(rocket_data); break; } // Switch back to STATE_SAFE after a certain amount of time passes if((current_time - pyro_test_entry_time) > safety_pyro_test_disarm_time) { - dispatch.pyroTest_to_safe_timed(commands); + dispatch.pyroTest_to_safe_timed(rocket_data); state = FSMState::STATE_SAFE; } @@ -391,16 +391,16 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla case FSMState::STATE_IDLE: // Force transtion to safe if requested + clear all transition flags. - if(commands.should_transition_safe) { + if(rocket_data.command_flags.should_transition_safe) { state = FSMState::STATE_SAFE; - dispatch.idle_to_safe_forced(commands); + dispatch.idle_to_safe_forced(rocket_data); break; } // once a significant amount of acceleration is detected change states if (state_estimate.acceleration > booster_idle_to_first_boost_acceleration_threshold) { launch_time = current_time; - dispatch.idle_to_firstBoost(commands); + dispatch.idle_to_firstBoost(rocket_data); state = FSMState::STATE_FIRST_BOOST; } @@ -408,13 +408,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla case FSMState::STATE_FIRST_BOOST: if ((state_estimate.acceleration < booster_idle_to_first_boost_acceleration_threshold) && ((current_time - launch_time) < booster_idle_to_first_boost_time_threshold)) { state = FSMState::STATE_IDLE; - dispatch.firstBoost_to_idle(commands); + dispatch.firstBoost_to_idle(rocket_data); break; } if (state_estimate.acceleration < booster_coast_detection_acceleration_threshold) { burnout_time = current_time; state = FSMState::STATE_BURNOUT; - dispatch.firstBoost_to_burnout(commands); + dispatch.firstBoost_to_burnout(rocket_data); } break; @@ -422,13 +422,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla case FSMState::STATE_BURNOUT: if ((state_estimate.acceleration >= booster_coast_detection_acceleration_threshold) && ((current_time - burnout_time) < booster_first_boost_to_burnout_time_threshold)) { state = FSMState::STATE_FIRST_BOOST; - dispatch.burnout_to_firstBoost(commands); + dispatch.burnout_to_firstBoost(rocket_data); break; } if ((current_time - burnout_time) > booster_first_boost_to_burnout_time_threshold) { first_separation_time = current_time; - dispatch.burnout_to_firstSeparation(commands); + dispatch.burnout_to_firstSeparation(rocket_data); state = FSMState::STATE_FIRST_SEPARATION; } break; @@ -436,13 +436,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla case FSMState::STATE_FIRST_SEPARATION: if (abs(state_estimate.jerk) < booster_first_separation_jerk_threshold) { state = FSMState::STATE_COAST; - dispatch.firstSeparation_to_coast_jerk(commands); + dispatch.firstSeparation_to_coast_jerk(rocket_data); break; } if ((current_time - first_separation_time) > booster_first_seperation_time_threshold) { state = FSMState::STATE_COAST; - dispatch.firstSeparation_to_coast_timed(commands); + dispatch.firstSeparation_to_coast_timed(rocket_data); } break; @@ -451,21 +451,21 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla if (state_estimate.vertical_speed <= booster_coast_to_apogee_vertical_speed_threshold) { apogee_time = current_time; state = FSMState::STATE_APOGEE; - dispatch.coast_to_apogee(commands); + dispatch.coast_to_apogee(rocket_data); } break; case FSMState::STATE_APOGEE: if (state_estimate.vertical_speed > booster_coast_to_apogee_vertical_speed_threshold && ((current_time - apogee_time) < booster_apogee_check_threshold)) { state = FSMState::STATE_COAST; - dispatch.apogee_to_coast(commands); + dispatch.apogee_to_coast(rocket_data); break; } if ((current_time - apogee_time) > booster_apogee_timer_threshold) { drogue_time = current_time; state = FSMState::STATE_DROGUE_DEPLOY; - dispatch.apogee_to_drogueDeploy(commands); + dispatch.apogee_to_drogueDeploy(rocket_data); } break; @@ -478,12 +478,12 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla if (abs(state_estimate.jerk) > booster_drogue_jerk_threshold) { state = FSMState::STATE_DROGUE; - dispatch.drogueDeploy_to_drogue_jerk(commands); + dispatch.drogueDeploy_to_drogue_jerk(rocket_data); break; } if ((current_time - drogue_time) > booster_drogue_timer_threshold) { state = FSMState::STATE_DROGUE; - dispatch.drogueDeploy_to_drouge_timed(commands); + dispatch.drogueDeploy_to_drouge_timed(rocket_data); } break; @@ -494,7 +494,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla if (state_estimate.altitude <= booster_main_deploy_altitude_threshold && (current_time - drogue_time) > booster_main_deploy_delay_after_drogue) { state = FSMState::STATE_MAIN_DEPLOY; main_time = current_time; - dispatch.drogue_to_mainDeploy(commands); + dispatch.drogue_to_mainDeploy(rocket_data); } break; @@ -508,13 +508,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla if (abs(state_estimate.jerk) > booster_main_jerk_threshold) { state = FSMState::STATE_MAIN; main_deployed_time = current_time; - dispatch.mainDeploy_to_main_jerk(commands); + dispatch.mainDeploy_to_main_jerk(rocket_data); break; } if ((current_time - main_time) > booster_main_to_main_deploy_timer_threshold) { state = FSMState::STATE_MAIN; - dispatch.mainDeploy_to_main_timed(commands); + dispatch.mainDeploy_to_main_timed(rocket_data); } break; @@ -522,7 +522,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla if (abs(state_estimate.vertical_speed) <= booster_landed_vertical_speed_threshold && (current_time - main_deployed_time) > booster_main_to_landed_lockout) { landed_time = current_time; state = FSMState::STATE_LANDED; - dispatch.main_to_landed(commands); + dispatch.main_to_landed(rocket_data); } break; @@ -532,9 +532,9 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla // Check for any telem transitions // Force transtion to safe if requested + clear all transition flags. - if(commands.should_transition_safe) { + if(rocket_data.command_flags.should_transition_safe) { state = FSMState::STATE_SAFE; - dispatch.landed_to_safe(commands); + dispatch.landed_to_safe(rocket_data); } break; @@ -542,7 +542,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla if ((abs(state_estimate.vertical_speed) > booster_landed_to_main_vertical_speed_threshold) && ((current_time - landed_time) > booster_landed_timer_threshold)) { state = FSMState::STATE_MAIN; - dispatch.landed_to_main(commands); + dispatch.landed_to_main(rocket_data); } break; diff --git a/MIDAS/src/finite-state-machines/fsm.h b/MIDAS/src/finite-state-machines/fsm.h index 23dbe87f..c21f30ec 100644 --- a/MIDAS/src/finite-state-machines/fsm.h +++ b/MIDAS/src/finite-state-machines/fsm.h @@ -33,7 +33,7 @@ class FSM { public: FSM() = default; - FSMState tick_fsm(FSMState& curr_state, StateEstimate state_estimate, CommandFlags& commands); + FSMState tick_fsm(FSMState& curr_state, StateEstimate state_estimate, RocketData& rocket_data); private: double launch_time; diff --git a/MIDAS/src/systems.cpp b/MIDAS/src/systems.cpp index 018815f4..ffe26dc8 100644 --- a/MIDAS/src/systems.cpp +++ b/MIDAS/src/systems.cpp @@ -157,7 +157,7 @@ DECLARE_THREAD(fsm, RocketSystems* arg) { CommandFlags& telemetry_commands = arg->rocket_data.command_flags; double current_time = pdTICKS_TO_MS(xTaskGetTickCount()); - FSMState next_state = fsm.tick_fsm(current_state, state_estimate, telemetry_commands); + FSMState next_state = fsm.tick_fsm(current_state, state_estimate, arg->rocket_data); arg->rocket_data.fsm_state.update(next_state);