|
|
@ -29,7 +29,7 @@ static zkey_t s_keys[] = { |
|
|
ZKEY_INIT("intervalkey", port_gpio_get_interval_key_state), //左3 |
|
|
ZKEY_INIT("intervalkey", port_gpio_get_interval_key_state), //左3 |
|
|
}; |
|
|
}; |
|
|
zkey_module_t key_module = ZMODULE_INIT(s_keys, onkey); |
|
|
zkey_module_t key_module = ZMODULE_INIT(s_keys, onkey); |
|
|
static zsimple_timer_t zsimple_timer_mem[20]; //最多同时存在20个定时器 |
|
|
|
|
|
|
|
|
static zsimple_timer_t zsimple_timer_mem[10]; //最多同时存在10个定时器 |
|
|
zsimple_timer_t* debuglighttimer; |
|
|
zsimple_timer_t* debuglighttimer; |
|
|
|
|
|
|
|
|
/*********************************************************************************************************************** |
|
|
/*********************************************************************************************************************** |
|
|
@ -200,6 +200,189 @@ void do_debug_light_state(zsimple_timer_t* handler) { |
|
|
port_debug_set(debug_led_state); |
|
|
port_debug_set(debug_led_state); |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
float get_ozone_power() { |
|
|
|
|
|
float powersum = 0; |
|
|
|
|
|
for (size_t i = 0; i < 5; i++) { |
|
|
|
|
|
powersum += port_adc_get_ozone_generator_power(); |
|
|
|
|
|
} |
|
|
|
|
|
return powersum / 5; |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
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. 以高度为例 定义卡尔曼结构体并初始化参数 |
|
|
|
|
|
KFP KFP_height = {0.02, 0, 0, 0, 0.03, 0.543}; |
|
|
|
|
|
|
|
|
|
|
|
/** |
|
|
|
|
|
*卡尔曼滤波器 |
|
|
|
|
|
*@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; |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
float derivative(float now) { |
|
|
|
|
|
static float last = 0; |
|
|
|
|
|
float diff = now - last; |
|
|
|
|
|
last = now; |
|
|
|
|
|
return diff; |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
#if 0 |
|
|
|
|
|
void printf_ozone_freq_and_power_table() { |
|
|
|
|
|
/** |
|
|
|
|
|
* @brief 打印臭氧发生器和频率之间的关系 |
|
|
|
|
|
*/ |
|
|
|
|
|
|
|
|
|
|
|
float startfreq = 10 * 1000; |
|
|
|
|
|
float stepfreq = 100; |
|
|
|
|
|
float nowfreq = startfreq; |
|
|
|
|
|
|
|
|
|
|
|
port_ozone_pwm_set_duty(nowfreq, 1000); |
|
|
|
|
|
port_ozone_pwm_start(); |
|
|
|
|
|
systicket_delay_ms(10); |
|
|
|
|
|
|
|
|
|
|
|
bool first = true; |
|
|
|
|
|
|
|
|
|
|
|
while (true) { |
|
|
|
|
|
|
|
|
|
|
|
port_ozone_pwm_set_duty(nowfreq, 3000); |
|
|
|
|
|
systicket_delay_ms(2); |
|
|
|
|
|
float power = get_ozone_power(); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// printf("%f %f\n", nowfreq, freq); |
|
|
|
|
|
// printf("{freq power:%f,%f}\r\n", nowfreq, power); |
|
|
|
|
|
// printf("{freq power:%f,%f}\r\n", nowfreq, power); |
|
|
|
|
|
if (first) { |
|
|
|
|
|
// kalmanFilter(&KFP_height, power); |
|
|
|
|
|
KFP_height.LastP = power; |
|
|
|
|
|
first = false; |
|
|
|
|
|
} |
|
|
|
|
|
float afterfilter = kalmanFilter(&KFP_height, power); |
|
|
|
|
|
printf("%f,%f,%f,%f\r\n", nowfreq, power, afterfilter,derivative(afterfilter)*10); |
|
|
|
|
|
|
|
|
|
|
|
// printf("{power:%f}\r\n", power); |
|
|
|
|
|
nowfreq += stepfreq; |
|
|
|
|
|
if (nowfreq >= 40 * 1000) break; |
|
|
|
|
|
} |
|
|
|
|
|
port_ozone_pwm_set_duty(33000, 3000); |
|
|
|
|
|
port_fan_set(true); |
|
|
|
|
|
while (true) { |
|
|
|
|
|
} |
|
|
|
|
|
port_ozone_pwm_stop(); |
|
|
|
|
|
|
|
|
|
|
|
while (true) { |
|
|
|
|
|
systicket_delay_ms(2); |
|
|
|
|
|
float power = get_ozone_power(); |
|
|
|
|
|
float afterfilter = kalmanFilter(&KFP_height, power); |
|
|
|
|
|
nowfreq+=50; |
|
|
|
|
|
printf("%f,%f,%f\r\n", nowfreq, power, afterfilter); |
|
|
|
|
|
} |
|
|
|
|
|
} |
|
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
|
|
#if 0 |
|
|
|
|
|
float kalmanFilter1(float input) { |
|
|
|
|
|
static KFP kfp = {0.02, 0, 0, 0, 0.03, 0.543}; |
|
|
|
|
|
return kalmanFilter(&kfp, input); |
|
|
|
|
|
} |
|
|
|
|
|
float kalmanFilter2(float input) { |
|
|
|
|
|
static KFP kfp = {0.02, 0, 0, 0, 0.03, 0.543}; |
|
|
|
|
|
return kalmanFilter(&kfp, input); |
|
|
|
|
|
} |
|
|
|
|
|
float kalmanFilter3(float input) { |
|
|
|
|
|
static KFP kfp = {0.02, 0, 0, 0, 0.03, 0.543}; |
|
|
|
|
|
return kalmanFilter(&kfp, input); |
|
|
|
|
|
} |
|
|
|
|
|
float kalmanFilter4(float input) { |
|
|
|
|
|
static KFP kfp = {0.02, 0, 0, 0, 0.03, 0.543}; |
|
|
|
|
|
return kalmanFilter(&kfp, input); |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
float 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; |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void printf_ozone_freq_and_power_table() { |
|
|
|
|
|
/** |
|
|
|
|
|
* @brief 打印臭氧发生器和频率之间的关系 |
|
|
|
|
|
*/ |
|
|
|
|
|
|
|
|
|
|
|
uint32_t startfreq = 10 * 1000; |
|
|
|
|
|
uint32_t stepfreq = 100; |
|
|
|
|
|
uint32_t nowfreq = startfreq; |
|
|
|
|
|
|
|
|
|
|
|
port_ozone_pwm_set_duty(nowfreq, 1000); |
|
|
|
|
|
// port_ozone_pwm_stop(); |
|
|
|
|
|
port_ozone_pwm_start(); |
|
|
|
|
|
systicket_delay_ms(10); |
|
|
|
|
|
|
|
|
|
|
|
bool first = true; |
|
|
|
|
|
|
|
|
|
|
|
while (true) { |
|
|
|
|
|
// printf("%d,%f,%f,%f,%f\r\n", nowfreq, // |
|
|
|
|
|
// /*kalmanFilter1*/(set_pwm_and_get_power(nowfreq, 2000)), // |
|
|
|
|
|
// /*kalmanFilter2*/(set_pwm_and_get_power(nowfreq, 4000)), // |
|
|
|
|
|
// /*kalmanFilter3*/(set_pwm_and_get_power(nowfreq, 6000)), // |
|
|
|
|
|
// /*kalmanFilter4*/(set_pwm_and_get_power(nowfreq, 8000)) // |
|
|
|
|
|
// ); |
|
|
|
|
|
|
|
|
|
|
|
// printf("{power:%f}\r\n", power); |
|
|
|
|
|
nowfreq += stepfreq; |
|
|
|
|
|
if (nowfreq >= 40 * 1000) break; |
|
|
|
|
|
} |
|
|
|
|
|
port_ozone_pwm_set_duty(33000, 3500); |
|
|
|
|
|
port_fan_set(true); |
|
|
|
|
|
while (true) { |
|
|
|
|
|
} |
|
|
|
|
|
} |
|
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
|
|
// void printf_ozone_freq_and_power_table() { |
|
|
|
|
|
// /** |
|
|
|
|
|
// * @brief 打印臭氧发生器和频率之间的关系 |
|
|
|
|
|
// */ |
|
|
|
|
|
|
|
|
|
|
|
// float startfreq = 20 * 1000; |
|
|
|
|
|
// float stepfreq = 50; |
|
|
|
|
|
// float nowfreq = startfreq; |
|
|
|
|
|
|
|
|
|
|
|
// // port_ozone_pwm_set_duty(nowfreq, 1000); |
|
|
|
|
|
// // port_ozone_pwm_start(); |
|
|
|
|
|
// // systicket_delay_ms(10); |
|
|
|
|
|
|
|
|
|
|
|
// while (true) { |
|
|
|
|
|
// // printf("{power:%f}\r\n", nowfreq); |
|
|
|
|
|
// printf("%d,%d\r\n", (int)nowfreq,(int)nowfreq); |
|
|
|
|
|
// systicket_delay_ms(100); |
|
|
|
|
|
// nowfreq += stepfreq; |
|
|
|
|
|
// // if (nowfreq >= 40 * 1000) break; |
|
|
|
|
|
// } |
|
|
|
|
|
// port_ozone_pwm_stop(); |
|
|
|
|
|
// } |
|
|
|
|
|
|
|
|
void init_all_subdevice_state() { |
|
|
void init_all_subdevice_state() { |
|
|
port_debug_set(false); |
|
|
port_debug_set(false); |
|
|
port_fan_set(false); |
|
|
port_fan_set(false); |
|
|
@ -230,15 +413,16 @@ int main(void) { |
|
|
zsimple_timer_module_init(zsimple_timer_mem, ZARR_SIZE(zsimple_timer_mem), systicket_get_now_ms); //定时器模块初始化 |
|
|
zsimple_timer_module_init(zsimple_timer_mem, ZARR_SIZE(zsimple_timer_mem), systicket_get_now_ms); //定时器模块初始化 |
|
|
zkey_init(&key_module); //按键初始化 |
|
|
zkey_init(&key_module); //按键初始化 |
|
|
|
|
|
|
|
|
// port_fan_set(true); |
|
|
|
|
|
// port_ozone_pwm_set_duty(30000, 10 * 1000); |
|
|
|
|
|
// port_ozone_pwm_start(); |
|
|
|
|
|
|
|
|
|
|
|
debuglighttimer = zsimple_timer_alloc(); |
|
|
debuglighttimer = zsimple_timer_alloc(); |
|
|
zsimple_timer_trigger_static(debuglighttimer, 300, INFINITE_TIMES /*触发次数:*/, true, do_debug_light_state); |
|
|
zsimple_timer_trigger_static(debuglighttimer, 300, INFINITE_TIMES /*触发次数:*/, true, do_debug_light_state); |
|
|
|
|
|
|
|
|
// lcs_status_light__set_color(/*r:*/ true, /*g:*/ false, /*b:*/ false); |
|
|
|
|
|
|
|
|
/** |
|
|
|
|
|
* @brief |
|
|
|
|
|
* 频率从20k起步，递增50hz，每次等待100ms计算功率 |
|
|
|
|
|
*/ |
|
|
|
|
|
|
|
|
|
|
|
printf_ozone_freq_and_power_table(); |
|
|
|
|
|
|
|
|
while (true) { |
|
|
while (true) { |
|
|
//按键扫描逻辑 |
|
|
//按键扫描逻辑 |
|
|
DO_IT_EACH_MS(KEY_PERIOD) { zkey_do_loop_in_each_period(NULL); } |
|
|
DO_IT_EACH_MS(KEY_PERIOD) { zkey_do_loop_in_each_period(NULL); } |
|
|
@ -247,7 +431,6 @@ int main(void) { |
|
|
zsimple_timer_schedule(); |
|
|
zsimple_timer_schedule(); |
|
|
lcs_shcedule(); |
|
|
lcs_shcedule(); |
|
|
process_countdwonevent(); |
|
|
process_countdwonevent(); |
|
|
printf("countddonw %d\n", thisDevice.countdonwnum_s); |
|
|
|
|
|
// test_all_light(); |
|
|
|
|
|
|
|
|
// printf("countddonw %d\n", thisDevice.countdonwnum_s); |
|
|
} |
|
|
} |
|
|
} |
|
|
} |
zhaohe
3 years ago