I have been working on a BLE project where two ESP32-WROOM-32 connected to BNO055 9DOF sensors send data to a ESP32 client. So far, I have been able to get a one to one connection between one server and the client. Would anyone have any suggestions as to how I can get the client to allow for two connections? Is there a way to manually connect or disconnect devices?
Code: Select all
// CLIENT | CENTRAL | RECEIVER (screen)
// SDA GPIO21 SCK GPIO22 for ESP32-WROOM-32
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <Adafruit_SSD1306.h>
#include <BLEDevice.h>
// BLE Server name (the other ESP32 name running the server sketch)
#define RG_Server "Right Glove"
#define RG_Server "Left Glove"
// replaced upper bytes with 0s, first two bytes will be for services and second two bytes characteristics
static BLEUUID RGlove_SERVICE_UUID("5adad05b-dee6-4c49-9a60-ce8cf0fe6a19");
// configure the characteristic to understand what kind of data the server is expecting to receive from the client
// or what the client will be receiving from the server
static BLEUUID IMU_CHAR_UUID("5adad051-dee6-4c49-9a60-ce8cf0fe6a19");
// Flags stating if should begin connecting and if the connection is up
static bool doConnect = false;
static bool connected = false;
// Address of the peripheral device. Address will be found during scanning...
static BLEAddress *pServerAddress;
// Characteristicd that we want to read
static BLERemoteCharacteristic *IMU_CHAR;
// Activate notify
const uint8_t notificationOn[] = {0x1, 0x0};
const uint8_t notificationOff[] = {0x0, 0x0};
/* SCREEN STUFF */
// screen dimensions
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 32
int ctr = 0;
// Variables to store orientation and acceleration values
// Flags to check whether new temperature and humidity readings are available
class MyClientCallback : public BLEClientCallbacks
{
void onConnect(BLEClient *pclient)
{
}
void onDisconnect(BLEClient *pclient)
{
connected = false;
Serial.println("onDisconnect");
}
};
class MyAdvertisedDeviceCallbacks : public BLEAdvertisedDeviceCallbacks
{
void onResult(BLEAdvertisedDevice advertisedDevice)
{
if (advertisedDevice.getName() == RG_Server)
{ // Check if the name of the advertiser matches
advertisedDevice.getScan()->stop(); // Scan can be stopped, we found what we are looking for
pServerAddress = new BLEAddress(advertisedDevice.getAddress()); // Address of advertiser is the one we need
doConnect = true; // Set indicator, stating that we are ready to connect
Serial.println("Device found. Connecting!");
}
}
};
// Connect to the BLE Server that has the name, Service, and Characteristics
bool connectToServer(BLEAddress pAddress)
{
BLEClient *pClient = BLEDevice::createClient();
pClient->setClientCallbacks(new MyClientCallback()); // not sure
// Connect to the remove BLE Server.
pClient->connect(pAddress);
Serial.println(" - Connected to server");
// Obtain a reference to the service we are after in the remote BLE server.
BLERemoteService *pRemoteService = pClient->getService(RGlove_SERVICE_UUID);
if (pRemoteService == nullptr)
{
Serial.print("Failed to find our service UUID: ");
Serial.println(RGlove_SERVICE_UUID.toString().c_str());
return (false);
}
// Obtain a reference to the characteristics in the service of the remote BLE server.
IMU_CHAR = pRemoteService->getCharacteristic(IMU_CHAR_UUID); // STACK SMASHING FAILURE FROM HERE
if (IMU_CHAR == nullptr)
{
Serial.print("Failed to find our characteristic UUID");
return false;
}
Serial.println(" - Found our characteristics");
// Assign callback functions for the Characteristics
IMU_CHAR->registerForNotify(IMU_NotifyCallback);
connected = true; // not sure
return true;
}
// When the BLE Server sends a new reading with the notify property
static void IMU_NotifyCallback(BLERemoteCharacteristic *pBLERemoteCharacteristic,
uint8_t *pData, size_t length, bool isNotify)
{
if (ctr == 0)
{
Serial.print("ORIENTATION X: ");
Serial.println((char *)pData);
ctr = ctr + 1;
}
else if (ctr == 1)
{
Serial.print("ORIENTATION Y: ");
Serial.println((char *)pData);
ctr = ctr + 1;
}
else if (ctr == 2)
{
Serial.print("ORIENTATION Z: ");
Serial.println((char *)pData);
ctr = ctr + 1;
}
else if (ctr == 3)
{
Serial.print("ACCELERATION X: ");
Serial.println((char *)pData);
ctr = ctr + 1;
}
else if (ctr == 4)
{
Serial.print("ACCELERATION Y: ");
Serial.println((char *)pData);
ctr = ctr + 1;
}
else if (ctr == 5)
{
Serial.print("ACCELERATION Z: ");
Serial.println((char *)pData);
ctr = 0;
}
}
void setup()
{
// Start serial communication
Serial.begin(115200);
Serial.println("Starting Arduino BLE Client application...");
// Init BLE device
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 30 seconds.
BLEScan *pBLEScan = BLEDevice::getScan();
pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
pBLEScan->setActiveScan(true);
pBLEScan->start(30);
}
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.
if (doConnect == true)
{
if (connectToServer(*pServerAddress))
{
Serial.println("We are now connected to the BLE Server.");
// Activate the Notify property of each Characteristic
IMU_CHAR->getDescriptor(BLEUUID((uint16_t)0x2902))->writeValue((uint8_t *)notificationOn, 2, true);
Serial.println("connected");
connected = true;
}
else
{
Serial.println("We have failed to connect to the server; Restart your device to scan for nearby BLE server again.");
}
doConnect = false;
}
// delay(1000); // Delay a second between loops.
}
Code: Select all
// SERVER | PERIPHERAL | TRANSMITTER (sensors)
// SDA GPIO21 SCK GPIO22 for ESP32-WROOM-32
// display for testing purposes only, delete when done with testing
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <Adafruit_SSD1306.h>
#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
#include <BLE2902.h>
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 32
// delay between fresh samples
#define BNO055_SAMPLERATE_DELAY_MS (100)
// Check I2C device address and correct line below (by default address is 0x29 or 0x28)
// id, address
Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28);
Adafruit_SSD1306 display = Adafruit_SSD1306(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire);
// replaced upper bytes with 0s, first two bytes will be for services and second two bytes characteristics
// BLEService
//#define RGlove_SERVICE_UUID BLEUUID((uint16_t)0x0000)
#define RGlove_SERVICE_UUID "5adad05b-dee6-4c49-9a60-ce8cf0fe6a19"
#define BLE_server "Right Glove"
// dumb values, easy to spot problem
float acc_x = -1000000, ori_x = -1000000;
float acc_y = -1000000, ori_y = -1000000;
float acc_z = -1000000, ori_z = -1000000;
float acc_xtest = 0, ori_xtest = 0;
float acc_ytest = 0, ori_ytest = 0;
float acc_ztest = 0, ori_ztest = 0;
BLECharacteristic IMU_CHAR( // x characteristic will only be read
"5adad051-dee6-4c49-9a60-ce8cf0fe6a19",
BLECharacteristic::PROPERTY_NOTIFY);
BLEDescriptor IMU_CHAR_descriptor(BLEUUID((uint16_t)0x2902)); // Add this line only if the characteristic has the Notify property
bool deviceConnected = false;
class MyServerCallbacks : public BLEServerCallbacks
{
void onConnect(BLEServer *pServer)
{
deviceConnected = true;
};
void onDisconnect(BLEServer *pServer)
{
deviceConnected = false;
}
};
void setup()
{
Serial.begin(115200);
/* Initialise the sensor */
if (!bno.begin())
{
// There was a problem detecting the BNO055 ... check your connections
Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
while (1)
;
}
if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C))
{ // Address 0x3C for 128x64
Serial.println(F("SSD1306 allocation failed"));
for (;;)
; // Don't proceed, loop forever
}
display.display();
delay(500); // Pause for 2 seconds
display.setTextSize(1);
display.setTextColor(WHITE);
display.setRotation(0);
delay(1000);
bno.setExtCrystalUse(true);
Serial.println("Starting BLE work!");
// Create BLE Device name
BLEDevice::init(BLE_server);
// create BLE Server
BLEServer *pServer = BLEDevice::createServer();
pServer->setCallbacks(new MyServerCallbacks());
// Create BLE Service
BLEService *RGlove_SERVICE = pServer->createService(RGlove_SERVICE_UUID);
// Create BLE Characteristics
RGlove_SERVICE->addCharacteristic(&IMU_CHAR);
IMU_CHAR_descriptor.setValue("Orientation and Acceleration");
/* Add Descriptors to the Characteristic*/
IMU_CHAR.addDescriptor(new BLE2902()); // Add this line only if the characteristic has the Notify property
// start continuously transmitting BLE advertising packets
// and will be visible to remote BLE central devices until it receives a new new connection
pServer->getAdvertising()->addServiceUUID(RGlove_SERVICE_UUID); // not in reference?
// Start the service
RGlove_SERVICE->start();
pServer->getAdvertising()->start();
Serial.println("Characteristic defined! Waiting a client connection to notify...");
}
void loop()
{
sensors_event_t orientationData, accelerometerData;
bno.getEvent(&orientationData, Adafruit_BNO055::VECTOR_EULER);
bno.getEvent(&accelerometerData, Adafruit_BNO055::VECTOR_ACCELEROMETER);
display.clearDisplay();
display.setCursor(0, 0);
readOriValues(&orientationData);
readAccValues(&accelerometerData);
if (deviceConnected)
{
// int16_t value;
delay(200);
static char ori_x_arr[6];
static char ori_y_arr[6];
static char ori_z_arr[6];
static char acc_x_arr[5];
static char acc_y_arr[5];
static char acc_z_arr[5];
dtostrf(ori_x, 4, 1, ori_x_arr);
dtostrf(ori_y, 4, 1, ori_y_arr);
dtostrf(ori_z, 4, 1, ori_z_arr);
dtostrf(acc_x, 3, 1, acc_x_arr);
dtostrf(acc_y, 3, 1, acc_y_arr);
dtostrf(acc_z, 3, 1, acc_z_arr);
// setValue only accepts 8bit unisgned integers
IMU_CHAR.setValue(ori_x_arr);
IMU_CHAR.notify();
Serial.print("ori X:");
Serial.println(ori_x);
IMU_CHAR.setValue(ori_y_arr);
IMU_CHAR.notify();
Serial.print("ori Y:");
Serial.println(ori_y);
IMU_CHAR.setValue(ori_z_arr);
IMU_CHAR.notify();
Serial.print("ori Z:");
Serial.println(ori_z);
IMU_CHAR.setValue(acc_x_arr);
IMU_CHAR.notify();
Serial.print("acc X:");
Serial.println(acc_x);
IMU_CHAR.setValue(acc_y_arr);
IMU_CHAR.notify();
Serial.print("acc Y:");
Serial.println(acc_y);
IMU_CHAR.setValue(acc_z_arr);
IMU_CHAR.notify();
Serial.print("acc Z:");
Serial.println(acc_z);
}
display.setTextSize(1);
display.print("ori");
display.setTextSize(1.3);
display.setCursor(0, 8);
display.print(ori_x, 1);
display.print(", ");
display.print(ori_y, 1);
display.print(", ");
display.print(ori_z, 1);
display.setCursor(0, 17);
display.setTextSize(1);
display.print("acc");
display.setTextSize(1.3);
display.setCursor(0, 25);
display.print(acc_x, 1);
display.print(", ");
display.print(acc_y, 1);
display.print(", ");
display.print(acc_z, 1);
display.display();
}
void readOriValues(sensors_event_t *event)
{
// read orientation event
ori_x = event->orientation.x;
ori_y = event->orientation.y;
ori_z = event->orientation.z;
}
void readAccValues(sensors_event_t *event)
{
// read acceleration event
acc_x = event->acceleration.x;
acc_y = event->acceleration.y;
acc_z = event->acceleration.z;
}