oximeter_ble_client/app/src/heart_wave_sample_service.c.bak

#include "heart_wave_sample_service.h"

#include "app_event.h"
#include "app_event_distribute.h"
#include "basic/FIR.h"
#include "basic/HC_Chen_detect.h"
#include "basic/So_Chen_detect.h"
#include "basic/adaptive_algorithm.h"
#include "basic/Pan_Tompkins_detect.h"
#include "board/board_ecg_sensor.h"
#include "nrfx_timer.h"
 

static uint16_t m_capture_buffer_a[128];
static uint16_t m_capture_buffer_b[128];

static uint16_t* m_capture_buffer;
static uint16_t  m_capture_buffer_index = 0;

volatile static float m_sensor_display_data = 0;  // 0->100
static uint32_t       m_start_capture_tp;
static uint32_t       m_frame_index = 0;

static one_frame_data_t m_sensor_little_frame_cache[LITTLE_DATA_BLOCK_FRAME_NUM];
static uint32_t         m_little_frame_index;
static bool             m_prestart_flag;

static nrfx_timer_t m_timer = NRFX_TIMER_INSTANCE(HEART_WAVE_SAMPLE_TMR_INSTANCE);

typedef struct {
  int      heartSignalCnt;
  int      heart_rate;
  uint32_t last_qrs_point;
  uint32_t time_cnt;
  uint32_t origin_time_cnt;

  uint32_t index;
  float    index_f;
} QRS_t;

QRS_t m_qrs;

void QRS_reset() {
  m_qrs.heartSignalCnt = 0;
  m_qrs.heart_rate     = 0;
  m_qrs.time_cnt       = 0;
  m_qrs.index          = 0;
  m_qrs.index_f        = 0;
  FIR_reset_buffer();
}

void QRS_process(float value) {
  bool isPeak = false;
#if 1
  float       result = value;
  SignalPoint sp;
  sp.value         = result;
  sp.index         = m_qrs.time_cnt;
  SignalPoint peak = So_Chen_detect(sp, SAMPLING_RATE * 0.25f, 4, 16);
  if (peak.index != -1) {
    isPeak = true;
  }
#endif

#if 0
  isPeak = HC_Chen_detect(value);
#endif

#if 0
  static const float CV_LIMIT         = 50.0f;
  static const float THRESHOLD_FACTOR = 3.0f;
  double             mean             = CalculateMean(value);
  double             rms              = CalculateRootMeanSquare(value);
  double             cv               = CalculateCoefficientOfVariation(value);
  double             threshold;
  if (cv > CV_LIMIT) {
    threshold = rms;
  } else {
    threshold = rms * (cv / 100.0f) * THRESHOLD_FACTOR;
  }
  bool        is_peak;
  SignalPoint result;
  result = PeakDetect(value, m_qrs.time_cnt, threshold, &is_peak);
  if (result.index != -1) {
    if (is_peak) {
      isPeak = true;
    }
  }
#endif

#if 0


	 double result = value;
	 double bandpass;
	 double integral;
	 double square;

	 bandpass = result;
	 result = Derivative(result);
	 result = Squar(result);
	 square = result;
	 result = MovingWindowIntegral(result);
	 integral = result;

	 SignalPoint peak = ThresholdCalculate(m_qrs.time_cnt,value,bandpass,square,integral);

	 if(peak.index != -1){
      isPeak = true;
	 }

#endif

  // if (isPeak) {
  //   uint32_t time_diff   = m_qrs.time_cnt - m_qrs.last_qrs_point;
  //   m_qrs.last_qrs_point = m_qrs.time_cnt;
  //   m_qrs.heartSignalCnt++;
  //   // m_qrs.heart_rate = m_qrs.heartSignalCnt;
  //   if (m_qrs.last_qrs_point != 0) {
  //     m_qrs.heart_rate = 60 * (1 / (time_diff * 1.0 / SAMPLING_RATE));
  //   }
  // }

  m_qrs.time_cnt++;
}

static const void compute_heart_rate(float sample_data) {
  ZASSERT(SAMPLE_RATE == 500);
  m_qrs.index_f = m_frame_index / 500.0 * 360;
  if ((m_qrs.index_f - m_qrs.index) > 1) {
    m_qrs.index = m_qrs.index_f;
    float val   = sample_data;
    QRS_process(FIR_filter(val));
  }
}

static void swap_buffer() {
  if (m_capture_buffer == NULL) {
    m_capture_buffer       = m_capture_buffer_a;
    m_capture_buffer_index = 0;
    return;
  }

  if (m_capture_buffer == m_capture_buffer_a) {
    m_capture_buffer = m_capture_buffer_b;
  } else {
    m_capture_buffer = m_capture_buffer_a;
  }
  m_capture_buffer_index = 0;
  return;
}
static float amp_val(uint16_t val, uint16_t valcener, float amp) {
  float valf = (float)val - valcener;
  valf       = valf * amp;
  valf += valcener;

  if (valf >= 100) {
    valf = 100;
  }

  if (valf <= 0) {
    valf = 0;
  }
  return valf;
}

typedef struct {
  float value;
  float efectiveFactor;
} filter_t;

filter_t m_filter = {0, 0.8};

