motor_ctl-1/3.BasicFunction/Source/AngleObserver_discrete.c

/*
 * File: AngleObserver_discrete.c
 *
 * Code generated for Simulink model 'AngleObserver_discrete'.
 *
 * Model version                  : 1.28
 * Simulink Coder version         : 9.6 (R2021b) 14-May-2021
 * C/C++ source code generated on : Wed Apr 27 16:33:17 2022
 *
 * Target selection: ert.tlc
 * Embedded hardware selection: Custom Processor->Custom Processor
 * Code generation objectives:
 *    1. Execution efficiency
 *    2. RAM efficiency
 * Validation result: Not run
 */

#include "AngleObserver_discrete.h"
#ifndef UCHAR_MAX
#include <limits.h>
#endif

#if ( UCHAR_MAX != (0xFFU) ) || ( SCHAR_MAX != (0x7F) )
#error Code was generated for compiler with different sized uchar/char. \
Consider adjusting Test hardware word size settings on the \
Hardware Implementation pane to match your compiler word sizes as \
defined in limits.h of the compiler. Alternatively, you can \
select the Test hardware is the same as production hardware option and \
select the Enable portable word sizes option on the Code Generation > \
Verification pane for ERT based targets, which will disable the \
preprocessor word size checks.
#endif

#if ( USHRT_MAX != (0xFFFFU) ) || ( SHRT_MAX != (0x7FFF) )
#error Code was generated for compiler with different sized ushort/short. \
Consider adjusting Test hardware word size settings on the \
Hardware Implementation pane to match your compiler word sizes as \
defined in limits.h of the compiler. Alternatively, you can \
select the Test hardware is the same as production hardware option and \
select the Enable portable word sizes option on the Code Generation > \
Verification pane for ERT based targets, which will disable the \
preprocessor word size checks.
#endif

#if ( UINT_MAX != (0xFFFFFFFFU) ) || ( INT_MAX != (0x7FFFFFFF) )
#error Code was generated for compiler with different sized uint/int. \
Consider adjusting Test hardware word size settings on the \
Hardware Implementation pane to match your compiler word sizes as \
defined in limits.h of the compiler. Alternatively, you can \
select the Test hardware is the same as production hardware option and \
select the Enable portable word sizes option on the Code Generation > \
Verification pane for ERT based targets, which will disable the \
preprocessor word size checks.
#endif

#if ( ULONG_MAX != (0xFFFFFFFFU) ) || ( LONG_MAX != (0x7FFFFFFF) )
#error Code was generated for compiler with different sized ulong/long. \
Consider adjusting Test hardware word size settings on the \
Hardware Implementation pane to match your compiler word sizes as \
defined in limits.h of the compiler. Alternatively, you can \
select the Test hardware is the same as production hardware option and \
select the Enable portable word sizes option on the Code Generation > \
Verification pane for ERT based targets, which will disable the \
preprocessor word size checks.
#endif

/* Skipping ulong_long/long_long check: insufficient preprocessor integer range. */

/* Block signals and states (default storage) */
DW rtDW;

/* External inputs (root inport signals with default storage) */
ExtU rtU;

/* External outputs (root outports fed by signals with default storage) */
ExtY rtY;

/* Real-time model */
static RT_MODEL rtM_;
RT_MODEL *const rtM = &rtM_;

