cbb
a26dd1cc8e
- 实现循环缓冲区队列管理CAN消息接收 - 添加缓冲区满/空状态检查函数 - 实现消息出队列dequeue_rx_message功能 - 在中断处理程序中添加消息入队列机制 - 优化CAN消息处理流程，支持批量处理 refactor(motor): 优化电机控制类结构和安全机制 - 重命名PID控制器参数变量名以提高可读性 - 添加过热关机标志位thermal_shutdown和相关处理函数 - 实现过热关机shut_down_thermal和过热恢复resume_from_thermal功能 - 调整关机逻辑，添加drv_stop调用确保电机完全停止 - 修正电流环控制中的占空比设置，改为单相保持 feat(motor): 实现FOC控制算法改进和零点校准功能 - 实现电机零点校准函数calibrate_zero_position用于位置补偿 - 修正FOC控制器中的电气角度计算，集成KTH7823校准角度补偿 - 新增d轴电流计算功能 - 重构PID参数初始化方式，使用统一的电压限制配置 - 更新SVPWM相电压设置算法，改进角度归一化处理 - 修改缺省扇区输出占空比从0.0f改为0.5f以提高稳定性 fix(control): 修复控制回路方向和频率计算问题 - 修正角度控制回路中的误差计算方向 - 修正开环速度控制中的频率参数使用 - 修正速度环控制中的误差计算方向 - 修正MIT模式下多级PID控制器的参数设置和误差计算 refactor(filter): 修正编码器速度计算算法 - 移除速度计算中的冗余乘法因子2 feat(pid): 扩展PID控制器功能 - 添加reset方法用于重置PID内部状态 chore(build): 更新开发工具版本和ADC配置 - 更新csolution版本至2.13.0 - 调整ADC采样时间配置以优化性能
108 lines
3.2 KiB
C++
108 lines
3.2 KiB
C++
#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
|
|
/**
|
|
** @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;
|
|
}
|
|
|
|
void power_manage(void) {
|
|
float last_vbus = sampler.get_vbus_voltage();
|
|
sampler.update_vbus_voltage();
|
|
FOC.update_power(last_vbus);
|
|
float vbus = sampler.get_vbus_voltage();
|
|
if ((vbus < 22.0f) and (last_vbus > vbus) and vbus > 8.0f) {
|
|
FOC.shut_down();
|
|
flashEEP.save_system_config(true);
|
|
} else {
|
|
// 只有在没有过热关机时才恢复
|
|
if (!FOC.thermal_shutdown) {
|
|
FOC.resume();
|
|
}
|
|
}
|
|
}
|