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 <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() {
#if GATE_DRIVER_DRV8323
	// nFAULT 快速保护（低有效，1ms 轮询）：GDF/UVLO/OTSD 等硬件故障
	// → 立即封 PWM 并睡眠，锁存到 CAN 重新下发恢复命令为止。
	// VDS 过流当前配置为"仅上报不动作"，不会拉低 nFAULT，不在此误触发。
	if (drv_nfault_active()) {
		FOC.shut_down();
	}
#endif
	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;

	if (remainingTime10Ms) {
		// 过热停机状态
		// 检查是否可以恢复
		q16_16_t recovery_threshold =
			Q16_16_MUL(thermal_capacity, recoveryRate);
		if (thermal_value < recovery_threshold) {
			remainingTime10Ms = 1; // 可以恢复工作
		}
		if (remainingTime10Ms == 1) { // 过热恢复
			FOC.resume_from_thermal();
			CAN0_transmiter.ThermalStatus = 0;
			return --remainingTime10Ms;
		}
		return --remainingTime10Ms;
	} else {
		// 当前没有过热停机
		// 添加电流热积累：取三相有效相的最大绝对值（热量与电流方向无关，
		// 负半周同样发热）。采样失效帧(ERROR_ADC_VAL)不参与积累：
		// 失效值 255 按 mA 参与平方运算会超出 int/u32 范围，热积累不可控；
		// 三相持续全部失效说明采样链异常，按过热关机 fail-safe。
		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 (++current_error_frames >= CURRENT_ERROR_SHUTDOWN_COUNT) {
				current_error_frames = 0;
				FOC.shut_down_thermal();
				remainingTime10Ms = PROTECT_TIME;
				CAN0_transmiter.ThermalStatus = 1;
				return remainingTime10Ms;
			}
			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) {
		FOC.shut_down_thermal();
		remainingTime10Ms = PROTECT_TIME; // 过热停机
		CAN0_transmiter.ThermalStatus = 1;
		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%以上
		if (FOC.vbus_reference > 0 && vbus > FOC.vbus_reference * VBUS_RECOVERY_RATIO) {
			if (!FOC.thermal_shutdown) {
				FOC.resume();
				voltage_state = VoltageState::NORMAL;
			}
		}
		break;

	default:
		break;
	}
}