feat(FOC): 优化FOC控制算法并修复多个控制环路问题
- 重构cal_Iq_Id函数，移除电角度参数传递，改用预计算的sin/cos值 以避免重复三角函数计算，提升性能约15-300周期 - 修复角度同步Uq前馈学习中的运动方向判断逻辑，解决反转时的 震荡问题和正转正常反转震荡的根本原因 - 实现低速缩放因子函数get_mode3_velocity_scale，改进静摩擦 hunting抑制效果 - 优化电压检测逻辑，将阻塞式ADC读取改为连续模式零等待， 提升实时性能 - 修复初始化期间电压分母为零的问题，添加下限保护防止P增益 无穷大 - 实现PID控制器测量微分功能，消除目标阶跃时的微分冲击 - 改进积分抗饱和策略，添加外部饱和泄放功能，优化到位稳定 性能 perf(Filter): 优化中值滤波算法性能 - 针对ADC_FILTER_SIZE=6的固定场景实现最优排序网络， 用12次比较替代通用std::nth_element算法，显著提升性能 - 保留通用回退路径处理其他尺寸情况
This commit is contained in:
+95
-18
@@ -23,9 +23,19 @@ constexpr float ADC_SCALE = 3.3f / 4096.0f;
|
||||
constexpr float TEMP_FACTOR = 1000.0f / 4.3f;
|
||||
/** @brief VCC_FACTOR 常量定义 */
|
||||
constexpr float VCC_FACTOR = 11.0f;
|
||||
/** @brief CURRENT_FACTOR 常量定义 */
|
||||
#if GATE_DRIVER_DRV8323
|
||||
/** @brief CURRENT_FACTOR 常量定义（新板：DRV8323 片上 CSA）
|
||||
* SOx = VREF/2 ± I×R×G（SPI 已配置 VREF_DIV=1、GAIN 见 config.h）。
|
||||
* ADC_SCALE 输出为伏特，故系数单位为 mA/V = 1000/(G×R)：
|
||||
* G=10V/V、R=10mΩ → 10000 mA/V，量程约 ±16.5A。
|
||||
* 修改 config.h 的 DRV8323_CSA_GAIN_VV 后此系数自动跟随。 */
|
||||
constexpr float CURRENT_FACTOR =
|
||||
1000.0f / (DRV8323_CSA_GAIN_VV * DRV8323_SHUNT_OHM);
|
||||
#else
|
||||
/** @brief CURRENT_FACTOR 常量定义（老板：外置放大链，保持原标定） */
|
||||
constexpr float CURRENT_FACTOR =
|
||||
1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA
|
||||
#endif
|
||||
extern CAN_transmiter CAN0_transmiter;
|
||||
extern FOC_Controller FOC;
|
||||
|
||||
@@ -51,13 +61,20 @@ static inline float current_adc_to_ma(u16 raw_adc) {
|
||||
return (raw_adc * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
|
||||
}
|
||||
|
||||
// ADC1 连续转换运行标志：adc1_config() 启动后置位，
|
||||
// adc1_read_raw() 据此判断是否需要暂停/恢复连续模式。
|
||||
static bool adc1_continuous_active = false;
|
||||
|
||||
/*!
|
||||
\brief configure the ADC1 peripheral（VBUS / DRV温度 专用）
|
||||
\brief configure the ADC1 peripheral（VBUS 连续转换 / DRV温度 单次轮询）
|
||||
\param[in] none
|
||||
\param[out] none
|
||||
\retval none
|
||||
\note ADC1 独占慢速量：单通道单次软件触发，无 DMA、无中断、无连续模式，
|
||||
软件按需轮询读取，与 ADC0 的 PWM 同步电流采样互不干扰。
|
||||
\note ADC1 独占慢速量：ch6(VBUS) 连续转换，读取零等待；ch9(DRV NTC)
|
||||
由 adc1_read_raw() 按需暂停连续模式单次轮询。无 DMA、无中断，
|
||||
与 ADC0 的 PWM 同步注入组互不干扰（独立外设）。
|
||||
原实现每次读取都要"软件触发+死等EOC"：fast_voltage_check
|
||||
10kHz 调用，每次阻塞 ~1.5μs，纯浪费在等待上。
|
||||
*/
|
||||
void adc1_config(void) {
|
||||
/* enable ADC1 clock（ADC 时钟分频与 ADC0 共用 RCU 配置） */
|
||||
@@ -67,36 +84,96 @@ void adc1_config(void) {
|
||||
adc_mode_config(ADC_MODE_FREE);
|
||||
adc_data_alignment_config(ADC1, ADC_DATAALIGN_RIGHT);
|
||||
|
||||
/* 单通道单次转换：调用方每次配置 rank0 通道后软件触发 */
|
||||
/* VBUS 单通道连续转换：软件触发一次后自持续 */
|
||||
adc_channel_length_config(ADC1, ADC_REGULAR_CHANNEL, 1);
|
||||
adc_regular_channel_config(ADC1, 0, ADC_CHANNEL_6, ADC_SAMPLETIME_28POINT5);
|
||||
adc_external_trigger_source_config(ADC1, ADC_REGULAR_CHANNEL,
|
||||
ADC0_1_EXTTRIG_REGULAR_NONE);
|
||||
adc_external_trigger_config(ADC1, ADC_REGULAR_CHANNEL, ENABLE);
|
||||
|
||||
adc_enable(ADC1);
|
||||
/* ADC校准 */
|
||||
/* ADC校准（校准完成后再进入连续模式，避免校准期间 CONT 位参与） */
|
||||
adc_calibration_enable(ADC1);
|
||||
adc_special_function_config(ADC1, ADC_CONTINUOUS_MODE, ENABLE);
|
||||
|
||||
/* 启动连续转换并等待首个结果，保证 DR 立即有效 */
|
||||
adc_flag_clear(ADC1, ADC_FLAG_EOC);
|
||||
adc_software_trigger_enable(ADC1, ADC_REGULAR_CHANNEL);
|
||||
while (adc_flag_get(ADC1, ADC_FLAG_EOC) == RESET) {
|
||||
}
|
||||
adc_flag_clear(ADC1, ADC_FLAG_EOC);
|
||||
adc1_continuous_active = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief ADC1 单次软件触发转换并等待完成
|
||||
* @param[in] channel ADC 通道号（如 ADC_CHANNEL_6 / ADC_CHANNEL_9）
|
||||
* @return u16 转换原始值（超时返回上次 RDATA 残留值）
|
||||
* @note 仅在主循环任务上下文调用（所有任务由调度器串行派发，无并发）。
|
||||
* 单次转换约 41 周期 ≈ 1.4μs @30MHz ADC 时钟。
|
||||
*/
|
||||
/*!
|
||||
\brief 零等待读取最新 VBUS 转换结果
|
||||
\retval u16 最近一次完成转换的原始值
|
||||
\note 仅在 VBUS 连续模式运行时调用（主循环任务上下文）。
|
||||
读 DR 硬件清 EOC，下一次完成转换会重新刷新 DR；两次读取之间
|
||||
完成的转换被静默丢弃（F1 型 ADC 无溢出标志问题），
|
||||
读到的值陈旧度 ≤1 个转换周期（41 周期 @30MHz ≈ 1.4μs）。
|
||||
*/
|
||||
u16 adc1_read_vbus_latest(void) {
|
||||
return (u16)adc_regular_data_read(ADC1);
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief ADC1 单次软件触发转换并等待完成
|
||||
\param[in] channel ADC 通道号（如 ADC_CHANNEL_9）
|
||||
\retval u16 转换原始值（超时返回上次 RDATA 残留值）
|
||||
\note 仅在主循环任务上下文调用（所有任务由调度器串行派发，无并发，
|
||||
与 fast_voltage_check / update_vbus_voltage 不会交错执行）。
|
||||
单次转换约 41 周期 ≈ 1.4μs @30MHz ADC 时钟。
|
||||
连续模式运行中调用时：暂停→单次转换→恢复，VBUS 流中断约 3~5μs。
|
||||
*/
|
||||
u16 adc1_read_raw(u8 channel) {
|
||||
const bool was_continuous = adc1_continuous_active;
|
||||
u16 timeout = 0;
|
||||
adc_flag_clear(ADC1, ADC_FLAG_EOC);
|
||||
if (was_continuous) {
|
||||
adc_special_function_config(ADC1, ADC_CONTINUOUS_MODE, DISABLE);
|
||||
/* 先清 EOC 再等置位：连续模式下 EOC 常态置位（DR 久未读，
|
||||
* 后续转换溢出丢失），直接等会观察到陈旧标志而另有转换在飞行中。
|
||||
* 清除后观察到的置位必然来自"暂停瞬间在飞行中"的最后一次转换
|
||||
* 完成后才置位——此时 ADC 真正静止，后续配置/触发不会再被
|
||||
* 飞行中的转换干扰（SWSTART 被忽略、读到上一通道的 DR，
|
||||
* 曾表现为 DRV 温度偶发出错）。超时兜底覆盖"暂停恰好落在
|
||||
* 两次转换间隙"的无在飞行转换情形。 */
|
||||
adc_flag_clear(ADC1, ADC_FLAG_EOC);
|
||||
while (adc_flag_get(ADC1, ADC_FLAG_EOC) == RESET) {
|
||||
/* 超时只需覆盖一个转换周期（41 ADC 周期 ≈ 20 次循环），
|
||||
* 仅"暂停恰好落在转换间隙"时才会走到超时 */
|
||||
if (++timeout > 100U) break;
|
||||
}
|
||||
adc_flag_clear(ADC1, ADC_FLAG_EOC);
|
||||
}
|
||||
|
||||
adc_regular_channel_config(ADC1, 0, channel, ADC_SAMPLETIME_28POINT5);
|
||||
adc_software_trigger_enable(ADC1, ADC_REGULAR_CHANNEL);
|
||||
timeout = 0;
|
||||
while (adc_flag_get(ADC1, ADC_FLAG_EOC) == RESET) {
|
||||
if (++timeout > 2000U) { // 硬件异常保护，避免死等
|
||||
break;
|
||||
}
|
||||
}
|
||||
u16 raw = (u16)adc_regular_data_read(ADC1);
|
||||
adc_flag_clear(ADC1, ADC_FLAG_EOC);
|
||||
return (u16)adc_regular_data_read(ADC1);
|
||||
|
||||
if (was_continuous) {
|
||||
/* 恢复 VBUS 连续转换 */
|
||||
adc_regular_channel_config(ADC1, 0, ADC_CHANNEL_6,
|
||||
ADC_SAMPLETIME_28POINT5);
|
||||
adc_special_function_config(ADC1, ADC_CONTINUOUS_MODE, ENABLE);
|
||||
adc_software_trigger_enable(ADC1, ADC_REGULAR_CHANNEL);
|
||||
/* 等恢复后的首个 ch6 转换完成并读走（清 EOC），
|
||||
* 避免 vbus 零等待读取在接下来的 ~1.4μs 窗口内
|
||||
* 读到 ch9 的残留值 */
|
||||
timeout = 0;
|
||||
while (adc_flag_get(ADC1, ADC_FLAG_EOC) == RESET) {
|
||||
if (++timeout > 2000U) break;
|
||||
}
|
||||
(void)adc_regular_data_read(ADC1);
|
||||
}
|
||||
return raw;
|
||||
}
|
||||
|
||||
/*!
|
||||
@@ -172,7 +249,7 @@ u8 sampler::init() {
|
||||
currV_samples[i] = current_init;
|
||||
currW_samples[i] = current_init;
|
||||
|
||||
vbus_samples[i] = adc1_read_raw(ADC_CHANNEL_6) * ADC_SCALE * VCC_FACTOR;
|
||||
vbus_samples[i] = adc1_read_vbus_latest() * ADC_SCALE * VCC_FACTOR;
|
||||
|
||||
const float drv_raw = (float)adc1_read_raw(ADC_CHANNEL_9);
|
||||
float R_now = drv_raw / (4096 - drv_raw) * 4700;
|
||||
@@ -315,10 +392,10 @@ void sampler::update_current() {
|
||||
* @brief 更新供电电压
|
||||
*/
|
||||
void sampler::update_vbus_voltage() {
|
||||
// ADC1 单次轮询转换 ch6（电阻分压，低阻源）
|
||||
// ADC1 连续模式零等待读最新 VBUS（ch6，电阻分压，低阻源）
|
||||
// 环形缓冲区写入，替代 memmove
|
||||
vbus_samples[temp_vbus_head] =
|
||||
adc1_read_raw(ADC_CHANNEL_6) * ADC_SCALE * VCC_FACTOR;
|
||||
adc1_read_vbus_latest() * ADC_SCALE * VCC_FACTOR;
|
||||
temp_vbus_head++;
|
||||
if (temp_vbus_head >= ADC_FILTER_SIZE) temp_vbus_head = 0;
|
||||
|
||||
|
||||
Reference in New Issue
Block a user