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文档生成。
843 lines
27 KiB
C++
843 lines
27 KiB
C++
/**
|
||
* @file Algorithm/FOC.cpp
|
||
* @brief 算法模块源文件
|
||
* @note 本文件仅补充 Doxygen 文档注释，不改变代码逻辑。
|
||
*/
|
||
|
||
#include "FOC.h"
|
||
#include "Can.h"
|
||
#include "Filter.h"
|
||
#include "Timer.h"
|
||
#include "adc.h"
|
||
#include "ang_sensor.h"
|
||
#include "config.h"
|
||
#include "motor.h"
|
||
#include "pid.h"
|
||
#include "systick.h"
|
||
#include <arm_math.h>
|
||
#include <cmath>
|
||
#include <cstdio>
|
||
#include <cstring>
|
||
#include <gd32c10x.h>
|
||
|
||
FOC_Controller FOC; // FOC控制初始化
|
||
extern sampler sampler;
|
||
extern angle_sensor KTH7823;
|
||
extern CAN_transmiter CAN0_transmiter;
|
||
extern EncoderVelocityKalman Vel_kalman; // 速度滤波
|
||
extern CurrentKalman Current_kalman;
|
||
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;
|
||
|
||
// angle_el 已由调用方归一化到 [0, 360)，无需重复
|
||
float angle = angle_el;
|
||
|
||
// 线性插值查表
|
||
float index_f = angle / 360.0f * (float)COGGING_TABLE_SIZE;
|
||
u16 idx0 = (u16)index_f;
|
||
u16 idx1 = (idx0 + 1) % COGGING_TABLE_SIZE;
|
||
float frac = index_f - (float)idx0;
|
||
|
||
// 根据转向选择补偿表（带滞回，避免零速附近跳变）
|
||
static float *last_table = cogging_table;
|
||
if (rt_velocity < -0.5f) last_table = cogging_table_rev;
|
||
else if (rt_velocity > 0.5f) last_table = cogging_table;
|
||
float *table = last_table;
|
||
float comp = table[idx0] * (1.0f - frac) + table[idx1] * frac;
|
||
|
||
return -comp * cogging_comp_scale; // mA→V: 补偿电流(mA) × 换算系数(V/mA)
|
||
}
|
||
|
||
// === 齿槽转矩校准（DEBUG，平时不启用）===
|
||
// 启用方法：config.h 中 FOC_DEBUG 改为 1，通过 CAN 命令触发（RESET_ID bit4）
|
||
// 校准完成后把 C Array 硬编码到 init() 中，再改回 FOC_DEBUG 0
|
||
#if FOC_DEBUG
|
||
// 齿槽转矩校准静态数据
|
||
float FOC_Controller::cog_calib_run_data[6][COGGING_TABLE_SIZE] = {};
|
||
float FOC_Controller::cog_calib_accum[COGGING_TABLE_SIZE] = {};
|
||
u32 FOC_Controller::cog_calib_count[COGGING_TABLE_SIZE] = {};
|
||
|
||
/**
|
||
* @brief 齿槽转矩校准 - 非阻塞状态机
|
||
* 正转3次 + 反转3次，分开存储补偿表。
|
||
* 只打印最终平均数据和 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) {
|
||
case CogCalibState::IDLE:
|
||
if (!cogging_calib_request) return;
|
||
cogging_calib_request = false;
|
||
|
||
cog_calib_run = 0;
|
||
cog_calib_saved_mode = control_mode;
|
||
cog_calib_saved_vel = target_velocity;
|
||
cogging_comp_enabled = false;
|
||
memset(cog_calib_run_data, 0, sizeof(cog_calib_run_data));
|
||
|
||
printf("\r\n[COG] === Cogging Calibration: 3 fwd + 3 rev ===\r\n");
|
||
|
||
control_mode = 6;
|
||
target_velocity = SLOW_VEL;
|
||
cog_calib_settle_tick = 0;
|
||
cog_calib_state = CogCalibState::SETTLING;
|
||
return;
|
||
|
||
case CogCalibState::SETTLING:
|
||
cog_calib_settle_tick++;
|
||
if (cog_calib_settle_tick >= SETTLE_TICKS) {
|
||
cog_calib_state = CogCalibState::SAMPLING;
|
||
cog_calib_sample_count = 0;
|
||
memset(cog_calib_accum, 0, sizeof(cog_calib_accum));
|
||
memset(cog_calib_count, 0, sizeof(cog_calib_count));
|
||
}
|
||
return;
|
||
|
||
case CogCalibState::SAMPLING:
|
||
// 采样分频
|
||
static u32 sample_div = 0;
|
||
sample_div++;
|
||
if (sample_div < SAMPLE_DIV) return;
|
||
sample_div = 0;
|
||
|
||
{
|
||
float real_angle = KTH7823.getcurrent_sensorangle();
|
||
float angle_el = normalizeAngle360(real_angle * PP + KTH7823.calibration_angle);
|
||
|
||
float iq = rt_current;
|
||
|
||
if (fabsf(iq) < 5000.0f) {
|
||
u16 idx = (u16)(angle_el / 360.0f * (float)COGGING_TABLE_SIZE);
|
||
if (idx >= COGGING_TABLE_SIZE) idx = COGGING_TABLE_SIZE - 1;
|
||
cog_calib_accum[idx] += iq;
|
||
cog_calib_count[idx]++;
|
||
}
|
||
cog_calib_sample_count++;
|
||
}
|
||
if (cog_calib_sample_count >= TARGET_SAMPLES) {
|
||
target_velocity = 0;
|
||
cog_calib_state = CogCalibState::STOPPING;
|
||
cog_calib_stop_tick = 0;
|
||
}
|
||
return;
|
||
|
||
case CogCalibState::STOPPING:
|
||
cog_calib_stop_tick++;
|
||
if (cog_calib_stop_tick >= STOP_TICKS) {
|
||
// 处理本轮数据：去直流
|
||
float dc = 0;
|
||
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
|
||
cog_calib_run_data[cog_calib_run][i] =
|
||
(cog_calib_count[i] > 0) ? cog_calib_accum[i] / (float)cog_calib_count[i] : 0.0f;
|
||
dc += cog_calib_run_data[cog_calib_run][i];
|
||
}
|
||
dc /= (float)COGGING_TABLE_SIZE;
|
||
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
|
||
cog_calib_run_data[cog_calib_run][i] -= dc;
|
||
}
|
||
|
||
printf("[COG] Run %d/%d done. DC=%.2f mA\r\n",
|
||
cog_calib_run + 1, TOTAL_RUNS, dc);
|
||
|
||
cog_calib_run++;
|
||
if (cog_calib_run < TOTAL_RUNS) {
|
||
// run 0~2 正转，run 3~5 反转
|
||
control_mode = 6;
|
||
target_velocity = (cog_calib_run < 3) ? SLOW_VEL : -SLOW_VEL;
|
||
cog_calib_state = CogCalibState::SETTLING;
|
||
cog_calib_settle_tick = 0;
|
||
} else {
|
||
cog_calib_state = CogCalibState::DONE;
|
||
}
|
||
}
|
||
return;
|
||
|
||
case CogCalibState::DONE:
|
||
// 正转平均 (run 0~2)
|
||
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
|
||
cogging_table[i] = (cog_calib_run_data[0][i] +
|
||
cog_calib_run_data[1][i] +
|
||
cog_calib_run_data[2][i]) / 3.0f;
|
||
}
|
||
// 反转平均 (run 3~5)
|
||
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
|
||
cogging_table_rev[i] = (cog_calib_run_data[3][i] +
|
||
cog_calib_run_data[4][i] +
|
||
cog_calib_run_data[5][i]) / 3.0f;
|
||
}
|
||
cogging_comp_enabled = true;
|
||
control_mode = cog_calib_saved_mode;
|
||
target_velocity = cog_calib_saved_vel;
|
||
|
||
// 打印正转平均
|
||
printf("\r\n[COG] === FWD Average ===\r\n");
|
||
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
|
||
float angle_deg = (float)i / (float)COGGING_TABLE_SIZE * 360.0f;
|
||
printf("[COG] %.2f, %.2f\r\n", angle_deg, cogging_table[i]);
|
||
fwdgt_counter_reload();
|
||
}
|
||
|
||
// 打印反转平均
|
||
printf("\r\n[COG] === REV Average ===\r\n");
|
||
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
|
||
float angle_deg = (float)i / (float)COGGING_TABLE_SIZE * 360.0f;
|
||
printf("[COG] %.2f, %.2f\r\n", angle_deg, cogging_table_rev[i]);
|
||
fwdgt_counter_reload();
|
||
}
|
||
|
||
// 打印正转 C Array
|
||
printf("\r\n[COG] --- FWD C Array ---\r\n");
|
||
printf("static const float COGGING_TABLE_FWD[COGGING_TABLE_SIZE] = {\r\n");
|
||
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
|
||
if (i % 8 == 0) printf(" ");
|
||
printf("%.2ff", cogging_table[i]);
|
||
if (i < COGGING_TABLE_SIZE - 1) printf(", ");
|
||
if (i % 8 == 7 || i == COGGING_TABLE_SIZE - 1) printf("\r\n");
|
||
fwdgt_counter_reload();
|
||
}
|
||
printf("};\r\n");
|
||
|
||
// 打印反转 C Array
|
||
printf("\r\n[COG] --- REV C Array ---\r\n");
|
||
printf("static const float COGGING_TABLE_REV[COGGING_TABLE_SIZE] = {\r\n");
|
||
for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
|
||
if (i % 8 == 0) printf(" ");
|
||
printf("%.2ff", cogging_table_rev[i]);
|
||
if (i < COGGING_TABLE_SIZE - 1) printf(", ");
|
||
if (i % 8 == 7 || i == COGGING_TABLE_SIZE - 1) printf("\r\n");
|
||
fwdgt_counter_reload();
|
||
}
|
||
printf("};\r\n");
|
||
|
||
printf("\r\n[COG] === Done. ===\r\n");
|
||
cog_calib_state = CogCalibState::IDLE;
|
||
return;
|
||
}
|
||
}
|
||
#endif // FOC_DEBUG
|
||
|
||
// 初始变量及函数定义
|
||
#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);
|
||
// KTH7823.update();
|
||
|
||
// // 改进校准算法：获取当前传感器角度并计算补偿值
|
||
// float current_sensor_angle = KTH7823.getcurrent_sensorangle();
|
||
// float electrical_angle = fmodf(current_sensor_angle * PP, 360.0f); //
|
||
// 计算当前电角度
|
||
|
||
// // 计算需要的补偿值，使得当前位置对应0°电角度
|
||
// KTH7823.calibration_angle = -electrical_angle;
|
||
|
||
// // 对补偿角进行归一化处理，确保在[-180, 180]范围内
|
||
// KTH7823.calibration_angle = fmodf(KTH7823.calibration_angle + 180.0f,
|
||
// 360.0f); if (KTH7823.calibration_angle < 0) { KTH7823.calibration_angle
|
||
// += 360.0f;
|
||
// }
|
||
// KTH7823.calibration_angle -= 180.0f;
|
||
|
||
// Set_3Pwm(0, 0, 0); // 停止
|
||
}
|
||
|
||
// 通过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 angle_el) {
|
||
if (current_a == ERROR_ADC_VAL || current_b == ERROR_ADC_VAL ||
|
||
current_c == ERROR_ADC_VAL) {
|
||
return Current_kalman.getCurrent();
|
||
}
|
||
|
||
// 去除三相公共模偏置，使 Ia + Ib + Ic 更接近 0
|
||
const float current_avg = (current_a + current_b + current_c) / 3.0f;
|
||
current_a -= current_avg;
|
||
current_b -= current_avg;
|
||
current_c -= current_avg;
|
||
|
||
// 电气角度由调用方传入，避免重复计算
|
||
// Clarke变换 (使用三相电流)
|
||
float I_alpha, I_beta;
|
||
I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b -
|
||
(1.0f / 3.0f) * current_c;
|
||
I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c;
|
||
|
||
// Park变换
|
||
float sin_el, cos_el;
|
||
arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
|
||
float I_q, I_d;
|
||
I_q = I_beta * cos_el - I_alpha * sin_el;
|
||
I_d = I_beta * sin_el + I_alpha * cos_el;
|
||
|
||
// 返回Iq值，乘以1000以转换为mA
|
||
return Current_kalman.update(I_q * 1000 - KE_FACTOR * rt_velocity);
|
||
// return I_q * 1000 - KE_FACTOR * rt_velocity;
|
||
}
|
||
|
||
/**
|
||
* @brief 电机控制类初始化函数
|
||
*
|
||
* @return u8 0表示初始化成功，1表示初始化失败
|
||
*/
|
||
u8 FOC_Controller::init() {
|
||
motor_init(); // 电机驱动初始化
|
||
|
||
// 齿槽转矩补偿表（硬编码，3正+3反平均，用真实角度算Iq）
|
||
static const float COGGING_TABLE_FWD[COGGING_TABLE_SIZE] = {
|
||
29.28f, 28.84f, 28.26f, 28.83f, 29.79f, 30.26f, 29.52f, 28.77f,
|
||
28.23f, 27.68f, 26.65f, 25.19f, 23.92f, 22.59f, 19.61f, 16.77f,
|
||
13.32f, 10.40f, 7.64f, 3.51f, -0.83f, -4.96f, -8.19f, -11.11f,
|
||
-13.40f, -14.39f, -15.69f, -15.99f, -16.39f, -15.50f, -14.76f, -14.21f,
|
||
-13.76f, -13.88f, -13.22f, -11.18f, -10.65f, -10.54f, -10.62f, -10.23f,
|
||
-9.52f, -8.19f, -6.31f, -4.14f, -1.94f, -0.25f, 2.06f, 4.08f,
|
||
6.05f, 8.30f, 9.74f, 11.38f, 13.32f, 15.65f, 17.55f, 20.22f,
|
||
21.72f, 22.28f, 23.30f, 24.78f, 23.53f, 23.35f, 23.00f, 21.42f,
|
||
20.35f, 20.57f, 20.07f, 18.18f, 17.04f, 16.03f, 14.81f, 13.71f,
|
||
12.47f, 10.93f, 9.38f, 6.53f, 4.63f, 3.37f, 1.29f, -1.28f,
|
||
-5.08f, -11.11f, -16.35f, -21.00f, -24.61f, -28.30f, -31.11f, -33.48f,
|
||
-35.25f, -36.54f, -38.01f, -38.69f, -39.27f, -39.52f, -38.18f, -36.55f,
|
||
-34.96f, -33.14f, -31.92f, -31.45f, -31.32f, -30.25f, -29.60f, -28.72f,
|
||
-27.51f, -25.30f, -22.66f, -20.87f, -18.19f, -16.10f, -13.25f, -10.81f,
|
||
-8.00f, -4.67f, -2.28f, 0.87f, 3.83f, 7.25f, 10.56f, 13.70f,
|
||
17.70f, 19.90f, 22.14f, 23.62f, 24.53f, 26.03f, 26.96f, 27.95f,
|
||
};
|
||
static const float COGGING_TABLE_REV[COGGING_TABLE_SIZE] = {
|
||
22.45f, 24.95f, 26.05f, 26.01f, 26.11f, 25.87f, 26.20f, 26.23f,
|
||
26.19f, 25.87f, 25.97f, 26.09f, 24.77f, 24.22f, 23.71f, 22.10f,
|
||
19.84f, 17.60f, 15.36f, 12.99f, 9.54f, 5.35f, 1.71f, -1.94f,
|
||
-4.15f, -6.16f, -7.93f, -9.10f, -10.01f, -10.25f, -10.85f, -11.74f,
|
||
-11.98f, -10.78f, -9.65f, -8.89f, -8.14f, -6.85f, -5.94f, -5.37f,
|
||
-4.28f, -3.38f, -3.36f, -3.40f, -2.81f, -2.36f, -1.04f, -0.18f,
|
||
1.08f, 2.15f, 4.19f, 5.51f, 7.33f, 8.64f, 10.46f, 11.89f,
|
||
13.42f, 14.83f, 17.11f, 19.12f, 20.76f, 21.88f, 22.35f, 22.86f,
|
||
22.73f, 22.13f, 21.10f, 20.53f, 18.65f, 17.09f, 15.47f, 14.76f,
|
||
14.02f, 12.94f, 11.98f, 10.29f, 8.11f, 5.83f, 4.23f, 2.42f,
|
||
1.03f, -1.80f, -4.01f, -7.93f, -11.79f, -15.90f, -20.22f, -23.04f,
|
||
-25.66f, -28.80f, -30.45f, -32.11f, -33.17f, -33.97f, -34.68f, -34.26f,
|
||
-34.18f, -33.78f, -32.47f, -31.11f, -29.40f, -27.89f, -27.05f, -26.72f,
|
||
-25.59f, -24.86f, -23.61f, -22.97f, -22.89f, -21.81f, -20.78f, -19.23f,
|
||
-17.74f, -15.09f, -13.18f, -10.51f, -8.18f, -5.89f, -3.35f, -0.02f,
|
||
2.02f, 3.87f, 6.37f, 9.11f, 12.33f, 14.63f, 17.12f, 19.12f,
|
||
};
|
||
memcpy(cogging_table, COGGING_TABLE_FWD, sizeof(COGGING_TABLE_FWD));
|
||
memcpy(cogging_table_rev, COGGING_TABLE_REV, sizeof(COGGING_TABLE_REV));
|
||
cogging_comp_enabled = true;
|
||
|
||
FOC.cur_loop_p = DEFAULT_current_loop_p * 100 / limit_voltage;
|
||
FOC.cur_MIT_p = DEFAULT_current_MIT_p * 100 / limit_voltage;
|
||
FOC.vel_loop_p = DEFAULT_vel_loop_p * 100 / limit_voltage;
|
||
FOC.vel_angloop_p = DEFAULT_vel_angloop_p * 100;
|
||
FOC.vel_MIT_p = DEFAULT_vel_MIT_p * 100;
|
||
FOC.ang_loop_p = DEFAULT_angle_loop_p * 100 / limit_voltage;
|
||
FOC.ang_angloop_p = DEFAULT_angle_angloop_p * 100 / limit_voltage;
|
||
FOC.ang_MIT_p = DEFAULT_angle_MIT_p * 100 / limit_voltage;
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief 电机控制类构造函数
|
||
*
|
||
*/
|
||
FOC_Controller::FOC_Controller() { ; }
|
||
|
||
/**
|
||
* @brief 使用SVPWM算法设置电机输出
|
||
*
|
||
* @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) {
|
||
// ① 逆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;
|
||
|
||
// ② 扇区判定（替代 atan2f）
|
||
float X = Ubeta;
|
||
float Y = SQRT3DV2 * Ualpha - 0.5f * Ubeta;
|
||
float Z = -SQRT3DV2 * Ualpha - 0.5f * Ubeta;
|
||
|
||
u8 sector_code = (X > 0.0f ? 1u : 0u)
|
||
+ (Y > 0.0f ? 2u : 0u)
|
||
+ (Z > 0.0f ? 4u : 0u);
|
||
|
||
// 编码 → 扇区 查找表（由 atan2f 扇区定义反推，全角度数值验证通过）
|
||
static const u8 sector_lut[8] = {0, 2, 6, 1, 4, 3, 5, 0};
|
||
u8 sector = sector_lut[sector_code];
|
||
|
||
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;
|
||
|
||
// ④ 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;
|
||
duty_b = 0.5f * T0 + T2;
|
||
duty_c = 0.5f * T0;
|
||
break;
|
||
case 2:
|
||
duty_a = 0.5f * T0 + T1;
|
||
duty_b = 0.5f * T0 + T1 + T2;
|
||
duty_c = 0.5f * T0;
|
||
break;
|
||
case 3:
|
||
duty_a = 0.5f * T0;
|
||
duty_b = 0.5f * T0 + T1 + T2;
|
||
duty_c = 0.5f * T0 + T2;
|
||
break;
|
||
case 4:
|
||
duty_a = 0.5f * T0;
|
||
duty_b = 0.5f * T0 + T1;
|
||
duty_c = 0.5f * T0 + T1 + T2;
|
||
break;
|
||
case 5:
|
||
duty_a = 0.5f * T0 + T2;
|
||
duty_b = 0.5f * T0;
|
||
duty_c = 0.5f * T0 + T1 + T2;
|
||
break;
|
||
case 6:
|
||
duty_a = 0.5f * T0 + T1 + T2;
|
||
duty_b = 0.5f * T0;
|
||
duty_c = 0.5f * T0 + T1;
|
||
break;
|
||
default:
|
||
duty_a = 0.5f;
|
||
duty_b = 0.5f;
|
||
duty_c = 0.5f;
|
||
break;
|
||
}
|
||
Set_3Pwm(duty_a, duty_b, duty_c);
|
||
}
|
||
|
||
/**
|
||
* @brief 电机关机（通用关机，不带原因标记）
|
||
*
|
||
*/
|
||
void FOC_Controller::shut_down() {
|
||
drv_stop();
|
||
motor_sleep();
|
||
is_shut_down = true;
|
||
}
|
||
|
||
/**
|
||
* @brief 过热关机
|
||
*
|
||
*/
|
||
void FOC_Controller::shut_down_thermal() {
|
||
drv_stop();
|
||
motor_sleep();
|
||
is_shut_down = true;
|
||
thermal_shutdown = true;
|
||
}
|
||
|
||
/**
|
||
* @brief 过热恢复
|
||
*
|
||
*/
|
||
void FOC_Controller::resume_from_thermal() {
|
||
thermal_shutdown = false;
|
||
if (!voltage_error && !over_current) {
|
||
is_shut_down = false;
|
||
motor_resume();
|
||
}
|
||
}
|
||
/**
|
||
* @brief 占空比置0
|
||
*
|
||
*/
|
||
void FOC_Controller::drv_stop() { Set_3Pwm(0.0f, 0.0f, 0.0f); }
|
||
/**
|
||
* @brief 电机锁止
|
||
*/
|
||
float FOC_Controller::drv_hold() {
|
||
float hold_pwm_duty = HOLD_PWM_DUTY;
|
||
if (HOLD_PWM_DUTY > 0.95f) {
|
||
hold_pwm_duty = 0.95f;
|
||
} else if (HOLD_PWM_DUTY < 0.0f) {
|
||
hold_pwm_duty = 0.0f;
|
||
}
|
||
Set_3Pwm(hold_pwm_duty, 0, 0);
|
||
return hold_pwm_duty;
|
||
}
|
||
|
||
|
||
/**
|
||
* @brief PWM模式（调试）
|
||
*/
|
||
float FOC_Controller::PWM_out() {
|
||
float hold_pwm_duty = target_velocity/100;
|
||
Set_3Pwm(hold_pwm_duty, hold_pwm_duty, hold_pwm_duty);
|
||
return hold_pwm_duty;
|
||
}
|
||
|
||
|
||
/**
|
||
* @brief 电机恢复
|
||
*
|
||
*/
|
||
void FOC_Controller::resume() {
|
||
over_current = false;
|
||
voltage_error = false;
|
||
is_shut_down = false;
|
||
motor_resume();
|
||
}
|
||
/**
|
||
* @brief 更新电源电压
|
||
*
|
||
*/
|
||
void FOC_Controller::update_power(float voltage) {
|
||
if (voltage < 36.0f and voltage > 8.0f) {
|
||
voltage_error = false;
|
||
voltage_power_supply = voltage;
|
||
v_half = voltage * 0.5f;
|
||
v_safe = voltage * 1.1f;
|
||
} else {
|
||
voltage_error = true;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 快速电压检测（在电机控制循环中每100us调用一次）
|
||
*
|
||
* 直接从DMA缓冲区读取最新ADC值，检测电压跌幅
|
||
* 响应时间 < 200us
|
||
*
|
||
* 算法：
|
||
* 1. 使用峰值检测算法跟踪当前额定电压（vbus_reference）
|
||
* - 电压上升时，基准电压跟随
|
||
* - 电压下跌时，基准电压保持（不跟随下跌）
|
||
* 2. 计算电压跌幅 = 基准电压 - 当前电压
|
||
* 3. 关机条件（满足任一即可）：
|
||
* - 电压跌幅超过 VBUS_DROP_THRESHOLD
|
||
* - 电压低于绝对下限 VBUS_ABSOLUTE_MIN
|
||
*
|
||
* 参数定义在 config.h 中:
|
||
* - VBUS_DROP_THRESHOLD: 电压跌幅阈值 (默认5V)
|
||
* - 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;
|
||
|
||
// 更新基准电压（峰值检测）
|
||
// 电压上升时，基准电压跟随；电压下跌时，基准电压保持
|
||
if (current_vbus > vbus_reference) {
|
||
vbus_reference = current_vbus;
|
||
}
|
||
// 电压下跌时不更新基准电压，保持之前的峰值
|
||
|
||
// 更新上次电压值
|
||
last_vbus_fast = current_vbus;
|
||
|
||
// 计算电压跌幅（相对于基准电压）
|
||
float voltage_drop = vbus_reference - current_vbus;
|
||
|
||
// 关机条件（满足任一即可）：
|
||
// 1. 电压跌幅超过阈值
|
||
// 2. 电压低于绝对下限
|
||
bool drop_exceeded = (voltage_drop > VBUS_DROP_THRESHOLD);
|
||
bool below_absolute = (current_vbus < VBUS_ABSOLUTE_MIN);
|
||
|
||
if ((drop_exceeded || below_absolute) && (!is_shut_down)) {
|
||
// 电压跌落计数（防抖）
|
||
vbus_drop_count++;
|
||
|
||
if (vbus_drop_count >= VBUS_DEBOUNCE_COUNT) {
|
||
// 确认需要关机，立即停机
|
||
drv_stop();
|
||
motor_sleep();
|
||
is_shut_down = true;
|
||
emergency_shutdown = true; // 标记紧急关机，低频任务会执行保存
|
||
vbus_drop_count = 0; // 重置计数器
|
||
}
|
||
} else {
|
||
// 电压正常，重置计数器
|
||
vbus_drop_count = 0;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 速度开环控制
|
||
*
|
||
* @param target_velocity: 目标速度
|
||
* @return float: q轴电压
|
||
*/
|
||
float FOC_Controller::velocity_open_loop() {
|
||
// 通过时间间隔和目标速度计算需要转动的机械角度，存储在 shaft_angle 中
|
||
shaft_angle =
|
||
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'), angle_el_cog);
|
||
|
||
// 因为在开环控制中，Ud 始终为 0，只控制 Uq
|
||
const float Uq = _constrain(target_velocity * REDUCTION_RATIO * 0.005f,
|
||
2.0f, voltage_power_supply * 0.5f);
|
||
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
|
||
return limit_voltage;
|
||
}
|
||
|
||
/**
|
||
* @brief 角度闭环控制
|
||
*
|
||
* @param target_angle: 目标角度
|
||
* @return float: 当前角度
|
||
*/
|
||
float FOC_Controller::angle_control() {
|
||
// PID 控制计算
|
||
float Uq = angle_loop(target_angle - rt_angle);
|
||
|
||
// 获取电机轴角度
|
||
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'), angle_el_cog);
|
||
// 设置相电压
|
||
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
|
||
return shaft_angle;
|
||
}
|
||
|
||
/**
|
||
* @brief 速度闭环控制
|
||
*
|
||
* @param target_velocity: 目标速度
|
||
* @return float: 当前速度
|
||
*/
|
||
float FOC_Controller::velocity_control() {
|
||
// 速度误差计算
|
||
float velocity_error = (target_velocity - rt_velocity) * DIR * REDUCTION_RATIO;
|
||
|
||
// 目标速度突变检测：target 大幅变化时清积分
|
||
static float last_target_vel = 0.0f;
|
||
const float target_delta = target_velocity - last_target_vel;
|
||
if (fabsf(target_delta) > 5.0f) {
|
||
vel_loop.reset_integral();
|
||
}
|
||
last_target_vel = target_velocity;
|
||
|
||
// PID 控制计算
|
||
float Uq = vel_loop(velocity_error);
|
||
CAN0_transmiter.testfloat_union.f = Uq;
|
||
|
||
// 获取电机轴角度
|
||
shaft_angle = KTH7823.getcurrent_sensorangle();
|
||
// 电气角度（算一次，供 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, angle_el_cog);
|
||
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
|
||
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'), 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;
|
||
}
|
||
// 设置相电压
|
||
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
|
||
return 1;
|
||
}
|
||
|
||
/**
|
||
* @brief 带速度限制的角度闭环控制
|
||
*
|
||
* @param target_angle: 目标角度
|
||
* @param max_velocity: 最大速度限制 (单位: rad/s)
|
||
* @return float: 当前角度
|
||
*/
|
||
float FOC_Controller::angle_control_with_velocity_limit() {
|
||
// 计算角度误差
|
||
float angle_error = target_angle - rt_angle;
|
||
|
||
ang_inVelAngloop.set_P(ang_angloop_p);
|
||
// 使用角度PID控制器计算目标速度
|
||
float target_velocity = ang_inVelAngloop(angle_error);
|
||
|
||
// 使用速度PID控制器计算最终输出电压
|
||
// 根据目标速度大小，选择不同的PID参数
|
||
if (target_velocity < -120.0f) {
|
||
vel_inVelAngloop.set_P(vel_angloop_p * 2.0f);
|
||
} else if (target_velocity < -30.0f) {
|
||
vel_inVelAngloop.set_P(vel_angloop_p * 1.5f);
|
||
} else {
|
||
vel_inVelAngloop.set_P(vel_angloop_p);
|
||
}
|
||
float velocity_error = target_velocity - rt_velocity;
|
||
|
||
// 目标速度突变检测：target 大幅变化时清积分
|
||
static float last_vang_target_vel = 0.0f;
|
||
if (fabsf(target_velocity - last_vang_target_vel) > 5.0f) {
|
||
vel_inVelAngloop.reset_integral();
|
||
}
|
||
last_vang_target_vel = target_velocity;
|
||
|
||
static float last_velocity_error = 0.0f;
|
||
last_velocity_error = velocity_error;
|
||
|
||
float Uq = vel_inVelAngloop(velocity_error)* DIR;
|
||
|
||
// 获取电机轴角度
|
||
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'), angle_el_cog);
|
||
// 设置相电压
|
||
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
|
||
|
||
return shaft_angle;
|
||
}
|
||
|
||
/**
|
||
* @brief MIT控制
|
||
*
|
||
* @param target_angle: 目标角度
|
||
* @param max_velocity: 最大速度限制 (单位: rad/s)
|
||
* @return float: 当前角度
|
||
*/
|
||
float FOC_Controller::MIT_Mode() {
|
||
// 第一层：角度环（外环）
|
||
float angle_error = target_angle - rt_angle;
|
||
ang_inMITloop.set_P(ang_MIT_p);
|
||
float target_velocity =
|
||
ang_inMITloop(angle_error);
|
||
|
||
// 第二层：速度环（中环）
|
||
float velocity_error = (target_velocity - rt_velocity)* DIR;
|
||
|
||
// 目标速度变化检测：target 大幅变化时清积分
|
||
static float last_mit_target_vel = 0.0f;
|
||
if (fabsf(target_velocity - last_mit_target_vel) > 5.0f) {
|
||
vel_inMITloop.reset_integral();
|
||
}
|
||
last_mit_target_vel = target_velocity;
|
||
|
||
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'), angle_el_cog);
|
||
|
||
float current_error = target_current - rt_current;
|
||
current_inMITloop.set_P(cur_MIT_p);
|
||
float Uq = current_inMITloop(current_error);
|
||
|
||
// 设置相电压
|
||
setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
|
||
|
||
return shaft_angle;
|
||
}
|