[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:
1 parent
75966b174d
commit
cf934cf6b7
46 files changed
+1823
-1582
No files matched your search
@@ -1,600 +0,0 @@
|
||||
# CloudPlant — 架构审查与改进计划
|
||||
|
||||
> 生成日期:2026-05-12
|
||||
> 基于完整的代码级架构审查与多轮技术讨论
|
||||
|
||||
---
|
||||
|
||||
## 目录
|
||||
|
||||
1. [项目现状](#1-项目现状)
|
||||
2. [架构合规性分析](#2-架构合规性分析)
|
||||
3. [控制系统重构](#3-控制系统重构)
|
||||
4. [传感器与滤波器设计](#4-传感器与滤波器设计)
|
||||
5. [UART 通信子系统](#5-uart-通信子系统)
|
||||
6. [FPU 与性能确认](#6-fpu-与性能确认)
|
||||
7. [健壮性与安全](#7-健壮性与安全)
|
||||
8. [代码质量改进](#8-代码质量改进)
|
||||
9. [调试与可观测性](#9-调试与可观测性)
|
||||
10. [构建与工具链](#10-构建与工具链)
|
||||
11. [改进优先级矩阵](#11-改进优先级矩阵)
|
||||
|
||||
---
|
||||
|
||||
## 1. 项目现状
|
||||
|
||||
### 1.1 基本信息
|
||||
|
||||
| 属性 | 值 |
|
||||
|---|---|
|
||||
| 项目名 | CloudPlant |
|
||||
| 功能 | 二轴云台增稳控制器 |
|
||||
| MCU | STM32F407 (Cortex-M4, 168MHz) |
|
||||
| RTOS | FreeRTOS (CMSIS-RTOS v2) |
|
||||
| 构建系统 | CMake + STM32CubeMX |
|
||||
| 传感器 | JY901P IMU (Software I2C) |
|
||||
| 执行器 | 2× MF4010V2 无刷电机 (CAN2) |
|
||||
| 调试输出 | USART1 (printf) |
|
||||
|
||||
### 1.2 分层架构
|
||||
|
||||
```
|
||||
APP ─── 业务逻辑 / 任务编排
|
||||
↓
|
||||
Module ─── 设备抽象 / 功能封装
|
||||
↓
|
||||
Interface ─── 接口抽象 (ops 函数指针表)
|
||||
↓
|
||||
HAL ─── 外设操作 (STM32 HAL 调用)
|
||||
↓
|
||||
BSP ─── 硬件描述 (GPIO 映射 / 句柄映射)
|
||||
↓
|
||||
STM32 HAL / 寄存器
|
||||
```
|
||||
|
||||
| 层 | 目录 | 职责 |
|
||||
|---|---|---|
|
||||
| APP | `app/` | 初始化、传感器采集、PID 云台增稳、监控 |
|
||||
| Module | `modules/` | LED / Motor / CAN / PID / SoftI2C 功能封装 |
|
||||
| Interface | `interfaces/` | ops 结构定义 + 注册 + API 转发 |
|
||||
| HAL | `hal/stm32f4/` | 外设寄存器操作 |
|
||||
| BSP | `bsp/stm32f4/` | 引脚/句柄静态映射表 |
|
||||
|
||||
### 1.3 FreeRTOS 任务
|
||||
|
||||
| 任务 | 优先级 | 周期 | 功能 |
|
||||
|---|---|---|---|
|
||||
| `sensorTask` | High | ~200Hz(循环) | JY901P IMU 数据采集 |
|
||||
| `controlTask` | Normal | 100Hz(期望) | PID 控制 → 电机速度指令 |
|
||||
| `monitorTask` | Low | 按需/1Hz | 系统监控、UART 命令处理 |
|
||||
|
||||
---
|
||||
|
||||
## 2. 架构合规性分析
|
||||
|
||||
### 2.1 合规部分 ✅
|
||||
|
||||
- BSP 层所有 5 个文件仅含静态映射表和 getter 函数,无逻辑分支
|
||||
- Interface 层使用 ops 函数指针表 + `xxx_register_ops()` 注入实现
|
||||
- Module 层通过 Interface 访问硬件,无直接 HAL 调用
|
||||
- PID 模块 (`modules/control/pid/`) 纯数学运算,零硬件依赖
|
||||
- CAN / I2C 设备模块通过 `can_if` / `i2c_if` 接口通信
|
||||
|
||||
### 2.2 违规项 ❌
|
||||
|
||||
| # | 违规 | 严重度 | 位置 | 修复方案 |
|
||||
|---|---|---|---|---|
|
||||
| 1 | APP 直接包含 HAL 头文件 | FATAL | `app/app.h:2-8` | 删除 `hal_*.h` 引用,通过 Module 初始化 |
|
||||
| 2 | APP 直接调用 HAL 初始化函数 | FATAL | `app/app_init.c:7-11` | 将 HAL 初始化下沉到接口注册阶段 |
|
||||
| 3 | HAL 层函数名含业务语义 (LED) | ERROR | `hal/stm32f4/led/hal_led.c` | 重命名为 `hal_gpio_led_*` 或拆分语义到 Module |
|
||||
| 4 | `control_task.c` 和 `pid.c` 存在两套 PID 实现 | WARNING | 两个文件 | 统一使用 `modules/control/pid/pid.c` |
|
||||
| 5 | `sensor_task.c` 内联 I2C 时序函数 | WARNING | `sensor_task.c` | 移至 `modules/bus/soft_i2c/` 或 HAL 层 |
|
||||
|
||||
---
|
||||
|
||||
## 3. 控制系统重构
|
||||
|
||||
### 3.1 控制模式:从位置模式切换到速度模式(级联 PID)
|
||||
|
||||
**当前问题**:使用 `cmd_incr_pos` (0xA7) 增量位置模式,导致两层 PID 串行叠加,延迟翻倍。
|
||||
|
||||
**推荐方案**:位置外环 (MCU) + 速度内环 (MF4010V2 驱动器)。
|
||||
|
||||
```text
|
||||
IMU 角度 → MCU 位置 PID → 速度指令 → MF4010V2 速度 PID → 电流 PID → 电机
|
||||
```
|
||||
|
||||
```c
|
||||
// 控制循环伪代码
|
||||
while (1) {
|
||||
float roll = fAngle[0];
|
||||
float pitch = fAngle[1];
|
||||
|
||||
// 位置 PID:角度误差 → 速度指令 (DPS)
|
||||
float yaw_speed = PIDController_Compute(&pos_pid_roll, 0.0f, roll);
|
||||
float pitch_speed = PIDController_Compute(&pos_pid_pitch, 0.0f, pitch);
|
||||
|
||||
// 改用速度模式
|
||||
mf4010v2_set_speed(&motor_yaw, (int32_t)(yaw_speed * 100.0f));
|
||||
mf4010v2_set_speed(&motor_pitch, (int32_t)(pitch_speed * 100.0f));
|
||||
|
||||
vTaskDelayUntil(&xLastWakeTime, xPeriod);
|
||||
}
|
||||
```
|
||||
|
||||
### 3.2 控制周期:`vTaskDelayUntil` 替代 `osDelay`
|
||||
|
||||
**当前问题**:`osDelay(10)` 导致周期漂移累积累积。
|
||||
|
||||
```c
|
||||
// 旧(有抖动)
|
||||
osDelay(pdMS_TO_TICKS(10));
|
||||
|
||||
// 新(精确 100Hz)
|
||||
TickType_t xLastWakeTime = xTaskGetTickCount();
|
||||
vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(10));
|
||||
```
|
||||
|
||||
### 3.3 带宽分配
|
||||
|
||||
```
|
||||
JY901P 内部低通 → 256Hz (WitSetBandwidth)
|
||||
IMU 角度读取 → 200Hz (只读 Roll/Pitch/Yaw,3 个寄存器)
|
||||
控制环频率 → 100Hz (vTaskDelayUntil)
|
||||
有效控制带宽 → ~20Hz (抑制手持晃动)
|
||||
printf 输出 → 1Hz (不在控制环内打印)
|
||||
```
|
||||
|
||||
```c
|
||||
// 初始化时设置带宽
|
||||
WitInit(WIT_PROTOCOL_I2C, addr);
|
||||
WitSetBandwidth(BANDWIDTH_256HZ);
|
||||
|
||||
// 循环中只读角度寄存器
|
||||
WitReadReg(Roll, 3); // 仅 3 个寄存器,约 0.5ms
|
||||
```
|
||||
|
||||
### 3.4 PID 参数起始值
|
||||
|
||||
```c
|
||||
// 位置 PID 参数(输出为速度 DPS)
|
||||
pos_pid_roll.Kp = 80.0f; // P: 角度误差 → 速度指令
|
||||
pos_pid_roll.Ki = 0.5f; // I: 消除稳态误差
|
||||
pos_pid_roll.Kd = 30.0f; // D: 抑制过冲
|
||||
pos_pid_roll.output_max = 36000.0f; // 最大速度 360 DPS
|
||||
|
||||
pos_pid_pitch.Kp = 80.0f;
|
||||
pos_pid_pitch.Ki = 0.5f;
|
||||
pos_pid_pitch.Kd = 30.0f;
|
||||
pos_pid_pitch.output_max = 18000.0f; // Pitch 轴限幅 180 DPS
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 4. 传感器与滤波器设计
|
||||
|
||||
### 4.1 一阶低通滤波器(必加)
|
||||
|
||||
```c
|
||||
typedef struct {
|
||||
float alpha, prev_output;
|
||||
} LowPassFilter_t;
|
||||
|
||||
void LPF_Init(LowPassFilter_t *lpf, float cutoff_hz, float sample_rate_hz) {
|
||||
float dt = 1.0f / sample_rate_hz;
|
||||
float RC = 1.0f / (2.0f * 3.14159f * cutoff_hz);
|
||||
lpf->alpha = dt / (RC + dt);
|
||||
lpf->prev_output = 0.0f;
|
||||
}
|
||||
|
||||
static inline float LPF_Update(LowPassFilter_t *lpf, float input) {
|
||||
float output = lpf->alpha * input + (1.0f - lpf->alpha) * lpf->prev_output;
|
||||
lpf->prev_output = output;
|
||||
return output;
|
||||
}
|
||||
```
|
||||
|
||||
**使用**:
|
||||
|
||||
```c
|
||||
LowPassFilter_t lpf_roll, lpf_pitch;
|
||||
LPF_Init(&lpf_roll, 25.0f, 200.0f); // 25Hz 截止 @ 200Hz 采样
|
||||
LPF_Init(&lpf_pitch, 25.0f, 200.0f);
|
||||
float filt_roll = LPF_Update(&lpf_roll, fAngle[0]);
|
||||
```
|
||||
|
||||
### 4.2 陷波滤波器(电机振动抑制)
|
||||
|
||||
用于滤除特定频率的电机机械振动。双二阶 IIR 结构,仅衰减目标频率。
|
||||
|
||||
```c
|
||||
typedef struct {
|
||||
float b0, b1, b2, a1, a2;
|
||||
float x1, x2, y1, y2;
|
||||
} NotchFilter_t;
|
||||
|
||||
void Notch_Init(NotchFilter_t *nf, float center_hz, float bandwidth_hz, float sample_rate);
|
||||
float Notch_Update(NotchFilter_t *nf, float input);
|
||||
```
|
||||
|
||||
**使用**:实测电机运行时的 FFT 频谱,确定振动频率后再设置陷波频率。
|
||||
|
||||
### 4.3 滤波器串联顺序
|
||||
|
||||
```c
|
||||
// LPF 先做宽带降噪,Notch 再做定点消除
|
||||
float raw = fAngle[0];
|
||||
float lpf = LPF_Update(&lpf_roll, raw);
|
||||
float clean = Notch_Update(¬ch_roll, lpf);
|
||||
float cmd = PIDController_Compute(&pos_pid, target, clean);
|
||||
```
|
||||
|
||||
### 4.4 调试顺序
|
||||
|
||||
```
|
||||
步骤 1:不加滤波器,调稳 PID
|
||||
步骤 2:加一阶 LPF (25Hz),消除高频抖动
|
||||
步骤 3:电机跑起来,上位机 FFT 分析角度信号
|
||||
步骤 4:加陷波滤波器,精准消除振动频率
|
||||
步骤 5:微调 PID 参数
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 5. UART 通信子系统
|
||||
|
||||
### 5.1 架构
|
||||
|
||||
```
|
||||
USART1 RX IRQ ─→ StreamBuffer ─→ monitor_task ─→ CmdProcess()
|
||||
TX ─────── Mutex ──── printf / 命令响应
|
||||
```
|
||||
|
||||
### 5.2 Stream Buffer 创建
|
||||
|
||||
```c
|
||||
// freertos.c 中
|
||||
StreamBufferHandle_t uart_rx_stream;
|
||||
uart_rx_stream = xStreamBufferCreate(256, 1); // 256 字节,触发阈值 1
|
||||
```
|
||||
|
||||
### 5.3 中断回调(仅收发信号,不做业务)
|
||||
|
||||
```c
|
||||
extern StreamBufferHandle_t uart_rx_stream;
|
||||
extern uint8_t uart_rx_byte;
|
||||
|
||||
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
|
||||
if (huart->Instance == USART1) {
|
||||
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
|
||||
xStreamBufferSendFromISR(uart_rx_stream, &uart_rx_byte, 1, &xHigherPriorityTaskWoken);
|
||||
HAL_UART_Receive_IT(&huart1, &uart_rx_byte, 1);
|
||||
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
### 5.4 启动中断接收
|
||||
|
||||
```c
|
||||
// HAL 初始化完成后
|
||||
uint8_t uart_rx_byte;
|
||||
HAL_UART_Receive_IT(&huart1, &uart_rx_byte, 1);
|
||||
```
|
||||
|
||||
### 5.5 monitor_task 命令处理
|
||||
|
||||
```c
|
||||
void monitor_task(void) {
|
||||
uint8_t rx_buf[32];
|
||||
while (1) {
|
||||
size_t n = xStreamBufferReceive(uart_rx_stream, rx_buf, sizeof(rx_buf),
|
||||
pdMS_TO_TICKS(100));
|
||||
for (size_t i = 0; i < n; i++) {
|
||||
CopeCmdData(rx_buf[i]);
|
||||
}
|
||||
if (n > 0) CmdProcess();
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
### 5.6 TX 互斥锁(printf 与命令响应共享 USART1)
|
||||
|
||||
```c
|
||||
SemaphoreHandle_t uart_tx_mutex;
|
||||
uart_tx_mutex = xSemaphoreCreateMutex();
|
||||
|
||||
#define SAFE_PRINTF(fmt, ...) \
|
||||
do { \
|
||||
xSemaphoreTake(uart_tx_mutex, portMAX_DELAY); \
|
||||
printf(fmt, ##__VA_ARGS__); \
|
||||
xSemaphoreGive(uart_tx_mutex); \
|
||||
} while(0)
|
||||
```
|
||||
|
||||
### 5.7 命令集
|
||||
|
||||
| 命令 | 功能 |
|
||||
|---|---|
|
||||
| `a` | 加速度校准 |
|
||||
| `m` | 磁场校准开始 |
|
||||
| `e` | 磁场校准结束 |
|
||||
| `u` / `U` | 设置 IMU 带宽 5Hz / 256Hz |
|
||||
| `B` / `b` | UART 波特率 115200 / 9600 |
|
||||
| `r` | 查询电机故障状态 |
|
||||
| `p` | 打印当前角度 |
|
||||
| `d` | Dump 环形日志(预留) |
|
||||
|
||||
> 将 `sensor_task.c` 中的 `CopeCmdData()`/`CmdProcess()` 移至 `monitor_task`,避免死代码。
|
||||
|
||||
---
|
||||
|
||||
## 6. FPU 与性能确认
|
||||
|
||||
### 6.1 编译选项(当前缺失)
|
||||
|
||||
在 `CMakeLists.txt` 中补充:
|
||||
|
||||
```cmake
|
||||
target_compile_options(${CMAKE_PROJECT_NAME} PRIVATE
|
||||
-mcpu=cortex-m4
|
||||
-mthumb
|
||||
-mfloat-abi=hard
|
||||
-mfpu=fpv4-sp-d16
|
||||
)
|
||||
|
||||
target_link_options(${CMAKE_PROJECT_NAME} PRIVATE
|
||||
-mcpu=cortex-m4
|
||||
-mthumb
|
||||
-mfloat-abi=hard
|
||||
-mfpu=fpv4-sp-d16
|
||||
)
|
||||
```
|
||||
|
||||
### 6.2 验证 FPU 是否生效
|
||||
|
||||
```bash
|
||||
arm-none-eabi-objdump -d build/CloudPlant.elf | grep -c 'vadd\|vmul\|vldr\|vstr'
|
||||
# 输出 > 0 即 FPU 生效
|
||||
```
|
||||
|
||||
### 6.3 性能瓶颈
|
||||
|
||||
`printf` 格式化 `float` 会触发软件浮点库(`__aeabi_f2d` 等),是当前最大性能瓶颈。PID 计算中的 `float` 加减乘除由 FPU 直接处理(1 周期),无需改为定点数。
|
||||
|
||||
---
|
||||
|
||||
## 7. 健壮性与安全
|
||||
|
||||
### 7.1 看门狗
|
||||
|
||||
```c
|
||||
IWDG_HandleTypeDef hiwdg;
|
||||
hiwdg.Instance = IWDG;
|
||||
hiwdg.Init.Prescaler = IWDG_PRESCALER_64;
|
||||
hiwdg.Init.Reload = 625; // ~1s
|
||||
HAL_IWDG_Init(&hiwdg);
|
||||
|
||||
// monitor_task 中定期喂狗
|
||||
HAL_IWDG_Refresh(&hiwdg);
|
||||
```
|
||||
|
||||
### 7.2 任务栈溢出检测
|
||||
|
||||
```c
|
||||
// FreeRTOSConfig.h
|
||||
#define configCHECK_FOR_STACK_OVERFLOW 2
|
||||
#define configUSE_MALLOC_FAILED_HOOK 1
|
||||
|
||||
void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName) {
|
||||
// 立即停机 → 闪灯 → 等待看门狗复位
|
||||
mf4010v2_stop(&motor_yaw);
|
||||
mf4010v2_stop(&motor_pitch);
|
||||
while (1) { led_toggle(&led0); HAL_Delay(200); }
|
||||
}
|
||||
```
|
||||
|
||||
### 7.3 CAN 发送超时
|
||||
|
||||
```c
|
||||
// hal_can.c 当前是忙等 → 改为带超时
|
||||
uint32_t timeout = HAL_GetTick() + 50;
|
||||
while (HAL_CAN_IsTxMessagePending(hcan, TxMailbox)) {
|
||||
if (HAL_GetTick() > timeout) {
|
||||
HAL_CAN_AbortTxRequest(hcan, TxMailbox);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
### 7.4 电机限位保护
|
||||
|
||||
```c
|
||||
if (pitch_cmd > 9000) pitch_cmd = 9000; // +90°
|
||||
if (pitch_cmd < -9000) pitch_cmd = -9000;
|
||||
if (yaw_cmd > 18000) yaw_cmd = 18000; // ±180°
|
||||
if (yaw_cmd < -18000) yaw_cmd = -18000;
|
||||
```
|
||||
|
||||
### 7.5 IMU 离线检测
|
||||
|
||||
```c
|
||||
volatile uint32_t imu_last_update; // sensor_task 更新
|
||||
|
||||
// control_task 检查
|
||||
if (HAL_GetTick() - imu_last_update > 200) {
|
||||
mf4010v2_stop(&motor_yaw);
|
||||
mf4010v2_stop(&motor_pitch);
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 8. 代码质量改进
|
||||
|
||||
### 8.1 统一 PID 实现
|
||||
|
||||
删除 `control_task.c` 中的手写 `PID_t`,统一使用 `modules/control/pid/pid.c` 中的 `PIDController_t`。
|
||||
|
||||
### 8.2 配置常量集中管理
|
||||
|
||||
创建 `config.h`:
|
||||
|
||||
```c
|
||||
#define CONFIG_CONTROL_FREQ_HZ 100
|
||||
#define CONFIG_IMU_SAMPLE_RATE_HZ 200
|
||||
#define CONFIG_LPF_CUTOFF_HZ 25.0f
|
||||
#define CONFIG_MOTOR_YAW_CAN_ID 0x141
|
||||
#define CONFIG_MOTOR_PITCH_CAN_ID 0x142
|
||||
#define CONFIG_MOTOR_CAN_CHANNEL 1
|
||||
```
|
||||
|
||||
### 8.3 统一错误码
|
||||
|
||||
```c
|
||||
typedef enum {
|
||||
ERR_OK = 0,
|
||||
ERR_PARAM = -1,
|
||||
ERR_TIMEOUT = -2,
|
||||
ERR_BUS = -3,
|
||||
ERR_NOT_INIT = -4,
|
||||
ERR_HARDWARE = -5,
|
||||
} error_code_t;
|
||||
```
|
||||
|
||||
### 8.4 PID 参数持久化
|
||||
|
||||
在 Flash 末扇区存储校准数据:
|
||||
|
||||
```c
|
||||
#define CALIB_FLASH_ADDR 0x08060000
|
||||
|
||||
typedef struct {
|
||||
uint32_t magic; // 0xCA11B007
|
||||
float roll_kp, roll_ki, roll_kd;
|
||||
float pitch_kp, pitch_ki, pitch_kd;
|
||||
float imu_offset[3];
|
||||
uint32_t crc;
|
||||
} calibration_t;
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 9. 调试与可观测性
|
||||
|
||||
### 9.1 环形数据日志
|
||||
|
||||
```c
|
||||
typedef struct {
|
||||
uint32_t tick;
|
||||
float roll, pitch, yaw;
|
||||
int32_t motor_yaw_pos, motor_pitch_pos;
|
||||
float pid_output_roll, pid_output_pitch;
|
||||
} log_entry_t;
|
||||
|
||||
log_entry_t flight_log[1000]; // 100Hz × 10s
|
||||
volatile int log_idx;
|
||||
```
|
||||
|
||||
通过串口命令 `d` 触发 dump。
|
||||
|
||||
### 9.2 运行时变量监控协议
|
||||
|
||||
简易二进制协议,支持在线调 PID 参数:
|
||||
|
||||
```
|
||||
帧格式: 0xAA 0xBB [cmd:1] [addr:2] [len:2] [data:N] [crc:1]
|
||||
cmd: 0x01=读 0x02=写
|
||||
```
|
||||
|
||||
### 9.3 LED 状态码
|
||||
|
||||
| LED0 | LED1 | 含义 |
|
||||
|---|---|---|
|
||||
| 快闪 | 灭 | 正常运行 |
|
||||
| 常亮 | 灭 | 电机故障 |
|
||||
| 灭 | 快闪 | CAN 通信错误 |
|
||||
| 灭 | 常亮 | IMU 离线 |
|
||||
| 交替闪 | 交替闪 | 正在校准 |
|
||||
|
||||
---
|
||||
|
||||
## 10. 构建与工具链
|
||||
|
||||
### 10.1 CMake 增强
|
||||
|
||||
```cmake
|
||||
target_compile_options(${CMAKE_PROJECT_NAME} PRIVATE
|
||||
-Wall -Wextra
|
||||
-Werror=return-type
|
||||
-fstack-usage
|
||||
)
|
||||
|
||||
target_link_options(${CMAKE_PROJECT_NAME} PRIVATE
|
||||
-Wl,--print-memory-usage
|
||||
-Wl,-Map=${CMAKE_PROJECT_NAME}.map
|
||||
)
|
||||
```
|
||||
|
||||
### 10.2 PID 单元测试
|
||||
|
||||
```c
|
||||
// test_pid.c — 可在 PC 上直接运行
|
||||
void test_pid_zero_error(void) {
|
||||
PIDController_t pid;
|
||||
PIDController_Init(&pid);
|
||||
pid.Kp = 1.0f;
|
||||
assert(PIDController_Compute(&pid, 0.0f, 0.0f) == 0.0f);
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 11. 改进优先级矩阵
|
||||
|
||||
```
|
||||
影响 ↑
|
||||
┌─────────────────────────────────────
|
||||
高 │ ① 电机限位 ② 看门狗
|
||||
│ ③ CAN 超时 ④ IMU 离线检测
|
||||
│ ⑤ FPU 编译选项
|
||||
│
|
||||
中 │ ⑥ PID 参数持久化 ⑦ 统一 PID 模块
|
||||
│ ⑧ 栈溢出检测 ⑨ 错误码统一
|
||||
│ ⑩ 在线调参协议
|
||||
│
|
||||
低 │ ⑪ 数据日志 ⑫ 单元测试
|
||||
│ ⑬ LED 状态码 ⑭ 前馈补偿
|
||||
│
|
||||
└─────────────────────────────────────
|
||||
高 ←────────── 工作量 ──────────→ 低
|
||||
```
|
||||
|
||||
### 推荐执行顺序
|
||||
|
||||
**第一阶段(安全,< 1 天)**:① → ② → ③ → ④
|
||||
**第二阶段(性能,1-2 天)**:⑤ → 速度模式切换 → ⑦ → 低通滤波器
|
||||
**第三阶段(质量,按需)**:⑥ → ⑧ → ⑨ → ⑩
|
||||
**第四阶段(体验)**:⑪ → ⑫ → ⑬ → ⑭
|
||||
|
||||
---
|
||||
|
||||
## 附录:文件改动清单
|
||||
|
||||
| 文件 | 改动类型 | 说明 |
|
||||
|---|---|---|
|
||||
| `app/app.h` | 修改 | 删除 `hal_*.h` 直接引用 |
|
||||
| `app/app_init.c` | 修改 | 下沉 HAL 初始化到接口注册阶段 |
|
||||
| `app/control_task.c` | 重写 | 速度模式 + vTaskDelayUntil + 滤波器 + 统一 PID |
|
||||
| `app/sensor_task.c` | 修改 | 删除 CopeCmdData/CmdProcess;只读 3 寄存器;降频 printf |
|
||||
| `app/monitor_task.c` | 新建/重写 | UART 命令处理 + Stream Buffer 接收 + 看门狗 |
|
||||
| `hal/stm32f4/can/hal_can.c` | 修改 | CAN 发送加超时 |
|
||||
| `CMakeLists.txt` | 修改 | 添加 FPU 编译选项 + 警告选项 |
|
||||
| `config.h` | 新建 | 集中管理配置常量 |
|
||||
| `modules/filter/lowpass.c` | 新建 | 一阶低通滤波器 |
|
||||
| `modules/filter/notch.c` | 新建 | 陷波滤波器 |
|
||||
| `app/cmd_protocol.c` | 新建(可选) | 二进制调参协议 |
|
||||
|
||||
---
|
||||
|
||||
> **核心原则**:宁可重构,也不妥协分层。任何硬件访问必须经过 Interface,任何硬件位置必须由 BSP 描述,任何硬件操作必须在 HAL 完成,任何功能逻辑必须在 Module,任何决策必须在 APP。
|
||||
@@ -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.
|
||||
@@ -47,29 +47,40 @@ target_link_directories(${CMAKE_PROJECT_NAME} PRIVATE
|
||||
target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
||||
# Add user sources here
|
||||
3rdparty/jy901p/wit_c_sdk.c
|
||||
modules/filter/lowpass/lowpass.c
|
||||
modules/filter/moving_average/moving_average.c
|
||||
modules/filter/notch/notch.c
|
||||
modules/filter/rate_limiter/rate_limiter.c
|
||||
modules/device/motor/mf4010v2.c
|
||||
modules/device/led/led.c
|
||||
modules/device/can/can_device.c
|
||||
modules/control/pid/pid.c
|
||||
modules/bus/soft_i2c/soft_i2c.c
|
||||
modules/bus/i2c/i2c_bus.c
|
||||
modules/cmd_parser/cmd_parser.c
|
||||
bsp/stm32f4/can/bsp_can.c
|
||||
bsp/stm32f4/led/bsp_led.c
|
||||
bsp/stm32f4/i2c/bsp_i2c.c
|
||||
bsp/stm32f4/gpio/bsp_gpio.c
|
||||
bsp/stm32f4/soft_i2c/bsp_soft_i2c.c
|
||||
bsp/stm32f4/uart/bsp_uart.c
|
||||
hal/stm32f4/can/hal_can.c
|
||||
hal/stm32f4/led/hal_led.c
|
||||
hal/stm32f4/i2c/hal_i2c.c
|
||||
hal/stm32f4/gpio/hal_gpio.c
|
||||
hal/stm32f4/soft_i2c/hal_soft_i2c.c
|
||||
hal/stm32f4/uart/hal_uart.c
|
||||
app/app_init.c
|
||||
app/control_task.c
|
||||
app/sensor_task.c
|
||||
app/callback_task.c
|
||||
interfaces/led/led_if.c
|
||||
interfaces/can/can_if.c
|
||||
interfaces/i2c/i2c_if.c
|
||||
interfaces/gpio/gpio_if.c
|
||||
interfaces/soft_i2c/soft_i2c_if.c
|
||||
interfaces/uart/uart_if.c
|
||||
modules/bus/uart/uart.c
|
||||
|
||||
)
|
||||
|
||||
@@ -81,24 +92,34 @@ target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/modules/device/motor
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/modules/device/can
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/modules/sensor/imu
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/modules/filter/lowpass
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/modules/filter/moving_average
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/modules/filter/notch
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/modules/filter/rate_limiter
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/modules/control/pid
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/modules/bus/soft_i2c
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/modules/bus/i2c
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/modules/cmd_parser
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/can
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/led
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/i2c
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/gpio
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/soft_i2c
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/bsp/stm32f4/uart
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/app
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/led
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/can
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/i2c
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/gpio
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/soft_i2c
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/hal/stm32f4/uart
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/interfaces/led
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/interfaces/can
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/interfaces/i2c
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/interfaces/gpio
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/interfaces/soft_i2c
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/interfaces/uart
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/modules/bus/uart
|
||||
)
|
||||
|
||||
# Add project symbols (macros)
|
||||
|
||||
+22
-10
@@ -39,18 +39,21 @@ Mcu.IP4=NVIC
|
||||
Mcu.IP5=RCC
|
||||
Mcu.IP6=SYS
|
||||
Mcu.IP7=USART1
|
||||
Mcu.IPNb=8
|
||||
Mcu.IP8=USART6
|
||||
Mcu.IPNb=9
|
||||
Mcu.Name=STM32F407Z(E-G)Tx
|
||||
Mcu.Package=LQFP144
|
||||
Mcu.Pin0=PE3
|
||||
Mcu.Pin1=PE4
|
||||
Mcu.Pin10=PA9
|
||||
Mcu.Pin11=PA10
|
||||
Mcu.Pin12=PD0
|
||||
Mcu.Pin13=PD1
|
||||
Mcu.Pin14=PG9
|
||||
Mcu.Pin15=VP_FREERTOS_VS_CMSIS_V2
|
||||
Mcu.Pin16=VP_SYS_VS_tim1
|
||||
Mcu.Pin10=PC6
|
||||
Mcu.Pin11=PC7
|
||||
Mcu.Pin12=PA9
|
||||
Mcu.Pin13=PA10
|
||||
Mcu.Pin14=PD0
|
||||
Mcu.Pin15=PD1
|
||||
Mcu.Pin16=PG9
|
||||
Mcu.Pin17=VP_FREERTOS_VS_CMSIS_V2
|
||||
Mcu.Pin18=VP_SYS_VS_tim1
|
||||
Mcu.Pin2=PF0
|
||||
Mcu.Pin3=PF1
|
||||
Mcu.Pin4=PH0-OSC_IN
|
||||
@@ -59,7 +62,7 @@ Mcu.Pin6=PF12
|
||||
Mcu.Pin7=PF14
|
||||
Mcu.Pin8=PB12
|
||||
Mcu.Pin9=PB13
|
||||
Mcu.PinsNb=17
|
||||
Mcu.PinsNb=19
|
||||
Mcu.ThirdPartyNb=0
|
||||
Mcu.UserConstants=
|
||||
Mcu.UserName=STM32F407ZGTx
|
||||
@@ -82,6 +85,7 @@ NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:false\:true\:false\:true\:false
|
||||
NVIC.TIM1_UP_TIM10_IRQn=true\:15\:0\:false\:false\:true\:false\:false\:true\:true
|
||||
NVIC.TimeBase=TIM1_UP_TIM10_IRQn
|
||||
NVIC.TimeBaseIP=TIM1
|
||||
NVIC.USART1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
|
||||
PA10.Mode=Asynchronous
|
||||
PA10.Signal=USART1_RX
|
||||
@@ -91,6 +95,12 @@ PB12.Mode=CAN_Activate
|
||||
PB12.Signal=CAN2_RX
|
||||
PB13.Mode=CAN_Activate
|
||||
PB13.Signal=CAN2_TX
|
||||
PC6.Locked=true
|
||||
PC6.Mode=Asynchronous
|
||||
PC6.Signal=USART6_TX
|
||||
PC7.Locked=true
|
||||
PC7.Mode=Asynchronous
|
||||
PC7.Signal=USART6_RX
|
||||
PCC.Checker=false
|
||||
PCC.Display=Plot\: All Steps
|
||||
PCC.Line=STM32F407/417
|
||||
@@ -176,7 +186,7 @@ ProjectManager.ToolChainLocation=
|
||||
ProjectManager.UAScriptAfterPath=
|
||||
ProjectManager.UAScriptBeforePath=
|
||||
ProjectManager.UnderRoot=false
|
||||
ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_I2C2_Init-I2C2-false-HAL-true,4-MX_USART1_UART_Init-USART1-false-HAL-true,5-MX_CAN2_Init-CAN2-false-HAL-true,6-MX_CAN1_Init-CAN1-false-HAL-true
|
||||
ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_I2C2_Init-I2C2-false-HAL-true,4-MX_USART1_UART_Init-USART1-false-HAL-true,5-MX_CAN2_Init-CAN2-false-HAL-true,6-MX_CAN1_Init-CAN1-false-HAL-true,7-MX_USART6_UART_Init-USART6-false-HAL-true
|
||||
RCC.48MHZClocksFreq_Value=84000000
|
||||
RCC.AHBFreq_Value=168000000
|
||||
RCC.APB1CLKDivider=RCC_HCLK_DIV4
|
||||
@@ -213,6 +223,8 @@ RCC.VCOOutputFreq_Value=336000000
|
||||
RCC.VcooutputI2S=192000000
|
||||
USART1.IPParameters=VirtualMode
|
||||
USART1.VirtualMode=VM_ASYNC
|
||||
USART6.IPParameters=VirtualMode
|
||||
USART6.VirtualMode=VM_ASYNC
|
||||
VP_FREERTOS_VS_CMSIS_V2.Mode=CMSIS_V2
|
||||
VP_FREERTOS_VS_CMSIS_V2.Signal=FREERTOS_VS_CMSIS_V2
|
||||
VP_SYS_VS_tim1.Mode=TIM1
|
||||
|
||||
@@ -54,6 +54,7 @@ void UsageFault_Handler(void);
|
||||
void DebugMon_Handler(void);
|
||||
void TIM1_UP_TIM10_IRQHandler(void);
|
||||
void I2C2_EV_IRQHandler(void);
|
||||
void USART1_IRQHandler(void);
|
||||
/* USER CODE BEGIN EFP */
|
||||
|
||||
/* USER CODE END EFP */
|
||||
|
||||
@@ -34,11 +34,14 @@ extern "C" {
|
||||
|
||||
extern UART_HandleTypeDef huart1;
|
||||
|
||||
extern UART_HandleTypeDef huart6;
|
||||
|
||||
/* USER CODE BEGIN Private defines */
|
||||
|
||||
/* USER CODE END Private defines */
|
||||
|
||||
void MX_USART1_UART_Init(void);
|
||||
void MX_USART6_UART_Init(void);
|
||||
|
||||
/* USER CODE BEGIN Prototypes */
|
||||
|
||||
|
||||
+1
-1
@@ -136,7 +136,7 @@ void MX_FREERTOS_Init(void) {
|
||||
void StartMonitorTask(void *argument)
|
||||
{
|
||||
/* USER CODE BEGIN StartMonitorTask */
|
||||
monitor_task();
|
||||
callback_task();
|
||||
/* USER CODE END StartMonitorTask */
|
||||
}
|
||||
|
||||
|
||||
+2
-1
@@ -49,6 +49,7 @@ void MX_GPIO_Init(void)
|
||||
__HAL_RCC_GPIOF_CLK_ENABLE();
|
||||
__HAL_RCC_GPIOH_CLK_ENABLE();
|
||||
__HAL_RCC_GPIOB_CLK_ENABLE();
|
||||
__HAL_RCC_GPIOC_CLK_ENABLE();
|
||||
__HAL_RCC_GPIOA_CLK_ENABLE();
|
||||
__HAL_RCC_GPIOD_CLK_ENABLE();
|
||||
__HAL_RCC_GPIOG_CLK_ENABLE();
|
||||
@@ -71,7 +72,7 @@ void MX_GPIO_Init(void)
|
||||
|
||||
/*Configure GPIO pins : SDA_Pin SCL_Pin */
|
||||
GPIO_InitStruct.Pin = SDA_Pin|SCL_Pin;
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;
|
||||
GPIO_InitStruct.Pull = GPIO_PULLUP;
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
|
||||
HAL_GPIO_Init(GPIOF, &GPIO_InitStruct);
|
||||
|
||||
+6
-2
@@ -21,11 +21,14 @@
|
||||
#include "cmsis_os.h"
|
||||
#include "can.h"
|
||||
#include "i2c.h"
|
||||
#include "stm32f4xx_hal_uart.h"
|
||||
#include "usart.h"
|
||||
#include "gpio.h"
|
||||
#include <stdint.h>
|
||||
|
||||
/* Private includes ----------------------------------------------------------*/
|
||||
/* USER CODE BEGIN Includes */
|
||||
|
||||
/* USER CODE END Includes */
|
||||
|
||||
/* Private typedef -----------------------------------------------------------*/
|
||||
@@ -61,7 +64,7 @@ int __io_putchar(int ch)
|
||||
|
||||
/* Private user code ---------------------------------------------------------*/
|
||||
/* USER CODE BEGIN 0 */
|
||||
|
||||
uint8_t g_uart_rx_byte;
|
||||
|
||||
|
||||
/* USER CODE END 0 */
|
||||
@@ -99,8 +102,9 @@ int main(void)
|
||||
MX_USART1_UART_Init();
|
||||
MX_CAN2_Init();
|
||||
MX_CAN1_Init();
|
||||
MX_USART6_UART_Init();
|
||||
/* USER CODE BEGIN 2 */
|
||||
|
||||
HAL_UART_Receive_IT(&huart1, &g_uart_rx_byte, 1);
|
||||
|
||||
/* USER CODE END 2 */
|
||||
|
||||
|
||||
@@ -56,6 +56,7 @@
|
||||
|
||||
/* External variables --------------------------------------------------------*/
|
||||
extern I2C_HandleTypeDef hi2c2;
|
||||
extern UART_HandleTypeDef huart1;
|
||||
extern TIM_HandleTypeDef htim1;
|
||||
|
||||
/* USER CODE BEGIN EV */
|
||||
@@ -188,6 +189,20 @@ void I2C2_EV_IRQHandler(void)
|
||||
/* USER CODE END I2C2_EV_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles USART1 global interrupt.
|
||||
*/
|
||||
void USART1_IRQHandler(void)
|
||||
{
|
||||
/* USER CODE BEGIN USART1_IRQn 0 */
|
||||
|
||||
/* USER CODE END USART1_IRQn 0 */
|
||||
HAL_UART_IRQHandler(&huart1);
|
||||
/* USER CODE BEGIN USART1_IRQn 1 */
|
||||
|
||||
/* USER CODE END USART1_IRQn 1 */
|
||||
}
|
||||
|
||||
/* USER CODE BEGIN 1 */
|
||||
|
||||
|
||||
|
||||
@@ -25,6 +25,7 @@
|
||||
/* USER CODE END 0 */
|
||||
|
||||
UART_HandleTypeDef huart1;
|
||||
UART_HandleTypeDef huart6;
|
||||
|
||||
/* USART1 init function */
|
||||
|
||||
@@ -54,6 +55,35 @@ void MX_USART1_UART_Init(void)
|
||||
|
||||
/* USER CODE END USART1_Init 2 */
|
||||
|
||||
}
|
||||
/* USART6 init function */
|
||||
|
||||
void MX_USART6_UART_Init(void)
|
||||
{
|
||||
|
||||
/* USER CODE BEGIN USART6_Init 0 */
|
||||
|
||||
/* USER CODE END USART6_Init 0 */
|
||||
|
||||
/* USER CODE BEGIN USART6_Init 1 */
|
||||
|
||||
/* USER CODE END USART6_Init 1 */
|
||||
huart6.Instance = USART6;
|
||||
huart6.Init.BaudRate = 115200;
|
||||
huart6.Init.WordLength = UART_WORDLENGTH_8B;
|
||||
huart6.Init.StopBits = UART_STOPBITS_1;
|
||||
huart6.Init.Parity = UART_PARITY_NONE;
|
||||
huart6.Init.Mode = UART_MODE_TX_RX;
|
||||
huart6.Init.HwFlowCtl = UART_HWCONTROL_NONE;
|
||||
huart6.Init.OverSampling = UART_OVERSAMPLING_16;
|
||||
if (HAL_UART_Init(&huart6) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
/* USER CODE BEGIN USART6_Init 2 */
|
||||
|
||||
/* USER CODE END USART6_Init 2 */
|
||||
|
||||
}
|
||||
|
||||
void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
|
||||
@@ -80,10 +110,37 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
|
||||
GPIO_InitStruct.Alternate = GPIO_AF7_USART1;
|
||||
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
|
||||
|
||||
/* USART1 interrupt Init */
|
||||
HAL_NVIC_SetPriority(USART1_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(USART1_IRQn);
|
||||
/* USER CODE BEGIN USART1_MspInit 1 */
|
||||
|
||||
/* USER CODE END USART1_MspInit 1 */
|
||||
}
|
||||
else if(uartHandle->Instance==USART6)
|
||||
{
|
||||
/* USER CODE BEGIN USART6_MspInit 0 */
|
||||
|
||||
/* USER CODE END USART6_MspInit 0 */
|
||||
/* USART6 clock enable */
|
||||
__HAL_RCC_USART6_CLK_ENABLE();
|
||||
|
||||
__HAL_RCC_GPIOC_CLK_ENABLE();
|
||||
/**USART6 GPIO Configuration
|
||||
PC6 ------> USART6_TX
|
||||
PC7 ------> USART6_RX
|
||||
*/
|
||||
GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_7;
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
|
||||
GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
|
||||
GPIO_InitStruct.Alternate = GPIO_AF8_USART6;
|
||||
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
|
||||
|
||||
/* USER CODE BEGIN USART6_MspInit 1 */
|
||||
|
||||
/* USER CODE END USART6_MspInit 1 */
|
||||
}
|
||||
}
|
||||
|
||||
void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
|
||||
@@ -103,10 +160,30 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
|
||||
*/
|
||||
HAL_GPIO_DeInit(GPIOA, GPIO_PIN_9|GPIO_PIN_10);
|
||||
|
||||
/* USART1 interrupt Deinit */
|
||||
HAL_NVIC_DisableIRQ(USART1_IRQn);
|
||||
/* USER CODE BEGIN USART1_MspDeInit 1 */
|
||||
|
||||
/* USER CODE END USART1_MspDeInit 1 */
|
||||
}
|
||||
else if(uartHandle->Instance==USART6)
|
||||
{
|
||||
/* USER CODE BEGIN USART6_MspDeInit 0 */
|
||||
|
||||
/* USER CODE END USART6_MspDeInit 0 */
|
||||
/* Peripheral clock disable */
|
||||
__HAL_RCC_USART6_CLK_DISABLE();
|
||||
|
||||
/**USART6 GPIO Configuration
|
||||
PC6 ------> USART6_TX
|
||||
PC7 ------> USART6_RX
|
||||
*/
|
||||
HAL_GPIO_DeInit(GPIOC, GPIO_PIN_6|GPIO_PIN_7);
|
||||
|
||||
/* USER CODE BEGIN USART6_MspDeInit 1 */
|
||||
|
||||
/* USER CODE END USART6_MspDeInit 1 */
|
||||
}
|
||||
}
|
||||
|
||||
/* USER CODE BEGIN 1 */
|
||||
|
||||
@@ -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` |
|
||||
|
||||
---
|
||||
|
||||
> **一句话原则**: 每一个改进项都是可验证的独立单元,改一项测一项,不要一次性全部改动。
|
||||
@@ -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
|
||||
@@ -7,15 +7,57 @@
|
||||
#include "hal_can.h"
|
||||
#include "hal_led.h"
|
||||
#include "hal_soft_i2c.h"
|
||||
#include "hal_uart.h"
|
||||
#include "freertos.h"
|
||||
#include "pid.h"
|
||||
#include <stdio.h>
|
||||
#include <FreeRTOS.h>
|
||||
#include "cmsis_os.h"
|
||||
#include "cmsis_os2.h"
|
||||
#include "projdefs.h"
|
||||
#include "wit_c_sdk.h"
|
||||
|
||||
/* ============================================================================
|
||||
* IMU 传感器数据结构 — sensor_task → control_task 数据通道
|
||||
* ============================================================================ */
|
||||
typedef struct {
|
||||
float acc[3]; /* 加速度 X/Y/Z (g) */
|
||||
float gyro[3]; /* 角速度 X/Y/Z (deg/s) */
|
||||
float angle[3]; /* 欧拉角 Roll/Pitch/Yaw (deg) */
|
||||
} imu_data_t;
|
||||
|
||||
/* IMU 数据队列句柄(app_init 创建) */
|
||||
extern osMessageQueueId_t g_imu_queue, g_cmd_queue;
|
||||
extern osMutexId_t g_uart_mutex;
|
||||
extern uint8_t g_uart_rx_byte;
|
||||
|
||||
void log_printf(const char *fmt, ...);
|
||||
|
||||
/* 最大命令长度(包括结尾空字符) */
|
||||
#define MAX_CMD_LEN 64
|
||||
|
||||
void app_init(void);
|
||||
void sensor_task(void);
|
||||
void control_task(void);
|
||||
void monitor_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
|
||||
+38
-10
@@ -1,8 +1,20 @@
|
||||
#include "app.h"
|
||||
#include "cmsis_os2.h"
|
||||
#include <string.h>
|
||||
#include <stdarg.h>
|
||||
|
||||
|
||||
static led_t led0, led1, led2;
|
||||
|
||||
/* IMU 数据队列句柄 — sensor_task 和 control_task 共享 */
|
||||
osMessageQueueId_t g_imu_queue = NULL;
|
||||
|
||||
/* UART 命令队列 — ISR → callback_task */
|
||||
osMessageQueueId_t g_cmd_queue = NULL;
|
||||
|
||||
/* UART 发送互斥锁 — 防止多任务 printf 交叉 */
|
||||
osMutexId_t g_uart_mutex = NULL;
|
||||
|
||||
|
||||
void app_init(void)
|
||||
{
|
||||
@@ -10,8 +22,10 @@ void app_init(void)
|
||||
hal_i2c_init_all();
|
||||
hal_led_init_all();
|
||||
hal_can_init_all();
|
||||
hal_gpio_init_all();
|
||||
hal_gpio_init_all();
|
||||
hal_soft_i2c_init_all();
|
||||
hal_uart_init_all();
|
||||
|
||||
// 初始化外设
|
||||
led_init(&led0, 0);
|
||||
led_init(&led1, 1);
|
||||
@@ -21,19 +35,33 @@ void app_init(void)
|
||||
led_on(&led2);
|
||||
led_off(&led0);
|
||||
|
||||
// 创建 IMU 数据队列(sensor_task → control_task)
|
||||
// 深度 4,每消息 3×float×3 = 36 bytes
|
||||
g_imu_queue = osMessageQueueNew(1, sizeof(imu_data_t), NULL);
|
||||
g_cmd_queue = osMessageQueueNew(5, MAX_CMD_LEN, NULL);
|
||||
|
||||
// 创建 UART 互斥锁(保护 printf 不被多任务交叉打断)
|
||||
g_uart_mutex = osMutexNew(NULL);
|
||||
if (g_cmd_queue == NULL) {
|
||||
printf("[app_init] ERROR: Failed to create command queue\r\n");
|
||||
}
|
||||
if (g_imu_queue == NULL) {
|
||||
printf("[app_init] ERROR: Failed to create IMU queue\r\n");
|
||||
}
|
||||
|
||||
printf("[app_init] Hardware initialized\r\n");
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void monitor_task(void)
|
||||
void log_printf(const char *fmt, ...)
|
||||
{
|
||||
while (1) {
|
||||
// 简单的监控任务:每秒打印一次当前状态
|
||||
// printf("Pitch: %.2f deg, Roll: %.2f deg\n", g_imu.angle[1] / 100.0f, g_imu.angle[0] / 100.0f);
|
||||
vTaskDelay(pdMS_TO_TICKS(1000)); // 1Hz 更新率
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
+212
-98
@@ -1,156 +1,179 @@
|
||||
/**
|
||||
* @file control_task.c
|
||||
* @brief 二轴云台增稳控制任务
|
||||
* @brief 二轴云台增稳控制任务(速度控制模式)
|
||||
* @note 目标:保持 Roll=0, Pitch=0,Yaw 用于方向控制
|
||||
*
|
||||
* 控制架构:
|
||||
* IMU 数据 → PID 控制器 → 电机补偿角度 → 抵消基座倾斜
|
||||
* 控制架构(速度控制):
|
||||
* IMU 角度 → PID 控制器 → 角速度指令 → 电机速度闭环 → 抵消基座倾斜
|
||||
*
|
||||
* 数据来源:
|
||||
* 通过 FreeRTOS Message Queue 从 sensor_task 接收 imu_data_t
|
||||
*/
|
||||
|
||||
#include "app.h"
|
||||
#include "cmsis_os2.h"
|
||||
#include "mf4010v2.h"
|
||||
#include "pid.h"
|
||||
#include <stdio.h>
|
||||
|
||||
/* ============================================================================
|
||||
* 外部变量 (来自 sensor_task)
|
||||
* ============================================================================ */
|
||||
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_pitch = 0.0f;
|
||||
static float g_target_yaw = 0.0f; /* Yaw 目标可通过外部修改 */
|
||||
static float g_target_roll = 0.0f; /* Roll 目标 (deg) */
|
||||
static float g_target_pitch = 0.0f; /* Pitch 目标 (deg) */
|
||||
static float g_target_yaw = 0.0f; /* Yaw 目标 (deg) */
|
||||
|
||||
/* 电机实例 */
|
||||
static mf4010v2_t motor_yaw; /* Yaw 轴电机(外框) */
|
||||
static mf4010v2_t motor_pitch; /* Pitch 轴电机(内框) */
|
||||
static mf4010v2_t motor_yaw; /* Yaw 轴电机(外框,补偿 Roll) */
|
||||
static mf4010v2_t motor_pitch; /* Pitch 轴电机(内框,补偿 Pitch) */
|
||||
|
||||
/* PID 控制器 */
|
||||
typedef struct {
|
||||
float Kp, Ki, Kd;
|
||||
float integral;
|
||||
float prev_error;
|
||||
float output_max;
|
||||
} PID_t;
|
||||
static pid_t g_roll_pid; /* Roll 轴 PID */
|
||||
static pid_t g_pitch_pid; /* Pitch 轴 PID */
|
||||
|
||||
static PID_t g_roll_pid = {0};
|
||||
static PID_t g_pitch_pid = {0};
|
||||
static lowpass_t g_roll_lowpass;
|
||||
static lowpass_t g_pitch_lowpass;
|
||||
|
||||
/* 电机使能状态 — 由 callback_task 通过 UART 命令控制 */
|
||||
static bool g_motor_enabled[2] = {false, false};
|
||||
/* ============================================================================
|
||||
* PID 控制函数
|
||||
* 速度控制参数
|
||||
* ============================================================================ */
|
||||
|
||||
void PID_Init(PID_t *pid, float kp, float ki, float kd, float max)
|
||||
{
|
||||
pid->Kp = kp;
|
||||
pid->Ki = ki;
|
||||
pid->Kd = kd;
|
||||
pid->output_max = max;
|
||||
pid->integral = 0;
|
||||
pid->prev_error = 0;
|
||||
}
|
||||
/*
|
||||
* PID 输出为速度指令 (0.01 DPS/LSB)
|
||||
*
|
||||
* 电机 MF4010V2 速度范围:±204800 (即 ±2048 DPS)
|
||||
* 稳像场景使用较低速度:建议 ±36000 (即 ±360 DPS) 作为输出上限
|
||||
*/
|
||||
#define SPEED_OUTPUT_MAX 36000.0f /* 最大角速度 360 DPS (0.01 DPS) */
|
||||
|
||||
float PID_Compute(PID_t *pid, float target, float actual)
|
||||
{
|
||||
float error = target - actual;
|
||||
float output;
|
||||
/*
|
||||
* PID 参数说明(速度控制模式):
|
||||
*
|
||||
* Kp: 角度误差 → 速度指令的比例系数 (0.01 DPS / deg)
|
||||
* 例如 Kp=200, 则 1° 误差 → 200 DPS 的补偿速度
|
||||
*
|
||||
* Ki: 消除稳态误差的积分系数
|
||||
* 例如 Ki=5, 则 1° 持续误差每秒累积 5 DPS
|
||||
*
|
||||
* Kd: 抑制振荡的微分系数
|
||||
* 例如 Kd=50, 则角速度变化率反馈阻尼
|
||||
*/
|
||||
#define ROLL_KP 200.0f
|
||||
#define ROLL_KI 5.0f
|
||||
#define ROLL_KD 50.0f
|
||||
#define ROLL_ALPHA 0.7f
|
||||
|
||||
/* 比例项 */
|
||||
output = pid->Kp * error;
|
||||
|
||||
/* 积分项(带抗饱和) */
|
||||
pid->integral += error;
|
||||
if (pid->integral > pid->output_max) {
|
||||
pid->integral = pid->output_max;
|
||||
} else if (pid->integral < -pid->output_max) {
|
||||
pid->integral = -pid->output_max;
|
||||
}
|
||||
output += pid->Ki * pid->integral;
|
||||
|
||||
/* 微分项 */
|
||||
float derivative = error - pid->prev_error;
|
||||
output += pid->Kd * derivative;
|
||||
pid->prev_error = error;
|
||||
|
||||
/* 输出限幅 */
|
||||
if (output > pid->output_max) {
|
||||
output = pid->output_max;
|
||||
} else if (output < -pid->output_max) {
|
||||
output = -pid->output_max;
|
||||
}
|
||||
|
||||
return output;
|
||||
}
|
||||
#define PITCH_KP 200.0f
|
||||
#define PITCH_KI 5.0f
|
||||
#define PITCH_KD 50.0f
|
||||
|
||||
#define PITCH_ALPHA 0.7f
|
||||
/* ============================================================================
|
||||
* 控制任务
|
||||
* 控制任务(速度控制模式)
|
||||
* ============================================================================ */
|
||||
|
||||
void control_task(void)
|
||||
{
|
||||
// int32_t yaw_cmd, pitch_cmd;
|
||||
// float roll_comp, pitch_comp;
|
||||
imu_data_t imu;
|
||||
osStatus_t qstat;
|
||||
uint32_t last_tick = osKernelGetTickCount();
|
||||
|
||||
printf("[control_task] starting stabilization control\n");
|
||||
log_printf("[control_task] starting stabilization control (speed mode)\n");
|
||||
|
||||
/* 1. 初始化 PID 参数 */
|
||||
/* Roll 轴 PID:控制 Yaw 电机来补偿 Roll 倾斜 */
|
||||
// PID_Init(&g_roll_pid, 0.5f, 0.01f, 0.1f, 18000.0f); /* 最大补偿 180 度 */
|
||||
/* 1. 初始化 PID 控制器 */
|
||||
pid_init(&g_roll_pid);
|
||||
g_roll_pid.Kp = ROLL_KP;
|
||||
g_roll_pid.Ki = ROLL_KI;
|
||||
g_roll_pid.Kd = ROLL_KD;
|
||||
g_roll_pid.output_max = SPEED_OUTPUT_MAX;
|
||||
g_roll_pid.integral_max = SPEED_OUTPUT_MAX;
|
||||
|
||||
/* Pitch 轴 PID:控制 Pitch 电机补偿 Pitch 倾斜 */
|
||||
// PID_Init(&g_pitch_pid, 0.5f, 0.01f, 0.1f, 9000.0f); /* 最大补偿 90 度 */
|
||||
pid_init(&g_pitch_pid);
|
||||
g_pitch_pid.Kp = PITCH_KP;
|
||||
g_pitch_pid.Ki = PITCH_KI;
|
||||
g_pitch_pid.Kd = PITCH_KD;
|
||||
g_pitch_pid.output_max = SPEED_OUTPUT_MAX;
|
||||
g_pitch_pid.integral_max = SPEED_OUTPUT_MAX;
|
||||
|
||||
lowpass_init(&g_roll_lowpass, ROLL_ALPHA);
|
||||
lowpass_init(&g_pitch_lowpass, PITCH_ALPHA);
|
||||
|
||||
log_printf("[control_task] PID: Roll(Kp=%.1f Ki=%.1f Kd=%.1f) Pitch(Kp=%.1f Ki=%.1f Kd=%.1f)\n",
|
||||
ROLL_KP, ROLL_KI, ROLL_KD, PITCH_KP, PITCH_KI, PITCH_KD);
|
||||
|
||||
/* 2. 初始化电机 */
|
||||
// osDelay(pdMS_TO_TICKS(1000));
|
||||
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 */
|
||||
|
||||
/* 3. 电机关机状态启动 */
|
||||
mf4010v2_close(&motor_yaw);
|
||||
mf4010v2_close(&motor_pitch);
|
||||
|
||||
printf("[control_task] motor yaw is fault: %d\n", mf4010v2_is_fault(&motor_yaw));
|
||||
printf("[control_task] motor pitch is fault: %d\n", mf4010v2_is_fault(&motor_pitch));
|
||||
log_printf("[control_task] motor yaw is fault: %d\n", mf4010v2_is_fault(&motor_yaw));
|
||||
log_printf("[control_task] motor pitch is fault: %d\n", mf4010v2_is_fault(&motor_pitch));
|
||||
|
||||
/* 等待 IMU 稳定 */
|
||||
osDelay(pdMS_TO_TICKS(5000));
|
||||
|
||||
/* 4. 使能电机 */
|
||||
/* 4. 使能电机(零速启动) */
|
||||
mf4010v2_set_speed(&motor_yaw, 0);
|
||||
mf4010v2_set_speed(&motor_pitch, 0);
|
||||
mf4010v2_run(&motor_yaw);
|
||||
mf4010v2_run(&motor_pitch);
|
||||
|
||||
printf("[control_task] stabilization enabled\n");
|
||||
log_printf("[control_task] stabilization enabled (speed mode)\n");
|
||||
|
||||
/* 主控制循环 (100Hz) */
|
||||
/* ========================================================================
|
||||
* 主控制循环 (100Hz, osDelayUntil 精确周期性调度)
|
||||
*
|
||||
* 速度控制策略:
|
||||
* 1. 从队列读取最新 IMU 角度 (Roll, Pitch)
|
||||
* 2. PID 计算角速度补偿量
|
||||
* 3. 速度指令发送到电机(电机内部完成速度闭环)
|
||||
* ======================================================================== */
|
||||
while (1) {
|
||||
/* 读取 IMU 数据 */
|
||||
// float current_roll = fAngle[0]; /* Roll 角 */
|
||||
// float current_pitch = fAngle[1]; /* Pitch 角 */
|
||||
last_tick += pdMS_TO_TICKS(20); /* 100Hz 周期 */
|
||||
|
||||
// /* PID 计算补偿量 */
|
||||
// /* 目标:Roll=0, Pitch=0 */
|
||||
// roll_comp = PID_Compute(&g_roll_pid, g_target_roll, current_roll);
|
||||
// pitch_comp = PID_Compute(&g_pitch_pid, g_target_pitch, current_pitch);
|
||||
/* 从 sensor_task 获取最新 IMU 数据(不等待,由定时保证节奏) */
|
||||
qstat = osMessageQueueGet(g_imu_queue, &imu, NULL, 0);
|
||||
if (qstat == osOK) {
|
||||
imu.angle[0] = lowpass_update(&g_roll_lowpass, imu.angle[0]);
|
||||
imu.angle[1] = lowpass_update(&g_pitch_lowpass, imu.angle[1]);
|
||||
|
||||
// /* 运动学映射:补偿量 -> 电机角度 */
|
||||
// /* 对于 Pitch-Yaw 云台:
|
||||
// * - Pitch 倾斜 → Pitch 电机反向补偿
|
||||
// * - Roll 倾斜 → Yaw 电机 + Pitch 电机联合补偿(简化为 Yaw 电机)
|
||||
// */
|
||||
// yaw_cmd = (int32_t)(roll_comp * 100.0f); /* 转为 0.01 度 */
|
||||
// pitch_cmd = (int32_t)(pitch_comp * 100.0f);
|
||||
/* ---- Roll 轴速度控制 ---- */
|
||||
float roll_speed = pid_compute(&g_roll_pid,
|
||||
g_target_roll,
|
||||
imu.angle[0]); /* Roll */
|
||||
int32_t yaw_speed_cmd = (int32_t)roll_speed;
|
||||
|
||||
// /* 发送位置指令到电机 */
|
||||
// /* 使用增量位置控制,直接叠加补偿量 */
|
||||
// mf4010v2_set_angle(&motor_yaw, motor_yaw.current_angle + yaw_cmd);
|
||||
// mf4010v2_set_angle(&motor_pitch, motor_pitch.current_angle + pitch_cmd);
|
||||
/* ---- Pitch 轴速度控制 ---- */
|
||||
float pitch_speed = pid_compute(&g_pitch_pid,
|
||||
g_target_pitch,
|
||||
imu.angle[1]); /* Pitch */
|
||||
int32_t pitch_speed_cmd = (int32_t)pitch_speed;
|
||||
yaw_speed_cmd = yaw_speed_cmd > 36000 ? 36000 : (yaw_speed_cmd < -36000 ? -36000 : yaw_speed_cmd);
|
||||
pitch_speed_cmd = pitch_speed_cmd > 36000 ? 36000 : (pitch_speed_cmd < -36000 ? -36000 : pitch_speed_cmd);
|
||||
|
||||
/* 100Hz 控制循环 */
|
||||
osDelay(pdMS_TO_TICKS(10));
|
||||
if (g_motor_enabled[GIMBAL_MOTOR_YAW]) {
|
||||
mf4010v2_set_speed(&motor_yaw, yaw_speed_cmd);
|
||||
} else {
|
||||
mf4010v2_close(&motor_yaw);
|
||||
}
|
||||
if (g_motor_enabled[GIMBAL_MOTOR_PITCH]) {
|
||||
mf4010v2_set_speed(&motor_pitch, pitch_speed_cmd);
|
||||
} else {
|
||||
mf4010v2_close(&motor_pitch);
|
||||
}
|
||||
}
|
||||
|
||||
if (osDelayUntil(last_tick) != osOK) {
|
||||
last_tick = osKernelGetTickCount();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -163,3 +186,94 @@ 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;
|
||||
}
|
||||
+129
-120
@@ -1,7 +1,20 @@
|
||||
#include "REG.h"
|
||||
#include "app.h"
|
||||
#include "cmsis_os2.h"
|
||||
#include "soft_i2c.h"
|
||||
#include "i2c_bus.h"
|
||||
#include "wit_c_sdk.h"
|
||||
#include <stdint.h>
|
||||
|
||||
#define USE_SOFT_IIC 1
|
||||
#define USE_IIC 1
|
||||
#if USE_IIC
|
||||
#if USE_SOFT_IIC
|
||||
static soft_i2c_t si2c;
|
||||
#else
|
||||
static i2c_t i2c;
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#define ACC_UPDATE 0x01
|
||||
#define GYRO_UPDATE 0x02
|
||||
@@ -9,12 +22,13 @@ static soft_i2c_t si2c;
|
||||
#define MAG_UPDATE 0x08
|
||||
#define READ_UPDATE 0x80
|
||||
static volatile char s_cDataUpdate = 0, s_cCmd = 0xff;
|
||||
static void CmdProcess(void);
|
||||
static void AutoScanSensor(void);
|
||||
static bool g_sensor_enabled = true; /* 由 callback_task 通过 UART 命令控制 */
|
||||
static void CopeSensorData(uint32_t uiReg, uint32_t uiRegNum);
|
||||
static void Delayms(uint16_t ucMs);
|
||||
|
||||
#if USE_IIC
|
||||
static int32_t IICreadBytes(uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uint32_t uiLen) {
|
||||
#if USE_SOFT_IIC
|
||||
soft_i2c_start_frame(&si2c);
|
||||
soft_i2c_send_frame(&si2c, ucAddr);
|
||||
if(soft_i2c_wait_ack_frame(&si2c) != 0)return 0;
|
||||
@@ -30,9 +44,15 @@ static int32_t IICreadBytes(uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uin
|
||||
}
|
||||
soft_i2c_stop_frame(&si2c);
|
||||
return 1;
|
||||
#else
|
||||
return i2c_mem_read(&i2c, (uint16_t)ucAddr, ucReg, 1, p_ucVal, uiLen, 100) == 0 ? 1 : 0;
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
#if USE_IIC
|
||||
static int32_t IICwriteBytes (uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uint32_t uiLen) {
|
||||
#if USE_SOFT_IIC
|
||||
soft_i2c_start_frame(&si2c);
|
||||
soft_i2c_send_frame(&si2c, ucAddr);
|
||||
if(soft_i2c_wait_ack_frame(&si2c) != 0)return 0;
|
||||
@@ -45,122 +65,136 @@ static int32_t IICwriteBytes (uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, u
|
||||
}
|
||||
soft_i2c_stop_frame(&si2c);
|
||||
return 1;
|
||||
#else
|
||||
return i2c_mem_write(&i2c, (uint16_t)ucAddr, ucReg, 1, p_ucVal, uiLen, 100) == 0 ? 1 : 0;
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
static void print_float(const char* str, float v1, float v2, float v3) {
|
||||
char v1_sign = v1 > 0 ? '+' : '-';
|
||||
char v2_sign = v2 > 0 ? '+' : '-';
|
||||
char v3_sign = v3 > 0 ? '+' : '-';
|
||||
v1 = v1 > 0 ? v1 : -v1;
|
||||
v2 = v2 > 0 ? v2 : -v2;
|
||||
v3 = v3 > 0 ? v3 : -v3;
|
||||
int i1_x = (int)(v1 * 1000) / 1000;
|
||||
int i1_y = (int)(v1 * 1000) % 1000;
|
||||
int i2_x = (int)(v2 * 1000) / 1000;
|
||||
int i2_y = (int)(v2 * 1000) % 1000;
|
||||
int i3_x = (int)(v3 * 1000) / 1000;
|
||||
int i3_y = (int)(v3 * 1000) % 1000;
|
||||
log_printf("[sensor_task] %s: %c%03d.%03d, %c%03d.%03d, %c%03d.%03d\r\n", str, v1_sign, i1_x, i1_y, v2_sign, i2_x, i2_y, v3_sign, i3_x, i3_y);
|
||||
}
|
||||
void sensor_task(void)
|
||||
{
|
||||
float fAcc[3], fGyro[3], fAngle[3];
|
||||
int i;
|
||||
int ret;
|
||||
imu_data_t imu;
|
||||
#if USE_IIC
|
||||
#if USE_SOFT_IIC
|
||||
ret = soft_i2c_init(&si2c, 0);
|
||||
printf("[sensor] soft_i2c_init = %d\r\n", ret);
|
||||
#else
|
||||
ret = i2c_init(&i2c, 1);
|
||||
#endif
|
||||
#endif
|
||||
log_printf("[sensor_task] i2c init = %d\r\n", ret);
|
||||
#if USE_IIC
|
||||
WitInit(WIT_PROTOCOL_I2C, 0x50);
|
||||
WitI2cFuncRegister(IICwriteBytes, IICreadBytes);
|
||||
#else
|
||||
WitInit(WIT_PROTOCOL_NORMAL, 0x50);
|
||||
WitSerialWriteRegister();
|
||||
#endif
|
||||
WitRegisterCallBack(CopeSensorData);
|
||||
WitDelayMsRegister(Delayms);
|
||||
AutoScanSensor();
|
||||
|
||||
uint32_t last_tick = osKernelGetTickCount();
|
||||
|
||||
while (1)
|
||||
{
|
||||
WitReadReg(AX, 12);
|
||||
Delayms(5);
|
||||
CmdProcess();
|
||||
if(s_cDataUpdate)
|
||||
if(g_sensor_enabled) {
|
||||
WitReadReg(AX, 13);
|
||||
}
|
||||
last_tick += pdMS_TO_TICKS(2); /* 500Hz 轮询 */
|
||||
if(g_sensor_enabled && s_cDataUpdate)
|
||||
{
|
||||
for(i = 0; i < 3; i++)
|
||||
{
|
||||
fAcc[i] = sReg[AX+i] * 0.0054931640625f;
|
||||
fGyro[i] = sReg[GX+i] * 0.0054931640625f;
|
||||
fAngle[i] = sReg[Roll+i] * 0.0054931640625f;
|
||||
}
|
||||
if(s_cDataUpdate & ACC_UPDATE)
|
||||
{
|
||||
printf("raw:AX=%d AY=%d AZ=%d GX=%d GY=%d GZ=%d\r\n",
|
||||
sReg[AX], sReg[AY], sReg[AZ],
|
||||
sReg[GX], sReg[GY], sReg[GZ]);
|
||||
printf("acc:%.3f %.3f %.3f\r\n", fAcc[0], fAcc[1], fAcc[2]);
|
||||
s_cDataUpdate &= ~ACC_UPDATE;
|
||||
}
|
||||
if(s_cDataUpdate & GYRO_UPDATE)
|
||||
{
|
||||
printf("gyro:%.3f %.3f %.3f\r\n", fGyro[0], fGyro[1], fGyro[2]);
|
||||
s_cDataUpdate &= ~GYRO_UPDATE;
|
||||
imu.acc[i] = sReg[AX+i] * 0.0054931640625f;
|
||||
imu.gyro[i] = sReg[GX+i] * 0.0054931640625f;
|
||||
imu.angle[i] = sReg[Roll+i] * 0.0054931640625f;
|
||||
}
|
||||
int16_t temp = sReg[TEMP];
|
||||
// if(s_cDataUpdate & ACC_UPDATE)
|
||||
// {
|
||||
// print_float("acc", imu.acc[0], imu.acc[1], imu.acc[2]);
|
||||
// s_cDataUpdate &= ~ACC_UPDATE;
|
||||
// }
|
||||
// if(s_cDataUpdate & GYRO_UPDATE)
|
||||
// {
|
||||
// print_float("gyro", imu.gyro[0], imu.gyro[1], imu.gyro[2]);
|
||||
// s_cDataUpdate &= ~GYRO_UPDATE;
|
||||
// }
|
||||
if(s_cDataUpdate & ANGLE_UPDATE)
|
||||
{
|
||||
printf("angle:%.3f %.3f %.3f\r\n", fAngle[0], fAngle[1], fAngle[2]);
|
||||
print_float("angle", imu.angle[0], imu.angle[1], imu.angle[2]);
|
||||
s_cDataUpdate &= ~ANGLE_UPDATE;
|
||||
}
|
||||
if(s_cDataUpdate & MAG_UPDATE)
|
||||
if(s_cDataUpdate & READ_UPDATE)
|
||||
{
|
||||
printf("mag:%d %d %d\r\n", sReg[HX], sReg[HY], sReg[HZ]);
|
||||
s_cDataUpdate &= ~MAG_UPDATE;
|
||||
char sign = temp > 0 ? '+' : '-';
|
||||
if (temp < 0) {
|
||||
temp = -temp;
|
||||
}
|
||||
log_printf("[sensor_task] temp data: %c%3d.%02d\n", sign, temp/100, temp%100);
|
||||
s_cDataUpdate &= ~READ_UPDATE;
|
||||
}
|
||||
/* 将 IMU 数据推送到 control_task */
|
||||
if (g_imu_queue != NULL) {
|
||||
osMessageQueuePut(g_imu_queue, &imu, 0, 20);
|
||||
}
|
||||
}
|
||||
if (osDelayUntil(last_tick) != osOK) {
|
||||
last_tick = osKernelGetTickCount();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ============================================================================
|
||||
* Sensor task API — 由 callback_task 通过 UART 命令调用
|
||||
* ============================================================================ */
|
||||
|
||||
void CopeCmdData(unsigned char ucData)
|
||||
void sensor_enable(void)
|
||||
{
|
||||
static unsigned char s_ucData[50], s_ucRxCnt = 0;
|
||||
|
||||
s_ucData[s_ucRxCnt++] = ucData;
|
||||
if(s_ucRxCnt<3)return; //Less than three data returned
|
||||
if(s_ucRxCnt >= 50) s_ucRxCnt = 0;
|
||||
if(s_ucRxCnt >= 3)
|
||||
{
|
||||
if((s_ucData[1] == '\r') && (s_ucData[2] == '\n'))
|
||||
{
|
||||
s_cCmd = s_ucData[0];
|
||||
memset(s_ucData,0,50);//
|
||||
s_ucRxCnt = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
s_ucData[0] = s_ucData[1];
|
||||
s_ucData[1] = s_ucData[2];
|
||||
s_ucRxCnt = 2;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
g_sensor_enabled = true;
|
||||
log_printf("[sensor_task] enabled\r\n");
|
||||
}
|
||||
|
||||
static void CmdProcess(void)
|
||||
void sensor_disable(void)
|
||||
{
|
||||
switch(s_cCmd)
|
||||
{
|
||||
case 'a':
|
||||
if(WitStartAccCali() != WIT_HAL_OK)
|
||||
printf("\r\nSet AccCali Error\r\n");
|
||||
break;
|
||||
case 'm':
|
||||
if(WitStartMagCali() != WIT_HAL_OK)
|
||||
printf("\r\nStart MagCali Error\r\n");
|
||||
break;
|
||||
case 'e':
|
||||
if(WitStopMagCali() != WIT_HAL_OK)
|
||||
printf("\r\nEnd MagCali Error\r\n");
|
||||
break;
|
||||
case 'u':
|
||||
if(WitSetBandwidth(BANDWIDTH_5HZ) != WIT_HAL_OK)
|
||||
printf("\r\nSet Bandwidth Error\r\n");
|
||||
break;
|
||||
case 'U':
|
||||
if(WitSetBandwidth(BANDWIDTH_256HZ) != WIT_HAL_OK)
|
||||
printf("\r\nSet Bandwidth Error\r\n");
|
||||
break;
|
||||
case 'B':
|
||||
if(WitSetUartBaud(WIT_BAUD_115200) != WIT_HAL_OK)
|
||||
printf("\r\nSet Baud Error\r\n");
|
||||
break;
|
||||
case 'b':
|
||||
if(WitSetUartBaud(WIT_BAUD_9600) != WIT_HAL_OK)
|
||||
printf("\r\nSet Baud Error\r\n");
|
||||
break;
|
||||
default : return ;
|
||||
}
|
||||
s_cCmd = 0xff;
|
||||
g_sensor_enabled = false;
|
||||
log_printf("[sensor_task] disabled\r\n");
|
||||
}
|
||||
|
||||
void sensor_acc_cali_enable(void)
|
||||
{
|
||||
WitStartAccCali();
|
||||
}
|
||||
|
||||
void sensor_acc_cali_disable(void)
|
||||
{
|
||||
WitStopAccCali();
|
||||
}
|
||||
|
||||
void sensor_mag_cali_enable(void)
|
||||
{
|
||||
WitStartMagCali();
|
||||
}
|
||||
|
||||
void sensor_mag_cali_disable(void)
|
||||
{
|
||||
WitStopMagCali();
|
||||
}
|
||||
|
||||
static void CopeSensorData(uint32_t uiReg, uint32_t uiRegNum)
|
||||
@@ -172,19 +206,19 @@ static void CopeSensorData(uint32_t uiReg, uint32_t uiRegNum)
|
||||
{
|
||||
// case AX:
|
||||
// case AY:
|
||||
case AZ:
|
||||
s_cDataUpdate |= ACC_UPDATE;
|
||||
break;
|
||||
// case AZ:
|
||||
// s_cDataUpdate |= ACC_UPDATE;
|
||||
// break;
|
||||
// case GX:
|
||||
// case GY:
|
||||
case GZ:
|
||||
s_cDataUpdate |= GYRO_UPDATE;
|
||||
break;
|
||||
// case GZ:
|
||||
// s_cDataUpdate |= GYRO_UPDATE;
|
||||
// break;
|
||||
// case HX:
|
||||
// case HY:
|
||||
case HZ:
|
||||
s_cDataUpdate |= MAG_UPDATE;
|
||||
break;
|
||||
// case HZ:
|
||||
// s_cDataUpdate |= MAG_UPDATE;
|
||||
// break;
|
||||
// case Roll:
|
||||
// case Pitch:
|
||||
case Yaw:
|
||||
@@ -202,28 +236,3 @@ static void Delayms(uint16_t ucMs)
|
||||
{
|
||||
osDelay(pdMS_TO_TICKS(ucMs));
|
||||
}
|
||||
|
||||
static void AutoScanSensor(void)
|
||||
{
|
||||
int i, iRetry;
|
||||
|
||||
for(i = 0; i < 0x7F; i++)
|
||||
{
|
||||
WitInit(WIT_PROTOCOL_I2C, i);
|
||||
iRetry = 2;
|
||||
do
|
||||
{
|
||||
s_cDataUpdate = 0;
|
||||
WitReadReg(AX, 3);
|
||||
Delayms(5);
|
||||
if(s_cDataUpdate != 0)
|
||||
{
|
||||
printf("find %02X addr sensor\r\n", i);
|
||||
return ;
|
||||
}
|
||||
iRetry--;
|
||||
}while(iRetry);
|
||||
}
|
||||
printf("can not find sensor\r\n");
|
||||
printf("please check your connection\r\n");
|
||||
}
|
||||
@@ -4,9 +4,9 @@
|
||||
static bsp_soft_i2c_t soft_i2cs[] = {
|
||||
{
|
||||
.scl_port = GPIOF,
|
||||
.scl_pin = SCL_Pin,
|
||||
.scl_pin = SCL_Pin, // GPIO_PIN_14
|
||||
.sda_port = GPIOF,
|
||||
.sda_pin = SDA_Pin,
|
||||
.sda_pin = SDA_Pin, // GPIO_PIN_12
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@@ -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];
|
||||
}
|
||||
@@ -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
|
||||
@@ -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)构建响应式收发框架。
|
||||
@@ -3,6 +3,9 @@
|
||||
#include "can.h"
|
||||
#include "stm32f4xx_hal_can.h"
|
||||
|
||||
/* CAN 发送超时 (ms) */
|
||||
#define CAN_TX_TIMEOUT_MS 100U
|
||||
|
||||
|
||||
static int hal_can_init(int ch)
|
||||
{
|
||||
@@ -50,9 +53,13 @@ static int hal_can_send(int ch, const can_message_t *msg)
|
||||
if (HAL_CAN_AddTxMessage(hcan, &TxHeader, (uint8_t*)msg->data, &TxMailbox) != HAL_OK)
|
||||
return -1;
|
||||
|
||||
// 等待发送完成
|
||||
while (HAL_CAN_IsTxMessagePending(hcan, TxMailbox))
|
||||
;
|
||||
// 等待发送完成(带超时保护,防止 Bus-Off 死锁)
|
||||
uint32_t tickstart = HAL_GetTick();
|
||||
while (HAL_CAN_IsTxMessagePending(hcan, TxMailbox)) {
|
||||
if ((HAL_GetTick() - tickstart) > CAN_TX_TIMEOUT_MS) {
|
||||
return -1; // 超时 — CAN 总线异常
|
||||
}
|
||||
}
|
||||
|
||||
if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) == 0) {
|
||||
/* 发送完成,邮箱变为空闲 */
|
||||
@@ -117,4 +124,3 @@ void hal_can_init_all(void)
|
||||
{
|
||||
can_register_ops(&ops);
|
||||
}
|
||||
|
||||
@@ -764,25 +764,23 @@ static int hal_soft_i2c_wait_ack_frame(int ch) {
|
||||
if (!bus)
|
||||
return -1;
|
||||
dwt_init();
|
||||
sda_out(bus);
|
||||
gpio_set(bus->sda_port, bus->sda_pin); /* drive SDA high via PP */
|
||||
sda_in(bus); /* switch to input for reading */
|
||||
gpio_set(bus->sda_port, bus->sda_pin); /* pre-set ODR=1, safe in input mode */
|
||||
delay_us(5);
|
||||
sda_in(bus); /* switch to input first (high-Z + PUPDR pull-up) */
|
||||
gpio_set(bus->sda_port, bus->sda_pin); /* ODR=1, safe for future output mode */
|
||||
delay_us(1);
|
||||
/* clock the 9th SCL pulse so slave can assert ACK */
|
||||
gpio_set(bus->scl_port, bus->scl_pin); /* SCL high */
|
||||
delay_us(3); /* tSU;DAT for slave */
|
||||
uint32_t start = DWT->CYCCNT;
|
||||
while (gpio_read(bus->sda_port, bus->sda_pin)) {
|
||||
int ack = gpio_read(bus->sda_port, bus->sda_pin);
|
||||
while (ack) {
|
||||
if ((DWT->CYCCNT - start) > (SystemCoreClock / 100)) {
|
||||
sda_out(bus);
|
||||
gpio_reset(bus->scl_port, bus->scl_pin);
|
||||
gpio_set(bus->scl_port, bus->scl_pin);
|
||||
delay_us(5);
|
||||
gpio_reset(bus->scl_port, bus->scl_pin);
|
||||
return -2;
|
||||
}
|
||||
ack = gpio_read(bus->sda_port, bus->sda_pin);
|
||||
}
|
||||
gpio_set(bus->scl_port, bus->scl_pin);
|
||||
delay_us(5);
|
||||
gpio_reset(bus->scl_port, bus->scl_pin);
|
||||
delay_us(2);
|
||||
gpio_reset(bus->scl_port, bus->scl_pin); /* SCL low; end of ACK bit */
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -832,17 +830,15 @@ static int hal_soft_i2c_read_frame(int ch, int ack) {
|
||||
uint8_t ret = 0;
|
||||
if (!bus)
|
||||
return -1;
|
||||
sda_out(bus);
|
||||
gpio_set(bus->sda_port, bus->sda_pin); /* drive SDA high via PP */
|
||||
sda_in(bus);
|
||||
gpio_set(bus->sda_port, bus->sda_pin); /* pre-set ODR=1 */
|
||||
gpio_set(bus->sda_port, bus->sda_pin); /* ODR=1, safe for future output mode */
|
||||
for (int i = 0; i < 8; i ++) {
|
||||
scl_low(bus);
|
||||
delay_us(5);
|
||||
scl_high(bus);
|
||||
delay_us(10); /* tSU;DAT — let slave data settle */
|
||||
ret <<= 1;
|
||||
if (sda_read(bus)) ret ++;
|
||||
delay_us(5);
|
||||
}
|
||||
|
||||
hal_soft_i2c_send_ack_frame(ch, ack);
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
@@ -0,0 +1,6 @@
|
||||
#ifndef HAL_UART_H
|
||||
#define HAL_UART_H
|
||||
|
||||
void hal_uart_init_all(void);
|
||||
|
||||
#endif
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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
@@ -1,55 +1,56 @@
|
||||
#include "pid.h"
|
||||
#include <string.h>
|
||||
|
||||
void PIDController_Init(PIDController_t *pid)
|
||||
void pid_init(pid_t *pid)
|
||||
{
|
||||
if (!pid) return;
|
||||
pid->Kp = 0.0f;
|
||||
pid->Ki = 0.0f;
|
||||
pid->Kd = 0.0f;
|
||||
pid->integral = 0.0f;
|
||||
pid->prev_error = 0.0f;
|
||||
pid->output_max = 204800.0f; // 电机最大速度 (0.01 DPS)
|
||||
pid->prev_measurement = 0.0f;
|
||||
pid->output_max = 0.0f;
|
||||
pid->integral_max = 0.0f;
|
||||
}
|
||||
|
||||
float PIDController_Compute(PIDController_t *pid, float target, float actual)
|
||||
float pid_compute(pid_t *pid, float target, float actual)
|
||||
{
|
||||
if (!pid) return 0.0f;
|
||||
|
||||
float error = target - actual;
|
||||
|
||||
// 比例项
|
||||
/* 比例项 */
|
||||
float p_term = pid->Kp * error;
|
||||
|
||||
// 积分项 (带抗饱和)
|
||||
/* 微分项(测量值微分 — 避免目标突变微分冲击) */
|
||||
float d_term = pid->Kd * (-(actual - pid->prev_measurement));
|
||||
pid->prev_measurement = actual;
|
||||
|
||||
/* 积分项(带抗饱和限幅) */
|
||||
pid->integral += error;
|
||||
if (pid->integral > pid->output_max) {
|
||||
pid->integral = pid->output_max;
|
||||
} else if (pid->integral < -pid->output_max) {
|
||||
pid->integral = -pid->output_max;
|
||||
if (pid->integral_max > 0.0f) {
|
||||
if (pid->integral > pid->integral_max)
|
||||
pid->integral = pid->integral_max;
|
||||
else if (pid->integral < -pid->integral_max)
|
||||
pid->integral = -pid->integral_max;
|
||||
}
|
||||
float i_term = pid->Ki * pid->integral;
|
||||
|
||||
// 微分项
|
||||
float derivative = error - pid->prev_error;
|
||||
float d_term = pid->Kd * derivative;
|
||||
pid->prev_error = error;
|
||||
|
||||
float output = p_term + i_term + d_term;
|
||||
|
||||
// 输出限幅
|
||||
if (output > pid->output_max) {
|
||||
output = pid->output_max;
|
||||
} else if (output < -pid->output_max) {
|
||||
output = -pid->output_max;
|
||||
/* 输出限幅(0=不限幅) */
|
||||
if (pid->output_max > 0.0f) {
|
||||
if (output > pid->output_max)
|
||||
output = pid->output_max;
|
||||
else if (output < -pid->output_max)
|
||||
output = -pid->output_max;
|
||||
}
|
||||
|
||||
return output;
|
||||
}
|
||||
|
||||
void PIDController_Reset(PIDController_t *pid)
|
||||
void pid_reset(pid_t *pid)
|
||||
{
|
||||
if (!pid) return;
|
||||
pid->integral = 0.0f;
|
||||
pid->prev_error = 0.0f;
|
||||
pid->prev_measurement = 0.0f;
|
||||
}
|
||||
@@ -1,22 +1,21 @@
|
||||
#ifndef __PID_H__
|
||||
#define __PID_H__
|
||||
#ifndef PID_H
|
||||
#define PID_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
typedef struct {
|
||||
float Kp, Ki, Kd;
|
||||
float integral;
|
||||
float prev_error;
|
||||
float output_max;
|
||||
} PIDController_t;
|
||||
float prev_measurement; /* 上一拍测量值(微分用) */
|
||||
float output_max; /* 输出限幅(0=不限幅) */
|
||||
float integral_max; /* 积分限幅,抗饱和(0=不限幅) */
|
||||
} pid_t;
|
||||
|
||||
void PIDController_Init(PIDController_t *pid);
|
||||
float PIDController_Compute(PIDController_t *pid, float target, float actual);
|
||||
void PIDController_Reset(PIDController_t *pid);
|
||||
void pid_init(pid_t *pid);
|
||||
float pid_compute(pid_t *pid, float target, float actual);
|
||||
void pid_reset(pid_t *pid);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
|
||||
@@ -14,6 +14,9 @@
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
/* CAN 接收轮询超时(重试次数) */
|
||||
#define MF4010_RECV_RETRIES 2000
|
||||
|
||||
#define WRITE_FLAG(data, flag) do {(data) |= flag; } while(0)
|
||||
#define ERASE_FLAG(data, flag) do {(data) &= ~flag; } while(0)
|
||||
/**
|
||||
@@ -44,25 +47,41 @@ int mf4010v2_init(mf4010v2_t* motor, uint16_t id, int can_ch)
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 发送命令并接收响应(带超时)
|
||||
* @brief 发送命令并接收响应(带超时轮询)
|
||||
*/
|
||||
int mf4010v2_send_command(mf4010v2_t* motor, const uint8_t command[8])
|
||||
{
|
||||
can_message_t msg;
|
||||
int recv_ok = 0;
|
||||
|
||||
if (!motor) return MF4010_ERR_INVALID_PARAM;
|
||||
|
||||
msg.id = motor->id;
|
||||
memcpy(msg.data, command, 8);
|
||||
msg.length = 8;
|
||||
|
||||
int ret = can_if_send(motor->can_ch, &msg);
|
||||
if (ret != 0) {
|
||||
motor->error_count++;
|
||||
printf("Error: Failed to send CAN message (ID 0x%03X)\r\n", motor->id);
|
||||
// printf("Error: Failed to send CAN message (ID 0x%03X)\r\n", motor->id);
|
||||
return MF4010_ERR_CAN_SEND;
|
||||
}
|
||||
can_if_recv(motor->can_ch, &msg);
|
||||
if (msg.id != motor->id) {
|
||||
motor->error_count++;
|
||||
return MF4010_ERR_CAN_RECV;
|
||||
|
||||
/* 轮询等待电机响应(带超时) */
|
||||
for (int retry = 0; retry < MF4010_RECV_RETRIES; retry++) {
|
||||
ret = can_if_recv(motor->can_ch, &msg);
|
||||
if (ret == 0 && msg.id == motor->id) {
|
||||
recv_ok = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!recv_ok) {
|
||||
motor->error_count++;
|
||||
printf("Error: Timeout waiting for motor response (ID 0x%03X)\r\n", motor->id);
|
||||
return MF4010_ERR_NO_RESPONSE;
|
||||
}
|
||||
|
||||
memcpy(motor->last_response, msg.data, 8);
|
||||
motor->success_count++;
|
||||
return MF4010_OK;
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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.
Reference in new issue
Block a user