From 5d32bb6de9d41b1b562c4e4c64850b5378db13c2 Mon Sep 17 00:00:00 2001 From: awalol Date: Fri, 8 May 2026 23:57:11 +0800 Subject: [PATCH] haptics gain before resample --- src/audio.cpp | 46 +++++++++++++++++++++++++--------------------- 1 file changed, 25 insertions(+), 21 deletions(-) diff --git a/src/audio.cpp b/src/audio.cpp index 4c02769..c953202 100644 --- a/src/audio.cpp +++ b/src/audio.cpp @@ -35,6 +35,7 @@ alignas(8) static uint32_t audio_core1_stack[8192]; queue_t audio_fifo; static uint8_t opus_buf[200]; critical_section_t opus_cs; + struct audio_raw_element { float data[512 * 2]; }; @@ -60,24 +61,27 @@ void audio_loop() { WDL_ResampleSample *in_buf; int nframes = resampler.ResamplePrepare(frames, OUTPUT_CHANNELS, &in_buf); - float gain = mute[0] ? 0.0f : powf(10.0f, get_config().speaker_volume / 20.0f); + const float audio_gain = mute[0] ? 0.0f : powf(10.0f, get_config().speaker_volume / 20.0f); + const float haptics_gain = get_config().haptics_gain; for (int i = 0; i < nframes; i++) { - audio_buf[audio_buf_pos++] = raw[i * INPUT_CHANNELS] / 32768.0f * gain; - audio_buf[audio_buf_pos++] = raw[i * INPUT_CHANNELS + 1] / 32768.0f * gain; + audio_buf[audio_buf_pos++] = raw[i * INPUT_CHANNELS] / 32768.0f * audio_gain; + audio_buf[audio_buf_pos++] = raw[i * INPUT_CHANNELS + 1] / 32768.0f * audio_gain; if (audio_buf_pos == 512 * 2) { static audio_raw_element element{}; - memcpy(element.data,audio_buf,512 * 2 * 4); - if (queue_is_full(&audio_fifo)){ + memcpy(element.data, audio_buf, 512 * 2 * 4); + if (queue_is_full(&audio_fifo)) { queue_try_remove(&audio_fifo,NULL); } - if (!queue_try_add(&audio_fifo,&element)) { + if (!queue_try_add(&audio_fifo, &element)) { printf("[Audio] Warning: audio_fifo add failed\n"); } audio_buf_pos = 0; } - in_buf[i * 2] = (WDL_ResampleSample) raw[i * INPUT_CHANNELS + 2] / 32768.0f; - in_buf[i * 2 + 1] = (WDL_ResampleSample) raw[i * INPUT_CHANNELS + 3] / 32768.0f; + in_buf[i * 2] = static_cast(clamp(raw[i * INPUT_CHANNELS + 2] / 32768.0f * haptics_gain, + -1.0f, 1.0f)); + in_buf[i * 2 + 1] = static_cast(clamp(raw[i * INPUT_CHANNELS + 3] / 32768.0f * haptics_gain, + -1.0f, 1.0f)); } // 3. 48kHz -> 3kHz 重采样 @@ -89,8 +93,8 @@ void audio_loop() { // 4. 转换为int8并缓冲，满64字节即组包发送 for (int i = 0; i < out_frames; i++) { - int val_l = (int) (out_buf[i * 2] * 127.0f * max(get_config().haptics_gain,1.0f)); - int val_r = (int) (out_buf[i * 2 + 1] * 127.0f * max(get_config().haptics_gain,1.0f)); + int val_l = static_cast(out_buf[i * 2] * 127.0f); + int val_r = static_cast(out_buf[i * 2 + 1] * 127.0f); haptic_buf[haptic_buf_pos++] = (int8_t) clamp(val_l, -128, 127); // 似乎clamp有点多余？还是以防万一吧 haptic_buf[haptic_buf_pos++] = (int8_t) clamp(val_r, -128, 127); @@ -117,7 +121,7 @@ void audio_loop() { pkt[77] = (plug_headset ? 0x16 : 0x13) | 0 << 6 | 1 << 7; // Speaker: 0x13 pkt[78] = 200; critical_section_enter_blocking(&opus_cs); - memcpy(pkt + 79,opus_buf,200); + memcpy(pkt + 79, opus_buf, 200); critical_section_exit(&opus_cs); bt_write(pkt, sizeof(pkt)); @@ -130,9 +134,9 @@ void audio_init() { resampler.SetRates(48000, 3000); resampler.SetFeedMode(true); resampler.Prealloc(2, 24, 6); - queue_init(&audio_fifo,sizeof(audio_raw_element),2); + queue_init(&audio_fifo, sizeof(audio_raw_element), 2); critical_section_init(&opus_cs); - multicore_launch_core1_with_stack(core1_entry,audio_core1_stack,sizeof(audio_core1_stack)); + multicore_launch_core1_with_stack(core1_entry, audio_core1_stack, sizeof(audio_core1_stack)); } static OpusEncoder *encoder; @@ -140,7 +144,7 @@ static WDL_Resampler resampler_audio; void core1_entry() { int error = 0; - encoder = opus_encoder_create(48000,2,OPUS_APPLICATION_AUDIO,&error); + encoder = opus_encoder_create(48000, 2,OPUS_APPLICATION_AUDIO, &error); if (error != 0) { printf("[Audio] OpusEncoder create failed\n"); return; @@ -149,27 +153,27 @@ void core1_entry() { opus_encoder_ctl(encoder,OPUS_SET_BITRATE(200 * 8 * 100)); opus_encoder_ctl(encoder,OPUS_SET_VBR(false)); opus_encoder_ctl(encoder,OPUS_SET_COMPLEXITY(0)); // max 4 - resampler_audio.SetMode(true,0,false); - resampler_audio.SetRates(51200,48000); + resampler_audio.SetMode(true, 0, false); + resampler_audio.SetRates(51200, 48000); resampler_audio.SetFeedMode(true); resampler_audio.Prealloc(2, 512, 480); while (true) { static audio_raw_element audio_element{}; - queue_remove_blocking(&audio_fifo,&audio_element); + queue_remove_blocking(&audio_fifo, &audio_element); // 将 512 frames 重采样成 480 frames 以解决噪音问题。感谢 @Junhoo WDL_ResampleSample *in_buf; int nframes = resampler_audio.ResamplePrepare(512, 2, &in_buf); - for (int i = 0; i < nframes * 2;i++) { + for (int i = 0; i < nframes * 2; i++) { in_buf[i] = audio_element.data[i]; } static WDL_ResampleSample out_buf[480 * 2]; - resampler_audio.ResampleOut(out_buf,nframes,480,2); + resampler_audio.ResampleOut(out_buf, nframes, 480, 2); static uint8_t out[200]; - (void)opus_encode_float(encoder,out_buf,480,out,200); + (void) opus_encode_float(encoder, out_buf, 480, out, 200); critical_section_enter_blocking(&opus_cs); - memcpy(opus_buf,out,200); + memcpy(opus_buf, out, 200); critical_section_exit(&opus_cs); } }