Fastlaunchdetector

include/data_handling/DataSaver.h

@@ -27,6 +27,11 @@ class IDataSaver {
             // Default implementation does nothing
         }
 
+        // default method that does nothing, can be overridden
+        virtual void clearPostLaunchMode(){
+            // default implementation does nothing
+        }
+
 };
 
 

include/state_estimation/FastLaunchDetector.h

@@ -0,0 +1,45 @@
+#ifndef FAST_LAUNCH_DETECTOR_H
+#define FAST_LAUNCH_DETECTOR_H
+ // need to figure out how to impliment checking for launch from LaunchDetector
+#include "data_handling/CircularArray.h"
+#include "data_handling/DataPoint.h"
+#include "state_estimation/StateEstimationTypes.h"
+
+// Potential returns from the update function
+// Positive values are errors
+// Negative values are warnings
+enum FastLaunchDetectorStatus {
+    FLD_LAUNCH_DETECTED = 0,
+    FLD_ALREADY_LAUNCHED = -1,
+    FLD_ACL_TOO_LOW = -2, // The acceleration is too low for launch
+    FLD_DEFAULT_FAIL = 2,
+};
+
+
+class FastLaunchDetector
+{
+public:
+    /**
+    * Constructor
+    * @param accelerationThreshold_ms2: The threshold for acceleration to be considered a launch
+    * @param confirmationWindow_ms: The time window in ms to confirm the launch using LaunchDetector
+    */
+    FastLaunchDetector(float accelerationThreshold, uint32_t confirmationWindow_ms = 500);
+
+    int update(AccelerationTriplet accel);
+
+    bool hasLaunched() const { return launched; }
+    uint32_t getLaunchedTime() const { return launchedTime_ms; }
+    uint32_t getConfirmationWindow() const { return confirmationWindow_ms; }
+    void reset();
+
+private:
+    float accelerationThresholdSq_ms2;
+
+    bool launched;
+    uint32_t launchedTime_ms;
+    uint32_t confirmationWindow_ms;
+};
+
+
+#endif

include/state_estimation/StateMachine.h

@@ -5,6 +5,7 @@
 #include "state_estimation/ApogeeDetector.h"
 #include "state_estimation/LaunchDetector.h"
 #include "state_estimation/VerticalVelocityEstimator.h"
+#include "state_estimation/FastLaunchDetector.h"
 #include "state_estimation/BaseStateMachine.h"
 
 #include "data_handling/DataPoint.h"
@@ -14,7 +15,7 @@
 class StateMachine : public BaseStateMachine {
   public: 
     StateMachine(IDataSaver* dataSaver, LaunchDetector* launchDetector, ApogeeDetector* apogeeDetector, 
-                 VerticalVelocityEstimator* verticalVelocityEstimator);
+                 VerticalVelocityEstimator* verticalVelocityEstimator, FastLaunchDetector* fastLaunchDetector);
 
     int update(const AccelerationTriplet& accel, const DataPoint& alt) override;
 
@@ -26,6 +27,8 @@ class StateMachine : public BaseStateMachine {
     LaunchDetector* launchDetector;
     ApogeeDetector* apogeeDetector;
     VerticalVelocityEstimator* verticalVelocityEstimator;
+    FastLaunchDetector* fastLaunchDetector;
+    uint32_t fldLaunchTime_ms = 0;
 };
 
 

include/state_estimation/States.h

@@ -4,13 +4,14 @@
 enum FlightState {
     STATE_UNARMED,
     STATE_ARMED,
+    STATE_SOFT_ASCENT,
     STATE_ASCENT,
     STATE_POWERED_ASCENT,
     STATE_COAST_ASCENT,
     STATE_DESCENT,
     STATE_DROGUE_DEPLOYED,
     STATE_MAIN_DEPLOYED,
-    STATE_LANDED
+    STATE_LANDED,
 };
 
 #endif

src/state_estimation/FastLaunchDetector.cpp

@@ -0,0 +1,53 @@
+#include "state_estimation/FastLaunchDetector.h"
+
+// #define DEBUG
+#ifdef DEBUG
+#include "ArduinoHAL.h"
+#endif
+
+FastLaunchDetector::FastLaunchDetector(float accelerationThreshold_ms2, uint32_t confirmationWindow_ms)
+    : accelerationThresholdSq_ms2(accelerationThreshold_ms2 * accelerationThreshold_ms2),
+      launched(false),
+      launchedTime_ms(0),
+      confirmationWindow_ms(confirmationWindow_ms)
+{}
+
+int FastLaunchDetector::update(AccelerationTriplet accel){
+
+    // Calculate the magnitude of the acceleration squared
+    const float aclMagSq = accel.x.data * accel.x.data + accel.y.data * accel.y.data + accel.z.data * accel.z.data;
+
+    // Take the average of the timestamps
+    // Ideally these should all be the same
+    const uint32_t time_ms = (accel.x.timestamp_ms + accel.y.timestamp_ms + accel.z.timestamp_ms) / 3;
+
+    //if launch already detected, ignore further data
+    if (launched){
+        #ifdef DEBUG
+        Serial.println("FastLaunchDetector: Data point ignored because already launched");
+        #endif
+        return FLD_ALREADY_LAUNCHED;
+    }
+
+    //if accel higher than threshold, launch detected
+    if (aclMagSq > accelerationThresholdSq_ms2){
+        launched = true;
+        launchedTime_ms = time_ms;
+        return FLD_LAUNCH_DETECTED;
+    }
+
+    //if accel lower than threshold, acl too low
+    if (aclMagSq < accelerationThresholdSq_ms2) {
+        #ifdef DEBUG
+        Serial.println("FastLaunchDetector: Acceloration below threshold");
+        #endif
+        return FLD_ACL_TOO_LOW;
+    }
+
+    return FLD_DEFAULT_FAIL;
+}
+
+void FastLaunchDetector::reset(){
+    launched = false;
+    launchedTime_ms = 0;
+}

