Motor_v2/Robot_V2/sys/health.cpp

#include "health.h"
#include "config.h"
#include "Can.h"
#include "FOC.h"
#include "adc.h"
#include "flash.h"


extern sampler sampler;
extern CAN_transmiter CAN0_transmiter;
extern FOC_Controller FOC;
extern FlashEEPROM flashEEP;
ThermalController thermalController; // 热量控制器初始化

#define max(a, b, c)                                                           \
	((a) > (b) ? ((a) > (c) ? (a) : (c)) : ((b) > (c) ? (b) : (c)))
constexpr u16 PROTECT_TIME = 6000; // 过热保护时间，单位*10ms

// 电压管理状态机
static VoltageState voltage_state = VoltageState::NORMAL;
static float last_vbus_slow = 0.0f;	// 上次电压值（慢速任务用）
/**
** @brief  热能计算
** @param  None
** @retval 热量值
*/
u32 ThermalController::update() {
	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 {
		// 当前没有过热停机
		// 添加电流热积累
		u32 currents[3] = {(u32)(sampler.get_current('U') * 1000),
						   (u32)(sampler.get_current('V') * 1000),
						   (u32)(sampler.get_current('W') * 1000)};

		// 找最大电流（原始ADC值）
		current_max = max(currents[0], currents[1], currents[2]);
		if (current_max < 2000) {
			thermal_value += current_max * current_max * gapTimeMs *
							 0.00007f; // 计算当前电流热积累值
		} else if (current_max > 3800) {
			thermal_value += current_max * current_max * gapTimeMs *
							 0.01f; // 添加当前电流热积累值
		} else if (current_max > 3400) {
			thermal_value += current_max * current_max * gapTimeMs *
							 0.005f; // 计算当前电流热积累值
		} else if (current_max > 3000) {
			thermal_value += current_max * current_max * gapTimeMs *
							 0.0002f; // 计算当前电流热积累值
		} else if (current_max > 2000) {
			thermal_value += current_max * current_max * gapTimeMs *
							 0.0001f; // 计算当前电流热积累值
		}
	}

	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的电源电压参数
	FOC.update_power(last_vbus_slow);
	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;
	}
}