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:
+13
-57
@@ -14,6 +14,7 @@
|
||||
#include "flash.h"
|
||||
#include "motor.h"
|
||||
#include "pid.h"
|
||||
#include "run_inhibit.h"
|
||||
#include "systick.h"
|
||||
#include <arm_math.h>
|
||||
#include <cmath>
|
||||
@@ -47,7 +48,10 @@ extern FlashEEPROM flashEEP;
|
||||
void calibrate_zero_position(void) {
|
||||
// 运行中禁止校准:要求控制模式为停止(0)且基本静止,
|
||||
// 否则转子无法对齐到电气 0°,标定结果错误,甚至因电角度突变导致失控。
|
||||
if (FOC.control_mode != 0 || fabsf(FOC.rt_velocity) > CALIB_MAX_SPEED) {
|
||||
// 被禁止运行时同样拒绝:本函数会直接 motor_resume() 唤醒驱动,
|
||||
// 不判闸门就成了停机态下的旁路通道。
|
||||
if (FOC.control_mode != 0 || !run_inhibit.permitted() ||
|
||||
fabsf(FOC.rt_velocity) > CALIB_MAX_SPEED) {
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -387,43 +391,6 @@ inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
|
||||
Set_3Pwm(duty_a, duty_b, duty_c);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 电机关机(通用关机,不带原因标记)
|
||||
*
|
||||
*/
|
||||
void FOC_Controller::shut_down() {
|
||||
drv_stop();
|
||||
motor_sleep();
|
||||
is_shut_down = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 过热关机
|
||||
*
|
||||
*/
|
||||
void FOC_Controller::shut_down_thermal() {
|
||||
drv_stop();
|
||||
motor_sleep();
|
||||
is_shut_down = true;
|
||||
thermal_shutdown = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 过热恢复
|
||||
*
|
||||
*/
|
||||
void FOC_Controller::resume_from_thermal() {
|
||||
thermal_shutdown = false;
|
||||
if (!voltage_error && !over_current) {
|
||||
is_shut_down = false;
|
||||
motor_resume();
|
||||
}
|
||||
}
|
||||
/**
|
||||
* @brief 占空比置0
|
||||
*
|
||||
*/
|
||||
void FOC_Controller::drv_stop() { Set_3Pwm(0.0f, 0.0f, 0.0f); }
|
||||
/**
|
||||
* @brief 电机锁止
|
||||
*/
|
||||
@@ -449,28 +416,17 @@ float FOC_Controller::PWM_out() {
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 电机恢复
|
||||
*
|
||||
*/
|
||||
void FOC_Controller::resume() {
|
||||
over_current = false;
|
||||
voltage_error = false;
|
||||
is_shut_down = false;
|
||||
motor_resume();
|
||||
}
|
||||
/**
|
||||
* @brief 更新电源电压
|
||||
*
|
||||
* @note 读数不在合理区间(含采样链给的 ERROR_ADC_VAL 哨兵值)时保留上次值,
|
||||
* 不更新。这里不再置"电压故障"标志:欠压判定与停机都在
|
||||
* fast_voltage_check() → run_inhibit 那条路上做。
|
||||
*/
|
||||
void FOC_Controller::update_power(float voltage) {
|
||||
if (voltage < 36.0f and voltage > 8.0f) {
|
||||
voltage_error = false;
|
||||
voltage_power_supply = voltage;
|
||||
v_half = voltage * 0.5f;
|
||||
v_safe = voltage * 1.1f;
|
||||
} else {
|
||||
voltage_error = true;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -524,15 +480,15 @@ void FOC_Controller::fast_voltage_check(void) {
|
||||
bool drop_exceeded = (voltage_drop > VBUS_DROP_THRESHOLD);
|
||||
bool below_absolute = (current_vbus < VBUS_ABSOLUTE_MIN);
|
||||
|
||||
if ((drop_exceeded || below_absolute) && (!is_shut_down)) {
|
||||
// 只看本来源是否已经申请过:不查别的故障源,避免欠压判定被过热/nFAULT 挡住
|
||||
if ((drop_exceeded || below_absolute) &&
|
||||
(run_inhibit.sources() & INHIBIT_UNDERVOLTAGE) == 0U) {
|
||||
// 电压跌落计数(防抖)
|
||||
vbus_drop_count++;
|
||||
|
||||
if (vbus_drop_count >= VBUS_DEBOUNCE_COUNT) {
|
||||
// 确认需要关机,立即停机
|
||||
drv_stop();
|
||||
motor_sleep();
|
||||
is_shut_down = true;
|
||||
// 确认需要关机:只申请禁止位,停机动作由 run_inhibit 在控制任务里执行
|
||||
run_inhibit.request(INHIBIT_UNDERVOLTAGE);
|
||||
emergency_shutdown = true; // 标记紧急关机,低频任务会执行保存
|
||||
vbus_drop_count = 0; // 重置计数器
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user