Added state machine

.gitignore

@@ -1,2 +1,4 @@
 **/build/
-.DS_Store
+.DS_Store
+twister-out*
+.cache

apps/rockets/cloudburst/CMakeLists.txt

@@ -22,4 +22,23 @@ endif()
 find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})
 project(cloudburst)
 
-target_sources(app PRIVATE src/main.c src/data.c src/sensors/imu_thread.c src/logger_thread.c src/sensors/baro_thread.c)
+target_sources(app PRIVATE
+  src/main.c
+  src/data.c
+  src/sensors/imu_thread.c
+  src/logger_thread.c
+  src/sensors/baro_thread.c
+  src/state_machine/state_machine.c
+  src/state_machine/state_machine_common.c
+  src/state_machine/states/standby.c
+  src/state_machine/states/ascent.c
+  src/state_machine/states/mach_lock.c
+  src/state_machine/states/drogue_descent.c
+  src/state_machine/states/main_descent.c
+  src/state_machine/states/landed.c
+)
+
+target_include_directories(app PRIVATE
+  src
+  src/state_machine
+)

apps/rockets/cloudburst/prj.conf

@@ -10,6 +10,9 @@ CONFIG_SPI=y
 # Enable C++ support
 CONFIG_CPP=y
 
+# Enable SMF (state machine framework)
+CONFIG_SMF=y
+
 # Enable BMI08X sensor driver
 CONFIG_BMI08X=y
 CONFIG_BMI08X_ACCEL_TRIGGER_NONE=y
@@ -52,4 +55,4 @@ CONFIG_DEBUG_THREAD_INFO=y
 CONFIG_THREAD_MONITOR=y
 CONFIG_THREAD_NAME=y
 CONFIG_THREAD_STACK_INFO=n
-CONFIG_STACK_SENTINEL=y
+CONFIG_STACK_SENTINEL=y

apps/rockets/cloudburst/src/data.c

@@ -7,10 +7,12 @@ LOG_MODULE_REGISTER(data, LOG_LEVEL_INF);
 // Global instances
 struct imu_data g_imu_data;
 struct baro_data g_baro_data;
+struct state_data g_state_data;
 
 // Mutexes for thread safety
 K_MUTEX_DEFINE(imu_mutex);
 K_MUTEX_DEFINE(baro_mutex);
+K_MUTEX_DEFINE(state_mutex);
 
 // Setter functions
 void set_imu_data(const struct imu_data *src)
@@ -41,3 +43,17 @@ void get_baro_data(struct baro_data *dst)
     *dst = g_baro_data;
     k_mutex_unlock(&baro_mutex);
 }
+
+void set_state_data(const struct state_data *src)
+{
+    k_mutex_lock(&state_mutex, K_FOREVER);
+    g_state_data = *src;
+    k_mutex_unlock(&state_mutex);
+}
+
+void get_state_data(struct state_data *dst)
+{
+    k_mutex_lock(&state_mutex, K_FOREVER);
+    *dst = g_state_data;
+    k_mutex_unlock(&state_mutex);
+}

apps/rockets/cloudburst/src/data.h

@@ -3,6 +3,16 @@
 
 #include <stdint.h>
 
+typedef enum
+{
+    FLIGHT_STATE_STANDBY = 0,
+    FLIGHT_STATE_ASCENT,
+    FLIGHT_STATE_MACH_LOCK,
+    FLIGHT_STATE_DROGUE_DESCENT,
+    FLIGHT_STATE_MAIN_DESCENT,
+    FLIGHT_STATE_LANDED,
+} flight_state_id_t;
+
 // Data structures for sensor data
 struct imu_data
 {
@@ -36,10 +46,17 @@ struct baro_data
     int64_t timestamp;        // Timestamp in milliseconds
 };
 
+struct state_data
+{
+    flight_state_id_t state;
+    float ground_altitude;
+    int64_t timestamp;
+};
 
 // Global instances
 extern struct imu_data g_imu_data;
 extern struct baro_data g_baro_data;
+extern struct state_data g_state_data;
 
 // Getters and setters
 void set_imu_data(const struct imu_data *src);
@@ -48,4 +65,7 @@ void get_imu_data(struct imu_data *dst);
 void set_baro_data(const struct baro_data *src);
 void get_baro_data(struct baro_data *dst);
 
-#endif
+void set_state_data(const struct state_data *src);
+void get_state_data(struct state_data *dst);
+
+#endif

apps/rockets/cloudburst/src/log_format.h

@@ -10,6 +10,7 @@ struct log_frame
 
     struct imu_data imu;
     struct baro_data baro;
