cbb
9fbd9c3d30
对硬编码的两张 128 点补偿表做谐波分解后确认该补偿无效： - 能量集中在 k=1(11次/转) 16.1% 与 k=2(22次/转) 82.2%，合计 98.3%； 而 22 极电机的真实齿槽基频应落在槽极 LCM 处（12槽=132次/转 k=12、 18槽 k=18、24槽 k=24），实测幅值仅 0.46/0.12/0.08 mA，在噪声地板内。 即表中属于齿槽的成分不足总能量 0.1%，k=1/k=2 实为电角度零位偏差与 电流采样通道增益/零点失配的特征。 - 表按电角度查表，而电角度含每台独立的 calibration_angle（Flash 读回）。 相位偏 φ 时残差比为 2sin(kφ/2)：零点差 30° 电角度时 k=2 残差即等于 原幅值，补偿由减小扰动变为增大扰动。22 极下 2.7° 机械偏差即达此量级， 硬编码全局共用一张表不成立。 - 表采集于 bec7c11(2026-05)，其电流链路为改版前方案，此后陆续修正了 采样时刻落在上管导通窗、PA3/4/5 未配 AIN、零点配平未应用等问题。 用当时的链路去测量比其自身误差小三个数量级的量，采到的是链路误差。 删除内容：get_cogging_compensation 查表插值、FOC_DEBUG 校准状态机及 其打印函数与静态缓冲、init() 中两张硬编码表、COGGING_TABLE_SIZE/ COGGING_COMP_SCALE、tasks.cpp 调度 tick、RESET 帧 bit4 触发（该位保留）。 六个控制模式的 Uq 输出去掉补偿项；angle_el_cog 仍供 update_el_sincos 使用，改名回 angle_el。mode3 注释中"反转阻力差异由 REV 齿槽补偿表承担" 的描述一并订正（该依赖本已失效）。
240 lines
6.5 KiB
C++
240 lines
6.5 KiB
C++
/**
|
||
* @file Algorithm/FOC.h
|
||
* @brief 算法模块源文件
|
||
* @note 本文件仅补充 Doxygen 文档注释，不改变代码逻辑。
|
||
*/
|
||
|
||
// 函数声明
|
||
#ifndef _FOC_H
|
||
#define _FOC_H
|
||
#include "config.h"
|
||
#include "pid.h"
|
||
#include <cstdint>
|
||
|
||
/**
|
||
* @brief FOC_Controller 类或结构体
|
||
*/
|
||
class FOC_Controller {
|
||
|
||
public:
|
||
FOC_Controller();
|
||
|
||
// 初始化函数
|
||
/**
|
||
* @brief 初始化对象或模块
|
||
* @return u8 返回值
|
||
*/
|
||
u8 init();
|
||
|
||
u8 control_mode = 0;
|
||
float limit_voltage = 0.0f;
|
||
|
||
|
||
float target_angle = 0.0f;
|
||
float target_velocity = 0.0f;
|
||
float target_current = 0.0f;
|
||
|
||
|
||
float rt_angle = 0.0f;
|
||
float rt_velocity = 0.0f;
|
||
float rt_current = 0.0f;
|
||
|
||
bool over_current = false;
|
||
bool voltage_error = false;
|
||
bool is_shut_down = false;
|
||
bool thermal_shutdown = false; // 过热关机标志
|
||
bool emergency_shutdown = false; // 紧急电压跌落关机标志
|
||
|
||
float voltage_power_supply = 24.0f; // 电源电压
|
||
float v_safe = voltage_power_supply / 1.05f;
|
||
float duty_a = 0, duty_b = 0, duty_c = 0;
|
||
|
||
// 快速电压检测相关成员（基于跌幅检测）
|
||
float vbus_reference = 0.0f; // 基准电压（峰值检测，跟踪当前额定电压）
|
||
float last_vbus_fast = 0.0f; // 上次电压值（快速检测用）
|
||
u32 vbus_drop_count = 0; // 电压跌落计数器（防抖）
|
||
|
||
float cur_loop_p = DEFAULT_current_loop_p;
|
||
float vel_loop_p = DEFAULT_vel_loop_p;
|
||
float ang_loop_p = DEFAULT_angle_loop_p;
|
||
|
||
float vel_angloop_p = DEFAULT_vel_angloop_p;
|
||
float ang_angloop_p = DEFAULT_angle_angloop_p;
|
||
|
||
float ang_MIT_p = DEFAULT_angle_MIT_p;
|
||
float vel_MIT_p = DEFAULT_vel_MIT_p;
|
||
float cur_MIT_p = DEFAULT_current_MIT_p;
|
||
|
||
/**
|
||
* @brief velocity_open_loop 函数
|
||
* @return float 返回值
|
||
*/
|
||
float velocity_open_loop(void);
|
||
/**
|
||
* @brief angle_control 函数
|
||
* @return float 返回值
|
||
*/
|
||
float angle_control(void);
|
||
/**
|
||
* @brief angle_control_with_velocity_limit 函数
|
||
* @return float 返回值
|
||
*/
|
||
float angle_control_with_velocity_limit(void);
|
||
/**
|
||
* @brief MIT_Mode 函数
|
||
* @return float 返回值
|
||
*/
|
||
float MIT_Mode(void);
|
||
/**
|
||
* @brief velocity_control 函数
|
||
* @return float 返回值
|
||
*/
|
||
float velocity_control(void);
|
||
/**
|
||
* @brief current_control 函数
|
||
* @return float 返回值
|
||
*/
|
||
float current_control(void);
|
||
|
||
/**
|
||
* @brief 更新 power 相关状态
|
||
* @param[in] voltage 参数
|
||
* @return void
|
||
*/
|
||
void update_power(float voltage);
|
||
/**
|
||
* @brief DFOC_alignSensor 函数
|
||
* @param[in] _PP 参数
|
||
* @return void
|
||
*/
|
||
void DFOC_alignSensor(int _PP);
|
||
/**
|
||
* @brief shut_down 函数
|
||
* @return void
|
||
*/
|
||
void shut_down(void);
|
||
/**
|
||
* @brief resume 函数
|
||
* @return void
|
||
*/
|
||
void resume(void);
|
||
/**
|
||
* @brief drv_stop 函数
|
||
* @return void
|
||
*/
|
||
void drv_stop(void);
|
||
/**
|
||
* @brief drv_hold 函数
|
||
* @return float 返回值
|
||
*/
|
||
float drv_hold(void);
|
||
/**
|
||
* @brief PWM_out 函数
|
||
* @return float 返回值
|
||
*/
|
||
float PWM_out(void);
|
||
/**
|
||
* @brief shut_down_thermal 函数
|
||
* @return void
|
||
*/
|
||
void shut_down_thermal(void); // 过热关机
|
||
/**
|
||
* @brief resume_from_thermal 函数
|
||
* @return void
|
||
*/
|
||
void resume_from_thermal(void); // 过热恢复
|
||
/**
|
||
* @brief fast_voltage_check 函数
|
||
* @return void
|
||
*/
|
||
void fast_voltage_check(void); // 快速电压检测（100us周期调用）
|
||
|
||
// 此处默认值不作数
|
||
// 注意：构造期 limit_voltage 尚为 0，分母一律用 voltage_power_supply(24V)，
|
||
// 否则 P 初始化为 +inf；运行期各模式每周期 set_P/set_PID 覆盖。
|
||
// 内环 output_ramp 保持原值：实测收紧到 2000 V/s 会在制动段引入相位滞后，
|
||
// 导致到位抖动，已回退。
|
||
PIDController vel_loop =
|
||
PIDController{DEFAULT_vel_loop_p * 100 / voltage_power_supply, vel_loop_Ki,
|
||
vel_loop_Kd, 10000, voltage_power_supply};
|
||
PIDController angle_loop =
|
||
PIDController{DEFAULT_angle_loop_p * 100 / voltage_power_supply, 100.0f,
|
||
0.02f, 10000000, voltage_power_supply};
|
||
PIDController current_loop = PIDController{
|
||
DEFAULT_current_loop_p * 100 / voltage_power_supply, current_loop_Ki,
|
||
current_loop_Kd, 1500, voltage_power_supply};
|
||
PIDController vel_inVelAngloop =
|
||
PIDController{DEFAULT_vel_angloop_p * 100, vel_angloop_Ki,
|
||
vel_angloop_Kd, 1e6, v_safe};
|
||
PIDController ang_inVelAngloop =
|
||
PIDController{DEFAULT_angle_angloop_p * 100 / voltage_power_supply,
|
||
ang_angloop_Ki, ang_angloop_Kd, 1e4, 100};
|
||
|
||
PIDController ang_inMITloop =
|
||
PIDController{DEFAULT_angle_MIT_p * 100 / voltage_power_supply, angle_MIT_Ki,
|
||
angle_MIT_Kd, 1e5, 100};
|
||
PIDController vel_inMITloop = PIDController{
|
||
DEFAULT_vel_MIT_p * 100, vel_MIT_Ki, vel_MIT_Kd, 1e9, 1000};
|
||
PIDController current_inMITloop = PIDController{
|
||
DEFAULT_current_loop_p * 100 / voltage_power_supply, current_loop_Ki,
|
||
current_loop_Kd, 1500, voltage_power_supply};
|
||
|
||
/**
|
||
* @brief 更新 pid_limits 相关状态
|
||
* @return void
|
||
*/
|
||
void update_pid_limits() {
|
||
vel_loop.limit = voltage_power_supply;
|
||
angle_loop.limit = voltage_power_supply;
|
||
current_loop.limit = voltage_power_supply;
|
||
vel_inVelAngloop.limit = voltage_power_supply;
|
||
}
|
||
|
||
protected:
|
||
float shaft_angle = 0;
|
||
float Ualpha, Ubeta = 0;
|
||
float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
|
||
// 本周期电角度的 sin/cos：update_el_sincos() 每周期算一次，
|
||
// Park 变换与逆 Park 变换共用，避免同一角度算两次三角函数
|
||
float el_sin = 0.0f;
|
||
float el_cos = 0.0f;
|
||
/**
|
||
* @brief 每周期计算一次电角度 sin/cos 供 Park/逆Park 共用
|
||
* @param[in] angle_el 电角度（度）
|
||
* @return void
|
||
*/
|
||
inline void update_el_sincos(float angle_el);
|
||
/**
|
||
* @brief 设置 setSVPWM_PhaseVoltage 对应的参数或状态
|
||
* @param[in] Uq 参数
|
||
* @param[in] Ud 参数
|
||
* @return void
|
||
*/
|
||
void setSVPWM_PhaseVoltage(float Uq, float Ud);
|
||
/**
|
||
* @brief cal_Iq_Id 函数
|
||
* @param[in] current_a 参数
|
||
* @param[in] current_b 参数
|
||
* @param[in] current_c 参数
|
||
* @return float 返回值
|
||
* @note 电气角度三角函数用 update_el_sincos() 的预计算结果
|
||
*/
|
||
float cal_Iq_Id(float current_a, float current_b, float current_c);
|
||
float update_angle_sync_uq_feedforward(u8 mode, float table[],
|
||
u16 table_size, float mech_angle,
|
||
float target_vel, float actual_vel,
|
||
float velocity_error);
|
||
float get_mode3_velocity_p(float target_vel, float actual_vel);
|
||
float get_mode3_velocity_scale(float target_vel, float actual_vel,
|
||
float min_scale);
|
||
};
|
||
|
||
// 定义函数指针类型
|
||
typedef float (FOC_Controller::*FOCFunctionPtr)(void);
|
||
|
||
/**
|
||
* @brief calibrate_zero_position 函数
|
||
* @return void
|
||
*/
|
||
void calibrate_zero_position(void);
|
||
#endif |