207 lines
7.4 KiB
C
207 lines
7.4 KiB
C
|
/*
|
|||
|
|
* @Date: 2025-07-18 09:24:34
|
|||
|
|
* @Author: mypx
|
|||
|
|
* @LastEditors: mypx mypx_coder@163.com
|
|||
|
|
* @LastEditTime: 2025-09-26 08:58:26
|
|||
|
|
* @FilePath: data_process.c
|
|||
|
|
* @Description:
|
|||
|
|
* Copyright (c) 2025 by mypx, All Rights Reserved.
|
|||
|
|
*/
|
|||
|
|
#include "data_process.h"
|
|||
|
|
#include "data_sampling.h"
|
|||
|
|
#include "et_log.h"
|
|||
|
|
#include "pm_common.h"
|
|||
|
|
#include "pm_meas.h"
|
|||
|
|
#include "servo.h"
|
|||
|
|
#include <rtthread.h>
|
|||
|
|
#include <stdio.h>
|
|||
|
|
#include <string.h>
|
|||
|
|
|
|||
|
|
#if (ENABLE_IIR_FILTER == 1)
|
|||
|
|
#include "et_iir_filter.h"
|
|||
|
|
et_iir_filter_t iir_filter;
|
|||
|
|
|
|||
|
|
/* 这些系数使用巴特沃斯设计，形式为：
|
|||
|
|
H(z) = (b0 + b1*z^-1 + b2*z^-2) / (1 + a1*z^-1 + a2*z^-2)
|
|||
|
|
注意：a_coeffs[0] = 1.0 (通常为1) */
|
|||
|
|
float a_coeff[] = {1.000000000f, -0.855625000f, 0.212125000f}; // 分母系数 (a0=1, a1, a2)
|
|||
|
|
float b_coeff[] = {0.089125000f, 0.178250000f, 0.089125000f}; // 分子系数 (b0, b1, b2)
|
|||
|
|
// 对于1个二阶阶段，order=2，state_size=3
|
|||
|
|
#define FILTER_STAGES 1 // 二阶节数量 (每个二阶节贡献2阶)
|
|||
|
|
#define FILTER_ORDER (2 * FILTER_STAGES) // 实际滤波器阶数
|
|||
|
|
#define STATE_SIZE (FILTER_ORDER + 1) // 状态缓冲区大小
|
|||
|
|
// 状态缓冲区大小说明：
|
|||
|
|
// - 对于N阶滤波器，需要N+1个状态元素存储历史输入和输出
|
|||
|
|
// - 例如：2阶滤波器需要3个元素存储x[n], x[n-1], x[n-2]
|
|||
|
|
float in_state[STATE_SIZE] = {0};
|
|||
|
|
float out_state[STATE_SIZE] = {0};
|
|||
|
|
et_iir_filter_t filter;
|
|||
|
|
et_iir_config_t config;
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
static struct rt_thread data_process_thread;
|
|||
|
|
ALIGN(RT_ALIGN_SIZE)
|
|||
|
|
static rt_uint8_t data_process_stack[DATA_PROCESS_STACK_SIZE];
|
|||
|
|
float adc_tmp_buffer[DATA_PROCESS_UNIT_COUNT] = {0};
|
|||
|
|
float adc_filter_buffer[DATA_PROCESS_UNIT_COUNT] = {0};
|
|||
|
|
|
|||
|
|
#define ANALYSIS_SIZE DATA_PROCESS_UNIT_COUNT
|
|||
|
|
|
|||
|
|
static bool cali_flag = false;
|
|||
|
|
static bool rdy_flag = false;
|
|||
|
|
|
|||
|
|
pm_meas_t pm_meas;
|
|||
|
|
|
|||
|
|
static double trend_buffer[TREND_BUFFER_SIZE] = {0.0};
|
|||
|
|
|
|||
|
|
void set_pm_stage(int argc, char **argv)
|
|||
|
|
{
|
|||
|
|
if (argc != 2)
|
|||
|
|
{
|
|||
|
|
rt_kprintf("Usage: my_cmd <int_value>\n");
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int stage = atoi(argv[1]);
|
|||
|
|
pm_meas.state = stage;
|
|||
|
|
}
|
|||
|
|
MSH_CMD_EXPORT_ALIAS(set_pm_stage, sst, set state);
|
|||
|
|
uint32_t data_count = 0;
|
|||
|
|
|
|||
|
|
// 只考虑校准和测量状态，其他状态由采集线程 或者主线程管理
|
|||
|
|
void data_process_thread_entry(void *parameter)
|
|||
|
|
{
|
|||
|
|
(void)parameter;
|
|||
|
|
rt_err_t result = RT_EOK;
|
|||
|
|
int find_status = 0;
|
|||
|
|
uint16_t *raw_buf;
|
|||
|
|
uint16_t *used_buf;
|
|||
|
|
|
|||
|
|
while (1)
|
|||
|
|
{
|
|||
|
|
result = rt_mq_recv(&adc_mq, &raw_buf, sizeof(adc_raw_buffer), RT_WAITING_FOREVER);
|
|||
|
|
if (result == RT_EOK)
|
|||
|
|
{
|
|||
|
|
data_sampling_stop();
|
|||
|
|
if (get_find_zero_state(&pm_meas) == ZF_FAST_CLOSE_STAGE && fsm_state == false && pm_meas.p50 > 0.1f)
|
|||
|
|
sv630p_speed_mode_stop(&sv630p_handle);
|
|||
|
|
#if (ENABLE_IIR_FILTER == 1)
|
|||
|
|
if (et_iir_filter_process(&iir_filter, (const void *)raw_buf, (void *)adc_filter_buffer) == IIR_OK)
|
|||
|
|
{
|
|||
|
|
used_buf = adc_filter_buffer;
|
|||
|
|
}
|
|||
|
|
#else
|
|||
|
|
used_buf = raw_buf;
|
|||
|
|
#endif
|
|||
|
|
#if (SAMPLING_RAW_DATA == 1)
|
|||
|
|
for (int i = 0; i < DATA_PROCESS_UNIT_COUNT; i++)
|
|||
|
|
{
|
|||
|
|
ET_DBG("r:%d, f:%d", raw_buf[i], (uint16_t)adc_filter_buffer[i]);
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
#if (SAMPLING_RAW_DATA == 0)
|
|||
|
|
pm_error_led_on(); // for timing
|
|||
|
|
find_status = find_zero_process(&pm_meas, (const uint16_t *)used_buf, DATA_PROCESS_UNIT_COUNT);
|
|||
|
|
pm_error_led_off();
|
|||
|
|
#endif
|
|||
|
|
if (find_status == 0)
|
|||
|
|
{
|
|||
|
|
data_sampling_start();
|
|||
|
|
if (get_find_zero_state(&pm_meas) == ZF_FAST_CLOSE_STAGE && fsm_state == false && pm_meas.p50 > 0.1f)
|
|||
|
|
sv630p_speed_mode_resume(&sv630p_handle);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
ET_ERR("Something wrong");
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int data_process_init(void)
|
|||
|
|
{
|
|||
|
|
rt_err_t result;
|
|||
|
|
|
|||
|
|
// 初始化 pm_meas 结构体
|
|||
|
|
memset(&pm_meas, 0, sizeof(pm_meas_t));
|
|||
|
|
pm_meas.target_dir = MEASURE_DEF_DIR;
|
|||
|
|
|
|||
|
|
//etk_ringbuffer_init(&pm_meas.rb, trend_buffer, TREND_BUFFER_SIZE, ETK_RB_DOUBLE);
|
|||
|
|
trend_handle_init(&pm_meas.trend_handle);
|
|||
|
|
etk_slope_init(&pm_meas.slope_handle, trend_buffer, TREND_BUFFER_SIZE, ETK_RB_DOUBLE);
|
|||
|
|
// 确保事件初始化时名称正确
|
|||
|
|
result = rt_event_init(&pm_event, "pm_evt", RT_IPC_FLAG_FIFO);
|
|||
|
|
if (result != RT_EOK)
|
|||
|
|
{
|
|||
|
|
rt_kprintf("Event init failed: 0x%04X\n", result);
|
|||
|
|
return -1;
|
|||
|
|
}
|
|||
|
|
//valley_seek_init(&pm_meas.valley_seek);
|
|||
|
|
working_set_state(PM_MEAS_TEST_Mode);
|
|||
|
|
if (et_zero_cross_create(&pm_meas.zc, 100) != 0)
|
|||
|
|
{
|
|||
|
|
ET_ERR("zc create failed\n");
|
|||
|
|
return -1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int bin_center = et_goertzel_calc_bin(50, sampling_rate, ANALYSIS_SIZE);
|
|||
|
|
if (bin_center < 0)
|
|||
|
|
{
|
|||
|
|
ET_ERR("bin calc failed: %d\n", bin_center);
|
|||
|
|
return -1;
|
|||
|
|
}
|
|||
|
|
if (et_goertzel_init(&pm_meas.cfg, ANALYSIS_SIZE, bin_center, DC_OFFSET_ADC_VALUE) < 0)
|
|||
|
|
{
|
|||
|
|
ET_ERR("goertzel init failed\n");
|
|||
|
|
return -1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#if (ENABLE_IIR_FILTER == 1)
|
|||
|
|
config.direct_form_ii_f32.a_coeffs = a_coeff;
|
|||
|
|
config.direct_form_ii_f32.b_coeffs = b_coeff;
|
|||
|
|
config.direct_form_ii_f32.in_state = in_state;
|
|||
|
|
config.direct_form_ii_f32.out_state = out_state;
|
|||
|
|
et_iir_scale_t scale = IIR_SCALE_F32_IDENTITY;
|
|||
|
|
|
|||
|
|
ET_IIR_Result iir_ret = et_iir_filter_init(&iir_filter, "band-pass Filter", ARM_IIR_LATTICE_F32,
|
|||
|
|
FILTER_STAGES, // 二阶节阶段数 (注意：不是滤波器总阶数)
|
|||
|
|
1, // 块大小为1
|
|||
|
|
&scale, &config, IIR_SCALING_DISABLED);
|
|||
|
|
|
|||
|
|
if (iir_ret != IIR_OK)
|
|||
|
|
{
|
|||
|
|
ET_ERR("IIR filter init failed: %d\n", iir_ret);
|
|||
|
|
return -1;
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
etk_position_pid_init(&pm_meas.pid,
|
|||
|
|
1.8f, // kp: 提高比例增益，加快响应速度
|
|||
|
|
1.8f, // ki: 增加积分增益，加快接近目标时的收敛速度
|
|||
|
|
0.25f, // kd: 进一步降低微分作用，减少阻尼
|
|||
|
|
PID_TARGET_ZERO_POS_VAL, // 目标值设置为0.6
|
|||
|
|
2.5f, // 积分限幅，允许更多积分累积加快收敛
|
|||
|
|
-FZ_FAST_CLOSE_STAGE_RPM, // 最小输出(限制最大反转速度)
|
|||
|
|
FZ_FAST_CLOSE_STAGE_RPM, -1);
|
|||
|
|
result = rt_thread_init(&data_process_thread, // 线程控制块指针
|
|||
|
|
"d-process", // 线程名称
|
|||
|
|
data_process_thread_entry, // 入口函数
|
|||
|
|
RT_NULL, // 入口参数
|
|||
|
|
data_process_stack, // 栈起始地址
|
|||
|
|
sizeof(data_process_stack), // 栈大小
|
|||
|
|
DATA_PROCESS_PRIORITY, // 优先级（0-255）
|
|||
|
|
10); // 时间片
|
|||
|
|
|
|||
|
|
if (result == RT_EOK)
|
|||
|
|
{
|
|||
|
|
rt_kprintf("d-process thread initialized successfully\n");
|
|||
|
|
// 启动线程
|
|||
|
|
rt_thread_startup(&data_process_thread);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
rt_kprintf("Thread init failed: 0x%04X\n", result);
|
|||
|
|
}
|
|||
|
|
return 0;
|
|||
|
|
}
|