tianjialong
/
ozone_cp
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

完成臭氧发生棒频率自动匹配

master
zhaohe 3 years ago
parent
a341ac0bfd
commit
021ed1aedc
11 changed files with 813 additions and 669 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 19
      APP/kalmanFilter.c
  2. 20
      APP/kalmanFilter.h
  3. 35
      APP/service/frequency_sweep_service.c
  4. 37
      APP/service/ozone_control_service.c
  5. 4
      APP/service/thisdevice.h
  6. 7
      APP/state_machine.c
  7. 1
      APP/test.c
  8. 1155
      project_ozone/Listings/project_o.map
  9. 114
      project_ozone/project_o.uvgui.zel
  10. 85
      project_ozone/project_o.uvopt
  11. 5
      project_ozone/project_o.uvproj

19
APP/kalmanFilter.c
View File

@ -0,0 +1,19 @@
#include "kalmanFilter.h"
/**
*卡尔曼滤波器
*@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;
}

20
APP/kalmanFilter.h
View File

@ -0,0 +1,20 @@
#pragma once
/***********************************************************************************************************************
* ====================================================卡尔曼滤波===================================================== *
***********************************************************************************************************************/
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
/**
*卡尔曼滤波器
*@param KFP *kfp 卡尔曼结构体参数
* float input 需要滤波的参数的测量值（即传感器的采集值）
*@return 滤波后的参数（最优值）
*/
float kalmanFilter(KFP* kfp, float input);

35
APP/service/frequency_sweep_service.c
View File

