pinebuds/apps/anc/src/peak_detector.c

/***************************************************************************
 *
 * Copyright 2015-2019 BES.
 * All rights reserved. All unpublished rights reserved.
 *
 * No part of this work may be used or reproduced in any form or by any
 * means, or stored in a database or retrieval system, without prior written
 * permission of BES.
 *
 * Use of this work is governed by a license granted by BES.
 * This work contains confidential and proprietary information of
 * BES. which is protected by copyright, trade secret,
 * trademark and other intellectual property rights.
 *
 ****************************************************************************/
#include "peak_detector.h"
#include "math.h"

// #define PKD_FACTOR_UP   (0.6)
// #define PKD_FACTOR_DOWN (2.0)
// #define PKD_REDUCE_RATE (0.0335)    // -30dB

static enum AUD_BITS_T pkd_samp_bits;
static float pkd_alphaR = 0.0f;
static float pkd_alphaA = 0.0f;
static float pkd_factor1 = 0.0f;
static float pkd_factor2 = 0.0f;
static float pkd_reduce_rate = 1.0f;

#define FABS(x) ((x) >= 0.f ? (x) : -(x))
#define Max(a, b) ((a) > (b) ? (a) : (b))

// Depend on codec_dac_vol
// const float pkd_vol_multiple[18] = {0.089125, 0.0, 0.005623, 0.007943,
// 0.011220, 0.015849, 0.022387, 0.031623, 0.044668, 0.063096, 0.089125,
// 0.125893, 0.177828, 0.251189, 0.354813, 0.501187, 0.707946, 1.000000};

// static uint32_t test_num = 0;

// int app_bt_stream_local_volume_get(void);

// y = 20log(x)
static inline float convert_multiple_to_db(float multiple) {
  return 20 * (float)log10(multiple);
}

// x = 10^(y/20)
static inline float convert_db_to_multiple(float db) {
  return (float)pow(10, db / 20);
}

void peak_detector_init(void) {
  pkd_alphaR = 0.0f;
  pkd_alphaA = 0.0f;
  pkd_factor1 = 0.0f;
  pkd_factor2 = 0.0f;
  pkd_reduce_rate = 1.0f;
  // TRACE(3,"[%s] pkd_alphaR = %f, pkd_alphaA = %f", __func__,
  // (double)pkd_alphaR, (double)pkd_alphaA);
}

void peak_detector_setup(PEAK_DETECTOR_CFG_T *cfg) {
  pkd_samp_bits = cfg->bits;
  pkd_alphaR = (float)exp(-1 / (cfg->factor_down * cfg->fs));
  pkd_alphaA = (float)exp(-1 / (cfg->factor_up * cfg->fs));
  pkd_reduce_rate = convert_db_to_multiple(cfg->reduce_dB);
}

static void peak_detector_run_16bits(int16_t *buf, uint32_t len,
                                     float vol_multiple) {
  float normal_rate = 1.0;
  float tgt_rate = 1.0;

  for (uint32_t i = 0; i < len; i++) {
    pkd_factor1 = Max(buf[i], pkd_alphaR * pkd_factor1);
    pkd_factor2 = pkd_alphaA * pkd_factor2 + (1 - pkd_alphaA) * pkd_factor1;

    normal_rate = pkd_factor2 / 32768;

    tgt_rate = pkd_reduce_rate / normal_rate / vol_multiple;

    if (tgt_rate > 1.0) {
      tgt_rate = 1.0;
    }

    // rate += (tgt_rate - rate) / 10000.0;

    // if(pkd_factor2>)
    // {
    //  normal_rate = 0.25;
    // }
    // else
    // {
    //  normal_rate = 0.25;
    // }
    // normal_rate *= 1.0 - pkd_factor2/32768;

    buf[i] = (int16_t)(buf[i] * tgt_rate);
    // buf[i] = 0;
    //
    // TRACE(2,"%d, %d", buf[i], pkd_factor2);
  }

  // if(test_num == 500)
  // {
  //  test_num = 0;
  //  TRACE(0,"START>>>");
  //  TRACE(2,"vol_level = %d, pkd_vol_multiple = %f", vol_level,
  //  pkd_vol_multiple[vol_level]); TRACE(3,"buf = %d, pkd_alphaR = %f,
  //  pkd_alphaA = %f", buf[len-1], pkd_alphaR, pkd_alphaA);
  //  TRACE(4,"pkd_factor1 = %f, pkd_factor2 = %f, normal_rate = %f, tgt_rate =
  //  %f", pkd_factor1, pkd_factor2, normal_rate, tgt_rate); TRACE(0,"END<<<");
  //  // TRACE(7,"[%s] buf = %d, pkd_alphaR = %f, pkd_alphaA = %f, pkd_factor1 =
  //  %f, pkd_factor2 = %f, normal_rate = %f", __func__, buf[len-1], pkd_alphaR,
  //  pkd_alphaA, pkd_factor1, pkd_factor2, (1.0 - pkd_factor2/32768));
  // }

#if 0
    short sample;
    short sample_max = 0;
    short sample_min = 0;

    for (uint32_t i = 0; i < len; i++)
    {
        sample = buf[i];
        if(sample > sample_max)
        {
            sample_max = sample;
        }

        if(sample < sample_min)
        {
            sample_min = sample;
        }
    } 


    TRACE(2,"Max = %10d, Min = %10d",sample_max, sample_min);
#endif
}

static void peak_detector_run_24bits(int32_t *buf, uint32_t len,
                                     float vol_multiple) {
  float normal_rate = 1.0;
  float tgt_rate = 1.0;

  for (uint32_t i = 0; i < len; i++) {
    pkd_factor1 = Max(buf[i], pkd_alphaR * pkd_factor1);
    pkd_factor2 = pkd_alphaA * pkd_factor2 + (1 - pkd_alphaA) * pkd_factor1;

    normal_rate = pkd_factor2 / 32768;

    tgt_rate = pkd_reduce_rate / normal_rate / vol_multiple;

    if (tgt_rate > 1.0) {
      tgt_rate = 1.0;
    }

    // rate += (tgt_rate - rate) / 10000.0;

    // if(pkd_factor2>)
    // {
    //  normal_rate = 0.25;
    // }
    // else
    // {
    //  normal_rate = 0.25;
    // }
    // normal_rate *= 1.0 - pkd_factor2/32768;

    buf[i] = (int32_t)(buf[i] * tgt_rate);
    // buf[i] = 0;
    //
    // TRACE(2,"%d, %d", buf[i], pkd_factor2);
  }
}

void peak_detector_run(uint8_t *buf, uint32_t len, float vol_multiple) {
  // int vol_level = 0;

  if (pkd_samp_bits <= AUD_BITS_16) {
    len = len / sizeof(int16_t);
    peak_detector_run_16bits((int16_t *)buf, len, vol_multiple);
  } else {
    len = len / sizeof(int32_t);
    peak_detector_run_24bits((int32_t *)buf, len, vol_multiple);
  }

  // test_num++;

  // vol_level = app_bt_stream_local_volume_get();
}