|
@ -3,6 +3,7 @@ |
|
|
#include "../zes8p5066lib/basic.h" |
|
|
#include "../zes8p5066lib/basic.h" |
|
|
#include "../zes8p5066lib/systicket.h" |
|
|
#include "../zes8p5066lib/systicket.h" |
|
|
#include "frequency_sweep_service.h" |
|
|
#include "frequency_sweep_service.h" |
|
|
|
|
|
#include "kalmanFilter.h" |
|
|
#include "state_machine.h" |
|
|
#include "state_machine.h" |
|
|
|
|
|
|
|
|
/** |
|
|
/** |
|
@ -52,7 +53,7 @@ state_machine_t m_statemachine; |
|
|
static Context_t context; |
|
|
static Context_t context; |
|
|
const static float const_windonws = 0.3; |
|
|
const static float const_windonws = 0.3; |
|
|
const static float const_level1_expect_power = 3.0; |
|
|
const static float const_level1_expect_power = 3.0; |
|
|
const static float const_level2_expect_power = 6.0; |
|
|
|
|
|
|
|
|
const static float const_level2_expect_power = 5.0; |
|
|
const static float const_max_freq = 38000; |
|
|
const static float const_max_freq = 38000; |
|
|
|
|
|
|
|
|
static uint16_t get_resonant_frequency(uint16_t startfreq, uint16_t step, uint16_t endfreq) { |
|
|
static uint16_t get_resonant_frequency(uint16_t startfreq, uint16_t step, uint16_t endfreq) { |
|
@ -67,10 +68,10 @@ static uint16_t get_resonant_frequency(uint16_t startfreq, uint16_t step, uint16 |
|
|
bool inited = false; |
|
|
bool inited = false; |
|
|
for (uint16_t freq = startfreq; freq <= endfreq; freq += step) { |
|
|
for (uint16_t freq = startfreq; freq <= endfreq; freq += step) { |
|
|
float power = frequency_sweep_get_power(freq); |
|
|
float power = frequency_sweep_get_power(freq); |
|
|
if (inited) { |
|
|
|
|
|
|
|
|
if (!inited) { |
|
|
minpower = power; |
|
|
minpower = power; |
|
|
retfreq = freq; |
|
|
retfreq = freq; |
|
|
inited = false; |
|
|
|
|
|
|
|
|
inited = true; |
|
|
continue; |
|
|
continue; |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
@ -82,7 +83,8 @@ static uint16_t get_resonant_frequency(uint16_t startfreq, uint16_t step, uint16 |
|
|
return retfreq; |
|
|
return retfreq; |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
static float mf_get_ozone_power() { |
|
|
|
|
|
|
|
|
static KFP KFPConfig = KALMAN_FILTER_PARA; |
|
|
|
|
|
static float __mf_get_ozone_power() { |
|
|
float powersum = 0; |
|
|
float powersum = 0; |
|
|
for (size_t i = 0; i < 10; i++) { |
|
|
for (size_t i = 0; i < 10; i++) { |
|
|
powersum += port_adc_get_ozone_generator_power(); |
|
|
powersum += port_adc_get_ozone_generator_power(); |
|
@ -90,6 +92,9 @@ static float mf_get_ozone_power() { |
|
|
return powersum / 10; |
|
|
return powersum / 10; |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
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 get_expect_power() { |
|
|
float expectpower = 0; |
|
|
float expectpower = 0; |
|
|
if (thisDevice.level == klevel1) { |
|
|
if (thisDevice.level == klevel1) { |
|
@ -138,13 +143,16 @@ static state_machine_state_t* processWorkingState(state_machine_t* machine, stat |
|
|
port_ozone_pwm_set_duty(context.nowfreq, 5000); |
|
|
port_ozone_pwm_set_duty(context.nowfreq, 5000); |
|
|
port_ozone_pwm_start(); |
|
|
port_ozone_pwm_start(); |
|
|
context.adjustedToTheProperPower = false; |
|
|
context.adjustedToTheProperPower = false; |
|
|
|
|
|
mf_get_ozone_power_reset_filter(); |
|
|
|
|
|
printf("----------start working--------\n"); |
|
|
} else if (event == TIME_EVENT) { |
|
|
} else if (event == TIME_EVENT) { |
|
|
if (context.adjustedToTheProperPower) { |
|
|
if (context.adjustedToTheProperPower) { |
|
|
/** |
|
|
/** |
|
|
* 检查当前功率是否在期望功率窗口之外时候([expertpower+window,expertpower-window]), |
|
|
* 检查当前功率是否在期望功率窗口之外时候([expertpower+window,expertpower-window]), |
|
|
*/ |
|
|
*/ |
|
|
float nowpower = mf_get_ozone_power(); |
|
|
float nowpower = mf_get_ozone_power(); |
|
|
if (nowpower < get_expect_power() - const_windonws || nowpower > get_expect_power() + const_windonws) { |
|
|
|
|
|
|
|
|
if (nowpower < (get_expect_power() - const_windonws) || // |
|
|
|
|
|
nowpower > (get_expect_power() + const_windonws)) { |
|
|
context.adjustedToTheProperPower = false; |
|
|
context.adjustedToTheProperPower = false; |
|
|
if (nowpower < get_expect_power()) { |
|
|
if (nowpower < get_expect_power()) { |
|
|
context.changefreqdirection = true; |
|
|
context.changefreqdirection = true; |
|
@ -159,21 +167,23 @@ static state_machine_state_t* processWorkingState(state_machine_t* machine, stat |
|
|
if (context.changefreqdirection) { |
|
|
if (context.changefreqdirection) { |
|
|
float nowpower = mf_get_ozone_power(); |
|
|
float nowpower = mf_get_ozone_power(); |
|
|
if (nowpower < get_expect_power()) { |
|
|
if (nowpower < get_expect_power()) { |
|
|
context.nowfreq += 100; |
|
|
|
|
|
|
|
|
context.nowfreq += 25; |
|
|
if (context.nowfreq > const_max_freq) context.nowfreq = const_max_freq; |
|
|
if (context.nowfreq > const_max_freq) context.nowfreq = const_max_freq; |
|
|
printf("change freq to match power,freq %d, power %f-->%f \n", context.nowfreq, nowpower, get_expect_power()); |
|
|
|
|
|
port_ozone_pwm_set_duty(context.nowfreq, 5000); |
|
|
|
|
|
|
|
|
printf("change freq [ up ],freq %d, power %f-->%f \n", context.nowfreq, nowpower, get_expect_power()); |
|
|
|
|
|
port_ozone_pwm_set_duty(context.nowfreq, kconst_pwm_work_dutyns); |
|
|
} else { |
|
|
} else { |
|
|
|
|
|
// printf("reach %f->%f\n", nowpower, get_expect_power()); |
|
|
context.adjustedToTheProperPower = true; |
|
|
context.adjustedToTheProperPower = true; |
|
|
} |
|
|
} |
|
|
} else { |
|
|
} else { |
|
|
float nowpower = mf_get_ozone_power(); |
|
|
float nowpower = mf_get_ozone_power(); |
|
|
if (nowpower > get_expect_power()) { |
|
|
if (nowpower > get_expect_power()) { |
|
|
context.nowfreq -= 100; |
|
|
|
|
|
|
|
|
context.nowfreq -= 25; |
|
|
if (context.nowfreq < context.resonant_frequency) context.nowfreq = context.resonant_frequency; |
|
|
if (context.nowfreq < context.resonant_frequency) context.nowfreq = context.resonant_frequency; |
|
|
printf("change freq to match power,freq %d, power %f-->%f \n", context.nowfreq, nowpower, get_expect_power()); |
|
|
|
|
|
port_ozone_pwm_set_duty(context.nowfreq, 5000); |
|
|
|
|
|
|
|
|
printf("change freq [down],freq %d, power %f-->%f \n", context.nowfreq, nowpower, get_expect_power()); |
|
|
|
|
|
port_ozone_pwm_set_duty(context.nowfreq, kconst_pwm_work_dutyns); |
|
|
} else { |
|
|
} else { |
|
|
|
|
|
// printf("reach %f->%f\n", nowpower, get_expect_power()); |
|
|
context.adjustedToTheProperPower = true; |
|
|
context.adjustedToTheProperPower = true; |
|
|
} |
|
|
} |
|
|
} |
|
|
} |
|
@ -181,7 +191,7 @@ static state_machine_state_t* processWorkingState(state_machine_t* machine, stat |
|
|
} else if (event == EXIT_STATE) { |
|
|
} else if (event == EXIT_STATE) { |
|
|
port_ozone_pwm_stop(); |
|
|
port_ozone_pwm_stop(); |
|
|
} |
|
|
} |
|
|
return NULL; |
|
|
|
|
|
|
|
|
return NULL; |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
static state_machine_state_t* state_machine_process_event(state_machine_t* machine, state_machine_state_t* nowstate, int event) { |
|
|
static state_machine_state_t* state_machine_process_event(state_machine_t* machine, state_machine_state_t* nowstate, int event) { |
|
@ -218,8 +228,9 @@ void ozone_control_start() { state_machine_trigger_event(&m_statemachine, PRV_ST |
|
|
void ozone_control_stop() { state_machine_trigger_event(&m_statemachine, PRV_STOP_EVENT); } |
|
|
void ozone_control_stop() { state_machine_trigger_event(&m_statemachine, PRV_STOP_EVENT); } |
|
|
void ozone_control_schedule() { |
|
|
void ozone_control_schedule() { |
|
|
static uint32_t ticket = 0; |
|
|
static uint32_t ticket = 0; |
|
|
if (systicket_haspassedms(ticket) > 10 * 1000) { |
|
|
|
|
|
|
|
|
if (systicket_haspassedms(ticket) > 10) { |
|
|
ticket = systicket_get_now_ms(); |
|
|
ticket = systicket_get_now_ms(); |
|
|
state_machine_schedule_each10ms(&m_statemachine); |
|
|
state_machine_schedule_each10ms(&m_statemachine); |
|
|
} |
|
|
} |
|
|
|
|
|
frequency_sweep_schedule(); |
|
|
} |
|
|
} |