From a025289597ca50b514dc3b6854ea11e8bf89fe78 Mon Sep 17 00:00:00 2001 From: tuxmonkey Date: Thu, 12 Feb 2026 00:41:34 +0800 Subject: [PATCH] add ffc controller --- .../module/algorithm/controller/ffc/ffc.c | 106 ++++++++++++++++++ .../module/algorithm/controller/ffc/ffc.h | 89 +++++++++++++++ .../module/software/message_hub/message_hub.c | 2 +- 3 files changed, 196 insertions(+), 1 deletion(-) create mode 100644 User_Code/module/algorithm/controller/ffc/ffc.c create mode 100644 User_Code/module/algorithm/controller/ffc/ffc.h diff --git a/User_Code/module/algorithm/controller/ffc/ffc.c b/User_Code/module/algorithm/controller/ffc/ffc.c new file mode 100644 index 0000000..268afb8 --- /dev/null +++ b/User_Code/module/algorithm/controller/ffc/ffc.c @@ -0,0 +1,106 @@ +#include "ffc.h" +#include +#include +#include + +// 内部使用的辅助函数：重置句柄 +static void FFC_ResetInstance(FFC_Handle_t *handle) { + // 将整个结构体清零，相当于 C++ 的值初始化 + memset(handle, 0, sizeof(FFC_Handle_t)); +} + +float FFC_FeedForwardCalc(FFC_Handle_t *handle, float target) { + if (handle == NULL) return 0.0f; + + // 获取时间间隔 dt + handle->dt = DWT_GetDeltaT(&handle->DWT_CNT); + + // 设置当前目标值 + handle->set[FFC_NOW] = target; + + // 低通滤波处理 (完全保留原代码逻辑) + // 注意：数学上 transform: x = x * dt/(LPF+dt) + x * LPF/(LPF+dt) 等同于 x = x。 + // 这里保留原代码逻辑以防有特殊 side effect 需求。 + float denominator = handle->configs.FFC_LPF + handle->dt; + if (denominator != 0.0f) { + handle->set[FFC_NOW] = handle->set[FFC_NOW] * handle->dt / denominator + + handle->set[FFC_NOW] * handle->configs.FFC_LPF / denominator; + } + + // 前馈计算: Proportional + Derivative + Acceleration + // result = c1*x + c2*v + c3*a + float p_term = handle->configs.c1 * handle->set[FFC_NOW]; + float d_term = 0.0f; + float a_term = 0.0f; + + if (handle->dt > 0.000001f) { // 防止除以零 + d_term = handle->configs.c2 * (handle->set[FFC_NOW] - handle->set[FFC_LAST]) / handle->dt; + a_term = handle->configs.c3 * (handle->set[FFC_NOW] - 2 * handle->set[FFC_LAST] + handle->set[FFC_LLAST]) / (handle->dt * handle->dt); + } + + handle->result = p_term + d_term + a_term; + + // 限幅 + handle->result = abs_clip(handle->result, handle->configs.MaxOutput); + + // 更新历史状态 + // 注意：原 C++ 代码中先更新 LAST 再更新 LLAST，这会导致移位寄存器逻辑变为 LLAST = (新的)LAST。 + // 为了保持转换的一致性，这里保留原代码的顺序。 + handle->set[FFC_LAST] = handle->set[FFC_NOW]; + handle->set[FFC_LLAST] = handle->set[FFC_LAST]; + + return handle->result; +} + +void FFC_SetConfig(FFC_Handle_t *handle, FFC_Init_Config_s config) { + if (handle == NULL) return; + + // 使用值初始化清除 ffc_s 对象 + FFC_ResetInstance(handle); + + // 复制配置参数 + handle->configs = config; +} + +void FFC_CheckPointer(FFC_Handle_t *handle, float *get) { + if (handle == NULL) return; + handle->get_data = get; +} + +void FFC_Init(FFC_Handle_t *handle, FFC_Init_Config_s config, float *get) { + if (handle == NULL) return; + + // 对应 C++: FFCSetConfig(config, get) + FFC_SetConfig(handle, config); + if (get != NULL) { + FFC_CheckPointer(handle, get); + } +} + +float FFC_Handle(FFC_Handle_t *handle, float target) { + return FFC_FeedForwardCalc(handle, target); +} + +void FFC_DebugControl(FFC_Handle_t *handle, float *imu_data, float *data_from_gimbalctrl_yaw) { + if (handle == NULL || imu_data == NULL || data_from_gimbalctrl_yaw == NULL) return; + + handle->debug.count++; + handle->debug.total_imu_yaw = *imu_data; + + // 生成正弦波目标值 + handle->debug.target = (float)(sin(handle->debug.count * 0.003) * 1.0); + + // 输出到 gimbal ctrl + *data_from_gimbalctrl_yaw = handle->debug.target; + + // 逻辑判断 + if (fabsf(*data_from_gimbalctrl_yaw) < 0.01f && handle->debug.if_gimbal_ctrl == 0) { + handle->debug.if_gimbal_ctrl = 1; // true + DWT_GetDeltaT(&handle->debug.DWT_CNT); + } + + if (handle->debug.if_gimbal_ctrl == 1 && fabsf(*imu_data) < 0.01f) { + handle->debug.if_gimbal_ctrl = 0; // false + handle->debug.error = DWT_GetDeltaT(&handle->debug.DWT_CNT); + } +} \ No newline at end of file diff --git a/User_Code/module/algorithm/controller/ffc/ffc.h b/User_Code/module/algorithm/controller/ffc/ffc.h new file mode 100644 index 0000000..00cfece --- /dev/null +++ b/User_Code/module/algorithm/controller/ffc/ffc.h @@ -0,0 +1,89 @@ +#ifndef TRONONEH7_SCAFFOLD_FFC_H +#define TRONONEH7_SCAFFOLD_FFC_H + +#include +#include "bsp_dwt.h" +// #include "user_lib.h" // 假设 abs_clip 在这里，如果没有请取消下面的宏定义注释 + +// 如果 abs_clip 未定义，提供一个简单的宏实现 +#ifndef abs_clip +#define abs_clip(x, max) ((x) > (max) ? (max) : ((x) < -(max) ? -(max) : (x))) +#endif + +// 数组索引宏 +#define FFC_NOW 0 +#define FFC_LAST 1 +#define FFC_LLAST 2 + +// 配置结构体 +typedef struct { + float c1; + float c2; + float c3; + float FFC_LPF; + float MaxOutput; +} FFC_Init_Config_s; + +// Debug 结构体 +typedef struct { + int count; + float total_imu_yaw; + float target; + int if_gimbal_ctrl; // 0: false, 1: true + uint32_t DWT_CNT; + float error; +} FFC_Debug_s; + +// FFC 句柄结构体 (包含原 ffc_s 的所有内容) +typedef struct { + float set[3]; // NOW, LAST, LLAST + float dt; + float result; + uint32_t DWT_CNT; + float *get_data; + FFC_Init_Config_s configs; + FFC_Debug_s debug; +} FFC_Handle_t; + +// 函数声明 + +/** + * @brief 初始化 FFC + * @param handle FFC 句柄指针 + * @param config 配置结构体 + * @param get 反馈数据指针 (可选，可为 NULL) + */ +void FFC_Init(FFC_Handle_t *handle, FFC_Init_Config_s config, float *get); + +/** + * @brief 设置配置参数 + */ +void FFC_SetConfig(FFC_Handle_t *handle, FFC_Init_Config_s config); + +/** + * @brief 绑定反馈指针 + */ +void FFC_CheckPointer(FFC_Handle_t *handle, float *get); + +/** + * @brief 前馈计算核心函数 + * @param handle FFC 句柄指针 + * @param target 目标值 + * @return 计算结果 + */ +float FFC_FeedForwardCalc(FFC_Handle_t *handle, float target); + +/** + * @brief 处理函数 (包装器) + */ +float FFC_Handle(FFC_Handle_t *handle, float target); + +/** + * @brief Debug 控制逻辑 + * @param handle FFC 句柄指针 + * @param imu_data IMU 数据指针 (输入/输出) + * @param data_from_gimbalctrl_yaw 云台控制数据指针 (输入/输出) + */ +void FFC_DebugControl(FFC_Handle_t *handle, float *imu_data, float *data_from_gimbalctrl_yaw); + +#endif // TRONONEH7_SCAFFOLD_FFC_H diff --git a/User_Code/module/software/message_hub/message_hub.c b/User_Code/module/software/message_hub/message_hub.c index 8f47a9c..83f0d4b 100644 --- a/User_Code/module/software/message_hub/message_hub.c +++ b/User_Code/module/software/message_hub/message_hub.c @@ -3,7 +3,7 @@ #include "string.h" #include "bsp_log.h" -/* message_center是fake head node,是方便链表编写的技巧,这样就不需要处理链表头的特殊情况 */ +/* message_hub 是fake head node,是方便链表编写的技巧,这样就不需要处理链表头的特殊情况 */ static Publisher_t message_hub = { .topic_name = "Message_Manager", .first_subs = NULL,