pi函数编写参考

最新推荐文章于 2024-04-10 07:17:36 发布

原创最新推荐文章于 2024-04-10 07:17:36 发布 · 233 阅读

0 ·

CC 4.0 BY-SA版权

文章标签：

#mcu #嵌入式硬件

motor 专栏收录该内容

9 篇文章

订阅专栏

1：pi参考代码

/** \brief Performs PI control algorithm.
 *
 * \param[inout] pPi Pointer to PI status
 * \param[in] Error Difference between reference and actual value
 *
 * \return PI output
 * \ingroup math_api
 */
 
__STATIC_INLINE sint16 Mat_ExePi(TMat_Pi *pPi, sint16 Error)
{
  sint32 IOut;
  sint32 PiOut;
  sint32 Min;
  sint32 Max;
  sint32 Temp;

  /* I output = old output + error * I parameter */
  IOut = pPi->IOut + ((sint32)Error * (sint32)pPi->Ki);
  
  /* Limit I output */
  Min = ((sint32)(pPi->IMin)) << MAT_FIX_SHIFT;
  if (IOut < Min)
  {
    IOut = Min;
  }  
  else
  {
    Max = ((sint32)(pPi->IMax)) << MAT_FIX_SHIFT;
    if (IOut > Max)
    {
      IOut = Max;
    }  
  }    
  /* Store I output */
  pPi->IOut = IOut;

  /* PI output = upper half of (I output + saturate(error * P parameter) * (1<<PID_KP_SCALE_SHIFT)) */
  Temp = __ssat(Error * ((sint32)pPi->Kp), 31u);
  PiOut = (IOut + Temp) >> MAT_FIX_SHIFT;
  
  /* Limit PI output */
  Min = (sint32)(pPi->PiMin);
  if (PiOut < Min)
  {
    PiOut = Min;
  }  
  else
  {
    Max = (sint32)(pPi->PiMax);
    if (PiOut > Max)
    {
      PiOut = Max;
    }  
  }    
  return (sint16)PiOut;

} /* End of Mat_ExePi*/

2：__ssat函数

/**
  \brief   Signed Saturate
  \details Saturates a signed value.
  \param [in]  value  Value to be saturated
  \param [in]    sat  Bit position to saturate to (1..32)
  \return             Saturated value
 */
 
__attribute__((always_inline)) __STATIC_INLINE int32_t __ssat(int64_t val, uint32 sat)
{
  if ((sat >= 1U) && (sat <= 32U))
  {
    const int32_t max = (int32_t)((1U << (sat - 1U)) - 1U);
    const int32_t min = -1 - max ;
    if (val > max)
    {
      return max;
    }
    else if (val < min)
    {
      return min;
    }
  }
  return (int32_t)val;
}