cbb/Motor_v2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

refactor(FOC): 添加Doxygen文档注释并优化SVPWM算法性能

Browse Source 
添加了完整的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文档生成。
This commit is contained in:
cbb
2026-05-23 22:46:32 +08:00
parent bec7c11b87
commit 4c4e47e9e3
62 changed files with 10367 additions and 126 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/FOC.cpp
+122 -80
View File
@@ -1,3 +1,9 @@
/**
* @file Algorithm/FOC.cpp
* @brief 算法模块源文件
* @note 本文件仅补充 Doxygen 文档注释，不改变代码逻辑。
*/
#include "FOC.h"
#include "Can.h"
#include "Filter.h"
@@ -25,12 +31,16 @@ extern u16 adc_value[3]; // regular DMA缓冲区：MCU温度 / DRV温度 / VBUS
// 齿槽转矩补偿：查表 + 线性插值
// angle_el: 电角度（度）
// 返回: 补偿电压值 (V)，叠加到 Uq 上
/**
* @brief 获取 get_cogging_compensation 对应的数据或状态
* @param[in] angle_el 参数
* @return float 返回值
*/
inline float FOC_Controller::get_cogging_compensation(float angle_el) {
if (!cogging_comp_enabled) return 0.0f;
// 归一化到 [0, 360)
float angle = fmodf(angle_el, 360.0f);
if (angle < 0.0f) angle += 360.0f;
// angle_el 已由调用方归一化到 [0, 360)，无需重复
float angle = angle_el;
// 线性插值查表
float index_f = angle / 360.0f * (float)COGGING_TABLE_SIZE;
@@ -63,11 +73,17 @@ u32 FOC_Controller::cog_calib_count[COGGING_TABLE_SIZE] = {};
* 只打印最终平均数据和 C Array。
*/
void FOC_Controller::calibrate_cogging_tick(void) {
/** @brief SETTLE_TICKS 常量定义 */
constexpr u32 SETTLE_TICKS = 16000; // 800ms / 50us
/** @brief STOP_TICKS 常量定义 */
constexpr u32 STOP_TICKS = 10000; // 500ms / 50us
/** @brief TARGET_SAMPLES 常量定义 */
constexpr u32 TARGET_SAMPLES = 3 * COGGING_TABLE_SIZE * 8;
/** @brief SLOW_VEL 常量定义 */
constexpr float SLOW_VEL = 1.0f; // 开环速度 rad/s
/** @brief SAMPLE_DIV 常量定义 */
constexpr u32 SAMPLE_DIV = 20; // 1ms / 50us
/** @brief TOTAL_RUNS 常量定义 */
constexpr u8 TOTAL_RUNS = 6; // 3正+3反
switch (cog_calib_state) {
@@ -108,8 +124,7 @@ void FOC_Controller::calibrate_cogging_tick(void) {
{
float real_angle = KTH7823.getcurrent_sensorangle();
float angle_el = fmodf(real_angle * PP + KTH7823.calibration_angle, 360.0f);
if (angle_el < 0.0f) angle_el += 360.0f;
float angle_el = normalizeAngle360(real_angle * PP + KTH7823.calibration_angle);
float iq = rt_current;
@@ -226,10 +241,15 @@ void FOC_Controller::calibrate_cogging_tick(void) {
// 初始变量及函数定义
#define _constrain(amt, low, high) \
((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
/** @brief constrain 宏定义 */
#define constrain(value, min, max, target_min, target_max) \
((value - min) / (max - min) * (target_max - target_min))
// 校准步骤（代码模板）
/**
* @brief calibrate_zero_position 函数
* @return void
*/
void calibrate_zero_position(void) {
// Set_3Pwm(.25, 0, 0); // 起劲
// delay_1ms(1000);
@@ -254,8 +274,16 @@ void calibrate_zero_position(void) {
}
// 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
/**
* @brief cal_Iq_Id 函数
* @param[in] current_a 参数
* @param[in] current_b 参数
* @param[in] current_c 参数
* @param[in] angle_el 参数
* @return float 返回值
*/
inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
float current_c) {
float current_c, float angle_el) {
if (current_a == ERROR_ADC_VAL || current_b == ERROR_ADC_VAL ||
current_c == ERROR_ADC_VAL) {
return Current_kalman.getCurrent();
@@ -267,9 +295,7 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
current_b -= current_avg;
current_c -= current_avg;
// 计算电气角度
float angle_el = fmod(shaft_angle * PP + KTH7823.calibration_angle, 360.0f);
// 电气角度由调用方传入，避免重复计算
// Clarke变换 (使用三相电流)
float I_alpha, I_beta;
I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b -
@@ -359,60 +385,70 @@ FOC_Controller::FOC_Controller() { ; }
*
* @param Uq: q轴电压
* @param Ud: d轴电压
*
* 优化：atan2f → X/Y/Z 符号法判扇区（~15周期 vs atan2f ~300周期）
* 边界上扇区编号可能与 atan2f 差1，但 duty 输出完全一致（已数值验证）
*/
inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
// 计算电气角度
float angle_el = shaft_angle * PP + KTH7823.calibration_angle;
// 对电角度进行归一化，确保在[0, 360)范围内
angle_el = fmodf(angle_el, 360.0f);
// 帕克逆变换
inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud, float angle_el) {
// ① 逆Park变换（angle_el 由调用方传入）
float sin_el, cos_el;
arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
float Ualpha = Ud * cos_el - Uq * sin_el;
float Ubeta = Ud * sin_el + Uq * cos_el;
float Ubeta = Ud * sin_el + Uq * cos_el;
// 计算角度并归一化到 [0, 2π)
float angle = atan2f(Ubeta, Ualpha);
if (angle < 0.0f)
angle += 2.0f * PI;
// ② 扇区判定（替代 atan2f）
float X = Ubeta;
float Y = SQRT3DV2 * Ualpha - 0.5f * Ubeta;
float Z = -SQRT3DV2 * Ualpha - 0.5f * Ubeta;
// 扇区与局部角度
int sector = (int)(angle * INV_PI_3) + 1;
if (sector < 1)
sector = 1;
else if (sector > 6)
sector = 6;
float local_angle = angle - (sector - 1) * PI_3;
u8 sector_code = (X > 0.0f ? 1u : 0u)
+ (Y > 0.0f ? 2u : 0u)
+ (Z > 0.0f ? 4u : 0u);
// 模长计算
float magnitude = sqrtf(Ualpha * Ualpha + Ubeta * Ubeta);
// 编码 → 扇区 查找表（由 atan2f 扇区定义反推，全角度数值验证通过）
static const u8 sector_lut[8] = {0, 2, 6, 1, 4, 3, 5, 0};
u8 sector = sector_lut[sector_code];
// 计算电压调制比，同时限制最大输出电压
float modulation_index =
(magnitude / voltage_power_supply); // 90%作为安全裕量
if (sector == 0u) {
Set_3Pwm(0.5f, 0.5f, 0.5f);
return;
}
// ③ 模长与调制比（arm_sqrt_f32 替代 sqrtf，~14周期 vs ~100周期）
float magnitude;
arm_sqrt_f32(Ualpha * Ualpha + Ubeta * Ubeta, &magnitude);
float inv_Udc = 1.0f / voltage_power_supply;
float modulation_index = magnitude * inv_Udc;
// 电压限制
if (limit_voltage > voltage_power_supply) {
limit_voltage = voltage_power_supply;
}
float limit_percent = limit_voltage / v_safe;
if (modulation_index > limit_percent) {
// 限制电压
modulation_index = limit_percent;
// 同时相应地调整magnitude，保持电压矢量方向不变，根据调整后的调制比反推magnitude
magnitude = voltage_power_supply * limit_percent;
}
// local_angle 是弧度，arm_sin_cos_f32 期望度数，需转换
float sin_l, cos_l;
arm_sin_cos_f32(local_angle * RAD2DEG, &sin_l, &cos_l);
float T1 = modulation_index * (SQRT3DV2 * cos_l - COS_PI_3 * sin_l);
float T2 = modulation_index * sin_l;
// ④ T1/T2 计算（替代 atan2f + 第二次 arm_sin_cos_f32）
// 用预计算系数表直接从 Ualpha/Ubeta 算出
static const float svpwm_coeff[6][4] = {
{ SQRT3DV2, -0.5f, 0.0f, 1.0f}, // sector 1
{ SQRT3DV2, 0.5f, -SQRT3DV2, 0.5f}, // sector 2
{ 0.0f, 1.0f, -SQRT3DV2, -0.5f}, // sector 3
{ -SQRT3DV2, 0.5f, 0.0f, -1.0f}, // sector 4
{ -SQRT3DV2, -0.5f, SQRT3DV2, -0.5f}, // sector 5
{ 0.0f, -1.0f, SQRT3DV2, 0.5f}, // sector 6
};
float T1 = (svpwm_coeff[sector-1][0] * Ualpha + svpwm_coeff[sector-1][1] * Ubeta) * inv_Udc;
float T2 = (svpwm_coeff[sector-1][2] * Ualpha + svpwm_coeff[sector-1][3] * Ubeta) * inv_Udc;
// 过调制限制：等比缩放 T1/T2
if (modulation_index > limit_percent) {
float scale = limit_percent / modulation_index;
T1 *= scale;
T2 *= scale;
}
float T0 = 1.0f - T1 - T2;
// 计算占空比（按规范化时间映射）
// ⑤ 占空比计算（不变）
switch (sector) {
case 1:
duty_a = 0.5f * T0 + T1 + T2;
@@ -450,7 +486,6 @@ inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
duty_c = 0.5f;
break;
}
// 设置PWM输出
Set_3Pwm(duty_a, duty_b, duty_c);
}
@@ -561,6 +596,10 @@ void FOC_Controller::update_power(float voltage) {
* - VBUS_ABSOLUTE_MIN: 绝对最低电压 (默认8V)
* - VBUS_DEBOUNCE_COUNT: 防抖计数 (默认5次)
*/
/**
* @brief fast_voltage_check 函数
* @return void
*/
void FOC_Controller::fast_voltage_check(void) {
// 直接从DMA缓冲区读取最新ADC值（regular rank2 是 VBUS）
float current_vbus = adc_value[2] * (3.3f / 4096.0f) * 11.0f;
@@ -611,17 +650,16 @@ void FOC_Controller::fast_voltage_check(void) {
float FOC_Controller::velocity_open_loop() {
// 通过时间间隔和目标速度计算需要转动的机械角度，存储在 shaft_angle 中
shaft_angle =
fmod(shaft_angle + target_velocity * REDUCTION_RATIO / MOTOR_FREQUENCY,
360.0f);
normalizeAngle360(shaft_angle + target_velocity * REDUCTION_RATIO / MOTOR_FREQUENCY);
// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
sampler.get_current('W'), angle_el_cog);
// 因为在开环控制中，Ud 始终为 0，只控制 Uq
const float Uq = _constrain(target_velocity * REDUCTION_RATIO * 0.005f,
2.0f, voltage_power_supply * 0.5f);
// 齿槽转矩补偿
float angle_el_cog = fmodf(shaft_angle * PP + KTH7823.calibration_angle, 360.0f);
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
2.0f, voltage_power_supply * 0.5f);
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
return limit_voltage;
}
@@ -637,12 +675,12 @@ float FOC_Controller::angle_control() {
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
// 齿槽转矩补偿
float angle_el_cog = fmodf(shaft_angle * PP + KTH7823.calibration_angle, 360.0f);
sampler.get_current('W'), angle_el_cog);
// 设置相电压
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
return shaft_angle;
}
@@ -670,34 +708,38 @@ float FOC_Controller::velocity_control() {
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
// 齿槽转矩补偿
float angle_el_cog = fmodf(shaft_angle * PP + KTH7823.calibration_angle, 360.0f);
// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
// 设置相电压
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
sampler.get_current('W'), angle_el_cog);
return rt_velocity;
}
/**
* @brief current_control 函数
* @return float 返回值
*/
float FOC_Controller::current_control() {
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
CAN0_transmiter.testfloat_union.f = shaft_angle;
// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
// PID 控制计算
float Iq = rt_current =
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
sampler.get_current('W'), angle_el_cog);
float Uq = current_loop(target_current - Iq);
if (Uq > limit_voltage) {
Uq = limit_voltage;
} else if (Uq < -limit_voltage) {
Uq = -limit_voltage;
}
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
CAN0_transmiter.testfloat_union.f = shaft_angle;
// 齿槽转矩补偿
float angle_el_cog = fmodf(shaft_angle * PP + KTH7823.calibration_angle, 360.0f);
// 设置相电压
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
return 1;
}
@@ -742,13 +784,13 @@ float FOC_Controller::angle_control_with_velocity_limit() {
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
CAN0_transmiter.testfloat_union.f = shaft_angle;
// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
// 获取当前实际电流值
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
// 齿槽转矩补偿
float angle_el_cog = fmodf(shaft_angle * PP + KTH7823.calibration_angle, 360.0f);
sampler.get_current('W'), angle_el_cog);
// 设置相电压
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
return shaft_angle;
}
@@ -780,21 +822,21 @@ float FOC_Controller::MIT_Mode() {
vel_inMITloop.set_P(vel_MIT_p);
float target_current = vel_inMITloop(velocity_error);
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
// 第三层：电流环（内环）
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
sampler.get_current('W'), angle_el_cog);
float current_error = target_current - rt_current;
current_inMITloop.set_P(cur_MIT_p);
float Uq = current_inMITloop(current_error);
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
// 齿槽转矩补偿
float angle_el_cog = fmodf(shaft_angle * PP + KTH7823.calibration_angle, 360.0f);
// 设置相电压
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
return shaft_angle;
}