Files
Motor_v2/Robot_V2/device/ADC.cpp
T
cbb 7c4aaa5ee6 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算法,显著提升性能
- 保留通用回退路径处理其他尺寸情况
2026-09-01 02:46:25 +08:00

430 lines
16 KiB
C++
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
/**
* @file device/ADC.cpp
* @brief 底层设备驱动与外设封装源文件
* @note 本文件仅补充 Doxygen 文档注释,不改变代码逻辑。
*/
#include "adc.h"
#include "Can.h"
#include "FOC.h"
#include "Filter.h"
#include "config.h"
#include "initial.h"
#include "math.h"
#include "systick.h"
#include <cstdint>
#include <cstring>
// ADC 换算常量均为编译期常量,非可变全局状态。
// 用于 ADC 原始值到电压、温度、电流的固定比例换算。
/** @brief ADC_SCALE 常量定义 */
constexpr float ADC_SCALE = 3.3f / 4096.0f;
/** @brief TEMP_FACTOR 常量定义 */
constexpr float TEMP_FACTOR = 1000.0f / 4.3f;
/** @brief VCC_FACTOR 常量定义 */
constexpr float VCC_FACTOR = 11.0f;
#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;
// 全局采样器单例:系统只有一组 ADC 采样缓存,由 hardware_init() 调用 sampler.init() 初始化。
// ADC0 只跑 injected 组(PWM 同步):中断写 adc_current_value[](U/V/W)与
// adc_mcu_temp_raw(温度,挂注入序列尾部同步转换)并置位 adc_current_frame_ready。
// VBUS / DRV温度 由 ADC1 在慢速任务中软件触发轮询,与电流采样彻底隔离。
// 背景:此前温度/VBUS/DRV 放在 ADC0 regular 组连续扫描,与 injected 抢占同一 ADC。
// 温度通道提到 239.5 周期后,稳态下注入触发几乎必然打断温度采样中段:
// regular 被打断后从 rank0 重启而 DMA 计数不复位 → adc_value[] 数据错位;
// 采样电容长期挂在高内阻温度源电压上,干扰注入组首通道 → 相电流检测故障。
sampler sampler; // 采样器初始化
volatile u16 adc_mcu_temp_raw = 0; // ADC0 injected rank3: MCU温度原始值
static volatile u16 adc_current_value[3] = {0}; // injected 同步采样: U / V / W
static volatile u8 adc_current_frame_ready = 0;
/**
* @brief 将电流 ADC 原始值换算为毫安值
* @param[in] raw_adc 参数
* @return float 返回值
*/
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温度 单次轮询)
\param[in] none
\param[out] none
\retval none
\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 配置) */
rcu_periph_clock_enable(RCU_ADC1);
adc_deinit(ADC1);
adc_mode_config(ADC_MODE_FREE);
adc_data_alignment_config(ADC1, ADC_DATAALIGN_RIGHT);
/* 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校准(校准完成后再进入连续模式,避免校准期间 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 零等待读取最新 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;
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);
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;
}
/*!
\brief configure the ADC0 peripheral(PWM 同步电流采样专用)
\param[in] none
\param[out] none
\retval none
\note ADC0 只保留 injected 组,regular 组整体移除:
此前 regular 连续扫描与 injected 触发抢占同一 ADC,注入打断后
regular 从头重启而 DMA 不复位导致数据错位,且温度长采样被打断后
采样电容残余污染注入组首通道。移除 regular 后注入组再无打断源。
*/
void adc_config(void) {
/* enable ADC clock */
rcu_periph_clock_enable(RCU_ADC0);
/* config ADC clock(ADC0/ADC1 共用分频:120MHz/4 = 30MHz) */
rcu_adc_clock_config(RCU_CKADC_CKAPB2_DIV4);
/* 复位ADC */
adc_deinit(ADC0);
/* ADC模式配置 */
adc_mode_config(ADC_MODE_FREE);
/* 扫描模式:注入序列内 4 通道触发后自动连续转换 */
adc_special_function_config(ADC0, ADC_SCAN_MODE, ENABLE);
/* ADC数据右对齐配置 */
adc_data_alignment_config(ADC0, ADC_DATAALIGN_RIGHT);
/* 使能ADC温度传感器(内部通道仅连接 ADC0 的 ch16/ch17) */
adc_tempsensor_vrefint_enable();
/* injected 组:rank0=U, rank1=V, rank2=W, rank3=MCU温度,TIMER1 TRGO 触发。
* 温度传感器(ch16)为高内阻源,数据手册要求采样时间≥17.1μs,保持最大档
* 239.5 周期,且挂在序列尾部:整段转换不会被打断,每 PWM 周期同步采一次,
* 40kHz 过采样 + 中值滤波后温度比旧架构更稳。
* 原先的 rank0 dummy 已删除:dummy 用于吸收"注入打断 regular"产生的采样
* 电容残余电荷,现 regular 已移除,序列空闲期电容挂在温度通道(~1.3V),
* U 相(ch3 运放低阻输出) 的 55.5 周期采样足以完成重建。
* 序列总时长 68×3+252 = 456 周期 = 15.2μs < 25μs 触发周期。 */
adc_channel_length_config(ADC0, ADC_INSERTED_CHANNEL, 4);
adc_inserted_channel_config(ADC0, 0, ADC_CHANNEL_3, ADC_SAMPLETIME_55POINT5);
adc_inserted_channel_config(ADC0, 1, ADC_CHANNEL_4, ADC_SAMPLETIME_55POINT5);
adc_inserted_channel_config(ADC0, 2, ADC_CHANNEL_5, ADC_SAMPLETIME_55POINT5);
adc_inserted_channel_config(ADC0, 3, ADC_CHANNEL_16,
ADC_SAMPLETIME_239POINT5);
adc_external_trigger_source_config(ADC0, ADC_INSERTED_CHANNEL,
ADC0_1_EXTTRIG_INSERTED_T1_TRGO);
adc_external_trigger_config(ADC0, ADC_INSERTED_CHANNEL, ENABLE);
/* 使能ADC接口 */
adc_enable(ADC0);
/* ADC校准 */
adc_calibration_enable(ADC0);
/* injected 完成中断负责搬运完整三相样本与温度原始值 */
adc_interrupt_enable(ADC0, ADC_INT_EOIC);
}
/**
* @brief 采样类初始化
*/
u8 sampler::init() {
adc_config(); // ADC0:PWM 同步注入组(U/V/W + MCU温度)
adc1_config(); // ADC1:VBUS / DRV温度 慢速轮询
current_sample_head = 0;
adc_current_frame_ready = 0;
delay_1ms(1);
const float current_init = current_adc_to_ma(
static_cast<u16>((-CURRENT_OFFSET / ADC_SCALE) + 0.5f));
// 初始采样
for (int i = 0; i < ADC_FILTER_SIZE; i++) {
mcu_temp_samples[i] =
(1.43f - adc_mcu_temp_raw * ADC_SCALE) * TEMP_FACTOR + 35.0f;
currU_samples[i] = current_init;
currV_samples[i] = current_init;
currW_samples[i] = current_init;
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;
drv_temp_samples[i] =
1.0f / (0.00335f + 0.00025f * log(R_now / 10000.0f)) - 273.15f;
delay_1ms(1);
}
return 0;
}
/**
* @brief 获取MCU温度
* @return float
* 温度值,单位为摄氏度,范围应在-40到145之间,如果读取失败则返回0.0f
*/
float sampler::get_mcu_temp() {
return cached_mcu_temp;
}
/**
* @brief 获取电机驱动温度
* @return float 温度值,单位为摄氏度
*
* 通过热敏电阻分压电路计算温度值,使用Steinhart-Hart方程进行温度转换。
* 如果计算结果超出有效范围(-40到255摄氏度),则重新采样计算。
*/
float sampler::get_drv_temp() {
return cached_drv_temp;
}
/**
* @brief 获取电流值
* @param type 电流类型,'U'表示U相电流,'V'表示V相电流,'W'表示W相电流
* @return float 电流值,单位为安培
*
* 通过ADC采样计算各相电流值
*/
float sampler::get_current(char type) {
u8 idx = (current_sample_head == 0U) ? (ADC_FILTER_SIZE - 1U)
: (current_sample_head - 1U);
switch (type) {
case 'U': {
tempora_sample = currU_samples[idx];
break;
}
case 'V': {
tempora_sample = currV_samples[idx];
break;
}
case 'W': {
tempora_sample = currW_samples[idx];
break;
}
default: {
return ERROR_ADC_VAL;
}
}
if (tempora_sample < MIN_CURRENT or tempora_sample > MAX_CURRENT) {
return ERROR_ADC_VAL;
}
return tempora_sample;
}
/**
* @brief 获取供电电压
* @return float 电压值,单位为伏特
*
* 通过ADC采样计算供电电压值,使用迭代方式确保获取有效的电压读数。
* 如果计算结果超出有效范围(-40到255伏特),则重新采样计算。
*/
float sampler::get_vbus_voltage() {
return cached_vbus;
}
/**
* @brief 更新MCU温度
*
* 通过ADC采样计算MCU温度值,使用Steinhart-Hart方程进行温度转换。
* 如果计算结果超出有效范围(-40到255摄氏度),则重新采样计算。
*/
void sampler::update_mcu_temp() {
// 环形缓冲区写入,替代 memmove
mcu_temp_samples[mcu_temp_head] =
(1.43f - adc_mcu_temp_raw * ADC_SCALE) * TEMP_FACTOR + 35.0f;
mcu_temp_head++;
if (mcu_temp_head >= ADC_FILTER_SIZE) mcu_temp_head = 0;
// 更新缓存:中值滤波
tempora_sample = medium_filter(mcu_temp_samples, ADC_FILTER_SIZE);
if (tempora_sample < MIN_TEMP || tempora_sample > MAX_TEMP) {
cached_mcu_temp = ERROR_ADC_VAL;
} else {
cached_mcu_temp = tempora_sample;
}
}
/**
* @brief 更新MCU温度
*/
void sampler::update_drv_temp() {
// ADC1 单次轮询转换 ch9(NTC 分压,低阻源)
const float drv_raw = (float)adc1_read_raw(ADC_CHANNEL_9);
// 环形缓冲区写入,替代 memmove
float R_now = drv_raw / (4096 - drv_raw) * 4700;
drv_temp_samples[drv_temp_head] =
1.0f / (0.00335f + 0.00025f * log(R_now / 10000.0f)) - 273.15f;
drv_temp_head++;
if (drv_temp_head >= ADC_FILTER_SIZE) drv_temp_head = 0;
// 更新缓存:中值滤波
tempora_sample = medium_filter(drv_temp_samples, ADC_FILTER_SIZE);
if (tempora_sample < MIN_TEMP || tempora_sample > MAX_TEMP) {
cached_drv_temp = ERROR_ADC_VAL;
} else {
cached_drv_temp = tempora_sample;
}
}
/**
* @brief 更新相电流
*/
void sampler::update_current() {
if (adc_current_frame_ready == 0U) {
return;
}
const u8 idx = current_sample_head;
currU_samples[idx] = current_adc_to_ma(adc_current_value[0]);
currV_samples[idx] = current_adc_to_ma(adc_current_value[1]);
currW_samples[idx] = current_adc_to_ma(adc_current_value[2]);
current_sample_head++;
if (current_sample_head >= ADC_FILTER_SIZE) {
current_sample_head = 0;
}
adc_current_frame_ready = 0U;
}
/**
* @brief 更新供电电压
*/
void sampler::update_vbus_voltage() {
// ADC1 连续模式零等待读最新 VBUS(ch6,电阻分压,低阻源)
// 环形缓冲区写入,替代 memmove
vbus_samples[temp_vbus_head] =
adc1_read_vbus_latest() * ADC_SCALE * VCC_FACTOR;
temp_vbus_head++;
if (temp_vbus_head >= ADC_FILTER_SIZE) temp_vbus_head = 0;
// 更新缓存:中值滤波 + 范围检查
tempora_sample = medium_filter(vbus_samples, ADC_FILTER_SIZE);
const u8 mid = ADC_FILTER_SIZE / 2;
if (vbus_samples[mid] > MAX_VCC || vbus_samples[mid] < MIN_VCC) {
cached_vbus = ERROR_ADC_VAL;
} else {
cached_vbus = tempora_sample;
}
}
/**
* @brief ADC0_1_IRQHandler 中断处理函数
* @return void
*/
extern "C" void ADC0_1_IRQHandler(void) {
if (adc_interrupt_flag_get(ADC0, ADC_INT_FLAG_EOIC) != RESET) {
/* rank0/1/2 = U/V/W 同一 PWM 时刻样本,rank3 = MCU温度(序列尾部) */
adc_current_value[0] = adc_inserted_data_read(ADC0, ADC_INSERTED_CHANNEL_0);
adc_current_value[1] = adc_inserted_data_read(ADC0, ADC_INSERTED_CHANNEL_1);
adc_current_value[2] = adc_inserted_data_read(ADC0, ADC_INSERTED_CHANNEL_2);
adc_mcu_temp_raw = adc_inserted_data_read(ADC0, ADC_INSERTED_CHANNEL_3);
adc_current_frame_ready = 1U;
adc_interrupt_flag_clear(ADC0, ADC_INT_FLAG_EOIC);
extern TaskScheduler scheduler;
scheduler.taskFlags |= (1u << TASK_CURRENT_UPDATE);
}
}