rudism
/
pinebuds
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
pinebuds/apps/anc/src/anc_wnr.c

1372 lines
44 KiB
C
Raw Normal View History

add first commit
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 "string.h"
#include "app_utils.h"
#include "hal_trace.h"
#include "hal_aud.h"
#include "hal_timer.h"
#include "anc_process.h"
#include "audioflinger.h"
#include "hwtimer_list.h"
//#include "audio_dump.h"
//#include "anc_usb_app.h"
#include "speech_cfg.h"
#include "anc_wnr.h"
#include "wind_detection_2mic.h"
#include "cmsis_os.h"
#include "app_thread.h"
#include "app_ibrt_keyboard.h"
#include "app_ibrt_ui.h"
#include "bt_drv_interface.h"
#if defined(SPEECH_TX_24BIT)
#define _24BITS_ENABLE
#endif
// Wind detection algorithm
#define _SAMPLE_RATE (8000)
#define _FRAME_LEN (60)
#define _CHANNEL_NUM (2)
#if defined(_24BITS_ENABLE)
#define _SAMPLE_BITS (24)
typedef int WNR_PCM_T;
#else
#define _SAMPLE_BITS (16)
typedef short WNR_PCM_T;
#endif
#define SAMPLE_BYTES sizeof(WNR_PCM_T)
#define _FRAME_LEN_MAX (128)
#define _SAMPLE_BITS_MAX (32)
#define WINDINDICATOR_SIZE (10)
#define _LOOP_CNT (3)
//ff gain coefficient tunning
#define _NO_WIND_FF_GAIN_COEF (1.0)
#define _SMALL_WIND_FF_GAIN_COEF (0.5)
#define _STRONG_WIND_FF_GAIN_COEF (0.0)
#define _STRONG2SMALL_WIND_FF_MID_GAIN_COEF (0.25)
#define _SMALL2NO_WIND_FF_MID_GAIN_COEF (0.75)
//wind indictor threshold
#define _NO_WIND_THD (0.75)
#define _SMALL_WIND_THD (0.55)
#define _STRONG_WIND_THD (0.45)
//energy threshold
#define _POWER_THD (0.1)
//ff close time
#define _PERIOD (12)
#define _TARGET_TIME (4)
//#define TEST_MIPS
#ifdef TEST_MIPS
static uint32_t pre_ticks = 0;
static uint32_t start_ticks = 0;
static uint32_t end_ticks = 0;
static uint32_t used_mips = 0;
#endif
WindDetection2MicState *wind_st = NULL;
static WindDetection2MicConfig wind_cfg = {
.bypass = 0,
.power_thd = _POWER_THD,
};
// static uint8_t heap_buf[1024 * 12];
// Wind factor process
extern bool app_anc_work_status(void);
// Mic operation
#define ANC_WNR_STREAM_ID AUD_STREAM_ID_0
static anc_wnr_open_mode_t g_open_mode = ANC_WNR_OPEN_MODE_QTY;
// 2ch, pingpong
#define AF_STREAM_BUFF_SIZE (_FRAME_LEN * SAMPLE_BYTES * 2 * 2)
static uint8_t __attribute__((aligned(4))) af_stream_buff[AF_STREAM_BUFF_SIZE*_LOOP_CNT];
static uint8_t __attribute__((aligned(4))) af_stream_mic1[_FRAME_LEN_MAX * (_SAMPLE_BITS_MAX / 8)];
static uint8_t __attribute__((aligned(4))) af_stream_mic2[_FRAME_LEN_MAX * (_SAMPLE_BITS_MAX / 8)];
static int32_t g_sample_rate = _SAMPLE_RATE;
static int32_t g_frame_len = _FRAME_LEN;
static void _open_mic(void);
static void _close_mic(void);
#define WNR_SYNC_COUNTER_THRESHOLD_FOR_SCO 18//talk:180ms * 18 = 3240ms
#define WNR_SYNC_COUNTER_THRESHOLD_FOR_NORMAL 35//normal:90ms * 35 = 3150ms
//if not need to set ANC FF GAIN by trigger delay way, set macro to 0.
#define WNR_SYNC_TRIGGER_DELAY 300//unit:ms
//if not need to printf information about sync, set macro to 0.
#define WNR_SYNC_DEBUG_LOG 1
//default disable twostage mode so that reduce delay to set ANC FF GAIN, also to simplify code.
//in order to avoid pop voice, recommend to enable twostage mode if chip is based on 1303 lower platform.
//if need to use twostage mode to set ANC FF GAIN, set macro to 1.
#define WNR_SYNC_SET_ANC_FF_GAIN_TWOSTAGE 0
#if (WNR_SYNC_SET_ANC_FF_GAIN_TWOSTAGE == 1)
#define WNR_SYNC_TWOSTAGE_DELAY 300//unit:tick
uint8_t Twostage = 0;
uint8_t FFstate = 0;
#endif
static void app_wnr_sync_timer_handler(void const *param);
osTimerDef (APP_WNR_SYNC_TIMER, app_wnr_sync_timer_handler);
static osTimerId app_wnr_sync_timer = NULL;
#if (WNR_SYNC_SET_ANC_FF_GAIN_TWOSTAGE == 1)
static void app_wnr_twostage_handler(void const *param);
osTimerDef (APP_WNR_TWOSTAGE_TIMER, app_wnr_twostage_handler);
static osTimerId app_wnr_twostage_timer = NULL;
#endif
uint8_t g_wind_st = 0;
uint8_t cnt = 0;
uint8_t period = 12;
float windsum= 0.0;
float wind_indictor= 0.0;
static uint8_t g_set_Windstate = 0;
static uint8_t g_local_Windstate = 0;
static uint8_t g_peer_Windstate = 0;
static bool g_local_Wind_module_onoff = false;
static bool g_peer_Wind_module_onoff = false;
static uint8_t wnr_sync_counter_for_sco = 0;
static uint8_t wnr_sync_counter_for_normal = 0;
static anc_wnr_open_mode_t g_wnr_open_mode = ANC_WNR_OPEN_MODE_QTY;
static bool g_wnr_notify_flag = false;
static bool g_wnr_request_flag = false;
static bool g_wnr_sync_flag = false;
struct anc_wnr_state_queue_t
{
uint8_t buf_for_sco[WNR_SYNC_COUNTER_THRESHOLD_FOR_SCO];
uint8_t buf_for_normal[WNR_SYNC_COUNTER_THRESHOLD_FOR_NORMAL];
uint8_t index_for_sco;
uint8_t index_for_normal;
uint8_t number_for_sco;
uint8_t number_for_normal;
} g_anc_wnr_state_queue;
float windindicator[WINDINDICATOR_SIZE] ={1,1,1,1,1,1,1,1,1,1};
float windthd[3]={_NO_WIND_THD,_SMALL_WIND_THD,_STRONG_WIND_THD};//{0.75,0.35,0.30};//{0.75,0.40,0.35};{0.75,0.45,0.40};{0.75,0.50,0.45};
#ifndef HW_SUPPORT_SMOOOTHING_GAIN
#define ANC_TCTRL_NUM 2
#define ANC_TIMER_PERIOD_GAIN 50
#define TIMER_CNT 20
struct anc_tuning_ctrl {
uint8_t timer_init;
uint8_t timer_ff_gain_run;
const struct_anc_cfg *cfg;
enum ANC_TYPE_T type;
HWTIMER_ID timer_ff_gain_id;
};
static struct anc_tuning_ctrl anc_tctrl;
static uint32_t timer_cnt=0;
static bool ff_smoothing_flag = false;
#endif
static int32_t anc_ff_gain_l = 512;
static int32_t anc_ff_gain_r = 512;
static int32_t tgt_ff_gain_l, tgt_ff_gain_r;
static int32_t pre_ff_gain_l, pre_ff_gain_r;
#ifndef HW_SUPPORT_SMOOOTHING_GAIN
static int32_t delta_ff_gain_l, delta_ff_gain_r;
#endif
#ifndef HW_SUPPORT_SMOOOTHING_GAIN
void anc_gain_tuning_init(void)
{
timer_cnt = 0;
ff_smoothing_flag = false;
}
static void anc_tuning_ff_gain_timer_timeout(void *param)
{
timer_cnt++;
int32_t ff_gain_l=0;
int32_t ff_gain_r=0;
if((timer_cnt<delta_ff_gain_l)||(timer_cnt<delta_ff_gain_r)){
if(pre_ff_gain_l<tgt_ff_gain_l){
pre_ff_gain_l = pre_ff_gain_l + 1;
ff_gain_l = pre_ff_gain_l;
}
else if(pre_ff_gain_l>tgt_ff_gain_l){
pre_ff_gain_l = pre_ff_gain_l - 1;
ff_gain_l = pre_ff_gain_l;
}
if(pre_ff_gain_r<tgt_ff_gain_r){
pre_ff_gain_r = pre_ff_gain_r + 1;
ff_gain_r = pre_ff_gain_r;
}
else if(pre_ff_gain_r>tgt_ff_gain_r){
pre_ff_gain_r = pre_ff_gain_r - 1;
ff_gain_r = pre_ff_gain_r;
}
anc_set_gain(ff_gain_l, ff_gain_r, ANC_FEEDFORWARD);
TRACE(3,"[%s] ff_gain_l = %d, ff_gain_r = %d", __func__, ff_gain_l, ff_gain_r);
struct anc_tuning_ctrl *c = (struct anc_tuning_ctrl *)param;
hwtimer_start(c->timer_ff_gain_id, US_TO_TICKS(ANC_TIMER_PERIOD_GAIN));
ff_smoothing_flag = 1;
} else {
anc_set_gain(tgt_ff_gain_l, tgt_ff_gain_r, ANC_FEEDFORWARD);
ff_smoothing_flag = 0;
}
}
#endif
static void _set_anc_ff_gain(bool update_anc_gain, float gain_coef, enum ANC_TYPE_T type)
{
#ifdef HW_SUPPORT_SMOOOTHING_GAIN
if(update_anc_gain){
anc_get_gain(&pre_ff_gain_l, &pre_ff_gain_r, ANC_FEEDFORWARD);
anc_ff_gain_l = pre_ff_gain_l;
anc_ff_gain_r = pre_ff_gain_r;
TRACE(3, "[%s] Update anc_gain_l = %d, anc_gain_r = %d.", __func__, anc_ff_gain_l, anc_ff_gain_r);
}
tgt_ff_gain_l = (int32_t)(anc_ff_gain_l * gain_coef);
tgt_ff_gain_r = (int32_t)(anc_ff_gain_r * gain_coef);
TRACE(3, "[%s] tgt_ff_gain_l = %d, tgt_ff_gain_r = %d.", __func__, tgt_ff_gain_l, tgt_ff_gain_r);
anc_set_gain(tgt_ff_gain_l, tgt_ff_gain_r, ANC_FEEDFORWARD);
#else
struct anc_tuning_ctrl *c = &anc_tctrl;
//uint32_t cur_time = TICKS_TO_MS(hal_sys_timer_get());
//reset timer
if (c->timer_ff_gain_run) {
hwtimer_stop(c->timer_ff_gain_id);
}
anc_get_gain(&pre_ff_gain_l, &pre_ff_gain_r, ANC_FEEDFORWARD);
TRACE(3,"[%s] pre_ff_gain_l = %d, pre_ff_gain_r = %d.", __func__, pre_ff_gain_l, pre_ff_gain_r);
if(update_anc_gain && !ff_smoothing_flag){
anc_ff_gain_l = pre_ff_gain_l;
anc_ff_gain_r = pre_ff_gain_r;
TRACE(3,"[%s] Update anc_gain_l = %d, anc_gain_r = %d.", __func__, anc_ff_gain_l, anc_ff_gain_r);
}
tgt_ff_gain_l = (int32_t)(anc_ff_gain_l * gain_coef);
tgt_ff_gain_r = (int32_t)(anc_ff_gain_r * gain_coef);
delta_ff_gain_l = abs(tgt_ff_gain_l-pre_ff_gain_l);
delta_ff_gain_r = abs(tgt_ff_gain_r-pre_ff_gain_r);
TRACE(3,"[%s] tgt_ff_gain_l = %d, tgt_ff_gain_r = %d.", __func__, tgt_ff_gain_l, tgt_ff_gain_r);
timer_cnt=0;
//timer restart
hwtimer_start(c->timer_ff_gain_id, MS_TO_TICKS(ANC_TIMER_PERIOD_GAIN));
c->timer_ff_gain_run = 1;
//set gain directly
//anc_set_gain(gain_ch_l, gain_ch_r, type);
//AUD_TRACE(TRACE_MASK_INFO, "set anc gain :[%d], gain_ch_l=%d, gian_ch_r=%d, type=%d",
//cur_time, gain_ch_l, gain_ch_r, (int)type);
#endif
}
void anc_release_gain(void)
{
TRACE(1,"[%s] ...Release FF gain", __func__);
_set_anc_ff_gain(false, 1, ANC_FEEDFORWARD);
}
int32_t anc_wnr_ctrl(int32_t sample_rate, int32_t frame_len)
{
TRACE(3,"[%s] sample_rate = %d, frame_len = %d", __func__, sample_rate, frame_len);
g_sample_rate = sample_rate;
g_frame_len = frame_len;
if (g_sample_rate == 16000) {
g_frame_len >>= 1;
}
return 0;
}
static void app_wnr_reset_state_queue(void)
{
g_anc_wnr_state_queue.index_for_normal = 0;
g_anc_wnr_state_queue.index_for_sco = 0;
g_anc_wnr_state_queue.number_for_normal = 0;
g_anc_wnr_state_queue.number_for_sco = 0;
memset(g_anc_wnr_state_queue.buf_for_normal, 0, WNR_SYNC_COUNTER_THRESHOLD_FOR_NORMAL);
memset(g_anc_wnr_state_queue.buf_for_sco, 0, WNR_SYNC_COUNTER_THRESHOLD_FOR_SCO);
}
static void app_wnr_push_state_to_queue(uint8_t open_mode, uint8_t arg0)
{
if(open_mode == ANC_WNR_OPEN_MODE_STANDALONE)//normal mode...
{
g_anc_wnr_state_queue.buf_for_normal[g_anc_wnr_state_queue.index_for_normal] = arg0;
if(++g_anc_wnr_state_queue.index_for_normal >= WNR_SYNC_COUNTER_THRESHOLD_FOR_NORMAL)
{
g_anc_wnr_state_queue.index_for_normal = 0;
}
if(g_anc_wnr_state_queue.number_for_normal < WNR_SYNC_COUNTER_THRESHOLD_FOR_NORMAL)
{
g_anc_wnr_state_queue.number_for_normal++;
}
}
else//sco mode...
{
g_anc_wnr_state_queue.buf_for_sco[g_anc_wnr_state_queue.index_for_sco] = arg0;
if(++g_anc_wnr_state_queue.index_for_sco >= WNR_SYNC_COUNTER_THRESHOLD_FOR_SCO)
{
g_anc_wnr_state_queue.index_for_sco = 0;
}
if(g_anc_wnr_state_queue.number_for_sco < WNR_SYNC_COUNTER_THRESHOLD_FOR_SCO)
{
g_anc_wnr_state_queue.number_for_sco++;
}
}
}
static uint8_t app_wnr_get_state_from_queue(uint8_t open_mode)
{
uint8_t counter0 = 0;
uint8_t counter1 = 0;
uint8_t counter2 = 0;
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(2,"[%s] open_mode:%d", __func__, open_mode);
TRACE(2,"[%s] index_for_normal:%d", __func__, g_anc_wnr_state_queue.index_for_normal);
TRACE(2,"[%s] index_for_sco:%d", __func__, g_anc_wnr_state_queue.index_for_sco);
TRACE(2,"[%s] number_for_normal:%d", __func__, g_anc_wnr_state_queue.number_for_normal);
TRACE(2,"[%s] number_for_sco:%d", __func__, g_anc_wnr_state_queue.number_for_sco);
TRACE(1,"[%s] buf_for_normal:", __func__);
DUMP8("%d ", g_anc_wnr_state_queue.buf_for_normal, g_anc_wnr_state_queue.number_for_normal);
TRACE(1,"[%s] buf_for_sco:", __func__);
DUMP8("%d ", g_anc_wnr_state_queue.buf_for_sco, g_anc_wnr_state_queue.number_for_sco);
#endif
if(open_mode == ANC_WNR_OPEN_MODE_STANDALONE)//normal mode...
{
for(uint8_t i = 0; i<g_anc_wnr_state_queue.number_for_normal; i++)
{
if(g_anc_wnr_state_queue.buf_for_normal[i] == 0)
{
counter0++;
continue;
}
if(g_anc_wnr_state_queue.buf_for_normal[i] == 1)
{
counter1++;
continue;
}
if(g_anc_wnr_state_queue.buf_for_normal[i] == 2)
{
counter2++;
continue;
}
}
}
else//sco mode...
{
for(uint8_t i = 0; i<g_anc_wnr_state_queue.number_for_sco; i++)
{
if(g_anc_wnr_state_queue.buf_for_sco[i] == 0)
{
counter0++;
continue;
}
if(g_anc_wnr_state_queue.buf_for_sco[i] == 1)
{
counter1++;
continue;
}
if(g_anc_wnr_state_queue.buf_for_sco[i] == 2)
{
counter2++;
continue;
}
}
}
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(3,"counter0:%d counter1:%d counter2:%d", counter0, counter1, counter2);
#endif
if(counter0 >= counter1)
{
if(counter0 >= counter2)
{
return 0;
}
else
{
return 2;
}
}
else
{
if(counter1 >= counter2)
{
return 1;
}
else
{
return 2;
}
}
}
static void app_wnr_trigger_internal_event(uint32_t evt, uint32_t arg0, uint32_t arg1, uint32_t arg2)
{
APP_MESSAGE_BLOCK msg;
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(5,"[%s] evt:%d arg0:%d arg1:%d arg2:%d", __func__, evt, arg0, arg1, arg2);
#endif
msg.mod_id = APP_MODUAL_WNR;
msg.msg_body.message_id = evt;
msg.msg_body.message_Param0 = arg0;
msg.msg_body.message_Param1 = arg1;
msg.msg_body.message_Param2 = arg2;
app_mailbox_put(&msg);
}
static void app_wnr_share_module_info(void)
{
uint8_t buf[3] = {0};
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(2,"[%s] local_module_onoff:%d", __func__, g_local_Wind_module_onoff);
#endif
buf[0] = IBRT_ACTION_SYNC_WNR;
buf[1] = (uint8_t)APP_WNR_SHARE_MODULE_INFO;
buf[2] = (uint8_t)g_local_Wind_module_onoff;
app_ibrt_ui_send_user_action(buf, 3);
}
void app_wnr_sync_state(void)
{
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(1,"[%s] enter...", __func__);
#endif
wnr_sync_counter_for_sco = 0;
wnr_sync_counter_for_normal = 0;
g_wnr_notify_flag = false;
g_wnr_request_flag = false;
g_peer_Wind_module_onoff = false;
g_peer_Windstate = 0;
if(g_local_Wind_module_onoff == true)
{
g_wnr_sync_flag = true;
}
}
#if (WNR_SYNC_SET_ANC_FF_GAIN_TWOSTAGE == 1)
static void app_wnr_twostage_handler(void const *param)
{
float gain_coef = 0.0;
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(1,"[%s] enter...", __func__);
#endif
if((Twostage==1)&&(FFstate==7))
{
FFstate = 1;//25%->50%
Twostage = 0;
gain_coef = _SMALL_WIND_FF_GAIN_COEF;
_set_anc_ff_gain(false, gain_coef, ANC_FEEDFORWARD);
TRACE(1,"[%s] Enable 0.5 FF ANC.", __func__);
}
else if((Twostage==1)&&(FFstate==6))
{
FFstate = 0;//75%->100%
Twostage = 0;
gain_coef = _NO_WIND_FF_GAIN_COEF;
_set_anc_ff_gain(false, gain_coef, ANC_FEEDFORWARD);
TRACE(1,"[%s] Enable FF ANC.", __func__);
}
}
#endif
static void app_wnr_sync_timer_handler(void const *param)
{
uint8_t temp = *((uint8_t *)param);
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(2,"[%s] arg0:%d", __func__, temp);
#endif
if(g_local_Wind_module_onoff == true)
app_wnr_trigger_internal_event(APP_WNR_EXCUTE_TRIGGER, (uint32_t)temp, 0, 0);
}
static void app_wnr_notify_detect_result(void)
{
uint8_t buf[3] = {0};
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(2,"[%s] local_Windstate:%d", __func__, g_local_Windstate);
#endif
g_wnr_sync_flag = false;
g_wnr_notify_flag = true;
buf[0] = IBRT_ACTION_SYNC_WNR;
buf[1] = (uint8_t)APP_WNR_NOTIFY_DETECT_RESULT;
buf[2] = g_local_Windstate;
app_ibrt_ui_send_user_action(buf, 3);
}
static void app_wnr_request_detect_result(void)
{
uint8_t buf[3] = {0};
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(2,"[%s] local_Windstate:%d", __func__, g_local_Windstate);
#endif
g_wnr_sync_flag = false;
g_wnr_request_flag = true;
buf[0] = IBRT_ACTION_SYNC_WNR;
buf[1] = (uint8_t)APP_WNR_REQUEST_DETECT_RESULT;
buf[2] = g_local_Windstate;
app_ibrt_ui_send_user_action(buf, 3);
}
static void app_wnr_response_detect_result(uint32_t arg0)
{
uint8_t buf[3] = {0};
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(1,"[%s] enter...", __func__);
#endif
if(g_wnr_notify_flag == true)
{
g_wnr_notify_flag = false;
return;
}
g_wnr_sync_flag = false;
g_local_Windstate = app_wnr_get_state_from_queue(g_wnr_open_mode);
g_peer_Windstate = (uint8_t)arg0;
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(3,"[%s] local_Windstate:%d peer_Windstate:%d", __func__, g_local_Windstate, g_peer_Windstate);
#endif
buf[0] = IBRT_ACTION_SYNC_WNR;
buf[1] = (uint8_t)APP_WNR_RESPONSE_DETECT_RESULT;
buf[2] = g_local_Windstate;
app_ibrt_ui_send_user_action(buf, 3);
}
static void app_wnr_process_detect_result(uint32_t arg0)
{
if(g_wnr_request_flag == false)
{
g_local_Windstate = app_wnr_get_state_from_queue(g_wnr_open_mode);
}
g_wnr_sync_flag = false;
g_wnr_request_flag = false;
g_peer_Windstate = (uint8_t)arg0;
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(3,"[%s] local_Windstate:%d peer_wnr_state:%d", __func__, g_local_Windstate, arg0);
#endif
if(g_local_Windstate >= g_peer_Windstate)
{
if(g_local_Wind_module_onoff == true)
app_wnr_trigger_internal_event(APP_WNR_SET_TRIGGER, (uint32_t)g_local_Windstate, 0, 0);
}
else
{
if(g_local_Wind_module_onoff == true)
app_wnr_trigger_internal_event(APP_WNR_SET_TRIGGER, (uint32_t)g_peer_Windstate, 0, 0);
}
}
static void app_wnr_set_trigger(uint32_t arg0, uint32_t arg1)
{
ibrt_ctrl_t *p_ibrt_ctrl = app_tws_ibrt_get_bt_ctrl_ctx();
uint32_t current_ticks = 0;
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(2,"[%s] set_Windstate:%d", __func__, arg0);
#endif
if((app_tws_ibrt_tws_link_connected() == true) && (p_ibrt_ctrl->nv_role == IBRT_MASTER) && (g_peer_Wind_module_onoff == true))
{
uint8_t buf[7] = {0};
current_ticks = bt_syn_get_curr_ticks(p_ibrt_ctrl->tws_conhandle);
uint32_t tg_ticks = current_ticks + MS_TO_TICKS(WNR_SYNC_TRIGGER_DELAY);
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(3,"[%s] current_ticks:%d tg_ticks:%d", __func__, current_ticks, tg_ticks);
#endif
buf[0] = IBRT_ACTION_SYNC_WNR;
buf[1] = (uint8_t)APP_WNR_SET_TRIGGER;
buf[2] = (uint8_t)arg0;
buf[3] = (uint8_t)((tg_ticks&0xff000000)>>24);
buf[4] = (uint8_t)((tg_ticks&0x00ff0000)>>16);
buf[5] = (uint8_t)((tg_ticks&0x0000ff00)>>8);
buf[6] = (uint8_t)(tg_ticks&0x000000ff);
app_ibrt_ui_send_user_action(buf, 7);
if((app_wnr_sync_timer != NULL) && (tg_ticks != current_ticks))
{
g_set_Windstate = (uint8_t)arg0;
osTimerStop(app_wnr_sync_timer);
osTimerStart(app_wnr_sync_timer, WNR_SYNC_TRIGGER_DELAY);
}
else
{
if(g_local_Wind_module_onoff == true)
app_wnr_trigger_internal_event(APP_WNR_EXCUTE_TRIGGER, arg0, 0, 0);
}
return;
}
if((app_tws_ibrt_tws_link_connected() == true) && (p_ibrt_ctrl->nv_role == IBRT_SLAVE) && (g_peer_Wind_module_onoff == true))
{
g_wnr_notify_flag = false;
current_ticks = bt_syn_get_curr_ticks(p_ibrt_ctrl->tws_conhandle);
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(3,"[%s] current_ticks:%d tg_ticks:%d", __func__, current_ticks, arg1);
#endif
if(arg1 > current_ticks)
{
uint32_t diff_ticks = arg1 - current_ticks;
g_set_Windstate = (uint8_t)arg0;
if(app_wnr_sync_timer != NULL)
{
osTimerStop(app_wnr_sync_timer);
osTimerStart(app_wnr_sync_timer, TICKS_TO_MS(diff_ticks));
}
}
else
{
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(1,"[%s] tg_ticks < current_ticks...", __func__);
#endif
if(g_local_Wind_module_onoff == true)
app_wnr_trigger_internal_event(APP_WNR_EXCUTE_TRIGGER, arg0, 0, 0);
}
return;
}
if((app_tws_ibrt_tws_link_connected() == false) || (g_peer_Wind_module_onoff == false))
{
if(g_local_Wind_module_onoff == true)
app_wnr_trigger_internal_event(APP_WNR_EXCUTE_TRIGGER, arg0, 0, 0);
return;
}
}
static void app_wnr_excute_trigger(uint32_t arg0)
{
uint8_t t_Windstate = (uint8_t)arg0;
float gain_coef = 0.0;
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(3,"[%s] last_Windstate:%d set_Windstate:%d", __func__, g_wind_st, t_Windstate);
#endif
if((t_Windstate != g_wind_st)&&(app_anc_work_status()))
{
if(g_wind_st > t_Windstate)
{
#if (WNR_SYNC_SET_ANC_FF_GAIN_TWOSTAGE == 1)
Twostage = 1;
#endif
if(t_Windstate==1){
#if (WNR_SYNC_SET_ANC_FF_GAIN_TWOSTAGE == 1)
FFstate = 7;//25%
gain_coef = _STRONG2SMALL_WIND_FF_MID_GAIN_COEF;
_set_anc_ff_gain(false, gain_coef, ANC_FEEDFORWARD);
TRACE(1,"[%s] Enable 0.25 FF ANC.", __func__);
#else
gain_coef = _SMALL_WIND_FF_GAIN_COEF;
_set_anc_ff_gain(false, gain_coef, ANC_FEEDFORWARD);
TRACE(1,"[%s] Enable 0.5 FF ANC.", __func__);
#endif
}//2->1 0%->25%->50%
else if(t_Windstate==0){
#if (WNR_SYNC_SET_ANC_FF_GAIN_TWOSTAGE == 1)
FFstate = 6;//75%
gain_coef = _SMALL2NO_WIND_FF_MID_GAIN_COEF;
_set_anc_ff_gain(false, gain_coef, ANC_FEEDFORWARD);
TRACE(1,"[%s] Enable 0.75 FF ANC.", __func__);
#else
gain_coef = _NO_WIND_FF_GAIN_COEF;
_set_anc_ff_gain(false, gain_coef, ANC_FEEDFORWARD);
TRACE(1,"[%s] Enable FF ANC.", __func__);
#endif
}//1->0 50%->75%->100%
}
else
{
if(t_Windstate==2)//0%
{
#if (WNR_SYNC_SET_ANC_FF_GAIN_TWOSTAGE == 1)
FFstate = 2;
#endif
gain_coef = _STRONG_WIND_FF_GAIN_COEF;
_set_anc_ff_gain(false, gain_coef, ANC_FEEDFORWARD);
TRACE(1,"[%s] Disable FF ANC.", __func__);
}
else if(t_Windstate==1)//50%
{
#if (WNR_SYNC_SET_ANC_FF_GAIN_TWOSTAGE == 1)
FFstate = 1;
#endif
gain_coef = _SMALL_WIND_FF_GAIN_COEF;
_set_anc_ff_gain(false, gain_coef, ANC_FEEDFORWARD);
TRACE(1,"[%s] Enable 0.5 FF ANC.", __func__);
}
else
{
#if (WNR_SYNC_SET_ANC_FF_GAIN_TWOSTAGE == 1)
FFstate = 0;
#endif
gain_coef = _NO_WIND_FF_GAIN_COEF;
_set_anc_ff_gain(false, gain_coef, ANC_FEEDFORWARD);
TRACE(1,"[%s] Enable FF ANC.", __func__);
}
}
g_wind_st = t_Windstate;
}
#if (WNR_SYNC_SET_ANC_FF_GAIN_TWOSTAGE == 1)
if(((Twostage==1)&&(FFstate==7))||((Twostage==1)&&(FFstate==6)))
{
if(app_wnr_twostage_timer != NULL)
{
osTimerStop(app_wnr_twostage_timer);
osTimerStart(app_wnr_twostage_timer, WNR_SYNC_TWOSTAGE_DELAY);
}
}
#endif
}
static int app_wnr_internal_event_process(APP_MESSAGE_BODY *msg_body)
{
uint32_t evt = msg_body->message_id;
uint32_t arg0 = msg_body->message_Param0;
uint32_t arg1 = msg_body->message_Param1;
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(4,"[%s] evt:%d arg0:%d arg1:%d", __func__, evt, arg0, arg1);
#endif
wnr_sync_counter_for_sco = 0;
wnr_sync_counter_for_normal = 0;
switch (evt)
{
case APP_WNR_NOTIFY_DETECT_RESULT:app_wnr_notify_detect_result();break;
case APP_WNR_REQUEST_DETECT_RESULT:app_wnr_request_detect_result();break;
case APP_WNR_RESPONSE_DETECT_RESULT:app_wnr_response_detect_result(arg0);break;
case APP_WNR_PROCESS_DETECT_RESULT:app_wnr_process_detect_result(arg0);break;
case APP_WNR_SET_TRIGGER:app_wnr_set_trigger(arg0, arg1);break;
case APP_WNR_EXCUTE_TRIGGER:app_wnr_excute_trigger(arg0);break;
default:TRACE(2,"[%s] invalid evt:%d", __func__, evt);break;
}
return 0;
}
void app_wnr_cmd_receive_process(uint8_t *p_buff, uint16_t length)
{
ibrt_ctrl_t *p_ibrt_ctrl = app_tws_ibrt_get_bt_ctrl_ctx();
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(1,"[%s] enter...", __func__);
#endif
if((p_buff[0] == IBRT_ACTION_SYNC_WNR) && (p_buff[1] == APP_WNR_NOTIFY_DETECT_RESULT) && (app_tws_ibrt_tws_link_connected() == true) && (p_ibrt_ctrl->nv_role == IBRT_MASTER))
{
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(1,"NOTIFY_DETECT_RESULT peer_wnr_state:%d", p_buff[2]);
#endif
g_peer_Wind_module_onoff = true;
if(g_local_Wind_module_onoff == true)
app_wnr_trigger_internal_event(APP_WNR_PROCESS_DETECT_RESULT, (uint32_t)p_buff[2], 0, 0);
return;
}
if((p_buff[0] == IBRT_ACTION_SYNC_WNR) && (p_buff[1] == APP_WNR_REQUEST_DETECT_RESULT) && (app_tws_ibrt_tws_link_connected() == true) && (p_ibrt_ctrl->nv_role == IBRT_SLAVE))
{
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(1,"REQUEST_DETECT_RESULT peer_wnr_state:%d", p_buff[2]);
#endif
g_peer_Wind_module_onoff = true;
if(g_local_Wind_module_onoff == true)
app_wnr_trigger_internal_event(APP_WNR_RESPONSE_DETECT_RESULT, (uint32_t)p_buff[2], 0, 0);
return;
}
if((p_buff[0] == IBRT_ACTION_SYNC_WNR) && (p_buff[1] == APP_WNR_RESPONSE_DETECT_RESULT) && (app_tws_ibrt_tws_link_connected() == true) && (p_ibrt_ctrl->nv_role == IBRT_MASTER))
{
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(1,"RESPONSE_DETECT_RESULT peer_wnr_state:%d", p_buff[2]);
#endif
g_peer_Wind_module_onoff = true;
if(g_local_Wind_module_onoff == true)
app_wnr_trigger_internal_event(APP_WNR_PROCESS_DETECT_RESULT, (uint32_t)p_buff[2], 0, 0);
return;
}
if((p_buff[0] == IBRT_ACTION_SYNC_WNR) && (p_buff[1] == APP_WNR_SET_TRIGGER) && (app_tws_ibrt_tws_link_connected() == true) && (p_ibrt_ctrl->nv_role == IBRT_SLAVE))
{
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(1,"SET_TRIGGER set_Windstate:%d", p_buff[2]);
#endif
uint32_t tg_ticks = 0;
tg_ticks += p_buff[3];
tg_ticks = tg_ticks << 8;
tg_ticks += p_buff[4];
tg_ticks = tg_ticks << 8;
tg_ticks += p_buff[5];
tg_ticks = tg_ticks << 8;
tg_ticks += p_buff[6];
if(g_local_Wind_module_onoff == true)
app_wnr_trigger_internal_event(APP_WNR_SET_TRIGGER, (uint32_t)p_buff[2], tg_ticks, 0);
return;
}
if((p_buff[0] == IBRT_ACTION_SYNC_WNR) && (p_buff[1] == APP_WNR_SHARE_MODULE_INFO))
{
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(1,"APP_WNR_SHARE_MODULE_INFO peer_module_onoff:%d", p_buff[2]);
#endif
g_peer_Wind_module_onoff = (bool)p_buff[2];
if((app_tws_ibrt_tws_link_connected() == true) && (p_ibrt_ctrl->nv_role == IBRT_MASTER) && (g_local_Wind_module_onoff == true) && (g_peer_Wind_module_onoff == true))
{
wnr_sync_counter_for_normal = 0;
wnr_sync_counter_for_sco = 0;
if(g_local_Wind_module_onoff == true)
{
g_wnr_sync_flag = true;
}
return;
}
if((app_tws_ibrt_tws_link_connected() == true) && (p_ibrt_ctrl->nv_role == IBRT_SLAVE) && (g_local_Wind_module_onoff == true) && (g_peer_Wind_module_onoff == true))
{
wnr_sync_counter_for_normal = 0;
wnr_sync_counter_for_sco = 0;
if(g_local_Wind_module_onoff == true)
{
g_wnr_sync_flag = true;
}
return;
}
return;
}
}
static bool wnr_open_flg = 0;
int32_t anc_wnr_open(anc_wnr_open_mode_t mode)
{
if(wnr_open_flg == 1){
return 0;
}
TRACE(4,"[%s] mode = %d, g_sample_rate = %d, g_frame_len = %d", __func__, mode, g_sample_rate, g_frame_len);
hal_sysfreq_req(APP_SYSFREQ_USER_ANC_WNR, HAL_CMU_FREQ_26M);
TRACE(2,"[%s] Sys freq: %d", __func__, hal_sys_timer_calc_cpu_freq(5, 0));
app_wnr_reset_state_queue();
app_set_threadhandle(APP_MODUAL_WNR, app_wnr_internal_event_process);
g_local_Wind_module_onoff = true;
if (app_wnr_sync_timer == NULL)
{
app_wnr_sync_timer = osTimerCreate (osTimer(APP_WNR_SYNC_TIMER), osTimerOnce, &g_set_Windstate);
}
#if (WNR_SYNC_SET_ANC_FF_GAIN_TWOSTAGE == 1)
if (app_wnr_twostage_timer == NULL)
{
app_wnr_twostage_timer = osTimerCreate(osTimer(APP_WNR_TWOSTAGE_TIMER), osTimerOnce, NULL);
}
#endif
g_set_Windstate = 0;
g_local_Windstate = 0;
g_peer_Windstate = 0;
g_wnr_notify_flag = false;
g_wnr_request_flag = false;
g_wnr_sync_flag = false;
g_wind_st = 0;
anc_ff_gain_l = 512;
anc_ff_gain_r = 512;
app_wnr_share_module_info();
if (mode == ANC_WNR_OPEN_MODE_STANDALONE) {
g_wnr_open_mode = ANC_WNR_OPEN_MODE_STANDALONE;//normal mode...
wnr_sync_counter_for_normal = 0;
wnr_sync_counter_for_sco = 0;
g_sample_rate = _SAMPLE_RATE;
g_frame_len = _FRAME_LEN;
wind_st = WindDetection2Mic_create(_SAMPLE_RATE,_SAMPLE_BITS, _FRAME_LEN, &wind_cfg);
_open_mic();
//audio_dump_init(_FRAME_LEN, sizeof(short), 2);
} else if (mode == ANC_WNR_OPEN_MODE_CONFIGURE) {
g_wnr_open_mode = ANC_WNR_OPEN_MODE_CONFIGURE;//sco mode...
wnr_sync_counter_for_sco = 0;
wnr_sync_counter_for_normal = 0;
ASSERT(g_sample_rate == 8000 || g_sample_rate == 16000, "[%s] g_sample_rate(%d) is invalid.", __func__, g_sample_rate);
ASSERT(g_frame_len == 60 || g_frame_len == 120, "[%s] g_frame_len(%d) is invalid.", __func__, g_frame_len);
wind_st = WindDetection2Mic_create(_SAMPLE_RATE,_SAMPLE_BITS, g_frame_len, &wind_cfg);
//audio_dump_init(g_frame_len, sizeof(int), 2);
} else {
ASSERT(0, "[%s] mode(%d) is invalid.", __func__, mode);
}
#ifndef HW_SUPPORT_SMOOOTHING_GAIN
anc_gain_tuning_init();
struct anc_tuning_ctrl *c = &anc_tctrl;
if (!c->timer_init) {
c->timer_ff_gain_id = hwtimer_alloc(anc_tuning_ff_gain_timer_timeout, (void *)c);
c->timer_ff_gain_run = 0;
c->timer_init = 1;
}
#endif
g_open_mode = mode;
TRACE(1,"[%s] End...", __func__);
wnr_open_flg = 1;
return 0;
}
int32_t anc_wnr_close(void)
{
TRACE(1,"[%s] ...", __func__);
#ifndef HW_SUPPORT_SMOOOTHING_GAIN
struct anc_tuning_ctrl *c = &anc_tctrl;
if (c->timer_ff_gain_run) {
hwtimer_stop(c->timer_ff_gain_id);
}
hwtimer_free(c->timer_ff_gain_id);
c->timer_init = 0;
#endif
if (g_open_mode == ANC_WNR_OPEN_MODE_STANDALONE) {
_close_mic();
g_open_mode = ANC_WNR_OPEN_MODE_QTY;
}
WindDetection2Mic_destroy(wind_st);
// size_t total = 0, used = 0, max_used = 0;
// speech_memory_info(&total, &used, &max_used);
// TRACE(3,"ANC WNR MALLOC MEM: total - %d, used - %d, max_used - %d.", total, used, max_used);
// ASSERT(used == 0, "[%s] used != 0", __func__);
app_wnr_reset_state_queue();
app_set_threadhandle(APP_MODUAL_WNR, NULL);
g_local_Wind_module_onoff = false;
if (app_wnr_sync_timer != NULL)
{
osTimerStop(app_wnr_sync_timer);
}
#if (WNR_SYNC_SET_ANC_FF_GAIN_TWOSTAGE == 1)
if (app_wnr_twostage_timer != NULL)
{
osTimerStop(app_wnr_twostage_timer);
}
#endif
g_wnr_open_mode = ANC_WNR_OPEN_MODE_QTY;
wnr_sync_counter_for_normal = 0;
wnr_sync_counter_for_sco = 0;
g_set_Windstate = 0;
g_local_Windstate = 0;
g_peer_Windstate = 0;
g_wnr_notify_flag = false;
g_wnr_request_flag = false;
g_wnr_sync_flag = false;
g_wind_st = 0;
anc_ff_gain_l = 512;
anc_ff_gain_r = 512;
app_wnr_share_module_info();
hal_sysfreq_req(APP_SYSFREQ_USER_ANC_WNR, HAL_CMU_FREQ_32K);
wnr_open_flg = 0;
return 0;
}
// TODO: Provide API to configure performance
static int32_t anc_wnr_process_frame(WNR_PCM_T *inF, WNR_PCM_T *inR, uint32_t frame_len)
{
//static short mutetimer = 0;
//short targettimer = _TARGET_TIME;
float windictor = 0.0;
float wind_power;
ibrt_ctrl_t *p_ibrt_ctrl = app_tws_ibrt_get_bt_ctrl_ctx();
if(cnt<=_PERIOD)
{
#if _SAMPLE_BITS==16
windictor = WindDetection2Mic_process_16bit(wind_st, inF, inR, frame_len, &wind_power);
#else
windictor = WindDetection2Mic_process_24bit(wind_st, inF, inR, frame_len, &wind_power);
#endif
//TRACE(2,"[%s] wind_power = %d.", __func__, (int)(wind_power*10000));
windsum = windsum + windictor;
cnt = cnt + 1;
//TRACE(2,"[%s] cnt = %d, windsum = %d.", __func__, cnt, (int)windsum);
}
if(cnt==period)
{
cnt = 0;
wind_indictor = 0.8 * wind_indictor + 0.2 * windsum/period;
windsum = 0.0;
//TRACE(2,"[%s] wind_indictor = %d.", __func__, (int)(wind_indictor*1000));
//float gain_coef;
static uint8_t Windstate = 0;
//Windstate = wind_state_detect(g_wind_st, wind_indictor);
//TRACE(2,"[%s] windstate = %d.", __func__, Windstate);
//mutetimer = mutetimer + 1;
wind_state_detect(g_wind_st, wind_indictor, windindicator, windthd, &Windstate);
for(int i = WINDINDICATOR_SIZE-1;i>0;i--)
{
windindicator[i] = windindicator[i-1];
}
windindicator[0] = wind_indictor;
//TRACE(2,"[%s] windstate = %d.", __func__, Windstate);
app_wnr_push_state_to_queue(g_wnr_open_mode, Windstate);
if(g_wnr_open_mode == ANC_WNR_OPEN_MODE_STANDALONE)
{
if(++wnr_sync_counter_for_normal >= WNR_SYNC_COUNTER_THRESHOLD_FOR_NORMAL)
{
wnr_sync_counter_for_normal = 0;
wnr_sync_counter_for_sco = 0;
g_local_Windstate = app_wnr_get_state_from_queue(g_wnr_open_mode);
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(1,"[%s] OPEN_MODE_STANDALONE", __func__);
TRACE(2,"[%s] local_Windstate:%d", __func__, g_local_Windstate);
TRACE(2,"[%s] last_Windstate:%d", __func__, g_wind_st);
#endif
if((app_tws_ibrt_tws_link_connected() == true) && (p_ibrt_ctrl->nv_role == IBRT_MASTER) && (g_wnr_sync_flag == true))
{
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(1,"[%s] REQUEST_DETECT_RESULT", __func__);
#endif
if(g_local_Wind_module_onoff == true)
app_wnr_trigger_internal_event(APP_WNR_REQUEST_DETECT_RESULT, 0, 0, 0);
return 0;
}
if((app_tws_ibrt_tws_link_connected() == true) && (p_ibrt_ctrl->nv_role == IBRT_SLAVE) && (g_wnr_sync_flag == true))
{
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(1,"[%s] NOTIFY_DETECT_RESULT", __func__);
#endif
if(g_local_Wind_module_onoff == true)
app_wnr_trigger_internal_event(APP_WNR_NOTIFY_DETECT_RESULT, 0, 0, 0);
return 0;
}
if((app_tws_ibrt_tws_link_connected() == true) && (p_ibrt_ctrl->nv_role == IBRT_MASTER) && (g_local_Windstate != g_wind_st) && (g_peer_Wind_module_onoff == true))
{
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(1,"[%s] REQUEST_DETECT_RESULT", __func__);
#endif
if(g_local_Wind_module_onoff == true)
app_wnr_trigger_internal_event(APP_WNR_REQUEST_DETECT_RESULT, 0, 0, 0);
return 0;
}
if((app_tws_ibrt_tws_link_connected() == true) && (p_ibrt_ctrl->nv_role == IBRT_SLAVE) && (g_local_Windstate != g_wind_st) && (g_peer_Wind_module_onoff == true))
{
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(1,"[%s] NOTIFY_DETECT_RESULT", __func__);
#endif
if(g_local_Wind_module_onoff == true)
app_wnr_trigger_internal_event(APP_WNR_NOTIFY_DETECT_RESULT, 0, 0, 0);
return 0;
}
if(((app_tws_ibrt_tws_link_connected() == false) || (g_peer_Wind_module_onoff == false)) && (g_local_Windstate != g_wind_st))
{
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(1,"[%s] SET_TRIGGER", __func__);
#endif
if(g_local_Wind_module_onoff == true)
app_wnr_trigger_internal_event(APP_WNR_SET_TRIGGER, (uint32_t)g_local_Windstate, 0, 0);
return 0;
}
}
}
else
{
if(++wnr_sync_counter_for_sco >= WNR_SYNC_COUNTER_THRESHOLD_FOR_SCO)
{
wnr_sync_counter_for_sco = 0;
wnr_sync_counter_for_normal = 0;
g_local_Windstate = app_wnr_get_state_from_queue(g_wnr_open_mode);
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(1,"[%s] OPEN_MODE_CONFIGURE", __func__);
TRACE(2,"[%s] local_Windstate:%d", __func__, g_local_Windstate);
TRACE(2,"[%s] last_Windstate:%d", __func__, g_wind_st);
#endif
if((app_tws_ibrt_tws_link_connected() == true) && (p_ibrt_ctrl->nv_role == IBRT_MASTER) && (g_wnr_sync_flag == true))
{
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(1,"[%s] REQUEST_DETECT_RESULT", __func__);
#endif
if(g_local_Wind_module_onoff == true)
app_wnr_trigger_internal_event(APP_WNR_REQUEST_DETECT_RESULT, 0, 0, 0);
return 0;
}
if((app_tws_ibrt_tws_link_connected() == true) && (p_ibrt_ctrl->nv_role == IBRT_SLAVE) && (g_wnr_sync_flag == true))
{
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(1,"[%s] NOTIFY_DETECT_RESULT", __func__);
#endif
if(g_local_Wind_module_onoff == true)
app_wnr_trigger_internal_event(APP_WNR_NOTIFY_DETECT_RESULT, 0, 0, 0);
return 0;
}
if((app_tws_ibrt_tws_link_connected() == true) && (p_ibrt_ctrl->nv_role == IBRT_MASTER) && (g_local_Windstate != g_wind_st) && (g_peer_Wind_module_onoff == true))
{
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(1,"[%s] REQUEST_DETECT_RESULT", __func__);
#endif
if(g_local_Wind_module_onoff == true)
app_wnr_trigger_internal_event(APP_WNR_REQUEST_DETECT_RESULT, 0, 0, 0);
return 0;
}
if((app_tws_ibrt_tws_link_connected() == true) && (p_ibrt_ctrl->nv_role == IBRT_SLAVE) && (g_local_Windstate != g_wind_st) && (g_peer_Wind_module_onoff == true))
{
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(1,"[%s] NOTIFY_DETECT_RESULT", __func__);
#endif
if(g_local_Wind_module_onoff == true)
app_wnr_trigger_internal_event(APP_WNR_NOTIFY_DETECT_RESULT, 0, 0, 0);
return 0;
}
if(((app_tws_ibrt_tws_link_connected() == false) || (g_peer_Wind_module_onoff == false)) && (g_local_Windstate != g_wind_st))
{
#if(WNR_SYNC_DEBUG_LOG == 1)
TRACE(1,"[%s] SET_TRIGGER", __func__);
#endif
if(g_local_Wind_module_onoff == true)
app_wnr_trigger_internal_event(APP_WNR_SET_TRIGGER, (uint32_t)g_local_Windstate, 0, 0);
return 0;
}
}
}
}
return 0;
}
static void inline stereo_resample_16k(WNR_PCM_T *pcm_buf, uint32_t pcm_len, WNR_PCM_T *mic1, WNR_PCM_T *mic2)
{
uint32_t frame_len = pcm_len / _CHANNEL_NUM;
const float num[3] = {0.020083, 0.040166, 0.020083};
const float den[3] = {1.000000, -1.561018, 0.641351};
static float y0 = 0, y1 = 0, y2 = 0, x0 = 0, x1 = 0, x2 = 0;
static float Y0 = 0, Y1 = 0, Y2 = 0, X0 = 0, X1 = 0, X2 = 0;
for (uint32_t i=0; i<frame_len; i++) {
x0 = pcm_buf[_CHANNEL_NUM * i + 0];
X0 = pcm_buf[_CHANNEL_NUM * i + 1];
y0 = x0 * num[0] + x1 * num[1] + x2 * num[2] - y1 * den[1] - y2 * den[2];
Y0 = X0 * num[0] + X1 * num[1] + X2 * num[2] - Y1 * den[1] - Y2 * den[2];
y2 = y1;
y1 = y0;
x2 = x1;
x1 = x0;
Y2 = Y1;
Y1 = Y0;
X2 = X1;
X1 = X0;
if (i%2 == 0) {
#if defined(_24BITS_ENABLE)
mic1[i/2] = speech_ssat_int24((int32_t)y0);
mic2[i/2] = speech_ssat_int24((int32_t)Y0);
#else
mic1[i/2] = speech_ssat_int16((int32_t)y0);
mic2[i/2] = speech_ssat_int16((int32_t)Y0);
#endif
}
}
}
int32_t anc_wnr_process(void *pcm_buf, uint32_t pcm_len)
{
if(wnr_open_flg == 0){
TRACE(2,"[%s] WARNING: wnr_open_flg = %d", __func__, wnr_open_flg);
return 0;
}
TRACE(2,"[%s] pcm_len = %d", __func__, pcm_len);
//audio_dump_clear_up();
WNR_PCM_T *tmp_buf= (WNR_PCM_T *)pcm_buf;
if (g_open_mode != ANC_WNR_OPEN_MODE_CONFIGURE) {
return 1;
}
// resample 16k-->8k
if (g_sample_rate == 16000) {
stereo_resample_16k(tmp_buf, pcm_len, (WNR_PCM_T *)af_stream_mic1, (WNR_PCM_T *)af_stream_mic2);
// 2ch --> 1ch, 16k --> 8k
pcm_len = pcm_len / _CHANNEL_NUM / 2;
}
else{
WNR_PCM_T *mic1 = (WNR_PCM_T *)af_stream_mic1;
WNR_PCM_T *mic2 = (WNR_PCM_T *)af_stream_mic2;
pcm_len = pcm_len / _CHANNEL_NUM;
for (uint32_t i=0; i<pcm_len; i++) {
mic1[i] = tmp_buf[2*i];
mic2[i] = tmp_buf[2*i + 1];
}
}
//TRACE(2,"[%s] new pcm_len = %d", __func__, pcm_len);
anc_wnr_process_frame((WNR_PCM_T *)af_stream_mic1, (WNR_PCM_T *)af_stream_mic2, pcm_len);
return 0;
}
//uint32_t wnr_ticks;
static uint32_t anc_wnr_callback(uint8_t *buf, uint32_t len)
{
//TRACE(2,"[%s] len = %d", __func__, len);
//audio_dump_clear_up();
#ifdef TEST_MIPS
start_ticks = hal_fast_sys_timer_get();
#endif
int32_t frame_len = len / SAMPLE_BYTES / _CHANNEL_NUM/ _LOOP_CNT;
ASSERT(frame_len == _FRAME_LEN, "[%s] frame len(%d) is invalid.", __func__, frame_len);
WNR_PCM_T *pcm_buf = (WNR_PCM_T *)buf;
WNR_PCM_T *mic1 = (WNR_PCM_T *)af_stream_mic1;
WNR_PCM_T *mic2 = (WNR_PCM_T *)af_stream_mic2;
for(int32_t j=0; j<_LOOP_CNT; j++){
for (int32_t i=0; i<frame_len; i++) {
mic1[i] = pcm_buf[_CHANNEL_NUM*i + 0];
mic2[i] = pcm_buf[_CHANNEL_NUM*i + 1];
}
anc_wnr_process_frame((WNR_PCM_T *)mic1, (WNR_PCM_T *)mic2, frame_len);
pcm_buf += _FRAME_LEN * _CHANNEL_NUM;
}
#ifdef TEST_MIPS
end_ticks = hal_fast_sys_timer_get();
used_mips = (end_ticks - start_ticks) * 1000 / (start_ticks - pre_ticks);
TRACE(2,"[%s] Usage: %d in a thousand (MIPS).", __func__, used_mips);
//wnr_ticks = start_ticks;
//TRACE(2,"[%s] WNR frame takes %d ms.", __func__, FAST_TICKS_TO_MS((start_ticks - pre_ticks)*100));
pre_ticks = start_ticks;
#endif
return len;
}
static void _open_mic(void)
{
struct AF_STREAM_CONFIG_T stream_cfg;
TRACE(1,"[%s] ...", __func__);
memset(&stream_cfg, 0, sizeof(stream_cfg));
stream_cfg.channel_num = (enum AUD_CHANNEL_NUM_T)_CHANNEL_NUM;
stream_cfg.sample_rate = (enum AUD_SAMPRATE_T)_SAMPLE_RATE;
stream_cfg.bits = (enum AUD_BITS_T)_SAMPLE_BITS;
stream_cfg.vol = 12;
stream_cfg.chan_sep_buf = false;
stream_cfg.device = AUD_STREAM_USE_INT_CODEC;
stream_cfg.io_path = AUD_INPUT_PATH_ANC_WNR;
stream_cfg.handler = anc_wnr_callback;
stream_cfg.data_size = sizeof(af_stream_buff);
stream_cfg.data_ptr = af_stream_buff;
ASSERT(stream_cfg.channel_num == 2, "[%s] channel number(%d) is invalid.", __func__, stream_cfg.channel_num);
TRACE(3,"[%s] sample_rate:%d, data_size:%d", __func__, stream_cfg.sample_rate, stream_cfg.data_size);
TRACE(2,"[%s] af_stream_buff = %p", __func__, af_stream_buff);
af_stream_open(ANC_WNR_STREAM_ID, AUD_STREAM_CAPTURE, &stream_cfg);
af_stream_start(ANC_WNR_STREAM_ID, AUD_STREAM_CAPTURE);
}
static void _close_mic(void)
{
TRACE(1,"[%s] ...", __func__);
af_stream_stop(ANC_WNR_STREAM_ID, AUD_STREAM_CAPTURE);
af_stream_close(ANC_WNR_STREAM_ID, AUD_STREAM_CAPTURE);
}