TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 11:37: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

348
modules/algorithm/controller.c
View File

@@ -1,16 +1,6 @@
/**
******************************************************************************
* @file controller.c
* @author Wang Hongxi
* @version V1.1.3
* @date 2021/7/3
* @brief DWT定时器用于计算控制周期 OLS用于提取信号微分
******************************************************************************
* @attention
*
******************************************************************************
*/
#include "controller.h"
#include <memory.h>
/******************************* PID CONTROL *********************************/
// PID优化环节函数声明
@@ -25,67 +15,23 @@ static void f_Proportion_Limit(PID_t *pid);
static void f_PID_ErrorHandle(PID_t *pid);
/**
* @brief PID初始化 PID initialize
* @param[in] PID结构体 PID structure
* @param[in] 略
* @retval 返回空 null
* @brief
*
* @param pid
* @param config
*/
void PID_Init(
PID_t *pid,
float max_out,
float intergral_limit,
float deadband,
float kp,
float Ki,
float Kd,
float A,
float B,
float output_lpf_rc,
float derivative_lpf_rc,
uint16_t ols_order,
uint8_t improve)
void PID_Init(PID_t* pid,PID_Init_config_s *config)
{
pid->DeadBand = deadband;
pid->IntegralLimit = intergral_limit;
pid->MaxOut = max_out;
pid->Ref = 0;
memcpy(pid, config, sizeof(PID_Init_config_s));
memset(&pid->Measure,0,sizeof(PID_t)-sizeof(PID_Init_config_s));
// // DWT定时器计数变量清零
// // reset DWT Timer count counter
// pid->DWT_CNT = 0;
pid->Kp = kp;
pid->Ki = Ki;
pid->Kd = Kd;
pid->ITerm = 0;
// 变速积分参数
// coefficient of changing integration rate
pid->CoefA = A;
pid->CoefB = B;
pid->Output_LPF_RC = output_lpf_rc;
pid->Derivative_LPF_RC = derivative_lpf_rc;
// 最小二乘提取信号微分初始化
// differential signal is distilled by OLS
pid->OLS_Order = ols_order;
OLS_Init(&pid->OLS, ols_order);
// DWT定时器计数变量清零
// reset DWT Timer count counter
pid->DWT_CNT = 0;
// 设置PID优化环节
pid->Improve = improve;
// 设置PID异常处理 目前仅包含电机堵转保护
pid->ERRORHandler.ERRORCount = 0;
pid->ERRORHandler.ERRORType = PID_ERROR_NONE;
pid->Output = 0;
// // 设置PID异常处理 目前仅包含电机堵转保护
// pid->ERRORHandler.ERRORCount = 0;
// pid->ERRORHandler.ERRORType = PID_ERROR_NONE;
}
/**
@@ -106,21 +52,11 @@ float PID_Calculate(PID_t *pid, float measure, float ref)
pid->Ref = ref;
pid->Err = pid->Ref - pid->Measure;
if (pid->User_Func1_f != NULL)
pid->User_Func1_f(pid);
if (abs(pid->Err) > pid->DeadBand)
{
pid->Pout = pid->Kp * pid->Err;
pid->ITerm = pid->Ki * pid->Err * pid->dt;
if (pid->OLS_Order > 2)
pid->Dout = pid->Kd * OLS_Derivative(&pid->OLS, pid->dt, pid->Err);
else
pid->Dout = pid->Kd * (pid->Err - pid->Last_Err) / pid->dt;
if (pid->User_Func2_f != NULL)
pid->User_Func2_f(pid);
pid->Dout = pid->Kd * (pid->Err - pid->Last_Err) / pid->dt;
// 梯形积分
if (pid->Improve & Trapezoid_Intergral)
@@ -139,20 +75,14 @@ float PID_Calculate(PID_t *pid, float measure, float ref)
f_Integral_Limit(pid);
pid->Iout += pid->ITerm;
pid->Output = pid->Pout + pid->Iout + pid->Dout;
// 输出滤波
if (pid->Improve & OutputFilter)
f_Output_Filter(pid);
// 输出限幅
f_Output_Limit(pid);
// 无关紧要
f_Proportion_Limit(pid);
}
pid->Last_Measure = pid->Measure;
pid->Last_Output = pid->Output;
pid->Last_Dout = pid->Dout;
@@ -166,7 +96,6 @@ static void f_Trapezoid_Intergral(PID_t *pid)
{
pid->ITerm = pid->Ki * ((pid->Err + pid->Last_Err) / 2) * pid->dt;
}
static void f_Changing_Integration_Rate(PID_t *pid)
@@ -213,11 +142,7 @@ static void f_Integral_Limit(PID_t *pid)
static void f_Derivative_On_Measurement(PID_t *pid)
{
if (pid->OLS_Order > 2)
pid->Dout = pid->Kd * OLS_Derivative(&pid->OLS, pid->dt, -pid->Measure);
else
pid->Dout = pid->Kd * (pid->Last_Measure - pid->Measure) / pid->dt;
pid->Dout = pid->Kd * (pid->Last_Measure - pid->Measure) / pid->dt;
}
static void f_Derivative_Filter(PID_t *pid)
@@ -244,18 +169,6 @@ static void f_Output_Limit(PID_t *pid)
}
}
static void f_Proportion_Limit(PID_t *pid)
{
if (pid->Pout > pid->MaxOut)
{
pid->Pout = pid->MaxOut;
}
if (pid->Pout < -(pid->MaxOut))
{
pid->Pout = -(pid->MaxOut);
}
}
// PID ERRORHandle Function
static void f_PID_ErrorHandle(PID_t *pid)
{
@@ -278,227 +191,4 @@ static void f_PID_ErrorHandle(PID_t *pid)
// Motor blocked over 1000times
pid->ERRORHandler.ERRORType = Motor_Blocked;
}
}
/*************************** FEEDFORWARD CONTROL *****************************/
/**
* @brief 前馈控制初始化
* @param[in] 前馈控制结构体
* @param[in] 略
* @retval 返回空
*/
void Feedforward_Init(
Feedforward_t *ffc,
float max_out,
float *c,
float lpf_rc,
uint16_t ref_dot_ols_order,
uint16_t ref_ddot_ols_order)
{
ffc->MaxOut = max_out;
// 设置前馈控制器参数 详见前馈控制结构体定义
// set parameters of feed-forward controller (see struct definition)
if (c != NULL && ffc != NULL)
{
ffc->c[0] = c[0];
ffc->c[1] = c[1];
ffc->c[2] = c[2];
}
else
{
ffc->c[0] = 0;
ffc->c[1] = 0;
ffc->c[2] = 0;
ffc->MaxOut = 0;
}
ffc->LPF_RC = lpf_rc;
// 最小二乘提取信号微分初始化
// differential signal is distilled by OLS
ffc->Ref_dot_OLS_Order = ref_dot_ols_order;
ffc->Ref_ddot_OLS_Order = ref_ddot_ols_order;
if (ref_dot_ols_order > 2)
OLS_Init(&ffc->Ref_dot_OLS, ref_dot_ols_order);
if (ref_ddot_ols_order > 2)
OLS_Init(&ffc->Ref_ddot_OLS, ref_ddot_ols_order);
ffc->DWT_CNT = 0;
ffc->Output = 0;
}
/**
* @brief PID计算
* @param[in] PID结构体
* @param[in] 测量值
* @param[in] 期望值
* @retval 返回空
*/
float Feedforward_Calculate(Feedforward_t *ffc, float ref)
{
ffc->dt = DWT_GetDeltaT((void *)&ffc->DWT_CNT);
ffc->Ref = ref * ffc->dt / (ffc->LPF_RC + ffc->dt) +
ffc->Ref * ffc->LPF_RC / (ffc->LPF_RC + ffc->dt);
// 计算一阶导数
// calculate first derivative
if (ffc->Ref_dot_OLS_Order > 2)
ffc->Ref_dot = OLS_Derivative(&ffc->Ref_dot_OLS, ffc->dt, ffc->Ref);
else
ffc->Ref_dot = (ffc->Ref - ffc->Last_Ref) / ffc->dt;
// 计算二阶导数
// calculate second derivative
if (ffc->Ref_ddot_OLS_Order > 2)
ffc->Ref_ddot = OLS_Derivative(&ffc->Ref_ddot_OLS, ffc->dt, ffc->Ref_dot);
else
ffc->Ref_ddot = (ffc->Ref_dot - ffc->Last_Ref_dot) / ffc->dt;
// 计算前馈控制输出
// calculate feed-forward controller output
ffc->Output = ffc->c[0] * ffc->Ref + ffc->c[1] * ffc->Ref_dot + ffc->c[2] * ffc->Ref_ddot;
ffc->Output = float_constrain(ffc->Output, -ffc->MaxOut, ffc->MaxOut);
ffc->Last_Ref = ffc->Ref;
ffc->Last_Ref_dot = ffc->Ref_dot;
return ffc->Output;
}
/*************************LINEAR DISTURBANCE OBSERVER *************************/
void LDOB_Init(
LDOB_t *ldob,
float max_d,
float deadband,
float *c,
float lpf_rc,
uint16_t measure_dot_ols_order,
uint16_t measure_ddot_ols_order)
{
ldob->Max_Disturbance = max_d;
ldob->DeadBand = deadband;
// 设置线性扰动观测器参数 详见LDOB结构体定义
// set parameters of linear disturbance observer (see struct definition)
if (c != NULL && ldob != NULL)
{
ldob->c[0] = c[0];
ldob->c[1] = c[1];
ldob->c[2] = c[2];
}
else
{
ldob->c[0] = 0;
ldob->c[1] = 0;
ldob->c[2] = 0;
ldob->Max_Disturbance = 0;
}
// 设置Q(s)带宽 Q(s)选用一阶惯性环节
// set bandwidth of Q(s) Q(s) is chosen as a first-order low-pass form
ldob->LPF_RC = lpf_rc;
// 最小二乘提取信号微分初始化
// differential signal is distilled by OLS
ldob->Measure_dot_OLS_Order = measure_dot_ols_order;
ldob->Measure_ddot_OLS_Order = measure_ddot_ols_order;
if (measure_dot_ols_order > 2)
OLS_Init(&ldob->Measure_dot_OLS, measure_dot_ols_order);
if (measure_ddot_ols_order > 2)
OLS_Init(&ldob->Measure_ddot_OLS, measure_ddot_ols_order);
ldob->DWT_CNT = 0;
ldob->Disturbance = 0;
}
float LDOB_Calculate(LDOB_t *ldob, float measure, float u)
{
ldob->dt = DWT_GetDeltaT((void *)&ldob->DWT_CNT);
ldob->Measure = measure;
ldob->u = u;
// 计算一阶导数
// calculate first derivative
if (ldob->Measure_dot_OLS_Order > 2)
ldob->Measure_dot = OLS_Derivative(&ldob->Measure_dot_OLS, ldob->dt, ldob->Measure);
else
ldob->Measure_dot = (ldob->Measure - ldob->Last_Measure) / ldob->dt;
// 计算二阶导数
// calculate second derivative
if (ldob->Measure_ddot_OLS_Order > 2)
ldob->Measure_ddot = OLS_Derivative(&ldob->Measure_ddot_OLS, ldob->dt, ldob->Measure_dot);
else
ldob->Measure_ddot = (ldob->Measure_dot - ldob->Last_Measure_dot) / ldob->dt;
// 估计总扰动
// estimate external disturbances and internal disturbances caused by model uncertainties
ldob->Disturbance = ldob->c[0] * ldob->Measure + ldob->c[1] * ldob->Measure_dot + ldob->c[2] * ldob->Measure_ddot - ldob->u;
ldob->Disturbance = ldob->Disturbance * ldob->dt / (ldob->LPF_RC + ldob->dt) +
ldob->Last_Disturbance * ldob->LPF_RC / (ldob->LPF_RC + ldob->dt);
ldob->Disturbance = float_constrain(ldob->Disturbance, -ldob->Max_Disturbance, ldob->Max_Disturbance);
// 扰动输出死区
// deadband of disturbance output
if (abs(ldob->Disturbance) > ldob->DeadBand * ldob->Max_Disturbance)
ldob->Output = ldob->Disturbance;
else
ldob->Output = 0;
ldob->Last_Measure = ldob->Measure;
ldob->Last_Measure_dot = ldob->Measure_dot;
ldob->Last_Disturbance = ldob->Disturbance;
return ldob->Output;
}
/*************************** Tracking Differentiator ***************************/
void TD_Init(TD_t *td, float r, float h0)
{
td->r = r;
td->h0 = h0;
td->x = 0;
td->dx = 0;
td->ddx = 0;
td->last_dx = 0;
td->last_ddx = 0;
}
float TD_Calculate(TD_t *td, float input)
{
static float d, a0, y, a1, a2, a, fhan;
td->dt = DWT_GetDeltaT((void *)&td->DWT_CNT);
if (td->dt > 0.5f)
return 0;
td->Input = input;
d = td->r * td->h0 * td->h0;
a0 = td->dx * td->h0;
y = td->x - td->Input + a0;
a1 = sqrt(d * (d + 8 * abs(y)));
a2 = a0 + sign(y) * (a1 - d) / 2;
a = (a0 + y) * (sign(y + d) - sign(y - d)) / 2 + a2 * (1 - (sign(y + d) - sign(y - d)) / 2);
fhan = -td->r * a / d * (sign(a + d) - sign(a - d)) / 2 -
td->r * sign(a) * (1 - (sign(a + d) - sign(a - d)) / 2);
td->ddx = fhan;
td->dx += (td->ddx + td->last_ddx) * td->dt / 2;
td->x += (td->dx + td->last_dx) * td->dt / 2;
td->last_ddx = td->ddx;
td->last_dx = td->dx;
return td->x;
}
}

