FOC ST MediumFrequencyTask 分析(代码注释)

整体理解

理解为启动时为开环模式加速启动,到一定阶段转为FOC闭环处理。在运行过程中会根据外部输入命令进行处理。
流程简介:
在这里插入图片描述

在这里插入图片描述

STC_SPEED_MODE和STC_TORQUE_MODE在运行过程中的转换

开始启动的时候即revup 加速过程实际上是STC_TORQUE_MODE模式,运行到RUN的状态即FOC过程的时候转换为速度模式。
在SWITCH_OVER阶段使用 MCI_ExecBufferedCommands(&Mci[M1]);调用进行转换

调用关系为

系统定时器->MC_RunMotorControlTasks-> MC_Scheduler()
或 Systick is not executed due low priority so is necessary to TIMx_BRK_M1_IRQHandler call .系统定时器没有调用的情况下TIMx_BRK_M1_IRQHandler需要调用。
TIMx_BRK_M1_IRQHandler->MC_Scheduler()

MC_Scheduler(hMFTaskCounterM1递减计数为0)->MediumFrequencyTask

/**
  * @brief Executes medium frequency periodic Motor Control tasks
  *
  * This function performs some of the control duties on Motor 1 according to the
  * present state of its state machine. In particular, duties requiring a periodic
  * execution at a medium frequency rate (such as the speed controller for instance)
  * are executed here.
  */
