File content as of revision 4:9d37f163d64c:

#ifndef SPEEDESTIMATOR_H
#define SPEEDESTIMATOR_H

#include "main.h"

struct speedEstimator {

    int last_position;
    float position_int;
    int dead_ticks;

    static const int fast_gain = SPEED_ESTIMATOR_FAST_GAIN;
    static const int deadband = SPEED_ESTIMATOR_DEADBAND;

    /**
     * constructor
     *
     * Sets the default properties of the struct
     */
    speedEstimator(int position) {

        // Set initial positions
        last_position = position;
        position_int = 1.0f*position;
        dead_ticks = 1;
    };

    float get(int position) {

        /**
         * Slow estimation
         */
        static float yz1 = 0.0f;
        static float xz0 = 0.0f;
        static float xz1 = 0.0f;
        static float dp  = 0;
        static float dp_dead = 0;

        dp = position - last_position;

        if(dp==0) {
            if(dead_ticks < 10000)
                dead_ticks++;
            dp = dp_dead/dead_ticks;
        } else {
            if(dp > POSITION_ANTIALIAS)
                dp-=POSITION_RESOLUTION;
            else if(dp < -POSITION_ANTIALIAS)
                dp+=POSITION_RESOLUTION;

            dp = dp/dead_ticks;
            dp_dead = dp;
            dead_ticks = 1;
        }

        xz0 = SAMPLE_TIME_INVERSE_S*dp;
        yz1 = SPEED_ESTIMATOR_FILTER * xz1 + (1-SPEED_ESTIMATOR_FILTER) * yz1;
        xz1 = xz0;

        last_position = position;

        /**
         * Fast estimation
         */
        static float fast_sum;
        static float pre_int_sum;

        fast_sum = position - (position_int + SAMPLE_TIME_US*0.5f*(float)1e-6*yz1);
        pre_int_sum = yz1+speedEstimator::fast_gain*(fast_sum > speedEstimator::deadband ? fast_sum - speedEstimator::deadband : (fast_sum < -speedEstimator::deadband ? fast_sum + speedEstimator::deadband : 0));
        position_int+=SAMPLE_TIME_US*1e-6*pre_int_sum;

        return pre_int_sum;

    };

};

#endif