393 lines
9.5 KiB
C
393 lines
9.5 KiB
C
/**
|
|
******************************************************************************
|
|
* @file user_lib.c
|
|
* @author Wang Hongxi
|
|
* @version V1.0.0
|
|
* @date 2021/2/18
|
|
* @brief
|
|
******************************************************************************
|
|
* @attention
|
|
*
|
|
******************************************************************************
|
|
*/
|
|
#include "stdlib.h"
|
|
#include "string.h"
|
|
#include "user_lib.h"
|
|
#include "math.h"
|
|
#include "main.h"
|
|
|
|
#ifdef _CMSIS_OS_H
|
|
#define user_malloc pvPortMalloc
|
|
#else
|
|
#define user_malloc malloc
|
|
#endif
|
|
|
|
uint8_t GlobalDebugMode = 7;
|
|
|
|
//快速开方
|
|
float Sqrt(float x)
|
|
{
|
|
float y;
|
|
float delta;
|
|
float maxError;
|
|
|
|
if (x <= 0)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
// initial guess
|
|
y = x / 2;
|
|
|
|
// refine
|
|
maxError = x * 0.001f;
|
|
|
|
do
|
|
{
|
|
delta = (y * y) - x;
|
|
y -= delta / (2 * y);
|
|
} while (delta > maxError || delta < -maxError);
|
|
|
|
return y;
|
|
}
|
|
|
|
//快速求平方根倒数
|
|
/*
|
|
float invSqrt(float num)
|
|
{
|
|
float halfnum = 0.5f * num;
|
|
float y = num;
|
|
long i = *(long *)&y;
|
|
i = 0x5f375a86- (i >> 1);
|
|
y = *(float *)&i;
|
|
y = y * (1.5f - (halfnum * y * y));
|
|
return y;
|
|
}*/
|
|
|
|
/**
|
|
* @brief 斜波函数初始化
|
|
* @author RM
|
|
* @param[in] 斜波函数结构体
|
|
* @param[in] 间隔的时间，单位 s
|
|
* @param[in] 最大值
|
|
* @param[in] 最小值
|
|
* @retval 返回空
|
|
*/
|
|
void ramp_init(ramp_function_source_t *ramp_source_type, float frame_period, float max, float min)
|
|
{
|
|
ramp_source_type->frame_period = frame_period;
|
|
ramp_source_type->max_value = max;
|
|
ramp_source_type->min_value = min;
|
|
ramp_source_type->input = 0.0f;
|
|
ramp_source_type->out = 0.0f;
|
|
}
|
|
|
|
/**
|
|
* @brief 斜波函数计算，根据输入的值进行叠加， 输入单位为 /s 即一秒后增加输入的值
|
|
* @author RM
|
|
* @param[in] 斜波函数结构体
|
|
* @param[in] 输入值
|
|
* @retval 返回空
|
|
*/
|
|
float ramp_calc(ramp_function_source_t *ramp_source_type, float input)
|
|
{
|
|
ramp_source_type->input = input;
|
|
ramp_source_type->out += ramp_source_type->input * ramp_source_type->frame_period;
|
|
if (ramp_source_type->out > ramp_source_type->max_value)
|
|
{
|
|
ramp_source_type->out = ramp_source_type->max_value;
|
|
}
|
|
else if (ramp_source_type->out < ramp_source_type->min_value)
|
|
{
|
|
ramp_source_type->out = ramp_source_type->min_value;
|
|
}
|
|
return ramp_source_type->out;
|
|
}
|
|
|
|
//绝对值限制
|
|
float abs_limit(float num, float Limit)
|
|
{
|
|
if (num > Limit)
|
|
{
|
|
num = Limit;
|
|
}
|
|
else if (num < -Limit)
|
|
{
|
|
num = -Limit;
|
|
}
|
|
return num;
|
|
}
|
|
|
|
//判断符号位
|
|
float sign(float value)
|
|
{
|
|
if (value >= 0.0f)
|
|
{
|
|
return 1.0f;
|
|
}
|
|
else
|
|
{
|
|
return -1.0f;
|
|
}
|
|
}
|
|
|
|
//浮点死区
|
|
float float_deadband(float Value, float minValue, float maxValue)
|
|
{
|
|
if (Value < maxValue && Value > minValue)
|
|
{
|
|
Value = 0.0f;
|
|
}
|
|
return Value;
|
|
}
|
|
|
|
//int26死区
|
|
int16_t int16_deadline(int16_t Value, int16_t minValue, int16_t maxValue)
|
|
{
|
|
if (Value < maxValue && Value > minValue)
|
|
{
|
|
Value = 0;
|
|
}
|
|
return Value;
|
|
}
|
|
|
|
//限幅函数
|
|
float float_constrain(float Value, float minValue, float maxValue)
|
|
{
|
|
if (Value < minValue)
|
|
return minValue;
|
|
else if (Value > maxValue)
|
|
return maxValue;
|
|
else
|
|
return Value;
|
|
}
|
|
|
|
//限幅函数
|
|
int16_t int16_constrain(int16_t Value, int16_t minValue, int16_t maxValue)
|
|
{
|
|
if (Value < minValue)
|
|
return minValue;
|
|
else if (Value > maxValue)
|
|
return maxValue;
|
|
else
|
|
return Value;
|
|
}
|
|
|
|
//循环限幅函数
|
|
float loop_float_constrain(float Input, float minValue, float maxValue)
|
|
{
|
|
if (maxValue < minValue)
|
|
{
|
|
return Input;
|
|
}
|
|
|
|
if (Input > maxValue)
|
|
{
|
|
float len = maxValue - minValue;
|
|
while (Input > maxValue)
|
|
{
|
|
Input -= len;
|
|
}
|
|
}
|
|
else if (Input < minValue)
|
|
{
|
|
float len = maxValue - minValue;
|
|
while (Input < minValue)
|
|
{
|
|
Input += len;
|
|
}
|
|
}
|
|
return Input;
|
|
}
|
|
|
|
//弧度格式化为-PI~PI
|
|
|
|
//角度格式化为-180~180
|
|
float theta_format(float Ang)
|
|
{
|
|
return loop_float_constrain(Ang, -180.0f, 180.0f);
|
|
}
|
|
|
|
int float_rounding(float raw)
|
|
{
|
|
static int integer;
|
|
static float decimal;
|
|
integer = (int)raw;
|
|
decimal = raw - integer;
|
|
if (decimal > 0.5f)
|
|
integer++;
|
|
return integer;
|
|
}
|
|
|
|
/**
|
|
* @brief 最小二乘法初始化
|
|
* @param[in] 最小二乘法结构体
|
|
* @param[in] 样本数
|
|
* @retval 返回空
|
|
*/
|
|
void OLS_Init(Ordinary_Least_Squares_t *OLS, uint16_t order)
|
|
{
|
|
OLS->Order = order;
|
|
OLS->Count = 0;
|
|
OLS->x = (float *)user_malloc(sizeof(float) * order);
|
|
OLS->y = (float *)user_malloc(sizeof(float) * order);
|
|
OLS->k = 0;
|
|
OLS->b = 0;
|
|
memset((void *)OLS->x, 0, sizeof(float) * order);
|
|
memset((void *)OLS->y, 0, sizeof(float) * order);
|
|
memset((void *)OLS->t, 0, sizeof(float) * 4);
|
|
}
|
|
|
|
/**
|
|
* @brief 最小二乘法拟合
|
|
* @param[in] 最小二乘法结构体
|
|
* @param[in] 信号新样本距上一个样本时间间隔
|
|
* @param[in] 信号值
|
|
*/
|
|
void OLS_Update(Ordinary_Least_Squares_t *OLS, float deltax, float y)
|
|
{
|
|
static float temp = 0;
|
|
temp = OLS->x[1];
|
|
for (uint16_t i = 0; i < OLS->Order - 1; ++i)
|
|
{
|
|
OLS->x[i] = OLS->x[i + 1] - temp;
|
|
OLS->y[i] = OLS->y[i + 1];
|
|
}
|
|
OLS->x[OLS->Order - 1] = OLS->x[OLS->Order - 2] + deltax;
|
|
OLS->y[OLS->Order - 1] = y;
|
|
|
|
if (OLS->Count < OLS->Order)
|
|
{
|
|
OLS->Count++;
|
|
}
|
|
memset((void *)OLS->t, 0, sizeof(float) * 4);
|
|
for (uint16_t i = OLS->Order - OLS->Count; i < OLS->Order; ++i)
|
|
{
|
|
OLS->t[0] += OLS->x[i] * OLS->x[i];
|
|
OLS->t[1] += OLS->x[i];
|
|
OLS->t[2] += OLS->x[i] * OLS->y[i];
|
|
OLS->t[3] += OLS->y[i];
|
|
}
|
|
|
|
OLS->k = (OLS->t[2] * OLS->Order - OLS->t[1] * OLS->t[3]) / (OLS->t[0] * OLS->Order - OLS->t[1] * OLS->t[1]);
|
|
OLS->b = (OLS->t[0] * OLS->t[3] - OLS->t[1] * OLS->t[2]) / (OLS->t[0] * OLS->Order - OLS->t[1] * OLS->t[1]);
|
|
|
|
OLS->StandardDeviation = 0;
|
|
for (uint16_t i = OLS->Order - OLS->Count; i < OLS->Order; ++i)
|
|
{
|
|
OLS->StandardDeviation += fabsf(OLS->k * OLS->x[i] + OLS->b - OLS->y[i]);
|
|
}
|
|
OLS->StandardDeviation /= OLS->Order;
|
|
}
|
|
|
|
/**
|
|
* @brief 最小二乘法提取信号微分
|
|
* @param[in] 最小二乘法结构体
|
|
* @param[in] 信号新样本距上一个样本时间间隔
|
|
* @param[in] 信号值
|
|
* @retval 返回斜率k
|
|
*/
|
|
float OLS_Derivative(Ordinary_Least_Squares_t *OLS, float deltax, float y)
|
|
{
|
|
static float temp = 0;
|
|
temp = OLS->x[1];
|
|
for (uint16_t i = 0; i < OLS->Order - 1; ++i)
|
|
{
|
|
OLS->x[i] = OLS->x[i + 1] - temp;
|
|
OLS->y[i] = OLS->y[i + 1];
|
|
}
|
|
OLS->x[OLS->Order - 1] = OLS->x[OLS->Order - 2] + deltax;
|
|
OLS->y[OLS->Order - 1] = y;
|
|
|
|
if (OLS->Count < OLS->Order)
|
|
{
|
|
OLS->Count++;
|
|
}
|
|
|
|
memset((void *)OLS->t, 0, sizeof(float) * 4);
|
|
for (uint16_t i = OLS->Order - OLS->Count; i < OLS->Order; ++i)
|
|
{
|
|
OLS->t[0] += OLS->x[i] * OLS->x[i];
|
|
OLS->t[1] += OLS->x[i];
|
|
OLS->t[2] += OLS->x[i] * OLS->y[i];
|
|
OLS->t[3] += OLS->y[i];
|
|
}
|
|
|
|
OLS->k = (OLS->t[2] * OLS->Order - OLS->t[1] * OLS->t[3]) / (OLS->t[0] * OLS->Order - OLS->t[1] * OLS->t[1]);
|
|
|
|
OLS->StandardDeviation = 0;
|
|
for (uint16_t i = OLS->Order - OLS->Count; i < OLS->Order; ++i)
|
|
{
|
|
OLS->StandardDeviation += fabsf(OLS->k * OLS->x[i] + OLS->b - OLS->y[i]);
|
|
}
|
|
OLS->StandardDeviation /= OLS->Order;
|
|
|
|
return OLS->k;
|
|
}
|
|
|
|
/**
|
|
* @brief 获取最小二乘法提取信号微分
|
|
* @param[in] 最小二乘法结构体
|
|
* @retval 返回斜率k
|
|
*/
|
|
float Get_OLS_Derivative(Ordinary_Least_Squares_t *OLS)
|
|
{
|
|
return OLS->k;
|
|
}
|
|
|
|
/**
|
|
* @brief 最小二乘法平滑信号
|
|
* @param[in] 最小二乘法结构体
|
|
* @param[in] 信号新样本距上一个样本时间间隔
|
|
* @param[in] 信号值
|
|
* @retval 返回平滑输出
|
|
*/
|
|
float OLS_Smooth(Ordinary_Least_Squares_t *OLS, float deltax, float y)
|
|
{
|
|
static float temp = 0;
|
|
temp = OLS->x[1];
|
|
for (uint16_t i = 0; i < OLS->Order - 1; ++i)
|
|
{
|
|
OLS->x[i] = OLS->x[i + 1] - temp;
|
|
OLS->y[i] = OLS->y[i + 1];
|
|
}
|
|
OLS->x[OLS->Order - 1] = OLS->x[OLS->Order - 2] + deltax;
|
|
OLS->y[OLS->Order - 1] = y;
|
|
|
|
if (OLS->Count < OLS->Order)
|
|
{
|
|
OLS->Count++;
|
|
}
|
|
|
|
memset((void *)OLS->t, 0, sizeof(float) * 4);
|
|
for (uint16_t i = OLS->Order - OLS->Count; i < OLS->Order; ++i)
|
|
{
|
|
OLS->t[0] += OLS->x[i] * OLS->x[i];
|
|
OLS->t[1] += OLS->x[i];
|
|
OLS->t[2] += OLS->x[i] * OLS->y[i];
|
|
OLS->t[3] += OLS->y[i];
|
|
}
|
|
|
|
OLS->k = (OLS->t[2] * OLS->Order - OLS->t[1] * OLS->t[3]) / (OLS->t[0] * OLS->Order - OLS->t[1] * OLS->t[1]);
|
|
OLS->b = (OLS->t[0] * OLS->t[3] - OLS->t[1] * OLS->t[2]) / (OLS->t[0] * OLS->Order - OLS->t[1] * OLS->t[1]);
|
|
|
|
OLS->StandardDeviation = 0;
|
|
for (uint16_t i = OLS->Order - OLS->Count; i < OLS->Order; ++i)
|
|
{
|
|
OLS->StandardDeviation += fabsf(OLS->k * OLS->x[i] + OLS->b - OLS->y[i]);
|
|
}
|
|
OLS->StandardDeviation /= OLS->Order;
|
|
|
|
return OLS->k * OLS->x[OLS->Order - 1] + OLS->b;
|
|
}
|
|
|
|
/**
|
|
* @brief 获取最小二乘法平滑信号
|
|
* @param[in] 最小二乘法结构体
|
|
* @retval 返回平滑输出
|
|
*/
|
|
float Get_OLS_Smooth(Ordinary_Least_Squares_t *OLS)
|
|
{
|
|
return OLS->k * OLS->x[OLS->Order - 1] + OLS->b;
|
|
}
|