FSM Silsim CI and Data Viewer

.github/workflows/fsm_test.yml

@@ -0,0 +1,116 @@
+name: CI -- FSM SILSIM Tests
+
+on:
+  push:
+    branches:
+      - '**'
+    paths:
+      - 'MIDAS/src/finite-state-machines/**'
+      - 'MIDAS/test/fsm_test/**'
+      - 'MIDAS/src/rocket_state.h'
+      - 'MIDAS/src/command_flags.h'
+  pull_request:
+    paths:
+      - 'MIDAS/src/finite-state-machines/**'
+      - 'MIDAS/test/fsm_test/**'
+      - 'MIDAS/src/rocket_state.h'
+      - 'MIDAS/src/command_flags.h'
+  workflow_dispatch:
+
+jobs:
+  fsm_test:
+    name: FSM SILSIM Tests
+    runs-on: ubuntu-latest
+    defaults:
+      run:
+        working-directory: MIDAS
+    steps:
+      - uses: actions/checkout@v3
+
+      - name: Cache pip
+        uses: actions/cache@v3
+        with:
+          path: ~/.cache/pip
+          key: ${{ runner.os }}-pip-${{ hashFiles('**/requirements.txt') }}
+          restore-keys: ${{ runner.os }}-pip-
+
+      - name: Cache PlatformIO
+        uses: actions/cache@v3
+        with:
+          path: ~/.platformio
+          key: ${{ runner.os }}-pio-${{ hashFiles('**/platformio.ini') }}
+          restore-keys: ${{ runner.os }}-pio-
+
+      - name: Set up Python
+        uses: actions/setup-python@v4
+        with:
+          python-version: '3.11'
+
+      - name: Install dependencies
+        run: |
+          python -m pip install --upgrade pip
+          pip install --upgrade platformio pandas matplotlib
+
+      - name: Build FSM simulator
+        run: pio run -e fsm_sim
+
+      - name: Run tests
+        id: tests
+        run: |
+          mkdir -p test/fsm_test/plots
+          PASS=0
+          FAIL=0
+          FAILED=""
+
+          for tc in test/fsm_test/test_cases/*.json; do
+            name=$(basename "$tc" .json)
+            echo "::group::Test: $name"
+
+            if python test/fsm_test/tester/run_test.py "$tc" \
+              --save-plot "test/fsm_test/plots/${name}.png"; then
+              PASS=$((PASS + 1))
+            else
+              FAIL=$((FAIL + 1))
+              FAILED="${FAILED}${name}, "
+            fi
+            echo "::endgroup::"
+          done
+
+          TOTAL=$((PASS + FAIL))
+          if [ $FAIL -eq 0 ]; then
+            SUMMARY="checkmark"
+          else
+            SUMMARY="failed: ${FAILED%, }"
+          fi
+          echo "pass=$PASS" >> "$GITHUB_OUTPUT"
+          echo "total=$TOTAL" >> "$GITHUB_OUTPUT"
+          echo "summary=$SUMMARY" >> "$GITHUB_OUTPUT"
+
+          [ $FAIL -eq 0 ]
+
+      - name: Upload plots
+        if: always()
+        uses: actions/upload-artifact@v4
+        with:
+          name: fsm-silsim-plots
+          path: MIDAS/test/fsm_test/plots/*.png
+
+      - name: Find existing comment
+        if: always() && github.event_name == 'pull_request'
+        uses: peter-evans/find-comment@v3
+        id: find-comment
+        with:
+          issue-number: ${{ github.event.pull_request.number }}
+          comment-author: 'github-actions[bot]'
+          body-includes: '<!-- fsm-silsim-results -->'
+
+      - name: Comment on PR
+        if: always() && github.event_name == 'pull_request'
+        uses: peter-evans/create-or-update-comment@v4
+        with:
+          comment-id: ${{ steps.find-comment.outputs.comment-id }}
+          issue-number: ${{ github.event.pull_request.number }}
+          edit-mode: replace
+          body: |
+            <!-- fsm-silsim-results -->
+            **FSM SILSIM:** ${{ steps.tests.outputs.pass }}/${{ steps.tests.outputs.total }} passed ${{ steps.tests.outputs.summary == 'checkmark' && '&#10003;' || format('({0})', steps.tests.outputs.summary) }} — [plots](${{ github.server_url }}/${{ github.repository }}/actions/runs/${{ github.run_id }})

MIDAS/.gitignore

@@ -4,4 +4,5 @@
 /src/esp_eeprom_checksum.h
 **/.DS_Store
 *.launch
