From c5805dcc8cc91a9878669d5e2916b245fbcbbe55 Mon Sep 17 00:00:00 2001
From: awalol <awalolcn@gmail.com>
Date: Sun, 26 Apr 2026 15:14:26 +0800
Subject: [PATCH] feat: headset auto switch

---
 src/audio.cpp | 18 +++++++++++++-----
 src/audio.h   |  1 +
 src/main.cpp  |  3 +++
 src/usb.cpp   |  4 ++--
 src/usb.h     |  2 +-
 5 files changed, 20 insertions(+), 8 deletions(-)

diff --git a/src/audio.cpp b/src/audio.cpp
index f9f581c..95f95c8 100644
--- a/src/audio.cpp
+++ b/src/audio.cpp
@@ -26,6 +26,7 @@
 static WDL_Resampler resampler;
 static uint8_t reportSeqCounter = 0;
 static uint8_t packetCounter = 0;
+static bool plug_headset = false;
 alignas(8) static uint32_t audio_core1_stack[8192];
 queue_t audio_fifo;
 queue_t opus_fifo;
@@ -36,6 +37,10 @@ struct opus_element {
     uint8_t data[200];
 };
 
+void set_headset(bool state) {
+    plug_headset = state;
+}
+
 void audio_loop() {
     // 1. 读取 USB 音频数据
     if (!tud_audio_available()) return;
@@ -54,8 +59,8 @@ void audio_loop() {
     int nframes = resampler.ResamplePrepare(frames, OUTPUT_CHANNELS, &in_buf);
 
     for (int i = 0; i < nframes; i++) {
-        audio_buf[audio_buf_pos++] = raw[i * INPUT_CHANNELS] / 32768.0f;
-        audio_buf[audio_buf_pos++] = raw[i * INPUT_CHANNELS + 1] / 32768.0f;
+        audio_buf[audio_buf_pos++] = raw[i * INPUT_CHANNELS] / 32768.0f * (volume[0] - 1.0f);
+        audio_buf[audio_buf_pos++] = raw[i * INPUT_CHANNELS + 1] / 32768.0f * (volume[0] - 1.0f);
         if (audio_buf_pos == 512 * 2) {
             audio_raw_element element{};
             memcpy(element.data,audio_buf,512 * 2 * 4);
@@ -81,8 +86,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 * (volume[0] ? : 1));
-        int val_r = (int) (out_buf[i * 2 + 1] * 127.0f * (volume[0] ? : 1));
+        int val_l = (int) (out_buf[i * 2] * 127.0f * volume[1]);
+        int val_r = (int) (out_buf[i * 2 + 1] * 127.0f * volume[1]);
         haptic_buf[haptic_buf_pos++] = (int8_t) std::clamp(val_l, -128, 127); // 似乎clamp有点多余？还是以防万一吧
         haptic_buf[haptic_buf_pos++] = (int8_t) std::clamp(val_r, -128, 127);
 
@@ -106,7 +111,10 @@ void audio_loop() {
         pkt[12] = SAMPLE_SIZE;
         memcpy(pkt + 13, haptic_buf, SAMPLE_SIZE);
         if (!queue_is_empty(&opus_fifo)) {
-            pkt[77] = 0x16 | 0 << 6 | 1 << 7;
+            pkt[77] = (plug_headset ? 0x16 : 0x13) | 0 << 6 | 1 << 7; // Speaker: 0x13
+                                              // L Headset Mono: 0x14
+                                              // L Headset R Speaker: 0x15
+                                              // Headset: 0x16
             pkt[78] = 200;
             opus_element opus_element{};
             if (!queue_try_remove(&opus_fifo,&opus_element)) {
diff --git a/src/audio.h b/src/audio.h
index 616be14..38aa1b0 100644
--- a/src/audio.h
+++ b/src/audio.h
@@ -8,5 +8,6 @@
 void audio_init();
 void audio_loop();
 void core1_entry();
+void set_headset(bool state);
 
 #endif //DS5_BRIDGE_AUDIO_H
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index 2f152c2..2d69a4c 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -35,6 +35,9 @@ void interrupt_loop() {
 void on_bt_data(CHANNEL_TYPE channel, uint8_t *data, uint16_t len) {
     // printf("[Main] BT data callback: channel=%u len=%u\n", channel, len);
     if (channel == INTERRUPT && data[1] == 0x31) {
+        if ((data[56] & 1) != (interrupt_in_data[53] & 1)) {
+            set_headset(data[56] & 1);
+        }
         memcpy(interrupt_in_data, data + 3, 63);
     }
 }
diff --git a/src/usb.cpp b/src/usb.cpp
index 5b147bd..55eaac8 100644
--- a/src/usb.cpp
+++ b/src/usb.cpp
@@ -6,7 +6,7 @@
 #include "bsp/board_api.h"
 
 uint8_t mute[2]; // 0: SPEAKER(0x02) 1: MIC(0x05)
-int16_t volume[2]; // 0: SPEAKER(0x02) 1: MIC(0x05)
+float volume[2] = {1.0f}; // 0: SPEAKER(0x02) 1: MIC(0x05)
 
 #define UAC1_ENTITY_SPK_FEATURE_UNIT    0x02
 #define UAC1_ENTITY_MIC_FEATURE_UNIT    0x05
@@ -65,7 +65,7 @@ static bool audio10_set_req_entity(tusb_control_request_t const *p_request, uint
                         // Only 1st form is supported
                         TU_VERIFY(p_request->wLength == 2);
 
-                        volume[index] = (int16_t) tu_unaligned_read16(pBuff) / 256;
+                        volume[index] = static_cast<float>(tu_unaligned_read16(pBuff)) / 256;
 
                         TU_LOG2("    Set Volume: %d dB of entity: %u\r\n", volume[index], entityID);
                         return true;
diff --git a/src/usb.h b/src/usb.h
index 6fe8b47..f975db0 100644
--- a/src/usb.h
+++ b/src/usb.h
@@ -6,6 +6,6 @@
 #define DS5_BRIDGE_USB_H
 
 extern uint8_t mute[2]; // 0: SPEAKER(0x02) 1: MIC(0x05)
-extern int16_t volume[2]; // 0: SPEAKER(0x02) 1: MIC(0x05)
+extern float volume[2]; // 0: SPEAKER(0x02) 1: MIC(0x05)
 
 #endif //DS5_BRIDGE_USB_H
\ No newline at end of file