#include <stdio.h>
#include <math.h>
#include "sys.h"
#include "dacs.h"
#include "board.h"
#include "./mode_pwmp_hd.h"
#include "delay.h"
#include "at_hc24.h"
#include "entity.h"
#include "eeprom_m95.h"
#include "pid.h"
#include "filter.h"
#include <stdlib.h>
#include <string.h>
#include "pdctrl.h"
#include "app.h"
#include "data_type_def.h"

#define ACTUAL_FILTER 0

mode_pwmp_hd_t *mode_pwmp_hd;
pwmp_hd_adjust_t *mode_pwmp_hd_adjust;
pid_autotune_hd_t *pid_autotune_hd;
pwmp_hd_test_t *mode_pwmp_hd_test;
execute_rsp_hd_t rsp; // 执行结果

static execute_rsp_hd_t *execute_dac_plan1(void); // 执行方案1
static execute_rsp_hd_t *execute_dac_plan2(void); // 执行方案2
static execute_rsp_hd_t *execute_dac_plan3(void); // 执行方案3
static execute_rsp_hd_t *execute_dac_plan4(void); // 执行方案4
// static uint8_t execute_run_count = 0;	      // 执行计数

static void _process_start(void)
{
    pwmp_hd_process_state_set(PWMP_HD_PROCESS_CONTROL);
}

static void _process_stop(void)
{
    pwmp_hd_process_state_set(PWMP_HD_PROCESS_STOP);
}

static void _adjust_start(void)
{
    pwmp_hd_process_state_set(PWMP_HD_PROCESS_ADJUST);
}

static void _adjust_stop(void)
{
    pwmp_hd_process_state_set(PWMP_HD_PROCESS_ADJUST_STOP);
}

static uint8_t _adjust_result(void)
{
    if (mode_pwmp_hd->pwmp_save->storage.tuned_flag == TUNED_ONGOING)
    {
        udevice.tuned_flag = TUNED_ONGOING;
    }
    else if (mode_pwmp_hd->pwmp_save->storage.tuned_flag == TUNED_SUCCESS)
    {
        udevice.tuned_flag = TUNED_SUCCESS;
    }
    else
    {
        udevice.tuned_flag = TUNED_FAILED;
    }

    return udevice.tuned_flag;
}

static uint8_t _adjust_step_count(void)
{
    return mode_pwmp_hd->lcd_adjust_state;
}

static uint8_t _adjust_step_current(void)
{
    return mode_pwmp_hd_adjust->adjust_state;
}

static void _adjust_data(mode_adjust_data_t *data)
{
    data->pos0 = mode_pwmp_hd->pwmp_save->storage.trip_0;               ///< 位置0 ad
    data->pos100 = mode_pwmp_hd->pwmp_save->storage.trip_100;           ///< 位置100 ad
    data->pot0 = mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin;      ///< 位置0 输出
    data->pot100 = mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin;    ///< 位置100 输出
    data->current0 = mode_pwmp_hd->pwmp_save->storage.current0;         ///< 位置0 电流(毫安，放大100倍, 0.01ma=1)
    data->current100 = mode_pwmp_hd->pwmp_save->storage.current100;     ///< 位置100 电流(毫安,放大100倍，0.01ma=1)
    data->open_time = mode_pwmp_hd->pwmp_save->storage.all_open_time;   ///< 全开时间(秒)
    data->close_time = mode_pwmp_hd->pwmp_save->storage.all_close_time; ///< 全关时间(秒)
    data->kp = mode_pwmp_hd->pwmp_save->storage.kp;                     ///< pid 比例系数
    data->ki = mode_pwmp_hd->pwmp_save->storage.ki;                     ///< pid 积分系数
    data->kd = mode_pwmp_hd->pwmp_save->storage.kd;                     ///< pid 微分系数
}

static BOOL _control_idle(void)
{
    return TRUE;
}

static BOOL _adjust_isrun(void)
{
    return mode_pwmp_hd->process_state == PWMP_HD_PROCESS_ADJUST;
}

/*程序状态设置*/
void pwmp_hd_process_state_set(mode_pwmp_hd_process_state_e state)
{
    if (mode_pwmp_hd->process_state == state && mode_pwmp_hd->process_state == PWMP_HD_PROCESS_ADJUST)
    {
        mode_pwmp_hd->process_state = PWMP_HD_PROCESS_STOP;
    }
    else
    {
        mode_pwmp_hd->process_state = state;
        switch (state)
        {
        case PWMP_HD_PROCESS_ADJUST:
            mode_pwmp_hd->adjust_state = PWMP_HD_ADJUST_IDEL;
            break;
        default:
            break;
        }
    }
}

/*行程更新*/
static void pwmp_control_update(filter_e type)
{
    mode_pwmp_hd_control_t *p = &mode_pwmp_hd->control;
    rt_data.actual_travel = get_actual_travel(type);
    rt_data.actual_travel = actual_travel_deal(rt_data.actual_travel);
    pid_actual = get_pid_travel(rt_data.actual_travel);

    if (mode_pwmp_hd->process_state == PWMP_HD_PROCESS_ADJUST)
    {
        p->ctrl_target = p->ctrl_target;
    }
    else if (mode_pwmp_hd->process_state == PWMP_HD_PROCESS_TEST)
    {
        p->ctrl_target = mode_pwmp_hd_test->ctrl_target;
    }
    else
    {
        p->ctrl_target = lpf_window_update(mode_pwmp_hd->filter.handle, pid_target);
    }
#if ACTUAL_FILTER == 1
    if (fabs(p->real_error) < 0.5)
    {
        p->ctrl_feedback = lpf_window_update(mode_pwmp_hd->filter.handle, pid_actual);
    }
    else
    {
        p->ctrl_feedback = pid_actual;
        lpf_window_reset(mode_pwmp_hd->filter.handle);
    }
#else
    p->ctrl_feedback = pid_actual;
#endif
    p->real_error = p->ctrl_target - p->ctrl_feedback;
    p->current_adc = adc_raw[ADC_PSB_CHANNEL];
    p->small_trip_current_adc = adc_result_average(ADCS_1, ADC_IPSB_CHANNEL);
    /*测试模式赋值*/
    mode_pwmp_hd_test->psb_adc = p->current_adc;
    mode_pwmp_hd_test->small_trip_current_adc = p->small_trip_current_adc;
    mode_pwmp_hd_test->ctrl_feedback = p->ctrl_feedback;
    mode_pwmp_hd_test->real_error = p->real_error;
}

/*PID参数获取*/
static float32 get_pwmp_hd_control_kp(void)
{
    if (mode_pwmp_hd->pwmp_save->storage.kp < 2)
    {
        // return mode_pwmp_hd->pwmp_save->storage.kp * 8;
        return mode_pwmp_hd->pwmp_save->storage.kp * 15;
    }
    else
    {
        return mode_pwmp_hd->pwmp_save->storage.kp * 4;
    }
}

static float32 get_pwmp_hd_control_ki(void)
{
    return mode_pwmp_hd->pwmp_save->storage.ki < 0.001 ? mode_pwmp_hd->pwmp_save->storage.ki * 20 : 0.01;
}

static float32 get_pwmp_hd_control_kd(void)
{
    // return mode_pwmp_hd->pwmp_save->storage.kd < 0.01 ? mode_pwmp_hd->pwmp_save->storage.kd : 0.001;
    if (udevice.value_travel_type == TRAVEL_TYPE_STRAIGHT)
    {
        return mode_pwmp_hd->pwmp_save->storage.kd = 0;
    }
    else
    {

        if (mode_pwmp_hd->pwmp_save->storage.kd <= 20)
        {
            return mode_pwmp_hd->pwmp_save->storage.kd;
        }
        else
        {
            return mode_pwmp_hd->pwmp_save->storage.kd = 20;
        }
    }
}

/*将自己结构体变量中的参数保存到公共参数中*/
static void pwmp_public_params_update()
{
    udevice.pos0_travel_vol = mode_pwmp_hd->pwmp_save->storage.trip_0;
    udevice.pos100_travel_vol = mode_pwmp_hd->pwmp_save->storage.trip_100;
    udevice.output_min = mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin;
    udevice.output_max = mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin;
    udevice.apid_kp = mode_pwmp_hd->pwmp_save->storage.kp;
    udevice.apid_ki = mode_pwmp_hd->pwmp_save->storage.ki;
    udevice.apid_kd = mode_pwmp_hd->pwmp_save->storage.kd;
    udevice.tuned_flag = mode_pwmp_hd->pwmp_save->storage.prov_flag;
}

//////////////////////////////*控制BEGIN*//////////////////////////////////////////////////////
void mode_pwmp_hd_stop(mode_pwmp_hd_t *handle)
{
    // 释放内存
    osel_memset((uint8_t *)&handle->control, 0, sizeof(mode_pwmp_hd_control_t));
}

/**
 * @brief 算法方案初始化
 * @param {execute_plan_hd_e} plan
 * @return {*}
 * @note
 */
void execute_pid_init(execute_plan_hd_e plan)
{
    switch (plan)
    {
        // HD：位置式PID算法
    case EXECUTE_PLAN_1:
        _pid.type = PID_TYPE_CUSTOM_HANGDIAN;
        pid_constructor(&_pid);
        _pid.pid_u.hd.set_ctrl_prm_position(&_pid.pid_u.hd, get_pwmp_hd_control_kp(), get_pwmp_hd_control_ki(), get_pwmp_hd_control_kd());
        break;
    case EXECUTE_PLAN_2:

        break;
    case EXECUTE_PLAN_3:

        break;
    case EXECUTE_PLAN_4:

    default:
        break;
    }
}

/**
 * @brief 算法方案执行
 * @param {execute_plan_hd_e} plan
 * @return {*}
 * @note
 */
