商场扫地机器人 —— 嵌入式软件工程师全栈实战手册
🤖 商场扫地机器人 —— 嵌入式软件工程师全栈实战手册
定位：STM32F405 + FreeRTOS + 惯导弓字算法 | BOM ≤ ¥1000
目标：让你能从零搭建固件，并能清晰地向任何人复述整个项目架构
一、项目全局架构图（你必须能画出这张图）
┌─────────────────────────────────────────────────────────────────┐
│ 商场扫地机器人固件 │
│ STM32F405RGT6 + FreeRTOS │
│ │
│ ┌─────────────────────────────────────────────────────────┐ │
│ │ 应用层 (业务逻辑) │ │
│ │ │ │
│ │ ┌──────────┐ ┌──────────┐ ┌──────────┐ │ │
│ │ │ 任务调度 │ │ 清扫策略 │ │ 异常处理 │ │ │
│ │ │ 状态机 │ │ 弓字+沿边 │ │ 故障恢复 │ │ │
│ │ └────┬─────┘ └────┬─────┘ └────┬─────┘ │ │
│ │ │ │ │ │ │
│ └────────┼──────────────┼──────────────┼───────────────────┘ │
│ │ │ │ │
│ ┌────────▼──────────────▼──────────────▼───────────────────┐ │
│ │ 中间层 (算法与服务) │ │
│ │ │ │
│ │ ┌─────────┐ ┌─────────┐ ┌─────────┐ ┌─────────┐ │ │
│ │ │ 航迹推算 │ │ PID控制 │ │ 路径规划 │ │ 传感器 │ │ │
│ │ │ EKF融合 │ │ 速度环 │ │ 弓字生成 │ │ 融合层 │ │ │
│ │ └────┬────┘ └────┬────┘ └────┬────┘ └────┬────┘ │ │
│ └───────┼───────────┼───────────┼───────────┼─────────────┘ │
│ │ │ │ │ │
│ ┌───────▼───────────▼───────────▼───────────▼─────────────┐ │
│ │ 驱动层 (外设抽象) │ │
│ │ │ │
│ │ ┌──────┐ ┌──────┐ ┌──────┐ ┌──────┐ ┌──────┐ │ │
│ │ │ I2C │ │ SPI │ │ UART │ │ TIM │ │ GPIO │ │ │
│ │ │ IMU │ │ 编码器│ │ WiFi │ │ PWM │ │ 碰撞 │ │ │
│ │ │ ToF │ │ │ │ 调试 │ │ FOC │ │ 水泵 │ │ │
│ │ └──────┘ └──────┘ └──────┘ └──────┘ └──────┘ │ │
│ └─────────────────────────────────────────────────────────┘ │
│ │
│ ┌─────────────────────────────────────────────────────────┐ │
│ │ OS层 (FreeRTOS) │ │
│ │ 任务调度 | 队列 | 信号量 | 互斥锁 | 定时器 | 事件标志 │ │
│ └─────────────────────────────────────────────────────────┘ │
│ │
│ ┌─────────────────────────────────────────────────────────┐ │
│ │ 硬件层 (STM32 HAL) │ │
│ │ RCC | GPIO | TIM | I2C | SPI | UART | ADC | DMA | NVIC │ │
│ └─────────────────────────────────────────────────────────┘ │
└─────────────────────────────────────────────────────────────────┘
复述口诀：硬件层提供外设能力 → 驱动层封装成API → 中间层实现算法 → 应用层组织业务 → FreeRTOS调度一切
二、开发环境搭建（第0步）
2.1 工具链清单
| 工具 | 版本 | 用途 | 备注 |
|---|---|---|---|
| STM32CubeMX | 6.x | 图形化配置引脚/时钟/外设/FreeRTOS | 生成初始化代码 |
| Keil MDK-ARM | 5.x | 编译/调试/下载 | 或选IAR/GCC |
| STM32CubeIDE | 1.x | (备选) 集成Eclipse+GCC | 免费，适合习惯Eclipse的人 |
| J-Link / ST-Link | V2+ | 下载+在线调试+断点 | ST-Link便宜够用 |
| 串口调试助手 | SSCOM / XCOM | 日志输出+指令调试 | 必备 |
| STM32CubeMonitor | 1.x | 实时变量监控+波形显示 | 调PID神器 |
| 逻辑分析仪 | Saleae/杂牌 | 抓I2C/SPI/PWM时序 | ¥50淘宝杂牌够用 |
2.2 CubeMX工程配置清单
打开CubeMX，选择 STM32F405RGTx，按以下清单逐一配置：
📌 引脚分配 (Pinout)
────────────────────────────────────────────
PA0-PA3 → ADC1_IN0~IN3 (电流检测×2 + 电压检测×2)
PA8-PA10 → TIM1_CH1~CH3 (FOC/六步三相PWM U/V/W → DRV8313 左轮)
PA11 → TIM1_BKIN (硬件保护输入)
PB6-PB7 → I2C1_SCL/SDA (IMU ICM-42688)
PB8-PB9 → I2C2_SCL/SDA (ToF VL53L1X ×4, 带I2C Switch)
PC0-PC3 → GPIO_Output (VL53L1X I2C Switch选择线)
PC4 → GPIO_Input (碰撞开关左)
PC5 → GPIO_Input (碰撞开关右)
PC6 → TIM3_CH1 (主刷电机PWM)
PC7 → TIM3_CH2 (边刷左PWM)
PC8 → TIM3_CH3 (边刷右PWM)
PC9 → GPIO_Output (水泵开关)
PD2 → GPIO_Input (风机霍尔A)
PD5-PD6 → USART2_TX/RX (调试串口)
PD12-PD14 → TIM4_CH1~CH3 (风机/右轮三相PWM)
PE0 → GPIO_Input (E-STOP急停按钮)
PE2 → TIM9_CH1 (编码器A相 - 左轮)
PE3 → GPIO_Input (编码器B相 - 左轮)
PE4 → TIM10_CH1 (编码器A相 - 右轮)
PE5 → GPIO_Input (编码器B相 - 右轮)
📌 时钟配置 (Clock Configuration)
────────────────────────────────────────────
HSE = 8MHz (外部晶振)
PLL_M = 8, PLL_N = 336, PLL_P = 2, PLL_Q = 7
→ SYSCLK = 168MHz
→ AHB = 168MHz, APB1 = 42MHz, APB2 = 84MHz
📌 外设配置 (Peripherals)
────────────────────────────────────────────
I2C1 → 400KHz (Fast Mode), 7-bit地址
I2C2 → 400KHz
TIM1 → 3路PWM, 死区200ns, 频率20kHz (左轮驱动)
TIM3 → 3路PWM, 频率10kHz (刷子电机)
TIM4 → 3路PWM, 频率20kHz (右轮驱动/风机)
TIM9 → 编码器模式, 4倍频 (左轮)
TIM10→ 编码器模式, 4倍频 (右轮)
USART2 → 115200, 8N1 (调试/ESP8266)
ADC1 → 12-bit, 扫描模式, DMA传输
IWDG → 预分频64, 重载值625 → 超时约1秒
📌 Middleware配置
────────────────────────────────────────────
FREERTOS → CMSIS_V2 (Interface)
→ 添加以下任务:
Task_MotionControl (优先级: Highest)
Task_Navigation (优先级: High)
Task_SensorMonitor (优先级: Above Normal)
Task_CleaningControl (优先级: Normal)
Task_Communication (优先级: Below Normal)
Task_SafetyMonitor (优先级: Highest)
→ 添加以下队列:
Queue_SensorData (深度20, 元素: SensorPacket_t)
Queue_StatusReport (深度5, 元素: StatusReport_t)
Queue_FaultEvent (深度5, 元素: FaultEvent_t)
→ 添加以下信号量/互斥锁:
Mutex_I2C1 (互斥锁, I2C1总线保护)
Mutex_I2C2 (互斥锁, I2C2总线保护)
Sem_CollisionDetected(二值信号量)
三、工程目录结构（按规范组织代码）
robot_vacuum/
├── Core/ ← CubeMX生成的基础文件(不要手动改)
│ ├── Inc/
│ └── Src/
│ ├── main.c
│ ├── stm32f4xx_it.c ← 中断服务函数
│ └── freertos.c ← FreeRTOS默认任务创建
│
├── Drivers/ ← 驱动层(你写的外设驱动)
│ ├── bsp_imu/ ← ICM-42688 IMU驱动
│ ├── bsp_tof/ ← VL53L1X ToF驱动
│ ├── bsp_encoder/ ← MT6701编码器驱动
│ ├── bsp_motor/ ← DRV8313 BLDC驱动(六步换相)
│ ├── bsp_ultrasonic/ ← HC-SR04P超声波驱动
│ ├── bsp_buzzer/ ← 蜂鸣器驱动
│ ├── bsp_led/ ← WS2812B LED驱动
│ └── bsp_uart_wifi/ ← ESP8266 AT指令驱动
│
├── Middlewares/ ← 中间层(算法与服务)
│ ├── algo_odometry/ ← 航迹推算(EKF/互补滤波)
│ ├── algo_pid/ ← 通用PID控制器
│ ├── algo_path/ ← 路径规划(弓字形)
│ ├── algo_navigation/ ← 导航状态机
│ └── algo_charging/ ← 自动回充算法
│
├── App/ ← 应用层(FreeRTOS任务实现)
│ ├── task_motion.c ← 运动控制任务
│ ├── task_navigation.c ← 导航任务
│ ├── task_sensor.c ← 传感器监控任务
│ ├── task_cleaning.c ← 清洁控制任务
│ ├── task_comm.c ← 通信任务
│ ├── task_safety.c ← 安全监控任务
│ ├── app_config.h ← 全局配置宏定义
│ └── app_typedef.h ← 全局类型定义
│
└── Libs/ ← 第三方库
├── FreeRTOS/
├── CMSIS_DSP/ ← ARM数学库(FPU加速)
└── ST_VL53L1X_API/ ← ST官方ToF API
复述要点：
Drivers管硬件怎么动 →Middlewares管算法怎么算 →App管任务怎么跑 → 三层之间只通过.h头文件暴露接口，禁止跨层直接调用。
四、核心数据结构定义（先定义，后写逻辑）
// ============================================
// app_typedef.h — 全局类型定义
// ============================================
#ifndef __APP_TYPEDEF_H
#define __APP_TYPEDEF_H
#include <stdint.h>
#include <stdbool.h>
/* ─────────── 1. 速度指令结构体 ─────────── */
typedef struct {
float linear_x; // 前进速度 m/s (正=前进, 负=后退)
float angular_z; // 旋转速度 rad/s (正=逆时针)
uint32_t timestamp_ms;
} VelocityCmd_t;
/* ─────────── 2. IMU姿态数据 ─────────── */
typedef struct {
float yaw; // 航向角 rad
float gyro_z; // Z轴角速度 rad/s
float accel_x; // X轴加速度 m/s²
float accel_y; // Y轴加速度 m/s²
} ImuData_t;
/* ─────────── 3. 里程计位姿 ─────────── */
typedef struct {
float x; // 全局坐标X (米)
float y; // 全局坐标Y (米)
float yaw; // 全局航向 (rad)
float v_linear; // 当前线速度 m/s
float v_angular; // 当前角速度 rad/s
} Odometry_t;
/* ─────────── 4. 传感器数据包 ─────────── */
typedef struct {
uint16_t cliff_distance_mm[4]; // 跌落ToF: 前左/前右/后左/后右
bool cliff_alarm[4]; // true=检测到悬崖
uint16_t ultra_distance_mm[2]; // 超声波: 前方/侧方
bool bump_left; // 碰撞开关
bool bump_right;
float battery_voltage; // 电池电压 V
} SensorPacket_t;
/* ─────────── 5. 导航状态枚举 (状态机核心) ─────────── */
typedef enum {
NAV_STATE_IDLE = 0, // 待机
NAV_STATE_CLEANING, // 正常清扫(弓字形)
NAV_STATE_OBSTACLE_AVOID, // 避障中
NAV_STATE_STUCK_RECOVERY, // 被困脱困
NAV_STATE_RETURNING, // 回充中
NAV_STATE_CHARGING, // 充电中
NAV_STATE_ERROR, // 故障
} NavState_t;
/* ─────────── 6. 故障事件 ─────────── */
typedef enum {
FAULT_NONE = 0,
FAULT_CLIFF_DETECTED, // 悬崖
FAULT_COLLISION, // 碰撞
FAULT_OVER_CURRENT, // 过流
FAULT_LOW_BATTERY, // 低电量
FAULT_ESTOP_PRESSED, // 急停按下
} FaultCode_t;
typedef struct {
FaultCode_t code;
uint32_t timestamp_ms;
uint8_t severity; // 0=警告, 1=严重(需停机)
} FaultEvent_t;
/* ─────────── 7. 状态上报包 (→WiFi) ─────────── */
typedef struct {
NavState_t nav_state;
Odometry_t odom;
float battery_voltage;
FaultCode_t current_fault;
} StatusReport_t;
#endif // __APP_TYPEDEF_H
五、驱动层实现细节（逐个模块）
5.1 IMU驱动：ICM-42688-P (I2C)
// ============================================
// bsp_icm42688.c — 核心实现
// ============================================
#include "bsp_icm42688.h"
#include "main.h"
extern I2C_HandleTypeDef hi2c1;
extern osMutexId_t mutexI2C1Handle;
#define GYRO_SENSITIVITY 16.384f // ±2000dps
#define ACCEL_SENSITIVITY 8192.0f // ±4g
static float gyro_offset_z = 0.0f;
static HAL_StatusTypeDef icm_write_reg(uint8_t reg, uint8_t val) {
uint8_t buf[2] = {reg, val};
osMutexAcquire(mutexI2C1Handle, osWaitForever);
HAL_StatusTypeDef ret = HAL_I2C_Master_Transmit(&hi2c1, 0x68 << 1, buf, 2, 100);
osMutexRelease(mutexI2C1Handle);
return ret;
}
static HAL_StatusTypeDef icm_read_regs(uint8_t reg, uint8_t *buf, uint8_t len) {
osMutexAcquire(mutexI2C1Handle, osWaitForever);
HAL_I2C_Master_Transmit(&hi2c1, 0x68 << 1, ®, 1, 100);
HAL_StatusTypeDef ret = HAL_I2C_Master_Receive(&hi2c1, 0x68 << 1, buf, len, 100);
osMutexRelease(mutexI2C1Handle);
return ret;
}
HAL_StatusTypeDef ICM42688_Init(void) {
uint8_t whoami = 0;
icm_read_regs(0x75, &whoami, 1);
if (whoami != 0x47) return HAL_ERROR;
icm_write_reg(0x4F, 0x36); // 陀螺仪: ±2000dps, 200Hz
icm_write_reg(0x50, 0x56); // 加速度: ±4g, 200Hz
icm_write_reg(0x4E, 0x0F); // 开启低噪声模式
HAL_Delay(50);
// 静止零点校准
float sum_gz = 0;
for (int i = 0; i < 100; i++) {
int16_t accel[3], gyro[3];
ICM42688_ReadRaw(accel, gyro);
sum_gz += (float)gyro[2] / GYRO_SENSITIVITY;
HAL_Delay(5);
}
gyro_offset_z = sum_gz / 100.0f;
return HAL_OK;
}
HAL_StatusTypeDef ICM42688_GetImuData(ImuData_t *imu) {
int16_t accel[3], gyro[3];
uint8_t buf[12];
HAL_StatusTypeDef ret = icm_read_regs(0x1F, buf, 12);
if (ret != HAL_OK) return ret;
accel[0] = (int16_t)((buf[0] << 8) | buf[1]);
gyro[2] = (int16_t)((buf[10] << 8) | buf[11]);
imu->accel_x = (float)accel[0] / ACCEL_SENSITIVITY * 9.81f;
imu->gyro_z = (float)gyro[2] / GYRO_SENSITIVITY * 0.0174533f - gyro_offset_z;
return HAL_OK;
}
5.2 编码器驱动：硬件TIM编码器模式
// ============================================
// bsp_mt6701.c
// ============================================
#include "bsp_mt6701.h"
#include "main.h"
extern TIM_HandleTypeDef htim9; // 左轮
extern TIM_HandleTypeDef htim10; // 右轮
#define WHEEL_PERIMETER_M 0.204f // 轮周长 (假设轮径65mm)
#define COUNTS_PER_REV 65536 // 16bit定时器4倍频后一圈总脉冲
static int32_t total_ticks[2] = {0, 0};
static int16_t last_ticks[2] = {0, 0};
void Encoder_Init(void) {
HAL_TIM_Encoder_Start(&htim9, TIM_CHANNEL_ALL);
HAL_TIM_Encoder_Start(&htim10, TIM_CHANNEL_ALL);
}
int32_t Encoder_GetTicks(uint8_t wheel) {
TIM_HandleTypeDef *htim = (wheel == 0) ? &htim9 : &htim10;
int16_t raw = (int16_t)__HAL_TIM_GET_COUNTER(htim);
int16_t delta = raw - last_ticks[wheel];
total_ticks[wheel] += delta;
last_ticks[wheel] = raw;
return total_ticks[wheel];
}
float Encoder_GetVelocity(uint8_t wheel, float dt_sec) {
int32_t curr = Encoder_GetTicks(wheel);
static int32_t last[2] = {0, 0};
int32_t delta = curr - last[wheel];
last[wheel] = curr;
float dist = (float)delta / COUNTS_PER_REV * WHEEL_PERIMETER_M;
return dist / dt_sec; // m/s
}
5.3 电机驱动：DRV8313 BLDC 六步换相
// ============================================
// bsp_motor.c — BLDC 六步换相 + PID速度控制
// ============================================
#include "bsp_motor.h"
#include "algo_pid.h"
extern TIM_HandleTypeDef htim1; // 左轮
extern TIM_HandleTypeDef htim4; // 右轮
static PID_t speed_pid[2];
static float target_speed[2] = {0, 0};
static float current_pwm[2] = {0, 0};
void Motor_Init(void) {
PID_Init(&speed_pid[0], 2.0f, 0.5f, 0.1f, 100.0f, 0.0f, 50.0f);
PID_Init(&speed_pid[1], 2.0f, 0.5f, 0.1f, 100.0f, 0.0f, 50.0f);
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
// ... 启动其他通道
}
void Motor_SetSpeed(uint8_t wheel, float speed_ms) {
target_speed[wheel] = speed_ms;
}
// 在1kHz任务中调用
void Motor_UpdatePID(float v_left, float v_right) {
float v[2] = {v_left, v_right};
TIM_HandleTypeDef *htim[2] = {&htim1, &htim4};
for (int i = 0; i < 2; i++) {
float error = target_speed[i] - v[i];
current_pwm[i] = PID_Calculate(&speed_pid[i], error);
// 简化：直接将PWM占空比应用到当前导通相
// 实际需结合霍尔状态查表输出
uint16_t pwm_val = (uint16_t)(current_pwm[i] * 10.0f); // 假设ARR=1000
__HAL_TIM_SET_COMPARE(htim[i], TIM_CHANNEL_1, pwm_val);
}
}
void Motor_StopAll(void) {
target_speed[0] = 0; target_speed[1] = 0;
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0);
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 0);
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 0);
// 右轮同理...
}
六、中间层算法实现
6.1 通用PID控制器
// ============================================
// algo_pid.c
// ============================================
#include "algo_pid.h"
void PID_Init(PID_t *pid, float kp, float ki, float kd,
float out_max, float out_min, float integral_max) {
pid->kp = kp; pid->ki = ki; pid->kd = kd;
pid->output_max = out_max; pid->output_min = out_min;
pid->integral_max = integral_max;
pid->integral = 0; pid->prev_error = 0;
}
float PID_Calculate(PID_t *pid, float error) {
pid->integral += error;
if (pid->integral > pid->integral_max) pid->integral = pid->integral_max;
if (pid->integral < -pid->integral_max) pid->integral = -pid->integral_max;
float derivative = error - pid->prev_error;
pid->prev_error = error;
float output = pid->kp * error + pid->ki * pid->integral + pid->kd * derivative;
if (output > pid->output_max) output = pid->output_max;
if (output < pid->output_min) output = pid->output_min;
return output;
}
6.2 航迹推算（Dead Reckoning + 陀螺仪融合）
// ============================================
// algo_odometry.c — 里程计核心算法
// ============================================
#include "algo_odometry.h"
#include "arm_math.h"
static Odometry_t odom = {0};
static uint32_t last_update_ms = 0;
#define WHEEL_BASE_M 0.28f // 轮距
void Odometry_Init(void) {
odom.x = 0; odom.y = 0; odom.yaw = 0;
last_update_ms = HAL_GetTick();
}
// 50Hz周期调用
void Odometry_Update(ImuData_t *imu, float v_left, float v_right) {
uint32_t now_ms = HAL_GetTick();
float dt = (now_ms - last_update_ms) / 1000.0f;
if (dt <= 0 || dt > 0.1f) { last_update_ms = now_ms; return; }
last_update_ms = now_ms;
float v_center = (v_left + v_right) / 2.0f;
float omega_enc = (v_right - v_left) / WHEEL_BASE_M;
// 互补滤波: 95%陀螺仪 + 5%编码器
float omega = 0.95f * imu->gyro_z + 0.05f * omega_enc;
odom.yaw += omega * dt;
while (odom.yaw > 3.14159f) odom.yaw -= 6.28318f;
while (odom.yaw < -3.14159f) odom.yaw += 6.28318f;
float mid_yaw = odom.yaw - omega * dt / 2.0f;
odom.x += v_center * arm_cos_f32(mid_yaw) * dt;
odom.y += v_center * arm_sin_f32(mid_yaw) * dt;
odom.v_linear = v_center;
odom.v_angular = omega;
}
Odometry_t Odometry_Get(void) { return odom; }
6.3 导航状态机（顶层调度器）
// ============================================
// nav_fsm.c — 导航有限状态机
// ============================================
#include "nav_fsm.h"
static NavState_t current_state = NAV_STATE_IDLE;
static uint32_t state_enter_ms = 0;
void NavFSM_Update(SensorPacket_t *sensors, Odometry_t *odo) {
NavState_t next_state = current_state;
// ════ 全局中断条件 ════════
if (sensors->cliff_alarm[0] || sensors->cliff_alarm[1]) {
Motor_StopAll();
next_state = NAV_STATE_STUCK_RECOVERY;
}
if (sensors->battery_voltage < 13.2f && current_state == NAV_STATE_CLEANING) {
next_state = NAV_STATE_RETURNING;
}
// ════════ 状态转移逻辑 ════════
switch (current_state) {
case NAV_STATE_IDLE:
if (start_cmd_received) next_state = NAV_STATE_CLEANING;
break;
case NAV_STATE_CLEANING: {
// 执行弓字形算法获取速度指令...
VelocityCmd_t cmd = Boustro_Update(odo);
float v_l = cmd.linear_x - cmd.angular_z * WHEEL_BASE_M / 2.0f;
float v_r = cmd.linear_x + cmd.angular_z * WHEEL_BASE_M / 2.0f;
Motor_SetSpeed(0, v_l);
Motor_SetSpeed(1, v_r);
if (sensors->bump_left || sensors->bump_right || sensors->ultra_distance_mm[0] < 150) {
Motor_StopAll();
next_state = NAV_STATE_OBSTACLE_AVOID;
}
break;
}
case NAV_STATE_OBSTACLE_AVOID: {
uint32_t elapsed = HAL_GetTick() - state_enter_ms;
if (elapsed < 300) {
Motor_SetSpeed(0, -0.15f); Motor_SetSpeed(1, -0.15f); // 后退
} else if (elapsed < 1500) {
Motor_SetSpeed(0, -0.3f); Motor_SetSpeed(1, 0.3f); // 右转
} else {
next_state = NAV_STATE_CLEANING;
}
break;
}
case NAV_STATE_ERROR:
Motor_StopAll();
break;
default: break;
}
if (next_state != current_state) {
current_state = next_state;
state_enter_ms = HAL_GetTick();
}
}
七、FreeRTOS任务实现（App层）
7.1 任务总览与调度关系
| 优先级 | 任务名 | 周期 | 职责 |
|---|---|---|---|
| Highest(6) | Task_SafetyMonitor | 1ms | 急停/过流/跌落紧急处理、喂狗 |
| Highest(6) | Task_MotionControl | 1ms | PID更新+换相 |
| High(5) | Task_Navigation | 20ms | 状态机+路径规划+里程计 |
| AboveNorm(4) | Task_SensorMonitor | 10ms | 传感器采集+滤波 |
| Normal(3) | Task_CleaningControl | 50ms | 清洁机构控制 |
| BelowNorm(2) | Task_Communication | 100ms | WiFi上报+接收指令 |
7.2 核心任务代码
// ============================================
// task_safety.c — 安全监控 (最高优先级, 1ms)
// ============================================
void Task_SafetyMonitor(void *pvParameters) {
TickType_t xLastWake = xTaskGetTickCount();
for (;;) {
if (HAL_GPIO_ReadPin(GPIOE, GPIO_PIN_0) == GPIO_PIN_RESET) { // E-STOP
Motor_StopAll();
FaultEvent_t fault = {FAULT_ESTOP_PRESSED, HAL_GetTick(), 1};
xQueueSend(queueFaultHandle, &fault, 0);
}
HAL_IWDG_Refresh(&hiwdg); // 喂狗
vTaskDelayUntil(&xLastWake, pdMS_TO_TICKS(1));
}
}
// ============================================
// task_navigation.c — 导航主任务 (20ms)
// ============================================
void Task_Navigation(void *pvParameters) {
TickType_t xLastWake = xTaskGetTickCount();
SensorPacket_t sensors;
for (;;) {
xQueueReceive(queueSensorDataHandle, &sensors, 0);
ImuData_t imu;
ICM42688_GetImuData(&imu);
float v_l = Encoder_GetVelocity(0, 0.02f);
float v_r = Encoder_GetVelocity(1, 0.02f);
Odometry_Update(&imu, v_l, v_r);
Odometry_t odo = Odometry_Get();
NavFSM_Update(&sensors, &odo);
vTaskDelayUntil(&xLastWake, pdMS_TO_TICKS(20));
}
}
// ============================================
// task_motion.c — 运动控制 (1ms)
// ============================================
void Task_MotionControl(void *pvParameters) {
TickType_t xLastWake = xTaskGetTickCount();
for (;;) {
float v_l = Encoder_GetVelocity(0, 0.001f);
float v_r = Encoder_GetVelocity(1, 0.001f);
Motor_UpdatePID(v_l, v_r);
vTaskDelayUntil(&xLastWake, pdMS_TO_TICKS(1));
}
}
八、任务间通信拓扑图（面试必画）
┌──────────────────┐
│ Task_SafetyMon │ (1ms, Highest)
│ 急停/过流/喂狗 │
└───────┬──────────┘
│ Queue_FaultEvent
▼
┌──────────────┐ ┌──────────────────┐ ┌──────────────────┐
│Task_Sensor │───>│ Task_Navigation │<───│ Task_Comm │
│ (10ms) │ Q │ (20ms) │ Q │ (100ms) │
│ IMU/ToF/超声│ S │ 状态机+里程计 │ S │ WiFi上报/接收 │
└──────────────┘ └───────┬──────────┘ └──────────────────┘
│ 直接调用 Motor_SetSpeed()
▼
┌──────────────────┐
│Task_MotionCtrl │ (1ms, Highest)
│ PID计算+PWM输出 │
└──────────────────┘
复述要点：传感器任务采集数据放入队列 → 导航任务从队列取出数据+运行状态机 → 直接调用电机API设置速度 → 运动控制任务1ms周期执行PID。任务间只用队列通信，不共享全局变量。
九、一句话复述整个项目（面试/汇报用）
"我做了一个基于 STM32F405 + FreeRTOS 的商场扫地机器人固件。
底层用 DRV8313 六步换相 驱动两个BLDC轮子，硬件TIM编码器模式 测速，ICM-42688 IMU 测姿态。
中间层实现了 陀螺仪+编码器互补滤波的航迹推算，以及 弓字形全覆盖路径规划。
顶层是一个 多状态的有限状态机，管理清扫、避障、脱困、回充的全流程。
6个FreeRTOS任务按优先级调度：1ms安全监控和PID控制 → 20ms导航决策 → 10ms传感器采集，任务间用队列通信，I2C总线用互斥锁保护。
整机BOM成本控制在1000元以内。"
🔧 三大核心模块深度实现手册
本文档是《商场扫地机器人实战手册》的技术深潜篇，逐行拆解 DRV8313六步换相、TIM编码器测速、ICM-42688姿态解算 的完整实现。
一、DRV8313 BLDC 六步换相驱动
1.1 硬件接线原理
DRV8313 有 3个IN引脚 (PWM输入) 和 3个EN引脚 (使能控制)，这是理解六步换相的关键。
DRV8313
┌───────────────────┐
STM32 │ │ BLDC Motor
───── │ │ ──────────
PA8 ──────►│ IN1 OUT1 ──────┼──────► U相
PA9 ──────►│ IN2 OUT2 ──────┼──────► V相
PA10 ─────►│ IN3 OUT3 ──────┼──────► W相
│ │
PB0 ──────►│ EN1 │
PB1 ──────►│ EN2 │ ┌── HALL_A ──► PC6 (GPIO_EXTI)
PB2 ──────►│ EN3 │ ├── HALL_B ──► PC7 (GPIO_EXTI)
│ │ └── HALL_C ──► PC8 (GPIO_EXTI)
24V ──────►│ VM │
GND ──────►│ GND PGND1~3 │
└───────────────────┘
1.2 DRV8313 真值表（必须背下来）
| INx | ENx | 上桥MOS | 下桥MOS | 相线状态 |
|---|---|---|---|---|
| PWM | 1 | PWM | 互补PWM | H_PWM-L_PWM (FOC用) |
| 1 | 1 | 常开 | 关断 | 高电平 (H_ON) |
| 0 | 1 | 关断 | 常开 | 低电平 (L_ON) |
| X | 0 | 关断 | 关断 | 悬空/高阻 (FLOAT) |
六步换相采用 H_PWM-L_ON 模式：
- 导通相（上桥PWM）：
INx = PWM, ENx = 1- 回流相（下桥常开）：
INx = 0, ENx = 1- 悬空相（完全关断）：
ENx = 0（INx随意）
1.3 CubeMX 配置
📌 TIM1 配置 (高级定时器, 3路PWM)
─────────────────────────────────
Channel1 → PA8 → PWM Generation CH1
Channel2 → PA9 → PWM Generation CH2
Channel3 → PA10 → PWM Generation CH3
Prescaler = 0 (不分频, 168MHz)
Counter Mode = Up
Counter Period = 999 (ARR=999, PWM频率 = 168MHz/1/1000 = 168kHz → 太高!)
✅ 正确配置:
Prescaler = 7 (168MHz / (7+1) = 21MHz)
Counter Period = 1049 (ARR=1049, PWM频率 = 21MHz/1050 = 20kHz)
→ 20kHz 超出人耳范围, 电机静音运行
Pulse Mode = PWM mode 1
Output compare N polarity = Low (互补通道极性)
Dead time = 20 (约200ns死区, 防止上下管直通)
⚠️ 注意: 我们不用互补通道! 只用CH1/CH2/CH3的正向PWM。
EN引脚用普通GPIO控制。
📌 EN引脚配置
─────────────────────────────────
PB0 → GPIO_Output (EN1, 默认低电平)
PB1 → GPIO_Output (EN2, 默认低电平)
PB2 → GPIO_Output (EN3, 默认低电平)
📌 霍尔传感器输入 (外部中断)
─────────────────────────────────
PC6 → GPIO_EXTI6 (HALL_A, 上升+下降沿触发)
PC7 → GPIO_EXTI7 (HALL_B, 上升+下降沿触发)
PC8 → GPIO_EXTI8 (HALL_C, 上升+下降沿触发)
全部配置为: Input Pull-up, External Interrupt Mode
1.4 六步换相核心代码
// ============================================
// bsp_motor.c — DRV8313 六步换相完整实现
// ============================================
#include "bsp_motor.h"
#include "main.h"
#include "algo_pid.h"
extern TIM_HandleTypeDef htim1;
// ─── DRV8313 引脚宏定义 ───
#define EN1_PIN GPIO_PIN_0
#define EN2_PIN GPIO_PIN_1
#define EN3_PIN GPIO_PIN_2
#define EN_GPIO GPIOB
#define IN1_TIM &htim1
#define IN1_CH TIM_CHANNEL_1
#define IN2_TIM &htim1
#define IN2_CH TIM_CHANNEL_2
#define IN3_TIM &htim1
#define IN3_CH TIM_CHANNEL_3
#define PWM_ARR 1049 // 与CubeMX中Counter Period一致
// ─── 霍尔传感器引脚 ───
#define HALL_A_PIN GPIO_PIN_6
#define HALL_B_PIN GPIO_PIN_7
#define HALL_C_PIN GPIO_PIN_8
#define HALL_GPIO GPIOC
// ─── PID控制器 ───
static PID_t speed_pid;
static float target_speed_ms = 0.0f;
// ═══════════════════════════════════════════
// 六步换相表 (H_PWM-L_ON 模式)
// ═══════════════════════════════════════════
//
// BLDC电机有6个换相扇区, 由3个霍尔信号组合决定:
// 霍尔状态 = (HALL_C << 2) | (HALL_B << 1) | HALL_A
// 有效状态: 1,2,3,4,5,6 (0和7无效)
//
// 对于每个状态:
// - PWM相: INx=PWM, ENx=1 (上桥PWM调制)
// - ON相: INx=0, ENx=1 (下桥常开, 作为回流路径)
// - OFF相: ENx=0 (该相悬空)
typedef struct {
uint8_t pwm_phase; // 0=U相, 1=V相, 2=W相
uint8_t on_phase; // 0=U相, 1=V相, 2=W相
uint8_t off_phase; // 0=U相, 1=V相, 2=W相
int8_t direction; // +1=正转扭矩方向, -1=反转
} CommStep_t;
// 正转换相表 (顺时针)
static const CommStep_t comm_fwd[7] = {
// idx PWM相 ON相 OFF相 方向
{0, 0, 0, 0}, // 0: 无效
{0, 1, 2, +1}, // 1: U-PWM, V-ON, W-OFF (扇区1)
{0, 2, 1, +1}, // 2: U-PWM, W-ON, V-OFF (扇区2)
{2, 0, 1, +1}, // 3: W-PWM, U-ON, V-OFF (扇区3)
{2, 1, 0, +1}, // 4: W-PWM, V-ON, U-OFF (扇区4)
{1, 2, 0, +1}, // 5: V-PWM, W-ON, U-OFF (扇区5)
{1, 0, 2, +1}, // 6: V-PWM, U-ON, W-OFF (扇区6)
};
// ─── 内部辅助: 设置某一相为PWM输出 ───
static void set_phase_pwm(uint8_t phase, uint16_t duty) {
TIM_HandleTypeDef *tim;
uint32_t channel;
switch (phase) {
case 0: tim = IN1_TIM; channel = IN1_CH; break;
case 1: tim = IN2_TIM; channel = IN2_CH; break;
case 2: tim = IN3_TIM; channel = IN3_CH; break;
default: return;
}
__HAL_TIM_SET_COMPARE(tim, channel, duty);
}
// ─── 内部辅助: 设置某一相为低电平(下桥常开) ───
static void set_phase_low(uint8_t phase) {
TIM_HandleTypeDef *tim;
uint32_t channel;
switch (phase) {
case 0: tim = IN1_TIM; channel = IN1_CH; break;
case 1: tim = IN2_TIM; channel = IN2_CH; break;
case 2: tim = IN3_TIM; channel = IN3_CH; break;
default: return;
}
// PWM占空比=0 → INx=0 → 上桥关断
// ENx=1 → 下桥导通 (因为INx=0时DRV8313下桥常开)
__HAL_TIM_SET_COMPARE(tim, channel, 0);
}
// ─── 内部辅助: 设置某一相为悬空(高阻) ───
static void set_phase_float(uint8_t phase) {
switch (phase) {
case 0: HAL_GPIO_WritePin(EN_GPIO, EN1_PIN, GPIO_PIN_RESET); break;
case 1: HAL_GPIO_WritePin(EN_GPIO, EN2_PIN, GPIO_PIN_RESET); break;
case 2: HAL_GPIO_WritePin(EN_GPIO, EN3_PIN, GPIO_PIN_RESET); break;
}
}
// ─── 内部辅助: 使能某一相 ───
static void set_phase_enable(uint8_t phase) {
switch (phase) {
case 0: HAL_GPIO_WritePin(EN_GPIO, EN1_PIN, GPIO_PIN_SET); break;
case 1: HAL_GPIO_WritePin(EN_GPIO, EN2_PIN, GPIO_PIN_SET); break;
case 2: HAL_GPIO_WritePin(EN_GPIO, EN3_PIN, GPIO_PIN_SET); break;
}
}
// ═══════════════════════════════════════════
// 核心函数: 执行一次换相
// ═══════════════════════════════════════════
// 参数:
// hall_state: 霍尔状态 1~6
// duty: PWM占空比 0~PWM_ARR
// forward: true=正转, false=反转
static uint16_t current_duty = 0;
static uint8_t current_hall = 0;
static bool motor_forward = true;
void Motor_Commutate(uint8_t hall_state, uint16_t duty, bool forward) {
if (hall_state == 0 || hall_state == 7) return; // 无效状态
current_hall = hall_state;
current_duty = duty;
motor_forward = forward;
// 先全部悬空 (安全)
set_phase_float(0);
set_phase_float(1);
set_phase_float(2);
// 查表获取换相信息
CommStep_t step;
if (forward) {
step = comm_fwd[hall_state];
} else {
// 反转: 交换PWM相和ON相
step = comm_fwd[7 - hall_state]; // 简化处理
}
// 1. 悬空相: EN=0
set_phase_float(step.off_phase);
// 2. ON相(下桥常开): EN=1, IN=0
set_phase_enable(step.on_phase);
set_phase_low(step.on_phase);
// 3. PWM相(上桥PWM): EN=1, IN=PWM
set_phase_enable(step.pwm_phase);
set_phase_pwm(step.pwm_phase, duty);
}
// ═══════════════════════════════════════════
// 读取当前霍尔状态
// ═══════════════════════════════════════════
uint8_t Motor_ReadHallState(void) {
uint8_t state = 0;
if (HAL_GPIO_ReadPin(HALL_GPIO, HALL_A_PIN) == GPIO_PIN_SET) state |= 0x01;
if (HAL_GPIO_ReadPin(HALL_GPIO, HALL_B_PIN) == GPIO_PIN_SET) state |= 0x02;
if (HAL_GPIO_ReadPin(HALL_GPIO, HALL_C_PIN) == GPIO_PIN_SET) state |= 0x04;
return state; // 返回值: 1~6 (有效), 0或7 (无效/过渡态)
}
// ═══════════════════════════════════════════
// 初始化
// ═══════════════════════════════════════════
void Motor_Init(void) {
// 1. EN引脚全部拉低 (电机不转)
HAL_GPIO_WritePin(EN_GPIO, EN1_PIN | EN2_PIN | EN3_PIN, GPIO_PIN_RESET);
// 2. PWM全部输出0
__HAL_TIM_SET_COMPARE(IN1_TIM, IN1_CH, 0);
__HAL_TIM_SET_COMPARE(IN2_TIM, IN2_CH, 0);
__HAL_TIM_SET_COMPARE(IN3_TIM, IN3_CH, 0);
// 3. 启动TIM1 PWM输出
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
// 4. 初始化PID
PID_Init(&speed_pid,
2.5f, // Kp
0.8f, // Ki
0.05f, // Kd
(float)PWM_ARR, // 输出上限 = ARR
0.0f, // 输出下限
300.0f); // 积分上限
// 5. 等待500ms让电荷泵充电
HAL_Delay(500);
}
// ═══════════════════════════════════════════
// 设定目标速度 (m/s)
// ═══════════════════════════════════════════
void Motor_SetTargetSpeed(float speed_ms) {
target_speed_ms = speed_ms;
if (speed_ms > 0) {
motor_forward = true;
} else if (speed_ms < 0) {
motor_forward = false;
target_speed_ms = -speed_ms; // 取绝对值
} else {
// 速度=0: 刹车
Motor_Brake();
return;
}
}
// ═══════════════════════════════════════════
// PID速度环更新 (在1kHz任务中调用!)
// ═══════════════════════════════════════════
void Motor_SpeedLoopUpdate(float actual_speed_ms) {
if (target_speed_ms < 0.01f) {
// 目标速度≈0, 不输出
current_duty = 0;
Motor_Commutate(current_hall, 0, motor_forward);
return;
}
// PID计算
float error = target_speed_ms - actual_speed_ms;
float pid_out = PID_Calculate(&speed_pid, error);
// 限制占空比
uint16_t duty = (uint16_t)pid_out;
if (duty > PWM_ARR) duty = PWM_ARR;
if (duty < 50) duty = 50; // 最小占空比约5%, 低于此电机不转
current_duty = duty;
// 用当前霍尔状态刷新换相 (更新PWM占空比)
Motor_Commutate(current_hall, duty, motor_forward);
}
// ═══════════════════════════════════════════
// 霍尔中断处理 (GPIO外部中断回调)
// ═══════════════════════════════════════════
// 在 stm32f4xx_it.c 的 HAL_GPIO_EXTI_Callback 中调用
void Motor_HallInterruptHandler(void) {
// 1. 读取新的霍尔状态
uint8_t new_hall = Motor_ReadHallState();
// 2. 如果状态有效且发生变化, 执行换相
if (new_hall >= 1 && new_hall <= 6 && new_hall != current_hall) {
Motor_Commutate(new_hall, current_duty, motor_forward);
}
}
// ═══════════════════════════════════════════
// 刹车 (短路制动: 三相下桥全部导通)
// ═══════════════════════════════════════════
void Motor_Brake(void) {
target_speed_ms = 0;
PID_Reset(&speed_pid);
// 三相全部 IN=0, EN=1 → 三相下桥全部导通 → 电机短路制动
__HAL_TIM_SET_COMPARE(IN1_TIM, IN1_CH, 0);
__HAL_TIM_SET_COMPARE(IN2_TIM, IN2_CH, 0);
__HAL_TIM_SET_COMPARE(IN3_TIM, IN3_CH, 0);
HAL_GPIO_WritePin(EN_GPIO, EN1_PIN, GPIO_PIN_SET);
HAL_GPIO_WritePin(EN_GPIO, EN2_PIN, GPIO_PIN_SET);
HAL_GPIO_WritePin(EN_GPIO, EN3_PIN, GPIO_PIN_SET);
}
// ═══════════════════════════════════════════
// 完全释放 (三相全部悬空, 电机自由滑行)
// ═══════════════════════════════════════════
void Motor_FreeStop(void) {
target_speed_ms = 0;
PID_Reset(&speed_pid);
current_duty = 0;
HAL_GPIO_WritePin(EN_GPIO, EN1_PIN, GPIO_PIN_RESET);
HAL_GPIO_WritePin(EN_GPIO, EN2_PIN, GPIO_PIN_RESET);
HAL_GPIO_WritePin(EN_GPIO, EN3_PIN, GPIO_PIN_RESET);
}
1.5 霍尔中断注册
// ============================================
// stm32f4xx_it.c — 霍尔外部中断
// ============================================
// PC6/PC7/PC8 共用 EXTI9_5 中断向量
void EXTI9_5_IRQHandler(void) {
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_6); // HALL_A
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_7); // HALL_B
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_8); // HALL_C
}
// HAL库回调函数 (在 main.c 或 bsp_motor.c 中实现)
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
// 无论是哪个霍尔引脚触发, 都执行换相
if (GPIO_Pin == GPIO_PIN_6 ||
GPIO_Pin == GPIO_PIN_7 ||
GPIO_Pin == GPIO_PIN_8) {
Motor_HallInterruptHandler();
}
}
1.6 调试步骤（非常重要！）
Step 1: 验证PWM波形 (不接电机!)
├── 示波器/逻辑分析仪接PA8/PA9/PA10
├── 手动设置占空比50%, 确认三路PWM频率=20kHz
├── 确认死区时间≈200ns (上下沿不重叠)
└── ✅ 通过条件: 波形干净, 无毛刺
Step 2: 验证EN控制
├── 万用表量EN1/EN2/EN3引脚
├── 代码中依次拉高/拉低, 确认电平变化
└── ✅ 通过条件: EN引脚响应正确
Step 3: 验证霍尔信号
├── 手转电机轴, 串口打印霍尔状态
├── 确认状态序列为: 5→1→3→2→6→4→5... (正转)
├── 确认不会出现0或7 (如果出现说明接线有误)
└── ✅ 通过条件: 6个状态循环出现, 无跳变
Step 4: 开环测试 (最关键!)
├── 电源限流1A! (串联灯泡或电源限流模式)
├── 固定占空比200, 按5→1→3→2→6→4顺序换相
├── 每步延时10ms, 电机应该缓慢转动
├── 如果不转/抖动: 换相顺序错了, 交换两相
├── 如果反转: 交换任意两相电机线
└── ✅ 通过条件: 电机平稳转动, 电流<0.5A
Step 5: 闭环PID测试
├── 接入编码器反馈
├── 设定目标速度0.1m/s, 观察实际速度
├── 先只加Kp, 逐渐加大直到电机开始转
├── 再加Ki消除稳态误差
└── ✅ 通过条件: 实际速度跟踪目标速度, 无振荡
复述要点：DRV8313 六步换相的核心是 "霍尔中断触发查表 → 切换三相状态"。每个扇区中，一相接PWM（上桥调制），一相接低电平（下桥常开回流），一相悬空。PID每1ms更新一次占空比，但只在换相时才切换三相角色。
二、硬件TIM编码器模式测速
2.1 工作原理
STM32的定时器有硬件编码器接口，能自动对AB相脉冲计数并判断方向，CPU零负担。
MT6701磁编码器 (安装在电机轴上)
┌──────────────────┐
│ A相 ─────────────┼──────► PE2 (TIM9_CH1)
│ B相 ─────────────┼──────► PE3 (TIM9_CH2)
│ VCC (3.3V) │
│ GND │
└──────────────────┘
AB相正交信号:
正转 → 反转 ←
A: ┌──┐ ┌──┐ A: ┌──┐ ┌──┐
│ │ │ │ │ │ │ │
───┘ └──┘ └── ──┘ └──┘ └─
B: ┌──┐ ┌── B: ──┐ ┌──┐
│ │ │ │ │ │
─────┘ └──┘ ──┐ └──┘ └
4倍频: 在A和B的每个上升沿和下降沿都计数
→ 每转脉冲数 = 编码器CPR × 4 × 减速比
2.2 CubeMX 配置
📌 TIM9 配置 (左轮编码器)
─────────────────────────────────
Clock Source = Internal Clock
Channel1 = Encoder Mode (TI1 and TI2)
Channel2 = Encoder Mode (TI1 and TI2) ← 选这个! 两通道都计数
Encoder Mode = Encoder Mode TI12 (4倍频)
📌 Channel1 Settings:
→ 对应引脚 PE2 (自动映射)
→ No filter (如果信号干净)
→ IC1 Polarity = Rising Edge
→ IC1 Selection = Direct TI
📌 Channel2 Settings:
→ 对应引脚 PE3
→ IC2 Polarity = Rising Edge
→ IC2 Selection = Direct TI
📌 Counter Settings:
Prescaler = 0
Counter Period = 65535 (0xFFFF, 16位最大值)
Counter Mode = Up (方向由硬件自动判断)
auto-reload preload = Enable
📌 NVIC: 不需要开中断! (轮询读取即可, 除非需要精确计数)
📌 GPIO Settings (PE2, PE3):
GPIO mode = Alternate Function Push Pull
GPIO Pull-up/Pull-down = Pull-up (重要! 编码器信号需要上拉)
Maximum output speed = Low
─────────────────────────────────
📌 TIM10 配置 (右轮编码器)
与TIM9完全相同, 引脚自动映射为PF6/PF7
2.3 编码器驱动完整代码
// ============================================
// bsp_encoder.c — 硬件编码器测速完整实现
// ============================================
#include "bsp_encoder.h"
#include "main.h"
#include "arm_math.h"
extern TIM_HandleTypeDef htim9; // 左轮
extern TIM_HandleTypeDef htim10; // 右轮
// ═══════════════════════════════════════════
// 机械参数 (根据你的实际电机修改!)
// ═══════════════════════════════════════════
#define ENCODER_CPR 16384 // MT6701: 14bit = 16384 脉冲/转
#define MOTOR_REDUCTION 30 // 电机减速比 1:30
#define WHEEL_DIAMETER_MM 65.0f // 轮径 65mm
// 轮子转一圈, 编码器产生的总脉冲数 (4倍频后)
#define COUNTS_PER_REV (ENCODER_CPR * 4 * MOTOR_REDUCTION)
// 每个脉冲对应的行走距离 (米)
#define MM_PER_COUNT (3.14159265f * WHEEL_DIAMETER_MM / COUNTS_PER_REV)
#define M_PER_COUNT (MM_PER_COUNT / 1000.0f)
// ═══════════════════════════════════════════
// 内部状态 (每个轮子独立)
// ═══════════════════════════════════════════
typedef struct {
TIM_HandleTypeDef *htim;
// 32位累计脉冲 (软件扩展, 解决16位溢出)
int32_t total_counts;
uint16_t last_cnt;
// 速度计算
float velocity_ms; // 当前速度 m/s
int32_t last_counts; // 上次采样时的累计脉冲
uint32_t last_sample_ms; // 上次采样时间
} EncoderState_t;
static EncoderState_t enc[2]; // [0]=左轮, [1]=右轮
// ═══════════════════════════════════════════
// 初始化
// ═══════════════════════════════════════════
void Encoder_Init(void) {
// 左轮
enc[0].htim = &htim9;
enc[0].total_counts = 0;
enc[0].last_cnt = 0;
enc[0].velocity_ms = 0;
enc[0].last_counts = 0;
enc[0].last_sample_ms = HAL_GetTick();
// 右轮
enc[1].htim = &htim10;
enc[1].total_counts = 0;
enc[1].last_cnt = 0;
enc[1].velocity_ms = 0;
enc[1].last_counts = 0;
enc[1].last_sample_ms = HAL_GetTick();
// 启动编码器模式
HAL_TIM_Encoder_Start(&htim9, TIM_CHANNEL_ALL);
HAL_TIM_Encoder_Start(&htim10, TIM_CHANNEL_ALL);
// 清零计数器
__HAL_TIM_SET_COUNTER(&htim9, 0);
__HAL_TIM_SET_COUNTER(&htim10, 0);
}
// ═══════════════════════════════════════════
// 核心: 读取32位累计脉冲数
// ═══════════════════════════════════════════
//
// 关键问题: TIM计数器是16位的 (0~65535)
// 当正转从65535溢出到0时, 或反转从0溢出到65535时
// 必须用软件检测溢出并扩展到32位
//
// 原理:
// delta = current_cnt - last_cnt (int16_t强转!)
//
// 正转溢出: last=65000, curr=100
// uint16差 = 100-65000 = -64900 (uint溢出)
// int16差 = (int16_t)(-64900) = +536 (正确! 正转了536步)
//
// 反转溢出: last=100, curr=65000
// uint16差 = 65000-100 = 64900
// int16差 = (int16_t)(64900) = -536 (正确! 反转了536步)
//
// int16_t强转是处理16位计数器溢出的核心技巧!
int32_t Encoder_GetTotalCounts(uint8_t wheel_id) {
EncoderState_t *e = &enc[wheel_id];
// 读取当前16位计数器值
uint16_t current_cnt = (uint16_t)__HAL_TIM_GET_COUNTER(e->htim);
// 计算增量 (int16_t强转自动处理溢出!)
int16_t delta = (int16_t)(current_cnt - e->last_cnt);
// 累加到32位总计数
e->total_counts += delta;
// 更新上次值
e->last_cnt = current_cnt;
return e->total_counts;
}
// ═══════════════════════════════════════════
// 核心: 计算当前速度 (m/s)
// ═══════════════════════════════════════════
//
// 测速方法: M法测速 (在固定时间间隔内计数脉冲数)
//
// velocity = (脉冲增量 × 每脉冲距离) / 时间间隔
//
// 调用频率建议: 10ms~20ms (太短噪声大, 太长响应慢)
float Encoder_UpdateVelocity(uint8_t wheel_id) {
EncoderState_t *e = &enc[wheel_id];
// 1. 获取当前累计脉冲
int32_t current_counts = Encoder_GetTotalCounts(wheel_id);
// 2. 计算时间间隔
uint32_t now_ms = HAL_GetTick();
uint32_t dt_ms = now_ms - e->last_sample_ms;
// 防止除零和过短间隔
if (dt_ms < 5) {
return e->velocity_ms; // 间隔太短, 返回上次值
}
// 3. 计算脉冲增量
int32_t delta_counts = current_counts - e->last_counts;
// 4. 计算速度
float distance_m = (float)delta_counts * M_PER_COUNT;
float dt_sec = (float)dt_ms / 1000.0f;
float raw_velocity = distance_m / dt_sec;
// 5. 一阶低通滤波 (平滑噪声)
// α = 0.3, 新值权重30%, 旧值权重70%
#define VEL_FILTER_ALPHA 0.3f
e->velocity_ms = VEL_FILTER_ALPHA * raw_velocity
+ (1.0f - VEL_FILTER_ALPHA) * e->velocity_ms;
// 6. 零速死区 (消除静止时的微小抖动)
if (fabsf(e->velocity_ms) < 0.005f) {
e->velocity_ms = 0.0f;
}
// 7. 更新采样状态
e->last_counts = current_counts;
e->last_sample_ms = now_ms;
return e->velocity_ms;
}
// ═══════════════════════════════════════════
// 获取累计行走距离 (米)
// ═══════════════════════════════════════════
float Encoder_GetDistance(uint8_t wheel_id) {
int32_t counts = Encoder_GetTotalCounts(wheel_id);
return (float)counts * M_PER_COUNT;
}
// ═══════════════════════════════════════════
// 获取轮子转速 (RPM)
// ═══════════════════════════════════════════
float Encoder_GetRPM(uint8_t wheel_id) {
float v = enc[wheel_id].velocity_ms;
float circumference_m = 3.14159265f * WHEEL_DIAMETER_MM / 1000.0f;
// RPM = (m/s) / (m/rev) × 60
return (v / circumference_m) * 60.0f;
}
// ═══════════════════════════════════════════
// 清零 (回充对接成功后调用)
// ═══════════════════════════════════════════
void Encoder_Reset(uint8_t wheel_id) {
EncoderState_t *e = &enc[wheel_id];
e->total_counts = 0;
e->last_counts = 0;
e->last_cnt = 0;
__HAL_TIM_SET_COUNTER(e->htim, 0);
}
2.4 差速运动学（两轮速度→机器人位姿变化）
// ============================================
// 差速运动学: 左右轮速度 → 机器人线速度/角速度
// ============================================
#define WHEEL_BASE_M 0.28f // 轮距 280mm
typedef struct {
float v_linear; // 机器人线速度 m/s
float v_angular; // 机器人角速度 rad/s
} ChassisVelocity_t;
ChassisVelocity_t Kinematics_Forward(float v_left, float v_right) {
ChassisVelocity_t vel;
// 线速度 = (左轮速度 + 右轮速度) / 2
vel.v_linear = (v_left + v_right) / 2.0f;
// 角速度 = (右轮速度 - 左轮速度) / 轮距
// 正=逆时针, 负=顺时针
vel.v_angular = (v_right - v_left) / WHEEL_BASE_M;
return vel;
}
// 逆运动学: 机器人速度 → 左右轮目标速度
typedef struct {
float v_left;
float v_right;
} WheelSpeeds_t;
WheelSpeeds_t Kinematics_Inverse(float v_linear, float v_angular) {
WheelSpeeds_t ws;
ws.v_left = v_linear - v_angular * WHEEL_BASE_M / 2.0f;
ws.v_right = v_linear + v_angular * WHEEL_BASE_M / 2.0f;
return ws;
}
2.5 调试步骤
Step 1: 验证计数方向
├── 手转左轮前进方向, 串口打印 total_counts
├── 确认数值递增 (如果递减, 交换AB两根线)
├── 右轮同理
└── ✅ 前进=计数增加
Step 2: 验证脉冲总数
├── 手转轮子精确1圈 (在轮子上画标记)
├── 串口打印 total_counts 的增量
├── 应该 ≈ COUNTS_PER_REV = 16384 × 4 × 30 = 1,966,080
├── 如果差很多: 检查减速比和编码器CPR参数
└── ✅ 一圈脉冲数与理论值误差<1%
Step 3: 验证速度计算
├── 设定电机速度0.2m/s, 让轮子空转
├── 串口打印 velocity_ms
├── 稳定后应在 0.19~0.21 范围内
├── 如果抖动大: 增大滤波系数α
└── ✅ 速度稳定, 噪声<0.01m/s
Step 4: 验证溢出处理
├── 让轮子持续转动5分钟以上
├── 观察 total_counts 是否持续正确累加
├── 不应出现突变或归零
└── ✅ 长时间运行累计值稳定
复述要点：硬件编码器模式让TIM自动计数AB相脉冲，CPU完全不需要参与。软件通过
int16_t强转巧妙处理16位计数器溢出，扩展到32位累计值。速度用M法（固定时间内数脉冲）计算，加一阶低通滤波平滑噪声。
三、ICM-42688 IMU 姿态解算
3.1 重要纠正：ICM-42688 没有 DMP！
⚠️ ICM-42688-P 没有硬件DMP（那是MPU6050等老芯片的特性）。姿态解算必须在STM32上用软件实现。
推荐使用 Mahony AHRS算法：代码量小（<50行）、计算量低（<0.1ms/次）、精度足够扫地机使用。
3.2 为什么需要姿态解算？
陀螺仪: 短期精确, 但积分会漂移 (走1分钟偏2~3°)
加速度计: 长期稳定, 但对振动极敏感 (电机一开就乱跳)
Mahony算法 = 互补滤波的升级版
→ 用加速度计修正陀螺仪的漂移
→ 用陀螺仪抑制加速度计的振动噪声
→ 两者融合得到稳定准确的姿态角
3.3 CubeMX I2C 配置
📌 I2C1 配置 (连接ICM-42688)
─────────────────────────────────
PB6 → I2C1_SCL
PB7 → I2C1_SDA
I2C Speed = Fast Mode (400KHz)
Own Address = 0 (主机模式不需要)
Rise Time = 100ns
Fall Time = 10ns
📌 ICM-42688 硬件接线
─────────────────────────────────
VDD → 3.3V (独立LDO供电, 不要和电机共用!)
GND → GND
SCL → PB6 (串33Ω电阻)
SDA → PB7 (串33Ω电阻)
SDA → 4.7kΩ上拉至3.3V
SCL → 4.7kΩ上拉至3.3V
INT1 → PA1 (可选, 数据就绪中断)
CLKIN→ 不接 (使用内部振荡器)
3.4 ICM-42688 寄存器初始化
// ============================================
// bsp_icm42688.c — ICM-42688 完整驱动
// ============================================
#include "bsp_icm42688.h"
#include "main.h"
extern I2C_HandleTypeDef hi2c1;
extern osMutexId_t mutexI2C1Handle;
#define ICM_ADDR (0x68 << 1) // 7位地址0x68, 左移1位
// ─── 关键寄存器地址 ───
#define REG_DEVICE_CONFIG 0x11
#define REG_DRIVE_CONFIG 0x13
#define REG_INT_CONFIG 0x14
#define REG_FIFO_CONFIG 0x16
#define REG_TEMP_DATA1 0x1D
#define REG_ACCEL_DATA_X1 0x1F // 加速度X高字节
#define REG_ACCEL_DATA_X0 0x20 // 加速度X低字节
#define REG_GYRO_DATA_X1 0x25 // 陀螺仪X高字节
#define REG_GYRO_DATA_X0 0x26
#define REG_INT_STATUS 0x2D
#define REG_SIGNAL_PATH_RESET 0x4B
#define REG_PWR_MGMT0 0x4E
#define REG_GYRO_CONFIG0 0x4F
#define REG_ACCEL_CONFIG0 0x50
#define REG_GYRO_CONFIG1 0x51
#define REG_ACCEL_CONFIG1 0x53
#define REG_WHO_AM_I 0x75
// ─── 量程配置 ───
// 陀螺仪: ±2000dps → 灵敏度 16.384 LSB/(°/s)
// 加速度: ±4g → 灵敏度 8192 LSB/g
#define GYRO_SCALE 16.384f
#define ACCEL_SCALE 8192.0f
// ─── 内部状态 ───
static float gyro_offset[3] = {0, 0, 0}; // 陀螺仪零偏
// ─── I2C读写封装 ───
static void icm_write(uint8_t reg, uint8_t val) {
uint8_t buf[2] = {reg, val};
osMutexAcquire(mutexI2C1Handle, osWaitForever);
HAL_I2C_Master_Transmit(&hi2c1, ICM_ADDR, buf, 2, 50);
osMutexRelease(mutexI2C1Handle);
}
static void icm_read(uint8_t reg, uint8_t *buf, uint8_t len) {
osMutexAcquire(mutexI2C1Handle, osWaitForever);
HAL_I2C_Master_Transmit(&hi2c1, ICM_ADDR, ®, 1, 50);
HAL_I2C_Master_Receive(&hi2c1, ICM_ADDR, buf, len, 50);
osMutexRelease(mutexI2C1Handle);
}
// ═══════════════════════════════════════════
// 初始化序列 (严格按照数据手册上电时序!)
// ═══════════════════════════════════════════
HAL_StatusTypeDef ICM42688_Init(void) {
// 1. 检查芯片ID
uint8_t whoami = 0;
icm_read(REG_WHO_AM_I, &whoami, 1);
if (whoami != 0x47) {
return HAL_ERROR; // 通信失败或芯片不对
}
// 2. 软复位
icm_write(REG_DEVICE_CONFIG, 0x01); // SOFT_RESET = 1
HAL_Delay(2); // 等待复位完成 (至少1ms)
// 3. 配置SPI模式 (即使我们用I2C, 也需要配置)
icm_write(REG_DRIVE_CONFIG, 0x05); // I2C模式, Slew rate
// 4. 配置陀螺仪: ±2000dps, ODR=200Hz
// Bits[7:5] = GYRO_FS_SEL = 000 (±2000dps)
// Bits[3:0] = GYRO_ODR = 0110 (200Hz)
icm_write(REG_GYRO_CONFIG0, 0x06);
// 5. 配置加速度计: ±4g, ODR=200Hz
// Bits[7:5] = ACCEL_FS_SEL = 010 (±4g)
// Bits[3:0] = ACCEL_ODR = 0110 (200Hz)
icm_write(REG_ACCEL_CONFIG0, 0x56);
// 6. 配置陀螺仪滤波器
icm_write(REG_GYRO_CONFIG1, 0x0A); // 低通滤波 约53Hz
// 7. 配置加速度计滤波器
icm_write(REG_ACCEL_CONFIG1, 0x15); // 低通滤波 约53Hz
// 8. 开启传感器 (低噪声模式)
// Bits[1:0] = ACCEL_MODE = 11 (低噪声)
// Bits[3:2] = GYRO_MODE = 11 (低噪声)
icm_write(REG_PWR_MGMT0, 0x0F);
// 9. 等待传感器稳定 (数据手册要求至少10ms)
HAL_Delay(20);
// 10. 陀螺仪零偏校准 (设备必须静止!)
ICM42688_CalibrateGyro();
return HAL_OK;
}
// ═══════════════════════════════════════════
// 陀螺仪零偏校准 (上电静止时调用)
// ═══════════════════════════════════════════
void ICM42688_CalibrateGyro(void) {
float sum[3] = {0, 0, 0};
int samples = 200;
for (int i = 0; i < samples; i++) {
int16_t raw[3];
uint8_t buf[6];
icm_read(REG_GYRO_DATA_X1, buf, 6);
raw[0] = (int16_t)((buf[0] << 8) | buf[1]);
raw[1] = (int16_t)((buf[2] << 8) | buf[3]);
raw[2] = (int16_t)((buf[4] << 8) | buf[5]);
sum[0] += (float)raw[0];
sum[1] += (float)raw[1];
sum[2] += (float)raw[2];
HAL_Delay(5); // 5ms × 200 = 1秒
}
// 计算平均零偏 (原始值单位)
gyro_offset[0] = sum[0] / samples;
gyro_offset[1] = sum[1] / samples;
gyro_offset[2] = sum[2] / samples;
}
// ═══════════════════════════════════════════
// 读取6轴原始数据并转换为物理单位
// ═══════════════════════════════════════════
void ICM42688_ReadData(ImuRawData_t *data) {
uint8_t buf[12];
// 连续读取12字节: 加速度XYZ(6) + 陀螺仪XYZ(6)
icm_read(REG_ACCEL_DATA_X1, buf, 12);
// 解析加速度 (大端序, 高字节在前)
int16_t ax = (int16_t)((buf[0] << 8) | buf[1]);
int16_t ay = (int16_t)((buf[2] << 8) | buf[3]);
int16_t az = (int16_t)((buf[4] << 8) | buf[5]);
// 解析陀螺仪
int16_t gx = (int16_t)((buf[6] << 8) | buf[7]);
int16_t gy = (int16_t)((buf[8] << 8) | buf[9]);
int16_t gz = (int16_t)((buf[10] << 8) | buf[11]);
// 转换为物理单位
// 加速度 → m/s² (乘以9.81)
data->accel_x = ((float)ax / ACCEL_SCALE) * 9.81f;
data->accel_y = ((float)ay / ACCEL_SCALE) * 9.81f;
data->accel_z = ((float)az / ACCEL_SCALE) * 9.81f;
// 陀螺仪 → rad/s (减去零偏, 乘以转换系数)
#define DEG_TO_RAD 0.01745329f
data->gyro_x = ((float)gx - gyro_offset[0]) / GYRO_SCALE * DEG_TO_RAD;
data->gyro_y = ((float)gy - gyro_offset[1]) / GYRO_SCALE * DEG_TO_RAD;
data->gyro_z = ((float)gz - gyro_offset[2]) / GYRO_SCALE * DEG_TO_RAD;
}
3.5 Mahony AHRS 姿态解算算法（核心！）
// ============================================
// algo_mahony.c — Mahony AHRS 姿态解算
// ============================================
//
// 参考文献:
// "Nonlinear Complementary Filters on the Special Orthogonal Group"
// by Robert Mahony, Tarek Hamel, Jean-Michel Pflimlin (2008)
//
// 原理简述:
// 1. 用陀螺仪积分得到姿态预测 (短期精确)
// 2. 用加速度计计算重力方向 (长期稳定)
// 3. 比较预测的重力方向与实际测量的重力方向
// 4. 用PI控制器计算修正量, 反馈给陀螺仪积分
// 5. 最终输出四元数表示的姿态
//
// 输出: 四元数 q[0..3] → 可转换为欧拉角 (Roll/Pitch/Yaw)
#include "algo_mahony.h"
#include "arm_math.h"
// ─── PI控制器参数 ───
// Kp: 比例增益, 越大修正越快但越容易受加速度计噪声影响
// Ki: 积分增益, 消除陀螺仪长期漂移
//
// 扫地机调参建议:
// Kp = 1.0~2.0 (商场地面振动不大, 可以稍高)
// Ki = 0.005~0.01
#define MAHONY_KP 1.5f
#define MAHONY_KI 0.008f
// ─── 四元数状态 ───
static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
// ─── PI积分误差 ───
static float ex_int = 0.0f, ey_int = 0.0f, ez_int = 0.0f;
// ─── 输出欧拉角 ───
static float euler_roll = 0, euler_pitch = 0, euler_yaw = 0;
// ═══════════════════════════════════════════
// 初始化
// ═══════════════════════════════════════════
void Mahony_Init(void) {
q0 = 1.0f; q1 = 0.0f; q2 = 0.0f; q3 = 0.0f; // 单位四元数
ex_int = 0; ey_int = 0; ez_int = 0;
}
// ═══════════════════════════════════════════
// 核心更新函数 (200Hz调用, 与IMU ODR一致)
// ═══════════════════════════════════════════
// 参数:
// gx, gy, gz: 陀螺仪 rad/s
// ax, ay, az: 加速度计 m/s²
// dt: 采样周期 (秒), 如 0.005f (200Hz)
void Mahony_Update(float gx, float gy, float gz,
float ax, float ay, float az,
float dt) {
float norm;
float vx, vy, vz;
float ex, ey, ez;
float pa, pb, pc;
float q0q0, q0q1, q0q2, q0q3;
float q1q1, q1q2, q1q3;
float q2q2, q2q3, q3q3;
// ──────── Step 1: 加速度计归一化 ────────
norm = arm_sqrt_f32(ax*ax + ay*ay + az*az, &norm) ? norm : 0.0f;
// 使用 CMSIS-DSP 快速开方倒数
norm = 1.0f / norm;
ax *= norm;
ay *= norm;
az *= norm;
// ──────── Step 2: 估计重力方向 (从四元数推算) ────────
// 这是旋转矩阵的第三列, 表示机体坐标系下的重力向量
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
vx = 2.0f * (q1q3 - q0q2);
vy = 2.0f * (q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
// ──────── Step 3: 计算误差 (叉积) ────────
// error = measured_gravity × estimated_gravity
// 叉积结果就是误差向量, 方向是修正方向
ex = (ay * vz - az * vy);
ey = (az * vx - ax * vz);
ez = (ax * vy - ay * vx);
// ──────── Step 4: PI控制器 ────────
// 积分项 (消除陀螺仪长期漂移)
ex_int += MAHONY_KI * ex * dt;
ey_int += MAHONY_KI * ey * dt;
ez_int += MAHONY_KI * ez * dt;
// 积分限幅 (防止积分饱和)
#define INT_LIMIT 0.1f
if (ex_int > INT_LIMIT) ex_int = INT_LIMIT;
if (ex_int < -INT_LIMIT) ex_int = -INT_LIMIT;
if (ey_int > INT_LIMIT) ey_int = INT_LIMIT;
if (ey_int < -INT_LIMIT) ey_int = -INT_LIMIT;
if (ez_int > INT_LIMIT) ez_int = INT_LIMIT;
if (ez_int < -INT_LIMIT) ez_int = -INT_LIMIT;
// 比例项 + 积分项 → 修正角速度
pa = MAHONY_KP * ex + ex_int;
pb = MAHONY_KP * ey + ey_int;
pc = MAHONY_KP * ez + ez_int;
// ──────── Step 5: 修正陀螺仪并积分四元数 ────────
gx += pa;
gy += pb;
gz += pc;
// 四元数微分方程积分 (一阶龙格库塔)
float dq0 = 0.5f * (-q1*gx - q2*gy - q3*gz);
float dq1 = 0.5f * ( q0*gx + q2*gz - q3*gy);
float dq2 = 0.5f * ( q0*gy - q1*gz + q3*gx);
float dq3 = 0.5f * ( q0*gz + q1*gy - q2*gx);
q0 += dq0 * dt;
q1 += dq1 * dt;
q2 += dq2 * dt;
q3 += dq3 * dt;
// ──────── Step 6: 四元数归一化 ────────
norm = q0*q0 + q1*q1 + q2*q2 + q3*q3;
if (norm > 0.0f) {
norm = 1.0f / arm_sqrt_f32(norm, &norm);
// 使用更精确的开方
float inv_norm = 1.0f / sqrtf(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 *= inv_norm;
q1 *= inv_norm;
q2 *= inv_norm;
q3 *= inv_norm;
}
// ──────── Step 7: 四元数 → 欧拉角 ────────
// Roll (横滚, 绕X轴) — 扫地机不太需要
euler_roll = atan2f(2.0f*(q0*q1 + q2*q3),
1.0f - 2.0f*(q1*q1 + q2*q2));
// Pitch (俯仰, 绕Y轴) — 扫地机不太需要
float sin_pitch = 2.0f * (q0*q2 - q3*q1);
if (sin_pitch > 1.0f) sin_pitch = 1.0f;
if (sin_pitch < -1.0f) sin_pitch = -1.0f;
euler_pitch = asinf(sin_pitch);
// Yaw (航向, 绕Z轴) — ⭐ 扫地机最需要的角度!
euler_yaw = atan2f(2.0f*(q0*q3 + q1*q2),
1.0f - 2.0f*(q2*q2 + q3*q3));
}
// ═══════════════════════════════════════════
// 获取姿态角 (弧度)
// ═══════════════════════════════════════════
float Mahony_GetYaw(void) { return euler_yaw; }
float Mahony_GetPitch(void) { return euler_pitch; }
float Mahony_GetRoll(void) { return euler_roll; }
// ═══════════════════════════════════════════
// 获取四元数
// ═══════════════════════════════════════════
void Mahony_GetQuaternion(float *out_q) {
out_q[0] = q0;
out_q[1] = q1;
out_q[2] = q2;
out_q[3] = q3;
}
3.6 IMU数据读取 + Mahony 更新（在FreeRTOS任务中）
// ============================================
// task_sensor.c 中的 IMU 处理部分
// ============================================
// IMU数据读取 + 姿态解算 (200Hz, 即5ms一次)
// 使用独立的高优先级定时器触发, 或在传感器任务中分频调用
static void IMU_ProcessLoop(void) {
ImuRawData_t raw;
// 1. 读取IMU原始数据 (I2C读取, 约0.5ms)
ICM42688_ReadData(&raw);
// 2. 运行Mahony姿态解算 (约0.05ms, STM32F4 FPU加速)
// dt = 5ms = 0.005s (200Hz)
Mahony_Update(raw.gyro_x, raw.gyro_y, raw.gyro_z,
raw.accel_x, raw.accel_y, raw.accel_z,
0.005f);
// 3. 获取航向角 (扫地机只用Yaw)
float yaw = Mahony_GetYaw();
// 4. 将yaw存入共享结构体 (加互斥锁或用原子操作)
g_imu_data.yaw = yaw;
g_imu_data.gyro_z = raw.gyro_z;
g_imu_data.accel_x = raw.accel_x;
g_imu_data.accel_y = raw.accel_y;
}
// 在传感器任务中, 用分频器实现200Hz调用:
void Task_SensorMonitor(void *pvParameters) {
TickType_t xLastWake = xTaskGetTickCount();
uint8_t imu_divider = 0;
for (;;) {
// IMU: 每5ms一次 (任务周期5ms)
IMU_ProcessLoop();
// 其他传感器: 分频调用
if (++imu_divider >= 4) { // 每20ms
CliffSensor_Update(&pkt);
imu_divider = 0;
}
vTaskDelayUntil(&xLastWake, pdMS_TO_TICKS(5)); // 5ms周期
}
}
3.7 调试步骤
Step 1: I2C通信验证
├── 读WHO_AM_I寄存器, 应返回0x47
├── 如果失败: 检查上拉电阻、地址、接线
└── ✅ 读到正确ID
Step 2: 原始数据验证
├── 水平静止放置, 串口打印6轴数据
├── accel_z ≈ +9.81 m/s² (重力加速度)
├── accel_x ≈ 0, accel_y ≈ 0 (±0.2以内)
├── gyro_x/y/z ≈ 0 (±0.02 rad/s以内)
└── ✅ 数据合理, 噪声低
Step 3: 陀螺仪零偏验证
├── 观察校准后的 gyro_offset 值
├── 应该在 -500~+500 (原始值) 范围内
├── 如果偏移太大: 校准时板子没放稳
└── ✅ 零偏合理
Step 4: Mahony姿态验证 (最关键!)
├── 串口实时打印 Yaw 角度
├── 静止不动: Yaw漂移应 < 1°/分钟
├── 手转90°: Yaw应跟随变化约90° (±3°误差可接受)
├── 转360°回到原位: Yaw应回到初始值 (±5°误差)
├── 开机跑5分钟: Yaw累计漂移应 < 5°
└── ✅ 角度跟随准确, 漂移可控
Step 5: 振动环境验证
├── 把IMU装在机器人底盘上, 开启电机
├── 观察Yaw角是否出现跳变
├── 如果跳变大: 降低Kp (从1.5降到0.8)
├── 如果响应慢: 提高Kp (从1.5提高到2.5)
└── ✅ 电机运行时Yaw角稳定
3.8 Mahony 调参指南
┌─────────────────────────────────────────────────────┐
│ Kp (比例增益) 调参 │
├──────────────┬──────────────────────────────────────┤
│ Kp 太小 │ 修正慢, 陀螺仪漂移得不到及时修正 │
│ (< 0.5) │ 表现: 转完后角度慢慢才跟上 │
├──────────────┼──────────────────────────────────────┤
│ Kp 合适 │ 角度快速跟随, 静止时稳定 │
│ (1.0 ~ 2.0) │ 表现: 手转板子, 角度实时跟随 │
├──────────────┼──────────────────────────────────────┤
│ Kp 太大 │ 加速度计噪声被放大, 角度抖动 │
│ (> 3.0) │ 表现: 静止时Yaw角不停小幅度跳动 │
└──────────────┴──────────────────────────────────────┘
┌─────────────────────────────────────────────────────┐
│ Ki (积分增益) 调参 │
├──────────────┼──────────────────────────────────────┤
│ Ki = 0 │ 无积分, 陀螺仪会持续漂移 │
├──────────────┼──────────────────────────────────────┤
│ Ki 合适 │ 长时间静止后角度不漂移 │
│ (0.005~0.02) │ 表现: 放10分钟, Yaw变化<2° │
├──────────────┼──────────────────────────────────────┤
│ Ki 太大 │ 积分饱和, 角度过冲 │
│ (> 0.05) │ 表现: 转动停止后角度继续"惯性"变化 │
└──────────────┴──────────────────────────────────────┘
复述要点：ICM-42688 没有DMP，姿态解算完全靠STM32软件实现。采用 Mahony AHRS算法，核心思想是用加速度计测量的重力方向去修正陀螺仪积分产生的姿态漂移。算法输入是6轴原始数据，输出是四元数，再转换成欧拉角。扫地机最关心的是 Yaw（航向角），用于航迹推算中的方向保持。200Hz调用，每次计算<0.05ms，STM32F4的FPU完全能胜任。
四、三大模块协作时序图
时间轴 ──────────────────────────────────────────────────────►
0ms 5ms 10ms 15ms 20ms
│ │ │ │ │
▼ ▼ ▼ ▼ ▼
┌─IMU读取+Mahony更新 (200Hz)──────────────────────────┐
│ I2C读6轴 → Mahony_Update → 更新Yaw角 │
└─────────────────────────────────────────────────────┘
┌─霍尔中断 (异步, ~每2ms一次)────────────────────────┐
│ GPIO_EXTI → 读霍尔状态 → 查表换相 → 刷新PWM │
└─────────────────────────────────────────────────────┘
┌─编码器测速 (50Hz)───────────────────────────────────┐
│ 读TIM计数 → 算速度 → 低通滤波 │
│ ↓ │
│ PID速度环更新 (1kHz) → 新占空比 │
└─────────────────────────────────────────────────────┘
┌─导航决策 (50Hz)─────────────────────────────────────┐
│ 读Yaw + 读速度 → 里程计更新 │
│ 状态机判断 → 下发速度指令 │
└─────────────────────────────────────────────────────┘
复述整个系统：IMU以200Hz读取数据并运行Mahony算法输出Yaw角；编码器以50Hz测速，PID以1kHz更新电机PWM；霍尔中断异步触发六步换相。导航任务50Hz综合Yaw角和轮速做航迹推算，状态机根据位姿和传感器数据决策下一步动作。三个模块各司其职，通过FreeRTOS队列和共享变量协作。
浙公网安备 33010602011771号