src/state_estimation/StateMachine.cpp

@@ -8,23 +8,27 @@
 StateMachine::StateMachine(IDataSaver* dataSaver,
                            LaunchDetector* launchDetector,
                            ApogeeDetector* apogeeDetector,
-                           VerticalVelocityEstimator* verticalVelocityEstimator)
+                           VerticalVelocityEstimator* verticalVelocityEstimator,
+                           FastLaunchDetector* fastLaunchDetector)
     : dataSaver(dataSaver),
       launchDetector(launchDetector),
       apogeeDetector(apogeeDetector),
       verticalVelocityEstimator(verticalVelocityEstimator),
+      fastLaunchDetector(fastLaunchDetector),
       state(STATE_ARMED)
 {
 }
 
 int StateMachine::update(const AccelerationTriplet& accel, const DataPoint& alt) {
     // Update the state
     int lpStatus = LP_DEFAULT_FAIL; 
+    int fldStatus = FLD_DEFAULT_FAIL;
 
     switch (state) {
         case STATE_ARMED:
             // Serial.println("lp update");
             lpStatus = launchDetector->update(accel);
+            fldStatus = fastLaunchDetector->update(accel);
             // Serial.println(lpStatus);
             if (launchDetector->isLaunched()) {
                 // Change state to ascent
@@ -45,7 +49,68 @@ int StateMachine::update(const AccelerationTriplet& accel, const DataPoint& alt)
                 // Update the vertical velocity estimator
                 verticalVelocityEstimator->update(accel, alt);
             }
+            if (fastLaunchDetector->hasLaunched()) {
+                // Change state to soft ascent
+                state = STATE_SOFT_ASCENT;
+
+                // Save the FLD launch time
+                fldLaunchTime_ms = fastLaunchDetector->getLaunchedTime();
+
+                // Log the state change
+                dataSaver->saveDataPoint(
+                    DataPoint(accel.x.timestamp_ms, STATE_SOFT_ASCENT),
+                    STATE_CHANGE
+                );
+
+                // Put the data saver into post-launch mode
+                dataSaver->launchDetected(fastLaunchDetector->getLaunchedTime());
+            }
+            break;
+
+        case STATE_SOFT_ASCENT:
+        /*
+        * In soft ascent, we are waiting for confirmation of launch from the LaunchDetector.
+        * If LaunchDetector confirms launch within the confirmation window, we transition to ASCENT.
+        * If the confirmation window passes without confirmation, we revert to ARMED
+        * and clear post-launch mode.
+        */
+            // Serial.println("lp update");
+            lpStatus = launchDetector->update(accel);
+            // Serial.println(lpStatus);
+            if (launchDetector->isLaunched()) {
+                // Change state to ascent
+                state = STATE_ASCENT;
+
+                // Log the state change
+                dataSaver->saveDataPoint(
+                    DataPoint(accel.x.timestamp_ms, STATE_ASCENT),
+                    STATE_CHANGE
+                );
+
+                // Start the apogee detection system
+                apogeeDetector->init({alt.data, alt.timestamp_ms});
+
+                // Update the vertical velocity estimator
+                verticalVelocityEstimator->update(accel, alt);
+            }
+            if (accel.x.timestamp_ms - fldLaunchTime_ms > fastLaunchDetector->getConfirmationWindow()) {
+                // If the confirmation window has passed without launch detected by LaunchDetector,
+                // revert to ARMED state
+                state = STATE_ARMED;
+                fldLaunchTime_ms = 0;
+                fastLaunchDetector->reset();
+
+                // Log the state change
+                dataSaver->saveDataPoint(
+                    DataPoint(accel.x.timestamp_ms, STATE_ARMED),
+                    STATE_CHANGE
+                );
+
+                // Clear post-launch mode
+                dataSaver->clearPostLaunchMode();
+            }
             break;
+
         case STATE_ASCENT:
             // Serial.println("apogee update");
             // Update the vertical velocity estimator