diff --git a/ARCHITECTURE_IMPROVEMENT_PLAN.md b/ARCHITECTURE_IMPROVEMENT_PLAN.md deleted file mode 100644 index 47401bc..0000000 --- a/ARCHITECTURE_IMPROVEMENT_PLAN.md +++ /dev/null @@ -1,600 +0,0 @@ -# CloudPlant — 架构审查与改进计划 - -> 生成日期：2026-05-12 -> 基于完整的代码级架构审查与多轮技术讨论 - ---- - -## 目录 - -1. [项目现状](#1-项目现状) -2. [架构合规性分析](#2-架构合规性分析) -3. [控制系统重构](#3-控制系统重构) -4. [传感器与滤波器设计](#4-传感器与滤波器设计) -5. [UART 通信子系统](#5-uart-通信子系统) -6. [FPU 与性能确认](#6-fpu-与性能确认) -7. [健壮性与安全](#7-健壮性与安全) -8. [代码质量改进](#8-代码质量改进) -9. [调试与可观测性](#9-调试与可观测性) -10. [构建与工具链](#10-构建与工具链) -11. [改进优先级矩阵](#11-改进优先级矩阵) - ---- - -## 1. 项目现状 - -### 1.1 基本信息 - -| 属性 | 值 | -|---|---| -| 项目名 | CloudPlant | -| 功能 | 二轴云台增稳控制器 | -| MCU | STM32F407 (Cortex-M4, 168MHz) | -| RTOS | FreeRTOS (CMSIS-RTOS v2) | -| 构建系统 | CMake + STM32CubeMX | -| 传感器 | JY901P IMU (Software I2C) | -| 执行器 | 2× MF4010V2 无刷电机 (CAN2) | -| 调试输出 | USART1 (printf) | - -### 1.2 分层架构 - -``` -APP ─── 业务逻辑 / 任务编排 - ↓ -Module ─── 设备抽象 / 功能封装 - ↓ -Interface ─── 接口抽象 (ops 函数指针表) - ↓ -HAL ─── 外设操作 (STM32 HAL 调用) - ↓ -BSP ─── 硬件描述 (GPIO 映射 / 句柄映射) - ↓ -STM32 HAL / 寄存器 -``` - -| 层 | 目录 | 职责 | -|---|---|---| -| APP | `app/` | 初始化、传感器采集、PID 云台增稳、监控 | -| Module | `modules/` | LED / Motor / CAN / PID / SoftI2C 功能封装 | -| Interface | `interfaces/` | ops 结构定义 + 注册 + API 转发 | -| HAL | `hal/stm32f4/` | 外设寄存器操作 | -| BSP | `bsp/stm32f4/` | 引脚/句柄静态映射表 | - -### 1.3 FreeRTOS 任务 - -| 任务 | 优先级 | 周期 | 功能 | -|---|---|---|---| -| `sensorTask` | High | ~200Hz（循环） | JY901P IMU 数据采集 | -| `controlTask` | Normal | 100Hz（期望） | PID 控制 → 电机速度指令 | -| `monitorTask` | Low | 按需/1Hz | 系统监控、UART 命令处理 | - ---- - -## 2. 架构合规性分析 - -### 2.1 合规部分 ✅ - -- BSP 层所有 5 个文件仅含静态映射表和 getter 函数，无逻辑分支 -- Interface 层使用 ops 函数指针表 + `xxx_register_ops()` 注入实现 -- Module 层通过 Interface 访问硬件，无直接 HAL 调用 -- PID 模块 (`modules/control/pid/`) 纯数学运算，零硬件依赖 -- CAN / I2C 设备模块通过 `can_if` / `i2c_if` 接口通信 - -### 2.2 违规项 ❌ - -| # | 违规 | 严重度 | 位置 | 修复方案 | -|---|---|---|---|---| -| 1 | APP 直接包含 HAL 头文件 | FATAL | `app/app.h:2-8` | 删除 `hal_*.h` 引用，通过 Module 初始化 | -| 2 | APP 直接调用 HAL 初始化函数 | FATAL | `app/app_init.c:7-11` | 将 HAL 初始化下沉到接口注册阶段 | -| 3 | HAL 层函数名含业务语义 (LED) | ERROR | `hal/stm32f4/led/hal_led.c` | 重命名为 `hal_gpio_led_*` 或拆分语义到 Module | -| 4 | `control_task.c` 和 `pid.c` 存在两套 PID 实现 | WARNING | 两个文件 | 统一使用 `modules/control/pid/pid.c` | -| 5 | `sensor_task.c` 内联 I2C 时序函数 | WARNING | `sensor_task.c` | 移至 `modules/bus/soft_i2c/` 或 HAL 层 | - ---- - -## 3. 控制系统重构 - -### 3.1 控制模式：从位置模式切换到速度模式（级联 PID） - -**当前问题**：使用 `cmd_incr_pos` (0xA7) 增量位置模式，导致两层 PID 串行叠加，延迟翻倍。 - -**推荐方案**：位置外环 (MCU) + 速度内环 (MF4010V2 驱动器)。 - -```text -IMU 角度 → MCU 位置 PID → 速度指令 → MF4010V2 速度 PID → 电流 PID → 电机 -``` - -```c -// 控制循环伪代码 -while (1) { - float roll = fAngle[0]; - float pitch = fAngle[1]; - - // 位置 PID：角度误差 → 速度指令 (DPS) - float yaw_speed = PIDController_Compute(&pos_pid_roll, 0.0f, roll); - float pitch_speed = PIDController_Compute(&pos_pid_pitch, 0.0f, pitch); - - // 改用速度模式 - mf4010v2_set_speed(&motor_yaw, (int32_t)(yaw_speed * 100.0f)); - mf4010v2_set_speed(&motor_pitch, (int32_t)(pitch_speed * 100.0f)); - - vTaskDelayUntil(&xLastWakeTime, xPeriod); -} -``` - -### 3.2 控制周期：`vTaskDelayUntil` 替代 `osDelay` - -**当前问题**：`osDelay(10)` 导致周期漂移累积累积。 - -```c -// 旧（有抖动） -osDelay(pdMS_TO_TICKS(10)); - -// 新（精确 100Hz） -TickType_t xLastWakeTime = xTaskGetTickCount(); -vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(10)); -``` - -### 3.3 带宽分配 - -``` -JY901P 内部低通 → 256Hz (WitSetBandwidth) -IMU 角度读取 → 200Hz (只读 Roll/Pitch/Yaw，3 个寄存器) -控制环频率 → 100Hz (vTaskDelayUntil) -有效控制带宽 → ~20Hz (抑制手持晃动) -printf 输出 → 1Hz (不在控制环内打印) -``` - -```c -// 初始化时设置带宽 -WitInit(WIT_PROTOCOL_I2C, addr); -WitSetBandwidth(BANDWIDTH_256HZ); - -// 循环中只读角度寄存器 -WitReadReg(Roll, 3); // 仅 3 个寄存器，约 0.5ms -``` - -### 3.4 PID 参数起始值 - -```c -// 位置 PID 参数（输出为速度 DPS） -pos_pid_roll.Kp = 80.0f; // P: 角度误差 → 速度指令 -pos_pid_roll.Ki = 0.5f; // I: 消除稳态误差 -pos_pid_roll.Kd = 30.0f; // D: 抑制过冲 -pos_pid_roll.output_max = 36000.0f; // 最大速度 360 DPS - -pos_pid_pitch.Kp = 80.0f; -pos_pid_pitch.Ki = 0.5f; -pos_pid_pitch.Kd = 30.0f; -pos_pid_pitch.output_max = 18000.0f; // Pitch 轴限幅 180 DPS -``` - ---- - -## 4. 传感器与滤波器设计 - -### 4.1 一阶低通滤波器（必加） - -```c -typedef struct { - float alpha, prev_output; -} LowPassFilter_t; - -void LPF_Init(LowPassFilter_t *lpf, float cutoff_hz, float sample_rate_hz) { - float dt = 1.0f / sample_rate_hz; - float RC = 1.0f / (2.0f * 3.14159f * cutoff_hz); - lpf->alpha = dt / (RC + dt); - lpf->prev_output = 0.0f; -} - -static inline float LPF_Update(LowPassFilter_t *lpf, float input) { - float output = lpf->alpha * input + (1.0f - lpf->alpha) * lpf->prev_output; - lpf->prev_output = output; - return output; -} -``` - -**使用**： - -```c -LowPassFilter_t lpf_roll, lpf_pitch; -LPF_Init(&lpf_roll, 25.0f, 200.0f); // 25Hz 截止 @ 200Hz 采样 -LPF_Init(&lpf_pitch, 25.0f, 200.0f); -float filt_roll = LPF_Update(&lpf_roll, fAngle[0]); -``` - -### 4.2 陷波滤波器（电机振动抑制） - -用于滤除特定频率的电机机械振动。双二阶 IIR 结构，仅衰减目标频率。 - -```c -typedef struct { - float b0, b1, b2, a1, a2; - float x1, x2, y1, y2; -} NotchFilter_t; - -void Notch_Init(NotchFilter_t *nf, float center_hz, float bandwidth_hz, float sample_rate); -float Notch_Update(NotchFilter_t *nf, float input); -``` - -**使用**：实测电机运行时的 FFT 频谱，确定振动频率后再设置陷波频率。 - -### 4.3 滤波器串联顺序 - -```c -// LPF 先做宽带降噪，Notch 再做定点消除 -float raw = fAngle[0]; -float lpf = LPF_Update(&lpf_roll, raw); -float clean = Notch_Update(¬ch_roll, lpf); -float cmd = PIDController_Compute(&pos_pid, target, clean); -``` - -### 4.4 调试顺序 - -``` -步骤 1：不加滤波器，调稳 PID -步骤 2：加一阶 LPF (25Hz)，消除高频抖动 -步骤 3：电机跑起来，上位机 FFT 分析角度信号 -步骤 4：加陷波滤波器，精准消除振动频率 -步骤 5：微调 PID 参数 -``` - ---- - -## 5. UART 通信子系统 - -### 5.1 架构 - -``` -USART1 RX IRQ ─→ StreamBuffer ─→ monitor_task ─→ CmdProcess() - TX ─────── Mutex ──── printf / 命令响应 -``` - -### 5.2 Stream Buffer 创建 - -```c -// freertos.c 中 -StreamBufferHandle_t uart_rx_stream; -uart_rx_stream = xStreamBufferCreate(256, 1); // 256 字节，触发阈值 1 -``` - -### 5.3 中断回调（仅收发信号，不做业务） - -```c -extern StreamBufferHandle_t uart_rx_stream; -extern uint8_t uart_rx_byte; - -void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { - if (huart->Instance == USART1) { - BaseType_t xHigherPriorityTaskWoken = pdFALSE; - xStreamBufferSendFromISR(uart_rx_stream, &uart_rx_byte, 1, &xHigherPriorityTaskWoken); - HAL_UART_Receive_IT(&huart1, &uart_rx_byte, 1); - portYIELD_FROM_ISR(xHigherPriorityTaskWoken); - } -} -``` - -### 5.4 启动中断接收 - -```c -// HAL 初始化完成后 -uint8_t uart_rx_byte; -HAL_UART_Receive_IT(&huart1, &uart_rx_byte, 1); -``` - -### 5.5 monitor_task 命令处理 - -```c -void monitor_task(void) { - uint8_t rx_buf[32]; - while (1) { - size_t n = xStreamBufferReceive(uart_rx_stream, rx_buf, sizeof(rx_buf), - pdMS_TO_TICKS(100)); - for (size_t i = 0; i < n; i++) { - CopeCmdData(rx_buf[i]); - } - if (n > 0) CmdProcess(); - } -} -``` - -### 5.6 TX 互斥锁（printf 与命令响应共享 USART1） - -```c -SemaphoreHandle_t uart_tx_mutex; -uart_tx_mutex = xSemaphoreCreateMutex(); - -#define SAFE_PRINTF(fmt, ...) \ - do { \ - xSemaphoreTake(uart_tx_mutex, portMAX_DELAY); \ - printf(fmt, ##__VA_ARGS__); \ - xSemaphoreGive(uart_tx_mutex); \ - } while(0) -``` - -### 5.7 命令集 - -| 命令 | 功能 | -|---|---| -| `a` | 加速度校准 | -| `m` | 磁场校准开始 | -| `e` | 磁场校准结束 | -| `u` / `U` | 设置 IMU 带宽 5Hz / 256Hz | -| `B` / `b` | UART 波特率 115200 / 9600 | -| `r` | 查询电机故障状态 | -| `p` | 打印当前角度 | -| `d` | Dump 环形日志（预留） | - -> 将 `sensor_task.c` 中的 `CopeCmdData()`/`CmdProcess()` 移至 `monitor_task`，避免死代码。 - ---- - -## 6. FPU 与性能确认 - -### 6.1 编译选项（当前缺失） - -在 `CMakeLists.txt` 中补充： - -```cmake -target_compile_options(${CMAKE_PROJECT_NAME} PRIVATE - -mcpu=cortex-m4 - -mthumb - -mfloat-abi=hard - -mfpu=fpv4-sp-d16 -) - -target_link_options(${CMAKE_PROJECT_NAME} PRIVATE - -mcpu=cortex-m4 - -mthumb - -mfloat-abi=hard - -mfpu=fpv4-sp-d16 -) -``` - -### 6.2 验证 FPU 是否生效 - -```bash -arm-none-eabi-objdump -d build/CloudPlant.elf | grep -c 'vadd\|vmul\|vldr\|vstr' -# 输出 > 0 即 FPU 生效 -``` - -### 6.3 性能瓶颈 - -`printf` 格式化 `float` 会触发软件浮点库（`__aeabi_f2d` 等），是当前最大性能瓶颈。PID 计算中的 `float` 加减乘除由 FPU 直接处理（1 周期），无需改为定点数。 - ---- - -## 7. 健壮性与安全 - -### 7.1 看门狗 - -```c -IWDG_HandleTypeDef hiwdg; -hiwdg.Instance = IWDG; -hiwdg.Init.Prescaler = IWDG_PRESCALER_64; -hiwdg.Init.Reload = 625; // ~1s -HAL_IWDG_Init(&hiwdg); - -// monitor_task 中定期喂狗 -HAL_IWDG_Refresh(&hiwdg); -``` - -### 7.2 任务栈溢出检测 - -```c -// FreeRTOSConfig.h -#define configCHECK_FOR_STACK_OVERFLOW 2 -#define configUSE_MALLOC_FAILED_HOOK 1 - -void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName) { - // 立即停机 → 闪灯 → 等待看门狗复位 - mf4010v2_stop(&motor_yaw); - mf4010v2_stop(&motor_pitch); - while (1) { led_toggle(&led0); HAL_Delay(200); } -} -``` - -### 7.3 CAN 发送超时 - -```c -// hal_can.c 当前是忙等 → 改为带超时 -uint32_t timeout = HAL_GetTick() + 50; -while (HAL_CAN_IsTxMessagePending(hcan, TxMailbox)) { - if (HAL_GetTick() > timeout) { - HAL_CAN_AbortTxRequest(hcan, TxMailbox); - return -1; - } -} -``` - -### 7.4 电机限位保护 - -```c -if (pitch_cmd > 9000) pitch_cmd = 9000; // +90° -if (pitch_cmd < -9000) pitch_cmd = -9000; -if (yaw_cmd > 18000) yaw_cmd = 18000; // ±180° -if (yaw_cmd < -18000) yaw_cmd = -18000; -``` - -### 7.5 IMU 离线检测 - -```c -volatile uint32_t imu_last_update; // sensor_task 更新 - -// control_task 检查 -if (HAL_GetTick() - imu_last_update > 200) { - mf4010v2_stop(&motor_yaw); - mf4010v2_stop(&motor_pitch); -} -``` - ---- - -## 8. 代码质量改进 - -### 8.1 统一 PID 实现 - -删除 `control_task.c` 中的手写 `PID_t`，统一使用 `modules/control/pid/pid.c` 中的 `PIDController_t`。 - -### 8.2 配置常量集中管理 - -创建 `config.h`： - -```c -#define CONFIG_CONTROL_FREQ_HZ 100 -#define CONFIG_IMU_SAMPLE_RATE_HZ 200 -#define CONFIG_LPF_CUTOFF_HZ 25.0f -#define CONFIG_MOTOR_YAW_CAN_ID 0x141 -#define CONFIG_MOTOR_PITCH_CAN_ID 0x142 -#define CONFIG_MOTOR_CAN_CHANNEL 1 -``` - -### 8.3 统一错误码 - -```c -typedef enum { - ERR_OK = 0, - ERR_PARAM = -1, - ERR_TIMEOUT = -2, - ERR_BUS = -3, - ERR_NOT_INIT = -4, - ERR_HARDWARE = -5, -} error_code_t; -``` - -### 8.4 PID 参数持久化 - -在 Flash 末扇区存储校准数据： - -```c -#define CALIB_FLASH_ADDR 0x08060000 - -typedef struct { - uint32_t magic; // 0xCA11B007 - float roll_kp, roll_ki, roll_kd; - float pitch_kp, pitch_ki, pitch_kd; - float imu_offset[3]; - uint32_t crc; -} calibration_t; -``` - ---- - -## 9. 调试与可观测性 - -### 9.1 环形数据日志 - -```c -typedef struct { - uint32_t tick; - float roll, pitch, yaw; - int32_t motor_yaw_pos, motor_pitch_pos; - float pid_output_roll, pid_output_pitch; -} log_entry_t; - -log_entry_t flight_log[1000]; // 100Hz × 10s -volatile int log_idx; -``` - -通过串口命令 `d` 触发 dump。 - -### 9.2 运行时变量监控协议 - -简易二进制协议，支持在线调 PID 参数： - -``` -帧格式: 0xAA 0xBB [cmd:1] [addr:2] [len:2] [data:N] [crc:1] -cmd: 0x01=读 0x02=写 -``` - -### 9.3 LED 状态码 - -| LED0 | LED1 | 含义 | -|---|---|---| -| 快闪 | 灭 | 正常运行 | -| 常亮 | 灭 | 电机故障 | -| 灭 | 快闪 | CAN 通信错误 | -| 灭 | 常亮 | IMU 离线 | -| 交替闪 | 交替闪 | 正在校准 | - ---- - -## 10. 构建与工具链 - -### 10.1 CMake 增强 - -```cmake -target_compile_options(${CMAKE_PROJECT_NAME} PRIVATE - -Wall -Wextra - -Werror=return-type - -fstack-usage -) - -target_link_options(${CMAKE_PROJECT_NAME} PRIVATE - -Wl,--print-memory-usage - -Wl,-Map=${CMAKE_PROJECT_NAME}.map -) -``` - -### 10.2 PID 单元测试 - -```c -// test_pid.c — 可在 PC 上直接运行 -void test_pid_zero_error(void) { - PIDController_t pid; - PIDController_Init(&pid); - pid.Kp = 1.0f; - assert(PIDController_Compute(&pid, 0.0f, 0.0f) == 0.0f); -} -``` - ---- - -## 11. 改进优先级矩阵 - -``` - 影响 ↑ - ┌───────────────────────────────────── - 高 │ ① 电机限位 ② 看门狗 - │ ③ CAN 超时 ④ IMU 离线检测 - │ ⑤ FPU 编译选项 - │ - 中 │ ⑥ PID 参数持久化 ⑦ 统一 PID 模块 - │ ⑧ 栈溢出检测 ⑨ 错误码统一 - │ ⑩ 在线调参协议 - │ - 低 │ ⑪ 数据日志 ⑫ 单元测试 - │ ⑬ LED 状态码 ⑭ 前馈补偿 - │ - └───────────────────────────────────── - 高 ←────────── 工作量 ──────────→ 低 -``` - -### 推荐执行顺序 - -**第一阶段（安全，< 1 天）**：① → ② → ③ → ④ -**第二阶段（性能，1-2 天）**：⑤ → 速度模式切换 → ⑦ → 低通滤波器 -**第三阶段（质量，按需）**：⑥ → ⑧ → ⑨ → ⑩ -**第四阶段（体验）**：⑪ → ⑫ → ⑬ → ⑭ - ---- - -## 附录：文件改动清单 - -| 文件 | 改动类型 | 说明 | -|---|---|---| -| `app/app.h` | 修改 | 删除 `hal_*.h` 直接引用 | -| `app/app_init.c` | 修改 | 下沉 HAL 初始化到接口注册阶段 | -| `app/control_task.c` | 重写 | 速度模式 + vTaskDelayUntil + 滤波器 + 统一 PID | -| `app/sensor_task.c` | 修改 | 删除 CopeCmdData/CmdProcess；只读 3 寄存器；降频 printf | -| `app/monitor_task.c` | 新建/重写 | UART 命令处理 + Stream Buffer 接收 + 看门狗 | -| `hal/stm32f4/can/hal_can.c` | 修改 | CAN 发送加超时 | -| `CMakeLists.txt` | 修改 | 添加 FPU 编译选项 + 警告选项 | -| `config.h` | 新建 | 集中管理配置常量 | -| `modules/filter/lowpass.c` | 新建 | 一阶低通滤波器 | -| `modules/filter/notch.c` | 新建 | 陷波滤波器 | -| `app/cmd_protocol.c` | 新建（可选） | 二进制调参协议 | - ---- - -> **核心原则**：宁可重构，也不妥协分层。任何硬件访问必须经过 Interface，任何硬件位置必须由 BSP 描述，任何硬件操作必须在 HAL 完成，任何功能逻辑必须在 Module，任何决策必须在 APP。 diff --git a/CLAUDE.md b/CLAUDE.md new file mode 100644 index 0000000..c7edcfb --- /dev/null +++ b/CLAUDE.md @@ -0,0 +1,114 @@ +# CLAUDE.md + +This file provides guidance to Claude Code (claude.ai/code) when working with code in this repository. + +## Project Overview + +CloudPlant — a 2-axis gimbal stabilization controller for STM32F407xx (Cortex-M4F). Uses FreeRTOS (CMSIS V2 API) with a 100Hz PID control loop reading IMU angles over soft I2C and driving MF4010V2 brushless motors over CAN bus. + +## Build Commands + +```bash +# Configure (one-time; Debug is the default preset) +cmake --preset Debug + +# Build +cmake --build build/Debug + +# Or build with ninja directly +ninja -C build/Debug + +# Build a specific object file only +ninja -C build/Debug CMakeFiles/CloudPlant.dir/path/to/file.c.obj +``` + +The project uses CMake with Ninja as the generator and the `arm-none-eabi-gcc` toolchain (`cmake/gcc-arm-none-eabi.cmake`). A `starm-clang` toolchain file also exists as an alternative. Compile commands are exported to `build/Debug/compile_commands.json` for clangd. + +## Flash + +```bash +# Flash the Debug ELF (requires STM32CubeProgrammer CLI) +python flash.py build/Debug/CloudPlant.elf + +# Erase chip only +python flash.py --erase-only + +# Flash a release build +python flash.py build/Release/CloudPlant.elf +``` + +The script converts ELF to BIN with `arm-none-eabi-objcopy`, then flashes via `STM32_Programmer_CLI` at `0x08000000` over SWD. + +## Architecture: Strict 6-Layer Stack + +Every hardware access must follow this chain. Cross-layer calls are forbidden. + +``` +APP → Business logic, task orchestration + ↓ +Module → Device/algorithm encapsulation (motor, PID, LED, soft_i2c) + ↓ +Interface → Abstract ops-based API (_if.h with function pointer structs) + ↓ +HAL → Hardware operations using STM32 HAL, registers ops to Interface + ↓ +BSP → Hardware description only — pin maps, CAN handles, no logic + ↓ +Driver → STM32 HAL / CMSIS (generated by CubeMX, in Core/, Drivers/, Middlewares/) +``` + +**Key rules:** +- Module must NOT call HAL/BSP directly — it goes through Interface APIs. +- Interface defines a `xxx_ops_t` struct with function pointers; HAL implements and registers ops via `xxx_register_ops()`. +- BSP files contain only static data arrays (pin mappings, handle arrays) — no `if`/`for`/`while` or business logic. +- APP never calls HAL or BSP directly; it calls Module functions, which call Interface APIs. + +## Key Directories (User Code) + +| Directory | Purpose | +|-----------|---------| +| `app/` | RTOS tasks: `app_init`, `control_task`, `sensor_task`, `monitor_task` | +| `modules/device/motor/` | MF4010V2 brushless motor driver (CAN-based, speed/position/torque modes) | +| `modules/device/led/` | LED device abstraction | +| `modules/control/pid/` | Generic PID controller (float-based, anti-windup via output clamping) | +| `modules/bus/soft_i2c/` | Bit-banged I2C master (frame-level API) | +| `interfaces/` | Abstract APIs: `can_if`, `led_if`, `gpio_if`, `i2c_if`, `soft_i2c_if` | +| `hal/stm32f4/` | HAL implementations per peripheral — registers ops with interfaces | +| `bsp/stm32f4/` | Board-specific pin/instance mappings | +| `3rdparty/jy901p/` | JY901P IMU SDK (I2C protocol) | +| `3rdparty/cjson/` | cJSON library | + +## CubeMX Integration + +CubeMX-generated code lives in `Core/`, `Drivers/`, and `Middlewares/`. It is compiled as three static libraries defined in `cmake/stm32cubemx/CMakeLists.txt`: +- `stm32cubemx` (INTERFACE — headers and defines) +- `STM32_Drivers` (OBJECT — HAL driver sources) +- `FreeRTOS` (OBJECT — FreeRTOS kernel sources) + +User code is added to the main `CMakeLists.txt` via `target_sources()` and `target_include_directories()`. Global defines `USE_HAL_DRIVER`, `STM32F407xx`, and `STM32_THREAD_SAFE_STRATEGY=2` come from the CubeMX CMake. + +FreeRTOS task creation happens in `Core/Src/freertos.c` (CubeMX-managed). The tasks delegate to `app/` entry points: `StartSensorTask` → `sensor_task()`, `StartControlTask` → `control_task()`, `StartMonitorTask` → `monitor_task()`. + +## Data Flow (Gimbal Stabilization) + +``` +sensor_task (IMU poll via soft I2C) + → osMessageQueue (imu_data_t: acc, gyro, angle) + → control_task (100Hz PID loop) + → CAN bus (motor speed commands to MF4010V2) +``` + +The IMU queue (`g_imu_queue`) is created in `app_init()` with depth 4. `sensor_task` reads JY901P IMU registers via soft I2C and pushes data into the queue. `control_task` reads the queue with a 20ms timeout, runs two PID controllers (Roll → Yaw motor, Pitch → Pitch motor), and sends speed commands over CAN. + +## Interface Pattern + +Every hardware peripheral follows this pattern: + +``` +interfaces//_if.h — ops struct + register function + API declarations +interfaces//_if.c — dispatching through registered ops +hal/stm32f4//hal_.c — implements ops, registers them at init +bsp/stm32f4//bsp_.c — static hardware mapping +``` + +Module code (e.g., `modules/device/led/led.c`) calls `led_if_init()`, `led_if_write()`, etc. — never `hal_led_*` or `bsp_led_*` directly. diff --git a/CMakeLists.txt b/CMakeLists.txt index 3df9453..19f0522 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -47,29 +47,40 @@ target_link_directories(${CMAKE_PROJECT_NAME} PRIVATE target_sources(${CMAKE_PROJECT_NAME} PRIVATE # Add user sources here 3rdparty/jy901p/wit_c_sdk.c + modules/filter/lowpass/lowpass.c + modules/filter/moving_average/moving_average.c + modules/filter/notch/notch.c + modules/filter/rate_limiter/rate_limiter.c modules/device/motor/mf4010v2.c modules/device/led/led.c modules/device/can/can_device.c modules/control/pid/pid.c modules/bus/soft_i2c/soft_i2c.c + modules/bus/i2c/i2c_bus.c + modules/cmd_parser/cmd_parser.c bsp/stm32f4/can/bsp_can.c bsp/stm32f4/led/bsp_led.c bsp/stm32f4/i2c/bsp_i2c.c bsp/stm32f4/gpio/bsp_gpio.c bsp/stm32f4/soft_i2c/bsp_soft_i2c.c + bsp/stm32f4/uart/bsp_uart.c hal/stm32f4/can/hal_can.c hal/stm32f4/led/hal_led.c hal/stm32f4/i2c/hal_i2c.c hal/stm32f4/gpio/hal_gpio.c hal/stm32f4/soft_i2c/hal_soft_i2c.c + hal/stm32f4/uart/hal_uart.c app/app_init.c app/control_task.c app/sensor_task.c + app/callback_task.c interfaces/led/led_if.c interfaces/can/can_if.c interfaces/i2c/i2c_if.c interfaces/gpio/gpio_if.c interfaces/soft_i2c/soft_i2c_if.c + interfaces/uart/uart_if.c + modules/bus/uart/uart.c ) @@ -81,24 +92,34 @@ target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/modules/device/motor ${CMAKE_CURRENT_SOURCE_DIR}/modules/device/can ${CMAKE_CURRENT_SOURCE_DIR}/modules/sensor/imu + ${CMAKE_CURRENT_SOURCE_DIR}/modules/filter/lowpass + ${CMAKE_CURRENT_SOURCE_DIR}/modules/filter/moving_average + ${CMAKE_CURRENT_SOURCE_DIR}/modules/filter/notch + ${CMAKE_CURRENT_SOURCE_DIR}/modules/filter/rate_limiter ${CMAKE_CURRENT_SOURCE_DIR}/modules/control/pid ${CMAKE_CURRENT_SOURCE_DIR}/modules/bus/soft_i2c + ${CMAKE_CURRENT_SOURCE_DIR}/modules/bus/i2c + ${CMAKE_CURRENT_SOURCE_DIR}/modules/cmd_parser ${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/can ${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/led ${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/i2c ${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/gpio ${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/soft_i2c + ${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/uart ${CMAKE_CURRENT_SOURCE_DIR}/app ${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/led ${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/can ${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/i2c ${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/gpio ${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/soft_i2c + ${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/uart ${CMAKE_CURRENT_SOURCE_DIR}/interfaces/led ${CMAKE_CURRENT_SOURCE_DIR}/interfaces/can ${CMAKE_CURRENT_SOURCE_DIR}/interfaces/i2c ${CMAKE_CURRENT_SOURCE_DIR}/interfaces/gpio ${CMAKE_CURRENT_SOURCE_DIR}/interfaces/soft_i2c + ${CMAKE_CURRENT_SOURCE_DIR}/interfaces/uart + ${CMAKE_CURRENT_SOURCE_DIR}/modules/bus/uart ) # Add project symbols (macros) diff --git a/CloudPlant.ioc b/CloudPlant.ioc index 3b6622f..10be2da 100644 --- a/CloudPlant.ioc +++ b/CloudPlant.ioc @@ -39,18 +39,21 @@ Mcu.IP4=NVIC Mcu.IP5=RCC Mcu.IP6=SYS Mcu.IP7=USART1 -Mcu.IPNb=8 +Mcu.IP8=USART6 +Mcu.IPNb=9 Mcu.Name=STM32F407Z(E-G)Tx Mcu.Package=LQFP144 Mcu.Pin0=PE3 Mcu.Pin1=PE4 -Mcu.Pin10=PA9 -Mcu.Pin11=PA10 -Mcu.Pin12=PD0 -Mcu.Pin13=PD1 -Mcu.Pin14=PG9 -Mcu.Pin15=VP_FREERTOS_VS_CMSIS_V2 -Mcu.Pin16=VP_SYS_VS_tim1 +Mcu.Pin10=PC6 +Mcu.Pin11=PC7 +Mcu.Pin12=PA9 +Mcu.Pin13=PA10 +Mcu.Pin14=PD0 +Mcu.Pin15=PD1 +Mcu.Pin16=PG9 +Mcu.Pin17=VP_FREERTOS_VS_CMSIS_V2 +Mcu.Pin18=VP_SYS_VS_tim1 Mcu.Pin2=PF0 Mcu.Pin3=PF1 Mcu.Pin4=PH0-OSC_IN @@ -59,7 +62,7 @@ Mcu.Pin6=PF12 Mcu.Pin7=PF14 Mcu.Pin8=PB12 Mcu.Pin9=PB13 -Mcu.PinsNb=17 +Mcu.PinsNb=19 Mcu.ThirdPartyNb=0 Mcu.UserConstants= Mcu.UserName=STM32F407ZGTx @@ -82,6 +85,7 @@ NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:false\:true\:false\:true\:false NVIC.TIM1_UP_TIM10_IRQn=true\:15\:0\:false\:false\:true\:false\:false\:true\:true NVIC.TimeBase=TIM1_UP_TIM10_IRQn NVIC.TimeBaseIP=TIM1 +NVIC.USART1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false PA10.Mode=Asynchronous PA10.Signal=USART1_RX @@ -91,6 +95,12 @@ PB12.Mode=CAN_Activate PB12.Signal=CAN2_RX PB13.Mode=CAN_Activate PB13.Signal=CAN2_TX +PC6.Locked=true +PC6.Mode=Asynchronous +PC6.Signal=USART6_TX +PC7.Locked=true +PC7.Mode=Asynchronous +PC7.Signal=USART6_RX PCC.Checker=false PCC.Display=Plot\: All Steps PCC.Line=STM32F407/417 @@ -176,7 +186,7 @@ ProjectManager.ToolChainLocation= ProjectManager.UAScriptAfterPath= ProjectManager.UAScriptBeforePath= ProjectManager.UnderRoot=false -ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_I2C2_Init-I2C2-false-HAL-true,4-MX_USART1_UART_Init-USART1-false-HAL-true,5-MX_CAN2_Init-CAN2-false-HAL-true,6-MX_CAN1_Init-CAN1-false-HAL-true +ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_I2C2_Init-I2C2-false-HAL-true,4-MX_USART1_UART_Init-USART1-false-HAL-true,5-MX_CAN2_Init-CAN2-false-HAL-true,6-MX_CAN1_Init-CAN1-false-HAL-true,7-MX_USART6_UART_Init-USART6-false-HAL-true RCC.48MHZClocksFreq_Value=84000000 RCC.AHBFreq_Value=168000000 RCC.APB1CLKDivider=RCC_HCLK_DIV4 @@ -213,6 +223,8 @@ RCC.VCOOutputFreq_Value=336000000 RCC.VcooutputI2S=192000000 USART1.IPParameters=VirtualMode USART1.VirtualMode=VM_ASYNC +USART6.IPParameters=VirtualMode +USART6.VirtualMode=VM_ASYNC VP_FREERTOS_VS_CMSIS_V2.Mode=CMSIS_V2 VP_FREERTOS_VS_CMSIS_V2.Signal=FREERTOS_VS_CMSIS_V2 VP_SYS_VS_tim1.Mode=TIM1 diff --git a/Core/Inc/stm32f4xx_it.h b/Core/Inc/stm32f4xx_it.h index 75ff1e5..18210b8 100644 --- a/Core/Inc/stm32f4xx_it.h +++ b/Core/Inc/stm32f4xx_it.h @@ -54,6 +54,7 @@ void UsageFault_Handler(void); void DebugMon_Handler(void); void TIM1_UP_TIM10_IRQHandler(void); void I2C2_EV_IRQHandler(void); +void USART1_IRQHandler(void); /* USER CODE BEGIN EFP */ /* USER CODE END EFP */ diff --git a/Core/Inc/usart.h b/Core/Inc/usart.h index a13cfca..91b024d 100644 --- a/Core/Inc/usart.h +++ b/Core/Inc/usart.h @@ -34,11 +34,14 @@ extern "C" { extern UART_HandleTypeDef huart1; +extern UART_HandleTypeDef huart6; + /* USER CODE BEGIN Private defines */ /* USER CODE END Private defines */ void MX_USART1_UART_Init(void); +void MX_USART6_UART_Init(void); /* USER CODE BEGIN Prototypes */ diff --git a/Core/Src/freertos.c b/Core/Src/freertos.c index fda4a12..db4f58e 100644 --- a/Core/Src/freertos.c +++ b/Core/Src/freertos.c @@ -136,7 +136,7 @@ void MX_FREERTOS_Init(void) { void StartMonitorTask(void *argument) { /* USER CODE BEGIN StartMonitorTask */ - monitor_task(); + callback_task(); /* USER CODE END StartMonitorTask */ } diff --git a/Core/Src/gpio.c b/Core/Src/gpio.c index f138f73..08498b7 100644 --- a/Core/Src/gpio.c +++ b/Core/Src/gpio.c @@ -49,6 +49,7 @@ void MX_GPIO_Init(void) __HAL_RCC_GPIOF_CLK_ENABLE(); __HAL_RCC_GPIOH_CLK_ENABLE(); __HAL_RCC_GPIOB_CLK_ENABLE(); + __HAL_RCC_GPIOC_CLK_ENABLE(); __HAL_RCC_GPIOA_CLK_ENABLE(); __HAL_RCC_GPIOD_CLK_ENABLE(); __HAL_RCC_GPIOG_CLK_ENABLE(); @@ -71,7 +72,7 @@ void MX_GPIO_Init(void) /*Configure GPIO pins : SDA_Pin SCL_Pin */ GPIO_InitStruct.Pin = SDA_Pin|SCL_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD; GPIO_InitStruct.Pull = GPIO_PULLUP; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; HAL_GPIO_Init(GPIOF, &GPIO_InitStruct); @@ -87,4 +88,4 @@ void MX_GPIO_Init(void) /* USER CODE BEGIN 2 */ -/* USER CODE END 2 */ \ No newline at end of file +/* USER CODE END 2 */ diff --git a/Core/Src/main.c b/Core/Src/main.c index 39aa660..9895c8e 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -21,11 +21,14 @@ #include "cmsis_os.h" #include "can.h" #include "i2c.h" +#include "stm32f4xx_hal_uart.h" #include "usart.h" #include "gpio.h" +#include /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ + /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ @@ -61,7 +64,7 @@ int __io_putchar(int ch) /* Private user code ---------------------------------------------------------*/ /* USER CODE BEGIN 0 */ - +uint8_t g_uart_rx_byte; /* USER CODE END 0 */ @@ -99,8 +102,9 @@ int main(void) MX_USART1_UART_Init(); MX_CAN2_Init(); MX_CAN1_Init(); + MX_USART6_UART_Init(); /* USER CODE BEGIN 2 */ - + HAL_UART_Receive_IT(&huart1, &g_uart_rx_byte, 1); /* USER CODE END 2 */ diff --git a/Core/Src/stm32f4xx_it.c b/Core/Src/stm32f4xx_it.c index 036af6c..becccdd 100644 --- a/Core/Src/stm32f4xx_it.c +++ b/Core/Src/stm32f4xx_it.c @@ -56,6 +56,7 @@ /* External variables --------------------------------------------------------*/ extern I2C_HandleTypeDef hi2c2; +extern UART_HandleTypeDef huart1; extern TIM_HandleTypeDef htim1; /* USER CODE BEGIN EV */ @@ -188,6 +189,20 @@ void I2C2_EV_IRQHandler(void) /* USER CODE END I2C2_EV_IRQn 1 */ } +/** + * @brief This function handles USART1 global interrupt. + */ +void USART1_IRQHandler(void) +{ + /* USER CODE BEGIN USART1_IRQn 0 */ + + /* USER CODE END USART1_IRQn 0 */ + HAL_UART_IRQHandler(&huart1); + /* USER CODE BEGIN USART1_IRQn 1 */ + + /* USER CODE END USART1_IRQn 1 */ +} + /* USER CODE BEGIN 1 */ diff --git a/Core/Src/usart.c b/Core/Src/usart.c index 8d72c05..0d712c4 100644 --- a/Core/Src/usart.c +++ b/Core/Src/usart.c @@ -25,6 +25,7 @@ /* USER CODE END 0 */ UART_HandleTypeDef huart1; +UART_HandleTypeDef huart6; /* USART1 init function */ @@ -54,6 +55,35 @@ void MX_USART1_UART_Init(void) /* USER CODE END USART1_Init 2 */ +} +/* USART6 init function */ + +void MX_USART6_UART_Init(void) +{ + + /* USER CODE BEGIN USART6_Init 0 */ + + /* USER CODE END USART6_Init 0 */ + + /* USER CODE BEGIN USART6_Init 1 */ + + /* USER CODE END USART6_Init 1 */ + huart6.Instance = USART6; + huart6.Init.BaudRate = 115200; + huart6.Init.WordLength = UART_WORDLENGTH_8B; + huart6.Init.StopBits = UART_STOPBITS_1; + huart6.Init.Parity = UART_PARITY_NONE; + huart6.Init.Mode = UART_MODE_TX_RX; + huart6.Init.HwFlowCtl = UART_HWCONTROL_NONE; + huart6.Init.OverSampling = UART_OVERSAMPLING_16; + if (HAL_UART_Init(&huart6) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN USART6_Init 2 */ + + /* USER CODE END USART6_Init 2 */ + } void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) @@ -80,10 +110,37 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) GPIO_InitStruct.Alternate = GPIO_AF7_USART1; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + /* USART1 interrupt Init */ + HAL_NVIC_SetPriority(USART1_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(USART1_IRQn); /* USER CODE BEGIN USART1_MspInit 1 */ /* USER CODE END USART1_MspInit 1 */ } + else if(uartHandle->Instance==USART6) + { + /* USER CODE BEGIN USART6_MspInit 0 */ + + /* USER CODE END USART6_MspInit 0 */ + /* USART6 clock enable */ + __HAL_RCC_USART6_CLK_ENABLE(); + + __HAL_RCC_GPIOC_CLK_ENABLE(); + /**USART6 GPIO Configuration + PC6 ------> USART6_TX + PC7 ------> USART6_RX + */ + GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_7; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + GPIO_InitStruct.Alternate = GPIO_AF8_USART6; + HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); + + /* USER CODE BEGIN USART6_MspInit 1 */ + + /* USER CODE END USART6_MspInit 1 */ + } } void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle) @@ -103,10 +160,30 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle) */ HAL_GPIO_DeInit(GPIOA, GPIO_PIN_9|GPIO_PIN_10); + /* USART1 interrupt Deinit */ + HAL_NVIC_DisableIRQ(USART1_IRQn); /* USER CODE BEGIN USART1_MspDeInit 1 */ /* USER CODE END USART1_MspDeInit 1 */ } + else if(uartHandle->Instance==USART6) + { + /* USER CODE BEGIN USART6_MspDeInit 0 */ + + /* USER CODE END USART6_MspDeInit 0 */ + /* Peripheral clock disable */ + __HAL_RCC_USART6_CLK_DISABLE(); + + /**USART6 GPIO Configuration + PC6 ------> USART6_TX + PC7 ------> USART6_RX + */ + HAL_GPIO_DeInit(GPIOC, GPIO_PIN_6|GPIO_PIN_7); + + /* USER CODE BEGIN USART6_MspDeInit 1 */ + + /* USER CODE END USART6_MspDeInit 1 */ + } } /* USER CODE BEGIN 1 */ diff --git a/IMPROVEMENT_PLAN.md b/IMPROVEMENT_PLAN.md deleted file mode 100644 index be8c6ec..0000000 --- a/IMPROVEMENT_PLAN.md +++ /dev/null @@ -1,497 +0,0 @@ -# CloudPlant 二轴云台增稳控制器 — 改进计划 - -> 基于 2026-05-12 架构审查 & 控制链路设计讨论 -> 本文档记录所有已识别的问题、设计决策和实现路径，作为后续开发的指令文档。 - ---- - -## 目录 - -1. [项目现状](#1-项目现状) -2. [架构合规性问题](#2-架构合规性问题) -3. [控制链路重设计](#3-控制链路重设计) -4. [滤波器设计](#4-滤波器设计) -5. [UART 命令接口](#5-uart-命令接口) -6. [FPU 与定点数](#6-fpu-与定点数) -7. [改进清单（按优先级）](#7-改进清单按优先级) -8. [代码模板参考](#8-代码模板参考) - ---- - -## 1. 项目现状 - -| 项目 | 值 | -|---|---| -| **名称** | CloudPlant | -| **功能** | 二轴云台增稳控制器 | -| **MCU** | STM32F407 (Cortex-M4, 168MHz) | -| **RTOS** | FreeRTOS (CMSIS-RTOS v2) | -| **传感器** | JY901P IMU (软 I2C, 最大 200Hz) | -| **执行器** | MF4010V2 无刷电机 × 2 (CAN2, ID: 0x141/0x142) | -| **调试口** | USART1 (printf) | -| **控制模式** | 增量位置闭环 (0xA7) — **需要改为速度模式** | - -### 分层架构 - -``` -APP (app/) ─ 业务逻辑 / 任务编排 - ↓ -Module (modules/) ─ 设备抽象 / 功能封装 - ↓ -Interface (interfaces/) ─ ops 函数指针表 + 注册机制 - ↓ -HAL (hal/stm32f4/) ─ 外设寄存器操作 - ↓ -BSP (bsp/stm32f4/) ─ 引脚/句柄静态映射表 - ↓ -STM32 HAL / 寄存器 -``` - -### FreeRTOS 任务 - -| 任务 | 优先级 | 周期 | 当前状态 | -|---|---|---|---| -| `sensor_task` | High | 循环 (~100Hz) | 读 12 个 IMU 寄存器 + printf 日志 | -| `control_task` | Normal | osDelay(10ms) | PID 逻辑被注释，电机启停测试 | -| `monitor_task` | Low | 1s | 占位，空循环 | - ---- - -## 2. 架构合规性问题 - -### FATAL 级 - -| # | 问题 | 位置 | 修复 | -|---|---|---|---| -| 1 | APP 直接包含 HAL 头文件 | `app/app.h:2-8` | 删除 `hal_*.h` 的 include，通过 Module 访问 | -| 2 | APP 直接调用 HAL 初始化 | `app/app_init.c:7-11` | 将 `hal_*_init_all()` 移到更底层或独立初始化函数 | - -### ERROR 级 - -| # | 问题 | 位置 | 修复 | -|---|---|---|---| -| 3 | HAL 函数名含业务语义 | `hal_led.c`, `hal_can.c` 等 | `hal_led_write` → 改为通用命名或接受现状（功能正确） | -| 4 | 两套 PID 并存 | `control_task.c` vs `pid.c` | 删掉 `control_task.c` 内的手写 PID，统一用 `pid.c` | - -### WARNING 级 - -| # | 问题 | 位置 | -|---|---|---| -| 5 | `sensor_task.c` 内联 I2C 函数 `IICreadBytes/IICwriteBytes` | 应移到 Module 层 | -| 6 | `can_if_recv()` 阻塞无超时 | `mf4010v2_send_command()` 内 send 后直接 recv | -| 7 | 魔法数字散落各处 | `1000`, `5`, `18000`, `0x141` 等 | - ---- - -## 3. 控制链路重设计 - -### 带宽确立 - -``` -JY901P 内部低通: 256Hz (WitSetBandwidth) -IMU 角度输出: 200Hz (JY901P 极限) -控制环频率: 100Hz (vTaskDelayUntil) -有效控制带宽: ~20Hz (能抑制的扰动频率上限) -``` - -### 控制模式选择 - -| 模式 | 推荐？ | 原因 | -|---|---|---| -| 位置模式 (0xA7) | ❌ 不推荐 | 两层 PID 串在一起，互相打架 | -| **速度模式 (0xA2)** | ✅ 推荐 | 你的位置 PID → MF4010V2 速度环，分层清晰 | -| 力矩模式 (0xA1) | ⚠️ 可选 | 需要自写速度环，增加调试复杂度 | - -### 目标实现 — 速度模式级联控制 - -``` -IMU 角度 → 一阶 LPF (25Hz) → 陷波滤波器 → 你的位置 PID → 速度指令 → MF4010V2 速度模式 → 电机 -``` - -### 采样优化 - -**当前**: `WitReadReg(AX, 12)` — 读 12 个寄存器 (AX~Yaw)，约 1.8ms - -**改为**: `WitReadReg(Roll, 3)` — 只读 Roll/Pitch/Yaw 三个角度，约 0.5ms - -### 控制循环模板 (`control_task.c`) - -```c -void control_task(void) -{ - // ---- 滤波器 ---- - LowPassFilter_t lpf_roll, lpf_pitch; - LPF_Init(&lpf_roll, 25.0f, CONFIG_IMU_SAMPLE_RATE_HZ); - LPF_Init(&lpf_pitch, 25.0f, CONFIG_IMU_SAMPLE_RATE_HZ); - - // ---- 位置 PID ---- - PIDController_t pos_pid_roll, pos_pid_pitch; - PIDController_Init(&pos_pid_roll); - PIDController_Init(&pos_pid_pitch); - pos_pid_roll.Kp = 80.0f; // 起始值，需整定 - pos_pid_roll.Ki = 0.5f; - pos_pid_roll.Kd = 30.0f; - pos_pid_roll.output_max = 36000.0f; // 最大 360 DPS - pos_pid_pitch.Kp = 80.0f; - pos_pid_pitch.Ki = 0.5f; - pos_pid_pitch.Kd = 30.0f; - pos_pid_pitch.output_max = 18000.0f; // 最大 180 DPS - - // ---- 电机初始化 ---- - mf4010v2_init(&motor_yaw, 0x141, 1); - mf4010v2_init(&motor_pitch, 0x142, 1); - mf4010v2_run(&motor_yaw); - mf4010v2_run(&motor_pitch); - - // ---- 控制循环 (100Hz, 无抖动) ---- - TickType_t xLastWakeTime = xTaskGetTickCount(); - const TickType_t xPeriod = pdMS_TO_TICKS(1000 / CONFIG_CONTROL_FREQ_HZ); - - while (1) { - float filt_roll = LPF_Update(&lpf_roll, fAngle[0]); - float filt_pitch = LPF_Update(&lpf_pitch, fAngle[1]); - - float yaw_speed = PIDController_Compute(&pos_pid_roll, 0.0f, filt_roll); - float pitch_speed = PIDController_Compute(&pos_pid_pitch, 0.0f, filt_pitch); - - mf4010v2_set_speed(&motor_yaw, (int32_t)(yaw_speed * 100.0f)); - mf4010v2_set_speed(&motor_pitch, (int32_t)(pitch_speed * 100.0f)); - - vTaskDelayUntil(&xLastWakeTime, xPeriod); - } -} -``` - -**关键变化**: -- `osDelay` → `vTaskDelayUntil` (消抖) -- `mf4010v2_set_angle` → `mf4010v2_set_speed` (速度模式) -- 调速 PID 输出含义: 从角度增量 → 速度指令 -- `printf` 全部移除，调试输出降级到 `monitor_task` 的 1Hz 频道 - ---- - -## 4. 滤波器设计 - -### 4.1 一阶低通滤波器 (必加) - -```c -typedef struct { - float alpha; - float prev_output; -} LowPassFilter_t; - -void LPF_Init(LowPassFilter_t *lpf, float cutoff_hz, float sample_rate_hz) { - float dt = 1.0f / sample_rate_hz; - float RC = 1.0f / (2.0f * 3.14159f * cutoff_hz); - lpf->alpha = dt / (RC + dt); - lpf->prev_output = 0.0f; -} - -static inline float LPF_Update(LowPassFilter_t *lpf, float input) { - float output = lpf->alpha * input + (1.0f - lpf->alpha) * lpf->prev_output; - lpf->prev_output = output; - return output; -} -``` - -**截止频率选择**: 25Hz (200Hz 采样, 控制带宽的 1.25×) - -### 4.2 陷波滤波器 (电机振动时加) - -```c -typedef struct { - float b0, b1, b2, a1, a2; - float x1, x2, y1, y2; -} NotchFilter_t; - -void Notch_Init(NotchFilter_t *nf, float center_hz, float bandwidth_hz, float sample_rate); -float Notch_Update(NotchFilter_t *nf, float input); -``` - -**使用流程**: -1. 不加陷波，让电机跑起来 -2. 串口 dump IMU 数据，上位机 FFT 找振动峰值频率 -3. 根据 FFT 结果设置 `center_hz` - -**串联顺序**: `raw_angle → LPF(25Hz) → Notch(实测频率) → PID` - -### 4.3 滑动平均 (调试用快速方案) - -8 点滑动平均, 200Hz 采样时截止频率 ≈ 16Hz。在不知道振动频率时替代陷波。 - ---- - -## 5. UART 命令接口 - -### 架构 - -```text -USART1 RX 中断 - → xStreamBufferSendFromISR() (ISR, ~2μs) - → Stream Buffer (256 bytes) - → xStreamBufferReceive() (monitor_task) - → CopeCmdData() → CmdProcess() - → Wit SDK API / 自定义命令 -``` - -### ISR 实现 (仅 3 行) - -```c -void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) -{ - if (huart->Instance == USART1) { - BaseType_t xHigherPriorityTaskWoken = pdFALSE; - xStreamBufferSendFromISR(uart_rx_stream, &uart_rx_byte, 1, &xHigherPriorityTaskWoken); - HAL_UART_Receive_IT(&huart1, &uart_rx_byte, 1); // 重新注册 - portYIELD_FROM_ISR(xHigherPriorityTaskWoken); - } -} -``` - -### TX 互斥 (printf vs 命令响应) - -```c -SemaphoreHandle_t uart_tx_mutex; - -// 在 MX_FREERTOS_Init() 创建 -uart_tx_mutex = xSemaphoreCreateMutex(); - -// 宏替换 printf -#define SAFE_PRINTF(fmt, ...) \ - do { xSemaphoreTake(uart_tx_mutex, portMAX_DELAY); \ - printf(fmt, ##__VA_ARGS__); \ - xSemaphoreGive(uart_tx_mutex); } while(0) -``` - -### 自定义命令扩展 - -```c -case 'r': // 电机状态 -case 'p': // 当前角度 -case 'd': // dump 数据日志 -case 'k': // 在线调 PID (e.g. "k 80.0 0.5 30.0\r\n") -``` - ---- - -## 6. FPU 与定点数 - -### 结论 - -**不需要定点数。** STM32F407 硬件 FPU 单周期完成 `float` 加减乘。需确认编译选项： - -```cmake -# CMakeLists.txt 必须包含 -target_compile_options(${CMAKE_PROJECT_NAME} PRIVATE - -mfloat-abi=hard -mfpu=fpv4-sp-d16 -) -target_link_options(${CMAKE_PROJECT_NAME} PRIVATE - -mfloat-abi=hard -mfpu=fpv4-sp-d16 -) -``` - -验证方法： -```bash -arm-none-eabi-objdump -d build/CloudPlant.elf | grep -c 'vadd\|vmul' # 应 > 50 -``` - -**注意**: `printf("%f")` 会触发软浮点库，不要在控制环里用。 - ---- - -## 7. 改进清单（按优先级） - -### 🔴 P0 — 安全 (不做可能烧硬件) - -| # | 项目 | 说明 | -|---|---|---| -| P0-1 | **电机软件限位** | Pitch ±90°, Yaw ±180°, 超限立即停机 | -| P0-2 | **独立看门狗 (IWDG)** | 1s 超时，`monitor_task` 喂狗，任务卡死自动复位 | -| P0-3 | **CAN 发送超时** | `hal_can_send()` 忙等改 50ms 超时 + `HAL_CAN_AbortTxRequest` | -| P0-4 | **IMU 离线检测** | 200ms 无 IMU 数据 → 电机急停 | - -### 🟡 P1 — 正确性 (核心功能) - -| # | 项目 | 说明 | -|---|---|---| -| P1-1 | **vTaskDelayUntil 替换 osDelay** | 控制环频率精确到 ±1ms | -| P1-2 | **速度模式替换位置模式** | `mf4010v2_set_speed` 替代 `set_angle` | -| P1-3 | **LPF 滤波器** | 一阶低通 25Hz 截止，串联在 IMU 读取和 PID 之间 | -| P1-4 | **IMU 只读角度寄存器** | `WitReadReg(Roll, 3)` 替代 `WitReadReg(AX, 12)` | -| P1-5 | **WitSetBandwidth(256Hz)** | 在 `AutoScanSensor()` 后调用 | -| P1-6 | **printf 移出控制环** | 降到 1Hz，只在 `monitor_task` 打印 | -| P1-7 | **统一 PID 模块** | 删除 `control_task.c` 内手写 PID，用 `pid.c` 的 `PIDController_t` | -| P1-8 | **FPU 编译选项** | `-mfloat-abi=hard -mfpu=fpv4-sp-d16` | - -### 🟢 P2 — 健壮性 - -| # | 项目 | 说明 | -|---|---|---| -| P2-1 | **栈溢出检测** | `configCHECK_FOR_STACK_OVERFLOW = 2` | -| P2-2 | **参数持久化** | PID 参数存 Flash 末扇区 | -| P2-3 | **UART 命令接口** | ISR → StreamBuffer → monitor_task，实现串口调参 | -| P2-4 | **陷波滤波器** | 实测电机振动频率后加入 | -| P2-5 | **LED 状态编码** | 快闪/常亮/交替表示不同故障 | - -### 🔵 P3 — 工程化 - -| # | 项目 | 说明 | -|---|---|---| -| P3-1 | **配置常量集中** | 创建 `config.h`，消除魔法数字 | -| P3-2 | **错误码统一** | 统一 `error_code_t` 枚举 | -| P3-3 | **数据日志环形缓冲** | 100Hz × 10s 黑匣子，串口 dump | -| P3-4 | **架构合规修复** | APP 不直接 include HAL 头文件 | -| P3-5 | **CMake 编译警告** | `-Wall -Wextra -Werror=return-type` | -| P3-6 | **PID 单元测试** | `pid.c` 在 PC 上用 CMocka/Unity 测 | - -### 🟣 P4 — 性能优化（可选） - -| # | 项目 | 说明 | -|---|---|---| -| P4-1 | **重力前馈** | 补偿姿态相关的重力矩 | -| P4-2 | **增益调度** | 不同姿态角使用不同 PID 增益 | -| P4-3 | **在线调参上位机** | 简易二进制协议，无需重编译 | - ---- - -## 8. 代码模板参考 - -### 8.1 config.h 模板 - -```c -#ifndef CONFIG_H -#define CONFIG_H - -// ---- 系统 ---- -#define CONFIG_CONTROL_FREQ_HZ 100 -#define CONFIG_IMU_SAMPLE_RATE_HZ 200 -#define CONFIG_MONITOR_PERIOD_MS 1000 - -// ---- 滤波器 ---- -#define CONFIG_LPF_CUTOFF_HZ 25.0f -#define CONFIG_NOTCH_CENTER_HZ 0.0f // 0 = 禁用, 实测后填入 - -// ---- 电机 ---- -#define CONFIG_MOTOR_YAW_CAN_ID 0x141 -#define CONFIG_MOTOR_PITCH_CAN_ID 0x142 -#define CONFIG_MOTOR_CAN_CHANNEL 1 -#define CONFIG_MOTOR_PITCH_MAX_DEG 90.0f -#define CONFIG_MOTOR_YAW_MAX_DEG 180.0f - -// ---- PID 初始值 ---- -#define CONFIG_PID_ROLL_KP 80.0f -#define CONFIG_PID_ROLL_KI 0.5f -#define CONFIG_PID_ROLL_KD 30.0f -#define CONFIG_PID_PITCH_KP 80.0f -#define CONFIG_PID_PITCH_KI 0.5f -#define CONFIG_PID_PITCH_KD 30.0f - -// ---- 安全 ---- -#define CONFIG_IMU_TIMEOUT_MS 200 -#define CONFIG_CAN_TIMEOUT_MS 50 -#define CONFIG_WATCHDOG_TIMEOUT_MS 1000 - -// ---- 栈大小 ---- -#define CONFIG_STACK_CONTROL 512 -#define CONFIG_STACK_SENSOR 512 -#define CONFIG_STACK_MONITOR 384 - -#endif -``` - -### 8.2 sensor_task 精简版 - -```c -void sensor_task(void) -{ - float fAcc[3], fGyro[3], fAngle[3]; - int ret = soft_i2c_init(&si2c, 0); - - WitInit(WIT_PROTOCOL_I2C, 0x50); - WitI2cFuncRegister(IICwriteBytes, IICreadBytes); - WitRegisterCallBack(CopeSensorData); - WitDelayMsRegister(Delayms); - AutoScanSensor(); - WitSetBandwidth(BANDWIDTH_256HZ); // ← 新增 - - while (1) { - WitReadReg(Roll, 3); // ← 只读角度, 原来读 12 个 - Delayms(2); // ← 5ms → 2ms - CmdProcess(); // ← 仅传感器相关命令, 其余移到 monitor_task - - if (s_cDataUpdate & ANGLE_UPDATE) { - for (int i = 0; i < 3; i++) { - fAngle[i] = sReg[Roll+i] * 0.0054931640625f; - } - imu_last_update = HAL_GetTick(); // ← 新增心跳 - s_cDataUpdate &= ~ANGLE_UPDATE; - } - } -} -``` - -### 8.3 CAN 超时修复 - -```c -// hal_can.c — hal_can_send() 修改 -static int hal_can_send(int ch, const can_message_t *msg) -{ - // ... 前置代码不变 ... - - uint32_t TxMailbox; - if (HAL_CAN_AddTxMessage(hcan, &TxHeader, (uint8_t*)msg->data, &TxMailbox) != HAL_OK) - return -1; - - // 带超时的等待发送完成 - uint32_t tickstart = HAL_GetTick(); - while (HAL_CAN_IsTxMessagePending(hcan, TxMailbox)) { - if ((HAL_GetTick() - tickstart) > CONFIG_CAN_TIMEOUT_MS) { - HAL_CAN_AbortTxRequest(hcan, TxMailbox); - return -1; - } - } - return 0; -} -``` - -### 8.4 错误码统一 - -```c -// error_codes.h -typedef enum { - ERR_OK = 0, - ERR_PARAM = -1, - ERR_TIMEOUT = -2, - ERR_NOT_INIT = -3, - ERR_HARDWARE = -4, - ERR_CAN_SEND = -5, - ERR_CAN_RECV = -6, - ERR_I2C_NAK = -7, - ERR_IMU_OFFLINE = -8, - ERR_MOTOR_LIMIT = -9, - ERR_MOTOR_FAULT = -10, -} error_code_t; -``` - ---- - -## 附录: 需要修改的文件清单 - -| 文件 | 改动 | -|---|---| -| `app/control_task.c` | 重写：速度模式 + vTaskDelayUntil + 滤波器 + 统一 PID | -| `app/sensor_task.c` | 精简：只读 3 个角度, 移除 CopeCmdData/CmdProcess | -| `app/monitor_task.c` | 新建：UART 命令接口 + 1Hz 监控 + 喂狗 | -| `app/app.h` | 移除 HAL 头文件, 添加全局变量声明 | -| `app/app_init.c` | 调整 HAL 初始化调用位置 | -| `CMakeLists.txt` | 添加 FPU 编译选项, 栈分析, 内存报告 | -| `hal/stm32f4/can/hal_can.c` | CAN 发送加超时 | -| `modules/control/pid/pid.c` | 确认输出限幅逻辑 (当前 output_max 写死在 204800) | -| `Core/Inc/config.h` | **新建**：所有配置常量 | -| `Core/Inc/error_codes.h` | **新建**：统一错误码 | -| `Core/Inc/filter.h` | **新建**：LPF/Notch/MAF 结构体定义 | -| `Core/Inc/FreeRTOSConfig.h` | 开启 `configCHECK_FOR_STACK_OVERFLOW` | - ---- - -> **一句话原则**: 每一个改进项都是可验证的独立单元，改一项测一项，不要一次性全部改动。 diff --git a/TangYuJieBoard.ioc b/TangYuJieBoard.ioc deleted file mode 100644 index df6b16c..0000000 --- a/TangYuJieBoard.ioc +++ /dev/null @@ -1,179 +0,0 @@ -#MicroXplorer Configuration settings - do not modify -CAD.formats= -CAD.pinconfig= -CAD.provider= -CAN1.BS1=CAN_BS1_2TQ -CAN1.BS2=CAN_BS2_4TQ -CAN1.CalculateBaudRate=1000000 -CAN1.CalculateTimeBit=1000 -CAN1.CalculateTimeQuantum=142.85714285714286 -CAN1.IPParameters=CalculateTimeQuantum,CalculateTimeBit,CalculateBaudRate,BS1,BS2,Prescaler -CAN1.Prescaler=6 -CAN2.BS1=CAN_BS1_2TQ -CAN2.BS2=CAN_BS2_4TQ -CAN2.CalculateBaudRate=1000000 -CAN2.CalculateTimeBit=1000 -CAN2.CalculateTimeQuantum=142.85714285714286 -CAN2.IPParameters=CalculateTimeQuantum,CalculateTimeBit,CalculateBaudRate,Prescaler,BS2,BS1,NART -CAN2.NART=ENABLE -CAN2.Prescaler=6 -FREERTOS.FootprintOK=true -FREERTOS.IPParameters=Tasks01,configENABLE_FPU,configUSE_NEWLIB_REENTRANT,configMINIMAL_STACK_SIZE,configTOTAL_HEAP_SIZE,FootprintOK -FREERTOS.Tasks01=monitorTask,8,256,StartMonitorTask,Default,NULL,Dynamic,NULL,NULL;controlTask,24,256,StartControlTask,Default,NULL,Dynamic,NULL,NULL;sensorTask,40,256,StartSensorTask,Default,NULL,Dynamic,NULL,NULL -FREERTOS.configENABLE_FPU=1 -FREERTOS.configMINIMAL_STACK_SIZE=256 -FREERTOS.configTOTAL_HEAP_SIZE=32768 -FREERTOS.configUSE_NEWLIB_REENTRANT=1 -File.Version=6 -GPIO.groupedBy= -I2C2.I2C_Mode=I2C_Fast -I2C2.IPParameters=I2C_Mode -KeepUserPlacement=false -Mcu.CPN=STM32F407ZGT6 -Mcu.Family=STM32F4 -Mcu.IP0=CAN1 -Mcu.IP1=CAN2 -Mcu.IP2=FREERTOS -Mcu.IP3=I2C2 -Mcu.IP4=NVIC -Mcu.IP5=RCC -Mcu.IP6=SYS -Mcu.IP7=USART1 -Mcu.IPNb=8 -Mcu.Name=STM32F407Z(E-G)Tx -Mcu.Package=LQFP144 -Mcu.Pin0=PF0 -Mcu.Pin1=PF1 -Mcu.Pin10=VP_FREERTOS_VS_CMSIS_V2 -Mcu.Pin11=VP_SYS_VS_tim1 -Mcu.Pin2=PH0-OSC_IN -Mcu.Pin3=PH1-OSC_OUT -Mcu.Pin4=PB12 -Mcu.Pin5=PB13 -Mcu.Pin6=PA9 -Mcu.Pin7=PA10 -Mcu.Pin8=PD0 -Mcu.Pin9=PD1 -Mcu.PinsNb=12 -Mcu.ThirdPartyNb=0 -Mcu.UserConstants= -Mcu.UserName=STM32F407ZGTx -MxCube.Version=6.14.0 -MxDb.Version=DB.6.0.140 -NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false -NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false -NVIC.ForceEnableDMAVector=true -NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false -NVIC.I2C2_EV_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true -NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false -NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false -NVIC.PendSV_IRQn=true\:15\:0\:false\:false\:false\:true\:false\:false\:false -NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4 -NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:false\:false\:false\:false\:false -NVIC.SavedPendsvIrqHandlerGenerated=true -NVIC.SavedSvcallIrqHandlerGenerated=true -NVIC.SavedSystickIrqHandlerGenerated=true -NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:false\:true\:false\:true\:false -NVIC.TIM1_UP_TIM10_IRQn=true\:15\:0\:false\:false\:true\:false\:false\:true\:true -NVIC.TimeBase=TIM1_UP_TIM10_IRQn -NVIC.TimeBaseIP=TIM1 -NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false -PA10.Mode=Asynchronous -PA10.Signal=USART1_RX -PA9.Mode=Asynchronous -PA9.Signal=USART1_TX -PB12.Mode=CAN_Activate -PB12.Signal=CAN2_RX -PB13.Mode=CAN_Activate -PB13.Signal=CAN2_TX -PD0.Locked=true -PD0.Mode=CAN_Activate -PD0.Signal=CAN1_RX -PD1.Locked=true -PD1.Mode=CAN_Activate -PD1.Signal=CAN1_TX -PF0.Mode=I2C -PF0.Signal=I2C2_SDA -PF1.Mode=I2C -PF1.Signal=I2C2_SCL -PH0-OSC_IN.Mode=HSE-External-Oscillator -PH0-OSC_IN.Signal=RCC_OSC_IN -PH1-OSC_OUT.Mode=HSE-External-Oscillator -PH1-OSC_OUT.Signal=RCC_OSC_OUT -PinOutPanel.RotationAngle=0 -ProjectManager.AskForMigrate=true -ProjectManager.BackupPrevious=true -ProjectManager.CompilerLinker=GCC -ProjectManager.CompilerOptimize=6 -ProjectManager.ComputerToolchain=false -ProjectManager.CoupleFile=true -ProjectManager.CustomerFirmwarePackage= -ProjectManager.DefaultFWLocation=true -ProjectManager.DeletePrevious=true -ProjectManager.DeviceId=STM32F407ZGTx -ProjectManager.FirmwarePackage=STM32Cube FW_F4 V1.28.3 -ProjectManager.FreePins=false -ProjectManager.HalAssertFull=false -ProjectManager.HeapSize=0x200 -ProjectManager.KeepUserCode=true -ProjectManager.LastFirmware=true -ProjectManager.LibraryCopy=0 -ProjectManager.MainLocation=Core/Src -ProjectManager.MultiThreaded=true -ProjectManager.NoMain=false -ProjectManager.PreviousToolchain= -ProjectManager.ProjectBuild=false -ProjectManager.ProjectFileName=CloudPlant.ioc -ProjectManager.ProjectName=CloudPlant -ProjectManager.ProjectStructure= -ProjectManager.RegisterCallBack= -ProjectManager.StackSize=0x400 -ProjectManager.TargetToolchain=CMake -ProjectManager.ThreadSafeStrategy=Cortex-M4NS\:GenericStrategy2, -ProjectManager.ToolChainLocation= -ProjectManager.UAScriptAfterPath= -ProjectManager.UAScriptBeforePath= -ProjectManager.UnderRoot=false -ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_I2C2_Init-I2C2-false-HAL-true,4-MX_USART1_UART_Init-USART1-false-HAL-true,5-MX_CAN2_Init-CAN2-false-HAL-true -RCC.48MHZClocksFreq_Value=84000000 -RCC.AHBFreq_Value=168000000 -RCC.APB1CLKDivider=RCC_HCLK_DIV4 -RCC.APB1Freq_Value=42000000 -RCC.APB1TimFreq_Value=84000000 -RCC.APB2CLKDivider=RCC_HCLK_DIV2 -RCC.APB2Freq_Value=84000000 -RCC.APB2TimFreq_Value=168000000 -RCC.CortexFreq_Value=168000000 -RCC.EthernetFreq_Value=168000000 -RCC.FCLKCortexFreq_Value=168000000 -RCC.FamilyName=M -RCC.HCLKFreq_Value=168000000 -RCC.HSE_VALUE=8000000 -RCC.HSICalibrationValue=16 -RCC.HSI_VALUE=16000000 -RCC.I2SClocksFreq_Value=192000000 -RCC.IPParameters=48MHZClocksFreq_Value,AHBFreq_Value,APB1CLKDivider,APB1Freq_Value,APB1TimFreq_Value,APB2CLKDivider,APB2Freq_Value,APB2TimFreq_Value,CortexFreq_Value,EthernetFreq_Value,FCLKCortexFreq_Value,FamilyName,HCLKFreq_Value,HSE_VALUE,HSICalibrationValue,HSI_VALUE,I2SClocksFreq_Value,LSE_VALUE,LSI_VALUE,MCO2PinFreq_Value,PLLCLKFreq_Value,PLLM,PLLN,PLLQCLKFreq_Value,PLLSourceVirtual,RTCFreq_Value,RTCHSEDivFreq_Value,SYSCLKFreq_VALUE,SYSCLKSource,VCOI2SOutputFreq_Value,VCOInputFreq_Value,VCOOutputFreq_Value,VcooutputI2S -RCC.LSE_VALUE=32768 -RCC.LSI_VALUE=32000 -RCC.MCO2PinFreq_Value=168000000 -RCC.PLLCLKFreq_Value=168000000 -RCC.PLLM=4 -RCC.PLLN=168 -RCC.PLLQCLKFreq_Value=84000000 -RCC.PLLSourceVirtual=RCC_PLLSOURCE_HSE -RCC.RTCFreq_Value=32000 -RCC.RTCHSEDivFreq_Value=4000000 -RCC.SYSCLKFreq_VALUE=168000000 -RCC.SYSCLKSource=RCC_SYSCLKSOURCE_PLLCLK -RCC.VCOI2SOutputFreq_Value=384000000 -RCC.VCOInputFreq_Value=2000000 -RCC.VCOOutputFreq_Value=336000000 -RCC.VcooutputI2S=192000000 -USART1.IPParameters=VirtualMode -USART1.VirtualMode=VM_ASYNC -VP_FREERTOS_VS_CMSIS_V2.Mode=CMSIS_V2 -VP_FREERTOS_VS_CMSIS_V2.Signal=FREERTOS_VS_CMSIS_V2 -VP_SYS_VS_tim1.Mode=TIM1 -VP_SYS_VS_tim1.Signal=SYS_VS_tim1 -board=custom -rtos.0.ip=FREERTOS diff --git a/app/app.h b/app/app.h index 9e069c8..721751c 100644 --- a/app/app.h +++ b/app/app.h @@ -1,21 +1,63 @@ -#ifndef __APP_H__ -#define __APP_H__ -#include "hal_gpio.h" -#include "led.h" -#include "mf4010v2.h" -#include "hal_i2c.h" -#include "hal_can.h" -#include "hal_led.h" -#include "hal_soft_i2c.h" -#include "freertos.h" -#include "pid.h" -#include -#include -#include "cmsis_os.h" -#include "projdefs.h" -#include "wit_c_sdk.h" -void app_init(void); -void sensor_task(void); -void control_task(void); -void monitor_task(void); -#endif +#ifndef __APP_H__ +#define __APP_H__ +#include "hal_gpio.h" +#include "led.h" +#include "mf4010v2.h" +#include "hal_i2c.h" +#include "hal_can.h" +#include "hal_led.h" +#include "hal_soft_i2c.h" +#include "hal_uart.h" +#include "freertos.h" +#include "pid.h" +#include +#include +#include "cmsis_os.h" +#include "cmsis_os2.h" +#include "projdefs.h" +#include "wit_c_sdk.h" + +/* ============================================================================ + * IMU 传感器数据结构 — sensor_task → control_task 数据通道 + * ============================================================================ */ +typedef struct { + float acc[3]; /* 加速度 X/Y/Z (g) */ + float gyro[3]; /* 角速度 X/Y/Z (deg/s) */ + float angle[3]; /* 欧拉角 Roll/Pitch/Yaw (deg) */ +} imu_data_t; + +/* IMU 数据队列句柄（app_init 创建） */ +extern osMessageQueueId_t g_imu_queue, g_cmd_queue; +extern osMutexId_t g_uart_mutex; +extern uint8_t g_uart_rx_byte; + +void log_printf(const char *fmt, ...); + +/* 最大命令长度（包括结尾空字符） */ +#define MAX_CMD_LEN 64 + +void app_init(void); +void sensor_task(void); +void control_task(void); +void callback_task(void); + +/* Control task API — from callback_task */ +#define GIMBAL_MOTOR_YAW 0 +#define GIMBAL_MOTOR_PITCH 1 + +void gimbal_motor_enable(void); +void gimbal_motor_disable(void); +void gimbal_motor_enable_id(int id); +void gimbal_motor_disable_id(int id); +int gimbal_is_motor_enabled(void); +int gimbal_set_pid(const char *axis, const char *param, float value); +int gimbal_get_pid(const char *axis, float *kp, float *ki, float *kd); + +/* Sensor task API — from callback_task */ +void sensor_enable(void); +void sensor_disable(void); +void sensor_acc_cali_enable(void); +void sensor_acc_cali_disable(void); +void sensor_mag_cali_enable(void); +void sensor_mag_cali_disable(void); +#endif diff --git a/app/app_init.c b/app/app_init.c index e68e586..d4f0b4d 100644 --- a/app/app_init.c +++ b/app/app_init.c @@ -1,39 +1,67 @@ -#include "app.h" - - -static led_t led0, led1, led2; - - -void app_init(void) -{ - // 初始化硬件抽象层 - hal_i2c_init_all(); - hal_led_init_all(); - hal_can_init_all(); - hal_gpio_init_all(); - hal_soft_i2c_init_all(); - // 初始化外设 - led_init(&led0, 0); - led_init(&led1, 1); - led_init(&led2, 2); - led_on(&led0); - led_on(&led1); - led_on(&led2); - led_off(&led0); - - - printf("[app_init] Hardware initialized\r\n"); -} - - - - - -void monitor_task(void) -{ - while (1) { - // 简单的监控任务：每秒打印一次当前状态 - // printf("Pitch: %.2f deg, Roll: %.2f deg\n", g_imu.angle[1] / 100.0f, g_imu.angle[0] / 100.0f); - vTaskDelay(pdMS_TO_TICKS(1000)); // 1Hz 更新率 - } -} \ No newline at end of file +#include "app.h" +#include "cmsis_os2.h" +#include +#include + + +static led_t led0, led1, led2; + +/* IMU 数据队列句柄 — sensor_task 和 control_task 共享 */ +osMessageQueueId_t g_imu_queue = NULL; + +/* UART 命令队列 — ISR → callback_task */ +osMessageQueueId_t g_cmd_queue = NULL; + +/* UART 发送互斥锁 — 防止多任务 printf 交叉 */ +osMutexId_t g_uart_mutex = NULL; + + +void app_init(void) +{ + // 初始化硬件抽象层 + hal_i2c_init_all(); + hal_led_init_all(); + hal_can_init_all(); + hal_gpio_init_all(); + hal_soft_i2c_init_all(); + hal_uart_init_all(); + + // 初始化外设 + led_init(&led0, 0); + led_init(&led1, 1); + led_init(&led2, 2); + led_on(&led0); + led_on(&led1); + led_on(&led2); + led_off(&led0); + + // 创建 IMU 数据队列（sensor_task → control_task） + // 深度 4，每消息 3×float×3 = 36 bytes + g_imu_queue = osMessageQueueNew(1, sizeof(imu_data_t), NULL); + g_cmd_queue = osMessageQueueNew(5, MAX_CMD_LEN, NULL); + + // 创建 UART 互斥锁（保护 printf 不被多任务交叉打断） + g_uart_mutex = osMutexNew(NULL); + if (g_cmd_queue == NULL) { + printf("[app_init] ERROR: Failed to create command queue\r\n"); + } + if (g_imu_queue == NULL) { + printf("[app_init] ERROR: Failed to create IMU queue\r\n"); + } + + printf("[app_init] Hardware initialized\r\n"); +} + +void log_printf(const char *fmt, ...) +{ + va_list args; + if (g_uart_mutex != NULL) { + osMutexAcquire(g_uart_mutex, osWaitForever); + } + va_start(args, fmt); + vprintf(fmt, args); + va_end(args); + if (g_uart_mutex != NULL) { + osMutexRelease(g_uart_mutex); + } +} diff --git a/app/callback_task.c b/app/callback_task.c new file mode 100644 index 0000000..3625414 --- /dev/null +++ b/app/callback_task.c @@ -0,0 +1,191 @@ +#include "app.h" +#include "cmsis_os2.h" +#include +#include "FreeRTOS.h" +#include "queue.h" +#include "stm32f4xx_hal.h" + +typedef enum { + USER_CMD_IDLE = 0, + USER_CMD_RECEIVE, +} user_cmd_state; + +static struct { + user_cmd_state state; + char buffer[MAX_CMD_LEN]; + uint8_t index; + osMessageQueueId_t cmd_queue; +} g_cmd_parser; + +/* + * 支持的命令（格式：@command\n）: + * "motor enable" — 使能全部电机（零速启动） + * "motor enable yaw" — 使能 Yaw 电机 + * "motor enable pitch" — 使能 Pitch 电机 + * "motor disable" — 关闭全部电机（惰行 coast） + * "motor disable yaw" — 关闭 Yaw 电机 + * "motor disable pitch" — 关闭 Pitch 电机 + * "sensor enable" — 恢复 IMU 读取 + * "sensor disable" — 暂停 IMU 读取 + * "sensorcali acc enable" — 开始加速度计标定 + * "sensorcali acc disable" — 停止加速度计标定 + * "sensorcali mag enable" — 开始磁场标定 + * "sensorcali mag disable" — 停止磁场标定 + */ + +void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) +{ + if (huart->Instance != USART1) return; + + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + uint8_t rx = g_uart_rx_byte; + + switch (g_cmd_parser.state) { + case USER_CMD_IDLE: + if (rx == '@') { + g_cmd_parser.state = USER_CMD_RECEIVE; + g_cmd_parser.index = 0; + } + break; + + case USER_CMD_RECEIVE: + if (rx == '\n') { + /* 命令结束 —— null-terminate 并入队 */ + if (g_cmd_parser.cmd_queue != NULL) { + g_cmd_parser.buffer[g_cmd_parser.index] = '\0'; + xQueueSendFromISR(g_cmd_parser.cmd_queue, + g_cmd_parser.buffer, + &xHigherPriorityTaskWoken); + } + g_cmd_parser.state = USER_CMD_IDLE; + g_cmd_parser.index = 0; + } else if (rx == '\r') { + /* 忽略回车符（兼容终端发送 \r\n） */ + } else if (rx == '@') { + /* 重新开始 —— 取消当前输入 */ + g_cmd_parser.index = 0; + } else { + if (g_cmd_parser.index < MAX_CMD_LEN - 1) { + g_cmd_parser.buffer[g_cmd_parser.index++] = (char)rx; + } else { + /* 缓冲区溢出 — 丢弃并等待下一个 @ */ + g_cmd_parser.state = USER_CMD_IDLE; + g_cmd_parser.index = 0; + } + } + break; + } + + /* 重新使能 UART 接收中断 */ + HAL_UART_Receive_IT(huart, &g_uart_rx_byte, 1); + + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); +} + +void callback_task(void) +{ + char cmd_buf[MAX_CMD_LEN]; + + g_cmd_parser.state = USER_CMD_IDLE; + g_cmd_parser.index = 0; + g_cmd_parser.cmd_queue = g_cmd_queue; + + while (1) { + osStatus_t qstat = osMessageQueueGet(g_cmd_queue, cmd_buf, NULL, osWaitForever); + if (qstat != osOK) { + continue; + } + if (strcmp(cmd_buf, "help") == 0) { + log_printf("[cmd] available commands:\r\n" + " help ==> show this message\r\n" + " motor enable [yaw|pitch] ==> enable motor(s), default both\r\n" + " motor disable [yaw|pitch] ==> disable motor(s), default both\r\n" + " sensor enable ==> resume IMU data publishing\r\n" + " sensor disable ==> pause IMU data publishing\r\n" + " sensorcali acc enable ==> start accelerometer calibration\r\n" + " sensorcali acc disable ==> stop accelerometer calibration\r\n" + " sensorcali mag enable ==> start magnetometer calibration\r\n" + " sensorcali mag disable ==> stop magnetometer calibration\r\n" + " pid ==> show all PID coefficients\r\n" + " pid