/* Model step function */
void Trigger(void)
{
  int64_T rtb_Saturation2;
  int32_T rtb_Add2;
  int32_T rtb_Add3_i;
  int32_T rtb_Add5;

  /* RootInportFunctionCallGenerator generated from: '<Root>/Trigger' incorporates:
   *  SubSystem: '<Root>/AngleObserver_discrete'
   */
  /* ArithShift: '<S8>/Shift Arithmetic' incorporates:
   *  ArithShift: '<S7>/Shift Arithmetic'
   *  Constant: '<S1>/Constant4'
   *  Inport: '<Root>/AngleIn'
   *  Sum: '<S1>/Add2'
   *  Sum: '<S1>/Add3'
   *  UnitDelay: '<S1>/Unit Delay1'
   */
  rtb_Add2 = (((int16_T)(rtU.AngleIn - 16384) << 17) - rtDW.UnitDelay1_DSTATE) >>
    17;

  /* Sum: '<S4>/Add3' incorporates:
   *  Constant: '<S1>/Constant1'
   *  DataTypeConversion: '<S8>/Data Type Conversion'
   *  Product: '<S1>/Product1'
   *  Saturate: '<S8>/Saturation'
   *  UnitDelay: '<S4>/Unit Delay1'
   */
  rtb_Add3_i = (int16_T)rtb_Add2 * 84 + rtDW.UnitDelay1_DSTATE_k;

  /* Sum: '<S6>/Add5' incorporates:
   *  ArithShift: '<S1>/Shift Arithmetic2'
   *  Constant: '<S1>/Constant2'
   *  DataTypeConversion: '<S8>/Data Type Conversion'
   *  Product: '<S1>/Product2'
   *  Saturate: '<S8>/Saturation'
   *  Sum: '<S1>/Sum2'
   *  UnitDelay: '<S6>/Unit Delay3'
   */
  rtb_Add5 = ((17765 * (int16_T)rtb_Add2 + rtb_Add3_i) >> 12) +
    rtDW.UnitDelay3_DSTATE;

  /* ArithShift: '<S1>/Shift Arithmetic1' incorporates:
   *  Constant: '<S1>/Constant'
   *  Product: '<S1>/Product'
   *  Saturate: '<S1>/Saturation2'
   */
  rtb_Saturation2 = (rtb_Add5 * 10LL) >> 10;

  /* Saturate: '<S1>/Saturation2' */
  if (rtb_Saturation2 > 32167LL) {
    /* Outport: '<Root>/Speed' incorporates:
     *  DataTypeConversion: '<S1>/Data Type Conversion3'
     */
    rtY.Speed = 32167;
  } else if (rtb_Saturation2 < -32768LL) {
    /* Outport: '<Root>/Speed' incorporates:
     *  DataTypeConversion: '<S1>/Data Type Conversion3'
     */
    rtY.Speed = MIN_int16_T;
  } else {
    /* Outport: '<Root>/Speed' incorporates:
     *  DataTypeConversion: '<S1>/Data Type Conversion3'
     */
    rtY.Speed = (int16_T)rtb_Saturation2;
  }

  /* End of Saturate: '<S1>/Saturation2' */

  /* Sum: '<S5>/Add2' incorporates:
   *  ArithShift: '<S2>/Shift Arithmetic'
   *  ArithShift: '<S2>/Shift Arithmetic1'
   *  DataTypeConversion: '<S8>/Data Type Conversion'
   *  Gain: '<S1>/Gain6'
   *  Saturate: '<S8>/Saturation'
   *  Sum: '<S1>/Sum3'
   *  UnitDelay: '<S5>/Unit Delay'
   */
  rtb_Add2 = ((((24127 * (int16_T)rtb_Add2) >> 7) + rtb_Add5) << 5) +
    rtDW.UnitDelay_DSTATE;

  /* Outport: '<Root>/Angle_Filtered' incorporates:
   *  ArithShift: '<S3>/Shift Arithmetic'
   *  Constant: '<S1>/Constant5'
   *  Sum: '<S1>/Add4'
   *  Sum: '<S5>/Add2'
   */
  rtY.Angle_Filtered = (int16_T)((rtb_Add2 >> 17) + 16384);

  /* Update for UnitDelay: '<S1>/Unit Delay1' */
  rtDW.UnitDelay1_DSTATE = rtb_Add2;

  /* Update for UnitDelay: '<S4>/Unit Delay1' */
  rtDW.UnitDelay1_DSTATE_k = rtb_Add3_i;

  /* Update for UnitDelay: '<S6>/Unit Delay3' */
  rtDW.UnitDelay3_DSTATE = rtb_Add5;

  /* Update for UnitDelay: '<S5>/Unit Delay' */
  rtDW.UnitDelay_DSTATE = rtb_Add2;

  /* End of Outputs for RootInportFunctionCallGenerator generated from: '<Root>/Trigger' */
}

/* Model initialize function */
void AngleObserver_discrete_initialize(void)
{
  /* (no initialization code required) */
}

/*
 * File trailer for generated code.
 *
 * [EOF]
 */