diff --git a/Robot_V2/Algorithm/FOC.cpp b/Robot_V2/Algorithm/FOC.cpp index 6715a72..59005f8 100644 --- a/Robot_V2/Algorithm/FOC.cpp +++ b/Robot_V2/Algorithm/FOC.cpp @@ -14,6 +14,7 @@ #include "flash.h" #include "motor.h" #include "pid.h" +#include "run_inhibit.h" #include "systick.h" #include #include @@ -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; // 重置计数器 } diff --git a/Robot_V2/Algorithm/FOC.h b/Robot_V2/Algorithm/FOC.h index 1d8c683..88e4acb 100644 --- a/Robot_V2/Algorithm/FOC.h +++ b/Robot_V2/Algorithm/FOC.h @@ -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 diff --git a/Robot_V2/Robot_V2.cproject.yml b/Robot_V2/Robot_V2.cproject.yml index b5ae6b8..05f3228 100644 --- a/Robot_V2/Robot_V2.cproject.yml +++ b/Robot_V2/Robot_V2.cproject.yml @@ -14,6 +14,8 @@ project: - file: sys/initial.h - file: sys/health.h - file: sys/health.cpp + - file: sys/run_inhibit.h + - file: sys/run_inhibit.cpp - file: sys/flash.h - file: sys/flash.cpp - file: sys/config.h diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 465f36e..ca31683 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -18,6 +18,7 @@ #include "gd32c10x_gpio.h" #include "initial.h" #include "motor.h" +#include "run_inhibit.h" #include "systick.h" #include #include @@ -766,7 +767,8 @@ void CAN_transmiter::update(void) { mcu_temp = sampler.get_mcu_temp() * 100; motor_temp = sampler.get_drv_temp() * 100; Power_Voltage = sampler.get_vbus_voltage() * 1000; - MotorStatus = gpio_input_bit_get(GPIOB, GPIO_PIN_2) == RESET; + // MotorStatus 不在此读取:改由 health.cpp 的 1ms nFAULT 轮询写入, + // 那份判据同时决定是否停机,避免"上报的位"和"停机的依据"分家。 EEPROMStatus = flashEEP.EEPROMStatus; Target_Control_Mode = FOC.control_mode; controller_Mode = FOC.control_mode; @@ -788,7 +790,9 @@ void CAN_transmiter::update(void) { u8 CAN_transmiter::send_status_frame() { // 打包 status 数据 - 只清零实际使用的 48 字节 // v0.74:字节 32~35 新增 InitStatus(初始化错误码位图,0=全部正常, - // 定义见 config.h INIT_ERR_*,含电流检测配平超限/无采样帧),36~47 预留 + // 定义见 config.h INIT_ERR_*,含电流检测配平超限/无采样帧) + // 本版:字节 36~39 新增 RunInhibit(当前"禁止运行"来源位图,0=允许运行, + // 定义见 sys/run_inhibit.h InhibitSource)。DBC 尚未加信号,抓原始帧即可读。 memset(transmit_data, 0, 48); pack_u16_to_bytes(transmit_data, 0, mcu_temp); pack_u16_to_bytes(transmit_data, 2, motor_temp); @@ -808,6 +812,7 @@ u8 CAN_transmiter::send_status_frame() { transmit_data[30] = (test_u16 >> 8) & 0xFF; transmit_data[31] = cpu_usage; pack_u32_to_bytes(transmit_data, 32, (u32)init_error_flags); + pack_u32_to_bytes(transmit_data, 36, run_inhibit.sources()); // 使用节点 ID 构建发送 ID (高 16 位为节点 ID) return CANFD_AsyncSend(build_can_id(CANFD_STATUS_ID), 48, transmit_data); } @@ -1111,15 +1116,11 @@ void CAN_transmiter::handle_node_message(can_receive_message_struct ¤t_mes void CAN_transmiter::handle_control_command(can_receive_message_struct ¤t_message) { // 控制帧布局:模式、电压限制、目标电流/速度/角度、三组 P 参数。 - /* 收到非零控制模式即视为驱动意图:v2 板 EN 经 R17 10k 下拉、DRV8313 - * 上电默认睡眠(上电配平后也保持睡眠态),控制路径里没有其他唤醒点, - * 不在此唤醒则所有闭环控制都落在 Hi-Z 桥臂上永远无效。 - * 模式 0(drv_hold 主动锁止,A 相恒 HOLD_PWM_DUTY)不唤醒:上位机 - * 上电先发模式 0 当"静止待机"时不至于意外通电锁止发热;驱动已在 - * 唤醒态时模式 0 仍是锁止。幂等;关机态(过流/过压/过热)不唤醒, - * 恢复仍由 power_manage / resume_from_thermal 统一管理。 */ - if (current_message.rx_data[0] != 0U && !FOC.is_shut_down) { - motor_resume(); + // 这里不再直接碰驱动使能:唤醒功率级是 run_inhibit 闸门在 50us 控制任务里做的。 + // 非零模式 = 上位机使能意图,撤销"未使能"这一禁止位;模式 0(静止待机/锁止) + // 不解除,保持上电默认睡眠。被禁止期间本函数照常收下参数,但控制函数不会被派发。 + if (current_message.rx_data[0] != 0U) { + run_inhibit.release(INHIBIT_NOT_ENABLED); } FOC.control_mode = current_message.rx_data[0]; FOC.limit_voltage = float(unpack_bytes_to_u16(current_message.rx_data, 1)) / 1000; @@ -1160,6 +1161,10 @@ void CAN_transmiter::handle_reset_command(can_receive_message_struct ¤t_me u8 eigth_byte = current_message.rx_data[7]; if (first_byte & 0x01) { + // motReset = 撤销 nFAULT 这一禁止申请。之后能否真的重新运行由闸门看位图; + // 若故障源还在,1ms 的 nFAULT 轮询会立刻重新 request(),而驱动唤醒建立期 + // (DRV_WAKE_SETTLE_50US)内输出保持 0,不会带故障出 torque。 + run_inhibit.release(INHIBIT_DRV_NFAULT); motor_reset(); testfloat_union.f = KTH7823.getcurrent_sensorangle(); } diff --git a/Robot_V2/device/Can.h b/Robot_V2/device/Can.h index 65c6bd8..96010ec 100644 --- a/Robot_V2/device/Can.h +++ b/Robot_V2/device/Can.h @@ -207,8 +207,9 @@ class CAN_transmiter { float_union_t w_current_union = {0}; // W 相电流 float_union_t v_current_union = {0}; // V 相电流 - bool MotorStatus = 0; // 电机状态码 u8 EEPROMStatus = 0; // EEPROM 状态码 + // MotorStatus 不在此处:判据与上报都收在 health 的 1ms nFAULT 轮询里, + // 声明见 public 段(与 MagStatus/ThermalStatus 同处),避免出现第二个写手。 can_trasnmit_message_struct transmit_message; // 发送消息结构体 u8 transmit_mailbox = CAN_NOMAILBOX; // 初始化为无 mailbox 状态 @@ -377,6 +378,9 @@ class CAN_transmiter { bool MagStatus = 0; // 磁状态码 bool ThermalStatus = 0; // 热能状态码 + // nFAULT 状态位(STATUS 帧 motStatus)。唯一写手 = health.cpp 的 1ms nFAULT + // 轮询,值与"是否允许运行"的判据同源;CAN update() 不再自行读引脚。 + bool MotorStatus = 0; // 门驱动故障状态码 /** * @brief communication_check 函数 * @return void diff --git a/Robot_V2/device/motor.cpp b/Robot_V2/device/motor.cpp index d15947a..061b23f 100644 --- a/Robot_V2/device/motor.cpp +++ b/Robot_V2/device/motor.cpp @@ -23,7 +23,7 @@ extern void KTH7823_spi_init(void); /** * @brief 门驱动芯片引脚预初始化 * @note 新板:DRVSLP(PB12) 置低保持 DRV8323 睡眠、片选 PB14 释放、 - * PB2(nFAULT) 浮空输入。必须在任何可能拉高使能脚的代码之前调用; + * PB2(nFAULT) 内部上拉输入。必须在任何可能拉高使能脚的代码之前调用; * 老板:无操作(原时序在 timer1_gpio_config 中保持不变)。 */ void gate_driver_preinit(void) { @@ -35,8 +35,8 @@ void gate_driver_preinit(void) { /* SPI 片选默认释放(SCLK 空闲低时才允许 CS 翻转) */ gpio_init(GPIOB, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_14); gpio_bit_set(GPIOB, GPIO_PIN_14); - /* nFAULT 低有效 */ - gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_2); + /* nFAULT 低有效,开漏输出 → 内部上拉,避免浮空误读成故障 */ + gpio_init(GPIOB, GPIO_MODE_IPU, GPIO_OSPEED_50MHZ, GPIO_PIN_2); #endif } @@ -221,8 +221,11 @@ void motor_init(void) { timer1_pwm_config(); // 使能GPIOB时钟 rcu_periph_clock_enable(RCU_GPIOB); - // 配置PB2为输入引脚,用于读取电机状态(nFAULT) - gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_2); + // 配置PB2为输入引脚,用于读取电机状态(nFAULT)。 + // 用内部上拉(GPIO_MODE_IPU)而不是浮空:nFAULT 是开漏输出,无故障时靠 + // 上拉保持高。现在 PB2 电平直接决定"允不允许运行",浮空输入在接线未确认 + // 或无上拉时会随机读低,把整机锁死;内部上拉(~40kΩ)与外部上拉并联无害。 + gpio_init(GPIOB, GPIO_MODE_IPU, GPIO_OSPEED_50MHZ, GPIO_PIN_2); /* 配置GPIOB的PB14为推挽输出 SPICS */ gpio_init(GPIOB, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_14 | GPIO_PIN_12); diff --git a/Robot_V2/sys/config.h b/Robot_V2/sys/config.h index 052126d..0f494e7 100644 --- a/Robot_V2/sys/config.h +++ b/Robot_V2/sys/config.h @@ -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; + diff --git a/Robot_V2/sys/health.cpp b/Robot_V2/sys/health.cpp index 78a5fd2..ec82d5f 100644 --- a/Robot_V2/sys/health.cpp +++ b/Robot_V2/sys/health.cpp @@ -11,6 +11,7 @@ #include "adc.h" #include "flash.h" #include "motor.h" +#include "run_inhibit.h" #include @@ -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; diff --git a/Robot_V2/sys/run_inhibit.cpp b/Robot_V2/sys/run_inhibit.cpp new file mode 100644 index 0000000..a7c55a9 --- /dev/null +++ b/Robot_V2/sys/run_inhibit.cpp @@ -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--; + } +} diff --git a/Robot_V2/sys/run_inhibit.h b/Robot_V2/sys/run_inhibit.h new file mode 100644 index 0000000..d4520d6 --- /dev/null +++ b/Robot_V2/sys/run_inhibit.h @@ -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 diff --git a/Robot_V2/tasks.cpp b/Robot_V2/tasks.cpp index 7807563..b2378aa 100644 --- a/Robot_V2/tasks.cpp +++ b/Robot_V2/tasks.cpp @@ -10,6 +10,7 @@ #include "adc.h" #include "ang_sensor.h" #include "health.h" +#include "run_inhibit.h" #include "systick.h" #include "gd32c10x.h" @@ -102,15 +103,18 @@ static void run_motor_control_task(void) { FOC.fast_voltage_check(); } + // 运行闸门:全工程唯一执行停机/唤醒功率级的地方。放在模式判断之前, + // 这样即使 control_mode 非法(>=8)也照样封住输出。 + run_inhibit.update(); + if (!run_inhibit.ready()) { + return; // 被禁止运行,或刚解除还在驱动唤醒建立期(输出已由闸门保持 0) + } + if (FOC.control_mode >= 8) return; // 模式合法时再派发控制函数,防止数组越界。 reset_control_loops_on_mode_change(); - if (!FOC.is_shut_down) { - (FOC.*control_functions[FOC.control_mode])(); - } else { - FOC.drv_stop(); - } + (FOC.*control_functions[FOC.control_mode])(); } /**