==> set PID param (v÷10=实际值, motor disabled)\r\n" + " axis: roll|pitch, p: kp|ki|kd, v: float\r\n"); + } else if (strncmp(cmd_buf, "motor enable", 12) == 0) { + const char *p = cmd_buf + 12; + while (*p == ' ') p++; + if (*p == '\0') { + gimbal_motor_enable(); + log_printf("[cmd] both motors enabled\r\n"); + } else if (strcmp(p, "yaw") == 0) { + gimbal_motor_enable_id(GIMBAL_MOTOR_YAW); + log_printf("[cmd] yaw motor enabled\r\n"); + } else if (strcmp(p, "pitch") == 0) { + gimbal_motor_enable_id(GIMBAL_MOTOR_PITCH); + log_printf("[cmd] pitch motor enabled\r\n"); + } else { + log_printf("[cmd] unknown motor: %s\r\n", p); + } + } else if (strncmp(cmd_buf, "motor disable", 13) == 0) { + const char *p = cmd_buf + 13; + while (*p == ' ') p++; + if (*p == '\0') { + gimbal_motor_disable(); + log_printf("[cmd] both motors disabled\r\n"); + } else if (strcmp(p, "yaw") == 0) { + gimbal_motor_disable_id(GIMBAL_MOTOR_YAW); + log_printf("[cmd] yaw motor disabled\r\n"); + } else if (strcmp(p, "pitch") == 0) { + gimbal_motor_disable_id(GIMBAL_MOTOR_PITCH); + log_printf("[cmd] pitch motor disabled\r\n"); + } else { + log_printf("[cmd] unknown motor: %s\r\n", p); + } + } else if (strcmp(cmd_buf, "sensor enable") == 0) { + sensor_enable(); + log_printf("[cmd] sensor enabled\r\n"); + } else if (strcmp(cmd_buf, "sensor disable") == 0) { + sensor_disable(); + log_printf("[cmd] sensor disabled\r\n"); + } else if (strcmp(cmd_buf, "sensorcali acc enable") == 0) { + sensor_acc_cali_enable(); + log_printf("[cmd] acc calibration started\r\n"); + } else if (strcmp(cmd_buf, "sensorcali acc disable") == 0) { + sensor_acc_cali_disable(); + log_printf("[cmd] acc calibration stopped\r\n"); + } else if (strcmp(cmd_buf, "sensorcali mag enable") == 0) { + sensor_mag_cali_enable(); + log_printf("[cmd] mag calibration started\r\n"); + } else if (strcmp(cmd_buf, "sensorcali mag disable") == 0) { + sensor_mag_cali_disable(); + log_printf("[cmd] mag calibration stopped\r\n"); + } else if (strcmp(cmd_buf, "pid") == 0) { + /* 查看当前 PID 系数（整数显示，值 = 实际系数 × 10） */ + float kp, ki, kd; + gimbal_get_pid("roll", &kp, &ki, &kd); + log_printf("[cmd] Roll: Kp=%d Ki=%d Kd=%d\r\n", + (int)(kp * 10.0f), (int)(ki * 10.0f), (int)(kd * 10.0f)); + gimbal_get_pid("pitch", &kp, &ki, &kd); + log_printf("[cmd] Pitch: Kp=%d Ki=%d Kd=%d\r\n", + (int)(kp * 10.0f), (int)(ki * 10.0f), (int)(kd * 10.0f)); + } else if (strncmp(cmd_buf, "pid ", 4) == 0) { + /* 调参：pid （value ÷ 10 = 实际系数） */ + char axis[8], param[8]; + int int_val; + if (sscanf(cmd_buf, "pid %7s %7s %d", axis, param, &int_val) == 3) { + if (gimbal_is_motor_enabled()) { + log_printf("[cmd] ERROR: disable motor first (motor disable)\r\n"); + } else { + float value = (float)int_val / 10.0f; + if (gimbal_set_pid(axis, param, value) == 0) { + log_printf("[cmd] pid.%s.%s = %d\r\n", axis, param, int_val); + } else { + log_printf("[cmd] usage: pid \r\n"); + } + } + } else { + log_printf("[cmd] usage: pid \r\n"); + } + } else { + log_printf("[cmd] unknown: %s\r\n", cmd_buf); + } + } +} diff --git a/app/control_task.c b/app/control_task.c index 1994dd0..4acfa0b 100644 --- a/app/control_task.c +++ b/app/control_task.c @@ -1,165 +1,279 @@ -/** - * @file control_task.c - * @brief 二轴云台增稳控制任务 - * @note 目标：保持 Roll=0, Pitch=0，Yaw 用于方向控制 - * - * 控制架构： - * IMU 数据 → PID 控制器 → 电机补偿角度 → 抵消基座倾斜 - */ - -#include "app.h" -#include "cmsis_os2.h" -#include "mf4010v2.h" -#include - -/* ============================================================================ - * 外部变量 (来自 sensor_task) - * ============================================================================ */ -extern float fAcc[3], fGyro[3], fAngle[3]; /* fAngle[0]=Roll, fAngle[1]=Pitch, fAngle[2]=Yaw */ - -/* ============================================================================ - * 云台配置 - * ============================================================================ */ - -/* 目标角度：保持水平 */ -static float g_target_roll = 0.0f; -static float g_target_pitch = 0.0f; -static float g_target_yaw = 0.0f; /* Yaw 目标可通过外部修改 */ - -/* 电机实例 */ -static mf4010v2_t motor_yaw; /* Yaw 轴电机（外框） */ -static mf4010v2_t motor_pitch; /* Pitch 轴电机（内框） */ - -/* PID 控制器 */ -typedef struct { - float Kp, Ki, Kd; - float integral; - float prev_error; - float output_max; -} PID_t; - -static PID_t g_roll_pid = {0}; -static PID_t g_pitch_pid = {0}; - -/* ============================================================================ - * PID 控制函数 - * ============================================================================ */ - -void PID_Init(PID_t *pid, float kp, float ki, float kd, float max) -{ - pid->Kp = kp; - pid->Ki = ki; - pid->Kd = kd; - pid->output_max = max; - pid->integral = 0; - pid->prev_error = 0; -} - -float PID_Compute(PID_t *pid, float target, float actual) -{ - float error = target - actual; - float output; - - /* 比例项 */ - output = pid->Kp * error; - - /* 积分项（带抗饱和） */ - pid->integral += error; - if (pid->integral > pid->output_max) { - pid->integral = pid->output_max; - } else if (pid->integral < -pid->output_max) { - pid->integral = -pid->output_max; - } - output += pid->Ki * pid->integral; - - /* 微分项 */ - float derivative = error - pid->prev_error; - output += pid->Kd * derivative; - pid->prev_error = error; - - /* 输出限幅 */ - if (output > pid->output_max) { - output = pid->output_max; - } else if (output < -pid->output_max) { - output = -pid->output_max; - } - - return output; -} - -/* ============================================================================ - * 控制任务 - * ============================================================================ */ - -void control_task(void) -{ - // int32_t yaw_cmd, pitch_cmd; - // float roll_comp, pitch_comp; - - printf("[control_task] starting stabilization control\n"); - - /* 1. 初始化 PID 参数 */ - /* Roll 轴 PID：控制 Yaw 电机来补偿 Roll 倾斜 */ - // PID_Init(&g_roll_pid, 0.5f, 0.01f, 0.1f, 18000.0f); /* 最大补偿 180 度 */ - - /* Pitch 轴 PID：控制 Pitch 电机补偿 Pitch 倾斜 */ - // PID_Init(&g_pitch_pid, 0.5f, 0.01f, 0.1f, 9000.0f); /* 最大补偿 90 度 */ - - /* 2. 初始化电机 */ - // osDelay(pdMS_TO_TICKS(1000)); - mf4010v2_init(&motor_yaw, 0x141, 1); /* Yaw 电机 ID: 0x141, CAN2 */ - mf4010v2_init(&motor_pitch, 0x142, 1); /* Pitch 电机 ID: 0x142, CAN2 */ - - /* 3. 电机关机状态启动 */ - mf4010v2_close(&motor_yaw); - mf4010v2_close(&motor_pitch); - - printf("[control_task] motor yaw is fault: %d\n", mf4010v2_is_fault(&motor_yaw)); - printf("[control_task] motor pitch is fault: %d\n", mf4010v2_is_fault(&motor_pitch)); - /* 等待 IMU 稳定 */ - osDelay(pdMS_TO_TICKS(5000)); - - /* 4. 使能电机 */ - mf4010v2_run(&motor_yaw); - mf4010v2_run(&motor_pitch); - - printf("[control_task] stabilization enabled\n"); - - /* 主控制循环 (100Hz) */ - while (1) { - /* 读取 IMU 数据 */ - // float current_roll = fAngle[0]; /* Roll 角 */ - // float current_pitch = fAngle[1]; /* Pitch 角 */ - - // /* PID 计算补偿量 */ - // /* 目标：Roll=0, Pitch=0 */ - // roll_comp = PID_Compute(&g_roll_pid, g_target_roll, current_roll); - // pitch_comp = PID_Compute(&g_pitch_pid, g_target_pitch, current_pitch); - - // /* 运动学映射：补偿量 -> 电机角度 */ - // /* 对于 Pitch-Yaw 云台： - // * - Pitch 倾斜 → Pitch 电机反向补偿 - // * - Roll 倾斜 → Yaw 电机 + Pitch 电机联合补偿（简化为 Yaw 电机） - // */ - // yaw_cmd = (int32_t)(roll_comp * 100.0f); /* 转为 0.01 度 */ - // pitch_cmd = (int32_t)(pitch_comp * 100.0f); - - // /* 发送位置指令到电机 */ - // /* 使用增量位置控制，直接叠加补偿量 */ - // mf4010v2_set_angle(&motor_yaw, motor_yaw.current_angle + yaw_cmd); - // mf4010v2_set_angle(&motor_pitch, motor_pitch.current_angle + pitch_cmd); - - /* 100Hz 控制循环 */ - osDelay(pdMS_TO_TICKS(10)); - } -} - -/* ============================================================================ - * 外部 API - * ============================================================================ */ - -/* 设置 Yaw 目标方向 */ -void gimbal_set_yaw_target(float yaw_deg) -{ - g_target_yaw = yaw_deg; -} +/** + * @file control_task.c + * @brief 二轴云台增稳控制任务（速度控制模式） + * @note 目标：保持 Roll=0, Pitch=0，Yaw 用于方向控制 + * + * 控制架构（速度控制）： + * IMU 角度 → PID 控制器 → 角速度指令 → 电机速度闭环 → 抵消基座倾斜 + * + * 数据来源： + * 通过 FreeRTOS Message Queue 从 sensor_task 接收 imu_data_t + */ + +#include "app.h" +#include "cmsis_os2.h" +#include "mf4010v2.h" +#include "pid.h" +#include +#include +#include "lowpass.h" +/* ============================================================================ + * 云台配置 + * ============================================================================ */ + +/* 目标角度：保持水平 */ +static float g_target_roll = 0.0f; /* Roll 目标 (deg) */ +static float g_target_pitch = 0.0f; /* Pitch 目标 (deg) */ +static float g_target_yaw = 0.0f; /* Yaw 目标 (deg) */ + +/* 电机实例 */ +static mf4010v2_t motor_yaw; /* Yaw 轴电机（外框，补偿 Roll） */ +static mf4010v2_t motor_pitch; /* Pitch 轴电机（内框，补偿 Pitch） */ + +/* PID 控制器 */ +static pid_t g_roll_pid; /* Roll 轴 PID */ +static pid_t g_pitch_pid; /* Pitch 轴 PID */ + +static lowpass_t g_roll_lowpass; +static lowpass_t g_pitch_lowpass; + +/* 电机使能状态 — 由 callback_task 通过 UART 命令控制 */ +static bool g_motor_enabled[2] = {false, false}; +/* ============================================================================ + * 速度控制参数 + * ============================================================================ */ + +/* + * PID 输出为速度指令 (0.01 DPS/LSB) + * + * 电机 MF4010V2 速度范围：±204800 (即 ±2048 DPS) + * 稳像场景使用较低速度：建议 ±36000 (即 ±360 DPS) 作为输出上限 + */ +#define SPEED_OUTPUT_MAX 36000.0f /* 最大角速度 360 DPS (0.01 DPS) */ + +/* + * PID 参数说明（速度控制模式）: + * + * Kp: 角度误差 → 速度指令的比例系数 (0.01 DPS / deg) + * 例如 Kp=200, 则 1° 误差 → 200 DPS 的补偿速度 + * + * Ki: 消除稳态误差的积分系数 + * 例如 Ki=5, 则 1° 持续误差每秒累积 5 DPS + * + * Kd: 抑制振荡的微分系数 + * 例如 Kd=50, 则角速度变化率反馈阻尼 + */ +#define ROLL_KP 200.0f +#define ROLL_KI 5.0f +#define ROLL_KD 50.0f +#define ROLL_ALPHA 0.7f + +#define PITCH_KP 200.0f +#define PITCH_KI 5.0f +#define PITCH_KD 50.0f + +#define PITCH_ALPHA 0.7f +/* ============================================================================ + * 控制任务（速度控制模式） + * ============================================================================ */ + +void control_task(void) +{ + imu_data_t imu; + osStatus_t qstat; + uint32_t last_tick = osKernelGetTickCount(); + + log_printf("[control_task] starting stabilization control (speed mode)\n"); + + /* 1. 初始化 PID 控制器 */ + pid_init(&g_roll_pid); + g_roll_pid.Kp = ROLL_KP; + g_roll_pid.Ki = ROLL_KI; + g_roll_pid.Kd = ROLL_KD; + g_roll_pid.output_max = SPEED_OUTPUT_MAX; + g_roll_pid.integral_max = SPEED_OUTPUT_MAX; + + pid_init(&g_pitch_pid); + g_pitch_pid.Kp = PITCH_KP; + g_pitch_pid.Ki = PITCH_KI; + g_pitch_pid.Kd = PITCH_KD; + g_pitch_pid.output_max = SPEED_OUTPUT_MAX; + g_pitch_pid.integral_max = SPEED_OUTPUT_MAX; + + lowpass_init(&g_roll_lowpass, ROLL_ALPHA); + lowpass_init(&g_pitch_lowpass, PITCH_ALPHA); + + log_printf("[control_task] PID: Roll(Kp=%.1f Ki=%.1f Kd=%.1f) Pitch(Kp=%.1f Ki=%.1f Kd=%.1f)\n", + ROLL_KP, ROLL_KI, ROLL_KD, PITCH_KP, PITCH_KI, PITCH_KD); + + /* 2. 初始化电机 */ + mf4010v2_init(&motor_yaw, 0x141, 1); /* Yaw 电机 ID: 0x141, CAN2 */ + mf4010v2_init(&motor_pitch, 0x142, 1); /* Pitch 电机 ID: 0x142, CAN2 */ + + /* 3. 电机关机状态启动 */ + mf4010v2_close(&motor_yaw); + mf4010v2_close(&motor_pitch); + + log_printf("[control_task] motor yaw is fault: %d\n", mf4010v2_is_fault(&motor_yaw)); + log_printf("[control_task] motor pitch is fault: %d\n", mf4010v2_is_fault(&motor_pitch)); + + /* 等待 IMU 稳定 */ + osDelay(pdMS_TO_TICKS(5000)); + + /* 4. 使能电机（零速启动） */ + mf4010v2_set_speed(&motor_yaw, 0); + mf4010v2_set_speed(&motor_pitch, 0); + mf4010v2_run(&motor_yaw); + mf4010v2_run(&motor_pitch); + + log_printf("[control_task] stabilization enabled (speed mode)\n"); + + /* ======================================================================== + * 主控制循环 (100Hz, osDelayUntil 精确周期性调度) + * + * 速度控制策略： + * 1. 从队列读取最新 IMU 角度 (Roll, Pitch) + * 2. PID 计算角速度补偿量 + * 3. 速度指令发送到电机（电机内部完成速度闭环） + * ======================================================================== */ + while (1) { + last_tick += pdMS_TO_TICKS(20); /* 100Hz 周期 */ + + /* 从 sensor_task 获取最新 IMU 数据（不等待，由定时保证节奏） */ + qstat = osMessageQueueGet(g_imu_queue, &imu, NULL, 0); + if (qstat == osOK) { + imu.angle[0] = lowpass_update(&g_roll_lowpass, imu.angle[0]); + imu.angle[1] = lowpass_update(&g_pitch_lowpass, imu.angle[1]); + + /* ---- Roll 轴速度控制 ---- */ + float roll_speed = pid_compute(&g_roll_pid, + g_target_roll, + imu.angle[0]); /* Roll */ + int32_t yaw_speed_cmd = (int32_t)roll_speed; + + /* ---- Pitch 轴速度控制 ---- */ + float pitch_speed = pid_compute(&g_pitch_pid, + g_target_pitch, + imu.angle[1]); /* Pitch */ + int32_t pitch_speed_cmd = (int32_t)pitch_speed; + yaw_speed_cmd = yaw_speed_cmd > 36000 ? 36000 : (yaw_speed_cmd < -36000 ? -36000 : yaw_speed_cmd); + pitch_speed_cmd = pitch_speed_cmd > 36000 ? 36000 : (pitch_speed_cmd < -36000 ? -36000 : pitch_speed_cmd); + + if (g_motor_enabled[GIMBAL_MOTOR_YAW]) { + mf4010v2_set_speed(&motor_yaw, yaw_speed_cmd); + } else { + mf4010v2_close(&motor_yaw); + } + if (g_motor_enabled[GIMBAL_MOTOR_PITCH]) { + mf4010v2_set_speed(&motor_pitch, pitch_speed_cmd); + } else { + mf4010v2_close(&motor_pitch); + } + } + + if (osDelayUntil(last_tick) != osOK) { + last_tick = osKernelGetTickCount(); + } + } +} + +/* ============================================================================ + * 外部 API + * ============================================================================ */ + +/* 设置 Yaw 目标方向 */ +void gimbal_set_yaw_target(float yaw_deg) +{ + g_target_yaw = yaw_deg; +} + +/* 重置 PID 积分（用于模式切换或异常恢复） */ +void gimbal_reset_pid(void) +{ + pid_reset(&g_roll_pid); + pid_reset(&g_pitch_pid); +} + +/* 使能电机 — 零速启动，重置 PID */ +void gimbal_motor_enable(void) +{ + gimbal_motor_enable_id(GIMBAL_MOTOR_YAW); + gimbal_motor_enable_id(GIMBAL_MOTOR_PITCH); +} + +/* 关闭电机 — 进入惰行（coast）状态 */ +void gimbal_motor_disable(void) +{ + gimbal_motor_disable_id(GIMBAL_MOTOR_YAW); + gimbal_motor_disable_id(GIMBAL_MOTOR_PITCH); +} + +/* 使能单个电机 */ +void gimbal_motor_enable_id(int id) +{ + g_motor_enabled[id] = true; + mf4010v2_t *motor = (id == GIMBAL_MOTOR_YAW) ? &motor_yaw : &motor_pitch; + pid_t *pid = (id == GIMBAL_MOTOR_YAW) ? &g_roll_pid : &g_pitch_pid; + mf4010v2_set_speed(motor, 0); + mf4010v2_run(motor); + pid_reset(pid); +} + +/* 关闭单个电机 */ +void gimbal_motor_disable_id(int id) +{ + g_motor_enabled[id] = false; + mf4010v2_t *motor = (id == GIMBAL_MOTOR_YAW) ? &motor_yaw : &motor_pitch; + mf4010v2_close(motor); +} + +/* 查询电机是否使能（任意一个使能即返回真） */ +int gimbal_is_motor_enabled(void) +{ + return (int)(g_motor_enabled[GIMBAL_MOTOR_YAW] || g_motor_enabled[GIMBAL_MOTOR_PITCH]); +} + +/* 调参接口 — 仅可在电机 disabled 后调用 */ +int gimbal_set_pid(const char *axis, const char *param, float value) +{ + pid_t *pid; + + if (strcmp(axis, "roll") == 0) { + pid = &g_roll_pid; + } else if (strcmp(axis, "pitch") == 0) { + pid = &g_pitch_pid; + } else { + return -1; /* 未知轴 */ + } + + if (strcmp(param, "kp") == 0) { + pid->Kp = value; + } else if (strcmp(param, "ki") == 0) { + pid->Ki = value; + } else if (strcmp(param, "kd") == 0) { + pid->Kd = value; + } else { + return -1; /* 未知参数 */ + } + + return 0; +} + +/* 获取 PID 系数（忽略 axis 大小写） */ +int gimbal_get_pid(const char *axis, float *kp, float *ki, float *kd) +{ + pid_t *pid; + + if (strcmp(axis, "roll") == 0) { + pid = &g_roll_pid; + } else if (strcmp(axis, "pitch") == 0) { + pid = &g_pitch_pid; + } else { + return -1; + } + + *kp = pid->Kp; + *ki = pid->Ki; + *kd = pid->Kd; + return 0; +} diff --git a/app/sensor_task.c b/app/sensor_task.c index bc97e23..6c99ca2 100644 --- a/app/sensor_task.c +++ b/app/sensor_task.c @@ -1,7 +1,20 @@ +#include "REG.h" #include "app.h" #include "cmsis_os2.h" #include "soft_i2c.h" +#include "i2c_bus.h" +#include "wit_c_sdk.h" +#include + +#define USE_SOFT_IIC 1 +#define USE_IIC 1 +#if USE_IIC +#if USE_SOFT_IIC static soft_i2c_t si2c; +#else +static i2c_t i2c; +#endif +#endif #define ACC_UPDATE 0x01 #define GYRO_UPDATE 0x02 @@ -9,12 +22,13 @@ static soft_i2c_t si2c; #define MAG_UPDATE 0x08 #define READ_UPDATE 0x80 static volatile char s_cDataUpdate = 0, s_cCmd = 0xff; -static void CmdProcess(void); -static void AutoScanSensor(void); +static bool g_sensor_enabled = true; /* 由 callback_task 通过 UART 命令控制 */ static void CopeSensorData(uint32_t uiReg, uint32_t uiRegNum); static void Delayms(uint16_t ucMs); +#if USE_IIC static int32_t IICreadBytes(uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uint32_t uiLen) { +#if USE_SOFT_IIC soft_i2c_start_frame(&si2c); soft_i2c_send_frame(&si2c, ucAddr); if(soft_i2c_wait_ack_frame(&si2c) != 0)return 0; @@ -30,9 +44,15 @@ static int32_t IICreadBytes(uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uin } soft_i2c_stop_frame(&si2c); return 1; +#else + return i2c_mem_read(&i2c, (uint16_t)ucAddr, ucReg, 1, p_ucVal, uiLen, 100) == 0 ? 1 : 0; +#endif } +#endif +#if USE_IIC static int32_t IICwriteBytes (uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uint32_t uiLen) { +#if USE_SOFT_IIC soft_i2c_start_frame(&si2c); soft_i2c_send_frame(&si2c, ucAddr); if(soft_i2c_wait_ack_frame(&si2c) != 0)return 0; @@ -40,127 +60,141 @@ static int32_t IICwriteBytes (uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, u if(soft_i2c_wait_ack_frame(&si2c) != 0)return 0; for(uint32_t i = 0; i < uiLen; i++) { - soft_i2c_send_frame(&si2c, *p_ucVal++); + soft_i2c_send_frame(&si2c, *p_ucVal++); if(soft_i2c_wait_ack_frame(&si2c) != 0)return 0; } soft_i2c_stop_frame(&si2c); return 1; +#else + return i2c_mem_write(&i2c, (uint16_t)ucAddr, ucReg, 1, p_ucVal, uiLen, 100) == 0 ? 1 : 0; +#endif } +#endif +static void print_float(const char* str, float v1, float v2, float v3) { + char v1_sign = v1 > 0 ? '+' : '-'; + char v2_sign = v2 > 0 ? '+' : '-'; + char v3_sign = v3 > 0 ? '+' : '-'; + v1 = v1 > 0 ? v1 : -v1; + v2 = v2 > 0 ? v2 : -v2; + v3 = v3 > 0 ? v3 : -v3; + int i1_x = (int)(v1 * 1000) / 1000; + int i1_y = (int)(v1 * 1000) % 1000; + int i2_x = (int)(v2 * 1000) / 1000; + int i2_y = (int)(v2 * 1000) % 1000; + int i3_x = (int)(v3 * 1000) / 1000; + int i3_y = (int)(v3 * 1000) % 1000; + log_printf("[sensor_task] %s: %c%03d.%03d, %c%03d.%03d, %c%03d.%03d\r\n", str, v1_sign, i1_x, i1_y, v2_sign, i2_x, i2_y, v3_sign, i3_x, i3_y); +} void sensor_task(void) { - float fAcc[3], fGyro[3], fAngle[3]; int i; int ret; + imu_data_t imu; +#if USE_IIC +#if USE_SOFT_IIC ret = soft_i2c_init(&si2c, 0); - printf("[sensor] soft_i2c_init = %d\r\n", ret); +#else + ret = i2c_init(&i2c, 1); +#endif +#endif + log_printf("[sensor_task] i2c init = %d\r\n", ret); +#if USE_IIC WitInit(WIT_PROTOCOL_I2C, 0x50); WitI2cFuncRegister(IICwriteBytes, IICreadBytes); +#else + WitInit(WIT_PROTOCOL_NORMAL, 0x50); + WitSerialWriteRegister(); +#endif WitRegisterCallBack(CopeSensorData); WitDelayMsRegister(Delayms); - AutoScanSensor(); + + uint32_t last_tick = osKernelGetTickCount(); + while (1) { - WitReadReg(AX, 12); - Delayms(5); - CmdProcess(); - if(s_cDataUpdate) + if(g_sensor_enabled) { + WitReadReg(AX, 13); + } + last_tick += pdMS_TO_TICKS(2); /* 500Hz 轮询 */ + if(g_sensor_enabled && s_cDataUpdate) { for(i = 0; i < 3; i++) { - fAcc[i] = sReg[AX+i] * 0.0054931640625f; - fGyro[i] = sReg[GX+i] * 0.0054931640625f; - fAngle[i] = sReg[Roll+i] * 0.0054931640625f; - } - if(s_cDataUpdate & ACC_UPDATE) - { - printf("raw:AX=%d AY=%d AZ=%d GX=%d GY=%d GZ=%d\r\n", - sReg[AX], sReg[AY], sReg[AZ], - sReg[GX], sReg[GY], sReg[GZ]); - printf("acc:%.3f %.3f %.3f\r\n", fAcc[0], fAcc[1], fAcc[2]); - s_cDataUpdate &= ~ACC_UPDATE; - } - if(s_cDataUpdate & GYRO_UPDATE) - { - printf("gyro:%.3f %.3f %.3f\r\n", fGyro[0], fGyro[1], fGyro[2]); - s_cDataUpdate &= ~GYRO_UPDATE; + imu.acc[i] = sReg[AX+i] * 0.0054931640625f; + imu.gyro[i] = sReg[GX+i] * 0.0054931640625f; + imu.angle[i] = sReg[Roll+i] * 0.0054931640625f; } + int16_t temp = sReg[TEMP]; + // if(s_cDataUpdate & ACC_UPDATE) + // { + // print_float("acc", imu.acc[0], imu.acc[1], imu.acc[2]); + // s_cDataUpdate &= ~ACC_UPDATE; + // } + // if(s_cDataUpdate & GYRO_UPDATE) + // { + // print_float("gyro", imu.gyro[0], imu.gyro[1], imu.gyro[2]); + // s_cDataUpdate &= ~GYRO_UPDATE; + // } if(s_cDataUpdate & ANGLE_UPDATE) { - printf("angle:%.3f %.3f %.3f\r\n", fAngle[0], fAngle[1], fAngle[2]); + print_float("angle", imu.angle[0], imu.angle[1], imu.angle[2]); s_cDataUpdate &= ~ANGLE_UPDATE; } - if(s_cDataUpdate & MAG_UPDATE) + if(s_cDataUpdate & READ_UPDATE) { - printf("mag:%d %d %d\r\n", sReg[HX], sReg[HY], sReg[HZ]); - s_cDataUpdate &= ~MAG_UPDATE; + char sign = temp > 0 ? '+' : '-'; + if (temp < 0) { + temp = -temp; + } + log_printf("[sensor_task] temp data: %c%3d.%02d\n", sign, temp/100, temp%100); + s_cDataUpdate &= ~READ_UPDATE; + } + /* 将 IMU 数据推送到 control_task */ + if (g_imu_queue != NULL) { + osMessageQueuePut(g_imu_queue, &imu, 0, 20); } } + if (osDelayUntil(last_tick) != osOK) { + last_tick = osKernelGetTickCount(); + } } } +/* ============================================================================ + * Sensor task API — 由 callback_task 通过 UART 命令调用 + * ============================================================================ */ -void CopeCmdData(unsigned char ucData) +void sensor_enable(void) { - static unsigned char s_ucData[50], s_ucRxCnt = 0; - - s_ucData[s_ucRxCnt++] = ucData; - if(s_ucRxCnt<3)return; //Less than three data returned - if(s_ucRxCnt >= 50) s_ucRxCnt = 0; - if(s_ucRxCnt >= 3) - { - if((s_ucData[1] == '\r') && (s_ucData[2] == '\n')) - { - s_cCmd = s_ucData[0]; - memset(s_ucData,0,50);// - s_ucRxCnt = 0; - } - else - { - s_ucData[0] = s_ucData[1]; - s_ucData[1] = s_ucData[2]; - s_ucRxCnt = 2; - - } - } - + g_sensor_enabled = true; + log_printf("[sensor_task] enabled\r\n"); } -static void CmdProcess(void) +void sensor_disable(void) { - switch(s_cCmd) - { - case 'a': - if(WitStartAccCali() != WIT_HAL_OK) - printf("\r\nSet AccCali Error\r\n"); - break; - case 'm': - if(WitStartMagCali() != WIT_HAL_OK) - printf("\r\nStart MagCali Error\r\n"); - break; - case 'e': - if(WitStopMagCali() != WIT_HAL_OK) - printf("\r\nEnd MagCali Error\r\n"); - break; - case 'u': - if(WitSetBandwidth(BANDWIDTH_5HZ) != WIT_HAL_OK) - printf("\r\nSet Bandwidth Error\r\n"); - break; - case 'U': - if(WitSetBandwidth(BANDWIDTH_256HZ) != WIT_HAL_OK) - printf("\r\nSet Bandwidth Error\r\n"); - break; - case 'B': - if(WitSetUartBaud(WIT_BAUD_115200) != WIT_HAL_OK) - printf("\r\nSet Baud Error\r\n"); - break; - case 'b': - if(WitSetUartBaud(WIT_BAUD_9600) != WIT_HAL_OK) - printf("\r\nSet Baud Error\r\n"); - break; - default : return ; - } - s_cCmd = 0xff; + g_sensor_enabled = false; + log_printf("[sensor_task] disabled\r\n"); +} + +void sensor_acc_cali_enable(void) +{ + WitStartAccCali(); +} + +void sensor_acc_cali_disable(void) +{ + WitStopAccCali(); +} + +void sensor_mag_cali_enable(void) +{ + WitStartMagCali(); +} + +void sensor_mag_cali_disable(void) +{ + WitStopMagCali(); } static void CopeSensorData(uint32_t uiReg, uint32_t uiRegNum) @@ -172,19 +206,19 @@ static void CopeSensorData(uint32_t uiReg, uint32_t uiRegNum) { // case AX: // case AY: - case AZ: - s_cDataUpdate |= ACC_UPDATE; - break; + // case AZ: + // s_cDataUpdate |= ACC_UPDATE; + // break; // case GX: // case GY: - case GZ: - s_cDataUpdate |= GYRO_UPDATE; - break; + // case GZ: + // s_cDataUpdate |= GYRO_UPDATE; + // break; // case HX: // case HY: - case HZ: - s_cDataUpdate |= MAG_UPDATE; - break; + // case HZ: + // s_cDataUpdate |= MAG_UPDATE; + // break; // case Roll: // case Pitch: case Yaw: @@ -202,28 +236,3 @@ static void Delayms(uint16_t ucMs) { osDelay(pdMS_TO_TICKS(ucMs)); } - -static void AutoScanSensor(void) -{ - int i, iRetry; - - for(i = 0; i < 0x7F; i++) - { - WitInit(WIT_PROTOCOL_I2C, i); - iRetry = 2; - do - { - s_cDataUpdate = 0; - WitReadReg(AX, 3); - Delayms(5); - if(s_cDataUpdate != 0) - { - printf("find %02X addr sensor\r\n", i); - return ; - } - iRetry--; - }while(iRetry); - } - printf("can not find sensor\r\n"); - printf("please check your connection\r\n"); -} \ No newline at end of file diff --git a/bsp/stm32f4/soft_i2c/bsp_soft_i2c.c b/bsp/stm32f4/soft_i2c/bsp_soft_i2c.c index b2dccce..efe4183 100644 --- a/bsp/stm32f4/soft_i2c/bsp_soft_i2c.c +++ b/bsp/stm32f4/soft_i2c/bsp_soft_i2c.c @@ -4,15 +4,15 @@ static bsp_soft_i2c_t soft_i2cs[] = { { .scl_port = GPIOF, - .scl_pin = SCL_Pin, + .scl_pin = SCL_Pin, // GPIO_PIN_14 .sda_port = GPIOF, - .sda_pin = SDA_Pin, + .sda_pin = SDA_Pin, // GPIO_PIN_12 } }; bsp_soft_i2c_t* bsp_soft_i2c_get_handle(int ch) { if (ch < 0 || ch >= (int)(sizeof(soft_i2cs) / sizeof(bsp_soft_i2c_t))) { return NULL; - } + } return &soft_i2cs[ch]; } \ No newline at end of file diff --git a/bsp/stm32f4/uart/bsp_uart.c b/bsp/stm32f4/uart/bsp_uart.c new file mode 100644 index 0000000..ceafadf --- /dev/null +++ b/bsp/stm32f4/uart/bsp_uart.c @@ -0,0 +1,16 @@ +#include "bsp_uart.h" +#include "usart.h" + +extern UART_HandleTypeDef huart1; + +static bsp_uart_t uarts[] = { + {&huart1}, /* UART1: PA9(TX), PA10(RX), 115200-8N1 */ + {&huart6} +}; + +bsp_uart_t *bsp_uart_get_handle(int ch) +{ + if (ch < 0 || ch >= (int)(sizeof(uarts) / sizeof(uarts[0]))) + return 0; + return &uarts[ch]; +} diff --git a/bsp/stm32f4/uart/bsp_uart.h b/bsp/stm32f4/uart/bsp_uart.h new file mode 100644 index 0000000..5b45de6 --- /dev/null +++ b/bsp/stm32f4/uart/bsp_uart.h @@ -0,0 +1,12 @@ +#ifndef BSP_UART_H +#define BSP_UART_H + +#include "stm32f4xx_hal.h" + +typedef struct { + UART_HandleTypeDef *huart; +} bsp_uart_t; + +bsp_uart_t *bsp_uart_get_handle(int ch); + +#endif diff --git a/docs/adr/adr-uart-layers.md b/docs/adr/adr-uart-layers.md new file mode 100644 index 0000000..b0e3def --- /dev/null +++ b/docs/adr/adr-uart-layers.md @@ -0,0 +1,45 @@ +# ADR-005: UART 分层架构设计 + +## Status +Accepted + +## Context +项目需要统一的 UART 抽象层，支持打印调试、传感器通信和对外数据交互。目前 USART1 已由 CubeMX 配置 +（115200-8N1，PA9/PA10），但缺少分层封装，应用代码直接调用 HAL_UART_Transmit 会破坏 6 层架构约束。 + +需求： +- 支持多种传输模式：阻塞（polling）、中断（IT）、DMA +- 通过 CAN/LED 相同的 Interface → HAL → BSP → Module 四层结构集成 +- 不干扰 CubeMX 生成的 MX_USART1_UART_Init() 初始化流程 +- 为未来多 UART 实例（如 USART2/USART3）预留扩展 + +## Decision +采用与 CAN/LED/I2C 一致的 ops 回调注册模式实现 UART 分层架构： + +``` +Module (uart_t) → Interface (uart_if) → HAL (hal_uart) → BSP (bsp_uart) +``` + +操作集合覆盖： +- 基础：init, deinit +- 阻塞：send, receive（含 timeout） +- 中断：send_it, receive_it +- DMA：send_dma, receive_dma +- 控制：abort +- 单字节：putc, getc + +## Alternatives Considered +- **直接封装 HAL_UART_Transmit/Receive** — 最简单，但破坏分层约束，且无法在 Module 层做状态管理。 +- **使用 retarget-stdio / _write 钩子 + syscall** — 只能覆盖 printf 场景，无法支持中断/DMA 接收。 +- **通过 DMA + 环形缓冲区的完整驱动** — 功能更强，但增加复杂性，适合后续独立优化。 + +## Consequences +- Positive: 与现有架构完全一致，贡献者无需学习新模式。 +- Positive: 11 个 ops 覆盖大部分 UART 使用场景，且接口可组合使用（如 init + send + receive + deinit）。 +- Positive: HAL 实现中通过 `h->gState` 检测避免重复初始化，兼容 CubeMX 的 MX_USART1_UART_Init()。 +- Negative: 当前只实现了 USART1 的 BSP，多实例需手工扩展 bsp_uart.c 数组。 +- Negative: 未实现 DMA 完成回调的异步通知，DMA 函数为"即发即忘"式调用。 + +## Trade-offs +异构传输模式支持 vs 实现复杂度：采用全 ops 声明但简化实现的策略。DMA/IT 函数透传 HAL 调用， +高层模块后续可围绕回调机制（HAL_UART_TxCpltCallback）构建响应式收发框架。 diff --git a/hal/stm32f4/can/hal_can.c b/hal/stm32f4/can/hal_can.c index ab0dd95..47504d6 100644 --- a/hal/stm32f4/can/hal_can.c +++ b/hal/stm32f4/can/hal_can.c @@ -1,120 +1,126 @@ -#include "can_if.h" -#include "bsp_can.h" -#include "can.h" -#include "stm32f4xx_hal_can.h" - - -static int hal_can_init(int ch) -{ - CAN_HandleTypeDef *hcan = bsp_can_get_handle(ch)->hcan; - if (!hcan) return -1; - - CAN_FilterTypeDef sFilterConfig; - sFilterConfig.FilterBank = 0; - sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; - sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT; - sFilterConfig.FilterIdHigh = 0x0000; - sFilterConfig.FilterIdLow = 0x0000; - sFilterConfig.FilterMaskIdHigh = 0x0000; - sFilterConfig.FilterMaskIdLow = 0x0000; - sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO0; - sFilterConfig.FilterActivation = CAN_FILTER_ENABLE; - - if (HAL_CAN_ConfigFilter(hcan, &sFilterConfig) != HAL_OK) - return -1; - - // 启动 CAN - if (HAL_CAN_Start(hcan) != HAL_OK) - return -1; - - return 0; -} - -static int hal_can_send(int ch, const can_message_t *msg) -{ - if (!msg || msg->length > 8) - return -1; - - CAN_HandleTypeDef *hcan = bsp_can_get_handle(ch)->hcan; - if (!hcan) return -1; - - CAN_TxHeaderTypeDef TxHeader; - TxHeader.StdId = msg->id & 0x7FF; - TxHeader.ExtId = 0; - TxHeader.IDE = CAN_ID_STD; - TxHeader.RTR = CAN_RTR_DATA; - TxHeader.DLC = msg->length; - TxHeader.TransmitGlobalTime = DISABLE; - - uint32_t TxMailbox; - if (HAL_CAN_AddTxMessage(hcan, &TxHeader, (uint8_t*)msg->data, &TxMailbox) != HAL_OK) - return -1; - - // 等待发送完成 - while (HAL_CAN_IsTxMessagePending(hcan, TxMailbox)) - ; - - if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) == 0) { - /* 发送完成，邮箱变为空闲 */ - } - return 0; -} - -static int hal_can_recv(int ch, can_message_t *msg) -{ - if (!msg) - return -1; - - CAN_HandleTypeDef *hcan = bsp_can_get_handle(ch)->hcan; - if (!hcan) return -1; - - // 检查是否有消息 - if (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO0) == 0) - return -1; - - CAN_RxHeaderTypeDef RxHeader; - if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader, msg->data) != HAL_OK) - return -1; - - msg->id = RxHeader.StdId; - msg->length = RxHeader.DLC; - - return 0; -} - -static int hal_can_filter_config(int ch, uint8_t filter_id, uint32_t id, uint32_t mask) -{ - CAN_HandleTypeDef *hcan = bsp_can_get_handle(ch)->hcan; - if (!hcan) return -1; - - CAN_FilterTypeDef sFilterConfig; - sFilterConfig.FilterBank = filter_id; - sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; - sFilterConfig.FilterScale = CAN_FILTERSCALE_16BIT; - sFilterConfig.FilterIdHigh = (id >> 16) & 0xFFFF; - sFilterConfig.FilterIdLow = id & 0xFFFF; - sFilterConfig.FilterMaskIdHigh = (mask >> 16) & 0xFFFF; - sFilterConfig.FilterMaskIdLow = mask & 0xFFFF; - sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO0; - sFilterConfig.FilterActivation = CAN_FILTER_ENABLE; - sFilterConfig.SlaveStartFilterBank = 14; - - if (HAL_CAN_ConfigFilter(hcan, &sFilterConfig) != HAL_OK) - return -1; - - return 0; -} - -// 注册接口 -static const can_ops_t ops = { - .init = hal_can_init, - .send = hal_can_send, - .recv = hal_can_recv, - .filter_config = hal_can_filter_config -}; - -void hal_can_init_all(void) -{ - can_register_ops(&ops); -} - +#include "can_if.h" +#include "bsp_can.h" +#include "can.h" +#include "stm32f4xx_hal_can.h" + +/* CAN 发送超时 (ms) */ +#define CAN_TX_TIMEOUT_MS 100U + + +static int hal_can_init(int ch) +{ + CAN_HandleTypeDef *hcan = bsp_can_get_handle(ch)->hcan; + if (!hcan) return -1; + + CAN_FilterTypeDef sFilterConfig; + sFilterConfig.FilterBank = 0; + sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; + sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT; + sFilterConfig.FilterIdHigh = 0x0000; + sFilterConfig.FilterIdLow = 0x0000; + sFilterConfig.FilterMaskIdHigh = 0x0000; + sFilterConfig.FilterMaskIdLow = 0x0000; + sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO0; + sFilterConfig.FilterActivation = CAN_FILTER_ENABLE; + + if (HAL_CAN_ConfigFilter(hcan, &sFilterConfig) != HAL_OK) + return -1; + + // 启动 CAN + if (HAL_CAN_Start(hcan) != HAL_OK) + return -1; + + return 0; +} + +static int hal_can_send(int ch, const can_message_t *msg) +{ + if (!msg || msg->length > 8) + return -1; + + CAN_HandleTypeDef *hcan = bsp_can_get_handle(ch)->hcan; + if (!hcan) return -1; + + CAN_TxHeaderTypeDef TxHeader; + TxHeader.StdId = msg->id & 0x7FF; + TxHeader.ExtId = 0; + TxHeader.IDE = CAN_ID_STD; + TxHeader.RTR = CAN_RTR_DATA; + TxHeader.DLC = msg->length; + TxHeader.TransmitGlobalTime = DISABLE; + + uint32_t TxMailbox; + if (HAL_CAN_AddTxMessage(hcan, &TxHeader, (uint8_t*)msg->data, &TxMailbox) != HAL_OK) + return -1; + + // 等待发送完成（带超时保护，防止 Bus-Off 死锁） + uint32_t tickstart = HAL_GetTick(); + while (HAL_CAN_IsTxMessagePending(hcan, TxMailbox)) { + if ((HAL_GetTick() - tickstart) > CAN_TX_TIMEOUT_MS) { + return -1; // 超时 — CAN 总线异常 + } + } + + if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) == 0) { + /* 发送完成，邮箱变为空闲 */ + } + return 0; +} + +static int hal_can_recv(int ch, can_message_t *msg) +{ + if (!msg) + return -1; + + CAN_HandleTypeDef *hcan = bsp_can_get_handle(ch)->hcan; + if (!hcan) return -1; + + // 检查是否有消息 + if (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO0) == 0) + return -1; + + CAN_RxHeaderTypeDef RxHeader; + if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader, msg->data) != HAL_OK) + return -1; + + msg->id = RxHeader.StdId; + msg->length = RxHeader.DLC; + + return 0; +} + +static int hal_can_filter_config(int ch, uint8_t filter_id, uint32_t id, uint32_t mask) +{ + CAN_HandleTypeDef *hcan = bsp_can_get_handle(ch)->hcan; + if (!hcan) return -1; + + CAN_FilterTypeDef sFilterConfig; + sFilterConfig.FilterBank = filter_id; + sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; + sFilterConfig.FilterScale = CAN_FILTERSCALE_16BIT; + sFilterConfig.FilterIdHigh = (id >> 16) & 0xFFFF; + sFilterConfig.FilterIdLow = id & 0xFFFF; + sFilterConfig.FilterMaskIdHigh = (mask >> 16) & 0xFFFF; + sFilterConfig.FilterMaskIdLow = mask & 0xFFFF; + sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO0; + sFilterConfig.FilterActivation = CAN_FILTER_ENABLE; + sFilterConfig.SlaveStartFilterBank = 14; + + if (HAL_CAN_ConfigFilter(hcan, &sFilterConfig) != HAL_OK) + return -1; + + return 0; +} + +// 注册接口 +static const can_ops_t ops = { + .init = hal_can_init, + .send = hal_can_send, + .recv = hal_can_recv, + .filter_config = hal_can_filter_config +}; + +void hal_can_init_all(void) +{ + can_register_ops(&ops); +} diff --git a/hal/stm32f4/soft_i2c/hal_soft_i2c.c b/hal/stm32f4/soft_i2c/hal_soft_i2c.c index 448c981..edc918d 100644 --- a/hal/stm32f4/soft_i2c/hal_soft_i2c.c +++ b/hal/stm32f4/soft_i2c/hal_soft_i2c.c @@ -764,25 +764,23 @@ static int hal_soft_i2c_wait_ack_frame(int ch) { if (!bus) return -1; dwt_init(); - sda_out(bus); - gpio_set(bus->sda_port, bus->sda_pin); /* drive SDA high via PP */ - sda_in(bus); /* switch to input for reading */ - gpio_set(bus->sda_port, bus->sda_pin); /* pre-set ODR=1, safe in input mode */ - delay_us(5); + sda_in(bus); /* switch to input first (high-Z + PUPDR pull-up) */ + gpio_set(bus->sda_port, bus->sda_pin); /* ODR=1, safe for future output mode */ + delay_us(1); + /* clock the 9th SCL pulse so slave can assert ACK */ + gpio_set(bus->scl_port, bus->scl_pin); /* SCL high */ + delay_us(3); /* tSU;DAT for slave */ uint32_t start = DWT->CYCCNT; - while (gpio_read(bus->sda_port, bus->sda_pin)) { + int ack = gpio_read(bus->sda_port, bus->sda_pin); + while (ack) { if ((DWT->CYCCNT - start) > (SystemCoreClock / 100)) { - sda_out(bus); - gpio_reset(bus->scl_port, bus->scl_pin); - gpio_set(bus->scl_port, bus->scl_pin); - delay_us(5); gpio_reset(bus->scl_port, bus->scl_pin); return -2; } + ack = gpio_read(bus->sda_port, bus->sda_pin); } - gpio_set(bus->scl_port, bus->scl_pin); - delay_us(5); - gpio_reset(bus->scl_port, bus->scl_pin); + delay_us(2); + gpio_reset(bus->scl_port, bus->scl_pin); /* SCL low; end of ACK bit */ return 0; } @@ -832,17 +830,15 @@ static int hal_soft_i2c_read_frame(int ch, int ack) { uint8_t ret = 0; if (!bus) return -1; - sda_out(bus); - gpio_set(bus->sda_port, bus->sda_pin); /* drive SDA high via PP */ sda_in(bus); - gpio_set(bus->sda_port, bus->sda_pin); /* pre-set ODR=1 */ + gpio_set(bus->sda_port, bus->sda_pin); /* ODR=1, safe for future output mode */ for (int i = 0; i < 8; i ++) { scl_low(bus); delay_us(5); scl_high(bus); + delay_us(10); /* tSU;DAT — let slave data settle */ ret <<= 1; if (sda_read(bus)) ret ++; - delay_us(5); } hal_soft_i2c_send_ack_frame(ch, ack); diff --git a/hal/stm32f4/uart/hal_uart.c b/hal/stm32f4/uart/hal_uart.c new file mode 100644 index 0000000..ec7a019 --- /dev/null +++ b/hal/stm32f4/uart/hal_uart.c @@ -0,0 +1,170 @@ +#include "hal_uart.h" +#include "uart_if.h" +#include "bsp_uart.h" + +static int hal_uart_init(int ch, const uart_config_t *cfg) +{ + bsp_uart_t *uart = bsp_uart_get_handle(ch); + if (!uart || !uart->huart || !cfg) + return -1; + + /* 仅在需要时重新配置；若已在 CubeMX 初始化后无需再调 HAL_UART_Init */ + UART_HandleTypeDef *h = uart->huart; + + /* 检查是否已经由 CubeMX 初始化过（Instance 非空即可） */ + if (h->gState != HAL_UART_STATE_RESET) + return 0; /* 已初始化，跳过 */ + + h->Instance = USART1; + h->Init.BaudRate = cfg->baud_rate; + h->Init.WordLength = cfg->word_length; + h->Init.StopBits = cfg->stop_bits; + h->Init.Parity = cfg->parity; + h->Init.Mode = UART_MODE_TX_RX; + h->Init.HwFlowCtl = UART_HWCONTROL_NONE; + h->Init.OverSampling = UART_OVERSAMPLING_16; + + if (HAL_UART_Init(h) != HAL_OK) + return -1; + + return 0; +} + +static int hal_uart_deinit(int ch) +{ + bsp_uart_t *uart = bsp_uart_get_handle(ch); + if (!uart || !uart->huart) + return -1; + + if (HAL_UART_DeInit(uart->huart) != HAL_OK) + return -1; + + return 0; +} + +static int hal_uart_send(int ch, const uint8_t *data, size_t size, uint32_t timeout) +{ + bsp_uart_t *uart = bsp_uart_get_handle(ch); + if (!uart || !uart->huart || !data) + return -1; + + if (HAL_UART_Transmit(uart->huart, (uint8_t *)data, size, timeout) != HAL_OK) + return -1; + + return 0; +} + +static int hal_uart_receive(int ch, uint8_t *data, size_t size, uint32_t timeout) +{ + bsp_uart_t *uart = bsp_uart_get_handle(ch); + if (!uart || !uart->huart || !data) + return -1; + + if (HAL_UART_Receive(uart->huart, data, size, timeout) != HAL_OK) + return -1; + + return 0; +} + +static int hal_uart_send_it(int ch, const uint8_t *data, size_t size) +{ + bsp_uart_t *uart = bsp_uart_get_handle(ch); + if (!uart || !uart->huart || !data) + return -1; + + if (HAL_UART_Transmit_IT(uart->huart, (uint8_t *)data, size) != HAL_OK) + return -1; + + return 0; +} + +static int hal_uart_receive_it(int ch, uint8_t *data, size_t size) +{ + bsp_uart_t *uart = bsp_uart_get_handle(ch); + if (!uart || !uart->huart || !data) + return -1; + + if (HAL_UART_Receive_IT(uart->huart, data, size) != HAL_OK) + return -1; + + return 0; +} + +static int hal_uart_send_dma(int ch, const uint8_t *data, size_t size) +{ + bsp_uart_t *uart = bsp_uart_get_handle(ch); + if (!uart || !uart->huart || !data) + return -1; + + if (HAL_UART_Transmit_DMA(uart->huart, (uint8_t *)data, size) != HAL_OK) + return -1; + + return 0; +} + +static int hal_uart_receive_dma(int ch, uint8_t *data, size_t size) +{ + bsp_uart_t *uart = bsp_uart_get_handle(ch); + if (!uart || !uart->huart || !data) + return -1; + + if (HAL_UART_Receive_DMA(uart->huart, data, size) != HAL_OK) + return -1; + + return 0; +} + +static int hal_uart_abort(int ch) +{ + bsp_uart_t *uart = bsp_uart_get_handle(ch); + if (!uart || !uart->huart) + return -1; + + if (HAL_UART_Abort(uart->huart) != HAL_OK) + return -1; + + return 0; +} + +static int hal_uart_putc(int ch, uint8_t c) +{ + bsp_uart_t *uart = bsp_uart_get_handle(ch); + if (!uart || !uart->huart) + return -1; + + if (HAL_UART_Transmit(uart->huart, &c, 1, HAL_MAX_DELAY) != HAL_OK) + return -1; + + return 0; +} + +static int hal_uart_getc(int ch, uint8_t *c) +{ + bsp_uart_t *uart = bsp_uart_get_handle(ch); + if (!uart || !uart->huart || !c) + return -1; + + if (HAL_UART_Receive(uart->huart, c, 1, HAL_MAX_DELAY) != HAL_OK) + return -1; + + return 0; +} + +static const uart_ops_t ops = { + .init = hal_uart_init, + .deinit = hal_uart_deinit, + .send = hal_uart_send, + .receive = hal_uart_receive, + .send_it = hal_uart_send_it, + .receive_it = hal_uart_receive_it, + .send_dma = hal_uart_send_dma, + .receive_dma = hal_uart_receive_dma, + .abort = hal_uart_abort, + .putc = hal_uart_putc, + .getc = hal_uart_getc, +}; + +void hal_uart_init_all(void) +{ + uart_register_ops(&ops); +} diff --git a/hal/stm32f4/uart/hal_uart.h b/hal/stm32f4/uart/hal_uart.h new file mode 100644 index 0000000..ae0d24b --- /dev/null +++ b/hal/stm32f4/uart/hal_uart.h @@ -0,0 +1,6 @@ +#ifndef HAL_UART_H +#define HAL_UART_H + +void hal_uart_init_all(void); + +#endif diff --git a/interfaces/uart/uart_if.c b/interfaces/uart/uart_if.c new file mode 100644 index 0000000..dd9a3de --- /dev/null +++ b/interfaces/uart/uart_if.c @@ -0,0 +1,85 @@ +#include "uart_if.h" + +static const uart_ops_t *g_uart_ops = 0; + +void uart_register_ops(const uart_ops_t *ops) +{ + g_uart_ops = ops; +} + +int uart_if_init(int ch, const uart_config_t *cfg) +{ + if (!g_uart_ops || !g_uart_ops->init) + return -1; + return g_uart_ops->init(ch, cfg); +} + +int uart_if_deinit(int ch) +{ + if (!g_uart_ops || !g_uart_ops->deinit) + return -1; + return g_uart_ops->deinit(ch); +} + +int uart_if_send(int ch, const uint8_t *data, size_t size, uint32_t timeout) +{ + if (!g_uart_ops || !g_uart_ops->send) + return -1; + return g_uart_ops->send(ch, data, size, timeout); +} + +int uart_if_receive(int ch, uint8_t *data, size_t size, uint32_t timeout) +{ + if (!g_uart_ops || !g_uart_ops->receive) + return -1; + return g_uart_ops->receive(ch, data, size, timeout); +} + +int uart_if_send_it(int ch, const uint8_t *data, size_t size) +{ + if (!g_uart_ops || !g_uart_ops->send_it) + return -1; + return g_uart_ops->send_it(ch, data, size); +} + +int uart_if_receive_it(int ch, uint8_t *data, size_t size) +{ + if (!g_uart_ops || !g_uart_ops->receive_it) + return -1; + return g_uart_ops->receive_it(ch, data, size); +} + +int uart_if_send_dma(int ch, const uint8_t *data, size_t size) +{ + if (!g_uart_ops || !g_uart_ops->send_dma) + return -1; + return g_uart_ops->send_dma(ch, data, size); +} + +int uart_if_receive_dma(int ch, uint8_t *data, size_t size) +{ + if (!g_uart_ops || !g_uart_ops->receive_dma) + return -1; + return g_uart_ops->receive_dma(ch, data, size); +} + +int uart_if_abort(int ch) +{ + if (!g_uart_ops || !g_uart_ops->abort) + return -1; + return g_uart_ops->abort(ch); +} + +int uart_if_putc(int ch, uint8_t c) +{ + if (!g_uart_ops || !g_uart_ops->putc) + return -1; + return g_uart_ops->putc(ch, c); +} + +int uart_if_getc(int ch, uint8_t *c) +{ + if (!g_uart_ops || !g_uart_ops->getc) + return -1; + return g_uart_ops->getc(ch, c); +} diff --git a/interfaces/uart/uart_if.h b/interfaces/uart/uart_if.h new file mode 100644 index 0000000..f57525e --- /dev/null +++ b/interfaces/uart/uart_if.h @@ -0,0 +1,46 @@ +#ifndef UART_IF_H +#define UART_IF_H + +#include +#include + +/* UART 配置参数 */ +typedef struct { + uint32_t baud_rate; + uint32_t word_length; /* UART_WORDLENGTH_8B / UART_WORDLENGTH_9B */ + uint32_t stop_bits; /* UART_STOPBITS_1 / UART_STOPBITS_2 */ + uint32_t parity; /* UART_PARITY_NONE / UART_PARITY_EVEN / UART_PARITY_ODD */ +} uart_config_t; + +/* 默认配置: 115200-8N1 */ +#define UART_CONFIG_DEFAULT { 115200, UART_WORDLENGTH_8B, UART_STOPBITS_1, UART_PARITY_NONE } + +typedef struct { + int (*init)(int ch, const uart_config_t *cfg); + int (*deinit)(int ch); + int (*send)(int ch, const uint8_t *data, size_t size, uint32_t timeout); + int (*receive)(int ch, uint8_t *data, size_t size, uint32_t timeout); + int (*send_it)(int ch, const uint8_t *data, size_t size); + int (*receive_it)(int ch, uint8_t *data, size_t size); + int (*send_dma)(int ch, const uint8_t *data, size_t size); + int (*receive_dma)(int ch, uint8_t *data, size_t size); + int (*abort)(int ch); + int (*putc)(int ch, uint8_t c); + int (*getc)(int ch, uint8_t *c); +} uart_ops_t; + +void uart_register_ops(const uart_ops_t *ops); + +int uart_if_init(int ch, const uart_config_t *cfg); +int uart_if_deinit(int ch); +int uart_if_send(int ch, const uint8_t *data, size_t size, uint32_t timeout); +int uart_if_receive(int ch, uint8_t *data, size_t size, uint32_t timeout); +int uart_if_send_it(int ch, const uint8_t *data, size_t size); +int uart_if_receive_it(int ch, uint8_t *data, size_t size); +int uart_if_send_dma(int ch, const uint8_t *data, size_t size); +int uart_if_receive_dma(int ch, uint8_t *data, size_t size); +int uart_if_abort(int ch); +int uart_if_putc(int ch, uint8_t c); +int uart_if_getc(int ch, uint8_t *c); + +#endif diff --git a/modules/bus/i2c/i2c_bus.c b/modules/bus/i2c/i2c_bus.c new file mode 100644 index 0000000..bbeddc2 --- /dev/null +++ b/modules/bus/i2c/i2c_bus.c @@ -0,0 +1,34 @@ +#include "i2c_bus.h" +#include "i2c_if.h" +#include + + +int i2c_init(i2c_t* obj, int ch) { + if (!obj || obj->initialized) { + return -1; + } + obj->ch = ch; + obj->initialized = true; + i2c_if_init(ch); + return 0; +} +int i2c_mem_write(i2c_t* obj, uint16_t dev_addr, uint16_t mem_addr, uint16_t mem_add_size, const uint8_t *data, uint16_t size, uint32_t timeout) { + if (!obj || obj->initialized == false) + return -1; + return i2c_if_mem_write(obj->ch, dev_addr, mem_addr, mem_add_size, data, size, timeout); +} +int i2c_mem_read(i2c_t* obj, uint16_t dev_addr, uint16_t mem_addr, uint16_t mem_add_size, uint8_t *data, uint16_t size, uint32_t timeout) { + if (!obj || obj->initialized == false) + return -1; + return i2c_if_mem_read(obj->ch, dev_addr, mem_addr, mem_add_size, data, size, timeout); +} +int i2c_write(i2c_t* obj, uint16_t dev_addr, const uint8_t *data, uint16_t size, uint32_t timeout) { + if (!obj || obj->initialized == false) + return -1; + return i2c_if_write(obj->ch, dev_addr, data, size, timeout); +} +int i2c_read(i2c_t* obj, uint16_t dev_addr, uint8_t *data, uint16_t size, uint32_t timeout) { + if (!obj || obj->initialized == false) + return -1; + return i2c_if_read(obj->ch, dev_addr, data, size, timeout); +} \ No newline at end of file diff --git a/modules/bus/i2c/i2c_bus.h b/modules/bus/i2c/i2c_bus.h new file mode 100644 index 0000000..702edb9 --- /dev/null +++ b/modules/bus/i2c/i2c_bus.h @@ -0,0 +1,17 @@ +#ifndef I2C_BUS_H +#define I2C_BUS_H + +#include +#include + +typedef struct { + int ch; + bool initialized; +} i2c_t; +int i2c_init(i2c_t* obj, int ch); +int i2c_mem_write(i2c_t* obj, uint16_t dev_addr, uint16_t mem_addr, uint16_t mem_add_size, const uint8_t *data, uint16_t size, uint32_t timeout); +int i2c_mem_read(i2c_t* obj, uint16_t dev_addr, uint16_t mem_addr, uint16_t mem_add_size, uint8_t *data, uint16_t size, uint32_t timeout); +int i2c_write(i2c_t* obj, uint16_t dev_addr, const uint8_t *data, uint16_t size, uint32_t timeout); +int i2c_read(i2c_t* obj, uint16_t dev_addr, uint8_t *data, uint16_t size, uint32_t timeout); + +#endif diff --git a/modules/bus/uart/uart.c b/modules/bus/uart/uart.c new file mode 100644 index 0000000..9449426 --- /dev/null +++ b/modules/bus/uart/uart.c @@ -0,0 +1,76 @@ +#include "uart.h" + +int uart_init(uart_t *uart, int ch, const uart_config_t *cfg) +{ + if (!uart || !cfg) + return -1; + + uart->ch = ch; + uart->config = *cfg; + + int ret = uart_if_init(ch, cfg); + if (ret == 0) + uart->initialized = 1; + + return ret; +} + +int uart_deinit(uart_t *uart) +{ + if (!uart || !uart->initialized) + return -1; + + int ret = uart_if_deinit(uart->ch); + if (ret == 0) + uart->initialized = 0; + + return ret; +} + +int uart_send(uart_t *uart, const uint8_t *data, size_t size, uint32_t timeout) +{ + if (!uart || !uart->initialized || !data) + return -1; + + return uart_if_send(uart->ch, data, size, timeout); +} + +int uart_receive(uart_t *uart, uint8_t *data, size_t size, uint32_t timeout) +{ + if (!uart || !uart->initialized || !data) + return -1; + + return uart_if_receive(uart->ch, data, size, timeout); +} + +int uart_send_it(uart_t *uart, const uint8_t *data, size_t size) +{ + if (!uart || !uart->initialized || !data) + return -1; + + return uart_if_send_it(uart->ch, data, size); +} + +int uart_receive_it(uart_t *uart, uint8_t *data, size_t size) +{ + if (!uart || !uart->initialized || !data) + return -1; + + return uart_if_receive_it(uart->ch, data, size); +} + +int uart_putc(uart_t *uart, uint8_t c) +{ + if (!uart || !uart->initialized) + return -1; + + return uart_if_putc(uart->ch, c); +} + +int uart_getc(uart_t *uart, uint8_t *c) +{ + if (!uart || !uart->initialized || !c) + return -1; + + return uart_if_getc(uart->ch, c); +} diff --git a/modules/bus/uart/uart.h b/modules/bus/uart/uart.h new file mode 100644 index 0000000..569c8b3 --- /dev/null +++ b/modules/bus/uart/uart.h @@ -0,0 +1,23 @@ +#ifndef UART_H +#define UART_H + +#include +#include +#include "uart_if.h" + +typedef struct { + int ch; + int initialized; + uart_config_t config; +} uart_t; + +int uart_init(uart_t *uart, int ch, const uart_config_t *cfg); +int uart_deinit(uart_t *uart); +int uart_send(uart_t *uart, const uint8_t *data, size_t size, uint32_t timeout); +int uart_receive(uart_t *uart, uint8_t *data, size_t size, uint32_t timeout); +int uart_send_it(uart_t *uart, const uint8_t *data, size_t size); +int uart_receive_it(uart_t *uart, uint8_t *data, size_t size); +int uart_putc(uart_t *uart, uint8_t c); +int uart_getc(uart_t *uart, uint8_t *c); + +#endif diff --git a/modules/cmd_parser/cmd_parser.c b/modules/cmd_parser/cmd_parser.c new file mode 100644 index 0000000..ac128a4 --- /dev/null +++ b/modules/cmd_parser/cmd_parser.c @@ -0,0 +1,51 @@ +#include "cmd_parser.h" +#include + +void cmd_parser_init(cmd_parser_t *parser) +{ + parser->count = 0; +} + +int cmd_parser_register(cmd_parser_t *parser, const char *name, cmd_handler_t handler) +{ + size_t i; + + if (parser->count >= CMD_PARSER_MAX_CMDS) return -1; + + i = strlen(name); + if (i >= CMD_PARSER_NAME_LEN) i = CMD_PARSER_NAME_LEN - 1; + memcpy(parser->entries[parser->count].name, name, i); + parser->entries[parser->count].name[i] = '\0'; + parser->entries[parser->count].handler = handler; + parser->count++; + return 0; +} + +int cmd_parser_dispatch(cmd_parser_t *parser, char *buf, void *ctx) +{ + char *argv[CMD_PARSER_MAX_ARGS]; + int argc = 0; + int i; + char *p = buf; + + while (*p) { + while (*p == ' ') p++; + if (*p == '\0') break; + + argv[argc++] = p; + if (argc >= CMD_PARSER_MAX_ARGS) break; + + while (*p && *p != ' ') p++; + if (*p) *p++ = '\0'; + } + + if (argc == 0) return -1; + + for (i = 0; i < parser->count; i++) { + if (strcmp(argv[0], parser->entries[i].name) == 0) { + return parser->entries[i].handler(ctx, argc, argv); + } + } + + return -1; +} diff --git a/modules/cmd_parser/cmd_parser.h b/modules/cmd_parser/cmd_parser.h new file mode 100644 index 0000000..c1abd8f --- /dev/null +++ b/modules/cmd_parser/cmd_parser.h @@ -0,0 +1,32 @@ +#ifndef CMD_PARSER_H +#define CMD_PARSER_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#define CMD_PARSER_MAX_CMDS 32 +#define CMD_PARSER_NAME_LEN 20 +#define CMD_PARSER_MAX_ARGS 16 + +typedef int (*cmd_handler_t)(void *ctx, int argc, char **argv); + +typedef struct { + struct { + char name[CMD_PARSER_NAME_LEN]; + cmd_handler_t handler; + } entries[CMD_PARSER_MAX_CMDS]; + int count; +} cmd_parser_t; + +void cmd_parser_init(cmd_parser_t *parser); +int cmd_parser_register(cmd_parser_t *parser, const char *name, cmd_handler_t handler); +int cmd_parser_dispatch(cmd_parser_t *parser, char *buf, void *ctx); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/modules/control/pid/pid.c b/modules/control/pid/pid.c index 366fff8..ad16455 100644 --- a/modules/control/pid/pid.c +++ b/modules/control/pid/pid.c @@ -1,55 +1,56 @@ #include "pid.h" -#include -void PIDController_Init(PIDController_t *pid) +void pid_init(pid_t *pid) { if (!pid) return; pid->Kp = 0.0f; pid->Ki = 0.0f; pid->Kd = 0.0f; pid->integral = 0.0f; - pid->prev_error = 0.0f; - pid->output_max = 204800.0f; // 电机最大速度 (0.01 DPS) + pid->prev_measurement = 0.0f; + pid->output_max = 0.0f; + pid->integral_max = 0.0f; } -float PIDController_Compute(PIDController_t *pid, float target, float actual) +float pid_compute(pid_t *pid, float target, float actual) { if (!pid) return 0.0f; float error = target - actual; - // 比例项 + /* 比例项 */ float p_term = pid->Kp * error; - // 积分项 (带抗饱和) + /* 微分项（测量值微分 — 避免目标突变微分冲击） */ + float d_term = pid->Kd * (-(actual - pid->prev_measurement)); + pid->prev_measurement = actual; + + /* 积分项（带抗饱和限幅） */ pid->integral += error; - if (pid->integral > pid->output_max) { - pid->integral = pid->output_max; - } else if (pid->integral < -pid->output_max) { - pid->integral = -pid->output_max; + if (pid->integral_max > 0.0f) { + if (pid->integral > pid->integral_max) + pid->integral = pid->integral_max; + else if (pid->integral < -pid->integral_max) + pid->integral = -pid->integral_max; } float i_term = pid->Ki * pid->integral; - // 微分项 - float derivative = error - pid->prev_error; - float d_term = pid->Kd * derivative; - pid->prev_error = error; - float output = p_term + i_term + d_term; - // 输出限幅 - if (output > pid->output_max) { - output = pid->output_max; - } else if (output < -pid->output_max) { - output = -pid->output_max; + /* 输出限幅（0=不限幅） */ + if (pid->output_max > 0.0f) { + if (output > pid->output_max) + output = pid->output_max; + else if (output < -pid->output_max) + output = -pid->output_max; } return output; } -void PIDController_Reset(PIDController_t *pid) +void pid_reset(pid_t *pid) { if (!pid) return; pid->integral = 0.0f; - pid->prev_error = 0.0f; + pid->prev_measurement = 0.0f; } diff --git a/modules/control/pid/pid.h b/modules/control/pid/pid.h index acc18d9..77fce60 100644 --- a/modules/control/pid/pid.h +++ b/modules/control/pid/pid.h @@ -1,22 +1,21 @@ -#ifndef __PID_H__ -#define __PID_H__ +#ifndef PID_H +#define PID_H #ifdef __cplusplus extern "C" { #endif -#include - typedef struct { float Kp, Ki, Kd; float integral; - float prev_error; - float output_max; -} PIDController_t; + float prev_measurement; /* 上一拍测量值（微分用） */ + float output_max; /* 输出限幅（0=不限幅） */ + float integral_max; /* 积分限幅，抗饱和（0=不限幅） */ +} pid_t; -void PIDController_Init(PIDController_t *pid); -float PIDController_Compute(PIDController_t *pid, float target, float actual); -void PIDController_Reset(PIDController_t *pid); +void pid_init(pid_t *pid); +float pid_compute(pid_t *pid, float target, float actual); +void pid_reset(pid_t *pid); #ifdef __cplusplus } diff --git a/modules/device/motor/mf4010v2.c b/modules/device/motor/mf4010v2.c index 8f23665..efaf4b6 100644 --- a/modules/device/motor/mf4010v2.c +++ b/modules/device/motor/mf4010v2.c @@ -1,320 +1,339 @@ -/** - * @file mf4010v2.c - * @brief MF4010V2 无刷电机驱动程序 - * @note 改进版本： - * - 添加错误处理和返回值 - * - 添加 CAN 超时机制 - * - 使用安全的内联函数替代危险宏 - * - 使用 HAL 层解耦硬件依赖 - */ - -#include "mf4010v2.h" -#include "can_if.h" -#include -#include -#include - -#define WRITE_FLAG(data, flag) do {(data) |= flag; } while(0) -#define ERASE_FLAG(data, flag) do {(data) &= ~flag; } while(0) -/** - * @brief 初始化电机结构体 - */ -int mf4010v2_init(mf4010v2_t* motor, uint16_t id, int can_ch) -{ - if(motor == NULL) { - return MF4010_ERR_INVALID_PARAM; - } - - if(id > 0x7FF) { - printf("Error: Invalid CAN ID 0x%03X (must be 0x000-0x7FF)\r\n", id); - return MF4010_ERR_INVALID_ID; - } - - motor->can_ch = can_ch; - motor->id = id; - motor->target_angle = 0; - motor->current_angle = 0; - memset(motor->last_response, 0, sizeof(motor->last_response)); - can_if_init(motor->can_ch); - motor->last_comm_time = 0; - motor->error_count = 0; - motor->success_count = 0; - motor->flags = MF4010_FLAG_ENABLED; - return MF4010_OK; -} - -/** - * @brief 发送命令并接收响应（带超时） - */ -int mf4010v2_send_command(mf4010v2_t* motor, const uint8_t command[8]) -{ - can_message_t msg; - msg.id = motor->id; - memcpy(msg.data, command, 8); - msg.length = 8; - int ret = can_if_send(motor->can_ch, &msg); - if (ret != 0) { - motor->error_count++; - printf("Error: Failed to send CAN message (ID 0x%03X)\r\n", motor->id); - return MF4010_ERR_CAN_SEND; - } - can_if_recv(motor->can_ch, &msg); - if (msg.id != motor->id) { - motor->error_count++; - return MF4010_ERR_CAN_RECV; - } - memcpy(motor->last_response, msg.data, 8); - motor->success_count++; - return MF4010_OK; -} - -/** - * @brief 设置电机目标角度 (位置闭环) - * @param motor 电机结构体 - * @param angle_0_01deg 目标角度，单位 0.01 度 - */ -int mf4010v2_set_angle(mf4010v2_t* motor, int32_t angle_0_01deg) -{ - if (!motor) return MF4010_ERR_INVALID_PARAM; - - uint8_t cmd[8]; - // 使用增量位置控制 (A7 命令) - // 计算角度增量 - int32_t delta = angle_0_01deg - motor->current_angle; - - // 限制单次增量在合理范围内 - if (delta > 18000) delta = 18000; // 最多 +180 度 - if (delta < -18000) delta = -18000; // 最多 -180 度 - - cmd_incr_pos(cmd, delta); - motor->target_angle = angle_0_01deg; - - return mf4010v2_send_command(motor, cmd); -} - -/** - * @brief 设置电机速度 (速度闭环) - * @param motor 电机结构体 - * @param speed_0_01dps 目标速度，单位 0.01 DPS - */ -int mf4010v2_set_speed(mf4010v2_t* motor, int32_t speed_0_01dps) -{ - if (!motor) return MF4010_ERR_INVALID_PARAM; - - uint8_t cmd[8]; - cmd_speed_control(cmd, speed_0_01dps); - return mf4010v2_send_command(motor, cmd); -} - -/** - * @brief 启动电机 - */ -void mf4010v2_run(mf4010v2_t* motor) -{ - if (!motor) return; - uint8_t cmd[8] = COMMAND_MOTOR_RUNNING; - mf4010v2_send_command(motor, cmd); - motor->flags |= MF4010_FLAG_RUNNING; -} - -/** - * @brief 停止电机 - */ -void mf4010v2_stop(mf4010v2_t* motor) -{ - if (!motor) return; - uint8_t cmd[8] = COMMAND_MOTOR_STOP; - mf4010v2_send_command(motor, cmd); - motor->flags &= ~MF4010_FLAG_RUNNING; -} - -void mf4010v2_close(mf4010v2_t* motor) { - if (!motor) return; - uint8_t cmd[8] = COMMAND_MOTOR_CLOSE; - int ret = mf4010v2_send_command(motor, cmd); - if (ret == MF4010_ERR_CAN_SEND) { - WRITE_FLAG(motor->flags, MF4010_FLAG_FAULT); - return; - } else if (ret == MF4010_ERR_CAN_RECV) { - WRITE_FLAG(motor->flags, MF4010_FLAG_FAULT); - return; - } else { - if (0 == memcmp(cmd, motor->last_response, sizeof(cmd)/sizeof(cmd[0]))) { - ERASE_FLAG(motor->flags, MF4010_FLAG_ENABLED); - return ; - } else { - WRITE_FLAG(motor->flags, MF4010_FLAG_FAULT); - return ; - } - } - -} - -int mf4010v2_read_pid_param(mf4010v2_t* motor, pid_param* pid) { - if (!motor) return MF4010_ERR_INVALID_PARAM; - uint8_t cmd[8] = {0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - mf4010v2_send_command(motor, cmd); - pid->angle_kp = motor->last_response[2]; - pid->angle_ki = motor->last_response[3]; - pid->speed_kp = motor->last_response[4]; - pid->speed_ki = motor->last_response[5]; - pid->iq_kp = motor->last_response[6]; - pid->iq_ki = motor->last_response[7]; - return MF4010_OK; -} -/* ============================================================================ - * 命令生成函数实现 - * ============================================================================ */ - -void cmd_torque_control(uint8_t* buf, int16_t iq) -{ - buf[0] = 0xA1; - buf[1] = 0x00; - buf[2] = 0x00; - buf[3] = 0x00; - buf[4] = (uint8_t)(iq & 0xFF); - buf[5] = (uint8_t)((iq >> 8) & 0xFF); - buf[6] = 0x00; - buf[7] = 0x00; -} - - - -void cmd_speed_control(uint8_t* buf, int32_t speed) -{ - buf[0] = 0xA2; - buf[1] = 0x00; - buf[2] = 0x00; - buf[3] = 0x00; - buf[4] = (uint8_t)(speed & 0xFF); - buf[5] = (uint8_t)((speed >> 8) & 0xFF); - buf[6] = (uint8_t)((speed >> 16) & 0xFF); - buf[7] = (uint8_t)((speed >> 24) & 0xFF); -} - - - -void cmd_mult_ring_pos(uint8_t* buf, int32_t angleControl) { - buf[0] = 0xA3; - buf[1] = 0x00; - buf[2] = 0x00; - buf[3] = 0x00; - buf[4] = *(uint8_t *)(&angleControl); - buf[5] = *((uint8_t *)(&angleControl)+1); - buf[6] = *((uint8_t *)(&angleControl)+2); - buf[7] = *((uint8_t *)(&angleControl)+3); -} - -void cmd_mult_ring_pos_with_speed(uint8_t* buf, uint16_t maxSpeed, int32_t angleControl) { - buf[0] = 0xA4; - buf[1] = 0x00; - buf[2] = *(uint8_t *)(&maxSpeed); - buf[3] = *((uint8_t *)(&maxSpeed)+1); - buf[4] = *(uint8_t *)(&angleControl); - buf[5] = *((uint8_t *)(&angleControl)+1); - buf[6] = *((uint8_t *)(&angleControl)+2); - buf[7] = *((uint8_t *)(&angleControl)+3); -} - -void cmd_single_ring_pos(uint8_t* buf, uint8_t direction, uint32_t angle) -{ - buf[0] = 0xA5; - buf[1] = direction & 0x01; - buf[2] = 0x00; - buf[3] = 0x00; - buf[4] = (uint8_t)(angle & 0xFF); - buf[5] = (uint8_t)((angle >> 8) & 0xFF); - buf[6] = (uint8_t)((angle >> 16) & 0xFF); - buf[7] = (uint8_t)((angle >> 24) & 0xFF); -} - -void cmd_single_ring_pos_with_speed(uint8_t* buf, uint8_t spinDirection, uint16_t maxSpeed, uint32_t angleControl) -{ - buf[0] = 0xA6; - buf[1] = spinDirection & 0x01; - buf[2] = (uint8_t)(maxSpeed & 0xFF); - buf[3] = (uint8_t)((maxSpeed >> 8) & 0xFF); - buf[4] = (uint8_t)(angleControl & 0xFF); - buf[5] = (uint8_t)((angleControl >> 8) & 0xFF); - buf[6] = (uint8_t)((angleControl >> 16) & 0xFF); - buf[7] = (uint8_t)((angleControl >> 24) & 0xFF); -} - -void cmd_incr_pos(uint8_t* buf, int32_t angle_increment) -{ - buf[0] = 0xA7; - buf[1] = 0x00; - buf[2] = 0x00; - buf[3] = 0x00; - buf[4] = (uint8_t)(angle_increment & 0xFF); - buf[5] = (uint8_t)((angle_increment >> 8) & 0xFF); - buf[6] = (uint8_t)((angle_increment >> 16) & 0xFF); - buf[7] = (uint8_t)((angle_increment >> 24) & 0xFF); -} - -void cmd_incr_pos_with_speed(uint8_t* buf, uint16_t max_speed, int32_t angle_increment) { - buf[0] = 0xA8; - buf[1] = 0x00; - buf[2] = (uint8_t)(max_speed & 0xFF); - buf[3] = (uint8_t)((max_speed >> 8) & 0xFF); - buf[4] = (uint8_t)(angle_increment & 0xFF); - buf[5] = (uint8_t)((angle_increment >> 8) & 0xFF); - buf[6] = (uint8_t)((angle_increment >> 16) & 0xFF); - buf[7] = (uint8_t)((angle_increment >> 24) & 0xFF); -} - -void cmd_write_pid_params(uint8_t* buf, uint8_t location, - uint8_t angle_kp, uint8_t angle_ki, - uint8_t speed_kp, uint8_t speed_ki, - uint8_t iq_kp, uint8_t iq_ki) -{ - buf[0] = location; - buf[1] = 0x00; - buf[2] = angle_kp; - buf[3] = angle_ki; - buf[4] = speed_kp; - buf[5] = speed_ki; - buf[6] = iq_kp; - buf[7] = iq_ki; -} - -/* ============================================================================ - * 辅助函数实现 - * ============================================================================ */ - -bool mf4010v2_is_connected(mf4010v2_t* motor) -{ - return (motor != NULL) && (motor->flags & MF4010_FLAG_CONNECTED); -} - -bool mf4010v2_is_running(mf4010v2_t* motor) -{ - return (motor != NULL) && (motor->flags & MF4010_FLAG_RUNNING); -} - -bool mf4010v2_is_fault(mf4010v2_t* motor) -{ - return (motor != NULL) && (motor->flags & MF4010_FLAG_FAULT); -} - -uint16_t mf4010v2_get_error_count(mf4010v2_t* motor) -{ - return (motor != NULL) ? motor->error_count : 0; -} - -void mf4010v2_reset_errors(mf4010v2_t* motor) -{ - if(motor != NULL) { - motor->error_count = 0; - } -} - -bool mf4010v2_response_has_error(mf4010v2_t* motor) -{ - return (motor != NULL) && ((motor->last_response[0] & 0x80) != 0); -} - -uint8_t mf4010v2_get_error_code(mf4010v2_t* motor) -{ - return (motor != NULL) ? (motor->last_response[0] & 0x7F) : 0; -} \ No newline at end of file +/** + * @file mf4010v2.c + * @brief MF4010V2 无刷电机驱动程序 + * @note 改进版本： + * - 添加错误处理和返回值 + * - 添加 CAN 超时机制 + * - 使用安全的内联函数替代危险宏 + * - 使用 HAL 层解耦硬件依赖 + */ + +#include "mf4010v2.h" +#include "can_if.h" +#include +#include +#include + +/* CAN 接收轮询超时（重试次数） */ +#define MF4010_RECV_RETRIES 2000 + +#define WRITE_FLAG(data, flag) do {(data) |= flag; } while(0) +#define ERASE_FLAG(data, flag) do {(data) &= ~flag; } while(0) +/** + * @brief 初始化电机结构体 + */ +int mf4010v2_init(mf4010v2_t* motor, uint16_t id, int can_ch) +{ + if(motor == NULL) { + return MF4010_ERR_INVALID_PARAM; + } + + if(id > 0x7FF) { + printf("Error: Invalid CAN ID 0x%03X (must be 0x000-0x7FF)\r\n", id); + return MF4010_ERR_INVALID_ID; + } + + motor->can_ch = can_ch; + motor->id = id; + motor->target_angle = 0; + motor->current_angle = 0; + memset(motor->last_response, 0, sizeof(motor->last_response)); + can_if_init(motor->can_ch); + motor->last_comm_time = 0; + motor->error_count = 0; + motor->success_count = 0; + motor->flags = MF4010_FLAG_ENABLED; + return MF4010_OK; +} + +/** + * @brief 发送命令并接收响应（带超时轮询） + */ +int mf4010v2_send_command(mf4010v2_t* motor, const uint8_t command[8]) +{ + can_message_t msg; + int recv_ok = 0; + + if (!motor) return MF4010_ERR_INVALID_PARAM; + + msg.id = motor->id; + memcpy(msg.data, command, 8); + msg.length = 8; + + int ret = can_if_send(motor->can_ch, &msg); + if (ret != 0) { + motor->error_count++; + // printf("Error: Failed to send CAN message (ID 0x%03X)\r\n", motor->id); + return MF4010_ERR_CAN_SEND; + } + + /* 轮询等待电机响应（带超时） */ + for (int retry = 0; retry < MF4010_RECV_RETRIES; retry++) { + ret = can_if_recv(motor->can_ch, &msg); + if (ret == 0 && msg.id == motor->id) { + recv_ok = 1; + break; + } + } + + if (!recv_ok) { + motor->error_count++; + printf("Error: Timeout waiting for motor response (ID 0x%03X)\r\n", motor->id); + return MF4010_ERR_NO_RESPONSE; + } + + memcpy(motor->last_response, msg.data, 8); + motor->success_count++; + return MF4010_OK; +} + +/** + * @brief 设置电机目标角度 (位置闭环) + * @param motor 电机结构体 + * @param angle_0_01deg 目标角度，单位 0.01 度 + */ +int mf4010v2_set_angle(mf4010v2_t* motor, int32_t angle_0_01deg) +{ + if (!motor) return MF4010_ERR_INVALID_PARAM; + + uint8_t cmd[8]; + // 使用增量位置控制 (A7 命令) + // 计算角度增量 + int32_t delta = angle_0_01deg - motor->current_angle; + + // 限制单次增量在合理范围内 + if (delta > 18000) delta = 18000; // 最多 +180 度 + if (delta < -18000) delta = -18000; // 最多 -180 度 + + cmd_incr_pos(cmd, delta); + motor->target_angle = angle_0_01deg; + + return mf4010v2_send_command(motor, cmd); +} + +/** + * @brief 设置电机速度 (速度闭环) + * @param motor 电机结构体 + * @param speed_0_01dps 目标速度，单位 0.01 DPS + */ +int mf4010v2_set_speed(mf4010v2_t* motor, int32_t speed_0_01dps) +{ + if (!motor) return MF4010_ERR_INVALID_PARAM; + + uint8_t cmd[8]; + cmd_speed_control(cmd, speed_0_01dps); + return mf4010v2_send_command(motor, cmd); +} + +/** + * @brief 启动电机 + */ +void mf4010v2_run(mf4010v2_t* motor) +{ + if (!motor) return; + uint8_t cmd[8] = COMMAND_MOTOR_RUNNING; + mf4010v2_send_command(motor, cmd); + motor->flags |= MF4010_FLAG_RUNNING; +} + +/** + * @brief 停止电机 + */ +void mf4010v2_stop(mf4010v2_t* motor) +{ + if (!motor) return; + uint8_t cmd[8] = COMMAND_MOTOR_STOP; + mf4010v2_send_command(motor, cmd); + motor->flags &= ~MF4010_FLAG_RUNNING; +} + +void mf4010v2_close(mf4010v2_t* motor) { + if (!motor) return; + uint8_t cmd[8] = COMMAND_MOTOR_CLOSE; + int ret = mf4010v2_send_command(motor, cmd); + if (ret == MF4010_ERR_CAN_SEND) { + WRITE_FLAG(motor->flags, MF4010_FLAG_FAULT); + return; + } else if (ret == MF4010_ERR_CAN_RECV) { + WRITE_FLAG(motor->flags, MF4010_FLAG_FAULT); + return; + } else { + if (0 == memcmp(cmd, motor->last_response, sizeof(cmd)/sizeof(cmd[0]))) { + ERASE_FLAG(motor->flags, MF4010_FLAG_ENABLED); + return ; + } else { + WRITE_FLAG(motor->flags, MF4010_FLAG_FAULT); + return ; + } + } + +} + +int mf4010v2_read_pid_param(mf4010v2_t* motor, pid_param* pid) { + if (!motor) return MF4010_ERR_INVALID_PARAM; + uint8_t cmd[8] = {0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + mf4010v2_send_command(motor, cmd); + pid->angle_kp = motor->last_response[2]; + pid->angle_ki = motor->last_response[3]; + pid->speed_kp = motor->last_response[4]; + pid->speed_ki = motor->last_response[5]; + pid->iq_kp = motor->last_response[6]; + pid->iq_ki = motor->last_response[7]; + return MF4010_OK; +} +/* ============================================================================ + * 命令生成函数实现 + * ============================================================================ */ + +void cmd_torque_control(uint8_t* buf, int16_t iq) +{ + buf[0] = 0xA1; + buf[1] = 0x00; + buf[2] = 0x00; + buf[3] = 0x00; + buf[4] = (uint8_t)(iq & 0xFF); + buf[5] = (uint8_t)((iq >> 8) & 0xFF); + buf[6] = 0x00; + buf[7] = 0x00; +} + + + +void cmd_speed_control(uint8_t* buf, int32_t speed) +{ + buf[0] = 0xA2; + buf[1] = 0x00; + buf[2] = 0x00; + buf[3] = 0x00; + buf[4] = (uint8_t)(speed & 0xFF); + buf[5] = (uint8_t)((speed >> 8) & 0xFF); + buf[6] = (uint8_t)((speed >> 16) & 0xFF); + buf[7] = (uint8_t)((speed >> 24) & 0xFF); +} + + + +void cmd_mult_ring_pos(uint8_t* buf, int32_t angleControl) { + buf[0] = 0xA3; + buf[1] = 0x00; + buf[2] = 0x00; + buf[3] = 0x00; + buf[4] = *(uint8_t *)(&angleControl); + buf[5] = *((uint8_t *)(&angleControl)+1); + buf[6] = *((uint8_t *)(&angleControl)+2); + buf[7] = *((uint8_t *)(&angleControl)+3); +} + +void cmd_mult_ring_pos_with_speed(uint8_t* buf, uint16_t maxSpeed, int32_t angleControl) { + buf[0] = 0xA4; + buf[1] = 0x00; + buf[2] = *(uint8_t *)(&maxSpeed); + buf[3] = *((uint8_t *)(&maxSpeed)+1); + buf[4] = *(uint8_t *)(&angleControl); + buf[5] = *((uint8_t *)(&angleControl)+1); + buf[6] = *((uint8_t *)(&angleControl)+2); + buf[7] = *((uint8_t *)(&angleControl)+3); +} + +void cmd_single_ring_pos(uint8_t* buf, uint8_t direction, uint32_t angle) +{ + buf[0] = 0xA5; + buf[1] = direction & 0x01; + buf[2] = 0x00; + buf[3] = 0x00; + buf[4] = (uint8_t)(angle & 0xFF); + buf[5] = (uint8_t)((angle >> 8) & 0xFF); + buf[6] = (uint8_t)((angle >> 16) & 0xFF); + buf[7] = (uint8_t)((angle >> 24) & 0xFF); +} + +void cmd_single_ring_pos_with_speed(uint8_t* buf, uint8_t spinDirection, uint16_t maxSpeed, uint32_t angleControl) +{ + buf[0] = 0xA6; + buf[1] = spinDirection & 0x01; + buf[2] = (uint8_t)(maxSpeed & 0xFF); + buf[3] = (uint8_t)((maxSpeed >> 8) & 0xFF); + buf[4] = (uint8_t)(angleControl & 0xFF); + buf[5] = (uint8_t)((angleControl >> 8) & 0xFF); + buf[6] = (uint8_t)((angleControl >> 16) & 0xFF); + buf[7] = (uint8_t)((angleControl >> 24) & 0xFF); +} + +void cmd_incr_pos(uint8_t* buf, int32_t angle_increment) +{ + buf[0] = 0xA7; + buf[1] = 0x00; + buf[2] = 0x00; + buf[3] = 0x00; + buf[4] = (uint8_t)(angle_increment & 0xFF); + buf[5] = (uint8_t)((angle_increment >> 8) & 0xFF); + buf[6] = (uint8_t)((angle_increment >> 16) & 0xFF); + buf[7] = (uint8_t)((angle_increment >> 24) & 0xFF); +} + +void cmd_incr_pos_with_speed(uint8_t* buf, uint16_t max_speed, int32_t angle_increment) { + buf[0] = 0xA8; + buf[1] = 0x00; + buf[2] = (uint8_t)(max_speed & 0xFF); + buf[3] = (uint8_t)((max_speed >> 8) & 0xFF); + buf[4] = (uint8_t)(angle_increment & 0xFF); + buf[5] = (uint8_t)((angle_increment >> 8) & 0xFF); + buf[6] = (uint8_t)((angle_increment >> 16) & 0xFF); + buf[7] = (uint8_t)((angle_increment >> 24) & 0xFF); +} + +void cmd_write_pid_params(uint8_t* buf, uint8_t location, + uint8_t angle_kp, uint8_t angle_ki, + uint8_t speed_kp, uint8_t speed_ki, + uint8_t iq_kp, uint8_t iq_ki) +{ + buf[0] = location; + buf[1] = 0x00; + buf[2] = angle_kp; + buf[3] = angle_ki; + buf[4] = speed_kp; + buf[5] = speed_ki; + buf[6] = iq_kp; + buf[7] = iq_ki; +} + +/* ============================================================================ + * 辅助函数实现 + * ============================================================================ */ + +bool mf4010v2_is_connected(mf4010v2_t* motor) +{ + return (motor != NULL) && (motor->flags & MF4010_FLAG_CONNECTED); +} + +bool mf4010v2_is_running(mf4010v2_t* motor) +{ + return (motor != NULL) && (motor->flags & MF4010_FLAG_RUNNING); +} + +bool mf4010v2_is_fault(mf4010v2_t* motor) +{ + return (motor != NULL) && (motor->flags & MF4010_FLAG_FAULT); +} + +uint16_t mf4010v2_get_error_count(mf4010v2_t* motor) +{ + return (motor != NULL) ? motor->error_count : 0; +} + +void mf4010v2_reset_errors(mf4010v2_t* motor) +{ + if(motor != NULL) { + motor->error_count = 0; + } +} + +bool mf4010v2_response_has_error(mf4010v2_t* motor) +{ + return (motor != NULL) && ((motor->last_response[0] & 0x80) != 0); +} + +uint8_t mf4010v2_get_error_code(mf4010v2_t* motor) +{ + return (motor != NULL) ? (motor->last_response[0] & 0x7F) : 0; +} diff --git a/modules/filter/lowpass/lowpass.c b/modules/filter/lowpass/lowpass.c new file mode 100644 index 0000000..768f3a6 --- /dev/null +++ b/modules/filter/lowpass/lowpass.c @@ -0,0 +1,34 @@ +#include "lowpass.h" + +#ifndef M_PI +#define M_PI (3.14159265358979323846f) +#endif + +void lowpass_init(lowpass_t *f, float alpha) +{ + if (!f) return; + f->y = 0.0f; + f->alpha = (alpha > 1.0f) ? 1.0f : (alpha < 0.0f) ? 0.0f : alpha; +} + +void lowpass_init_fc(lowpass_t *f, float fc, float dt) +{ + if (!f || dt <= 0.0f) return; + float rc = 1.0f / (2.0f * M_PI * fc); + float alpha = dt / (rc + dt); + lowpass_init(f, alpha); +} + +float lowpass_update(lowpass_t *f, float x) +{ + if (!f) return 0.0f; + f->y = f->y + f->alpha * (x - f->y); + return f->y; +} + +float lowpass_reset(lowpass_t *f, float x) +{ + if (!f) return 0.0f; + f->y = x; + return f->y; +} diff --git a/modules/filter/lowpass/lowpass.h b/modules/filter/lowpass/lowpass.h new file mode 100644 index 0000000..d52a9a3 --- /dev/null +++ b/modules/filter/lowpass/lowpass.h @@ -0,0 +1,22 @@ +#ifndef LOWPASS_H +#define LOWPASS_H + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct { + float y; /* 当前滤波输出 */ + float alpha; /* 平滑系数 (0..1): 越大截止频率越高 */ +} lowpass_t; + +void lowpass_init(lowpass_t *f, float alpha); +void lowpass_init_fc(lowpass_t *f, float fc, float dt); +float lowpass_update(lowpass_t *f, float x); +float lowpass_reset(lowpass_t *f, float x); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/modules/filter/moving_average/moving_average.c b/modules/filter/moving_average/moving_average.c new file mode 100644 index 0000000..b29bd62 --- /dev/null +++ b/modules/filter/moving_average/moving_average.c @@ -0,0 +1,46 @@ +#include "moving_average.h" +#include + +int movavg_init(movavg_t *f, float *buffer, int size) +{ + if (!f || !buffer || size < 1) return -1; + + f->buffer = buffer; + f->size = size; + f->index = 0; + f->count = 0; + f->sum = 0.0f; + + memset(f->buffer, 0, size * sizeof(float)); + return 0; +} + +float movavg_update(movavg_t *f, float x) +{ + if (!f || !f->buffer || f->size < 1) return 0.0f; + + f->sum -= f->buffer[f->index]; + f->buffer[f->index] = x; + f->sum += x; + + f->index = (f->index + 1) % f->size; + if (f->count < f->size) + f->count++; + + return f->sum / (float)f->count; +} + +float movavg_get(const movavg_t *f) +{ + if (!f || !f->buffer || f->count == 0) return 0.0f; + return f->sum / (float)f->count; +} + +void movavg_reset(movavg_t *f) +{ + if (!f || !f->buffer) return; + f->index = 0; + f->count = 0; + f->sum = 0.0f; + memset(f->buffer, 0, f->size * sizeof(float)); +} diff --git a/modules/filter/moving_average/moving_average.h b/modules/filter/moving_average/moving_average.h new file mode 100644 index 0000000..bb7057b --- /dev/null +++ b/modules/filter/moving_average/moving_average.h @@ -0,0 +1,25 @@ +#ifndef MOVING_AVERAGE_H +#define MOVING_AVERAGE_H + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct { + float *buffer; + float sum; + int size; + int index; + int count; +} movavg_t; + +int movavg_init(movavg_t *f, float *buffer, int size); +float movavg_update(movavg_t *f, float x); +float movavg_get(const movavg_t *f); +void movavg_reset(movavg_t *f); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/modules/filter/notch/notch.c b/modules/filter/notch/notch.c new file mode 100644 index 0000000..431e8b7 --- /dev/null +++ b/modules/filter/notch/notch.c @@ -0,0 +1,46 @@ +#include "notch.h" +#include + +#ifndef M_PI +#define M_PI 3.14159265358979323846f +#endif + +void notch_init(notch_t *f, float freq, float Q, float dt) +{ + if (!f || dt <= 0.0f || Q <= 0.0f) return; + + float w0 = 2.0f * M_PI * freq * dt; + float alpha = sinf(w0) / (2.0f * Q); + float cos_w0 = cosf(w0); + float norm = 1.0f / (1.0f + alpha); + + f->b0 = norm; + f->b1 = -2.0f * cos_w0 * norm; + f->b2 = norm; + f->a1 = f->b1; + f->a2 = (1.0f - alpha) * norm; + + notch_reset(f); +} + +float notch_update(notch_t *f, float x) +{ + if (!f) return 0.0f; + + float y = f->b0 * x + f->b1 * f->x1 + f->b2 * f->x2 + - f->a1 * f->y1 - f->a2 * f->y2; + + f->x2 = f->x1; + f->x1 = x; + f->y2 = f->y1; + f->y1 = y; + + return y; +} + +void notch_reset(notch_t *f) +{ + if (!f) return; + f->x1 = f->x2 = 0.0f; + f->y1 = f->y2 = 0.0f; +} diff --git a/modules/filter/notch/notch.h b/modules/filter/notch/notch.h new file mode 100644 index 0000000..cb3db4e --- /dev/null +++ b/modules/filter/notch/notch.h @@ -0,0 +1,24 @@ +#ifndef NOTCH_H +#define NOTCH_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* 二阶 IIR 陷波滤波器，用于抑制特定频率的机械共振 */ +typedef struct { + float b0, b1, b2; + float a1, a2; + float x1, x2; /* 输入历史 */ + float y1, y2; /* 输出历史 */ +} notch_t; + +void notch_init(notch_t *f, float freq, float Q, float dt); +float notch_update(notch_t *f, float x); +void notch_reset(notch_t *f); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/modules/filter/rate_limiter/rate_limiter.c b/modules/filter/rate_limiter/rate_limiter.c new file mode 100644 index 0000000..8b3e26d --- /dev/null +++ b/modules/filter/rate_limiter/rate_limiter.c @@ -0,0 +1,32 @@ +#include "rate_limiter.h" + +void rate_limiter_init(rate_limiter_t *rl, float max_rate) +{ + if (!rl) return; + rl->y = 0.0f; + rl->max_rate = (max_rate < 0.0f) ? 0.0f : max_rate; +} + +float rate_limiter_update(rate_limiter_t *rl, float x, float dt) +{ + if (!rl) return 0.0f; + if (dt <= 0.0f) return rl->y; + + float max_step = rl->max_rate * dt; + float diff = x - rl->y; + + if (diff > max_step) + rl->y += max_step; + else if (diff < -max_step) + rl->y -= max_step; + else + rl->y = x; + + return rl->y; +} + +void rate_limiter_reset(rate_limiter_t *rl, float x) +{ + if (!rl) return; + rl->y = x; +} diff --git a/modules/filter/rate_limiter/rate_limiter.h b/modules/filter/rate_limiter/rate_limiter.h new file mode 100644 index 0000000..450529d --- /dev/null +++ b/modules/filter/rate_limiter/rate_limiter.h @@ -0,0 +1,22 @@ +#ifndef RATE_LIMITER_H +#define RATE_LIMITER_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* 速率限制器 / 斜坡发生器：约束每秒钟的最大变化量 */ +typedef struct { + float y; /* 当前输出 */ + float max_rate; /* 最大变化率（单位/秒） */ +} rate_limiter_t; + +void rate_limiter_init(rate_limiter_t *rl, float max_rate); +float rate_limiter_update(rate_limiter_t *rl, float x, float dt); +void rate_limiter_reset(rate_limiter_t *rl, float x); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/release b/release index d31fb8a..07f1a2e 160000 --- a/release +++ b/release @@ -1 +1 @@ -Subproject commit d31fb8a6690b987ce4e5a4a772b2f0f19fc007eb +Subproject commit 07f1a2efda7b3fd6e6ee4236136a487086cdcbe1