diff --git a/MIDAS/src/finite-state-machines/fsm.cpp b/MIDAS/src/finite-state-machines/fsm.cpp index 729ccda4..e72e572f 100644 --- a/MIDAS/src/finite-state-machines/fsm.cpp +++ b/MIDAS/src/finite-state-machines/fsm.cpp @@ -501,4 +501,4 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla } return state; } -#endif +#endif \ No newline at end of file diff --git a/MIDAS/src/finite-state-machines/fsm.h b/MIDAS/src/finite-state-machines/fsm.h index 54e1a13f..e29c211d 100644 --- a/MIDAS/src/finite-state-machines/fsm.h +++ b/MIDAS/src/finite-state-machines/fsm.h @@ -4,7 +4,9 @@ #include "FreeRTOSConfig.h" #include "fsm_states.h" -#include "thresholds.h" + + + #include "sensor_data.h" #include "Buffer.h" #include "rocket_state.h" diff --git a/MIDAS/src/finite-state-machines/thresholds.h b/MIDAS/src/finite-state-machines/thresholds.h deleted file mode 100644 index 02320c19..00000000 --- a/MIDAS/src/finite-state-machines/thresholds.h +++ /dev/null @@ -1,147 +0,0 @@ -#pragma once - -// ---------------------------------- -// SAFETY THRESHOLDS -// ---------------------------------- - -// Transition back to STATE_SAFE if this much time has passed without firing a pyro (ms) -#define safety_pyro_test_disarm_time 10000 - -// ---------------------------------- -// SECOND STAGE THRESHOLDS -// ---------------------------------- - -// Regardless of sensor inputs, stay on pyro firing states for at LEAST this time. (ms) -#define sustainer_pyro_firing_time_minimum 200 - -// Transition to FIRST_BOOST if acceleration is greater than this (G) -#define sustainer_idle_to_first_boost_acceleration_threshold 3 - -// Return state to IDLE if not boosting for this amount of time (ms) -#define sustainer_idle_to_first_boost_time_threshold 1000 - -// Transition to SECOND_BOOST from SUSTAINER_IGNITION if acceleration greater than this (G) -#define sustainer_ignition_to_second_boost_acceleration_threshold 4 - -// Return state to SECOND_BOOST if not boosting for this amount of time (ms) -#define sustainer_second_boost_to_coast_time_threshold 1000 - -// Transition to COAST if acceleration is less than this value (g) -#define sustainer_coast_detection_acceleration_threshold 0.2 - -// Reach apogee state when vertical speed is less than or equal to this value (m/s) -#define sustainer_coast_to_apogee_vertical_speed_threshold 15 - -// Revert back to COAST if the vertical speed in apogee is too high (was 0 before which may have caused it keep jumping back to COAST) (m/s) -#define sustainer_apogee_backto_coast_vertical_speed_threshold 10 - -// Revert back to COAST if apogee was too brief (ms) -#define sustainer_apogee_check_threshold 1000 - -// Move on to DROGUE_DEPLOT after being in apogee for this amount of time (ms) -#define sustainer_apogee_timer_threshold 1000 - -// Move on to DROGUE after a second of reaching apogee (ms) -#define sustainer_drogue_timer_threshold 3000 - -// Move on to MAIN after passing this amount of time (ms) -#define sustainer_main_to_main_deploy_timer_threshold 3000 - -// Height required to deploy the main parachutes (m) -#define sustainer_main_deploy_altitude_threshold 1006 - -// Return to SUSTAINER_IGNITION if not in SECOND_BOOST for this amount of time (ms) -#define sustainer_ignition_to_second_boost_time_threshold 1000 - -// Transition straight to coast after a certain amount of time not detecting second stage boost (ms) -#define sustainer_ignition_to_coast_timer_threshold 5000 - -// Revert back to main if the landed was too short (ms) -#define sustainer_landed_timer_threshold 5000 - -// Return state to FIRST_BOOST if not in BURNOUT for this amount of time (ms) -#define sustainer_coast_time 3000 - -// Transition to LANDED from MAIN if vertical speed is less than this threshold (m/s) -#define sustainer_landed_vertical_speed_threshold 3 - -// Lock out further transitions from LANDED after this much time passes in the LANDED state. (ms) -#define sustainer_landed_time_lockout 60000 - -// Prevent us from inadvertently entering the LANDED state when we're at a low velocity at main deploy. (ms) -#define sustainer_main_to_landed_lockout 5000 - -// Stores a small jerk value (m/s^3) -#define sustainer_drogue_jerk_threshold 200 - -// Stores a small jerk value (m/s^3) -#define sustainer_main_jerk_threshold 300 - - -// ---------------------------------- -// FIRST STAGE THRESHOLDS -// ---------------------------------- - -// Regardless of sensor inputs, stay on pyro firing states for at LEAST this time. (ms) -#define booster_pyro_firing_time_minimum 200 - -// Transition to FIRST_BOOST if acceleration is greater than this (G) -#define booster_idle_to_first_boost_acceleration_threshold 3 - -// Return state to IDLE if not boosting for this amount of time (ms) -#define booster_idle_to_first_boost_time_threshold 1000 - -// Move on regardless if it separates or not i.e. if state is FIRST_SEPERATION for over this amount of time (ms) -#define booster_first_seperation_time_threshold 3000 - -// Transition to COAST if acceleration is less than this value (g) -#define booster_coast_detection_acceleration_threshold 0.2 - -// Reach apogee state when vertical speed is less than or equal to this value (m/s) -#define booster_coast_to_apogee_vertical_speed_threshold 20 - -// Revert back to COAST if apogee was too brief (ms) -#define booster_apogee_check_threshold 1000 - -// Move on to DROGUE_DEPLOT after being in apogee for this amount of time (ms) -#define booster_apogee_timer_threshold 1000 - -// Move on to DROGUE after a second of reaching apogee (ms) -#define booster_drogue_timer_threshold 3000 - -// Move on to MAIN after passing this amount of time (ms) -#define booster_main_to_main_deploy_timer_threshold 3000 - -// Height required to deploy the main parachutes (m) -// [STARGAZER 1.4] This is a "dontcare" value --> The booster does not have a drogue, we transition immediately to MAIN -#define booster_main_deploy_altitude_threshold 999999 - -// Return to SUSTAINER_IGNITION if not in SECOND_BOOST for this amount of time (ms) -#define booster_ignition_to_second_boost_time_threshold 1000 - -// Transition straight to coast after a certain amount of time not detecting second stage boost (ms) -#define booster_ignition_to_coast_timer_threshold 5000 - -// Revert back to main if the landed was too short (ms) -#define booster_landed_timer_threshold 5000 - -// Return state to FIRST_BOOST if not in BURNOUT for this amount of time (ms) -#define booster_first_boost_to_burnout_time_threshold 1000 - -// Transition to LANDED from MAIN if vertical speed is less than this threshold (G) -#define booster_landed_vertical_speed_threshold 4 - -// Lock out further transitions from LANDED after this much time passes in the LANDED state. (ms) -#define booster_landed_time_lockout 60000 - -// Prevent us from inadvertently entering the LANDED state when we're at a low velocity at main deploy. (ms) -#define booster_main_to_landed_lockout 5000 - -// Stores a small jerk value (m/s^3) -#define booster_first_separation_jerk_threshold 300 - -// Stores a small jerk value (m/s^3) -#define booster_drogue_jerk_threshold 200 - -// Stores a small jerk value (m/s^3) -#define booster_main_jerk_threshold 300 diff --git a/MIDAS/src/finite-state-machines/thresholds/stargazer1-4.h b/MIDAS/src/finite-state-machines/thresholds/stargazer1-4.h new file mode 100644 index 00000000..f4c32d22 --- /dev/null +++ b/MIDAS/src/finite-state-machines/thresholds/stargazer1-4.h @@ -0,0 +1,102 @@ +#pragma once + +// ---------------------------------- +// SECOND STAGE THRESHOLDS +// ---------------------------------- +struct SustainerThresholds { + struct Idle { + static constexpr int to_first_boost_acceleration_threshold = 3; // G + static constexpr int to_first_boost_time_threshold = 1000; // ms + } idle; + + struct Ignition { + static constexpr int to_second_boost_acceleration_threshold = 3; // G + static constexpr int to_second_boost_time_threshold = 1000; // ms + static constexpr int to_coast_timer_threshold = 5000; // ms + } ignition; + + struct SecondBoost { + static constexpr int to_coast_time_threshold = 1000; // ms + } second_boost; + + struct Coast { + static constexpr double detection_acceleration_threshold = 0.2; // m/s^2 + static constexpr int to_apogee_vertical_speed_threshold = 15; // m/s + } coast; + + struct Apogee { + static constexpr int backto_coast_vertical_speed_threshold = 10; // m/s + static constexpr int check_threshold = 1000; // ms + static constexpr int timer_threshold = 1000; // ms + } apogee; + + struct Drogue { + static constexpr int timer_threshold = 3000; // ms + static constexpr int jerk_threshold = 200; // m/s^3 + } drogue; + + struct Main { + static constexpr int to_main_deploy_timer_threshold = 3000; // ms + static constexpr int deploy_altitude_threshold = 1006; // m + static constexpr int jerk_threshold = 300; // m/s^3 + } main; + + struct Landed { + static constexpr int timer_threshold = 5000; // ms + static constexpr int vertical_speed_threshold = 3; // m/s + } landed; + + struct Burnout { + static constexpr int to_first_boost_time_threshold = 1000; // ms + } burnout; +}; + +// ---------------------------------- +// FIRST STAGE THRESHOLDS +// ---------------------------------- +struct BoosterThresholds { + struct Idle { + static constexpr int to_first_boost_acceleration_threshold = 3; // G + static constexpr int to_first_boost_time_threshold = 1000; // ms + } idle; + + struct FirstSeparation { + static constexpr int time_threshold = 3000; // ms + static constexpr int jerk_threshold = 300; // m/s^3 + } first_separation; + + struct Coast { + static constexpr double detection_acceleration_threshold = 0.2; // G + static constexpr int to_apogee_vertical_speed_threshold = 20; // m/s + } coast; + + struct Apogee { + static constexpr int check_threshold = 1000; // ms + static constexpr int timer_threshold = 1000; // ms + } apogee; + + struct Drogue { + static constexpr int timer_threshold = 3000; // ms + static constexpr int jerk_threshold = 200; // m/s^3 + } drogue; + + struct Main { + static constexpr int to_main_deploy_timer_threshold = 3000; // ms + static constexpr int deploy_altitude_threshold = 3000; // m + static constexpr int jerk_threshold = 300; // m/s^3 + } main; + + struct Landed { + static constexpr int timer_threshold = 5000; // ms + static constexpr int vertical_speed_threshold = 4; // m/s + } landed; + + struct Ignition { + static constexpr int to_second_boost_time_threshold = 1000; // ms + static constexpr int to_coast_timer_threshold = 5000; // ms + } ignition; + + struct Burnout { + static constexpr int to_first_boost_time_threshold = 1000; // ms + } burnout; +};