Exemplo n.º 1
0
/*
 * 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);
}
Exemplo n.º 2
0
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;
*/
}