2022-08-15 04:20:27 -05:00
|
|
|
/***************************************************************************
|
|
|
|
*
|
|
|
|
* 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"
|
2023-02-01 14:52:54 -06:00
|
|
|
#include "math.h"
|
2022-08-15 04:20:27 -05:00
|
|
|
|
|
|
|
// #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;
|
|
|
|
|
2023-02-01 14:52:54 -06:00
|
|
|
#define FABS(x) ((x) >= 0.f ? (x) : -(x))
|
|
|
|
#define Max(a, b) ((a) > (b) ? (a) : (b))
|
2022-08-15 04:20:27 -05:00
|
|
|
|
|
|
|
// Depend on codec_dac_vol
|
2023-02-01 14:52:54 -06:00
|
|
|
// 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};
|
2022-08-15 04:20:27 -05:00
|
|
|
|
|
|
|
// static uint32_t test_num = 0;
|
|
|
|
|
|
|
|
// int app_bt_stream_local_volume_get(void);
|
|
|
|
|
|
|
|
// y = 20log(x)
|
2023-02-01 14:52:54 -06:00
|
|
|
static inline float convert_multiple_to_db(float multiple) {
|
|
|
|
return 20 * (float)log10(multiple);
|
2022-08-15 04:20:27 -05:00
|
|
|
}
|
|
|
|
|
|
|
|
// x = 10^(y/20)
|
2023-02-01 14:52:54 -06:00
|
|
|
static inline float convert_db_to_multiple(float db) {
|
|
|
|
return (float)pow(10, db / 20);
|
2022-08-15 04:20:27 -05:00
|
|
|
}
|
|
|
|
|
2023-02-01 14:52:54 -06:00
|
|
|
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);
|
2022-08-15 04:20:27 -05:00
|
|
|
}
|
|
|
|
|
2023-02-01 14:52:54 -06:00
|
|
|
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);
|
2022-08-15 04:20:27 -05:00
|
|
|
}
|
|
|
|
|
2023-02-01 14:52:54 -06:00
|
|
|
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;
|
2022-08-15 04:20:27 -05:00
|
|
|
|
2023-02-01 14:52:54 -06:00
|
|
|
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;
|
2022-08-15 04:20:27 -05:00
|
|
|
|
2023-02-01 14:52:54 -06:00
|
|
|
normal_rate = pkd_factor2 / 32768;
|
2022-08-15 04:20:27 -05:00
|
|
|
|
2023-02-01 14:52:54 -06:00
|
|
|
tgt_rate = pkd_reduce_rate / normal_rate / vol_multiple;
|
2022-08-15 04:20:27 -05:00
|
|
|
|
2023-02-01 14:52:54 -06:00
|
|
|
if (tgt_rate > 1.0) {
|
|
|
|
tgt_rate = 1.0;
|
2022-08-15 04:20:27 -05:00
|
|
|
}
|
|
|
|
|
2023-02-01 14:52:54 -06:00
|
|
|
// rate += (tgt_rate - rate) / 10000.0;
|
|
|
|
|
|
|
|
// if(pkd_factor2>)
|
|
|
|
// {
|
|
|
|
// normal_rate = 0.25;
|
|
|
|
// }
|
|
|
|
// else
|
2022-08-15 04:20:27 -05:00
|
|
|
// {
|
2023-02-01 14:52:54 -06:00
|
|
|
// normal_rate = 0.25;
|
2022-08-15 04:20:27 -05:00
|
|
|
// }
|
2023-02-01 14:52:54 -06:00
|
|
|
// 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));
|
|
|
|
// }
|
2022-08-15 04:20:27 -05:00
|
|
|
|
|
|
|
#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
|
|
|
|
}
|
|
|
|
|
2023-02-01 14:52:54 -06:00
|
|
|
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;
|
2022-08-15 04:20:27 -05:00
|
|
|
|
2023-02-01 14:52:54 -06:00
|
|
|
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;
|
2022-08-15 04:20:27 -05:00
|
|
|
|
2023-02-01 14:52:54 -06:00
|
|
|
normal_rate = pkd_factor2 / 32768;
|
2022-08-15 04:20:27 -05:00
|
|
|
|
2023-02-01 14:52:54 -06:00
|
|
|
tgt_rate = pkd_reduce_rate / normal_rate / vol_multiple;
|
2022-08-15 04:20:27 -05:00
|
|
|
|
2023-02-01 14:52:54 -06:00
|
|
|
if (tgt_rate > 1.0) {
|
|
|
|
tgt_rate = 1.0;
|
2022-08-15 04:20:27 -05:00
|
|
|
}
|
2023-02-01 14:52:54 -06:00
|
|
|
|
|
|
|
// 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);
|
|
|
|
}
|
2022-08-15 04:20:27 -05:00
|
|
|
}
|
|
|
|
|
2023-02-01 14:52:54 -06:00
|
|
|
void peak_detector_run(uint8_t *buf, uint32_t len, float vol_multiple) {
|
|
|
|
// int vol_level = 0;
|
2022-08-15 04:20:27 -05:00
|
|
|
|
2023-02-01 14:52:54 -06:00
|
|
|
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);
|
|
|
|
}
|
2022-08-15 04:20:27 -05:00
|
|
|
|
2023-02-01 14:52:54 -06:00
|
|
|
// test_num++;
|
2022-08-15 04:20:27 -05:00
|
|
|
|
2023-02-01 14:52:54 -06:00
|
|
|
// vol_level = app_bt_stream_local_volume_get();
|
2022-08-15 04:20:27 -05:00
|
|
|
}
|