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; // 重置计数器
|
||||
}
|
||||
|
||||
@@ -39,10 +39,8 @@ class FOC_Controller {
|
||||
float rt_velocity = 0.0f;
|
||||
float rt_current = 0.0f;
|
||||
|
||||
bool over_current = false;
|
||||
bool voltage_error = false;
|
||||
bool is_shut_down = false;
|
||||
bool thermal_shutdown = false; // 过热关机标志
|
||||
// 关机事件标记：通知低频任务把当前状态落盘（不是"能不能运行"的判据，
|
||||
// 运行许可一律看 sys/run_inhibit.h）
|
||||
bool emergency_shutdown = false; // 紧急电压跌落关机标志
|
||||
|
||||
float voltage_power_supply = 24.0f; // 电源电压
|
||||
@@ -108,21 +106,6 @@ class FOC_Controller {
|
||||
* @return void
|
||||
*/
|
||||
void DFOC_alignSensor(int _PP);
|
||||
/**
|
||||
* @brief shut_down 函数
|
||||
* @return void
|
||||
*/
|
||||
void shut_down(void);
|
||||
/**
|
||||
* @brief resume 函数
|
||||
* @return void
|
||||
*/
|
||||
void resume(void);
|
||||
/**
|
||||
* @brief drv_stop 函数
|
||||
* @return void
|
||||
*/
|
||||
void drv_stop(void);
|
||||
/**
|
||||
* @brief drv_hold 函数
|
||||
* @return float 返回值
|
||||
@@ -133,16 +116,6 @@ class FOC_Controller {
|
||||
* @return float 返回值
|
||||
*/
|
||||
float PWM_out(void);
|
||||
/**
|
||||
* @brief shut_down_thermal 函数
|
||||
* @return void
|
||||
*/
|
||||
void shut_down_thermal(void); // 过热关机
|
||||
/**
|
||||
* @brief resume_from_thermal 函数
|
||||
* @return void
|
||||
*/
|
||||
void resume_from_thermal(void); // 过热恢复
|
||||
/**
|
||||
* @brief fast_voltage_check 函数
|
||||
* @return void
|
||||
|
||||
Reference in New Issue
Block a user