/*

 * File: SMO_arctan_PLL.c

 *

 * Code generated for Simulink model 'SMO_arctan_PLL'.

 *

 * Model version                  : 1.812

 * Simulink Coder version         : 9.4 (R2020b) 29-Jul-2020

 * C/C++ source code generated on : Tue Apr 11 20:18:35 2023

 *

 * Target selection: ert.tlc

 * Embedded hardware selection: ARM Compatible->ARM Cortex-M

 * Code generation objectives:

 *    1. Execution efficiency

 *    2. RAM efficiency

 * Validation result: Not run

 */



#include "SMO_arctan_PLL.h"

#define NumBitsPerChar                 8U



extern real_T rt_remd_snf(real_T u0, real_T u1);

static real_T rtGetNaN(void);

static real32_T rtGetNaNF(void);

extern real_T rtInf;

extern real_T rtMinusInf;

extern real_T rtNaN;

extern real32_T rtInfF;

extern real32_T rtMinusInfF;

extern real32_T rtNaNF;

static void rt_InitInfAndNaN(size_t realSize);

static boolean_T rtIsInf(real_T value);

static boolean_T rtIsInfF(real32_T value);

static boolean_T rtIsNaN(real_T value);

static boolean_T rtIsNaNF(real32_T value);

typedef struct {

  struct {

    uint32_T wordH;

    uint32_T wordL;

  } words;

} BigEndianIEEEDouble;



typedef struct {

  struct {

    uint32_T wordL;

    uint32_T wordH;

  } words;

} LittleEndianIEEEDouble;



typedef struct {

  union {

    real32_T wordLreal;

    uint32_T wordLuint;

  } wordL;

} IEEESingle;



real_T rtInf;

real_T rtMinusInf;

real_T rtNaN;

real32_T rtInfF;

real32_T rtMinusInfF;

real32_T rtNaNF;

static real_T rtGetInf(void);

static real32_T rtGetInfF(void);

static real_T rtGetMinusInf(void);

static real32_T rtGetMinusInfF(void);



/*===========*

 * Constants *

 *===========*/

#define RT_PI                          3.14159265358979323846

#define RT_PIF                         3.1415927F

#define RT_LN_10                       2.30258509299404568402

#define RT_LN_10F                      2.3025851F

#define RT_LOG10E                      0.43429448190325182765

#define RT_LOG10EF                     0.43429449F

#define RT_E                           2.7182818284590452354

#define RT_EF                          2.7182817F



/*

 * UNUSED_PARAMETER(x)

 *   Used to specify that a function parameter (argument) is required but not

 *   accessed by the function body.

 */

#ifndef UNUSED_PARAMETER

#if defined(__LCC__)

#define UNUSED_PARAMETER(x)                                      /* do nothing */

#else



/*

 * This is the semi-ANSI standard way of indicating that an

 * unused function parameter is required.

 */

#define UNUSED_PARAMETER(x)            (void) (x)

#endif

#endif



/*

 * Initialize rtNaN needed by the generated code.

 * NaN is initialized as non-signaling. Assumes IEEE.

 */

static real_T rtGetNaN(void)

{

  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);

  real_T nan = 0.0;

  if (bitsPerReal == 32U) {

    nan = rtGetNaNF();

  } else {

    union {

      LittleEndianIEEEDouble bitVal;

      real_T fltVal;

    } tmpVal;



    tmpVal.bitVal.words.wordH = 0xFFF80000U;

    tmpVal.bitVal.words.wordL = 0x00000000U;

    nan = tmpVal.fltVal;

  }



  return nan;

}



/*

 * Initialize rtNaNF needed by the generated code.

 * NaN is initialized as non-signaling. Assumes IEEE.

 */

static real32_T rtGetNaNF(void)

{

  IEEESingle nanF = { { 0 } };



  nanF.wordL.wordLuint = 0xFFC00000U;

  return nanF.wordL.wordLreal;

}



/*

 * Initialize the rtInf, rtMinusInf, and rtNaN needed by the

 * generated code. NaN is initialized as non-signaling. Assumes IEEE.

 */

static void rt_InitInfAndNaN(size_t realSize)

{

  (void) (realSize);

  rtNaN = rtGetNaN();

  rtNaNF = rtGetNaNF();

  rtInf = rtGetInf();

  rtInfF = rtGetInfF();

  rtMinusInf = rtGetMinusInf();

  rtMinusInfF = rtGetMinusInfF();

}



/* Test if value is infinite */

static boolean_T rtIsInf(real_T value)

{

  return (boolean_T)((value==rtInf || value==rtMinusInf) ? 1U : 0U);

}



