File content as of revision 87:6d95f853dab3:

#include "PosVelFilter.hpp"
//#include "StaticDefs.hpp"

PosVelFilter::PosVelFilter() {  // if heading_flag is set, operate on sine and cosine, return atan2(sin, cos) as postion and velocity??
    x1 = 0; // pseudo position state
    x2 = 0; // pseudo velocity state
    x1c = 0;
    x2c = 0;
    x1s = 0;
    x2s = 0;
    w_n = 1.0; // natural frequency of the filter:   bigger increases frequency response
    _headingFlag = false;
}

//run the pos-vel estimate filter
void PosVelFilter::update(float deltaT, float counts) {
    dt = deltaT;
    
    if(!_headingFlag) {
      x1_dot = x2;
      x2_dot = (-2.0*w_n*x2) - (w_n*w_n)*x1 + (w_n*w_n)*counts;

      position = x1;
      velocity = x2;

      x1 += x1_dot*dt;
      x2 += x2_dot*dt;
    }
    if(_headingFlag) {
        counts_c = cos(counts * _PI/180.0);  // math.h cos and sine operate on radians , counts is heading degrees
        counts_s = sin(counts * _PI/180.0);
        // the cosine part
        x1c_dot = x2c;
        x2c_dot = (-2.0*w_n*x2c) - (w_n*w_n)*x1c + (w_n*w_n)*counts_c;

        position_c = x1c;
        velocity_c = x2c;

        x1c += x1c_dot*dt;
        x2c += x2c_dot*dt;
          //   now the sine portion
        x1s_dot = x2s;
        x2s_dot = (-2.0*w_n*x2s) - (w_n*w_n)*x1s + (w_n*w_n)*counts_s;

        position_s = x1s;
        velocity_s = x2s;

        x1s += x1s_dot*dt;
        x2s += x2s_dot*dt;
      // now reconstitute the angle and the angular velocity
        position = atan2(position_s, position_c)  * 180.0/_PI;
        pnew = atan2(x1s,x1c) * 180/_PI;  // the next value, since the update has happened by now.
        velocity = (pnew - position) ;  // don't multiply by dt yet, now just an angular difference
        if(position < 0 ) { position = position +360; }
        if(velocity  > 180 ) { velocity -= 360;  }
        if(velocity <= -180)  { velocity += 360 ;}
        velocity *= (1.0/dt);
    }
}

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

float PosVelFilter::getVelocity() {
    return velocity;
}
void PosVelFilter::setHeadingFlag(bool heading_flag) {
    _headingFlag = heading_flag;
}

    

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

void PosVelFilter::writeWn(float wn) {
    w_n = wn;
}