static float Filter(filter_t* filter, float newInput) {
  float newv    = ((float)filter->value * (1.0f - filter->efectiveFactor)) + ((float)newInput * filter->efectiveFactor);
  filter->value = newv;
  return newv;
}

/*******************************************************************************
 *                                С�������ϱ�                                 *
 *******************************************************************************/
static inline void prvf_little_block_cache_push_one_frame(uint16_t data) {
  if (m_little_frame_index >= LITTLE_DATA_BLOCK_FRAME_NUM) {
    return;
  }
  m_sensor_little_frame_cache[m_little_frame_index].data = data;
  m_little_frame_index++;
}

static inline bool prvf_light_block_cache_is_full(void) {
  if (m_little_frame_index >= LITTLE_DATA_BLOCK_FRAME_NUM) {
    return true;
  }
  return false;
}
static inline void prvf_light_block_cache_clear(void) { m_little_frame_index = 0; }
static inline void prvf_light_block_trigger_event() {
  static app_event_t event;
  event.eventType = kevent_capture_little_data_block_event;
  for (uint32_t i = 0; i < LITTLE_DATA_BLOCK_FRAME_NUM; i++) {
    event.val.little_data_block.data[i].data = m_sensor_little_frame_cache[i].data;
  }
  event.val.little_data_block.frameIndex = m_frame_index - LITTLE_DATA_BLOCK_FRAME_NUM;
  // ZLOGI("%d", event.val.little_data_block.frameIndex);
  AppEvent_pushEvent(&event);
}

void nrfx_timer_event_handler(nrf_timer_event_t event_type, void* p_context) {  //
  uint16_t val       = BoardEcgSensor_plod_get_ecg_val();                       // 12bit
  float    val_af100 = (float)val / 4096.0f * 100;
  if (m_prestart_flag) {
    compute_heart_rate(val_af100);
    return;
  } else {
    compute_heart_rate(val_af100);
  }

  /*******************************************************************************
   *                             ��ʾ���ݼ��㲢��ֵ                              *
   *******************************************************************************/

  m_frame_index++;
  val_af100             = amp_val(val_af100, 45, 3.5f);
  val_af100             = Filter(&m_filter, val_af100);
  m_sensor_display_data = val_af100;

  /*******************************************************************************
   *                                �������ݻ���                                 *
   *******************************************************************************/
  if (m_capture_buffer == NULL) {
    swap_buffer();
  }

  if (m_capture_buffer_index < 128) {
    m_capture_buffer[m_capture_buffer_index++] = val;
  }

  if (m_capture_buffer_index == 128) {
    app_event_t evt;
    evt.eventType              = kevent_capture_256data_event;
    evt.val.capture_data_cache = (uint8_t*)m_capture_buffer;
    swap_buffer();
    AppEvent_pushEvent(&evt);
  }

  /*******************************************************************************
   *                            ʵʱ���������¼��ϱ�                             *
   *******************************************************************************/
  /**
   * @brief �������ݣ�������С���ݿ��¼�
   */
  prvf_little_block_cache_push_one_frame(val);
  if (prvf_light_block_cache_is_full()) {
    prvf_light_block_trigger_event();
    prvf_light_block_cache_clear();
  }
}

void hwss_init(void) {
  static bool m_timer_inited = false;
  if (!m_timer_inited) {
    /**
     * @brief ��ʼ����ʱ��
     */
    static nrfx_timer_config_t timer_cfg = {
        .frequency          = NRF_TIMER_FREQ_500kHz,
        .mode               = NRF_TIMER_MODE_TIMER,
        .bit_width          = NRF_TIMER_BIT_WIDTH_24,
        .p_context          = NULL,
        .interrupt_priority = NRFX_TIMER_DEFAULT_CONFIG_IRQ_PRIORITY,
    };

    // nrfx_timer_init(&m_timer, &timer_cfg, nrfx_timer_event_handler);
    ZERROR_CHECK(nrfx_timer_init(&m_timer, &timer_cfg, nrfx_timer_event_handler));
    uint32_t timer_ticks = nrfx_timer_ms_to_ticks(&m_timer, 2);  //
    ZASSERT(SAMPLE_RATE == 500);
    nrfx_timer_extended_compare(&m_timer, NRF_TIMER_CC_CHANNEL0, timer_ticks, NRF_TIMER_SHORT_COMPARE0_CLEAR_MASK, true);
    m_timer_inited = true;
  }
}
void hwss_uninit(void) { nrfx_timer_disable(&m_timer); }

void hwss_pre_start_capture(void) {
  m_start_capture_tp = znordic_getpower_on_s();
  swap_buffer();
  m_frame_index = 0;

  QRS_reset();

  prvf_light_block_cache_clear();
  nrfx_timer_enable(&m_timer);
  m_prestart_flag = true;
}

void hwss_start_capture(void) { m_prestart_flag = false; }
void hwss_stop_capture(void) {
  nrfx_timer_disable(&m_timer);
  m_frame_index = 0;
  prvf_light_block_cache_clear();
}

float hwss_read_val(void) {
  __disable_irq();
  float val = m_sensor_display_data;
  __enable_irq();
  return val;
}
float hwss_read_heart_rate(void) {  //
  return m_qrs.heart_rate;
}

int hwss_has_captured_time_ms() { return (znordic_getpower_on_s() - m_start_capture_tp) * 1000; }