execute_rsp_hd_t *execute_dac(execute_plan_hd_e plan)
{
    switch (plan)
    {
    case EXECUTE_PLAN_1:
        return execute_dac_plan1(); // 位置式PID
    case EXECUTE_PLAN_2:
        return execute_dac_plan2();
    case EXECUTE_PLAN_3:
        return execute_dac_plan3();
    case EXECUTE_PLAN_4:
        return execute_dac_plan4();
    default:
        rsp.code = EXECUTE_HD_NONE;
        return &rsp;
    }
}
static execute_rsp_hd_t *execute_dac_plan1()
{
    static float32 Control_diff;
    // uint8_t flag_stable = 0;
    Control_diff = mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin - mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin;
    float32 out = 0.0;

    // 杭电：输出限幅的参数设置
    _pid.pid_u.hd.set_out_prm_position(&_pid.pid_u.hd, 100, 0);
    out = _pid.pid_u.hd.pid_position(&_pid.pid_u.hd, mode_pwmp_hd->control.real_error);
    out = out / 100;
    if (udevice.value_travel_type == TRAVEL_TYPE_STRAIGHT)
    {
        /* 一阶段加权系数*/
        static float32 index_min_first = 0.5;
        static float32 index_max_first = 0.5;
        /*二阶段加权系数*/
        static float32 index_min_second = 0.3;
        static float32 index_max_second = 0.3;
        if (fabs(mode_pwmp_hd->control.real_error) >= 5.7)
        {                                                                                                                                                                                     // 对PID输出结果进行归一化处理
            out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin + Control_diff * index_max_first) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_first); // 映射实际控制器的范围
            out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_first;
        }
        else if (fabs(mode_pwmp_hd->control.real_error) < 5.7 && fabs(mode_pwmp_hd->control.real_error) >= 1)
        {                                                                                                                                                                                       // 对PID输出结果进行归一化处理
            out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin + Control_diff * index_max_second) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_second); // 映射实际控制器的范围
            out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_second;
        }
        else
        {
            out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin); // 映射实际控制器的范围
            out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin;
        }
    }
    else if (udevice.value_travel_type == TRAVEL_TYPE_ANGULAR)
    {
#if 0
        /* 一阶段加权系数*/
        static float32 index_min_first = 1.2;
        static float32 index_max_first = 1.2;
        /*二阶段加权系数*/
        static float32 index_min_second = 0.8;
        static float32 index_max_second = 0.8;
        /*三阶段加权系数*/
        static float32 index_min_third = 0;
        static float32 index_max_third = 0;
        if (fabs(mode_pwmp_hd->control.real_error) >= 5.7)
        {                                                                                                                                                                                     // 对PID输出结果进行归一化处理
            out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin + Control_diff * index_max_first) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_first); // 映射实际控制器的范围
            out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_first;
        }
        else if (fabs(mode_pwmp_hd->control.real_error) < 5.7 && fabs(mode_pwmp_hd->control.real_error) >= 1)
        {                                                                                                                                                                                       // 对PID输出结果进行归一化处理
            out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin + Control_diff * index_max_second) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_second); // 映射实际控制器的范围
            out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index_min_second;
        }
        else
        {
            out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin - Control_diff * index_max_third) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin + Control_diff * index_min_third); // 映射实际控制器的范围
            out += (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin + Control_diff * index_min_third);
        }
#else
        float32 index = fabs(1 - exp(-(pow(mode_pwmp_hd->control.real_error, 2) - 1)));
        if (fabs(mode_pwmp_hd->control.real_error) >= 1)
        {
            out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin + Control_diff * index) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index); // 映射实际控制器的范围
            out += mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin - Control_diff * index;
        }
        else if (fabs(mode_pwmp_hd->control.real_error) > 0.99 && fabs(mode_pwmp_hd->control.real_error) < 1)
        {
            out = mode_pwmp_hd->pwmp_save->storage.startup_value;
        }
        else if (fabs(mode_pwmp_hd->control.real_error) < 1)
        {
            out *= (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin) - (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin); // 映射实际控制器的范围
            out += (mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin);
        }
#endif
    }

    rsp.code = EXECUTE_HD_OUT;
    rsp.dac = (uint32_t)out;

    return &rsp;
}
/// <summary>
///
/// </summary>
/// <param name=""></param>
/// <returns></returns>

static execute_rsp_hd_t *execute_dac_plan2()
{
    return 0;
}

/// <summary>
///
/// </summary>
/// <returns></returns>

static execute_rsp_hd_t *execute_dac_plan3()
{
    return 0;
}

/// <summary>
///
/// </summary>
/// <returns></returns>
static execute_rsp_hd_t *execute_dac_plan4()
{
    return 0;
}
//////////////////////////////*控制END*//////////////////////////////////////////////////////

//////////////////////////////*整定相关函数BEGIN*//////////////////////////////////////////////////////
/*PID自整定方法*/
static void pid_autotune_way_set(pid_autotune_way_e state)
{
    pid_autotune_hd->autotune_way = state;
}

/*整定过程中判断阀门状态：移动、停止、停止但仍在等待*/
static valve_position_change_e pwmp_adjust_hd_valve_position_change(uint8_t *state, uint8_t next_state, uint8_t diff_adc_max)
{
    uint16_t diff_adc = 0;
    // uint16_t adc = get_actual_travel_adc();
    uint16_t adc = get_actual_travel_adc();
    pid_autotune_hd->data.adjust_tmp_actual = adc;

    if (mode_pwmp_hd_adjust->psb_adc != adc)
    {
        diff_adc = ABS(mode_pwmp_hd_adjust->psb_adc - adc);
    }

    if (diff_adc > diff_adc_max)
    {
        // 位置还在改变，继续等待
        mode_pwmp_hd_adjust->psb_adc = adc;
        return POSITION_CHANGE;
    }
    else
    {
        // 位置不变
        if (mode_pwmp_hd_adjust->wait_count == 0)
        {
            // 等待次数也到了，位置不再发生变化，可以切换下一个状态
            *state = next_state;
            return POSITION_NO_CHANGE_FOREVER;
        }
        else
        { // 当位置不变的时候才开始等待
            mode_pwmp_hd_adjust->wait_count--;
            return POSITION_NO_CHANGE;
        }
    }
}

/*获得切线的横坐标（时间）
输入：actual_in为纵坐标，表示阀位的行程
*/
static float32 get_time_tangent(float actual_in)
{
    return (actual_in - pid_autotune_hd->data.b) / pid_autotune_hd->data.k;
}

//////////////////////////////*整定相关函数END*/////////////////////////////////////////////////////////

