feat(motor): 引入运行禁止机制重构电机控制安全系统
- 添加 run_inhibit.h/cpp 模块作为全局运行闸门，统一管理所有停机/唤醒逻辑 - 移除 FOC 控制器中的 5 个相互引用的开关机函数（shut_down/resume 等） - 将 nFAULT 硬件故障检测从仅上报改为运行门控制，连续采样故障时申请禁止位 - 改进电压检测逻辑，欠压判定现在通过 run_inhibit 统一执行停机动作 - 修正 PB2(nFAULT) 引脚配置为内部上拉输入，避免浮空误读导致系统锁死 - 在 CAN 状态帧中新增 RunInhibit 位图（字节 36-39），实时反映禁止运行的故障源 - 电流采样失效、过热故障现在通过禁止位管理而非直接操作驱动状态 - 校准函数增加运行许可检查，防止在停机状态下意外唤醒驱动
This commit is contained in:
+60
-35
@@ -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;
|
||||
|
||||
|
||||
Reference in New Issue
Block a user