add ffc controller

User_Code/module/algorithm/controller/ffc/ffc.c

@@ -0,0 +1,106 @@
+#include "ffc.h"
+#include <math.h>
+#include <string.h>
+#include <stdlib.h>
+
+// 内部使用的辅助函数：重置句柄
+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);
+    }
+}

User_Code/module/algorithm/controller/ffc/ffc.h

@@ -0,0 +1,89 @@
+#ifndef TRONONEH7_SCAFFOLD_FFC_H
+#define TRONONEH7_SCAFFOLD_FFC_H
+
+#include <stdint.h>
+#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

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,