Feat/state machine callbacks

include/state_estimation/BaseStateMachine.h

@@ -1,17 +1,31 @@
 #ifndef BASE_STATE_MACHINE_H
 #define BASE_STATE_MACHINE_H
 
+#include <array>
+#include <cstddef>
+#include <cstdint>
+
 #include "data_handling/DataPoint.h"
 #include "state_estimation/StateEstimationTypes.h"
 #include "state_estimation/States.h"
 
 /**
- * @brief Abstract interface for flight state machines driven by IMU/altimeter data.
+ * @brief Base class for flight state machines driven by IMU/altimeter data.
  * @note When to use: derive a concrete state machine to map sensor inputs to
- *       discrete flight phases without changing call sites.
+ *       discrete flight phases without changing call sites. This class owns
+ *       current-state tracking plus on-entry callback dispatch with no dynamic
+ *       memory allocation.
  */
 class BaseStateMachine {
     public:
+        // Type alias for a function pointer with void return and no args
+        using StateEntryCallback = void (*)();
+
+        static constexpr std::size_t kMaxStateEntryCallbacks = 32;
+
+        explicit BaseStateMachine(FlightState initialState = STATE_UNARMED);
+        virtual ~BaseStateMachine() = default;
+
         /**
          * @brief Advance the state machine with the latest measurements.
          * @param accel Acceleration vector readings.
@@ -26,7 +40,43 @@ class BaseStateMachine {
          * @note When to use: downstream logic (ejection, logging, UI) queries
          *       this to decide actions.
          */
-        virtual uint8_t getState() const = 0;
+        virtual uint8_t getState() const;
+
+        /**
+         * @brief Register a callback to invoke each time a target state is entered.
+         * @param targetState The state that triggers the callback.
+         * @param functionPtr Function to call when entering @p targetState.
+         * @return true if callback was registered, false for nullptr, duplicate,
+         *         or full callback buffer.
+         */
+        bool registerOnStateEntry(FlightState targetState, StateEntryCallback functionPtr);
+
+        static constexpr std::size_t getMaxStateEntryCallbacks() {
+            return kMaxStateEntryCallbacks;
+        }
+
+    protected:
+        /**
+         * @brief Transition to a new state and trigger registered on-entry callbacks.
+         * @param newState State to transition into.
+         * @return true if state changed, false if already in @p newState.
+         */
+        bool changeState(FlightState newState);
+
+        /**
+         * @brief Current state as FlightState enum.
+         */
+        FlightState getFlightState() const;
+
+    private:
+        struct StateCallbackRegistration {
+            FlightState state;
+            StateEntryCallback functionPtr;
+        };
+
+        FlightState state_;
+        std::size_t callbackCount_ = 0;
+        std::array<StateCallbackRegistration, kMaxStateEntryCallbacks> onStateEntryCallbacks_{};
 };
 
-#endif
+#endif

include/state_estimation/BurnoutStateMachine.h

