LaunchDetector.h

Go to the documentation of this file.

/*
 * Simple launch prediction algorithm
 * Simply, create an instance of this class and update it with
 * new acceleration data as often as possible.
 */
 
#ifndef LAUNCH_DETECTOR_H
#define LAUNCH_DETECTOR_H
 
#include "data_handling/CircularArray.h"
#include "data_handling/DataPoint.h"
#include "state_estimation/StateEstimationTypes.h"
 
constexpr float ACCEPTABLE_PERCENT_DIFFERENCE_WINDOW_INTERVAL = 0.5F;
constexpr std::size_t CIRCULAR_ARRAY_ALLOCATED_SLOTS = 100; // 100 slots allocated for the circular array (100 * sizeof(DataPoint)) = 800 bytes allocated)
 
// Potential returns from the update function
// Positive values are errors
// Negative values are warnings
enum LaunchDetectorStatus {
    LP_LAUNCH_DETECTED = 0,
    LP_ALREADY_LAUNCHED = -1,
    LP_YOUNGER_TIMESTAMP = -2, // The timestamp is younger than the last timestamp
    LP_INITIAL_POPULATION = -3, // The window is not full yet
    LP_DATA_TOO_FAST = -4, // The data came in faster than the desired window
    LP_WINDOW_DATA_STALE = 1, // The data given makes the window data relatively too old, resets the window
    LP_WINDOW_TIME_RANGE_TOO_SMALL = -5, // The window is full, but the time difference between the head and the tail is too little
    LP_WINDOW_TIME_RANGE_TOO_LARGE = -6, // The window is full, but the time difference between the head and the tail is too large
    LP_WINDOW_NOT_FULL = -7, // The window is not full yet
    LP_ACL_TOO_LOW = -8, // The acceleration is too low for launch
    LP_DEFAULT_FAIL = 2,
};

class LaunchDetector
{
public:
    LaunchDetector(float accelerationThreshold_ms2,
                    uint16_t windowSize_ms,
                    uint16_t windowInterval_ms);

    int update(AccelerationTriplet accel);
    bool isLaunched() {return launched;}
    uint32_t getLaunchedTime() {return launchedTime_ms;}
    float getMedianAccelerationSquared() {return median_acceleration_squared;}
    void reset();
 
    // --------------
    // Testing Methods
    // --------------
    // Gives a pointer to the window
    CircularArray<DataPoint, CIRCULAR_ARRAY_ALLOCATED_SLOTS>* getWindowPtr() {return &AclMagSqWindow_ms2;}
    // Gives the threshold in ms^2 squared
    float getThreshold() {return accelerationThresholdSq_ms2;}
    // Gives the window interval in ms
    uint16_t getWindowInterval() {return windowInterval_ms;}
    uint16_t getAcceptableTimeDifference() {return acceptableTimeDifference_ms;}
 
 
private:
    // The threshold for acceleration to be considered a launch squared
    float accelerationThresholdSq_ms2;
    uint16_t windowInterval_ms;
 
    // Min and max window sizes calculated as +- 10% of the window size
    uint16_t min_window_size_ms = 0; // If the calculated time range is less than this, don't try to detect launch
    uint16_t max_window_size_ms = 0; // If the calculated time range is greater than this, don't try to detect launch
 
    uint16_t acceptableTimeDifference_ms;
    // The window holding the acceleration magnitude squared b/c sqrt is expensive
    CircularArray<DataPoint, CIRCULAR_ARRAY_ALLOCATED_SLOTS> AclMagSqWindow_ms2;
    bool launched;
    uint32_t launchedTime_ms;
 
    float median_acceleration_squared;
};
 
#endif