InclinationDetector.h

Go to the documentation of this file.

#ifndef INCLINATION_DETECTOR_H
#define INCLINATION_DETECTOR_H
#include <Arduino.h>
#include <queue>
#include <utility>
 
enum class Inclination : uint8_t
{
    Decline = 0,
    Level = 1,
    Incline = 2,
};
 
class InclinationDetector
{
    public:
    InclinationDetector();
 
    void set_decline_angle(float threshold);
    void set_incline_angle(float threshold);
 
    Inclination check(const bool is_stance, const bool fsr_calibrating, const float norm_angle);
    
    private:
    /* Exposed through getters/setters */
    std::pair<float, float> _decline_stance_phase_percent;
    float _incline_angle_theshold;
    float _decline_angle_threshold;
 
    /* Internal */
    bool _previous_fsr_calibrating;
    int _calibration_step_count;
    const int _steps_until_calibrated = 4;
    bool _previous_should_calibrate;
    bool _returned_this_stance;
    Inclination _prev_estimate;
    bool _prev_stance;
    float _prev_stance_time;
    float _current_stance_start;
 
    /* Feature Data for Threshold Determination */
    float _incline_features_sum;
    float _decline_features_sum;
    float _calibrated_stance_entry_angle;
    float _calibrated_stance_exit_angle;
 
    enum class Edge: int
    {
        None = 0,
        Rising = 1,
        Falling = 2, 
    };
 
    Edge _check_for_edge(const bool is_stance);
    float _update_stance_phase(const Edge edge, const bool is_stance);
    void _calculate_gait_features();
    void _update_gait_features(const Edge edge, const float norm_angle);
    bool _incline_check(const float norm_angle);
    bool _decline_check(const float norm_angle);
    bool _should_calibrate(const bool fsr_calibrating, const InclinationDetector::Edge edge);
};
 
#endif