c2812pmsmsim1.c

dsp控制电机时与位置传感器相关程序 以及位置传感器的说明

/*
 * File: c2812pmsmsim1.c
 *
 * Real-Time Workshop code generated for Simulink model c2812pmsmsim1.
 *
 * Model version                        : 1.287
 * Real-Time Workshop file version      : 7.0  (R2007b)  02-Aug-2007
 * Real-Time Workshop file generated on : Mon May 26 17:20:51 2008
 * TLC version                          : 7.0 (Jul 26 2007)
 * C source code generated on           : Mon May 26 17:20:56 2008
 */

#include "c2812pmsmsim1.h"
#include "c2812pmsmsim1_private.h"

/* Block signals (auto storage) */
BlockIO_c2812pmsmsim1 c2812pmsmsim1_B;

/* Block states (auto storage) */
D_Work_c2812pmsmsim1 c2812pmsmsim1_DWork;

/* Real-time model */
RT_MODEL_c2812pmsmsim1 c2812pmsmsim1_M_;
RT_MODEL_c2812pmsmsim1 *c2812pmsmsim1_M = &c2812pmsmsim1_M_;
static void rate_monotonic_scheduler(void);

/* Set which subrates need to run this base step (base rate always runs).
 * This function must be called prior to calling the model step function
 * in order to "remember" which rates need to run this base step.  The
 * buffering of events allows for overlapping preemption.
 */
void c2812pmsmsim1_SetEventsForThisBaseStep(boolean_T *eventFlags)
{
  /* Task runs when its counter is zero, computed via rtmStepTask macro */
  eventFlags[1] = ((boolean_T)rtmStepTask(c2812pmsmsim1_M, 1));
}

/* This function updates active task flag for each subrate
 * and rate transition flags for tasks that exchagne data.
 * The function assumes rate-monotonic multitasking scheduler.
 * The function must be called at model base rate so that
 * the generated code self-manages all its subrates and rate
 * transition flags.
 */
static void rate_monotonic_scheduler(void)
{
  /* Compute which subrates run during the next base time step.  Subrates
   * are an integer multiple of the base rate counter.  Therefore, the subtask
   * counter is reset when it reaches its limit (zero means run).
   */
  if (++c2812pmsmsim1_M->Timing.TaskCounters.TID[1] == 200) {/* Sample time: [0.01s, 0.0s] */
    c2812pmsmsim1_M->Timing.TaskCounters.TID[1] = 0;
  }
}

