cbb
4c4e47e9e3
添加了完整的Doxygen文档注释到FOC算法模块的所有函数、类和变量，提升了代码可读性和维护性。同时优化了SVPWM算法实现，将atan2f替换为符号法扇区判断，显著提升计算效率（约15周期vs atan2f 300周期），并改进了arm_sin_cos_f32替代sqrtf提高性能。 feat(FOC): 统一电气角度计算避免重复运算 重构代码以统一电气角度计算，在各个控制循环中只计算一次电气角度并传递给需要的函数，避免了多次重复的fmodf运算和角度归一化操作，提升了实时控制性能。 refactor(Filter): 改进α-β观测器角度处理精度 修改角度卡尔曼滤波器在归一化角度空间[0,360)内工作，避免大累积角度的浮点精度损失问题，确保静止状态下不会产生虚假速度，同时保持与下游代码的兼容性。 refactor: 为算法模块添加完整文档注释 为所有算法模块(FOC、Filter、pid、thermal)添加了详细的Doxygen文档注释，包括函数说明、参数描述、返回值说明等，便于代码理解和API文档生成。
150 lines
4.3 KiB
C++
150 lines
4.3 KiB
C++
/**
|
|
* @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"
|
|
|
|
|
|
extern sampler sampler;
|
|
extern CAN_transmiter CAN0_transmiter;
|
|
extern FOC_Controller FOC;
|
|
extern FlashEEPROM flashEEP;
|
|
ThermalController thermalController; // 热量控制器初始化
|
|
|
|
/** @brief max 宏定义 */
|
|
#define max(a, b, c) \
|
|
((a) > (b) ? ((a) > (c) ? (a) : (c)) : ((b) > (c) ? (b) : (c)))
|
|
/** @brief PROTECT_TIME 常量定义 */
|
|
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;
|
|
}
|
|
}
|