File content as of revision 1:ec4117689673:

#include "PosVelFilter.hpp"
#include "StaticDefs.hpp"
#include "conversions.hpp"

PosVelFilter::PosVelFilter()
{
    x1 = 0;
    x2 = 0;
    //w_n is the natural frequency of the filter bigger increases frequency response
    w_n = 5.0;

}

void PosVelFilter::update()
{
    //run the pos/vel estimate filter
    //this derives the timing from last run
    last_time = curr_time;
    curr_time = systemTime().read();
    dt = curr_time-last_time;

    // fetch the current distance reading from adc and convert to mm
    conv_distance = counts_to_dist(adc().ch0_filt);

    x1_dot = x2;
    x2_dot = (-2.0*w_n*x2)-(w_n*w_n)*x1+(w_n*w_n)*conv_distance;

    position = x1;
    velocity = x2;

    x1 += x1_dot*dt;
    x2 += x2_dot*dt;

}

void PosVelFilter::init()
{
    // run filter for 2 seconds on startup
    float start = systemTime().read();
    float time = start;
    pc().printf("\n\rWarming Up \n\r");
 
    while ((time - start) < 1.0) {
        update();

        pc().printf("%5.3f \r", velocity);
        time = systemTime().read();
    }
    pc().printf("\n\r");
    while (1) {
        update();

        if (abs(velocity) > 0.005) {
            pc().printf("Waiting to stablize. velocity: %5.3f \r", velocity);

        } else {
            pc().printf("\n\rreading stabilized\n\r");
            break;
        }
    }

}


float PosVelFilter::getPosition()
{
    return position;
}

float PosVelFilter::getVelocity()
{
    return velocity;
}

float PosVelFilter::getDt()
{
    return dt;
}

void PosVelFilter::writeWn(float wn)
{
    w_n = wn;
    pc().printf("\n\rWn set to: %f", w_n);
}