MC100/Simulink/SMO_arctan_PLL_ert_rtw/SMO_arctan_PLL.c

/*
 * 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]
 */