You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
270 lines
9.6 KiB
270 lines
9.6 KiB
#include "ozone_control_service.h"
|
|
|
|
#include "../zes8p5066lib/basic.h"
|
|
#include "../zes8p5066lib/systicket.h"
|
|
#include "frequency_sweep_service.h"
|
|
#include "kalmanFilter.h"
|
|
#include "state_machine.h"
|
|
//
|
|
#include "least_square_method.h"
|
|
|
|
/**
|
|
* @brief
|
|
*
|
|
* 状态机:
|
|
* 不工作 ------> 扫描频率 ----> 工作
|
|
*
|
|
**/
|
|
|
|
#define PRV_START_EVENT (STATE_MACHINE_PUBLIC_EVENT + 1)
|
|
#define PRV_STOP_EVENT (STATE_MACHINE_PUBLIC_EVENT + 2)
|
|
|
|
state_machine_state_t m_states[] = {
|
|
[kIdleState] =
|
|
{
|
|
.name = "kIdleState",
|
|
.stateId = kIdleState,
|
|
},
|
|
[kBeforeWorkingStateSweepFrequency] =
|
|
{
|
|
.name = "kBeforeWorkingStateSweepFrequency",
|
|
.stateId = kBeforeWorkingStateSweepFrequency,
|
|
},
|
|
[kWorkingState] = //
|
|
{
|
|
.name = "kWorkingState",
|
|
.stateId = kWorkingState,
|
|
}
|
|
//
|
|
};
|
|
state_machine_t m_statemachine;
|
|
|
|
static ozone_control_working_state_t s_workingstate;
|
|
|
|
static uint16_t get_resonant_frequency(uint16_t startfreq, uint16_t step, uint16_t endfreq) {
|
|
/**
|
|
* @brief
|
|
* 计算谐振频率，频率和功率的图像接近下图，由此可知，当设备工作正常时，在一定频率范围内,查找功率最小的点即可。
|
|
*
|
|
* http://192.168.1.3:3000/project_ozone_generator/doc/src/branch/master/ref/20220815频率-功率关系图.png
|
|
*/
|
|
float minpower = 0;
|
|
uint16_t retfreq = 0;
|
|
bool inited = false;
|
|
for (uint16_t freq = startfreq; freq <= endfreq; freq += step) {
|
|
float power = frequency_sweep_get_power(freq);
|
|
if (!inited) {
|
|
minpower = power;
|
|
retfreq = freq;
|
|
inited = true;
|
|
continue;
|
|
}
|
|
|
|
if (power < minpower) {
|
|
minpower = power;
|
|
retfreq = freq;
|
|
}
|
|
}
|
|
return retfreq;
|
|
}
|
|
|
|
static KFP KFPConfig = KALMAN_FILTER_PARA;
|
|
static float __mf_get_ozone_power() {
|
|
float powersum = 0;
|
|
for (size_t i = 0; i < 10; i++) {
|
|
powersum += port_adc_get_ozone_generator_power();
|
|
}
|
|
return powersum / 10;
|
|
}
|
|
|
|
static float mf_fan_get_power() {
|
|
float fanpower = 0;
|
|
for (size_t i = 0; i < 20; i++) {
|
|
fanpower += port_adc_get_fan_power();
|
|
}
|
|
return fanpower / 20;
|
|
}
|
|
|
|
static void mf_get_ozone_power_reset_filter() { KFPConfig.LastP = __mf_get_ozone_power(); }
|
|
static float mf_get_ozone_power() { return kalmanFilter(&KFPConfig, __mf_get_ozone_power()); }
|
|
|
|
float get_expect_power() {
|
|
float expectpower = 0;
|
|
if (thisDevice.level == klevel1) {
|
|
expectpower = LEVEL1_EXPECT_POWER;
|
|
} else if (thisDevice.level == klevel2) {
|
|
expectpower = LEVEL2_EXPECT_POWER;
|
|
}
|
|
return expectpower;
|
|
}
|
|
|
|
static float gety(int x) { return frequency_sweep_get_power((uint16_t)x); }
|
|
static float compute_slope(uint16_t startfreq, uint16_t step, uint16_t endfreq) {
|
|
float k = 0;
|
|
float c = 0;
|
|
least_square_method(startfreq, step, endfreq, gety, &c, &k);
|
|
return k;
|
|
}
|
|
static float compute_avarage_power(uint16_t startfreq, uint16_t step, uint16_t endfreq) {
|
|
float sumpower = 0;
|
|
int count = 0;
|
|
for (uint16_t freq = startfreq; freq <= endfreq; freq += step) {
|
|
count++;
|
|
float power = frequency_sweep_get_power(freq);
|
|
sumpower += power;
|
|
}
|
|
return sumpower / count;
|
|
}
|
|
|
|
static state_machine_state_t* processBeforeWorkingStateSweepFrequency(state_machine_t* machine, state_machine_state_t* nowstate, int event) {
|
|
/**
|
|
* @brief 扫频
|
|
*/
|
|
if (event == ENTER_STATE) {
|
|
frequency_sweep_start(20000, 100, 41000, 3000);
|
|
} else if (event == TIME_EVENT) {
|
|
if (frequency_sweep_is_finished()) {
|
|
s_workingstate.resonant_frequency = get_resonant_frequency(25000, 100, 35000); //谐振频率点
|
|
s_workingstate.slope_when_freq40k = compute_slope(39000, 100, 41000); //利用最小二乘法计算出45K频率的斜率
|
|
s_workingstate.avarage_power = compute_avarage_power(20000, 100, 41000); //平均功率
|
|
printf("----------Summarize--------\n");
|
|
printf("-resonant_frequency: %d\n", s_workingstate.resonant_frequency);
|
|
printf("-slope_when_freq40k: %f\n", s_workingstate.slope_when_freq40k);
|
|
printf("-avarage_power : %f\n", s_workingstate.avarage_power);
|
|
s_workingstate.nowfreq = s_workingstate.resonant_frequency;
|
|
return &m_states[kWorkingState];
|
|
}
|
|
} else if (event == EXIT_STATE) {
|
|
frequency_sweep_stop();
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
static state_machine_state_t* processWorkingState(state_machine_t* machine, state_machine_state_t* nowstate, int event) {
|
|
/**
|
|
* @brief 检查当前功率是否正常
|
|
*
|
|
* 当功率在期望功率窗口之外时候([expertpower+window,expertpower-window])，
|
|
* 开始调整当前频率，直到功率接近期望功率。
|
|
*/
|
|
|
|
if (event == ENTER_STATE) {
|
|
/**
|
|
* @brief 在谐振点启动，此时臭氧功率最小
|
|
*/
|
|
if (s_workingstate.nowfreq < s_workingstate.resonant_frequency) {
|
|
s_workingstate.nowfreq = s_workingstate.resonant_frequency;
|
|
}
|
|
port_ozone_pwm_set_duty(s_workingstate.nowfreq, 5000);
|
|
port_ozone_pwm_start();
|
|
s_workingstate.adjustedToTheProperPower = false;
|
|
mf_get_ozone_power_reset_filter();
|
|
printf("----------start working--------\n");
|
|
} else if (event == TIME_EVENT) {
|
|
float nowpower = mf_get_ozone_power();
|
|
float fanpower = mf_fan_get_power();
|
|
if (s_workingstate.adjustedToTheProperPower) {
|
|
/**
|
|
* 检查当前功率是否在期望功率窗口之外时候([expertpower+window,expertpower-window])，
|
|
*/
|
|
if (nowpower < (get_expect_power() - EXPECT_POWER_WINDONWS) || //
|
|
nowpower > (get_expect_power() + EXPECT_POWER_WINDONWS)) {
|
|
s_workingstate.adjustedToTheProperPower = false;
|
|
printf("change freq [ no ],freq %d, ozonePower %f-->%f fanPower:%f\n", s_workingstate.nowfreq, nowpower, get_expect_power(), fanpower);
|
|
if (nowpower < get_expect_power()) {
|
|
s_workingstate.changefreqdirection = true;
|
|
} else {
|
|
s_workingstate.changefreqdirection = false;
|
|
}
|
|
}
|
|
} else {
|
|
/**
|
|
* 调整功率到期望功率
|
|
*/
|
|
if (s_workingstate.changefreqdirection) {
|
|
if (nowpower < get_expect_power()) {
|
|
s_workingstate.nowfreq += 25;
|
|
if (s_workingstate.nowfreq > MAX_FREQ) s_workingstate.nowfreq = MAX_FREQ;
|
|
printf("change freq [ up ],freq %d, ozonePower %f-->%f fanPower:%f\n", s_workingstate.nowfreq, nowpower, get_expect_power(), fanpower);
|
|
port_ozone_pwm_set_duty(s_workingstate.nowfreq, kconst_pwm_work_dutyns);
|
|
} else {
|
|
// printf("reach %f->%f\n", nowpower, get_expect_power());
|
|
s_workingstate.adjustedToTheProperPower = true;
|
|
}
|
|
} else {
|
|
if (nowpower > get_expect_power()) {
|
|
s_workingstate.nowfreq -= 25;
|
|
if (s_workingstate.nowfreq < s_workingstate.resonant_frequency) s_workingstate.nowfreq = s_workingstate.resonant_frequency;
|
|
printf("change freq [down],freq %d, ozonePower %f-->%f fanPower:%f\n", s_workingstate.nowfreq, nowpower, get_expect_power(), fanpower);
|
|
port_ozone_pwm_set_duty(s_workingstate.nowfreq, kconst_pwm_work_dutyns);
|
|
} else {
|
|
// printf("reach %f->%f\n", nowpower, get_expect_power());
|
|
s_workingstate.adjustedToTheProperPower = true;
|
|
}
|
|
}
|
|
}
|
|
} else if (event == EXIT_STATE) {
|
|
port_ozone_pwm_stop();
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
static state_machine_state_t* state_machine_process_event(state_machine_t* machine, state_machine_state_t* nowstate, int event) {
|
|
/**
|
|
* @brief 处理Start和Stop事件
|
|
*/
|
|
if (event == PRV_START_EVENT || event == PRV_STOP_EVENT) {
|
|
if (event == PRV_START_EVENT) {
|
|
printf("start\n");
|
|
return &m_states[kBeforeWorkingStateSweepFrequency];
|
|
} else {
|
|
printf("stop\n");
|
|
return &m_states[kIdleState];
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @brief 处理其他事件
|
|
*/
|
|
if /* */ (nowstate == &m_states[kIdleState]) {
|
|
} else if (nowstate == &m_states[kBeforeWorkingStateSweepFrequency]) {
|
|
return processBeforeWorkingStateSweepFrequency(machine, nowstate, event);
|
|
} else if (nowstate == &m_states[kWorkingState]) {
|
|
return processWorkingState(machine, nowstate, event);
|
|
}
|
|
return NULL;
|
|
}
|
|
/***********************************************************************************************************************
|
|
* ======================================================Export======================================================= *
|
|
***********************************************************************************************************************/
|
|
|
|
void ozone_control_init() { state_machine_init(&m_statemachine, m_states, ZARR_SIZE(m_states), state_machine_process_event); }
|
|
void ozone_control_start() { state_machine_trigger_event(&m_statemachine, PRV_START_EVENT); }
|
|
void ozone_control_stop() { state_machine_trigger_event(&m_statemachine, PRV_STOP_EVENT); }
|
|
void ozone_control_schedule() {
|
|
static uint32_t ticket = 0;
|
|
if (systicket_haspassedms(ticket) > 10) {
|
|
ticket = systicket_get_now_ms();
|
|
state_machine_schedule_each10ms(&m_statemachine);
|
|
}
|
|
frequency_sweep_schedule();
|
|
}
|
|
ozone_control_working_state_id_t ozone_control_get_working_state_id() {
|
|
if (state_machine_get_now_state(&m_statemachine) == &m_states[kIdleState]) {
|
|
return kIdleState;
|
|
}
|
|
if (state_machine_get_now_state(&m_statemachine) == &m_states[kBeforeWorkingStateSweepFrequency]) {
|
|
return kBeforeWorkingStateSweepFrequency;
|
|
}
|
|
if (state_machine_get_now_state(&m_statemachine) == &m_states[kWorkingState]) {
|
|
return kWorkingState;
|
|
}
|
|
return kIdleState;
|
|
}
|
|
ozone_control_working_state_t* ozone_control_get_working_state() { return &s_workingstate; }
|
|
|
|
float ozone_control_get_ozone_power() { return mf_get_ozone_power(); }
|
|
|
|
float ozone_control_get_expect_power() { return get_expect_power(); }
|
|
|
|
float ozone_control_get_fan_power() { return mf_fan_get_power(); }
|