__weak void TSK_MediumFrequencyTaskM1(void)
{
  int16_t wAux = 0;
  bool IsSpeedReliable = STO_PLL_CalcAvrgMecSpeedUnit(&STO_PLL_M1, &wAux);
  PQD_CalcElMotorPower(pMPM[M1]);//检测速度是不是可靠,如果不可靠后续运行时需要错误处理
  if (MCI_GetCurrentFaults(&Mci[M1]) == MC_NO_FAULTS)//检测是电流获取是否正常
  {
    if (MCI_GetOccurredFaults(&Mci[M1]) == MC_NO_FAULTS)//检测是否有错误
    {
      switch (Mci[M1].State)
      {
        case IDLE://空闲状态
        {
          if ((MCI_START == Mci[M1].DirectCommand) || (MCI_MEASURE_OFFSETS == Mci[M1].DirectCommand))
          {//收到运行命令
            RUC_Clear(&RevUpControlM1, MCI_GetImposedMotorDirection(&Mci[M1]));//初始化内部转速控制器状态
           if (pwmcHandle[M1]->offsetCalibStatus == false)//没有校准转入校准
           {
             PWMC_CurrentReadingCalibr(pwmcHandle[M1], CRC_START);//通过读取没有电流时的电流值电压值作为校准,每次启动都需要调用。
             Mci[M1].State = OFFSET_CALIB;
           }
           else//校准了就使能定时器相关通道,转入CHARGE_BOOT_CAP ,启动模式
           {
             /* calibration already done. Enables only TIM channels */
             pwmcHandle[M1]->OffCalibrWaitTimeCounter = 1u;
             PWMC_CurrentReadingCalibr(pwmcHandle[M1], CRC_EXEC);//通过读取没有电流时的电流值电压值作为校准,每次启动都需要调用。
             R3_2_TurnOnLowSides(pwmcHandle[M1]);//开启PWM定时器相关设置,有可能电容开始充电
             TSK_SetChargeBootCapDelayM1(CHARGE_BOOT_CAP_TICKS);//记录电容充电开始时间
             Mci[M1].State = CHARGE_BOOT_CAP;//转入BOOT 启动模式

           }

          }
          else
          {
            /* nothing to be done, FW stays in IDLE state */
          }
          break;
        }
        case OFFSET_CALIB:
          {
            if (MCI_STOP == Mci[M1].DirectCommand)//收到停止命令
            {
              TSK_MF_StopProcessing(&Mci[M1], M1);
            }
            else
            {
              if (PWMC_CurrentReadingCalibr(pwmcHandle[M1], CRC_EXEC))//通过读取没有电流时的电流值电压值作为校准,每次启动都需要调用。
              {
                if (MCI_MEASURE_OFFSETS == Mci[M1].DirectCommand)//根据以前的状态返回对应的应有状态。
                {
                  FOC_Clear(M1);
                  MPM_Clear((MotorPowMeas_Handle_t*) pMPM[M1]);
                  Mci[M1].DirectCommand = MCI_NO_COMMAND;
                  Mci[M1].State = IDLE;
                }
                else//从IDLE转启动启动跳过来的话转入启动
                {
                  R3_2_TurnOnLowSides(pwmcHandle[M1]);//前有介绍
                  TSK_SetChargeBootCapDelayM1(CHARGE_BOOT_CAP_TICKS);//前面有介绍
                  Mci[M1].State = CHARGE_BOOT_CAP;
                }
              }
              else
              {
                /* nothing to be done, FW waits for offset calibration to finish */
              }
            }
            break;
          }
        case CHARGE_BOOT_CAP:
        {
          if (MCI_STOP == Mci[M1].DirectCommand)//收到停止命令转入停止
          {
            TSK_MF_StopProcessing(&Mci[M1], M1);
          }
          else
          {
            if (TSK_ChargeBootCapDelayHasElapsedM1())//如果有开机电容需要判断到充电完成时间够再运行
            {
              R3_2_SwitchOffPWM(pwmcHandle[M1]);//关闭PWM
             FOCVars[M1].bDriveInput = EXTERNAL;//设置影响FOC_CalcCurrRef,使用INTERNAL是内部根据速度等计算获得,使用EXTERNAL
             STC_SetSpeedSensor( pSTC[M1], &VirtualSpeedSensorM1._Super );//设置速度传感器为虚拟传感器
              STO_PLL_Clear(&STO_PLL_M1);//STO滑模观测的状态清0,后续到一定速度后会启动检测
              FOC_Clear( M1 );//清除FOC状态
              Mci[M1].State = START;//转到开始状态
              PWMC_SwitchOnPWM(pwmcHandle[M1]);//开启PWM
            }
            else
            {
              /* nothing to be done, FW waits for bootstrap capacitor to charge */
            }
          }
          break;
        }
        case START:
        {
          if (MCI_STOP == Mci[M1].DirectCommand)//如果是停止命令则停止
          {
            TSK_MF_StopProcessing(&Mci[M1], M1);
          }
          else
          {
            /* Mechanical speed as imposed by the Virtual Speed Sensor during the Rev Up phase. */
            int16_t hForcedMecSpeedUnit;
            qd_t IqdRef;
            bool ObserverConverged = false;
            /* Execute the Rev Up procedure */
            if(! RUC_Exec(&RevUpControlM1))//进行加速过程,加速可以由多个步骤的加速设置组合而成
            {
            /* The time allowed for the startup sequence has expired 时间超过了允许的启动时间*/
              MCI_FaultProcessing(&Mci[M1], MC_START_UP, 0);

           }
           else
           {
             /* Execute the torque open loop current start-up ramp:进行扭矩开环启动上坡
              * Compute the Iq reference current as configured in the Rev Up sequence计算Iq电流参考加速的过程序列 */
             IqdRef.q = STC_CalcTorqueReference( pSTC[M1] );//计算Iq值
             IqdRef.d = FOCVars[M1].UserIdref;//Id使用用户设定值
             /* Iqd reference current used by the High Frequency Loop to generate the PWM output */
             FOCVars[M1].Iqdref = IqdRef;
           }
			//计算虚拟速度传感器的速度值
           (void) VSS_CalcAvrgMecSpeedUnit(&VirtualSpeedSensorM1, &hForcedMecSpeedUnit);
           /* check that startup stage where the observer has to be used has been reached 启动到了可以使用观测器的状态,可以使用滑膜观察器就可以使用闭环控制*/
           if (true == RUC_FirstAccelerationStageReached(&RevUpControlM1))
            {
             ObserverConverged = STO_PLL_IsObserverConverged(&STO_PLL_M1, &hForcedMecSpeedUnit);
             STO_SetDirection(&STO_PLL_M1, (int8_t)MCI_GetImposedMotorDirection(&Mci[M1]));
			//将命令设置为从虚拟速度传感器启动转换阶段到其他速度传感器。
              (void)VSS_SetStartTransition(&VirtualSpeedSensorM1, ObserverConverged);
            }

            if (ObserverConverged)//观测器是否收敛
            {
              qd_t StatorCurrent = MCM_Park(FOCVars[M1].Ialphabeta, SPD_GetElAngle(&STO_PLL_M1._Super));
              /* Start switch over ramp. This ramp will transition from the revup to the closed loop FOC. 结束斜坡,加速转换成闭环的FOC*/
              REMNG_Init(pREMNG[M1]);
              (void)REMNG_ExecRamp(pREMNG[M1], FOCVars[M1].Iqdref.q, 0);
              (void)REMNG_ExecRamp(pREMNG[M1], StatorCurrent.q, TRANSITION_DURATION);

              Mci[M1].State = SWITCH_OVER;
            }
          }
          break;
        }

        case SWITCH_OVER:
        {
          if (MCI_STOP == Mci[M1].DirectCommand)//是否停止
          {
            TSK_MF_StopProcessing(&Mci[M1], M1);
          }
          else
          {
            bool LoopClosed;
            int16_t hForcedMecSpeedUnit;

            if(! RUC_Exec(&RevUpControlM1))//进行加速过程

            {
              /* The time allowed for the startup sequence has expired */
              MCI_FaultProcessing(&Mci[M1], MC_START_UP, 0);
            }
            else

            {
              /* Compute the virtual speed and positions of the rotor.
                 The function returns true if the virtual speed is in the reliability range 计算转子的虚拟速度和位置。  如果虚拟速度在可靠性范围内,则该函数返回 true*/
              LoopClosed = VSS_CalcAvrgMecSpeedUnit(&VirtualSpeedSensorM1, &hForcedMecSpeedUnit);
              /* Check if the transition ramp has completed. */
              bool tempBool;
              //从虚拟速度传感器启动转换阶段到其他速度传感器是否完成判断。
              tempBool = VSS_TransitionEnded(&VirtualSpeedSensorM1);
              LoopClosed = LoopClosed || tempBool;

              /* If any of the above conditions is true, the loop is considered closed.
                 The state machine transitions to the START_RUN state. 如果上述任一条件为真,则认为循环是闭合的。状态机将转换为START_RUN状态。*/
              if (true ==  LoopClosed)
              {
                #if ( PID_SPEED_INTEGRAL_INIT_DIV == 0 )
                PID_SetIntegralTerm(&PIDSpeedHandle_M1, 0);
                #else
                PID_SetIntegralTerm(&PIDSpeedHandle_M1,
                                    (((int32_t)FOCVars[M1].Iqdref.q * (int16_t)PID_GetKIDivisor(&PIDSpeedHandle_M1))
                                    / PID_SPEED_INTEGRAL_INIT_DIV));
                #endif

                /* USER CODE BEGIN MediumFrequencyTask M1 1 */

                /* USER CODE END MediumFrequencyTask M1 1 */
                STC_SetSpeedSensor(pSTC[M1], &STO_PLL_M1._Super); /*Observer has converged*/
                FOC_InitAdditionalMethods(M1);
                FOC_CalcCurrRef( M1 );
                STC_ForceSpeedReferenceToCurrentSpeed(pSTC[M1]); /* Init the reference speed to current speed */
                MCI_ExecBufferedCommands(&Mci[M1]); /* Exec the speed ramp after changing of the speed sensor 斜坡起步后执行相关的命令,加减速等*/
                Mci[M1].State = RUN;
              }
            }
          }
          break;
        }

        case RUN://正常FOC运行
        {
          if (MCI_STOP == Mci[M1].DirectCommand)//停止命令
          {
            TSK_MF_StopProcessing(&Mci[M1], M1);
          }
          else
          {
            /* USER CODE BEGIN MediumFrequencyTask M1 2 */

            /* USER CODE END MediumFrequencyTask M1 2 */

            MCI_ExecBufferedCommands(&Mci[M1]);//前面有介绍
            FOC_CalcCurrRef(M1);//计算Iq电流值

            if(!IsSpeedReliable)
            {
              MCI_FaultProcessing(&Mci[M1], MC_SPEED_FDBK, 0);
            }

          }
          break;
        }

        case STOP:
        {
          if (TSK_StopPermanencyTimeHasElapsedM1())
          {

            STC_SetSpeedSensor(pSTC[M1], &VirtualSpeedSensorM1._Super);  	/*  sensor-less */
            VSS_Clear(&VirtualSpeedSensorM1); /* Reset measured speed in IDLE */

            /* USER CODE BEGIN MediumFrequencyTask M1 5 */

            /* USER CODE END MediumFrequencyTask M1 5 */
            Mci[M1].DirectCommand = MCI_NO_COMMAND;
            Mci[M1].State = IDLE;

          }
          else
          {
            /* nothing to do, FW waits for to stop */
          }
          break;
        }

        case FAULT_OVER:
        {
          if (MCI_ACK_FAULTS == Mci[M1].DirectCommand)
          {
            Mci[M1].DirectCommand = MCI_NO_COMMAND;
            Mci[M1].State = IDLE;

          }
          else
          {
            /* nothing to do, FW stays in FAULT_OVER state until acknowledgement */
          }
        }
        break;

        case FAULT_NOW:
        {
          Mci[M1].State = FAULT_OVER;
        }

        default:
          break;
       }
    }
    else
    {
      Mci[M1].State = FAULT_OVER;
    }
  }
  else
  {
    Mci[M1].State = FAULT_NOW;
  }

  /* USER CODE BEGIN MediumFrequencyTask M1 6 */

  /* USER CODE END MediumFrequencyTask M1 6 */
}

