motor_ctl-3/User/Src/math_tools.c

#include "math_tools.h"

/*
滑动均值滤波
InputData：最新采集值
Array：缓存队列
Length：缓存大小
运行一次后，缓存队列最后一个数据为最新采集值，前面数据依次前移，返回值为缓存队列平均值
*/
uint16_t MovingAverageFilter(uint16_t InputData, uint16_t* Array, uint16_t Length)
{
	uint16_t Result, i;
	
	Result = GetAverageData(Array, Length);
	
	for(i=0; i<(Length - 1); i++)
	{
	  Array[i] = Array[i+1];
	}
	
	Array[Length - 1] = InputData;

	return Result;
}

/*
取平均值
Input：输入数组
Length：有效数据长度
返回值为输入数组平均值 
*/
uint16_t GetAverageData(uint16_t* Input, uint16_t Length)
{
  uint16_t i,Result;
	uint32_t Sum = 0;
	
	for(i=0; i<Length; i++)
	{
	  Sum += Input[i];
	}
	
	Result = Sum / Length;
	return Result;
}

/*
三段曲线计算
0< InputData < Th1 :       y = 1 - (Th1 - InputData) * K1
Th1 < InputData < Th2 :    y = 1
InputData> Th2 :           y = 1 - (InputData - Th2) * K2
K1, K2, Result均放大1024倍
*/
uint16_t Function_Linear_3Stage(uint16_t Th1, int16_t K1, uint16_t Th2, int16_t K2, uint16_t InputData)
{
  int32_t Result;
	
	if(InputData < Th1)
	{
	  Result = 1024 - (Th1 - InputData) * K1; 
	}
	else if(InputData < Th2)
	{
	  Result = 1024;
	}
	else
	{
	  Result = 1024 - (InputData - Th2) * K2;
	}
	Result = (Result <= 0) ? 0 : Result;
	
	return (uint16_t)Result;
}

/*
系数增益计算
InputData <= Th:     y =  Min + (Max - Min) / Th * InputData
InputData > Th:      y = Max;
Min, Max均放大1024倍
*/
uint16_t Coefficient_GainCal(uint16_t Min, uint16_t Max, uint16_t Th, uint16_t InputData)
{
  uint16_t Result;
	uint16_t K;
	
	//计算K
	K = (Max - Min) * 1024 / Th;
	if(InputData > Th)
	{
		Result = Max;
		return Result;
	}
	else
	{
		Result = Min + (K * InputData >> 10);
		return Result;
	}
}

/*
三角波发生函数
Time_Zero：起始时刻，单位ms
Time1:上升结束点时刻，单位ms
Time2:下降结束点时刻，单位ms
PeakValue:Time1时刻输出值
Time_Zero和Time2时刻输出为0
*/
uint16_t TriangleWaveGenerate(uint32_t Time_Zero, uint16_t Time1, uint16_t Time2, uint16_t PeakValue)
{
  uint32_t K1 , K2;
	uint32_t DiffTime;
	uint16_t Result = 0;
	
	K1 = PeakValue * 1000 / Time1;
	K2 = PeakValue * 1000 / (Time2 - Time1);
	
	DiffTime = HAL_GetTick() - Time_Zero;
	DiffTime %= Time2;
	if(DiffTime < Time1)
	{
	  Result = DiffTime * K1 / 1000;
	}
	else if(DiffTime < Time2)
	{
	  Result = (Time2 - DiffTime) * K2 / 1000;
	}
	
	return Result;
}

/*
方波发生函数
Time_Zero：起始时刻，单位ms
Time1:输出结束点时刻，单位ms
Time2:输出PeakValue结束点时刻，单位ms
PeakValue:Time1-Time2时刻输出值
Time_Zero和Time2时刻输出为0
*/
uint16_t SquareWaveGenerate(uint32_t Time_Zero, uint16_t Time1, uint16_t Time2, uint16_t PeakValue)
{
  uint32_t DiffTime;
	uint16_t Result = 0;
		
	DiffTime = HAL_GetTick() - Time_Zero;
	DiffTime %= Time2;
	if(DiffTime < Time1)
	{
	  Result = 0;
	}
	else if(DiffTime < Time2)
	{
	  Result = PeakValue;
	}
	
	return Result;
}

/*
斜坡发生函数
Time_Zero：起始时刻，单位ms
Time1:输出结束点时刻，单位ms
PeakValue:最终输出值
*/
uint16_t RampWaveGenerate(uint32_t Time_Zero, uint16_t Time1, uint16_t PeakValue)
{
  uint32_t DiffTime;
	uint32_t K;
	uint16_t Result = 0;
	
	K = PeakValue * 1000 / Time1;
	
	DiffTime = HAL_GetTick() - Time_Zero;
	if(DiffTime < Time1)
	{
	  Result = DiffTime * K / 1000;
	}
	else
	{
	  Result = PeakValue;
	}
	
	return Result;
}

