[feat] UART command interface, per-motor control, filtering modules

- Add callback_task with ISR-based UART command parser (@command\n protocol)
- Add log_printf with mutex protection to prevent printf interleaving
- Add per-motor enable/disable (motor enable yaw|pitch)
- Add PID tuning via UART (pid roll kp 15)
- Add cmd_parser module (registration + tokenize + dispatch)
- Add UART layer architecture (interface → HAL → BSP)
- Add filter modules (lowpass, moving_average, notch, rate_limiter)
- Rewrite I2C bus and UART bus modules
- Rewrite PID controller and MF4010V2 motor driver
- Fix soft I2C driver

Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>
This commit is contained in:
robinsonandClaude Opus 4.7 committed 2026-05-21 01:38:11 +08:00
1 parent 75966b174d
commit cf934cf6b7
46 files changed
+2372 -2131

No files matched your search

-600
View File
@@ -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(&notch_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。
+114
View File
@@ -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/<periph>/<periph>_if.h — ops struct + register function + API declarations
interfaces/<periph>/<periph>_if.c — dispatching through registered ops
hal/stm32f4/<periph>/hal_<periph>.c — implements ops, registers them at init
bsp/stm32f4/<periph>/bsp_<periph>.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.
+21
View File
@@ -47,29 +47,40 @@ target_link_directories(${CMAKE_PROJECT_NAME} PRIVATE
target_sources(${CMAKE_PROJECT_NAME} PRIVATE target_sources(${CMAKE_PROJECT_NAME} PRIVATE
# Add user sources here # Add user sources here
3rdparty/jy901p/wit_c_sdk.c 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/motor/mf4010v2.c
modules/device/led/led.c modules/device/led/led.c
modules/device/can/can_device.c modules/device/can/can_device.c
modules/control/pid/pid.c modules/control/pid/pid.c
modules/bus/soft_i2c/soft_i2c.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/can/bsp_can.c
bsp/stm32f4/led/bsp_led.c bsp/stm32f4/led/bsp_led.c
bsp/stm32f4/i2c/bsp_i2c.c bsp/stm32f4/i2c/bsp_i2c.c
bsp/stm32f4/gpio/bsp_gpio.c bsp/stm32f4/gpio/bsp_gpio.c
bsp/stm32f4/soft_i2c/bsp_soft_i2c.c bsp/stm32f4/soft_i2c/bsp_soft_i2c.c
bsp/stm32f4/uart/bsp_uart.c
hal/stm32f4/can/hal_can.c hal/stm32f4/can/hal_can.c
hal/stm32f4/led/hal_led.c hal/stm32f4/led/hal_led.c
hal/stm32f4/i2c/hal_i2c.c hal/stm32f4/i2c/hal_i2c.c
hal/stm32f4/gpio/hal_gpio.c hal/stm32f4/gpio/hal_gpio.c
hal/stm32f4/soft_i2c/hal_soft_i2c.c hal/stm32f4/soft_i2c/hal_soft_i2c.c
hal/stm32f4/uart/hal_uart.c
app/app_init.c app/app_init.c
app/control_task.c app/control_task.c
app/sensor_task.c app/sensor_task.c
app/callback_task.c
interfaces/led/led_if.c interfaces/led/led_if.c
interfaces/can/can_if.c interfaces/can/can_if.c
interfaces/i2c/i2c_if.c interfaces/i2c/i2c_if.c
interfaces/gpio/gpio_if.c interfaces/gpio/gpio_if.c
interfaces/soft_i2c/soft_i2c_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/motor
${CMAKE_CURRENT_SOURCE_DIR}/modules/device/can ${CMAKE_CURRENT_SOURCE_DIR}/modules/device/can
${CMAKE_CURRENT_SOURCE_DIR}/modules/sensor/imu ${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/control/pid
${CMAKE_CURRENT_SOURCE_DIR}/modules/bus/soft_i2c ${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/can
${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/led ${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/led
${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/i2c ${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/i2c
${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/gpio ${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/gpio
${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/soft_i2c ${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/soft_i2c
${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/uart
${CMAKE_CURRENT_SOURCE_DIR}/app ${CMAKE_CURRENT_SOURCE_DIR}/app
${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/led ${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/led
${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/can ${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/can
${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/i2c ${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/i2c
${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/gpio ${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/gpio
${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/soft_i2c ${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/led
${CMAKE_CURRENT_SOURCE_DIR}/interfaces/can ${CMAKE_CURRENT_SOURCE_DIR}/interfaces/can
${CMAKE_CURRENT_SOURCE_DIR}/interfaces/i2c ${CMAKE_CURRENT_SOURCE_DIR}/interfaces/i2c
${CMAKE_CURRENT_SOURCE_DIR}/interfaces/gpio ${CMAKE_CURRENT_SOURCE_DIR}/interfaces/gpio
${CMAKE_CURRENT_SOURCE_DIR}/interfaces/soft_i2c ${CMAKE_CURRENT_SOURCE_DIR}/interfaces/soft_i2c
${CMAKE_CURRENT_SOURCE_DIR}/interfaces/uart
${CMAKE_CURRENT_SOURCE_DIR}/modules/bus/uart
) )
# Add project symbols (macros) # Add project symbols (macros)
+22 -10
View File
@@ -39,18 +39,21 @@ Mcu.IP4=NVIC
Mcu.IP5=RCC Mcu.IP5=RCC
Mcu.IP6=SYS Mcu.IP6=SYS
Mcu.IP7=USART1 Mcu.IP7=USART1
Mcu.IPNb=8 Mcu.IP8=USART6
Mcu.IPNb=9
Mcu.Name=STM32F407Z(E-G)Tx Mcu.Name=STM32F407Z(E-G)Tx
Mcu.Package=LQFP144 Mcu.Package=LQFP144
Mcu.Pin0=PE3 Mcu.Pin0=PE3
Mcu.Pin1=PE4 Mcu.Pin1=PE4
Mcu.Pin10=PA9 Mcu.Pin10=PC6
Mcu.Pin11=PA10 Mcu.Pin11=PC7
Mcu.Pin12=PD0 Mcu.Pin12=PA9
Mcu.Pin13=PD1 Mcu.Pin13=PA10
Mcu.Pin14=PG9 Mcu.Pin14=PD0
Mcu.Pin15=VP_FREERTOS_VS_CMSIS_V2 Mcu.Pin15=PD1
Mcu.Pin16=VP_SYS_VS_tim1 Mcu.Pin16=PG9
Mcu.Pin17=VP_FREERTOS_VS_CMSIS_V2
Mcu.Pin18=VP_SYS_VS_tim1
Mcu.Pin2=PF0 Mcu.Pin2=PF0
Mcu.Pin3=PF1 Mcu.Pin3=PF1
Mcu.Pin4=PH0-OSC_IN Mcu.Pin4=PH0-OSC_IN
@@ -59,7 +62,7 @@ Mcu.Pin6=PF12
Mcu.Pin7=PF14 Mcu.Pin7=PF14
Mcu.Pin8=PB12 Mcu.Pin8=PB12
Mcu.Pin9=PB13 Mcu.Pin9=PB13
Mcu.PinsNb=17 Mcu.PinsNb=19
Mcu.ThirdPartyNb=0 Mcu.ThirdPartyNb=0
Mcu.UserConstants= Mcu.UserConstants=
Mcu.UserName=STM32F407ZGTx 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.TIM1_UP_TIM10_IRQn=true\:15\:0\:false\:false\:true\:false\:false\:true\:true
NVIC.TimeBase=TIM1_UP_TIM10_IRQn NVIC.TimeBase=TIM1_UP_TIM10_IRQn
NVIC.TimeBaseIP=TIM1 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 NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
PA10.Mode=Asynchronous PA10.Mode=Asynchronous
PA10.Signal=USART1_RX PA10.Signal=USART1_RX
@@ -91,6 +95,12 @@ PB12.Mode=CAN_Activate
PB12.Signal=CAN2_RX PB12.Signal=CAN2_RX
PB13.Mode=CAN_Activate PB13.Mode=CAN_Activate
PB13.Signal=CAN2_TX 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.Checker=false
PCC.Display=Plot\: All Steps PCC.Display=Plot\: All Steps
PCC.Line=STM32F407/417 PCC.Line=STM32F407/417
@@ -176,7 +186,7 @@ ProjectManager.ToolChainLocation=
ProjectManager.UAScriptAfterPath= ProjectManager.UAScriptAfterPath=
ProjectManager.UAScriptBeforePath= ProjectManager.UAScriptBeforePath=
ProjectManager.UnderRoot=false 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.48MHZClocksFreq_Value=84000000
RCC.AHBFreq_Value=168000000 RCC.AHBFreq_Value=168000000
RCC.APB1CLKDivider=RCC_HCLK_DIV4 RCC.APB1CLKDivider=RCC_HCLK_DIV4
@@ -213,6 +223,8 @@ RCC.VCOOutputFreq_Value=336000000
RCC.VcooutputI2S=192000000 RCC.VcooutputI2S=192000000
USART1.IPParameters=VirtualMode USART1.IPParameters=VirtualMode
USART1.VirtualMode=VM_ASYNC USART1.VirtualMode=VM_ASYNC
USART6.IPParameters=VirtualMode
USART6.VirtualMode=VM_ASYNC
VP_FREERTOS_VS_CMSIS_V2.Mode=CMSIS_V2 VP_FREERTOS_VS_CMSIS_V2.Mode=CMSIS_V2
VP_FREERTOS_VS_CMSIS_V2.Signal=FREERTOS_VS_CMSIS_V2 VP_FREERTOS_VS_CMSIS_V2.Signal=FREERTOS_VS_CMSIS_V2
VP_SYS_VS_tim1.Mode=TIM1 VP_SYS_VS_tim1.Mode=TIM1
+1
View File
@@ -54,6 +54,7 @@ void UsageFault_Handler(void);
void DebugMon_Handler(void); void DebugMon_Handler(void);
void TIM1_UP_TIM10_IRQHandler(void); void TIM1_UP_TIM10_IRQHandler(void);
void I2C2_EV_IRQHandler(void); void I2C2_EV_IRQHandler(void);
void USART1_IRQHandler(void);
/* USER CODE BEGIN EFP */ /* USER CODE BEGIN EFP */
/* USER CODE END EFP */ /* USER CODE END EFP */
+3
View File
@@ -34,11 +34,14 @@ extern "C" {
extern UART_HandleTypeDef huart1; extern UART_HandleTypeDef huart1;
extern UART_HandleTypeDef huart6;
/* USER CODE BEGIN Private defines */ /* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */ /* USER CODE END Private defines */
void MX_USART1_UART_Init(void); void MX_USART1_UART_Init(void);
void MX_USART6_UART_Init(void);
/* USER CODE BEGIN Prototypes */ /* USER CODE BEGIN Prototypes */
+1 -1
View File
@@ -136,7 +136,7 @@ void MX_FREERTOS_Init(void) {
void StartMonitorTask(void *argument) void StartMonitorTask(void *argument)
{ {
/* USER CODE BEGIN StartMonitorTask */ /* USER CODE BEGIN StartMonitorTask */
monitor_task(); callback_task();
/* USER CODE END StartMonitorTask */ /* USER CODE END StartMonitorTask */
} }
+3 -2
View File
@@ -49,6 +49,7 @@ void MX_GPIO_Init(void)
__HAL_RCC_GPIOF_CLK_ENABLE(); __HAL_RCC_GPIOF_CLK_ENABLE();
__HAL_RCC_GPIOH_CLK_ENABLE(); __HAL_RCC_GPIOH_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE(); __HAL_RCC_GPIOB_CLK_ENABLE();
__HAL_RCC_GPIOC_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE(); __HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOD_CLK_ENABLE(); __HAL_RCC_GPIOD_CLK_ENABLE();
__HAL_RCC_GPIOG_CLK_ENABLE(); __HAL_RCC_GPIOG_CLK_ENABLE();
@@ -71,7 +72,7 @@ void MX_GPIO_Init(void)
/*Configure GPIO pins : SDA_Pin SCL_Pin */ /*Configure GPIO pins : SDA_Pin SCL_Pin */
GPIO_InitStruct.Pin = 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.Pull = GPIO_PULLUP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
HAL_GPIO_Init(GPIOF, &GPIO_InitStruct); HAL_GPIO_Init(GPIOF, &GPIO_InitStruct);
@@ -87,4 +88,4 @@ void MX_GPIO_Init(void)
/* USER CODE BEGIN 2 */ /* USER CODE BEGIN 2 */
/* USER CODE END 2 */ /* USER CODE END 2 */
+6 -2
View File
@@ -21,11 +21,14 @@
#include "cmsis_os.h" #include "cmsis_os.h"
#include "can.h" #include "can.h"
#include "i2c.h" #include "i2c.h"
#include "stm32f4xx_hal_uart.h"
#include "usart.h" #include "usart.h"
#include "gpio.h" #include "gpio.h"
#include <stdint.h>
/* Private includes ----------------------------------------------------------*/ /* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */ /* USER CODE BEGIN Includes */
/* USER CODE END Includes */ /* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/ /* Private typedef -----------------------------------------------------------*/
@@ -61,7 +64,7 @@ int __io_putchar(int ch)
/* Private user code ---------------------------------------------------------*/ /* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */ /* USER CODE BEGIN 0 */
uint8_t g_uart_rx_byte;
/* USER CODE END 0 */ /* USER CODE END 0 */
@@ -99,8 +102,9 @@ int main(void)
MX_USART1_UART_Init(); MX_USART1_UART_Init();
MX_CAN2_Init(); MX_CAN2_Init();
MX_CAN1_Init(); MX_CAN1_Init();
MX_USART6_UART_Init();
/* USER CODE BEGIN 2 */ /* USER CODE BEGIN 2 */
HAL_UART_Receive_IT(&huart1, &g_uart_rx_byte, 1);
/* USER CODE END 2 */ /* USER CODE END 2 */
+15
View File
@@ -56,6 +56,7 @@
/* External variables --------------------------------------------------------*/ /* External variables --------------------------------------------------------*/
extern I2C_HandleTypeDef hi2c2; extern I2C_HandleTypeDef hi2c2;
extern UART_HandleTypeDef huart1;
extern TIM_HandleTypeDef htim1; extern TIM_HandleTypeDef htim1;
/* USER CODE BEGIN EV */ /* USER CODE BEGIN EV */
@@ -188,6 +189,20 @@ void I2C2_EV_IRQHandler(void)
/* USER CODE END I2C2_EV_IRQn 1 */ /* 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 */ /* USER CODE BEGIN 1 */
+77
View File
@@ -25,6 +25,7 @@
/* USER CODE END 0 */ /* USER CODE END 0 */
UART_HandleTypeDef huart1; UART_HandleTypeDef huart1;
UART_HandleTypeDef huart6;
/* USART1 init function */ /* USART1 init function */
@@ -54,6 +55,35 @@ void MX_USART1_UART_Init(void)
/* USER CODE END USART1_Init 2 */ /* 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) void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
@@ -80,10 +110,37 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
GPIO_InitStruct.Alternate = GPIO_AF7_USART1; GPIO_InitStruct.Alternate = GPIO_AF7_USART1;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); 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 BEGIN USART1_MspInit 1 */
/* USER CODE END 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) 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); 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 BEGIN USART1_MspDeInit 1 */
/* USER CODE END 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 */ /* USER CODE BEGIN 1 */
-497
View File
@@ -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` |
---
> **一句话原则**: 每一个改进项都是可验证的独立单元,改一项测一项,不要一次性全部改动。
-179
View File
@@ -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
+63 -21
View File
@@ -1,21 +1,63 @@
#ifndef __APP_H__ #ifndef __APP_H__
#define __APP_H__ #define __APP_H__
#include "hal_gpio.h" #include "hal_gpio.h"
#include "led.h" #include "led.h"
#include "mf4010v2.h" #include "mf4010v2.h"
#include "hal_i2c.h" #include "hal_i2c.h"
#include "hal_can.h" #include "hal_can.h"
#include "hal_led.h" #include "hal_led.h"
#include "hal_soft_i2c.h" #include "hal_soft_i2c.h"
#include "freertos.h" #include "hal_uart.h"
#include "pid.h" #include "freertos.h"
#include <stdio.h> #include "pid.h"
#include <FreeRTOS.h> #include <stdio.h>
#include "cmsis_os.h" #include <FreeRTOS.h>
#include "projdefs.h" #include "cmsis_os.h"
#include "wit_c_sdk.h" #include "cmsis_os2.h"
void app_init(void); #include "projdefs.h"
void sensor_task(void); #include "wit_c_sdk.h"
void control_task(void);
void monitor_task(void); /* ============================================================================
#endif * 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
+67 -39
View File
@@ -1,39 +1,67 @@
#include "app.h" #include "app.h"
#include "cmsis_os2.h"
#include <string.h>
static led_t led0, led1, led2; #include <stdarg.h>
void app_init(void) static led_t led0, led1, led2;
{
// 初始化硬件抽象层 /* IMU 数据队列句柄 — sensor_task 和 control_task 共享 */
hal_i2c_init_all(); osMessageQueueId_t g_imu_queue = NULL;
hal_led_init_all();
hal_can_init_all(); /* UART 命令队列 — ISR → callback_task */
hal_gpio_init_all(); osMessageQueueId_t g_cmd_queue = NULL;
hal_soft_i2c_init_all();
// 初始化外设 /* UART 发送互斥锁 — 防止多任务 printf 交叉 */
led_init(&led0, 0); osMutexId_t g_uart_mutex = NULL;
led_init(&led1, 1);
led_init(&led2, 2);
led_on(&led0); void app_init(void)
led_on(&led1); {
led_on(&led2); // 初始化硬件抽象层
led_off(&led0); hal_i2c_init_all();
hal_led_init_all();
hal_can_init_all();
printf("[app_init] Hardware initialized\r\n"); hal_gpio_init_all();
} hal_soft_i2c_init_all();
hal_uart_init_all();
// 初始化外设
led_init(&led0, 0);
led_init(&led1, 1);
void monitor_task(void) led_init(&led2, 2);
{ led_on(&led0);
while (1) { led_on(&led1);
// 简单的监控任务:每秒打印一次当前状态 led_on(&led2);
// printf("Pitch: %.2f deg, Roll: %.2f deg\n", g_imu.angle[1] / 100.0f, g_imu.angle[0] / 100.0f); led_off(&led0);
vTaskDelay(pdMS_TO_TICKS(1000)); // 1Hz 更新率
} // 创建 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);
}
}
+191
View File
@@ -0,0 +1,191 @@
#include "app.h"
#include "cmsis_os2.h"
#include <string.h>
#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 <axis> <p> <v> ==> 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 <roll|pitch> <kp|ki|kd> <value> (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 <roll|pitch> <kp|ki|kd> <value>\r\n");
}
}
} else {
log_printf("[cmd] usage: pid <roll|pitch> <kp|ki|kd> <value>\r\n");
}
} else {
log_printf("[cmd] unknown: %s\r\n", cmd_buf);
}
}
}
+279 -165
View File
@@ -1,165 +1,279 @@
/** /**
* @file control_task.c * @file control_task.c
* @brief 二轴云台增稳控制任务 * @brief 二轴云台增稳控制任务(速度控制模式)
* @note 目标:保持 Roll=0, Pitch=0,Yaw 用于方向控制 * @note 目标:保持 Roll=0, Pitch=0,Yaw 用于方向控制
* *
* 控制架构: * 控制架构(速度控制):
* IMU 数据 → PID 控制器 → 电机补偿角度 → 抵消基座倾斜 * IMU 角度 → PID 控制器 → 角速度指令 → 电机速度闭环 → 抵消基座倾斜
*/ *
* 数据来源:
#include "app.h" * 通过 FreeRTOS Message Queue 从 sensor_task 接收 imu_data_t
#include "cmsis_os2.h" */
#include "mf4010v2.h"
#include <stdio.h> #include "app.h"
#include "cmsis_os2.h"
/* ============================================================================ #include "mf4010v2.h"
* 外部变量 (来自 sensor_task) #include "pid.h"
* ============================================================================ */ #include <stdio.h>
extern float fAcc[3], fGyro[3], fAngle[3]; /* fAngle[0]=Roll, fAngle[1]=Pitch, fAngle[2]=Yaw */ #include <string.h>
#include "lowpass.h"
/* ============================================================================ /* ============================================================================
* 云台配置 * 云台配置
* ============================================================================ */ * ============================================================================ */
/* 目标角度:保持水平 */ /* 目标角度:保持水平 */
static float g_target_roll = 0.0f; static float g_target_roll = 0.0f; /* Roll 目标 (deg) */
static float g_target_pitch = 0.0f; static float g_target_pitch = 0.0f; /* Pitch 目标 (deg) */
static float g_target_yaw = 0.0f; /* Yaw 目标可通过外部修改 */ static float g_target_yaw = 0.0f; /* Yaw 目标 (deg) */
/* 电机实例 */ /* 电机实例 */
static mf4010v2_t motor_yaw; /* Yaw 轴电机(外框) */ static mf4010v2_t motor_yaw; /* Yaw 轴电机(外框,补偿 Roll) */
static mf4010v2_t motor_pitch; /* Pitch 轴电机(内框) */ static mf4010v2_t motor_pitch; /* Pitch 轴电机(内框,补偿 Pitch) */
/* PID 控制器 */ /* PID 控制器 */
typedef struct { static pid_t g_roll_pid; /* Roll 轴 PID */
float Kp, Ki, Kd; static pid_t g_pitch_pid; /* Pitch 轴 PID */
float integral;
float prev_error; static lowpass_t g_roll_lowpass;
float output_max; static lowpass_t g_pitch_lowpass;
} PID_t;
/* 电机使能状态 — 由 callback_task 通过 UART 命令控制 */
static PID_t g_roll_pid = {0}; static bool g_motor_enabled[2] = {false, false};
static PID_t g_pitch_pid = {0}; /* ============================================================================
* 速度控制参数
/* ============================================================================ * ============================================================================ */
* PID 控制函数
* ============================================================================ */ /*
* PID 输出为速度指令 (0.01 DPS/LSB)
void PID_Init(PID_t *pid, float kp, float ki, float kd, float max) *
{ * 电机 MF4010V2 速度范围:±204800 (即 ±2048 DPS)
pid->Kp = kp; * 稳像场景使用较低速度:建议 ±36000 (即 ±360 DPS) 作为输出上限
pid->Ki = ki; */
pid->Kd = kd; #define SPEED_OUTPUT_MAX 36000.0f /* 最大角速度 360 DPS (0.01 DPS) */
pid->output_max = max;
pid->integral = 0; /*
pid->prev_error = 0; * PID 参数说明(速度控制模式):
} *
* Kp: 角度误差 → 速度指令的比例系数 (0.01 DPS / deg)
float PID_Compute(PID_t *pid, float target, float actual) * 例如 Kp=200, 则 1° 误差 → 200 DPS 的补偿速度
{ *
float error = target - actual; * Ki: 消除稳态误差的积分系数
float output; * 例如 Ki=5, 则 1° 持续误差每秒累积 5 DPS
*
/* 比例项 */ * Kd: 抑制振荡的微分系数
output = pid->Kp * error; * 例如 Kd=50, 则角速度变化率反馈阻尼
*/
/* 积分项(带抗饱和) */ #define ROLL_KP 200.0f
pid->integral += error; #define ROLL_KI 5.0f
if (pid->integral > pid->output_max) { #define ROLL_KD 50.0f
pid->integral = pid->output_max; #define ROLL_ALPHA 0.7f
} else if (pid->integral < -pid->output_max) {
pid->integral = -pid->output_max; #define PITCH_KP 200.0f
} #define PITCH_KI 5.0f
output += pid->Ki * pid->integral; #define PITCH_KD 50.0f
/* 微分项 */ #define PITCH_ALPHA 0.7f
float derivative = error - pid->prev_error; /* ============================================================================
output += pid->Kd * derivative; * 控制任务(速度控制模式)
pid->prev_error = error; * ============================================================================ */
/* 输出限幅 */ void control_task(void)
if (output > pid->output_max) { {
output = pid->output_max; imu_data_t imu;
} else if (output < -pid->output_max) { osStatus_t qstat;
output = -pid->output_max; uint32_t last_tick = osKernelGetTickCount();
}
log_printf("[control_task] starting stabilization control (speed mode)\n");
return output;
} /* 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;
void control_task(void) g_roll_pid.integral_max = SPEED_OUTPUT_MAX;
{
// int32_t yaw_cmd, pitch_cmd; pid_init(&g_pitch_pid);
// float roll_comp, pitch_comp; g_pitch_pid.Kp = PITCH_KP;
g_pitch_pid.Ki = PITCH_KI;
printf("[control_task] starting stabilization control\n"); g_pitch_pid.Kd = PITCH_KD;
g_pitch_pid.output_max = SPEED_OUTPUT_MAX;
/* 1. 初始化 PID 参数 */ g_pitch_pid.integral_max = SPEED_OUTPUT_MAX;
/* Roll 轴 PID:控制 Yaw 电机来补偿 Roll 倾斜 */
// PID_Init(&g_roll_pid, 0.5f, 0.01f, 0.1f, 18000.0f); /* 最大补偿 180 度 */ lowpass_init(&g_roll_lowpass, ROLL_ALPHA);
lowpass_init(&g_pitch_lowpass, PITCH_ALPHA);
/* Pitch 轴 PID:控制 Pitch 电机补偿 Pitch 倾斜 */
// PID_Init(&g_pitch_pid, 0.5f, 0.01f, 0.1f, 9000.0f); /* 最大补偿 90 度 */ 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. 初始化电机 */
// osDelay(pdMS_TO_TICKS(1000)); /* 2. 初始化电机 */
mf4010v2_init(&motor_yaw, 0x141, 1); /* Yaw 电机 ID: 0x141, CAN2 */ mf4010v2_init(&motor_yaw, 0x141, 1); /* Yaw 电机 ID: 0x141, CAN2 */
mf4010v2_init(&motor_pitch, 0x142, 1); /* Pitch 电机 ID: 0x142, CAN2 */ mf4010v2_init(&motor_pitch, 0x142, 1); /* Pitch 电机 ID: 0x142, CAN2 */
/* 3. 电机关机状态启动 */ /* 3. 电机关机状态启动 */
mf4010v2_close(&motor_yaw); mf4010v2_close(&motor_yaw);
mf4010v2_close(&motor_pitch); mf4010v2_close(&motor_pitch);
printf("[control_task] motor yaw is fault: %d\n", mf4010v2_is_fault(&motor_yaw)); log_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)); log_printf("[control_task] motor pitch is fault: %d\n", mf4010v2_is_fault(&motor_pitch));
/* 等待 IMU 稳定 */
osDelay(pdMS_TO_TICKS(5000)); /* 等待 IMU 稳定 */
osDelay(pdMS_TO_TICKS(5000));
/* 4. 使能电机 */
mf4010v2_run(&motor_yaw); /* 4. 使能电机(零速启动) */
mf4010v2_run(&motor_pitch); mf4010v2_set_speed(&motor_yaw, 0);
mf4010v2_set_speed(&motor_pitch, 0);
printf("[control_task] stabilization enabled\n"); mf4010v2_run(&motor_yaw);
mf4010v2_run(&motor_pitch);
/* 主控制循环 (100Hz) */
while (1) { log_printf("[control_task] stabilization enabled (speed mode)\n");
/* 读取 IMU 数据 */
// float current_roll = fAngle[0]; /* Roll 角 */ /* ========================================================================
// float current_pitch = fAngle[1]; /* Pitch 角 */ * 主控制循环 (100Hz, osDelayUntil 精确周期性调度)
*
// /* PID 计算补偿量 */ * 速度控制策略:
// /* 目标:Roll=0, Pitch=0 */ * 1. 从队列读取最新 IMU 角度 (Roll, Pitch)
// roll_comp = PID_Compute(&g_roll_pid, g_target_roll, current_roll); * 2. PID 计算角速度补偿量
// pitch_comp = PID_Compute(&g_pitch_pid, g_target_pitch, current_pitch); * 3. 速度指令发送到电机(电机内部完成速度闭环)
* ======================================================================== */
// /* 运动学映射:补偿量 -> 电机角度 */ while (1) {
// /* 对于 Pitch-Yaw 云台: last_tick += pdMS_TO_TICKS(20); /* 100Hz 周期 */
// * - Pitch 倾斜 → Pitch 电机反向补偿
// * - Roll 倾斜 → Yaw 电机 + Pitch 电机联合补偿(简化为 Yaw 电机) /* 从 sensor_task 获取最新 IMU 数据(不等待,由定时保证节奏) */
// */ qstat = osMessageQueueGet(g_imu_queue, &imu, NULL, 0);
// yaw_cmd = (int32_t)(roll_comp * 100.0f); /* 转为 0.01 度 */ if (qstat == osOK) {
// pitch_cmd = (int32_t)(pitch_comp * 100.0f); imu.angle[0] = lowpass_update(&g_roll_lowpass, imu.angle[0]);
imu.angle[1] = lowpass_update(&g_pitch_lowpass, imu.angle[1]);
// /* 发送位置指令到电机 */
// /* 使用增量位置控制,直接叠加补偿量 */ /* ---- Roll 轴速度控制 ---- */
// mf4010v2_set_angle(&motor_yaw, motor_yaw.current_angle + yaw_cmd); float roll_speed = pid_compute(&g_roll_pid,
// mf4010v2_set_angle(&motor_pitch, motor_pitch.current_angle + pitch_cmd); g_target_roll,
imu.angle[0]); /* Roll */
/* 100Hz 控制循环 */ int32_t yaw_speed_cmd = (int32_t)roll_speed;
osDelay(pdMS_TO_TICKS(10));
} /* ---- Pitch 轴速度控制 ---- */
} float pitch_speed = pid_compute(&g_pitch_pid,
g_target_pitch,
/* ============================================================================ imu.angle[1]); /* Pitch */
* 外部 API 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);
/* 设置 Yaw 目标方向 */
void gimbal_set_yaw_target(float yaw_deg) if (g_motor_enabled[GIMBAL_MOTOR_YAW]) {
{ mf4010v2_set_speed(&motor_yaw, yaw_speed_cmd);
g_target_yaw = yaw_deg; } 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;
}
+130 -121
View File
@@ -1,7 +1,20 @@
#include "REG.h"
#include "app.h" #include "app.h"
#include "cmsis_os2.h" #include "cmsis_os2.h"
#include "soft_i2c.h" #include "soft_i2c.h"
#include "i2c_bus.h"
#include "wit_c_sdk.h"
#include <stdint.h>
#define USE_SOFT_IIC 1
#define USE_IIC 1
#if USE_IIC
#if USE_SOFT_IIC
static soft_i2c_t si2c; static soft_i2c_t si2c;
#else
static i2c_t i2c;
#endif
#endif
#define ACC_UPDATE 0x01 #define ACC_UPDATE 0x01
#define GYRO_UPDATE 0x02 #define GYRO_UPDATE 0x02
@@ -9,12 +22,13 @@ static soft_i2c_t si2c;
#define MAG_UPDATE 0x08 #define MAG_UPDATE 0x08
#define READ_UPDATE 0x80 #define READ_UPDATE 0x80
static volatile char s_cDataUpdate = 0, s_cCmd = 0xff; static volatile char s_cDataUpdate = 0, s_cCmd = 0xff;
static void CmdProcess(void); static bool g_sensor_enabled = true; /* 由 callback_task 通过 UART 命令控制 */
static void AutoScanSensor(void);
static void CopeSensorData(uint32_t uiReg, uint32_t uiRegNum); static void CopeSensorData(uint32_t uiReg, uint32_t uiRegNum);
static void Delayms(uint16_t ucMs); 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) { 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_start_frame(&si2c);
soft_i2c_send_frame(&si2c, ucAddr); soft_i2c_send_frame(&si2c, ucAddr);
if(soft_i2c_wait_ack_frame(&si2c) != 0)return 0; 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); soft_i2c_stop_frame(&si2c);
return 1; 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) { 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_start_frame(&si2c);
soft_i2c_send_frame(&si2c, ucAddr); soft_i2c_send_frame(&si2c, ucAddr);
if(soft_i2c_wait_ack_frame(&si2c) != 0)return 0; 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; if(soft_i2c_wait_ack_frame(&si2c) != 0)return 0;
for(uint32_t i = 0; i < uiLen; i++) 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; if(soft_i2c_wait_ack_frame(&si2c) != 0)return 0;
} }
soft_i2c_stop_frame(&si2c); soft_i2c_stop_frame(&si2c);
return 1; 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) void sensor_task(void)
{ {
float fAcc[3], fGyro[3], fAngle[3];
int i; int i;
int ret; int ret;
imu_data_t imu;
#if USE_IIC
#if USE_SOFT_IIC
ret = soft_i2c_init(&si2c, 0); 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); WitInit(WIT_PROTOCOL_I2C, 0x50);
WitI2cFuncRegister(IICwriteBytes, IICreadBytes); WitI2cFuncRegister(IICwriteBytes, IICreadBytes);
#else
WitInit(WIT_PROTOCOL_NORMAL, 0x50);
WitSerialWriteRegister();
#endif
WitRegisterCallBack(CopeSensorData); WitRegisterCallBack(CopeSensorData);
WitDelayMsRegister(Delayms); WitDelayMsRegister(Delayms);
AutoScanSensor();
uint32_t last_tick = osKernelGetTickCount();
while (1) while (1)
{ {
WitReadReg(AX, 12); if(g_sensor_enabled) {
Delayms(5); WitReadReg(AX, 13);
CmdProcess(); }
if(s_cDataUpdate) last_tick += pdMS_TO_TICKS(2); /* 500Hz 轮询 */
if(g_sensor_enabled && s_cDataUpdate)
{ {
for(i = 0; i < 3; i++) for(i = 0; i < 3; i++)
{ {
fAcc[i] = sReg[AX+i] * 0.0054931640625f; imu.acc[i] = sReg[AX+i] * 0.0054931640625f;
fGyro[i] = sReg[GX+i] * 0.0054931640625f; imu.gyro[i] = sReg[GX+i] * 0.0054931640625f;
fAngle[i] = sReg[Roll+i] * 0.0054931640625f; imu.angle[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;
} }
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) 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; 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]); char sign = temp > 0 ? '+' : '-';
s_cDataUpdate &= ~MAG_UPDATE; 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; g_sensor_enabled = true;
log_printf("[sensor_task] enabled\r\n");
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;
}
}
} }
static void CmdProcess(void) void sensor_disable(void)
{ {
switch(s_cCmd) g_sensor_enabled = false;
{ log_printf("[sensor_task] disabled\r\n");
case 'a': }
if(WitStartAccCali() != WIT_HAL_OK)
printf("\r\nSet AccCali Error\r\n"); void sensor_acc_cali_enable(void)
break; {
case 'm': WitStartAccCali();
if(WitStartMagCali() != WIT_HAL_OK) }
printf("\r\nStart MagCali Error\r\n");
break; void sensor_acc_cali_disable(void)
case 'e': {
if(WitStopMagCali() != WIT_HAL_OK) WitStopAccCali();
printf("\r\nEnd MagCali Error\r\n"); }
break;
case 'u': void sensor_mag_cali_enable(void)
if(WitSetBandwidth(BANDWIDTH_5HZ) != WIT_HAL_OK) {
printf("\r\nSet Bandwidth Error\r\n"); WitStartMagCali();
break; }
case 'U':
if(WitSetBandwidth(BANDWIDTH_256HZ) != WIT_HAL_OK) void sensor_mag_cali_disable(void)
printf("\r\nSet Bandwidth Error\r\n"); {
break; WitStopMagCali();
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;
} }
static void CopeSensorData(uint32_t uiReg, uint32_t uiRegNum) 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 AX:
// case AY: // case AY:
case AZ: // case AZ:
s_cDataUpdate |= ACC_UPDATE; // s_cDataUpdate |= ACC_UPDATE;
break; // break;
// case GX: // case GX:
// case GY: // case GY:
case GZ: // case GZ:
s_cDataUpdate |= GYRO_UPDATE; // s_cDataUpdate |= GYRO_UPDATE;
break; // break;
// case HX: // case HX:
// case HY: // case HY:
case HZ: // case HZ:
s_cDataUpdate |= MAG_UPDATE; // s_cDataUpdate |= MAG_UPDATE;
break; // break;
// case Roll: // case Roll:
// case Pitch: // case Pitch:
case Yaw: case Yaw:
@@ -202,28 +236,3 @@ static void Delayms(uint16_t ucMs)
{ {
osDelay(pdMS_TO_TICKS(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");
}
+3 -3
View File
@@ -4,15 +4,15 @@
static bsp_soft_i2c_t soft_i2cs[] = { static bsp_soft_i2c_t soft_i2cs[] = {
{ {
.scl_port = GPIOF, .scl_port = GPIOF,
.scl_pin = SCL_Pin, .scl_pin = SCL_Pin, // GPIO_PIN_14
.sda_port = GPIOF, .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) { bsp_soft_i2c_t* bsp_soft_i2c_get_handle(int ch) {
if (ch < 0 || ch >= (int)(sizeof(soft_i2cs) / sizeof(bsp_soft_i2c_t))) { if (ch < 0 || ch >= (int)(sizeof(soft_i2cs) / sizeof(bsp_soft_i2c_t))) {
return NULL; return NULL;
} }
return &soft_i2cs[ch]; return &soft_i2cs[ch];
} }
+16
View File
@@ -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];
}
+12
View File
@@ -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
+45
View File
@@ -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)构建响应式收发框架。
+126 -120
View File
@@ -1,120 +1,126 @@
#include "can_if.h" #include "can_if.h"
#include "bsp_can.h" #include "bsp_can.h"
#include "can.h" #include "can.h"
#include "stm32f4xx_hal_can.h" #include "stm32f4xx_hal_can.h"
/* CAN 发送超时 (ms) */
static int hal_can_init(int ch) #define CAN_TX_TIMEOUT_MS 100U
{
CAN_HandleTypeDef *hcan = bsp_can_get_handle(ch)->hcan;
if (!hcan) return -1; static int hal_can_init(int ch)
{
CAN_FilterTypeDef sFilterConfig; CAN_HandleTypeDef *hcan = bsp_can_get_handle(ch)->hcan;
sFilterConfig.FilterBank = 0; if (!hcan) return -1;
sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT; CAN_FilterTypeDef sFilterConfig;
sFilterConfig.FilterIdHigh = 0x0000; sFilterConfig.FilterBank = 0;
sFilterConfig.FilterIdLow = 0x0000; sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
sFilterConfig.FilterMaskIdHigh = 0x0000; sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
sFilterConfig.FilterMaskIdLow = 0x0000; sFilterConfig.FilterIdHigh = 0x0000;
sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO0; sFilterConfig.FilterIdLow = 0x0000;
sFilterConfig.FilterActivation = CAN_FILTER_ENABLE; sFilterConfig.FilterMaskIdHigh = 0x0000;
sFilterConfig.FilterMaskIdLow = 0x0000;
if (HAL_CAN_ConfigFilter(hcan, &sFilterConfig) != HAL_OK) sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO0;
return -1; sFilterConfig.FilterActivation = CAN_FILTER_ENABLE;
// 启动 CAN if (HAL_CAN_ConfigFilter(hcan, &sFilterConfig) != HAL_OK)
if (HAL_CAN_Start(hcan) != HAL_OK) return -1;
return -1;
// 启动 CAN
return 0; if (HAL_CAN_Start(hcan) != HAL_OK)
} return -1;
static int hal_can_send(int ch, const can_message_t *msg) return 0;
{ }
if (!msg || msg->length > 8)
return -1; static int hal_can_send(int ch, const can_message_t *msg)
{
CAN_HandleTypeDef *hcan = bsp_can_get_handle(ch)->hcan; if (!msg || msg->length > 8)
if (!hcan) return -1; return -1;
CAN_TxHeaderTypeDef TxHeader; CAN_HandleTypeDef *hcan = bsp_can_get_handle(ch)->hcan;
TxHeader.StdId = msg->id & 0x7FF; if (!hcan) return -1;
TxHeader.ExtId = 0;
TxHeader.IDE = CAN_ID_STD; CAN_TxHeaderTypeDef TxHeader;
TxHeader.RTR = CAN_RTR_DATA; TxHeader.StdId = msg->id & 0x7FF;
TxHeader.DLC = msg->length; TxHeader.ExtId = 0;
TxHeader.TransmitGlobalTime = DISABLE; TxHeader.IDE = CAN_ID_STD;
TxHeader.RTR = CAN_RTR_DATA;
uint32_t TxMailbox; TxHeader.DLC = msg->length;
if (HAL_CAN_AddTxMessage(hcan, &TxHeader, (uint8_t*)msg->data, &TxMailbox) != HAL_OK) TxHeader.TransmitGlobalTime = DISABLE;
return -1;
uint32_t TxMailbox;
// 等待发送完成 if (HAL_CAN_AddTxMessage(hcan, &TxHeader, (uint8_t*)msg->data, &TxMailbox) != HAL_OK)
while (HAL_CAN_IsTxMessagePending(hcan, TxMailbox)) return -1;
;
// 等待发送完成(带超时保护,防止 Bus-Off 死锁)
if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) == 0) { uint32_t tickstart = HAL_GetTick();
/* 发送完成,邮箱变为空闲 */ while (HAL_CAN_IsTxMessagePending(hcan, TxMailbox)) {
} if ((HAL_GetTick() - tickstart) > CAN_TX_TIMEOUT_MS) {
return 0; return -1; // 超时 — CAN 总线异常
} }
}
static int hal_can_recv(int ch, can_message_t *msg)
{ if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) == 0) {
if (!msg) /* 发送完成,邮箱变为空闲 */
return -1; }
return 0;
CAN_HandleTypeDef *hcan = bsp_can_get_handle(ch)->hcan; }
if (!hcan) return -1;
static int hal_can_recv(int ch, can_message_t *msg)
// 检查是否有消息 {
if (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO0) == 0) if (!msg)
return -1; return -1;
CAN_RxHeaderTypeDef RxHeader; CAN_HandleTypeDef *hcan = bsp_can_get_handle(ch)->hcan;
if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader, msg->data) != HAL_OK) if (!hcan) return -1;
return -1;
// 检查是否有消息
msg->id = RxHeader.StdId; if (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO0) == 0)
msg->length = RxHeader.DLC; return -1;
return 0; CAN_RxHeaderTypeDef RxHeader;
} if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader, msg->data) != HAL_OK)
return -1;
static int hal_can_filter_config(int ch, uint8_t filter_id, uint32_t id, uint32_t mask)
{ msg->id = RxHeader.StdId;
CAN_HandleTypeDef *hcan = bsp_can_get_handle(ch)->hcan; msg->length = RxHeader.DLC;
if (!hcan) return -1;
return 0;
CAN_FilterTypeDef sFilterConfig; }
sFilterConfig.FilterBank = filter_id;
sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; static int hal_can_filter_config(int ch, uint8_t filter_id, uint32_t id, uint32_t mask)
sFilterConfig.FilterScale = CAN_FILTERSCALE_16BIT; {
sFilterConfig.FilterIdHigh = (id >> 16) & 0xFFFF; CAN_HandleTypeDef *hcan = bsp_can_get_handle(ch)->hcan;
sFilterConfig.FilterIdLow = id & 0xFFFF; if (!hcan) return -1;
sFilterConfig.FilterMaskIdHigh = (mask >> 16) & 0xFFFF;
sFilterConfig.FilterMaskIdLow = mask & 0xFFFF; CAN_FilterTypeDef sFilterConfig;
sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO0; sFilterConfig.FilterBank = filter_id;
sFilterConfig.FilterActivation = CAN_FILTER_ENABLE; sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
sFilterConfig.SlaveStartFilterBank = 14; sFilterConfig.FilterScale = CAN_FILTERSCALE_16BIT;
sFilterConfig.FilterIdHigh = (id >> 16) & 0xFFFF;
if (HAL_CAN_ConfigFilter(hcan, &sFilterConfig) != HAL_OK) sFilterConfig.FilterIdLow = id & 0xFFFF;
return -1; sFilterConfig.FilterMaskIdHigh = (mask >> 16) & 0xFFFF;
sFilterConfig.FilterMaskIdLow = mask & 0xFFFF;
return 0; sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO0;
} sFilterConfig.FilterActivation = CAN_FILTER_ENABLE;
sFilterConfig.SlaveStartFilterBank = 14;
// 注册接口
static const can_ops_t ops = { if (HAL_CAN_ConfigFilter(hcan, &sFilterConfig) != HAL_OK)
.init = hal_can_init, return -1;
.send = hal_can_send,
.recv = hal_can_recv, return 0;
.filter_config = hal_can_filter_config }
};
// 注册接口
void hal_can_init_all(void) static const can_ops_t ops = {
{ .init = hal_can_init,
can_register_ops(&ops); .send = hal_can_send,
} .recv = hal_can_recv,
.filter_config = hal_can_filter_config
};
void hal_can_init_all(void)
{
can_register_ops(&ops);
}
+13 -17
View File
@@ -764,25 +764,23 @@ static int hal_soft_i2c_wait_ack_frame(int ch) {
if (!bus) if (!bus)
return -1; return -1;
dwt_init(); dwt_init();
sda_out(bus); sda_in(bus); /* switch to input first (high-Z + PUPDR pull-up) */
gpio_set(bus->sda_port, bus->sda_pin); /* drive SDA high via PP */ gpio_set(bus->sda_port, bus->sda_pin); /* ODR=1, safe for future output mode */
sda_in(bus); /* switch to input for reading */ delay_us(1);
gpio_set(bus->sda_port, bus->sda_pin); /* pre-set ODR=1, safe in input mode */ /* clock the 9th SCL pulse so slave can assert ACK */
delay_us(5); gpio_set(bus->scl_port, bus->scl_pin); /* SCL high */
delay_us(3); /* tSU;DAT for slave */
uint32_t start = DWT->CYCCNT; 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)) { 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); gpio_reset(bus->scl_port, bus->scl_pin);
return -2; return -2;
} }
ack = gpio_read(bus->sda_port, bus->sda_pin);
} }
gpio_set(bus->scl_port, bus->scl_pin); delay_us(2);
delay_us(5); gpio_reset(bus->scl_port, bus->scl_pin); /* SCL low; end of ACK bit */
gpio_reset(bus->scl_port, bus->scl_pin);
return 0; return 0;
} }
@@ -832,17 +830,15 @@ static int hal_soft_i2c_read_frame(int ch, int ack) {
uint8_t ret = 0; uint8_t ret = 0;
if (!bus) if (!bus)
return -1; return -1;
sda_out(bus);
gpio_set(bus->sda_port, bus->sda_pin); /* drive SDA high via PP */
sda_in(bus); 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 ++) { for (int i = 0; i < 8; i ++) {
scl_low(bus); scl_low(bus);
delay_us(5); delay_us(5);
scl_high(bus); scl_high(bus);
delay_us(10); /* tSU;DAT — let slave data settle */
ret <<= 1; ret <<= 1;
if (sda_read(bus)) ret ++; if (sda_read(bus)) ret ++;
delay_us(5);
} }
hal_soft_i2c_send_ack_frame(ch, ack); hal_soft_i2c_send_ack_frame(ch, ack);
+170
View File
@@ -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);
}
+6
View File
@@ -0,0 +1,6 @@
#ifndef HAL_UART_H
#define HAL_UART_H
void hal_uart_init_all(void);
#endif
+85
View File
@@ -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);
}
+46
View File
@@ -0,0 +1,46 @@
#ifndef UART_IF_H
#define UART_IF_H
#include <stdint.h>
#include <stddef.h>
/* 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
+34
View File
@@ -0,0 +1,34 @@
#include "i2c_bus.h"
#include "i2c_if.h"
#include <stdbool.h>
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);
}
+17
View File
@@ -0,0 +1,17 @@
#ifndef I2C_BUS_H
#define I2C_BUS_H
#include <stdint.h>
#include <stdbool.h>
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
+76
View File
@@ -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);
}
+23
View File
@@ -0,0 +1,23 @@
#ifndef UART_H
#define UART_H
#include <stdint.h>
#include <stddef.h>
#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
+51
View File
@@ -0,0 +1,51 @@
#include "cmd_parser.h"
#include <string.h>
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;
}
+32
View File
@@ -0,0 +1,32 @@
#ifndef CMD_PARSER_H
#define CMD_PARSER_H
#include <stdint.h>
#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
+24 -23
View File
@@ -1,55 +1,56 @@
#include "pid.h" #include "pid.h"
#include <string.h>
void PIDController_Init(PIDController_t *pid) void pid_init(pid_t *pid)
{ {
if (!pid) return; if (!pid) return;
pid->Kp = 0.0f; pid->Kp = 0.0f;
pid->Ki = 0.0f; pid->Ki = 0.0f;
pid->Kd = 0.0f; pid->Kd = 0.0f;
pid->integral = 0.0f; pid->integral = 0.0f;
pid->prev_error = 0.0f; pid->prev_measurement = 0.0f;
pid->output_max = 204800.0f; // 电机最大速度 (0.01 DPS) 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; if (!pid) return 0.0f;
float error = target - actual; float error = target - actual;
// 比例项 /* 比例项 */
float p_term = pid->Kp * error; float p_term = pid->Kp * error;
// 积分项 (带抗饱和) /* 微分项(测量值微分 — 避免目标突变微分冲击) */
float d_term = pid->Kd * (-(actual - pid->prev_measurement));
pid->prev_measurement = actual;
/* 积分项(带抗饱和限幅) */
pid->integral += error; pid->integral += error;
if (pid->integral > pid->output_max) { if (pid->integral_max > 0.0f) {
pid->integral = pid->output_max; if (pid->integral > pid->integral_max)
} else if (pid->integral < -pid->output_max) { pid->integral = pid->integral_max;
pid->integral = -pid->output_max; else if (pid->integral < -pid->integral_max)
pid->integral = -pid->integral_max;
} }
float i_term = pid->Ki * pid->integral; 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; float output = p_term + i_term + d_term;
// 输出限幅 /* 输出限幅(0=不限幅) */
if (output > pid->output_max) { if (pid->output_max > 0.0f) {
output = pid->output_max; if (output > pid->output_max)
} else if (output < -pid->output_max) { output = pid->output_max;
output = -pid->output_max; else if (output < -pid->output_max)
output = -pid->output_max;
} }
return output; return output;
} }
void PIDController_Reset(PIDController_t *pid) void pid_reset(pid_t *pid)
{ {
if (!pid) return; if (!pid) return;
pid->integral = 0.0f; pid->integral = 0.0f;
pid->prev_error = 0.0f; pid->prev_measurement = 0.0f;
} }
+9 -10
View File
@@ -1,22 +1,21 @@
#ifndef __PID_H__ #ifndef PID_H
#define __PID_H__ #define PID_H
#ifdef __cplusplus #ifdef __cplusplus
extern "C" { extern "C" {
#endif #endif
#include <stdint.h>
typedef struct { typedef struct {
float Kp, Ki, Kd; float Kp, Ki, Kd;
float integral; float integral;
float prev_error; float prev_measurement; /* 上一拍测量值(微分用) */
float output_max; float output_max; /* 输出限幅(0=不限幅) */
} PIDController_t; float integral_max; /* 积分限幅,抗饱和(0=不限幅) */
} pid_t;
void PIDController_Init(PIDController_t *pid); void pid_init(pid_t *pid);
float PIDController_Compute(PIDController_t *pid, float target, float actual); float pid_compute(pid_t *pid, float target, float actual);
void PIDController_Reset(PIDController_t *pid); void pid_reset(pid_t *pid);
#ifdef __cplusplus #ifdef __cplusplus
} }
+339 -320
View File
@@ -1,320 +1,339 @@
/** /**
* @file mf4010v2.c * @file mf4010v2.c
* @brief MF4010V2 无刷电机驱动程序 * @brief MF4010V2 无刷电机驱动程序
* @note 改进版本: * @note 改进版本:
* - 添加错误处理和返回值 * - 添加错误处理和返回值
* - 添加 CAN 超时机制 * - 添加 CAN 超时机制
* - 使用安全的内联函数替代危险宏 * - 使用安全的内联函数替代危险宏
* - 使用 HAL 层解耦硬件依赖 * - 使用 HAL 层解耦硬件依赖
*/ */
#include "mf4010v2.h" #include "mf4010v2.h"
#include "can_if.h" #include "can_if.h"
#include <stdint.h> #include <stdint.h>
#include <stdio.h> #include <stdio.h>
#include <string.h> #include <string.h>
#define WRITE_FLAG(data, flag) do {(data) |= flag; } while(0) /* CAN 接收轮询超时(重试次数) */
#define ERASE_FLAG(data, flag) do {(data) &= ~flag; } while(0) #define MF4010_RECV_RETRIES 2000
/**
* @brief 初始化电机结构体 #define WRITE_FLAG(data, flag) do {(data) |= flag; } while(0)
*/ #define ERASE_FLAG(data, flag) do {(data) &= ~flag; } while(0)
int mf4010v2_init(mf4010v2_t* motor, uint16_t id, int can_ch) /**
{ * @brief 初始化电机结构体
if(motor == NULL) { */
return MF4010_ERR_INVALID_PARAM; int mf4010v2_init(mf4010v2_t* motor, uint16_t id, int can_ch)
} {
if(motor == NULL) {
if(id > 0x7FF) { return MF4010_ERR_INVALID_PARAM;
printf("Error: Invalid CAN ID 0x%03X (must be 0x000-0x7FF)\r\n", id); }
return MF4010_ERR_INVALID_ID;
} if(id > 0x7FF) {
printf("Error: Invalid CAN ID 0x%03X (must be 0x000-0x7FF)\r\n", id);
motor->can_ch = can_ch; return MF4010_ERR_INVALID_ID;
motor->id = id; }
motor->target_angle = 0;
motor->current_angle = 0; motor->can_ch = can_ch;
memset(motor->last_response, 0, sizeof(motor->last_response)); motor->id = id;
can_if_init(motor->can_ch); motor->target_angle = 0;
motor->last_comm_time = 0; motor->current_angle = 0;
motor->error_count = 0; memset(motor->last_response, 0, sizeof(motor->last_response));
motor->success_count = 0; can_if_init(motor->can_ch);
motor->flags = MF4010_FLAG_ENABLED; motor->last_comm_time = 0;
return MF4010_OK; 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]) /**
{ * @brief 发送命令并接收响应(带超时轮询)
can_message_t msg; */
msg.id = motor->id; int mf4010v2_send_command(mf4010v2_t* motor, const uint8_t command[8])
memcpy(msg.data, command, 8); {
msg.length = 8; can_message_t msg;
int ret = can_if_send(motor->can_ch, &msg); int recv_ok = 0;
if (ret != 0) {
motor->error_count++; if (!motor) return MF4010_ERR_INVALID_PARAM;
printf("Error: Failed to send CAN message (ID 0x%03X)\r\n", motor->id);
return MF4010_ERR_CAN_SEND; msg.id = motor->id;
} memcpy(msg.data, command, 8);
can_if_recv(motor->can_ch, &msg); msg.length = 8;
if (msg.id != motor->id) {
motor->error_count++; int ret = can_if_send(motor->can_ch, &msg);
return MF4010_ERR_CAN_RECV; if (ret != 0) {
} motor->error_count++;
memcpy(motor->last_response, msg.data, 8); // printf("Error: Failed to send CAN message (ID 0x%03X)\r\n", motor->id);
motor->success_count++; return MF4010_ERR_CAN_SEND;
return MF4010_OK; }
}
/* 轮询等待电机响应(带超时) */
/** for (int retry = 0; retry < MF4010_RECV_RETRIES; retry++) {
* @brief 设置电机目标角度 (位置闭环) ret = can_if_recv(motor->can_ch, &msg);
* @param motor 电机结构体 if (ret == 0 && msg.id == motor->id) {
* @param angle_0_01deg 目标角度,单位 0.01 度 recv_ok = 1;
*/ break;
int mf4010v2_set_angle(mf4010v2_t* motor, int32_t angle_0_01deg) }
{ }
if (!motor) return MF4010_ERR_INVALID_PARAM;
if (!recv_ok) {
uint8_t cmd[8]; motor->error_count++;
// 使用增量位置控制 (A7 命令) printf("Error: Timeout waiting for motor response (ID 0x%03X)\r\n", motor->id);
// 计算角度增量 return MF4010_ERR_NO_RESPONSE;
int32_t delta = angle_0_01deg - motor->current_angle; }
// 限制单次增量在合理范围内 memcpy(motor->last_response, msg.data, 8);
if (delta > 18000) delta = 18000; // 最多 +180 度 motor->success_count++;
if (delta < -18000) delta = -18000; // 最多 -180 度 return MF4010_OK;
}
cmd_incr_pos(cmd, delta);
motor->target_angle = angle_0_01deg; /**
* @brief 设置电机目标角度 (位置闭环)
return mf4010v2_send_command(motor, cmd); * @param motor 电机结构体
} * @param angle_0_01deg 目标角度,单位 0.01 度
*/
/** int mf4010v2_set_angle(mf4010v2_t* motor, int32_t angle_0_01deg)
* @brief 设置电机速度 (速度闭环) {
* @param motor 电机结构体 if (!motor) return MF4010_ERR_INVALID_PARAM;
* @param speed_0_01dps 目标速度,单位 0.01 DPS
*/ uint8_t cmd[8];
int mf4010v2_set_speed(mf4010v2_t* motor, int32_t speed_0_01dps) // 使用增量位置控制 (A7 命令)
{ // 计算角度增量
if (!motor) return MF4010_ERR_INVALID_PARAM; int32_t delta = angle_0_01deg - motor->current_angle;
uint8_t cmd[8]; // 限制单次增量在合理范围内
cmd_speed_control(cmd, speed_0_01dps); if (delta > 18000) delta = 18000; // 最多 +180 度
return mf4010v2_send_command(motor, cmd); if (delta < -18000) delta = -18000; // 最多 -180 度
}
cmd_incr_pos(cmd, delta);
/** motor->target_angle = angle_0_01deg;
* @brief 启动电机
*/ return mf4010v2_send_command(motor, cmd);
void mf4010v2_run(mf4010v2_t* motor) }
{
if (!motor) return; /**
uint8_t cmd[8] = COMMAND_MOTOR_RUNNING; * @brief 设置电机速度 (速度闭环)
mf4010v2_send_command(motor, cmd); * @param motor 电机结构体
motor->flags |= MF4010_FLAG_RUNNING; * @param speed_0_01dps 目标速度,单位 0.01 DPS
} */
int mf4010v2_set_speed(mf4010v2_t* motor, int32_t speed_0_01dps)
/** {
* @brief 停止电机 if (!motor) return MF4010_ERR_INVALID_PARAM;
*/
void mf4010v2_stop(mf4010v2_t* motor) uint8_t cmd[8];
{ cmd_speed_control(cmd, speed_0_01dps);
if (!motor) return; return mf4010v2_send_command(motor, cmd);
uint8_t cmd[8] = COMMAND_MOTOR_STOP; }
mf4010v2_send_command(motor, cmd);
motor->flags &= ~MF4010_FLAG_RUNNING; /**
} * @brief 启动电机
*/
void mf4010v2_close(mf4010v2_t* motor) { void mf4010v2_run(mf4010v2_t* motor)
if (!motor) return; {
uint8_t cmd[8] = COMMAND_MOTOR_CLOSE; if (!motor) return;
int ret = mf4010v2_send_command(motor, cmd); uint8_t cmd[8] = COMMAND_MOTOR_RUNNING;
if (ret == MF4010_ERR_CAN_SEND) { mf4010v2_send_command(motor, cmd);
WRITE_FLAG(motor->flags, MF4010_FLAG_FAULT); motor->flags |= MF4010_FLAG_RUNNING;
return; }
} else if (ret == MF4010_ERR_CAN_RECV) {
WRITE_FLAG(motor->flags, MF4010_FLAG_FAULT); /**
return; * @brief 停止电机
} else { */
if (0 == memcmp(cmd, motor->last_response, sizeof(cmd)/sizeof(cmd[0]))) { void mf4010v2_stop(mf4010v2_t* motor)
ERASE_FLAG(motor->flags, MF4010_FLAG_ENABLED); {
return ; if (!motor) return;
} else { uint8_t cmd[8] = COMMAND_MOTOR_STOP;
WRITE_FLAG(motor->flags, MF4010_FLAG_FAULT); mf4010v2_send_command(motor, cmd);
return ; motor->flags &= ~MF4010_FLAG_RUNNING;
} }
}
void mf4010v2_close(mf4010v2_t* motor) {
} if (!motor) return;
uint8_t cmd[8] = COMMAND_MOTOR_CLOSE;
int mf4010v2_read_pid_param(mf4010v2_t* motor, pid_param* pid) { int ret = mf4010v2_send_command(motor, cmd);
if (!motor) return MF4010_ERR_INVALID_PARAM; if (ret == MF4010_ERR_CAN_SEND) {
uint8_t cmd[8] = {0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; WRITE_FLAG(motor->flags, MF4010_FLAG_FAULT);
mf4010v2_send_command(motor, cmd); return;
pid->angle_kp = motor->last_response[2]; } else if (ret == MF4010_ERR_CAN_RECV) {
pid->angle_ki = motor->last_response[3]; WRITE_FLAG(motor->flags, MF4010_FLAG_FAULT);
pid->speed_kp = motor->last_response[4]; return;
pid->speed_ki = motor->last_response[5]; } else {
pid->iq_kp = motor->last_response[6]; if (0 == memcmp(cmd, motor->last_response, sizeof(cmd)/sizeof(cmd[0]))) {
pid->iq_ki = motor->last_response[7]; ERASE_FLAG(motor->flags, MF4010_FLAG_ENABLED);
return MF4010_OK; return ;
} } else {
/* ============================================================================ WRITE_FLAG(motor->flags, MF4010_FLAG_FAULT);
* 命令生成函数实现 return ;
* ============================================================================ */ }
}
void cmd_torque_control(uint8_t* buf, int16_t iq)
{ }
buf[0] = 0xA1;
buf[1] = 0x00; int mf4010v2_read_pid_param(mf4010v2_t* motor, pid_param* pid) {
buf[2] = 0x00; if (!motor) return MF4010_ERR_INVALID_PARAM;
buf[3] = 0x00; uint8_t cmd[8] = {0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
buf[4] = (uint8_t)(iq & 0xFF); mf4010v2_send_command(motor, cmd);
buf[5] = (uint8_t)((iq >> 8) & 0xFF); pid->angle_kp = motor->last_response[2];
buf[6] = 0x00; pid->angle_ki = motor->last_response[3];
buf[7] = 0x00; 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_speed_control(uint8_t* buf, int32_t speed) }
{ /* ============================================================================
buf[0] = 0xA2; * 命令生成函数实现
buf[1] = 0x00; * ============================================================================ */
buf[2] = 0x00;
buf[3] = 0x00; void cmd_torque_control(uint8_t* buf, int16_t iq)
buf[4] = (uint8_t)(speed & 0xFF); {
buf[5] = (uint8_t)((speed >> 8) & 0xFF); buf[0] = 0xA1;
buf[6] = (uint8_t)((speed >> 16) & 0xFF); buf[1] = 0x00;
buf[7] = (uint8_t)((speed >> 24) & 0xFF); buf[2] = 0x00;
} buf[3] = 0x00;
buf[4] = (uint8_t)(iq & 0xFF);
buf[5] = (uint8_t)((iq >> 8) & 0xFF);
buf[6] = 0x00;
void cmd_mult_ring_pos(uint8_t* buf, int32_t angleControl) { buf[7] = 0x00;
buf[0] = 0xA3; }
buf[1] = 0x00;
buf[2] = 0x00;
buf[3] = 0x00;
buf[4] = *(uint8_t *)(&angleControl); void cmd_speed_control(uint8_t* buf, int32_t speed)
buf[5] = *((uint8_t *)(&angleControl)+1); {
buf[6] = *((uint8_t *)(&angleControl)+2); buf[0] = 0xA2;
buf[7] = *((uint8_t *)(&angleControl)+3); buf[1] = 0x00;
} buf[2] = 0x00;
buf[3] = 0x00;
void cmd_mult_ring_pos_with_speed(uint8_t* buf, uint16_t maxSpeed, int32_t angleControl) { buf[4] = (uint8_t)(speed & 0xFF);
buf[0] = 0xA4; buf[5] = (uint8_t)((speed >> 8) & 0xFF);
buf[1] = 0x00; buf[6] = (uint8_t)((speed >> 16) & 0xFF);
buf[2] = *(uint8_t *)(&maxSpeed); buf[7] = (uint8_t)((speed >> 24) & 0xFF);
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_mult_ring_pos(uint8_t* buf, int32_t angleControl) {
} buf[0] = 0xA3;
buf[1] = 0x00;
void cmd_single_ring_pos(uint8_t* buf, uint8_t direction, uint32_t angle) buf[2] = 0x00;
{ buf[3] = 0x00;
buf[0] = 0xA5; buf[4] = *(uint8_t *)(&angleControl);
buf[1] = direction & 0x01; buf[5] = *((uint8_t *)(&angleControl)+1);
buf[2] = 0x00; buf[6] = *((uint8_t *)(&angleControl)+2);
buf[3] = 0x00; buf[7] = *((uint8_t *)(&angleControl)+3);
buf[4] = (uint8_t)(angle & 0xFF); }
buf[5] = (uint8_t)((angle >> 8) & 0xFF);
buf[6] = (uint8_t)((angle >> 16) & 0xFF); void cmd_mult_ring_pos_with_speed(uint8_t* buf, uint16_t maxSpeed, int32_t angleControl) {
buf[7] = (uint8_t)((angle >> 24) & 0xFF); buf[0] = 0xA4;
} buf[1] = 0x00;
buf[2] = *(uint8_t *)(&maxSpeed);
void cmd_single_ring_pos_with_speed(uint8_t* buf, uint8_t spinDirection, uint16_t maxSpeed, uint32_t angleControl) buf[3] = *((uint8_t *)(&maxSpeed)+1);
{ buf[4] = *(uint8_t *)(&angleControl);
buf[0] = 0xA6; buf[5] = *((uint8_t *)(&angleControl)+1);
buf[1] = spinDirection & 0x01; buf[6] = *((uint8_t *)(&angleControl)+2);
buf[2] = (uint8_t)(maxSpeed & 0xFF); buf[7] = *((uint8_t *)(&angleControl)+3);
buf[3] = (uint8_t)((maxSpeed >> 8) & 0xFF); }
buf[4] = (uint8_t)(angleControl & 0xFF);
buf[5] = (uint8_t)((angleControl >> 8) & 0xFF); void cmd_single_ring_pos(uint8_t* buf, uint8_t direction, uint32_t angle)
buf[6] = (uint8_t)((angleControl >> 16) & 0xFF); {
buf[7] = (uint8_t)((angleControl >> 24) & 0xFF); buf[0] = 0xA5;
} buf[1] = direction & 0x01;
buf[2] = 0x00;
void cmd_incr_pos(uint8_t* buf, int32_t angle_increment) buf[3] = 0x00;
{ buf[4] = (uint8_t)(angle & 0xFF);
buf[0] = 0xA7; buf[5] = (uint8_t)((angle >> 8) & 0xFF);
buf[1] = 0x00; buf[6] = (uint8_t)((angle >> 16) & 0xFF);
buf[2] = 0x00; buf[7] = (uint8_t)((angle >> 24) & 0xFF);
buf[3] = 0x00; }
buf[4] = (uint8_t)(angle_increment & 0xFF);
buf[5] = (uint8_t)((angle_increment >> 8) & 0xFF); void cmd_single_ring_pos_with_speed(uint8_t* buf, uint8_t spinDirection, uint16_t maxSpeed, uint32_t angleControl)
buf[6] = (uint8_t)((angle_increment >> 16) & 0xFF); {
buf[7] = (uint8_t)((angle_increment >> 24) & 0xFF); buf[0] = 0xA6;
} buf[1] = spinDirection & 0x01;
buf[2] = (uint8_t)(maxSpeed & 0xFF);
void cmd_incr_pos_with_speed(uint8_t* buf, uint16_t max_speed, int32_t angle_increment) { buf[3] = (uint8_t)((maxSpeed >> 8) & 0xFF);
buf[0] = 0xA8; buf[4] = (uint8_t)(angleControl & 0xFF);
buf[1] = 0x00; buf[5] = (uint8_t)((angleControl >> 8) & 0xFF);
buf[2] = (uint8_t)(max_speed & 0xFF); buf[6] = (uint8_t)((angleControl >> 16) & 0xFF);
buf[3] = (uint8_t)((max_speed >> 8) & 0xFF); buf[7] = (uint8_t)((angleControl >> 24) & 0xFF);
buf[4] = (uint8_t)(angle_increment & 0xFF); }
buf[5] = (uint8_t)((angle_increment >> 8) & 0xFF);
buf[6] = (uint8_t)((angle_increment >> 16) & 0xFF); void cmd_incr_pos(uint8_t* buf, int32_t angle_increment)
buf[7] = (uint8_t)((angle_increment >> 24) & 0xFF); {
} buf[0] = 0xA7;
buf[1] = 0x00;
void cmd_write_pid_params(uint8_t* buf, uint8_t location, buf[2] = 0x00;
uint8_t angle_kp, uint8_t angle_ki, buf[3] = 0x00;
uint8_t speed_kp, uint8_t speed_ki, buf[4] = (uint8_t)(angle_increment & 0xFF);
uint8_t iq_kp, uint8_t iq_ki) buf[5] = (uint8_t)((angle_increment >> 8) & 0xFF);
{ buf[6] = (uint8_t)((angle_increment >> 16) & 0xFF);
buf[0] = location; buf[7] = (uint8_t)((angle_increment >> 24) & 0xFF);
buf[1] = 0x00; }
buf[2] = angle_kp;
buf[3] = angle_ki; void cmd_incr_pos_with_speed(uint8_t* buf, uint16_t max_speed, int32_t angle_increment) {
buf[4] = speed_kp; buf[0] = 0xA8;
buf[5] = speed_ki; buf[1] = 0x00;
buf[6] = iq_kp; buf[2] = (uint8_t)(max_speed & 0xFF);
buf[7] = iq_ki; 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);
* ============================================================================ */ }
bool mf4010v2_is_connected(mf4010v2_t* motor) void cmd_write_pid_params(uint8_t* buf, uint8_t location,
{ uint8_t angle_kp, uint8_t angle_ki,
return (motor != NULL) && (motor->flags & MF4010_FLAG_CONNECTED); uint8_t speed_kp, uint8_t speed_ki,
} uint8_t iq_kp, uint8_t iq_ki)
{
bool mf4010v2_is_running(mf4010v2_t* motor) buf[0] = location;
{ buf[1] = 0x00;
return (motor != NULL) && (motor->flags & MF4010_FLAG_RUNNING); buf[2] = angle_kp;
} buf[3] = angle_ki;
buf[4] = speed_kp;
bool mf4010v2_is_fault(mf4010v2_t* motor) buf[5] = speed_ki;
{ buf[6] = iq_kp;
return (motor != NULL) && (motor->flags & MF4010_FLAG_FAULT); buf[7] = iq_ki;
} }
uint16_t mf4010v2_get_error_count(mf4010v2_t* motor) /* ============================================================================
{ * 辅助函数实现
return (motor != NULL) ? motor->error_count : 0; * ============================================================================ */
}
bool mf4010v2_is_connected(mf4010v2_t* motor)
void mf4010v2_reset_errors(mf4010v2_t* motor) {
{ return (motor != NULL) && (motor->flags & MF4010_FLAG_CONNECTED);
if(motor != NULL) { }
motor->error_count = 0;
} bool mf4010v2_is_running(mf4010v2_t* motor)
} {
return (motor != NULL) && (motor->flags & MF4010_FLAG_RUNNING);
bool mf4010v2_response_has_error(mf4010v2_t* motor) }
{
return (motor != NULL) && ((motor->last_response[0] & 0x80) != 0); bool mf4010v2_is_fault(mf4010v2_t* motor)
} {
return (motor != NULL) && (motor->flags & MF4010_FLAG_FAULT);
uint8_t mf4010v2_get_error_code(mf4010v2_t* motor) }
{
return (motor != NULL) ? (motor->last_response[0] & 0x7F) : 0; 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;
}
+34
View File
@@ -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;
}
+22
View File
@@ -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
@@ -0,0 +1,46 @@
#include "moving_average.h"
#include <string.h>
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));
}
@@ -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
+46
View File
@@ -0,0 +1,46 @@
#include "notch.h"
#include <math.h>
#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;
}
+24
View File
@@ -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
@@ -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;
}
@@ -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
+1 -1
Submodule release updated: d31fb8a669...07f1a2efda.