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

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

16
modules/imu/ins_task.c
View File

@@ -16,7 +16,7 @@
#include "bsp_temperature.h"
#include "spi.h"
INS_t INS;
INS_t INS;
IMU_Param_t IMU_Param;
PID_t TempCtrl = {0};
@@ -44,7 +44,14 @@ void INS_Init(void)
IMU_QuaternionEKF_Init(10, 0.001, 10000000, 1, 0);
// imu heat init
PID_Init(&TempCtrl, 2000, 300, 0, 1000, 20, 0, 0, 0, 0, 0, 0, 0);
PID_Init_config_s config = {.MaxOut = 2000,
.IntegralLimit = 300,
.DeadBand = 0,
.Kp = 1000,
.Ki = 20,
.Kd = 0,
.Improve = 0x01}; // enable integratiaon limit
PID_Init(&TempCtrl,&config);
// noise of accel is relatively big and of high freq,thus lpf is used
INS.AccelLPF = 0.0085;
@@ -99,7 +106,7 @@ void INS_Task(void)
INS.Yaw = QEKF_INS.Yaw;
INS.Pitch = QEKF_INS.Pitch;
INS.Roll = QEKF_INS.Roll;
INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
}
// temperature control
@@ -117,7 +124,6 @@ void INS_Task(void)
count++;
}
/**
* @brief Transform 3dvector from BodyFrame to EarthFrame
* @param[1] vector in BodyFrame
@@ -232,7 +238,7 @@ static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[
/**
* @brief 温度控制
*
*
*/
void IMU_Temperature_Ctrl(void)
{

6
modules/motor/HT04.c
View File

@@ -40,7 +40,7 @@ joint_instance* HTMotorInit(can_instance_config config)
{
static uint8_t idx;
joint_motor_info[idx]=(joint_instance*)malloc(sizeof(joint_instance));
joint_motor_info[idx++]->motor_can_instace=CANRegister(config);
CANRegister(&joint_motor_info[idx++]->motor_can_instace,config);
}
void JointControl(joint_instance* _instance,float current)
@@ -50,7 +50,7 @@ void JointControl(joint_instance* _instance,float current)
tmp = float_to_uint(current, T_MIN, T_MAX, 12);
_instance->motor_can_instace->rx_buff[6] = tmp>>8;
_instance->motor_can_instace->rx_buff[7] = tmp&0xff;
CANTransmit(&_instance->motor_can_instace);
CANTransmit(_instance->motor_can_instace);
}
void SetJointMode(joint_mode cmd,joint_instance* _instance)
@@ -58,5 +58,5 @@ void SetJointMode(joint_mode cmd,joint_instance* _instance)
static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00};
buf[7]=(uint8_t)cmd;
memcpy(_instance->motor_can_instace->rx_buff,buf,8*sizeof(uint8_t));
CANTransmit(&_instance->motor_can_instace);
CANTransmit(_instance->motor_can_instace);
}

2
modules/motor/LK9025.c
View File

@@ -23,7 +23,7 @@ driven_instance* LKMotroInit(can_instance_config config)
static uint8_t idx;
driven_motor_info[idx]=(driven_instance*)malloc(sizeof(driven_instance));
config.can_module_callback=DecodeDriven;
driven_motor_info[idx++]->motor_can_instance=CANRegister(config);
CANRegister(driven_motor_info[idx++]->motor_can_instance,config);
}
void DrivenControl(int16_t motor1_current,int16_t motor2_current)

220
modules/motor/dji_motor.c
View File

