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/include/state_estimation/FastLaunchDetector.h b/include/state_estimation/FastLaunchDetector.h new file mode 100644 index 0000000..7c4e7cf --- /dev/null +++ b/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 \ No newline at end of file diff --git a/include/state_estimation/StateMachine.h b/include/state_estimation/StateMachine.h index 59ef658..03a4d76 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 "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; }; diff --git a/include/state_estimation/States.h b/include/state_estimation/States.h index be81695..016253e 100644 --- a/include/state_estimation/States.h +++ b/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 \ 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..867fb86 --- /dev/null +++ b/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; +} \ No newline at end of file diff --git a/src/state_estimation/StateMachine.cpp b/src/state_estimation/StateMachine.cpp index 4899da3..0129bef 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,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