ESP32 BLE client connect to multiple servers about arduino-esp32 HOT 18 OPEN

/**
 * A BLE client example that is rich in capabilities.
 * There is a lot new capabilities implemented.
 * Copyright © 2022 Arduino SA
 * author unknown
 * updated by chegewara
 * modified for 3 connections by Sylvain Lareau
 *  usefull exemple https://www.youtube.com/watch?v=iCKIIMrphtg

 * IMPORTANT: might not work with older version
   Arduino Version: 2.0.1-nightly-20221024
   ESP32 2.0.5
The program is not final  and could easely be improve and simplified...
  
 */

#include "BLEDevice.h"


int LED_BUILTIN = 2;
bool toggle;


// The remote service we wish to connect to

//static BLEUUID0 serviceUUID("0000180d-0000-1000-8000-00805f9b34fb");  //0x180D
//BLEUUID become a type, like int or boolean
static BLEUUID serviceUUID0(BLEUUID((uint16_t)0x180D )); //Heart Rate
static BLEUUID charUUID0(BLEUUID((uint16_t)0x2A37));

static BLEUUID serviceUUID1(BLEUUID((uint16_t)0x1818 ));  // Cycling Power
static BLEUUID charUUID1(BLEUUID((uint16_t)0x2A63));

static BLEUUID serviceUUID2(BLEUUID((uint16_t)0xFEE0 ));  // SPO2
static BLEUUID charUUID2(BLEUUID((uint16_t)0xFEE1));


static boolean doConnect0 = false;
static boolean connected0 = false;

static boolean doConnect1 = false;
static boolean connected1 = false;

static boolean doConnect2 = false;
static boolean connected2 = false;

static boolean doScan0 = false;
static boolean doScan1 = false;
static boolean doScan2 = false;

static boolean notification0 = false;
static boolean notification1 = false;
static boolean notification2 = false;

static BLERemoteCharacteristic* pRemoteCharacteristic;
static BLEAdvertisedDevice* myDevice;


static void notifyCallback0(
  BLERemoteCharacteristic* pBLERemoteCharacteristic,
  uint8_t* pData,
  size_t length,
  bool isNotify) {
    //Serial.print("Notify callback for characteristic ");
   // Serial.print(pBLERemoteCharacteristic->getUUID().toString().c_str());
   // Serial.print(" of data length ");
   // Serial.println(length);
   // Serial.print("data: ");
    //Serial.println((char*)pData);


    Serial.print("HR_sensor value: ");
    Serial.print(pData[1], DEC);



}

static void notifyCallback1(
  BLERemoteCharacteristic* pBLERemoteCharacteristic,
  uint8_t* pData,
  size_t length,
  bool isNotify) {
    
    int  CPower =  pData[2] +  256 * pData[3];

    Serial.print("Power value: ");
    Serial.println(CPower);
   
}
static void notifyCallback2(
  BLERemoteCharacteristic* pBLERemoteCharacteristic,
  uint8_t* pData,
  size_t length,
  bool isNotify) {
   
    if (length == 4) {
      int SPO2 = pData[2];
      Serial.print("SPO2: "); Serial.println(SPO2);
      int PR =  pData[3];  // pulse rate
      Serial.print("PI: "); Serial.println(PR);
    }
}

class MyClientCallback0 : public BLEClientCallbacks {
   void onConnect(BLEClient* pclient) {
  }
  void onDisconnect(BLEClient* pclient) {
    connected0 = false;
    notification0 = false;
    Serial.println("onDisconnect 0 ");
  }
};

class MyClientCallback1 : public BLEClientCallbacks {
   void onConnect(BLEClient* pclient) {
  }
  void onDisconnect(BLEClient* pclient) {
    connected1 = false;
    notification1 = false;
    Serial.println("onDisconnect 1 ");
  }
};

class MyClientCallback2 : public BLEClientCallbacks {
   void onConnect(BLEClient* pclient) {
  }
  void onDisconnect(BLEClient* pclient) {
    connected2 = false;
    notification2 = false;
    Serial.println("onDisconnect 2");
  }
};

