9 changed files with 976 additions and 771 deletions
-
24APP/main.c
-
135APP/service/frequency_sweep_service.c
-
12APP/service/frequency_sweep_service.h
-
153APP/service/ozone_control_service.c
-
1APP/service/ozone_control_service.h
-
1340project_ozone/Listings/project_o.map
-
27project_ozone/project_o.uvgui.zel
-
50project_ozone/project_o.uvopt
-
5project_ozone/project_o.uvproj
@ -0,0 +1,135 @@ |
|||
#include "frequency_sweep_service.h" |
|||
|
|||
#include "zes8p5066lib/basic.h" |
|||
#include "zes8p5066lib/systicket.h" |
|||
|
|||
/*********************************************************************************************************************** |
|||
* ====================================================卡尔曼滤波===================================================== * |
|||
***********************************************************************************************************************/ |
|||
typedef struct { |
|||
float LastP; //上次估算协方差 初始化值为0.02 |
|||
float Now_P; //当前估算协方差 初始化值为0 |
|||
float out; //卡尔曼滤波器输出 初始化值为0 |
|||
float Kg; //卡尔曼增益 初始化值为0 |
|||
float Q; //过程噪声协方差 初始化值为0.001 |
|||
float R; //观测噪声协方差 初始化值为0.543 |
|||
} KFP; // Kalman Filter parameter |
|||
|
|||
// 2. 以高度为例 定义卡尔曼结构体并初始化参数 |
|||
/** |
|||
*卡尔曼滤波器 |
|||
*@param KFP *kfp 卡尔曼结构体参数 |
|||
* float input 需要滤波的参数的测量值(即传感器的采集值) |
|||
*@return 滤波后的参数(最优值) |
|||
*/ |
|||
float kalmanFilter(KFP* kfp, float input) { |
|||
//预测协方差方程:k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差 |
|||
kfp->Now_P = kfp->LastP + kfp->Q; |
|||
//卡尔曼增益方程:卡尔曼增益 = k时刻系统估算协方差 / (k时刻系统估算协方差 + 观测噪声协方差) |
|||
kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R); |
|||
//更新最优值方程:k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * (测量值 - 状态变量的预测值) |
|||
kfp->out = kfp->out + kfp->Kg * (input - kfp->out); //因为这一次的预测值就是上一次的输出值 |
|||
//更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。 |
|||
kfp->LastP = (1 - kfp->Kg) * kfp->Now_P; |
|||
return kfp->out; |
|||
} |
|||
|
|||
/*********************************************************************************************************************** |
|||
* ====================================================THIS_MODULE==================================================== * |
|||
***********************************************************************************************************************/ |
|||
|
|||
struct { |
|||
float power_table[350]; |
|||
uint16_t startfreq; |
|||
uint16_t endfreq; |
|||
uint16_t step; |
|||
bool is_schedule; |
|||
bool firstloop; |
|||
uint16_t nowfreq; |
|||
uint32_t startticket; |
|||
uint32_t dutyns; |
|||
} this; |
|||
|
|||
KFP KFPConfig = {0.02, 0, 0, 0, 0.05, 0.543}; |
|||
|
|||
/*********************************************************************************************************************** |
|||
* ====================================================PowerTable===================================================== * |
|||
***********************************************************************************************************************/ |
|||
static void mf_setpower(uint16_t freq, float power) { |
|||
uint16_t index = (freq - this.startfreq) / this.step; |
|||
|
|||
if (index >= ZARR_SIZE(this.power_table)) return; |
|||
this.power_table[index] = power; |
|||
} |
|||
static float mf_getpower(uint16_t freq) { |
|||
uint16_t index = (freq - this.startfreq) / this.step; |
|||
|
|||
if (index >= ZARR_SIZE(this.power_table)) return 0; |
|||
if (index > (this.endfreq - this.startfreq) / this.step) { |
|||
return 0; |
|||
} |
|||
return this.power_table[index]; |
|||
} |
|||
|
|||
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; |
|||
} |
|||
/*********************************************************************************************************************** |
|||
* ======================================================Extern======================================================= * |
|||
***********************************************************************************************************************/ |
|||
void frequency_sweep_start(uint32_t startfreq, uint32_t step, uint32_t endfreq, uint16_t dutyns) { |
|||
this.startfreq = startfreq; |
|||
this.endfreq = endfreq; |
|||
this.step = step; |
|||
this.dutyns = dutyns; |
|||
|
|||
this.firstloop = true; |
|||
this.is_schedule = true; |
|||
this.startticket = systicket_get_now_ms(); |
|||
this.nowfreq = this.startfreq; |
|||
printf("cls\n"); |
|||
} |
|||
|
|||
bool frequency_sweep_is_finished() { return !this.is_schedule; } |
|||
float frequency_sweep_get_power(uint16_t freq) { return mf_getpower(freq); } |
|||
void frequency_sweep_schedule() { |
|||
if (!this.is_schedule) { |
|||
return; |
|||
} |
|||
|
|||
/// loop |
|||
if (systicket_haspassedms(this.startticket) > 1) { |
|||
/** |
|||
* @brief 设置频率和占空比 |
|||
*/ |
|||
port_ozone_pwm_set_duty(this.nowfreq, this.dutyns); |
|||
port_ozone_pwm_start(); |
|||
systicket_delay_ms(3); |
|||
/** |
|||
* @brief 读取当前功率 |
|||
*/ |
|||
float power = mf_get_ozone_power(); |
|||
if (this.firstloop) { |
|||
KFPConfig.LastP = power; |
|||
} |
|||
|
|||
float afterfileter = kalmanFilter(&KFPConfig, power); |
|||
mf_setpower(this.nowfreq, afterfileter); |
|||
printf("%d,%f,%f\n", this.nowfreq, afterfileter, power); |
|||
/** |
|||
* @brief 更新频率 |
|||
*/ |
|||
this.nowfreq += this.step; |
|||
if (this.nowfreq > this.endfreq) { |
|||
this.is_schedule = false; |
|||
} |
|||
port_ozone_pwm_stop(); |
|||
// |
|||
this.firstloop = false; |
|||
this.startticket = systicket_get_now_ms(); |
|||
} |
|||
} |
@ -0,0 +1,12 @@ |
|||
#pragma once |
|||
#include <stdbool.h> |
|||
#include <stdint.h> |
|||
|
|||
#include "port.h" |
|||
#include "thisdevice.h" |
|||
|
|||
void frequency_sweep_start(uint32_t startfreq, uint32_t step, uint32_t endfreq, uint16_t dutyns); |
|||
|
|||
bool frequency_sweep_is_finished(); |
|||
float frequency_sweep_get_power(uint16_t freq); |
|||
void frequency_sweep_schedule(); |
@ -1,82 +1,97 @@ |
|||
#if 0 |
|||
#include "ozone_pwm_control.h" |
|||
#include "pwm.h" |
|||
static bool s_ozone_pwm_control_enable_falg = false; |
|||
static bool s_pwm_modble_hardware_control_flag; //硬件pwm使能标志位 |
|||
#include "ozone_control_service.h" |
|||
|
|||
static uint32_t s_hardware_frequency; // hz |
|||
static uint32_t s_hardware_duty; //单位ms,保存现在的PWM占空比 |
|||
static uint32_t s_large_period; //单位ms,保存现在的定时周期 |
|||
static uint32_t s_large_duty = 100; //单位ms,保存现在的定时占空比 |
|||
static uint32_t begin_ticket; |
|||
#include "frequency_sweep_service.h" |
|||
#if 0 |
|||
|
|||
static void prv_pwm_module_set_pwm_duty(uint32_t frequencyhz, uint32_t duty) { |
|||
set_pwm_modbul_freq_duty(frequencyhz, duty); |
|||
s_pwm_modble_hardware_control_flag = true; |
|||
} |
|||
static void prv_pwm_stop(void) { |
|||
set_pwm_modbul_freq_duty(s_hardware_frequency, 0); //关闭PWM输出 |
|||
s_pwm_modble_hardware_control_flag = false; |
|||
float get_ozone_power() { |
|||
float powersum = 0; |
|||
for (size_t i = 0; i < 20; i++) { |
|||
powersum += port_adc_get_ozone_generator_power(); |
|||
} |
|||
return powersum / 20; |
|||
} |
|||
static bool prv_pwm_is_enable(void) { return s_pwm_modble_hardware_control_flag; } |
|||
static void prv_update_begin_ticket(void) { |
|||
begin_ticket = get_sys_ticket(); //更新大周期的开始时间(间歇时间的开始的时间) |
|||
static float mf_set_pwm_and_get_power(uint32_t freq, uint32_t dutyns) { |
|||
port_ozone_pwm_set_duty(freq, dutyns); |
|||
systicket_delay_ms(2); |
|||
float power1 = get_ozone_power(); |
|||
return power1; |
|||
} |
|||
|
|||
|
|||
bool ozone_pwm_control_is_enable(void) { return s_ozone_pwm_control_enable_falg; } |
|||
bool ozone_pwm_control_hardware_is_enable(void) { return s_pwm_modble_hardware_control_flag; } |
|||
|
|||
/** |
|||
* @brief 一直工作的时间定时的占空比为100, |
|||
* 间歇时间设置300000 |
|||
* @param freq |
|||
* @param duty |
|||
*/ |
|||
void ozone_pwm_control_module_set_pwm_output_1(uint32_t freqhz, uint8_t hardware_duty) { |
|||
ozone_pwm_control_module_set_pwm_output_2(freqhz, hardware_duty, 100 * 1000, 100); |
|||
} |
|||
/** |
|||
* @brief 定时和PWM |
|||
* |
|||
* @param hardware_period pwm频率 |
|||
* @param hardware_duty pwm占空比 |
|||
* @param large_period 定时周期 |
|||
* @param large_duty 定时占空比 |
|||
*/ |
|||
void ozone_pwm_control_module_set_pwm_output_2(uint32_t freqhz, uint8_t hardware_duty, uint32_t large_period, uint8_t large_duty) { |
|||
s_hardware_frequency = freqhz; |
|||
s_hardware_duty = hardware_duty; |
|||
s_large_period = large_period; |
|||
s_large_duty = large_duty; |
|||
prv_update_begin_ticket(); |
|||
s_ozone_pwm_control_enable_falg = true; |
|||
prv_pwm_module_set_pwm_duty(freqhz, hardware_duty); |
|||
} |
|||
void frequency_sweep(uint32_t startfreq, uint32_t step, uint32_t endfreq, uint16_t dutyns) { |
|||
/** |
|||
* @brief 扫频 |
|||
*/ |
|||
s_power_table_startfreq = startfreq; |
|||
s_frequency_sweep_step = step; |
|||
s_power_table_endfreq = endfreq; |
|||
uint16_t nowfreq = startfreq; |
|||
|
|||
void ozone_pwm_control_module_stop_pwm(void) { s_ozone_pwm_control_enable_falg = false; } |
|||
bool firstloop = true; |
|||
KFP KFPConfig = {0.02, 0, 0, 0, 0.03, 0.543}; |
|||
|
|||
void ozone_pwm_control_module_loop(void) { |
|||
static uint32_t hardware_frequency; |
|||
static uint32_t hardware_duty; |
|||
if (ozone_pwm_control_is_enable() == false) { //没有使能直接退出 |
|||
if (prv_pwm_is_enable()) { |
|||
prv_pwm_stop(); |
|||
} |
|||
return; |
|||
} |
|||
if (port_haspassedms(begin_ticket) % s_large_period <= s_large_period * s_large_duty / 100) { |
|||
if (!prv_pwm_is_enable()) { |
|||
printf("set work\r\n"); |
|||
prv_pwm_module_set_pwm_duty(hardware_frequency, hardware_duty); |
|||
port_ozone_pwm_set_duty(startfreq, dutyns); |
|||
port_ozone_pwm_start(); |
|||
systicket_delay_ms(3); |
|||
printf("frequency_sweep:startfreq:%d, step:%d, endfreq:%d, dutyns:%d\n", startfreq, step, endfreq, dutyns); |
|||
for (size_t i = 0; i < ARRAY_SIZE(s_power_table); i++) { |
|||
nowfreq = startfreq + i * s_frequency_sweep_step; |
|||
float power = mf_set_pwm_and_get_power(nowfreq, dutyns); |
|||
if (firstloop) { |
|||
firstloop = false; |
|||
KFPConfig.LastP = power; |
|||
} |
|||
} else { |
|||
if (prv_pwm_is_enable()) { |
|||
printf("set rest\r\n"); |
|||
hardware_frequency = s_hardware_frequency; //将关闭之前的频率进行保存 |
|||
hardware_duty = s_hardware_duty; //将关闭之前的占空比进行保存 |
|||
prv_pwm_stop(); |
|||
float afterfileter = kalmanFilter(&KFPConfig, power); |
|||
setpower(nowfreq, power); |
|||
/** |
|||
* @brief dumptable |
|||
*/ |
|||
printf("%d,%f,%f\n", nowfreq, s_power_table[i], afterfileter); |
|||
if (nowfreq >= s_power_table_endfreq) { |
|||
break; |
|||
} |
|||
} |
|||
port_ozone_pwm_stop(); |
|||
} |
|||
|
|||
float find_resonant_frequency(uint32_t startfreq, uint32_t endfreq) { |
|||
/** |
|||
* @brief |
|||
*/ |
|||
float minpower = 0; |
|||
|
|||
|
|||
} |
|||
#endif |
|||
static struct { bool working; } this; |
|||
|
|||
void ozone_control_init(ozone_control_config_t* config) { |
|||
/** |
|||
* @brief 扫描频率 |
|||
*/ |
|||
|
|||
while (!frequency_sweep_is_finished()) { |
|||
} |
|||
} |
|||
#endif |
|||
|
|||
void ozone_control_set_level(level_t level); |
|||
void ozone_control_start() { |
|||
frequency_sweep_start( |
|||
/*startfreq:*/ 20000, // |
|||
/*step :*/ 100, // |
|||
/*endfreq :*/ 40000, // |
|||
/*dutyns :*/ 5000 // |
|||
); |
|||
this.working = true; |
|||
} |
|||
void ozone_control_stop() {} |
|||
|
|||
void ozone_control_schedule() { |
|||
// |
|||
frequency_sweep_schedule(); |
|||
// |
|||
|
|||
|
|||
|
|||
} |
1340
project_ozone/Listings/project_o.map
File diff suppressed because it is too large
View File
File diff suppressed because it is too large
View File
Write
Preview
Loading…
Cancel
Save
Reference in new issue