/* Test if single-precision value is infinite */

static boolean_T rtIsInfF(real32_T value)

{

  return (boolean_T)(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U);

}



/* Test if value is not a number */

static boolean_T rtIsNaN(real_T value)

{

  boolean_T result = (boolean_T) 0;

  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);

  if (bitsPerReal == 32U) {

    result = rtIsNaNF((real32_T)value);

  } else {

    union {

      LittleEndianIEEEDouble bitVal;

      real_T fltVal;

    } tmpVal;



    tmpVal.fltVal = value;

    result = (boolean_T)((tmpVal.bitVal.words.wordH & 0x7FF00000) == 0x7FF00000 &&

                         ( (tmpVal.bitVal.words.wordH & 0x000FFFFF) != 0 ||

                          (tmpVal.bitVal.words.wordL != 0) ));

  }



  return result;

}



/* Test if single-precision value is not a number */

static boolean_T rtIsNaNF(real32_T value)

{

  IEEESingle tmp;

  tmp.wordL.wordLreal = value;

  return (boolean_T)( (tmp.wordL.wordLuint & 0x7F800000) == 0x7F800000 &&

                     (tmp.wordL.wordLuint & 0x007FFFFF) != 0 );

}



/*

 * Initialize rtInf needed by the generated code.

 * Inf is initialized as non-signaling. Assumes IEEE.

 */

static real_T rtGetInf(void)

{

  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);

  real_T inf = 0.0;

  if (bitsPerReal == 32U) {

    inf = rtGetInfF();

  } else {

    union {

      LittleEndianIEEEDouble bitVal;

      real_T fltVal;

    } tmpVal;



    tmpVal.bitVal.words.wordH = 0x7FF00000U;

    tmpVal.bitVal.words.wordL = 0x00000000U;

    inf = tmpVal.fltVal;

  }



  return inf;

}



/*

 * Initialize rtInfF needed by the generated code.

 * Inf is initialized as non-signaling. Assumes IEEE.

 */

static real32_T rtGetInfF(void)

{

  IEEESingle infF;

  infF.wordL.wordLuint = 0x7F800000U;

  return infF.wordL.wordLreal;

}



/*

 * Initialize rtMinusInf needed by the generated code.

 * Inf is initialized as non-signaling. Assumes IEEE.

 */

static real_T rtGetMinusInf(void)

{

  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);

  real_T minf = 0.0;

  if (bitsPerReal == 32U) {

    minf = rtGetMinusInfF();

  } else {

    union {

      LittleEndianIEEEDouble bitVal;

      real_T fltVal;

    } tmpVal;



    tmpVal.bitVal.words.wordH = 0xFFF00000U;

    tmpVal.bitVal.words.wordL = 0x00000000U;

    minf = tmpVal.fltVal;

  }



  return minf;

}



/*

 * Initialize rtMinusInfF needed by the generated code.

 * Inf is initialized as non-signaling. Assumes IEEE.

 */

static real32_T rtGetMinusInfF(void)

{

  IEEESingle minfF;

  minfF.wordL.wordLuint = 0xFF800000U;

  return minfF.wordL.wordLreal;

}



real_T rt_remd_snf(real_T u0, real_T u1)

{

  real_T q;

  real_T y;

  if (rtIsNaN(u0) || rtIsNaN(u1) || rtIsInf(u0)) {

    y = (rtNaN);

  } else if (rtIsInf(u1)) {

    y = u0;

  } else if ((u1 != 0.0) && (u1 != trunc(u1))) {

    q = fabs(u0 / u1);

    if (!(fabs(q - floor(q + 0.5)) > DBL_EPSILON * q)) {

      y = 0.0 * u0;

    } else {

      y = fmod(u0, u1);

    }

  } else {

    y = fmod(u0, u1);

  }



  return y;

}



/* Model step function */

void SMO_arctan_PLL_step(RT_MODEL *const rtM, real_T rtU_Ialfabeta[2], real_T

  rtU_Ualfabeta[2], real_T *rtY_theta, real_T *rtY_we)