/* Model step function for TID0 */
void c2812pmsmsim1_step0(void)         /* Sample time: [0.00005s, 0.0s] */
{
  /* local block i/o variables */
  int32_T rtb_Switch1[2];
  int32_T rtb_MagnitudeIQN;
  int32_T rtb_InverseParkTransformation_o;
  int32_T rtb_SpaceVectorGenerator_o1;
  int32_T rtb_SpaceVectorGenerator_o2;
  int32_T rtb_SpaceVectorGenerator_o3;
  int32_T rtb_Add_b[3];
  int32_T rtb_shift[2];
  int32_T rtb_Switch1_l;
  int32_T rtb_IQN1toIQN2[3];
  int32_T rtb_InverseParkTransformation_j;
  uint16_T rtb_Switch;
  uint16_T rtb_Add_e;
  uint16_T rtb_BitwiseOperator;
  uint16_T rtb_DataTypeConversion[3];

  {                                    /* Sample time: [0.00005s, 0.0s] */
    rate_monotonic_scheduler();
  }

  /* UnitDelay: '<S15>/Unit Delay' */
  rtb_shift[0] = c2812pmsmsim1_DWork.UnitDelay_DSTATE[0];
  rtb_shift[1] = c2812pmsmsim1_DWork.UnitDelay_DSTATE[1];

  /* C28x IQmath Library (stiiqmath_iqmag) - '<S15>/Magnitude IQN' */
  {
    rtb_MagnitudeIQN = _IQ17mag (((int32_T)2127822848L), ((int32_T)131072L));
  }

  /* Switch: '<S15>/Switch1' incorporates:
   *  Constant: '<S13>/Constant4'
   *  Constant: '<S14>/Constant4'
   */
  if (rtb_MagnitudeIQN >= 131072L) {
    rtb_Switch1[0] = rtb_shift[0];
    rtb_Switch1[1] = rtb_shift[1];
  } else {
    rtb_Switch1[0] = 2127822848L;
    rtb_Switch1[1] = 131072L;
  }

  /* S-Function Block: <S2>/C28xQEP (c28xqep) */
  {
    c2812pmsmsim1_B.C28xQEP = EvaRegs.T2CNT;
  }

  /* S-Function Block: <S19>/CAPFIFOA1 (smemsrc) */
  {
    /* Memory Mapped Input */
    const uint16_T *memind = (uint16_T *) 29730U;
    c2812pmsmsim1_B.CAPFIFOA1 = *(uint16_T*)(memind++);
  }

  /* S-Function (sfix_bitop): '<S19>/Bitwise Operator' */
  rtb_BitwiseOperator = c2812pmsmsim1_B.CAPFIFOA1 & 8192U;

  /* If: '<S19>/If' */
  if (rtb_BitwiseOperator > 0U) {
    /* S-Function Block: <S28>/CAP1FBOT (smemsrc) */
    {
      /* Memory Mapped Input */
      const uint16_T *memind = (uint16_T *) 29737U;
      c2812pmsmsim1_B.CAP1FBOT = *(uint16_T*)(memind++);
    }
  }

  /* Switch: '<S19>/Switch' incorporates:
   *  UnitDelay: '<S19>/Unit Delay2'
   */
  if (rtb_BitwiseOperator != 0U) {
    rtb_Switch = c2812pmsmsim1_B.CAP1FBOT;
  } else {
    rtb_Switch = c2812pmsmsim1_DWork.UnitDelay2_DSTATE;
  }

  /* Sum: '<S22>/Add' incorporates:
   *  Constant: '<S22>/offset1'
   */
  rtb_Add_e = (uint16_T)((850 + (int16_T)c2812pmsmsim1_B.C28xQEP) - (int16_T)
    rtb_Switch);

  /* Switch: '<S22>/Switch' incorporates:
   *  Constant: '<S22>/negative direction offset'
   *  Sum: '<S22>/Add2'
   */
  if (rtb_Add_e >= 32768U) {
    rtb_Add_e = (uint16_T)((int16_T)rtb_Add_e - 1535);
  }

  /* Sum: '<S22>/Add1' incorporates:
   *  Constant: '<S22>/Encoder Resolution'
   *  Product: '<S22>/Divide'
   *  Product: '<S22>/Product'
   */
  rtb_Add_e = (uint16_T)((int16_T)rtb_Add_e - (int16_T)(rtb_Add_e / 2000U *
    2000U));

  /* DataTypeConversion: '<S22>/Data Type Conversion' incorporates:
   *  Gain: '<S22>/Gain'
   */
  rtb_InverseParkTransformation_j = (int32_T)(uint16_T)(1.024000049E+000F *
    (real32_T)rtb_Add_e);

  /* C28x IQmath Library (stiiqmath_iqntoiq) - '<S22>/IQN1 to IQN2' */
  {
    rtb_Switch1_l = (long)(rtb_InverseParkTransformation_j << 6);
  }

  /* ArithShift: '<S27>/shift' */
  c2812pmsmsim1_B.shift = rtb_BitwiseOperator >> 13;

  /* Sum: '<S19>/Add' incorporates:
   *  UnitDelay: '<S19>/Unit Delay1'
   */
  rtb_Add_e = c2812pmsmsim1_B.shift + c2812pmsmsim1_DWork.UnitDelay1_DSTATE;

  /* Switch: '<S19>/Switch1' */
  if (!(rtb_Add_e != 0U)) {
    rtb_Add_e = c2812pmsmsim1_B.shift;
  }

  /* Saturate: '<S19>/Saturation' */
  c2812pmsmsim1_B.Saturation = rt_MIN(rtb_Add_e, 1U);

  /* C28x DMC Library (tidmcrampcntl) - '<S24>/Ramp Control' */
  {
    int32_T* ptrrampDlyCntl = &c2812pmsmsim1_DWork.RampControl_RAMP_DLY_CNTL;
    int32_T* ptrOldSetPoint = &c2812pmsmsim1_DWork.RampControl_PREV_SETPOINT;
    if (((int32_T)13107L) != *ptrOldSetPoint ) {
      /* Original location of the update, now moved after the "if" branch */
      if (*ptrrampDlyCntl >= (long) 1.0) {
        if (((int32_T)13107L) >= *ptrOldSetPoint) {
          c2812pmsmsim1_B.RampControl_o1 = *ptrOldSetPoint + _IQ17(0.0000305);
          if (c2812pmsmsim1_B.RampControl_o1 > _IQ17(1.0))
            c2812pmsmsim1_B.RampControl_o1 = _IQ17(1.0);
          *ptrrampDlyCntl = 0;
        } else {
          c2812pmsmsim1_B.RampControl_o1 = *ptrOldSetPoint - _IQ17(0.0000305);
          if (c2812pmsmsim1_B.RampControl_o1 < _IQ17(0.0))
            c2812pmsmsim1_B.RampControl_o1 = _IQ17(0.0);
          *ptrrampDlyCntl = 0;
        }

        *ptrOldSetPoint++ = c2812pmsmsim1_B.RampControl_o1;
      }

      /* Moved the update here to get more consistent Simulink time change */
      *ptrrampDlyCntl += 1;
    } else {
      c2812pmsmsim1_B.RampControl_o1 = *ptrOldSetPoint;
      c2812pmsmsim1_B.RampControl_o2 = 0x7FFFFFFF;
    }

    ptrrampDlyCntl++;
  }

  /* C28x DMC Library (tidmcrampgen) - '<S24>/Ramp Generator' */
  {
    int32_T* angleregPtr = &c2812pmsmsim1_DWork.RampGenerator_ANGLE_REG;
    *angleregPtr += _IQ17mpy (c2812pmsmsim1_B.RampControl_o1, _IQ17(0.005));
    if (*angleregPtr > _IQ17(1))
      *angleregPtr -= _IQ17(1);
    else if (*angleregPtr < _IQ17(-1))
      *angleregPtr += _IQ17(1);
    rtb_InverseParkTransformation_j = _IQ17mpy (*angleregPtr++, ((int32_T)
      131072L)) + ((int32_T)0L);
    if (rtb_InverseParkTransformation_j > _IQ17(1))
      rtb_InverseParkTransformation_j -= _IQ17(1);
    else if (rtb_InverseParkTransformation_j < _IQ17(-1))
      rtb_InverseParkTransformation_j += _IQ17(1);
  }

  /* Switch: '<S4>/Switch1' */
  if (!(c2812pmsmsim1_B.Saturation != 0U)) {
    rtb_Switch1_l = rtb_InverseParkTransformation_j;
  }

  /* C28x DMC Library (tidmcipark) - '<S10>/Inverse Park Transformation' */
  {
    rtb_InverseParkTransformation_j = _IQ17mpy(rtb_Switch1[0], _IQ17cosPU
      (rtb_Switch1_l) ) - _IQ17mpy(rtb_Switch1[1], _IQ17sinPU(rtb_Switch1_l) );
    rtb_InverseParkTransformation_o = _IQ17mpy(rtb_Switch1[1], _IQ17cosPU
      (rtb_Switch1_l) ) + _IQ17mpy(rtb_Switch1[0], _IQ17sinPU(rtb_Switch1_l) );
  }

  /* C28x DMC Library (tidmcsvgendq) - '<S10>/Space Vector Generator' */
  {
    int16_T sector;
    int32_T Va, Vb, Vc;
    sector = 0;
    Va = rtb_InverseParkTransformation_o;
    Vb = _IQ17mpy (_IQ17(-0.5),rtb_InverseParkTransformation_o) + _IQ17mpy
      (_IQ17(0.8660254),rtb_InverseParkTransformation_j);
    Vc = _IQ17mpy (_IQ17(-0.5),rtb_InverseParkTransformation_o) - _IQ17mpy
      (_IQ17(0.8660254),rtb_InverseParkTransformation_j);
    if (Va > 0)
      sector = 1;
    if (Vb > 0)
      sector = sector + 2;
    if (Vc > 0)
      sector = sector + 4;
    Vb = _IQ17mpy (_IQ17(0.5),rtb_InverseParkTransformation_o) + _IQ17mpy (_IQ17
      (0.8660254),rtb_InverseParkTransformation_j);
    Vc = _IQ17mpy (_IQ17(0.5),rtb_InverseParkTransformation_o) - _IQ17mpy (_IQ17
      (0.8660254),rtb_InverseParkTransformation_j);
    if (sector==0) {
      rtb_SpaceVectorGenerator_o1 = _IQ17(0.5);
      rtb_SpaceVectorGenerator_o2 = _IQ17(0.5);
      rtb_SpaceVectorGenerator_o3 = _IQ17(0.5);
    } else if (sector==1) {
      rtb_SpaceVectorGenerator_o2 = _IQ17mpy (_IQ17(0.5), (_IQ17(1) - Vc - Vb));
      rtb_SpaceVectorGenerator_o1 = rtb_SpaceVectorGenerator_o2 + Vc;
      rtb_SpaceVectorGenerator_o3 = rtb_SpaceVectorGenerator_o1 + Vb;
    } else if (sector==2) {
      rtb_SpaceVectorGenerator_o1 = _IQ17mpy (_IQ17(0.5), (_IQ17(1) - Vb + Va));
      rtb_SpaceVectorGenerator_o3 = rtb_SpaceVectorGenerator_o1 + Vb;
      rtb_SpaceVectorGenerator_o2 = rtb_SpaceVectorGenerator_o3 - Va;
    } else if (sector==3) {
      rtb_SpaceVectorGenerator_o1 = _IQ17mpy (_IQ17(0.5), (_IQ17(1) + Vc - Va));
      rtb_SpaceVectorGenerator_o2 = rtb_SpaceVectorGenerator_o1 - Vc;
      rtb_SpaceVectorGenerator_o3 = rtb_SpaceVectorGenerator_o2 + Va;
    } else if (sector==4) {
      rtb_SpaceVectorGenerator_o3 = _IQ17mpy (_IQ17(0.5), (_IQ17(1) + Va - Vc));
      rtb_SpaceVectorGenerator_o2 = rtb_SpaceVectorGenerator_o3 - Va;
      rtb_SpaceVectorGenerator_o1 = rtb_SpaceVectorGenerator_o2 + Vc;
    } else if (sector==5) {
      rtb_SpaceVectorGenerator_o2 = _IQ17mpy (_IQ17(0.5), (_IQ17(1) - Va + Vb));
      rtb_SpaceVectorGenerator_o3 = rtb_SpaceVectorGenerator_o2 + Va;
      rtb_SpaceVectorGenerator_o1 = rtb_SpaceVectorGenerator_o3 - Vb;
    } else if (sector==6) {
      rtb_SpaceVectorGenerator_o3 = _IQ17mpy (_IQ17(0.5), (_IQ17(1) + Vb + Vc));
      rtb_SpaceVectorGenerator_o1 = rtb_SpaceVectorGenerator_o3 - Vb;
      rtb_SpaceVectorGenerator_o2 = rtb_SpaceVectorGenerator_o1 - Vc;
    }

    rtb_SpaceVectorGenerator_o1 = _IQ17mpy (_IQ17(2),
      (rtb_SpaceVectorGenerator_o1 - _IQ17(0.5)));
    rtb_SpaceVectorGenerator_o2 = _IQ17mpy (_IQ17(2),
      (rtb_SpaceVectorGenerator_o2 - _IQ17(0.5)));
    rtb_SpaceVectorGenerator_o3 = _IQ17mpy (_IQ17(2),
      (rtb_SpaceVectorGenerator_o3 - _IQ17(0.5)));
  }

  /* Sum: '<S11>/Add' incorporates:
   *  Constant: '<S11>/Constant'
   */
  rtb_Add_b[0] = 131072L + rtb_SpaceVectorGenerator_o1;
  rtb_Add_b[1] = 131072L + rtb_SpaceVectorGenerator_o2;
  rtb_Add_b[2] = 131072L + rtb_SpaceVectorGenerator_o3;

  /* C28x IQmath Library (stiiqmath_iqntoiq) - '<S11>/IQN1 to IQN2' */
  {
    rtb_IQN1toIQN2[0] = (long)(rtb_Add_b[0] >> 6);
    rtb_IQN1toIQN2[1] = (long)(rtb_Add_b[1] >> 6);
    rtb_IQN1toIQN2[2] = (long)(rtb_Add_b[2] >> 6);
  }

  /* DataTypeConversion: '<S11>/Data Type Conversion' */
  rtb_DataTypeConversion[0] = (uint16_T)rtb_IQN1toIQN2[0];
  rtb_DataTypeConversion[1] = (uint16_T)rtb_IQN1toIQN2[1];
  rtb_DataTypeConversion[2] = (uint16_T)rtb_IQN1toIQN2[2];

  /* S-Function Block: <Root>/C28x PWM (c28xpwm) */
  {
    EvaRegs.CMPR1 = (uint16_T) (rtb_DataTypeConversion[0] / 2);
    EvaRegs.CMPR2 = (uint16_T) (rtb_DataTypeConversion[1] / 2);
    EvaRegs.CMPR3 = (uint16_T) (rtb_DataTypeConversion[2] / 2);
  }

  /* S-Function Block: <S2>/C28x ADC (c28xadc) */
  {
    AdcRegs.ADCTRL2.bit.RST_SEQ1 = 0x1;// Sequencer reset
    AdcRegs.ADCTRL2.bit.SOC_SEQ1 = 0x1;// Software start of conversion
    while (AdcRegs.ADCST.bit.INT_SEQ1 == 0) {