cbb/Motor_v2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(motor): 引入运行禁止机制重构电机控制安全系统

Browse Source 
- 添加 run_inhibit.h/cpp 模块作为全局运行闸门，统一管理所有停机/唤醒逻辑
- 移除 FOC 控制器中的 5 个相互引用的开关机函数（shut_down/resume 等）
- 将 nFAULT 硬件故障检测从仅上报改为运行门控制，连续采样故障时申请禁止位
- 改进电压检测逻辑，欠压判定现在通过 run_inhibit 统一执行停机动作
- 修正 PB2(nFAULT) 引脚配置为内部上拉输入，避免浮空误读导致系统锁死
- 在 CAN 状态帧中新增 RunInhibit 位图（字节 36-39），实时反映禁止运行的故障源
- 电流采样失效、过热故障现在通过禁止位管理而非直接操作驱动状态
- 校准函数增加运行许可检查，防止在停机状态下意外唤醒驱动
This commit is contained in:
cbb
2026-10-13 00:47:52 +08:00
parent 72cc10b40d
commit 86ca7666fa
11 changed files with 255 additions and 144 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/sys/config.h
+13 -1
View File
@@ -73,7 +73,10 @@ typedef volatile uint32_t vu32;
constexpr u16 SOFTWARE_VERSION_MAJOR = CFG_VER_YEAR % 100; // 年后两位，如 26
constexpr u16 SOFTWARE_VERSION_MINOR = CFG_VER_MONTH; // 月
constexpr u16 SOFTWARE_VERSION_PATCH = CFG_VER_DAY; // 日
constexpr u16 SOFTWARE_VERSION_BUILD = 1; // 当天第几次构建，手动递增
// 当天第几次构建（日期由 __DATE__ 自动带出）：
// 26.10.10.2 = nFAULT(MotorStatus) 由"仅上报"改为运行门
// 26.10.11.1 = 停机/解除统一收进 RunInhibit 闸门，删掉 FOC 里 5 个互引的开关机函数
constexpr u16 SOFTWARE_VERSION_BUILD = 1;
// 硬件版本：全手写，仅在 PCB 改版时修改（编译 APP 不影响）
constexpr u16 HARDWARE_VERSION_MAJOR = 25;
@@ -406,6 +409,15 @@ constexpr float VBUS_ABSOLUTE_MIN = 12.0f; // 绝对最低电压 (V)
constexpr u32 VBUS_DEBOUNCE_COUNT = 5; // 快速检测：防抖计数 (5次×100us = 500us)
constexpr float VBUS_RECOVERY_RATIO = 0.9f; // 恢复比例（基准电压的90%）
// ===================== 门驱动 nFAULT（STATUS 帧 motStatus / MotorStatus）=====================
// PB2 读门驱动 nFAULT（开漏低有效，固件已配内部上拉）。DRV8313/DRV8323 的
// UVLO、热关断、过流都会把它拉低，因此它就是"电机侧出错"的硬件凭据。
// 轮询点在 thermalController.update()（TASK_THERMAL_CONTROL，1ms）。
constexpr u8 DRV_FAULT_DEBOUNCE_MS = 3u; // 连续 N 次 1ms 采样为低才判故障
// 解除禁止后驱动需要重新建立：DRV8313 手册 7.4.1 给 ≈1ms，按 50us 控制节拍取 20 拍。
// 建立期内输出保持 0、不派发控制函数（见 sys/run_inhibit.cpp）。
constexpr u8 DRV_WAKE_SETTLE_50US = 20u;
Robot_V2/sys/health.cpp
+60 -35
View File
@@ -11,6 +11,7 @@
#include "adc.h"
#include "flash.h"
#include "motor.h"
#include "run_inhibit.h"
#include <cmath>
@@ -35,14 +36,32 @@ static float last_vbus_slow = 0.0f; // 上次电压值（慢速任务用）
** @retval 热量值
*/
u32 ThermalController::update() {
#if GATE_DRIVER_DRV8323
// nFAULT 快速保护（低有效，1ms 轮询）：GDF/UVLO/OTSD 等硬件故障
// → 立即封 PWM 并睡眠，锁存到 CAN 重新下发恢复命令为止。
// VDS 过流当前配置为"仅上报不动作"，不会拉低 nFAULT，不在此误触发。
if (drv_nfault_active()) {
FOC.shut_down();
// nFAULT 检测（STATUS 帧 motStatus / MotorStatus）：本函数 1ms 执行一次。
// 旧实现整段被 #if GATE_DRIVER_DRV8323 圈住，而当前固件按 DRV8313 编译
// （config.h 该宏 = 0）→ 驱动报故障（UVLO / 热关断 / OCP 锁存，三者都会
// 拉低 nFAULT）只上报不动作，控制环照常输出。现在两板都判：
// 连续 DRV_FAULT_DEBOUNCE_MS 次采到低电平 → 申请禁止位，
// 停机由 run_inhibit 在 50us 控制任务里执行；解除只认上位机 motReset。
// DRV8323 板的 VDS 过流配置为"仅上报不动作"，不会拉低 nFAULT，不在此触发。
static u8 nfault_busy_ms = 0;
const bool nfault = (drv_nfault_active() != 0U);
if (nfault) {
if (++nfault_busy_ms >= DRV_FAULT_DEBOUNCE_MS) {
nfault_busy_ms = 0;
run_inhibit.request(INHIBIT_DRV_NFAULT);
}
} else {
nfault_busy_ms = 0;
}
#endif
// MotorStatus 唯一写手：闸门里还有没有 nFAULT 这一位，或此刻引脚确实在低
CAN0_transmiter.MotorStatus =
(run_inhibit.sources() & INHIBIT_DRV_NFAULT) != 0U || nfault;
// 上报位与闸门同源：每拍从禁止位图派生，不要再在各处置 0/1
// （历史上"停机了但状态位说正常"就是多处写造成的）
CAN0_transmiter.ThermalStatus =
(run_inhibit.sources() & INHIBIT_THERMAL) != 0U;
thermal_value = Q16_16_MUL(thermal_value, thermalRate);
// 添加温度积累
int drv_temp = (int)sampler.get_drv_temp();
@@ -53,6 +72,22 @@ u32 ThermalController::update() {
mcu_temp = 0;
thermal_value += drv_temp * drv_temp + mcu_temp * mcu_temp;
// 三相电流采样有效性先算好：热积累与"冷却到点该不该撤销采样失效禁止位"都要用
const float raw_current[3] = {sampler.get_current('U'),
sampler.get_current('V'),
sampler.get_current('W')};
u32 currents[3] = {0, 0, 0};
bool valid[3] = {false, false, false};
for (u8 i = 0; i < 3; i++) {
if (raw_current[i] == ERROR_ADC_VAL) {
continue;
}
// get_current 已保证有效值 |I| ≤ MAX_CURRENT(40A)，转 mA ≤ 40000
currents[i] = (u32)(fabsf(raw_current[i]) * 1000.0f);
valid[i] = true;
}
const bool any_valid = valid[0] || valid[1] || valid[2];
if (remainingTime10Ms) {
// 过热停机状态
// 检查是否可以恢复
@@ -61,9 +96,11 @@ u32 ThermalController::update() {
if (thermal_value < recovery_threshold) {
remainingTime10Ms = 1; // 可以恢复工作
}
if (remainingTime10Ms == 1) { // 过热恢复
FOC.resume_from_thermal();
CAN0_transmiter.ThermalStatus = 0;
if (remainingTime10Ms == 1) { // 冷却到点：各自撤销自己申请的禁止位
run_inhibit.release(INHIBIT_THERMAL);
if (any_valid) {
run_inhibit.release(INHIBIT_ISENSE_FAIL);
}
return --remainingTime10Ms;
}
return --remainingTime10Ms;
@@ -72,29 +109,16 @@ u32 ThermalController::update() {
// 添加电流热积累：取三相有效相的最大绝对值（热量与电流方向无关，
// 负半周同样发热）。采样失效帧(ERROR_ADC_VAL)不参与积累：
// 失效值 255 按 mA 参与平方运算会超出 int/u32 范围，热积累不可控；
// 三相持续全部失效说明采样链异常，按过热关机 fail-safe。
// 三相持续全部失效说明采样链异常，按独立原因 fail-safe 停机
// （不再并入"过热"上报，上位机看到的是 INHIBIT_ISENSE_FAIL）。
static u16 current_error_frames = 0;
const float raw_current[3] = {sampler.get_current('U'),
sampler.get_current('V'),
sampler.get_current('W')};
u32 currents[3] = {0, 0, 0};
bool valid[3] = {false, false, false};
for (u8 i = 0; i < 3; i++) {
if (raw_current[i] == ERROR_ADC_VAL) {
continue;
}
// get_current 已保证有效值 |I| ≤ MAX_CURRENT(40A)，转 mA ≤ 40000
currents[i] = (u32)(fabsf(raw_current[i]) * 1000.0f);
valid[i] = true;
}
if (!(valid[0] || valid[1] || valid[2])) {
if (!any_valid) {
if (++current_error_frames >= CURRENT_ERROR_SHUTDOWN_COUNT) {
current_error_frames = 0;
FOC.shut_down_thermal();
run_inhibit.request(INHIBIT_ISENSE_FAIL);
remainingTime10Ms = PROTECT_TIME;
CAN0_transmiter.ThermalStatus = 1;
return remainingTime10Ms;
return thermal_value;
}
return thermal_value;
}
@@ -132,9 +156,10 @@ u32 ThermalController::update() {
}
if (thermal_value > thermal_capacity) {
FOC.shut_down_thermal();
// 过热：只申请本模块的禁止位，停机由 run_inhibit 执行；
// ThermalStatus 由位图派生（见本函数开头），此处不再单独写
run_inhibit.request(INHIBIT_THERMAL);
remainingTime10Ms = PROTECT_TIME; // 过热停机
CAN0_transmiter.ThermalStatus = 1;
return remainingTime10Ms;
}
return thermal_value;
@@ -181,12 +206,12 @@ void power_manage(void) {
case VoltageState::SHUTDOWN_SAVE:
// 关机保存状态：检查是否可以恢复
// 恢复条件：电压恢复到基准电压的90%以上
// 恢复条件：电压恢复到基准电压的90%以上。
// 只撤销欠压这一位；是否真的能运行由闸门看位图是否清空决定，
// 这里不查其它故障源（过热/nFAULT 各自有各自的解除方）。
if (FOC.vbus_reference > 0 && vbus > FOC.vbus_reference * VBUS_RECOVERY_RATIO) {
if (!FOC.thermal_shutdown) {
FOC.resume();
voltage_state = VoltageState::NORMAL;
}
run_inhibit.release(INHIBIT_UNDERVOLTAGE);
voltage_state = VoltageState::NORMAL;
}
break;
Robot_V2/sys/run_inhibit.cpp
+50
View File
@@ -0,0 +1,50 @@
/**
* @file sys/run_inhibit.cpp
* @brief 电机"禁止运行"闸门实现
*/
#include "run_inhibit.h"
#include "Timer.h"
#include "motor.h"
// 全局单例：定义放本文件，声明在 main.cpp 之外由各调用方 extern（见 run_inhibit.h）
RunInhibit run_inhibit;
/**
* @brief 申请禁止
*/
void RunInhibit::request(u32 src) { bits |= src; }
/**
* @brief 撤销本来源的禁止申请
*/
void RunInhibit::release(u32 src) { bits &= ~src; }
/**
* @brief 执行闸门：禁止/唤醒建立期封 PWM，禁止时让驱动睡眠；解除后唤醒并等建立时间
* @note 睡眠（nSLEEP 拉低）本身会复位 DRV8313 内部逻辑含故障锁存，所以
* "芯片侧锁存"由这里顺带清掉；"要不要继续禁止"完全由位图决定。
* 解除后不立即派发控制函数：DRV8313 手册 7.4.1 要求唤醒后约 1ms 才
* 完全工作，这段建立期内输出保持 0。若故障源还在，检测方会在下一轮
* 轮询里重新 request()，本模块不需要知道是谁、为什么。
*/
void RunInhibit::update(void) {
if (bits != 0U) {
if (!stopped) {
motor_sleep();
stopped = true;
}
// 建立窗口每拍重置：解除后要重新等满 DRV_WAKE_SETTLE_50US
wake_ticks = DRV_WAKE_SETTLE_50US;
Set_3Pwm(0.0f, 0.0f, 0.0f);
return;
}
if (stopped) {
motor_resume();
stopped = false;
}
if (wake_ticks != 0U) {
Set_3Pwm(0.0f, 0.0f, 0.0f);
wake_ticks--;
}
}
Robot_V2/sys/run_inhibit.h
+77
View File
@@ -0,0 +1,77 @@
/**
* @file sys/run_inhibit.h
* @brief 电机"禁止运行"闸门：唯一有权决定桥臂能不能输出的模块。
* @note 分层约定（新增故障必须照此办，不要在检测处写停机动作）：
* - 故障检测方 只调 request(来源位) —— 申请禁止，不碰 PWM/使能脚
* - 故障解除方 只调 release(来源位) —— 撤销自己的申请，不查别人的位
* - 功率级动作 只发生在本模块 update() —— 由 50us 控制任务驱动
* 因此任何一处都不需要知道其它故障源的存在；"能不能运行"= 位图是否为空。
*/
#ifndef _RUN_INHIBIT_H
#define _RUN_INHIBIT_H
#include "config.h"
/**
* @brief 禁止来源位图（每一位一个独立申请方，可并存）
*/
enum InhibitSource : u32 {
INHIBIT_DRV_NFAULT = 1u << 0, // 门驱动 nFAULT(PB2) 有效：UVLO/热关断/OCP 锁存
INHIBIT_UNDERVOLTAGE = 1u << 1, // 母线跌幅超阈值或低于绝对下限
INHIBIT_THERMAL = 1u << 2, // 热模型过热（含温度采集正常时的电流热积累）
INHIBIT_ISENSE_FAIL = 1u << 3, // 三相电流采样持续失效（原先被并入"过热"上报）
// 上位机还没下发过非零模式：保持驱动睡眠。这不是故障，而是"未被使能"，
// 放在同一张位图里是为了让功率级只有一个执行者。老板 v2 的 EN 有 10k 下拉、
// DRV8313 上电默认睡眠；若上电即唤醒，模式 0(drv_hold 恒 10% 占空比锁止)
// 会立刻给 A 相通流发热，而"上电先发模式 0 当静止待机"是现有上位机行为。
INHIBIT_NOT_ENABLED = 1u << 7,
};
/**
* @brief RunInhibit 类：禁止运行闸门
*/
class RunInhibit {
// 上电即禁止：等上位机首次下发非零模式解除（见 INHIBIT_NOT_ENABLED）
u32 bits = INHIBIT_NOT_ENABLED;
bool stopped = false; // 上一拍是否已施加停机（避免每 50us 重复翻转使能脚）
u8 wake_ticks = 0; // 唤醒建立倒计时（50us 节拍）
public:
/**
* @brief 申请禁止（幂等；同一位重复申请无副作用）
* @param[in] src InhibitSource 位
* @return void
*/
void request(u32 src);
/**
* @brief 撤销本来源的禁止申请；位图清空后才允许运行
* @param[in] src InhibitSource 位
* @return void
*/
void release(u32 src);
/**
* @brief 是否允许运行（只看位图，不看唤醒状态）
* @return bool true=允许
*/
bool permitted() const { return bits == 0U; }
/**
* @brief 允许运行且驱动唤醒建立已完成，控制函数可派发
* @return bool true=可派发
*/
bool ready() const { return bits == 0U && wake_ticks == 0U; }
/**
* @brief 当前禁止来源位图（上报/诊断用）
* @return u32 位图
*/
u32 sources() const { return bits; }
/**
* @brief 执行闸门：50us 控制任务每拍调用，唯一碰 PWM 与驱动使能的地方
* @return void
*/
void update(void);
};
extern RunInhibit run_inhibit;
#endif