bool connectToServer(int sensor, BLEUUID serviceUUID, BLEUUID charUUID) {// BLEUUID = type like integer
    Serial.print("Forming a connection to ");
    Serial.println(myDevice->getAddress().toString().c_str());
    
    BLEClient*  pClient  = BLEDevice::createClient();
    Serial.println(" - Created client");

         if (sensor == 0) pClient->setClientCallbacks(new MyClientCallback0());
    else if (sensor == 1) pClient->setClientCallbacks(new MyClientCallback1());
    else if (sensor == 2) pClient->setClientCallbacks(new MyClientCallback2());

    // Connect to the remove BLE Server.
    pClient->connect(myDevice);  // if you pass BLEAdvertisedDevice instead of address, it will be recognized type of peer device address (public or private)
    Serial.print(" - Connected to server ");
    Serial.println(sensor);

    pClient->setMTU(517); //set client to request maximum MTU from server (default is 23 otherwise)
  
    // Obtain a reference to the service we are after in the remote BLE server.
    BLERemoteService* pRemoteService = pClient->getService(serviceUUID);
    if (pRemoteService == nullptr) {
      Serial.print("Failed to find our service UUID sensor ");
      Serial.print (sensor);
      Serial.print (" : ");
      Serial.println(serviceUUID0.toString().c_str());
      pClient->disconnect();
      return false;
    }
    Serial.print(" - Found our service ");
    Serial.println (sensor);


    // Obtain a reference to the characteristic in the service of the remote BLE server.
    pRemoteCharacteristic = pRemoteService->getCharacteristic(charUUID);
    if (pRemoteCharacteristic == nullptr) {
      Serial.print("Failed to find our characteristic UUID: ");
      Serial.println(charUUID0.toString().c_str());
      pClient->disconnect();
      return false;
    }

    Serial.print(" - Found our characteristic ");
    Serial.println (sensor);

    // Read the value of the characteristic.
    if(pRemoteCharacteristic->canRead()) {
      std::string value = pRemoteCharacteristic->readValue();
      Serial.print("The characteristic value was: ");
      Serial.println(value.c_str());
    }

    if (sensor == 0) {
      if(pRemoteCharacteristic->canNotify())
        pRemoteCharacteristic->registerForNotify(notifyCallback0);
      connected0 = true;
    return true;
    }
    else  if (sensor == 1) {
      if(pRemoteCharacteristic->canNotify())
        pRemoteCharacteristic->registerForNotify(notifyCallback1);
      connected1 = true;
    return true;
    }
    else  if (sensor == 2) {
      if(pRemoteCharacteristic->canNotify())
      pRemoteCharacteristic->registerForNotify(notifyCallback2);
      connected2 = true;
    return true;
    }
}

/**
 * Scan for BLE servers and find the one that advertises the service we are looking for.
 */
class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
 /**
   * Called for each advertising BLE server.
   */
  void onResult(BLEAdvertisedDevice advertisedDevice) {
    Serial.print("BLE Advertised Device found: ");
    Serial.println(advertisedDevice.toString().c_str());

    // We have found a device, let us now see if it contains the service we are looking for.
    if (advertisedDevice.haveServiceUUID() && advertisedDevice.isAdvertisingService(serviceUUID0)) {
      //BLEDevice::getScan()->stop();
      myDevice = new BLEAdvertisedDevice(advertisedDevice);  // not used?
      doConnect0 = true;
      doScan0 = true;
      Serial.print("Found serviceUUID0");
    } // Found our server

    // We have found a device, let us now see if it contains the service we are looking for.
    if (advertisedDevice.haveServiceUUID() && advertisedDevice.isAdvertisingService(serviceUUID1)) {
      //BLEDevice::getScan()->stop();
      myDevice = new BLEAdvertisedDevice(advertisedDevice);
      doConnect1 = true;
      doScan1 = true;
      Serial.print("Found serviceUUID1");
    } // Found our server1

    if (advertisedDevice.haveServiceUUID() && advertisedDevice.isAdvertisingService(serviceUUID2)) {
      //BLEDevice::getScan()->stop();
      myDevice = new BLEAdvertisedDevice(advertisedDevice);
      doConnect2 = true;
      doScan2 = true;
      Serial.print("Found serviceUUID2");
    } // Found our server2

  } // onResult
}; // MyAdvertisedDeviceCallbacks

//*********************************
void setup() {
  Serial.begin(1000000);
  Serial.println("Starting Arduino BLE Client application...");

  pinMode (LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, LOW);

  BLEDevice::init("");

  // Retrieve a Scanner and set the callback we want to use to be informed when we
  // have detected a new device.  Specify that we want active scanning and start the
  // scan to run for 5 seconds.

// I have played with those value, but lack good documentation... Help needed
  BLEScan* pBLEScan = BLEDevice::getScan();
  pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
  pBLEScan->setInterval(10000);   // millisec ,  will affect the program main loop frequency 10000 = 10sec
  pBLEScan->setWindow(400);  // millisec
  pBLEScan->setActiveScan(true);
  pBLEScan->start(5, false); // sec
  //pBLEScan->start(0);
} // End of setup.