156
modules/algorithm/controller.h
View File

@@ -19,7 +19,6 @@
#include "string.h"
#include "stdlib.h"
#include "bsp_dwt.h"
#include "user_lib.h"
#include "arm_math.h"
#include <math.h>
@@ -27,13 +26,6 @@
#define abs(x) ((x > 0) ? x : -x)
#endif
#ifndef user_malloc
#ifdef _CMSIS_OS_H
#define user_malloc pvPortMalloc
#else
#define user_malloc malloc
#endif
#endif
/******************************* PID CONTROL *********************************/
typedef enum pid_Improvement_e
@@ -55,19 +47,31 @@ typedef enum errorType_e
Motor_Blocked = 0x01U
} ErrorType_e;
typedef __packed struct
typedef struct
{
uint64_t ERRORCount;
ErrorType_e ERRORType;
} PID_ErrorHandler_t;
typedef __packed struct pid_t
typedef struct
{
float Ref;
//---------------------------------- init config block
// config parameter
float Kp;
float Ki;
float Kd;
float MaxOut;
float IntegralLimit;
float DeadBand;
float CoefA; //For Changing Integral
float CoefB; //ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
float Output_LPF_RC; // RC = 1/omegac
float Derivative_LPF_RC;
uint8_t Improve;
//-----------------------------------
// for calculating
float Measure;
float Last_Measure;
float Err;
@@ -83,134 +87,42 @@ typedef __packed struct pid_t
float Last_Output;
float Last_Dout;
float Ref;
uint32_t DWT_CNT;
float dt;
PID_ErrorHandler_t ERRORHandler;
} PID_t;
/* 用于PID初始化的结构体*/
typedef struct
{
// config parameter
float Kp;
float Ki;
float Kd;
float MaxOut;
float IntegralLimit;
float DeadBand;
float ControlPeriod;
float CoefA; //For Changing Integral
float CoefB; //ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
float Output_LPF_RC; // RC = 1/omegac
float Derivative_LPF_RC;
uint16_t OLS_Order;
Ordinary_Least_Squares_t OLS;
uint32_t DWT_CNT;
float dt;
uint8_t Improve;
PID_ErrorHandler_t ERRORHandler;
} PID_Init_config_s;
void (*User_Func1_f)(struct pid_t *pid);
void (*User_Func2_f)(struct pid_t *pid);
} PID_t;
void PID_Init(
PID_t *pid,
float max_out,
float intergral_limit,
float deadband,
float kp,
float ki,
float kd,
float A,
float B,
float output_lpf_rc,
float derivative_lpf_rc,
uint16_t ols_order,
uint8_t improve);
void PID_Init(PID_t* pid,PID_Init_config_s* config);
float PID_Calculate(PID_t *pid, float measure, float ref);
/*************************** FEEDFORWARD CONTROL *****************************/
typedef __packed struct
{
float c[3]; // G(s) = 1/(c2s^2 + c1s + c0)
float Ref;
float Last_Ref;
float DeadBand;
uint32_t DWT_CNT;
float dt;
float LPF_RC; // RC = 1/omegac
float Ref_dot;
float Ref_ddot;
float Last_Ref_dot;
uint16_t Ref_dot_OLS_Order;
Ordinary_Least_Squares_t Ref_dot_OLS;
uint16_t Ref_ddot_OLS_Order;
Ordinary_Least_Squares_t Ref_ddot_OLS;
float Output;
float MaxOut;
} Feedforward_t;
void Feedforward_Init(
Feedforward_t *ffc,
float max_out,
float *c,
float lpf_rc,
uint16_t ref_dot_ols_order,
uint16_t ref_ddot_ols_order);
float Feedforward_Calculate(Feedforward_t *ffc, float ref);
/************************* LINEAR DISTURBANCE OBSERVER *************************/
typedef __packed struct
{
float c[3]; // G(s) = 1/(c2s^2 + c1s + c0)
float Measure;
float Last_Measure;
float u; // system input
float DeadBand;
uint32_t DWT_CNT;
float dt;
float LPF_RC; // RC = 1/omegac
float Measure_dot;
float Measure_ddot;
float Last_Measure_dot;
uint16_t Measure_dot_OLS_Order;
Ordinary_Least_Squares_t Measure_dot_OLS;
uint16_t Measure_ddot_OLS_Order;
Ordinary_Least_Squares_t Measure_ddot_OLS;
float Disturbance;
float Output;
float Last_Disturbance;
float Max_Disturbance;
} LDOB_t;
void LDOB_Init(
LDOB_t *ldob,
float max_d,
float deadband,
float *c,
float lpf_rc,
uint16_t measure_dot_ols_order,
uint16_t measure_ddot_ols_order);
float LDOB_Calculate(LDOB_t *ldob, float measure, float u);
/*************************** Tracking Differentiator ***************************/
typedef __packed struct
typedef struct
{
float Input;

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;
}
}