{

  DW *rtDW = rtM->dwork;

  real_T rtb_Add1;

  real_T rtb_Add1_h;

  real_T rtb_Product4;

  real_T rtb_Sum;

  real_T rtb_Sum2;

  real_T rtb_Sum5;

  real_T rtb_Switch_e;



  /* Sum: '<S7>/Sum2' incorporates:

   *  DiscreteIntegrator: '<S7>/Discrete-Time Integrator'

   *  Inport: '<Root>/Ialfa,beta'

   */

  rtb_Sum2 = rtDW->DiscreteTimeIntegrator_DSTATE_e - rtU_Ialfabeta[0];



  /* Switch: '<S58>/Switch' incorporates:

   *  Abs: '<S58>/Abs'

   *  Constant: '<S7>/Constant1'

   *  Constant: '<S7>/Constant3'

   *  Product: '<S58>/Divide'

   *  Product: '<S58>/Divide1'

   *  Product: '<S58>/Divide2'

   *  Sum: '<S58>/Add'

   */

  if (fabs(rtb_Sum2) - 120.0 > 0.0) {

    /* Signum: '<S58>/Sign' */

    if (rtb_Sum2 < 0.0) {

      rtb_Sum2 = -1.0;

    } else if (rtb_Sum2 > 0.0) {

      rtb_Sum2 = 1.0;

    } else if (rtb_Sum2 == 0.0) {

      rtb_Sum2 = 0.0;

    } else {

      rtb_Sum2 = (rtNaN);

    }



    /* End of Signum: '<S58>/Sign' */

    rtb_Sum2 *= 100.0;

  } else {

    rtb_Sum2 = rtb_Sum2 / 120.0 * 100.0;

  }



  /* End of Switch: '<S58>/Switch' */



  /* Sum: '<S7>/Sum5' incorporates:

   *  DiscreteIntegrator: '<S7>/Discrete-Time Integrator1'

   *  Inport: '<Root>/Ialfa,beta'

   */

  rtb_Sum5 = rtDW->DiscreteTimeIntegrator1_DSTATE - rtU_Ialfabeta[1];



  /* Switch: '<S57>/Switch' incorporates:

   *  Abs: '<S57>/Abs'

   *  Constant: '<S7>/Constant4'

   *  Constant: '<S7>/Constant5'

   *  Product: '<S57>/Divide'

   *  Product: '<S57>/Divide1'

   *  Product: '<S57>/Divide2'

   *  Sum: '<S57>/Add'

   */

  if (fabs(rtb_Sum5) - 120.0 > 0.0) {

    /* Signum: '<S57>/Sign' */

    if (rtb_Sum5 < 0.0) {

      rtb_Sum5 = -1.0;

    } else if (rtb_Sum5 > 0.0) {

      rtb_Sum5 = 1.0;

    } else if (rtb_Sum5 == 0.0) {

      rtb_Sum5 = 0.0;

    } else {

      rtb_Sum5 = (rtNaN);

    }



    /* End of Signum: '<S57>/Sign' */

    rtb_Switch_e = rtb_Sum5 * 100.0;

  } else {

    rtb_Switch_e = rtb_Sum5 / 120.0 * 100.0;

  }



  /* End of Switch: '<S57>/Switch' */



  /* Sqrt: '<S6>/Sqrt' incorporates:

   *  Constant: '<S6>/Constant'

   *  Math: '<S6>/Math Function'

   *  Math: '<S6>/Math Function1'

   *  Sum: '<S6>/Add'

   */

  rtb_Sum5 = sqrt((rtb_Sum2 * rtb_Sum2 + rtb_Switch_e * rtb_Switch_e) + 1.0E-6);



  /* Sum: '<S6>/Sum' incorporates:

   *  DiscreteIntegrator: '<S6>/Discrete-Time Integrator'

   *  Gain: '<S6>/Gain'

   *  Product: '<S6>/Divide'

   *  Product: '<S6>/Divide2'

   *  Product: '<S6>/Product'

   *  Product: '<S6>/Product1'

   *  Trigonometry: '<S6>/Trigonometric Function'

   *  Trigonometry: '<S6>/Trigonometric Function1'

   */

  rtb_Sum5 = -(rtb_Sum2 / rtb_Sum5) * cos(rtDW->DiscreteTimeIntegrator_DSTATE) -

    rtb_Switch_e / rtb_Sum5 * sin(rtDW->DiscreteTimeIntegrator_DSTATE);



  /* Sum: '<S49>/Sum' incorporates:

   *  DiscreteIntegrator: '<S40>/Integrator'

   *  Gain: '<S45>/Proportional Gain'

   */

  rtb_Sum = 12566.370614359172 * rtb_Sum5 + rtDW->Integrator_DSTATE;



  /* Sum: '<S5>/Add1' incorporates:

   *  Constant: '<S5>/Filter_Constant'

   *  Constant: '<S5>/One'

   *  Product: '<S5>/Product'

   *  Product: '<S5>/Product1'

   *  UnitDelay: '<S5>/Unit Delay'

   */

  rtb_Add1 = rtb_Sum * 0.01885 + 0.98115 * rtDW->UnitDelay_DSTATE;



  /* Sum: '<S4>/Add1' incorporates:

   *  Constant: '<S4>/Filter_Constant'

   *  Constant: '<S4>/One'

   *  Product: '<S4>/Product'

   *  Product: '<S4>/Product1'

   *  UnitDelay: '<S4>/Unit Delay'

   */

  rtb_Add1_h = rtb_Add1 * 0.1 + 0.9 * rtDW->UnitDelay_DSTATE_h;



  /* Outport: '<Root>/we' incorporates:

   *  Constant: '<S1>/P'

   *  Gain: '<S1>/Gain3'

   *  Product: '<S1>/Divide1'

   */

  *rtY_we = 9.5492965855137211 * rtb_Add1_h / 4.0;



  /* Product: '<S7>/Product4' incorporates:

   *  Constant: '<S7>/Constant2'

   */

  rtb_Product4 = rtb_Add1 * 0.000178;



  /* Sum: '<S7>/   ' incorporates:

   *  DiscreteIntegrator: '<S7>/Discrete-Time Integrator'

   *  DiscreteIntegrator: '<S7>/Discrete-Time Integrator1'

   *  Gain: '<S7>/ 1//L'

   *  Gain: '<S7>/ 1//L '

   *  Gain: '<S7>/Gain5'

   *  Gain: '<S7>/R//L '

   *  Inport: '<Root>/Ualfa,beta'

   *  Product: '<S7>/Product3'

   */

  rtb_Switch_e = ((5617.9775280898875 * rtU_Ualfabeta[1] - 61.797752808988761 *

                   rtDW->DiscreteTimeIntegrator1_DSTATE) - 5617.9775280898875 *

                  rtb_Switch_e) + 5617.9775280898875 * rtb_Product4 *

    rtDW->DiscreteTimeIntegrator_DSTATE_e;



  /* Outport: '<Root>/theta' incorporates:

   *  Constant: '<S2>/Constant1'

   *  DiscreteIntegrator: '<S6>/Discrete-Time Integrator'

   *  Math: '<S2>/Rem1'

   *  Sum: '<S2>/Add1'

   */

  *rtY_theta = rt_remd_snf(rtDW->DiscreteTimeIntegrator_DSTATE +

    6.2831853071795862, 6.2831853071795862);



  /* Update for DiscreteIntegrator: '<S6>/Discrete-Time Integrator' */

  rtDW->DiscreteTimeIntegrator_DSTATE += 6.0E-5 * rtb_Sum;



  /* Update for DiscreteIntegrator: '<S7>/Discrete-Time Integrator' incorporates:

   *  DiscreteIntegrator: '<S7>/Discrete-Time Integrator1'

   *  Gain: '<S7>/1//L'

   *  Gain: '<S7>/1//L '

   *  Gain: '<S7>/Gain4'

   *  Gain: '<S7>/R//L'

   *  Inport: '<Root>/Ualfa,beta'

   *  Product: '<S7>/Product2'

   *  Sum: '<S7>/ '

   */

  rtDW->DiscreteTimeIntegrator_DSTATE_e += (((5617.9775280898875 *

    rtU_Ualfabeta[0] - 61.797752808988761 *

    rtDW->DiscreteTimeIntegrator_DSTATE_e) - 5617.9775280898875 * rtb_Sum2) -

    5617.9775280898875 * rtb_Product4 * rtDW->DiscreteTimeIntegrator1_DSTATE) *

    6.0E-5;



  /* Update for DiscreteIntegrator: '<S7>/Discrete-Time Integrator1' */

  rtDW->DiscreteTimeIntegrator1_DSTATE += 6.0E-5 * rtb_Switch_e;



  /* Update for DiscreteIntegrator: '<S40>/Integrator' incorporates:

   *  Gain: '<S37>/Integral Gain'

   */

  rtDW->Integrator_DSTATE += 3.9478417604357429E+7 * rtb_Sum5 * 6.0E-5;



  /* Update for UnitDelay: '<S5>/Unit Delay' */

  rtDW->UnitDelay_DSTATE = rtb_Add1;



  /* Update for UnitDelay: '<S4>/Unit Delay' */

  rtDW->UnitDelay_DSTATE_h = rtb_Add1_h;

}



/* Model initialize function */

void SMO_arctan_PLL_initialize(RT_MODEL *const rtM)

{

  /* Registration code */



  /* initialize non-finites */

  rt_InitInfAndNaN(sizeof(real_T));

  UNUSED_PARAMETER(rtM);

}



/*

 * File trailer for generated code.

 *

 * [EOF]

 */