ozone_cp/APP/service/ozone_control_service.c

#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"

/**
 * @brief
 *
 *  状态机:
 *            不工作 ------> 扫描频率 ----> 工作
 *
 **/

#define PRV_START_EVENT (STATE_MACHINE_PUBLIC_EVENT + 1)
#define PRV_STOP_EVENT (STATE_MACHINE_PUBLIC_EVENT + 2)

typedef struct {
  uint16_t resonant_frequency;    //谐振频率点
  uint16_t nowfreq;               //当前频率
  bool changefreqdirection;       //频率调整方向，如果当前功率大于期望功率，则减少频率，反之亦然
  bool adjustedToTheProperPower;  //是否已经调整到恰当的功率
} Context_t;

typedef enum {
  kIdleState,
  kBeforeWorkingStateSweepFrequency,
  kWorkingState,
} StateId_t;

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 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 = 5.0;
const static float const_max_freq = 38000;

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 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 = const_level1_expect_power;
  } else if (thisDevice.level == klevel2) {
    expectpower = const_level2_expect_power;
  }
  return expectpower;
}

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, 40000, 3000);
  } else if (event == TIME_EVENT) {
    if (frequency_sweep_is_finished()) {
      context.resonant_frequency = get_resonant_frequency(25000, 100, 35000);
      printf("----------Summarize--------\n");
      printf("-resonant_frequency: %d\n", context.resonant_frequency);
      context.nowfreq = context.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 (context.nowfreq < context.resonant_frequency) {
      context.nowfreq = context.resonant_frequency;
    }
    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)) {
        context.adjustedToTheProperPower = false;
        if (nowpower < get_expect_power()) {
          context.changefreqdirection = true;
        } else {
          context.changefreqdirection = false;
        }
      }
    } else {
      /**
       * 调整功率到期望功率
       */
      if (context.changefreqdirection) {
        float nowpower = mf_get_ozone_power();
        if (nowpower < get_expect_power()) {
          context.nowfreq += 25;
          if (context.nowfreq > const_max_freq) context.nowfreq = const_max_freq;
          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 -= 25;
          if (context.nowfreq < context.resonant_frequency) context.nowfreq = context.resonant_frequency;
          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;
        }
      }
    }
  } 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();
}