@@ -1,6 +1,7 @@
#include "dji_motor.h"
static uint8_t idx = 0; // register idx
static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用
/* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */
static dji_motor_instance *dji_motor_info[DJI_MOTOR_CNT] = {NULL};
@@ -29,60 +30,8 @@ static can_instance sender_assignment[6] =
static uint8_t sender_enable_flag[6] = {0};
/**
* @brief 根据电调/拨码开关上的ID,计算发送ID和接收ID,并对电机进行分组以便处理多电机控制命令
*
* @param config
*/
static void MotorSenderGrouping(can_instance_config *config)
{
uint8_t motor_id = config->tx_id - 1;
uint8_t motor_rx_id;
uint8_t motor_send_num;
uint8_t motor_grouping;
switch (dji_motor_info[idx]->motor_type)
{
case M2006:
case M3508:
if (motor_id < 4)
{
dji_motor_info[idx]->message_num = motor_id;
dji_motor_info[idx]->sender_group = config->can_handle == &hcan1 ? 1 : 4;
}
else
{
dji_motor_info[idx]->message_num = motor_id - 4;
dji_motor_info[idx]->sender_group = config->can_handle == &hcan1 ? 0 : 3;
}
config->rx_id = 0x200 + motor_id;
sender_enable_flag[dji_motor_info[idx]->sender_group] = 1;
break;
case GM6020:
if (motor_id < 4)
{
dji_motor_info[idx]->message_num = motor_id;
dji_motor_info[idx]->sender_group = config->can_handle == &hcan1 ? 0 : 3;
}
else
{
dji_motor_info[idx]->message_num = motor_id - 4;
dji_motor_info[idx]->sender_group = config->can_handle == &hcan1 ? 2 : 5;
}
config->rx_id = 0x204 + motor_id;
sender_enable_flag[dji_motor_info[idx]->sender_group] = 1;
break;
// other motors should not be registered here
default:
break;
}
}
/**
* @todo 待添加此功能.
*
* @brief 当注册的电机id冲突时,会进入这个函数并提示冲突的ID
*
* @todo 通过segger jlink 发送日志
*/
static void IDcrash_Handler()
{
@@ -91,17 +40,86 @@ static void IDcrash_Handler()
};
}
/**
* @brief 根据电调/拨码开关上的ID,计算发送ID和接收ID,并对电机进行分组以便处理多电机控制命令
*
* @param config
*/
static void MotorSenderGrouping(can_instance_config *config)
{
uint8_t motor_id = config->tx_id - 1; // 下标从零开始,先减一方便赋值
uint8_t motor_send_num;
uint8_t motor_grouping;
switch (dji_motor_info[idx]->motor_type)
{
case M2006:
case M3508:
if (motor_id < 4) // 根据ID分组
{
motor_send_num = motor_id;
motor_grouping = config->can_handle == &hcan1 ? 1 : 4;
}
else
{
motor_send_num = motor_id - 4;
motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
}
// 计算接收id并设置分组发送id
config->rx_id = 0x200 + motor_id+1;
sender_enable_flag[motor_grouping] = 1;
dji_motor_info[idx]->message_num = motor_send_num;
dji_motor_info[idx]->sender_group = motor_grouping;
// 检查是否发生id冲突
for (size_t i = 0; i < idx; i++)
{
if (dji_motor_info[i]->motor_can_instance.can_handle == config->can_handle && dji_motor_info[i]->motor_can_instance.rx_id == config->rx_id)
IDcrash_Handler();
}
break;
case GM6020:
if (motor_id < 4)
{
motor_send_num = motor_id;
motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
}
else
{
motor_send_num = motor_id - 4;
motor_grouping = config->can_handle == &hcan1 ? 2 : 5;
}
config->rx_id = 0x204 + motor_id;
sender_enable_flag[motor_grouping] = 1;
dji_motor_info[idx]->message_num = motor_send_num;
dji_motor_info[idx]->sender_group = motor_grouping;
for (size_t i = 0; i < idx; i++)
{
if (dji_motor_info[i]->motor_can_instance.can_handle == config->can_handle && dji_motor_info[i]->motor_can_instance.rx_id == config->rx_id)
IDcrash_Handler();
}
break;
default: // other motors should not be registered here
break;
}
}
/**
* @brief 根据返回的can_instance对反馈报文进行解析
*
* @param _instance 收到数据的instance,通过遍历与所有电机进行对比
* @param _instance 收到数据的instance,通过遍历与所有电机进行对比以选择正确的实例
*/
static void DecodeDJIMotor(can_instance *_instance)
{
uint8_t *rxbuff = _instance->rx_buff;
for (size_t i = 0; i < DJI_MOTOR_CNT; i++)
{
if (dji_motor_info[i]->motor_can_instance == _instance)
if (&dji_motor_info[i]->motor_can_instance == _instance)
{
dji_motor_info[i]->motor_measure.last_ecd = dji_motor_info[i]->motor_measure.ecd;
dji_motor_info[i]->motor_measure.ecd = (uint16_t)(rxbuff[0] << 8 | rxbuff[1]);
@@ -113,52 +131,108 @@ static void DecodeDJIMotor(can_instance *_instance)
}
}
dji_motor_instance *DJIMotorInit(can_instance_config config,
// 电机初始化,返回一个电机实例
dji_motor_instance *DJIMotorInit(can_instance_config can_config,
Motor_Control_Setting_s motor_setting,
Motor_Controller_Init_s controller_init,
Motor_Type_e type)
{
dji_motor_info[idx] = (dji_motor_instance *)malloc(sizeof(dji_motor_instance));
// motor setting
// motor basic setting
dji_motor_info[idx]->motor_type = type;
dji_motor_info[idx]->motor_settings = motor_setting;
// motor controller init
// @todo : PID init
PID_Init(&dji_motor_info[idx]->motor_controller.current_PID, &controller_init.current_PID);
PID_Init(&dji_motor_info[idx]->motor_controller.speed_PID, &controller_init.speed_PID);
PID_Init(&dji_motor_info[idx]->motor_controller.angle_PID, &controller_init.angle_PID);
dji_motor_info[idx]->motor_controller.other_angle_feedback_ptr = controller_init.other_angle_feedback_ptr;
dji_motor_info[idx]->motor_controller.other_speed_feedback_ptr = controller_init.other_speed_feedback_ptr;
// group motors, because 4 motors share the same CAN control message
MotorSenderGrouping(&config);
MotorSenderGrouping(&can_config);
// register motor to CAN bus
config.can_module_callback = DecodeDJIMotor;
dji_motor_info[idx]->motor_can_instance = CANRegister(config);
can_config.can_module_callback = DecodeDJIMotor; // set callback
CANRegister(&dji_motor_info[idx]->motor_can_instance, can_config);
return dji_motor_info[idx++];
}
// 改变反馈来源
void DJIMotorChangeFeed(dji_motor_instance *motor,Closeloop_Type_e loop,Feedback_Source_e type)
{
if(loop==ANGLE_LOOP)
{
motor->motor_settings.angle_feedback_source=type;
}
if(loop==SPEED_LOOP)
{
motor->motor_settings.speed_feedback_source=type;
}
}
// 设置参考值
void DJIMotorSetRef(dji_motor_instance *motor, float ref)
{
motor->motor_controller.pid_ref = ref;
}
void DJIMotorControl()
{
static uint8_t group, num, set;
{
// 预先通过静态变量定义避免反复释放分配栈空间,直接保存一次指针引用从而减小访存的开销
static uint8_t group, num;
static int16_t set;
static dji_motor_instance* motor;
static Motor_Control_Setting_s *motor_setting;
static Motor_Controller_s *motor_controller;
static dji_motor_measure *motor_measure;
static float pid_measure;
// 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
for (size_t i = 0; i < DJI_MOTOR_CNT; i++)
{
if (dji_motor_info[i])
{
// @todo: 计算PID
// calculate pid output
// ...
group = dji_motor_info[i]->sender_group;
num = dji_motor_info[i]->message_num;
set = (int16_t)dji_motor_info[i]->motor_controller.pid_output;
// sender_assignment[group].rx_buff[num]= 0xff & PIDoutPIDoutput>>8;
// sender_assignment[group].rx_buff[num]= 0xff & PIDoutput;
motor=dji_motor_info[i];
motor_setting=&motor->motor_settings;
motor_controller=&motor->motor_controller;
motor_measure=&motor->motor_measure;
if (motor_setting->close_loop_type & ANGLE_LOOP) // 计算位置环
{
if (motor_setting->angle_feedback_source == OTHER_FEED)
pid_measure=*motor_controller->other_angle_feedback_ptr;
else // MOTOR_FEED
pid_measure=motor_measure->total_angle;
// 更新pid_ref进入下一个环
motor_controller->pid_ref=PID_Calculate(&motor_controller->angle_PID,pid_measure,motor_controller->pid_ref);
}
if (motor_setting->close_loop_type & SPEED_LOOP) // 计算速度环
{
if (motor_setting->speed_feedback_source == OTHER_FEED)
pid_measure=*motor_controller->other_speed_feedback_ptr;
else
pid_measure=motor_measure->speed_rpm;
motor_controller->pid_ref=PID_Calculate(&motor_controller->speed_PID,pid_measure,motor_controller->pid_ref);
}
if (motor_setting->close_loop_type & CURRENT_LOOP) //计算电流环
{
motor_controller->pid_ref=PID_Calculate(&motor_controller->current_PID,motor_measure->given_current,motor_controller->pid_ref);
}
set = (int16_t)motor_controller->pid_ref;
if (motor_setting->reverse_flag == MOTOR_DIRECTION_REVERSE) //设置反转
set *= -1;
// 分组填入发送数据
group = motor->sender_group;
num = motor->message_num;
sender_assignment[group].tx_buff[num] = 0xff & set >> 8;
sender_assignment[group].tx_buff[num+1] = 0xff & set;
}
else
else // 遇到空指针说明所有遍历结束,退出循环
break;
}

43
modules/motor/dji_motor.h
View File

@@ -7,6 +7,18 @@
#include "controller.h"
#include "motor_def.h"
/* DJI电机CAN反馈信息*/
typedef struct
{
uint16_t ecd;
uint16_t last_ecd;
int16_t speed_rpm;
int16_t given_current;
uint8_t temperate;
int16_t total_round;
int32_t total_angle;
} dji_motor_measure;
/**
* @brief DJI intelligent motor typedef
*
@@ -14,16 +26,7 @@
typedef struct
{
/* motor measurement recv from CAN feedback */
struct
{
uint16_t ecd;
uint16_t last_ecd;
int16_t speed_rpm;
int16_t given_current;
uint8_t temperate;
int16_t total_round;
int32_t total_angle;
} motor_measure;
dji_motor_measure motor_measure;
/* basic config of a motor*/
Motor_Control_Setting_s motor_settings;
@@ -32,7 +35,7 @@ typedef struct
Motor_Controller_s motor_controller;
/* the CAN instance own by motor instance*/
can_instance *motor_can_instance;
can_instance motor_can_instance;
/* sender assigment*/
uint8_t sender_group;
@@ -44,8 +47,6 @@ typedef struct
/**
* @todo 加入ID冲突的检查机制,如果发现注册的ID冲突,进入IDcrash_Handler()的死循环中
*
* @brief 调用此函数注册一个DJI智能电机,需要传递较多的初始化参数,请在application初始化的时候调用此函数
* 推荐传参时像标准库一样构造initStructure然后传入此函数.
* recommend: type xxxinitStructure = {
@@ -55,8 +56,8 @@ typedef struct
* 请注意不要在一条总线上挂载过多的电机(超过6个),若一定要这么做,请降低每个电机的反馈频率(设为500Hz),
* 并减小DJIMotorControl()任务的运行频率.
*
* @attention 当前并没有对电机的ID冲突进行检查,请保证在注册电机的时候,他们的反馈ID不会产生冲突!
* M3508和M2006的反馈报文都是0x200+id,而GM6020的反馈是0x204+id,请注意前两者和后者的id不要冲突.
* @attention M3508和M2006的反馈报文都是0x200+id,而GM6020的反馈是0x204+id,请注意前两者和后者的id不要冲突.
* 如果产生冲突,在初始化电机的时候会进入IDcrash_Handler(),可以通过debug来判断是否出现冲突.
*
* @param config 电机can设置,对于DJI电机仅需要将tx_id设置为电调闪动次数(c620,615,610)或拨码开关的值(GM6020)
* 你不需要自己查表计算发送和接收id,我们会帮你处理好!
@@ -87,8 +88,18 @@ void DJIMotorSetRef(dji_motor_instance *motor, float ref);
/**
* @brief 该函数被motor_task调用运行在rtos上,motor_stask内通过osDelay()确定控制频率
* @brief 切换反馈的目标来源,如将角速度和角度的来源换为IMU(小陀螺模式常用)
*
* @param motor 要切换反馈数据来源的电机
* @param loop 要切换反馈数据来源的控制闭环
* @param type 目标反馈模式
*/
void DJIMotorChangeFeed(dji_motor_instance *motor,Closeloop_Type_e loop,Feedback_Source_e type);
/**
* @brief 该函数被motor_task调用运行在rtos上,motor_stask内通过osDelay()确定控制频率
* @todo 增加前馈功能
*/
void DJIMotorControl();

85
modules/motor/motor_def.h
View File

@@ -1,27 +1,31 @@
#ifndef MOTOR_DEF_H
#define MOTOR_DEF_H
typedef enum
typedef enum
{
CURRENT=0,
SPEED=1,
ANGLE=2
CURRENT_LOOP = 0b0001,
SPEED_LOOP = 0b0010,
ANGLE_LOOP = 0b0100,
// only for check
_ = 0b0011,
__ = 0b0110,
___ = 0b0111
} Closeloop_Type_e;
typedef enum
{
MOTOR=0,
OTHER=1
MOTOR_FEED = 0,
OTHER_FEED = 1
} Feedback_Source_e;
typedef enum
{
CLOCKWISE=0,
COUNTER_CLOCKWISE=1
MOTOR_DIRECTION_NORMAL = 0,
MOTOR_DIRECTION_REVERSE = 1
} Reverse_Flag_e;
typedef struct
typedef struct
{
Closeloop_Type_e close_loop_type;
Reverse_Flag_e reverse_flag;
@@ -30,46 +34,37 @@ typedef struct
} Motor_Control_Setting_s;
typedef struct
{
float *other_angle_feedback_ptr;
float *other_speed_feedback_ptr;
PID_t current_PID;
PID_t speed_PID;
PID_t angle_PID;
// 将会作为每个环的输入和输出
float pid_ref;
} Motor_Controller_s;
typedef enum
{
GM6020 = 0,
M3508 = 1,
M2006 = 2,
LK9025 = 3,
HT04 = 4
} Motor_Type_e;
typedef struct
{
float* other_angle_feedback_ptr;
float* other_speed_feedback_ptr;
float *other_angle_feedback_ptr;
float *other_speed_feedback_ptr;
PID_t* current_PID;
PID_t* speed_PID;
PID_t* angle_PID;
PID_Init_config_s current_PID;
PID_Init_config_s speed_PID;
PID_Init_config_s angle_PID;
float pid_ref;
float pid_output;
} Motor_Controller_s;
typedef enum
{
GM6020=0,
M3508=1,
M2006=2,
LK9025=3,
HT04=4
} Motor_Type_e;
typedef struct
{
/* data */
} PID_config_s;
typedef struct
{
float* other_angle_feedback_ptr;
float* other_speed_feedback_ptr;
PID_config_s current_PID;
PID_config_s speed_PID;
PID_config_s angle_PID;
} Motor_Controller_Init_s;
#endif // !MOTOR_DEF_H

31
modules/motor/motor_task.c
View File

@@ -1,32 +1,11 @@
#include "motor_task.h"
static dji_motor_instance* dji_motor_info[DJI_MOTOR_CNT];
static joint_instance* joint_motor_info[HT_MOTOR_CNT];
static driven_instance* driven_motor_info[LK_MOTOR_CNT];
void RegisterMotor(Motor_Type_e motor_type,void* motor_instance)
{
static uint8_t dji_idx,joint_idx,driven_idx;
switch (motor_type)
{
case GM6020:
case M3508:
case M2006:
dji_motor_info[dji_idx++]=(dji_motor_instance*)motor_instance;
break;
case LK9025:
driven_motor_info[driven_idx++]=(driven_instance*)motor_instance;
break;
case HT04:
joint_motor_info[joint_idx++]=(joint_instance*)motor_instance;
break;
}
}
void MotorControlTask()
{
DJIMotorControl();
//LKMotorControl();
//HTMotorControl();
}

5
modules/motor/motor_task.h
View File

@@ -4,13 +4,8 @@
#include "LK9025.h"
#include "HT04.h"
#include "dji_motor.h"
#include "motor_def.h"
void MotorControlTask();
void RegisterMotor(Motor_Type_e motor_type,void* motor_instance);
#endif // !MOTOR_TASK_H

8
modules/remote/remote_control.h
View File

@@ -51,14 +51,14 @@
#define KEY_PRESSED_OFFSET_V ((uint16_t)1 << 14)
#define KEY_PRESSED_OFFSET_B ((uint16_t)1 << 15)
/* ----------------------- Data Struct ------------------------------------- */
typedef __packed struct
typedef struct
{
__packed struct
struct
{
int16_t ch[5];
char s[2];
} rc;
__packed struct
struct
{
int16_t x;
int16_t y;
@@ -66,7 +66,7 @@ typedef __packed struct
uint8_t press_l;
uint8_t press_r;
} mouse;
__packed struct
struct
{
uint16_t v;
} key;