+    struct state_data state;
 };
 
 #endif

apps/rockets/cloudburst/src/logger_thread.c

@@ -60,7 +60,8 @@ static int write_csv_header(void) {
                          "Baro_Timestamp(ms),"
                          "Baro0_Pressure(Pa),Baro0_Temperature(C),Baro0_Altitude(m),Baro0_NIS,Baro0_Faults,Baro0_Healthy,"
                          "Baro1_Pressure(Pa),Baro1_Temperature(C),Baro1_Altitude(m),Baro1_NIS,Baro1_Faults,Baro1_Healthy,"
-                         "KF_Altitude(m),KF_AltVar,KF_Velocity(m/s),KF_VelVar\n";
+                         "KF_Altitude(m),KF_AltVar,KF_Velocity(m/s),KF_VelVar,"
+                         "State,State_Ground_Altitude(m),State_Timestamp(ms)\n";
     int ret;
 
     // Write the header row to the log file
@@ -133,7 +134,7 @@ static int format_log_entry(const struct log_frame *frame, char *buffer, size_t
                     "%lld,%lld,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%lld,"
                     "%.3f,%.3f,%.3f,%.3f,%u,%d,"
                     "%.3f,%.3f,%.3f,%.3f,%u,%d,"
-                    "%.3f,%.3f,%.3f,%.3f\n",
+                    "%.3f,%.3f,%.3f,%.3f,%d,%.3f,%lld\n",
                     frame->log_timestamp,
                     frame->imu.timestamp, // IMU timestamp
                     (double)frame->imu.accel[0],
@@ -158,7 +159,10 @@ static int format_log_entry(const struct log_frame *frame, char *buffer, size_t
                     (double)frame->baro.altitude,
                     (double)frame->baro.alt_variance,
                     (double)frame->baro.velocity,
-                    (double)frame->baro.vel_variance);
+                    (double)frame->baro.vel_variance,
+                    (int)frame->state.state,
+                    (double)frame->state.ground_altitude,
+                    frame->state.timestamp);
 }
 
 static void write_log_frame_to_file(const struct log_frame *frame) {
@@ -204,6 +208,7 @@ static void logger_thread_fn(void *p1, void *p2, void *p3) {
         frame.log_timestamp = k_uptime_get();
         get_imu_data(&frame.imu);
         get_baro_data(&frame.baro);
+        get_state_data(&frame.state);
 
         // Write the data to the log file
         write_log_frame_to_file(&frame);

apps/rockets/cloudburst/src/main.c

@@ -6,6 +6,7 @@
 #include "sensors/imu_thread.h"
 #include "sensors/baro_thread.h"
 #include "logger_thread.h"
+#include "state_machine/state_machine.h"
 #include "data.h"
 
 LOG_MODULE_REGISTER(falcon_main, LOG_LEVEL_INF);
@@ -18,6 +19,7 @@ int main(void)
     start_imu_thread();
     start_logger_thread();
     start_baro_thread();
+    start_state_machine_thread();
 
     return 0;
 }

apps/rockets/cloudburst/src/state_machine/state_machine.c

@@ -0,0 +1,156 @@
+#include <string.h>
+
+#include <zephyr/kernel.h>
+#include <zephyr/logging/log.h>
+#include <zephyr/smf.h>
+
+#include "state_machine.h"
+#include "state_machine_internal.h"
+#include "state_machine_states.h"
+
+LOG_MODULE_REGISTER(state_machine, LOG_LEVEL_INF);
+
+#define STATE_THREAD_STACK_SIZE 2048
+#define STATE_THREAD_PRIORITY   5
+#define STATE_THREAD_PERIOD_MS  20
+
+static K_THREAD_STACK_DEFINE(state_stack, STATE_THREAD_STACK_SIZE);
+static struct k_thread state_thread;
+static struct flight_sm state_machine;
+
+static const struct smf_state flight_states[];
+static void state_machine_reset(int64_t start_ms);
+
+/**
+ * @brief Transition the SMF context to a new state with logging.
+ */
+void transition_to(struct flight_sm *sm, flight_state_id_t next_state)
+{
+    if (next_state == sm->current_id) {
+        return;
+    }
+
+    LOG_INF("State change: %s -> %s",
+            flight_state_to_string(sm->current_id),
+            flight_state_to_string(next_state));
+    smf_set_state(SMF_CTX(sm), &flight_states[next_state]);
+}
+
+static const struct smf_state flight_states[] = {
+    [FLIGHT_STATE_STANDBY] =
+        SMF_CREATE_STATE(state_standby_entry, state_standby_run, NULL, NULL, NULL),
+    [FLIGHT_STATE_ASCENT] =
+        SMF_CREATE_STATE(state_ascent_entry, state_ascent_run, NULL, NULL, NULL),
+    [FLIGHT_STATE_MACH_LOCK] =
+        SMF_CREATE_STATE(state_mach_lock_entry, state_mach_lock_run, NULL, NULL, NULL),
+    [FLIGHT_STATE_DROGUE_DESCENT] =
+        SMF_CREATE_STATE(state_drogue_descent_entry, state_drogue_descent_run, NULL, NULL, NULL),
+    [FLIGHT_STATE_MAIN_DESCENT] =
+        SMF_CREATE_STATE(state_main_descent_entry, state_main_descent_run, NULL, NULL, NULL),
+    [FLIGHT_STATE_LANDED] =
+        SMF_CREATE_STATE(state_landed_entry, state_landed_run, NULL, NULL, NULL),
+};
+
+/**
+ * @brief State machine thread loop that drives SMF with baro samples.
+ */
+static void state_machine_thread_fn(void *p1, void *p2, void *p3)
+{
+    struct baro_data baro;
+
+    while (1) {
+        get_baro_data(&baro);
+        int64_t now_ms = (baro.timestamp > 0) ? baro.timestamp : k_uptime_get();
+
+        state_machine.sample.altitude_m = baro.altitude;
+        state_machine.sample.velocity_mps = baro.velocity;
+        state_machine.sample.timestamp_ms = now_ms;
+
+        smf_run_state(SMF_CTX(&state_machine));
+        flight_state_id_t current = state_machine.current_id;
+
+        struct state_data data = {
+            .state = current,
+            .ground_altitude = state_machine.ground_altitude_m,
+            .timestamp = now_ms,
+        };
+        set_state_data(&data);
+
+        k_sleep(K_MSEC(STATE_THREAD_PERIOD_MS));
+    }
+}
+
+/**
+ * @brief Start the state machine thread and initialize SMF.
+ */
+void start_state_machine_thread(void)
+{
+    state_machine_reset(k_uptime_get());
+
+    k_thread_create(
+        &state_thread,
+        state_stack,
+        K_THREAD_STACK_SIZEOF(state_stack),
+        state_machine_thread_fn,
+        NULL, NULL, NULL,
+        STATE_THREAD_PRIORITY,
+        0,
+        K_NO_WAIT
+    );
+}
+
+#if defined(CONFIG_ZTEST)
+void state_machine_test_reset(int64_t start_ms)
+{
+    state_machine_reset(start_ms);
+}
+
+void state_machine_test_step(float altitude_m, float velocity_mps, int64_t timestamp_ms)
+{
+    state_machine.sample.altitude_m = altitude_m;
+    state_machine.sample.velocity_mps = velocity_mps;
+    state_machine.sample.timestamp_ms = timestamp_ms;
+    smf_run_state(SMF_CTX(&state_machine));
+
+    struct state_data data = {
+        .state = state_machine.current_id,
+        .ground_altitude = state_machine.ground_altitude_m,
+        .timestamp = timestamp_ms,
+    };
+    set_state_data(&data);
+}
+
+void state_machine_test_setup_state(flight_state_id_t state, float ground_altitude_m, int64_t timestamp_ms)
+{
+    state_machine_reset(timestamp_ms);
+    state_machine.ground_altitude_m = ground_altitude_m;
+    state_machine.ground_ready = true;
+    state_machine.sample.timestamp_ms = timestamp_ms;
+    transition_to(&state_machine, state);
+}
+
+flight_state_id_t state_machine_test_get_state(void)
+{
+    return state_machine.current_id;
+}
+
+float state_machine_test_get_ground_altitude(void)
+{
+    return state_machine.ground_altitude_m;
+}
+
+bool state_machine_test_get_drogue_fire_triggered(void)
+{
+    return state_machine.drogue_fire_triggered;
+}
+#endif
+
+/**
+ * @brief Reset and initialize the state machine context.
+ */
+static void state_machine_reset(int64_t start_ms)
+{
+    memset(&state_machine, 0, sizeof(state_machine));
+    state_machine.sample.timestamp_ms = start_ms;
+    smf_set_initial(SMF_CTX(&state_machine), &flight_states[FLIGHT_STATE_STANDBY]);
+}

apps/rockets/cloudburst/src/state_machine/state_machine.h

@@ -0,0 +1,9 @@
+#ifndef STATE_MACHINE_H
+#define STATE_MACHINE_H
+
+#include "data.h"
+
+void start_state_machine_thread(void);
+const char *flight_state_to_string(flight_state_id_t state);
+
+#endif