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

finish the beta version of dji_motor

Browse Source
This commit is contained in:
NeoZng
2022-10-31 20:20:16 +08:00
parent 3dd4f1066c
commit c113ca81e0
20 changed files with 618 additions and 1137 deletions
Download Patch File Download Diff File Expand all files Collapse all files

204
modules/algorithm/user_lib.c
View File

@@ -51,36 +51,8 @@ float Sqrt(float x)
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 即一秒后增加输入的值
@@ -217,176 +189,4 @@ int float_rounding(float raw)
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;
}
}