diff --git a/MIDAS/src/finite-state-machines/fsm.cpp b/MIDAS/src/finite-state-machines/fsm.cpp index d76a29fe..a9807769 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) > sustainer_burnout_to_first_boost_time_threshold))) { state = FSMState::STATE_FIRST_BOOST; break; } diff --git a/MIDAS/src/finite-state-machines/tester/fsm.py b/MIDAS/src/finite-state-machines/tester/fsm.py index b3def8c0..caa5dfe2 100644 --- a/MIDAS/src/finite-state-machines/tester/fsm.py +++ b/MIDAS/src/finite-state-machines/tester/fsm.py @@ -50,6 +50,7 @@ class SustainerFSM: launch_time: float = 0.0 burnout_time: float = 0.0 + burnout_time_elapsed_with_sudden_acceleration_change: float = 0.0 sustainer_ignition_time: float = 0.0 second_boost_time: float = 0.0 coast_time: float = 0.0 @@ -90,13 +91,24 @@ def tick_fsm(self, state_estimate: StateEstimate) -> None: self.state = FSMState.STATE_BURNOUT reason_transition = f"Transitioned FIRST_BOOST TO BURNOUT due to low acceleration. Acceleration is currently {acceleration}m/s^2" case FSMState.STATE_BURNOUT: - # if low acceleration is too brief than go on to the previous state - if ((acceleration >= thresholds.SUSTAINER_COAST_DETECTION_ACCELERATION_THRESHOLD) and ((current_time - self.burnout_time) < thresholds.SUSTAINER_COAST_TIME)): - self.state = FSMState.STATE_FIRST_BOOST - reason_transition = f"Transitioned BURNOUT TO FIRST_BOOST due to short dip of acceleration. Acceleration is currently {acceleration}m/s^2 and it has been {current_time - self.burnout_time}ms since burnout_time" + #If we have acceptable acceleration, we reset the error + # timer for the acceleration so that it doesn't carry over. + if(acceleration < thresholds.SUSTAINER_COAST_DETECTION_ACCELERATION_THRESHOLD): + self.burnout_time_elapsed_with_sudden_acceleration_change = 0.0 + #If we have a acceleration above the threshold + # We check if the timer has started. + # If it hasn't, we start it, if it has, we see if we need to go back to first boost. + elif ((acceleration >= thresholds.SUSTAINER_COAST_DETECTION_ACCELERATION_THRESHOLD) and ((current_time - self.burnout_time) < thresholds.SUSTAINER_COAST_TIME)): + if self.burnout_time_elapsed_with_sudden_acceleration_change == 0.0: + self.burnout_time_elapsed_with_sudden_acceleration_change = current_time + else: + if current_time - self.burnout_time_elapsed_with_sudden_acceleration_change > thresholds.SUSTAINER_BURNOUT_TO_FIRST_BOOST_TIME_THRESHOLD: + self.state = FSMState.STATE_FIRST_BOOST + reason_transition = f"Transitioned BURNOUT TO FIRST_BOOST due to short dip of acceleration. Acceleration is currently {acceleration}m/s^2 and it has been {current_time - self.burnout_time}ms since burnout_time" # if in burnout for long enough then go on to the next state (time transition) - elif ((current_time - self.burnout_time) > thresholds.SUSTAINER_COAST_TIME): + #Despite our timer, we want to check if we need to transition to the next state. + if ((current_time - self.burnout_time) > thresholds.SUSTAINER_COAST_TIME): self.sustainer_ignition_time = current_time self.state = FSMState.STATE_SUSTAINER_IGNITION reason_transition = f"Transitioned BURNOUT TO SUSTAINER_IGNITION due to long enough time after burnout. It has been {current_time - self.burnout_time}ms since burnout_time" diff --git a/MIDAS/src/finite-state-machines/tester/fsm_thresholds.py b/MIDAS/src/finite-state-machines/tester/fsm_thresholds.py index 864f9904..23c548f5 100644 --- a/MIDAS/src/finite-state-machines/tester/fsm_thresholds.py +++ b/MIDAS/src/finite-state-machines/tester/fsm_thresholds.py @@ -153,3 +153,6 @@ # Stores a small jerk value (m/s^3) BOOSTER_MAIN_JERK_THRESHOLD = 300 + +#Minimum duration of sustained acceleration before fallback to boost (ms) +SUSTAINER_BURNOUT_TO_FIRST_BOOST_TIME_THRESHOLD = 250 diff --git a/MIDAS/src/finite-state-machines/thresholds.h b/MIDAS/src/finite-state-machines/thresholds.h index 4334fbea..2d6ccdb6 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 duration of sustained acceleration before fallback to boost (ms) +#define sustainer_burnout_to_first_boost_time_threshold 250 \ No newline at end of file