RUC 爬坡控制

根据设置控制速度和电流值,其中STC_ExecRamp是获取曲线的参数,

VSS_SetMecAcceleration将曲线对应参设置给电机控制。

/**
  * @brief  Main revup controller procedure executing overall programmed phases.
  * @param  pHandle: Pointer on Handle structure of RevUp controller.
  *  @retval Boolean set to false when entire revup phases have been completed.
  */
__weak bool RUC_Exec(RevUpCtrl_Handle_t *pHandle)
{
  bool retVal = true;
#ifdef NULL_PTR_REV_UP_CTL
  if (MC_NULL == pHandle)
  {
    retVal = false;
  }
  else
  {
#endif
    if (pHandle->hPhaseRemainingTicks > 0U)//加速时间没到0 减1
    {
      /* Decrease the hPhaseRemainingTicks.*/
      pHandle->hPhaseRemainingTicks--;

    } /* hPhaseRemainingTicks > 0 */

    if (0U == pHandle->hPhaseRemainingTicks)//一个加速过程是否结束
    {
      if (pHandle->pCurrentPhaseParams != MC_NULL)//后续没有加速过程,是否还有有的话使用新的加速过程
      {
        /* If it becomes zero the current phase has been completed.*/
        /* Gives the next command to STC and VSS.*/
        (void)STC_ExecRamp(pHandle->pSTC, pHandle->pCurrentPhaseParams->hFinalTorque * pHandle->hDirection,(uint32_t)(pHandle->pCurrentPhaseParams->hDurationms));

        VSS_SetMecAcceleration(pHandle->pVSS,pHandle->pCurrentPhaseParams->hFinalMecSpeedUnit * pHandle->hDirection,pHandle->pCurrentPhaseParams->hDurationms);//设置加速曲线参数

        /* Compute hPhaseRemainingTicks.*/
        pHandle->hPhaseRemainingTicks = (uint16_t)((((uint32_t)pHandle->pCurrentPhaseParams->hDurationms) * ((uint32_t)pHandle->hRUCFrequencyHz)) / 1000U );//设置对应曲线的运行时间
        pHandle->hPhaseRemainingTicks++;//这个我认为可以可无,估计是想保证不为0

        /*Set the next phases parameter pointer.*/
        pHandle->pCurrentPhaseParams = pHandle->pCurrentPhaseParams->pNext; //指向下个电流设置曲线阶段//cstat !MISRAC2012-Rule-11.5
        /*Increases the rev up stages counter.*/
        pHandle->bStageCnt++;//过程计数,用于知道第几步了。
      }
      else//所有加速过程结束,后续没有新的加速设置
      {
        retVal = false;
      }
    }
#ifdef NULL_PTR_REV_UP_CTL
  }
#endif
  return (retVal);
}

