File content as of revision 10:3b952ea7fad4:

#include "ZTC.h"
#include "SensorFusion.h"

///ZTC
float K_h[3] = {1636.8, 80.0, 0.0};///{1261.8, 70, 0};///{2088, 28.65, 0};///
float K_m[3] = {3458.8, 117.0, 0.0};///{1643.7, 80.2, 0};///{2536.8, 100, 0};///
float K_l[3] = {2143.8, 0.0, 0.0};
float K_s[3] = {0,0,0};
float K_hphi[3] = {0.100359*3.0, 0.034599*3.0, -0.05653*3.0};///{10.0359, 3.4599, -5.653};///
//float K_mphi[3]= {3.7165*0.03, 2.6747*0.03, -2.0436*0.03};
float K_mphi[3]= {6.3409*0.03, 4.1028*0.03, -5.0338*0.03};
float u = 0.0;
float phi_hat = 0.0;
float phi_hat_old = 0.0;
float phi_hat_aw = 0.0;
float dphi_hat= 0.0;
float roll_ref = 0.0;
float gamma_ref = 0.0;
float gamma_rad = 0.0;
float gamma_rad_old = 0.0;
int8_t gamma_degree = 0;

void calc_PD(float* K_in, float phi_hat_in)
{
    u = -K_in[0]*(roll_angle - phi_hat_in - roll_ref) - K_in[1]*droll_angle;
}

void calc_Phi(float* K_phi_in)
{
    dphi_hat = -K_phi_in[0]*(roll_angle - phi_hat - roll_ref) - K_phi_in[1]*droll_angle - K_phi_in[2]*(gamma_rad-gamma_ref);
    phi_hat = phi_hat_old + (dphi_hat + phi_hat_aw)*sample_time;
}

void calc_Gamma(float u_in, float alpha, float multr)
{
    gamma_rad = lpf(u_in,gamma_rad_old,alpha)/multr - GAMMA_OFFSET; ///38
    gamma_rad_old = gamma_rad;
}

void anti_wdup(void)
{
    if(phi_hat > PHI_HAT_UB)
    {
        phi_hat_aw = K_aw * (PHI_HAT_UB - phi_hat);
        phi_hat = PHI_HAT_UB;
    }
    else if(phi_hat < PHI_HAT_LB)
    {
        phi_hat_aw = K_aw * (PHI_HAT_LB - phi_hat);
        phi_hat = PHI_HAT_LB;
    }
    else
    {
        phi_hat_aw = 0.0;
    }
    phi_hat_old = phi_hat;
}