/*
* euler2quat - Convert Pitch, Roll and Yaw to quaternion
* @param[in] roll Roll [rad]
* @param[in] pitch Pitch [rad]
* @param[in] yaw Yaw [rad]
* @param[out] q Quaternion
*/
void euler2quat(float roll, float pitch, float yaw, quaternion_t *q)
{
/* Prepare the angles for conversion to quaternions */
pitch *= 0.5f;
roll *= 0.5f;
yaw *= 0.5f;
q->q0 = fast_cos(roll) * fast_cos(pitch) * fast_cos(yaw) + fast_sin(roll) * fast_sin(pitch) * fast_sin(yaw);
q->q1 = fast_sin(roll) * fast_cos(pitch) * fast_cos(yaw) - fast_cos(roll) * fast_sin(pitch) * fast_sin(yaw);
q->q2 = fast_cos(roll) * fast_sin(pitch) * fast_cos(yaw) + fast_sin(roll) * fast_cos(pitch) * fast_sin(yaw);
q->q3 = fast_cos(roll) * fast_cos(pitch) * fast_sin(yaw) - fast_sin(roll) * fast_sin(pitch) * fast_cos(yaw);
}
void flux_linkage_estimator (float T,float V_sD,float V_sQ,float i_sD,float i_sQ,float R_s,float electric_frequency, float* psisD, float* psisQ, float*psis,float* psis_alpha)
{
static float previous_psi_sD=0.0f;
static float previous_psi_sQ=0.0f;
float LPF_psi_sD=0.0f;
float LPF_psi_sQ=0.0f;
float LPF_lag_angle=0.0f;
float LPF_psi_s_alpha=0.0f;
float LPF_psi_s=0.0f;
//fixed cutoff frequency
//LPF_psi_sD = ( previous_psi_sD+T*(V_sD-i_sD*R_s) )/(1.0f+T*( 2.0f*PI_CTE*F_CUTOFF));
//LPF_psi_sQ = ( previous_psi_sQ+T*(V_sQ-i_sQ*R_s) )/(1.0f+T*( 2.0f*PI_CTE*F_CUTOFF));
if (K_LPF*electric_frequency<5.0f)
{
LPF_psi_sD = ( previous_psi_sD+T*(V_sD-i_sD*R_s) )/(1.0f+T*( 2.0f*PI_CTE*F_CUTOFF));
LPF_psi_sQ = ( previous_psi_sQ+T*(V_sQ-i_sQ*R_s) )/(1.0f+T*( 2.0f*PI_CTE*F_CUTOFF));
//LPF_psi_sD = ( previous_psi_sD+T*(V_sD) )/(1.0f+T*( 2.0f*PI_CTE*F_CUTOFF));
//LPF_psi_sQ = ( previous_psi_sQ+T*(V_sQ) )/(1.0f+T*( 2.0f*PI_CTE*F_CUTOFF));
}
else
{
LPF_psi_sD = ( previous_psi_sD+T*(V_sD-i_sD*R_s) )/(1.0f+T*( K_LPF*2.0f*PI_CTE*electric_frequency));
LPF_psi_sQ = ( previous_psi_sQ+T*(V_sQ-i_sQ*R_s) )/(1.0f+T*( K_LPF*2.0f*PI_CTE*electric_frequency));
//LPF_psi_sD = ( previous_psi_sD+T*(V_sD) )/(1.0f+T*( K_LPF*2.0f*PI_CTE*electric_frequency));
//LPF_psi_sQ = ( previous_psi_sQ+T*(V_sQ) )/(1.0f+T*( K_LPF*2.0f*PI_CTE*electric_frequency));
}
/**/
//LPF_psi_sD = ( previous_psi_sD+T*(V_sD) )/(1.0f+T*( 2.0f*PI_CTE*F_CUTOFF));
//LPF_psi_sQ = ( previous_psi_sQ+T*(V_sQ) )/(1.0f+T*( 2.0f*PI_CTE*F_CUTOFF));
//LPF_psi_s = stator_flux_linkage_magnite_psi_s (LPF_psi_sD,LPF_psi_sQ);
//LPF_psi_s_alpha = fast_vector_angle (LPF_psi_sQ,LPF_psi_sD);
//LPF_lag_angle = 1.0f;
/*
LPF_psi_sD = ( previous_psi_sD+T*(V_sD-i_sD*R_s) )/(1.0f+T*( 2.0f*PI_CTE*F_CUTOFF));
LPF_psi_sQ = ( previous_psi_sQ+T*(V_sQ-i_sQ*R_s) )/(1.0f+T*( 2.0f*PI_CTE*F_CUTOFF));
*/
fast_vector_angle_and_magnitude(LPF_psi_sQ,LPF_psi_sD,&LPF_psi_s,&LPF_psi_s_alpha);
LPF_lag_angle = 0.0f;
*psis = LPF_psi_s;
*psis_alpha = LPF_psi_s_alpha+LPF_lag_angle;
previous_psi_sD = LPF_psi_s*fast_cos (*psis_alpha);
previous_psi_sQ = LPF_psi_s*fast_sine(*psis_alpha);
*psisD=previous_psi_sD;
*psisQ=previous_psi_sQ;
/*
//fixed wcutoff, no lag compensation
LPF_psi_sD = ( previous_psi_sD+T*(V_sD-i_sD*R_s) )/(1.0f+T*( 2.0f*PI_CTE*F_CUTOFF));
LPF_psi_sQ = ( previous_psi_sQ+T*(V_sQ-i_sQ*R_s) )/(1.0f+T*( 2.0f*PI_CTE*F_CUTOFF));
previous_psi_sD=LPF_psi_sD;
previous_psi_sQ=LPF_psi_sQ;
*psisD=previous_psi_sD;
*psisQ=previous_psi_sQ;
*/
//no filter, neglecting currents
/*
LPF_psi_sD = ( previous_psi_sD+T*(V_sD-i_sD*R_s) )/(1.0f+T*( 2.0f*PI_CTE*F_CUTOFF));
LPF_psi_sQ = ( previous_psi_sQ+T*(V_sQ-i_sD*R_s) )/(1.0f+T*( 2.0f*PI_CTE*F_CUTOFF));
//LPF_psi_sD = ( previous_psi_sD+T*(V_sD) )/(1.0f+T*( 2.0f*PI_CTE*F_CUTOFF));
//LPF_psi_sQ = ( previous_psi_sQ+T*(V_sQ) )/(1.0f+T*( 2.0f*PI_CTE*F_CUTOFF));
previous_psi_sD=LPF_psi_sD;
previous_psi_sQ=LPF_psi_sQ;
*psisD=previous_psi_sD;
*psisQ=previous_psi_sQ;
*/
}