-*.pyc
+*.pyc
+/test/fsm_test/output/*

MIDAS/platformio.ini

@@ -8,6 +8,9 @@
 ; Please visit documentation for the other options and examples
 ; https://docs.platformio.org/page/projectconf.html
 
+[env]
+test_ignore = fsm_test
+
 [env:mcu_main_sustainer]
 platform = espressif32@6.7.0
 board = esp32-s3-devkitc-1
@@ -97,6 +100,24 @@ build_unflags =
 	-std=gnu++11
 lib_deps = sparkfun/SparkFun MMC5983MA Magnetometer Arduino Library@^1.1.4
 
+[env:fsm_sim]
+platform = native
+build_type = debug
+build_flags =
+    -DSILSIM
+    -DFSM_SIMULATOR
+    -std=gnu++2a
+    -Isrc
+    -Isrc/finite-state-machines
+    -Ilib/CRCpp
+    -Itest/fsm_test/src
+build_unflags =
+    -std=gnu++11
+build_src_filter =
+    -<*>
+    +<finite-state-machines/fsm.cpp>
+extra_scripts = pre:test/fsm_test/srcinc.py
+
 [env:mcu_silsim_booster]
 platform = native
 build_type = debug

MIDAS/src/command_flags.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#include <cstdint>
+
+/**
+ * @struct CommandFlags
+ *
+ * @brief Stores the status of commands from telemetry as boolean flags, commands are set whenever the corresponding telemetry command comes in.
+ * Works in both directions, say to toggle states based on FSM transitions
+ */
+struct CommandFlags {
+    bool should_reset_kf = false;               // CommandType::RESET_KF
+    bool should_transition_safe = false;        // CommandType::SWITCH_TO_SAFE
+    bool should_transition_armed = false;       // CommandType::SWITCH_TO_ARMED
+    bool should_transition_pyro_test = false;   // CommandType::SWITCH_TO_PYRO_TEST
+    bool should_fire_pyro_a = false;            // CommandType::FIRE_PYRO_A
+    bool should_fire_pyro_b = false;            // CommandType::FIRE_PYRO_B
+    bool should_fire_pyro_c = false;            // CommandType::FIRE_PYRO_C
+    bool should_fire_pyro_d = false;            // CommandType::FIRE_PYRO_D
+    // FSM Transition commands
+    bool FSM_should_set_cam_feed_cam1 = false;  // Triggered at launch (IDLE --> FIRST_BOOST)
+    bool FSM_should_power_save = false;         // Triggered after 60 seconds in LANDED state.
+    bool FSM_should_swap_camera_feed = false;   // Triggered COAST --> DROGUE
+};
+
+struct MErrorFlags {
+    uint8_t fsm_crc_err  : 1 = 0;
+    uint8_t reserved     : 7 = 0;
+};

MIDAS/src/finite-state-machines/fsm.cpp

@@ -2,14 +2,19 @@
 
 #include "fsm.h"
 #include "thresholds.h"
+
+#ifdef FSM_SIMULATOR
+#include "sensor_data.h"
+#include "command_flags.h"
+#else
 #include "rocket_state.h"
 
 /**
  * @brief Helper to calculate the average value of a buffered sensor data
- * 
+ *
  * @param sensor Buffered sensor struct
- * @param get_item Lambda get function 
- * 
+ * @param get_item Lambda get function
+ *
  * @return Average value
 */
 template<typename T, size_t count>
