MIT_MatlabToolbox/trunk/matlab/Simulation/DroneRS_Compensator_ert_shrlib_rtw/DroneRS_Compensator.c
/* Function for MATLAB Function: '<S5>/EstimatorAttitude' */
Function for MATLAB Function: '/EstimatorAttitude'
Function for MATLAB Function : ' / EstimatorAttitude '
static real_T DroneRS_Compensator_genpnorm(const real_T x[3])
{
  real_T y;
  real_T scale;
  real_T absxk;
  real_T t;
  scale = 2.2250738585072014E-308;
  absxk = fabs(x[0]);
  if (absxk > 2.2250738585072014E-308) {
    y = 1.0;
    scale = absxk;
  } else {
    t = absxk / 2.2250738585072014E-308;
    y = t * t;
  }
  absxk = fabs(x[1]);
  if (absxk > scale) {
    t = scale / absxk;
    y = y * t * t + 1.0;
    scale = absxk;
  } else {
    t = absxk / scale;
    y += t * t;
  }
  absxk = fabs(x[2]);
  if (absxk > scale) {
    t = scale / absxk;
    y = y * t * t + 1.0;
    scale = absxk;
  } else {
    t = absxk / scale;
    y += t * t;
  }
  return scale * sqrt(y);
}
static real_T DroneRS_Compensator_genpnorm ( const real_T x [ 3 ] ) { real_T y ; real_T scale ; real_T absxk ; real_T t ; scale = 2.2250738585072014E-308 ; absxk = fabs ( x [ 0 ] ) ; if ( absxk > 2.2250738585072014E-308 ) { y = 1.0 ; scale = absxk ; } else { t = absxk / 2.2250738585072014E-308 ; y = t * t ; } absxk = fabs ( x [ 1 ] ) ; if ( absxk > scale ) { t = scale / absxk ; y = y * t * t + 1.0 ; scale = absxk ; } else { t = absxk / scale ; y += t * t ; } absxk = fabs ( x [ 2 ] ) ; if ( absxk > scale ) { t = scale / absxk ; y = y * t * t + 1.0 ; scale = absxk ; } else { t = absxk / scale ; y += t * t ; } return scale * sqrt ( y ) ; }
Function for MATLAB Function: '<S5>/EstimatorAttitude'
Function for MATLAB Function : ' <S5 > / EstimatorAttitude '