TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 03:27:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

init commit

Browse Source
This commit is contained in:
NeoZng
2022-10-20 17:13:02 +08:00
commit 78f85b7fda
518 changed files with 409143 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

392
modules/algorithm/user_lib.c Normal file
View File

@@ -0,0 +1,392 @@
/**
******************************************************************************
* @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;
}