515 lines
16 KiB
C
515 lines
16 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"
|
||
#include "mode_diagnosis.h"
|
||
|
||
mode_params_u mode_params __attribute__((section("NOINIT"), zero_init)); // 模式参数
|
||
mode_t mode;
|
||
|
||
/**
|
||
* @brief 改变自动重装载寄存器的值
|
||
* @param {uint16_t} autoload
|
||
* @return {*}
|
||
* @note
|
||
*/
|
||
static void mode_autoload_change(uint16_t autoload)
|
||
{
|
||
LL_TIM_SetAutoReload(MODE_TIM, autoload);
|
||
}
|
||
|
||
/**
|
||
* @brief 工作模式参数保存,回调处理
|
||
* @return {*}
|
||
* @note
|
||
*/
|
||
static void mode_params_save_cb(void)
|
||
{
|
||
fal_execution_sem_update();
|
||
}
|
||
|
||
/**
|
||
* @brief 气压统计
|
||
* @return {*}
|
||
* @note 调用周期：> 100ms,小于改值S口会读到0值
|
||
*/
|
||
void mode_pressure_statistics(void)
|
||
{
|
||
if (mode_diagnosis_get()->run == FALSE)
|
||
{
|
||
static uint8_t wait_tick = 0; // 定时器周期是100ms，因为功耗关闭传感器到恢复数据采集需要一定时间
|
||
float32 pressure = 0.0f;
|
||
if (++wait_tick < 3) // 300ms内不处理
|
||
{
|
||
return;
|
||
}
|
||
|
||
if (udevice.press_sensor_enable != TRUE)
|
||
{
|
||
return;
|
||
}
|
||
static uint8_t id = PRESSURE_PARAM_MAX;
|
||
static float32 pressure_cross_point_last = 0.0f;
|
||
|
||
switch (id)
|
||
{
|
||
case PRESSURE_PARAM_S:
|
||
pressure = get_pressure_limit_deal(pressure_kpa2unit(get_pressure(PRESSURE_PARAM_S), udevice.press_unit), udevice.press_unit);
|
||
// rt_data.pressure_s = lpf_window_update(&mode_get()->pressure_s_window, pressure);
|
||
rt_data.pressure_s = pressure;
|
||
if (pressure != 0.0f)
|
||
{
|
||
id = PRESSURE_PARAM_A;
|
||
BP_S_POWER_OFF();
|
||
BP_A_POWER_ON();
|
||
}
|
||
else
|
||
{
|
||
id = PRESSURE_PARAM_S;
|
||
BP_S_POWER_ON();
|
||
}
|
||
rt_data.pressure_s_kpa = pressure_unit2kpa(rt_data.pressure_s, udevice.press_unit);
|
||
break;
|
||
case PRESSURE_PARAM_A:
|
||
pressure = get_pressure_limit_deal(pressure_kpa2unit(get_pressure(PRESSURE_PARAM_A), udevice.press_unit), udevice.press_unit);
|
||
// rt_data.pressure_a = lpf_window_update(&mode_get()->pressure_a_window, pressure);
|
||
rt_data.pressure_a = pressure;
|
||
if (rt_data.pressure_s != 0) // S口压力不为0的情况下才读取A口和B口的压力
|
||
{
|
||
id = PRESSURE_PARAM_B;
|
||
BP_A_POWER_OFF();
|
||
BP_B_POWER_ON();
|
||
}
|
||
else
|
||
{
|
||
rt_data.pressure_a = 0;
|
||
id = PRESSURE_PARAM_S;
|
||
BP_A_POWER_OFF();
|
||
BP_S_POWER_ON();
|
||
}
|
||
rt_data.pressure_a_kpa = pressure_unit2kpa(rt_data.pressure_a, udevice.press_unit);
|
||
break;
|
||
case PRESSURE_PARAM_B:
|
||
pressure = get_pressure_limit_deal(pressure_kpa2unit(get_pressure(PRESSURE_PARAM_B), udevice.press_unit), udevice.press_unit);
|
||
// rt_data.pressure_b = lpf_window_update(&mode_get()->pressure_b_window, pressure);
|
||
rt_data.pressure_b = pressure;
|
||
id = PRESSURE_PARAM_S;
|
||
BP_B_POWER_OFF();
|
||
BP_S_POWER_ON();
|
||
if (rt_data.pressure_s != 0) // S口压力不为0的情况下才读取A口和B口的压力
|
||
{
|
||
pressure_cross_point_last = ((rt_data.pressure_a + rt_data.pressure_b) * 0.5f / rt_data.pressure_s) * 100;
|
||
if (pressure_cross_point_last <= 40)
|
||
{
|
||
rt_data.pressure_cross_point = 0;
|
||
break;
|
||
}
|
||
else if (pressure_cross_point_last >= 80)
|
||
{
|
||
rt_data.pressure_cross_point = 0;
|
||
break;
|
||
}
|
||
rt_data.pressure_cross_point = lpf_update(&mode_get()->pressure_cross_point_lpf, pressure_cross_point_last);
|
||
}
|
||
else
|
||
{
|
||
rt_data.pressure_b = 0;
|
||
}
|
||
|
||
rt_data.pressure_b_kpa = pressure_unit2kpa(rt_data.pressure_b, udevice.press_unit);
|
||
break;
|
||
case PRESSURE_PARAM_MAX:
|
||
id = PRESSURE_PARAM_S;
|
||
BP_S_POWER_ON();
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
wait_tick = 0;
|
||
}
|
||
else
|
||
{
|
||
if (BP_S_IS_POWER_ON() == FALSE)
|
||
{
|
||
BP_S_POWER_ON();
|
||
}
|
||
|
||
if (BP_A_IS_POWER_ON() == FALSE)
|
||
{
|
||
BP_A_POWER_ON();
|
||
}
|
||
|
||
if (BP_B_IS_POWER_ON() == FALSE)
|
||
{
|
||
BP_B_POWER_ON();
|
||
}
|
||
|
||
rt_data.pressure_s = get_pressure_limit_deal(pressure_kpa2unit(get_pressure(PRESSURE_PARAM_S), udevice.press_unit), udevice.press_unit);
|
||
if (rt_data.pressure_s != 0.0f)
|
||
{
|
||
rt_data.pressure_s_kpa = pressure_unit2kpa(rt_data.pressure_s, udevice.press_unit);
|
||
rt_data.pressure_a = get_pressure_limit_deal(pressure_kpa2unit(get_pressure(PRESSURE_PARAM_A), udevice.press_unit), udevice.press_unit);
|
||
rt_data.pressure_a_kpa = pressure_unit2kpa(rt_data.pressure_a, udevice.press_unit);
|
||
rt_data.pressure_b = get_pressure_limit_deal(pressure_kpa2unit(get_pressure(PRESSURE_PARAM_B), udevice.press_unit), udevice.press_unit);
|
||
rt_data.pressure_b_kpa = pressure_unit2kpa(rt_data.pressure_b, udevice.press_unit);
|
||
}
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 行程统计(100ms以上运行一次)
|
||
* @return {*}
|
||
* @note
|
||
*/
|
||
void mode_travel_statistics(void)
|
||
{
|
||
float32 current = 0.0f; // 输入电流
|
||
float32 deviation = 0.0f; // 系统偏差
|
||
// 输入电流
|
||
current = get_current(FILTER_AVERAGE);
|
||
rt_data.loop_current = kalman_update(&mode_get()->loop_current_km, current);
|
||
|
||
// 获取目标行程
|
||
if (udevice.inst_mode == INST_IS)
|
||
{
|
||
// 模拟模式：通过4~20mA计算目标
|
||
if (*mode_get()->ctrl.mode == ANALOG_CTRL_MODE)
|
||
{
|
||
rt_data.target_travel = i2psb(rt_data.loop_current);
|
||
rt_save.travel_set_pt = rt_data.target_travel;
|
||
}
|
||
// 数字模式：通过HART指令设定目标
|
||
else if (*mode_get()->ctrl.mode == DIGITAL_CTRL_MODE)
|
||
{
|
||
rt_data.target_travel = rt_save.travel_set_pt;
|
||
}
|
||
// 测试模式：通过上位机下发目标
|
||
else if (*mode_get()->ctrl.mode == TEST_CTRL_MODE)
|
||
{
|
||
rt_data.target_travel = rt_data.test_target;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (*mode_get()->ctrl.mode == TEST_CTRL_MODE)
|
||
{
|
||
rt_data.target_travel = rt_data.test_target;
|
||
}
|
||
else
|
||
{
|
||
rt_data.target_travel = 0;
|
||
// 手动加载模式
|
||
if (udevice.manual_loader == ALLOW_SERVO_CTRL)
|
||
{
|
||
rt_data.target_travel = i2psb(rt_data.loop_current);
|
||
rt_save.travel_set_pt = rt_data.target_travel;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 目标行程处理
|
||
if (udevice.crc != 0)
|
||
{
|
||
rt_data.target_travel = target_travel_deal(rt_data.target_travel);
|
||
}
|
||
|
||
rt_data.pid_target = get_pid_travel(rt_data.target_travel);
|
||
if (rt_data.pid_target > 60)
|
||
{
|
||
__NOP();
|
||
}
|
||
|
||
// 实际行程处理
|
||
if (rt_data.actual_travel > udevice.cutoff_limit_lo && rt_data.actual_travel < udevice.cutoff_limit_hi)
|
||
{
|
||
deviation = udevice.deviation;
|
||
}
|
||
|
||
rt_data.servo_feedback = rt_data.actual_travel + deviation;
|
||
|
||
rt_data.crossover_out_u = get_actual_travel_adc();
|
||
|
||
// 显示内容
|
||
rt_data.show_target = get_show_travel(rt_data.target_travel);
|
||
if (ABS(rt_data.target_travel - rt_data.show_target) > MA_TO_PERCENT_01)
|
||
{
|
||
rt_data.show_target = get_show_travel(rt_data.target_travel);
|
||
lpf_window_reset(&mode_get()->show_target_window);
|
||
}
|
||
else
|
||
{
|
||
rt_data.show_target = get_show_travel(rt_data.target_travel);
|
||
rt_data.show_target = lpf_window_update(&mode_get()->show_target_window, rt_data.show_target);
|
||
}
|
||
|
||
rt_data.show_actual = get_show_travel(rt_data.servo_feedback);
|
||
rt_data.show_actual = get_show_actual_travel(rt_data.show_target, rt_data.show_actual, 0.1f);
|
||
rt_data.show_loop = rt_data.loop_current;
|
||
}
|
||
|
||
// 行程统计
|
||
static void mode_stroke_statistics(void)
|
||
{
|
||
static float32 last_travel = 0.0f;
|
||
static float32 last_target = 0.0f;
|
||
float32 err = ABS(rt_data.pid_actual - last_travel);
|
||
float32 err_target = ABS(rt_data.pid_target - last_target); // 目标位置的差值
|
||
float32 err_move = ABS(rt_data.pid_actual - last_target); // 实际位置和上次目标位置的差值
|
||
float32 err_act = ABS(rt_data.pid_actual - rt_data.pid_target); // 实际位置和目标位置的差值
|
||
if (mode_get()->interface_req.mode_is_adjusting() == FALSE) // 不在整定中
|
||
{
|
||
// 累计行程
|
||
if (err >= udevice.travel_sum_dead)
|
||
{
|
||
rt_save.travel_accum += err;
|
||
last_travel = rt_data.pid_actual;
|
||
}
|
||
|
||
// 设定的目标位置-当前目标位置>=死区
|
||
if (err_target >= udevice.act_sum_dead)
|
||
{
|
||
// 移动的距离大于等于死区，实际距离目标位置小于死区
|
||
if ((err_move >= udevice.act_sum_dead) && (err_act < udevice.act_sum_dead))
|
||
{
|
||
last_target = rt_data.pid_target;
|
||
rt_save.cycle_count++;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// 实际位置和目标位置差值大于死区，移动距离大于死区
|
||
if ((err_act > udevice.act_sum_dead) && (err > udevice.act_sum_dead))
|
||
{
|
||
rt_save.cycle_count++;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/*********************************************************************************************************************/
|
||
/**
|
||
* @brief 工作模式控制管理
|
||
* @param {uint8_t} work_mode
|
||
* @return {*}
|
||
* @note
|
||
*/
|
||
void mode_master_detection(void)
|
||
{
|
||
switch (*mode_get()->ctrl.mode)
|
||
{
|
||
case DIGITAL_CTRL_MODE:
|
||
case ANALOG_CTRL_MODE:
|
||
{
|
||
if (udevice.inst_mode == INST_IS || udevice.manual_loader == ALLOW_SERVO_CTRL)
|
||
{
|
||
if (mode_get()->ctrl.state != TRUE)
|
||
{
|
||
// 当前控制算法模块从禁止到允许
|
||
mode_get()->ctrl.state = TRUE;
|
||
mode_get()->interface_req.mode_process_start();
|
||
pdctrl_run(); // 输出软使能
|
||
}
|
||
}
|
||
else
|
||
{
|
||
mode_get()->ctrl.state = FALSE;
|
||
}
|
||
break;
|
||
}
|
||
case TEST_CTRL_MODE:
|
||
mode_get()->ctrl.state = TRUE;
|
||
break;
|
||
case STOP_MODE: // 停机
|
||
mode_get()->ctrl.state = FALSE;
|
||
// 关闭LCD
|
||
// 关闭诊断
|
||
// 由各自模块处理
|
||
break;
|
||
case WAIT_MODE: // 待机
|
||
mode_get()->ctrl.state = FALSE;
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 采集传感器数据转换接口
|
||
* @return {*}
|
||
* @note
|
||
*/
|
||
void mode_ctrl_gather(void)
|
||
{
|
||
if (mode_get()->ctrl.state == FALSE || mode_get()->positioner_model >= POSITIONER_MODEL_MAX) // 不进行算法控制
|
||
{
|
||
return;
|
||
}
|
||
|
||
if (mode_get()->interface_req.mode_is_adjusting() == FALSE)
|
||
{
|
||
mode_travel_statistics();
|
||
mode_stroke_statistics();
|
||
}
|
||
|
||
mode_pressure_statistics(); // 气压轮询读取
|
||
}
|
||
/**
|
||
* @brief 控制输出模式函数
|
||
*
|
||
* 该函数负责处理并输出实际行程数据。
|
||
*/
|
||
void mode_ctrl_output(void)
|
||
{
|
||
if (*mode_get()->ctrl.mode == DIGITAL_CTRL_MODE ||
|
||
*mode_get()->ctrl.mode == ANALOG_CTRL_MODE)
|
||
{
|
||
actual_travel_deal(rt_data.actual_travel); // 实际行程处理，对外输出
|
||
}
|
||
else if (*mode_get()->ctrl.mode == TEST_CTRL_MODE)
|
||
{
|
||
if (calib_param[CALIBPARA_VIP].is_calibration == TRUE)
|
||
{
|
||
pwm_output_position(rt_data.show_target);
|
||
}
|
||
else
|
||
{
|
||
pwm_duty2current(rt_data.loop_current);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
actual_travel_deal(0);
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 控制模式
|
||
* @return {*}
|
||
* @note 该模块在TIM7中执行
|
||
*/
|
||
void mode_ctrl_process(void)
|
||
{
|
||
if (mode_get()->ctrl.state == FALSE || mode_get()->positioner_model >= POSITIONER_MODEL_MAX) // 不进行算法控制
|
||
{
|
||
return;
|
||
}
|
||
mode_travel_statistics();
|
||
// 当电流达到4mA时才进行控制
|
||
switch (udevice.dev_algorithm_mode)
|
||
{
|
||
case MODE_CONSTANT_CONTROL_ALGORITHM:
|
||
mode_dac_process();
|
||
break;
|
||
case MODE_SPEED_CONTROL_ALGORITHM:
|
||
mode_pwmp_process();
|
||
break;
|
||
case MODE_FREQUENCY_DOMAIN_CONTROL_ALGORITHM:
|
||
mode_pwmp_hd_process();
|
||
break;
|
||
case MODE_VARIABLE_FREQUENCY_CONTROL_ALGORITHM:
|
||
mode_control_process();
|
||
break;
|
||
default:
|
||
DBG_ASSERT(FALSE __DBG_LINE);
|
||
break;
|
||
}
|
||
mode_diagnosis_inspection();
|
||
}
|
||
|
||
/**
|
||
* @brief 切换测试模式
|
||
* @param {BOOL} state
|
||
* @return {*}
|
||
* @note 测试模式
|
||
*/
|
||
void mode_enter_test(test_item_e item)
|
||
{
|
||
mode_get()->test_item = item;
|
||
}
|
||
|
||
/**
|
||
* @brief 工作模式初始化
|
||
* @return {*}
|
||
* @note
|
||
*/
|
||
void mode_init(void)
|
||
{
|
||
rt_data.mode_auto_load = (LL_TIM_GetAutoReload(MODE_TIM) + 1) * 0.1;
|
||
osel_memset((uint8_t *)&mode, 0, sizeof(mode_t));
|
||
mode_get()->positioner_model = udevice.dev_model;
|
||
mode_get()->ctrl.mode = (uint8_t *)&udevice.control_mode;
|
||
mode_autoload_change(rt_data.mode_auto_load * 10);
|
||
|
||
// 反初始化，释放内存
|
||
mode_dac_dinit();
|
||
mode_pwmp_dinit();
|
||
mode_pwmp_hd_dinit();
|
||
mode_control_dinit();
|
||
|
||
switch (udevice.dev_algorithm_mode)
|
||
{
|
||
case MODE_CONSTANT_CONTROL_ALGORITHM:
|
||
pdctrl_init(PDCTRL_DAC);
|
||
mode_dac_init(&mode_get()->interface_req, mode_get()->positioner_model, &mode_params.dac,
|
||
mode_params_save_cb);
|
||
break;
|
||
case MODE_SPEED_CONTROL_ALGORITHM:
|
||
pdctrl_init(PDCTRL_DAC);
|
||
mode_pwmp_init(&mode_get()->interface_req, mode_get()->positioner_model, &mode_params.pwmp,
|
||
mode_params_save_cb);
|
||
break;
|
||
case MODE_FREQUENCY_DOMAIN_CONTROL_ALGORITHM:
|
||
pdctrl_init(PDCTRL_DAC);
|
||
mode_pwmp_hd_init(&mode_get()->interface_req, mode_get()->positioner_model, &mode_params.pwmp_hd,
|
||
mode_params_save_cb);
|
||
break;
|
||
case MODE_VARIABLE_FREQUENCY_CONTROL_ALGORITHM:
|
||
pdctrl_init(PDCTRL_DAC);
|
||
mode_control_init(&mode_get()->interface_req, mode_get()->positioner_model, &mode_params.control,
|
||
mode_params_save_cb);
|
||
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);
|
||
|
||
// 滤波初始化
|
||
kalman_init(&mode_get()->loop_current_km, 0.03f);
|
||
mode_get()->pressure_cross_point_lpf.alpha = 0.1;
|
||
lpf_init(&mode_get()->pressure_cross_point_lpf);
|
||
lpf_window_init(&mode_get()->show_target_window, 10);
|
||
lpf_window_init(&mode_get()->pressure_s_window, 10);
|
||
lpf_window_init(&mode_get()->pressure_a_window, 10);
|
||
lpf_window_init(&mode_get()->pressure_b_window, 10);
|
||
mode_diagnosis_init(); // 控制模块诊断初始化,必须放在rt_data.mode_auto_load之后
|
||
}
|
||
|
||
/**
|
||
* @brief 工作模式获取
|
||
* @return {*}
|
||
* @note
|
||
*/
|
||
mode_t *mode_get(void)
|
||
{
|
||
return &mode;
|
||
}
|