diff --git a/MIDAS/src/finite-state-machines/fsm.cpp b/MIDAS/src/finite-state-machines/fsm.cpp index d76a29fe..de111a77 100644 --- a/MIDAS/src/finite-state-machines/fsm.cpp +++ b/MIDAS/src/finite-state-machines/fsm.cpp @@ -154,7 +154,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla case FSMState::STATE_FIRST_BOOST: // 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)) { + if ((state_estimate.acceleration < sustainer_idle_to_first_boost_acceleration_threshold) && ((current_time - launch_time) < sustainer_burnout_to_first_boost_time_threshold)) { state = FSMState::STATE_IDLE; break; } @@ -168,7 +168,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla case FSMState::STATE_BURNOUT: // 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)) { + if ((state_estimate.acceleration >= sustainer_coast_detection_acceleration_threshold) && ((current_time - burnout_time) < sustainer_coast_time && ((current_time - burnout_time) > minimum_time_for_burnout_to_first_boost))) { state = FSMState::STATE_FIRST_BOOST; break; } diff --git a/MIDAS/src/finite-state-machines/thresholds.h b/MIDAS/src/finite-state-machines/thresholds.h index 4334fbea..1b887e0b 100644 --- a/MIDAS/src/finite-state-machines/thresholds.h +++ b/MIDAS/src/finite-state-machines/thresholds.h @@ -238,4 +238,7 @@ // The minimum expected jerk for a main deployment event (m/s^3) // (Core Setting) -#define booster_main_jerk_threshold 300 \ No newline at end of file +#define booster_main_jerk_threshold 300 + +// Minimum time to wait before deciding to go from BURNOUT to FIRST_BOOST. (ms) +#define sustainer_burnout_to_first_boost_time_threshold 250 \ No newline at end of file