diff --git a/Robot_V2/Algorithm/FOC.cpp b/Robot_V2/Algorithm/FOC.cpp index 54f02b2..2d4e6c2 100644 --- a/Robot_V2/Algorithm/FOC.cpp +++ b/Robot_V2/Algorithm/FOC.cpp @@ -17,7 +17,7 @@ extern angle_sensor KTH7823; extern CAN_transmiter CAN0_transmiter; extern EncoderVelocityKalman Vel_kalman; // 速度滤波 extern CurrentKalman Current_kalman; -extern u16 adc_value[6]; // ADC DMA缓冲区，用于快速电压检测 +extern u16 adc_value[3]; // regular DMA缓冲区：MCU温度 / DRV温度 / VBUS // 初始变量及函数定义 #define _constrain(amt, low, high) \ @@ -52,6 +52,17 @@ void calibrate_zero_position(void) { // 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq) inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b, float current_c) { + 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; + // 计算电气角度 float angle_el = fmod(shaft_angle * PP - KTH7823.calibration_angle, 360.0f); @@ -351,8 +362,8 @@ void FOC_Controller::update_power(float voltage) { * - VBUS_DEBOUNCE_COUNT: 防抖计数 (默认5次) */ void FOC_Controller::fast_voltage_check(void) { - // 直接从DMA缓冲区读取最新ADC值（通道5是Vbus） - float current_vbus = adc_value[5] * (3.3f / 4096.0f) * 11.0f; + // 直接从DMA缓冲区读取最新ADC值（regular rank2 是 VBUS） + float current_vbus = adc_value[2] * (3.3f / 4096.0f) * 11.0f; // 更新基准电压（峰值检测） // 电压上升时，基准电压跟随；电压下跌时，基准电压保持 diff --git a/Robot_V2/device/ADC.cpp b/Robot_V2/device/ADC.cpp index 6c9deff..0528a75 100644 --- a/Robot_V2/device/ADC.cpp +++ b/Robot_V2/device/ADC.cpp @@ -3,6 +3,7 @@ #include "FOC.h" #include "Filter.h" #include "config.h" +#include "initial.h" #include "math.h" #include "systick.h" #include @@ -17,7 +18,14 @@ constexpr float CURRENT_FACTOR = extern CAN_transmiter CAN0_transmiter; extern FOC_Controller FOC; sampler sampler; // 采样器初始化 -u16 adc_value[6] = {0}; // 显式初始化数组元素为0 +u16 adc_value[3] = {0}; // regular DMA: MCU温度 / DRV温度 / VBUS +static volatile u16 adc_current_value[3] = {0}; // injected 同步采样: U / V / W +static volatile u8 adc_current_frame_ready = 0; + +static inline float current_adc_to_ma(u16 raw_adc) { + return (raw_adc * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR; +} + /*! \brief configure the DMA peripheral \param[in] none @@ -41,7 +49,7 @@ void dma_config(void) { dma_data_parameter.periph_width = DMA_PERIPHERAL_WIDTH_16BIT; dma_data_parameter.memory_width = DMA_MEMORY_WIDTH_16BIT; dma_data_parameter.direction = DMA_PERIPHERAL_TO_MEMORY; - dma_data_parameter.number = 6; + dma_data_parameter.number = 3; dma_data_parameter.priority = DMA_PRIORITY_HIGH; dma_init(DMA0, DMA_CH0, &dma_data_parameter); @@ -67,38 +75,48 @@ void adc_config(void) { adc_deinit(ADC0); /* ADC模式配置 */ adc_mode_config(ADC_MODE_FREE); - /* 使能ADC连续转换功能 */ + /* regular 组保留连续扫描，用于温度/VBUS 后台采样 */ adc_special_function_config(ADC0, ADC_CONTINUOUS_MODE, ENABLE); - /* 使能ADC扫描模式 */ adc_special_function_config(ADC0, ADC_SCAN_MODE, ENABLE); + /* injected 组用于 PWM 同步电流采样，关闭自动串联，独立由 TIMER1 触发 */ + adc_special_function_config(ADC0, ADC_INSERTED_CHANNEL_AUTO, DISABLE); /* ADC数据右对齐配置 */ adc_data_alignment_config(ADC0, ADC_DATAALIGN_RIGHT); /* 使能ADC温度传感器和内部参考电压 */ adc_tempsensor_vrefint_enable(); /* 配置ADC通道长度 */ - adc_channel_length_config(ADC0, ADC_REGULAR_CHANNEL, 6); + adc_channel_length_config(ADC0, ADC_REGULAR_CHANNEL, 3); /* 配置ADC规则通道 */ adc_regular_channel_config(ADC0, 0, ADC_CHANNEL_16, ADC_SAMPLETIME_41POINT5); adc_regular_channel_config(ADC0, 1, ADC_CHANNEL_9, ADC_SAMPLETIME_28POINT5); - adc_regular_channel_config(ADC0, 2, ADC_CHANNEL_3, ADC_SAMPLETIME_28POINT5); - adc_regular_channel_config(ADC0, 3, ADC_CHANNEL_4, ADC_SAMPLETIME_28POINT5); - adc_regular_channel_config(ADC0, 4, ADC_CHANNEL_5, ADC_SAMPLETIME_28POINT5); - adc_regular_channel_config(ADC0, 5, ADC_CHANNEL_6, ADC_SAMPLETIME_28POINT5); + adc_regular_channel_config(ADC0, 2, ADC_CHANNEL_6, ADC_SAMPLETIME_28POINT5); - /* ADC触发配置 */ + /* regular 组继续软件触发连续转换：本组只服务低速量 */ adc_external_trigger_source_config(ADC0, ADC_REGULAR_CHANNEL, ADC0_1_EXTTRIG_REGULAR_NONE); adc_external_trigger_config(ADC0, ADC_REGULAR_CHANNEL, ENABLE); + /* injected 组改为 TIMER1 CH3 导出的 TRGO 触发，实现三相同一采样时刻锁存 */ + 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_3, ADC_SAMPLETIME_55POINT5); + adc_inserted_channel_config(ADC0, 2, ADC_CHANNEL_4, ADC_SAMPLETIME_55POINT5); + adc_inserted_channel_config(ADC0, 3, ADC_CHANNEL_5, ADC_SAMPLETIME_55POINT5); + 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); /* 使能ADC DMA功能 */ adc_dma_mode_enable(ADC0); - /* 使能ADC软件触发 */ + /* injected 完成中断负责搬运完整三相样本 */ + adc_interrupt_enable(ADC0, ADC_INT_EOIC); + /* 先启动 regular 组连续后台采样 */ adc_software_trigger_enable(ADC0, ADC_REGULAR_CHANNEL); } /** @@ -107,20 +125,22 @@ void adc_config(void) { u8 sampler::init() { dma_config(); // DMA采样ADC初始化 adc_config(); // ADC初始化 + current_sample_head = 0; + adc_current_frame_ready = 0; delay_1ms(1); + + const float current_init = current_adc_to_ma( + static_cast((-CURRENT_OFFSET / ADC_SCALE) + 0.5f)); // 初始采样 - for (int i = 0; i < ADC_FILTER_SIZE + 1; i++) { + for (int i = 0; i < ADC_FILTER_SIZE; i++) { mcu_temp_samples[i] = (1.43f - adc_value[0] * ADC_SCALE) * TEMP_FACTOR + 35.0f; - currU_samples[i] = - (adc_value[2] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR; - currV_samples[i] = - (adc_value[3] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR; - currW_samples[i] = - (adc_value[4] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR; + currU_samples[i] = current_init; + currV_samples[i] = current_init; + currW_samples[i] = current_init; - vbus_samples[i] = adc_value[5] * ADC_SCALE * VCC_FACTOR; + vbus_samples[i] = adc_value[2] * ADC_SCALE * VCC_FACTOR; float R_now = (float)adc_value[1] / (4096 - (float)adc_value[1]) * 4700; drv_temp_samples[i] = @@ -135,7 +155,7 @@ u8 sampler::init() { * 温度值，单位为摄氏度，范围应在-40到145之间，如果读取失败则返回0.0f */ float sampler::get_mcu_temp() { - tempora_sample = medium_filter(mcu_temp_samples, ADC_FILTER_SIZE + 1); + tempora_sample = medium_filter(mcu_temp_samples, ADC_FILTER_SIZE); // 检查温度读数是否在有效范围内 if (tempora_sample < MIN_TEMP or tempora_sample > MAX_TEMP) { return ERROR_ADC_VAL; @@ -151,7 +171,7 @@ float sampler::get_mcu_temp() { * 如果计算结果超出有效范围(-40到255摄氏度)，则重新采样计算。 */ float sampler::get_drv_temp() { - tempora_sample = medium_filter(drv_temp_samples, ADC_FILTER_SIZE + 1); + tempora_sample = medium_filter(drv_temp_samples, ADC_FILTER_SIZE); // 检查温度读数是否在有效范围内 if (tempora_sample < MIN_TEMP || tempora_sample > MAX_TEMP) { return ERROR_ADC_VAL; @@ -167,24 +187,18 @@ float sampler::get_drv_temp() { * 通过ADC采样计算各相电流值 */ float sampler::get_current(char type) { - - u8 idx = current_sample_index; + u8 idx = (current_sample_head == 0U) ? (ADC_FILTER_SIZE - 1U) + : (current_sample_head - 1U); switch (type) { case 'U': { - // return currU_samples[current_sample_index]; - // tempora_sample = medium_filter(currU_samples, ADC_FILTER_SIZE + 1); tempora_sample = currU_samples[idx]; break; } case 'V': { - // return currV_samples[current_sample_index]; - // tempora_sample = medium_filter(currV_samples, ADC_FILTER_SIZE + 1); tempora_sample = currV_samples[idx]; break; } case 'W': { - // return currW_samples[current_sample_index]; - // tempora_sample = medium_filter(currW_samples, ADC_FILTER_SIZE + 1); tempora_sample = currW_samples[idx]; break; } @@ -207,10 +221,10 @@ float sampler::get_current(char type) { * 如果计算结果超出有效范围(-40到255伏特)，则重新采样计算。 */ float sampler::get_vbus_voltage() { - tempora_sample = medium_filter(vbus_samples, ADC_FILTER_SIZE + 1); + tempora_sample = medium_filter(vbus_samples, ADC_FILTER_SIZE); // 检查电压读数是否在有效范围内 - if (vbus_samples[ADC_FILTER_SIZE / 2] > MAX_VCC or - vbus_samples[ADC_FILTER_SIZE / 2] < MIN_VCC) { + const u8 mid = ADC_FILTER_SIZE / 2; + if (vbus_samples[mid] > MAX_VCC or vbus_samples[mid] < MIN_VCC) { return ERROR_ADC_VAL; } return tempora_sample; @@ -224,8 +238,8 @@ float sampler::get_vbus_voltage() { */ void sampler::update_mcu_temp() { memmove(mcu_temp_samples, mcu_temp_samples + 1, - ADC_FILTER_SIZE * sizeof(float)); - mcu_temp_samples[ADC_FILTER_SIZE] = + (ADC_FILTER_SIZE - 1) * sizeof(float)); + mcu_temp_samples[ADC_FILTER_SIZE - 1] = (1.43f - adc_value[0] * ADC_SCALE) * TEMP_FACTOR + 35.0f; } @@ -234,9 +248,9 @@ void sampler::update_mcu_temp() { */ void sampler::update_drv_temp() { memmove(drv_temp_samples, drv_temp_samples + 1, - ADC_FILTER_SIZE * sizeof(float)); + (ADC_FILTER_SIZE - 1) * sizeof(float)); float R_now = (float)adc_value[1] / (4096 - (float)adc_value[1]) * 4700; - drv_temp_samples[ADC_FILTER_SIZE] = + drv_temp_samples[ADC_FILTER_SIZE - 1] = 1.0f / (0.00335f + 0.00025f * log(R_now / 10000.0f)) - 273.15f; } @@ -244,27 +258,41 @@ void sampler::update_drv_temp() { * @brief 更新相电流 */ void sampler::update_current() { - u8 idx = current_sample_index; - extern FOC_Controller FOC; - // 优化计算 - const float scale = ADC_SCALE * CURRENT_FACTOR; - const float offset = CURRENT_OFFSET * CURRENT_FACTOR; - - currU_samples[idx] = adc_value[2] * scale + offset; - currV_samples[idx] = adc_value[3] * scale + offset; - currW_samples[idx] = adc_value[4] * scale + offset; - - // 更新索引 - current_sample_index++; - if (current_sample_index >= ADC_FILTER_SIZE - 1) { - current_sample_index = 0; + 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() { - memmove(vbus_samples, vbus_samples + 1, ADC_FILTER_SIZE * sizeof(float)); - vbus_samples[ADC_FILTER_SIZE] = adc_value[5] * ADC_SCALE * VCC_FACTOR; -} \ No newline at end of file + memmove(vbus_samples, vbus_samples + 1, (ADC_FILTER_SIZE - 1) * sizeof(float)); + vbus_samples[ADC_FILTER_SIZE - 1] = adc_value[2] * ADC_SCALE * VCC_FACTOR; +} + +extern "C" void ADC0_1_IRQHandler(void) { + if (adc_interrupt_flag_get(ADC0, ADC_INT_FLAG_EOIC) != RESET) { + /* 丢弃 rank0 dummy，真实样本使用 rank1/2/3，对应 U/V/W 同一 PWM 时刻 */ + (void)adc_inserted_data_read(ADC0, ADC_INSERTED_CHANNEL_0); + adc_current_value[0] = adc_inserted_data_read(ADC0, ADC_INSERTED_CHANNEL_1); + adc_current_value[1] = adc_inserted_data_read(ADC0, ADC_INSERTED_CHANNEL_2); + adc_current_value[2] = 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); + } +} diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp index c5377d5..1366d24 100644 --- a/Robot_V2/device/Timer.cpp +++ b/Robot_V2/device/Timer.cpp @@ -10,7 +10,7 @@ #include "initial.h" volatile u32 total_us100_counter = 0; // 总计数器，每 100 微秒增加 1 -extern u16 adc_value[7]; +extern u16 adc_value[3]; extern FOC_Controller FOC; extern TaskScheduler scheduler; extern CAN_transmiter CAN0_transmiter; @@ -59,6 +59,7 @@ void nvic_config(void) { nvic_priority_group_set(NVIC_PRIGROUP_PRE2_SUB2); /* 使能 TIMER2 中断，设置抢占优先级为 1，子优先级为 1 */ nvic_irq_enable(TIMER2_IRQn, 0, 1); + nvic_irq_enable(ADC0_1_IRQn, 0, 0); nvic_irq_enable(TIMER1_IRQn, 0, 2); } @@ -216,7 +217,8 @@ void timer1_pwm_config(void) { // 配置 TIMER1 基本参数 timer_struct_para_init(&timer_initpara); timer_initpara.prescaler = TIMER1_PRESCALER; // 预分频器 - timer_initpara.counterdirection = TIMER_COUNTER_UP; // 向上计数 + timer_initpara.alignedmode = TIMER_COUNTER_CENTER_BOTH; // 中心对齐，给采样留对称平台窗 + timer_initpara.counterdirection = TIMER_COUNTER_UP; timer_initpara.period = TIMER1_PERIOD; // 计数周期 timer_initpara.clockdivision = TIMER_CKDIV_DIV1; timer_init(TIMER1, &timer_initpara); @@ -231,9 +233,9 @@ void timer1_pwm_config(void) { timer_channel_output_config(TIMER1, TIMER_CH_1, &timer_ocinitpara); timer_channel_output_config(TIMER1, TIMER_CH_2, &timer_ocinitpara); - // 配置第四个通道 (CH3) 用于在 PWM 周期 x% 位置触发中断 + // 配置第四个通道 (CH3) 为固定采样点，只用于导出 ADC 触发 timer_ocinitpara.outputstate = - TIMER_CCX_DISABLE; // 不输出 PWM 波形，仅用于触发中断 + TIMER_CCX_DISABLE; // 不输出 PWM 波形，仅用于比较事件 timer_ocinitpara.ocpolarity = TIMER_OC_POLARITY_HIGH; timer_channel_output_config(TIMER1, TIMER_CH_3, &timer_ocinitpara); @@ -244,13 +246,16 @@ void timer1_pwm_config(void) { timer_channel_output_mode_config(TIMER1, TIMER_CH_3, TIMER_OC_MODE_PWM0); // CH3 也设为 PWM 模式 + /* 将 CH3 预装载比较点作为 TRGO 导出，供 ADC injected 组同步采样 */ + timer_master_output_trigger_source_select(TIMER1, TIMER_TRI_OUT_SRC_O3CPRE); + timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_0, TIMER1_DEFAULT_PULSE); timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_1, TIMER1_DEFAULT_PULSE); timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_2, TIMER1_DEFAULT_PULSE); - // 设置 CH3 比较值为 PWM 周期 x% 位置 + // 设置 CH3 为固定 ADC 采样点，避免与三相 PWM duty 耦合 timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_3, TIMER1_CH3_COMPARE_VALUE); @@ -258,26 +263,18 @@ void timer1_pwm_config(void) { timer_primary_output_config(TIMER1, ENABLE); timer_auto_reload_shadow_enable(TIMER1); - // 使能 CH3 比较中断 - timer_interrupt_enable(TIMER1, TIMER_INT_CH3); - // 启动 TIMER1 timer_enable(TIMER1); } -// 添加 TIMER1-PWM 中断处理函数 +// CH3 现在仅作为 TRGO 采样点保留；采样完成由 ADC injected 中断接管 /** * @brief TIMER1 中断处理函数 * @return void */ void TIMER1_IRQHandler(void) { - // 检查 CH3 比较中断标志并处理 if (timer_interrupt_flag_get(TIMER1, TIMER_INT_FLAG_CH3) != RESET) { - // 清除 CH3 比较中断标志 timer_interrupt_flag_clear(TIMER1, TIMER_INT_FLAG_CH3); - - scheduler.taskFlags |= (1 << TASK_CURRENT_UPDATE); - // 例如触发 ADC 采样或其他操作 } } @@ -313,4 +310,4 @@ void Set_3Pwm(float duty_cycle0, float duty_cycle1, float duty_cycle2) { TIMER_CH0CV(TIMER1) = (uint32_t)(duty_cycle0 * TIMER1_PERIOD); TIMER_CH1CV(TIMER1) = (uint32_t)(duty_cycle1 * TIMER1_PERIOD); TIMER_CH2CV(TIMER1) = (uint32_t)(duty_cycle2 * TIMER1_PERIOD); -} \ No newline at end of file +} diff --git a/Robot_V2/device/adc.h b/Robot_V2/device/adc.h index 3283034..b69af96 100644 --- a/Robot_V2/device/adc.h +++ b/Robot_V2/device/adc.h @@ -9,13 +9,13 @@ extern "C" { class sampler { private: - float vbus_samples[ADC_FILTER_SIZE + 1]; - float currU_samples[ADC_FILTER_SIZE + 1]; - float currV_samples[ADC_FILTER_SIZE + 1]; - float currW_samples[ADC_FILTER_SIZE + 1]; - float mcu_temp_samples[ADC_FILTER_SIZE + 1]; - float drv_temp_samples[ADC_FILTER_SIZE + 1]; - u8 current_sample_index; + float vbus_samples[ADC_FILTER_SIZE]; + float currU_samples[ADC_FILTER_SIZE]; + float currV_samples[ADC_FILTER_SIZE]; + float currW_samples[ADC_FILTER_SIZE]; + float mcu_temp_samples[ADC_FILTER_SIZE]; + float drv_temp_samples[ADC_FILTER_SIZE]; + u8 current_sample_head; float tempora_sample; @@ -32,6 +32,8 @@ class sampler { float get_current(char); }; +void ADC0_1_IRQHandler(void); + #ifdef __cplusplus } #endif