150 lines
3.6 KiB
C
150 lines
3.6 KiB
C
|
/*
|
|||
|
|
* @Author: mypx
|
|||
|
|
* @Date: 2025-08-11 08:30:53
|
|||
|
|
* @LastEditTime: 2025-09-24 16:10:18
|
|||
|
|
* @LastEditors: mypx mypx_coder@163.com
|
|||
|
|
* @Description:
|
|||
|
|
*/
|
|||
|
|
#ifndef __PM_MEAS_H__
|
|||
|
|
#define __PM_MEAS_H__
|
|||
|
|
#include "et_goertzel.h"
|
|||
|
|
#include "et_zero_cross.h"
|
|||
|
|
#include "etk_pid.h"
|
|||
|
|
#include "etk_slope.h"
|
|||
|
|
#include "pm_common.h"
|
|||
|
|
|
|||
|
|
#define FZ_CALI_MAX_COUNT 10
|
|||
|
|
#define ZC_COUNT_STABLE_TIMES 5
|
|||
|
|
#define ZERO_POS_THRESHOLD 0.5f
|
|||
|
|
#define PID_TARGET_ZERO_POS_VAL 0.0f
|
|||
|
|
#define RUN_OFFSET_ANGLE_RPM (1000)
|
|||
|
|
|
|||
|
|
|
|||
|
|
#define FZ_UNKNOWN_STAGE_RPM 1000
|
|||
|
|
#define FZ_SATURATED_STAGE_RPM 1000
|
|||
|
|
#define FZ_WAVE_ENTER_STAGE_RPM 1000
|
|||
|
|
#define FZ_PREPARE_STAGE_RPM 1000
|
|||
|
|
#define FZ_FAST_CLOSE_STAGE_RPM 700
|
|||
|
|
#define FZ_PRECISE_ADJUST_MIN_RPM (0.6f * MOTOR_TO_ENCODER_FACTOR) // 0.6rpm->0.001
|
|||
|
|
|
|||
|
|
#define FZ_SATURATED_POWER_MAX 5.0f
|
|||
|
|
#define FZ_SATURATED_SLOPE_MAX 50.0f
|
|||
|
|
|
|||
|
|
|
|||
|
|
#define FZ_FAST_CLOSE_ENTER_FREQ_MIN 48.0f
|
|||
|
|
#define FZ_FAST_CLOSE_ENTER_FREQ_MAX 52.0f
|
|||
|
|
|
|||
|
|
#define FZ_PRECISE_ADJUST_LEAVE_POWER_MAX FZ_PRECISE_ADJUST_ENTER_POWER_MAX
|
|||
|
|
|
|||
|
|
#define FZ_PREPARE_LEAVE_SLOPE_MIN -100000
|
|||
|
|
#define FZ_PREPARE_LEAVE_FREQ_MIN 48.0f
|
|||
|
|
#define FZ_PREPARE_LEAVE_FREQ_MAX 52.0f
|
|||
|
|
|
|||
|
|
extern bool fsm_state;
|
|||
|
|
|
|||
|
|
typedef enum
|
|||
|
|
{
|
|||
|
|
TREND_UNKNOWN = 0, // 未知（首次调用，无历史数据）
|
|||
|
|
TREND_STABLE, // 稳定（无连续6次升/降，或当前值与上次相等）
|
|||
|
|
TREND_INCREASE, // 上升（连续6次大于上次值）
|
|||
|
|
TREND_DECREASE // 下降（连续6次小于上次值）
|
|||
|
|
} TrendStatus;
|
|||
|
|
#define TREND_MAX_CNT 20
|
|||
|
|
|
|||
|
|
typedef enum
|
|||
|
|
{
|
|||
|
|
ZF_UNKNOWN_STAGE = 0,
|
|||
|
|
ZF_SATURATED_STAGE,
|
|||
|
|
ZF_WAVE_ENTER_STAGE, // little magnitude 50hz wave detected
|
|||
|
|
ZF_PREPARE_ENTER_STAGE, // big magnitude 50hz wave detected
|
|||
|
|
ZF_FAST_CLOSE_STAGE, // near zero, fast close
|
|||
|
|
ZF_ZERO_STAGE, // zero found, and judgement next setup
|
|||
|
|
ZF_FAST_LEAVE_STAGE,
|
|||
|
|
ZF_PREPARE_LEAVE_STAGE, // ZF_PREPARE_ENTER_STAGE->ZF_WAVE_ENTER_STAGE or ZF_WAVE_ENTER_STAGE->ZF_SATURATED_STAGE
|
|||
|
|
ZF_WAVE_LEAVE_STAGE,
|
|||
|
|
ZF_STATE_MAX,
|
|||
|
|
} ZeroFindState;
|
|||
|
|
|
|||
|
|
typedef enum
|
|||
|
|
{
|
|||
|
|
PM_FSM_STOP = 0,
|
|||
|
|
PM_FSM_RUNNING,
|
|||
|
|
} FzFsmState;
|
|||
|
|
|
|||
|
|
typedef struct
|
|||
|
|
{
|
|||
|
|
double last_val;
|
|||
|
|
uint8_t inc_cnt;
|
|||
|
|
uint8_t dec_cnt;
|
|||
|
|
} trend_handle_t;
|
|||
|
|
|
|||
|
|
typedef enum
|
|||
|
|
{
|
|||
|
|
STAGE_RESULT_NO,
|
|||
|
|
STAGE_RESULT_OK,
|
|||
|
|
STAGE_RESULT_UNDETERMINED,
|
|||
|
|
} StageResult;
|
|||
|
|
|
|||
|
|
typedef struct
|
|||
|
|
{
|
|||
|
|
uint8_t is_updated : 1;
|
|||
|
|
uint8_t is_print : 1;
|
|||
|
|
float angle;
|
|||
|
|
double p50;
|
|||
|
|
float slope;
|
|||
|
|
float freq;
|
|||
|
|
uint32_t time; // enter time(ms)
|
|||
|
|
} fz_stage_t;
|
|||
|
|
|
|||
|
|
typedef struct
|
|||
|
|
{
|
|||
|
|
uint32_t start_time; // ms
|
|||
|
|
uint32_t end_time; // ms
|
|||
|
|
float angle; // zero pos
|
|||
|
|
float target_angle;
|
|||
|
|
float power;
|
|||
|
|
float cali_angle;
|
|||
|
|
int8_t m_dir;
|
|||
|
|
} calib_info_t;
|
|||
|
|
|
|||
|
|
typedef struct
|
|||
|
|
{
|
|||
|
|
ZeroFindState state;
|
|||
|
|
|
|||
|
|
fz_stage_t stage_info[ZF_STATE_MAX];
|
|||
|
|
|
|||
|
|
uint8_t calib_cnt;
|
|||
|
|
int target_dir; // 1: forward, -1: backward
|
|||
|
|
calib_info_t calib_info[FZ_CALI_MAX_COUNT];
|
|||
|
|
|
|||
|
|
uint8_t zero_stable_count;
|
|||
|
|
et_zero_cross_t zc;
|
|||
|
|
et_goertzel_config_t cfg;
|
|||
|
|
etk_pos_pid_t pid;
|
|||
|
|
double p50;
|
|||
|
|
float freq;
|
|||
|
|
etk_slope_t slope_handle;
|
|||
|
|
float slope;
|
|||
|
|
double last_p50_slope;
|
|||
|
|
TrendStatus trend;
|
|||
|
|
trend_handle_t trend_handle;
|
|||
|
|
} pm_meas_t;
|
|||
|
|
|
|||
|
|
typedef struct
|
|||
|
|
{
|
|||
|
|
FzFsmState fz_fsm_state;
|
|||
|
|
pm_meas_t meas;
|
|||
|
|
PmMeasMode meas_mode;
|
|||
|
|
} pm_fsm_t;
|
|||
|
|
|
|||
|
|
ZeroFindState get_find_zero_state(pm_meas_t *fz);
|
|||
|
|
|
|||
|
|
void trend_handle_init(trend_handle_t *handle);
|
|||
|
|
|
|||
|
|
|
|||
|
|
int find_zero_process(pm_meas_t *fz, const uint16_t *adc_val, uint32_t len);
|
|||
|
|
|
|||
|
|
int pm_fsm_init(pm_fsm_t *fsm);
|
|||
|
|
|
|||
|
|
#endif
|