559 lines
20 KiB
C
559 lines
20 KiB
C
|
/*
|
|||
|
|
* @Author: mypx
|
|||
|
|
* @Date: 2025-08-11 08:30:45
|
|||
|
|
* @LastEditTime: 2025-09-24 16:12:52
|
|||
|
|
* @LastEditors: mypx mypx_coder@163.com
|
|||
|
|
* @Description:
|
|||
|
|
*/
|
|||
|
|
#include "pm_meas.h"
|
|||
|
|
#include "data_process.h"
|
|||
|
|
#include "data_sampling.h"
|
|||
|
|
#include "et_waveform_analysis.h"
|
|||
|
|
#include "etk_diff.h"
|
|||
|
|
#include "etk_ringbuffer.h"
|
|||
|
|
#include "servo.h"
|
|||
|
|
#include <float.h>
|
|||
|
|
#include <rtthread.h>
|
|||
|
|
#include <stdio.h>
|
|||
|
|
|
|||
|
|
|
|||
|
|
static pm_fsm_t *pm_fsm = NULL;
|
|||
|
|
|
|||
|
|
pm_fsm_t *get_pm_fsm(void)
|
|||
|
|
{
|
|||
|
|
return pm_fsm;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void fsm_state_toggle(void)
|
|||
|
|
{
|
|||
|
|
pm_fsm_t *fsm = get_pm_fsm();
|
|||
|
|
if (!fsm)
|
|||
|
|
return;
|
|||
|
|
fsm->fz_fsm_state = !fsm->fz_fsm_state;
|
|||
|
|
}
|
|||
|
|
MSH_CMD_EXPORT_ALIAS(fsm_state_toggle, fsm, calculate state toggle);
|
|||
|
|
|
|||
|
|
void calibrate_mode_toggle(void)
|
|||
|
|
{
|
|||
|
|
pm_fsm_t *fsm = get_pm_fsm();
|
|||
|
|
if (!fsm)
|
|||
|
|
return;
|
|||
|
|
fsm->fz_fsm_state = false;
|
|||
|
|
fsm->meas_mode = fsm->meas_mode == PM_MEAS_NORMAL_Mode ? PM_MEAS_TEST_Mode : PM_MEAS_NORMAL_Mode;
|
|||
|
|
}
|
|||
|
|
MSH_CMD_EXPORT_ALIAS(calibrate_mode_toggle, cali, calibrate mode toggle);
|
|||
|
|
|
|||
|
|
void trend_handle_init(trend_handle_t *handle)
|
|||
|
|
{
|
|||
|
|
handle->last_val = NAN;
|
|||
|
|
handle->inc_cnt = 0;
|
|||
|
|
handle->dec_cnt = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
TrendStatus get_data_trend(trend_handle_t *handle, double new_val)
|
|||
|
|
{
|
|||
|
|
const double EPS = 1e-6; // 浮点数比较精度（避免误差）
|
|||
|
|
|
|||
|
|
// 1. 首次调用：无历史数据，返回“未知”并初始化上次值
|
|||
|
|
if (isnan(handle->last_val))
|
|||
|
|
{
|
|||
|
|
handle->last_val = new_val;
|
|||
|
|
return TREND_UNKNOWN;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 2. 比较本次与上次数据，更新计数
|
|||
|
|
if (new_val > handle->last_val + EPS)
|
|||
|
|
{
|
|||
|
|
// 数据上升
|
|||
|
|
handle->inc_cnt++;
|
|||
|
|
handle->dec_cnt = 0;
|
|||
|
|
}
|
|||
|
|
else if (new_val < handle->last_val - EPS)
|
|||
|
|
{
|
|||
|
|
// 数据下降
|
|||
|
|
handle->dec_cnt++;
|
|||
|
|
handle->inc_cnt = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
handle->last_val = new_val;
|
|||
|
|
// 3. 判断是否达到“连续6次”的触发阈值（原逻辑：cnt > 5）
|
|||
|
|
if (handle->inc_cnt > TREND_MAX_CNT)
|
|||
|
|
{
|
|||
|
|
//inc_cnt = 0; // 触发后清零，重新开始计数
|
|||
|
|
return TREND_INCREASE;
|
|||
|
|
}
|
|||
|
|
if (handle->dec_cnt > TREND_MAX_CNT)
|
|||
|
|
{
|
|||
|
|
//dec_cnt = 0; // 触发后清零，重新开始计数
|
|||
|
|
return TREND_DECREASE;
|
|||
|
|
}
|
|||
|
|
return TREND_STABLE;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void trend_handle_reset(trend_handle_t *handle)
|
|||
|
|
{
|
|||
|
|
handle->last_val = NAN;
|
|||
|
|
handle->inc_cnt = 0;
|
|||
|
|
handle->dec_cnt = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void measurement_info_print(pm_meas_t *fz)
|
|||
|
|
{
|
|||
|
|
ET_WARN("------------------------Calibration Info Start----------------------------");
|
|||
|
|
ET_INFO("Find zero time: %lu ms", fz->calib_info[0].end_time - fz->calib_info[0].start_time);
|
|||
|
|
float min_angle = FLT_MAX;
|
|||
|
|
float max_angle = -FLT_MAX;
|
|||
|
|
|
|||
|
|
for (int i = 0; i < fz->calib_cnt; i++)
|
|||
|
|
{
|
|||
|
|
#if (ENABLE_CALIB_DOUBLE_DIR == 1)
|
|||
|
|
if (fz->calib_info[i].m_dir == -1)
|
|||
|
|
fz->calib_info[i].cali_angle = fz->calib_info[i].angle - CALIB_OFFSET;
|
|||
|
|
else
|
|||
|
|
#endif
|
|||
|
|
fz->calib_info[i].cali_angle = fz->calib_info[i].angle;
|
|||
|
|
if (fz->calib_info[i].cali_angle < min_angle)
|
|||
|
|
{
|
|||
|
|
min_angle = fz->calib_info[i].cali_angle;
|
|||
|
|
}
|
|||
|
|
if (fz->calib_info[i].cali_angle > max_angle)
|
|||
|
|
{
|
|||
|
|
max_angle = fz->calib_info[i].cali_angle;
|
|||
|
|
}
|
|||
|
|
// ET_INFO("[%d]: Used time:%d ms, start:%.3f, end zero pos:%.3f, cali_angle:%.2f, power:%.3f", i,
|
|||
|
|
// fz->calib_info[i].end_time - fz->calib_info[i].start_time, fz->calib_info[i].target_angle,
|
|||
|
|
// fz->calib_info[i].angle, fz->calib_info[i].cali_angle, fz->calib_info[i].power);
|
|||
|
|
ET_INFO("[%d]: Used time:%d ms, start:%.3f, end zero pos:%.3f, power:%.3f", i,
|
|||
|
|
fz->calib_info[i].end_time - fz->calib_info[i].start_time, fz->calib_info[i].target_angle,
|
|||
|
|
fz->calib_info[i].angle, fz->calib_info[i].power);
|
|||
|
|
}
|
|||
|
|
ET_INFO("Final min_angle: %.3f, max_angle: %.3f", min_angle, max_angle);
|
|||
|
|
if (min_angle != FLT_MAX && max_angle != FLT_MIN)
|
|||
|
|
{
|
|||
|
|
float accuracy = (max_angle - min_angle) / ENCODER_TO_POLARIZER_RATIO;
|
|||
|
|
ET_INFO("Accuracy: %.3f (max_angle-min_angle=%.3f, ratio=%d)", accuracy, max_angle - min_angle,
|
|||
|
|
ENCODER_TO_POLARIZER_RATIO);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
ET_WARN("Accuracy not calculated: min_angle=%.3f, max_angle=%.3f", min_angle, max_angle);
|
|||
|
|
}
|
|||
|
|
ET_WARN("------------------------Calibration Info End----------------------------");
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void accuracy_auto_measurement_once(pm_meas_t *fz, float angle)
|
|||
|
|
{
|
|||
|
|
ET_INFO("Start to calibrate again:%d", fz->calib_cnt);
|
|||
|
|
|
|||
|
|
#if (CALIB_TO_FIX_STAGE == 1)
|
|||
|
|
float diff = angle - fz->stage_info[ZF_PREPARE_ENTER_STAGE].angle;
|
|||
|
|
#else
|
|||
|
|
float diff = CALIB_OSCI_ANGLE;
|
|||
|
|
#endif
|
|||
|
|
ET_INFO("diff:%.2f", diff);
|
|||
|
|
float target_angle = 0.0f;
|
|||
|
|
#if (ENABLE_CALIB_DOUBLE_DIR == 1)
|
|||
|
|
if (fz->calib_cnt % 2 == 0)
|
|||
|
|
{
|
|||
|
|
target_angle = angle - diff;
|
|||
|
|
ET_INFO("Odd times, turn back");
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
target_angle = angle + diff;
|
|||
|
|
ET_INFO("Even times, turn forward");
|
|||
|
|
}
|
|||
|
|
#else
|
|||
|
|
target_angle = angle + diff * CALIB_ONE_DIR;
|
|||
|
|
#endif
|
|||
|
|
uint32_t delay = convert_encoder_angle_rpm_to_to_time(diff, RUN_OFFSET_ANGLE_RPM);
|
|||
|
|
ET_DBG("wait %d ms to target:%.2f", delay, target_angle);
|
|||
|
|
int speed = (target_angle > angle) ? -RUN_OFFSET_ANGLE_RPM
|
|||
|
|
: RUN_OFFSET_ANGLE_RPM; // FIXME:note encoder dir and motor dir
|
|||
|
|
sv630p_speed_mode_start(&sv630p_handle, speed);
|
|||
|
|
fz->target_dir = (speed > 0) ? -1 : 1;
|
|||
|
|
ET_WARN("SPEED:%d", speed);
|
|||
|
|
while (1)
|
|||
|
|
{
|
|||
|
|
angle = get_real_time_angle_degree();
|
|||
|
|
//ET_DBG("angle:%.2f", angle);
|
|||
|
|
// 根据方向判断是否达到目标角度，处理负数角度情况
|
|||
|
|
if (((speed > 0) && (target_angle > angle)) || ((speed < 0) && (target_angle < angle)))
|
|||
|
|
{
|
|||
|
|
sv630p_speed_mode_stop(&sv630p_handle);
|
|||
|
|
ET_INFO("Set angle:%.3f, reached:%.3f", target_angle, angle);
|
|||
|
|
#if (CALIB_TO_FIX_STAGE == 1)
|
|||
|
|
fz->state = ZF_PREPARE_ENTER_STAGE;
|
|||
|
|
ET_WARN("Back to ZF_PREPARE_ENTER_STAGE state");
|
|||
|
|
#else
|
|||
|
|
fz->state = ZF_FAST_CLOSE_STAGE;
|
|||
|
|
ET_WARN("Back to ZF_FAST_CLOSE_STAGE state");
|
|||
|
|
#endif
|
|||
|
|
uint8_t index = fz->calib_cnt % FZ_CALI_MAX_COUNT;
|
|||
|
|
fz->calib_info[index].target_angle = target_angle;
|
|||
|
|
ET_INFO("update start calib index:%d", index);
|
|||
|
|
fz->calib_info[index].start_time = rt_tick_get_millisecond();
|
|||
|
|
//fz->target_dir *= -1; // reverse dir
|
|||
|
|
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 饱和高噪声状态判断，不会误判为其他状态，任何情形都无需从这个状态反转
|
|||
|
|
StageResult is_saturate_starting(pm_meas_t *fz)
|
|||
|
|
{
|
|||
|
|
if (fabs(fz->slope) < FZ_SATURATED_SLOPE_MAX && fz->p50 < FZ_SATURATED_POWER_MAX && (fz->freq < 10))
|
|||
|
|
{
|
|||
|
|
//fz->state = ZF_SATURATED_STAGE;
|
|||
|
|
return STAGE_RESULT_OK;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
return STAGE_RESULT_NO;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int goto_saturated_stage(pm_meas_t *fz)
|
|||
|
|
{
|
|||
|
|
fz->state = ZF_SATURATED_STAGE;
|
|||
|
|
ET_WARN("power state: ZF_SATURATED_STAGE");
|
|||
|
|
return sv630p_speed_update(&sv630p_handle, FZ_SATURATED_STAGE_RPM * fz->target_dir);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
StageResult is_wave_enter_stage(pm_meas_t *fz)
|
|||
|
|
{
|
|||
|
|
if (fz->slope > 100000)
|
|||
|
|
return STAGE_RESULT_OK;
|
|||
|
|
else
|
|||
|
|
return STAGE_RESULT_NO;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
StageResult is_prepare_enter_stage(pm_meas_t *fz)
|
|||
|
|
{
|
|||
|
|
if (fz->p50 > 100000 && fz->trend == TREND_DECREASE && fz->state != ZF_WAVE_LEAVE_STAGE)
|
|||
|
|
return STAGE_RESULT_OK;
|
|||
|
|
else
|
|||
|
|
return STAGE_RESULT_NO;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int goto_prepare_enter_stage(pm_meas_t *fz, double p50, double slope, float freq, double angle)
|
|||
|
|
{
|
|||
|
|
fz->state = ZF_PREPARE_ENTER_STAGE;
|
|||
|
|
if (fz->stage_info[ZF_PREPARE_ENTER_STAGE].is_updated == 0)
|
|||
|
|
{
|
|||
|
|
fz->stage_info[ZF_PREPARE_ENTER_STAGE].angle = angle;
|
|||
|
|
fz->stage_info[ZF_PREPARE_ENTER_STAGE].time = rt_tick_get_millisecond();
|
|||
|
|
fz->stage_info[ZF_PREPARE_ENTER_STAGE].p50 = p50;
|
|||
|
|
fz->stage_info[ZF_PREPARE_ENTER_STAGE].slope = slope;
|
|||
|
|
fz->stage_info[ZF_PREPARE_ENTER_STAGE].freq = freq;
|
|||
|
|
fz->stage_info[ZF_PREPARE_ENTER_STAGE].is_updated = 1;
|
|||
|
|
}
|
|||
|
|
ET_WARN("enter ZF_PREPARE_ENTER_STAGE angle:%.3f", angle);
|
|||
|
|
return sv630p_speed_update(&sv630p_handle, FZ_PREPARE_STAGE_RPM * fz->target_dir);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int goto_prepare_leave_stage(pm_meas_t *fz, double angle)
|
|||
|
|
{
|
|||
|
|
fz->target_dir = fz->target_dir * -1;
|
|||
|
|
if (fz->stage_info[ZF_PREPARE_LEAVE_STAGE].is_updated == 0)
|
|||
|
|
{
|
|||
|
|
fz->stage_info[ZF_PREPARE_LEAVE_STAGE].is_updated = 1;
|
|||
|
|
fz->stage_info[ZF_PREPARE_LEAVE_STAGE].angle = angle;
|
|||
|
|
fz->stage_info[ZF_PREPARE_LEAVE_STAGE].time = rt_tick_get_millisecond();
|
|||
|
|
fz->stage_info[ZF_PREPARE_LEAVE_STAGE].p50 = fz->p50;
|
|||
|
|
fz->stage_info[ZF_PREPARE_LEAVE_STAGE].slope = fz->slope;
|
|||
|
|
fz->stage_info[ZF_PREPARE_LEAVE_STAGE].freq = fz->freq;
|
|||
|
|
}
|
|||
|
|
ET_WARN("ZF_PREPARE_LEAVE_STAGE, angle:%.3f", angle);
|
|||
|
|
return sv630p_speed_update(&sv630p_handle, FZ_PREPARE_STAGE_RPM * fz->target_dir);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
StageResult is_fast_close_enter(pm_meas_t *fz)
|
|||
|
|
{
|
|||
|
|
if (fz->p50 <= 100000 && fz->trend == TREND_DECREASE && fz->state == ZF_PREPARE_ENTER_STAGE)
|
|||
|
|
return STAGE_RESULT_OK;
|
|||
|
|
else
|
|||
|
|
return STAGE_RESULT_NO;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int goto_fast_close_stage(pm_meas_t *fz, double p50, double slope, float freq, double angle)
|
|||
|
|
{
|
|||
|
|
fz->state = ZF_FAST_CLOSE_STAGE;
|
|||
|
|
if (fz->stage_info[ZF_FAST_CLOSE_STAGE].is_updated == 0)
|
|||
|
|
{
|
|||
|
|
fz->stage_info[ZF_FAST_CLOSE_STAGE].is_updated = 1;
|
|||
|
|
fz->stage_info[ZF_FAST_CLOSE_STAGE].angle = angle;
|
|||
|
|
fz->stage_info[ZF_FAST_CLOSE_STAGE].time = rt_tick_get_millisecond();
|
|||
|
|
fz->stage_info[ZF_FAST_CLOSE_STAGE].p50 = p50;
|
|||
|
|
fz->stage_info[ZF_FAST_CLOSE_STAGE].slope = slope;
|
|||
|
|
fz->stage_info[ZF_FAST_CLOSE_STAGE].freq = freq;
|
|||
|
|
}
|
|||
|
|
ET_WARN("FAST_CLOSE, angle:%.3f\r\n", angle);
|
|||
|
|
return sv630p_speed_update(&sv630p_handle, FZ_FAST_CLOSE_STAGE_RPM * fz->target_dir);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
StageResult is_zero_stage(pm_meas_t *fz)
|
|||
|
|
{
|
|||
|
|
if (fabs(fz->slope) < 0.5 && fz->p50 < ZERO_POS_THRESHOLD && (fz->freq >= 99.95 && fz->freq < 100.3f)
|
|||
|
|
&& fz->trend == TREND_STABLE)
|
|||
|
|
return STAGE_RESULT_OK;
|
|||
|
|
else
|
|||
|
|
return STAGE_RESULT_NO;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
StageResult is_fast_leave_stage(pm_meas_t *fz)
|
|||
|
|
{
|
|||
|
|
if ((fz->p50 <= 100000) && (fz->slope > 10000)
|
|||
|
|
&& (fz->freq > FZ_FAST_CLOSE_ENTER_FREQ_MIN && fz->freq < FZ_FAST_CLOSE_ENTER_FREQ_MAX)
|
|||
|
|
&& fz->state != ZF_SATURATED_STAGE)
|
|||
|
|
return STAGE_RESULT_OK;
|
|||
|
|
else
|
|||
|
|
return STAGE_RESULT_NO;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
StageResult is_prepare_leave_stage(pm_meas_t *fz)
|
|||
|
|
{
|
|||
|
|
if (fz->p50 >= 100000 && fz->trend == TREND_INCREASE
|
|||
|
|
&& (fz->state == ZF_FAST_CLOSE_STAGE || fz->state == ZF_ZERO_STAGE))
|
|||
|
|
return STAGE_RESULT_OK;
|
|||
|
|
else
|
|||
|
|
return STAGE_RESULT_NO;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
StageResult is_wave_leave_stage(pm_meas_t *fz)
|
|||
|
|
{
|
|||
|
|
if ((fz->slope < -100000) || (fz->freq > 52 && fz->freq < 98 && fz->p50 < 1000 && fz->trend == TREND_DECREASE)
|
|||
|
|
|| (fz->freq < 1.0 && fz->trend == TREND_DECREASE))
|
|||
|
|
return STAGE_RESULT_OK;
|
|||
|
|
else
|
|||
|
|
return STAGE_RESULT_NO;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
ZeroFindState get_find_zero_state(pm_meas_t *fz)
|
|||
|
|
{
|
|||
|
|
return fz->state;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void calculate_stage(pm_meas_t *fz)
|
|||
|
|
{
|
|||
|
|
if (is_saturate_starting(fz) == STAGE_RESULT_OK && fz->stage_info[ZF_SATURATED_STAGE].is_print == false)
|
|||
|
|
{
|
|||
|
|
ET_WARN("enter saturated");
|
|||
|
|
fz->stage_info[ZF_SATURATED_STAGE].is_print = true;
|
|||
|
|
fz->state = ZF_SATURATED_STAGE;
|
|||
|
|
}
|
|||
|
|
if (is_wave_enter_stage(fz) == STAGE_RESULT_OK && fz->stage_info[ZF_WAVE_ENTER_STAGE].is_print == false)
|
|||
|
|
{
|
|||
|
|
ET_WARN("enter 50hz started");
|
|||
|
|
fz->stage_info[ZF_WAVE_ENTER_STAGE].is_print = true;
|
|||
|
|
fz->stage_info[ZF_SATURATED_STAGE].is_print = false;
|
|||
|
|
fz->state = ZF_WAVE_ENTER_STAGE;
|
|||
|
|
}
|
|||
|
|
if (is_prepare_enter_stage(fz) == STAGE_RESULT_OK && fz->stage_info[ZF_PREPARE_ENTER_STAGE].is_print == false)
|
|||
|
|
{
|
|||
|
|
ET_WARN("enter prepare stage");
|
|||
|
|
fz->stage_info[ZF_PREPARE_ENTER_STAGE].is_print = true;
|
|||
|
|
fz->state = ZF_PREPARE_ENTER_STAGE;
|
|||
|
|
fz->stage_info[ZF_WAVE_ENTER_STAGE].is_print = false;
|
|||
|
|
}
|
|||
|
|
if (is_fast_close_enter(fz) == STAGE_RESULT_OK && fz->stage_info[ZF_FAST_CLOSE_STAGE].is_print == false)
|
|||
|
|
{
|
|||
|
|
ET_WARN("enter fast close stage, dir:%d, target dir :%d", sv630p_get_dir(&sv630p_handle), fz->target_dir);
|
|||
|
|
fz->stage_info[ZF_FAST_CLOSE_STAGE].is_print = true;
|
|||
|
|
fz->stage_info[ZF_PREPARE_ENTER_STAGE].is_print = false;
|
|||
|
|
fz->state = ZF_FAST_CLOSE_STAGE;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (is_zero_stage(fz) == STAGE_RESULT_OK && fz->stage_info[ZF_ZERO_STAGE].is_print == false)
|
|||
|
|
{
|
|||
|
|
ET_WARN("enter zero state");
|
|||
|
|
fz->stage_info[ZF_ZERO_STAGE].is_print = true;
|
|||
|
|
if (fz->state == ZF_UNKNOWN_STAGE)
|
|||
|
|
fz->state = ZF_ZERO_STAGE; // 开机判断
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
fz->stage_info[ZF_ZERO_STAGE].is_print = false;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (is_fast_leave_stage(fz) == STAGE_RESULT_OK && fz->stage_info[ZF_FAST_LEAVE_STAGE].is_print == false)
|
|||
|
|
{
|
|||
|
|
ET_WARN("fast leave stage");
|
|||
|
|
fz->stage_info[ZF_FAST_LEAVE_STAGE].is_print = true;
|
|||
|
|
//fz->stage_info[ZF_PRECISE_ADJUST_LEAVE_STAGE].is_print = false;
|
|||
|
|
fz->state = ZF_FAST_LEAVE_STAGE;
|
|||
|
|
}
|
|||
|
|
if (is_prepare_leave_stage(fz) == STAGE_RESULT_OK && fz->stage_info[ZF_PREPARE_LEAVE_STAGE].is_print == false)
|
|||
|
|
{
|
|||
|
|
ET_WARN("prepare leave stage");
|
|||
|
|
fz->stage_info[ZF_PREPARE_LEAVE_STAGE].is_print = true;
|
|||
|
|
fz->stage_info[ZF_FAST_LEAVE_STAGE].is_print = false;
|
|||
|
|
fz->stage_info[ZF_WAVE_LEAVE_STAGE].is_print = false;
|
|||
|
|
fz->state = ZF_PREPARE_LEAVE_STAGE;
|
|||
|
|
}
|
|||
|
|
if (is_wave_leave_stage(fz) == STAGE_RESULT_OK && fz->stage_info[ZF_WAVE_LEAVE_STAGE].is_print == false)
|
|||
|
|
{
|
|||
|
|
ET_WARN("small 50hz leave stage");
|
|||
|
|
fz->stage_info[ZF_WAVE_LEAVE_STAGE].is_print = true;
|
|||
|
|
fz->stage_info[ZF_PREPARE_LEAVE_STAGE].is_print = false;
|
|||
|
|
fz->state = ZF_WAVE_LEAVE_STAGE;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int find_zero_process(pm_meas_t *fz, const uint16_t *adc_buf, uint32_t len)
|
|||
|
|
{
|
|||
|
|
pm_fsm_t *fsm = get_pm_fsm();
|
|||
|
|
if (!fsm)
|
|||
|
|
{
|
|||
|
|
ET_ERR("fsm is not init");
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
double rpm = 0.0;
|
|||
|
|
//static uint8_t wait_time = 0;
|
|||
|
|
float angle;
|
|||
|
|
|
|||
|
|
fz->p50 = et_goertzel_compute_power_u16(&fz->cfg, adc_buf);
|
|||
|
|
fz->slope = etk_slope_update_slope(&fz->slope_handle, fz->p50);
|
|||
|
|
|
|||
|
|
// if (wait_time++ < BUFFER_SIZE)
|
|||
|
|
// return 0;
|
|||
|
|
angle = get_real_time_angle_degree();
|
|||
|
|
|
|||
|
|
fz->freq = et_zero_cross_freq_u16(&fz->zc, adc_buf, len, sampling_rate, DC_OFFSET_ADC_VALUE);
|
|||
|
|
fz->trend = get_data_trend(&fz->trend_handle, fz->p50);
|
|||
|
|
calculate_stage(fz);
|
|||
|
|
|
|||
|
|
ET_DBG("[%d]tr:%d, s:%.2f, p50:%.2f, freq:%.2f, angle:%.2f", fz->state, fz->trend, fz->slope, fz->p50, fz->freq,
|
|||
|
|
angle);
|
|||
|
|
if (fsm->fz_fsm_state)
|
|||
|
|
{
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
if (fz->calib_info[0].start_time == 0)
|
|||
|
|
{
|
|||
|
|
fz->calib_info[0].start_time = rt_tick_get_millisecond();
|
|||
|
|
fz->calib_info[0].angle = 0;
|
|||
|
|
fz->calib_info[0].target_angle = 0;
|
|||
|
|
}
|
|||
|
|
switch (fz->state)
|
|||
|
|
{
|
|||
|
|
case ZF_UNKNOWN_STAGE:
|
|||
|
|
case ZF_SATURATED_STAGE:
|
|||
|
|
fz->target_dir = MEASURE_DEF_DIR;
|
|||
|
|
sv630p_speed_update(&sv630p_handle, FZ_UNKNOWN_STAGE_RPM * fz->target_dir);
|
|||
|
|
break;
|
|||
|
|
case ZF_WAVE_ENTER_STAGE:
|
|||
|
|
sv630p_speed_update(&sv630p_handle, FZ_WAVE_ENTER_STAGE_RPM * fz->target_dir);
|
|||
|
|
break;
|
|||
|
|
case ZF_PREPARE_ENTER_STAGE:
|
|||
|
|
sv630p_speed_update(&sv630p_handle, FZ_PREPARE_STAGE_RPM * fz->target_dir);
|
|||
|
|
break;
|
|||
|
|
case ZF_FAST_CLOSE_STAGE:
|
|||
|
|
// 优化方向反转逻辑，减少过零点超调
|
|||
|
|
if (get_data_trend(&fz->trend_handle, fz->p50) == TREND_INCREASE && fz->slope > 200 && fz->p50 < 40000)
|
|||
|
|
{
|
|||
|
|
fz->target_dir *= -1;
|
|||
|
|
etk_position_pid_reset(&fz->pid); // 重置PID避免累积误差影响
|
|||
|
|
trend_handle_reset(&fz->trend_handle); // 防止很快的重复进来
|
|||
|
|
ET_WARN("passed the zero point and rotated in the opposite direction (slope=%.1f, p50=%.0f)", fz->slope,
|
|||
|
|
fz->p50);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (fz->p50 < ZERO_POS_THRESHOLD)
|
|||
|
|
{
|
|||
|
|
fz->zero_stable_count++;
|
|||
|
|
if (fz->zero_stable_count > ZC_COUNT_STABLE_TIMES || fz->p50 < 0.03)
|
|||
|
|
{
|
|||
|
|
fz->state = ZF_ZERO_STAGE;
|
|||
|
|
fz->calib_info[fz->calib_cnt].m_dir = sv630p_get_last_rpm(&sv630p_handle) > 0 ? 1 : -1;
|
|||
|
|
sv630p_speed_mode_stop(&sv630p_handle);
|
|||
|
|
uint8_t index = fz->calib_cnt % FZ_CALI_MAX_COUNT;
|
|||
|
|
fz->calib_info[index].angle = angle;
|
|||
|
|
fz->calib_info[index].end_time = rt_tick_get_millisecond();
|
|||
|
|
fz->calib_info[index].power = fz->p50;
|
|||
|
|
ET_INFO("update END calib index:%d", index);
|
|||
|
|
ET_WARN("Find zero done:%.3f", angle);
|
|||
|
|
fz->calib_cnt++;
|
|||
|
|
etk_position_pid_reset(&fz->pid);
|
|||
|
|
fz->target_dir = MEASURE_DEF_DIR;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
fz->zero_stable_count = 0;
|
|||
|
|
#if 1
|
|||
|
|
// 根据p50值大小采用三级控制策略，优化不同阶段的控制效果
|
|||
|
|
if (fz->p50 > 40000)
|
|||
|
|
{
|
|||
|
|
rpm = 850.0f;
|
|||
|
|
rpm = rpm * (fz->target_dir);
|
|||
|
|
}
|
|||
|
|
else if (fz->p50 > 10000)
|
|||
|
|
{
|
|||
|
|
rpm = etk_position_pid_update(&fz->pid, fz->p50, 0.535, 0.7f);
|
|||
|
|
rpm = rpm * fz->target_dir;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
#endif
|
|||
|
|
{
|
|||
|
|
// p50较小时使用保守PID控制，降低微分滤波系数以提高稳定性
|
|||
|
|
rpm = etk_position_pid_update(&fz->pid, fz->p50, 0.535, 0.9f);
|
|||
|
|
if (rpm > 350.0f)
|
|||
|
|
rpm = 350.0f;
|
|||
|
|
if (rpm < -350.0f)
|
|||
|
|
rpm = -350.0f;
|
|||
|
|
rpm = rpm * fz->target_dir;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (fabs(rpm) < FZ_PRECISE_ADJUST_MIN_RPM)
|
|||
|
|
{
|
|||
|
|
rpm = (rpm >= 0) ? FZ_PRECISE_ADJUST_MIN_RPM : -FZ_PRECISE_ADJUST_MIN_RPM;
|
|||
|
|
}
|
|||
|
|
ET_DBG("rpm:%.2f", rpm);
|
|||
|
|
sv630p_speed_update(&sv630p_handle, rpm);
|
|||
|
|
}
|
|||
|
|
break;
|
|||
|
|
case ZF_ZERO_STAGE:
|
|||
|
|
if (fsm->meas_mode == PM_MEAS_TEST_Mode && fz->calib_cnt < FZ_CALI_MAX_COUNT)
|
|||
|
|
{
|
|||
|
|
fz->calib_info[fz->calib_cnt].start_time = rt_tick_get_millisecond();
|
|||
|
|
accuracy_auto_measurement_once(fz, angle);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
sv630p_speed_mode_stop(&sv630p_handle);
|
|||
|
|
measurement_info_print(fz);
|
|||
|
|
fsm->fz_fsm_state = true; // test use
|
|||
|
|
fz->calib_cnt = 0;
|
|||
|
|
fsm->meas_mode = PM_MEAS_NORMAL_Mode;
|
|||
|
|
fz->target_dir = MEASURE_DEF_DIR;
|
|||
|
|
//ET_WARN("Mesaure time:%d ms", rt_tick_get_millisecond() - fz->calib_start_time);
|
|||
|
|
}
|
|||
|
|
break;
|
|||
|
|
case ZF_FAST_LEAVE_STAGE:
|
|||
|
|
fz->target_dir *= -1;
|
|||
|
|
fz->state = ZF_FAST_CLOSE_STAGE;
|
|||
|
|
sv630p_speed_update(&sv630p_handle, FZ_FAST_CLOSE_STAGE_RPM * fz->target_dir);
|
|||
|
|
break;
|
|||
|
|
case ZF_PREPARE_LEAVE_STAGE:
|
|||
|
|
goto_prepare_leave_stage(fz, angle);
|
|||
|
|
fz->state = ZF_PREPARE_ENTER_STAGE;
|
|||
|
|
break;
|
|||
|
|
case ZF_WAVE_LEAVE_STAGE:
|
|||
|
|
fz->target_dir *= -1;
|
|||
|
|
fz->state = ZF_WAVE_ENTER_STAGE;
|
|||
|
|
break;
|
|||
|
|
case ZF_STATE_MAX:
|
|||
|
|
break;
|
|||
|
|
default:
|
|||
|
|
ET_INFO("not process state %d", fz->state);
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int pm_fsm_init(pm_fsm_t *fsm)
|
|||
|
|
{
|
|||
|
|
pm_fsm = fsm;
|
|||
|
|
return 0;
|
|||
|
|
}
|