//////////////////////////////////*状态机模式BEGIN*/////////////////////////////////////////////////////
/*整定等待状态*/
static void pwmp_adjust_hd_idle(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
{
    if (!FSM_IS_WAIT(*state))
    {
        mode_pwmp_hd->auto_tune_state = PWMP_HD_ADJUST_RESULT_TUNING;
        sys_millis_reset();
        osel_memset((uint8_t *)&mode_pwmp_hd->pwmp_save->storage, 0, sizeof(mode_pwmp_hd_storage_data_t));
        mode_pwmp_hd->pwmp_save->storage.tuned_flag = TUNED_ONGOING;
        /*为指针开辟存储空间*/
        if (mode_pwmp_hd_adjust == NULL)
        {
            mode_pwmp_hd_adjust = osel_mem_alloc(sizeof(pwmp_hd_adjust_t));
            DBG_ASSERT(mode_pwmp_hd_adjust != NULL __DBG_LINE);
        }
        osel_memset((uint8_t *)mode_pwmp_hd_adjust, 0, sizeof(pwmp_hd_adjust_t));

        if (pid_autotune_hd == NULL)
        {
            pid_autotune_hd = (pid_autotune_hd_t *)osel_mem_alloc(sizeof(pid_autotune_hd_t));
        }
        osel_memset((uint8_t *)pid_autotune_hd, 0, sizeof(pid_autotune_hd_t));

        /*输出初始化*/
        mode_pwmp_hd_adjust->arr_default = mode_pwmp_hd->arr_default;
        mode_pwmp_hd_adjust->arr_current = 0;

        mode_pwmp_hd_adjust->adjust_state = (mode_pwmp_hd_adjust_state_e)*state;
        FSM_WAIT(mode_pwmp_hd_adjust->adjust_state); // 等放气完毕就可以切入到else里面了
        *state = PWMP_HD_ADJUST_BLEEDING;
    }
    else
    {
        valve_position_change_e s = pwmp_adjust_hd_valve_position_change(state, next_state, DIFF_ADC);

        switch (s)
        {
        case POSITION_NO_CHANGE_FOREVER:
            // 位置不再改变，记录此时阀门位置AD值
            mode_pwmp_hd_adjust->adc_record_1 = pid_autotune_hd->data.adjust_tmp_actual;
            mode_pwmp_hd_adjust->adc_record_0 = pid_autotune_hd->data.adjust_tmp_actual;
            // 记录0位置电流的大小
            if (udevice.value_action_type == ATO)
            {
                mode_pwmp_hd_adjust->current_0 = ip2current();
            }
            else
            {
                mode_pwmp_hd_adjust->current_100 = ip2current();
            }
            break;
        case POSITION_CHANGE:
            *state = PWMP_HD_ADJUST_IDEL;
            // 将等待时间设置为1s
            mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 10;
            break;
        default:
            break;
        }
    }
}

/*整定粗调0*/
static void pwmp_adjust_hd_rough_position0(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
{
    mode_pwmp_hd_adjust->psb_adc = get_actual_travel_adc();
    uint16_t current_adc = 0;
    if (!FSM_IS_WAIT(*state))
    {
        // 以10%增加，提高DAC输出，找到能推动阀门的最小值
        mode_pwmp_hd_adjust->arr_current = 200;
        mode_pwmp_hd_adjust->arr_last = mode_pwmp_hd_adjust->arr_current;
        pdctrl_out(mode_pwmp_hd_adjust->arr_current);
        mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 20;
        mode_pwmp_hd_adjust->adjust_state = (mode_pwmp_hd_adjust_state_e)*state;
        FSM_WAIT(*state); // 设置等待状态
    }
    else
    {
        valve_position_change_e s = pwmp_adjust_hd_valve_position_change(state, next_state, DIFF_ADC);

        current_adc = pid_autotune_hd->data.adjust_tmp_actual;
        // 3.28修改：将DIFF_ADC_MAX由10改为6
        if (ABS(current_adc - mode_pwmp_hd_adjust->adc_record_1) >= 10U) // 阀位发生改变
        {
            if (mode_pwmp_hd_adjust->arr_last > 4000)
            {
                // 整定失败：位置反馈错误
                *state = PWMP_HD_ADJUST_FAIL;
            }
            else
            {
                /*得到一个粗略的，小于启动量的值*/
                mode_pwmp_hd_adjust->arr_record_1 = mode_pwmp_hd_adjust->arr_last > 1000 ? mode_pwmp_hd_adjust->arr_last * 0.9 : mode_pwmp_hd_adjust->arr_last;
                /*判断位置反馈磁条是正装还是反装*/
                mode_pwmp_hd_adjust->adc_0_100_flag = mode_pwmp_hd_adjust->adc_record_0 < current_adc ? TRUE : FALSE;
            }
            mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 50;
            *state = next_state;
        }
        else // 阀位未发生变化：继续增大输出
        {
            switch (s)
            {
            case POSITION_NO_CHANGE_FOREVER: // 位置不再改变
            {

                if (ABS(current_adc - mode_pwmp_hd_adjust->adc_record_1) <= DIFF_ADC)
                {
                    // 没有发生变化
                    mode_pwmp_hd_adjust->arr_last = mode_pwmp_hd_adjust->arr_current;
                    if (mode_pwmp_hd_adjust->arr_last < 500)
                    {
                        mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_current * 1.1;
                    }
                    else
                    {
                        mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_current * 1.05;
                    }
                    /*若输出信号为最大值时认定为整定错误*/
                    if (mode_pwmp_hd_adjust->arr_current > 4000)
                    {
                        // 整定失败：位置反馈错误
                        *state = PWMP_HD_ADJUST_FAIL;
                        return;
                    }
                    pdctrl_out(mode_pwmp_hd_adjust->arr_current);
                    mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 30;
                    *state = PWMP_HD_ADJUST_ROUGH_POSITION0;
                    FSM_WAIT(*state); // 设置等待状态
                }
                else
                {
                    DBG_ASSERT(FALSE __DBG_LINE);
                }
                break;
            }
            case POSITION_CHANGE:
                DBG_ASSERT(FALSE __DBG_LINE);
                break;
            case POSITION_NO_CHANGE:
            {
                break;
            }
            default:
                break;
            }
        }
    }
}

/*放气值（0信号）的整定*/
static void pwmp_adjust_hd_initiate_position0(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
{
    BOOL flag = FALSE;
    mode_pwmp_hd_adjust->psb_adc = get_actual_travel_adc();
    if (!FSM_IS_WAIT(*state))
    {
        if (mode_pwmp_hd_adjust->wait_count-- > 0)
        {
            // 等待放气完毕
            return;
        }
        mode_pwmp_hd_adjust->flag_stable_begin = TRUE;
        mode_pwmp_hd_adjust->flag_stable_end = FALSE;
        mode_pwmp_hd_adjust->arr_last = mode_pwmp_hd_adjust->arr_current;
        mode_pwmp_hd_adjust->last_adc = mode_pwmp_hd_adjust->psb_adc;
        mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 20;
        FSM_WAIT(*state); // 设置等待状态
    }
    else
    {
        // if (mode_pwmp_hd_adjust->adc_0_100_flag)
        // {
        // 	if (mode_pwmp_hd_adjust->psb_adc <= mode_pwmp_hd_adjust->last_adc)
        // 	{
        // 		if (ABS(mode_pwmp_hd_adjust->psb_adc - mode_pwmp_hd_adjust->last_adc) > DIFF_ADC)
        // 		{
        // 			mode_pwmp_hd_adjust->arr_record_0 = mode_pwmp_hd_adjust->arr_last + DIFF_ADC;
        // 			flag = TRUE;
        // 		}
        // 	}
        // }
        // else
        // {
        // 	if (mode_pwmp_hd_adjust->psb_adc >= mode_pwmp_hd_adjust->last_adc)
        // 	{
        // 		if (ABS(mode_pwmp_hd_adjust->psb_adc - mode_pwmp_hd_adjust->last_adc) > DIFF_ADC)
        // 		{
        // 			mode_pwmp_hd_adjust->arr_record_0 = mode_pwmp_hd_adjust->arr_last + DIFF_ADC;
        // 			flag = TRUE;
        // 		}
        // 	}
        // }

        /*求出阀位稳定信号*/
        // 阀位变化小于3且不为初始阀位附近时认定进入阀位稳定区域
        if (abs(mode_pwmp_hd_adjust->psb_adc - mode_pwmp_hd_adjust->last_adc) < 2 && abs(mode_pwmp_hd_adjust->psb_adc - mode_pwmp_hd_adjust->adc_record_1) > 50)
        {
            if (mode_pwmp_hd_adjust->flag_stable_begin)
            {
                mode_pwmp_hd_adjust->arr_record_stable_begin = mode_pwmp_hd_adjust->arr_current;
                mode_pwmp_hd_adjust->flag_stable_begin = FALSE;
                mode_pwmp_hd_adjust->flag_stable_end = TRUE;
            }
        }

        if (mode_pwmp_hd_adjust->adc_0_100_flag) // adc_0_100_flag true:ad值增大 false:ad值减小
        {
            if (mode_pwmp_hd_adjust->psb_adc - mode_pwmp_hd_adjust->last_adc < -DIFF_ADC && mode_pwmp_hd_adjust->flag_stable_end)
            {
                mode_pwmp_hd_adjust->flag_stable_end = FALSE;
                // 阀位回落
                mode_pwmp_hd_adjust->arr_record_stable_end = mode_pwmp_hd_adjust->arr_current;
                mode_pwmp_hd_adjust->arr_record_stable = (mode_pwmp_hd_adjust->arr_record_stable_begin + mode_pwmp_hd_adjust->arr_record_stable_end) / 2;
            }
        }
        else
        {
            if (mode_pwmp_hd_adjust->psb_adc - mode_pwmp_hd_adjust->last_adc > DIFF_ADC && mode_pwmp_hd_adjust->flag_stable_end)
            {
                mode_pwmp_hd_adjust->flag_stable_end = FALSE;
                // 阀位回落
                mode_pwmp_hd_adjust->arr_record_stable_end = mode_pwmp_hd_adjust->arr_current;
                mode_pwmp_hd_adjust->arr_record_stable = (mode_pwmp_hd_adjust->arr_record_stable_begin + mode_pwmp_hd_adjust->arr_record_stable_end) / 2;
            }
        }

        // 判断阀位是否回落到底部位置
        if (ABS(mode_pwmp_hd_adjust->psb_adc - mode_pwmp_hd_adjust->adc_record_1) < 6)
        {
            mode_pwmp_hd_adjust->arr_record_0 = mode_pwmp_hd_adjust->arr_current;
            flag = TRUE;
        }

        if (flag == TRUE)
        {
            mode_pwmp_hd_adjust->arr_record_2 = 4095; // 给定最大值输出
            *state = next_state;
            pdctrl_out(0); // 这里要先放气5秒然后计算全开时间
            mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 50;
        }
        else
        {
            mode_pwmp_hd_adjust->wait_count--;
            if (flag == FALSE && mode_pwmp_hd_adjust->wait_count == 0)
            {
                uint16_t last = mode_pwmp_hd_adjust->arr_last;
                mode_pwmp_hd_adjust->arr_last = mode_pwmp_hd_adjust->arr_current;
                mode_pwmp_hd_adjust->arr_current = last - 4;

                pdctrl_out(mode_pwmp_hd_adjust->arr_current);
                if (ABS(mode_pwmp_hd_adjust->psb_adc - mode_pwmp_hd_adjust->last_adc) > 6)
                {
                    mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 10;
                }
                else
                {
                    mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 30;
                }

                mode_pwmp_hd_adjust->last_adc = mode_pwmp_hd_adjust->psb_adc;
            }
        }
    }
}

// /*启动值细调*/
// static void pwmp_adjust_hd_accurate_position0(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
// {
// 	uint16_t current_adc = 0;
// 	if (!FSM_IS_WAIT(*state))
// 	{
// 		mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_record_1;
// 		mode_pwmp_hd_adjust->arr_last = mode_pwmp_hd_adjust->arr_current;
// 		mode_pwmp_hd_adjust->adjust_state = (mode_pwmp_hd_adjust_state_e)*state;
// 		FSM_WAIT(mode_pwmp_hd_adjust->adjust_state);
// 		*state = PWMP_HD_ADJUST_BLEEDING;
// 	}
// 	else
// 	{
// 		valve_position_change_e s = pwmp_adjust_hd_valve_position_change(state, next_state, DIFF_ADC);

// 		current_adc = pid_autotune_hd->data.adjust_tmp_actual;
// 		/*执行器发生动作*/
// 		if (ABS(current_adc - mode_pwmp_hd_adjust->adc_record_1) >= DIFF_ADC)
// 		{
// 			mode_pwmp_hd_adjust->arr_record_1 = mode_pwmp_hd_adjust->arr_last;
// 			mode_pwmp_hd_adjust->arr_record_2 = 4095; // 给定最大值输出
// 			*state = next_state;
// 			pdctrl_out(0); // 这里要先放气5秒然后计算全开时间
// 			mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 50;
// 		}
// 		else
// 		{
// 			switch (s)
// 			{
// 			case POSITION_NO_CHANGE_FOREVER: // 位置不再改变
// 			{
// 				/*若执行器在0位置*/
// 				if (ABS(current_adc - mode_pwmp_hd_adjust->adc_record_1) <= DIFF_ADC)
// 				{
// 					// 阀位没有发生变化，以每6增大信号输出
// 					mode_pwmp_hd_adjust->arr_last = mode_pwmp_hd_adjust->arr_current;
// 					mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_current + 6;
// 					pdctrl_out(mode_pwmp_hd_adjust->arr_current);
// 					mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 30;
// 					*state = PWMP_HD_ADJUST_ACCURATE_POSITION0;
// 					FSM_WAIT(*state); // 设置等待状态
// 				}
// 				else
// 				{
// 					DBG_ASSERT(FALSE __DBG_LINE);
// 				}
// 				break;
// 			}
// 			case POSITION_CHANGE:
// 				DBG_ASSERT(FALSE __DBG_LINE);
// 				break;
// 			default:
// 				break;
// 			}
// 		}
// 	}
// }

/*粗略整定得到满位置adc位置反馈*/
static void pwmp_adjust_hd_rough_position100(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
{
    uint16_t current_adc = 0;
    // 找最大推动值arr
    if (!FSM_IS_WAIT(*state))
    {
        /*上一步赋值的count*/
        if (mode_pwmp_hd_adjust->wait_count-- > 0)
        {
            // 等待放气完毕
            return;
        }
        sys_millis_reset();
        if (udevice.value_action_type == ATO) // 若调节阀为气开阀
        {
            mode_pwmp_hd_adjust->all_open_time_full = sys_millis(); // 记录满信号全开起始时间
        }
        else
        {
            mode_pwmp_hd_adjust->all_close_time_full = sys_millis(); // 记录满信号全关起始时间
        }
        mode_pwmp_hd_adjust->tmp_time = 0;

        pdctrl_out(mode_pwmp_hd_adjust->arr_record_2);                       // 以最大值进行输出
        mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 50; // 等待时间设置为5s
        mode_pwmp_hd_adjust->adjust_state = (mode_pwmp_hd_adjust_state_e)*state;
        FSM_WAIT(*state); // 设置等待状态
    }
    else
    {
        valve_position_change_e s = pwmp_adjust_hd_valve_position_change(state, next_state, DIFF_ADC);
        current_adc = pid_autotune_hd->data.adjust_tmp_actual;

        switch (s)
        {
        case POSITION_NO_CHANGE_FOREVER:
            // 位置不再改变，记录此时阀门位置AD值
            mode_pwmp_hd_adjust->adc_record_2 = current_adc;
            if (udevice.value_action_type == ATO) // 若调节阀为气开阀
            {
                mode_pwmp_hd_adjust->tmp_time = sys_millis();
                mode_pwmp_hd_adjust->all_open_time_full = mode_pwmp_hd_adjust->tmp_time - mode_pwmp_hd_adjust->all_open_time_full;
            }
            else // 若调节阀为气关阀
            {
                mode_pwmp_hd_adjust->tmp_time = sys_millis();
                mode_pwmp_hd_adjust->all_close_time_full = mode_pwmp_hd_adjust->tmp_time - mode_pwmp_hd_adjust->all_close_time_full;
            }
            break;
        case POSITION_CHANGE:
            mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 50;
            mode_pwmp_hd_adjust->tmp_time = 0;
            break;
        case POSITION_NO_CHANGE:
            if (mode_pwmp_hd_adjust->tmp_time == 0)
            {
                mode_pwmp_hd_adjust->tmp_time = sys_millis();
            }
            break;
        default:
            break;
        }
    }
}

/*排气值整定*/
static void pwmp_adjust_hd_bleeding_position0(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
{
    if (!FSM_IS_WAIT(*state))
    {
        mode_pwmp_hd_adjust->adjust_state = (mode_pwmp_hd_adjust_state_e)*state;
        FSM_WAIT(*state); // 设置等待状态
    }
    else
    {
        valve_position_change_e s = pwmp_adjust_hd_valve_position_change(state, next_state, DIFF_ADC);
        mode_pwmp_hd_adjust->psb_adc = pid_autotune_hd->data.adjust_tmp_actual;

        switch (s)
        {
        case POSITION_NO_CHANGE_FOREVER: // 位置不再改变
        {

            break;
        }
        case POSITION_CHANGE:

            break;
        default:
            break;
        }
    }
}

/*精调冲顶值*/
static void pwmp_adjust_hd_accurate_position100(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
{
    if (!FSM_IS_WAIT(*state))
    {
        mode_pwmp_hd_adjust->all_bleed_time_flag = true;
        mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_record_1; // 初始值为前一步整定出的最小启动值
        mode_pwmp_hd_adjust->arr_last = mode_pwmp_hd_adjust->arr_current;
        mode_pwmp_hd_adjust->adjust_state = (mode_pwmp_hd_adjust_state_e)*state;
        mode_pwmp_hd_adjust->preheat = TRUE;
        FSM_WAIT(mode_pwmp_hd_adjust->adjust_state); // 等放气完毕就可以切入到else里面了
        *state = PWMP_HD_ADJUST_BLEEDING;
    }
    else
    {
        valve_position_change_e s = pwmp_adjust_hd_valve_position_change(state, next_state, DIFF_ADC);
        mode_pwmp_hd_adjust->psb_adc = pid_autotune_hd->data.adjust_tmp_actual;

        /*当前反馈AD值与最大位置AD值的差值*/
        uint16_t adc_diff = ABS(mode_pwmp_hd_adjust->psb_adc - mode_pwmp_hd_adjust->adc_record_2);

        if (mode_pwmp_hd_adjust->arr_current == 0 && mode_pwmp_hd_adjust->preheat == TRUE)
        {
            // 放气完毕会进入到这里
            mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_record_1 + 20;
            pdctrl_out(mode_pwmp_hd_adjust->arr_current);
            mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 30;
            return;
        }

        switch (s)
        {
        case POSITION_NO_CHANGE_FOREVER: // 位置不再改变
        {
            if (adc_diff <= 10)
            {
                *state = next_state;
                // 记录在100阀位的adc值
                mode_pwmp_hd_adjust->arr_record_2 = mode_pwmp_hd_adjust->arr_current;
                // 记录在100阀位时的电流大小
                if (udevice.value_action_type == ATO)
                {
                    mode_pwmp_hd_adjust->current_100 = ip2current();
                }
                else
                {
                    mode_pwmp_hd_adjust->current_0 = ip2current();
                }
            }
            else
            {
                uint8_t offset = 0;

                /*每次增大的信号值*/
                offset = 3;

                /*若快到顶时等待10s，否则等待1s*/
                if (adc_diff < 20)
                {
                    mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 100;
                }
                else
                {
                    /*40mm————30；16mm————40*/
                    mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 30;
                }
                mode_pwmp_hd_adjust->last_adc = mode_pwmp_hd_adjust->psb_adc;
                mode_pwmp_hd_adjust->arr_last = mode_pwmp_hd_adjust->arr_current;
                mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_current + offset;

                pdctrl_out(mode_pwmp_hd_adjust->arr_current);
                *state = PWMP_HD_ADJUST_ACCURATE_POSITION100;
                FSM_WAIT(*state); // 设置等待状态
            }

            break;
        }
        case POSITION_CHANGE:
            if (adc_diff < 20)
            {
                mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 100;
            }
            else
            {
                mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 20;
            }

            break;
        default:
            break;
        }
    }
}

/*计算控制的全关时间：以ATO类型角度命名*/
static void pwmp_adjust_hd_all_close_time(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
{
    if (!FSM_IS_WAIT(*state))
    {
        mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_record_0 - 100; // 此处的输入信号大小根据实际控制的最小值来设定

        /*记录起始的全开时间*/
        sys_millis_reset();
        if (udevice.value_action_type == ATO)
        {
            mode_pwmp_hd_adjust->all_close_time = sys_millis();
        }
        else
        {
            mode_pwmp_hd_adjust->all_open_time = sys_millis();
        }
        mode_pwmp_hd_adjust->tmp_time = 0;

        pdctrl_out(mode_pwmp_hd_adjust->arr_current);
        FSM_WAIT(*state); // 设置等待状态
    }
    else
    {
        mode_pwmp_hd_adjust->psb_adc = get_actual_travel_adc();
        if (abs(mode_pwmp_hd_adjust->psb_adc - mode_pwmp_hd_adjust->adc_record_0) < DIFF_ADC)
        {
            if (udevice.value_action_type == ATO) // 若调节阀为气开阀
            {
                mode_pwmp_hd_adjust->tmp_time = sys_millis();
                mode_pwmp_hd_adjust->all_close_time = mode_pwmp_hd_adjust->tmp_time - mode_pwmp_hd_adjust->all_close_time;
            }
            else // 若调节阀为气关阀
            {
                mode_pwmp_hd_adjust->tmp_time = sys_millis();
                mode_pwmp_hd_adjust->all_open_time = mode_pwmp_hd_adjust->tmp_time - mode_pwmp_hd_adjust->all_close_time;
            }
            *state = next_state;
        }
    }
}

/*计算控制的全开时间*/
static void pwmp_adjust_hd_all_open_time(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
{
    if (!FSM_IS_WAIT(*state))
    {
        mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_record_2 + 100; // 此处的输入信号大小根据实际控制的最大值来设定

        /*记录起始的全开时间*/
        sys_millis_reset();
        if (udevice.value_action_type == ATO)
        {
            mode_pwmp_hd_adjust->all_open_time = sys_millis();
        }
        else
        {
            mode_pwmp_hd_adjust->all_close_time = sys_millis();
        }
        mode_pwmp_hd_adjust->tmp_time = 0;

        pdctrl_out(mode_pwmp_hd_adjust->arr_current);
        FSM_WAIT(*state); // 设置等待状态
    }
    else
    {
        mode_pwmp_hd_adjust->psb_adc = get_actual_travel_adc();
        if (abs(mode_pwmp_hd_adjust->psb_adc - mode_pwmp_hd_adjust->adc_record_2) < DIFF_ADC)
        {
            if (udevice.value_action_type == ATO)
            {
                mode_pwmp_hd_adjust->tmp_time = sys_millis();
                mode_pwmp_hd_adjust->all_open_time = mode_pwmp_hd_adjust->tmp_time - mode_pwmp_hd_adjust->all_open_time;
            }
            else
            {
                mode_pwmp_hd_adjust->tmp_time = sys_millis();
                mode_pwmp_hd_adjust->all_close_time = mode_pwmp_hd_adjust->tmp_time - mode_pwmp_hd_adjust->all_close_time;
            }
            *state = next_state;
        }
    }
}

/*计算阀门自整定参数*/
static void pwmp_adjust_hd_calculate(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
{
    if (!FSM_IS_WAIT(*state))
    {
        /*行程上下限*/
        mode_pwmp_hd->pwmp_save->storage.trip_0 = mode_pwmp_hd_adjust->adc_record_0;
        mode_pwmp_hd->pwmp_save->storage.trip_100 = mode_pwmp_hd_adjust->adc_record_2;
        mode_pwmp_hd->pwmp_save->storage.ad_diff = ABS(mode_pwmp_hd->pwmp_save->storage.trip_0 - mode_pwmp_hd->pwmp_save->storage.trip_100);

        /*控制区间*/
        mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin = mode_pwmp_hd_adjust->arr_record_2;
        mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin = mode_pwmp_hd_adjust->arr_record_0;
        mode_pwmp_hd->pwmp_save->storage.arr_diff = ABS(mode_pwmp_hd_adjust->arr_record_2 - mode_pwmp_hd_adjust->arr_record_0);
        mode_pwmp_hd->pwmp_save->storage.pwmp_max = mode_pwmp_hd_adjust->arr_record_2 + (mode_pwmp_hd->pwmp_save->storage.arr_diff);
        mode_pwmp_hd->pwmp_save->storage.pwmp_min = mode_pwmp_hd_adjust->arr_record_0 - (mode_pwmp_hd->pwmp_save->storage.arr_diff);

        /*启动值*/
        mode_pwmp_hd->pwmp_save->storage.startup_value = mode_pwmp_hd_adjust->arr_record_1;

        /*0与100阀位位置*/
        mode_pwmp_hd->pwmp_save->storage.current0 = mode_pwmp_hd_adjust->current_0;
        mode_pwmp_hd->pwmp_save->storage.current100 = mode_pwmp_hd_adjust->current_100;

        /*全开及全关时间*/
        mode_pwmp_hd->pwmp_save->storage.all_open_time = mode_pwmp_hd_adjust->all_open_time;
        mode_pwmp_hd->pwmp_save->storage.all_close_time = mode_pwmp_hd_adjust->all_close_time;
        mode_pwmp_hd->pwmp_save->storage.all_open_time_full = mode_pwmp_hd_adjust->all_open_time_full;
        mode_pwmp_hd->pwmp_save->storage.all_close_time_full = mode_pwmp_hd_adjust->all_close_time_full;

        /*判断调节阀类型（气开/气关），对最低行程和最高行程进行标定*/
        if (udevice.value_action_type == ATO)
        {
            mode_pwmp_hd->pwmp_save->storage.trip_100 = mode_pwmp_hd->pwmp_save->storage.trip_100;
            mode_pwmp_hd->pwmp_save->storage.trip_0 = mode_pwmp_hd->pwmp_save->storage.trip_0;
        }
        else
        {
            uint16_t tmp = mode_pwmp_hd->pwmp_save->storage.trip_100;
            mode_pwmp_hd->pwmp_save->storage.trip_100 = mode_pwmp_hd->pwmp_save->storage.trip_0;
            mode_pwmp_hd->pwmp_save->storage.trip_0 = tmp;
        }

        // 保存安装方向
        if (mode_pwmp_hd->pwmp_save->storage.trip_100 > mode_pwmp_hd->pwmp_save->storage.trip_0) // 上大下小，正装
        {
            udevice.dev_install_dir = 1;
        }
        else // 充气，ADC减小，正装
        {
            udevice.dev_install_dir = 0;
        }

        udevice.pos0_travel_vol = mode_pwmp_hd->pwmp_save->storage.trip_0;
        udevice.pos100_travel_vol = mode_pwmp_hd->pwmp_save->storage.trip_100;
        udevice.output_min = mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin;
        udevice.output_max = mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin;
        calib_parapos_perent();

        mode_pwmp_hd_adjust->adjust_state = (mode_pwmp_hd_adjust_state_e)*state;
        FSM_WAIT(mode_pwmp_hd_adjust->adjust_state); // 等放气完毕就可以切入到else里面了
        *state = PWMP_HD_ADJUST_BLEEDING;
    }
    else
    {
        *state = next_state;
    }
}
/*排气状态*/
static void pwmp_adjust_hd_bleeding(uint8_t *state)
{
    if (!FSM_IS_WAIT(*state))
    {
        mode_pwmp_hd_adjust->arr_current = mode_pwmp_hd_adjust->arr_default;
        mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 80;
        if (mode_pwmp_hd_adjust->all_bleed_time_flag == true)
        {
            sys_millis_reset();
            if (udevice.value_action_type == ATO)
            {
                mode_pwmp_hd_adjust->all_close_time_full = sys_millis();
            }
            else
            {
                mode_pwmp_hd_adjust->all_open_time_full = sys_millis();
            }
            mode_pwmp_hd_adjust->tmp_time = 0;
        }
        pdctrl_out(0);
        FSM_WAIT(*state); // 设置等待状态
    }
    else
    {
        valve_position_change_e s = pwmp_adjust_hd_valve_position_change(state, mode_pwmp_hd_adjust->adjust_state, DIFF_ADC);
        mode_pwmp_hd_adjust->psb_adc = mode_pwmp_hd_adjust->psb_adc;
        switch (s)
        {
        case POSITION_NO_CHANGE_FOREVER:
            if (mode_pwmp_hd_adjust->all_bleed_time_flag == true)
            {
                if (udevice.value_action_type == ATO) // 若调节阀为气开阀
                {
                    mode_pwmp_hd_adjust->tmp_time = sys_millis();
                    mode_pwmp_hd_adjust->all_close_time_full = mode_pwmp_hd_adjust->tmp_time - mode_pwmp_hd_adjust->all_close_time_full;
                }
                else // 若调节阀为气关阀
                {
                    mode_pwmp_hd_adjust->tmp_time = sys_millis();
                    mode_pwmp_hd_adjust->all_open_time_full = mode_pwmp_hd_adjust->tmp_time - mode_pwmp_hd_adjust->all_open_time_full;
                }
                mode_pwmp_hd_adjust->all_bleed_time_flag = false;
            }
            mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 50; // 排气状态结束后在跳转后的状态中等待5s
            mode_pwmp_hd_adjust->arr_current = 0;
            break;
        case POSITION_CHANGE:
            if (mode_pwmp_hd_adjust->adc_record_0 == 0)
            {
                mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 80;
            }
            else
            {
                mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 50;
            }
            mode_pwmp_hd_adjust->tmp_time = 0;
            break;
        case POSITION_NO_CHANGE:
            break;
        default:
            pwmp_hd_process_state_set(PWMP_HD_PROCESS_ADJUST_STOP);
            break;
        }
    }
}

/*停止模式*/
static void pwmp_adjust_hd_stop()
{
    if (mode_pwmp_hd_adjust != NULL)
    {
        osel_mem_free(mode_pwmp_hd_adjust);
        mode_pwmp_hd_adjust = NULL;
    }
    if (mode_pwmp_hd->auto_tune_state == PWMP_HD_ADJUST_RESULT_TUNING)
    {
        mode_pwmp_hd->auto_tune_state = PWMP_HD_ADJUST_RESULT_IDEL;
    }
    // 整定结束，将模式设置为控制模式
    pwmp_hd_process_state_set(PWMP_HD_PROCESS_CONTROL);
}

/*整定错误状态*/
static void pwmp_adjust_hd_fail(uint8_t *state)
{
    if (mode_pwmp_hd_adjust->arr_record_1 == 0)
    {
        // 阀位反馈错误
    }
    // 反馈结果（整定错误）
    mode_pwmp_hd->auto_tune_state = PWMP_HD_ADJUST_RESULT_FAIL;
}

/*PID参数整定模式*/
// 通过整定获得K、T、L（tao）
static void pwmp_adjust_hd_PID_tuning(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
{
    if (!FSM_IS_WAIT(*state))
    {
        /*记录运行时间*/
        sys_millis_reset();
        pid_autotune_hd->data.pid_autotune_time_origin = sys_millis(); // 记录整定起始时间
        mode_pwmp_hd_adjust->tmp_time = 0;
        /*整定方法选择*/
        pid_autotune_way_set(PID_AUTOTUNE_WAY_ZN);
        /*阶跃信号设置*/
        pid_autotune_hd->data.step_signal = mode_pwmp_hd_adjust->arr_record_2;
        pdctrl_out(pid_autotune_hd->data.step_signal);
        mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 40; // 等待四秒
        FSM_WAIT(*state);                                                    // 设置等待状态
    }
    else
    {
        valve_position_change_e s = pwmp_adjust_hd_valve_position_change(state, next_state, DIFF_ADC);
        /*获取实时行程*/
        pid_autotune_hd->data.cur_actual = pid_autotune_hd->data.adjust_tmp_actual;
        /*计算变化率*/
        pid_autotune_hd->data.actual_error = pid_autotune_hd->data.cur_actual - pid_autotune_hd->data.pre_actual;
        if (pid_autotune_hd->data.actual_error > 1000)
        {
            pid_autotune_hd->data.slope = 0;
        }
        else
        {
            pid_autotune_hd->data.slope = pid_autotune_hd->data.actual_error / TIME_CYCLE;
        }
        /*记录下变化率最大点值及行程坐标、时间*/
        if (pid_autotune_hd->data.slope > pid_autotune_hd->data.slope_Max)
        {
            pid_autotune_hd->data.slope_Max = pid_autotune_hd->data.slope;
            pid_autotune_hd->data.actual_slope_Max = pid_autotune_hd->data.cur_actual;
            mode_pwmp_hd_adjust->tmp_time = sys_millis();
            pid_autotune_hd->data.time_slope_Max = mode_pwmp_hd_adjust->tmp_time - pid_autotune_hd->data.pid_autotune_time_origin;
        }
        /*保存前一次的行程*/
        pid_autotune_hd->data.pre_actual = pid_autotune_hd->data.cur_actual;
        /*只有当阀位大于前端*/
        if (pid_autotune_hd->data.cur_actual > 2)
        {
            switch (s)
            {
            case POSITION_NO_CHANGE_FOREVER:
                /*获取稳定时的行程*/
                pid_autotune_hd->data.actual_stable = pid_autotune_hd->data.cur_actual;
                /*计算切线的k和b*/
                pid_autotune_hd->data.k = pid_autotune_hd->data.slope_Max;
                pid_autotune_hd->data.b = pid_autotune_hd->data.actual_slope_Max - (pid_autotune_hd->data.k * pid_autotune_hd->data.time_slope_Max);
                /*求得滞后时间L*/
                pid_autotune_hd->l_hd = get_time_tangent(0);
                /*求得时间系数T*/
                pid_autotune_hd->t_hd = get_time_tangent(pid_autotune_hd->data.actual_stable) - pid_autotune_hd->l_hd;
                /*K的计算*/
                pid_autotune_hd->k_hd = ((pid_autotune_hd->data.actual_stable) / (mode_pwmp_hd_adjust->adc_record_2 - mode_pwmp_hd_adjust->adc_record_0)) / ((pid_autotune_hd->data.step_signal) / (mode_pwmp_hd_adjust->arr_record_2 - mode_pwmp_hd_adjust->arr_record_0));

                *state = next_state;
                break;
            case POSITION_CHANGE:
                mode_pwmp_hd_adjust->wait_count = mode_pwmp_hd->wait_count_max * 40;
                break;
            case POSITION_NO_CHANGE:
                break;
            default:
                pwmp_hd_process_state_set(PWMP_HD_PROCESS_ADJUST_STOP);
                break;
            }
        }
    }
}

/*参数计算模式*/
static void pwmp_adjust_hd_pid_calculate(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
{
    switch (pid_autotune_hd->autotune_way)
    {
    case PID_AUTOTUNE_WAY_ZN:
        if ((pid_autotune_hd->l_hd / pid_autotune_hd->t_hd) <= 0.2)
        {
            pid_autotune_hd->p_auto_hd = (pid_autotune_hd->t_hd / (0.85 * pid_autotune_hd->l_hd * pid_autotune_hd->k_hd));
            pid_autotune_hd->ti_auto_hd = 2 * pid_autotune_hd->l_hd;
            pid_autotune_hd->td_auto_hd = 0.5 * pid_autotune_hd->l_hd;
        }
        else if ((pid_autotune_hd->l_hd / pid_autotune_hd->t_hd) > 0.2)
        {
            pid_autotune_hd->p_auto_hd = ((1 / pid_autotune_hd->k_hd) * (pid_autotune_hd->l_hd / pid_autotune_hd->t_hd) + 0.6) / (2.6 * (pid_autotune_hd->l_hd / pid_autotune_hd->t_hd) - 0.08);
            pid_autotune_hd->ti_auto_hd = 0.18 * pid_autotune_hd->t_hd + 0.19 * pid_autotune_hd->l_hd;
            pid_autotune_hd->td_auto_hd = 0.25 * pid_autotune_hd->ti_auto_hd;
        }
        pid_autotune_hd->i_auto_hd = pid_autotune_hd->p_auto_hd / pid_autotune_hd->ti_auto_hd;
        pid_autotune_hd->d_auto_hd = (pid_autotune_hd->td_auto_hd * pid_autotune_hd->p_auto_hd) / pid_autotune_hd->t_hd;
        break;
    case PID_AUTOTUNE_WAY_CC:
        /*Kp、Ti、Td的求取*/
        pid_autotune_hd->p_auto_hd = ((pid_autotune_hd->t_hd / (pid_autotune_hd->k_hd * pid_autotune_hd->l_hd)) * (4 / 3 + (pid_autotune_hd->l_hd / (pid_autotune_hd->t_hd * 4))));
        pid_autotune_hd->ti_auto_hd = pid_autotune_hd->l_hd * ((32 + 6 * (pid_autotune_hd->l_hd / pid_autotune_hd->t_hd)) / (13 + 8 * (pid_autotune_hd->l_hd / pid_autotune_hd->l_hd)));
        pid_autotune_hd->td_auto_hd = (4 * pid_autotune_hd->l_hd) / (11 + 2 * (pid_autotune_hd->l_hd / pid_autotune_hd->t_hd));
        /*Ki、Kd的求取*/
        pid_autotune_hd->i_auto_hd = pid_autotune_hd->p_auto_hd / pid_autotune_hd->ti_auto_hd;
        pid_autotune_hd->d_auto_hd = (pid_autotune_hd->td_auto_hd * pid_autotune_hd->p_auto_hd) / pid_autotune_hd->t_hd;
        break;
    default:
        break;
    }
    mode_pwmp_hd->pwmp_save->storage.kp = fabs(pid_autotune_hd->p_auto_hd);
    mode_pwmp_hd->pwmp_save->storage.ki = fabs(pid_autotune_hd->i_auto_hd);
    mode_pwmp_hd->pwmp_save->storage.kd = fabs(pid_autotune_hd->d_auto_hd);
    *state = next_state; // 将状态切换
}

/**/

/*参数保存模式*/
static void pwmp_adjust_hd_save(uint8_t *state, mode_pwmp_hd_adjust_state_e next_state)
{
    *state = next_state;
    if (mode_pwmp_hd->pwmp_save->storage.tuned_flag == TUNED_ONGOING)
    {
        mode_pwmp_hd->pwmp_save->storage.tuned_flag = TUNED_SUCCESS;
        mode_pwmp_hd->auto_tune_state = PWMP_HD_ADJUST_RESULT_SUCCESS;
        pwmp_public_params_update();
    }
    else
    {
        mode_pwmp_hd->pwmp_save->storage.tuned_flag = TUNED_FAILED;
    }
    mode_pwmp_hd->params_save_cb();
}

//////////////////////////////////*状态机模式END*//////////////////////////////////////////////////////

//////////////////////////////*整定程序入口BEGIN*//////////////////////////////////////////////////////

void pwmp_adjust(uint8_t *state)
{
    uint8_t ts = *state;
    BIT_CLR(ts, BIT7); // 高位清零
    switch (ts)
    {
    case PWMP_HD_ADJUST_ROUGH_POSITION0:
        pwmp_adjust_hd_rough_position0(state, PWMP_HD_ADJUST_INITIATE);
        break;
    case PWMP_HD_ADJUST_INITIATE:
        pwmp_adjust_hd_initiate_position0(state, PWMP_HD_ADJUST_ROUGH_POSITION100);
        break;
    case PWMP_HD_ADJUST_ROUGH_POSITION100:
        pwmp_adjust_hd_rough_position100(state, PWMP_HD_ADJUST_BLEEDING_POSITION0);
        break;
    case PWMP_HD_ADJUST_BLEEDING_POSITION0:
        pwmp_adjust_hd_bleeding_position0(state, PWMP_HD_ADJUST_ACCURATE_POSITION100);
        break;
    case PWMP_HD_ADJUST_ACCURATE_POSITION100:
        pwmp_adjust_hd_accurate_position100(state, PWMP_HD_ADJUST_ALL_CLOSE_TIME);
        break;
    case PWMP_HD_ADJUST_ALL_CLOSE_TIME:
        pwmp_adjust_hd_all_close_time(state, PWMP_HD_ADJUST_ALL_OPEN_TIME);
        break;
    case PWMP_HD_ADJUST_ALL_OPEN_TIME:
        pwmp_adjust_hd_all_open_time(state, PWMP_HD_ADJUST_CALCULATE);
        break;
    case PWMP_HD_ADJUST_CALCULATE:
        pwmp_adjust_hd_calculate(state, PWMP_HD_ADJUST_PID_TUNING);
        break;
    case PWMP_HD_ADJUST_PID_TUNING:
        pwmp_adjust_hd_PID_tuning(state, PWMP_HD_ADJUST_PID_CALCULATE);
        break;
    case PWMP_HD_ADJUST_PID_CALCULATE:
        pwmp_adjust_hd_pid_calculate(state, PWMP_HD_ADJUST_SAVE);
        break;
    case PWMP_HD_ADJUST_SAVE:
        pwmp_adjust_hd_save(state, PWMP_HD_ADJUST_STOP);
        break;
    case PWMP_HD_ADJUST_BLEEDING:
        pwmp_adjust_hd_bleeding(state);
        break;
    case PWMP_HD_ADJUST_IDEL:
        pwmp_adjust_hd_idle(state, PWMP_HD_ADJUST_ROUGH_POSITION0);
        break;
    case PWMP_HD_ADJUST_STOP:
        pwmp_hd_process_state_set(PWMP_HD_PROCESS_ADJUST_STOP);
        break;
    case PWMP_HD_ADJUST_FAIL:
        pwmp_adjust_hd_fail(state);
        pwmp_hd_process_state_set(PWMP_HD_PROCESS_ADJUST_STOP);
        break;
    default:
        pwmp_hd_process_state_set(PWMP_HD_PROCESS_ADJUST_STOP);
        break;
    }

    switch (ts)
    {
    case PWMP_HD_ADJUST_IDEL:
        mode_pwmp_hd->lcd_adjust_state = LCD_ADJUST_IDEL;
        break;
    case PWMP_HD_ADJUST_ROUGH_POSITION0:
        mode_pwmp_hd->lcd_adjust_state = LCD_ADJUST_POSITION0;
        break;
    case PWMP_HD_ADJUST_ACCURATE_POSITION0:
        mode_pwmp_hd->lcd_adjust_state = LCD_ADJUST_POSITION0;
        break;
    case PWMP_HD_ADJUST_ROUGH_POSITION100:
        mode_pwmp_hd->lcd_adjust_state = LCD_ADJUST_POSITION100;
        break;
    case PWMP_HD_ADJUST_ACCURATE_POSITION100:
        mode_pwmp_hd->lcd_adjust_state = LCD_ADJUST_POSITION100;
        break;
    case PWMP_HD_ADJUST_BLEEDING_TIMER:
        mode_pwmp_hd->lcd_adjust_state = LCD_ADJUST_TIMER;
        break;
    case PWMP_HD_ADJUST_AERATE_TIMER:
        mode_pwmp_hd->lcd_adjust_state = LCD_ADJUST_TIMER;
        break;
    case PWMP_HD_ADJUST_TUNING:
        mode_pwmp_hd->lcd_adjust_state = LCD_ADJUST_TUNING;
        break;
    case PWMP_HD_ADJUST_SAVE:
        mode_pwmp_hd->lcd_adjust_state = LCD_ADJUST_SAVE;
        break;
    case PWMP_HD_ADJUST_STOP:
        mode_pwmp_hd->lcd_adjust_state = LCD_ADJUST_STOP;
        break;
    default:
        break;
    }
}
//////////////////////////////*整定程序入口END*//////////////////////////////////////////////////////

//////////////////////////////*测试模式开始*///////////////////////////////////////////////////////////////////
/*整定过程中判断阀门状态：移动、停止、停止但仍在等待*/
/*
static valve_position_change_e pwmp_test_hd_valve_position_change(uint8_t *state, uint8_t next_state, uint8_t diff_adc_max)
{
    uint16_t diff_adc = 0;
    uint16_t adc = mode_pwmp_hd_test->psb_adc;

    if (mode_pwmp_hd_test->psb_adc != adc)
    {
        diff_adc = ABS(mode_pwmp_hd_test->psb_adc - adc);
    }

    if (diff_adc > diff_adc_max)
    {
        // 位置还在改变，继续等待
        mode_pwmp_hd_test->psb_adc = adc;
        return POSITION_CHANGE;
    }
    else
    {
        // 位置不变
        if (mode_pwmp_hd_test->wait_count == 0)
        {
            // 等待次数也到了，位置不再发生变化，可以切换下一个状态
            *state = next_state;
            return POSITION_NO_CHANGE_FOREVER;
        }
        else
        { // 当位置不变的时候才开始等待
            mode_pwmp_hd_test->wait_count--;
            return POSITION_NO_CHANGE;
        }
    }
}
*/
static void pwmp_test_hd_velocity_test(uint8_t *state, mode_pwmp_hd_test_state_e next_state)
{
    pwmp_control_update(FILTER_AVERAGE);
    if (!FSM_IS_WAIT(*state))
    {
        mode_pwmp_hd_test->arr_current = udevice.output_max; // 以最大的信号值输出
        pdctrl_out(mode_pwmp_hd_test->arr_current);
        FSM_WAIT(*state); // 设置等待状态
    }
    else
    {

        if (abs(mode_pwmp_hd_test->psb_adc - udevice.pos0_travel_vol) < DIFF_ADC_MAX)
        {
            *state = next_state;
        }
    }
}
/*开环控制*/
static void pwmp_test_hd_open_loop_output()
{
    pwmp_control_update(FILTER_AVERAGE);
    pdctrl_out(mode_pwmp_hd->output);
    mode_pwmp_hd->duty_percent = ((float32)mode_pwmp_hd->output / (float32)DAC_MAX) * 100;
    mode_pwmp_hd->current_electric = ip2current();
}

/*采样得到小行程最大值*/
static void pwmp_test_hd_get_small_travel_high(uint8_t *state, mode_pwmp_hd_test_small_travel_state_e next_state)
{
    pwmp_control_update(FILTER_AVERAGE);
    if (!FSM_IS_WAIT(*state))
    {
        mode_pwmp_hd_test->arr_current = 4095; // 以最大的信号值输出
        pdctrl_out(mode_pwmp_hd_test->arr_current);
        FSM_WAIT(*state); // 设置等待状态
    }
    else
    {
        if (mode_pwmp_hd_test->small_trip_current_adc > mode_pwmp_hd_test->small_trip_adc_max)
        {
            mode_pwmp_hd_test->small_trip_adc_max = mode_pwmp_hd_test->small_trip_current_adc;
        }
        if (abs(mode_pwmp_hd_test->psb_adc - udevice.pos0_travel_vol) < DIFF_ADC_MAX)
        {
            *state = next_state;
        }
    }
}

/*采样得到小行程最小值*/
static void pwmp_test_hd_get_small_travel_low(uint8_t *state, mode_pwmp_hd_test_small_travel_state_e next_state)
{
    pwmp_control_update(FILTER_AVERAGE);
    if (!FSM_IS_WAIT(*state))
    {
        mode_pwmp_hd_test->small_trip_adc_min = 4095; // 因为初始化将其赋值为0，为防止出现bug故将其初始化为4095
        mode_pwmp_hd_adjust->arr_current = 0;         // 以最小的信号值输出
        pdctrl_out(mode_pwmp_hd_adjust->arr_current);
        FSM_WAIT(*state); // 设置等待状态
    }
    else
    {
        if (mode_pwmp_hd_test->small_trip_current_adc < mode_pwmp_hd_test->small_trip_adc_min)
        {
            mode_pwmp_hd_test->small_trip_adc_min = mode_pwmp_hd_test->small_trip_current_adc;
        }
        if (abs(mode_pwmp_hd_test->psb_adc - udevice.pos0_travel_vol) < DIFF_ADC_MAX)
        {
            *state = next_state;
        }
    }
}

/*采样得到处于50%阀位时稳定的控制信号*/
static void pwmp_test_hd_get_small_travel_50_per_arr(uint8_t *state, mode_pwmp_hd_test_small_travel_state_e next_state)
{
    execute_rsp_hd_t *execute_res;
    if (!FSM_IS_WAIT(*state))
    {
        mode_pwmp_hd_test->ctrl_target = 50.0; // 将目标阀位设置为50%
        mode_pwmp_hd_test->wait_count = 2000;  // 等待20秒
        FSM_WAIT(*state);                      // 设置等待状态
    }
    else
    {
        pwmp_control_update(FILTER_MEDIAN);
        if (rt_data.loop_current >= 4.0)
        {
            execute_res = execute_dac(EXECUTE_PLAN); // PID计算结果经过执行器处理
            if (execute_res->code == EXECUTE_HD_OUT) // 执行DAC输出
            {
                pdctrl_out(execute_res->dac);
            }
        }
        /*当误差大于0.5%时重置等待时间*/
        if (fabs(mode_pwmp_hd_test->real_error) > 0.5)
        {
            mode_pwmp_hd_test->wait_count = 2000;
        }
        else
        {
            mode_pwmp_hd_test->wait_count--;
            if (mode_pwmp_hd_test->wait_count == 0)
            {
                mode_pwmp_hd_test->arr_50_percent = execute_res->dac;
                mode_pwmp_hd_test->small_trip_adc_50_percent = mode_pwmp_hd_test->small_trip_current_adc;
                *state = next_state;
            }
        }
    }
}

/*得到小行程稳定区间上限*/
static void pwmp_test_hd_get_small_travel_stable_range_up(uint8_t *state, mode_pwmp_hd_test_small_travel_state_e next_state)
{
    pwmp_control_update(FILTER_AVERAGE);
    if (!FSM_IS_WAIT(*state))
    {
        mode_pwmp_hd_test->wait_count = 400;                                // 等待8秒
        mode_pwmp_hd_test->ctrl_target = 50.0;                              // 将目标阀位设置为50%
        mode_pwmp_hd_test->arr_current = mode_pwmp_hd_test->arr_50_percent; // 以50%阀位稳定信号值输出
        pdctrl_out(mode_pwmp_hd_test->arr_current);
        FSM_WAIT(*state); // 设置等待状态
    }
    else
    {
        if (fabs(mode_pwmp_hd_test->real_error) <= 0.5)
        {
            if (!(mode_pwmp_hd_test->wait_count--))
            {
                mode_pwmp_hd_test->arr_current++;
                mode_pwmp_hd_test->flag_stable = 1;
                mode_pwmp_hd_test->wait_count = 400;
            }
        }
        else
        {
            if (mode_pwmp_hd_test->flag_stable)
            {
                mode_pwmp_hd_test->small_trip_adc_stable_up = mode_pwmp_hd_test->small_trip_current_adc;
                mode_pwmp_hd_test->wait_count = 800; // 等待8秒
                *state = next_state;
            }
        }
        pdctrl_out(mode_pwmp_hd_test->arr_current);
    }
}

/*得到小行程稳定区间下限*/
static void pwmp_test_hd_get_small_travel_stable_range_down(uint8_t *state, mode_pwmp_hd_test_small_travel_state_e next_state)
{
    pwmp_control_update(FILTER_AVERAGE);
    if (!FSM_IS_WAIT(*state))
    {
        /*等待上一次延时的时间*/
        if (mode_pwmp_hd_test->wait_count-- > 0)
        {
            return;
        }
        mode_pwmp_hd_test->wait_count = 400;                                // 等待4秒
        mode_pwmp_hd_test->ctrl_target = 50.0;                              // 将目标阀位设置为50%
        mode_pwmp_hd_test->arr_current = mode_pwmp_hd_test->arr_50_percent; // 以50%阀位稳定信号值输出
        pdctrl_out(mode_pwmp_hd_test->arr_current);
        FSM_WAIT(*state); // 设置等待状态
    }
    else
    {
        if (fabs(mode_pwmp_hd_test->real_error) <= 0.5)
        {
            if (!(mode_pwmp_hd_test->wait_count--))
            {
                mode_pwmp_hd_test->arr_current--;
                mode_pwmp_hd_test->flag_stable = 1;
                mode_pwmp_hd_test->wait_count = 400;
            }
        }
        else
        {
            if (mode_pwmp_hd_test->flag_stable)
            {
                mode_pwmp_hd_test->small_trip_adc_stable_down = mode_pwmp_hd_test->small_trip_current_adc;
                *state = next_state;
            }
        }
        pdctrl_out(mode_pwmp_hd_test->arr_current);
    }
}

//////////////////////////////*测试模式结束*////////////////////////////////////////////////////////////////////////

//////////////////////////////*小回路测试模式入口*///////////////////////////////////////////////////////////////////
void small_travel_test(uint8_t *state)
{
    uint8_t ts = *state;
    BIT_CLR(ts, BIT7); // 高位清零
    switch (ts)
    {
    case PWMP_HD_TEST_GET_SMALL_TRAVEL_HIGH:
        pwmp_test_hd_get_small_travel_high(state, PWMP_HD_TEST_GET_SMALL_TRAVEL_LOW);
        break;
    case PWMP_HD_TEST_GET_SMALL_TRAVEL_LOW:
        pwmp_test_hd_get_small_travel_low(state, PWMP_HD_TEST_GET_SMALL_TRAVEL_50_PER_ARR);
        break;
    case PWMP_HD_TEST_GET_SMALL_TRAVEL_50_PER_ARR:
        pwmp_test_hd_get_small_travel_50_per_arr(state, PWMP_HD_TEST_GET_SMALL_TRAVEL_STABLE_RANGE_UP);
        break;
    case PWMP_HD_TEST_GET_SMALL_TRAVEL_STABLE_RANGE_UP:
        pwmp_test_hd_get_small_travel_stable_range_up(state, PWMP_HD_TEST_GET_SMALL_TRAVEL_STABLE_RANGE_DOWN);
        break;
    case PWMP_HD_TEST_GET_SMALL_TRAVEL_STABLE_RANGE_DOWN:
        pwmp_test_hd_get_small_travel_stable_range_down(state, PWMP_HD_TEST_OPEN_LOOP_OUTPUT_TEST);
        break;
    case PWMP_HD_TEST_OPEN_LOOP_OUTPUT_TEST:
        mode_pwmp_hd_test->test_state = PWMP_HD_TEST_OPEN_LOOP_OUTPUT;
        break;
    default:
        mode_pwmp_hd_test->test_state = PWMP_HD_TEST_OPEN_LOOP_OUTPUT;
        break;
    }
}
/////////////////////////////*小回路测试模式出口*////////////////////////////////////////////////////////////////////

//////////////////////////////*测试模式入口BEGIN*///////////////////////////////////////////////////////////////////
void pwmp_test(uint8_t *state)
{
    uint8_t ts = *state;
    ts = PWMP_HD_TEST_OPEN_LOOP_OUTPUT;
    BIT_CLR(ts, BIT7); // 高位清零
    switch (ts)
    {
    case VELOCITY_TEST:
        pwmp_test_hd_velocity_test(state, SMALL_TRAVEL_TEST);
        break;
    case SMALL_TRAVEL_TEST:
        small_travel_test((uint8_t *)&mode_pwmp_hd_test->small_travel_test_state);
    case PWMP_HD_TEST_OPEN_LOOP_OUTPUT:
        pwmp_test_hd_open_loop_output();
        break;
    default:
        pwmp_test_hd_open_loop_output();
        break;
    }
}
//////////////////////////////*测试模式入口END*/////////////////////////////////////////////////////////////////////

//////////////////////////////*入口BEGIN*//////////////////////////////////////////////////////
void mode_pwmp_hd_process(void)
{
    mode_pwmp_hd_control_t *p = &mode_pwmp_hd->control;
    execute_rsp_hd_t *execute_res;
    p->enter_count++;
    if (p->enter_count % WAIT_COUNT_MAX == 0) // 100ms输出一次
    {
        if (BIT_IS_SET(hc_24_state, BIT2))
        {
            // 蓝牙输出
            char ble_data[128];
            uint8_t ble_len = 0;
            osel_memset((uint8_t *)ble_data, 0, 128);
            // sprintf(ble_data, "%f,%f,%f,%d,%f\r\n", pid_target, show_actual, p->real_error, mode_pwm->output, rt_data.loop_current );
            if (mode_pwmp_hd->process_state == PWMP_HD_PROCESS_TEST)
            {
                // sprintf(ble_data, "%f,%f,%f,%f\r\n", mode_pwmp_hd->control.ctrl_target, mode_pwmp_hd->control.ctrl_feedback, mode_pwmp_hd->control.real_error, rt_data.loop_current );
                if (mode_pwmp_hd_test->test_state == PWMP_HD_TEST_OPEN_LOOP_OUTPUT)
                {
                    sprintf(ble_data, "%f,%d,%d,%f\r\n", mode_pwmp_hd->control.ctrl_feedback, mode_pwmp_hd_test->small_trip_current_adc, mode_pwmp_hd->output, mode_pwmp_hd->current_electric);
                }
                else
                {
                    sprintf(ble_data, "%f,%f,%f,%d,%d\r\n", mode_pwmp_hd_test->ctrl_feedback, mode_pwmp_hd_test->ctrl_target, mode_pwmp_hd_test->real_error, mode_pwmp_hd_test->small_trip_current_adc, rsp.dac);
                }
            }
            else if (mode_pwmp_hd->process_state == PWMP_HD_PROCESS_CONTROL || mode_pwmp_hd->process_state == PWMP_HD_PROCESS_STOP)
            {
                sprintf(ble_data, "%f,%f,%f,%d,%d\r\n", mode_pwmp_hd->control.ctrl_feedback, mode_pwmp_hd->control.ctrl_target, mode_pwmp_hd->control.real_error, mode_pwmp_hd->control.small_trip_current_adc, rsp.dac);
            }
            else if (mode_pwmp_hd->process_state == PWMP_HD_PROCESS_ADJUST)
            {
                sprintf(ble_data, "%d,%d\r\n", mode_pwmp_hd_adjust->psb_adc, mode_pwmp_hd_adjust->arr_current);
            }

            ble_len = osel_mstrlen((unsigned char *)ble_data);
            if (ble_len != 0)
            {
                h24_bluetooth_output_dbg((uint8_t *)ble_data, ble_len);
            }

            // 蓝牙输出
        }
        p->enter_count = 0;
    }

    switch (mode_pwmp_hd->process_state)
    {
    case PWMP_HD_PROCESS_CONTROL:
    {
        pwmp_control_update(FILTER_MEDIAN);
        if (rt_data.loop_current >= LOOP_CURRENT_MIN)
        {
            if (rt_data.loop_current < 4.5)
            {
                pdctrl_out(mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin * 0.8);
            }
            else if (rt_data.loop_current > 19.5)
            {
                pdctrl_out(mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin * 1.2);
            }
            else if (rt_data.loop_current >= 4.5 && rt_data.loop_current <= 19.5)
            {
                execute_res = execute_dac(EXECUTE_PLAN); // PID计算结果经过执行器处理
                if (execute_res->code == EXECUTE_HD_OUT) // 执行DAC输出
                {
                    pdctrl_out(execute_res->dac);
                }
            }
        }
        else
        {
            pdctrl_out(0);
        }
        break;
    }
    case PWMP_HD_PROCESS_ADJUST:
    {
        pwmp_adjust((uint8_t *)&mode_pwmp_hd->adjust_state);
        break;
    }
    case PWMP_HD_PROCESS_ADJUST_STOP:
    {
        pwmp_adjust_hd_stop();
        break;
    }
    case PWMP_HD_PROCESS_STOP:
    {
        mode_pwmp_hd_stop((mode_pwmp_hd_t *)&mode_pwmp_hd);
        break;
    }
    case PWMP_HD_PROCESS_TEST:
    {
        pwmp_test((uint8_t *)&mode_pwmp_hd_test->test_state);
        break;
    }
    default:
        break;
    }
}
//////////////////////////////*入口END*//////////////////////////////////////////////////////

/*程序初始化*/
void mode_pwmp_hd_init(mode_interface_req_t *req, uint16_t positioner_model, mode_pwmp_hd_params_u *params, void (*params_save_cb)(void))
{
    DBG_ASSERT(req != NULL __DBG_LINE);
    DBG_ASSERT(params != NULL __DBG_LINE); // 断言检查
    DBG_ASSERT(params_save_cb != NULL __DBG_LINE);
    req->mode_process_start = _process_start;
    req->mode_process_stop = _process_stop;
    req->mode_adjust_start = _adjust_start;
    req->mode_adjust_stop = _adjust_stop;
    req->mode_get_adjust_data = _adjust_data;
    req->mode_adjust_result = _adjust_result;
    req->mode_adjust_step_count = _adjust_step_count;
    req->mode_adjust_step_current = _adjust_step_current;
    req->mode_control_idle = _control_idle;
    req->mode_is_adjusting = _adjust_isrun;

    /*大电流输出使能开关*/
#if VIP_H_EN_HD == 1
    VIP_H_EN_ENABLE();
#endif
    /*为指针开辟存储空间*/
    // 控制
    if (mode_pwmp_hd == NULL)
    {
        mode_pwmp_hd = (mode_pwmp_hd_t *)osel_mem_alloc(sizeof(mode_pwmp_hd_t));
    }
    osel_memset((uint8_t *)mode_pwmp_hd, 0, sizeof(mode_pwmp_hd_t));
    // 整定
    if (mode_pwmp_hd_adjust == NULL)
    {
        mode_pwmp_hd_adjust = (pwmp_hd_adjust_t *)osel_mem_alloc(sizeof(pwmp_hd_adjust_t));
    }
    osel_memset((uint8_t *)mode_pwmp_hd_adjust, 0, sizeof(pwmp_hd_adjust_t));
    // 调试
    if (mode_pwmp_hd_test == NULL)
    {
        mode_pwmp_hd_test = (pwmp_hd_test_t *)osel_mem_alloc(sizeof(pwmp_hd_test_t));
    }
    osel_memset((uint8_t *)mode_pwmp_hd_test, 0, sizeof(pwmp_hd_test_t));
    /*滤波初始化*/
    mode_pwmp_hd->filter.handle = lpf_window_init(20);
    /*绑定要保存的参数与函数*/
    mode_pwmp_hd->pwmp_save = params;
    mode_pwmp_hd->params_save_cb = params_save_cb;
    /*设定PWMP方式的工作模式*/
    pwmp_hd_process_state_set(PWMP_HD_PROCESS_CONTROL);
    /*PID初始化*/
    execute_pid_init(EXECUTE_PLAN);
    /*计数最大值*/
    mode_pwmp_hd->wait_count_max = 100 / ((MODE_DEFAULT_AUTOLOAD + 1) * 0.1); // （=10，每减一消耗10ms，总消耗时间100ms）
    /*保存参数*/
    mode_pwmp_hd->params_save_cb();

    /*更新公共参数*/
    pwmp_public_params_update();
    calib_parapos_perent();
}

/*程序反初始化*/
void mode_pwmp_hd_dinit(void)
{
    if (mode_pwmp_hd != NULL)
    {
        pwmp_hd_process_state_set(PWMP_HD_PROCESS_STOP);
        osel_mem_free(mode_pwmp_hd);
        mode_pwmp_hd = NULL;
    }
}