From 8bb12a49c6dbbedc0e3dc3e532ae572e4a5c8979 Mon Sep 17 00:00:00 2001 From: Peter Shepherd Date: Wed, 15 Oct 2025 10:34:49 -0400 Subject: [PATCH 01/10] feat: add fastlaunchdetector header and update implimentation --- include/state_estimation/FastLaunchDetector.h | 43 +++++++++++++++++++ src/state_estimation/FastLaunchDetector.cpp | 21 +++++++++ 2 files changed, 64 insertions(+) create mode 100644 include/state_estimation/FastLaunchDetector.h create mode 100644 src/state_estimation/FastLaunchDetector.cpp diff --git a/include/state_estimation/FastLaunchDetector.h b/include/state_estimation/FastLaunchDetector.h new file mode 100644 index 0000000..bafd4c4 --- /dev/null +++ b/include/state_estimation/FastLaunchDetector.h @@ -0,0 +1,43 @@ +#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 { + LP_LAUNCH_DETECTED = 0, + LP_ALREADY_LAUNCHED = -1, + LP_YOUNGER_TIMESTAMP = -2, // The timestamp is younger than the last timestamp + LP_ACL_TOO_LOW = -8, // The acceleration is too low for launch + LP_DEFAULT_FAIL = 2, +}; + + +class FastLaunchDetector +{ +public: + /** + * Constructor + * @param accelerationThreshold_ms2: The threshold for acceleration to be considered a launch + */ + FastLaunchDetector(float accelerationThreshold); + + int update(AccelerationTriplet accel); + + bool hasLaunched() const { return launched; } + uint32_t getLaunchedTime() const { return launchedTime_ms; } + void reset(); + +private: + float accelerationThresholdSq_ms2; + + bool launched; + uint32_t launchedTime_ms; +}; + + +#endif \ No newline at end of file diff --git a/src/state_estimation/FastLaunchDetector.cpp b/src/state_estimation/FastLaunchDetector.cpp new file mode 100644 index 0000000..0dd9974 --- /dev/null +++ b/src/state_estimation/FastLaunchDetector.cpp @@ -0,0 +1,21 @@ +#include "state_estimation/FastLaunchDetector.h" + +FastLaunchDetector::FastLaunchDetector(float accelerationThreshold_ms2) + : accelerationThresholdSq_ms2(accelerationThreshold_ms2 * accelerationThreshold_ms2) +{} + +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 (aclMagSq > accelerationThresholdSq_ms2){ + launched = true; + launchedTime_ms = time_ms; + return LP_LAUNCH_DETECTED; + } +} \ No newline at end of file From 46e26c9c97d66cd4698c016b0f14cac74d00abd7 Mon Sep 17 00:00:00 2001 From: Peter Shepherd Date: Wed, 15 Oct 2025 10:52:00 -0400 Subject: [PATCH 02/10] feat: added FastLaunchDetector update checks: already_launched, acl_too_low --- include/state_estimation/FastLaunchDetector.h | 9 +++---- src/state_estimation/FastLaunchDetector.cpp | 25 ++++++++++++++++++- 2 files changed, 28 insertions(+), 6 deletions(-) diff --git a/include/state_estimation/FastLaunchDetector.h b/include/state_estimation/FastLaunchDetector.h index bafd4c4..25883e2 100644 --- a/include/state_estimation/FastLaunchDetector.h +++ b/include/state_estimation/FastLaunchDetector.h @@ -9,11 +9,10 @@ // Positive values are errors // Negative values are warnings enum FastLaunchDetectorStatus { - LP_LAUNCH_DETECTED = 0, - LP_ALREADY_LAUNCHED = -1, - LP_YOUNGER_TIMESTAMP = -2, // The timestamp is younger than the last timestamp - LP_ACL_TOO_LOW = -8, // The acceleration is too low for launch - LP_DEFAULT_FAIL = 2, + LAUNCH_DETECTED = 0, + ALREADY_LAUNCHED = -1, + ACL_TOO_LOW = -2, // The acceleration is too low for launch + DEFAULT_FAIL = 2, }; diff --git a/src/state_estimation/FastLaunchDetector.cpp b/src/state_estimation/FastLaunchDetector.cpp index 0dd9974..78e43c6 100644 --- a/src/state_estimation/FastLaunchDetector.cpp +++ b/src/state_estimation/FastLaunchDetector.cpp @@ -1,5 +1,10 @@ #include "state_estimation/FastLaunchDetector.h" +// #define DEBUG +#ifdef DEBUG +#include "ArduinoHAL.h" +#endif + FastLaunchDetector::FastLaunchDetector(float accelerationThreshold_ms2) : accelerationThresholdSq_ms2(accelerationThreshold_ms2 * accelerationThreshold_ms2) {} @@ -13,9 +18,27 @@ int FastLaunchDetector::update(AccelerationTriplet accel){ // 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 ALREADY_LAUNCHED; + } + + //if accel higher than threshold, launch detected if (aclMagSq > accelerationThresholdSq_ms2){ launched = true; launchedTime_ms = time_ms; - return LP_LAUNCH_DETECTED; + return LAUNCH_DETECTED; } + + //if accel lower than threshold, acl too low + if (aclMagSq < accelerationThresholdSq_ms2) { + #ifdef DEBUG + Serial.println("FastLaunchDetector: Acceloration below threshold"); + #endif + return ACL_TOO_LOW; + } + } \ No newline at end of file From 5583974d5eb72881fd031c37b96dea1dc62b3da2 Mon Sep 17 00:00:00 2001 From: Peter Shepherd Date: Wed, 15 Oct 2025 10:55:38 -0400 Subject: [PATCH 03/10] feat: added FastLaunchDetector reset() --- src/state_estimation/FastLaunchDetector.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/state_estimation/FastLaunchDetector.cpp b/src/state_estimation/FastLaunchDetector.cpp index 78e43c6..7a26078 100644 --- a/src/state_estimation/FastLaunchDetector.cpp +++ b/src/state_estimation/FastLaunchDetector.cpp @@ -40,5 +40,9 @@ int FastLaunchDetector::update(AccelerationTriplet accel){ #endif return ACL_TOO_LOW; } +} +void FastLaunchDetector::reset(){ + launched = false; + launchedTime_ms = 0; } \ No newline at end of file From 89409f299d24daf87af0c3510dd171bc1c6eb17f Mon Sep 17 00:00:00 2001 From: Peter Shepherd Date: Wed, 15 Oct 2025 18:20:34 -0400 Subject: [PATCH 04/10] fix: default fail added to update --- src/state_estimation/FastLaunchDetector.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/state_estimation/FastLaunchDetector.cpp b/src/state_estimation/FastLaunchDetector.cpp index 7a26078..bda1226 100644 --- a/src/state_estimation/FastLaunchDetector.cpp +++ b/src/state_estimation/FastLaunchDetector.cpp @@ -40,6 +40,8 @@ int FastLaunchDetector::update(AccelerationTriplet accel){ #endif return ACL_TOO_LOW; } + + return DEFAULT_FAIL; } void FastLaunchDetector::reset(){ From 0474a35a29914fd4e531f4d29bc22e352cc06510 Mon Sep 17 00:00:00 2001 From: Peter Shepherd Date: Wed, 29 Oct 2025 11:10:20 -0400 Subject: [PATCH 05/10] feat: add soft ascent state in StateMachine with FLD checking and LD confirmation --- include/state_estimation/FastLaunchDetector.h | 13 +++-- include/state_estimation/StateMachine.h | 5 +- include/state_estimation/States.h | 3 +- src/state_estimation/FastLaunchDetector.cpp | 8 +-- src/state_estimation/StateMachine.cpp | 58 ++++++++++++++++++- 5 files changed, 75 insertions(+), 12 deletions(-) diff --git a/include/state_estimation/FastLaunchDetector.h b/include/state_estimation/FastLaunchDetector.h index 25883e2..7c4e7cf 100644 --- a/include/state_estimation/FastLaunchDetector.h +++ b/include/state_estimation/FastLaunchDetector.h @@ -9,10 +9,10 @@ // Positive values are errors // Negative values are warnings enum FastLaunchDetectorStatus { - LAUNCH_DETECTED = 0, - ALREADY_LAUNCHED = -1, - ACL_TOO_LOW = -2, // The acceleration is too low for launch - DEFAULT_FAIL = 2, + FLD_LAUNCH_DETECTED = 0, + FLD_ALREADY_LAUNCHED = -1, + FLD_ACL_TOO_LOW = -2, // The acceleration is too low for launch + FLD_DEFAULT_FAIL = 2, }; @@ -22,13 +22,15 @@ class FastLaunchDetector /** * 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); + 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: @@ -36,6 +38,7 @@ class FastLaunchDetector bool launched; uint32_t launchedTime_ms; + uint32_t confirmationWindow_ms; }; diff --git a/include/state_estimation/StateMachine.h b/include/state_estimation/StateMachine.h index 9bd5ebe..474e28a 100644 --- a/include/state_estimation/StateMachine.h +++ b/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 "data_handling/DataPoint.h" #include "data_handling/DataSaver.h" @@ -13,7 +14,7 @@ class StateMachine { public: StateMachine(IDataSaver* dataSaver, LaunchDetector* launchDetector, ApogeeDetector* apogeeDetector, - VerticalVelocityEstimator* verticalVelocityEstimator); + VerticalVelocityEstimator* verticalVelocityEstimator, FastLaunchDetector* fastLaunchDetector); int update(const AccelerationTriplet& accel, const DataPoint& alt); @@ -25,6 +26,8 @@ class StateMachine { LaunchDetector* launchDetector; ApogeeDetector* apogeeDetector; VerticalVelocityEstimator* verticalVelocityEstimator; + FastLaunchDetector* fastLaunchDetector; + uint32_t fldLaunchTime_ms = 0; }; diff --git a/include/state_estimation/States.h b/include/state_estimation/States.h index be81695..6f7e8ed 100644 --- a/include/state_estimation/States.h +++ b/include/state_estimation/States.h @@ -10,7 +10,8 @@ enum FlightState { STATE_DESCENT, STATE_DROGUE_DEPLOYED, STATE_MAIN_DEPLOYED, - STATE_LANDED + STATE_LANDED, + STATE_SOFT_ASCENT // for FastLaunchDetector, will transition to STATE_ASCENT once confirmed by LaunchDetector }; #endif \ No newline at end of file diff --git a/src/state_estimation/FastLaunchDetector.cpp b/src/state_estimation/FastLaunchDetector.cpp index bda1226..294948e 100644 --- a/src/state_estimation/FastLaunchDetector.cpp +++ b/src/state_estimation/FastLaunchDetector.cpp @@ -23,14 +23,14 @@ int FastLaunchDetector::update(AccelerationTriplet accel){ #ifdef DEBUG Serial.println("FastLaunchDetector: Data point ignored because already launched"); #endif - return ALREADY_LAUNCHED; + return FLD_ALREADY_LAUNCHED; } //if accel higher than threshold, launch detected if (aclMagSq > accelerationThresholdSq_ms2){ launched = true; launchedTime_ms = time_ms; - return LAUNCH_DETECTED; + return FLD_LAUNCH_DETECTED; } //if accel lower than threshold, acl too low @@ -38,10 +38,10 @@ int FastLaunchDetector::update(AccelerationTriplet accel){ #ifdef DEBUG Serial.println("FastLaunchDetector: Acceloration below threshold"); #endif - return ACL_TOO_LOW; + return FLD_ACL_TOO_LOW; } - return DEFAULT_FAIL; + return FLD_DEFAULT_FAIL; } void FastLaunchDetector::reset(){ diff --git a/src/state_estimation/StateMachine.cpp b/src/state_estimation/StateMachine.cpp index 4899da3..ea87446 100644 --- a/src/state_estimation/StateMachine.cpp +++ b/src/state_estimation/StateMachine.cpp @@ -8,11 +8,13 @@ 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) { } @@ -20,11 +22,13 @@ StateMachine::StateMachine(IDataSaver* dataSaver, 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,6 +49,22 @@ 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_ASCENT: // Serial.println("apogee update"); @@ -62,6 +82,42 @@ int StateMachine::update(const AccelerationTriplet& accel, const DataPoint& alt) } break; + case STATE_SOFT_ASCENT: + // 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; + + // Log the state change + dataSaver->saveDataPoint( + DataPoint(accel.x.timestamp_ms, STATE_ARMED), + STATE_CHANGE + ); + + // Clear post-launch mode + dataSaver->clearPostLaunchMode(); + } + break; + case STATE_DESCENT: // Do nothing break; From b034453358ce74dea6dac42de2efb08071e5f0e5 Mon Sep 17 00:00:00 2001 From: Peter Shepherd Date: Wed, 29 Oct 2025 11:16:50 -0400 Subject: [PATCH 06/10] add confirmation window to fld constructor and soft ascent comments --- src/state_estimation/FastLaunchDetector.cpp | 7 +++++-- src/state_estimation/StateMachine.cpp | 6 ++++++ 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/state_estimation/FastLaunchDetector.cpp b/src/state_estimation/FastLaunchDetector.cpp index 294948e..867fb86 100644 --- a/src/state_estimation/FastLaunchDetector.cpp +++ b/src/state_estimation/FastLaunchDetector.cpp @@ -5,8 +5,11 @@ #include "ArduinoHAL.h" #endif -FastLaunchDetector::FastLaunchDetector(float accelerationThreshold_ms2) - : accelerationThresholdSq_ms2(accelerationThreshold_ms2 * accelerationThreshold_ms2) +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){ diff --git a/src/state_estimation/StateMachine.cpp b/src/state_estimation/StateMachine.cpp index ea87446..8c231e0 100644 --- a/src/state_estimation/StateMachine.cpp +++ b/src/state_estimation/StateMachine.cpp @@ -83,6 +83,12 @@ int StateMachine::update(const AccelerationTriplet& accel, const DataPoint& alt) 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); From f37acf24ffd1b9bd74807aad58685e1e72fdfb7e Mon Sep 17 00:00:00 2001 From: Peter Shepherd Date: Wed, 29 Oct 2025 19:03:59 -0400 Subject: [PATCH 07/10] fix: statemachine resets fldLaunchTime_ms after no launch confirmation --- src/state_estimation/StateMachine.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/state_estimation/StateMachine.cpp b/src/state_estimation/StateMachine.cpp index 8c231e0..39bb79b 100644 --- a/src/state_estimation/StateMachine.cpp +++ b/src/state_estimation/StateMachine.cpp @@ -112,6 +112,7 @@ int StateMachine::update(const AccelerationTriplet& accel, const DataPoint& alt) // If the confirmation window has passed without launch detected by LaunchDetector, // revert to ARMED state state = STATE_ARMED; + fldLaunchTime_ms = 0; // Log the state change dataSaver->saveDataPoint( From 6a92f7dadd84faef73d5d46941a4f05c31ce9fec Mon Sep 17 00:00:00 2001 From: Peter Shepherd Date: Wed, 29 Oct 2025 19:20:33 -0400 Subject: [PATCH 08/10] add clearpostlaunchmode default implementation and reset fld in state machine when no confirmation --- include/data_handling/DataSaver.h | 5 +++++ src/state_estimation/StateMachine.cpp | 1 + 2 files changed, 6 insertions(+) diff --git a/include/data_handling/DataSaver.h b/include/data_handling/DataSaver.h index 1505089..0e9608c 100644 --- a/include/data_handling/DataSaver.h +++ b/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 + } + }; diff --git a/src/state_estimation/StateMachine.cpp b/src/state_estimation/StateMachine.cpp index 39bb79b..72173a7 100644 --- a/src/state_estimation/StateMachine.cpp +++ b/src/state_estimation/StateMachine.cpp @@ -113,6 +113,7 @@ int StateMachine::update(const AccelerationTriplet& accel, const DataPoint& alt) // revert to ARMED state state = STATE_ARMED; fldLaunchTime_ms = 0; + fastLaunchDetector->reset(); // Log the state change dataSaver->saveDataPoint( From b4ddaea6c218f88084ef5ddfaec45867683944b6 Mon Sep 17 00:00:00 2001 From: Peter Shepherd Date: Wed, 5 Nov 2025 18:01:17 -0500 Subject: [PATCH 09/10] fix: change states order for better testing --- include/state_estimation/States.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/state_estimation/States.h b/include/state_estimation/States.h index 6f7e8ed..016253e 100644 --- a/include/state_estimation/States.h +++ b/include/state_estimation/States.h @@ -4,6 +4,7 @@ enum FlightState { STATE_UNARMED, STATE_ARMED, + STATE_SOFT_ASCENT, STATE_ASCENT, STATE_POWERED_ASCENT, STATE_COAST_ASCENT, @@ -11,7 +12,6 @@ enum FlightState { STATE_DROGUE_DEPLOYED, STATE_MAIN_DEPLOYED, STATE_LANDED, - STATE_SOFT_ASCENT // for FastLaunchDetector, will transition to STATE_ASCENT once confirmed by LaunchDetector }; #endif \ No newline at end of file From 24208dc409a6fbaa052d8638b3461a4d42fd8459 Mon Sep 17 00:00:00 2001 From: Peter Shepherd Date: Wed, 3 Dec 2025 14:53:40 -0500 Subject: [PATCH 10/10] make statemachine STATE update flow more natural --- src/state_estimation/StateMachine.cpp | 31 ++++++++++++++------------- 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/src/state_estimation/StateMachine.cpp b/src/state_estimation/StateMachine.cpp index 72173a7..0129bef 100644 --- a/src/state_estimation/StateMachine.cpp +++ b/src/state_estimation/StateMachine.cpp @@ -66,21 +66,6 @@ int StateMachine::update(const AccelerationTriplet& accel, const DataPoint& alt) dataSaver->launchDetected(fastLaunchDetector->getLaunchedTime()); } break; - case STATE_ASCENT: - // Serial.println("apogee update"); - // Update the vertical velocity estimator - verticalVelocityEstimator->update(accel, alt); - apogeeDetector->update(verticalVelocityEstimator); - if (apogeeDetector->isApogeeDetected()) { - state = STATE_DESCENT; - - // Log the state change - dataSaver->saveDataPoint( - DataPoint(accel.x.timestamp_ms, STATE_DESCENT), - STATE_CHANGE - ); - } - break; case STATE_SOFT_ASCENT: /* @@ -125,6 +110,22 @@ int StateMachine::update(const AccelerationTriplet& accel, const DataPoint& alt) dataSaver->clearPostLaunchMode(); } break; + + case STATE_ASCENT: + // Serial.println("apogee update"); + // Update the vertical velocity estimator + verticalVelocityEstimator->update(accel, alt); + apogeeDetector->update(verticalVelocityEstimator); + if (apogeeDetector->isApogeeDetected()) { + state = STATE_DESCENT; + + // Log the state change + dataSaver->saveDataPoint( + DataPoint(accel.x.timestamp_ms, STATE_DESCENT), + STATE_CHANGE + ); + } + break; case STATE_DESCENT: // Do nothing