@@ -24,10 +29,10 @@ double sensor_average(BufferedSensorData<T, count>& sensor, double (* get_item)(
 
 /**
  * @brief Helper to calculate the derivative over a buffered sensor data
- * 
+ *
  * @param sensor Buffered sensor struct
- * @param get_item Lambda get function 
- * 
+ * @param get_item Lambda get function
+ *
  * @return Derivative
 */
 template<typename T, size_t count>
@@ -73,6 +78,7 @@ StateEstimate::StateEstimate(RocketData& state) {
         return (double) data.altitude;
     });
 }
+#endif
 
 bool FSM::set_cfg(const FSMConfiguration& new_cfg) {
     // Check if the new config has a valid CRC.
@@ -208,6 +214,17 @@ FSMData FSM::tick_fsm(FSMTickData& fsm_data) {
             }
             // -- END FALSE BURNOUT DETECTION --
 
+            // -- BEGIN NEXT STAGE IGNITION DETECT --
+            // After burnout is confirmed, check for another motor ignition (multistage)
+            if(cur_state_lockin && state_estimate.acceleration > fsms_boost_xl) {
+                time_entered_cur_state = current_time;
+                cur_state_lockin = false;
+                apogee_detect_start = 0; // Reset apogee detection
+                state = FSMState::STATE_BOOST;
+                break;
+            }
+            // -- END NEXT STAGE IGNITION DETECT --
+
             // -- BEGIN APOGEE DETECT --
             // Condition 1: Vertical speed low
             bool apog_detect_low_speed = (state_estimate.vertical_speed <= fsms_apogee_detect_spd);
@@ -216,39 +233,26 @@ FSMData FSM::tick_fsm(FSMTickData& fsm_data) {
             bool apog_detect_cruise_lockout = (!config.thresholds.cruise_lockout_en || kf_data.velocity.vx <= fsms_cruise_lockout_spd);
             // Note: This evaluates to TRUE (no lockout) if the lockout is disabled, OR if the condition is met
 
-
             if (apog_detect_low_speed && apog_detect_cruise_lockout) {
-                // Begin apogee detect
-                time_entered_cur_state = current_time;
-                // Note: The apogee does not have a "lock in" feature, as it just transitions to DROGUE when the timer expires.
-                //       In order to preserve this state's "lock in" in case of erroneous apogee detection, the flag is not reset.
-                state = FSMState::STATE_APOGEE;
-
-                // And swap camera feed
-                commands.FSM_should_swap_camera_feed = true;
-            }
-            // -- END APOGEE DETECT --
-            break;
-        }
-
-        case FSMState::STATE_APOGEE: {
-            // We run the apogee detection algorithm above again. Read above for explanation
-            bool apog_detect_low_speed = (state_estimate.vertical_speed <= fsms_apogee_detect_spd);
-            bool apog_detect_cruise_lockout = (!config.thresholds.cruise_lockout_en || kf_data.velocity.vx <= fsms_cruise_lockout_spd);
+                // Start the consecutive timer if not already running
+                if (apogee_detect_start == 0) {
+                    apogee_detect_start = current_time;
+                }
 
-            // If either condition is not met, go back to COAST.
-            if (!apog_detect_low_speed || !apog_detect_cruise_lockout) {
-                state = FSMState::STATE_COAST;
-                // Note: We do not reset the lock in flag, as we assume the COAST state is locked in.
-                break;
-            }
+                // Transition to DROGUE only after conditions are met for fsms_apogee_lockin_t consecutive ms
+                if (current_time - apogee_detect_start >= fsms_apogee_lockin_t) {
+                    time_entered_cur_state = current_time;
+                    apogee_time = current_time;
+                    state = FSMState::STATE_DROGUE;
+                    apogee_detect_start = 0;
 
-            // If we are still in this state after the apogee lock in timer, go straight to DROGUE, do not pass GO.
-            if(current_time - time_entered_cur_state > fsms_apogee_lockin_t) {
-                time_entered_cur_state = current_time;
-                apogee_time = current_time;
-                state = FSMState::STATE_DROGUE;
+                    commands.FSM_should_swap_camera_feed = true;
+                }
+            } else {
+                // Conditions not met: reset the timer
+                apogee_detect_start = 0;
             }
+            // -- END APOGEE DETECT --
             break;
         }
 
@@ -262,16 +266,26 @@ FSMData FSM::tick_fsm(FSMTickData& fsm_data) {
             }
             break;
 
-        case FSMState::STATE_MAIN:
-            // If we detect very low vertical movement, assume we are LANDED.
-            // In case of deployments close to apogee, prevent transitions to LANDED for some time to allow systemt to settle into parachute descent.
-            // 200ms debounce timer to prevent ping-ponging between MAIN and LANDED.
-            if ((abs(state_estimate.vertical_speed) <= fsms_landed_detect_spd) && (current_time - apogee_time) > fsms_landed_t_lockout && (current_time - time_entered_cur_state) > 200) {
-                time_entered_cur_state = current_time;
-                cur_state_lockin = false; // Reset flag for landing state lock in logic
-                state = FSMState::STATE_LANDED;
+        case FSMState::STATE_MAIN: {
+            bool landed_speed = (abs(state_estimate.vertical_speed) <= fsms_landed_detect_spd);
+            bool landed_lockout_passed = (current_time - apogee_time) > fsms_landed_t_lockout;
+
+            if (landed_speed && landed_lockout_passed) {
+                if (landed_detect_start == 0) {
+                    landed_detect_start = current_time;
+                }
+
+                if (current_time - landed_detect_start >= fsms_landed_entry_t) {
+                    time_entered_cur_state = current_time;
+                    cur_state_lockin = false;
+                    state = FSMState::STATE_LANDED;
+                    landed_detect_start = 0;
+                }
+            } else {
+                landed_detect_start = 0;
             }
             break;
+        }
 
         case FSMState::STATE_LANDED:
             // Landing lock-in

MIDAS/src/finite-state-machines/fsm.h

@@ -31,6 +31,11 @@ struct StateEstimate {
     double vertical_speed;
 
     explicit StateEstimate(RocketData& state);
+
+#ifdef FSM_SIMULATOR
+    StateEstimate(double alt, double accel, double jrk, double vspd)
+        : altitude(alt), acceleration(accel), jerk(jrk), vertical_speed(vspd) {}
+#endif
 };
 
 struct FSMPyroAction {
@@ -50,14 +55,23 @@ struct FSMPyroAction {
         if (!enable) { return false; }
         if (fsm_state != fsm_trigger) { return false; }
         if(max_tilt != -1 && cur_tilt > max_tilt) { return false; }
-        if(after_motor != 0 && cur_motor <= after_motor) { return false; }
+        if(after_motor != 0 && cur_motor < after_motor) { return false; }
         if(launch_t_gt != -1 && cur_time_since_launch < launch_t_gt) { return false; }
         if(launch_t_lt != -1 && cur_time_since_launch > launch_t_lt) { return false; }
         if(vx_min != -1 && cur_vx < vx_min) { return false; }
         if(vx_max != -1 && cur_vx > vx_max) { return false; }
 
         return true;
     };
+
+    // Checks that conditions that- need to be met the entire "delay" time are met.
+    // If these conditions aren't met, the pyro event is reset without consuming it
+    bool soft_conditions_met(FSMState fsm_state) const {
+        if (!enable) { return false; }
+        if (fsm_state != fsm_trigger) { return false; }
+
+        return true;
+    }
 };
 
 struct FSMUserThresholds {
@@ -138,6 +152,8 @@ class FSM {
     double launch_time;
     double apogee_time;
     double time_entered_cur_state;
+    double apogee_detect_start = 0;  // (0 = not active)
+    double landed_detect_start = 0; // (0 = not active)
     bool cur_state_lockin;
 };
 

MIDAS/src/finite-state-machines/fsm_states.h

@@ -12,7 +12,6 @@ enum FSMState {
     STATE_ARMED,
     STATE_BOOST,
     STATE_COAST,
-    STATE_APOGEE,
     STATE_DROGUE,
     STATE_MAIN,
     STATE_LANDED,