// This is the Arduino main loop function.
void loop() {

  // If the flag "doConnect" is true then we have scanned for and found the desired
  // BLE Server with which we wish to connect.  Now we connect to it.  Once we are 
  // connected we set the connected flag to be true.

/*Serial.print("doConnect0 ");
Serial.println(doConnect0);
Serial.print("doConnect1 ");
Serial.println(doConnect1);
Serial.print("doConnect2 ");
Serial.println(doConnect2);*/

  if (toggle) digitalWrite(LED_BUILTIN, HIGH);
  else digitalWrite(LED_BUILTIN, LOW);
  toggle = !toggle;




  if (doConnect0 == true) {
    if (connectToServer(0, serviceUUID0, charUUID0)) {
      Serial.println("We are now connected to the BLE Server 0.");
      } else {
      Serial.println("We have failed to connect to the server 0; there is nothin more we will do.");
    }
    doConnect0 = false;
    }

  if (doConnect1 == true) {
    if (connectToServer(1, serviceUUID1, charUUID1)) {
      Serial.println("We are now connected to the BLE Server 1.");
    } else {
      Serial.println("We have failed to connect to the server 1; there is nothin more we will do.");
    }
    doConnect1 = false;
  }

  if (doConnect2 == true) {
     if (connectToServer(2, serviceUUID2, charUUID2)) {
      Serial.println("We are now connected to the BLE Server 2.");
    } else {
      Serial.println("We have failed to connect to the server 2 ; there is nothin more we will do.");
    }
    doConnect2 = false;
  }
/*
Serial.print("connected0 "); Serial.println(connected0);
Serial.print("connected1 ");Serial.println(connected1);
Serial.print("connected2 ");Serial.println(connected2);
Serial.print("notification0 ");Serial.println(notification0);
Serial.print("notification1 ");Serial.println(notification1);
Serial.print("notification2 ");Serial.println(notification2);
*/

/*Serial.print("doScan0 ");
Serial.println(doScan0);
Serial.print("doScan1 ");
Serial.println(doScan1);
Serial.print("doScan1 ");
Serial.println(doScan1);*/



//   Need to repeat  getScan()->start   or it wont connect to the second+ server: 
if (connected0 && connected1 && connected2) BLEDevice::getScan()->stop();
    else BLEDevice::getScan()->start(5, false);


  // Turn notification on
  if (connected0) {
    if (notification0 == false) {
      Serial.println(F("Turning Notification On for sensor 0")); // F is an optimisation thing...
      const uint8_t onPacket[] = {0x1, 0x0};  // not shure about these number but it works
      pRemoteCharacteristic->getDescriptor(BLEUUID((uint16_t)0x2902))->writeValue((uint8_t*)onPacket, 2, true);
      notification0 = true;
    } }
    else if(doScan0){  // try to reconnect
   // BLEDevice::getScan()->start(0);  
  BLEDevice::getScan()->start(5, false);    
  }

  

  if (connected1) {
    if (notification1 == false) {
      Serial.println(F("Turning Notification On for sensor 1"));
      const uint8_t onPacket[] = {0x1, 0x0}; // not shure about these number but it works
      pRemoteCharacteristic->getDescriptor(BLEUUID((uint16_t)0x2902))->writeValue((uint8_t*)onPacket, 2, true);
      notification1 = true;
    } }
    else if(doScan1){
    BLEDevice::getScan()->start(5, false);  // this is just example to start scan after disconnect, most likely there is better way to do it in arduino
   
  }

  if (connected2) {
    if (notification2 == false) {
      Serial.println(F("Turning Notification On for sensor 2"));
      const uint8_t onPacket[] = {0x1, 0x0}; // not shure about these number but it works
      pRemoteCharacteristic->getDescriptor(BLEUUID((uint16_t)0x2902))->writeValue((uint8_t*)onPacket, 2, true);
      notification2 = true;
    } }
    else if(doScan2){
      BLEDevice::getScan()->start(5, false);  // this is just example to start scan after disconnect, most likely there is better way to do it in arduino
    }
  
  delay(1000); // Delay a second between loops.
} // End of loop

{
    BLEDevice::init("");
    BLEScan *scn = BLEDevice::getScan();
    scn->setAdvertisedDeviceCallbacks(&scan_cb);
    scn->setActiveScan(false);
}

then I have 2 clients as example:
ClientExt1 client1;
ClientExt2 client2;

then call this in loop task
for(;;)
{
      if (should_scan_for_connection(client)) // what ever condition to start the search
      {
            BLEScan *scn = BLEDevice::getScan();
            scn->start(2, false);
            if (check_for_found_device(client1)) // BLEAdvertisedDeviceCallbacks was called
            {  
                 here you can use 
                client1.connect(BLEAddress address, esp_ble_addr_type_t type) 
                with data from the event advertisedDevice.getAddress()
            }      
            if (check_for_found_device(client2)) // BLEAdvertisedDeviceCallbacks was called
            {  
                 same here for the other client
            }      
      }
     else
     {
          block the thread until something changes
     }
}

scan_cb is the instance class MyAdvertisedDeviceCallbacks : public BLEAdvertisedDeviceCallbacks

for event when device is found and you need to check if it is something compatible in this method:
void MyAdvertisedDeviceCallbacks::onResult(BLEAdvertisedDevice advertisedDevice)
{
    check uuid or some other stuff matching your device here
    if you find the device you can stop the scan here immediately and store the address.
}
 
the advertisedDevice contains address to make connection advertisedDevice.getAddress()
so you can use that to connect to client.

you can create multiple instance of BLEClient, even extend this class with you own data. It woks as global instance or on heap with "new BLEClient". However managing dynamic data is harder to do right. Just do not use createClient. If you make more instances of the same ClientExt1 you check the client pointer inside any share callback so it still works for multiples.

class ClientExt1 : public BLEClient
{
    // data specific to your client
}