BT connection WIP2

RP2040_Tracker.ino

@@ -66,11 +66,15 @@ static const uint8_t midi_char_uuid_be[] = {0x77, 0x72, 0xE5, 0xDB, 0x38, 0x68,
 
 static gatt_client_service_t found_service;
 static gatt_client_service_t target_service;
+static gatt_client_characteristic_t target_characteristic;
+static uint16_t midi_cccd_handle = 0;
 
 enum DiscoveryState {
   DISCOVERY_IDLE,
   DISCOVERY_FINDING_SERVICE,
   DISCOVERY_FINDING_CHARACTERISTIC,
+  DISCOVERY_FINDING_CCCD,
+  DISCOVERY_ENABLING_NOTIFICATIONS,
   DISCOVERY_COMPLETE
 };
 static DiscoveryState discovery_state = DISCOVERY_IDLE;
@@ -167,10 +171,46 @@ void handle_gatt_client_event(uint8_t packet_type, uint16_t channel, uint8_t *pa
               // The Value Handle is typically located 2 bytes before the UUID (in this specific event format)
               midi_char_value_handle = little_endian_read_16(packet, i - 2);
               Serial.print(F("Handle: 0x")); Serial.println(midi_char_value_handle, HEX);
+              // Capture the characteristic struct ONLY if this is the correct one
+              gatt_event_characteristic_query_result_get_characteristic(packet, &target_characteristic);
            }
         }
       }
       break;
+    case GATT_EVENT_ALL_CHARACTERISTIC_DESCRIPTORS_QUERY_RESULT:
+      {
+        Serial.print(F("Desc Packet: "));
+        for(int i=0; i<size; i++) {
+           Serial.print(packet[i], HEX); Serial.print(" ");
+        }
+        Serial.println();
+
+        uint16_t handle = little_endian_read_16(packet, 8);
+        const uint8_t *uuid = &packet[10];
+        Serial.print(F("Desc Handle: 0x")); Serial.print(handle, HEX);
+
+        // Check for CCCD (0x2902) in standard 128-bit base UUID (Little Endian)
+        uint8_t cccd_uuid128[] = {0xFB, 0x34, 0x9B, 0x5F, 0x80, 0x00, 0x00, 0x80, 0x00, 0x10, 0x00, 0x00, 0x02, 0x29, 0x00, 0x00};
+
+        if (memcmp(uuid, cccd_uuid128, 16) == 0) {
+          midi_cccd_handle = handle;
+          Serial.println(F(" -> Found CCCD!"));
+        } else {
+          Serial.println();
+        }
+      }
+      break;
+    case GATT_EVENT_NOTIFICATION:
+      {
+        uint16_t value_length = gatt_event_notification_get_value_length(packet);
+        const uint8_t *value = gatt_event_notification_get_value(packet);
+        Serial.print(F("RX MIDI: "));
+        for (int i = 0; i < value_length; i++) {
+          Serial.print(value[i], HEX); Serial.print(" ");
+        }
+        Serial.println();
+      }
+      break;
     case GATT_EVENT_QUERY_COMPLETE:
       if (discovery_state == DISCOVERY_FINDING_SERVICE) {
         if (service_found) {
@@ -184,13 +224,28 @@ void handle_gatt_client_event(uint8_t packet_type, uint16_t channel, uint8_t *pa
         }
       } else if (discovery_state == DISCOVERY_FINDING_CHARACTERISTIC) {
         if (midi_char_value_handle != 0) {
-           Serial.println(F("BLE MIDI Ready to send!"));
+           Serial.println(F("MIDI Char found. Searching for CCCD..."));
-           discovery_state = DISCOVERY_COMPLETE;
+           discovery_state = DISCOVERY_FINDING_CCCD;
-           midi_ready = true;
+           gatt_client_discover_characteristic_descriptors(handle_gatt_client_event, con_handle, &target_characteristic);
         } else {
            Serial.println(F("MIDI Characteristic not found."));
            discovery_state = DISCOVERY_IDLE;
         }
+      } else if (discovery_state == DISCOVERY_FINDING_CCCD) {
+        if (midi_cccd_handle != 0) {
+           Serial.println(F("CCCD found. Enabling Notifications..."));
+           discovery_state = DISCOVERY_ENABLING_NOTIFICATIONS;
+           uint8_t data[] = {0x01, 0x00}; // Enable Notification (Little Endian)
+           gatt_client_write_characteristic_descriptor_using_descriptor_handle(handle_gatt_client_event, con_handle, midi_cccd_handle, 2, data);
+        } else {
+           Serial.println(F("CCCD not found. Sending without notifications."));
+           discovery_state = DISCOVERY_COMPLETE;
+           midi_ready = true;
+        }
+      } else if (discovery_state == DISCOVERY_ENABLING_NOTIFICATIONS) {
+         Serial.println(F("Notifications Enabled. BLE MIDI Ready!"));
+         discovery_state = DISCOVERY_COMPLETE;
+         midi_ready = true;
       }
       break;
   }
@@ -245,10 +300,15 @@ void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint
         discovery_state = DISCOVERY_FINDING_SERVICE;
         service_found = false;
         midi_char_value_handle = 0;
+        midi_cccd_handle = 0;
         midi_ready = false;
         
         // Discover ALL Services to debug/find MIDI
         gatt_client_discover_primary_services(handle_gatt_client_event, con_handle);
+        
+        // Optimize Connection
+        gatt_client_send_mtu_negotiation(handle_gatt_client_event, con_handle);
+        gap_request_connection_parameter_update(con_handle, 12, 24, 0, 3000); // 15ms - 30ms interval
       }
       break;
     case HCI_EVENT_DISCONNECTION_COMPLETE:
@@ -323,6 +383,9 @@ void setup() {
   sequence[2].note = 62; // D4
   sequence[4].note = 64; // E4
   sequence[6].note = 65; // F4
+  sequence[8].note = 65; // F4
+  sequence[9].note = 62; // D4
+  sequence[12].note = 64; // E4
 
   display.clearDisplay();
   display.display();
@@ -341,9 +404,11 @@ void setup() {
 void sendMidi(uint8_t status, uint8_t note, uint8_t velocity) {
 #ifdef ENABLE_BTSTACK
   if (con_handle != HCI_CON_HANDLE_INVALID && midi_char_value_handle != 0) {
-    unsigned long t = millis();
+    // Use 0 timestamp for "immediate" execution to rule out timing/sync issues
-    uint8_t header = 0x80 | ((t >> 7) & 0x3F);
+    // Header: 10xxxxxx (Bit 7=1, Bit 6=0 for MIDI, Bits 5-0 = high 6 bits of timestamp)
-    uint8_t timestamp = 0x80 | (t & 0x7F);
+    // Timestamp: 1xxxxxxx (Bit 7=1, Bits 6-0 = low 7 bits of timestamp)
+    uint8_t header = 0x80; 
+    uint8_t timestamp = 0x80;
     uint8_t midiPacket[] = { header, timestamp, status, note, velocity };
     
     __lockBluetooth();
@@ -357,7 +422,11 @@ void sendMidi(uint8_t status, uint8_t note, uint8_t velocity) {
     if (err) {
       Serial.print(F("BLE Write Error: 0x")); Serial.println(err, HEX);
     } else {
-      Serial.print(F("MIDI TX: ")); Serial.println(note);
+      Serial.print(F("MIDI TX: ")); 
+      for(int i=0; i<sizeof(midiPacket); i++) {
+        Serial.print(midiPacket[i], HEX); Serial.print(" ");
+      }
+      Serial.println();
     }
   }
 #endif