41
modules/algorithm/user_lib.h
View File

@@ -89,11 +89,11 @@ extern uint8_t GlobalDebugMode;
typedef struct
{
float input; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float out; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float min_value; //<2F>޷<EFBFBD><DEB7><EFBFBD>Сֵ
float max_value; //<2F>޷<EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>ֵ
float frame_period; //ʱ<><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float input; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float out; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float min_value; //<2F>޷<EFBFBD><DEB7><EFBFBD>Сֵ
float max_value; //<2F>޷<EFBFBD><DEB7><EFBFBD><EFBFBD>ֵ
float frame_period; //ʱ<><CAB1><EFBFBD><EFBFBD>
} ramp_function_source_t;
typedef __packed struct
@@ -112,41 +112,34 @@ typedef __packed struct
float t[4];
} Ordinary_Least_Squares_t;
//<2F><><EFBFBD>ٿ<EFBFBD><D9BF><EFBFBD>
//<2F><><EFBFBD>ٿ<EFBFBD><D9BF><EFBFBD>
float Sqrt(float x);
//б<><D0B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>
//б<><D0B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>
void ramp_init(ramp_function_source_t *ramp_source_type, float frame_period, float max, float min);
//б<><D0B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//б<><D0B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float ramp_calc(ramp_function_source_t *ramp_source_type, float input);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float abs_limit(float num, float Limit);
//<2F>жϷ<D0B6><CFB7><EFBFBD>λ
//<2F>жϷ<D0B6><CFB7><EFBFBD>λ
float sign(float value);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float float_deadband(float Value, float minValue, float maxValue);
// int26<32><36><EFBFBD><EFBFBD>
// int26<32><36><EFBFBD><EFBFBD>
int16_t int16_deadline(int16_t Value, int16_t minValue, int16_t maxValue);
//<2F>޷<EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>
//<2F>޷<EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>
float float_constrain(float Value, float minValue, float maxValue);
//<2F>޷<EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>
//<2F>޷<EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>
int16_t int16_constrain(int16_t Value, int16_t minValue, int16_t maxValue);
//ѭ<><D1AD><EFBFBD>޷<EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>
//ѭ<><D1AD><EFBFBD>޷<EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>
float loop_float_constrain(float Input, float minValue, float maxValue);
//<2F>Ƕ<EFBFBD> <20><><EFBFBD>޷<EFBFBD> 180 ~ -180
//<2F>Ƕ<EFBFBD> <20><><EFBFBD>޷<EFBFBD> 180 ~ -180
float theta_format(float Ang);
int float_rounding(float raw);
//<2F><><EFBFBD>ȸ<EFBFBD>ʽ<EFBFBD><CABD>Ϊ-PI~PI
//<2F><><EFBFBD>ȸ<EFBFBD>ʽ<EFBFBD><CABD>Ϊ-PI~PI
#define rad_format(Ang) loop_float_constrain((Ang), -PI, PI)
void OLS_Init(Ordinary_Least_Squares_t *OLS, uint16_t order);
void OLS_Update(Ordinary_Least_Squares_t *OLS, float deltax, float y);
float OLS_Derivative(Ordinary_Least_Squares_t *OLS, float deltax, float y);
float OLS_Smooth(Ordinary_Least_Squares_t *OLS, float deltax, float y);
float Get_OLS_Derivative(Ordinary_Least_Squares_t *OLS);
float Get_OLS_Smooth(Ordinary_Least_Squares_t *OLS);
#endif