Motor_v2/Robot_V2/sys/health.cpp

/**
 * @file sys/health.cpp
 * @brief 系统服务与平台支持源文件
 * @note 本文件仅补充 Doxygen 文档注释，不改变代码逻辑。
 */

#include "health.h"
#include "config.h"
#include "Can.h"
#include "FOC.h"
#include "adc.h"
#include "flash.h"
#include "motor.h"
#include "run_inhibit.h"
#include <cmath>


extern sampler sampler;
extern CAN_transmiter CAN0_transmiter;
extern FOC_Controller FOC;
extern FlashEEPROM flashEEP;
// 全局热管理控制器单例：由 main 任务周期调用 update()，用于维护过热保护状态。
ThermalController thermalController; // 热量控制器初始化

/** @brief PROTECT_TIME 常量定义 */
constexpr u16 PROTECT_TIME = 6000; // 过热保护时间，单位*10ms
// 三相电流连续全部失效帧数（×10ms）阈值，达到后按过热关机处理
constexpr u16 CURRENT_ERROR_SHUTDOWN_COUNT = 100;

// 电压管理状态机：仅 power_manage() 慢速任务访问，文件内封装。
static VoltageState voltage_state = VoltageState::NORMAL;
static float last_vbus_slow = 0.0f;	// 上次电压值（慢速任务用）
/**
** @brief  热能计算
** @param  None
** @retval 热量值
*/
u32 ThermalController::update() {
	// 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;
	}
	// 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();
	int mcu_temp = (int)sampler.get_mcu_temp();
	if (drv_temp == 255)
		drv_temp = 0;
	if (mcu_temp == 255)
		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) {
		// 过热停机状态
		// 检查是否可以恢复
		q16_16_t recovery_threshold =
			Q16_16_MUL(thermal_capacity, recoveryRate);
		if (thermal_value < recovery_threshold) {
			remainingTime10Ms = 1; // 可以恢复工作
		}
		if (remainingTime10Ms == 1) { // 冷却到点：各自撤销自己申请的禁止位
			run_inhibit.release(INHIBIT_THERMAL);
			if (any_valid) {
				run_inhibit.release(INHIBIT_ISENSE_FAIL);
			}
			return --remainingTime10Ms;
		}
		return --remainingTime10Ms;
	} else {
		// 当前没有过热停机
		// 添加电流热积累：取三相有效相的最大绝对值（热量与电流方向无关，
		// 负半周同样发热）。采样失效帧(ERROR_ADC_VAL)不参与积累：
		// 失效值 255 按 mA 参与平方运算会超出 int/u32 范围，热积累不可控；
		// 三相持续全部失效说明采样链异常，按独立原因 fail-safe 停机
		// （不再并入"过热"上报，上位机看到的是 INHIBIT_ISENSE_FAIL）。
		static u16 current_error_frames = 0;

		if (!any_valid) {
			if (++current_error_frames >= CURRENT_ERROR_SHUTDOWN_COUNT) {
				current_error_frames = 0;
				run_inhibit.request(INHIBIT_ISENSE_FAIL);
				remainingTime10Ms = PROTECT_TIME;
				return thermal_value;
			}
			return thermal_value;
		}
		current_error_frames = 0;

		// 找最大电流（mA，绝对值）；个别相失效时用剩余有效相
		u32 max_current = 0;
		for (u8 i = 0; i < 3; i++) {
			if (valid[i] && currents[i] > max_current) {
				max_current = currents[i];
			}
		}
		current_max = (u16)max_current;
		// 平方运算先转 float：40A 档 40000²×10 超 int 范围，整数运算会溢出
		const float imax = (float)current_max;
		// 电流系数按"绝对值全周期"口径标定：取 fabs 后正负半周都积累，
		// 交流负载下积累速度约为旧版(仅正半周)的 2 倍，故系数统一取旧值一半，
		// 保持原实测触发时机不变；温度积累与恢复阈值不受此补偿影响。
		if (current_max < 2000) {
			thermal_value += (u32)(imax * imax * gapTimeMs *
								 0.000035f); // 计算当前电流热积累值
		} else if (current_max > 3800) {
			thermal_value += (u32)(imax * imax * gapTimeMs *
								 0.005f); // 添加当前电流热积累值
		} else if (current_max > 3400) {
			thermal_value += (u32)(imax * imax * gapTimeMs *
								 0.0025f); // 计算当前电流热积累值
		} else if (current_max > 3000) {
			thermal_value += (u32)(imax * imax * gapTimeMs *
								 0.0001f); // 计算当前电流热积累值
		} else if (current_max > 2000) {
			thermal_value += (u32)(imax * imax * gapTimeMs *
								 0.00005f); // 计算当前电流热积累值
		}
	}

	if (thermal_value > thermal_capacity) {
		// 过热：只申请本模块的禁止位，停机由 run_inhibit 执行；
		// ThermalStatus 由位图派生（见本函数开头），此处不再单独写
		run_inhibit.request(INHIBIT_THERMAL);
		remainingTime10Ms = PROTECT_TIME; // 过热停机
		return remainingTime10Ms;
	}
	return thermal_value;
}
/**
** @brief  温度检测
** @param  None
*/
void angsens_MG_check(void) {
	bool MGH = gpio_input_bit_get(GPIOB, GPIO_PIN_10);
	bool MGL = gpio_input_bit_get(GPIOB, GPIO_PIN_11);
	CAN0_transmiter.MagStatus = MGH or MGL;
}

/**
 * @brief 电压管理（低频任务，约10ms调用一次）
 *
 * 负责：
 * 1. 执行Flash保存（阻塞操作，不能在高频循环中执行）
 * 2. 状态恢复判断（基于基准电压的比例恢复）
 * 3. 更新FOC的电源电压参数
 */
void power_manage(void) {
	sampler.update_vbus_voltage();
	float vbus = sampler.get_vbus_voltage();

	// 更新FOC的电源电压参数：使用本周期刚采到的 vbus，避免上一拍电压滞后放大速度/电流耦合。
	FOC.update_power(vbus);
	last_vbus_slow = vbus;

	// 如果快速检测已触发紧急关机，执行保存
	if (FOC.emergency_shutdown) {
		flashEEP.save_system_config(true);
		FOC.emergency_shutdown = false; // 清除紧急关机标志
		voltage_state = VoltageState::SHUTDOWN_SAVE;
		return;
	}

	// 状态机处理
	switch (voltage_state) {
	case VoltageState::NORMAL:
		// 正常状态：无需操作
		break;

	case VoltageState::SHUTDOWN_SAVE:
		// 关机保存状态：检查是否可以恢复
		// 恢复条件：电压恢复到基准电压的90%以上。
		// 只撤销欠压这一位；是否真的能运行由闸门看位图是否清空决定，
		// 这里不查其它故障源（过热/nFAULT 各自有各自的解除方）。
		if (FOC.vbus_reference > 0 && vbus > FOC.vbus_reference * VBUS_RECOVERY_RATIO) {
			run_inhibit.release(INHIBIT_UNDERVOLTAGE);
			voltage_state = VoltageState::NORMAL;
		}
		break;

	default:
		break;
	}
}