/*
获取最大值
*/
uint16_t GetMaxData(uint16_t* Array, uint16_t Length)
{
  uint16_t TempData = 0, i;
	
	for(i=0; i<Length; i++)
	{
	  if(TempData < Array[i])
		{
		  TempData = Array[i];
		}
	}
	
	return TempData;
}

/*
获取最小值
*/
uint16_t GetMinData(uint16_t* Array, uint16_t Length)
{
  uint16_t TempData = 0xFFFF, i;
	
	for(i=0; i<Length; i++)
	{
	  if(TempData > Array[i])
		{
		  TempData = Array[i];
		}
	}
	
	return TempData;
}

/*
计算标准差
*/
uint32_t GetStandardDeviation(uint16_t* Input, uint16_t Length)
{
  uint16_t AvgData, i;
	int32_t Deviation = 0;
	int32_t DiffData = 0;
	
	AvgData = GetAverageData(Input, Length);
	for(i=0; i<Length; i++)
	{
	  DiffData = (AvgData - Input[i]);
		Deviation += DiffData * DiffData;
	}
	DiffData = _sqrt(Deviation);
	
	return((uint32_t)DiffData);
}

/*
计算标准差,返回平均值
*/
uint16_t Standard_deviation_aver(uint16_t *data,uint8_t len, uint16_t *pAver)
{
	uint8_t 	i=0,j=0;
	float aver=0;
	double			sum=0,deviation=0;
	for(i=0;i<len;i++)
	{
		 sum+=data[i];	
	}
	aver=sum/len;
	*pAver = aver;
	for(j=0;j<len;j++)
	{
		deviation+=(data[j]-aver)*(data[j]-aver);
	}
	//deviation = deviation / len;
	deviation=(uint16_t)sqrt(deviation);
	return	deviation;
}

/*
检测数组中是否有重复数值
*/
TrueOrFalse_Flag_Struct_t CheckIsHasDouble(uint8_t* Array, uint16_t Length)
{
	uint16_t i = 0, j = 0;
  for (i = 0; i < Length; ++i)
  {
    for (j = i + 1; j < Length; ++j)
    {
      if (Array[i] == Array[j])
      {
        return TRUE;
      }
    }
  }
 
  return FALSE;
}

/******************************************************************************
* @Function:  uint32_t StepCalc(int16_t V_max,	\
                 int16_t V_Tc,	\
                 int16_t V_AccDecT)
* @Discrible: 步进计算   
* @Param:   V_max设定值最大值；V_Tc计算周期；V_AccDecT步进到最大值的总时间；
* @Return:  步进值 
* @Others:    
******************************************************************************
* @Recode    date       version	        author             modify 
*          ------------------------------------------------------------------
*          20180824      V1.1          Damon             modify
* 
*******************************************************************************/
uint32_t StepCalc(int16_t V_max,	\
                 int16_t V_Tc,	\
                 int16_t V_AccDecT)
{
		return ((uint32_t)V_max << 16) * V_Tc / V_AccDecT;
}


/******************************************************************************
* @Function:  int16_t accDecProcess(int16_t V_Set,	\
											uint32_t V_AccStep,  \
                      uint32_t V_DecStep)
* @Discrible: 任意量的加减速处理
* @Param:  目标设定值；加速步进；减速步进；
* @Return: 当前设定值  
* @Others:    
******************************************************************************
* @Recode    date       version	        author             modify 
*          ------------------------------------------------------------------
*          20190815      V1.2          Damon             modify
* 
*******************************************************************************/
int16_t accDecProcess(int16_t V_Set,	\
											uint32_t V_AccStep,  \
                      uint32_t V_DecStep, \
											int32_t* V_SetMiddle)
{
	int16_t Tmp16;
	int32_t Tmp32;
  int16_t V_Ret;
	
	Tmp32 = *V_SetMiddle;
	Tmp16 = (int16_t)( (*V_SetMiddle) >> 16);
	
	if(Tmp16 < V_Set)			// 加速 是否需要加状态标志后面考虑
	{
		Tmp32 += V_AccStep;
		V_Ret = (int16_t)(Tmp32 >> 16);
		if(V_Ret > V_Set)
		{
			V_Ret = V_Set;
			Tmp32 = (int32_t) V_Ret << 16;
		}
	}
	else if(Tmp16 > V_Set)	// 减速 是否需要加状态标志后面考虑
	{
		Tmp16 = ((int32_t) V_Set << 16) + V_DecStep;
		if(Tmp32 < Tmp16)
		{
			V_Ret = V_Set;
			Tmp32 = (int32_t) V_Ret << 16;
		}
		else
		{
			Tmp32 -= V_DecStep;
			V_Ret = (int16_t)(Tmp32 >> 16);
			if(V_Ret == 0)
			{
				Tmp32 = 0;
			}
		}
	}
	else	// 稳速 是否需要加状态标志后面考虑
	{
		V_Ret = V_Set;
	}
	
	*V_SetMiddle = Tmp32;
	
	return V_Ret;
}