VSS_CalcAvrgMecSpeedUnit

/**
  * @brief  This method must be called with the same periodicity
  *         on which speed control is executed.
  *         This method computes and stores rotor instantaneous el speed (express
  *         in dpp considering the measurement frequency) in order to provide it
  *         to SPD_CalcElAngle function and SPD_GetElAngle.
  *         Then compute store and return - through parameter
  *         hMecSpeedUnit - the rotor average mech speed, expressed in the unit
  *         defined by #SPEED_UNIT. Then return the reliability state of the
  *         sensor (always true).
  * @param  pHandle: handler of the current instance of the VirtualSpeedSensor component
  * @param  hMecSpeedUnit pointer to int16_t, used to return the rotor average
  *         mechanical speed (SPED_UNIT)
  * @retval true = sensor information is reliable
  *         false = sensor information is not reliable
  */
__weak bool VSS_CalcAvrgMecSpeedUnit(VirtualSpeedSensor_Handle_t *pHandle, int16_t *hMecSpeedUnit)
{
  bool SpeedSensorReliability;
#ifdef NULL_PTR_VIR_SPD_SEN
  if ((MC_NULL == pHandle) || (MC_NULL == hMecSpeedUnit))
  {
    SpeedSensorReliability = false;
  }
  else
  {
#endif
    if (pHandle->hRemainingStep > 1u)//一个步数的计数判断,等于加速被切换成了多个阶段加速
    {
      pHandle->wElSpeedDpp32 += pHandle->wElAccDppP32;//一个32位的计数累加器一次加一个小的速度 32位精度较高,可以实现缓慢加速


//实际用的是是速度的高16位
#ifndef FULL_MISRA_C_COMPLIANCY_VIRT_SPD_SENS
      //cstat !MISRAC2012-Rule-1.3_n !ATH-shift-neg !MISRAC2012-Rule-10.1_R6
      pHandle->_Super.hElSpeedDpp = (int16_t)(pHandle->wElSpeedDpp32 >> 16);
#else
      pHandle->_Super.hElSpeedDpp = (int16_t)(pHandle->wElSpeedDpp32 / 65536);
#endif

      /* Convert dpp into MecUnit */
      //从每步的步速加的值计算电机的速度
      *hMecSpeedUnit = (int16_t)((((int32_t)pHandle->_Super.hElSpeedDpp)
                               * ((int32_t )pHandle->_Super.hMeasurementFrequency) * SPEED_UNIT)
                               / (((int32_t)pHandle->_Super.DPPConvFactor) * ((int32_t)pHandle->_Super.bElToMecRatio)));
      //保存电机的速度值
      pHandle->_Super.hAvrMecSpeedUnit = *hMecSpeedUnit;
      pHandle->hRemainingStep--;//步数减少
    }
    else if (1U == pHandle->hRemainingStep)//到最后一部前
    {
      *hMecSpeedUnit = pHandle->hFinalMecSpeedUnit;//电机速度应该等于设定速度了
      pHandle->_Super.hAvrMecSpeedUnit = *hMecSpeedUnit;//保存设定速度
      pHandle->_Super.hElSpeedDpp = (int16_t)((((int32_t)*hMecSpeedUnit) * ((int32_t)pHandle->_Super.DPPConvFactor)) / (((int32_t)SPEED_UNIT) * ((int32_t)pHandle->_Super.hMeasurementFrequency)));//从电机的设定速度计算每部的速度
      pHandle->_Super.hElSpeedDpp *= ((int16_t)pHandle->_Super.bElToMecRatio);//这个可以和上面合并
      pHandle->hRemainingStep = 0U;//加速步骤完成
    }
    else
    {
      *hMecSpeedUnit = pHandle->_Super.hAvrMecSpeedUnit;
    }
    /* If the transition is not done yet, we already know that speed is not reliable */
    //在TSK_HighFrequencyTask->VSS_CalcElAngle中会调用一无状态观测器到有状态观测转换的过程。转换步数完成后会设置转换结束pHandle->bTransitionEnded = true;
    if (false == pHandle->bTransitionEnded)
    {
      pHandle->_Super.bSpeedErrorNumber = pHandle->_Super.bMaximumSpeedErrorsNumber;
      SpeedSensorReliability = false;
    }
    else
    {
      SpeedSensorReliability = SPD_IsMecSpeedReliable(&pHandle->_Super, hMecSpeedUnit);
    }
#ifdef NULL_PTR_VIR_SPD_SEN
  }
#endif
  return (SpeedSensorReliability);
}
  • 8
    点赞
  • 33
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值