From 54c5b87709442912d6a7c761429dd810e79045be Mon Sep 17 00:00:00 2001 From: Aadityavoru Date: Fri, 31 Jan 2025 17:50:55 -0600 Subject: [PATCH 1/9] added the yaml files --- MIDAS/src/finite-state-machines/fsm.cpp | 273 ++++++++++++++------- MIDAS/src/finite-state-machines/sg1-4.yaml | 62 +++++ 2 files changed, 245 insertions(+), 90 deletions(-) create mode 100644 MIDAS/src/finite-state-machines/sg1-4.yaml diff --git a/MIDAS/src/finite-state-machines/fsm.cpp b/MIDAS/src/finite-state-machines/fsm.cpp index 1b32d9b8..f062c911 100644 --- a/MIDAS/src/finite-state-machines/fsm.cpp +++ b/MIDAS/src/finite-state-machines/fsm.cpp @@ -1,10 +1,126 @@ #include +#include +#include +#include +#include #include "fsm.h" -#include "thresholds.h" +// #include "thresholds.h" + +// ------------------------------------------------------------------------ +// 1) Define static variables for each threshold that was previously a macro +// These names match exactly the ones in your original code. +// ------------------------------------------------------------------------ +static double sustainer_idle_to_first_boost_acceleration_threshold; +static double sustainer_idle_to_first_boost_time_threshold; +static double sustainer_ignition_to_second_boost_acceleration_threshold; +static double sustainer_ignition_to_second_boost_time_threshold; +static double sustainer_second_boost_to_coast_time_threshold; +static double sustainer_coast_detection_acceleration_threshold; +static double sustainer_coast_to_apogee_vertical_speed_threshold; +static double sustainer_apogee_backto_coast_vertical_speed_threshold; +static double sustainer_apogee_check_threshold; +static double sustainer_apogee_timer_threshold; +static double sustainer_drogue_timer_threshold; +static double sustainer_main_to_main_deploy_timer_threshold; +static double sustainer_main_deploy_altitude_threshold; +static double sustainer_ignition_to_coast_timer_threshold; +static double sustainer_landed_timer_threshold; +static double sustainer_landed_vertical_speed_threshold; +static double sustainer_first_boost_to_burnout_time_threshold; +static double sustainer_drogue_jerk_threshold; +static double sustainer_main_jerk_threshold; + +static double booster_idle_to_first_boost_acceleration_threshold; +static double booster_idle_to_first_boost_time_threshold; +static double booster_first_seperation_time_threshold; +static double booster_coast_detection_acceleration_threshold; +static double booster_coast_to_apogee_vertical_speed_threshold; +static double booster_apogee_check_threshold; +static double booster_apogee_timer_threshold; +static double booster_drogue_timer_threshold; +static double booster_main_to_main_deploy_timer_threshold; +static double booster_main_deploy_altitude_threshold; +static double booster_ignition_to_second_boost_time_threshold; +static double booster_ignition_to_coast_timer_threshold; +static double booster_landed_timer_threshold; +static double booster_first_boost_to_burnout_time_threshold; +static double booster_landed_vertical_speed_threshold; +static double booster_first_separation_jerk_threshold; +static double booster_drogue_jerk_threshold; +static double booster_main_jerk_threshold; + +// ------------------------------------------------------------------------ +// 2) Use a static boolean to ensure the YAML is only loaded once +// ------------------------------------------------------------------------ +static bool yamlLoaded = false; + +// ------------------------------------------------------------------------ +// 3) Load thresholds from YAML +// This function will parse the YAML file and populate the static variables. +// ------------------------------------------------------------------------ +static void loadYamlThresholds() { + if (yamlLoaded) { + return; // Already loaded, skip + } -// helper functions + try { + YAML::Node config = YAML::LoadFile("sg1-4.yaml"); + + // Load second stage thresholds + auto s = config["second_stage_thresholds"]; + sustainer_idle_to_first_boost_acceleration_threshold = s["idle_to_first_boost"]["acceleration_threshold"].as(); + sustainer_idle_to_first_boost_time_threshold = s["idle_to_first_boost"]["time_threshold"].as(); + sustainer_ignition_to_second_boost_acceleration_threshold = s["ignition_to_second_boost"]["acceleration_threshold"].as(); + sustainer_ignition_to_second_boost_time_threshold = s["ignition_to_second_boost"]["time_threshold"].as(); + sustainer_second_boost_to_coast_time_threshold = s["second_boost_to_coast"]["time_threshold"].as(); + sustainer_coast_detection_acceleration_threshold = s["coast_detection"]["acceleration_threshold"].as(); + sustainer_coast_to_apogee_vertical_speed_threshold = s["coast_to_apogee"]["vertical_speed_threshold"].as(); + sustainer_apogee_backto_coast_vertical_speed_threshold = s["apogee"]["backto_coast_vertical_speed_threshold"].as(); + sustainer_apogee_check_threshold = s["apogee"]["check_threshold"].as(); + sustainer_apogee_timer_threshold = s["apogee"]["timer_threshold"].as(); + sustainer_drogue_timer_threshold = s["drogue"]["timer_threshold"].as(); + sustainer_main_to_main_deploy_timer_threshold = s["main"]["to_main_deploy_timer_threshold"].as(); + sustainer_main_deploy_altitude_threshold = s["main"]["deploy_altitude_threshold"].as(); + sustainer_ignition_to_coast_timer_threshold = s["ignition"]["to_coast_timer_threshold"].as(); + sustainer_landed_timer_threshold = s["landed"]["timer_threshold"].as(); + sustainer_landed_vertical_speed_threshold = s["landed"]["vertical_speed_threshold"].as(); + sustainer_first_boost_to_burnout_time_threshold = s["first_boost"]["to_burnout_time_threshold"].as(); + sustainer_drogue_jerk_threshold = s["drogue_jerk_threshold"].as(); + sustainer_main_jerk_threshold = s["main_jerk_threshold"].as(); + + // Load first stage thresholds + auto b = config["first_stage_thresholds"]; + booster_idle_to_first_boost_acceleration_threshold = b["idle_to_first_boost"]["acceleration_threshold"].as(); + booster_idle_to_first_boost_time_threshold = b["idle_to_first_boost"]["time_threshold"].as(); + booster_first_seperation_time_threshold = b["first_separation"]["time_threshold"].as(); + booster_coast_detection_acceleration_threshold = b["coast_detection"]["acceleration_threshold"].as(); + booster_coast_to_apogee_vertical_speed_threshold = b["coast_to_apogee"]["vertical_speed_threshold"].as(); + booster_apogee_check_threshold = b["apogee"]["check_threshold"].as(); + booster_apogee_timer_threshold = b["apogee"]["timer_threshold"].as(); + booster_drogue_timer_threshold = b["drogue"]["timer_threshold"].as(); + booster_main_to_main_deploy_timer_threshold = b["main"]["to_main_deploy_timer_threshold"].as(); + booster_main_deploy_altitude_threshold = b["main"]["deploy_altitude_threshold"].as(); + booster_ignition_to_second_boost_time_threshold = b["ignition"]["to_second_boost_time_threshold"].as(); + booster_ignition_to_coast_timer_threshold = b["ignition"]["to_coast_timer_threshold"].as(); + booster_landed_timer_threshold = b["landed"]["timer_threshold"].as(); + booster_first_boost_to_burnout_time_threshold = b["first_boost"]["to_burnout_time_threshold"].as(); + booster_landed_vertical_speed_threshold = b["landed"]["vertical_speed_threshold"].as(); + booster_first_separation_jerk_threshold = b["first_separation_jerk_threshold"].as(); + booster_drogue_jerk_threshold = b["drogue_jerk_threshold"].as(); + booster_main_jerk_threshold = b["main_jerk_threshold"].as(); + + yamlLoaded = true; + } + catch (const std::exception &e) { + std::cerr << "Error loading thresholds from YAML: " << e.what() << std::endl; + std::abort(); + } +} +// ------------------------------------------------------------------------ +// Helper functions from your code +// ------------------------------------------------------------------------ template double sensor_average(BufferedSensorData& sensor, double (* get_item)(T&)) { auto arr = sensor.template getBufferRecent(); @@ -17,31 +133,34 @@ double sensor_average(BufferedSensorData& sensor, double (* get_item)( template double sensor_derivative(BufferedSensorData& sensor, double (* get_item)(T&)) { - auto arr = sensor.template getBufferRecent(); + auto arr = sensor.template getBufferRecent(); auto times = sensor.template getTimesRecent(); - size_t i = 0; + size_t i = 0; - double first_average = 0.0; + double first_average = 0.0; double first_average_time = 0.0; for (; i < count / 2; i++) { - first_average += get_item(arr[i]); + first_average += get_item(arr[i]); first_average_time += pdTICKS_TO_MS(times[i]) / 1000.0; } - first_average /= (count / 2.0); + first_average /= (count / 2.0); first_average_time /= (count / 2.0); - double second_average = 0.0; + double second_average = 0.0; double second_average_time = 0.0; for (; i < count; i++) { - second_average += get_item(arr[i]); + second_average += get_item(arr[i]); second_average_time += pdTICKS_TO_MS(times[i]) / 1000.0; } - second_average /= (count / 2.0); + second_average /= (count / 2.0); second_average_time /= (count / 2.0); + return (second_average - first_average) / (second_average_time - first_average_time); } - +// ------------------------------------------------------------------------ +// StateEstimate constructor from your code +// ------------------------------------------------------------------------ StateEstimate::StateEstimate(RocketData& state) { acceleration = sensor_average(state.high_g, [](HighGData& data) { return (double) data.ax; @@ -57,45 +176,43 @@ StateEstimate::StateEstimate(RocketData& state) { }); } - +// ------------------------------------------------------------------------ +// The FSM code rewritten to load from YAML but keep variable names unchanged +// ------------------------------------------------------------------------ #ifdef IS_SUSTAINER - FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate) { - //get current time + // 1) Ensure thresholds are loaded + loadYamlThresholds(); + + // 2) get current time double current_time = pdTICKS_TO_MS(xTaskGetTickCount()); switch (state) { case FSMState::STATE_IDLE: - // 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; - state = FSMState::STATE_FIRST_BOOST; + state = FSMState::STATE_FIRST_BOOST; } - break; 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_idle_to_first_boost_time_threshold)) { state = FSMState::STATE_IDLE; 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; - state = FSMState::STATE_BURNOUT; + state = FSMState::STATE_BURNOUT; } break; 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_first_boost_to_burnout_time_threshold)) { + if ((state_estimate.acceleration >= sustainer_coast_detection_acceleration_threshold) && + ((current_time - burnout_time) < sustainer_first_boost_to_burnout_time_threshold)) { state = FSMState::STATE_FIRST_BOOST; break; } - - // if in burnout for long enough then go on to the next state (time transition) if ((current_time - burnout_time) > sustainer_first_boost_to_burnout_time_threshold) { sustainer_ignition_time = current_time; state = FSMState::STATE_SUSTAINER_IGNITION; @@ -103,229 +220,205 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate) { break; case FSMState::STATE_SUSTAINER_IGNITION: - // 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; - state = FSMState::STATE_COAST; + state = FSMState::STATE_COAST; break; } - - // 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; - state = FSMState::STATE_SECOND_BOOST; + state = FSMState::STATE_SECOND_BOOST; } - break; case FSMState::STATE_SECOND_BOOST: - // 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)) { + 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; break; } - - // if low acceleration detected go to next state if (state_estimate.acceleration < sustainer_coast_detection_acceleration_threshold) { coast_time = current_time; - state = FSMState::STATE_COAST; + state = FSMState::STATE_COAST; } break; case FSMState::STATE_COAST: - // 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)) { + 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; break; } - - // if speed slows down enough then go on to the next stage if (state_estimate.vertical_speed <= sustainer_coast_to_apogee_vertical_speed_threshold) { apogee_time = current_time; - state = FSMState::STATE_APOGEE; + state = FSMState::STATE_APOGEE; } break; case FSMState::STATE_APOGEE: - // 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)) { + if ((state_estimate.vertical_speed) > sustainer_apogee_backto_coast_vertical_speed_threshold && + ((current_time - apogee_time) < sustainer_apogee_check_threshold)) { state = FSMState::STATE_COAST; break; } - - // transition to next state after a certain amount of time if ((current_time - apogee_time) > sustainer_apogee_timer_threshold) { drogue_time = current_time; - state = FSMState::STATE_DROGUE_DEPLOY; + state = FSMState::STATE_DROGUE_DEPLOY; } break; case FSMState::STATE_DROGUE_DEPLOY: - // if detected a sharp change in jerk then go to next state - if (abs(state_estimate.jerk) < sustainer_drogue_jerk_threshold) { + if (std::abs(state_estimate.jerk) < sustainer_drogue_jerk_threshold) { 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) { state = FSMState::STATE_DROGUE; } - break; case FSMState::STATE_DROGUE: - // if altitude low enough then next state if (state_estimate.altitude <= sustainer_main_deploy_altitude_threshold) { - state = FSMState::STATE_MAIN_DEPLOY; + state = FSMState::STATE_MAIN_DEPLOY; main_time = current_time; } break; case FSMState::STATE_MAIN_DEPLOY: - // if detected a sharp change in jerk then go to the next state - if (abs(state_estimate.jerk) < sustainer_main_jerk_threshold) { + if (std::abs(state_estimate.jerk) < sustainer_main_jerk_threshold) { state = FSMState::STATE_MAIN; break; } - - // 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; } break; case FSMState::STATE_MAIN: - // if slowed down enough then go on to the next state - if (abs(state_estimate.vertical_speed) <= sustainer_landed_vertical_speed_threshold) { + if (std::abs(state_estimate.vertical_speed) <= sustainer_landed_vertical_speed_threshold) { landed_time = current_time; - state = FSMState::STATE_LANDED; + state = FSMState::STATE_LANDED; } break; case FSMState::STATE_LANDED: - // if the slow speed was too brief then return to previous state - if ((abs(state_estimate.vertical_speed) > sustainer_landed_vertical_speed_threshold) && ((current_time - landed_time) > sustainer_landed_timer_threshold)) { + if ((std::abs(state_estimate.vertical_speed) > sustainer_landed_vertical_speed_threshold) && + ((current_time - landed_time) > sustainer_landed_timer_threshold)) { state = FSMState::STATE_MAIN; } break; - } return state; } - #else -// this is similar to the previous function but contains less states FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate) { + // 1) Ensure thresholds are loaded + loadYamlThresholds(); + double current_time = pdTICKS_TO_MS(xTaskGetTickCount()); switch (state) { case FSMState::STATE_IDLE: if (state_estimate.acceleration > booster_idle_to_first_boost_acceleration_threshold) { launch_time = current_time; - state = FSMState::STATE_FIRST_BOOST; + state = FSMState::STATE_FIRST_BOOST; } - break; 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)) { + 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; break; } if (state_estimate.acceleration < booster_coast_detection_acceleration_threshold) { burnout_time = current_time; - state = FSMState::STATE_BURNOUT; + state = FSMState::STATE_BURNOUT; } - break; case FSMState::STATE_BURNOUT: - if ((state_estimate.acceleration >= booster_coast_detection_acceleration_threshold) && ((current_time - burnout_time) < booster_first_boost_to_burnout_time_threshold)) { + 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; break; } - if ((current_time - burnout_time) > booster_first_boost_to_burnout_time_threshold) { first_separation_time = current_time; - state = FSMState::STATE_FIRST_SEPARATION; + state = FSMState::STATE_FIRST_SEPARATION; } break; case FSMState::STATE_FIRST_SEPARATION: - if (abs(state_estimate.jerk) < booster_first_separation_jerk_threshold) { + if (std::abs(state_estimate.jerk) < booster_first_separation_jerk_threshold) { state = FSMState::STATE_COAST; break; } - if ((current_time - first_separation_time) > booster_first_seperation_time_threshold) { state = FSMState::STATE_COAST; } - break; case FSMState::STATE_COAST: if (state_estimate.vertical_speed <= booster_coast_to_apogee_vertical_speed_threshold) { apogee_time = current_time; - state = FSMState::STATE_APOGEE; + state = FSMState::STATE_APOGEE; } 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)) { + if ((state_estimate.vertical_speed > booster_coast_to_apogee_vertical_speed_threshold) && + ((current_time - apogee_time) < booster_apogee_check_threshold)) { state = FSMState::STATE_COAST; break; } - if ((current_time - apogee_time) > booster_apogee_timer_threshold) { drogue_time = current_time; - state = FSMState::STATE_DROGUE_DEPLOY; + state = FSMState::STATE_DROGUE_DEPLOY; } break; case FSMState::STATE_DROGUE_DEPLOY: - if (abs(state_estimate.jerk) < booster_drogue_jerk_threshold) { + if (std::abs(state_estimate.jerk) < booster_drogue_jerk_threshold) { state = FSMState::STATE_DROGUE; break; } if ((current_time - drogue_time) > booster_drogue_timer_threshold) { state = FSMState::STATE_DROGUE; } - break; case FSMState::STATE_DROGUE: if (state_estimate.altitude <= booster_main_deploy_altitude_threshold) { - state = FSMState::STATE_MAIN_DEPLOY; + state = FSMState::STATE_MAIN_DEPLOY; main_time = current_time; } break; case FSMState::STATE_MAIN_DEPLOY: - if (abs(state_estimate.jerk) < booster_main_jerk_threshold) { + if (std::abs(state_estimate.jerk) < booster_main_jerk_threshold) { state = FSMState::STATE_MAIN; break; } - if ((current_time - main_time) > booster_main_to_main_deploy_timer_threshold) { state = FSMState::STATE_MAIN; } break; case FSMState::STATE_MAIN: - if (abs(state_estimate.vertical_speed) <= booster_landed_vertical_speed_threshold) { + if (std::abs(state_estimate.vertical_speed) <= booster_landed_vertical_speed_threshold) { landed_time = current_time; - state = FSMState::STATE_LANDED; + state = FSMState::STATE_LANDED; } break; case FSMState::STATE_LANDED: - if ((abs(state_estimate.vertical_speed) > booster_landed_vertical_speed_threshold) && ((current_time - landed_time) > booster_landed_timer_threshold)) { + if ((std::abs(state_estimate.vertical_speed) > booster_landed_vertical_speed_threshold) && + ((current_time - landed_time) > booster_landed_timer_threshold)) { state = FSMState::STATE_MAIN; } break; - } return state; } diff --git a/MIDAS/src/finite-state-machines/sg1-4.yaml b/MIDAS/src/finite-state-machines/sg1-4.yaml new file mode 100644 index 00000000..2cca8c27 --- /dev/null +++ b/MIDAS/src/finite-state-machines/sg1-4.yaml @@ -0,0 +1,62 @@ +second_stage_thresholds: + idle_to_first_boost: + acceleration_threshold: 3 + time_threshold: 1000 + ignition_to_second_boost: + acceleration_threshold: 3 + time_threshold: 1000 + second_boost_to_coast: + time_threshold: 1000 + coast_detection: + acceleration_threshold: 0.2 + coast_to_apogee: + vertical_speed_threshold: 15 + apogee: + backto_coast_vertical_speed_threshold: 10 + check_threshold: 1000 + timer_threshold: 1000 + drogue: + timer_threshold: 3000 + main: + to_main_deploy_timer_threshold: 3000 + deploy_altitude_threshold: 3000 + ignition: + to_second_boost_time_threshold: 1000 + to_coast_timer_threshold: 5000 + landed: + timer_threshold: 5000 + vertical_speed_threshold: 20 + first_boost: + to_burnout_time_threshold: 1000 + drogue_jerk_threshold: 200 + main_jerk_threshold: 300 + +first_stage_thresholds: + idle_to_first_boost: + acceleration_threshold: 3 + time_threshold: 1000 + first_separation: + time_threshold: 3000 + coast_detection: + acceleration_threshold: 0.2 + coast_to_apogee: + vertical_speed_threshold: 20 + apogee: + check_threshold: 1000 + timer_threshold: 1000 + drogue: + timer_threshold: 3000 + main: + to_main_deploy_timer_threshold: 3000 + deploy_altitude_threshold: 3000 + ignition: + to_second_boost_time_threshold: 1000 + to_coast_timer_threshold: 5000 + landed: + timer_threshold: 5000 + vertical_speed_threshold: 20 + first_boost: + to_burnout_time_threshold: 1000 + first_separation_jerk_threshold: 300 + drogue_jerk_threshold: 200 + main_jerk_threshold: 300 From d15ede32e44840d14d089d6aa423c6fb3d40b3e4 Mon Sep 17 00:00:00 2001 From: Aadityavoru Date: Fri, 31 Jan 2025 18:06:31 -0600 Subject: [PATCH 2/9] json parser complete --- MIDAS/platformio.ini | 2 + MIDAS/src/finite-state-machines/fsm.cpp | 160 +++++++++++---------- MIDAS/src/finite-state-machines/fsm.h | 2 +- MIDAS/src/finite-state-machines/sg1-4.json | 86 +++++++++++ MIDAS/src/finite-state-machines/sg1-4.yaml | 62 -------- 5 files changed, 170 insertions(+), 142 deletions(-) create mode 100644 MIDAS/src/finite-state-machines/sg1-4.json delete mode 100644 MIDAS/src/finite-state-machines/sg1-4.yaml diff --git a/MIDAS/platformio.ini b/MIDAS/platformio.ini index 43f37175..7da7149e 100644 --- a/MIDAS/platformio.ini +++ b/MIDAS/platformio.ini @@ -14,6 +14,7 @@ build_unflags = lib_deps = adafruit/Adafruit LIS3MDL@^1.2.1 ; Magnetometer driver stevemarple/MicroNMEA@^2.0.6 ; NMEA Parsing library (for GPS messages) + bblanchon/ArduinoJson @ ^6.19.4 build_src_filter = +<*> - + - @@ -33,6 +34,7 @@ build_unflags = lib_deps = adafruit/Adafruit LIS3MDL@^1.2.1 ; Magnetometer driver stevemarple/MicroNMEA@^2.0.6 ; NMEA Parsing library (for GPS messages) + bblanchon/ArduinoJson @ ^6.19.4 build_src_filter = +<*> - + - diff --git a/MIDAS/src/finite-state-machines/fsm.cpp b/MIDAS/src/finite-state-machines/fsm.cpp index f062c911..e1d8e1f9 100644 --- a/MIDAS/src/finite-state-machines/fsm.cpp +++ b/MIDAS/src/finite-state-machines/fsm.cpp @@ -2,15 +2,11 @@ #include #include #include -#include +#include #include "fsm.h" -// #include "thresholds.h" -// ------------------------------------------------------------------------ -// 1) Define static variables for each threshold that was previously a macro -// These names match exactly the ones in your original code. -// ------------------------------------------------------------------------ + static double sustainer_idle_to_first_boost_acceleration_threshold; static double sustainer_idle_to_first_boost_time_threshold; static double sustainer_ignition_to_second_boost_acceleration_threshold; @@ -50,77 +46,87 @@ static double booster_first_separation_jerk_threshold; static double booster_drogue_jerk_threshold; static double booster_main_jerk_threshold; -// ------------------------------------------------------------------------ -// 2) Use a static boolean to ensure the YAML is only loaded once -// ------------------------------------------------------------------------ -static bool yamlLoaded = false; - -// ------------------------------------------------------------------------ -// 3) Load thresholds from YAML -// This function will parse the YAML file and populate the static variables. -// ------------------------------------------------------------------------ -static void loadYamlThresholds() { - if (yamlLoaded) { - return; // Already loaded, skip +#include + +static bool jsonLoaded = false; + +static void loadJsonThresholds() { + if (jsonLoaded) { + return; + } + + if (!SPIFFS.begin(true)) { + Serial.println("⚠️ SPIFFS mount failed! Ensure SPIFFS is enabled."); + return; } - try { - YAML::Node config = YAML::LoadFile("sg1-4.yaml"); - - // Load second stage thresholds - auto s = config["second_stage_thresholds"]; - sustainer_idle_to_first_boost_acceleration_threshold = s["idle_to_first_boost"]["acceleration_threshold"].as(); - sustainer_idle_to_first_boost_time_threshold = s["idle_to_first_boost"]["time_threshold"].as(); - sustainer_ignition_to_second_boost_acceleration_threshold = s["ignition_to_second_boost"]["acceleration_threshold"].as(); - sustainer_ignition_to_second_boost_time_threshold = s["ignition_to_second_boost"]["time_threshold"].as(); - sustainer_second_boost_to_coast_time_threshold = s["second_boost_to_coast"]["time_threshold"].as(); - sustainer_coast_detection_acceleration_threshold = s["coast_detection"]["acceleration_threshold"].as(); - sustainer_coast_to_apogee_vertical_speed_threshold = s["coast_to_apogee"]["vertical_speed_threshold"].as(); - sustainer_apogee_backto_coast_vertical_speed_threshold = s["apogee"]["backto_coast_vertical_speed_threshold"].as(); - sustainer_apogee_check_threshold = s["apogee"]["check_threshold"].as(); - sustainer_apogee_timer_threshold = s["apogee"]["timer_threshold"].as(); - sustainer_drogue_timer_threshold = s["drogue"]["timer_threshold"].as(); - sustainer_main_to_main_deploy_timer_threshold = s["main"]["to_main_deploy_timer_threshold"].as(); - sustainer_main_deploy_altitude_threshold = s["main"]["deploy_altitude_threshold"].as(); - sustainer_ignition_to_coast_timer_threshold = s["ignition"]["to_coast_timer_threshold"].as(); - sustainer_landed_timer_threshold = s["landed"]["timer_threshold"].as(); - sustainer_landed_vertical_speed_threshold = s["landed"]["vertical_speed_threshold"].as(); - sustainer_first_boost_to_burnout_time_threshold = s["first_boost"]["to_burnout_time_threshold"].as(); - sustainer_drogue_jerk_threshold = s["drogue_jerk_threshold"].as(); - sustainer_main_jerk_threshold = s["main_jerk_threshold"].as(); - - // Load first stage thresholds - auto b = config["first_stage_thresholds"]; - booster_idle_to_first_boost_acceleration_threshold = b["idle_to_first_boost"]["acceleration_threshold"].as(); - booster_idle_to_first_boost_time_threshold = b["idle_to_first_boost"]["time_threshold"].as(); - booster_first_seperation_time_threshold = b["first_separation"]["time_threshold"].as(); - booster_coast_detection_acceleration_threshold = b["coast_detection"]["acceleration_threshold"].as(); - booster_coast_to_apogee_vertical_speed_threshold = b["coast_to_apogee"]["vertical_speed_threshold"].as(); - booster_apogee_check_threshold = b["apogee"]["check_threshold"].as(); - booster_apogee_timer_threshold = b["apogee"]["timer_threshold"].as(); - booster_drogue_timer_threshold = b["drogue"]["timer_threshold"].as(); - booster_main_to_main_deploy_timer_threshold = b["main"]["to_main_deploy_timer_threshold"].as(); - booster_main_deploy_altitude_threshold = b["main"]["deploy_altitude_threshold"].as(); - booster_ignition_to_second_boost_time_threshold = b["ignition"]["to_second_boost_time_threshold"].as(); - booster_ignition_to_coast_timer_threshold = b["ignition"]["to_coast_timer_threshold"].as(); - booster_landed_timer_threshold = b["landed"]["timer_threshold"].as(); - booster_first_boost_to_burnout_time_threshold = b["first_boost"]["to_burnout_time_threshold"].as(); - booster_landed_vertical_speed_threshold = b["landed"]["vertical_speed_threshold"].as(); - booster_first_separation_jerk_threshold = b["first_separation_jerk_threshold"].as(); - booster_drogue_jerk_threshold = b["drogue_jerk_threshold"].as(); - booster_main_jerk_threshold = b["main_jerk_threshold"].as(); - - yamlLoaded = true; + File file = SPIFFS.open("/sg1-4.json", "r"); + if (!file) { + Serial.println("❌ Error: Could not open /sg1-4.json"); + return; } - catch (const std::exception &e) { - std::cerr << "Error loading thresholds from YAML: " << e.what() << std::endl; - std::abort(); + + // Create a JSON document (adjust size as needed) + StaticJsonDocument<2048> doc; + + // Deserialize JSON + DeserializationError error = deserializeJson(doc, file); + file.close(); // Close file after parsing + + if (error) { + Serial.print("❌ JSON parsing failed: "); + Serial.println(error.f_str()); + return; } + + // Load second stage thresholds + JsonObject s = doc["second_stage_thresholds"]; + sustainer_idle_to_first_boost_acceleration_threshold = s["idle_to_first_boost"]["acceleration_threshold"] | 3; + sustainer_idle_to_first_boost_time_threshold = s["idle_to_first_boost"]["time_threshold"] | 1000; + sustainer_ignition_to_second_boost_acceleration_threshold = s["ignition_to_second_boost"]["acceleration_threshold"] | 3; + sustainer_ignition_to_second_boost_time_threshold = s["ignition_to_second_boost"]["time_threshold"] | 1000; + sustainer_second_boost_to_coast_time_threshold = s["second_boost_to_coast"]["time_threshold"] | 1000; + sustainer_coast_detection_acceleration_threshold = s["coast_detection"]["acceleration_threshold"] | 0.2; + sustainer_coast_to_apogee_vertical_speed_threshold = s["coast_to_apogee"]["vertical_speed_threshold"] | 15; + sustainer_apogee_backto_coast_vertical_speed_threshold = s["apogee"]["backto_coast_vertical_speed_threshold"] | 10; + sustainer_apogee_check_threshold = s["apogee"]["check_threshold"] | 1000; + sustainer_apogee_timer_threshold = s["apogee"]["timer_threshold"] | 1000; + sustainer_drogue_timer_threshold = s["drogue"]["timer_threshold"] | 3000; + sustainer_main_to_main_deploy_timer_threshold = s["main"]["to_main_deploy_timer_threshold"] | 3000; + sustainer_main_deploy_altitude_threshold = s["main"]["deploy_altitude_threshold"] | 3000; + sustainer_ignition_to_coast_timer_threshold = s["ignition"]["to_coast_timer_threshold"] | 5000; + sustainer_landed_timer_threshold = s["landed"]["timer_threshold"] | 5000; + sustainer_landed_vertical_speed_threshold = s["landed"]["vertical_speed_threshold"] | 20; + sustainer_first_boost_to_burnout_time_threshold = s["first_boost"]["to_burnout_time_threshold"] | 1000; + sustainer_drogue_jerk_threshold = s["drogue_jerk_threshold"] | 200; + sustainer_main_jerk_threshold = s["main_jerk_threshold"] | 300; + + // Load first stage thresholds + JsonObject b = doc["first_stage_thresholds"]; + booster_idle_to_first_boost_acceleration_threshold = b["idle_to_first_boost"]["acceleration_threshold"] | 3; + booster_idle_to_first_boost_time_threshold = b["idle_to_first_boost"]["time_threshold"] | 1000; + booster_first_seperation_time_threshold = b["first_separation"]["time_threshold"] | 3000; + booster_coast_detection_acceleration_threshold = b["coast_detection"]["acceleration_threshold"] | 0.2; + booster_coast_to_apogee_vertical_speed_threshold = b["coast_to_apogee"]["vertical_speed_threshold"] | 20; + booster_apogee_check_threshold = b["apogee"]["check_threshold"] | 1000; + booster_apogee_timer_threshold = b["apogee"]["timer_threshold"] | 1000; + booster_drogue_timer_threshold = b["drogue"]["timer_threshold"] | 3000; + booster_main_to_main_deploy_timer_threshold = b["main"]["to_main_deploy_timer_threshold"] | 3000; + booster_main_deploy_altitude_threshold = b["main"]["deploy_altitude_threshold"] | 3000; + booster_ignition_to_second_boost_time_threshold = b["ignition"]["to_second_boost_time_threshold"] | 1000; + booster_ignition_to_coast_timer_threshold = b["ignition"]["to_coast_timer_threshold"] | 5000; + booster_landed_timer_threshold = b["landed"]["timer_threshold"] | 5000; + booster_first_boost_to_burnout_time_threshold = b["first_boost"]["to_burnout_time_threshold"] | 1000; + booster_landed_vertical_speed_threshold = b["landed"]["vertical_speed_threshold"] | 20; + booster_first_separation_jerk_threshold = b["first_separation_jerk_threshold"] | 300; + booster_drogue_jerk_threshold = b["drogue_jerk_threshold"] | 200; + booster_main_jerk_threshold = b["main_jerk_threshold"] | 300; + + jsonLoaded = true; + Serial.println("✅ JSON Thresholds Loaded Successfully!"); } -// ------------------------------------------------------------------------ -// Helper functions from your code -// ------------------------------------------------------------------------ + template double sensor_average(BufferedSensorData& sensor, double (* get_item)(T&)) { auto arr = sensor.template getBufferRecent(); @@ -158,9 +164,7 @@ double sensor_derivative(BufferedSensorData& sensor, double (* get_ite return (second_average - first_average) / (second_average_time - first_average_time); } -// ------------------------------------------------------------------------ -// StateEstimate constructor from your code -// ------------------------------------------------------------------------ + StateEstimate::StateEstimate(RocketData& state) { acceleration = sensor_average(state.high_g, [](HighGData& data) { return (double) data.ax; @@ -176,13 +180,11 @@ StateEstimate::StateEstimate(RocketData& state) { }); } -// ------------------------------------------------------------------------ -// The FSM code rewritten to load from YAML but keep variable names unchanged -// ------------------------------------------------------------------------ + #ifdef IS_SUSTAINER FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate) { // 1) Ensure thresholds are loaded - loadYamlThresholds(); + loadJsonThresholds(); // 2) get current time double current_time = pdTICKS_TO_MS(xTaskGetTickCount()); @@ -314,7 +316,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate) { FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate) { // 1) Ensure thresholds are loaded - loadYamlThresholds(); + loadJsonThresholds(); double current_time = pdTICKS_TO_MS(xTaskGetTickCount()); diff --git a/MIDAS/src/finite-state-machines/fsm.h b/MIDAS/src/finite-state-machines/fsm.h index ed2b2257..e096ff7c 100644 --- a/MIDAS/src/finite-state-machines/fsm.h +++ b/MIDAS/src/finite-state-machines/fsm.h @@ -7,7 +7,7 @@ #include "FreeRTOSConfig.h" #include "fsm_states.h" -#include "thresholds.h" +//#include "thresholds.h" #include "sensor_data.h" #include "Buffer.h" #include "rocket_state.h" diff --git a/MIDAS/src/finite-state-machines/sg1-4.json b/MIDAS/src/finite-state-machines/sg1-4.json new file mode 100644 index 00000000..abaf84bb --- /dev/null +++ b/MIDAS/src/finite-state-machines/sg1-4.json @@ -0,0 +1,86 @@ +{ + "second_stage_thresholds": { + "idle_to_first_boost": { + "acceleration_threshold": 3, + "time_threshold": 1000 + }, + "ignition_to_second_boost": { + "acceleration_threshold": 3, + "time_threshold": 1000 + }, + "second_boost_to_coast": { + "time_threshold": 1000 + }, + "coast_detection": { + "acceleration_threshold": 0.2 + }, + "coast_to_apogee": { + "vertical_speed_threshold": 15 + }, + "apogee": { + "backto_coast_vertical_speed_threshold": 10, + "check_threshold": 1000, + "timer_threshold": 1000 + }, + "drogue": { + "timer_threshold": 3000 + }, + "main": { + "to_main_deploy_timer_threshold": 3000, + "deploy_altitude_threshold": 3000 + }, + "ignition": { + "to_second_boost_time_threshold": 1000, + "to_coast_timer_threshold": 5000 + }, + "landed": { + "timer_threshold": 5000, + "vertical_speed_threshold": 20 + }, + "first_boost": { + "to_burnout_time_threshold": 1000 + }, + "drogue_jerk_threshold": 200, + "main_jerk_threshold": 300 + }, + "first_stage_thresholds": { + "idle_to_first_boost": { + "acceleration_threshold": 3, + "time_threshold": 1000 + }, + "first_separation": { + "time_threshold": 3000 + }, + "coast_detection": { + "acceleration_threshold": 0.2 + }, + "coast_to_apogee": { + "vertical_speed_threshold": 20 + }, + "apogee": { + "check_threshold": 1000, + "timer_threshold": 1000 + }, + "drogue": { + "timer_threshold": 3000 + }, + "main": { + "to_main_deploy_timer_threshold": 3000, + "deploy_altitude_threshold": 3000 + }, + "ignition": { + "to_second_boost_time_threshold": 1000, + "to_coast_timer_threshold": 5000 + }, + "landed": { + "timer_threshold": 5000, + "vertical_speed_threshold": 20 + }, + "first_boost": { + "to_burnout_time_threshold": 1000 + }, + "first_separation_jerk_threshold": 300, + "drogue_jerk_threshold": 200, + "main_jerk_threshold": 300 + } +} diff --git a/MIDAS/src/finite-state-machines/sg1-4.yaml b/MIDAS/src/finite-state-machines/sg1-4.yaml deleted file mode 100644 index 2cca8c27..00000000 --- a/MIDAS/src/finite-state-machines/sg1-4.yaml +++ /dev/null @@ -1,62 +0,0 @@ -second_stage_thresholds: - idle_to_first_boost: - acceleration_threshold: 3 - time_threshold: 1000 - ignition_to_second_boost: - acceleration_threshold: 3 - time_threshold: 1000 - second_boost_to_coast: - time_threshold: 1000 - coast_detection: - acceleration_threshold: 0.2 - coast_to_apogee: - vertical_speed_threshold: 15 - apogee: - backto_coast_vertical_speed_threshold: 10 - check_threshold: 1000 - timer_threshold: 1000 - drogue: - timer_threshold: 3000 - main: - to_main_deploy_timer_threshold: 3000 - deploy_altitude_threshold: 3000 - ignition: - to_second_boost_time_threshold: 1000 - to_coast_timer_threshold: 5000 - landed: - timer_threshold: 5000 - vertical_speed_threshold: 20 - first_boost: - to_burnout_time_threshold: 1000 - drogue_jerk_threshold: 200 - main_jerk_threshold: 300 - -first_stage_thresholds: - idle_to_first_boost: - acceleration_threshold: 3 - time_threshold: 1000 - first_separation: - time_threshold: 3000 - coast_detection: - acceleration_threshold: 0.2 - coast_to_apogee: - vertical_speed_threshold: 20 - apogee: - check_threshold: 1000 - timer_threshold: 1000 - drogue: - timer_threshold: 3000 - main: - to_main_deploy_timer_threshold: 3000 - deploy_altitude_threshold: 3000 - ignition: - to_second_boost_time_threshold: 1000 - to_coast_timer_threshold: 5000 - landed: - timer_threshold: 5000 - vertical_speed_threshold: 20 - first_boost: - to_burnout_time_threshold: 1000 - first_separation_jerk_threshold: 300 - drogue_jerk_threshold: 200 - main_jerk_threshold: 300 From 509af90efa3a2a210371e890e953b7c1ef3ff3a6 Mon Sep 17 00:00:00 2001 From: Aadityavoru Date: Fri, 31 Jan 2025 18:10:36 -0600 Subject: [PATCH 3/9] fixed thresholds --- MIDAS/src/finite-state-machines/sg1-4.json | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/MIDAS/src/finite-state-machines/sg1-4.json b/MIDAS/src/finite-state-machines/sg1-4.json index abaf84bb..a843d9bc 100644 --- a/MIDAS/src/finite-state-machines/sg1-4.json +++ b/MIDAS/src/finite-state-machines/sg1-4.json @@ -27,7 +27,8 @@ }, "main": { "to_main_deploy_timer_threshold": 3000, - "deploy_altitude_threshold": 3000 + "__comment": "This is the altitude at which the main parachute will deploy in meters above sea level as it adds the altitude of the rocket to the altitude of the launch pad", + "deploy_altitude_threshold": 1006 }, "ignition": { "to_second_boost_time_threshold": 1000, @@ -35,7 +36,7 @@ }, "landed": { "timer_threshold": 5000, - "vertical_speed_threshold": 20 + "vertical_speed_threshold": 3 }, "first_boost": { "to_burnout_time_threshold": 1000 @@ -74,7 +75,7 @@ }, "landed": { "timer_threshold": 5000, - "vertical_speed_threshold": 20 + "vertical_speed_threshold": 5 }, "first_boost": { "to_burnout_time_threshold": 1000 From 12eff9e69733797765661669bd6620fb1d04e3e8 Mon Sep 17 00:00:00 2001 From: Aadityavoru Date: Fri, 31 Jan 2025 18:23:33 -0600 Subject: [PATCH 4/9] fix changes and use .h files instead --- MIDAS/platformio.ini | 2 - MIDAS/src/finite-state-machines/fsm.cpp | 316 +++++------------- MIDAS/src/finite-state-machines/fsm.h | 2 +- .../{thresholds.h => sg1-4.h} | 6 +- MIDAS/src/finite-state-machines/sg1-4.json | 87 ----- 5 files changed, 92 insertions(+), 321 deletions(-) rename MIDAS/src/finite-state-machines/{thresholds.h => sg1-4.h} (96%) delete mode 100644 MIDAS/src/finite-state-machines/sg1-4.json diff --git a/MIDAS/platformio.ini b/MIDAS/platformio.ini index 7da7149e..43f37175 100644 --- a/MIDAS/platformio.ini +++ b/MIDAS/platformio.ini @@ -14,7 +14,6 @@ build_unflags = lib_deps = adafruit/Adafruit LIS3MDL@^1.2.1 ; Magnetometer driver stevemarple/MicroNMEA@^2.0.6 ; NMEA Parsing library (for GPS messages) - bblanchon/ArduinoJson @ ^6.19.4 build_src_filter = +<*> - + - @@ -34,7 +33,6 @@ build_unflags = lib_deps = adafruit/Adafruit LIS3MDL@^1.2.1 ; Magnetometer driver stevemarple/MicroNMEA@^2.0.6 ; NMEA Parsing library (for GPS messages) - bblanchon/ArduinoJson @ ^6.19.4 build_src_filter = +<*> - + - diff --git a/MIDAS/src/finite-state-machines/fsm.cpp b/MIDAS/src/finite-state-machines/fsm.cpp index f119ae21..93f2794d 100644 --- a/MIDAS/src/finite-state-machines/fsm.cpp +++ b/MIDAS/src/finite-state-machines/fsm.cpp @@ -1,142 +1,9 @@ #include -#include -#include -#include -#include #include "fsm.h" +#include "sg1-4.h" - - -static double sustainer_idle_to_first_boost_acceleration_threshold; -static double sustainer_idle_to_first_boost_time_threshold; -static double sustainer_ignition_to_second_boost_acceleration_threshold; -static double sustainer_ignition_to_second_boost_time_threshold; -static double sustainer_second_boost_to_coast_time_threshold; -static double sustainer_coast_detection_acceleration_threshold; -static double sustainer_coast_to_apogee_vertical_speed_threshold; -static double sustainer_apogee_backto_coast_vertical_speed_threshold; -static double sustainer_apogee_check_threshold; -static double sustainer_apogee_timer_threshold; -static double sustainer_drogue_timer_threshold; -static double sustainer_main_to_main_deploy_timer_threshold; -static double sustainer_main_deploy_altitude_threshold; -static double sustainer_ignition_to_coast_timer_threshold; -static double sustainer_landed_timer_threshold; -static double sustainer_landed_vertical_speed_threshold; -static double sustainer_first_boost_to_burnout_time_threshold; -static double sustainer_drogue_jerk_threshold; -static double sustainer_main_jerk_threshold; - -static double booster_idle_to_first_boost_acceleration_threshold; -static double booster_idle_to_first_boost_time_threshold; -static double booster_first_seperation_time_threshold; -static double booster_coast_detection_acceleration_threshold; -static double booster_coast_to_apogee_vertical_speed_threshold; -static double booster_apogee_check_threshold; -static double booster_apogee_timer_threshold; -static double booster_drogue_timer_threshold; -static double booster_main_to_main_deploy_timer_threshold; -static double booster_main_deploy_altitude_threshold; -static double booster_ignition_to_second_boost_time_threshold; -static double booster_ignition_to_coast_timer_threshold; -static double booster_landed_timer_threshold; -static double booster_first_boost_to_burnout_time_threshold; -static double booster_landed_vertical_speed_threshold; -static double booster_first_separation_jerk_threshold; -static double booster_drogue_jerk_threshold; -static double booster_main_jerk_threshold; - -#include - -static bool jsonLoaded = false; - -static void loadJsonThresholds() { - if (jsonLoaded) { - return; - } - - if (!SPIFFS.begin(true)) { - Serial.println("⚠️ SPIFFS mount failed! Ensure SPIFFS is enabled."); - return; - } - - File file = SPIFFS.open("/sg1-4.json", "r"); - if (!file) { - Serial.println("❌ Error: Could not open /sg1-4.json"); - return; - } - - // Create a JSON document (adjust size as needed) - StaticJsonDocument<2048> doc; - - // Deserialize JSON - DeserializationError error = deserializeJson(doc, file); - file.close(); // Close file after parsing - - if (error) { - Serial.print("❌ JSON parsing failed: "); - Serial.println(error.f_str()); - return; - } - - // Load second stage thresholds - JsonObject s = doc["second_stage_thresholds"]; - sustainer_idle_to_first_boost_acceleration_threshold = s["idle_to_first_boost"]["acceleration_threshold"] | 3; - sustainer_idle_to_first_boost_time_threshold = s["idle_to_first_boost"]["time_threshold"] | 1000; - sustainer_ignition_to_second_boost_acceleration_threshold = s["ignition_to_second_boost"]["acceleration_threshold"] | 3; - sustainer_ignition_to_second_boost_time_threshold = s["ignition_to_second_boost"]["time_threshold"] | 1000; - sustainer_second_boost_to_coast_time_threshold = s["second_boost_to_coast"]["time_threshold"] | 1000; - sustainer_coast_detection_acceleration_threshold = s["coast_detection"]["acceleration_threshold"] | 0.2; - sustainer_coast_to_apogee_vertical_speed_threshold = s["coast_to_apogee"]["vertical_speed_threshold"] | 15; - sustainer_apogee_backto_coast_vertical_speed_threshold = s["apogee"]["backto_coast_vertical_speed_threshold"] | 10; - sustainer_apogee_check_threshold = s["apogee"]["check_threshold"] | 1000; - sustainer_apogee_timer_threshold = s["apogee"]["timer_threshold"] | 1000; - sustainer_drogue_timer_threshold = s["drogue"]["timer_threshold"] | 3000; - sustainer_main_to_main_deploy_timer_threshold = s["main"]["to_main_deploy_timer_threshold"] | 3000; - sustainer_main_deploy_altitude_threshold = s["main"]["deploy_altitude_threshold"] | 3000; - sustainer_ignition_to_coast_timer_threshold = s["ignition"]["to_coast_timer_threshold"] | 5000; - sustainer_landed_timer_threshold = s["landed"]["timer_threshold"] | 5000; - sustainer_landed_vertical_speed_threshold = s["landed"]["vertical_speed_threshold"] | 20; - sustainer_first_boost_to_burnout_time_threshold = s["first_boost"]["to_burnout_time_threshold"] | 1000; - sustainer_drogue_jerk_threshold = s["drogue_jerk_threshold"] | 200; - sustainer_main_jerk_threshold = s["main_jerk_threshold"] | 300; - - // Load first stage thresholds - JsonObject b = doc["first_stage_thresholds"]; - booster_idle_to_first_boost_acceleration_threshold = b["idle_to_first_boost"]["acceleration_threshold"] | 3; - booster_idle_to_first_boost_time_threshold = b["idle_to_first_boost"]["time_threshold"] | 1000; - booster_first_seperation_time_threshold = b["first_separation"]["time_threshold"] | 3000; - booster_coast_detection_acceleration_threshold = b["coast_detection"]["acceleration_threshold"] | 0.2; - booster_coast_to_apogee_vertical_speed_threshold = b["coast_to_apogee"]["vertical_speed_threshold"] | 20; - booster_apogee_check_threshold = b["apogee"]["check_threshold"] | 1000; - booster_apogee_timer_threshold = b["apogee"]["timer_threshold"] | 1000; - booster_drogue_timer_threshold = b["drogue"]["timer_threshold"] | 3000; - booster_main_to_main_deploy_timer_threshold = b["main"]["to_main_deploy_timer_threshold"] | 3000; - booster_main_deploy_altitude_threshold = b["main"]["deploy_altitude_threshold"] | 3000; - booster_ignition_to_second_boost_time_threshold = b["ignition"]["to_second_boost_time_threshold"] | 1000; - booster_ignition_to_coast_timer_threshold = b["ignition"]["to_coast_timer_threshold"] | 5000; - booster_landed_timer_threshold = b["landed"]["timer_threshold"] | 5000; - booster_first_boost_to_burnout_time_threshold = b["first_boost"]["to_burnout_time_threshold"] | 1000; - booster_landed_vertical_speed_threshold = b["landed"]["vertical_speed_threshold"] | 20; - booster_first_separation_jerk_threshold = b["first_separation_jerk_threshold"] | 300; - booster_drogue_jerk_threshold = b["drogue_jerk_threshold"] | 200; - booster_main_jerk_threshold = b["main_jerk_threshold"] | 300; - - jsonLoaded = true; - Serial.println("✅ JSON Thresholds Loaded Successfully!"); -} - - - -/** - * @brief Helper to calculate the average value of a buffered sensor data - * - * @param sensor Buffered sensor struct - * @param get_item Lambda get function - * - * @return Average value -*/ +// helper functions template double sensor_average(BufferedSensorData& sensor, double (* get_item)(T&)) { @@ -148,44 +15,33 @@ double sensor_average(BufferedSensorData& sensor, double (* get_item)( return sum / count; } -/** - * @brief Helper to calculate the derivative over a buffered sensor data - * - * @param sensor Buffered sensor struct - * @param get_item Lambda get function - * - * @return Derivative -*/ template double sensor_derivative(BufferedSensorData& sensor, double (* get_item)(T&)) { - auto arr = sensor.template getBufferRecent(); + auto arr = sensor.template getBufferRecent(); auto times = sensor.template getTimesRecent(); - size_t i = 0; + size_t i = 0; - double first_average = 0.0; + double first_average = 0.0; double first_average_time = 0.0; for (; i < count / 2; i++) { - first_average += get_item(arr[i]); + first_average += get_item(arr[i]); first_average_time += pdTICKS_TO_MS(times[i]) / 1000.0; } - first_average /= (count / 2.0); + first_average /= (count / 2.0); first_average_time /= (count / 2.0); - double second_average = 0.0; + double second_average = 0.0; double second_average_time = 0.0; for (; i < count; i++) { - second_average += get_item(arr[i]); + second_average += get_item(arr[i]); second_average_time += pdTICKS_TO_MS(times[i]) / 1000.0; } - second_average /= (count / 2.0); + second_average /= (count / 2.0); second_average_time /= (count / 2.0); - return (second_average - first_average) / (second_average_time - first_average_time); } -/** - * @brief Populates StateEstimate struct with the correct values for accel, alt, jerk, and speed -*/ + StateEstimate::StateEstimate(RocketData& state) { acceleration = sensor_average(state.high_g, [](HighGData& data) { return (double) data.ax; @@ -204,50 +60,42 @@ StateEstimate::StateEstimate(RocketData& state) { #ifdef IS_SUSTAINER - - -/** - * @brief Sustainer FSM tick function, which will advance the current state if necessary - * - * @param state current FSM state - * @param state_estimate StateEstimate struct for the current estimate for accel, alt, jerk, and speed - * - * @return New FSM State -*/ - FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate) { - // 1) Ensure thresholds are loaded - loadJsonThresholds(); - - // 2) get current time + //get current time double current_time = pdTICKS_TO_MS(xTaskGetTickCount()); switch (state) { case FSMState::STATE_IDLE: + // 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; - state = FSMState::STATE_FIRST_BOOST; + state = FSMState::STATE_FIRST_BOOST; } + break; case FSMState::STATE_FIRST_BOOST: - if ((state_estimate.acceleration < sustainer_idle_to_first_boost_acceleration_threshold) && - ((current_time - launch_time) < sustainer_idle_to_first_boost_time_threshold)) { + // 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; 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; - state = FSMState::STATE_BURNOUT; + state = FSMState::STATE_BURNOUT; } break; case FSMState::STATE_BURNOUT: - if ((state_estimate.acceleration >= sustainer_coast_detection_acceleration_threshold) && - ((current_time - burnout_time) < sustainer_first_boost_to_burnout_time_threshold)) { + // 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_first_boost_to_burnout_time_threshold)) { state = FSMState::STATE_FIRST_BOOST; break; } + + // if in burnout for long enough then go on to the next state (time transition) if ((current_time - burnout_time) > sustainer_first_boost_to_burnout_time_threshold) { sustainer_ignition_time = current_time; state = FSMState::STATE_SUSTAINER_IGNITION; @@ -255,217 +103,229 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate) { break; case FSMState::STATE_SUSTAINER_IGNITION: + // 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; - state = FSMState::STATE_COAST; + state = FSMState::STATE_COAST; break; } + + // 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; - state = FSMState::STATE_SECOND_BOOST; + state = FSMState::STATE_SECOND_BOOST; } + break; case FSMState::STATE_SECOND_BOOST: - if ((state_estimate.acceleration < sustainer_ignition_to_second_boost_acceleration_threshold) && - ((current_time - second_boost_time) < sustainer_ignition_to_second_boost_time_threshold)) { + // 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; break; } + + // if low acceleration detected go to next state if (state_estimate.acceleration < sustainer_coast_detection_acceleration_threshold) { coast_time = current_time; - state = FSMState::STATE_COAST; + state = FSMState::STATE_COAST; } break; case FSMState::STATE_COAST: - if ((state_estimate.acceleration > sustainer_coast_detection_acceleration_threshold) && - ((current_time - coast_time) < sustainer_second_boost_to_coast_time_threshold)) { + // 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; break; } + + // if speed slows down enough then go on to the next stage if (state_estimate.vertical_speed <= sustainer_coast_to_apogee_vertical_speed_threshold) { apogee_time = current_time; - state = FSMState::STATE_APOGEE; + state = FSMState::STATE_APOGEE; } break; case FSMState::STATE_APOGEE: - if ((state_estimate.vertical_speed) > sustainer_apogee_backto_coast_vertical_speed_threshold && - ((current_time - apogee_time) < sustainer_apogee_check_threshold)) { + // 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; break; } + + // transition to next state after a certain amount of time if ((current_time - apogee_time) > sustainer_apogee_timer_threshold) { drogue_time = current_time; - state = FSMState::STATE_DROGUE_DEPLOY; + state = FSMState::STATE_DROGUE_DEPLOY; } break; case FSMState::STATE_DROGUE_DEPLOY: - if (std::abs(state_estimate.jerk) < sustainer_drogue_jerk_threshold) { + // if detected a sharp change in jerk then go to next state + if (abs(state_estimate.jerk) < sustainer_drogue_jerk_threshold) { 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) { state = FSMState::STATE_DROGUE; } + break; case FSMState::STATE_DROGUE: + // if altitude low enough then next state if (state_estimate.altitude <= sustainer_main_deploy_altitude_threshold) { - state = FSMState::STATE_MAIN_DEPLOY; + state = FSMState::STATE_MAIN_DEPLOY; main_time = current_time; } break; case FSMState::STATE_MAIN_DEPLOY: - if (std::abs(state_estimate.jerk) < sustainer_main_jerk_threshold) { + // 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; break; } + + // 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; } break; case FSMState::STATE_MAIN: - if (std::abs(state_estimate.vertical_speed) <= sustainer_landed_vertical_speed_threshold) { + // if slowed down enough then go on to the next state + if (abs(state_estimate.vertical_speed) <= sustainer_landed_vertical_speed_threshold) { landed_time = current_time; - state = FSMState::STATE_LANDED; + state = FSMState::STATE_LANDED; } break; case FSMState::STATE_LANDED: - if ((std::abs(state_estimate.vertical_speed) > sustainer_landed_vertical_speed_threshold) && - ((current_time - landed_time) > sustainer_landed_timer_threshold)) { + // if the slow speed was too brief then return to previous state + if ((abs(state_estimate.vertical_speed) > sustainer_landed_vertical_speed_threshold) && ((current_time - landed_time) > sustainer_landed_timer_threshold)) { state = FSMState::STATE_MAIN; } break; + } return state; } -#else - -/** - * @brief Booster FSM tick function, which will advance the current state if necessary - * - * This is similar to the previous function but contains less states - * - * @param state current FSM state - * @param state_estimate StateEstimate struct for the current estimate for accel, alt, jerk, and speed - * - * @return New FSM State -*/ +#else +// this is similar to the previous function but contains less states FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate) { - // 1) Ensure thresholds are loaded - loadJsonThresholds(); - double current_time = pdTICKS_TO_MS(xTaskGetTickCount()); switch (state) { case FSMState::STATE_IDLE: if (state_estimate.acceleration > booster_idle_to_first_boost_acceleration_threshold) { launch_time = current_time; - state = FSMState::STATE_FIRST_BOOST; + state = FSMState::STATE_FIRST_BOOST; } + break; 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)) { + 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; break; } if (state_estimate.acceleration < booster_coast_detection_acceleration_threshold) { burnout_time = current_time; - state = FSMState::STATE_BURNOUT; + state = FSMState::STATE_BURNOUT; } + break; case FSMState::STATE_BURNOUT: - if ((state_estimate.acceleration >= booster_coast_detection_acceleration_threshold) && - ((current_time - burnout_time) < booster_first_boost_to_burnout_time_threshold)) { + 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; break; } + if ((current_time - burnout_time) > booster_first_boost_to_burnout_time_threshold) { first_separation_time = current_time; - state = FSMState::STATE_FIRST_SEPARATION; + state = FSMState::STATE_FIRST_SEPARATION; } break; case FSMState::STATE_FIRST_SEPARATION: - if (std::abs(state_estimate.jerk) < booster_first_separation_jerk_threshold) { + if (abs(state_estimate.jerk) < booster_first_separation_jerk_threshold) { state = FSMState::STATE_COAST; break; } + if ((current_time - first_separation_time) > booster_first_seperation_time_threshold) { state = FSMState::STATE_COAST; } + break; case FSMState::STATE_COAST: if (state_estimate.vertical_speed <= booster_coast_to_apogee_vertical_speed_threshold) { apogee_time = current_time; - state = FSMState::STATE_APOGEE; + state = FSMState::STATE_APOGEE; } 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)) { + if (state_estimate.vertical_speed > booster_coast_to_apogee_vertical_speed_threshold && ((current_time - apogee_time) < booster_apogee_check_threshold)) { state = FSMState::STATE_COAST; break; } + if ((current_time - apogee_time) > booster_apogee_timer_threshold) { drogue_time = current_time; - state = FSMState::STATE_DROGUE_DEPLOY; + state = FSMState::STATE_DROGUE_DEPLOY; } break; case FSMState::STATE_DROGUE_DEPLOY: - if (std::abs(state_estimate.jerk) < booster_drogue_jerk_threshold) { + if (abs(state_estimate.jerk) < booster_drogue_jerk_threshold) { state = FSMState::STATE_DROGUE; break; } if ((current_time - drogue_time) > booster_drogue_timer_threshold) { state = FSMState::STATE_DROGUE; } + break; case FSMState::STATE_DROGUE: if (state_estimate.altitude <= booster_main_deploy_altitude_threshold) { - state = FSMState::STATE_MAIN_DEPLOY; + state = FSMState::STATE_MAIN_DEPLOY; main_time = current_time; } break; case FSMState::STATE_MAIN_DEPLOY: - if (std::abs(state_estimate.jerk) < booster_main_jerk_threshold) { + if (abs(state_estimate.jerk) < booster_main_jerk_threshold) { state = FSMState::STATE_MAIN; break; } + if ((current_time - main_time) > booster_main_to_main_deploy_timer_threshold) { state = FSMState::STATE_MAIN; } break; case FSMState::STATE_MAIN: - if (std::abs(state_estimate.vertical_speed) <= booster_landed_vertical_speed_threshold) { + if (abs(state_estimate.vertical_speed) <= booster_landed_vertical_speed_threshold) { landed_time = current_time; - state = FSMState::STATE_LANDED; + state = FSMState::STATE_LANDED; } break; case FSMState::STATE_LANDED: - if ((std::abs(state_estimate.vertical_speed) > booster_landed_vertical_speed_threshold) && - ((current_time - landed_time) > booster_landed_timer_threshold)) { + if ((abs(state_estimate.vertical_speed) > booster_landed_vertical_speed_threshold) && ((current_time - landed_time) > booster_landed_timer_threshold)) { state = FSMState::STATE_MAIN; } break; + } return state; } diff --git a/MIDAS/src/finite-state-machines/fsm.h b/MIDAS/src/finite-state-machines/fsm.h index 5277d3d6..c370dee0 100644 --- a/MIDAS/src/finite-state-machines/fsm.h +++ b/MIDAS/src/finite-state-machines/fsm.h @@ -6,7 +6,7 @@ #include "fsm_states.h" -//#include "thresholds.h" +#include "sg1-4.h" #include "sensor_data.h" #include "Buffer.h" diff --git a/MIDAS/src/finite-state-machines/thresholds.h b/MIDAS/src/finite-state-machines/sg1-4.h similarity index 96% rename from MIDAS/src/finite-state-machines/thresholds.h rename to MIDAS/src/finite-state-machines/sg1-4.h index 59fdd9fb..74b5ca9a 100644 --- a/MIDAS/src/finite-state-machines/thresholds.h +++ b/MIDAS/src/finite-state-machines/sg1-4.h @@ -38,7 +38,7 @@ #define sustainer_main_to_main_deploy_timer_threshold 3000 // Height required to deploy the main parachutes -#define sustainer_main_deploy_altitude_threshold 3000 +#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 @@ -53,7 +53,7 @@ #define sustainer_first_boost_to_burnout_time_threshold 1000 // Transition to LANDED from MAIN if vertical speed is less than this threshold -#define sustainer_landed_vertical_speed_threshold 20 +#define sustainer_landed_vertical_speed_threshold 3 // Stores a small jerk value #define sustainer_drogue_jerk_threshold 200 @@ -108,7 +108,7 @@ #define booster_first_boost_to_burnout_time_threshold 1000 // Transition to LANDED from MAIN if vertical speed is less than this threshold -#define booster_landed_vertical_speed_threshold 20 +#define booster_landed_vertical_speed_threshold 4 // Stores a small jerk value #define booster_first_separation_jerk_threshold 300 diff --git a/MIDAS/src/finite-state-machines/sg1-4.json b/MIDAS/src/finite-state-machines/sg1-4.json deleted file mode 100644 index a843d9bc..00000000 --- a/MIDAS/src/finite-state-machines/sg1-4.json +++ /dev/null @@ -1,87 +0,0 @@ -{ - "second_stage_thresholds": { - "idle_to_first_boost": { - "acceleration_threshold": 3, - "time_threshold": 1000 - }, - "ignition_to_second_boost": { - "acceleration_threshold": 3, - "time_threshold": 1000 - }, - "second_boost_to_coast": { - "time_threshold": 1000 - }, - "coast_detection": { - "acceleration_threshold": 0.2 - }, - "coast_to_apogee": { - "vertical_speed_threshold": 15 - }, - "apogee": { - "backto_coast_vertical_speed_threshold": 10, - "check_threshold": 1000, - "timer_threshold": 1000 - }, - "drogue": { - "timer_threshold": 3000 - }, - "main": { - "to_main_deploy_timer_threshold": 3000, - "__comment": "This is the altitude at which the main parachute will deploy in meters above sea level as it adds the altitude of the rocket to the altitude of the launch pad", - "deploy_altitude_threshold": 1006 - }, - "ignition": { - "to_second_boost_time_threshold": 1000, - "to_coast_timer_threshold": 5000 - }, - "landed": { - "timer_threshold": 5000, - "vertical_speed_threshold": 3 - }, - "first_boost": { - "to_burnout_time_threshold": 1000 - }, - "drogue_jerk_threshold": 200, - "main_jerk_threshold": 300 - }, - "first_stage_thresholds": { - "idle_to_first_boost": { - "acceleration_threshold": 3, - "time_threshold": 1000 - }, - "first_separation": { - "time_threshold": 3000 - }, - "coast_detection": { - "acceleration_threshold": 0.2 - }, - "coast_to_apogee": { - "vertical_speed_threshold": 20 - }, - "apogee": { - "check_threshold": 1000, - "timer_threshold": 1000 - }, - "drogue": { - "timer_threshold": 3000 - }, - "main": { - "to_main_deploy_timer_threshold": 3000, - "deploy_altitude_threshold": 3000 - }, - "ignition": { - "to_second_boost_time_threshold": 1000, - "to_coast_timer_threshold": 5000 - }, - "landed": { - "timer_threshold": 5000, - "vertical_speed_threshold": 5 - }, - "first_boost": { - "to_burnout_time_threshold": 1000 - }, - "first_separation_jerk_threshold": 300, - "drogue_jerk_threshold": 200, - "main_jerk_threshold": 300 - } -} From 5f47cd4426c40a9c67550203f5bc43de3af7b9a8 Mon Sep 17 00:00:00 2001 From: Aadityavoru Date: Fri, 31 Jan 2025 18:24:03 -0600 Subject: [PATCH 5/9] ciruclar inputs resolved --- MIDAS/src/finite-state-machines/fsm.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/MIDAS/src/finite-state-machines/fsm.cpp b/MIDAS/src/finite-state-machines/fsm.cpp index 93f2794d..054e7009 100644 --- a/MIDAS/src/finite-state-machines/fsm.cpp +++ b/MIDAS/src/finite-state-machines/fsm.cpp @@ -1,7 +1,6 @@ #include #include "fsm.h" -#include "sg1-4.h" // helper functions From 64cd6cdd14ce747ace9429fc6404eb2a5533b490 Mon Sep 17 00:00:00 2001 From: Aadityavoru Date: Sun, 2 Feb 2025 14:34:54 -0600 Subject: [PATCH 6/9] made the thresholds files more readable --- MIDAS/src/finite-state-machines/fsm.cpp | 84 +++++----- MIDAS/src/finite-state-machines/fsm.h | 1 - MIDAS/src/finite-state-machines/sg1-4.h | 204 +++++++++++------------- 3 files changed, 136 insertions(+), 153 deletions(-) diff --git a/MIDAS/src/finite-state-machines/fsm.cpp b/MIDAS/src/finite-state-machines/fsm.cpp index 054e7009..6f2d1ad2 100644 --- a/MIDAS/src/finite-state-machines/fsm.cpp +++ b/MIDAS/src/finite-state-machines/fsm.cpp @@ -1,6 +1,8 @@ #include #include "fsm.h" +#include "sg1-4.h" + // helper functions @@ -66,7 +68,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate) { switch (state) { case FSMState::STATE_IDLE: // once a significant amount of acceleration is detected change states - if (state_estimate.acceleration > sustainer_idle_to_first_boost_acceleration_threshold) { + if (state_estimate.acceleration > SustainerThresholds::Idle::to_first_boost_acceleration_threshold) { launch_time = current_time; state = FSMState::STATE_FIRST_BOOST; } @@ -75,13 +77,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate) { 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 < SustainerThresholds::Idle::to_first_boost_acceleration_threshold) && ((current_time - launch_time) < SustainerThresholds::Idle::to_first_boost_time_threshold)) { state = FSMState::STATE_IDLE; break; } - // once acceleartion decreases to a the threshold go on the next state - if (state_estimate.acceleration < sustainer_coast_detection_acceleration_threshold) { + // once acceleration decreases to a the threshold go on the next state + if (state_estimate.acceleration < SustainerThresholds::Coast::detection_acceleration_threshold) { burnout_time = current_time; state = FSMState::STATE_BURNOUT; } @@ -89,13 +91,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate) { 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_first_boost_to_burnout_time_threshold)) { + if ((state_estimate.acceleration >= SustainerThresholds::Coast::detection_acceleration_threshold) && ((current_time - burnout_time) < SustainerThresholds::Burnout::to_first_boost_time_threshold)) { state = FSMState::STATE_FIRST_BOOST; break; } // if in burnout for long enough then go on to the next state (time transition) - if ((current_time - burnout_time) > sustainer_first_boost_to_burnout_time_threshold) { + if ((current_time - burnout_time) > SustainerThresholds::Burnout::to_first_boost_time_threshold) { sustainer_ignition_time = current_time; state = FSMState::STATE_SUSTAINER_IGNITION; } @@ -103,14 +105,14 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate) { case FSMState::STATE_SUSTAINER_IGNITION: // another time transition into coast after a certain amount of time - if ((current_time - sustainer_ignition_time) > sustainer_ignition_to_coast_timer_threshold) { + if ((current_time - sustainer_ignition_time) > SustainerThresholds::Ignition::to_coast_timer_threshold) { coast_time = current_time; state = FSMState::STATE_COAST; break; } // once a high enough acceleration is detected then go to next state - if (state_estimate.acceleration > sustainer_ignition_to_second_boost_acceleration_threshold) { + if (state_estimate.acceleration > SustainerThresholds::Ignition::to_second_boost_acceleration_threshold) { second_boost_time = current_time; state = FSMState::STATE_SECOND_BOOST; } @@ -118,14 +120,14 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate) { break; case FSMState::STATE_SECOND_BOOST: - // 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)) { + // if high acceleration is too brief then return to previous state + if ((state_estimate.acceleration < SustainerThresholds::Ignition::to_second_boost_acceleration_threshold) && ((current_time - second_boost_time) < SustainerThresholds::Ignition::to_second_boost_time_threshold)) { state = FSMState::STATE_SUSTAINER_IGNITION; break; } // if low acceleration detected go to next state - if (state_estimate.acceleration < sustainer_coast_detection_acceleration_threshold) { + if (state_estimate.acceleration < SustainerThresholds::Coast::detection_acceleration_threshold) { coast_time = current_time; state = FSMState::STATE_COAST; } @@ -133,13 +135,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate) { case FSMState::STATE_COAST: // 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)) { + if ((state_estimate.acceleration > SustainerThresholds::Coast::detection_acceleration_threshold) && ((current_time - coast_time) < SustainerThresholds::SecondBoost::to_coast_time_threshold)) { state = FSMState::STATE_SECOND_BOOST; break; } // if speed slows down enough then go on to the next stage - if (state_estimate.vertical_speed <= sustainer_coast_to_apogee_vertical_speed_threshold) { + if (state_estimate.vertical_speed <= SustainerThresholds::Coast::to_apogee_vertical_speed_threshold) { apogee_time = current_time; state = FSMState::STATE_APOGEE; } @@ -147,13 +149,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate) { case FSMState::STATE_APOGEE: // 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)) { + if ((state_estimate.vertical_speed) > SustainerThresholds::Apogee::backto_coast_vertical_speed_threshold && ((current_time - apogee_time) < SustainerThresholds::Apogee::check_threshold)) { state = FSMState::STATE_COAST; break; } // transition to next state after a certain amount of time - if ((current_time - apogee_time) > sustainer_apogee_timer_threshold) { + if ((current_time - apogee_time) > SustainerThresholds::Apogee::timer_threshold) { drogue_time = current_time; state = FSMState::STATE_DROGUE_DEPLOY; } @@ -161,13 +163,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate) { case FSMState::STATE_DROGUE_DEPLOY: // if detected a sharp change in jerk then go to next state - if (abs(state_estimate.jerk) < sustainer_drogue_jerk_threshold) { + if (abs(state_estimate.jerk) < SustainerThresholds::Drogue::jerk_threshold) { 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) { + // if no transition after a certain amount of time then just move on to next state + if ((current_time - drogue_time) > SustainerThresholds::Drogue::timer_threshold) { state = FSMState::STATE_DROGUE; } @@ -175,7 +177,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate) { case FSMState::STATE_DROGUE: // if altitude low enough then next state - if (state_estimate.altitude <= sustainer_main_deploy_altitude_threshold) { + if (state_estimate.altitude <= SustainerThresholds::Main::deploy_altitude_threshold) { state = FSMState::STATE_MAIN_DEPLOY; main_time = current_time; } @@ -183,20 +185,20 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate) { case FSMState::STATE_MAIN_DEPLOY: // if detected a sharp change in jerk then go to the next state - if (abs(state_estimate.jerk) < sustainer_main_jerk_threshold) { + if (abs(state_estimate.jerk) < SustainerThresholds::Main::jerk_threshold) { state = FSMState::STATE_MAIN; break; } - // 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) { + // if no transition after a certain amount of time then just move on to next state + if ((current_time - main_time) > SustainerThresholds::Main::to_main_deploy_timer_threshold) { state = FSMState::STATE_MAIN; } break; case FSMState::STATE_MAIN: // if slowed down enough then go on to the next state - if (abs(state_estimate.vertical_speed) <= sustainer_landed_vertical_speed_threshold) { + if (abs(state_estimate.vertical_speed) <= SustainerThresholds::Landed::vertical_speed_threshold) { landed_time = current_time; state = FSMState::STATE_LANDED; } @@ -204,7 +206,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate) { case FSMState::STATE_LANDED: // if the slow speed was too brief then return to previous state - if ((abs(state_estimate.vertical_speed) > sustainer_landed_vertical_speed_threshold) && ((current_time - landed_time) > sustainer_landed_timer_threshold)) { + if ((abs(state_estimate.vertical_speed) > SustainerThresholds::Landed::vertical_speed_threshold) && ((current_time - landed_time) > SustainerThresholds::Landed::timer_threshold)) { state = FSMState::STATE_MAIN; } break; @@ -221,7 +223,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate) { switch (state) { case FSMState::STATE_IDLE: - if (state_estimate.acceleration > booster_idle_to_first_boost_acceleration_threshold) { + if (state_estimate.acceleration > BoosterThresholds::Idle::to_first_boost_acceleration_threshold) { launch_time = current_time; state = FSMState::STATE_FIRST_BOOST; } @@ -229,11 +231,11 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate) { break; 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)) { + if ((state_estimate.acceleration < BoosterThresholds::Idle::to_first_boost_acceleration_threshold) && ((current_time - launch_time) < BoosterThresholds::Idle::to_first_boost_time_threshold)) { state = FSMState::STATE_IDLE; break; } - if (state_estimate.acceleration < booster_coast_detection_acceleration_threshold) { + if (state_estimate.acceleration < BoosterThresholds::Coast::detection_acceleration_threshold) { burnout_time = current_time; state = FSMState::STATE_BURNOUT; } @@ -241,86 +243,86 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate) { break; case FSMState::STATE_BURNOUT: - if ((state_estimate.acceleration >= booster_coast_detection_acceleration_threshold) && ((current_time - burnout_time) < booster_first_boost_to_burnout_time_threshold)) { + if ((state_estimate.acceleration >= BoosterThresholds::Coast::detection_acceleration_threshold) && ((current_time - burnout_time) < BoosterThresholds::Burnout::to_first_boost_time_threshold)) { state = FSMState::STATE_FIRST_BOOST; break; } - if ((current_time - burnout_time) > booster_first_boost_to_burnout_time_threshold) { + if ((current_time - burnout_time) > BoosterThresholds::Burnout::to_first_boost_time_threshold) { first_separation_time = current_time; state = FSMState::STATE_FIRST_SEPARATION; } break; case FSMState::STATE_FIRST_SEPARATION: - if (abs(state_estimate.jerk) < booster_first_separation_jerk_threshold) { + if (abs(state_estimate.jerk) < BoosterThresholds::FirstSeparation::jerk_threshold) { state = FSMState::STATE_COAST; break; } - if ((current_time - first_separation_time) > booster_first_seperation_time_threshold) { + if ((current_time - first_separation_time) > BoosterThresholds::FirstSeparation::time_threshold) { state = FSMState::STATE_COAST; } break; case FSMState::STATE_COAST: - if (state_estimate.vertical_speed <= booster_coast_to_apogee_vertical_speed_threshold) { + if (state_estimate.vertical_speed <= BoosterThresholds::Coast::to_apogee_vertical_speed_threshold) { apogee_time = current_time; state = FSMState::STATE_APOGEE; } 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)) { + if (state_estimate.vertical_speed > BoosterThresholds::Coast::to_apogee_vertical_speed_threshold && ((current_time - apogee_time) < BoosterThresholds::Apogee::check_threshold)) { state = FSMState::STATE_COAST; break; } - if ((current_time - apogee_time) > booster_apogee_timer_threshold) { + if ((current_time - apogee_time) > BoosterThresholds::Apogee::timer_threshold) { drogue_time = current_time; state = FSMState::STATE_DROGUE_DEPLOY; } break; case FSMState::STATE_DROGUE_DEPLOY: - if (abs(state_estimate.jerk) < booster_drogue_jerk_threshold) { + if (abs(state_estimate.jerk) < BoosterThresholds::Drogue::jerk_threshold) { state = FSMState::STATE_DROGUE; break; } - if ((current_time - drogue_time) > booster_drogue_timer_threshold) { + if ((current_time - drogue_time) > BoosterThresholds::Drogue::timer_threshold) { state = FSMState::STATE_DROGUE; } break; case FSMState::STATE_DROGUE: - if (state_estimate.altitude <= booster_main_deploy_altitude_threshold) { + if (state_estimate.altitude <= BoosterThresholds::Main::deploy_altitude_threshold) { state = FSMState::STATE_MAIN_DEPLOY; main_time = current_time; } break; case FSMState::STATE_MAIN_DEPLOY: - if (abs(state_estimate.jerk) < booster_main_jerk_threshold) { + if (abs(state_estimate.jerk) < BoosterThresholds::Main::jerk_threshold) { state = FSMState::STATE_MAIN; break; } - if ((current_time - main_time) > booster_main_to_main_deploy_timer_threshold) { + if ((current_time - main_time) > BoosterThresholds::Main::to_main_deploy_timer_threshold) { state = FSMState::STATE_MAIN; } break; case FSMState::STATE_MAIN: - if (abs(state_estimate.vertical_speed) <= booster_landed_vertical_speed_threshold) { + if (abs(state_estimate.vertical_speed) <= BoosterThresholds::Landed::vertical_speed_threshold) { landed_time = current_time; state = FSMState::STATE_LANDED; } break; case FSMState::STATE_LANDED: - if ((abs(state_estimate.vertical_speed) > booster_landed_vertical_speed_threshold) && ((current_time - landed_time) > booster_landed_timer_threshold)) { + if ((abs(state_estimate.vertical_speed) > BoosterThresholds::Landed::vertical_speed_threshold) && ((current_time - landed_time) > BoosterThresholds::Landed::timer_threshold)) { state = FSMState::STATE_MAIN; } break; diff --git a/MIDAS/src/finite-state-machines/fsm.h b/MIDAS/src/finite-state-machines/fsm.h index c370dee0..ad4dd8fe 100644 --- a/MIDAS/src/finite-state-machines/fsm.h +++ b/MIDAS/src/finite-state-machines/fsm.h @@ -6,7 +6,6 @@ #include "fsm_states.h" -#include "sg1-4.h" #include "sensor_data.h" #include "Buffer.h" diff --git a/MIDAS/src/finite-state-machines/sg1-4.h b/MIDAS/src/finite-state-machines/sg1-4.h index 74b5ca9a..f4c32d22 100644 --- a/MIDAS/src/finite-state-machines/sg1-4.h +++ b/MIDAS/src/finite-state-machines/sg1-4.h @@ -3,118 +3,100 @@ // ---------------------------------- // SECOND STAGE THRESHOLDS // ---------------------------------- - -// Transition to FIRST_BOOST if acceleration is greater than this -#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 -#define sustainer_ignition_to_second_boost_acceleration_threshold 3 - -// 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 -#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) -#define sustainer_apogee_backto_coast_vertical_speed_threshold 10 - -// Revert back to COAST if apogee was too brief -#define sustainer_apogee_check_threshold 1000 - -// Move on to DROGUE_DEPLOT after being in apogee for this amount of time -#define sustainer_apogee_timer_threshold 1000 - -// Move on to DROGUE after a second of reaching apogee -#define sustainer_drogue_timer_threshold 3000 - -// Move on to MAIN after passing this amount of time -#define sustainer_main_to_main_deploy_timer_threshold 3000 - -// Height required to deploy the main parachutes -#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 -#define sustainer_ignition_to_coast_timer_threshold 5000 - -// Revert back to main if the landed was too short -#define sustainer_landed_timer_threshold 5000 - -// Return state to FIRST_BOOST if not in BURNOUT for this amount of time (ms) -#define sustainer_first_boost_to_burnout_time_threshold 1000 - -// Transition to LANDED from MAIN if vertical speed is less than this threshold -#define sustainer_landed_vertical_speed_threshold 3 - -// Stores a small jerk value -#define sustainer_drogue_jerk_threshold 200 - -// Stores a small jerk value -#define sustainer_main_jerk_threshold 300 - +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 // ---------------------------------- - -// Transition to FIRST_BOOST if acceleration is greater than this -#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 -#define booster_coast_to_apogee_vertical_speed_threshold 20 - -// Revert back to COAST if apogee was too brief -#define booster_apogee_check_threshold 1000 - -// Move on to DROGUE_DEPLOT after being in apogee for this amount of time -#define booster_apogee_timer_threshold 1000 - -// Move on to DROGUE after a second of reaching apogee -#define booster_drogue_timer_threshold 3000 - -// Move on to MAIN after passing this amount of time -#define booster_main_to_main_deploy_timer_threshold 3000 - -// Height required to deploy the main parachutes -#define booster_main_deploy_altitude_threshold 3000 -// 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 -#define booster_ignition_to_coast_timer_threshold 5000 - -// Revert back to main if the landed was too short -#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 -#define booster_landed_vertical_speed_threshold 4 - -// Stores a small jerk value -#define booster_first_separation_jerk_threshold 300 - -// Stores a small jerk value -#define booster_drogue_jerk_threshold 200 - -// Stores a small jerk value -#define booster_main_jerk_threshold 300 +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; +}; From 683f262fcca648788f7055c0d4f49aab5c99b856 Mon Sep 17 00:00:00 2001 From: Aadityavoru Date: Sun, 2 Feb 2025 14:38:00 -0600 Subject: [PATCH 7/9] more clear directory structure --- MIDAS/src/finite-state-machines/fsm.cpp | 2 +- .../{sg1-4.h => thresholds/stargazer1-4.h} | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename MIDAS/src/finite-state-machines/{sg1-4.h => thresholds/stargazer1-4.h} (100%) diff --git a/MIDAS/src/finite-state-machines/fsm.cpp b/MIDAS/src/finite-state-machines/fsm.cpp index 6f2d1ad2..a926c374 100644 --- a/MIDAS/src/finite-state-machines/fsm.cpp +++ b/MIDAS/src/finite-state-machines/fsm.cpp @@ -1,7 +1,7 @@ #include #include "fsm.h" -#include "sg1-4.h" +#include "thresholds/stargazer1-4.h" // helper functions diff --git a/MIDAS/src/finite-state-machines/sg1-4.h b/MIDAS/src/finite-state-machines/thresholds/stargazer1-4.h similarity index 100% rename from MIDAS/src/finite-state-machines/sg1-4.h rename to MIDAS/src/finite-state-machines/thresholds/stargazer1-4.h From da00033f03fcbde9eab2c4b792ce2c0bfd205d22 Mon Sep 17 00:00:00 2001 From: Aadityavoru Date: Sun, 2 Feb 2025 14:50:48 -0600 Subject: [PATCH 8/9] ef --- MIDAS/src/finite-state-machines/thresholds.h | 147 ------------------- 1 file changed, 147 deletions(-) delete mode 100644 MIDAS/src/finite-state-machines/thresholds.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 From e2e52de7de28cb100a4176de84bb493e70877b44 Mon Sep 17 00:00:00 2001 From: Aadityavoru Date: Sun, 2 Feb 2025 15:57:00 -0600 Subject: [PATCH 9/9] fixed --- MIDAS/src/finite-state-machines/fsm.cpp | 89 +++++++++++++++---------- 1 file changed, 52 insertions(+), 37 deletions(-) diff --git a/MIDAS/src/finite-state-machines/fsm.cpp b/MIDAS/src/finite-state-machines/fsm.cpp index b1b4abae..e72e572f 100644 --- a/MIDAS/src/finite-state-machines/fsm.cpp +++ b/MIDAS/src/finite-state-machines/fsm.cpp @@ -1,11 +1,16 @@ #include #include "fsm.h" -#include "thresholds/stargazer1-4.h" - - -// helper functions +#include "thresholds.h" +/** + * @brief Helper to calculate the average value of a buffered sensor data + * + * @param sensor Buffered sensor struct + * @param get_item Lambda get function + * + * @return Average value +*/ template double sensor_average(BufferedSensorData& sensor, double (* get_item)(T&)) { auto arr = sensor.template getBufferRecent(); @@ -16,6 +21,14 @@ double sensor_average(BufferedSensorData& sensor, double (* get_item)( return sum / count; } +/** + * @brief Helper to calculate the derivative over a buffered sensor data + * + * @param sensor Buffered sensor struct + * @param get_item Lambda get function + * + * @return Derivative +*/ template double sensor_derivative(BufferedSensorData& sensor, double (* get_item)(T&)) { auto arr = sensor.template getBufferRecent(); @@ -42,7 +55,9 @@ double sensor_derivative(BufferedSensorData& sensor, double (* get_ite return (second_average - first_average) / (second_average_time - first_average_time); } - +/** + * @brief Populates StateEstimate struct with the correct values for accel, alt, jerk, and speed +*/ StateEstimate::StateEstimate(RocketData& state) { acceleration = sensor_average(state.high_g, [](HighGData& data) { return (double) data.ax; @@ -129,7 +144,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla } // once a significant amount of acceleration is detected change states - if (state_estimate.acceleration > SustainerThresholds::Idle::to_first_boost_acceleration_threshold) { + if (state_estimate.acceleration > sustainer_idle_to_first_boost_acceleration_threshold) { launch_time = current_time; state = FSMState::STATE_FIRST_BOOST; } @@ -138,13 +153,13 @@ 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 < SustainerThresholds::Idle::to_first_boost_acceleration_threshold) && ((current_time - launch_time) < SustainerThresholds::Idle::to_first_boost_time_threshold)) { + 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; break; } - // once acceleration decreases to a the threshold go on the next state - if (state_estimate.acceleration < SustainerThresholds::Coast::detection_acceleration_threshold) { + // 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; state = FSMState::STATE_BURNOUT; } @@ -167,14 +182,14 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla case FSMState::STATE_SUSTAINER_IGNITION: // This state probably does not need a pyro lockout, since we have a back-transition from STATE_SECOND_BOOST // another time transition into coast after a certain amount of time - if ((current_time - sustainer_ignition_time) > SustainerThresholds::Ignition::to_coast_timer_threshold) { + if ((current_time - sustainer_ignition_time) > sustainer_ignition_to_coast_timer_threshold) { coast_time = current_time; state = FSMState::STATE_COAST; break; } // once a high enough acceleration is detected then go to next state - if (state_estimate.acceleration > SustainerThresholds::Ignition::to_second_boost_acceleration_threshold) { + if (state_estimate.acceleration > sustainer_ignition_to_second_boost_acceleration_threshold) { second_boost_time = current_time; state = FSMState::STATE_SECOND_BOOST; } @@ -182,14 +197,14 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla break; case FSMState::STATE_SECOND_BOOST: - // if high acceleration is too brief then return to previous state - if ((state_estimate.acceleration < SustainerThresholds::Ignition::to_second_boost_acceleration_threshold) && ((current_time - second_boost_time) < SustainerThresholds::Ignition::to_second_boost_time_threshold)) { + // 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; break; } // if low acceleration detected go to next state - if (state_estimate.acceleration < SustainerThresholds::Coast::detection_acceleration_threshold) { + if (state_estimate.acceleration < sustainer_coast_detection_acceleration_threshold) { coast_time = current_time; state = FSMState::STATE_COAST; } @@ -197,13 +212,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla case FSMState::STATE_COAST: // if the low acceleration detected was too brief then return to previous state - if ((state_estimate.acceleration > SustainerThresholds::Coast::detection_acceleration_threshold) && ((current_time - coast_time) < SustainerThresholds::SecondBoost::to_coast_time_threshold)) { + 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; break; } // if speed slows down enough then go on to the next stage - if (state_estimate.vertical_speed <= SustainerThresholds::Coast::to_apogee_vertical_speed_threshold) { + if (state_estimate.vertical_speed <= sustainer_coast_to_apogee_vertical_speed_threshold) { apogee_time = current_time; state = FSMState::STATE_APOGEE; } @@ -211,13 +226,13 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla case FSMState::STATE_APOGEE: // if the slow speed was too brief then return to previous state - if ((state_estimate.vertical_speed) > SustainerThresholds::Apogee::backto_coast_vertical_speed_threshold && ((current_time - apogee_time) < SustainerThresholds::Apogee::check_threshold)) { + if ((state_estimate.vertical_speed) > sustainer_apogee_backto_coast_vertical_speed_threshold && ((current_time - apogee_time) < sustainer_apogee_check_threshold)) { state = FSMState::STATE_COAST; break; } // transition to next state after a certain amount of time - if ((current_time - apogee_time) > SustainerThresholds::Apogee::timer_threshold) { + if ((current_time - apogee_time) > sustainer_apogee_timer_threshold) { drogue_time = current_time; state = FSMState::STATE_DROGUE_DEPLOY; } @@ -235,8 +250,8 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla break; } - // if no transition after a certain amount of time then just move on to next state - if ((current_time - drogue_time) > SustainerThresholds::Drogue::timer_threshold) { + // 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) { state = FSMState::STATE_DROGUE; } @@ -244,7 +259,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla case FSMState::STATE_DROGUE: // if altitude low enough then next state - if (state_estimate.altitude <= SustainerThresholds::Main::deploy_altitude_threshold) { + if (state_estimate.altitude <= sustainer_main_deploy_altitude_threshold) { state = FSMState::STATE_MAIN_DEPLOY; main_time = current_time; } @@ -263,8 +278,8 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla break; } - // if no transition after a certain amount of time then just move on to next state - if ((current_time - main_time) > SustainerThresholds::Main::to_main_deploy_timer_threshold) { + // 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; } break; @@ -284,7 +299,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) > SustainerThresholds::Landed::vertical_speed_threshold) && ((current_time - landed_time) > SustainerThresholds::Landed::timer_threshold)) { + if ((abs(state_estimate.vertical_speed) > sustainer_landed_vertical_speed_threshold) && ((current_time - landed_time) > sustainer_landed_timer_threshold)) { state = FSMState::STATE_MAIN; } break; @@ -369,11 +384,11 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla break; case FSMState::STATE_FIRST_BOOST: - if ((state_estimate.acceleration < BoosterThresholds::Idle::to_first_boost_acceleration_threshold) && ((current_time - launch_time) < BoosterThresholds::Idle::to_first_boost_time_threshold)) { + 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; break; } - if (state_estimate.acceleration < BoosterThresholds::Coast::detection_acceleration_threshold) { + if (state_estimate.acceleration < booster_coast_detection_acceleration_threshold) { burnout_time = current_time; state = FSMState::STATE_BURNOUT; } @@ -381,43 +396,43 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla break; case FSMState::STATE_BURNOUT: - if ((state_estimate.acceleration >= BoosterThresholds::Coast::detection_acceleration_threshold) && ((current_time - burnout_time) < BoosterThresholds::Burnout::to_first_boost_time_threshold)) { + 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; break; } - if ((current_time - burnout_time) > BoosterThresholds::Burnout::to_first_boost_time_threshold) { + if ((current_time - burnout_time) > booster_first_boost_to_burnout_time_threshold) { first_separation_time = current_time; state = FSMState::STATE_FIRST_SEPARATION; } break; case FSMState::STATE_FIRST_SEPARATION: - if (abs(state_estimate.jerk) < BoosterThresholds::FirstSeparation::jerk_threshold) { + if (abs(state_estimate.jerk) < booster_first_separation_jerk_threshold) { state = FSMState::STATE_COAST; break; } - if ((current_time - first_separation_time) > BoosterThresholds::FirstSeparation::time_threshold) { + if ((current_time - first_separation_time) > booster_first_seperation_time_threshold) { state = FSMState::STATE_COAST; } break; case FSMState::STATE_COAST: - if (state_estimate.vertical_speed <= BoosterThresholds::Coast::to_apogee_vertical_speed_threshold) { + if (state_estimate.vertical_speed <= booster_coast_to_apogee_vertical_speed_threshold) { apogee_time = current_time; state = FSMState::STATE_APOGEE; } break; case FSMState::STATE_APOGEE: - if (state_estimate.vertical_speed > BoosterThresholds::Coast::to_apogee_vertical_speed_threshold && ((current_time - apogee_time) < BoosterThresholds::Apogee::check_threshold)) { + if (state_estimate.vertical_speed > booster_coast_to_apogee_vertical_speed_threshold && ((current_time - apogee_time) < booster_apogee_check_threshold)) { state = FSMState::STATE_COAST; break; } - if ((current_time - apogee_time) > BoosterThresholds::Apogee::timer_threshold) { + if ((current_time - apogee_time) > booster_apogee_timer_threshold) { drogue_time = current_time; state = FSMState::STATE_DROGUE_DEPLOY; } @@ -434,14 +449,14 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla state = FSMState::STATE_DROGUE; break; } - if ((current_time - drogue_time) > BoosterThresholds::Drogue::timer_threshold) { + if ((current_time - drogue_time) > booster_drogue_timer_threshold) { state = FSMState::STATE_DROGUE; } break; case FSMState::STATE_DROGUE: - if (state_estimate.altitude <= BoosterThresholds::Main::deploy_altitude_threshold) { + if (state_estimate.altitude <= booster_main_deploy_altitude_threshold) { state = FSMState::STATE_MAIN_DEPLOY; main_time = current_time; } @@ -460,7 +475,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla break; } - if ((current_time - main_time) > BoosterThresholds::Main::to_main_deploy_timer_threshold) { + if ((current_time - main_time) > booster_main_to_main_deploy_timer_threshold) { state = FSMState::STATE_MAIN; } break; @@ -486,4 +501,4 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla } return state; } -#endif +#endif \ No newline at end of file