go/User/lib/calc_lib.c

#include "calc_lib.h"

//微秒延时
void user_delay_us(uint16_t us)
{
	 for(; us > 0; us--)
	 {
		 for(uint8_t i = 50; i > 0; i--)
		 {
				;
		 }
	 }
}

//毫秒延时
void user_delay_ms(uint16_t ms)
{
	 for(; ms > 0; ms--)
	 {
		user_delay_us(1000);
	 }
}

//浮点数范围限制
void abs_limit_fp(fp32 *num, fp32 Limit)
{
    if (*num > Limit)
    {
        *num = Limit;
    }
    else if (*num < -Limit)
    {
        *num = -Limit;
    }
}

//整数范围限制
void abs_limit_int(int64_t *num, int64_t Limit)
{
    if (*num > Limit)
    {
        *num = Limit;
    }
    else if (*num < -Limit)
    {
        *num = -Limit;
    }
}

//循环限幅
fp32 loop_fp32_constrain(fp32 Input, fp32 minValue, fp32 maxValue)
{
    if (maxValue < minValue)
    {
        return Input;
    }

    if (Input > maxValue)
    {
        fp32 len = maxValue - minValue;
        while (Input > maxValue)
        {
            Input -= len;
        }
    }
    else if (Input < minValue)
    {
        fp32 len = maxValue - minValue;
        while (Input < minValue)
        {
            Input += len;
        }
    }
    return Input;
}

int32_t loop_int32_constrain(int32_t Input, int32_t minValue, int32_t maxValue)
{
    if (maxValue < minValue)
    {
        return Input;
    }
			 if (Input > maxValue)
			{
        int32_t len = maxValue - minValue;
        while (Input > maxValue)
        {
            Input -= len;
        }
			}
			else if (Input < minValue)
			{
        int32_t len = maxValue - minValue;
        while (Input < minValue)
        {
            Input += len;
        }
			}
		
   
    return Input;
}

int map(int x, int in_min, int in_max, int out_min, int out_max)		//映射函数
{
	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

fp32 map_fp32(fp32 x, fp32 in_min, fp32 in_max, fp32 out_min, fp32 out_max)		//映射函数
{
	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}