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

great changes

Browse Source
This commit is contained in:
tuxmonkey
2026-02-23 23:49:47 +08:00
parent 58299949c5
commit a941a3719a
6 changed files with 116 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

72
User_Code/module/algorithm/controller/pid/pid.c
View File

@@ -1,5 +1,5 @@
/**
* @file controller.c
* @file pid.c
* @author wanghongxi
* @author modified by TuxMonkey
* @brief PID控制器及前馈控制器
@@ -82,6 +82,61 @@ static void f_Output_Filter(PIDInstance *pid)
pid->Last_Output * pid->Output_LPF_RC / (pid->Output_LPF_RC + pid->dt);
}
// 简单前馈模式: f_out = K_F * (Target - Pre_Target) / dt
static void f_FeedForward_Control_Simple(PIDInstance *pid)
{
if (pid->dt > 0.000001f) { // 防止除以零
pid->FFC_Output = pid->FFC_K * (pid->Ref - pid->FFC_Set_History[0]) / pid->dt;
} else {
pid->FFC_Output = 0.0f;
}
// 更新设定值历史
pid->FFC_Set_History[0] = pid->Ref; // 保存当前设定值用于下次计算
}
// 复杂前馈模式: P + D + A 三阶前馈
static void f_FeedForward_Control_Complex(PIDInstance *pid)
{
// 更新设定值历史
pid->FFC_Set_History[2] = pid->FFC_Set_History[1]; // LLAST = LAST
pid->FFC_Set_History[1] = pid->FFC_Set_History[0]; // LAST = NOW
pid->FFC_Set_History[0] = pid->Ref; // NOW = 当前设定值
// 低通滤波处理
float denominator = pid->FFC_LPF_RC + pid->dt;
if (denominator > 0.000001f) {
pid->FFC_Set_History[0] = pid->FFC_Set_History[0] * pid->dt / denominator +
pid->FFC_Set_History[0] * pid->FFC_LPF_RC / denominator;
}
// 前馈计算: P + D + A
float p_term = pid->FFC_Kp * pid->FFC_Set_History[0]; // 比例项
float d_term = 0.0f;
float a_term = 0.0f;
if (pid->dt > 0.000001f) { // 防止除以零
// 微分项: 速度前馈
d_term = pid->FFC_Kd * (pid->FFC_Set_History[0] - pid->FFC_Set_History[1]) / pid->dt;
// 加速度项: 加速度前馈
a_term = pid->FFC_Ka * (pid->FFC_Set_History[0] - 2 * pid->FFC_Set_History[1] + pid->FFC_Set_History[2]) / (pid->dt * pid->dt);
}
pid->FFC_Output = p_term + d_term + a_term;
}
// 前馈输出限幅
static void f_FeedForward_Limit(PIDInstance *pid)
{
if (pid->FFC_Output > pid->MaxOut) {
pid->FFC_Output = pid->MaxOut;
}
if (pid->FFC_Output < -(pid->MaxOut)) {
pid->FFC_Output = -(pid->MaxOut);
}
}
// 前馈控制计算（后续考虑写成电流/速度前馈）
static void f_FeedForward_Control(PIDInstance *pid)
@@ -226,9 +281,18 @@ float PIDCalculate(PIDInstance *pid, float measure, float ref)
pid->Iout += pid->ITerm; // 累加积分
pid->Output = pid->Pout + pid->Iout + pid->Dout; // 计算输出
// 前馈控制
if (pid->Improve & PID_FeedForward)
f_FeedForward_Control(pid);
// 前馈控制 (根据模式选择)
if (pid->Improve & PID_FeedForward) {
if (pid->Improve & PID_FFC_SimpleMode) {
// 简单前馈模式
f_FeedForward_Control_Simple(pid);
} else {
// 复杂前馈模式
f_FeedForward_Control_Complex(pid);
}
// 前馈输出限幅
f_FeedForward_Limit(pid);
}
// 输出滤波
if (pid->Improve & PID_OutputFilter)

35
User_Code/module/algorithm/controller/pid/pid.h
View File

@@ -28,16 +28,17 @@
// PID 优化环节使能标志位
typedef enum
{
PID_IMPROVE_NONE = 0b000000000, // 无优化 0
PID_Integral_Limit = 0b000000001, // 积分限幅 1
PID_Derivative_On_Measurement = 0b000000010, // 微分先行 2
PID_Trapezoid_Intergral = 0b000000100, // 梯形积分 4
PID_Proportional_On_Measurement = 0b000001000, // 比例先行 8
PID_OutputFilter = 0b000010000, // 输出滤波 16
PID_ChangingIntegrationRate = 0b000100000, // 变速积分 32
PID_DerivativeFilter = 0b001000000, // 微分滤波 64
PID_ErrorHandle = 0b010000000, // 错误处理 128
PID_FeedForward = 0b100000000, // 前馈控制 256
PID_IMPROVE_NONE = 0b0000000000, // 无优化 0
PID_Integral_Limit = 0b0000000001, // 积分限幅 1
PID_Derivative_On_Measurement = 0b0000000010, // 微分先行 2
PID_Trapezoid_Intergral = 0b0000000100, // 梯形积分 4
PID_Proportional_On_Measurement = 0b0000001000, // 比例先行 8
PID_OutputFilter = 0b0000010000, // 输出滤波 16
PID_ChangingIntegrationRate = 0b0000100000, // 变速积分 32
PID_DerivativeFilter = 0b0001000000, // 微分滤波 64
PID_ErrorHandle = 0b0010000000, // 错误处理 128
PID_FeedForward = 0b0100000000, // 前馈控制 256
PID_FFC_SimpleMode = 0b1000000000, // 简单前馈模式 (与PID_FeedForward组合使用) 512
} PID_Improvement_e;
/* PID 报错类型枚举*/
@@ -74,9 +75,10 @@ typedef struct
//-----------------------------------
// Feed Forward Control (FFC) parameters
float FFC_Kp; // 前馈比例系数
float FFC_Kd; // 前馈微分系数
float FFC_Ka; // 前馈加速度系数
float FFC_K; // 简单前馈增益系数
float FFC_Kp; // 前馈比例系数 (复杂模式)
float FFC_Kd; // 前馈微分系数 (复杂模式)
float FFC_Ka; // 前馈加速度系数 (复杂模式)
float FFC_LPF_RC; // 前馈低通滤波器系数
float FFC_Output; // 前馈输出值
float FFC_Set_History[3]; // 前馈设定值历史 [NOW, LAST, LLAST]
@@ -125,9 +127,10 @@ typedef struct // config parameter
float Derivative_LPF_RC;
// Feed Forward Control (FFC) parameters
float FFC_Kp; // 前馈比例系数
float FFC_Kd; // 前馈微分系数
float FFC_Ka; // 前馈加速度系数
float FFC_K; // 简单前馈增益系数
float FFC_Kp; // 前馈比例系数 (复杂模式)
float FFC_Kd; // 前馈微分系数 (复杂模式)
float FFC_Ka; // 前馈加速度系数 (复杂模式)
float FFC_LPF_RC; // 前馈低通滤波器系数
} PID_Init_Config_s;

