Ensure connectBLE doesn't return before HCI conn (#3229)

gap_connect() is async, so we need to pause until either it connects or
we timeout so that we can ger an accurate connection state.

Fixes #3221 (again)

Author: earlephilhower

libraries/BluetoothHCI/src/BluetoothHCI.h

@@ -49,6 +49,10 @@ class BluetoothHCI {
 
     void scanFree(); // Free allocated scan buffers
 
+    bool connected() {
+        return _hciConn != HCI_CON_HANDLE_INVALID;
+    }
+
     friend class BluetoothHIDMaster;
 
 protected:

libraries/BluetoothHIDMaster/src/BluetoothHIDMaster.cpp

@@ -225,7 +225,22 @@ bool BluetoothHIDMaster::connectBLE(const uint8_t *addr, int addrType) {
     }
     uint8_t a[6];
     memcpy(a, addr, sizeof(a));
-    return ERROR_CODE_SUCCESS == gap_connect(a, (bd_addr_type_t)addrType);
+    if (ERROR_CODE_SUCCESS != gap_connect(a, (bd_addr_type_t)addrType)) {
+        return false;
+    }
+    // GAP connection running async.  Wait for HCI connect
+    uint32_t now = millis();
+    while (millis() - now < 5000) {
+        if (_hci.connected()) {
+            break;
+        }
+        delay(25);
+    }
+    if (!_hci.connected()) {
+        gap_connect_cancel();
+        return false;
+    }
+    return _hci.connected();
 }
 
 bool BluetoothHIDMaster::connectBLE() {