339 lines
8.8 KiB
C
339 lines
8.8 KiB
C
/**
|
||
* @file mode.c
|
||
* @author xxx
|
||
* @date 2023-09-18 10:00:52
|
||
* @brief 此文件用于实现不同工作模式的功能
|
||
* @copyright Copyright (c) 2023 by xxx, All Rights Reserved.
|
||
*/
|
||
|
||
#include <math.h>
|
||
#include "app.h"
|
||
#include "main.h"
|
||
#include "mode.h"
|
||
|
||
mode_params_t mode_params; // 模式参数
|
||
static mode_t _mode;
|
||
|
||
static uint8_t deal_dev_work_mode(void)
|
||
{
|
||
uint8_t mode = udevice.dev_work_mode;
|
||
|
||
if (udevice.dev_work_mode == FORBIDEN_MODE || udevice.dev_work_mode == TEST_MODE)
|
||
{
|
||
return mode;
|
||
}
|
||
else
|
||
{
|
||
|
||
return mode;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 改变自动重装载寄存器的值
|
||
* @param {uint16_t} autoload
|
||
* @return {*}
|
||
* @note
|
||
*/
|
||
static void mode_autoload_change(uint16_t autoload)
|
||
{
|
||
LL_TIM_SetAutoReload(TIM7, autoload);
|
||
}
|
||
|
||
/**
|
||
* @brief 工作模式参数保存,回调处理
|
||
* @return {*}
|
||
* @note
|
||
*/
|
||
static void mode_params_save_cb(void)
|
||
{
|
||
fal_execution_kv_write(KEY_CALIBPARA_PARAM, (uint8_t *)&calib_param, (CALIBPARA_NUM * sizeof(calib_param_t)));
|
||
fal_execution_kv_write(KEY_MODE_PARAM, (uint8_t *)&mode_params, sizeof(mode_params_t));
|
||
fal_execution_kv_read(KEY_MODE_PARAM, (uint8_t *)&mode_params, sizeof(mode_params_t));
|
||
}
|
||
|
||
/**
|
||
* @brief 测试模式
|
||
* @return {*}
|
||
* @note
|
||
*/
|
||
void mode_test_process(void)
|
||
{
|
||
switch (mode_get()->test_project)
|
||
{
|
||
case MODE_DIAGNOSIS: // EPM特性诊断
|
||
mode_get()->ctrl.state = FALSE;
|
||
flow_event = FLOW_EVENT_DIAGNOSIS;
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 行程统计(100ms以上运行一次)
|
||
* @return {*}
|
||
* @note
|
||
*/
|
||
void mode_travel_statistics(void)
|
||
{
|
||
static float32 loop_current_last = 0.0f; // 上一次的电流
|
||
static uint8_t change_count = 0;
|
||
float32 loop = 0.0;
|
||
// 输入电流
|
||
loop = get_current();
|
||
rt_data.loop_current = get_current_deal(loop);
|
||
// 过滤掉不稳定的电流值
|
||
if (loop_current_last != rt_data.loop_current)
|
||
{
|
||
if (change_count++ >= 2)
|
||
{
|
||
loop_current_last = rt_data.loop_current;
|
||
change_count = 0;
|
||
}
|
||
else
|
||
{
|
||
return;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
change_count = 0;
|
||
lpf_window_reset(mode_get()->show_loop_lpf);
|
||
lpf_window_reset(mode_get()->show_actual_lpf);
|
||
}
|
||
|
||
// 获取目标行程
|
||
if (mode_get()->ctrl.mode == ON_LINE_MODE) // 在线模式
|
||
{
|
||
rt_data.target_travel = i2psb(rt_data.loop_current);
|
||
rt_data.target_travel = (uint16_t)(rt_data.target_travel * 10) * 0.1f;
|
||
}
|
||
else if (mode_get()->ctrl.mode == OFF_LINE_MODE) // 离线模式
|
||
{
|
||
rt_data.target_travel = udevice.travel_setpoint;
|
||
}
|
||
else if (mode_get()->ctrl.mode == TEST_MODE) // 测试模式
|
||
{
|
||
}
|
||
|
||
// 目标行程处理
|
||
rt_data.target_travel = target_travel_deal(rt_data.target_travel);
|
||
pid_target = get_pid_travel(rt_data.target_travel);
|
||
show_target = get_show_travel(rt_data.target_travel);
|
||
|
||
// 实际行程处理
|
||
rt_data.actual_travel = actual_travel_deal(rt_data.actual_travel);
|
||
show_actual = get_show_travel(rt_data.actual_travel);
|
||
// show_actual = lpf_window_update(mode_get()->show_actual_lpf, show_actual);
|
||
show_actual = kalman_update(&mode_get()->show_actual_kalman, show_actual);
|
||
show_loop = lpf_window_update(mode_get()->show_loop_lpf, rt_data.loop_current);
|
||
}
|
||
|
||
/**
|
||
* @brief 气压统计
|
||
* @return {*}
|
||
* @note
|
||
*/
|
||
static void pressure_statistics(void)
|
||
{
|
||
if (udevice.press_sensor_enable != TRUE)
|
||
{
|
||
return;
|
||
}
|
||
static uint8_t id = ADC_INDEX_CLOSEALL;
|
||
switch (id)
|
||
{
|
||
case ADC_INDEX_PRESSSOURCE:
|
||
rt_data.pressure_s = get_pressure(ADC_INDEX_PRESSSOURCE);
|
||
id = ADC_INDEX_PRESSOUTA;
|
||
BP_S_POWER_OFF();
|
||
BP_A_POWER_ON();
|
||
break;
|
||
case ADC_INDEX_PRESSOUTA:
|
||
rt_data.pressure_a = get_pressure(ADC_INDEX_PRESSOUTA);
|
||
id = ADC_INDEX_PRESSOUTB;
|
||
BP_A_POWER_OFF();
|
||
BP_B_POWER_ON();
|
||
break;
|
||
case ADC_INDEX_PRESSOUTB:
|
||
rt_data.pressure_b = get_pressure(ADC_INDEX_PRESSOUTB);
|
||
id = ADC_INDEX_PRESSSOURCE;
|
||
BP_B_POWER_OFF();
|
||
BP_S_POWER_ON();
|
||
break;
|
||
case ADC_INDEX_CLOSEALL:
|
||
id = ADC_INDEX_PRESSSOURCE;
|
||
BP_S_POWER_ON();
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 算法控制模块
|
||
* @return {*}
|
||
* @note 该模块在TIM7中执行
|
||
*/
|
||
void mode_process(void)
|
||
{
|
||
pressure_statistics(); // 气压轮询读取
|
||
|
||
if (mode_get()->ctrl.state == FALSE) // 不进行算法控制
|
||
{
|
||
return;
|
||
}
|
||
|
||
// travel_statistics因为在flow中被更新，如果UI更新时会导致延时，所以在这里更新
|
||
// 系统的tick是10ms一次,200ms更新一次数据
|
||
if (FALSE == mode_get()->interface_req.mode_is_adjusting())
|
||
{
|
||
if (sys_get_tick() - mode_get()->alog_control_ticks >= 20) // 200ms
|
||
{
|
||
mode_get()->alog_control_ticks = sys_get_tick();
|
||
mode_travel_statistics();
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (sys_get_tick() - mode_get()->alog_control_ticks >= 100) // 1000ms
|
||
{
|
||
mode_get()->alog_control_ticks = sys_get_tick();
|
||
mode_travel_statistics();
|
||
}
|
||
}
|
||
|
||
// 当电流达到4mA时才进行控制
|
||
if (rt_data.loop_current >= LOOP_CURRENT_MIN)
|
||
{
|
||
if (udevice.dev_algorithm_mode == MODE_CONSTANT_CONTROL_ALGORITHM)
|
||
{
|
||
}
|
||
else if (udevice.dev_algorithm_mode == MODE_SPEED_CONTROL_ALGORITHM)
|
||
{
|
||
}
|
||
else if (udevice.dev_algorithm_mode == MODE_FREQUENCY_DOMAIN_CONTROL_ALGORITHM)
|
||
{
|
||
mode_pwmp_hd_process();
|
||
}
|
||
else if (udevice.dev_algorithm_mode == MODE_VARIABLE_FREQUENCY_CONTROL_ALGORITHM)
|
||
{
|
||
}
|
||
else
|
||
{
|
||
DBG_ASSERT(FALSE __DBG_LINE);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
pdctrl_out(0);
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 模式处理
|
||
* @param {uint8_t} work_mode
|
||
* @return {*}
|
||
* @note
|
||
*/
|
||
void mode_detection(void)
|
||
{
|
||
mode_get()->ctrl.mode = deal_dev_work_mode();
|
||
|
||
switch (mode_get()->ctrl.mode)
|
||
{
|
||
case ON_LINE_MODE:
|
||
case OFF_LINE_MODE:
|
||
case WAIT_MODE:
|
||
if (mode_get()->ctrl.state != TRUE)
|
||
{
|
||
mode_get()->ctrl.state = TRUE;
|
||
pdctrl_run(); // 输出软使能
|
||
}
|
||
break;
|
||
case FORBIDEN_MODE:
|
||
if (mode_get()->ctrl.state != FALSE)
|
||
{
|
||
mode_get()->ctrl.state = FALSE;
|
||
pdctrl_stop(); // 输出软禁止
|
||
}
|
||
break;
|
||
case TEST_MODE:
|
||
mode_get()->ctrl.state = FALSE;
|
||
mode_test_process();
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 工作模式初始化
|
||
* @return {*}
|
||
* @note
|
||
*/
|
||
void mode_init(void)
|
||
{
|
||
osel_memset((uint8_t *)&_mode, 0, sizeof(mode_t));
|
||
|
||
mode_get()->positioner_model = udevice.dev_model;
|
||
mode_get()->alog_control_ticks = sys_get_tick();
|
||
mode_autoload_change(mode_default_autoload);
|
||
|
||
// 反初始化，释放内存
|
||
|
||
mode_pwmp_hd_dinit();
|
||
|
||
switch (udevice.dev_algorithm_mode)
|
||
{
|
||
case MODE_CONSTANT_CONTROL_ALGORITHM:
|
||
pdctrl_init(PDCTRL_DAC);
|
||
break;
|
||
case MODE_SPEED_CONTROL_ALGORITHM:
|
||
pdctrl_init(PDCTRL_PWMP);
|
||
break;
|
||
case MODE_FREQUENCY_DOMAIN_CONTROL_ALGORITHM:
|
||
//pdctrl_init(PDCTRL_PWMP);
|
||
pdctrl_init(PDCTRL_DAC);
|
||
mode_pwmp_hd_init(&mode_get()->interface_req, mode_get()->positioner_model, &mode_params.mode_pwmp_hd_params,
|
||
mode_params_save_cb);
|
||
break;
|
||
case MODE_VARIABLE_FREQUENCY_CONTROL_ALGORITHM:
|
||
pdctrl_init(PDCTRL_PWM);
|
||
break;
|
||
default:
|
||
DBG_ASSERT(FALSE __DBG_LINE);
|
||
break;
|
||
}
|
||
|
||
DBG_ASSERT(mode_get()->interface_req.mode_process_start != NULL __DBG_LINE);
|
||
DBG_ASSERT(mode_get()->interface_req.mode_process_stop != NULL __DBG_LINE);
|
||
DBG_ASSERT(mode_get()->interface_req.mode_adjust_start != NULL __DBG_LINE);
|
||
DBG_ASSERT(mode_get()->interface_req.mode_adjust_stop != NULL __DBG_LINE);
|
||
DBG_ASSERT(mode_get()->interface_req.mode_get_adjust_data != NULL __DBG_LINE);
|
||
DBG_ASSERT(mode_get()->interface_req.mode_adjust_result != NULL __DBG_LINE);
|
||
DBG_ASSERT(mode_get()->interface_req.mode_adjust_step_count != NULL __DBG_LINE);
|
||
DBG_ASSERT(mode_get()->interface_req.mode_adjust_step_current != NULL __DBG_LINE);
|
||
DBG_ASSERT(mode_get()->interface_req.mode_control_idle != NULL __DBG_LINE);
|
||
DBG_ASSERT(mode_get()->interface_req.mode_is_adjusting != NULL __DBG_LINE);
|
||
|
||
mode_get()->show_loop_lpf = lpf_window_init(10);
|
||
mode_get()->show_actual_lpf = lpf_window_init(10);
|
||
|
||
kalman_init(&mode_get()->loop_kalman);
|
||
kalman_init(&mode_get()->show_actual_kalman);
|
||
mode_get()->loop_kalman.filter_limit = 0.3f;
|
||
mode_get()->show_actual_kalman.filter_limit = 0.3f;
|
||
}
|
||
|
||
/**
|
||
* @brief 工作模式获取
|
||
* @return {*}
|
||
* @note
|
||
*/
|
||
mode_t *mode_get(void)
|
||
{
|
||
return &_mode;
|
||
}
|