@ -2,37 +2,8 @@
#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;
}
//
#include "kalmanFilter.h"
/***********************************************************************************************************************
* ====================================================THIS_MODULE==================================================== *
@ -50,7 +21,7 @@ struct {
uint32_t dutyns;
} this;
KFP KFPConfig = {0.02, 0, 0, 0, 0.05, 0.543};
static KFP KFPConfig = KALMAN_FILTER_PARA;
/***********************************************************************************************************************
* ====================================================PowerTable===================================================== *

37
APP/service/ozone_control_service.c
View File

@ -3,6 +3,7 @@
#include "../zes8p5066lib/basic.h"
#include "../zes8p5066lib/systicket.h"
#include "frequency_sweep_service.h"
#include "kalmanFilter.h"
#include "state_machine.h"
/**
@ -52,7 +53,7 @@ state_machine_t m_statemachine;
static Context_t context;
const static float const_windonws = 0.3;
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;
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;
for (uint16_t freq = startfreq; freq <= endfreq; freq += step) {
float power = frequency_sweep_get_power(freq);
if (inited) {
if (!inited) {
minpower = power;
retfreq = freq;
inited = false;
inited = true;
continue;
}
@ -82,7 +83,8 @@ static uint16_t get_resonant_frequency(uint16_t startfreq, uint16_t step, uint16
return retfreq;
}
static float mf_get_ozone_power() {
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();
@ -90,6 +92,9 @@ static float mf_get_ozone_power() {
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 expectpower = 0;
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_start();
context.adjustedToTheProperPower = false;
mf_get_ozone_power_reset_filter();
printf("----------start working--------\n");
} else if (event == TIME_EVENT) {
if (context.adjustedToTheProperPower) {
/**
* 检查当前功率是否在期望功率窗口之外时候([expertpower+window,expertpower-window])，
*/
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;
if (nowpower < get_expect_power()) {
context.changefreqdirection = true;
@ -159,21 +167,23 @@ static state_machine_state_t* processWorkingState(state_machine_t* machine, stat
if (context.changefreqdirection) {
float nowpower = mf_get_ozone_power();
if (nowpower < get_expect_power()) {
context.nowfreq += 100;
context.nowfreq += 25;
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 {
// printf("reach %f->%f\n", nowpower, get_expect_power());
context.adjustedToTheProperPower = true;
}
} else {
float nowpower = mf_get_ozone_power();
if (nowpower > get_expect_power()) {
context.nowfreq -= 100;
context.nowfreq -= 25;
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 {
// printf("reach %f->%f\n", nowpower, get_expect_power());
context.adjustedToTheProperPower = true;
}
}
@ -181,7 +191,7 @@ static state_machine_state_t* processWorkingState(state_machine_t* machine, stat
} else if (event == EXIT_STATE) {
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) {
@ -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_schedule() {
static uint32_t ticket = 0;
if (systicket_haspassedms(ticket) > 10 * 1000) {
if (systicket_haspassedms(ticket) > 10) {
ticket = systicket_get_now_ms();
state_machine_schedule_each10ms(&m_statemachine);
}
frequency_sweep_schedule();
}

4
APP/service/thisdevice.h
View File

@ -51,5 +51,9 @@ typedef struct {
#define INTERMITTENTMODE_PERIOD_S 55 //间歇工作模式的时间间隔,至少大于四倍的kconst_countdown_step_s
#define KEY_PERIOD 20 //
#define EXCEPTION_CHECK_PERIOD 50 //
#define kconst_pwm_work_dutyns 5000 //臭氧模块工作时候高电平时间
#define KALMAN_FILTER_PARA \
{ 0.02, 0, 0, 0, 0.05, 0.543 }
extern ThisDevice_t thisDevice;

7
APP/state_machine.c
View File

@ -4,7 +4,7 @@
void state_machine_init(state_machine_t* machine, state_machine_state_t* statetable, size_t len, state_machine_process_event_t process_event) {
machine->states = statetable;
machine->nstate = len;
machine->nowstate = &statetable[0];
machine->nowstate = NULL;
machine->process_event = process_event;
}
void state_machine_trigger_event(state_machine_t* machine, int event) {
@ -15,11 +15,11 @@ void state_machine_schedule_each10ms(state_machine_t* machine) {
machine->laststate = &machine->states[0];
machine->nowstate = &machine->states[0];
machine->nowstate->duration = 0;
printf("%s\n", machine->nowstate->name);
printf("%s enter\n", machine->nowstate->name);
machine->process_event(machine, machine->nowstate, ENTER_STATE);
}
if (!machine->nextstate && machine->nextstate != machine->nowstate) {
if (machine->nextstate && machine->nextstate != machine->nowstate) {
//状态退出
printf("%s exit\n", machine->nowstate->name);
machine->process_event(machine, machine->nowstate, EXIT_STATE);
@ -33,6 +33,7 @@ void state_machine_schedule_each10ms(state_machine_t* machine) {
printf("%s enter\n", machine->nowstate->name);
machine->process_event(machine, machine->nowstate, ENTER_STATE);
}
machine->nextstate = machine->process_event(machine, machine->nowstate, TIME_EVENT);
machine->nowstate->duration++;
}

1
APP/test.c
View File

@ -2,6 +2,7 @@
#include "port.h"
#include "zes8p5066lib/basic.h"
#include "zes8p5066lib/systicket.h"
void test_all_light(void) {
static uint32_t lastprocess = 0;
static uint8_t debug_led_state = 1;

1155
project_ozone/Listings/project_o.map
File diff suppressed because it is too large
View File

114
project_ozone/project_o.uvgui.zel
File diff suppressed because it is too large
View File

85
project_ozone/project_o.uvopt
View File

@ -148,7 +148,40 @@
<Name>UL2CM3(-O207 -S0 -C0 -FO7 -FD20000000 -FC1000 -FN1 -FF0ES8P5066 -FS00000000 -FL010000)</Name>
</SetRegEntry>
</TargetDriverDllRegistry>
<Breakpoint/>
<Breakpoint>
<Bp>
<Number>0</Number>
<Type>0</Type>
<LineNumber>223</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>11694</Address>
<ByteObject>0</ByteObject>
<HtxType>0</HtxType>
<ManyObjects>0</ManyObjects>
<SizeOfObject>0</SizeOfObject>
<BreakByAccess>0</BreakByAccess>
<BreakIfRCount>1</BreakIfRCount>
<Filename>..\APP\service\ozone_control_service.c</Filename>
<ExecCommand></ExecCommand>
<Expression>\\project_o\../APP/service/ozone_control_service.c\223</Expression>
</Bp>
<Bp>
<Number>1</Number>
<Type>0</Type>
<LineNumber>24</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>13818</Address>
<ByteObject>0</ByteObject>
<HtxType>0</HtxType>
<ManyObjects>0</ManyObjects>
<SizeOfObject>0</SizeOfObject>
<BreakByAccess>0</BreakByAccess>
<BreakIfRCount>1</BreakIfRCount>
<Filename>..\APP\state_machine.c</Filename>
<ExecCommand></ExecCommand>
<Expression>\\project_o\../APP/state_machine.c\24</Expression>
</Bp>
</Breakpoint>
<WatchWindow1>
<Ww>
<count>0</count>
@ -330,6 +363,18 @@
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>1</GroupNumber>
<FileNumber>10</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\APP\kalmanFilter.c</PathWithFileName>
<FilenameWithoutPath>kalmanFilter.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group>
<Group>
@ -340,7 +385,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>2</GroupNumber>
<FileNumber>10</FileNumber>
<FileNumber>11</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -352,7 +397,7 @@
</File>
<File>
<GroupNumber>2</GroupNumber>
<FileNumber>11</FileNumber>
<FileNumber>12</FileNumber>
<FileType>2</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -372,7 +417,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>12</FileNumber>
<FileNumber>13</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -384,7 +429,7 @@
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>13</FileNumber>
<FileNumber>14</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -396,7 +441,7 @@
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>14</FileNumber>
<FileNumber>15</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -408,7 +453,7 @@
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>15</FileNumber>
<FileNumber>16</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -420,7 +465,7 @@
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>16</FileNumber>
<FileNumber>17</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -432,7 +477,7 @@
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>17</FileNumber>
<FileNumber>18</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -444,7 +489,7 @@
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>18</FileNumber>
<FileNumber>19</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -456,7 +501,7 @@
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>19</FileNumber>
<FileNumber>20</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -468,7 +513,7 @@
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>20</FileNumber>
<FileNumber>21</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -480,7 +525,7 @@
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>21</FileNumber>
<FileNumber>22</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -492,7 +537,7 @@
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>22</FileNumber>
<FileNumber>23</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -512,7 +557,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>4</GroupNumber>
<FileNumber>23</FileNumber>
<FileNumber>24</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -524,7 +569,7 @@
</File>
<File>
<GroupNumber>4</GroupNumber>
<FileNumber>24</FileNumber>
<FileNumber>25</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -536,7 +581,7 @@
</File>
<File>
<GroupNumber>4</GroupNumber>
<FileNumber>25</FileNumber>
<FileNumber>26</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -548,7 +593,7 @@
</File>
<File>
<GroupNumber>4</GroupNumber>
<FileNumber>26</FileNumber>
<FileNumber>27</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -560,7 +605,7 @@
</File>
<File>
<GroupNumber>4</GroupNumber>
<FileNumber>27</FileNumber>
<FileNumber>28</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -572,7 +617,7 @@
</File>
<File>
<GroupNumber>4</GroupNumber>
<FileNumber>28</FileNumber>
<FileNumber>29</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>

5
project_ozone/project_o.uvproj
View File

@ -467,6 +467,11 @@
<FileType>1</FileType>
<FilePath>..\APP\service\human_computer_interaction_service.c</FilePath>
</File>
<File>
<FileName>kalmanFilter.c</FileName>
<FileType>1</FileType>
<FilePath>..\APP\kalmanFilter.c</FilePath>
</File>
</Files>
</Group>
<Group>

Write Preview
Loading…
Cancel
Save