fix gyro

CMakeLists.txt

@@ -88,7 +88,8 @@ target_link_libraries(ds5-bridge
         hardware_interp
         hardware_timer
         pico_btstack_classic
-        pico_cyw43_arch_threadsafe_background
+#        pico_cyw43_arch_threadsafe_background
+        pico_cyw43_arch_poll
         pico_btstack_cyw43
         tinyusb_device
         tinyusb_board

src/bt.cpp

@@ -443,16 +443,16 @@ void bt_write(uint8_t *data, uint16_t len) {
     }
 }
 
-uint8_t *get_feature_data(uint8_t reportId, uint16_t len) {
+std::vector<uint8_t> get_feature_data(uint8_t reportId, uint16_t len) {
     if (feature_data.find(reportId) == feature_data.end() || feature_data[reportId].empty()) {
         if (hid_control_cid != 0) {
             uint8_t get_feature[] = {0x43, reportId};
             l2cap_send(hid_control_cid, get_feature, len);
             printf("[L2CAP] Requesting Feature Report 0x%02X\n", reportId);
         }
-        return 0;
+        return {};
     }
-    return feature_data[reportId].data();
+    return feature_data[reportId];
 }
 
 void init_feature() {

src/bt.h

@@ -5,7 +5,8 @@
 #ifndef DS5_BRIDGE_BT_H
 #define DS5_BRIDGE_BT_H
 
-#include <stdint.h>
+#include <cstdint>
+#include <vector>
 
 enum CHANNEL_TYPE {
     INTERRUPT,
@@ -19,7 +20,7 @@ void bt_register_data_callback(bt_data_callback_t callback);
 void bt_send_packet(uint8_t *data, uint16_t len);
 void bt_send_control(uint8_t *data, uint16_t len);
 void bt_write(uint8_t* data,uint16_t len);
-uint8_t* get_feature_data(uint8_t reportId,uint16_t len);
+std::vector<uint8_t> get_feature_data(uint8_t reportId,uint16_t len);
 void init_feature();
 
 #endif //DS5_BRIDGE_BT_H

src/main.cpp

@@ -7,9 +7,9 @@
 #include "usb.h"
 #include "bt.h"
 #include "utils.h"
-#include "pico/multicore.h"
 #include "resample.h"
 #include "audio.h"
+#include "pico/cyw43_arch.h"
 
 int reportSeqCounter = 0;
 uint8_t packetCounter = 0;
@@ -53,12 +53,12 @@ uint16_t tud_hid_get_report_cb(uint8_t itf, uint8_t report_id, hid_report_type_t
     (void) buffer;
     (void) reqlen;
 
-    uint8_t *feature_data = get_feature_data(report_id, reqlen);
+    std::vector<uint8_t> feature_data = get_feature_data(report_id, reqlen);
-    if (feature_data) {
+    if (!feature_data.empty()) {
-        memcpy(buffer, feature_data, reqlen);
+        memcpy(buffer, feature_data.data() + 1, feature_data.size() - 1);
     }
 
-    return feature_data ? reqlen : 0;
+    return feature_data.empty() ? 0 : feature_data.size() - 1;
 }
 
 // Invoked when received SET_REPORT control request or
@@ -104,6 +104,7 @@ int main() {
     audio_init();
 
     while (1) {
+        cyw43_arch_poll();
         tud_task();
         audio_loop();
         interrupt_loop();