File content as of revision 2:9b068d2a7994:

#include "angle_estimator.h"
#include "mbed.h"

#define PI 3.1415927


using namespace std;

angle_estimator::angle_estimator(float Ts) : ax(PA_0), ay(PA_1), gz(PA_4){
    u2ax.setup(0.29719f,0.69768f,-9.81f,9.81f);     // use normalized input
    u2ay.setup(0.29890,0.70330f,-9.81f,9.81f);      // use normalized input
    u2gz.setup(-4.6517f * 3.3f,0.45495f);           // use normalized input
                // 4.6517f = 1/3.752e-3 [V / °/s] * pi/180
    float tau = 1.0f;
    f_ax.setup(tau,Ts,1.0);
    f_ay.setup(tau,Ts,1.0);
    f_gz.setup(tau,Ts,tau);

    }


angle_estimator::~angle_estimator() {}



// ************ calculate angle *****************
void angle_estimator::calc_angle_phi1(int do_reset){
    gyro = u2gz(gz.read());
    if(do_reset == 1){
        f_ax.reset(u2ax(ax.read()));
        f_ay.reset(u2ax(ay.read()));
        f_gz.reset(gyro);
        }
    phi1 = atan2(-f_ax(u2ax(ax.read())),f_ay(u2ay(ay.read()))) + PI/4.0f + f_gz(gyro);
    }

float angle_estimator::get_phi1(void){
return phi1;
}
float angle_estimator::get_gyro(void){
return gyro;
}