@@ -37,14 +37,7 @@ class BurnoutStateMachine : public BaseStateMachine {
      */
     int update(const AccelerationTriplet& accel, const DataPoint& alt) override;
 
-    /**
-     * @brief Current state identifier.
-     * @note When to use: consumers needing current phase (e.g., pyro logic).
-     */
-    uint8_t getState() const override;
-
   private:
-    uint8_t state_;
     IDataSaver* dataSaver_;
     LaunchDetector* launchDetector_;
     ApogeeDetector* apogeeDetector_;

include/state_estimation/README.md

@@ -5,7 +5,7 @@ Tools for detecting flight events, fusing sensors, and managing rocket flight-st
 ## Files
 - `ApogeeDetector.h`: Detects apogee when filtered altitude peaks and velocity goes negative. More robust than zero-velocity crossing, especially with noisy baro data.
 - `ApogeePredictor.h`: Projects time/altitude to apogee using current velocity and deceleration; use for active-aero or adaptive control while still climbing.
-- `BaseStateMachine.h`: Abstract interface for flight state machines.
+- `BaseStateMachine.h`: Shared state ownership and callback-registration base for flight state machines; callback storage is fixed-capacity (32 entries, no dynamic allocation).
 - `BurnoutStateMachine.h`: State machine variant with an explicit burnout phase before coast; use when burnout-specific logic or logging matters.
 - `GroundLevelEstimator.h`: Learns launch-site altitude pre-launch, then converts ASL to AGL after launch; use to normalize baro data.
 - `LaunchDetector.h`: Sliding-window accelerometer detector that marks liftoff when sustained acceleration exceeds a threshold; use to gate launch-critical events.

include/state_estimation/StateMachine.h

@@ -37,14 +37,7 @@ class StateMachine : public BaseStateMachine {
      */
     int update(const AccelerationTriplet& accel, const DataPoint& alt) override;
 
-    /**
-     * @brief Retrieve the current state value.
-     * @note When to use: downstream logic that needs to branch on flight phase.
-     */
-    uint8_t getState() const override;
-
   private:
-    uint8_t state_;
     IDataSaver* dataSaver_;
     LaunchDetector* launchDetector_;
     ApogeeDetector* apogeeDetector_;

include/state_estimation/States.h

@@ -1,25 +1,30 @@
 #ifndef FLIGHT_STATES_H
 #define FLIGHT_STATES_H
 
+#include <cstdint> // For uint8_t
+
 // Not all states are used by all state machines
 // Order matters, systems rely on comparisons like `current_state` > STATE_ASCENT to know if the 
 // flight has passed the ascent state and is now falling. So states are ordered from earliest to latest.
 // The literal value of these states should never be used because a new state can be added
-// in between existing states, shifting their values.
+// in between existing states, shifting their values. B/c we tag the version of Avionics used for each flight, 
+// we can come back here to see the numerical values of each state when decoding the logs. The ground station
+// keeps YAMLs of these state definitions every time they are changed. To avoid having a bunch of YAMLs we try
+// to keep these values semi-stable, which is why it includes states that aren't currently used to be forward-compatible. 
 // Keep in mind that there is a difference between states and events. For example,
 // apogee is an event that causes a transition from STATE_ASCENT to STATE_DESCENT, but it is not a state itself.
 // The state machines are in charge of detecting these transitions and updating the states. 
-enum FlightState {
-    STATE_UNARMED, // 0x00
-    STATE_ARMED, // 0x01
-    STATE_SOFT_ASCENT, // 0x02
-    STATE_ASCENT, // 0x03 Don't use the ascent state if you are already using powered ascent and coast ascent
-    STATE_POWERED_ASCENT, // 0x04
-    STATE_COAST_ASCENT, // 0x05
-    STATE_DESCENT, // 0x06
-    STATE_DROGUE_DEPLOYED, // 0x07
-    STATE_MAIN_DEPLOYED, // 0x08
-    STATE_LANDED, // 0x09
+enum FlightState : uint8_t {
+    STATE_UNARMED = 0,      // 0 This may not necessarily always be the 0th state, or the first state. A state could be added before it.
+    STATE_ARMED,            // 1
+    STATE_SOFT_ASCENT,      // 2
+    STATE_ASCENT,           // 3 Don't use the ascent state if you are already using powered ascent and coast ascent
+    STATE_POWERED_ASCENT,   // 4
+    STATE_COAST_ASCENT,     // 5
+    STATE_DESCENT,          // 6
+    STATE_DROGUE_DEPLOYED,  // 7
+    STATE_MAIN_DEPLOYED,    // 8
+    STATE_LANDED,           // 9
 };
 
 #endif

src/state_estimation/BaseStateMachine.cpp

@@ -0,0 +1,53 @@
+#include "state_estimation/BaseStateMachine.h"
+
+BaseStateMachine::BaseStateMachine(FlightState initialState) : state_(initialState) {}
+
+uint8_t BaseStateMachine::getState() const {
+    return static_cast<uint8_t>(state_);
+}
+
+bool BaseStateMachine::registerOnStateEntry(FlightState targetState, StateEntryCallback functionPtr) {
+    if (functionPtr == nullptr) {
+        return false;
+    }
+
+    // Searching for a duplicate registration
+    for (std::size_t i = 0; i < callbackCount_; i++) {
+        const StateCallbackRegistration& registration = onStateEntryCallbacks_[i];
+        if (registration.state == targetState && registration.functionPtr == functionPtr) {
+            return false;
+        }
+    }
+
+    // Checking if we have room for another callback
+    if (callbackCount_ >= kMaxStateEntryCallbacks) {
+        return false;
+    }
+
+    // Register the new callback
+    onStateEntryCallbacks_[callbackCount_] = {targetState, functionPtr};
+    callbackCount_++;
+    return true;
+}
+
+bool BaseStateMachine::changeState(FlightState newState) {
+    if (state_ == newState) {
+        return false;
+    }
+
+    state_ = newState;
+
+    // Calling the registered callbacks for the new state
+    for (std::size_t i = 0; i < callbackCount_; i++) {
+        const StateCallbackRegistration& registration = onStateEntryCallbacks_[i];
+        if (registration.state == state_) {
+            registration.functionPtr();
+        }
+    }
+
+    return true;
+}
+
+FlightState BaseStateMachine::getFlightState() const {
+    return state_;
+}

src/state_estimation/BurnoutStateMachine.cpp

@@ -9,7 +9,7 @@ BurnoutStateMachine::BurnoutStateMachine(IDataSaver* dataSaver,
                                          LaunchDetector* launchDetector,
                                          ApogeeDetector* apogeeDetector,
                                          VerticalVelocityEstimator* verticalVelocityEstimator)
-    : state_(STATE_ARMED),
+    : BaseStateMachine(STATE_ARMED),
       dataSaver_(dataSaver),
       launchDetector_(launchDetector),
       apogeeDetector_(apogeeDetector),
@@ -19,14 +19,14 @@ BurnoutStateMachine::BurnoutStateMachine(IDataSaver* dataSaver,
 }
 
 int BurnoutStateMachine::update(const AccelerationTriplet& accel, const DataPoint& alt) {
-    switch (state_) {
+    switch (getFlightState()) {
         case STATE_ARMED:
             // Serial.println("lp update");
             launchDetector_->update(accel);
             // Serial.println(lpStatus);
             if (launchDetector_->isLaunched()) {
                 // Change state to ascent.
-                state_ = STATE_POWERED_ASCENT;
+                changeState(STATE_POWERED_ASCENT);
 
                 // Log the state change.
                 Serial.println("To pa (launch detected)");
@@ -55,7 +55,7 @@ int BurnoutStateMachine::update(const AccelerationTriplet& accel, const DataPoin
             apogeeDetector_->update(verticalVelocityEstimator_);
             Serial.println(verticalVelocityEstimator_->getInertialVerticalAcceleration());
             if (verticalVelocityEstimator_->getInertialVerticalAcceleration() <= 0) { // when acceleration returns to less than gravity after launch, we're coasting
-                state_ = STATE_COAST_ASCENT;
+                changeState(STATE_COAST_ASCENT);
 
                 // Log the state change.
                 Serial.println("To ca");
@@ -72,7 +72,7 @@ int BurnoutStateMachine::update(const AccelerationTriplet& accel, const DataPoin
             verticalVelocityEstimator_->update(accel, alt);
             apogeeDetector_->update(verticalVelocityEstimator_);
             if (apogeeDetector_->isApogeeDetected()) {
-                state_ = STATE_DESCENT;
+                changeState(STATE_DESCENT);
 
                 // Log the state change.
                 Serial.println("To descent");
@@ -85,14 +85,10 @@ int BurnoutStateMachine::update(const AccelerationTriplet& accel, const DataPoin
             }
             break;
 
-        case STATE_DESCENT:
-            // Do nothing
+        default:
+            // All other states have no actions or transitions
             break;
     }
 
     return 0;
 }
-
-uint8_t BurnoutStateMachine::getState() const {
-    return state_;
-}