26
User_Code/module/motor/can_motors/dji_motor/dji_motor.c
View File

@@ -13,7 +13,7 @@ static DJIMotorInstance *dji_motor_instance[DJI_MOTOR_CNT] = {NULL}; // 会在co
#ifdef FDCAN
static CANInstance sender_assignment[9] = {
static FDCANInstance sender_assignment[9] = {
[0] = {.can_handle = &hfdcan1, .txconf.Identifier = 0x1ff, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
[1] = {.can_handle = &hfdcan1, .txconf.Identifier = 0x200, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
[2] = {.can_handle = &hfdcan1, .txconf.Identifier = 0x2ff, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
@@ -60,7 +60,7 @@ static uint8_t sender_enable_flag[9] = {0};
* @brief 根据电调/拨码开关上的ID,根据说明书的默认id分配方式计算发送ID和接收ID,
* 并对电机进行分组以便处理多电机控制命令
*/
static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *config)
static void MotorSenderGrouping(DJIMotorInstance *motor, FDCAN_Init_Config_s *config)
{
uint8_t motor_id = config->tx_id - 1; // 下标从零开始,先减一方便赋值
uint8_t motor_send_num;
@@ -68,11 +68,11 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
uint8_t grouping_offset;
//通过CAN计算分组偏移量
if(config->can_handle == &hcan1)
if(config->can_handle == &hfdcan1)
{
grouping_offset=0;
}
else if(config->can_handle == &hcan2)
else if(config->can_handle == &hfdcan2)
{
grouping_offset=3;
}
@@ -106,7 +106,7 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
// 检查是否发生id冲突
for (size_t i = 0; i < idx; ++i)
{
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
if (dji_motor_instance[i]->motor_fdcan_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_fdcan_instance->rx_id == config->rx_id)
{
LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
@@ -135,7 +135,7 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
for (size_t i = 0; i < idx; ++i)
{
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
if (dji_motor_instance[i]->motor_fdcan_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_fdcan_instance->rx_id == config->rx_id)
{
LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
@@ -157,7 +157,7 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
*
* @param _instance 收到数据的instance,通过遍历与所有电机进行对比以选择正确的实例
*/
static void DecodeDJIMotor(CANInstance *_instance)
static void DecodeDJIMotor(FDCANInstance *_instance)
{
// 这里对can instance的id进行了强制转换,从而获得电机的instance实例地址
// _instance指针指向的id是对应电机instance的地址,通过强制转换为电机instance的指针,再通过->运算符访问电机的成员motor_measure,最后取地址获得指针
@@ -189,8 +189,8 @@ static void DecodeDJIMotor(CANInstance *_instance)
static void DJIMotorLostCallback(void *motor_ptr)
{
DJIMotorInstance *motor = (DJIMotorInstance *)motor_ptr;
uint16_t can_bus = motor->motor_can_instance->can_handle == &hcan1 ? 1 : 2;
LOGWARNING("[dji_motor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_can_instance->tx_id);
uint16_t can_bus = motor->motor_fdcan_instance->can_handle == &hcan1 ? 1 : 2;
LOGWARNING("[dji_motor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_fdcan_instance->tx_id);
}
// 电机初始化,返回一个电机实例
@@ -214,12 +214,12 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
// 后续增加电机前馈控制器(速度和电流)
// 电机分组,因为至多4个电机可以共用一帧CAN控制报文
MotorSenderGrouping(instance, &config->can_init_config);
MotorSenderGrouping(instance, &config->fdcan_init_config);
// 注册电机到CAN总线
config->can_init_config.can_module_callback = DecodeDJIMotor; // set callback
config->can_init_config.id = instance; // set id,eq to address(it is identity)
instance->motor_can_instance = CANRegister(&config->can_init_config);
config->fdcan_init_config.can_module_callback = DecodeDJIMotor; // set callback
config->fdcan_init_config.id = instance; // set id,eq to address(it is identity)
instance->motor_fdcan_instance = CANRegister(&config->fdcan_init_config);
// 注册守护线程
Daemon_Init_Config_s daemon_config = {

6
User_Code/module/motor/can_motors/dji_motor/dji_motor.h
View File

@@ -6,7 +6,7 @@
#define TRONONEH7_SCAFFOLD_DJI_MOTOR_H
#include "bsp_fdcan.h"
#include "controller.h"
#include "pid.h"
#include "motor_def.h"
#include "stdint.h"
#include "daemon.h"
@@ -42,7 +42,7 @@ typedef struct
Motor_Control_Setting_s motor_settings; // 电机设置
Motor_Controller_s motor_controller; // 电机控制器
CANInstance *motor_can_instance; // 电机CAN实例
FDCANInstance *motor_fdcan_instance; // 电机CAN实例
// 分组发送设置
uint8_t sender_group;
uint8_t message_num;
@@ -50,7 +50,7 @@ typedef struct
Motor_Type_e motor_type; // 电机类型
Motor_Working_Type_e stop_flag; // 启停标志
DaemonInstance* daemon;
Daemon_Instance* daemon;
uint32_t feed_cnt;
float dt;
} DJIMotorInstance;

4
User_Code/module/motor/motor_def.h
View File

@@ -5,7 +5,7 @@
#ifndef TRONONEH7_SCAFFOLD_MOTOR_DEF_H
#define TRONONEH7_SCAFFOLD_MOTOR_DEF_H
#include "controller.h"
#include "pid.h"
#include "stdint.h"
#define LIMIT_MIN_MAX(x, min, max) (x) = (((x) <= (min)) ? (min) : (((x) >= (max)) ? (max) : (x)))
@@ -125,7 +125,7 @@ typedef struct
Motor_Controller_Init_s controller_param_init_config;
Motor_Control_Setting_s controller_setting_init_config;
Motor_Type_e motor_type;
CAN_Init_Config_s can_init_config;
FDCAN_Init_Config_s fdcan_init_config;
} Motor_Init_Config_s;